基于預瞄追蹤模型的農機導航路徑跟蹤控制方法

王 輝，王桂民，羅錫文，張智剛，高 陽，何 杰，岳斌斌

?

基于預瞄追蹤模型的農機導航路徑跟蹤控制方法

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

（1. 華南農業大學南方農業機械與裝備關鍵技術教育部重點實驗室，廣州 510642；2. 雷沃重工股份有限公司，濰坊 261206）

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

農業機械；模型；控制；上線；顛簸路面；路徑跟蹤

0 引 言

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

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

針對基于農機運動學模型控制方法中小角度線性化的缺點以及大田作業對農機自動導航系統的實際要求，本文提出一種預瞄追蹤模型的路徑跟蹤控制方法。參考農機運動學模型小角度線性化極點最優配置算法的證明過程，將本文方法的路徑跟蹤過程分成3個階段進行證明。通過仿真和試驗，驗證了該算法的有效性，可以滿足農業機械大田作業要求。

1 導航工作原理及控制參數作用分析

農機的自動導航控制主要是對農機進行橫向位置和航向偏差控制，控制農機跟蹤預先設置好的作業路徑，使其與設置路徑之間的橫向偏差和航向偏差保持在一定的精度范圍之內，從而滿足農業生產的需要。

現代農機自動導航系統一般由檢測單元、控制單元、執行單元及監控單元四部分構成。檢測單元負責實時、精準地獲取農機當前的位置與姿態信息；控制單元負責根據農機當前的位姿信息和目標路徑信息，按照某種控制策略計算出轉向角度的期望值；執行單元的作用是驅動農機轉向輪由當前值轉動至輪角期望值；監控單元是導航系統的人機交互界面，主要功能是導航狀態監視，系統參數設定和導航控制任務管理[1]。

控制單元是導航系統的核心，路徑跟蹤控制方法的選擇和參數整定是提高農機自動導航精度和穩定性的關鍵。假設基于位置偏差和航向偏差設計期望輪角的二元線性控制律

式中θ為期望車輪轉角，rad；P為車體與期望路徑（當前要跟蹤的規劃路徑）的位置偏差，m；ψ為車體與期望路徑的航向偏差，rad；為位置偏差系數；為航向偏差系數；1為位置偏差輪角決策量，rad；2為航向偏差輪角決策量，rad。

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

綜上所述，農機處于不同的位置偏差或航向偏差時，合理配置位置偏差輪角決策量和航向偏差輪角決策量的權重，可有效提高農機位置偏差和航向偏差的校正速度和穩定性。實際農機導航應用中的上線問題可歸結為初始位置偏差較大的直線路徑跟蹤問題；顛簸復雜農田路面造成農機出現甩頭擺尾滑移等問題可歸結為不確定因素的農機位置偏差或航向偏差瞬間變大問題。合理配置位置偏差輪角決策量和航向偏差輪角決策量的權重，可以提高農機自動導航系統的上線速度、上線穩定性和對復雜路面的適應性。

2 基于農機運動學模型的路徑跟蹤方法

2.1 農機運動學模型小角度線性化方法

農機運動學模型是導航控制算法設計的基礎，本研究采用的雷沃M904-D拖拉機是一種前輪轉向、四輪驅動的輪式農業機械。如不考慮拖拉機行走時的側偏和地面狀況等因素的影響時，基于拖拉機二輪轉向的特點，可將其簡化為二輪車模型，如圖1所示。在導航平面坐標系下得到農機運動學微分方程，如式（4）～（5）所示。

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

式中為時間，s。

直線路徑跟蹤中車輪轉角和航向偏差ψ為小角度值（一般≤5°），故可對方程進行線性化，得到二輪轉向拖拉機運動學微分方程的狀態空間矩陣形式

根據方程（6）可設計全狀態反饋的二元線性直線路徑跟蹤控制律

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

將式（7）帶入式（6）中即可得到閉環控制系統的狀態空間方程

利用極點配置思想，將此系統的極點放置在一個理想的位置以獲得最佳的系統響應。為了求出極點，可利用該系統的特征方程為0，即令

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

計算行列式得出

假設系統所需的理想極點為，則有[27]

比較式（10）和（11）2個等式，令相等的冪次方項系數相等，可得

試驗證明，典型二輪轉向農機車輛中，當=?1時，系統表現出良好的響應效果。因此，可得到二輪轉向車輛直線路徑跟蹤的轉向角計算公式[27]如下

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

