智能收獲機定位和自適應路徑追蹤方法

王立輝，石佳晨，王樂剛，焦中元，李耀明，陳 進，徐立章

（1.東南大學 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2.雷沃重工股份有限公司，山東 濰坊 261206；3.江蘇大學 現代農業裝備與技術教育部重點實驗室，江蘇 鎮江 212013）

0 引言

農業裝備的定位與導航技術是實現農業機械車輛路徑追蹤的基礎[1]，目前定位技術主要有全球衛星導航系統（ global navigation satellite system,GNSS）[2]、組合導航技術[3]和機器視覺定位技術[4]。依托先進的定位技術，可以實現大型智能農業機械車輛的路徑追蹤功能，實現農業機械車輛自動駕駛。文獻[5]設計了1 個模糊控制器，根據農機的位置偏差和航向偏差來輸出期望轉向角，使農機追蹤期望路徑，并通過仿真驗證其有效性。文獻[6]設計了1 種基于遺傳算法的模糊控制器，能夠提升拖拉機自動作業時的動態特性和魯棒性，提高拖拉機在不同條件下的自適應性。文獻[7]基于期望路徑的曲率估計理想速度，并根據橫向誤差和速度動態估計前視距離，能夠提升車輛在路徑追蹤時的精度。

收獲機的作業速度越大，位置偏差也越大[8-9]。收獲機存在控制延遲是導致這個現象的原因之一，這些控制延遲主要包含導航定位系統延遲和控制系統的延遲等[10]。智能稻麥聯合收獲機是1 種重載農業機械車輛，它的轉向控制系統以非線性、高延遲的電控液壓輔助轉向系統為主[11]。通過對具體的系統建模，合理地對具體控制模型的參數自適應調整，能夠有效減緩控制系統的控制延遲[12]。然而這種辦法，對于液壓轉向控制系統的模型參數獲取困難、轉向控制系統模型種類繁多的農業收獲機的實際應用存在一定困難。模型預測控制（model predictive control, MPC）能夠使得收獲機的路徑追蹤控制不依賴于精確的控制模型，抑制控制延遲。但是MPC 算法計算量巨大、計算時間長，會影響收獲機路徑追蹤的實時性[13]。使用分布式MPC、混合式MPC 方法可以大大提高計算速度，但是這種方法需要更多的硬件支持，增加了成本，不利于實際生產應用[14]。

國內外針對拖拉機等中小型農機的定位和路徑追蹤系統已頗有成就[15-18]，但是針對大型智能聯合收獲機的定位和路徑追蹤系統的研究較少。綜上所述，針對聯合收獲機的控制延遲較其他中小型農機更大，控制難度更大的現象，提出1 種適用于大型智能聯合收獲機的路徑追蹤方法，有著重要的研究意義。基于運動學模型的純追蹤算法，因其運算量小、實用性強、硬件成本需求低等優點，已廣泛應用到了智能農機的輔助駕駛系統和路徑追蹤系統中[19]。利用預測思想，校正和補償收獲機路徑追蹤時存在控制延遲導致的位置偏差[20]，可以很好地提升收獲機路徑追蹤的精度。為解決大型智能聯合收獲機存在控制延遲，導致路徑追蹤精度下降的問題，本文提出 1 種基于GNSS 的智能聯合收獲機自適應路徑追蹤方法：建立聯合收獲機的運動學模型，根據所建模型推導純追蹤算法。依托雙天線GNSS 實時動態差分（real-time kinematic, RTK）技術，獲取收獲機當前時刻的位置、航向和速度信息，預測未來時刻收獲機的位姿信息，校正當前時刻純追蹤算法的橫向誤差；將速度、航向誤差和校正后的橫向誤差輸入模糊控制器，根據速度等信息實時地自適應校正純追蹤算法中的前視距離參數，通過優化后的純追蹤算法，計算期望轉向角，控制收獲機的轉向，實現路徑追蹤控制。

1 聯合收獲機路徑追蹤系統設計

