基于履帶式聯合收獲機轉向特性的局部跟蹤路徑規劃

何永強 周 俊 袁立存 鄭彭元 梁子安

(南京農業大學工學院，南京 210031)

0 引言

農業機械自動導航控制是實現無人生產作業的關鍵技術，優化農機在非結構化環境條件下的跟蹤控制方法是近年來的研究熱點，對提高作業質量和作業效率具有重要意義[1-4]。

履帶式車輛與地面接觸面積大，對耕作層土壤破壞小、通過性好，其中履帶式聯合收獲機被廣泛應用于水田收獲作業，并逐步步入無人化階段。導航控制中履帶式與輪式車輛轉向原理不同，輪式車輛的轉向控制參數與轉向輪偏角數學關系明確，轉向控制頻率越高，跟蹤軌跡越平滑緊湊，越有利于農機通過連續小角度近似線性調整糾偏上線[5-9]。而履帶式聯合收獲機底盤行走系統通常采用變速箱加靜液壓無級變速器(Hydro static transmission，HST)，依靠多片式摩擦離合器單邊制動轉向，轉向半徑與摩擦制動程度有關，不易通過理論計算獲得轉向控制參數與實際轉向半徑之間的關系[10]，適用于履帶式車輛轉向特性的導航路徑跟蹤技術難題還未得到有效解決[11]。鑒于此，國內外學者針對履帶式聯合收獲機轉向控制參數與實際運動特性之間的關系進行了深入研究。NOGUCHI團隊[12-13]通過田間試驗建立了行走、轉向控制參數與轉向角速度之間的關系，利用履帶式聯合收獲機運動狀態方程解算到達目標位置所需的轉向控制量。關卓懷[14]基于最小二乘支持向量機(LS-SVM)提出在線識別履帶式聯合收獲機轉向控制模型，通過動態擬合實際轉向率和占空比的函數關系修正轉向控制模型，利用周期性圓弧-切線交替循環行進策略實施路徑跟蹤控制。上述學者通過離線或在線的方式獲得了轉向控制參數與實際轉向運動特性之間的關系，并利用小周期多段圓弧平滑銜接進行路徑跟蹤，提高了路徑跟蹤精度。但履帶式車輛位姿調整控制過程中，轉向半徑變化頻率較高，易加劇離合器結合沖擊[15-16]，且會引起液壓系統振蕩沖擊，進而導致液壓油路、管件加速損壞，不利于控制系統的穩定[17-19]。作業時收獲機沉陷、滑移、液壓系統時滯等現象導致高頻轉向激勵信號無法使車輛及時穩定響應預設動作，進而導致控制超調、系統響應滯后等影響跟蹤精度的問題[20-22]。因此，傳統小周期追蹤控制方法在單邊制動轉向履帶式農機的實際生產應用中還存在一定的局限性，研發適用于農機轉向特性的低頻導航控制方法對提高導航精度和系統穩定性具有重要意義。

針對上述問題，本文提出一種基于履帶式聯合收獲機轉向特性的預瞄-切線局部跟蹤路徑動態規劃算法，通過構建履帶式聯合收獲機跟蹤控制試驗平臺進行田間試驗研究，驗證該跟蹤控制算法的有效性，以期為履帶式農機導航跟蹤控制提供參考。

1 局部跟蹤路徑規劃

1.1 預瞄-切線局部跟蹤路徑動態規劃算法原理

局部跟蹤路徑是農機按照一定的跟蹤策略，規劃從當前位置趨近期望路徑的行駛路線，合理的局部跟蹤路徑可極大提高農機的導航精度和穩定性。預瞄-切線法動態規劃的跟蹤路徑由平滑連接的兩段弧線組成，第1段圓弧由收獲機當前位姿與1/2橫向偏差線上的預瞄點確定，第2段圓弧由收獲機在1/2橫向偏差線的實際位姿與期望路徑的相切幾何關系確定。如圖1所示，在車體坐標系xOy下收獲機當前位置為點O，y軸正方向為收獲機前進方向，x軸正方向指向車體右側，期望路徑為直線N1M2，線段ON1長度為初始橫向偏差de1；射線Oy為收獲機當前航向，OE1與N1M2平行，初始航向偏差為θe1；點G為ON1的中點，1/2橫向偏差線GE2平行于期望路徑N1M2。dmin為允許橫向偏差。

