王 輝，王桂民，羅錫文，張智剛，高 陽(yáng)，何 杰，岳斌斌

?

基于預(yù)瞄追蹤模型的農(nóng)機(jī)導(dǎo)航路徑跟蹤控制方法

王 輝1，王桂民2※，羅錫文1，張智剛1，高 陽(yáng)2，何 杰1，岳斌斌1

（1. 華南農(nóng)業(yè)大學(xué)南方農(nóng)業(yè)機(jī)械與裝備關(guān)鍵技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，廣州 510642；2. 雷沃重工股份有限公司，濰坊 261206）

農(nóng)機(jī)導(dǎo)航系統(tǒng)的上線性能和復(fù)雜路面抗干擾能力影響著農(nóng)田作業(yè)的質(zhì)量和效率，為提高農(nóng)機(jī)導(dǎo)航系統(tǒng)的上線速度、上線穩(wěn)定性和對(duì)復(fù)雜路面的適應(yīng)性，提出了一種預(yù)瞄追蹤模型的農(nóng)機(jī)導(dǎo)航路徑跟蹤控制方法。該方法實(shí)質(zhì)是對(duì)農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型方法的改進(jìn)，針對(duì)農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型小角度線性化算法中近似條件的缺點(diǎn)，采用預(yù)瞄追蹤輔助直線引導(dǎo)農(nóng)機(jī)快速穩(wěn)定跟蹤規(guī)劃路徑。該文參考農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型極點(diǎn)最優(yōu)配置算法證明過(guò)程，分3步證明了該控制方法的可行性，并通過(guò)仿真和試驗(yàn)驗(yàn)證了該方法的有效性。仿真結(jié)果顯示在不同的初始位置偏差和航向偏差條件下該方法都可以迅速消除偏差以穩(wěn)定跟蹤規(guī)劃路徑，位置偏差校正曲線平滑且超調(diào)量微小，說(shuō)明預(yù)瞄追蹤模型方法對(duì)提高農(nóng)機(jī)導(dǎo)航系統(tǒng)的上線性能和抗干擾能力是有效的。田間試驗(yàn)結(jié)果：在初始航向偏差為0，初始位置偏差分別為0.5、1、1.5 m條件下，上線時(shí)間分別為6.8、8.2、9.4 s，上線距離分別為6.73、8.11、9.33 m，超調(diào)量分別為5.2 、7.0 、8.5 cm；顛簸不平旱地路面直線路徑跟蹤的最大誤差不超過(guò)4.23 cm，誤差絕對(duì)值的平均值為1 cm，標(biāo)準(zhǔn)差為1.25 cm。數(shù)據(jù)表明采用該文提出的控制方法具有良好的上線和直線路徑跟蹤效果，滿足農(nóng)業(yè)機(jī)械的導(dǎo)航作業(yè)要求。

農(nóng)業(yè)機(jī)械；模型；控制；上線；顛簸路面；路徑跟蹤

0 引 言

農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航是精準(zhǔn)農(nóng)業(yè)技術(shù)體系中核心技術(shù)之一，廣泛應(yīng)用于耕作、播種、施肥、噴藥、收獲等農(nóng)業(yè)生產(chǎn)過(guò)程[1-2]。農(nóng)田作業(yè)導(dǎo)航系統(tǒng)應(yīng)用中，地頭的對(duì)行上線是一種較為頻繁且困難的操作，路面顛簸不平現(xiàn)象時(shí)常發(fā)生，為提高作業(yè)質(zhì)量和效率，要求農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)上線快速穩(wěn)定、直線跟蹤精度高以及對(duì)顛簸復(fù)雜路面抗干擾性好。影響農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)工作性能的兩項(xiàng)關(guān)鍵技術(shù)是：位置姿態(tài)信息的獲取和路徑跟蹤控制方法[1]。在農(nóng)機(jī)位置姿態(tài)信息精度足夠的情況下，路徑跟蹤控制方法的選擇和參數(shù)整定是提高農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)精度和穩(wěn)定性的關(guān)鍵。

國(guó)內(nèi)外眾多學(xué)者對(duì)路徑跟蹤控制方法進(jìn)行了研究，包括基于農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型的路徑跟蹤控制方法[3-10]、基于農(nóng)機(jī)動(dòng)力學(xué)模型的路徑跟蹤控制方法[11-17]和與模型無(wú)關(guān)的路徑跟蹤控制方法[18-26]?；谶\(yùn)動(dòng)學(xué)模型的方法主要是對(duì)模型進(jìn)行小角度線性化逼近，在常速假設(shè)條件下進(jìn)行控制器算法設(shè)計(jì)，這樣不但引進(jìn)了線性化誤差造成系統(tǒng)的穩(wěn)定性變差，而且在速度變化時(shí)控制器的魯棒性也變差?；趧?dòng)力學(xué)模型的控制方法雖然考慮了農(nóng)機(jī)的動(dòng)力學(xué)特征，但是動(dòng)力學(xué)模型參數(shù)很難在線實(shí)時(shí)辨識(shí)。在與模型無(wú)關(guān)的控制方法中，純追蹤方法前視距離的在線自適應(yīng)問(wèn)題至今仍沒(méi)有很好解決，尤其是變速條件下的前視距離在線自適應(yīng)。PID控制是一種消除誤差的控制策略，應(yīng)用廣泛，但是控制參數(shù)整定困難，需要一定的經(jīng)驗(yàn)和大量的參數(shù)整定試驗(yàn)。智能方法雖然具有仿人智能和非線性映射能力，但是其設(shè)計(jì)需要一定的經(jīng)驗(yàn)知識(shí)和復(fù)雜的學(xué)習(xí)訓(xùn)練過(guò)程。

針對(duì)基于農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型控制方法中小角度線性化的缺點(diǎn)以及大田作業(yè)對(duì)農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)的實(shí)際要求，本文提出一種預(yù)瞄追蹤模型的路徑跟蹤控制方法。參考農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型小角度線性化極點(diǎn)最優(yōu)配置算法的證明過(guò)程，將本文方法的路徑跟蹤過(guò)程分成3個(gè)階段進(jìn)行證明。通過(guò)仿真和試驗(yàn)，驗(yàn)證了該算法的有效性，可以滿足農(nóng)業(yè)機(jī)械大田作業(yè)要求。

1 導(dǎo)航工作原理及控制參數(shù)作用分析

農(nóng)機(jī)的自動(dòng)導(dǎo)航控制主要是對(duì)農(nóng)機(jī)進(jìn)行橫向位置和航向偏差控制，控制農(nóng)機(jī)跟蹤預(yù)先設(shè)置好的作業(yè)路徑，使其與設(shè)置路徑之間的橫向偏差和航向偏差保持在一定的精度范圍之內(nèi)，從而滿足農(nóng)業(yè)生產(chǎn)的需要。

現(xiàn)代農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)一般由檢測(cè)單元、控制單元、執(zhí)行單元及監(jiān)控單元四部分構(gòu)成。檢測(cè)單元負(fù)責(zé)實(shí)時(shí)、精準(zhǔn)地獲取農(nóng)機(jī)當(dāng)前的位置與姿態(tài)信息；控制單元負(fù)責(zé)根據(jù)農(nóng)機(jī)當(dāng)前的位姿信息和目標(biāo)路徑信息，按照某種控制策略計(jì)算出轉(zhuǎn)向角度的期望值；執(zhí)行單元的作用是驅(qū)動(dòng)農(nóng)機(jī)轉(zhuǎn)向輪由當(dāng)前值轉(zhuǎn)動(dòng)至輪角期望值；監(jiān)控單元是導(dǎo)航系統(tǒng)的人機(jī)交互界面，主要功能是導(dǎo)航狀態(tài)監(jiān)視，系統(tǒng)參數(shù)設(shè)定和導(dǎo)航控制任務(wù)管理[1]。

