本發(fā)明涉及一種自動(dòng)掉頭路徑規(guī)劃及其控制方法，特別涉及一種用于農(nóng)機(jī)無人駕駛的田間掉頭路徑規(guī)劃及其控制方法。

背景技術(shù)：

農(nóng)機(jī)在導(dǎo)航作業(yè)時(shí)一般需要進(jìn)行自動(dòng)掉頭對(duì)準(zhǔn)下一作業(yè)行的跟蹤，控制農(nóng)機(jī)準(zhǔn)確的掉頭技術(shù)是實(shí)現(xiàn)精準(zhǔn)農(nóng)業(yè)的關(guān)鍵，該技術(shù)可提高農(nóng)機(jī)的作業(yè)精度和作業(yè)效率，使駕駛員擺脫長(zhǎng)時(shí)間勞累的重復(fù)駕駛工作，降低勞動(dòng)力。

在現(xiàn)有的農(nóng)機(jī)路徑跟蹤方法上，控制農(nóng)機(jī)按照設(shè)定好的路徑行走的控制方法有多種，如BUG算法、人工勢(shì)場(chǎng)法、VFH算法、模糊邏輯算法、模糊神經(jīng)網(wǎng)絡(luò)算法等，這些算法的應(yīng)用場(chǎng)景均是復(fù)雜的作業(yè)環(huán)境，算法的邏輯復(fù)雜， 應(yīng)用于農(nóng)機(jī)作業(yè)不復(fù)雜的作業(yè)環(huán)境下，反應(yīng)速度較慢，反而降低其控制精度；另外，對(duì)于由若干個(gè)大小不同的田塊組成的田地，工作人員需攜帶多個(gè)導(dǎo)航系統(tǒng)，到一個(gè)田地工作時(shí)，工作人員需根據(jù)田塊的實(shí)際大小選擇一個(gè)與之對(duì)應(yīng)的導(dǎo)航系統(tǒng)安裝到農(nóng)機(jī)上，到另一塊寬幅不同的田塊時(shí)，將以上導(dǎo)航系統(tǒng)拆下來，將新的導(dǎo)航系統(tǒng)重新安裝到農(nóng)機(jī)上，無法根據(jù)農(nóng)機(jī)的實(shí)際作業(yè)模式進(jìn)行路徑控制，適用范圍小，成本高，操作麻煩。

技術(shù)實(shí)現(xiàn)要素：

針對(duì)現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的在于克服上述現(xiàn)有技術(shù)中的不足之處，解決現(xiàn)有技術(shù)中控制精度低且適用范圍小的技術(shù)問題，提供一種用于農(nóng)機(jī)無人駕駛的田間掉頭路徑規(guī)劃及其控制方法，本發(fā)明的控制精度高，適用范圍廣。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：一種用于農(nóng)機(jī)無人駕駛的田間掉頭路徑規(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)過程中，農(nóng)機(jī)通過傳感器獲得農(nóng)機(jī)位置信息，使用預(yù)瞄和PI控制器組合起來計(jì)算當(dāng)前的前輪轉(zhuǎn)向角，控制農(nóng)機(jī)的轉(zhuǎn)向角實(shí)現(xiàn)農(nóng)機(jī)的自動(dòng)掉頭。

本發(fā)明工作時(shí)，工作人員先根據(jù)田塊的寬幅輸入農(nóng)機(jī)的作業(yè)模式，通過傳感器感應(yīng)農(nóng)機(jī)周圍的環(huán)境信息，農(nóng)機(jī)掉頭時(shí)，農(nóng)機(jī)根據(jù)實(shí)際的作業(yè)模式選擇設(shè)定好的掉頭路徑，農(nóng)機(jī)通過傳感器檢測(cè)獲得農(nóng)機(jī)的位置信息，使用預(yù)瞄控制器和PI控制器相結(jié)合起來實(shí)時(shí)計(jì)算出農(nóng)機(jī)的前輪轉(zhuǎn)角，農(nóng)機(jī)在行走過程中通過實(shí)時(shí)調(diào)整農(nóng)機(jī)的前輪轉(zhuǎn)角，通過控制農(nóng)機(jī)的前輪轉(zhuǎn)向角使農(nóng)機(jī)沿著設(shè)定的曲線行走，從而實(shí)現(xiàn)農(nóng)機(jī)的掉頭；本發(fā)明中設(shè)定了不同作業(yè)模式下對(duì)應(yīng)的掉頭路徑，可通過農(nóng)機(jī)的實(shí)際作業(yè)模式選擇與其相對(duì)應(yīng)的掉頭路徑，集成度高，適用范圍更加廣泛，降低成本，操作簡(jiǎn)便；使用預(yù)瞄控制器和PI控制器的結(jié)合控制農(nóng)機(jī)的前輪轉(zhuǎn)向角使農(nóng)機(jī)沿著設(shè)定好的避障曲線行走，控制精度高；本發(fā)明可應(yīng)用于農(nóng)機(jī)在導(dǎo)航作業(yè)時(shí)的自動(dòng)掉頭的工作中。

