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

背景技術(shù)：

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

當(dāng)進(jìn)行避障時(shí)，現(xiàn)有技術(shù)中，針對(duì)比較小的障礙物，采用最短切線法設(shè)置避障路徑，最短切線法形成的避障路徑由兩段直線段和一段圓弧段組成，直線段分別與圓弧段相切，這種避障路徑雖然簡(jiǎn)單快捷，對(duì)于具有最小轉(zhuǎn)彎半徑的拖拉機(jī)很難按照折角進(jìn)行轉(zhuǎn)彎且很難控制，若控制農(nóng)機(jī)按照這種避障路徑行走，農(nóng)機(jī)的控制精度很低；當(dāng)進(jìn)行自動(dòng)掉頭時(shí)需對(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)速度較慢，反而降低其控制精度；總而言之，無(wú)論是路徑規(guī)劃還是路徑的控制方法，控制農(nóng)機(jī)按照設(shè)定好的路徑行走的精度很低，偏離設(shè)定的路徑曲線，導(dǎo)致農(nóng)機(jī)繞過(guò)障礙物或掉頭時(shí)行走的路程遠(yuǎn)，從開(kāi)始進(jìn)入設(shè)定曲線的起點(diǎn)至回到農(nóng)機(jī)原始直線行走路徑的時(shí)間長(zhǎng)。

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

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

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

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

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

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

步驟102：利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟2中的理論避障路徑得到實(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ī)位置信息，使用育瞄控制器和PI控制器的結(jié)合計(jì)算當(dāng)前的前輪轉(zhuǎn)向角，控制農(nóng)機(jī)的轉(zhuǎn)向角使農(nóng)機(jī)沿著設(shè)定的曲線行走實(shí)現(xiàn)農(nóng)機(jī)的自動(dòng)掉頭或避障。

本發(fā)明工作時(shí)，通過(guò)傳感器獲取農(nóng)機(jī)周?chē)沫h(huán)境信息，當(dāng)農(nóng)機(jī)的前方有障礙物時(shí)，做出避障決策，使用改進(jìn)的最短切線法計(jì)算出一條理論避障路徑，對(duì)理論避障路徑進(jìn)行優(yōu)化得到一條更加容易控制的實(shí)際避障路徑；當(dāng)農(nóng)機(jī)前方為地頭時(shí)，做出掉頭決策，輸入農(nóng)機(jī)的作業(yè)模式，農(nóng)機(jī)感應(yīng)周?chē)沫h(huán)境信息，農(nóng)機(jī)根據(jù)實(shí)際的作業(yè)模式選擇設(shè)定好的掉頭路徑；通過(guò)預(yù)瞄控制器和PI控制器的結(jié)合控制農(nóng)機(jī)的前輪轉(zhuǎn)向角使農(nóng)機(jī)沿著設(shè)定好的避障曲線行走，控制容易且控制精度高，從而實(shí)現(xiàn)農(nóng)機(jī)在導(dǎo)航作業(yè)時(shí)的自動(dòng)避障或自動(dòng)掉頭；本發(fā)明通過(guò)預(yù)瞄控制和PI控制器的結(jié)合計(jì)算出農(nóng)機(jī)前輪的轉(zhuǎn)向角，控制容易，控制農(nóng)機(jī)的前輪轉(zhuǎn)向角使農(nóng)機(jī)沿著設(shè)定好的曲線行走，控制精度高；同時(shí)，通過(guò)農(nóng)機(jī)的實(shí)際作業(yè)模式選擇相對(duì)應(yīng)的掉頭路徑，適應(yīng)范圍更加廣泛；另外，本發(fā)明通過(guò)改進(jìn)后的最短切線法計(jì)算出一條理論避障路徑，使用基于Bezier曲線的路徑優(yōu)化方法對(duì)理論避障路徑進(jìn)行優(yōu)化，使避障路徑更加容易控制；可應(yīng)用于農(nóng)機(jī)無(wú)人駕駛在田間導(dǎo)航作業(yè)時(shí)的自動(dòng)掉頭和自動(dòng)避障的控制工作中。

為了進(jìn)一步提高獲得理論避障路徑的可靠性，步驟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)彎半徑。

為了進(jìn)一步提高農(nóng)機(jī)掉頭路徑的精度，所述圓弧段一的半徑為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）。

步驟102中，利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟2中的理論避障路徑，具體的為，建立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ì)稱；

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