控制單元是導(dǎo)航系統(tǒng)的核心，路徑跟蹤控制方法的選擇和參數(shù)整定是提高農(nóng)機(jī)自動(dòng)導(dǎo)航精度和穩(wěn)定性的關(guān)鍵。假設(shè)基于位置偏差和航向偏差設(shè)計(jì)期望輪角的二元線性控制律

式中θ為期望車輪轉(zhuǎn)角，rad；P為車體與期望路徑（當(dāng)前要跟蹤的規(guī)劃路徑）的位置偏差，m；ψ為車體與期望路徑的航向偏差，rad；為位置偏差系數(shù)；為航向偏差系數(shù)；1為位置偏差輪角決策量，rad；2為航向偏差輪角決策量，rad。

式（1）至式（3）中，輸入是農(nóng)業(yè)機(jī)械與期望路徑的位置偏差P和航向偏差ψ，輸出是農(nóng)業(yè)機(jī)械的輪角。通過(guò)控制輪式拖拉機(jī)的輪角修正農(nóng)業(yè)機(jī)械與設(shè)定路線的位置偏差和航向偏差。期望輪角的決策量可分為兩部分：位置偏差輪角決策量1和航向偏差輪角決策量2。位置偏差輪角決策量的作用是使農(nóng)機(jī)沿著規(guī)劃路徑快速上線糾偏，其中位置偏差輪角決策量的權(quán)重越大，系統(tǒng)糾偏速度越快，但是可能會(huì)產(chǎn)生較大的控制震蕩，相當(dāng)于二階系統(tǒng)的欠阻尼階躍響應(yīng)；航向偏差輪角決策量的作用是使農(nóng)機(jī)沿著平行于規(guī)劃路徑的直線行走，具有預(yù)測(cè)的作用，有益于農(nóng)機(jī)的穩(wěn)定性，航向偏差輪角決策量的權(quán)重越大，系統(tǒng)糾偏速度慢，但是系統(tǒng)穩(wěn)定性增強(qiáng)。相當(dāng)于二階系統(tǒng)的過(guò)阻尼階躍響應(yīng)[26]。當(dāng)位置偏差較大時(shí)，應(yīng)增大位置偏差的輪角決策量權(quán)重以保證農(nóng)機(jī)快速地跟蹤至設(shè)定路徑，但是過(guò)大的位置偏差輪角決策量權(quán)重會(huì)造成農(nóng)機(jī)上線或直線跟蹤時(shí)震蕩嚴(yán)重甚至發(fā)散。當(dāng)航向偏差較大時(shí)，應(yīng)增大航向偏差輪角決策量權(quán)重以保證農(nóng)機(jī)快速的跟蹤目標(biāo)航向，但是過(guò)大的航向偏差輪角決策量權(quán)重會(huì)造成系統(tǒng)糾偏性能疲軟導(dǎo)致系統(tǒng)在直線路徑跟蹤時(shí)出現(xiàn)大S型彎。

綜上所述，農(nóng)機(jī)處于不同的位置偏差或航向偏差時(shí)，合理配置位置偏差輪角決策量和航向偏差輪角決策量的權(quán)重，可有效提高農(nóng)機(jī)位置偏差和航向偏差的校正速度和穩(wěn)定性。實(shí)際農(nóng)機(jī)導(dǎo)航應(yīng)用中的上線問(wèn)題可歸結(jié)為初始位置偏差較大的直線路徑跟蹤問(wèn)題；顛簸復(fù)雜農(nóng)田路面造成農(nóng)機(jī)出現(xiàn)甩頭擺尾滑移等問(wèn)題可歸結(jié)為不確定因素的農(nóng)機(jī)位置偏差或航向偏差瞬間變大問(wèn)題。合理配置位置偏差輪角決策量和航向偏差輪角決策量的權(quán)重，可以提高農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)的上線速度、上線穩(wěn)定性和對(duì)復(fù)雜路面的適應(yīng)性。

2 基于農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型的路徑跟蹤方法

2.1 農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型小角度線性化方法

農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型是導(dǎo)航控制算法設(shè)計(jì)的基礎(chǔ)，本研究采用的雷沃M904-D拖拉機(jī)是一種前輪轉(zhuǎn)向、四輪驅(qū)動(dòng)的輪式農(nóng)業(yè)機(jī)械。如不考慮拖拉機(jī)行走時(shí)的側(cè)偏和地面狀況等因素的影響時(shí)，基于拖拉機(jī)二輪轉(zhuǎn)向的特點(diǎn)，可將其簡(jiǎn)化為二輪車模型，如圖1所示。在導(dǎo)航平面坐標(biāo)系下得到農(nóng)機(jī)運(yùn)動(dòng)學(xué)微分方程，如式（4）～（5）所示。

注：Pe為車體與期望路徑的位置偏差，m；ψe為車體與期望路徑的航向偏差角，rad；R為車體后輪中心轉(zhuǎn)彎半徑，m；V為車體速度，m·s-1；θ為車輪轉(zhuǎn)角，rad；L為車體軸距，m。

式中為時(shí)間，s。

直線路徑跟蹤中車輪轉(zhuǎn)角和航向偏差ψ為小角度值（一般≤5°），故可對(duì)方程進(jìn)行線性化，得到二輪轉(zhuǎn)向拖拉機(jī)運(yùn)動(dòng)學(xué)微分方程的狀態(tài)空間矩陣形式

根據(jù)方程（6）可設(shè)計(jì)全狀態(tài)反饋的二元線性直線路徑跟蹤控制律

式中1為航向偏差反饋系數(shù)，2為位置偏差反饋系數(shù)。

將式（7）帶入式（6）中即可得到閉環(huán)控制系統(tǒng)的狀態(tài)空間方程

利用極點(diǎn)配置思想，將此系統(tǒng)的極點(diǎn)放置在一個(gè)理想的位置以獲得最佳的系統(tǒng)響應(yīng)。為了求出極點(diǎn)，可利用該系統(tǒng)的特征方程為0，即令

式中為拉氏變換的復(fù)頻域，為二階單位矩陣。

計(jì)算行列式得出

假設(shè)系統(tǒng)所需的理想極點(diǎn)為，則有[27]

比較式（10）和（11）2個(gè)等式，令相等的冪次方項(xiàng)系數(shù)相等，可得

試驗(yàn)證明，典型二輪轉(zhuǎn)向農(nóng)機(jī)車輛中，當(dāng)=?1時(shí)，系統(tǒng)表現(xiàn)出良好的響應(yīng)效果。因此，可得到二輪轉(zhuǎn)向車輛直線路徑跟蹤的轉(zhuǎn)向角計(jì)算公式[27]如下

實(shí)際應(yīng)用中定義車輛位置偏差、航向偏差相對(duì)跟蹤路徑左偏值為負(fù)，右偏值為正；車輛輪角相對(duì)輪角零位左偏值為正，右偏值為負(fù)。基于此，可將式（14）改寫成

2.2 影響農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型方法控制效果因素分析