為了進(jìn)一步提高農(nóng)機(jī)掉頭速度，本發(fā)明的步驟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為犁具寬度；此設(shè)計(jì)中，將農(nóng)機(jī)的作業(yè)模式分為播種和耕地，不同作業(yè)模式，對(duì)應(yīng)的掉頭路徑不同，優(yōu)化掉頭曲線，縮短農(nóng)機(jī)掉頭時(shí)間。

為了進(jìn)一步提高農(nóng)機(jī)掉頭效率，所述多線型路徑一由直線段一、圓弧段一和圓弧段二組成，圓弧段一和圓弧段二相切且對(duì)應(yīng)的半徑相同，農(nóng)機(jī)依次經(jīng)過直線段一、圓弧段一和圓弧段二實(shí)現(xiàn)掉頭；所述交叉型路徑由圓弧段三、直線段二和圓弧段四組成，圓弧段三和圓弧段四相交且關(guān)于直線段一的中心對(duì)稱，圓弧段三和圓弧段四的半徑大小相同，農(nóng)機(jī)依次經(jīng)過圓弧段三、直線段二和圓弧段四實(shí)現(xiàn)掉頭；所述圓弧路徑由圓弧段五、圓弧段六和圓弧段七組成，所述圓弧段六分別和圓弧段五、圓弧段七相切，所述圓弧段五和圓弧段七關(guān)于圓弧段六的中心線對(duì)稱設(shè)置，農(nóng)機(jī)依次經(jīng)過圓弧段五、圓弧段六和圓弧段七實(shí)現(xiàn)掉頭；所述多線型路徑二由圓弧段八、直線段三和圓弧段九組成，圓弧段八和圓弧段九關(guān)于直線段三的中心線對(duì)稱設(shè)置，農(nóng)機(jī)依次經(jīng)過圓弧段八、直線段九和圓弧段九實(shí)現(xiàn)掉頭。

為了進(jìn)一步提高農(nóng)機(jī)掉頭路徑的精度，所述多線型路徑一的各個(gè)線段的數(shù)學(xué)關(guān)系式為：

（1-1）

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

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

（1-2）

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

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

（1-3）

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

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

（1-4）

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

此設(shè)計(jì)中，根據(jù)不同作業(yè)模式建立不同的路徑模型，為準(zhǔn)確跟蹤曲線建立基礎(chǔ)。

為了提高跟蹤曲線的控制精度，步驟3中，利用預(yù)瞄控制器計(jì)算出理論前輪轉(zhuǎn)角，具體的為，確定農(nóng)機(jī)的前視距離l，取路徑上的一點(diǎn)為預(yù)瞄點(diǎn)（x₀，y₀），R為前視距離對(duì)應(yīng)的圓弧段的半徑， l、R和x之間的關(guān)系式為：

（2-1）

將農(nóng)機(jī)簡(jiǎn)化為二輪車，建立農(nóng)機(jī)的運(yùn)動(dòng)學(xué)模型：

（2-2）

根據(jù)阿克曼轉(zhuǎn)向幾何關(guān)系，農(nóng)機(jī)的轉(zhuǎn)彎半徑和前輪轉(zhuǎn)角、軸距的關(guān)系式為：

（2-3）