2.2 影響農機運動學模型方法控制效果因素分析

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

3 預瞄追蹤模型路徑跟蹤方法

3.1 預瞄追蹤模型方法描述

預瞄追蹤模型根據農機駕駛員在進行路徑跟蹤時的轉向操作規律建立。一個合格的駕駛員對農機操作發出的控制轉向指令都是遵循一定原則的。這個原則的主要目標是使農機的運動盡可能地與設定的軌跡一致[28-30]。在直線跟蹤時，駕駛員預瞄農機前方路徑規劃直線上的一點，通過控制車輪轉角跟蹤該點。當農機方向和預瞄方向偏差較大時，駕駛員對車輪轉角進行大角度控制，以快速校正預瞄航向偏差跟蹤預瞄路徑；當農機方向和預瞄方向偏差較小時，駕駛員對車輪轉角進行小角度控制，以穩定地跟蹤預瞄路徑。

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

由式（16）～（19）得

注：ψt為目標航向角，rad；ψc為車體航向角，rad；ψa為預瞄航向角，rad；φe為預瞄航向偏差角，rad；d為前視距離，m；P1為農機中心點；P2為預瞄追蹤點；A、B為規劃作業行1上的兩點。

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

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

上線后，農機進入規劃作業行直線的路徑跟蹤階段，控制規則仍然通過控制車輪轉角跟蹤預瞄直線12行駛，而此時預瞄直線12和作業規劃行直線基本重合，說明此控制律可以通過控制農機跟蹤預瞄直線12從而進行規劃作業行直線的路徑跟蹤。

3.3 農機預瞄追蹤模型的證明

將農機的路徑跟蹤過程分成3個階段，第一階段：消除初始導航階段的大預瞄航向偏差；第二階段：農機沿預瞄方向行駛漸進上線；第三階段：沿作業規劃行的直線路徑跟蹤。

第一階段，是一個大角度的航向跟蹤問題，張智剛等設計了目標航向跟蹤的PID校正環節，并進行了仿真，90°航向跟蹤過程中，轉向控制系統動態響應的上升時間大約在4 s以內[31-32]。說明基于航向偏差的PID控制方法對運動中的農機具有較好的大角度航向跟蹤能力。

第二階段，農機沿著預瞄追蹤直線12行駛，由于此階段預瞄航向偏差角φ和車輪轉角為小角度，將預瞄追蹤直線12作為輔助直線，本階段可看做是農機對預瞄追蹤直線12的直線跟蹤。采用農機運動學模型小角度線性化極點最優配置推導方法進行推導，此時農機車輪轉角可引用二輪轉向車輛直線路徑跟蹤的期望轉向角計算式（15），此時的航向偏差為農機車體當前航向ψ和預瞄航向ψ的偏差即預瞄航向偏差φ，位置偏差為農機中心點1到預瞄跟蹤直線12的距離P=0（因為點1在直線12上）。第二階段農機的最優車輪轉角計算公式為

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

由式（21）、（22）可得

對比預瞄追蹤模型車輪轉角決策式（20）和式（23）可知，取控制增益

此時兩式相同，第二階段農機預瞄追蹤模型公式得證。

第三階段，農機上線對行后，進行直線段的路徑跟蹤。此時位置偏差很小，故有

結合第二階段中的取值，此時可將預瞄追蹤模型車輪轉角決策式（20）簡化為

將式（26）與農機運動學模型小角度線性化公式（15）對比可知，取前視距離的大小等于2時，此時兩式相同，從而說明預瞄追蹤模型算法在直線跟蹤中有良好的路徑跟蹤效果。

綜上，可得出當控制增益2，前視距離的值等于2時，農機預瞄追蹤模型算法可在農機的上線和直線路徑跟蹤階段得到良好的控制效果。

4 運動學和預瞄兩種方法的比較

比較運動學模型極點最優算法計算式（15）和預瞄追蹤模型算法計算式（23），兩式的不同之處在于位置偏差輪角決策量不同，運動學模型公式中位置偏差輪角決策量1M

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

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

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