從式（15）可以看出，對(duì)基于農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型的微分方程式經(jīng)過(guò)小角度線性化和最優(yōu)閉環(huán)極點(diǎn)配置后，最終得到關(guān)于位置偏差和航向偏差的二元線性控制律，位置偏差系數(shù)和航向偏差系數(shù)與速度有關(guān)。推導(dǎo)過(guò)程中對(duì)航向偏差ψ和車輪轉(zhuǎn)角進(jìn)行了小角度線性化，因此此控制律只有在位置偏差、航向偏差以及輪角擺動(dòng)都很小的理想直線路徑跟蹤控制過(guò)程才能取得良好的控制效果。大田作業(yè)導(dǎo)航系統(tǒng)應(yīng)用中，由于地頭轉(zhuǎn)彎調(diào)頭對(duì)行上線時(shí)，人工擺放初始位置與規(guī)劃行偏差較大，顛簸復(fù)雜農(nóng)田路面造成農(nóng)機(jī)出現(xiàn)甩頭擺尾滑動(dòng)等現(xiàn)象，從而導(dǎo)致農(nóng)機(jī)位置或航向偏差瞬間變大等問(wèn)題，農(nóng)機(jī)不可能總是處于較為理想的直線路徑跟蹤控制過(guò)程中。農(nóng)田實(shí)際應(yīng)用環(huán)境迫切要求研究提高農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)上線速度、上線穩(wěn)定性以及對(duì)復(fù)雜路面適應(yīng)性的路徑跟蹤控制算法。

3 預(yù)瞄追蹤模型路徑跟蹤方法

3.1 預(yù)瞄追蹤模型方法描述

預(yù)瞄追蹤模型根據(jù)農(nóng)機(jī)駕駛員在進(jìn)行路徑跟蹤時(shí)的轉(zhuǎn)向操作規(guī)律建立。一個(gè)合格的駕駛員對(duì)農(nóng)機(jī)操作發(fā)出的控制轉(zhuǎn)向指令都是遵循一定原則的。這個(gè)原則的主要目標(biāo)是使農(nóng)機(jī)的運(yùn)動(dòng)盡可能地與設(shè)定的軌跡一致[28-30]。在直線跟蹤時(shí)，駕駛員預(yù)瞄農(nóng)機(jī)前方路徑規(guī)劃直線上的一點(diǎn)，通過(guò)控制車輪轉(zhuǎn)角跟蹤該點(diǎn)。當(dāng)農(nóng)機(jī)方向和預(yù)瞄方向偏差較大時(shí)，駕駛員對(duì)車輪轉(zhuǎn)角進(jìn)行大角度控制，以快速校正預(yù)瞄航向偏差跟蹤預(yù)瞄路徑；當(dāng)農(nóng)機(jī)方向和預(yù)瞄方向偏差較小時(shí)，駕駛員對(duì)車輪轉(zhuǎn)角進(jìn)行小角度控制，以穩(wěn)定地跟蹤預(yù)瞄路徑。

根據(jù)上述思想設(shè)計(jì)控制律如下：期望車輪轉(zhuǎn)角為農(nóng)機(jī)方向和預(yù)瞄方向偏差的PID控制，得到預(yù)瞄追蹤模型。具體描述如下：如圖2所示，建立導(dǎo)航平面坐標(biāo)系，定義：點(diǎn)1為農(nóng)機(jī)中心點(diǎn)；直線為作業(yè)規(guī)劃行直線，ψ為線在導(dǎo)航平面坐標(biāo)系下的航向角度，稱為目標(biāo)航向；規(guī)定點(diǎn)2為作業(yè)規(guī)劃行直線上農(nóng)機(jī)追蹤的一個(gè)目標(biāo)點(diǎn)，稱作預(yù)瞄追蹤點(diǎn)；直線12稱作預(yù)瞄追蹤直線，ψ為12線在導(dǎo)航平面坐標(biāo)系下的航向角度，稱作預(yù)瞄航向角；過(guò)點(diǎn)1向做垂線，垂足為，1之間的距離即為位置偏差P，位置偏差為有符號(hào)數(shù)，本文規(guī)定農(nóng)機(jī)在期望路徑前進(jìn)方向右側(cè)時(shí)的位置偏差為正（P>0），農(nóng)機(jī)在期望路徑左側(cè)時(shí)的位置偏差為負(fù)（P<0）；2之間的距離稱作前視距離；為車輪轉(zhuǎn)角，為有符號(hào)數(shù)，輪角相對(duì)零位角度值右偏值為負(fù)（<0），輪角相對(duì)零位角度值左偏值為正（>0）；ψ為車體在導(dǎo)航平面坐標(biāo)系下的當(dāng)前航向角；ψ為農(nóng)機(jī)當(dāng)前航向ψ和目標(biāo)航向ψ之間的偏差，稱作航向偏差角；φ為農(nóng)機(jī)當(dāng)前航向ψ和預(yù)瞄航向ψ之間的偏差，稱作預(yù)瞄航向偏差角。設(shè)計(jì)控制律，令農(nóng)機(jī)的車輪轉(zhuǎn)角為預(yù)瞄航向偏差φ的倍，稱為控制增益。則有

由式（16）～（19）得

注：ψt為目標(biāo)航向角，rad；ψc為車體航向角，rad；ψa為預(yù)瞄航向角，rad；φe為預(yù)瞄航向偏差角，rad；d為前視距離，m；P1為農(nóng)機(jī)中心點(diǎn)；P2為預(yù)瞄追蹤點(diǎn)；A、B為規(guī)劃作業(yè)行1上的兩點(diǎn)。

3.2 預(yù)瞄追蹤模型控制方法可行性分析

預(yù)瞄航向ψ是車體中心位置點(diǎn)1和規(guī)劃作業(yè)行直線上農(nóng)機(jī)預(yù)瞄追蹤點(diǎn)2的連線方向，控制律設(shè)計(jì)車輪轉(zhuǎn)角為預(yù)瞄航向偏差φ的PID控制，其控制規(guī)則的作用是使農(nóng)機(jī)在行駛過(guò)程中通過(guò)控制車輪轉(zhuǎn)角從而沿預(yù)瞄追蹤直線12行駛。在行駛過(guò)程中，一方面，農(nóng)機(jī)通過(guò)控制車輪轉(zhuǎn)角不斷糾正車身方向，使車體沿12方向行駛，在消除初始導(dǎo)航階段的較大預(yù)瞄航向偏差φ之后，農(nóng)機(jī)車體航向ψ和預(yù)瞄航向ψ保持大體一致；另一方面，農(nóng)機(jī)中心點(diǎn)1不斷移動(dòng)，沿規(guī)劃作業(yè)行方向行駛的分速度使農(nóng)機(jī)漸漸靠近規(guī)劃作業(yè)行上線（當(dāng)P<5 cm，ψ<3°時(shí)，認(rèn)為農(nóng)機(jī)上線）。上線時(shí)，車體航向ψ、預(yù)瞄航向ψ和目標(biāo)航向ψ基本重合，控制量接近零。以上分析說(shuō)明，農(nóng)機(jī)的上線過(guò)程是一個(gè)漸進(jìn)修正農(nóng)機(jī)車體航向ψ和位置偏差P靠近作業(yè)規(guī)劃行的過(guò)程，上線時(shí)位置偏差P、航向偏差ψ和車輪轉(zhuǎn)角都接近為0，不會(huì)產(chǎn)生大超調(diào)和震蕩。

上線后，農(nóng)機(jī)進(jìn)入規(guī)劃作業(yè)行直線的路徑跟蹤階段，控制規(guī)則仍然通過(guò)控制車輪轉(zhuǎn)角跟蹤預(yù)瞄直線12行駛，而此時(shí)預(yù)瞄直線12和作業(yè)規(guī)劃行直線基本重合，說(shuō)明此控制律可以通過(guò)控制農(nóng)機(jī)跟蹤預(yù)瞄直線12從而進(jìn)行規(guī)劃作業(yè)行直線的路徑跟蹤。