將（3-2）和（3-3）結(jié)合起來得到理論轉(zhuǎn)角的計(jì)算公式為：

（2-4）

其中，θ為農(nóng)機(jī)的航向偏差角，農(nóng)機(jī)后軸中心記為點(diǎn)A，農(nóng)機(jī)后軸中心A和預(yù)瞄點(diǎn)P連線記為AP，航向偏差角為農(nóng)機(jī)航向與AP之間的夾角，δ為農(nóng)機(jī)的理論前輪轉(zhuǎn)角，L為農(nóng)機(jī)的軸距，v為農(nóng)機(jī)的行駛速度，x₀、y₀為農(nóng)機(jī)的橫坐標(biāo)和縱坐標(biāo)，設(shè)定的曲線路徑上距離農(nóng)機(jī)中心最近的點(diǎn)即為M。

為了進(jìn)一步提高跟蹤曲線的控制精度，步驟3中，使用PI控制方法計(jì)算出補(bǔ)償前輪轉(zhuǎn)角，具體的包括以下步驟：

（301）根據(jù)農(nóng)機(jī)的位置和預(yù)瞄點(diǎn)計(jì)算出農(nóng)機(jī)的航向偏差角θ作為PI的誤差輸入e（k）；

（302）計(jì)算出當(dāng)前的積分累計(jì)誤差；

（303）PI控制輸出補(bǔ)償前輪轉(zhuǎn)角，補(bǔ)償前輪轉(zhuǎn)角的計(jì)算公式為：

（3）

其中，K_p為比例增益，K_i為積分增益，e（i）為i時(shí)間點(diǎn)下對(duì)應(yīng)的誤差輸入，k為總采樣時(shí)間點(diǎn)數(shù)，u（k）為PI控制的輸出，具體的為當(dāng)前的補(bǔ)償前輪轉(zhuǎn)角；

此設(shè)計(jì)中，考慮到農(nóng)機(jī)在實(shí)際運(yùn)行中，轉(zhuǎn)向關(guān)系并不是完全滿足預(yù)瞄控制器中的阿克曼轉(zhuǎn)向原理，會(huì)存在一定的控制誤差，將PI控制方法對(duì)預(yù)瞄控制方法進(jìn)行輔助控制，消除預(yù)瞄控制器帶來的誤差，進(jìn)一步提高控制精度。

作為本發(fā)明的進(jìn)一步改進(jìn)，步驟3中，使用預(yù)瞄和PI控制器組合起來計(jì)算當(dāng)前的前輪轉(zhuǎn)向角具體的為，通過預(yù)瞄控制器計(jì)算得到理論前輪轉(zhuǎn)角，PI控制器計(jì)算得到補(bǔ)償前輪轉(zhuǎn)角，將理論前輪轉(zhuǎn)角和補(bǔ)償前輪轉(zhuǎn)角相加后得到實(shí)際前輪轉(zhuǎn)角，將實(shí)際前輪轉(zhuǎn)角實(shí)時(shí)輸出給農(nóng)機(jī)模型，控制農(nóng)機(jī)的前輪轉(zhuǎn)角實(shí)現(xiàn)農(nóng)機(jī)的自動(dòng)避障。

作為本發(fā)明的進(jìn)一步改進(jìn)，所述傳感器包括位置傳感器和角度傳感器，所述角度傳感器檢測(cè)農(nóng)機(jī)的轉(zhuǎn)向角，所述位置傳感器獲得農(nóng)機(jī)的位置信息；農(nóng)機(jī)的前后側(cè)分別設(shè)有視覺機(jī)器相機(jī)，所述視覺機(jī)器相機(jī)獲取農(nóng)機(jī)所處環(huán)境的地理信息。

附圖說明

圖1為本發(fā)明中播種時(shí)的掉頭路徑軌跡圖。

圖2為本發(fā)明中耕地時(shí)小幅寬的掉頭路徑軌跡圖。

圖3為本發(fā)明中耕地時(shí)中幅寬的掉頭路徑軌跡圖。

圖4為本發(fā)明中耕地時(shí)大幅寬的掉頭路徑軌跡圖。

圖5為本發(fā)明的控制框圖。

圖6為本發(fā)明中的預(yù)瞄算法示意圖。

