插秧機導航路徑跟蹤改進純追蹤算法

李 革 王 宇 郭劉粉 童俊華,2 何 勇

(1.浙江理工大學機械與自動控制學院， 杭州 310018； 2.浙江省種植裝備技術重點實驗室， 杭州 310018；3.浙江大學生物系統工程與食品科學學院， 杭州 310058)

0 引言

隨著智能農業機械的不斷發展，自動導航技術成為研究農業領域的主要熱點之一[1]。而導航控制技術中的重點和難點是路徑跟蹤，路徑跟蹤的效果直接決定導航控制的優劣。因此，開展農業機械導航路徑跟蹤控制算法的研究對于智能農業機械的設計和開發有著重要的意義[2-5]。

國內外學者對導航控制算法做了許多的研究。NAGASAKAN等[6]在地頭以轉彎半徑為固定值的前提下，根據航向偏差來控制決策插秧機進行路徑跟蹤，該方法雖然實現了地頭轉向，但在與直線跟蹤銜接處出現了較大的超調量。KISE等[7]利用三次樣條函數進行基于最小轉彎半徑和最大擺角速率的轉向路徑規劃，設計了兩種地頭轉彎控制方法，盡管仿真結果表明在路徑跟蹤過程中的最大跟蹤誤差小于0.2 m，但該方法采用三次樣條函數對曲線路徑進行規劃，因此，要實現轉向必須對曲線路徑進行跟蹤，控制難度相對較大。羅錫文等[8]把拖拉機的運動模型看作是簡化的二輪車運動模型，設計了直線跟蹤導航PID控制器，控制器參數是通過對該系統進行時域階躍響應仿真分析和田間試驗效果綜合整定的，不適應曲線路徑跟蹤。黃沛琛等[9]采用BP神經網絡實現動態調整前視距離，提出了一種改進純追蹤模型的農業機械地頭轉向控制方法，該方法采用神經網絡控制，需要大量的高質量訓練成本，若僅對有限的農業車輛起始位姿樣本進行訓練，則具有局限性。李逃昌等[10]提出的模糊自適應純追蹤模型的農業機械路徑跟蹤方法的模糊控制規則是根據專家經驗制定的，其跟蹤誤差大，很難快速修正。

本文以井關插秧機為研究平臺，以約翰迪爾Starfire3000型接收機、GS2630型顯示器和ATU200型電動方向盤為主要硬件設備，設計基于GPS技術的改進純追蹤模型的插秧機導航路徑跟蹤控制算法。通過模型仿真和田間試驗相結合的方法，對該導航控制算法進行驗證分析。

1 車輛運動學模型

農業車輛為井關PZ60型插秧機，4輪驅動，全長2.96 m，發動機總功率8.3 kW，前輪液壓助力轉向，無級變速，前輪外徑0.6 m，后輪外徑0.9 m，軸距1.2 m。

圖1 車輛運動學模型Fig.1 Vehicle kinematics model

該車輛模型采用KELLY提出的簡化二輪車運動學模型[11]。在WGS- 84的高斯投影平面坐標系下對該簡化模型進行運動學分析。如圖1所示，δ為前輪轉角，以車輛縱向為基準，偏右為正，偏左為負。θ為車輛航向角，即車輛的縱向逆時針旋轉到y軸的夾角，取值范圍為0°～360°。L為車輛前后兩軸間距，對該模型進行運動學分析可得

式中v——車輛縱向速度

x′(t)——車輛在x軸方向分速度

y′(t)——車輛在y軸方向分速度

θ′(t)——車輛角速度

2 導航控制方法設計

2.1 前視距離的動態調整

前視距離是模仿人工駕駛車輛時向前觀察的距離，研究表明該距離與車輛速度以及前面路徑彎曲程度有關[12-14]。速度越大前視距離越大，彎度越大則前視距離越小，由此可設前視距離為

l=lp+k1v+k2Ω

式中lp——前視距離的基值

k1——速度系數，數值為正

k2——彎度系數，數值為負

Ω——彎度