3.3 農(nóng)機(jī)預(yù)瞄追蹤模型的證明

將農(nóng)機(jī)的路徑跟蹤過(guò)程分成3個(gè)階段，第一階段：消除初始導(dǎo)航階段的大預(yù)瞄航向偏差；第二階段：農(nóng)機(jī)沿預(yù)瞄方向行駛漸進(jìn)上線；第三階段：沿作業(yè)規(guī)劃行的直線路徑跟蹤。

第一階段，是一個(gè)大角度的航向跟蹤問(wèn)題，張智剛等設(shè)計(jì)了目標(biāo)航向跟蹤的PID校正環(huán)節(jié)，并進(jìn)行了仿真，90°航向跟蹤過(guò)程中，轉(zhuǎn)向控制系統(tǒng)動(dòng)態(tài)響應(yīng)的上升時(shí)間大約在4 s以內(nèi)[31-32]。說(shuō)明基于航向偏差的PID控制方法對(duì)運(yùn)動(dòng)中的農(nóng)機(jī)具有較好的大角度航向跟蹤能力。

第二階段，農(nóng)機(jī)沿著預(yù)瞄追蹤直線12行駛，由于此階段預(yù)瞄航向偏差角φ和車輪轉(zhuǎn)角為小角度，將預(yù)瞄追蹤直線12作為輔助直線，本階段可看做是農(nóng)機(jī)對(duì)預(yù)瞄追蹤直線12的直線跟蹤。采用農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型小角度線性化極點(diǎn)最優(yōu)配置推導(dǎo)方法進(jìn)行推導(dǎo)，此時(shí)農(nóng)機(jī)車輪轉(zhuǎn)角可引用二輪轉(zhuǎn)向車輛直線路徑跟蹤的期望轉(zhuǎn)向角計(jì)算式（15），此時(shí)的航向偏差為農(nóng)機(jī)車體當(dāng)前航向ψ和預(yù)瞄航向ψ的偏差即預(yù)瞄航向偏差φ，位置偏差為農(nóng)機(jī)中心點(diǎn)1到預(yù)瞄跟蹤直線12的距離P=0（因?yàn)辄c(diǎn)1在直線12上）。第二階段農(nóng)機(jī)的最優(yōu)車輪轉(zhuǎn)角計(jì)算公式為

由式（17）、（18）、（19）可知

由式（21）、（22）可得

對(duì)比預(yù)瞄追蹤模型車輪轉(zhuǎn)角決策式（20）和式（23）可知，取控制增益

此時(shí)兩式相同，第二階段農(nóng)機(jī)預(yù)瞄追蹤模型公式得證。

第三階段，農(nóng)機(jī)上線對(duì)行后，進(jìn)行直線段的路徑跟蹤。此時(shí)位置偏差很小，故有

結(jié)合第二階段中的取值，此時(shí)可將預(yù)瞄追蹤模型車輪轉(zhuǎn)角決策式（20）簡(jiǎn)化為

將式（26）與農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型小角度線性化公式（15）對(duì)比可知，取前視距離的大小等于2時(shí)，此時(shí)兩式相同，從而說(shuō)明預(yù)瞄追蹤模型算法在直線跟蹤中有良好的路徑跟蹤效果。

綜上，可得出當(dāng)控制增益2，前視距離的值等于2時(shí)，農(nóng)機(jī)預(yù)瞄追蹤模型算法可在農(nóng)機(jī)的上線和直線路徑跟蹤階段得到良好的控制效果。

4 運(yùn)動(dòng)學(xué)和預(yù)瞄兩種方法的比較

比較運(yùn)動(dòng)學(xué)模型極點(diǎn)最優(yōu)算法計(jì)算式（15）和預(yù)瞄追蹤模型算法計(jì)算式（23），兩式的不同之處在于位置偏差輪角決策量不同，運(yùn)動(dòng)學(xué)模型公式中位置偏差輪角決策量1M

預(yù)瞄追蹤模型公式中位置偏差輪角決策量1A

1M隨位置偏差的增大呈線性增加，1A隨位置偏差的增大呈反正切函數(shù)增加，如圖3所示。

圖3 兩種算法位置偏差輪角決策量變化趨勢(shì)

位置偏差輪角決策量隨位置偏差的增大呈線性增加顯然是不合適的，因?yàn)槲恢闷钤龃蟮揭欢ǔ潭葧r(shí)，航向偏差輪角決策量將失去作用（因航向偏差變動(dòng)范圍為（?p/2，p/2）），導(dǎo)致農(nóng)機(jī)大初始位置偏差路徑跟蹤時(shí)的失穩(wěn)或失控。預(yù)瞄追蹤模型中，航向偏差輪角決策量范圍和位置偏差輪角決策量范圍均為（?p/2，p/2），這有益于位置偏差輪角決策量和航向偏差輪角決策量保持合適的權(quán)重，不論農(nóng)機(jī)相對(duì)規(guī)劃路徑處于什么樣的位姿，導(dǎo)航?jīng)Q策出的輪角始終有益于農(nóng)機(jī)跟蹤預(yù)瞄直線12，從而快速穩(wěn)定追蹤目標(biāo)直線。

農(nóng)田實(shí)際導(dǎo)航應(yīng)用中的上線問(wèn)題可歸結(jié)為初始位置偏差較大的直線跟蹤問(wèn)題，顛簸復(fù)雜農(nóng)田路面造成農(nóng)機(jī)出現(xiàn)甩頭擺尾滑動(dòng)等問(wèn)題可歸結(jié)為不確定因素的農(nóng)機(jī)位置偏差或航向偏差瞬間變大問(wèn)題。以上2個(gè)問(wèn)題均難以避免和預(yù)測(cè)，提高路徑跟蹤速度和穩(wěn)定性是解決以上2個(gè)問(wèn)題的途徑之一。

根據(jù)上述分析，預(yù)瞄追蹤模型方法不論農(nóng)機(jī)相對(duì)規(guī)劃路徑處于何種位姿，導(dǎo)航?jīng)Q策出的輪角始終有益于農(nóng)機(jī)跟蹤預(yù)瞄直線，從而快速穩(wěn)定追蹤目標(biāo)直線。因此，采用預(yù)瞄追蹤模型算法可提高農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)的上線速度、上線穩(wěn)定性和對(duì)復(fù)雜路面的適應(yīng)性。

5 預(yù)瞄追蹤模型的仿真研究

為了驗(yàn)證預(yù)瞄追蹤模型在上線和復(fù)雜路面直線路徑跟蹤中的有效性，建立了Matlab/Simulink環(huán)境下預(yù)瞄追蹤模型的路徑跟蹤仿真模型。

實(shí)際中，上線時(shí)的初始位置偏差一般在1.5 m之內(nèi)，顛簸復(fù)雜路面直線路徑跟蹤時(shí)的位置偏差波動(dòng)范圍在±0.15 m范圍之內(nèi)，航向偏差波動(dòng)在±15°以內(nèi)。