圖7為本發(fā)明中PI控制的曲線跟蹤示意圖。

圖8為本發(fā)明中設(shè)定曲線與跟蹤曲線的仿真模擬曲線跟蹤對(duì)比圖。

圖9為本發(fā)明中的航向偏差模擬圖。

圖10為本發(fā)明中的橫向偏差模擬圖。

圖11為本發(fā)明中的前輪轉(zhuǎn)向角曲線圖。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步的說明。

如圖1～11所示的一種用于農(nóng)機(jī)無人駕駛的田間掉頭路徑規(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)過程中，農(nóng)機(jī)通過傳感器獲得農(nóng)機(jī)位置信息，使用曲線跟蹤方法獲得實(shí)時(shí)曲線曲率、航向偏差和橫向偏差，用狀態(tài)反饋控制器和自適應(yīng)控制器的結(jié)合計(jì)算當(dāng)前的前輪轉(zhuǎn)向角，控制農(nóng)機(jī)的轉(zhuǎn)向角實(shí)現(xiàn)農(nóng)機(jī)的自動(dòng)掉頭；

本發(fā)明的步驟1中，農(nóng)機(jī)的作業(yè)模式包括播種和耕地；另外，步驟1中的傳感器包括位置傳感器和角度傳感器，角度傳感器檢測(cè)農(nóng)機(jī)的轉(zhuǎn)向角，位置傳感器獲得農(nóng)機(jī)的位置信息；農(nóng)機(jī)的前后側(cè)分別設(shè)有視覺機(jī)器相機(jī)，視覺機(jī)器相機(jī)獲取農(nóng)機(jī)所處環(huán)境的地理信息；