作為本發(fā)明的進(jìn)一步改進(jìn)，步驟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è)置；小寬幅對(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，W為犁具寬度；此設(shè)計(jì)中，根據(jù)不同的作業(yè)模式設(shè)置不同的掉頭路徑，適用范圍更廣。

為了進(jìn)一步提高農(nóng)機(jī)掉頭路徑的精度，所述多線型路徑一的各個(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₂為圓弧段十一的圓心和圓弧段十二的圓心的連線；

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

為了進(jìn)一步提高獲得農(nóng)機(jī)運(yùn)動(dòng)學(xué)參數(shù)的準(zhǔn)確性，步驟2中，利用預(yù)瞄控制器計(jì)算出理論前輪轉(zhuǎn)角，具體的為，確定農(nóng)機(jī)的前視距離l，取路徑上的一點(diǎn)為預(yù)瞄點(diǎn)（x₀，y₀），R為前視距離對(duì)應(yīng)的圓弧段的半徑， l、R和x之間的關(guān)系式為：

（4-1）

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

（4-2）

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

（4-3）

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

（4-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ī)的行駛速度，設(shè)定的曲線路徑上距離農(nóng)機(jī)中心最近的點(diǎn)即為M。

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

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

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

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

（5）

其中，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)角；

將預(yù)瞄控制器輸出的理論前輪轉(zhuǎn)角和PI控制器輸出的補(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)避障；

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

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

附圖說(shuō)明

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

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

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

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

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

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

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

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

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

圖10為本發(fā)明中預(yù)瞄算法純追蹤模型幾何解析圖。

圖11為本發(fā)明中預(yù)瞄算法的曲線跟蹤示意圖。

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

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

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

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

具體實(shí)施方式

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

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

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

步驟102：利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟2中的理論避障路徑得到實(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ī)位置信息，使用育瞄控制器和PI控制器的結(jié)合計(jì)算當(dāng)前的前輪轉(zhuǎn)向角，控制農(nóng)機(jī)的轉(zhuǎn)向角使農(nóng)機(jī)沿著設(shè)定的曲線行走實(shí)現(xiàn)農(nóng)機(jī)的自動(dòng)掉頭或避障；

其中，傳感器包括位置傳感器和角度傳感器，角度傳感器檢測(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)境的地理信息；

步驟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，理論避障路徑由圓弧段一ρ1、直線段一l1、圓弧段二ρ2、直線段二l2和圓弧段三ρ3組成，圓弧段一ρ1的一端與農(nóng)機(jī)原始的直線路徑相切，圓弧段一ρ1的另一端與直線段一l1的一端相切，直線段一l1的另一端和直線段二l2的一端分別與圓弧段二ρ2相切，直線段二l2的另一端與圓弧段三ρ3相切，圓弧段二ρ2為特征圓上的一段，圓弧段一ρ1和圓弧段三ρ3關(guān)于圓弧段二ρ2的中心線對(duì)稱設(shè)置，農(nóng)機(jī)依次經(jīng)過(guò)圓弧段一ρ1、直線段一l1、圓弧段二ρ2、直線段二l2和圓弧段三ρ3繞過(guò)障礙物；其中，r_min為農(nóng)機(jī)的最小轉(zhuǎn)彎半徑，w為農(nóng)機(jī)的作業(yè)寬度，障礙物的外接圓半徑小于最小轉(zhuǎn)彎半徑。

為了進(jìn)一步提高農(nóng)機(jī)掉頭路徑的精度，圓弧段一ρ1的半徑為r_min，圓弧段三ρ3的半徑為r_min，圓弧段一ρ1的起點(diǎn)記為H點(diǎn)，圓弧段一ρ1的圓心記為O₁點(diǎn)，直線段一l1與農(nóng)機(jī)原始的直線路徑的相交點(diǎn)記為J，直線段一l1與圓弧段二ρ2的相切點(diǎn)記為D，農(nóng)機(jī)原始路徑與特征圓的相交點(diǎn)分別記為K和K’，JK=w/2，圓弧段二ρ2的圓心記為O點(diǎn)，O的坐標(biāo)設(shè)為（a，b），圓弧段二ρ2的中心點(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）；

步驟102中，利用基于Bezier曲線的路徑優(yōu)化方法優(yōu)化步驟2中的理論避障路徑，具體的為，建立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)圓弧段一ρ1的起點(diǎn)H（x₂，y₁），P3點(diǎn)對(duì)應(yīng)圓弧段二ρ2的中心點(diǎn)B（a，b+r），P1點(diǎn)對(duì)應(yīng)的坐標(biāo)為（（x₂+a）/2，y₁），P2點(diǎn)對(duì)應(yīng)的坐標(biāo)為（（x₂+a）/2，b+r），則實(shí)際故障路徑對(duì)應(yīng)的曲線的曲率半徑計(jì)算公式為：

（2-9）；