仿真中：設(shè)定速度=1 m/s，根據(jù)本文3.3章節(jié)描述，應(yīng)取前視距離=2 m，經(jīng)過(guò)仿真，前視距離=2 m時(shí)，系統(tǒng)上線和直線跟蹤穩(wěn)定。但為充分挖掘此算法的優(yōu)勢(shì)，提高導(dǎo)航系統(tǒng)的上線速度和偏差校正速度的同時(shí)不失穩(wěn)定性，經(jīng)過(guò)反復(fù)仿真和試驗(yàn)，適當(dāng)減小了前視距離，選取前視距離=1.5 m，此時(shí)，偏差校正能力強(qiáng)且系統(tǒng)穩(wěn)定。最大車輪轉(zhuǎn)角max=25°。雷沃M904-D拖拉機(jī)軸距=2.4 m，根據(jù)式（23）設(shè)置控制增益=4.8。圖4為初始航向偏差0、初始位置偏差0.5～1.5 m條件下的上線仿真路徑跟蹤誤差變化曲線圖，表1列出初始航向偏差為0（因用戶進(jìn)行導(dǎo)航上線操作時(shí)，一般會(huì)把農(nóng)機(jī)車輛擺放在平行或朝向規(guī)劃作業(yè)行的位置）、初始位置偏差為0.5～1.5 m條件下的上線性能參數(shù)，表2列出復(fù)雜路面直線跟蹤的仿真參數(shù)統(tǒng)計(jì)表。（農(nóng)機(jī)與規(guī)劃路徑的位置偏差P<0.05 m，航向偏差<3°時(shí)，稱農(nóng)機(jī)進(jìn)入上線直線路徑跟蹤狀態(tài)。）

圖4 上線路徑跟蹤仿真位置偏差變化曲線

表1 前視距離為1.5 m時(shí)上線性能仿真數(shù)據(jù)統(tǒng)計(jì)

注：初始航向偏差為0，初始位置偏差為0.5～1.5 m。

Note: Initial yaw deviation is 0, and the initial position deviation is 0.5-1.5 m.

表2 復(fù)雜路面直線跟蹤性能仿真數(shù)據(jù)統(tǒng)計(jì)

由圖4和表1可知，基于預(yù)瞄追蹤模型方法的上線路徑跟蹤，隨著初始位置偏差增加，上線時(shí)間和上線距離增長(zhǎng)。但從總體上看，在不同的初始位置偏差條件下都可以迅速消除橫向偏差，且無(wú)超調(diào)和震蕩，上線曲線變化平緩，說(shuō)明預(yù)瞄追蹤模型算法可有效提高農(nóng)機(jī)的上線速度和穩(wěn)定性。

由表2可知，基于預(yù)瞄追蹤模型方法的顛簸路面直線跟蹤，在出現(xiàn)小位置偏差、大航向偏差情況下，由于農(nóng)機(jī)很短時(shí)間內(nèi)來(lái)不及調(diào)整車身方向，可能會(huì)出現(xiàn)小幅超調(diào)。但從總體上看，在不同的初始位置偏差和航向偏差條件下都能夠迅速消除橫向位置偏差和航向偏差，進(jìn)而穩(wěn)定地跟蹤目標(biāo)直線，說(shuō)明預(yù)瞄追蹤模型方法在一定程度上可以提高農(nóng)機(jī)對(duì)復(fù)雜農(nóng)田的適應(yīng)性。

6 試驗(yàn)驗(yàn)證及結(jié)果討論

6.1 驗(yàn)證平臺(tái)

為了對(duì)提出的預(yù)瞄追蹤模型路徑跟蹤控制算法進(jìn)行驗(yàn)證，以雷沃M904-D拖拉機(jī)作為試驗(yàn)農(nóng)機(jī)，并在其上面安裝自主研發(fā)的雙天線GNSS農(nóng)機(jī)自動(dòng)導(dǎo)航控制平臺(tái)，該平臺(tái)由位姿傳感器、導(dǎo)航控制器、轉(zhuǎn)向執(zhí)行裝置、輪角傳感器和監(jiān)控顯示器構(gòu)成。其中，位置傳感器姿傳感器采用上海司南衛(wèi)星導(dǎo)航公司的雙天線板卡 K728，定位精度10 mm+1×10-6，航向精度0.2°/（為雙天線基線長(zhǎng)）。主天線安裝在車輛駕駛室頂部的右端，從天線安裝在車輛駕駛室頂部的左端（以拖拉機(jī)前進(jìn)方向?yàn)檎胺剑Ｗ藨B(tài)傳感器采用 XSENS 公司MTi-30慣性傳感器，橫滾、俯仰角測(cè)量精度為0.3°。慣性傳感器坐標(biāo)系三軸指向與拖拉機(jī)車體坐標(biāo)系三軸指向相同，安裝在拖拉機(jī)座椅下方。導(dǎo)航控制器和監(jiān)控顯示器采用雷沃重工股份有限公司的AGCS-II輔助自動(dòng)駕駛系統(tǒng)配套設(shè)備。轉(zhuǎn)向執(zhí)行裝置主件采用 EATON-KDG4V 型比例換向閥，通過(guò)加裝液壓管路與拖拉機(jī)轉(zhuǎn)向油路并聯(lián)，構(gòu)成電-液轉(zhuǎn)向控制機(jī)構(gòu)。輪角傳感器采用BEI公司的9902120CWHT 型霍爾角度傳感器，12位AD采樣精度，安裝在左前輪的轉(zhuǎn)向柱上，通過(guò)連桿將傳感器轉(zhuǎn)軸與前橋固連。圖5為雙天線GNSS農(nóng)機(jī)自動(dòng)導(dǎo)航控制平臺(tái)系統(tǒng)結(jié)構(gòu)圖，圖6為裝備農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)的雷沃M904-D拖拉機(jī)。

圖5 農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)結(jié)構(gòu)

圖6 裝備農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)的M904-D拖拉機(jī)

6.2 基于本文方法的農(nóng)機(jī)上線試驗(yàn)

以拖拉機(jī)四輪中心投影到地面上的點(diǎn)為定位點(diǎn)，在前視距離=1.5 m，速度=1 m/s，初始航向偏差0的條件下，進(jìn)行了0.5～1.5 m不同初始位置偏差條件下的上線試驗(yàn)，試驗(yàn)在華南農(nóng)業(yè)大學(xué)增城教學(xué)科研基地水泥路面進(jìn)行。

試驗(yàn)過(guò)程為：在試驗(yàn)路面上通過(guò)高精度GNSS采集、兩點(diǎn)，將由其確定的直線作為農(nóng)機(jī)跟蹤的目標(biāo)路徑。啟動(dòng)農(nóng)機(jī)并開(kāi)啟導(dǎo)航系統(tǒng)，通過(guò)導(dǎo)航系統(tǒng)的整體行偏移功能，將農(nóng)機(jī)平移至設(shè)定的初始位置偏差位置，清除整體行偏移數(shù)值，則農(nóng)機(jī)在路徑一端的初始位置偏差處上線。當(dāng)位置偏差穩(wěn)定在0.03 m內(nèi)時(shí)，通過(guò)人機(jī)交互界面發(fā)送停止導(dǎo)航命令，結(jié)束一次上線試驗(yàn)。在初始航向偏差為0、初始位置偏差為1 m時(shí)的路徑跟蹤效果如圖7所示，不同初始位置偏差條件下上線試驗(yàn)數(shù)據(jù)統(tǒng)計(jì)結(jié)果如表3所示。

圖7 1 m初始位置偏差路徑跟蹤效果

表3 不同初始位置偏差上線性能數(shù)據(jù)統(tǒng)計(jì)

