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

背景技術(shù)：

農(nóng)機在自動導航作業(yè)時多在環(huán)境部分未知的情況下運行，實現(xiàn)對人進行安全保護和對農(nóng)作物的傷害程度減到最低，同時又能最大的發(fā)揮自主導航農(nóng)業(yè)車輛的生產(chǎn)效率，將是一個重要的研究問題，同時農(nóng)機可能會遇到電線桿、小石塊等相對比較小的障礙物，需要農(nóng)機能自動繞過這些障礙物并迅速回到作業(yè)的路線。

現(xiàn)有技術(shù)中，針對比較小的障礙物，采用最短切線法設置避障路徑，最短切線法形成的避障路徑由兩段直線段和一段圓弧段組成，直線段分別與圓弧段相切，這種避障路徑雖然簡單快捷，對于具有最小轉(zhuǎn)彎半徑的拖拉機很難按照折角進行轉(zhuǎn)彎且很難控制，若控制農(nóng)機按照這種避障路徑行走，農(nóng)機的控制精度很低；另外，控制農(nóng)機按照設定好的避障路徑行走的控制方法有多種，如BUG算法、人工勢場法、VFH算法、模糊邏輯算法、模糊神經(jīng)網(wǎng)絡算法等，這些算法的應用場景均是復雜的作業(yè)環(huán)境，算法的邏輯復雜， 應用于農(nóng)機作業(yè)不復雜的作業(yè)環(huán)境下，反應速度較慢，反而降低其控制精度；總而言之，無論是路徑規(guī)劃還是避障路徑的控制方法，控制農(nóng)機按照設定好的路徑行走的精度很低，偏離設定的避障路徑，導致農(nóng)機繞過障礙物行走的路程遠，從開始進入避障路徑至回到農(nóng)機原始直線行走路徑的時間長。

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

針對現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的在于克服上述現(xiàn)有技術(shù)中的不足之處，解決現(xiàn)有技術(shù)中避障路徑難控制且控制精度低的技術(shù)問題，提供一種用于農(nóng)機無人駕駛的避障路徑規(guī)劃及其控制方法，本發(fā)明中的避障路徑容易控制且行走的路程短，控制精度高。

本發(fā)明的目的是這樣實現(xiàn)的：一種用于農(nóng)機無人駕駛的避障路徑規(guī)劃及其控制方法，農(nóng)機自動繞開障礙物的具體步驟為，

步驟1：通過傳感器獲取農(nóng)機環(huán)境信息做出避障決策；

步驟2：使用改進的最短切線法離線計算出一條理論避障路徑；

步驟3：利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟2中的理論避障路徑得到實際避障路徑，使得優(yōu)化后的路徑更加容易控制，使用預瞄和PI控制器組合起來控制農(nóng)機的前輪轉(zhuǎn)角使農(nóng)機沿著實際避障路徑行走繞開障礙物。

本發(fā)明工作時，農(nóng)機在行走過程中安裝在農(nóng)機上的傳感器感應農(nóng)機周圍的環(huán)境信息，當農(nóng)機前方有障礙物時，做出避障決策，使用改進的最短切線法計算出一條理論避障路徑，對理論避障路徑進行優(yōu)化得到一條更加容易控制的實際避障路徑，使用預瞄控制器和PI控制器相結(jié)合起來實時計算出農(nóng)機的前輪轉(zhuǎn)角，農(nóng)機在行走過程中通過實時調(diào)整農(nóng)機的前輪轉(zhuǎn)角使農(nóng)機沿著設定的實際避障路徑行走，從而實現(xiàn)農(nóng)機的自動避障；本發(fā)明通過改進后的最短切線法計算出一條理論避障路徑，使用基于Bezier曲線的路徑優(yōu)化方法對理論避障路徑進行優(yōu)化，使避障路徑更加容易控制，通過預瞄控制器和PI控制器的結(jié)合控制農(nóng)機的前輪轉(zhuǎn)向角使農(nóng)機沿著設定好的避障曲線行走，控制精度高；可應用于無人駕駛的農(nóng)機在田間作業(yè)時自動避障的工作中。

