基于預適應路徑追蹤的收割機RAC導航系統

張文利，郭 宇，陳開臻，偉利國，苑嚴偉※

（1.北京工業大學信息學部，北京100124；2.中國農業機械化科學研究院，北京100083）

0 引言

近年來，隨著衛星定位數據精度提高，各類路徑追蹤方法層出不窮，越來越多的高精度農機自動駕駛系統被研發、生產。通過在傳統的農用機械上部署自動導航系統，能夠實現農機的全自動作業，滿足了精準農業的需求［1-2］。

偉利國、李逃昌、Erkan等國內外眾多學者提出了使用基于模糊控制理論的農機路徑追蹤方法［3-9］；石翔、唐小濤等提出了一種對純追蹤模型中的前視距離參數使用模糊控制理論進行動態調整的方法［10-11］；羅錫文等提出了一種動態切換PID控制和模糊控制的農業機械導航控制方法［12］；黃沛琛等通過BP 神經網絡確定純追蹤模型中的前視距離參數［13］；Han 等使用迭代法實現了車輛模型參數估計，設計并實現了基于純追蹤模型的路徑跟蹤控制器［14］；陳寧等提出了一種改進型純追蹤控制器［15］；于萬鵬、馬志艷、謝忠華等提出了使用基于視覺的路徑追蹤方法［16-18］。

綜合分析已有研究成果可知，模糊純追蹤模型因其簡單有效，被廣泛應用于路徑追蹤領域，當采用模糊純追蹤模型作為農機路徑追蹤方法時，往往將農機簡化為Kelly提出的二輪車模型［19-21］。根據純追蹤模型原理可知，前視距離是影響追蹤效果的重要因素。引入模糊控制理論的模糊純追蹤模型，通過建立模糊控制規則表，對純追蹤模型的前視距離參數進行動態調整，能夠提高模型動態追蹤能力。然而模糊純追蹤模型是根據已經產生的位姿偏差對車輛進行校正，即每次前視距離參數的動態調整都是依據上一周期的位姿偏差結果，而不是當前位姿偏差結果，因此導致路徑追蹤模型誤差較大，追蹤精度較低。

因此，文章提出了一種預適應路徑追蹤模型，在模糊純追蹤模型的基礎上引入預測控制理論，通過內置收割機運動學模型，對收割機下一周期運動狀況進行預測，根據預測的當前周期位姿偏差結果調整前視距離參數，從而提高追蹤精度，降低誤 差。

1 收割機RAC導航系統

收割機RAC（Realtime Array Calibration）導航系統由衛星接收機、車載終端和電控方向盤組成（圖1）。車載終端通過分析衛星接收機傳入的收割機位置、航向信息，結合預先設定的軌跡，即可輸出收割機前輪轉角控制量，驅動電控方向盤控制收割機轉向。傳統的農機自動導航系統的設計中，為了獲得較為精準的衛星定位信息，常采用RTK衛星接收機作為定位裝置，然而RTK 設備價格昂貴，部署較為繁瑣，采用RTK 設備提高了投入成本和部署難度，然而有些農機作業并不需要精度太高即可正常工作，例如收割機作業時，并不需要cm級路徑追蹤精度。為了降低自動導航系統的投入成本，該文采用RAC 設備代替RTK 設備進行衛星信號的接收，由于RAC設備僅采用普通民用單頻信號GPS L1 或者北斗B1即可實現亞米級別定位，相比于RTK 設備，價格低廉，安裝調試簡單方便，精度也能滿足收割機作業需求。

圖1收割機RAC 導航系統框圖Fig.1 Block diagram of harvester automatic navigation system

2 預適應路徑追蹤模型的設計