圖1 預瞄-切線局部跟蹤路徑規劃

第1次轉向利用預瞄點快速逼近期望路徑：預瞄點M1在1/2橫向偏差線GE2上，線段OM1長度為前視距離Ld；設轉向圓弧的圓心為點Oc1，第1次轉向半徑為Rc1。收獲機通過第1次轉向到達預瞄點M1(設實際到達位置為點M′1)，此時直線M′1H為收獲機當前航向，航向偏差為θe2。橫向偏差為線段M′1N2長度即de2。若de2

圖2 局部跟蹤路徑規劃及導航控制算法流程圖

1.2 跟蹤弧線半徑確定

廣泛應用于車輛局部跟蹤路徑規劃的純追蹤模型是一種基于車輛運動學模型的模擬人工駕駛行為的幾何計算方法，通過計算車輛位姿與期望路徑之間的數學關系得到車輛逼近預瞄點的弧線路徑[23-25]，如圖3所示。

圖3 純追蹤模型幾何示意圖

車體坐標系xOy下，1/2橫向偏差線為NM，預瞄點M坐標為(x′,y′)，線段OM長度為前視距離Ld；線段ON長度為橫向偏差de1/2；OE與NM平行，航向偏差為θe1；點Oc為轉向圓弧的圓心，線段OOc長度為轉向半徑Rc1，線段OcQ的長度為l。規定橫向偏差de偏左為負、偏右為正，航向偏差θe偏左為負、偏右為正。由圖3中的幾何關系可知

(1)

由式(1)得第1段圓弧轉向半徑為

(2)

由式(2)可知跟蹤路徑圓弧半徑Rc1由前視距離Ld、橫向偏差de1和航向偏差θe1共同決定。由圖1易知第2次轉向后，軌跡圓弧與期望路徑相切，可得第2段圓弧轉向半徑為

(3)

收獲機逆時針轉向時的轉向半徑Rc1、Rc2為正，順時針轉向時為負。

1.3 前視距離動態調整

模糊控制器對非線性系統控制和專家經驗知識的表達效果顯著，尤其在復雜農田作業場景中應用廣泛[26]。為了確定聯合收獲機在不同位姿狀態的前視距離，本文以橫向偏差和航向偏差作為模糊控制器的輸入，前視距離作為輸出。橫向偏差de基本論域為[-0.6 m，0.6 m]，量化等級為{-1，-0.5，0，0.5，1}={NB，NS，ZO，PS，PB}，量化因子為5/3；航向偏差θe基本論域為[-20°，20°]，量化等級為{-1，-0.5，0，0.5，1}={NB，NS，ZO，PS，PB}，量化因子為1/20；前視距離基本論域為[1 m，5 m]，量化等級為{0.2，0.4，0.6，0.8，1}={S，M，Z，L，BL}，量化因子為1/5。采用三角隸屬度函數對變量進行模糊化，利用加權平均法進行反模糊化。模糊控制規則如表1所示，模糊控制曲面如圖4所示。

表1 模糊控制規則

圖4 模糊控制曲面

1.4 跟蹤路徑切線圓弧半徑確定

圖5 局部跟蹤路徑規劃策略

(4)

1.5 仿真對比分析

為驗證本研究提出的預瞄-切線局部跟蹤路徑動態規劃算法的控制效果，利用Matlab 2020b軟件分別對本算法和傳統純追蹤算法的上線過程進行仿真對比分析。設期望路徑為直線Y=0，收獲機位姿狀態更新方程為