首先確定預定義路徑，將該路徑以二維數組的形式存儲起來，可表示為(x(n),y(n))。當前車輛的GPS定位點Pr坐標為(x,y)，求取預定義路徑上與車輛當前位置最近的點的方法是通過計算預定義路徑上的每個點與當前車輛坐標點的距離，距離最小的那個點即為車輛當前位置與預定義路徑的最近點Pk坐標為(xk,yk)，由該最近點向后每隔i個點取1個點，再取3個點，由此可得到4個點，分別為Pk、Pk+i、Pk+2i和Pk+3i，其中i為正整數，取值根據前向預測距離而定，如圖2所示。用直線依次連接這4個點，Ω表示前面路徑的彎度，很顯然前面路徑越彎曲，Ω越大。由此可以達到動態調整前視距離的目的。最后根據前視距離可得到車輛的目標點。

圖2 確定彎度的方法Fig.2 Method of determining camber

2.2 導航偏差的設定

導航偏差分為橫向偏差和航向偏差，橫向偏差為車輛當前位置距預定義路徑最近的垂直距離，以車輛在路徑的右邊為正，左邊為負，該距離可以評價導航效果。航向偏差是車輛當前航向與目標航向的夾角，以車輛當前航向相對于目標航向右偏為正，左偏為負，將該值作為路徑跟蹤控制算法的輸入量。如圖3所示，Pr點為當前車輛的位置，以車輛后軸中心點為控制點，A點為在車輛縱向方向上截取適當前視距離所得到的點，Pk+j是預定義路徑上距A點最近的點，也就是目標點，則向量PrPk+j為目標方向，φ為航偏角，d為橫向偏差。

圖3 偏差的確定方法Fig.3 Method of determining deviations

2.3 路徑跟蹤控制算法的設計

在大量的理論和試驗研究的基礎上，設計了一種簡單有效的路徑跟蹤控制算法，其基本原理是利用上述得出的航向偏差作為控制器的輸入量，建立控制算法的幾何模型，理論推導出前輪的轉角公式，將前輪轉角作為控制器的輸出量，另外由于橫向偏差是衡量導航效果優劣的量，因此將橫向偏差作為觀測調節量，當橫向偏差過大時，給輸出量一個補償值，使橫向偏差快速減小。

具體算法是將預定義路徑以二維數組的形式存儲于車載工控機(GS2630型顯示器)中，由車載GPS接收機實時接收衛星定位信號和基站的差分定位信號，由此可解算出插秧機當前的位置坐標，工控機將該位置坐標與預定義路徑數組信息進行比對，通過計算得出距離當前車輛定位點最近的預定義路徑上的點，即能夠得出橫向偏差，再通過上述方法得出合適的前視距離，由前視距離來獲得路徑跟蹤的目標點，從而確定目標方向，將該信息與電子羅盤采集到的當前航向信息進行對比能夠得出航向偏差，將航向偏差作為輸入量輸入路徑跟蹤算法控制器中，同時將橫向偏差作為觀測量，最后輸出前輪轉角。該輸出值作為下位機的輸入量，計算后得出步進電動機的轉角，由步進電動機帶動方向盤轉動，進而帶動轉向機構轉動，從而達到預計的前輪轉角。

如圖4所示，Pr為當前車輛的位置點，Pk+j為目標點，R為轉彎半徑，C為圓心點，φ為航偏角，則在直角三角形BPrC中可得

(1)

由于φ為圓C的弦切角，因此可得

∠PrCPk+j=2φ

(2)

在等腰三角形PrCPk+j中，由正弦定理可得

(3)

式中lPrPk+j——點Pr與點Pk+j間的距離

由式(1)～(3)可得

(4)

從式(4)可以看出，由輸入量φ即可求得前輪轉角。該角度求出后輸入到下位機中，轉換成指令，由轉向系統完成轉向動作，從而實現路徑跟蹤。另外，式(4)是從幾何角度分析得出的，理論上證明可行，但該式不包含橫向偏差，因此需要將其進行改進。在設計控制器時[15-19]，將橫向偏差作為觀測量進行實時監測，如果橫向偏差過大，即當橫向偏差d≥dr時，則將前輪轉角δ乘以一個系數kdr(kdr>0)，由此可以增大前輪轉角，使其快速到達目標點，通過仿真和試驗得出：當dr=0.02、kdr=1.08時，導航效果最佳。這樣就能夠使車輛快速減小橫向誤差，提高導航效果，由于該方法在推導中并未對預定義路徑提出要求，因此這種算法既可實現直線路徑跟蹤，也可實現曲線路徑跟蹤，適應性較強。

圖4 改進純追蹤算法模型Fig.4 Improved pure tracking algorithm model

