拖拉機自動駕駛系統上線軌跡規劃方法

賈　全　張小超　苑嚴偉　趙　博　偉利國　付　拓

(中國農業機械化科學研究院， 北京 100083)

(i=1,2,…,m-3)

0　引言

拖拉機自動駕駛系統是當前農業機械智能化領域不可或缺的一部分[1-3]，具備自主行駛能力的農業車輛不僅有助于提高勞動生產率，而且有利于解決農村勞動力不足的問題。

路徑規劃算法[4-5]是自動駕駛系統的關鍵技術，當前的研究熱點大多集中在田間全局路徑規劃[6]以及地頭轉彎模式上，實現方法有幾何法、人工勢場法[7-8]、A*算法[9-10]、神經網絡法[11]以及蟻群算法[12]等。關于上線軌跡規劃的研究相對較少。因此，現有拖拉機自動駕駛系統只能依靠內置的路徑跟蹤算法[13-15]從車輛當前位置出發逼近全局路徑上的目標上線點，整個上線過程行駛距離較長，且在目標點處會出現震蕩，跟蹤誤差較大。拖拉機駕駛員為了提高上線精度，在上線過程中只能依靠手動駕駛，自動化程序較低。

為解決該問題，本文在現有研究基礎上，綜合考慮車輛運動學約束條件，對拖拉機從起始位置到全局路徑目標點的自動上線軌跡開展相關研究。

1　上線軌跡描述

如圖1所示，本文將全局路徑上的起始作業位置定義為上線點；將拖拉機從其初始位置出發行駛至全局路徑上線點的過程定義為上線過程，上線過程行駛過的距離稱為上線距離；將上線過程中拖拉機經過的一系列離散位置點序列s=(Sstart，S1，S2，…，Sn，Sgoal)定義為上線軌跡。圖1中，Sgoal是全局路徑的起始作業點，也是上線軌跡的目標點。

圖1　拖拉機上線軌跡示意圖Fig.1　Schematic diagram of tractor guided trajectory

本文把上線軌跡規劃問題定義為：在滿足多種非線性約束條件的軌跡簇si中尋找一條距離最短的上線軌跡作為最優目標路徑。函數描述為

(1)

式中soptimal——最優目標路徑

d——第i條軌跡各點間距

(2)

(3)

φ=arctan(Lκ)

(4)

式中L——拖拉機軸距

2　軌跡規劃算法

2.1　上線軌跡約束

2.1.1最大曲率約束

為保證上線軌跡具有可跟蹤性，需要對軌跡曲率進行限制。曲率與拖拉機轉彎半徑的關系式為

(5)

式中R——轉彎半徑

圖2所示為簡化二輪車模型[17]，拖拉機最小轉彎半徑主要受軸距和轉向前輪的最大轉向角限制，其數值是固定的，計算公式為

(6)

式中Rmin——最小轉彎半徑

φmax——前輪最大轉向角

圖2　簡化二輪車模型Fig.2　Simplified two-wheeled model

結合式(3)、(5)、(6)可知，上線軌跡各點處的曲率均須滿足約束

(7)

2.1.2起止點航向約束

為避免拖拉機在上線軌跡的起始時刻出現原地轉向或震蕩現象，需要對上線路徑的初始點航向進行約束。當路徑點Si=Sstart時，約束條件為

|θs-θv|≤ε

(8)

式中θs——路徑航向θv——車身航向

ε——航向閾值(取較小的正數)

為了保證拖拉機上線軌跡與全局路徑結合處的平滑度，提高上線點Sgoal處的路徑跟蹤精度，當路徑點Si=Sgoal時，須滿足約束

|θs-θg|≤ε

(9)

式中θg——目標點航向

2.1.3轉向角約束

拖拉機前輪轉向范圍決定了其路徑跟蹤的靈活性，如果上線軌跡某點處等效前輪轉角過大，該點處的路徑將不具備可跟蹤性。因此，需要對等效前輪轉角進行約束。由式(3)、(4)可得到等效前輪轉角，建立約束條件為

(10)

實際上線作業過程中，為保證路徑跟蹤精度，一般將拖拉機行駛速度控制在3 km/h以內。經測試，在該速度條件下只要規劃的上線軌跡滿足上述約束條件，拖拉機轉向系統均可有效跟蹤期望轉角信號。因此，本文上線軌跡規劃算法未考慮轉向角速度及角加速度約束。

2.2　B樣條曲線