為了進一步提高獲得理論避障路徑的可靠性，步驟2中，計算理論避障路徑具體地為，計算農(nóng)機前方障礙物的特征圓的大小、農(nóng)機與障礙物的距離，根據(jù)特征圓的大小設定安全距離，根據(jù)農(nóng)機的犁具寬度和農(nóng)機最小轉(zhuǎn)彎半徑，設定一條理論避障路徑。

為了使避障路徑更加容易控制，步驟2中，最短切線法具體的為，以障礙物的中心為圓心做特征圓，特征圓的半徑為r_min+w/2，理論避障路徑由圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三組成，圓弧段一的一端與農(nóng)機原始的直線路徑相切，圓弧段一的另一端與直線段一的一端相切，直線段一的另一端和直線段二的一端分別與圓弧段二相切，直線段二的另一端與圓弧段三相切，圓弧段二為特征圓上的一段，圓弧段一和圓弧段三關(guān)于圓弧段二的中心線對稱設置，農(nóng)機依次經(jīng)過圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三繞過障礙物，其中，r_min為農(nóng)機的最小轉(zhuǎn)彎半徑，w為農(nóng)機的作業(yè)寬度，障礙物的外接圓半徑小于最小轉(zhuǎn)彎半徑。

為了進一步提高農(nóng)機掉頭路徑的精度，所述圓弧段一的半徑為r_min，所述圓弧段三的半徑為r_min，圓弧段一的起點記為H點，圓弧段一的圓心記為O₁點，直線段一與農(nóng)機原始的直線路徑的相交點記為J，直線段一與圓弧段二的相切點記為D，農(nóng)機原始路徑與特征圓的相交點分別記為K和K’，JK=w/2，圓弧段二的圓心記為O點，O的坐標設為（a，b），圓弧段二的中心點記為B點，J點的坐標記為（x1，y1），JD的方程可以寫成：

（1-1）；

特征圓的方程可以寫成：

（1-2）

通過（1-1）和（1-2）可以求出k，D點為JD和特征圓的相交點，以此解出D點坐標；

設點O₁的坐標為（x₂,y₂），則點O₁到直線JD的距離為：

根據(jù)公式（1-3）和（1-4）求出O₁的坐標；則H點的坐標為（x₂，y₁），B點的坐標為（a，b+r）；

此設計對構(gòu)成理論避障路徑的各個線段建立數(shù)學關(guān)系的聯(lián)系，明確曲線的具體形狀，求出彎折點處坐標，方便下一步對理論避障路徑的優(yōu)化。

為了優(yōu)化本發(fā)明中利用改進的最短切線法設計的理論避障路徑，步驟3中，利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟2中的理論避障路徑，具體的為，建立Bezier方程，

(1)給定空間n+1個點的位置矢量，則參數(shù)曲線上各點坐標的插值公式為：

（2-1）

其中構(gòu)成該曲線的特征點， 是n次Bernstein基函數(shù)：

（2-2）

由上述公式，可以得出三次和二次Bezier曲線的數(shù)學表達式，當n=3時，Q(t)為三次多項式，有四個控制點，其矩陣形式表達為：

（2-3）

當n=2，Q(t)為二次多項式，有三個控制點，矩陣表達式為：

（2-4）

（2）Bezier曲線的曲率表達式為：

（2-5）

其中，y=f（x）表示曲線的方程，y’為曲線的一階導數(shù)，y”為二階導數(shù)；

曲率半徑為：

（2-6）；

此設計中，提出Bezier曲線優(yōu)化方法對理論避障路徑進行優(yōu)化，具體的將曲率不連續(xù)的理論避障路徑優(yōu)化成曲率連續(xù)的實際避障路徑，實際避障路徑更加容易控制。

為了提高Bezier曲線的可控性，選取三次Bezier曲線，針對三次Bezier曲線：

（2-7）

（2-8）

其中，X0、X1、X2、X3分別為P0點、P1點、P2點和P3點處的橫向坐標，Y0、Y1、Y2和Y3分別為P0點、P1點、P2點和P3處的縱向坐標；

P0點對應圓弧段一的起點H（x₂，y₁），P3點對應圓弧段二的中心點B（a，b+r），P1點（（x₂+a）/2，y₁），P2點（（x₂+a）/2，b+r），則實際故障路徑對應的曲線的曲率半徑計算公式為：