3 仿真

在Matlab/Simulink環境下，以兩輪車運動學模型為控制對象，對上述路徑跟蹤控制方法建立仿真模型[20]，由于兩輪車運動模型是在理想的條件下建立的，即不考慮側滑因素，因此需要加一個擾動模型，對車輛的位置進行干擾。該模型主要由5部分組成，分別是車輛模型、擾動模型、偏差求解、導航算法和轉向系統。將插秧機的速度設定為1 m/s，初始航向偏差為5°，初始橫向偏差為0.1 m，初始位置坐標為(0.9,1)，仿真時間為18 s，仿真步長為0.1 s。

通過Matlab/Simulink搭建系統仿真模型，如圖5所示。

圖5 路徑跟蹤仿真框圖Fig.5 Simulation block diagram of path tracking

預定義路徑如圖6所示，該路徑是井關PZ60型插秧機作業時的標準路徑，其轉彎半徑為0.9 m。

圖6 預定義路徑Fig.6 Predefined path tracking

從圖6可以看出，插秧機在田間作業時行走的路徑包括直線段和轉彎曲線段，因此要實現其完全自動導航，其控制算法必須滿足既能實現直線路徑跟蹤，又能實現曲線路徑跟蹤。為了清楚地表達跟蹤效果，利用Matlab編寫了路徑跟蹤效果圖界面，如圖7所示。

圖7 導航控制效果圖界面Fig.7 Navigation control effect diagram

從圖7右側的偏差波動圖可以看出，在跟蹤的起始階段有0.1 m的橫向偏差，仿真曲線也出現了一定程度的振蕩，但很快便與預定義路徑很好的重合，只有在轉彎的地方出現了少許偏差。在整個仿真路徑跟蹤過程中，橫向偏差基本維持在5 cm以內，而航向偏差除了在轉彎處出現較大的偏差外，基本維持在10°以內。仿真結果表明：該導航控制算法從理論上是可行的。

4 實車試驗

4.1 試驗與數據采集

試驗車為經過改造的井關PZ60型插秧機，GPS接收機采用約翰迪爾生產的GreenStar3000型接收機，該接收機集成了位置傳感器、速度傳感器和陀螺儀傳感器[21]，采樣頻率為5 Hz，數據通過CAN總線發送到控制器中。控制器使用約翰迪爾公司自主研發的GS2630型控制器，該控制器內集成3種導航模式，分別是：SF1模式，精度為±33 cm；SF2模式，精度為±10 cm；RTK模式，精度為±2 cm。前兩種導航模式只需1個GPS接收機即可，而最后一種導航模式需要2個GPS接收機，其中一個為基站。改進純追蹤導航控制算法存儲于計算機內，其含有導航控制軟件，兩者之間通過CAN總線進行數據通訊。自動方向盤采用約翰迪爾公司生產的AutoTrac200型轉向套件，將其安裝在井關PZ60型插秧機上，通過計算機操作即可實現導航控制。

改裝后的插秧機如圖8所示。通過CAN記錄儀采集數據，實時保存導航過程中產生的數據，主要包括車輛的位置、速度、航向、橫向偏差和航向偏差等，以便在試驗結束后對導航效果進行分析。

圖8 試驗插秧機Fig.8 Experimental transplanter

試驗在農場水田中進行，有較多的水，泥腳深度約為20 cm。首先人工駕駛插秧機沿標記好的路徑慢速行走，利用車載GPS接收機記錄路徑數據，將該數據修正后作為自動導航的預定義路徑。然后將插秧機駕駛到數據記錄的起始位置，開啟自動導航模式，實現無人駕駛。圖9為插秧機在水田中自動導航行駛現場圖。

4.2 數據分析

將自動導航時行駛的路徑數據和預定義路徑數據進行對比，可以得到路徑跟蹤的誤差。圖10為插秧機在水田中自動導航時的數據分析圖。

對比數據分析得出，路徑跟蹤的平均誤差為0.096 m，最大跟蹤誤差為0.183 m，最大跟蹤誤差出現在插秧機轉彎過程中，但直線作業段路徑跟蹤的平均誤差為0.058 m，最大跟蹤誤差為0.135 m，該誤差在插秧機作業允許的誤差范圍內，表明該導航控制算法具有良好的導航效果。綜合轉彎與直線路徑跟蹤數據，平均跟蹤誤差為0.077 m，最大跟蹤誤差為0.159 m。