工作流程如圖2所示，主要分為以下幾步：（1）農機在作業過程中，將本周期的車輛位置、航向信息輸入到模糊純追蹤模型中，結合模糊純追蹤模型內置追蹤路徑信息，實時計算本周期前輪轉角控制量。（2）該控制量作為預測模塊的輸入，結合預測模塊中內置的農機運動學模型，預先計算下周期車輛位置、航向信息。（3）同（1），該位置、航向信息作為輸入量再次被輸入到模糊純追蹤模型中，進而獲得預測前輪轉角控制量。（4）將獲得的本周期前輪轉角控制量、預測前輪轉角控制量、本周期位置航向信息和預測位置航向信息輸入到綜合模塊，綜合模塊根據上述農機位姿信息提前一周期判斷農機出軌情況，預先對前輪轉角控制量做出調整，控制農機進行路徑追蹤。

圖2預適應路徑追蹤模型流程圖Fig.2 Flow chart of pre-adaptive path tracking model

根據純追蹤模型原理，收割機前輪轉角控制量滿足公式（1）：

式（1）中，Cy為收割機前輪轉角控制量，m為收割機軸距，Ep為收割機到追蹤點的橫向誤差，Ey為收割機到追蹤點的航向誤差。

在純追蹤算法中，由于前視距離是固定的，因此在直線追蹤和曲線追蹤共同存在的場景下，容易發生出軌或者震蕩現象。通過引入模糊控制器，實時生成前視距離，解決該問題。根據模糊控制器設計原則，模糊控制器設計分為以下幾步。

①確定輸入變量

將車輛與追蹤點的偏移誤差和車輛與追蹤點的航向誤差作為輸入變量，將前視距離作為輸出變量。車輛與追蹤點的偏移誤差為L，車輛與追蹤點的航向誤差為Y，前視距離為E。

②確定論域、模糊集

L、Y和E的模糊集均為｛NB，NM，NS，ZO，PS，PM，PB｝，其中NB表示負大（Negative Big），NM表示負中（Negative Middle），NS表示負小（Negative Small），PS表示正小（Positive Small），PM表示正中（Positive Middle），PB表示正大（Positive Big），在模糊控制中用來表示隸屬關系；論域均為｛-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6｝。

③確定隸屬函數

如圖3所示，采用三角形隸屬函數，其中L、Y 和E均采用該隸屬函數。

圖3隸屬函數Fig.3 Membership function

④建立模糊控制規則表

如表1所示，根據農機在實際作業中的情況，總結經驗得到49條規則，建立前視距離的模糊控制規則表。

表1前視距離模糊控制規則表Table1 Fuzzy control rule table of forward looking distance

預測模塊可以在農機開始作業之前，將農機運動學模型輸入模塊中，當模糊純追蹤控制器計算出前輪轉角控制量后，預測模塊先行將該控制量輸入到農機運動學模型中，計算下一個周期農機位置的改變，通過該模塊，可以獲得“預測航向誤差”和“預測位置誤差”2個預測量。

如圖4所示，當通過模糊純追蹤控制器獲得t周期控制量Cy（t）后，將該控制量輸入預測模塊，根據公式（2）至（4），即可預測收割機t+1 周期運動狀態信息和誤差信息。

圖4預測模塊原理示意圖Fig.4 Schematic diagram of prediction module

式（2）至（4）中，Y為收割機航向，Cp為收割機位置，Cpx為Cp在X軸上的分量，Cpy為Cp在Y軸上的分量，v為收割機運行速度，Δt為控制周期時間。

將預測的運動狀態信息重新輸入到模糊純追蹤模型中，即可獲得收割機t+1周期預測控制量Cy（t+1）。在已知t周期和t+1周期收割機位置誤差Ep（t）和Ep（t+1）的情況下，根據公式（5）可以獲得收割機由t周期到t+1周期運動過程中的位置偏離系數K。

若K＞0，則說明根據預測結果，收割機在t+1周期沒有出軌，最終控制量為t周期控制量Cy（t）；反之，若K＜0，則說明收割機追蹤過程中出軌，需要對控制量進行修正，參照公式（6）：