聯合收獲機路徑追蹤系統依托雷沃谷神系列大型智能稻麥輪式聯合收獲機搭建，該系列聯合收獲機的軸距為3.75 m，轉向系統為電液輔助轉向系統，前輪為固定輪，后輪為轉向輪。路徑追蹤系統主要由雙天線RTK-GNSS 定位模塊、液壓轉向控制模塊和工控屏組成。RTK-GNSS 的定位精度已經能達到厘米級，但在實際作業時發現，單天線RTK-GNSS 系統無法獲得精確的收獲機的航向信息，尤其在收獲機作業速度較慢時，測量獲得的航向信息和實際的航向信息相差甚大。本文采用了雙天線測量法實時獲取收獲機的航向角信息：將RTK 系統的移動站的接收機天線1 和接收機天線2 分別固定到收獲機上，保持其相對位置不發生變化，即可以通過RTK 技術分別測得2 個天線在對應的坐標系中的位置信息，結合2 個天線在收獲機上的安裝位置，可以直接推算出收獲機的精確的航向角。整個系統的架構如圖1 所示。雙天線RTK-GNSS 采集收獲機的速度、位置和航向角信息后，轉向控制器STM32F767IG 將收獲機的位姿信息發送給工控屏。工控屏基于C++/QT 開發了聯合收獲機自動導航軟件，軟件根據路徑追蹤算法、當前時刻的位姿信息結合期望路徑，計算出期望轉向角后發送給轉向控制器。轉向控制器通過液壓驅動電路，驅動收獲機的液壓系統，控制后輪轉向，同時轉向控制器依托轉向角傳感器實時監測后輪的轉向角，形成閉環反饋控制。

圖1 基于GNSS 的路徑追蹤系統架構

圖1 中，PWM(pulse width modulation)為脈沖寬度調制。

2 收獲機自適應路徑追蹤方法

2.1 純追蹤算法

純追蹤算法是1 種基于運動學模型的算法。如圖2 所示，根據搭建的路徑追蹤系統，建立前輪固定、后輪轉向的簡化的二輪車模型[21]。

圖2 簡化的后輪轉向二輪車模型

在導航坐標系X － O －Y 中：A 為前輪動力輪；B 為后輪轉向輪；θws為轉向輪的轉向角； Lwb為前后軸的軸距；r 為轉向半徑。轉向輪的轉向角 θws和轉向半徑r 的數學關系為

純追蹤算法是模擬人的駕駛行為，選擇1 條最優的曲率半徑逼近期望路徑，實現車輛的路徑追蹤[22-23]。

如圖3 所示，在車體坐標系 Xb－ Ob－Yb中，點（ x(b),y(b)）為在當前的前視距離 Lld下，收獲機在期望路徑上的預瞄點。

圖3 純追蹤算法示意圖

根據圖3 中可得

轉向半徑r 和前視距離 Lld的關系為

式中 x(b)在車體坐標系中的值[24]為

式中： dXTE為橫向誤差，其定義為當前位置到期望路徑的距離，符號定義沿著期望路徑右側同向為正，反之為負；eθ 為航向誤差，其定義為從當前航向角逆時針轉向期望路徑角度的角度。

結合式（1），可得期望轉向角為

由式（5）可知，純追蹤算法計算期望轉向角基于當前時刻的橫向誤差和航向誤差，式中唯一的不確定量為前視距離。

2.2 基于速度的自適應延遲控制校正

收獲機在田間自動作業時，由于各種原因，從收獲機的控制器發出控制信號到執行機構執行完畢指令存在延遲。傳統的純追蹤算法，是以當前時刻的參數計算期望轉向角，沒有針對系統的滯后性進行校正。優化后的收獲機路徑追蹤算法使用校正后的橫向誤差計算期望轉向角，校正因控制延遲帶來的偏差，如圖4 所示。

圖4 預測位置示意圖

在導航坐標系X － O － Y中，點 m( xm, ym)為收獲機控制器發出控制信號的位置，當收獲機的作業速度為v 時，點 n( xn, yn)為收獲機的執行機構執行完畢的點。當其他條件不變時，若收獲機以比v 更快的速度 v2行駛，則會行駛到距離更遠的點n2( xn2, yn2)處，因控制延遲導致的位置偏差會更大。定義控制器發出信號到執行機構執行完畢的這段時間為控制延遲時間 td，根據速度v 估算控制延遲時間 td的長短，依據點m 預測點n，使用點n 的位置信息校正點m 的位置信息，可以校正收獲機因為控制延遲造成的偏差。

假設在控制延遲時間 td內收獲機以恒定速度r行駛，且輪胎的轉向角沒有發生變化，則這段時間內行駛的弧長 lmn為

點m 與點n 的距離為

式中 Δθh為延遲時間 td內航向角的變化量，且有

聯立式（7）和式（8），有