軌跡的表示方法有很多種，文獻[18]提到了7種曲線表示方法，并對它們的優缺點進行了對比分析。曲線最終形狀方便控制是B樣條的優點之一，為了改變B樣條曲線的形狀，可以修改一個或多個控制參數，如控制點的位置、節點位置或曲線的次數。另外，B樣條曲線分段組成，修改某一控制點只引起與該控制點相鄰的曲線形狀發生變化，而遠處的曲線形狀不受影響。

B樣條函數本身的特性[19-21]使其非常適合應用于路徑規劃場合，因此本文選用B樣條曲線進行上線軌跡規劃。

2.2.1B樣條曲線定義

B樣條曲線表達式為[14]

(11)

其中

Fk,n(u)=

(12)

式中n——B樣條曲線階次

Si,n(u)——對應節點u的第i段曲線上的點

Fk,n(u)——n次B樣條基函數

Pi+k——第i+k個控制點

其中，節點u∈[0,1]；設控制點個數為m，則i=1,2，…，m-n，表示整條曲線由m-n段B樣條曲線平滑連接而成，每段曲線由n+1個控制點生成。

由式(10)可知上線軌跡至少要保證2階可導，為滿足約束條件且降低計算量，本文選用3次B樣條曲線，即n=3。此時，B樣條基函數為

(13)

其中

式中u1、u2、u3——節點u的1、2、3次冪

將式(13)代入式(11)可知，上線路徑及其1、2階導數分別為

(14)

(15)

(16)

其中

P=[PiPi+1Pi+2Pi+3]T

(i=1,2,…,m-3)

式中，Pi～Pi+3是用于計算第i段B樣條方程的4個控制點。

2.2.2樣條曲線控制點確定

當樣條曲線的控制點較少時不易求得滿足約束的期望路徑，控制點過多則會造成方程求解困難，增加運算時間。根據經驗采用6個控制點P0～P5生成滿足約束條件的三階B樣條上線軌跡。

現有文獻大多采用優化算法直接求取N個控制點[16,22]，對于6個控制點P(x，y)需要優化的參數為12個，當使用迭代方法求解時運算量非常大。為了提高軌跡規劃速度，有必要研究曲線約束與控制點間的關系以簡化計算。

對于三階B樣條曲線，當3個相鄰的控制點在同一條直線上且間距相等時，生成的B樣條曲線與直線相切于中間的控制點，如圖3所示。

圖3　B樣條曲線控制點關系Fig.3　B-spline control points relationship

依據上線路徑初始點的航向角約束，選擇控制點P0、P1、P2。當3個控制點滿足如下條件，即可滿足拖拉機初始狀態約束：選定拖拉機初始位置為P1；P0、P1、P2三點共線；矢量P0P2的方向角與拖拉機初始航向一致；P0和P2關于P1對稱。

同理，依據目標點航向約束，確定控制點P3、P4、P5，并選定P4為目標點位置坐標。

設線段P0P1和P1P2的長度為l1，線段P3P4和P4P5的長度為l2。依據起止點航向約束選定控制點P1和P4后，只要確定l1和l2的數值即可求得其它控制點并生成上線軌跡，從而把最優軌跡規劃問題轉變為求l1和l2的最優解問題，相比求解12個參數，該方法只需要優化2個參數，有效減少了算法的運算量。

2.3　基于量子遺傳算法的參數優化

本文采用量子遺傳算法求解l1和l2，量子遺傳算法[22](Quantum genetic algorithm，QGA)是遺傳算法和量子計算結合產生的一種新的概率進化算法，比傳統遺傳算法具有更快的收斂速度和并行處理能力。

將l1、l2作為個體進行尋優，把上線軌跡的距離取負值作為適應度函數(軌跡越短，適應度值越大)，按照以下算法流程循環執行直至達到結束條件：

(1)初始化種群Q(t0)。

(2)對初始化種群Q(t0)中的每個個體(即l1和l2)進行一次測量，得到對應的確定解(上線軌跡距離)。

(3)對各個確定解進行適應度(軌跡距離取負值)評估。

(4)記錄最優個體和對應的適應度，以最優個體作為下一代進化目標。

(5)判斷是否滿足結束條件，是則退出，否則繼續計算。

(6)對種群Q(t)中的每個個體實施一次測量，得到對應的確定解。

(7)對各個確定解進行適應度評估。

(8)量子旋轉門更新種群，得到下一代種群Q(t+1)。

(9)記錄最優個體和對應的適應度。

(10)將迭代次數加1，返回步驟(5)。

3　仿真分析

3.1　仿真實例設計

設拖拉機模型采用前輪轉向，軸距為2.2 m，前輪轉角范圍為[-π/6，π/6]，單位為rad。由2.1節所述方法建立上線軌跡約束模型。

