王立輝，石佳晨，王樂剛，焦中元，李耀明，陳 進(jìn)，徐立章

（1.東南大學(xué) 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點實驗室，南京 210096；2.雷沃重工股份有限公司，山東 濰坊 261206；3.江蘇大學(xué) 現(xiàn)代農(nóng)業(yè)裝備與技術(shù)教育部重點實驗室，江蘇 鎮(zhèn)江 212013）

0 引言

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

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

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

1 聯(lián)合收獲機(jī)路徑追蹤系統(tǒng)設(shè)計

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

圖1 基于GNSS 的路徑追蹤系統(tǒng)架構(gòu)

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

2 收獲機(jī)自適應(yīng)路徑追蹤方法

2.1 純追蹤算法

純追蹤算法是1 種基于運動學(xué)模型的算法。如圖2 所示，根據(jù)搭建的路徑追蹤系統(tǒng)，建立前輪固定、后輪轉(zhuǎn)向的簡化的二輪車模型[21]。

圖2 簡化的后輪轉(zhuǎn)向二輪車模型

在導(dǎo)航坐標(biāo)系X － O －Y 中：A 為前輪動力輪；B 為后輪轉(zhuǎn)向輪；θws為轉(zhuǎn)向輪的轉(zhuǎn)向角； Lwb為前后軸的軸距；r 為轉(zhuǎn)向半徑。轉(zhuǎn)向輪的轉(zhuǎn)向角 θws和轉(zhuǎn)向半徑r 的數(shù)學(xué)關(guān)系為

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

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

圖3 純追蹤算法示意圖

根據(jù)圖3 中可得

轉(zhuǎn)向半徑r 和前視距離 Lld的關(guān)系為

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

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

結(jié)合式（1），可得期望轉(zhuǎn)向角為

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

2.2 基于速度的自適應(yīng)延遲控制校正

收獲機(jī)在田間自動作業(yè)時，由于各種原因，從收獲機(jī)的控制器發(fā)出控制信號到執(zhí)行機(jī)構(gòu)執(zhí)行完畢指令存在延遲。傳統(tǒng)的純追蹤算法，是以當(dāng)前時刻的參數(shù)計算期望轉(zhuǎn)向角，沒有針對系統(tǒng)的滯后性進(jìn)行校正。優(yōu)化后的收獲機(jī)路徑追蹤算法使用校正后的橫向誤差計算期望轉(zhuǎn)向角，校正因控制延遲帶來的偏差，如圖4 所示。

圖4 預(yù)測位置示意圖

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

假設(shè)在控制延遲時間 td內(nèi)收獲機(jī)以恒定速度r行駛，且輪胎的轉(zhuǎn)向角沒有發(fā)生變化，則這段時間內(nèi)行駛的弧長 lmn為

點m 與點n 的距離為

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

聯(lián)立式（7）和式（8），有

在車體坐標(biāo)系中，點m 相對于點n 的位置關(guān)系為

則在導(dǎo)航坐標(biāo)系中，點m 相對于點n 的位置關(guān)系為

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

這4 個環(huán)節(jié)中都存在延遲：①為導(dǎo)航定位系統(tǒng)的延遲，與導(dǎo)航定位系統(tǒng)的性能有關(guān)；②、③為控制算法計算的延遲，與算法復(fù)雜度和控制器的性能有關(guān)；④為轉(zhuǎn)向控制延遲，與收獲機(jī)的質(zhì)量、負(fù)載與慣性大小、執(zhí)行機(jī)構(gòu)性能和地面狀態(tài)等有關(guān)。因此，延遲時間 td可以看成由①～③組成的固定延遲時間和由④組成的動態(tài)延遲時間組成。而在基于運動學(xué)模型的純追蹤算法中，地面狀態(tài)的變化、收獲機(jī)負(fù)載與慣性的變化等，最終都會反映到速度的變化上，故動態(tài)時間與速度v 有關(guān)，可以根據(jù)速度自適應(yīng)調(diào)整延遲時間 td，克服收獲機(jī)因為控制延遲導(dǎo)致的路徑追蹤精度下降的問題。若采用線性關(guān)系，則 td和v 的關(guān)系可以表示為

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

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

2.3 前視距離的自適應(yīng)調(diào)整

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

本文設(shè)計了1 個3 輸入、單輸出的模糊控制器，將速度、航向誤差和校正后的橫向誤差作為模糊控制器的輸入，將前視距離作為模糊控制器的輸出。模糊控制器的輸入輸出的論域檔級決定了模糊控制器的性能，過少的檔級會使得模糊控制器的控制效果達(dá)不到預(yù)期情況，過多的檔級會使得模糊控制器的計算復(fù)雜從而導(dǎo)致計算的實時性下降。根據(jù)收獲機(jī)的實際運行情況，將速度從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}。同時根據(jù)各模糊變量確定其隸屬度函數(shù)，將隸屬度函數(shù)離散化，就得到了有限個點上的隸屬度。本研究將所有的隸屬度函數(shù)都定義為梯形函數(shù)。

模糊控制器的模糊規(guī)則，決定了在不同的輸入下前視距離的具體輸出。本文設(shè)計的規(guī)則為：1）當(dāng)收獲機(jī)轉(zhuǎn)彎或逼近期望路徑，需要滿足速度較小、有較大的航向或者和橫向誤差時，輸出較小的前視距離；2）當(dāng)收獲機(jī)在高速穩(wěn)定作業(yè)狀態(tài)，需要滿足速度較大、航向誤差和橫向誤差皆較小時，輸出較大的前視距離；3）當(dāng)收獲機(jī)臨近作業(yè)結(jié)束，需要滿足速度較小、航向誤差和橫向誤差皆較小時，輸出1 個中等的前視距離；4）當(dāng)收獲機(jī)收獲機(jī)前進(jìn)速度較大，航向或者橫向誤差存在條較大時，輸出較小的前視距離。

最終模糊控制器會根據(jù)輸入?yún)?shù)和模糊規(guī)則，輸出合適的前視距離，將輸出的前視距離代入至式（5），可以提高收獲機(jī)在不同的速度、橫向偏差和航向偏差下的路徑追蹤的精度和穩(wěn)定性，有效提高路徑追蹤算法的自適應(yīng)能力。

3 實驗與結(jié)果分析

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

圖5 主要設(shè)備安裝位置示意圖

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

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

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

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

圖6 路徑追蹤實驗效果

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

3 次實驗的橫向誤差的標(biāo)準(zhǔn)差如表1 所示。

實驗結(jié)果表明：本研究提出的路徑追蹤算法的效果優(yōu)于傳統(tǒng)的純追蹤算法，速度在1 和2.5 m/s時，橫向誤差的標(biāo)準(zhǔn)差都低于2.5 cm。

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

表1 橫向誤差實驗效果

4 結(jié)束語

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