基于模糊Stanley模型的農機全田塊路徑跟蹤算法研究

崔冰波 孫 宇 吉 峰 魏新華 朱永云 章少岑

(1.江蘇大學農業工程學院， 鎮江 212013； 2.江蘇大學現代農業裝備與技術教育部重點實驗室， 鎮江 212013)

0 引言

農業機械(簡稱農機)智能化是無人農場和智慧農業的重要支撐，而自動導航是智能化農機的核心技術[1]。農機自動導航為疫情防控和糧食安全提供了重要保障。路徑跟蹤算法控制農機沿預定軌跡實現自主行駛，是農機田間無人化自主作業的重要環節[2-3]。

農業機械常用路徑跟蹤控制方法包括PID控制、純追蹤控制、Stanley控制、神經網絡控制和最優控制等[4-6]。BORAH等[7]提出一種基于滾動時域的改進純追蹤控制算法，解決農機高速作業下跟蹤效果不理想問題。MOGENS等[8]基于線性時變模型預測控制研究農機在特定約束條件下的路徑跟蹤控制，取得了較高的跟蹤精度。張華強等[9]提出一種基于粒子群優化算法實時確定前視距離的改進純追蹤控制算法，提高了直線跟蹤精度。王輝等[10]提出一種基于預瞄追蹤模型的農機路徑跟蹤控制算法，提高了農機復雜路面抗干擾能力。

Stanley模型是一種基于橫向跟蹤誤差的非線性反饋函數，其橫向跟蹤誤差以指數收斂于零[11-12]，但其在農業機械自動導航上的應用報道較少。為提高農業機械全田塊自主作業導航精度和穩定性，本文提出一種基于改進Stanley模型的路徑跟蹤控制算法，采用模糊算法實現控制增益系數的自適應調整，提高農機自動轉彎路徑跟蹤精度，并減小初始誤差較大時的上線距離。以移動小車為試驗平臺，進行聯合收獲機全田塊作業路徑跟蹤試驗，以驗證算法的有效性。

1 Stanley路徑跟蹤算法

如圖1所示，Stanley模型前輪期望轉角的控制輸入由兩部分組成，分別為橫向偏差與航向偏差

δ(t)=δe(t)+δθ(t)

(1)

式中δ(t)——期望轉角

δe(t)——橫向偏差引起的期望轉角

δθ(t)——航向偏差引起的期望轉角

圖1 Stanley模型示意圖Fig.1 Schematic of Stanley model

單獨考慮橫向偏差影響，橫向偏差越大前輪期望轉角越大，假設車輛預期軌跡在距離前輪d(t)處與給定路徑上最近點切線相交，根據幾何關系得出非線性比例函數

(2)

式中k——增益系數e(t)——橫向偏差

v(t)——行駛速度

單獨考慮航向偏差的影響，前輪偏角與給定路徑切線方向一致，此時前輪期望轉角等于車輛航向與最近路徑點切線方向之間的夾角

δθ(t)=θe(t)

(3)

式中θe(t)——航向偏差

綜合考慮這兩方面的影響，得出前輪期望轉角函數為

(4)

2 參數自適應方法

常用的參數自適應方法有智能搜索算法、神經網絡算法和模糊算法等[13]。智能搜索算法主要包括遺傳算法、粒子群優化算法以及差分進化算法[14-16]。神經網絡算法根據激勵函數的不同可分為BP神經網絡、RBF神經網絡、PNN神經網絡以及GRNN神經網絡等[17-20]。相較于其他參數自適應方法，模糊算法具有計算量小、實時性好、魯棒性強、適合單片機系統等優點[21]，本文采用模糊算法對Stanley模型增益系數進行動態調整。

由式(4)可知，Stanley模型增益系數決定橫向偏差對車輪轉角影響的權重。增益系數較大可以快速減小橫向偏差，使農機快速行駛至規劃路徑上，但是過大的增益系數會造成直線跟蹤的振蕩。增益系數較小可以使農機運行平穩，降低算法收斂時的穩態跟蹤誤差，但初始大橫向誤差條件下收斂速度較慢。因此針對不同的橫向偏差，應選取時變的增益系數，尤其是對初始大誤差下上線距離和轉彎對行精度要求較高的全田塊路徑跟蹤。除此之外，航向偏差與橫向偏差對車輪轉角的影響具有方向性。當橫向偏差與航向偏差對車輪轉角作用方向相同時，可適當減小增益系數，使控制更加平穩。反之，當橫向偏差與航向偏差對車輪轉角作用方向相反時，可適當增大增益系數，提高算法收斂速度。

考慮農機實際工作情況，以橫向偏差和航向偏差為輸入變量構建隸屬度函數，限定前輪最大轉角為35°，初步制定模糊推理規則。以橫向偏差最大值和標準差為觀測量，進行多次試驗，尋找不同誤差輸入下最優的增益系數，修正模糊推理表。

