技術(shù)特征：

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

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

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

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

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

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

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

步驟2：農(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ú)人駕駛的路徑規(guī)劃及其控制方法，其特征在于，步驟101中，計(jì)算理論避障路徑具體的為，計(jì)算農(nóng)機(jī)前方障礙物的特征圓的大小、農(nóng)機(jī)與障礙物的距離，根據(jù)特征圓的大小設(shè)定安全距離，根據(jù)農(nóng)機(jī)的犁具寬度和農(nóng)機(jī)最小轉(zhuǎn)彎半徑，設(shè)定一條理論避障路徑；改進(jìn)的最短切線法具體的為，以障礙物的中心為圓心做特征圓，特征圓的半徑為r_min+w/2，理論避障路徑由圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三組成，圓弧段一的一端與農(nóng)機(jī)原始的直線路徑相切，圓弧段一的另一端與直線段一的一端相切，直線段一的另一端和直線段二的一端分別與圓弧段二相切，直線段二的另一端與圓弧段三相切，圓弧段二為特征圓上的一段，圓弧段一和圓弧段三關(guān)于圓弧段二的中心線對(duì)稱設(shè)置，農(nóng)機(jī)依次經(jīng)過(guò)圓弧段一、直線段一、圓弧段二、直線段二和圓弧段三繞過(guò)障礙物；其中，r_min為農(nóng)機(jī)的最小轉(zhuǎn)彎半徑，w為農(nóng)機(jī)的作業(yè)寬度，障礙物的外接圓半徑小于最小轉(zhuǎn)彎半徑。

3.根據(jù)權(quán)利要求2所述的用于農(nóng)機(jī)無(wú)人駕駛的路徑規(guī)劃及其控制方法，其特征在于，所述圓弧段一的半徑為r_min，所述圓弧段三的半徑為r_min，圓弧段一的起點(diǎn)記為H點(diǎn)，圓弧段一的圓心記為O₁點(diǎn)，直線段一與農(nóng)機(jī)原始的直線路徑的相交點(diǎn)記為J，直線段一與圓弧段二的相切點(diǎn)記為D，農(nóng)機(jī)原始路徑與特征圓的相交點(diǎn)分別記為K和K’，JK=w/2，圓弧段二的圓心記為O點(diǎn)，O的坐標(biāo)設(shè)為（a，b），圓弧段二的中心點(diǎn)記為B點(diǎn)，J點(diǎn)的坐標(biāo)記為（x1，y1），JD的方程可以寫(xiě)成：

（1-1）；

特征圓的方程可以寫(xiě)成：

（1-2）

通過(guò)（1-1）和（1-2）可以求出k，D點(diǎn)為JD和特征圓的相交點(diǎn)，以此解出D點(diǎn)坐標(biāo)；

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

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

4.根據(jù)權(quán)利要求3所述的用于農(nóng)機(jī)無(wú)人駕駛的路徑規(guī)劃及其控制方法，其特征在于，步驟102中，利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟101中的理論避障路徑，優(yōu)化后的實(shí)際避障路徑由實(shí)際避障曲線一和實(shí)際避障曲線二組成，具體的為，建立Bezier方程，

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

（2-1）

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

（2-2）

由上述公式，可以得出三次和二次Bezier曲線的數(shù)學(xué)表達(dá)式，當(dāng)n=3時(shí)，Q(t)為三次多項(xiàng)式，有四個(gè)控制點(diǎn)，其矩陣形式表達(dá)為：

（2-3）

當(dāng)n=2，Q(t)為二次多項(xiàng)式，有三個(gè)控制點(diǎn)，矩陣表達(dá)式為：

（2-4）

（2）Bezier曲線的曲率表達(dá)式為：

（2-5）

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

曲率半徑為：

（2-6）

選取三次Bezier曲線，針對(duì)三次Bezier曲線：

（2-7）

（2-8）

其中，X0、X1、X2、X3分別為P0點(diǎn)、P1點(diǎn)、P2點(diǎn)和P3點(diǎn)處的橫向坐標(biāo)，Y0、Y1、Y2和Y3分別為P0點(diǎn)、P1點(diǎn)、P2點(diǎn)和P3處的縱向坐標(biāo)；

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

（2-9）

其中，由點(diǎn)（x₂，y₁）、（a，b+r）、（（x₂+a）/2，y₁）和（（x₂+a）/2，b+r）生成的Bezier曲線為實(shí)際避障曲線一，實(shí)際避障曲線一和實(shí)際避障曲線二關(guān)于圓弧段二的中心對(duì)稱。

