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

背景技術(shù)：

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

當進行避障時，現(xiàn)有技術(shù)中，針對比較小的障礙物，采用最短切線法設(shè)置避障路徑，最短切線法形成的避障路徑由兩段直線段和一段圓弧段組成，直線段分別與圓弧段相切，這種避障路徑雖然簡單快捷，對于具有最小轉(zhuǎn)彎半徑的拖拉機很難按照折角進行轉(zhuǎn)彎且很難控制，若控制農(nóng)機按照這種避障路徑行走，農(nóng)機的控制精度很低；當進行自動掉頭時需對準下一作業(yè)行的跟蹤，控制農(nóng)機準確的掉頭技術(shù)是實現(xiàn)精準農(nóng)業(yè)的關(guān)鍵；該技術(shù)可提高農(nóng)機的作業(yè)精度和作業(yè)效率，使駕駛員擺脫長時間勞累的重復(fù)駕駛工作，降低勞動力。

在現(xiàn)有的農(nóng)機路徑跟蹤方法上，主要有基于模型的控制方法和與模型無關(guān)的控制方法。在模型的控制方法上，主要是基于運動學模型和動力學模型的路徑跟蹤方法?；谶\動學模型的控制方法是對模型進行小角度的線性化逼近，在常速假設(shè)條件下進行控制器設(shè)計這樣不但引入了線性化誤差，而且速度變化時控制器的魯棒性較差；基于動力學模型的控制方法雖然模型的精度高，但是動力學模型的參數(shù)很難實時獲取。在與模型無關(guān)的控制方法上面，純追蹤方法的前視距離的在線自適應(yīng)確定問題還沒有很好的解決，技術(shù)不成熟，控制精度低；智能方法雖然有傳統(tǒng)的控制方法無法比擬的仿人智能和非線性映射能力，但是設(shè)計需要一定的經(jīng)驗知識和復(fù)雜的學習訓練過程，總之，現(xiàn)有的路徑軌跡的控制方法不僅無法同時實現(xiàn)高的控制精度和實時獲取農(nóng)機的運動參數(shù)，而且對設(shè)計人員的要求很高，適應(yīng)性差。

另外，現(xiàn)有技術(shù)中，無法根據(jù)農(nóng)機的實際作業(yè)模式進行路徑控制，適用范圍小。

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

針對現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的在于克服上述現(xiàn)有技術(shù)中的不足之處，解決現(xiàn)有技術(shù)中控制精度低且無法實時獲取農(nóng)機的運動學模型參數(shù)的技術(shù)問題，提供一種用于農(nóng)機無人駕駛的路徑規(guī)劃及其控制方法，本發(fā)明可實時獲取農(nóng)機的運動學參數(shù)，控制精度高，規(guī)劃好的路徑易控制，適用范圍廣。

本發(fā)明的目的是這樣實現(xiàn)的：一種用于農(nóng)機無人駕駛的路徑規(guī)劃及其控制方法，具體包括以下步驟，

步驟1：通過傳感器獲取農(nóng)機環(huán)境信息做出掉頭或避障決策；若為避障決策，規(guī)劃避障路徑軌跡，具體包括以下步驟，

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

步驟102：利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟2中的理論避障路徑得到實際避障路徑；

若為掉頭決策，規(guī)劃掉頭路徑軌跡，具體包括以下步驟，

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

步驟102’：根據(jù)農(nóng)機實際的作業(yè)模式選擇對應(yīng)的掉頭路徑；

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

本發(fā)明工作時，通過傳感器獲取農(nóng)機周圍的環(huán)境信息，當農(nóng)機的前方有障礙物時，做出避障決策，使用改進的最短切線法計算出一條理論避障路徑，對理論避障路徑進行優(yōu)化得到一條更加容易控制的實際避障路徑；當農(nóng)機前方為地頭時，做出掉頭決策，輸入農(nóng)機的作業(yè)模式，農(nóng)機感應(yīng)周圍的環(huán)境信息，農(nóng)機根據(jù)實際的作業(yè)模式選擇設(shè)定好的掉頭路徑；使用曲線跟蹤方法實時獲得設(shè)定好路徑的曲線曲率、農(nóng)機航向偏差和橫向偏差，用狀態(tài)反饋控制器和自適應(yīng)控制器的結(jié)合計算出當前的前輪轉(zhuǎn)向角，通過控制農(nóng)機的前輪轉(zhuǎn)向角使農(nóng)機沿著設(shè)定的曲線行走，從而實現(xiàn)農(nóng)機在導(dǎo)航作業(yè)時的自動避障或自動掉頭。