(-Ri-1)

(5)

式中i——收獲機姿態序號

Xi——第i個姿態位置橫坐標

Yi——第i個姿態位置縱坐標

θei——第i個姿態航向偏差

Ri-1——第i-1次的轉向半徑

v——前進速度

T——轉向控制周期

為便于比較，前視距離Ld均設置為2.5 m，前進速度v為1.0 m/s，結合收獲機轉向控制瞬態響應特性設置轉向控制周期T為1.0 s，收獲機狀態觀測周期為0.2 s。當橫向偏差滿足條件de(i-1)dei≤0則認為收獲機上線，程序結束運行。分別進行兩組仿真對比試驗，記錄收獲機上線時的姿態信息，試驗結果如圖6和表2所示。

表2 仿真試驗結果

仿真試驗結果表明，初始位姿條件為X0=-0.5 m、Y0=0 m、θe0=-15°以及X0=-1.0 m、Y0=0 m、θe0=5°時，預瞄-切線法只需轉向2次，轉向控制次數比傳統算法減少50%以上，收獲機上線時的橫向偏差和航向偏差均小于傳統純追蹤算法。仿真結果表明，預瞄-切線局部跟蹤路徑動態規劃算法原理上可行，滿足跟蹤精度高和轉向控制次數低的要求，可以用于導航路徑跟蹤控制。

2 轉向控制模型構建

2.1 轉向控制模型構建方法

自動導航系統根據路徑跟蹤策略計算得出的速度控制參數V和轉向控制參數K，分別等效于收獲機手動操作模式下，前進操作手柄和轉向操作手柄產生的模擬量電壓信號值。工控機通過CAN通訊的方式，將控制參數傳輸給車載控制器作為輸入信號對電控液壓系統實施控制，分別實現行走和轉向動作。控制參數相同條件下，履帶式農機在不同地況條件下的沉陷、滑轉、滑移程度均有較大差異，尤其在地況參數未知的情況下，無法直接建立與農機轉向特性相適應的控制模型。當局部跟蹤路徑確定后，獲得適應于收獲機轉向特性的轉向控制參數尤為關鍵。

為了得到轉向控制參數K與轉向半徑R、前進控制參數V之間的關系，本研究在同一地塊土壤物理特性相近的前提條件下，通過田間試驗的方法獲得上述參數之間的數學關系，建立與收獲機實際轉向特性相適應的轉向控制模型。前期單因素試驗發現，轉向控制參數K與轉向半徑R呈自然對數函數關系(V=6 813 mV)，如圖7所示。

圖7 轉向控制參數與轉向半徑的關系

為探究收獲機實際前進速度v與前進控制參數V之間的關系，收獲機保持直行狀態下進行單因素試驗，每組重復3次，結果取平均值，試驗結果如圖8所示。分別利用線性函數、二次函數、三次函數和對數函數進行最小二乘擬合，擬合決定系數R2分別為0.970 7、0.970 7、0.993 4、0.963 7，在前進控制參數有效范圍內，三次函數擬合精度最高，擬合方程為

圖8 前進速度與前進控制參數的關系

v=-4.629×10-11V3+9.84×10-7V2-

0.006 53V+13.874

(6)

當前進速度一定時，履帶與地面存在滑移、滑轉現象，車輛實際轉向半徑大于理論轉向半徑，且隨著前進速度的增大，轉向半徑也隨之增大。因此，收獲機導航跟蹤路徑圓弧確定后，實際轉向控制時需要通過改變轉向控制值減小轉向半徑，補償因側滑導致的曲率減小量[27]。設收獲機前進速度控制參數與轉向控制參數呈線性關系，綜合上述分析構建轉向控制數學模型

K(R,V)=alnR+bRV+cV+d

(7)

式中a、b、c、d——轉向控制模型系數

田間實測數據集合為{(Ki,Ri,Vi)}(i=1,2,…,n)，利用最小二乘法對轉向控制模型曲面方程進行擬合，建立目標函數