從圖7和表3可以看出，田間測(cè)試結(jié)果與圖4和表1的仿真結(jié)果趨勢(shì)一致，但由于轉(zhuǎn)向執(zhí)行滯后等原因，效果稍差于仿真結(jié)果，上線時(shí)間誤差不超過(guò)1.17 s，上線距離誤差不超過(guò)1.12 m，這樣的誤差是可以接受的。對(duì)預(yù)瞄追蹤模型路徑跟蹤算法上線試驗(yàn)的上線時(shí)間、上線距離和超調(diào)量進(jìn)行統(tǒng)計(jì)分析，3個(gè)參數(shù)代表了農(nóng)機(jī)上線性能。由表3可知，隨著初始位置偏差增加，上線時(shí)間、上線距離、超調(diào)量呈增大趨勢(shì)。但從總體來(lái)看，基于預(yù)瞄追蹤模型的路徑跟蹤在不同的初始位置偏差條件下都可以迅速消除橫向偏差，超調(diào)量較小、震蕩也較小、上線曲線變化相對(duì)平緩，同時(shí)具有良好的動(dòng)態(tài)特性。表明預(yù)瞄追蹤模型算法對(duì)提高農(nóng)機(jī)導(dǎo)航系統(tǒng)的上線速度和穩(wěn)定性是有效的。

6.3 基于本文方法不同路況的直線路徑跟蹤試驗(yàn)

為了驗(yàn)證本文方法對(duì)復(fù)雜農(nóng)田的魯棒性和適應(yīng)性較好，分別在水泥路面和玉米秋收后顛簸不平的旱地路面2種路況環(huán)境下進(jìn)行了直線路徑跟蹤試驗(yàn)，試驗(yàn)在新疆額敏縣團(tuán)結(jié)農(nóng)場(chǎng)進(jìn)行。試驗(yàn)中，設(shè)定前視距離=1.5 m，速度=1 m/s。

試驗(yàn)過(guò)程為：在試驗(yàn)地塊上采集、兩點(diǎn)，將由其確定的直線作為農(nóng)機(jī)跟蹤的目標(biāo)路徑。然后在路徑線附近啟動(dòng)農(nóng)機(jī)，并啟動(dòng)導(dǎo)航控制系統(tǒng)，進(jìn)行自動(dòng)路徑跟蹤控制，當(dāng)農(nóng)機(jī)接近路徑的另一端時(shí)，通過(guò)人機(jī)界面發(fā)送停止導(dǎo)航命令，結(jié)束一次直線路徑跟蹤試驗(yàn)。在水泥路面和顛簸旱地路面的直線路徑跟蹤效果如圖8所示，試驗(yàn)數(shù)據(jù)統(tǒng)計(jì)結(jié)果如表4所示。

試驗(yàn)數(shù)據(jù)分析：對(duì)預(yù)瞄追蹤模型直線路徑跟蹤的橫向位置偏差均值、偏差絕對(duì)值的極值、偏差絕對(duì)值的均值和標(biāo)準(zhǔn)差進(jìn)行了統(tǒng)計(jì)分析。偏差的均值反映了導(dǎo)航系統(tǒng)的系統(tǒng)誤差，偏差絕對(duì)值的極值和偏差絕對(duì)值的均值反映了導(dǎo)航控制的精度，標(biāo)準(zhǔn)差反映了導(dǎo)航控制的穩(wěn)定性。

由表4可知，預(yù)瞄追蹤模型路徑跟蹤控制算法在水泥路面上時(shí)，導(dǎo)航控制偏差的極值為2.66 cm，偏差絕對(duì)值的平均值為0.54 cm，標(biāo)準(zhǔn)差為0.67 cm。在顛簸旱地環(huán)境下，導(dǎo)航控制偏差的極值為4.23 cm，偏差絕對(duì)值的平均值為1 cm，標(biāo)準(zhǔn)差為1.25 cm。本文算法在水泥路面上表現(xiàn)出良好的控制效果，控制精度高，穩(wěn)定性好；隨地況條件的下降，控制效果下降，但本文算法依然表現(xiàn)出良好的路況適應(yīng)性，在顛簸旱地路面導(dǎo)航控制精度依然較高，穩(wěn)定性良好?；陬A(yù)瞄追蹤模型算法的直線路徑跟蹤精度高，對(duì)復(fù)雜農(nóng)田地況的魯棒性和適應(yīng)性較好。考慮到顛簸地面不平對(duì)GNSS定位精度造成一定的影響，這樣的跟蹤精度和穩(wěn)定性是可以接受的，滿足農(nóng)業(yè)機(jī)械的作業(yè)要求。

圖8 不同路面環(huán)境下直線路徑跟蹤效果

表4 不同路面環(huán)境下橫向位置偏差統(tǒng)計(jì)

7 結(jié) 論

針對(duì)農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型極點(diǎn)最優(yōu)配置算法中，小角度線性化近似條件對(duì)較大初始位置偏差、較大初始航向偏差路徑跟蹤控制效果的影響，提出了一種預(yù)瞄追蹤模型的路徑跟蹤控制算法。經(jīng)證明該方法可在不同的較大初始位置偏差和航向偏差條件下，快速穩(wěn)定跟蹤規(guī)劃路徑，提高農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)的上線速度、上線穩(wěn)定性和對(duì)復(fù)雜路面的適應(yīng)性。

仿真分析了預(yù)瞄追蹤模型算法的上線和顛簸路面直線路徑跟蹤性能。數(shù)據(jù)顯示：初始航向偏差為0 時(shí)，在不同的初始位置偏差條件下該方法都可以迅速消除橫向偏差，上線曲線平緩無(wú)超調(diào)，表明本文提出的方法具有快速穩(wěn)定的上線性能；在初始航向偏差?15°～15°范圍，初始位置偏差0.15 m范圍內(nèi)，上線時(shí)間、上線距離和超調(diào)量均較小，表明該方法對(duì)于顛簸復(fù)雜路面的直線跟蹤具有很好的魯棒性和適應(yīng)性。

進(jìn)行了實(shí)車的上線和直線路徑跟蹤試驗(yàn)，在初始航向偏差為0，初始位置偏差分別為0.5、1、1.5 m條件下，上線時(shí)間分別為6.8、8.2、9.4 s，上線距離分別為6.73、8.11、9.33 m，超調(diào)量分別為5.2、7.0、8.5 cm；顛簸不平旱地路面直線路徑跟蹤的最大誤差為4.23 cm，誤差絕對(duì)值的平均值為1 cm，標(biāo)準(zhǔn)差為1.25 cm。試驗(yàn)結(jié)果表明本文提出的控制方法具有較好的上線和直線路徑跟蹤效果，根據(jù)經(jīng)驗(yàn)可滿足農(nóng)業(yè)機(jī)械的導(dǎo)航作業(yè)要求。

