[發(fā)明專利]用于農(nóng)機(jī)無(wú)人駕駛的自動(dòng)掉頭路徑規(guī)劃及其控制方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201710156037.7
• 申請(qǐng)日: 2017-03-16
• 公開(kāi)（公告）號(hào): CN107045347A
• 公開(kāi)（公告）日: 2017-08-15
• 發(fā)明（設(shè)計(jì)）人: 程方;林德相;袁洪良;王爍;徐濤;王浩;吳飛
• 申請(qǐng)（專利權(quán)）人: 無(wú)錫卡爾曼導(dǎo)航技術(shù)有限公司
• 主分類號(hào): G05D1/02
• 分類號(hào): G05D1/02
• 代理公司: 南京蘇科專利代理有限責(zé)任公司32102
• 代理人: 沈良菊

1.一種用于農(nóng)機(jī)無(wú)人駕駛的自動(dòng)掉頭路徑規(guī)劃及其控制方法，其特征在于，

步驟1：輸入農(nóng)機(jī)的作業(yè)模式；

步驟2：獲取農(nóng)機(jī)所處環(huán)境的地理信息，根據(jù)農(nóng)機(jī)實(shí)際的作業(yè)模式選擇對(duì)應(yīng)的掉頭路徑；

步驟3：農(nóng)機(jī)在前進(jìn)過(guò)程中，農(nóng)機(jī)通過(guò)傳感器獲得農(nóng)機(jī)位置信息，使用曲線跟蹤方法獲得實(shí)時(shí)曲線曲率和實(shí)時(shí)的農(nóng)機(jī)航向偏差、橫向偏差，用狀態(tài)反饋控制器和自適應(yīng)控制器的結(jié)合計(jì)算當(dāng)前的前輪轉(zhuǎn)向角，控制農(nóng)機(jī)的轉(zhuǎn)向角實(shí)現(xiàn)農(nóng)機(jī)的自動(dòng)掉頭。

2.根據(jù)權(quán)利要求1所述的用于農(nóng)機(jī)無(wú)人駕駛的自動(dòng)掉頭路徑規(guī)劃及其控制方法，其特征在于，步驟1中，農(nóng)機(jī)的作業(yè)模式包括播種和耕地；步驟2中，播種情況下對(duì)應(yīng)的掉頭路徑為多線型路徑一，耕地情況下對(duì)應(yīng)的掉頭路徑包括交叉型路徑、圓弧路徑和多線型路徑二，且根據(jù)不同的犁具寬度選擇不同的掉頭路徑，犁具寬度包括小寬幅、中寬幅和大寬幅，小寬幅、中寬幅和大寬幅作業(yè)模式下對(duì)應(yīng)的掉頭路徑分別為交叉型路徑、圓弧路徑和多線型路徑二，小寬幅對(duì)應(yīng)的犁具寬度的數(shù)值范圍為Rmin<W<1.5Rmin,中寬幅對(duì)應(yīng)的犁具寬度的數(shù)值范圍為1.5Rmin<W≤2Rmin,大寬幅對(duì)應(yīng)的犁具寬度的數(shù)值范圍為W>2Rmin，Rmin為農(nóng)機(jī)的最小轉(zhuǎn)彎半徑，W為犁具寬度。

3.根據(jù)權(quán)利要求2所述的用于農(nóng)機(jī)無(wú)人駕駛的自動(dòng)掉頭路徑規(guī)劃及其控制方法，其特征在于：所述多線型路徑一由直線段一、圓弧段一和圓弧段二組成，圓弧段一和圓弧段二相切且對(duì)應(yīng)的半徑相同，農(nóng)機(jī)依次經(jīng)過(guò)直線段一、圓弧段一和圓弧段二實(shí)現(xiàn)掉頭；所述交叉型路徑由圓弧段三、直線段二和圓弧段四組成，圓弧段三和圓弧段四相交且對(duì)應(yīng)的半徑大小相同，農(nóng)機(jī)依次經(jīng)過(guò)圓弧段三、直線段二和圓弧段四實(shí)現(xiàn)掉頭；所述圓弧路徑由圓弧段五、圓弧段六和圓弧段七組成，所述圓弧段六分別和圓弧段五、圓弧段七相切，所述圓弧段五和圓弧段七關(guān)于圓弧段六的中心線對(duì)稱設(shè)置，農(nóng)機(jī)依次經(jīng)過(guò)圓弧段五、圓弧段六和圓弧段七實(shí)現(xiàn)掉頭；所述多線型路徑二由圓弧段八、直線段三和圓弧段九組成，圓弧段八和圓弧段九關(guān)于直線段三的中心線對(duì)稱設(shè)置。

4.根據(jù)權(quán)利要求3所述的用于農(nóng)機(jī)無(wú)人駕駛的自動(dòng)掉頭路徑規(guī)劃及其控制方法，其特征在于，所述多線型路徑一的各個(gè)線段的數(shù)學(xué)關(guān)系式為：

R₁＝R₂＝R_min,AC＝W,R₁+R₁cosα＝M+R₁-R₁cosα,AE＝2R₁sinα (1)

其中，R1為圓弧段一對(duì)應(yīng)的半徑，R2為圓弧段二對(duì)應(yīng)的半徑，AE為直線段一的長(zhǎng)度，AC為農(nóng)機(jī)開(kāi)始轉(zhuǎn)彎的后軸中心與農(nóng)機(jī)轉(zhuǎn)彎結(jié)束的后軸中心間連接線段的長(zhǎng)度；

所述交叉型路徑一的各個(gè)線段的數(shù)學(xué)關(guān)系式為：

R₃＝R₄＝R_min,A₁B₁＝W,C₁D₁＝2R_min-W (2)

其中，R3為圓弧段三對(duì)應(yīng)的半徑，R4為圓弧段四對(duì)應(yīng)的半徑，C₁D₁為直線段二的長(zhǎng)度，A₁B₁農(nóng)機(jī)開(kāi)始轉(zhuǎn)彎的后軸中心與農(nóng)機(jī)轉(zhuǎn)彎結(jié)束的后軸中心間連接線段的長(zhǎng)度；

所述圓弧路徑的各個(gè)線段的數(shù)學(xué)關(guān)系式為：

其中，Rt為圓弧段五、圓弧段六和圓弧段七對(duì)應(yīng)的半徑，圓弧段五、圓弧段六和圓弧段七的圓心的連線構(gòu)成三角形，β為圓弧段六的圓心分別與圓弧段五和圓弧段七的圓心連線之間的夾角，α為圓弧段五的圓心分別與圓弧段六和圓弧段七的圓心連線之間的夾角，γ為圓弧段七的圓心分別與圓弧段五和圓弧段六的圓心連線之間的夾角；

所述多線型路徑二的各個(gè)線段的數(shù)學(xué)關(guān)系式為：

R₅＝R₆＝R_min.A₂D₂＝W,B₂C₂＝W-2R_min (4)

其中，R5為圓弧段八對(duì)應(yīng)的半徑，R6為圓弧段九對(duì)應(yīng)的半徑，B₂C₂為直線段三的長(zhǎng)度，A₂D₂為圓弧段八的圓心和圓弧段九的圓心的連線。