（2-9）；

此設計中，選擇理論避障路徑上的彎折點作為Bezier曲線優(yōu)化方法中的優(yōu)化點，優(yōu)化后的路徑更加簡單，曲率連續(xù)，易于控制。

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

（3-1）

將農(nóng)機簡化為二輪車，建立農(nóng)機的運動學模型：

（3-2）

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

（3-3）

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

（3-4）

其中，θ為農(nóng)機的航向偏差角，農(nóng)機后軸中心記為點A，農(nóng)機后軸中心A和預瞄點P連線記為AP，航向偏差角為農(nóng)機航向與AP之間的夾角，δ為農(nóng)機的理論前輪轉(zhuǎn)角，L為農(nóng)機的軸距，v為農(nóng)機的行駛速度，設定的曲線路徑上距離農(nóng)機中心最近的點即為M。

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

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

（302）計算出當前的積分累計誤差；

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

（4）

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

將預瞄控制器輸出的理論前輪轉(zhuǎn)角和PI控制器輸出的補償前輪轉(zhuǎn)角相加后為實際前輪轉(zhuǎn)角，實際前輪轉(zhuǎn)角實時輸出給農(nóng)機模型，控制農(nóng)機的前輪轉(zhuǎn)角實現(xiàn)農(nóng)機自動避障；此設計中，農(nóng)機在實際運行中，轉(zhuǎn)向關(guān)系并不是完全滿足預瞄控制器中的阿克曼轉(zhuǎn)向原理，會存在一定的控制誤差，將PI控制方法和預瞄控制方法相結(jié)合來消除預瞄控制器帶來的誤差，進一步提高控制精度。

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

附圖說明

圖1為本發(fā)明中的理論避障路徑軌跡圖。

圖2為本發(fā)明中三次Bezier曲線的特性圖一。

圖3為本發(fā)明中三次Bezier曲線的特性圖二。

圖4為本發(fā)明中的實際避障路徑軌跡圖。

圖5為本發(fā)明中實際避障路徑的曲率圖。

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

圖7為本發(fā)明中的預瞄算法跟蹤示意圖。

圖8為本發(fā)明中的曲線跟蹤示意圖。

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

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

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

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

具體實施方式

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

如圖1～12所示的一種用于農(nóng)機無人駕駛的避障路徑規(guī)劃及其控制方法，農(nóng)機自動繞開障礙物的具體步驟為，

步驟1：通過傳感器獲取農(nóng)機環(huán)境信息做出避障決策；

步驟2：使用改進的最短切線法離線計算出一條理論避障路徑；

步驟3：利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟2中的理論避障路徑得到實際避障路徑，使得優(yōu)化后的路徑更加容易控制，使用預瞄和PI控制器組合起來控制農(nóng)機的前輪轉(zhuǎn)角使農(nóng)機沿著實際避障路徑行走繞開障礙物；

本發(fā)明的步驟1中，傳感器包括位置傳感器、角度傳感器和機器視覺相機，角度傳感器檢測農(nóng)機的轉(zhuǎn)向角，位置傳感器獲得農(nóng)機的位置信息；視覺機器相機設有2個并分別設置在農(nóng)機的前后側(cè)，視覺機器相機獲取農(nóng)機所處環(huán)境的地理信息；

步驟2中，計算理論避障路徑具體地為，計算農(nóng)機前方障礙物的特征圓的大小、農(nóng)機與障礙物的距離，根據(jù)特征圓的大小設定安全距離，根據(jù)農(nóng)機的犁具寬度和農(nóng)機最小轉(zhuǎn)彎半徑，設定一條理論避障路徑。

為了使避障路徑更加容易控制，步驟2中，最短切線法具體的為，以障礙物的中心為圓心做特征圓，特征圓的半徑為r_min+w/2，如圖1所示，理論避障路徑由圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三組成，圓弧段一的一端與農(nóng)機原始的直線路徑相切，圓弧段一的另一端與直線段一的一端相切，直線段一的另一端和直線段二的一端分別與圓弧段二相切，直線段二的另一端與圓弧段三相切，圓弧段二為特征圓上的一段，圓弧段一和圓弧段三關(guān)于圓弧段二的中心線對稱設置，農(nóng)機依次經(jīng)過圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三繞過障礙物，其中，r_min為農(nóng)機的最小轉(zhuǎn)彎半徑，w為農(nóng)機的作業(yè)寬度，障礙物的外接圓半徑小于最小轉(zhuǎn)彎半徑；