2.1 隸屬度函數構建

如圖2所示，將橫向偏差的論域確定為[-3 m,3 m]。橫向偏差右大、右中、右小、零、左小、左中、左大分別對應NP、NM、NS、ZO、PS、PM、PB。

圖2 橫向偏差隸屬度函數Fig.2 Membership function of lateral deviation

圖3 航向偏差隸屬度函數Fig.3 Membership function of heading deviation

如圖3所示，將航向偏差的論域確定為[-30°,30°]。航向偏差右大、右中、右小、零、左小、左中、左大分別對應NP、NM、NS、ZO、PS、PM、PB。

如圖4所示，將增益系數的論域確定為[0, 1.2]。增益系數零、小、中、大分別對應ZO、PS、PM、PB。

圖4 增益系數隸屬度函數Fig.4 Membership function of gain coefficient

2.2 模糊推理表設計

模糊推理表具體規則為：航向偏差較小時，橫向偏差越大，增益系數越大；航向偏差與橫向偏差對車輪轉角影響方向相同，航向偏差越大，增益系數越小；航向偏差與橫向偏差對車輪轉角影響方向相反，航向偏差越大，增益系數越大；航向偏差較大時，與橫向偏差對前輪轉角影響方向相同，選較小增益系數；航向偏差較大時，與橫向偏差對前輪轉角影響方向相反，選較大增益系數。據此，共總結49條模糊推理規則，并通過試驗進行修正，制定如表1所示的模糊推理規則表。

2.3 解模糊化方法確定

解模糊化常用的方法有最大隸屬度法、重心法和加權平均法等[22]。重心法在工業上使用廣泛，輸出平滑，因此選擇重心法作為解模糊化的方法，其計算公式為[23]

表1 模糊推理規則Tab.1 Rule base for fuzzy logic control

(5)

式中u0——輸出系數m——級數

uk——各級系數μk(uk)——隸屬度

3 試驗與結果分析

3.1 試驗平臺搭建

為了驗證算法的有效性，搭建移動小車仿真試驗平臺。基站接收機型號為NovAtel OEM719、流動站接收機型號為OEM718D。雙天線RTK模式下定位精度1 cm+1ppm，其中1ppm表示離基站距離每超過1 km定位誤差增加1 mm。天線基線長2 m時定向精度0.08°，測量更新率5 Hz。電機驅動器型號為RMDS405，支持30 A電流。編碼器型號為歐姆龍E6B2，分辨率可達3 600 p/r(即轉一圈輸出3 600脈沖)。

移動小車試驗平臺結構如圖5所示，接收機通過天線和電臺同時接收衛星信號和基站差分改正數，進行RTK定位解算，實時輸出高精度位置與航向信息至導航上位機。上位機通過計算，輸出當前位置與規劃路徑間的橫向偏差、航向偏差。下位機根據導航偏差計算出期望轉向角，通過CAN總線以頻率5 Hz將控制信息發送至電機驅動器。電機驅動器將控制信息轉換為電壓，并實時接收編碼器的反饋，組成閉環控制系統，以頻率100 Hz控制電機。

圖5 路徑跟蹤算法驗證平臺Fig.5 Setup for path tracking system verification1.天線 2.電臺 3.接收機 4.編碼器 5.電機 6.上位機 7.下位機 8.顯示屏

3.2 路徑規劃方案

為驗證算法對全田塊農機路徑跟蹤不同階段適應性，模擬收獲機作業進行全田塊路徑跟蹤試驗，其中收獲機作業路徑采用外圈回字形，內圈AB線輔助導航的混合導航方式，不僅解決無動力換向功能收獲機的全田塊自動駕駛難題，而且有效提高收獲機作業效率，其全田塊路徑規劃方案如圖6所示。

圖6 聯合收獲機全田塊路徑規劃示意圖Fig.6 Field path planning schematic of combine harvester

收獲機按順序依次進行收獲作業，當剩余田塊寬度小于收獲機自動轉彎所需尺寸時，在最后一行直線段向駕駛員發出信號，并自動切換至人工輔助駕駛模式。收獲機全田塊作業過程分為上線階段、直線跟蹤階段和曲線跟蹤階段，其中直線跟蹤階段在算法改進前已有較好的跟蹤效果，故本次試驗重點研究上線階段以及曲線跟蹤階段。

3.3 試驗結果分析

小車直線跟蹤階段平均行駛速度約為2.5 m/s，過渡到曲線跟蹤階段時速度自動降為1 m/s左右。圖7為上線階段行駛軌跡，由圖7可知，改進后小車上線速度明顯加快，初始橫向偏差3 m時上線距離不超過5 m。圖8為曲線跟蹤階段小車行駛軌跡，曲線段橫向偏差與航向偏差始終處于變化狀態。固定增益系數的Stanley模型難以取得良好的路徑跟蹤效果，采用模糊算法改進后其增益系數根據導航偏差自適應調整，路徑跟蹤精度明顯提高。