將拖拉機初始位置Pstart定義為(0，0)，初始航向角θstart定義為0 rad。圖4中Pgoal是目標點位置，θgoal是目標點航向角，r是拖拉機初始位置和目標點的相對距離，θr是矢量PstartPgoal的方向角。

圖4　工況初始狀態示意圖Fig.4　Schematic diagram of initial state of conditions

為了使仿真實例更具代表性，設計4種作業工況。取θr初值為π/4，按照π/2等間隔分布；令θgoal與θstart的方向角分別相差0、π/4、3π/4、π，各工況目標點位置分布和航向如圖4所示，具體參數見表1。初始工況1～4分別代表以下4類作業工況：

初始工況1：|θr-θstart|∈[0，π/2]，|θgoal-θstart|∈[0，π/2)；初始工況2：|θr-θstart|∈(π/2，π]，|θgoal-θstart|∈[0，π/2)；初始工況3：|θr-θstart|∈(π/2，π]，|θgoal-θstart|∈[π/2，π]；初始工況4：|θr-θstart|∈[0，π/2]，|θgoal-θstart|∈[π/2，π]。

表1　4種初始作業工況參數Tab.1　Four initial operating conditions parameters

結合初始狀態描述和2.2.2節內容得各控制點計算方法如表2所示，其中相對距離r取20 m，l1和l2采用2.3節所述量子遺傳算法求解，為了盡可能找到全局解，取種群規模為60，優化代數為200。

表2　控制點計算方法Tab.2　Control points calculation method

3.2　仿真結果分析

按照仿真實例描述在Matlab軟件環境中編程實現各工況條件下上線軌跡規劃，采用量子遺傳算法優化得到各工況控制點分布如表3所示，上線軌跡仿真結果如圖5～9所示。

(1)量子遺傳算法優化過程見圖5。從圖5可知，4種工況條件下目標函數均可在40代左右迭代收斂到最佳值。l1、l2最優解及相應上線軌跡長度如表4所示，對比各初始狀態及上線軌跡長度可知，在相對距離r相同情況下4類作業工況所需最小上線距離由小到大排序為工況1、工況4、工況2、工況3。

表3　4種作業工況軌跡控制點Tab.3　Trajectory control points for four operating conditions　m

圖5　量子遺傳算法優化過程Fig.5　Quantum genetic algorithm optimization process

圖6　工況1上線軌跡規劃結果Fig.6　Guided trajectory planning results of case 1

圖7　工況2上線軌跡規劃結果Fig.7　Guided trajectory planning results of case 2

圖8　工況3上線軌跡規劃結果Fig.8　Guided trajectory planning results of case 3

圖9　工況4上線軌跡規劃結果Fig.9　Guided trajectory planning results of case 4

(2)各工況上線軌跡規劃結果如圖6～9所示。由圖6～9中的圖a和表4可知，4種工況條件下，均可通過優化配置B樣條控制點實現上線軌跡規劃；由圖6～9中的圖b可知，上線軌跡各點處曲率連續且均不大于0.26 m-1，上線初始點和目標點處曲率接近零，有利于提高起止點路徑跟蹤效果。

(3)由圖6～9中的圖c可知，拖拉機沿上線軌跡行駛時，最大等效前輪轉角均小于0.52 rad；等效前輪轉角在初始時刻接近于零，有利于避免車輛原地轉向情況；轉角變化較為平緩，有利于實現控制器對期望角度的跟蹤。

表4　l1、l2最優解及上線軌跡長度Tab.4　Optimal solution and guidance trajectory length　m

(4)由圖6～9中的圖d可知，車身航向在上線過程中平滑過渡，軌跡在起始點處的航向均在零附近，與起始點的真實航向接近；目標點處的航向與仿真實例中設計的目標點航向0、π/4、3π/4、π接近，滿足起止點航向約束條件。

4　田間試驗

4.1　試驗設計

田間試驗在中國農業機械化科學研究院北京農機試驗站進行。選用雷沃M1004型拖拉機作為試驗車輛，選用自主研發的BDLeader-301型農業機械導航系統作為控制平臺，試驗現場及平臺硬件組成如圖10所示。

圖10　裝配BDLeader-301型導航系統的M1004型拖拉機Fig.10　M1004 tractor equipped with BDLeader-301 autopilot system

圖11　田間試驗設計Fig.11　Field trial design