位置偏差輪角決策量隨位置偏差的增大呈線性增加顯然是不合適的，因為位置偏差增大到一定程度時，航向偏差輪角決策量將失去作用（因航向偏差變動范圍為（?p/2，p/2）），導致農機大初始位置偏差路徑跟蹤時的失穩或失控。預瞄追蹤模型中，航向偏差輪角決策量范圍和位置偏差輪角決策量范圍均為（?p/2，p/2），這有益于位置偏差輪角決策量和航向偏差輪角決策量保持合適的權重，不論農機相對規劃路徑處于什么樣的位姿，導航決策出的輪角始終有益于農機跟蹤預瞄直線12，從而快速穩定追蹤目標直線。

農田實際導航應用中的上線問題可歸結為初始位置偏差較大的直線跟蹤問題，顛簸復雜農田路面造成農機出現甩頭擺尾滑動等問題可歸結為不確定因素的農機位置偏差或航向偏差瞬間變大問題。以上2個問題均難以避免和預測，提高路徑跟蹤速度和穩定性是解決以上2個問題的途徑之一。

根據上述分析，預瞄追蹤模型方法不論農機相對規劃路徑處于何種位姿，導航決策出的輪角始終有益于農機跟蹤預瞄直線，從而快速穩定追蹤目標直線。因此，采用預瞄追蹤模型算法可提高農機自動導航系統的上線速度、上線穩定性和對復雜路面的適應性。

5 預瞄追蹤模型的仿真研究

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

實際中，上線時的初始位置偏差一般在1.5 m之內，顛簸復雜路面直線路徑跟蹤時的位置偏差波動范圍在±0.15 m范圍之內，航向偏差波動在±15°以內。

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

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

表1 前視距離為1.5 m時上線性能仿真數據統計

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

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

表2 復雜路面直線跟蹤性能仿真數據統計

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

由表2可知，基于預瞄追蹤模型方法的顛簸路面直線跟蹤，在出現小位置偏差、大航向偏差情況下，由于農機很短時間內來不及調整車身方向，可能會出現小幅超調。但從總體上看，在不同的初始位置偏差和航向偏差條件下都能夠迅速消除橫向位置偏差和航向偏差，進而穩定地跟蹤目標直線，說明預瞄追蹤模型方法在一定程度上可以提高農機對復雜農田的適應性。

6 試驗驗證及結果討論

6.1 驗證平臺

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

圖5 農機自動導航系統結構

圖6 裝備農機自動導航系統的M904-D拖拉機

6.2 基于本文方法的農機上線試驗

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

試驗過程為：在試驗路面上通過高精度GNSS采集、兩點，將由其確定的直線作為農機跟蹤的目標路徑。啟動農機并開啟導航系統，通過導航系統的整體行偏移功能，將農機平移至設定的初始位置偏差位置，清除整體行偏移數值，則農機在路徑一端的初始位置偏差處上線。當位置偏差穩定在0.03 m內時，通過人機交互界面發送停止導航命令，結束一次上線試驗。在初始航向偏差為0、初始位置偏差為1 m時的路徑跟蹤效果如圖7所示，不同初始位置偏差條件下上線試驗數據統計結果如表3所示。

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

表3 不同初始位置偏差上線性能數據統計

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

6.3 基于本文方法不同路況的直線路徑跟蹤試驗

為了驗證本文方法對復雜農田的魯棒性和適應性較好，分別在水泥路面和玉米秋收后顛簸不平的旱地路面2種路況環境下進行了直線路徑跟蹤試驗，試驗在新疆額敏縣團結農場進行。試驗中，設定前視距離=1.5 m，速度=1 m/s。

試驗過程為：在試驗地塊上采集、兩點，將由其確定的直線作為農機跟蹤的目標路徑。然后在路徑線附近啟動農機，并啟動導航控制系統，進行自動路徑跟蹤控制，當農機接近路徑的另一端時，通過人機界面發送停止導航命令，結束一次直線路徑跟蹤試驗。在水泥路面和顛簸旱地路面的直線路徑跟蹤效果如圖8所示，試驗數據統計結果如表4所示。

試驗數據分析：對預瞄追蹤模型直線路徑跟蹤的橫向位置偏差均值、偏差絕對值的極值、偏差絕對值的均值和標準差進行了統計分析。偏差的均值反映了導航系統的系統誤差，偏差絕對值的極值和偏差絕對值的均值反映了導航控制的精度，標準差反映了導航控制的穩定性。