5.根據(jù)權(quán)利要求1～4任一項(xiàng)所述的用于農(nóng)機(jī)無(wú)人駕駛的路徑規(guī)劃及其控制方法，其特征在于，步驟101’中，農(nóng)機(jī)的作業(yè)模式包括播種和耕地；步驟2中，播種情況下對(duì)應(yīng)的掉頭路徑為多線型路徑一，耕地情況下對(duì)應(yīng)的掉頭路徑包括交叉型路徑、圓弧路徑和多線型路徑二，且根據(jù)不同的犁具寬度選擇不同的掉頭路徑，犁具寬度包括小寬幅、中寬幅和大寬幅，小寬幅、中寬幅和大寬幅作業(yè)模式下對(duì)應(yīng)的掉頭路徑分別為交叉型路徑、圓弧路徑和多線型路徑二；所述多線型路徑一由直線段三、圓弧段四和圓弧段五組成，圓弧段四和圓弧段五相切且對(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è)置，農(nóng)機(jī)依次經(jīng)過(guò)圓弧段八、圓弧段九和圓弧段十實(shí)現(xiàn)掉頭；所述多線型路徑二由圓弧段十一、直線段六和圓弧段十二四組成，圓弧段十一和圓弧段十二關(guān)于直線段六的中心線對(duì)稱設(shè)置，農(nóng)機(jī)依次經(jīng)過(guò)圓弧段十一、直線段六和圓弧段十二四實(shí)現(xiàn)掉頭；小寬幅對(duì)應(yīng)的犁具寬度的數(shù)值范圍為r_min<w<1.5r_min,中寬幅對(duì)應(yīng)的犁具寬度的數(shù)值范圍為1.5r_min<w≤2 r_min,大寬幅對(duì)應(yīng)的犁具寬度的數(shù)值范圍為w>2r_min。

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

（3-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)系式為：

（3-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)系式為：

（3-3）

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

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

（3-4）；

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

7.根據(jù)權(quán)利要求6所述的用于農(nóng)機(jī)無(wú)人駕駛的路徑規(guī)劃及其控制方法，其特征在于，步驟2中，將農(nóng)機(jī)簡(jiǎn)化成二輪車(chē)模型進(jìn)行運(yùn)動(dòng)學(xué)分析，并利用曲線跟蹤方法建立農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型，如下式所示：

（4-1）

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

先使用鏈?zhǔn)娇刂评碚搶⑥r(nóng)機(jī)的非線性模型轉(zhuǎn)化成近似線性模型，再使用狀態(tài)反饋控制方法計(jì)算出控制率，當(dāng)農(nóng)機(jī)沿著曲線逆時(shí)針運(yùn)動(dòng)，控制率如下所示：

（4-2）

當(dāng)農(nóng)機(jī)沿著曲線順時(shí)針運(yùn)動(dòng)，控制率如公式（7）所示：

（4-3）

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

8.根據(jù)權(quán)利要求7所述的用于農(nóng)機(jī)無(wú)人駕駛的路徑規(guī)劃及其控制方法，其特征在于，步驟2中，使用狀態(tài)反饋控制器計(jì)算出理論前輪轉(zhuǎn)向角δ(y, θ)后，利用自適應(yīng)控制器解算出期望轉(zhuǎn)向補(bǔ)償角δc，將理論前輪轉(zhuǎn)向角δ(y, θ)和期望轉(zhuǎn)向補(bǔ)償角δc相加后得到農(nóng)機(jī)的實(shí)際轉(zhuǎn)向角δe并輸出給農(nóng)機(jī)模型,控制農(nóng)機(jī)的實(shí)際轉(zhuǎn)向角δe使農(nóng)機(jī)沿著設(shè)定的曲線行走。

9.根據(jù)權(quán)利要求8所述的用于農(nóng)機(jī)無(wú)人駕駛的路徑規(guī)劃及其控制方法，其特征在于,所述自適應(yīng)控制器有2個(gè)輸入變量，分別為橫向偏差變量y和橫向偏差微分dy，自適應(yīng)控制器的輸出是農(nóng)機(jī)的期望轉(zhuǎn)向補(bǔ)償角δc，其中變量dy作為農(nóng)機(jī)橫向偏差的趨勢(shì)，dy的計(jì)算公式如下所示：

（5-1）

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

將輸入輸出變量進(jìn)行模糊化

（1）橫向偏差y

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

（2）橫向偏差微分dy

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

（3）補(bǔ)償角δc

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

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

10.根據(jù)權(quán)利要求1～4任一項(xiàng)或6～9任一項(xiàng)所述的用于農(nóng)機(jī)無(wú)人駕駛的路徑規(guī)劃及其控制方法，其特征在于,所述傳感器包括位置傳感器和角度傳感器，所述角度傳感器檢測(cè)農(nóng)機(jī)的轉(zhuǎn)向角，所述位置傳感器獲得農(nóng)機(jī)的位置信息；農(nóng)機(jī)的前后側(cè)分別設(shè)有視覺(jué)機(jī)器相機(jī)，所述視覺(jué)機(jī)器相機(jī)獲取農(nóng)機(jī)所處環(huán)境的地理信息。