圖7 大初始偏差上線階段行駛軌跡Fig.7 Guided trajectory of large initial lateral error

圖8 轉彎曲線跟蹤階段行駛軌跡Fig.8 Curves of trajectory of turning

圖9 模糊Stanley模型橫向誤差Fig.9 Lateral error of fuzzy-based Stanley model

采用模糊算法改進前后的路徑跟蹤橫向誤差如圖9所示。直線跟蹤時，模糊Stanley模型減小了路徑跟蹤的上線距離，10 m以后2種方法的穩態誤差無明顯差異，最大橫向誤差小于3 cm，標準差小于1 cm。曲線跟蹤階段的穩態誤差改善明顯，路徑跟蹤誤差最大值由28 cm下降為3 cm，滿足轉彎對行精度要求。試驗過程發現直線路徑跟蹤存在明顯的穩態誤差，其可能原因包括轉向系統機械間隙和天線基線偏角引起的系統誤差。前者在農業機械上影響可以忽略，后者可以通過作業現場標定進一步補償[24]。

圖10 全田塊路徑跟蹤軌跡Fig.10 Field path tracking trajectory

為驗證所提模糊Stanley模型的全田塊自主作業適應性，基于圖6所示的聯合收獲機全田塊作業路徑開展試驗分析。為改善無人自主收獲作業效率，采用混合路徑規劃實現其全田塊導航，其中外圈為回字形路徑，內圈采用AB線導航作業。路徑跟蹤軌跡如圖10所示，由圖10可知，路徑跟蹤算法整體跟蹤效果良好，具體跟蹤誤差見表2。采用模糊Stanley模型不僅改善了路徑跟蹤的上線速度，也顯著減小了轉彎掉頭時曲線路徑跟蹤誤差，滿足農業機械全田塊路徑跟蹤的需求。

3.4 聯合收獲機田間試驗

以約翰迪爾C230型輪式聯合收獲機為試驗對象進一步驗證算法有效性，在其上安裝雙天線RTK定位定向系統并進行電控化改造。C230型收獲機作業幅寬為5 m，轉彎半徑約為8 m，其收獲作業時導航系統配置與小車試驗平臺一致，全田塊路徑規劃采用圖6所示方案。行駛控制系統包括電動方向盤、轉角傳感器、電動油門以及卸糧、脫粒相關控制系統等，如圖11所示。其中電動方向盤為上海聯適EMS2型轉向驅動單元，工作電流可達10 A，具有扭矩大、精度高等特點，IP65級防塵防水，適合農機惡劣作業環境。轉角傳感器型號為北京天海科公司的DWQCAB-V-CH，其線性度為0.02%FS，角分辨率0.022°，轉角傳感器安裝在收獲機轉向立軸正上方，與轉向中心立軸直連。

表2 收獲機全田塊作業路徑跟蹤誤差Tab.2 Field path tracking error of combine harvester

圖12 聯合收獲機路徑跟蹤軌跡Fig.12 Path tracking trajectory of combine harvester

聯合收獲機田間無人駕駛作業結果如圖12所示，其中直線段平均作業速度約為0.8 m/s，自動轉彎時平均作業速度約為0.6 m/s。受大型聯合收獲機轉彎半徑限制和手自動切換控制可靠性影響，目前僅驗證了外圈回字形自動轉彎收獲作業。由試驗結果可知，穩定作業時直線和曲線路徑跟蹤誤差均小于15 cm，然而受側滑和田壟等復雜田間土壤環境影響，收獲機最大橫向跟蹤誤差為0.63 m。對比分析移動小車試驗結果可知，大型聯合收獲機雙天線安裝誤差、復雜土壤條件和轉向系統時滯對Stanley模型增益系數的適應性有較大影響。

4 結論

(1)為改善農機全田塊路徑跟蹤算法適應性，基于模糊算法提出一種改進Stanley模型的路徑跟蹤控制算法，實時調整路徑跟蹤控制增益系數。場地試驗表明額定作業速度下車輛直線與曲線最大路徑跟蹤誤差均小于3 cm。

(2)采用移動小車對收獲機全田塊作業路徑進行路徑跟蹤試驗，結果表明本文算法顯著改善Stanley模型路徑跟蹤的適應性，初始橫向誤差3 m時，上線距離由12 m下降到5 m，滿足農業機械全田塊自動導航作業要求。

(3)聯合收獲機田間試驗表明，天線安裝精度影響路徑跟蹤穩態誤差，且導航系統位姿更新率和穩定性制約著農業機械自主作業的可靠性和作業效率。