圖9 插秧機水田自動導航現場圖Fig.9 Automatic navigation scene of transplanter in paddy field

圖10 插秧機水田作業路徑跟蹤試驗結果Fig.10 Experimental results of paddy field operation path tracking for rice transplanter

5 結論

(1)提出通過動態調整前視距離的方法對插秧機進行目標點的確定，這樣能夠減小插秧機在導航過程中的橫向偏差和航向偏差，有利于提高導航精度。

(2)提出了一種改進純追蹤模型的插秧機導航路徑跟蹤算法，該控制算法不僅可以進行直線路徑跟蹤，也可以進行曲線路徑跟蹤，并能適應車輛速度的變化，實現插秧機在田間的智能控制。其轉向靈活，有較好的實時性和魯棒性。

(3)設計的導航控制算法利用航向偏差實現對直線路徑和轉彎曲線路徑的跟蹤，并用橫向偏差作為觀測量實現對控制量的補償。試驗結果表明：當行駛速度為1 m/s時，平均跟蹤誤差為0.077 m，最大跟蹤誤差為0.159 m，滿足插秧機自動導航控制的要求。

1 張美娜，呂曉蘭，陶建平，等. 農用車輛自主導航控制系統設計與試驗[J/OL]. 農業機械學報,2016,47(7):42-47.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20160707&flag=1.DOI:10.6041/j.issn.1000-1298.2016.07.007.