圓弧段一的半徑為r_min，所述圓弧段三的半徑為r_min，圓弧段一的起點記為H點，圓弧段一的圓心記為O₁點，直線段一與農(nóng)機原始的直線路徑的相交點記為J，直線段一與圓弧段二的相切點記為D，農(nóng)機原始路徑與特征圓的相交點分別記為K和K’，JK=w/2，圓弧段二的圓心記為O點，O的坐標設為（a，b），圓弧段二的中心點記為B點，J點的坐標記為（x1，y1），JD的方程可以寫成：

（1-1）；

特征圓的方程可以寫成：

（1-2）

通過（1-1）和（1-2）可以求出k，D點為JD和特征圓的相交點，以此解出D點坐標；

設點O₁的坐標為（x₂,y₂），則點O₁到直線JD的距離為：

根據(jù)公式（1-3）和（1-4）求出O₁的坐標；則H點的坐標為（x₂，y₁），B點的坐標為（a，b+r）；

為了優(yōu)化本發(fā)明中利用改進的最短切線法設計的理論避障路徑，步驟3中，利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟2中的理論避障路徑，具體的為，建立Bezier方程，

(1)給定空間n+1個點的位置矢量，則參數(shù)曲線上各點坐標的插值公式為：

（2-1）

其中構(gòu)成該曲線的特征點，是n次Bernstein基函數(shù)：

（2-2）

由上述公式，可以得出三次和二次Bezier曲線的數(shù)學表達式，當n=3時，Q(t)為三次多項式，有四個控制點，其矩陣形式表達為：

（2-3）

當n=2，Q(t)為二次多項式，有三個控制點，矩陣表達式為：

（2-4）

（2）Bezier曲線的性質(zhì)

由公式(2-1)得到Bezier曲線兩端點的值：

當t=0時，

（2-10）

當t=1時，

（2-11）

對公式(2-1)求出Bezier曲線的導函數(shù)為：

（2-12）

在起點t=0時，

（2-13）

在起點t=1時，

（2-14）

二次Bezier曲線端點性質(zhì)：

（2-15）

三次Bezier曲線端點性質(zhì)為：

（2-16）

從分析Bezier曲線的性質(zhì)可以看出，在起點和終點處的切線方向和特征多邊形的第一條邊和最后一條邊的走向一致，則通過規(guī)劃Bezier曲線起始點和終點的切線方向，實現(xiàn)了對車輛初始位姿和目標位姿的確定；從圖2和圖3中可以看出，三次Bezier曲線都落在了特征多邊形P0P1P2P3內(nèi)，增加了Bezier曲線的可控性；

（3）Bezier曲線的曲率表達式為：

（2-5）

其中，y=f（x）表示曲線的方程，y’為曲線的一階導數(shù)，y”為二階導數(shù)；

曲率半徑為：

（2-6）；

針對以上分析，為了提高Bezier曲線的可控性，本發(fā)明選取三次Bezier曲線，針對三次Bezier曲線：

（2-7）

（2-8）

其中，X0、X1、X2、X3分別為P0點、P1點、P2點和P3點處的橫向坐標，Y0、Y1、Y2和Y3分別為P0點、P1點、P2點和P3處的縱向坐標；

P0點對應圓弧段一的起點H（x₂，y₁），P3點對應圓弧段二的中心點B（a，b+r），P1點（（x₂+a）/2，y₁），P2點（（x₂+a）/2，b+r），則實際故障路徑對應的曲線的曲率半徑計算公式為：

（2-9）；

用三次Bezier曲線優(yōu)化理論避障路徑形成的實際避障路徑由兩條關(guān)于圓弧段二的中心對稱設置的實際避障曲線ρ0組成（如圖6所示）；

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

（3-5）

由（3-5）可以得到：

（3-1）

將農(nóng)機簡化為二輪車，建立農(nóng)機的運動學模型：

（3-2）

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