與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果在于：

（1）本發(fā)明使用曲線跟蹤方法實時獲得設(shè)定好的路徑曲線曲率、航向偏差和橫向偏差，使用狀態(tài)反饋控制方法解算出當前的前輪轉(zhuǎn)向角，控制農(nóng)機的前輪轉(zhuǎn)向角使農(nóng)機沿著設(shè)定好的曲線行走，控制精度高；

（2）本發(fā)明通過農(nóng)機的實際作業(yè)模式選擇相對應(yīng)的掉頭路徑，適應(yīng)范圍更加廣泛；

（3）本發(fā)明通過改進后的最短切線法計算出一條理論避障路徑，使用基于Bezier曲線的路徑優(yōu)化方法對理論避障路徑進行優(yōu)化，使避障路徑更加容易控制；

本發(fā)明可應(yīng)用于農(nóng)機無人駕駛在田間導(dǎo)航作業(yè)時的自動掉頭和自動避障的控制工作中。

為了進一步提高獲得理論避障路徑的可靠性，步驟101中，計算理論避障路徑具體的為，計算農(nóng)機前方障礙物的特征圓的大小、農(nóng)機與障礙物的距離，根據(jù)特征圓的大小設(shè)定安全距離，根據(jù)農(nóng)機的犁具寬度和農(nóng)機最小轉(zhuǎn)彎半徑，設(shè)定一條理論避障路徑；改進的最短切線法具體的為，以障礙物的中心為圓心做特征圓，特征圓的半徑為r_min+w/2，理論避障路徑由圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三組成，圓弧段一的一端與農(nóng)機原始的直線路徑相切，圓弧段一的另一端與直線段一的一端相切，直線段一的另一端和直線段二的一端分別與圓弧段二相切，直線段二的另一端與圓弧段三相切，圓弧段二為特征圓上的一段，圓弧段一和圓弧段三關(guān)于圓弧段二的中心線對稱設(shè)置，農(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的坐標設(shè)為（a，b），圓弧段二的中心點記為B點，J點的坐標記為（x1，y1），JD的方程可以寫成：

（1-1）；

特征圓的方程可以寫成：

（1-2）

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

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

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

步驟102中，利用基于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’為曲線的一階導(dǎo)數(shù)，y”為二階導(dǎo)數(shù)；

曲率半徑為：

（2-6）

選取三次Bezier曲線，針對三次Bezier曲線：

（2-7）

（2-8）

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

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

（2-9）；

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

作為本發(fā)明的進一步改進，步驟101’中，農(nóng)機的作業(yè)模式包括播種和耕地；步驟2中，播種情況下對應(yīng)的掉頭路徑為多線型路徑一，耕地情況下對應(yīng)的掉頭路徑包括交叉型路徑、圓弧路徑和多線型路徑二，且根據(jù)不同的犁具寬度選擇不同的掉頭路徑，犁具寬度包括小寬幅、中寬幅和大寬幅，小寬幅、中寬幅和大寬幅作業(yè)模式下對應(yīng)的掉頭路徑分別為交叉型路徑、圓弧路徑和多線型路徑二；所述多線型路徑一由直線段三、圓弧段四和圓弧段五組成，圓弧段四和圓弧段五相切且對應(yīng)的半徑相同，農(nóng)機依次經(jīng)過直線段三、圓弧段四和圓弧段五實現(xiàn)掉頭；所述交叉型路徑由圓弧段六、直線段四和圓弧段七組成，圓弧段六和圓弧段七相交且關(guān)于直線段四的中心對稱設(shè)置，圓弧段六和圓弧段七的半徑大小相同，農(nóng)機依次經(jīng)過圓弧段六、直線段五和圓弧段七實現(xiàn)掉頭；所述圓弧路徑由圓弧段八、圓弧段九和圓弧段十組成，所述圓弧段九分別和圓弧段八、圓弧段十相切，所述圓弧段八和圓弧段十關(guān)于圓弧段九的中心線對稱設(shè)置，農(nóng)機依次經(jīng)過圓弧段八、圓弧段九和圓弧段十實現(xiàn)掉頭；所述多線型路徑二由圓弧段十一、直線段六和圓弧段十二四組成，圓弧段十一和圓弧段十二關(guān)于直線段六的中心對稱設(shè)置，農(nóng)機依次經(jīng)過圓弧段十一、直線段六和圓弧段十二四實現(xiàn)掉頭；小寬幅對應(yīng)的犁具寬度的數(shù)值范圍為r_min<w<1.5r_min,中寬幅對應(yīng)的犁具寬度的數(shù)值范圍為1.5r_min<w≤2 r_min,大寬幅對應(yīng)的犁具寬度的數(shù)值范圍為w>2r_min，w為犁具寬度；此設(shè)計中，根據(jù)不同的作業(yè)模式設(shè)置不同的掉頭路徑，適用范圍更廣。

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

（3-1）

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

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

（3-2）

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

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

（3-3）

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

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

（3-4）；

其中，R5為圓弧段十一對應(yīng)的半徑，R6為圓弧段十二對應(yīng)的半徑，B₂C₂表示直線段六，A₂D₂為圓弧段十一的圓心和圓弧段十二的圓心的連線；

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

為了進一步提高獲得農(nóng)機運動學參數(shù)的準確性，將農(nóng)機簡化成二輪車模型進行運動學分析，并利用曲線跟蹤方法建立農(nóng)機運動學模型，如下式所示：

（4-1）

其中，s表示M點沿著弧長運動的距離，M點是曲線路徑上距離農(nóng)機后軸中心最近的點；y表示農(nóng)機和M點的橫向偏差，θ是農(nóng)機航向偏差角，為轉(zhuǎn)向角加速度；規(guī)定當點沿著曲線順時針運動，曲率c為負，沿著曲線逆時針運動，曲率c為正；當農(nóng)機的后軸中心點在曲線的外側(cè)，橫向偏差y為正，當農(nóng)機的后軸中心點在曲線的內(nèi)側(cè)，橫向偏差y為負；

先使用鏈式控制理論將農(nóng)機的非線性模型轉(zhuǎn)化成近似線性模型，再使用狀態(tài)反饋控制方法計算出控制率，當農(nóng)機沿著曲線逆時針運動，控制率如下所示：

（4-2）

當農(nóng)機沿著曲線順時針運動，控制率如公式（7）所示：

（4-3）

其中，，為關(guān)于橫向偏差和農(nóng)機航向偏差角變量的理論前輪轉(zhuǎn)向角函數(shù)；

此設(shè)計中，先利用曲線跟蹤方法建立農(nóng)機運動模型，再使用鏈式控制理論將非線性的農(nóng)機模型轉(zhuǎn)化為近似線性模型，可實時獲得運動學參數(shù)，通過控制前輪轉(zhuǎn)向角達到控制農(nóng)機實際行走路徑的目的，誤差小，控制精度高。

為了進一步提高曲線跟蹤的控制精度，步驟2中，使用狀態(tài)反饋控制器計算出理論前輪轉(zhuǎn)向角δ(y, θ)后，利用自適應(yīng)控制器解算出期望轉(zhuǎn)向補償角δc，將理論前輪轉(zhuǎn)向角δ(y, θ)和期望轉(zhuǎn)向補償角δc相加后得到農(nóng)機的實際轉(zhuǎn)向角δe并輸出給農(nóng)機模型,控制農(nóng)機的實際轉(zhuǎn)向角δe使農(nóng)機沿著設(shè)定的曲線行走；此設(shè)計中，考慮到農(nóng)機的模型精確度將對農(nóng)機的控制品質(zhì)有較大的影響，在公式（4-1）中，我們認為c(x)vsinθdt近似為0，并且ds近似為dx，當跟蹤曲線的曲率較小時，假設(shè)條件基本可以滿足，隨著曲率的增大，農(nóng)機模型的精確度將會下降，控制性能會降低，加入期望轉(zhuǎn)向補償角δc后輔助控制，進一步提高農(nóng)機的控制精度。

作為本發(fā)明的進一步改進，所述自適應(yīng)控制器有2個輸入變量，分別為橫向偏差變量y和橫向偏差微分dy，自適應(yīng)控制器的輸出是農(nóng)機的期望轉(zhuǎn)向補償角δc，其中變量dy作為農(nóng)機橫向偏差的趨勢，dy的計算公式如下所示：

（5-1）

其中，y_t為t時刻農(nóng)機的橫向偏差，是時刻的橫向偏差，這里選取參數(shù)△t為1s；

將輸入輸出變量進行模糊化

（1）橫向偏差y

基本論域：[-60,60]，量化等級：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子為Ky=6/60=0.1；

（2）橫向偏差微分dy

基本論域：[-6,6]，量化等級：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子為Ky=6/6=1；

（3）補償角δc

基本論域:[-8,8]，量化等級：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子Ky=6/8=0.75；

橫向偏差y、橫向偏差微分dy和補償角δc 3個變量的模糊等級均為：負大(NB)、負中(NM)、負小(NS)、零(ZO)、正小(PS)、正中(PM)、正大(PB)，三個隸屬度函數(shù)均采用高斯函數(shù)。

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

附圖說明

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

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

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

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

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

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

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

圖8為本發(fā)明中三次Bizier曲線一特性圖一。

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

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

圖11為本發(fā)明中農(nóng)機與曲線路徑間的運動關(guān)系圖。

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

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

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

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

具體實施方式

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

如圖1～15所示的一種用于農(nóng)機無人駕駛的路徑規(guī)劃及其控制方法，步驟1：通過傳感器獲取農(nóng)機環(huán)境信息做出掉頭或避障決策；若為避障決策，規(guī)劃避障路徑軌跡，具體包括以下步驟，

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

步驟102：利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟2中的理論避障路徑得到實際避障路徑；

若為掉頭決策，規(guī)劃掉頭路徑軌跡，具體包括以下步驟，

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

步驟102’：根據(jù)農(nóng)機實際的作業(yè)模式選擇對應(yīng)的掉頭路徑；

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

其中，傳感器包括位置傳感器和角度傳感器，角度傳感器檢測農(nóng)機的轉(zhuǎn)向角，位置傳感器獲得農(nóng)機的位置信息；農(nóng)機的前后側(cè)分別設(shè)有視覺機器相機，視覺機器相機獲取農(nóng)機所處環(huán)境的地理信息；

步驟101中，計算理論避障路徑具體的為，計算農(nóng)機前方障礙物的特征圓的大小、農(nóng)機與障礙物的距離，根據(jù)特征圓的大小設(shè)定安全距離，根據(jù)農(nóng)機的犁具寬度和農(nóng)機最小轉(zhuǎn)彎半徑，設(shè)定一條理論避障路徑；改進的最短切線法具體的為，以障礙物的中心為圓心做特征圓，特征圓的半徑為r_min+w/2，如圖5所示，理論避障路徑由圓弧段一ρ1、直線段一l1、圓弧段二ρ2、直線段二l2和圓弧段三ρ3組成，圓弧段一ρ1的一端與農(nóng)機原始的直線路徑相切，圓弧段一ρ1的另一端與直線段一l1的一端相切，直線段一l1的另一端和直線段二l2的一端分別與圓弧段二ρ2相切，直線段二l2的另一端與圓弧段三ρ3相切，圓弧段二ρ2為特征圓上的一段，圓弧段一ρ1和圓弧段三ρ3關(guān)于圓弧段二ρ2的中心線對稱設(shè)置，農(nóng)機依次經(jīng)過圓弧段一ρ1、直線段一l1、圓弧段二ρ2、直線段二l2和圓弧段三ρ3繞過障礙物；其中，r_min為農(nóng)機的最小轉(zhuǎn)彎半徑，w為農(nóng)機的作業(yè)寬度，障礙物的外接圓半徑小于最小轉(zhuǎn)彎半徑；

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

（1-1）；

特征圓的方程可以寫成：

（1-2）

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

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

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

步驟102中，利用基于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’為曲線的一階導(dǎo)數(shù)，y”為二階導(dǎo)數(shù)；

曲率半徑為：

（2-6）

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

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

當t=0時，

（2-10）

當t=1時，

（2-11）

對公式(2-1)求出Bezier曲線的導(dǎo)函數(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)了對車輛初始位姿和目標位姿的確定；從圖8和圖9中可以看出，三次Bezier曲線都落在了特征多邊形P0P1P2P3內(nèi)，增加了Bezier曲線的可控性；

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

（2-5）

其中，y=f（x）表示曲線的方程，y’為曲線的一階導(dǎo)數(shù)，y”為二階導(dǎo)數(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點對應(yīng)圓弧段一ρ1的起點H（x₂，y₁），P3點對應(yīng)圓弧段二ρ2的中心點B（a，b+r），P1點對應(yīng)的坐標為（（x₂+a）/2，y₁），P2點對應(yīng)的坐標為（（x₂+a）/2，b+r），則實際故障路徑對應(yīng)的曲線的曲率半徑計算公式為：

（2-9）；

使用Bezier曲線優(yōu)化理論避障路徑后形成實際避障路徑軌跡，如圖6所示，實際避障路徑由兩條曲率連續(xù)的避障曲線ρ0組成。

步驟101’中，農(nóng)機的作業(yè)模式包括播種和耕地；步驟2中，播種情況下對應(yīng)的掉頭路徑為多線型路徑一，耕地情況下對應(yīng)的掉頭路徑包括交叉型路徑、圓弧路徑和多線型路徑二，且根據(jù)不同的犁具寬度選擇不同的掉頭路徑，犁具寬度包括小寬幅、中寬幅和大寬幅，小寬幅、中寬幅和大寬幅作業(yè)模式下對應(yīng)的掉頭路徑分別為交叉型路徑、圓弧路徑和多線型路徑二；如圖1所示，多線型路徑一由直線段三l3、圓弧段四ρ4和圓弧段五ρ5組成，圓弧段四ρ4和圓弧段五ρ5相切且對應(yīng)的半徑相同，農(nóng)機依次經(jīng)過直線段三l3、圓弧段四ρ4和圓弧段五ρ5實現(xiàn)掉頭；如圖2所示，交叉型路徑由圓弧段六ρ6、直線段四l4和圓弧段七ρ7組成，圓弧段六ρ6和圓弧段七ρ7相交且對應(yīng)的半徑大小相同，農(nóng)機依次經(jīng)過圓弧段六ρ6、直線段四12和圓弧段七ρ7實現(xiàn)掉頭；如圖3所示，圓弧路徑由圓弧段八ρ8、圓弧段九ρ9和圓弧段十ρ10組成，圓弧段九ρ9分別和圓弧段八ρ8、圓弧段十ρ10相切，圓弧段八ρ8和圓弧段十ρ10關(guān)于圓弧段九ρ9的中心線對稱設(shè)置，農(nóng)機依次經(jīng)過圓弧段八ρ8、圓弧段九ρ9和圓弧段十ρ10實現(xiàn)掉頭；如圖4所示，多線型路徑二由圓弧段十一ρ11、直線段六l6和圓弧段十二ρ12組成，圓弧段十一ρ11和圓弧段十二ρ12關(guān)于直線段六l6的中心線對稱設(shè)置；小寬幅對應(yīng)的犁具寬度的數(shù)值范圍為r_min<w<1.5r_min,中寬幅對應(yīng)的犁具寬度的數(shù)值范圍為1.5r_min<w≤2 r_min,大寬幅對應(yīng)的犁具寬度的數(shù)值范圍為w>2r_min；

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

（3-1）

其中，R1為圓弧段四ρ4對應(yīng)的半徑，R2為圓弧段五ρ5對應(yīng)的半徑，AE為直線段三l3的長度，AC為農(nóng)機開始轉(zhuǎn)彎的后軸中心與農(nóng)機轉(zhuǎn)彎結(jié)束的后軸中心間連接線段的長度；

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

（3-2）

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

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

（3-3）

其中，Rt為圓弧段八ρ8、圓弧段九ρ9和圓弧段十ρ10對應(yīng)的半徑，圓弧段八ρ8、圓弧段九ρ9和圓弧段十ρ10的圓心的連線構(gòu)成三角形，β為圓弧段九ρ9的圓心分別與圓弧段八ρ8和圓弧段十ρ10的圓心連線之間的夾角，α為圓弧段八ρ8的圓心分別與圓弧段九ρ9和圓弧段十ρ10的圓心連線之間的夾角，γ為圓弧段十ρ10的圓心分別與圓弧段八ρ8和圓弧段九ρ9的圓心連線之間的夾角；

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

（3-4）；

其中，R5為圓弧段十一ρ11對應(yīng)的半徑，R6為圓弧段十二ρ12對應(yīng)的半徑，B₂C₂表示直線段六l6，A₂D₂為圓弧段十一ρ11的圓心和圓弧段十二ρ12的圓心的連線；

為了進一步提高獲得農(nóng)機運動學參數(shù)的準確性，將農(nóng)機簡化成二輪車模型進行運動學分析，并利用曲線跟蹤方法（如圖11所示）建立農(nóng)機運動學模型，如下所示：

（4-1）

其中，s表示M點沿著弧長運動的距離，M點是曲線路徑上距離農(nóng)機后軸中心最近的點；y表示農(nóng)機和M點的橫向偏差，θ是農(nóng)機航向偏差角，為轉(zhuǎn)向角加速度；規(guī)定當點沿著曲線順時針運動，曲率c為負，沿著曲線逆時針運動，曲率c為正；當農(nóng)機的后軸中心點在曲線的外側(cè)，橫向偏差y為正，當農(nóng)機的后軸中心點在曲線的內(nèi)側(cè)，橫向偏差y為負；

由(4-1)可知農(nóng)機的模型是高度的非線性系統(tǒng)，為了應(yīng)用線性系統(tǒng)控制方法，需要將(4-2)進行近似線性化，我們可以使用鏈式控制理論將農(nóng)機模型轉(zhuǎn)化成近似線性模型，這個方法可以使我們使用線性控制方法應(yīng)用在非線性系統(tǒng)；

將農(nóng)機的非線性模型轉(zhuǎn)化成通用的鏈式系統(tǒng)，通用的三階鏈式系統(tǒng)的方程如下所示：

（4-4）

其中，是系統(tǒng)的狀態(tài)變量，是系統(tǒng)的控制變量，為了使方程(4-1)線性化，需要將狀態(tài)變量a1求導(dǎo)，記

(4-5)

則方程(4-5)可以寫成如下形式：

(4-6)

系統(tǒng)(4-4)中的后兩個方程(4-5)和(4-6)很明顯是線性系統(tǒng)，則我們也能推導(dǎo)出n維的鏈式系統(tǒng)中有n-1個線性子系統(tǒng)；

接著將農(nóng)機模型(4-1)轉(zhuǎn)化成鏈式系統(tǒng)的形式，設(shè)定a1=s，同時狀態(tài)變量a2和a3設(shè)定為y和θ相關(guān)的變量，我們可以選擇最簡單的形式，設(shè)定a2=y，

新的控制變量m1將寫成如下的形式：

(4-7)

同時，設(shè)定

(4-8)

將農(nóng)機模型(4-1)帶入公式(4-7)、(4-8)，可以將農(nóng)機模型(4-1)轉(zhuǎn)化成(4-5)的形式，推導(dǎo)過程如下所示：

(4-9)

其中，必須滿足以下條件：

(4-10)

因為模型(4-8)是線性系統(tǒng)，我們可以使用狀態(tài)反饋控制方法對系統(tǒng)進行控制，狀態(tài)反饋控制器的通用表達式如下所示：

(4-11)

將方程(4-11)帶入方程(4-8)中，我們可以得到如下的控制律：

(4-12)

該控制律是為了控制a2和a3趨近于0，同樣地，該結(jié)論可以用來控制y和θ趨近于0來達到曲線跟蹤控制的目的；

將方程(4-12)帶入方程(4-8)中，可以得到實際的控制表達式：

使用鏈式控制理論將農(nóng)機的非線性模型轉(zhuǎn)化成近似線性模型，當農(nóng)機沿著曲線逆時針運動，控制率如下所示：

（4-2）

當農(nóng)機沿著曲線順時針運動，控制率如下所示：

（4-3）

其中，，為關(guān)于橫向偏差和農(nóng)機航向偏差角變量的理論前輪轉(zhuǎn)向角函數(shù)；

考慮到農(nóng)機的模型精確度將對農(nóng)機的控制品質(zhì)有較大的影響，在公式（4-1）中，我們認為c(x)vsinθdt近似為0，并且ds近似為dx，當跟蹤曲線的曲率較小時，假設(shè)條件基本可以滿足，隨著曲率的增大，農(nóng)機模型的精確度將會下降，控制性能會降低，加入期望轉(zhuǎn)向補償角δc后輔助控制，具體的為，步驟2中，使用鏈式控制理論計算出理論前輪轉(zhuǎn)向角δ(y, θ)后，利用自適應(yīng)控制器解算出期望轉(zhuǎn)向補償角δc，將理論前輪轉(zhuǎn)向角δ(y, θ)和期望轉(zhuǎn)向補償角δc相加后得到農(nóng)機的實際轉(zhuǎn)向角δe并輸出給農(nóng)機模型,控制農(nóng)機的實際轉(zhuǎn)向角δe使農(nóng)機沿著設(shè)定的曲線行走；

自適應(yīng)控制器有2個輸入變量，分別為橫向偏差變量y和橫向偏差微分dy，自適應(yīng)控制器的輸出是農(nóng)機的期望轉(zhuǎn)向補償角δc，其中變量dy作為農(nóng)機橫向偏差的趨勢，dy的計算公式如下所示：

（5-1）

其中，y_t為t時刻農(nóng)機的橫向偏差，是時刻的橫向偏差，這里選取參數(shù)△t為1s；

將輸入輸出變量進行模糊化

（1）橫向偏差y

基本論域：[-60,60]，量化等級：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子為Ky=6/60=0.1；

（2）橫向偏差微分dy

基本論域：[-6,6]，量化等級：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子為Ky=6/6=1；

（3）補償角δc

基本論域:[-8,8]，量化等級：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子Ky=6/8=0.75；

橫向偏差y、橫向偏差微分dy和補償角δc 3個變量的模糊等級均為：負大(NB)、負中(NM)、負小(NS)、零(ZO)、正小(PS)、正中(PM)、正大(PB)，三個隸屬度函數(shù)均采用高斯函數(shù)；模糊控制規(guī)則如表1所示：

表1 模糊控制規(guī)則。

例如，當車體在曲線路徑的外側(cè)，橫向偏差為正的最大且橫向偏差有增大的趨勢時，自適應(yīng)控制器輸出負的最大期望轉(zhuǎn)向補償角δc，使農(nóng)機的橫向偏差減小，用模糊規(guī)則描述為：if y = PB and dy = PB, then δc = NB；其它的模糊規(guī)則見表1，總共有49條控制規(guī)則，車體在其它情況下與以上例子類似分析即可，這里對車體的任意情況不再一一說明。

使用matlab軟件對本發(fā)明進行仿真，設(shè)定一條路徑曲線，給定農(nóng)機的起始位置為[0,0],初始角度為0rad，選取Kd=0.6，Kp=0.09，利用本發(fā)明的控制方法對設(shè)定好的曲線進行跟蹤，圖12～圖15的橫坐標均為農(nóng)機的行駛距離，從圖12中可以看出，跟蹤曲線與設(shè)定曲線基本重合；從圖13中可以看出，橫向偏差維持在10cm左右；從圖14中可以看出，航向偏差大概在0.02rad左右，其中波動比較大的點主要是此處的曲線斜率較大，采樣點比較稀疏，導(dǎo)致M點不是很精確，但是在實際運行過程中，采樣點比較密集，會解決M點不精確的問題；從圖15中可以看出，前輪轉(zhuǎn)向角為1階慣性環(huán)節(jié)，沒有突變，和實際相符；通過以上分析，使用本發(fā)明中的控制方法進行農(nóng)機的掉頭路徑控制，控制精度高，農(nóng)機基本按照設(shè)定的曲線路徑行走。

本發(fā)明工作時，輸入農(nóng)機的作業(yè)模式，視覺機器相機采集農(nóng)機周圍的環(huán)境信息，根據(jù)周圍環(huán)境信息確認農(nóng)機是否掉頭，當檢測到農(nóng)機前方為地頭時，做出掉頭決策時，農(nóng)機根據(jù)實際的作業(yè)模式選擇設(shè)定好的掉頭路徑，當作業(yè)模式為播種時農(nóng)機的掉頭路徑選擇多線型路徑一，農(nóng)機通過傳感器檢測獲得農(nóng)機的位置信息，使用曲線跟蹤方法實時獲得設(shè)定好路徑的曲線曲率、農(nóng)機航向偏差和橫向偏差，使用鏈式控制理論將原本的農(nóng)機模型轉(zhuǎn)化為線性模型，用狀態(tài)反饋控制方法對鏈式系統(tǒng)進行控制，通過狀態(tài)反饋控制器得到理論前輪轉(zhuǎn)向角，通過自適應(yīng)控制器計算出期望補償轉(zhuǎn)向角，將理論前輪轉(zhuǎn)向角和期望補償轉(zhuǎn)向角相加后得到實際前輪轉(zhuǎn)向角并將實際前輪轉(zhuǎn)向角輸出給農(nóng)機模型，位置傳感器實時檢測農(nóng)機所在位置并將位置信息發(fā)送給狀態(tài)反饋控制器和自適應(yīng)控制器，通過控制農(nóng)機的前輪轉(zhuǎn)向角使農(nóng)機沿著設(shè)定的曲線行走，從而實現(xiàn)農(nóng)機的掉頭；當作業(yè)模式為耕地時，根據(jù)農(nóng)機上懸掛的犁具寬度選擇掉頭路徑，掉頭路徑軌跡選定后，按照上述相同的控制過程控制農(nóng)機的掉頭；當農(nóng)機前方有小障礙物時，做出避障決策，農(nóng)機進入避障導(dǎo)航狀態(tài)，農(nóng)機通過位置傳感器檢測獲得農(nóng)機的位置信息，計算農(nóng)機前方障礙物的特征圓的大小、農(nóng)機與障礙物的距離，根據(jù)農(nóng)機的犁具寬度和農(nóng)機最小轉(zhuǎn)彎半徑確定特征圓的大小以設(shè)定安全距離，使用改進的最短切線法設(shè)定理論避障路徑，但是由于理論避障路徑的曲率不連續(xù)，使農(nóng)機避障的控制精度降低，使用Bezier曲線優(yōu)化方法優(yōu)化理論避障路徑生成新的實際避障路徑，生成兩條對稱的曲率連續(xù)的避障曲線（如圖6所示），使用如上所述相同的控制方法控制農(nóng)機的轉(zhuǎn)向，使農(nóng)機沿著設(shè)定好的避障曲線行走，實現(xiàn)農(nóng)機的自動避障；本發(fā)明使用曲線跟蹤方法實時獲得設(shè)定好的路徑曲線曲率、航向偏差和橫向偏差，使用狀態(tài)反饋控制方法解算出當前的前輪轉(zhuǎn)向角，控制農(nóng)機的前輪轉(zhuǎn)向角使農(nóng)機沿著設(shè)定好的曲線行走，控制精度高；同時，本發(fā)明通過農(nóng)機的實際作業(yè)模式選擇相對應(yīng)的掉頭路徑，適應(yīng)范圍更加廣泛；另外，本發(fā)明通過改進后的最短切線法計算出一條理論避障路徑，使用基于Bezier曲線的路徑優(yōu)化方法對理論避障路徑進行優(yōu)化，優(yōu)化后的實際避障路徑的曲率連續(xù)（如圖7所示），使避障路徑更加容易控制；可應(yīng)用于農(nóng)機無人駕駛在田間導(dǎo)航作業(yè)時的自動掉頭和自動避障的控制工作中。

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