由仿真結果可知，在第3類工況條件下上線距離較長，曲率和航向變化較大，路徑跟蹤相比其它幾類工況較為復雜。因此，設計了圓形全局路徑，并針對第3類工況條件下的上線軌跡規劃方法開展田間對比試驗研究，如圖11所示。分別基于純追蹤算法[23]和上線軌跡規劃方法完成上線過程，拖拉機上線行駛速度為3 km/h。

試驗流程如圖12所示。由于試驗田塊限制，把圓形路徑的半徑設為10 m，拖拉機初始位置到目標點的距離r設為6.5 m，以拖拉機到達全局路徑終點作為終止條件。

圖12　田間試驗流程Fig.12　Flow chart of field test

圖13　田間試驗對比Fig.13　Comparison results of field test

4.2　試驗結果分析

田間試驗上線軌跡對比如圖13所示。從圖13可知，上線過程中基于上線軌跡規劃方法的跟蹤距離相對較短，實際計算結果為23.7 m；單獨采用純追蹤算法跟蹤效果較差，實測跟蹤軌跡長度為78.6 m。

圖14和圖15對比給出了前輪轉角的變化曲線。圖中橫軸表示拖拉機導航系統在自動跟蹤上線軌跡和全局路徑過程中，上位機軟件對拖拉機前輪角度的離散采樣點，縱軸表示拖拉機前輪轉角。由圖14可知，單獨采用純追蹤算法時，拖拉機前輪轉角在目標上線點附近出現了較大幅度的擺動，這會導致車身震蕩，影響該位置的路徑跟蹤精度和乘坐舒適性；當增加上線軌跡規劃方法后，前輪轉角在上線過程中變化連續，可以平穩地過渡到跟蹤全局路徑所期望的前輪轉角，如圖15所示。

圖14　純追蹤算法前輪轉角變化曲線Fig.14　Changing curve of front wheel angle by pure pursuit algorithm

圖15　上線軌跡規劃算法前輪轉角變化曲線Fig.15　Changing curve of front wheel angle by guided trajectory planning algorithm

圖16對比了2種上線跟蹤方法在全局路徑上的橫向跟蹤誤差，誤差定義見文獻[23]。圖中橫坐標表示拖拉機導航系統從全局路徑起始位置自動跟蹤至全局路徑終止位置過程中，上位機軟件對橫向跟蹤偏差的采樣時間，縱坐標表示橫向跟蹤誤差。從圖16可知，單獨采用純追蹤算法在全局路徑初始階段的跟蹤誤差較大，基于上線軌跡方法在全局路徑上的跟蹤結果相對平穩。

圖16　跟蹤誤差對比Fig.16　Comparison result of tracking errors

5　結論

(1)針對現有拖拉機自動導航系統在上線過程中的不足，提出了一種基于量子遺傳算法優化的B樣條理論上線軌跡規劃方法。綜合考慮最大曲率約束、起止點航向約束和最大轉向角約束，把最優軌跡規劃問題轉換為B樣條控制點參數優化問題，將上線距離作為優化目標，利用量子遺傳算法建立了期望軌跡函數。

(2)根據拖拉機上線作業初始狀態的不同，設計了4種典型作業工況驗證上線軌跡規劃算法。仿真結果表明：基于量子遺傳算法優化的B樣條理論可以求得滿足多個非線性約束條件的期望軌跡，軌跡曲率變化連續，有利于路徑跟蹤控制器進行曲線跟蹤控制。

(3)針對第3類作業工況，開展了田間對比試驗，試驗結果表明：相比當前拖拉機自動駕駛系統上線模式，基于量子遺傳算法優化的B樣條理論上線軌跡規劃方法有助于減小上線距離，提高了全局路徑的導航控制精度。

1ENGLISH A, ROSS P，BALL D, et al. Vision based guidance for robot navigation in agriculture[C]∥2014 IEEE International Conference on Robotics & Automation(ICRA), 2014:1693-1698.

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

LUO X W, ZHANG Z G, ZHAO Z X, 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)

3偉利國，張權，顏華，等. XDNZ630型水稻插秧機GPS自動導航系統[J]. 農業機械學報，2011，42(7)：186-190.

WEI Liguo, ZHANG Quan, YAN Hua, et al. GPS automatic navigation system design for XDNZ630 rice transplanter [J]. Transactions of the Chinese Society for Agricultural Machinery, 2011,42(7): 186-190.(in Chinese)

4GALCERAN E, CARRERAS M. A survey on coverage path planning for robotics[J]. Robotics and Autonomous Systems, 2013, 61(12): 1258-1276.

5HAMEED I A. Intelligent coverage path planning for agricultural robots and autonomous machines on three-dimensional terrain[J]. Journal of Intelligent & Robotic Systems, 2014, 74(3-4): 965-983.