[1] 胡靜濤，高雷，白曉平，等. 農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航技術(shù)研究進(jìn)展[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2015，31(10)：1－10.

Hu Jingtao, Gao Lei, Bai Xiaoping, et al. Review of research on automatic guidance of agricultural vehicles[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(10): 1－10. (in Chinese with English abstract)

[2] 姬長(zhǎng)英，周俊. 農(nóng)業(yè)機(jī)械導(dǎo)航技術(shù)發(fā)展分析[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2014，45(9)：44－54.

Ji Changying, Zhou Jun. Current situation of navigation technologies for agricultural machinery[J]. Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(9): 44－54. (in Chinese with English abstract)

[3] O’Connor M L. Carrier-phase Differential GPS for Automatic Control of Land Vehicles[D]. Stanford: Stanford University, 1997.

[4] 陳軍，朱忠祥，鳥(niǎo)巢諒，等. 拖拉機(jī)沿曲線路徑的跟蹤控制[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2006，22(11)：108－111.

Chen Jun, Zhu Zhongxiang, Torisu R, et al. On-tracking control of tractor running along curved path[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2006, 22(11): 108－111. (in Chinese with English abstract)

[5] Zhu Zhongxiang, Chen Jun, Yoshida Toyofumi, et al. Path tracking control of autonomous agricultural mobile robots[J]. Journal of Zhejiang University-Science A, 2007, 8(10): 1596－1603.

[6] 羅錫文，張智剛，趙祚喜，等. 東方紅X-804拖拉機(jī)的 DGPS 自動(dòng)導(dǎo)航控制系統(tǒng)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2009，25(11)：139－145.

Luo Xiwen, Zhang Zhigang, Zhao Zuoxi, et al. Design of DGPS navigation control system for Dong fang hong X-804 tractor[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2009, 25(11): 139－145. (in Chinese with English abstract)

[7] 劉兆朋，張智剛，羅錫文，等.雷沃ZP9500高地隙噴霧機(jī)的GNSS自動(dòng)導(dǎo)航作業(yè)系統(tǒng)設(shè)計(jì)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2018，34(1)：15－21.

Liu Zhaopeng, Zhang Zhigang, Luo Xiwen, et al. Design of automatic navigation operation system for Lovol ZP9500 high clearance boom sprayer based on GNSS[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2018, 34(1): 15－21. (in Chinese with English abstract)

[8] 李逃昌，胡靜濤，高雷，等. 一種與行駛速度無(wú)關(guān)的農(nóng)機(jī)路徑跟蹤方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2014，45(2)：59－65.

Li Taochang, Hu Jingtao, Gao Lei, et al. Agricultural machine path tracking method irrelevant to travel speed[J]. Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(2): 59－65. (in Chinese with English abstract)

[9] 白曉平，胡靜濤，高雷，等. 農(nóng)機(jī)導(dǎo)航自校正模型控制方法研究[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2015，46(2)：1－7.

Bai Xiaoping, Hu Jingtao, Gao Lei, et al. Self-tuning model control method for farm machine navigation[J]. Transactions of the Chinese Society for Agricultural Machinery, 2015, 46(2): 1－7. (in Chinese with English abstract)

[10] 陳軍，鳥(niǎo)巢諒，朱忠祥，等. 拖拉機(jī)在牧草地上自動(dòng)引導(dǎo)行走的控制[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2005，36(7)：104－107.

Chen Jun, Torisu R, Zhu Zhongxiang, et al. Study on automatic guidance for tractor on grassland[J]. Transactions of the Chinese Society for Agricultural Machinery, 2005, 36(7): 104－107. (in Chinese with English abstract)

[11] Bevly D M, Gerdes J C, Parkinson B W. A new yaw dynamic model for improved high speed control of a farm tractor[J]. ASME Journal of Dynamic Systems, Measurement, and Control, 2002, 124(4): 659－667.

[12] Bevly D M. High Speed, Dead Reckoning, and Towed Implement Control for Automatically Steered Farm Tractors Using GPS [D] .Stanford: Stanford University, 2001.

[13] Eaton R, Pota H, Katupitiya J. Path tracking control of agricultural tractors with compensation for steering dynamics[C]//Proceedings of the 48th IEEE Conference on Decision and Control. Shanghai, P.R. China, 2009: 7357－7362.

[14] Eaton R, Katupitiya H, Pota H, et al. Robust sliding mode control of an agricultural tractor under the influence of slip[C]//Proceedings of 2009 IEEE/ASME International Conference on Advanced Intelligent Mechatronics: 1873－1878.

[15] Zhang Q, Qiu H. A dynamic path search algorithm for tractor automatic navigation [J]. Transactions of the ASAE, 2004, 47(2): 639－646.

[16] Qiu H. Navigation Control for Autonomous Tractor Guidance[D]. Urbana-Champaign: University of Illinois at Urbana-Champaign, 2002.

[17] 馮雷. 基于GPS和傳感技術(shù)的農(nóng)用車輛自動(dòng)導(dǎo)航系統(tǒng)的研究[D]. 杭州：浙江大學(xué)，2004.

Feng Lei. Study on Navigation System for Off-Road Vehicle Guidance based on GPS and Sensor Technology[D]. Hangzhou: Zhejiang University, 2004. (in Chinese with English abstract)

[18] 孫木楠，孫懷江. 純追蹤模型研究[J]. 機(jī)器人，2001,23(增刊1)：36－39.

Sun Munan, Sun Huaijiang. A study on pure prusuit model[J]. Robot, 2001,23(Supp.1) : 36－39.

[19] 張智剛，羅錫文，趙祚喜，等. 基于Kalman濾波和純追蹤模型的農(nóng)業(yè)機(jī)械導(dǎo)航控制[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2009，40(增刊)：6－12.

Zhang Zhigang, Luo Xiwen, Zhao Zuoxi, et al. Trajectory tracking control method based on Kalman filter and pure pursuit model for agricultural vehicle[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009, 40(Supp.): 6－12. (in Chinese with English abstract)

[20] 黃沛琛，羅錫文，張智剛，改進(jìn)純追蹤模型的農(nóng)業(yè)機(jī)械地頭轉(zhuǎn)向控制方法[J]. 計(jì)算機(jī)工程與應(yīng)用，2010，46(21)：216－219.

Huang Peichen, Luo Xiwen, Zhang Zhigang. Control method of headland turning based on improved pure pursuit model for agricultural machine[J]. Computer Engineering and Applications, 2010, 46(21): 216－219. (in Chinese with English abstract)

[21] 連世江，陳軍，賈海政，等. 基于模糊控制的拖拉機(jī)轉(zhuǎn)向跟蹤控制研究[J]. 西北農(nóng)林科技大學(xué)學(xué)報(bào)：自然科學(xué)版，2009，37(9)：224－228.

Lian Shijiang, Chen Jun, Jia Haizheng, et al. Heading following control of tractor based on fuzzy control[J]. Journal of Northwest A＆F University: Natural Science Edition, 2009, 37(9): 224－228. (in Chinese with English abstract)

[22] 周建軍，張漫，汪懋華，等. 基于模糊控制的農(nóng)用車輛路線跟蹤[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2009，40(4)：151－156.

Zhou Jianjun, Zhang Man, Wang Maohua, et al. Path tracking for agricultural vehicle based on fuzzy control[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009, 40(4): 151－156. (in Chinese with English abstract)

[23] 陳軍，朱忠祥，鳥(niǎo)巢諒，等. 基于神經(jīng)網(wǎng)絡(luò)的農(nóng)用車輛自動(dòng)跟蹤控制[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2007，38(5)：131－133.

Chen Jun, Zhu Zhongxiang, Torisu R, et al. Automatic on-tracking control of farm vehicle based on neural network[J]. Transactions of the Chinese Society for Agricultural Machinery, 2007, 38(5): 131－133. (in Chinese with English abstract)

[24] 白曉鴿，陳軍，朱磊，等. 基于神經(jīng)網(wǎng)絡(luò)的拖拉機(jī)自動(dòng)導(dǎo)航系統(tǒng)[J]. 農(nóng)機(jī)化研究，2010，32(4)：75－77.

Bai Xiaoge, Chen Jun, Zhu Lei, et al. Automatic navigation control system of tractor based on neural network[J]. Journal of Agricultural Mechanization Research, 2010,32(4): 75－77. (in Chinese with English abstract)

[25] 劉兆祥，劉剛，籍穎，等. 基于自適應(yīng)模糊控制的拖拉機(jī)自動(dòng)導(dǎo)航系統(tǒng)[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2010，41(11)：148－152，162.

Liu Zhaoxiang, Liu Gang, Ji Ying, et al. Autonomous navigation system for agricultural tractor based on self-adapted fuzzy control[J]. Transactions of the Chinese Society for Agricultural Machinery, 2010, 41(11): 148－152, 162. (in Chinese with English abstract)

[26] 李逃昌，胡靜濤，高雷，等. 基于模糊自適應(yīng)純追蹤模型的農(nóng)業(yè)機(jī)械路徑跟蹤方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2013，44(1)：205－210.

Li Taochang, Hu Jingtao, Gao Lei, et al. Agricultural machine path tracking method based on fuzzy adaptive pure pursuit model[J]. Transactions of the Chinese Society for Agricultural Machinery, 2013, 44(1): 205－210. (in Chinese with English abstract)

[27] John W, Peake, San F. Method and Apparatus for Automatic Vehicle Guidance Using Continuous 2-D Poly-Point Path: US 813145B2[P]. 2012-05-06.

[28] 郭孔輝. 駕駛員－汽車閉環(huán)系統(tǒng)操縱運(yùn)動(dòng)預(yù)瞄最優(yōu)曲率模型[J]. 汽車工程，1984，6(3)：1－16.

Guo Konghui. Driver-vehicle closed-loop simulation of handling by “preview optimal curvature method”[J]. Automotive Engineering, 1984, 6(3): 1－16. (in Chinese with English abstract)

[29] 高振海. 汽車方向預(yù)瞄式自適應(yīng)PD控制算法[J]. 機(jī)械工程學(xué)報(bào)，2004，40(5)：101－105.

Gao Zhenhai. Vehicle direction preview adaptive PD control algorithm[J]. Chinese Journal of Mechnical Engneering, 2004, 40(5): 101－105. (in Chinese with English abstract)

[30] 明廷友. 智能汽車的軌跡跟隨控制研究[D]. 長(zhǎng)春：吉林大學(xué)，2016.

Ming Tingyou. Research on Trajectory Tracking Control for Intelligent Vehicle[D]. Changchun: Jilin University, 2016. (in Chinese with English abstract)

[31] 張智剛，羅錫文，李俊嶺. 輪式農(nóng)業(yè)機(jī)械自動(dòng)轉(zhuǎn)向控制系統(tǒng)研究[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2005，21(11)：77－80.

Zhang Zhigang, Luo Xiwen, Li Junling. Automatic steering control system of wheeled model farming machinery[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2005, 21(11): 77－80. (in Chinese with English abstract)

[32] 張智剛. 插秧機(jī)的DGPS自動(dòng)導(dǎo)航控制系統(tǒng)研究[D]. 廣州：華南農(nóng)業(yè)大學(xué)，2006.

Zhang Zhigang. Automatic Guidance System Based on DGPS for Rice Transplanter[D]. Guangzhou: South China Agricultural University.

Path tracking control method of agricultural machine navigation based on aiming pursuit model

Wang Hui1, Wang Guimin2※, Luo Xiwen1, Zhang Zhigang1, Gao Yang2, He Jie1, Yue Binbin1

(1.,510642; 2..,,261206,)

Agricultural machine automatic navigation is one of the key technologies in precision agriculture technology system, the in-depth study is important in scientific research, application and social values. In this paper, we investigated the navigation problems in the application of the farm work, including slow on-line speed, bad on-line stability, and poor adaptability of bumpy complex road surface. These problems can be summarized as the speed and stability problems of track tracking in the case of large position deviation or large course deviation. Through the analysis of the work principle and control parameters function of the navigation, a conclusion is made that the correct speed and stability of the position deviation and course deviation of agricultural machinery can be improved by allocating the weight of the position deviation wheel angle decision quantity and course deviation wheel angle decision quantity reasonably. Then in this paper, we developed the navigation control algorithm based on the agricultural machine kinematics model and the pole optimal configuration theory. Because of the small angle linearization of course deviation angle and wheel angleduring the deducing, the control law can achieve good control effect only in the ideal straight path tracking control with small position deviation, course deviation, and wheel angle. Based on this, a path tracking control method of aiming pursuit model for agricultural machine navigation was proposed aiming at improving the on-line speed, stability and adaptability to complex road surface of the automatic navigation system of agricultural machine. In this method, we selected a tracking target point on the planning path of agricultural machine ahead, and tracked the target point by controlling the steering wheel angle. The direction of the agricultural machine vehicle center point to the target point was called as the aiming course. The desired steering angle would be larger when the deviation was larger between the course of agricultural machine and the aiming course, with a rapid correction of aiming course deviation to achieve the goal of fast tracking the target path. On the other hand, the desired steering angle would be smaller when the deviation was smaller between the course of agricultural machine and the aiming course, with a stable tracking to aiming path to achieve the goal of stable tracking the target path. In this model, the steering wheel angle was designed to be K times of the aiming course deviation and K was called as control gain. The length of the projection of the agricultural machine vehicle center to the target point vector on the planning path was named as the preview distance. The control gain and the preview distance were two important parameters that affected the control effect of the model. In this paper, there were three steps to prove the feasibility of the method. The control gain K and the preview distance were set up by referring to the result of the pole optimal configuration method based on the kinematic model of agricultural machine. By comparing two methods formula, the position deviation wheel angle decision quantity had a linear relationship with the position deviation in the kinematics model method and the position deviation wheel angle decision quantity had an inverse tangent function relationship with the position deviation. The inverse tangent function relation was more beneficial to maintain proper weight of position deviation wheel angle decision quantity and course deviation wheel angle decision quantity that would make the path tracking control of agricultural machinery navigation more rapid and stable. Simulation analysis results of aiming pursuit model algorithm in different position and different course deviation showed that the proposed method had a fast and stable path tracking performance and good robustness and adaptability to the navigation path tracking. The test results of agricultural machine showed that the control method proposed in this paper had a good effect in the rapid responsibility and line tracking performance. In the case of 0.5, 1, 1.5 m initial position error, the on-line time was 6.8, 8.2, 9.4 s, respectively, the corresponding travelling distance was 6.73, 8.11 and 9.33 m, respectively and the corresponding overshoot was 5.2, 7.0, 8.5 cm, respectively. The maximum error of straight-line path tracking for bumpy uneven field was not more than 4.23 cm, the mean value of the absolute value of the error was 1 cm, and the standard deviation was 1.25 cm, which satisfied the operation requirements of agricultural machine.

agricultural machinery; models; control; on-line; bumpy pavement; path tracking

王 輝，王桂民，羅錫文，張智剛，高 陽(yáng)，何 杰，岳斌斌. 基于預(yù)瞄追蹤模型的農(nóng)機(jī)導(dǎo)航路徑跟蹤控制方法[J]. 農(nóng)業(yè)工程學(xué)報(bào)，2019，35(4)：11－19. doi：10.11975/j.issn.1002-6819.2019.04.002 http://www.tcsae.org

Wang Hui, Wang Guimin, Luo Xiwen, Zhang Zhigang, Gao Yang, He Jie, Yue Binbin. Path tracking control method of agricultural machine navigation based on aiming pursuit model[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(4): 11－19. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2019.04.002 http://www.tcsae.org

2019-01-27

2019-02-10

國(guó)家重點(diǎn)研發(fā)計(jì)劃項(xiàng)目（2017YFD0700400）；廣東省科技計(jì)劃項(xiàng)目（2016B020205003）

王 輝，博士生，主要從事農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航技術(shù)研究。 Email：scau_wh@163.com。中國(guó)農(nóng)業(yè)工程學(xué)會(huì)會(huì)員：06025802。

王桂民，高級(jí)工程師，主要從事農(nóng)業(yè)裝備的研究。 Email：wangguimin@lovol.com

10.11975/j.issn.1002-6819.2019.04.002

TP273; S24

A

1002-6819(2019)-04-0011-09