用三次Bezier曲線優(yōu)化理論避障路徑形成的實(shí)際避障路徑由兩條關(guān)于圓弧段二的中心對(duì)稱設(shè)置的實(shí)際避障曲線ρ0組成；

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

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

（3-1）

其中，R1為圓弧段四ρ4對(duì)應(yīng)的半徑，R2為圓弧段五ρ5對(duì)應(yīng)的半徑，AE為直線段三l3的長(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為圓弧段六ρ6對(duì)應(yīng)的半徑，R4為圓弧段七ρ7對(duì)應(yīng)的半徑，C₁D₁為直線段四l4的長(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為圓弧段八ρ8、圓弧段九ρ9和圓弧段十ρ10對(duì)應(yīng)的半徑，圓弧段八ρ8、圓弧段九ρ9和圓弧段十ρ10的圓心的連線構(gòu)成三角形，β為圓弧段九ρ9的圓心分別與圓弧段八ρ8和圓弧段十ρ10的圓心連線之間的夾角，α為圓弧段八ρ8的圓心分別與圓弧段九ρ9和圓弧段十ρ10的圓心連線之間的夾角，γ為圓弧段十ρ10的圓心分別與圓弧段八ρ8和圓弧段九ρ9的圓心連線之間的夾角；

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

（3-4）；

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

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

（4-5）

由（4-5）可以得到：

（4-1）

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

（4-2）

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

（4-3）

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

（4-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ī)的行駛速度，設(shè)定的曲線路徑上距離農(nóng)機(jī)中心最近的點(diǎn)即為M；

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

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

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

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

（5）

其中，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)角；

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

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

本發(fā)明工作時(shí)，視覺(jué)機(jī)器相機(jī)采集農(nóng)機(jī)周?chē)沫h(huán)境信息，根據(jù)周?chē)h(huán)境信息確認(rèn)農(nóng)機(jī)是否進(jìn)入避障，若檢測(cè)到農(nóng)機(jī)前方有小障礙物時(shí)，農(nóng)機(jī)進(jìn)入避障導(dǎo)航狀態(tài)，農(nóng)機(jī)通過(guò)傳感器檢測(cè)獲得農(nóng)機(jī)的位置信息，計(jì)算農(nóng)機(jī)前方障礙物的特征圓的大小、農(nóng)機(jī)與障礙物的距離，根據(jù)農(nóng)機(jī)的犁具寬度和農(nóng)機(jī)最小轉(zhuǎn)彎半徑確定特征圓的大小以設(shè)定安全距離，使用改進(jìn)的最短切線法設(shè)定理論避障路徑，但是由于理論避障路徑的曲率不連續(xù)，使農(nóng)機(jī)避障的控制精度降低，使用Bezier曲線優(yōu)化方法優(yōu)化理論避障路徑生成新的實(shí)際避障路徑，通過(guò)預(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控制器，通過(guò)控制農(nóng)機(jī)的前輪轉(zhuǎn)角使農(nóng)機(jī)沿著設(shè)定的曲線行走，從而使農(nóng)機(jī)自動(dòng)繞開(kāi)障礙物；當(dāng)檢測(cè)到農(nóng)機(jī)前方為地頭時(shí)，做出掉頭決策時(shí)，農(nóng)機(jī)根據(jù)實(shí)際的作業(yè)模式選擇設(shè)定好的掉頭路徑，當(dāng)作業(yè)模式為播種時(shí)農(nóng)機(jī)的掉頭路徑選擇多線型路徑一，農(nóng)機(jī)通過(guò)傳感器檢測(cè)獲得農(nóng)機(jī)的位置信息，根據(jù)以上所述控制方法控制農(nóng)機(jī)的掉頭，使農(nóng)機(jī)按照設(shè)定的掉頭路徑曲線行走；本發(fā)明通過(guò)改進(jìn)后的最短切線法計(jì)算出一條理論避障路徑，使用基于Bezier曲線的路徑優(yōu)化方法對(duì)理論避障路徑進(jìn)行優(yōu)化，使避障路徑更加容易控制，通過(guò)預(yù)瞄控制器和PI控制器的結(jié)合控制農(nóng)機(jī)的前輪轉(zhuǎn)向角使農(nóng)機(jī)沿著設(shè)定好的避障曲線行走，控制精度高；通過(guò)農(nóng)機(jī)的實(shí)際作業(yè)模式選擇相對(duì)應(yīng)的掉頭路徑，適應(yīng)范圍更加廣泛；可應(yīng)用于無(wú)人駕駛的農(nóng)機(jī)在田間導(dǎo)航作業(yè)時(shí)自動(dòng)避開(kāi)小障礙物和自動(dòng)掉頭的控制工作中。

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