在車體坐標系中，點m 相對于點n 的位置關系為

則在導航坐標系中，點m 相對于點n 的位置關系為

結合式（9）～式（11），唯一的不確定量是控制延遲時間 td。收獲機轉向控制的1 個完成流程為：①收獲機接通過RTK-GNSS 獲取自身位置、速度和航向信息；②收獲機路徑追蹤控制器通過路徑追蹤算法，結合期望路徑計算期望轉向角；③轉向控制器通過轉向控制算法，將期望轉向角轉換為電信號輸出至轉向執行機構上；④轉向執行機構根據電信號，執行收獲機的轉向。

這4 個環節中都存在延遲：①為導航定位系統的延遲，與導航定位系統的性能有關；②、③為控制算法計算的延遲，與算法復雜度和控制器的性能有關；④為轉向控制延遲，與收獲機的質量、負載與慣性大小、執行機構性能和地面狀態等有關。因此，延遲時間 td可以看成由①～③組成的固定延遲時間和由④組成的動態延遲時間組成。而在基于運動學模型的純追蹤算法中，地面狀態的變化、收獲機負載與慣性的變化等，最終都會反映到速度的變化上，故動態時間與速度v 有關，可以根據速度自適應調整延遲時間 td，克服收獲機因為控制延遲導致的路徑追蹤精度下降的問題。若采用線性關系，則 td和v 的關系可以表示為

式中：vμ 為速度影響系數；tdτ 為固定延遲時間。

根據速度v 估算延遲控制時間 td后，使用預測的未來時刻的位置n 計算橫向誤差，以替代式（5）中的橫向誤差，可以克服因滯后性和控制延遲導致的收獲機路徑追蹤的精度下降問題。

2.3 前視距離的自適應調整

由式（5）可知，影響期望路徑唯一的不確定量為前視距離，前視距離的大小會影響路徑追蹤算法的穩定性和精度[25]。若前視距離過小，收獲機將會快速逼近期望路徑，但是往往帶來較大的振蕩；若前視距離過大，收獲機將會花費較長時間平穩地逼近期望路徑。因此，算法應根據速度和收獲機其他狀態自適應調整前視距離的大小。模糊控制不依賴于具體的模型，能夠較好地處理不同的作業條件下收獲機前視距離的動態調整的問題，提高收獲機的算法自適應性[26]。

本文設計了1 個3 輸入、單輸出的模糊控制器，將速度、航向誤差和校正后的橫向誤差作為模糊控制器的輸入，將前視距離作為模糊控制器的輸出。模糊控制器的輸入輸出的論域檔級決定了模糊控制器的性能，過少的檔級會使得模糊控制器的控制效果達不到預期情況，過多的檔級會使得模糊控制器的計算復雜從而導致計算的實時性下降。根據收獲機的實際運行情況，將速度從0～4 m/s 分為3 個論域{L, M, H}，將橫向誤差從-4～4 m 分為7 個論域{LB, LM, LS, M, RS, RM,RB}、將航向偏差從-180°～180°分為7 個論域{LB,LM, LS, C, RS, RM, RB}、將前視距離從3～9 m分為7 個論域{VN, N, LN, M, LF, F, VF}。同時根據各模糊變量確定其隸屬度函數，將隸屬度函數離散化，就得到了有限個點上的隸屬度。本研究將所有的隸屬度函數都定義為梯形函數。

模糊控制器的模糊規則，決定了在不同的輸入下前視距離的具體輸出。本文設計的規則為：1）當收獲機轉彎或逼近期望路徑，需要滿足速度較小、有較大的航向或者和橫向誤差時，輸出較小的前視距離；2）當收獲機在高速穩定作業狀態，需要滿足速度較大、航向誤差和橫向誤差皆較小時，輸出較大的前視距離；3）當收獲機臨近作業結束，需要滿足速度較小、航向誤差和橫向誤差皆較小時，輸出1 個中等的前視距離；4）當收獲機收獲機前進速度較大，航向或者橫向誤差存在條較大時，輸出較小的前視距離。

最終模糊控制器會根據輸入參數和模糊規則，輸出合適的前視距離，將輸出的前視距離代入至式（5），可以提高收獲機在不同的速度、橫向偏差和航向偏差下的路徑追蹤的精度和穩定性，有效提高路徑追蹤算法的自適應能力。

3 實驗與結果分析