(8)

式中n——實測數據總組數

(9)

通過矩陣運算可求解出目標函數的各項系數，從而得到轉向控制數學模型。

2.2 實際轉向半徑測算

轉向過程中，將實時采集到的聯合收獲機在世界大地坐標系(WGS84)下的經緯度位置信息轉換為笛卡爾直角坐標系下的位置坐標(xi，yi)，存儲在動態數組[p(x1，y1)，p(x2，y2)，…，p(xnc，ync)]中，轉向完成后得到nc個坐標數據{(xi,yi)}(i=1,2,…,nc)。利用最小二乘法對離散樣本點進行圓弧擬合，設擬合圓的一般方程為

x2+y2+Ax+Bx+C=0

(10)

式中A、B、C——擬合圓方程系數

轉向半徑R和擬合精度η分別為

(11)

(12)

式中di——樣本觀測點到擬合圓圓心的距離

2.3 試驗平臺構建

為進一步驗證本研究提出的局部路徑跟蹤控制算法的有效性，基于中聯重科PL60(4LZT-6Z)型履帶式聯合收獲機進行電控化改裝，構建了導航路徑跟蹤控制試驗平臺。履帶式聯合收獲機主要技術參數如表3所示。

表3 履帶式聯合收獲機主要技術參數

導航控制系統結構如圖9所示，主要由位姿傳感器、人機交互終端、工控機和車載控制器組成。位姿傳感器由高精度RTK-GNSS定位裝置(P3-DU型，上海華測導航技術股份有限公司，位置精度±(10+1×10-6D)mm，其中D為接收機與基站的距離(km)，航向精度0.2°，測速精度0.03 m/s，數據采集頻率5 Hz)和慣性傳感器(MTi30型，荷蘭Xsens公司，橫滾姿態精度±0.2°)組成，為工控機提供履帶式聯合收獲機位置和姿態信息。人機交互終端用于自動導航作業參數設置和實時顯示收獲機的運行狀態等。工控機(UNO-2484G型，研華科技有限公司)主要用作路徑規劃、自動導航程序的運行和數據記錄保存等。車載控制器通過CAN總線接收工控機發出的控制指令對比例電磁閥進行控制，完成轉向和行走等動作。自動導航系統軟件在Visual Studio 2019集成開發環境中使用C#語言基于Windows窗體應用程序開發。

圖9 導航控制系統結構框圖

3 田間試驗

3.1 轉向運動特性試驗

為進一步驗證本文算法在田間實際跟蹤的實用性，在2021年11月利用試驗平臺進行了轉向性能試驗和直線導航跟蹤控制對比試驗，試驗場地為蘇州市臨湖農業專業合作社平整稻茬田，稻茬平均高度9.87 cm，土壤含水率為51.22%，堅實度為612.84 kPa，試驗現場如圖10所示。

圖10 履帶式聯合收獲機田間試驗

試驗前，利用人機交互終端設定轉向控制參數K和行走控制參數V，為了避免收獲機啟動前進時的抖動誤差，令收獲機直行3 s運行平穩后再進行自動轉向，并實時記錄位置數據。利用2.2節方法對轉向過程中的軌跡點進行圓弧擬合和半徑計算。收獲機轉向性能試驗過程中運動軌跡和圓弧擬合結果如圖11所示。

圖11 聯合收獲機運動軌跡及圓弧擬合結果

轉向運動特性的試驗方案和結果如表4所示，通過最小二乘法得到的左轉、右轉控制模型分別為

表4 轉向特性試驗方案和結果

KL(R,V)=285.34lnR-0.055V+2 675.185

(13)

KR(R,V)=-261.04lnR+0.023V+7 675.169

(14)