（3-3）

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

（3-4）

其中，θ為農(nóng)機的航向偏差角，農(nóng)機后軸中心記為點A，農(nóng)機后軸中心A和預瞄點P連線記為AP，航向偏差角為農(nóng)機航向與AP之間的夾角，δ為農(nóng)機的理論前輪轉(zhuǎn)角，L為農(nóng)機的軸距，v為農(nóng)機的行駛速度，設定的曲線路徑上距離農(nóng)機中心最近的點即為M；

步驟3中，使用PI控制方法計算出補償前輪轉(zhuǎn)角，具體的包括以下步驟：

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

（302）計算出當前的積分累計誤差；

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

（4）

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

將預瞄控制器輸出的理論前輪轉(zhuǎn)角和PI控制器輸出的補償前輪轉(zhuǎn)角相加后為實際前輪轉(zhuǎn)角，實際前輪轉(zhuǎn)角實時輸出給農(nóng)機模型，控制農(nóng)機的前輪轉(zhuǎn)角使農(nóng)機沿著規(guī)劃好的實際避障路徑行走，實現(xiàn)農(nóng)機的自動避障。

使用matlab對實際避障曲線曲率進行仿真，如圖5可以看出，實際避障曲線曲率連續(xù)；使用matlab軟件針對本發(fā)明中提出的PI控制和預瞄控制算法對設定曲線進行仿真，給定農(nóng)機的起始位置為（-13,1），初始航向角度為0rad，Kp取為2，Ki取為0.01；圖9～圖12的橫坐標均為農(nóng)機的行駛距離，從圖9中可以看出，跟蹤曲線與設定曲線基本重合；從圖10中可以看出，航向偏差大概在0.08rad左右；從圖11中可以看出，橫向偏差在10cm左右；從圖12中可以看出，前輪轉(zhuǎn)向角為1階慣性環(huán)節(jié)，沒有突變，和實際相符；通過以上分析，使用本發(fā)明中的控制方法進行農(nóng)機的掉頭路徑控制，控制精度高，農(nóng)機基本按照設定的曲線路徑行走。

本發(fā)明工作時，視覺機器相機采集農(nóng)機周圍的環(huán)境信息，根據(jù)周圍環(huán)境信息確認農(nóng)機是否進入避障，若檢測到農(nóng)機前方有小障礙物時，農(nóng)機進入避障導航狀態(tài)，農(nóng)機通過傳感器檢測獲得農(nóng)機的位置信息，計算農(nóng)機前方障礙物的特征圓的大小、農(nóng)機與障礙物的距離，根據(jù)農(nóng)機的犁具寬度和農(nóng)機最小轉(zhuǎn)彎半徑確定特征圓的大小以設定安全距離，使用改進的最短切線法設定理論避障路徑，但是由于理論避障路徑的曲率不連續(xù)，使農(nóng)機避障的控制精度降低，使用Bezier曲線優(yōu)化方法優(yōu)化理論避障路徑生成新的實際避障路徑，通過預瞄控制器得到理論前輪轉(zhuǎn)角，PI控制器補償預瞄控制器產(chǎn)生的控制誤差輸出補償前輪轉(zhuǎn)角，將理論前輪轉(zhuǎn)向角和期望補償轉(zhuǎn)向角相加后得到實際前輪轉(zhuǎn)角并將前輪轉(zhuǎn)角輸出給農(nóng)機模型，位置傳感器實時檢測農(nóng)機所在位置并將位置信息發(fā)送給預瞄控制器和PI控制器，通過控制農(nóng)機的前輪轉(zhuǎn)角使農(nóng)機沿著設定的曲線行走，從而使農(nóng)機自動繞開障礙物；本發(fā)明通過改進后的最短切線法計算出一條理論避障路徑，使用基于Bezier曲線的路徑優(yōu)化方法對理論避障路徑進行優(yōu)化，使避障路徑更加容易控制，通過預瞄控制器和PI控制器的結(jié)合控制農(nóng)機的前輪轉(zhuǎn)向角使農(nóng)機沿著設定好的避障曲線行走，控制精度高；可應用于無人駕駛的農(nóng)機在田間作業(yè)時自動避開小障礙物的工作中。

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