為了驗證基于GNSS 的聯合收獲機路徑追蹤方法在田地作業中的實際效果，在江蘇某農場進行了田間實驗。主要設備的安裝位置如圖5 所示。

圖5 主要設備安裝位置示意圖

將雙天線RTK-GNSS 定位系統安裝在收獲機車體頂部，角度傳感器使用非接觸式霍爾傳感器，安裝于轉向輪的轉向軸上。

實驗方法如下：由司機人工駕駛收獲機收割田間的小麥，并使用RTK-GNSS 記錄下此次路徑作為參考路徑，由司機人工掉頭后，進入自動駕駛的路徑追蹤階段，首先控制器根據參考路徑規劃出期望路徑：控制器通過RTK-GNSS 獲取當前時刻的位置，讀取參考路徑上的第1 個點，生成平移向量，將參考路徑上的后續點皆按平移向量平移，生成期望路徑，隨后控制器會根據收獲機的位置信息和期望路徑，結合本文提出的路徑追蹤算法計算期望轉向角，再將信號發送給轉向控制器，由轉向控制器完成轉向控制，追蹤這條期望路徑，直至最后1 個點，此次路徑追蹤結束后，由司機人工掉頭，重復上述過程。在路徑追蹤的過程中，記錄下收獲機的位置、航向、速度、轉向角、橫向誤差等所有信息，并使用RTK-GNSS 的坐標作為收獲機位置的真值。

實驗參數設定如下：衛星更新和控制頻率皆為5 Hz，轉向控制頻率為50 Hz，未優化時的前視距離為4 m，綜合考慮到算法復雜度、處理器性能、地面狀態和液壓轉向系統的遲滯性，將速度影響系數vμ 設為0.15 s2·m-1，固定延遲時間tdτ 設為0.1 s。

收獲機分別在速度為1.0 和2.5 m/s 的條件下，進行了優化前和優化后的路徑追蹤實驗。針對每種情況分別進行3 次實驗，其結果如圖6 所示。在基于C++/QT 開發的聯合收獲機自動導航軟件上顯示，當收獲機速度為1.0 m/s 時，使用優化后算法第1 次追蹤期望路徑時的路徑追蹤效果，穩定后的橫向誤差小于3 cm。

圖6 路徑追蹤實驗效果

3 次實驗橫向誤差的趨勢圖分別如圖7（a）、圖 7（b）、圖 7（c）所示。由于期望路徑的第1 個點為收獲機當前時刻點，所以初始橫向誤差皆為0 cm，而由于航向誤差的存在，收獲機會出現短暫的偏離期望路徑的情況，隨后立刻收斂。優化前的路徑跟蹤算法，在逼近期望路徑時會有較大的振蕩，且需要較長時間才能進入平穩行駛的情況，而基于本文提出的改進路徑追蹤算法，則能夠快速且穩定地進入平穩行駛的情況。

3 次實驗的橫向誤差的標準差如表1 所示。

實驗結果表明：本研究提出的路徑追蹤算法的效果優于傳統的純追蹤算法，速度在1 和2.5 m/s時，橫向誤差的標準差都低于2.5 cm。

圖7 收獲機橫向誤差與行駛距離的變化趨勢

表1 橫向誤差實驗效果

4 結束語

針對農業智能稻麥收獲機在田間自動作業時存在控制延遲而降低路徑追蹤精度的問題，提出1 種基于GNSS 的收獲機路徑追蹤方法。依托收獲機建立了運動學模型，推導了純追蹤算法；使用雙天線RTK-GNSS 定位系統，獲取收獲機的當前時刻的速度、位置和姿態信息，估計未來時刻的位置信息，校正當前時刻的位置并重新計算新的橫向誤差；設計了1 個模糊控制器，控制器將根據速度、航向誤差和校正后的橫向誤差自適應調節純追蹤算法中的前視距離；最后計算期望轉向角，同時轉向控制器控制著收獲機的轉向，實現路徑追蹤。設計了基于GNSS 的收獲機路徑追蹤系統：基于C++/QT 開發了聯合收獲機自動導航軟件，搭建了基于雙天線RTK-GNSS 的定位模塊和轉向控制反饋回路，并進行了實驗驗證。實驗結果表明：收獲機的控制延遲影響路徑追蹤精度的現象有所改善，在不同的速度和狀況下都能有效地追蹤期望路徑；路徑追蹤的精度有了提高，橫向誤差的標準差低于2.5 cm。