模型擬合的決定系數R2分別為0.978和0.980，表明擬合效果較好，可以用于收獲機導航路徑跟蹤控制，模型三維曲面如圖12所示。由轉向控制模型可知，轉向控制參數與轉向半徑呈自然對數關系，與前進控制參數呈線性關系。由于加工制造誤差、部件磨損等原因，通常會造成左、右轉向系統存在一定的結構非對稱差異，因此基于收獲機轉向運動特性生成的轉向控制模型在一定程度上可減小控制誤差，更加符合收獲機實際運動規律，有利于提高導航路徑跟蹤精度。

圖12 轉向控制參數與轉向半徑、前進控制參數之間的關系

3.2 直線導航跟蹤對比試驗

分別利用傳統純追蹤算法和本文算法進行25 m直線導航跟蹤試驗，初始位置的橫向偏差為-0.25 m、航向偏差為20°。依據實際作業工況，前進速度設定為0.4 m/s(V=6 765 mV)、0.8 m/s(V=7 696 mV)，允許橫向偏差設置為0.025 m，每組試驗重復3次，第2次直線路徑導航跟蹤試驗效果如圖13、14所示。

圖13 路徑跟蹤偏差對比結果(v=0.4 m/s)

圖14 路徑跟蹤偏差對比結果(v=0.8 m/s)

根據試驗結果，路徑跟蹤過程可歸納為3個階段：第1階段，收獲機橫向偏差較大時，為了快速逼近期望路徑，會以較小的半徑轉向運動，兩種算法此階段的橫向偏差和航向偏差變化幅度均較大；第2階段，當收獲機臨近期望路徑時，其橫向偏差較小，但還存在較大的航向偏差，此階段需要以較小的轉向半徑運動使得橫向偏差變化較小的同時迅速減小航向偏差，以達到上線的目的，本文算法比傳統算法上線效率更高；第3階段，收獲機上線后的橫向偏差和航向偏差均在小范圍內波動，以較大的轉向半徑運動進行微調糾偏，從而達到穩定跟蹤的目的，本文算法比傳統算法跟蹤穩定性更高。

對比兩種算法的直線路徑跟蹤效果，試驗結果均值如表5所示，當前進速度為0.4、0.8 m/s時，本文預瞄-切線算法與傳統純追蹤算法結果相比，橫向偏差的標準差分別減小19.04%、31.30%，航向偏差的標準差分別減小25.94%、9.16%，轉向控制次數分別減少47.22%、42.86%。表明本文提出的預瞄-切線算法在導航精度和控制過程的穩定性均優于傳統算法。且本文算法的轉向控制頻率較傳統算法大幅降低，對減小制動部件磨損、降低液壓系統振蕩沖擊、延長農機使用壽命等有明顯優勢。

表5 路徑跟蹤試驗結果對比

4 結論

(1)為降低履帶式聯合收獲機導航路徑跟蹤控制頻率和提高控制系統的穩定性，提出了一種預瞄-切線局部跟蹤路徑動態規劃算法，該算法控制思路簡潔清晰，易實現。

(2)通過分析轉向控制參數、前進控制參數和轉向半徑之間的關系，基于收獲機實際轉向運動特性建立了轉向控制數學模型，左轉、右轉控制模型擬合的決定系數R2分別為0.978、0.980。轉向控制參數與轉向半徑呈自然對數關系，與前進控制參數呈線性關系。

(3)構建了導航路徑跟蹤控制試驗平臺，田間試驗表明：利用本文算法進行跟蹤控制，當前進速度為0.4、0.8 m/s時，橫向偏差的標準差分別為0.048 9、0.050 7 m，航向偏差的標準差分別為3.94°、4.66°，轉向控制次數分別為19、12次；與傳統純追蹤算法相比，橫向偏差的標準差分別減小19.04%、31.30%，航向偏差的標準差分別減小25.94%、9.16%，轉向控制次數分別減少47.22%、42.86%。說明本文提出的預瞄-切線局部跟蹤路徑規劃算法整體優于傳統純追蹤算法，有效降低了轉向控制頻率，且提高了路徑跟蹤精度和控制系統的穩定性。