ZHANG Meina, Lü Xiaolan, TAO Jianping, et al. Design and experiment of automatic guidance control system in agricultural vehicle[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2016,47(7):42-47. (in Chinese)

2 REID J F, ZHANG Q, NOGUCHI N, et al. Agriculture automatic guidance research in North America [J]. Computers and Electronics in Agriculture,2000,25(1-2):155-167.

3 TORRI T. Research in autonomous agriculture vehicles in Japan [J]. Computers and Electronics in Agriculture, 2000,25:155-167.

4 張聞宇，丁幼春，王雪玲，等. 基于SVR逆向模型的拖拉機導航純追蹤控制方法[J/OL]. 農業機械學報,2016,47(1):30-35.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20160105&flag=1.DOI:10.6041/j.issn.1000-1298.2016.01.005.

ZHANG Wenyu, DING Youchun, WANG Xueling, et al. Purepursuit control method based on SVR inverse-model for tractor navigation [J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016,47(1): 30-35.(in Chinese)

5 田海清，應義斌，張方明. 農業車輛導航系統中自動控制技術的研究進展[J]. 農業機械學報，2005,36(7)：148-152.

TIAN Haiqing, YING Yibin, ZHANG Fangming. Development of automatic control technology for agricultural vehicle guidance[J]. Transactions of the Chinese Society for Agricultural Machinery, 2005,36(7): 148-152.(in Chinese)

6 NAGASAKAN Y, SAITO H, TAMAKI K, et al. An autonomous rice transplanter guided by global positioning system and inertial measurement unit [J]. Journal of Field Robotics, 2009,26(6-7): 537-548.

7 KISE M, NOGUCHI N, ISHII K, et al. Development of the agricultural autonomous tractor with an RTK- GPS and a FOG[C]∥IFAC: Proceedings of the 4th IFAC Symposium on Intelligent Autonomous Vehicles, 2001: 103-106.

8 羅錫文，張智剛，趙祚喜，等. 東方紅X- 804拖拉機的DGPS自動導航控制系統[J]. 農業工程學報，2009,25(11): 139-145.

LUO Xiwen, ZHANG Zhigang, ZHAO Zuoxi, et al. Design of DGPS navigation control system for Dongfanghong X- 804 tractor [J]. Transactions of the CSAE, 2009,25(11): 139-145. (in Chinese)

9 黃沛琛，羅錫文，張智剛. 改進純追蹤模型的農業機械地頭轉向控制方法[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 Application, 2010,46(21): 216-219. (in Chinese)

10 李逃昌，胡靜濤，高雷，等. 基于模糊自適應純追蹤模型的農業機械路徑跟蹤方法[J/OL]. 農業機械學報,2013,44(1): 205-210.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20130139&flag=1.DOI:10.6041/j.issn.1000-1298.2013.01.039.

LI Taochang, HU Jingtao, GAO Lei, et al. Agricultural machine path tracking method based on fuzzy adaptive pure pursuit model [J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2013,44(1): 205-210. (in Chinese)

11 張智剛，羅錫文，周志艷，等.久保田插秧機的GPS導航控制系統設計[J]. 農業機械學報,2006,37(7): 95-97.

ZHANG Zhigang, LUO Xiwen, ZHOU Zhiyan, et al. Design of GPS navigation control system for rice transplanter [J]. Transactions of the Chinese Society for Agricultural Machinery, 2006,37(7): 95-97. (in Chinese)

12 周建軍，張漫，汪懋華，等. 基于模糊控制的農用車輛路線跟蹤[J]. 農業機械學報,2009,40(4): 151-156.

ZHOU Jianjun, ZHANG Man, WANG Maohua, et al. Path tracking for agricultural vehicles based on fuzzy control[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009,40(4): 151-156. (in Chinese)

13 熊中剛，賀娟，敖邦乾，等. 基于路徑彎曲度動態預瞄搜索算法的車輛路徑跟蹤控制軟件系統設計[J]. 江蘇農業科學，2015,43(2): 370-373.

14 劉兆祥，劉剛，籍穎，等. 基于自適應模糊控制的拖拉機自動導航系統[J]. 農業機械學報,2010, 41(11):148-152.

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. (in Chinese)

15 薛金林,張順順.基于激光雷達的農業機器人導航控制研究[J/OL].農業機械學報,2014,45(9):55-60.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20140909&flag=1.DOI:10.6041/j.issn.1000-1298.2014.09.009.

XUE Jinlin,ZHANG Shunshun.Navigation of an agricultural robot based on laser radar[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2014,45(9):55-60.(in Chinese)

16 呂安濤，宋正河，毛恩榮. 拖拉機自動轉向最優控制方法的研究[J]. 農業工程學報,2006,22(8): 116 -119.

Lü Antao, SONG Zhenghe, MAO Enrong. Optimized control method for tractors automatic steering [J]. Transactions of the CSAE, 2006,22(8): 116-119. (in Chinese)

17 丁永前，王致情，林相澤，等. 自主跟隨車輛航向控制系統[J/OL].農業機械學報,2015,46(1):8-13.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20150102&flag=1.DOI:10.6041/j.issn.1000-1298.2015.01.002.

DING Yongqian,WANG Zhiqing,LIN Xiangze, et al.Heading control system of autonomous following vehicle[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2015,46(1):8-13. (in Chinese)

18 趙德安,賈偉寬,張云,等.農業機器人自主導航改進自適應濾波控制器研究[J/OL].農業機械學報,2015,46(5):1-6.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20150501&flag=1.DOI:10.6041/j.issn.1000-1298.2015.05.001.

ZHAO Dean,JIA Weikuan,ZHANG Yun, et al.Design of agricultural robot autonomous navigation control based on improved self-adaptive filter[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2015,46(5):1-6.(in Chinese)

19 張聞宇,丁幼春,廖慶喜,等.拖拉機液壓轉向變論域模糊控制器設計與試驗[J/OL].農業機械學報,2015,46(3):43-50.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20150307&flag=1.DOI:10.6041/j.issn.1000-1298.2015.03.007.

ZHANG Wenyu,DING Youchun,LIAO Qingxi,et al.Variable universe fuzzy controller for tractor hydraulic steering[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2015,46(3):43-50.(in Chinese)

20 遲德霞，任文濤，由佳翰，等. 水稻插秧機導航控制器設計與路徑追蹤仿真研究[J]. 沈陽農業大學學報，2016，47(3)：363-367.

CHI Dexia, REN Wentao,YOU Jiahan, et al.Design on navigation controller and path tracking simulation of rice transplanter [J]. Journal of Shenyang Agricultural Uinversity, 2016, 47(3): 363-367. (in Chinese)

21 張鐵民,李輝輝,陳大為,等.多源傳感器信息融合的農用小車路徑跟蹤導航系統[J/OL].農業機械學報,2015,46(3):37-42.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20150306&flag=1.DOI：10.6041/j.issn.1000-1298.2015.03.006.

ZHANG Tiemin,LI Huihui,CHEN Dawei, et al.Agricultural vehicle path tracking navigation system based on information fusion of multi source sensor[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2015,46(3):37-42. (in Chinese)