6劉剛,李笑,康熙,等.基于GNSS的農田平整自動導航路徑規劃方法[J/OL].農業機械學報,2016,47(增刊):21-29.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=2016s004&journal_id=jcsam. DOI：10.6041/j.issn.1000-1298.2016.S0.004.

LIU Gang,LI Xiao,KANG Xi,et al. Automatic navigation path planning method for land leveling based on GNSS[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2016,47(Supp.):21-29.(in Chinese)

7師五喜, 李福榮, 常紹平, 等. 基于遞階模糊系統的人工勢場法機器人路徑規劃[J]. 天津工業大學學報, 2014, 33(6): 45-50.

SHI W X, LI F R, CHANG S P, et al. Robot path planning of artificial potential field method based on hierarchical fuzzy system[J]. Journal of Tianjin Polytechnic University, 2014, 33(6): 45-50.(in Chinese)

8LI G, TAMURA Y, YAMASHITA A, et al. Effective improved artificial potential field-based regression search method for autonomous mobile robot path planning[J]. International Journal of Mechatronics and Automation, 2013, 3(3): 141-170.

9彭松,賈陽.月球車全局路徑規劃中的A*算法改進[J]. 航天器工程, 2010(4): 80-85.

10黃辰, 費繼友, 劉洋, 等.基于動態反饋A*蟻群算法的平滑路徑規劃方法[J/OL].農業機械學報,2017,48(4):34-40，102. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20170404&journal_id=jcsam. DOI：10.6041/j.issn.1000-1298.2017.04.004.

HUANG Chen, FEI Jiyou, LIU Yang, et al. Smooth path planning method based on dynamic feedback A*ant colony algorithm[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2017,48(4):34-40,102.(in Chinese)

11NOGUCHI N, TERAO H. Path planning of an agricultural mobile robot by neural network and genetic algorithm[J]. Computers and Electronics in Agriculture, 1997, 18(2-3): 187-204.

12ZHOU K, JENSEN A L, S?RENSEN C G, et al. Agricultural operations planning in fields with multiple obstacle areas[J]. Computers and Electronics in Agriculture, 2014, 109: 12-22.

13胡靜濤, 高雷, 白曉平, 等. 農業機械自動導航技術研究進展[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 CSAE, 2015, 31(10): 1-10.(in Chinese)

14孟慶寬，仇瑞承，張漫，等. 基于改進粒子群優化模糊控制的農業車輛導航系統[J/OL]. 農業機械學報，2015，46(3)：29-36，58. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no= 20150305&journal_ id=jcsam. DOI:10.6041/j.issn.1000-1298.2015.03.005.

MENG Qingkuan, QIU Ruicheng, ZHANG Man, et al. Navigation system of agricultural vehicle based on fuzzy logic controller with improved particle swarm optimization algorithm[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2015, 46(3): 29-36,58.(in Chinese)

15白曉平，胡靜濤，高雷，等. 農機導航自校正模型控制方法研究[J/OL]. 農業機械學報，2015，46(2)：1-7. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20150201&journal_id=jcsam.DOI:10.6041/j.issn.1000-1298.2015.02.001.

BAI Xiaoping, HU Jingtao, GAO Lei,et al. Self-tuning model control method for farm machine navigation[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2015, 46(2): 1-7.(in Chinese)

16李紅, 王文軍, 李克強. 基于 B 樣條理論的平行泊車路徑規劃[J]. 中國公路學報, 2016, 29(9): 143-151.

17SNIDER J M. Automatic steering methods for autonomous automobile path tracking[D]. Pittsburgh: Carnegie Mellon University, 2009.

18GALLINA P, GASPARETTO A. A technique to analytically formulate and to solve the 2-dimensional constrained trajectory planning problem for a mobile robot[J]. Journal of Intelligent & Robotic Systems, 2000, 27(3): 237-262.

19de BOOR C. A practical guide to splines[M]. New York: Springer-Verlag, 1978.

20COX M G. The numerical evaluation of B-splines[J]. IMA Journal of Applied Mathematics, 1972, 10(2): 134-149.

21DIERCKX P. Curve and surface fitting with splines[M]. Oxford:Oxford University Press, 1995.

22TAO Jin, SUN Qinglin, ZHU Erlin, et al. Quantum genetic algorithm based homing trajectory planning of parafoil system[C]∥Control Conference (CCC), 2015 34th Chinese. IEEE, 2015: 2523-2528.

23ZHANG Q, QIU H. A dynamic path search algorithm for tractor automatic navigation[J]. Transactions of the ASAE, 2004, 47(2): 639-646.