由表4可知，預瞄追蹤模型路徑跟蹤控制算法在水泥路面上時，導航控制偏差的極值為2.66 cm，偏差絕對值的平均值為0.54 cm，標準差為0.67 cm。在顛簸旱地環境下，導航控制偏差的極值為4.23 cm，偏差絕對值的平均值為1 cm，標準差為1.25 cm。本文算法在水泥路面上表現出良好的控制效果，控制精度高，穩定性好；隨地況條件的下降，控制效果下降，但本文算法依然表現出良好的路況適應性，在顛簸旱地路面導航控制精度依然較高，穩定性良好。基于預瞄追蹤模型算法的直線路徑跟蹤精度高，對復雜農田地況的魯棒性和適應性較好。考慮到顛簸地面不平對GNSS定位精度造成一定的影響，這樣的跟蹤精度和穩定性是可以接受的，滿足農業機械的作業要求。

圖8 不同路面環境下直線路徑跟蹤效果

表4 不同路面環境下橫向位置偏差統計

7 結 論

針對農機運動學模型極點最優配置算法中，小角度線性化近似條件對較大初始位置偏差、較大初始航向偏差路徑跟蹤控制效果的影響，提出了一種預瞄追蹤模型的路徑跟蹤控制算法。經證明該方法可在不同的較大初始位置偏差和航向偏差條件下，快速穩定跟蹤規劃路徑，提高農機自動導航系統的上線速度、上線穩定性和對復雜路面的適應性。

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

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

[1] 胡靜濤，高雷，白曉平，等. 農業機械自動導航技術研究進展[J]. 農業工程學報，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] 姬長英，周俊. 農業機械導航技術發展分析[J]. 農業機械學報，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] 陳軍，朱忠祥，鳥巢諒，等. 拖拉機沿曲線路徑的跟蹤控制[J]. 農業工程學報，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拖拉機的 DGPS 自動導航控制系統[J]. 農業工程學報，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高地隙噴霧機的GNSS自動導航作業系統設計[J]. 農業工程學報，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] 李逃昌，胡靜濤，高雷，等. 一種與行駛速度無關的農機路徑跟蹤方法[J]. 農業機械學報，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] 白曉平，胡靜濤，高雷，等. 農機導航自校正模型控制方法研究[J]. 農業機械學報，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] 陳軍，鳥巢諒，朱忠祥，等. 拖拉機在牧草地上自動引導行走的控制[J]. 農業機械學報，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和傳感技術的農用車輛自動導航系統的研究[D]. 杭州：浙江大學，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]. 機器人，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濾波和純追蹤模型的農業機械導航控制[J]. 農業機械學報，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]. 計算機工程與應用，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]. 西北農林科技大學學報：自然科學版，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] 周建軍，張漫，汪懋華，等. 基于模糊控制的農用車輛路線跟蹤[J]. 農業機械學報，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] 陳軍，朱忠祥，鳥巢諒，等. 基于神經網絡的農用車輛自動跟蹤控制[J]. 農業機械學報，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]. 農機化研究，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] 劉兆祥，劉剛，籍穎，等. 基于自適應模糊控制的拖拉機自動導航系統[J]. 農業機械學報，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] 李逃昌，胡靜濤，高雷，等. 基于模糊自適應純追蹤模型的農業機械路徑跟蹤方法[J]. 農業機械學報，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] 郭孔輝. 駕駛員－汽車閉環系統操縱運動預瞄最優曲率模型[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] 高振海. 汽車方向預瞄式自適應PD控制算法[J]. 機械工程學報，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]. 長春：吉林大學，2016.

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

[31] 張智剛，羅錫文，李俊嶺. 輪式農業機械自動轉向控制系統研究[J]. 農業工程學報，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] 張智剛. 插秧機的DGPS自動導航控制系統研究[D]. 廣州：華南農業大學，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

王 輝，王桂民，羅錫文，張智剛，高 陽，何 杰，岳斌斌. 基于預瞄追蹤模型的農機導航路徑跟蹤控制方法[J]. 農業工程學報，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

國家重點研發計劃項目（2017YFD0700400）；廣東省科技計劃項目（2016B020205003）

王 輝，博士生，主要從事農業機械自動導航技術研究。 Email：scau_wh@163.com。中國農業工程學會會員：06025802。

王桂民，高級工程師，主要從事農業裝備的研究。 Email：wangguimin@lovol.com

10.11975/j.issn.1002-6819.2019.04.002

TP273; S24

A

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