式（6）中，C′y（t）為前輪轉向角綜合控制量，為比例系數，理想狀態下，農機沿直線追蹤，且農機位置時刻處于直線上，農機航向與直線延伸方向相同。其中默認取值為0.5。若在實際測試中，出現出軌抑制效果不佳現象，則應該增大，若出現出軌抑制效果過度現象，則應該減少。

3 結果分析

該研究共進行了2 個相關實驗：模擬仿真實驗和農機應用實驗。在模擬仿真實驗中，分別對模糊純追蹤模型和預適應路徑追蹤模型進行模擬仿真，并針對其性能參數進行分析。在農機應用實驗中，通過在收割機上安裝搭載預適應路徑追蹤模型的控制器，控制收割機進行路徑追蹤并對其進行誤差分析。

3.1 模擬仿真實驗

通過Matlab+Simulink聯合平臺，分別搭建模糊純追蹤模型和預適應路徑追蹤模型，并對2種路徑追蹤模型進行模擬仿真，結果如圖5所示。

圖5 預適應路徑追蹤模型和模糊純追蹤模型仿真示意圖Fig.5 Pre-adaptive path tracking model and fuzzy pure pursuit model

在仿真結果中，模糊純追蹤模型誤差平均值為2.364 5 m，而預適應路徑追蹤模型的平均誤差為0.192 5 m，誤差下降了91.9%。

3.2 農機應用實驗

在收割機實際工作中，拖拉機行駛過程中會改變土地地形，容易造成多次測試環境不一致。該研究主要解決模糊純追蹤模型誤差大問題，測試環境對控制器影響應該適當降低，因此選擇在寬闊平整的柏油馬路上進行收割機路徑追蹤測試。將搭載了預適應路徑追蹤模型的車載終端安裝在拖拉機上，部署衛星接收機和電控方向盤。

分別在馬路兩端選取A點和B點，人工將收割機行駛至A 點，車頭方向朝向B點，進行路徑自動追蹤；當路徑追蹤到B點時，人工將車輛駛回A 點，并重復以上過程。測試過程中，使用筆記本電腦通過串口接收日志數據（A、B 點信息，當前位置信息，航向角信息，方向盤控制信息），最后通過上位機處理軟件進行數據可視化，可視化結果如圖6所示。設定追蹤路徑切線方向為縱向，收割機上衛星接收機位置到該垂線長度為橫向誤差，該實驗共進行了12組測試，對測試結果計算誤差，其結果如表2 所示。

圖6預適應路徑追蹤模型測試數據可視化示意圖Fig.6 Visualization schematic diagram of test data for pre-adaptive path tracking model

表2預適應路徑追蹤模型橫向誤差統計結果Table 2 Statistical result of lateral error of pre-adaptive path tracking model

經過測試，當收割機以3 km/h的速度行駛時，采用搭載了預適應路徑追蹤模型的收割機RAC導航系統，路徑追蹤誤差的平均值為0.63 m，考慮到RAC衛星接收機定位精度為亞米級，路徑追蹤誤差符合預期。

4 結論

該文通過引入預測控制理論，對模糊純追蹤模型響應速度慢的缺陷進行改良，構建適應能力較強的預適應路徑追蹤模型。該模型采用高性價比RAC衛星接收設備，在滿足收割機工作精度的前提下，降低了設備部署難度和設備價格。該文通過Matlab+Simulink聯合平臺對模糊純追蹤模型和預適應路徑追蹤模型進行仿真分析，發現預適應路徑追蹤模型追蹤效果更好，平均誤差得到大幅度降低。通過在收割機上使用RAC導航系統的實際測試發現，預適應路徑追蹤模型平均誤差值為0.63 m，滿足設計要求。

預適應路徑追蹤模型雖然經過了實際農機測試，但僅測試了速度在3 km/h 的低速狀態運行情況，而農機速度會對RAC衛星接收設備以及控制器的性能產生較大影響，在今后的研究中，將進一步研究高速條件下的導航系統追蹤方法，促使研究成果適應實際農機作業需要。