本發(fā)明的步驟2中，播種情況下對(duì)應(yīng)的掉頭路徑為多線型路徑一，如圖1所示，多線型路徑一由直線段一l1、圓弧段一ρ1和圓弧段二ρ2組成，圓弧段一ρ1和圓弧段二ρ2相切且對(duì)應(yīng)的半徑相同，農(nóng)機(jī)依次經(jīng)過直線段一l1、圓弧段一ρ1和圓弧段二ρ2實(shí)現(xiàn)掉頭；耕地情況下，根據(jù)不同的犁具寬度設(shè)置不同的掉頭路徑，犁具寬度包括小寬幅、中寬幅和大寬幅，小寬幅對(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為犁具寬度；小寬幅、中寬幅和大寬幅下對(duì)應(yīng)的掉頭路徑分別為交叉型路徑、圓弧路徑和多線型路徑二，交叉型路徑（如圖2所示）由圓弧段三ρ3、直線段二l2和圓弧段四ρ4組成，圓弧段三ρ3和圓弧段四ρ4相交且關(guān)于直線段二l2的中心對(duì)稱設(shè)置，圓弧段三ρ3和圓弧段四ρ4的半徑大小相同，農(nóng)機(jī)依次經(jīng)過圓弧段三ρ3、直線段二l2和圓弧段四ρ4實(shí)現(xiàn)掉頭；圓弧路徑（如圖3所示）由圓弧段五ρ5、圓弧段六ρ6和圓弧段七ρ7組成，圓弧段六ρ6分別和圓弧段五ρ5、圓弧段七ρ7相切，圓弧段五ρ5和圓弧段七ρ7關(guān)于圓弧段六ρ6的中心線對(duì)稱設(shè)置，農(nóng)機(jī)依次經(jīng)過圓弧段五ρ5、圓弧段六ρ6和圓弧段七ρ7實(shí)現(xiàn)掉頭；多線型路徑二（如圖4所示）由圓弧段八ρ8、直線段三l3和圓弧段九ρ9組成，圓弧段八ρ8和圓弧段九ρ9關(guān)于直線段三l3的中心線對(duì)稱設(shè)置，農(nóng)機(jī)依次經(jīng)過圓弧段八ρ8、直線段三l3和圓弧段九ρ9實(shí)現(xiàn)掉頭；

其中，多線型路徑一的各個(gè)線段的數(shù)學(xué)關(guān)系式為：

（1-1）

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

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

（1-2）

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

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

（1-3）

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

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

（1-4）

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

步驟3中，利用預(yù)瞄控制器計(jì)算出理論前輪轉(zhuǎn)角，具體的為，確定農(nóng)機(jī)的前視距離l，取路徑上的一點(diǎn)為預(yù)瞄點(diǎn)（x₀，y₀），R為前視距離對(duì)應(yīng)的圓弧段的半徑， l、R和x之間的關(guān)系式為：

（2-5）

由（3-5）可以得到：

（2-1）

將農(nóng)機(jī)簡(jiǎn)化為二輪車，建立農(nóng)機(jī)的運(yùn)動(dòng)學(xué)模型：

（2-2）

根據(jù)阿克曼轉(zhuǎn)向幾何關(guān)系，農(nóng)機(jī)的轉(zhuǎn)彎半徑和前輪轉(zhuǎn)角、軸距的關(guān)系式為：

（2-3）

將（3-2）和（3-3）結(jié)合起來得到理論轉(zhuǎn)角的計(jì)算公式為：

（2-4）

其中，θ為農(nóng)機(jī)的航向偏差角，農(nóng)機(jī)后軸中心記為點(diǎn)A，農(nóng)機(jī)后軸中心A和預(yù)瞄點(diǎn)P連線記為AP，航向偏差角為農(nóng)機(jī)航向與AP之間的夾角，δ為農(nóng)機(jī)的理論前輪轉(zhuǎn)角，L為農(nóng)機(jī)的軸距，v為農(nóng)機(jī)的行駛速度，x₀、y₀為農(nóng)機(jī)的橫坐標(biāo)和縱坐標(biāo)，設(shè)定的曲線路徑上距離農(nóng)機(jī)中心最近的點(diǎn)即為M，D為圓弧段的圓心與預(yù)瞄點(diǎn)P橫坐標(biāo)間的橫向距離；為農(nóng)機(jī)在x軸方向上的速度，為農(nóng)機(jī)在y軸方向上的速度，v為農(nóng)機(jī)的速度。

考慮到農(nóng)機(jī)在實(shí)際運(yùn)行中，轉(zhuǎn)向關(guān)系并不是完全滿足預(yù)瞄控制器中的阿克曼轉(zhuǎn)向原理，會(huì)存在一定的控制誤差，將PI控制方法對(duì)預(yù)瞄控制方法進(jìn)行輔助控制，消除預(yù)瞄控制器帶來的誤差，進(jìn)一步提高控制精度，其中，使用PI控制方法計(jì)算出補(bǔ)償前輪轉(zhuǎn)角，具體的包括以下步驟：

（301）根據(jù)農(nóng)機(jī)的位置和預(yù)瞄點(diǎn)計(jì)算出農(nóng)機(jī)的航向偏差角θ作為PI的誤差輸入e（k）；

（302）計(jì)算出當(dāng)前的積分累計(jì)誤差；

（303）PI控制輸出補(bǔ)償前輪轉(zhuǎn)角，補(bǔ)償前輪轉(zhuǎn)角的計(jì)算公式為：

（3）

其中，K_p為比例增益，K_i為積分增益，e（i）為i時(shí)間點(diǎn)下對(duì)應(yīng)的誤差輸入，k為總采樣時(shí)間點(diǎn)數(shù)，u（k）為PI控制的輸出，具體的為當(dāng)前的補(bǔ)償前輪轉(zhuǎn)角；

步驟3中，使用預(yù)瞄和PI控制器組合起來計(jì)算當(dāng)前的前輪轉(zhuǎn)向角具體的為，通過預(yù)瞄控制器計(jì)算得到理論前輪轉(zhuǎn)角，PI控制器計(jì)算得到補(bǔ)償前輪轉(zhuǎn)角，將理論前輪轉(zhuǎn)角和補(bǔ)償前輪轉(zhuǎn)角相加后得到實(shí)際前輪轉(zhuǎn)角，將實(shí)際前輪轉(zhuǎn)角實(shí)時(shí)輸出給農(nóng)機(jī)模型，控制農(nóng)機(jī)的前輪轉(zhuǎn)角實(shí)現(xiàn)農(nóng)機(jī)的自動(dòng)避障。

使用matlab軟件對(duì)本發(fā)明進(jìn)行仿真，設(shè)定由直線和圓弧構(gòu)成的曲線，給定農(nóng)機(jī)的起始位置為（-13,1），初始航向角度為0rad，Kp取為2，Ki取為0.01；利用本發(fā)明的控制方法對(duì)設(shè)定好的曲線進(jìn)行跟蹤，圖8～圖11的橫坐標(biāo)均為農(nóng)機(jī)的行駛距離，從圖8中可以看出，跟蹤曲線與設(shè)定曲線基本重合；從圖9中可以看出，橫向偏差維持在10cm左右；從圖10中可以看出，航向偏差大概在0.08rad左右；從圖11中可以看出，前輪轉(zhuǎn)向角為1階慣性環(huán)節(jié)，沒有突變，和實(shí)際相符；通過以上分析，使用本發(fā)明中的控制方法進(jìn)行農(nóng)機(jī)的掉頭路徑控制，控制精度高，農(nóng)機(jī)基本按照設(shè)定的曲線路徑行走。

本發(fā)明工作時(shí)，輸入農(nóng)機(jī)的作業(yè)模式，視覺機(jī)器相機(jī)采集農(nóng)機(jī)周圍的環(huán)境信息，根據(jù)周圍環(huán)境信息確認(rèn)農(nóng)機(jī)是否掉頭；農(nóng)機(jī)掉頭時(shí)，農(nóng)機(jī)根據(jù)實(shí)際的作業(yè)模式選擇設(shè)定好的掉頭路徑，當(dāng)作業(yè)模式為播種時(shí)，農(nóng)機(jī)的掉頭路徑選擇多線型路徑一，農(nóng)機(jī)通過傳感器檢測(cè)獲得農(nóng)機(jī)的位置信息，通過預(yù)瞄控制器得到理論前輪轉(zhuǎn)角，PI控制器補(bǔ)償預(yù)瞄控制器產(chǎn)生的控制誤差輸出補(bǔ)償前輪轉(zhuǎn)角，將理論前輪轉(zhuǎn)向角和期望補(bǔ)償轉(zhuǎn)向角相加后得到實(shí)際前輪轉(zhuǎn)角并將前輪轉(zhuǎn)角輸出給農(nóng)機(jī)模型，位置傳感器實(shí)時(shí)檢測(cè)農(nóng)機(jī)所在位置并將位置信息發(fā)送給預(yù)瞄控制器和PI控制器，通過控制農(nóng)機(jī)的前輪轉(zhuǎn)角使農(nóng)機(jī)沿著設(shè)定的曲線行走，從而實(shí)現(xiàn)農(nóng)機(jī)的掉頭；當(dāng)作業(yè)模式為耕地時(shí)，根據(jù)農(nóng)機(jī)上懸掛的犁具寬度選擇掉頭路徑，掉頭路徑軌跡選定后，按照上述相同的控制過程控制農(nóng)機(jī)的掉頭；本發(fā)明中設(shè)定了不同作業(yè)模式下對(duì)應(yīng)的掉頭路徑，可通過農(nóng)機(jī)的實(shí)際作業(yè)模式選擇與其相對(duì)應(yīng)的掉頭路徑，集成度高，適用范圍更加廣泛，降低成本，操作簡(jiǎn)便；使用預(yù)瞄控制器和PI控制器的結(jié)合控制農(nóng)機(jī)的前輪轉(zhuǎn)向角使農(nóng)機(jī)沿著設(shè)定好的避障曲線行走，控制精度高；本發(fā)明可應(yīng)用于農(nóng)機(jī)在導(dǎo)航作業(yè)時(shí)的自動(dòng)掉頭的工作中。

本發(fā)明并不局限于上述實(shí)施例，在本發(fā)明公開的技術(shù)方案的基礎(chǔ)上，本領(lǐng)域的技術(shù)人員根據(jù)所公開的技術(shù)內(nèi)容，不需要?jiǎng)?chuàng)造性的勞動(dòng)就可以對(duì)其中的一些技術(shù)特征作出一些替換和變形，這些替換和變形均在本發(fā)明保護(hù)范圍內(nèi)。