基于預(yù)適應(yīng)路徑追蹤的收割機(jī)RAC導(dǎo)航系統(tǒng)

張文利，郭 宇，陳開臻，偉利國，苑嚴(yán)偉※

（1.北京工業(yè)大學(xué)信息學(xué)部，北京100124；2.中國農(nóng)業(yè)機(jī)械化科學(xué)研究院，北京100083）

0 引言

近年來，隨著衛(wèi)星定位數(shù)據(jù)精度提高，各類路徑追蹤方法層出不窮，越來越多的高精度農(nóng)機(jī)自動駕駛系統(tǒng)被研發(fā)、生產(chǎn)。通過在傳統(tǒng)的農(nóng)用機(jī)械上部署自動導(dǎo)航系統(tǒng)，能夠?qū)崿F(xiàn)農(nóng)機(jī)的全自動作業(yè)，滿足了精準(zhǔn)農(nóng)業(yè)的需求［1-2］。

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

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

因此，文章提出了一種預(yù)適應(yīng)路徑追蹤模型，在模糊純追蹤模型的基礎(chǔ)上引入預(yù)測控制理論，通過內(nèi)置收割機(jī)運(yùn)動學(xué)模型，對收割機(jī)下一周期運(yùn)動狀況進(jìn)行預(yù)測，根據(jù)預(yù)測的當(dāng)前周期位姿偏差結(jié)果調(diào)整前視距離參數(shù)，從而提高追蹤精度，降低誤 差。

1 收割機(jī)RAC導(dǎo)航系統(tǒng)

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

圖1收割機(jī)RAC 導(dǎo)航系統(tǒng)框圖Fig.1 Block diagram of harvester automatic navigation system

2 預(yù)適應(yīng)路徑追蹤模型的設(shè)計(jì)

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

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

根據(jù)純追蹤模型原理，收割機(jī)前輪轉(zhuǎn)角控制量滿足公式（1）：

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

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

①確定輸入變量

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

②確定論域、模糊集

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

③確定隸屬函數(shù)

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

圖3隸屬函數(shù)Fig.3 Membership function

④建立模糊控制規(guī)則表

如表1所示，根據(jù)農(nóng)機(jī)在實(shí)際作業(yè)中的情況，總結(jié)經(jīng)驗(yàn)得到49條規(guī)則，建立前視距離的模糊控制規(guī)則表。

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

預(yù)測模塊可以在農(nóng)機(jī)開始作業(yè)之前，將農(nóng)機(jī)運(yùn)動學(xué)模型輸入模塊中，當(dāng)模糊純追蹤控制器計(jì)算出前輪轉(zhuǎn)角控制量后，預(yù)測模塊先行將該控制量輸入到農(nóng)機(jī)運(yùn)動學(xué)模型中，計(jì)算下一個周期農(nóng)機(jī)位置的改變，通過該模塊，可以獲得“預(yù)測航向誤差”和“預(yù)測位置誤差”2個預(yù)測量。

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

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

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

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

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

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

3 結(jié)果分析

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

3.1 模擬仿真實(shí)驗(yàn)

通過Matlab+Simulink聯(lián)合平臺，分別搭建模糊純追蹤模型和預(yù)適應(yīng)路徑追蹤模型，并對2種路徑追蹤模型進(jìn)行模擬仿真，結(jié)果如圖5所示。

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

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

3.2 農(nóng)機(jī)應(yīng)用實(shí)驗(yàn)

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

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

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

表2預(yù)適應(yīng)路徑追蹤模型橫向誤差統(tǒng)計(jì)結(jié)果Table 2 Statistical result of lateral error of pre-adaptive path tracking model

經(jīng)過測試，當(dāng)收割機(jī)以3 km/h的速度行駛時(shí)，采用搭載了預(yù)適應(yīng)路徑追蹤模型的收割機(jī)RAC導(dǎo)航系統(tǒng)，路徑追蹤誤差的平均值為0.63 m，考慮到RAC衛(wèi)星接收機(jī)定位精度為亞米級，路徑追蹤誤差符合預(yù)期。

4 結(jié)論

該文通過引入預(yù)測控制理論，對模糊純追蹤模型響應(yīng)速度慢的缺陷進(jìn)行改良，構(gòu)建適應(yīng)能力較強(qiáng)的預(yù)適應(yīng)路徑追蹤模型。該模型采用高性價(jià)比RAC衛(wèi)星接收設(shè)備，在滿足收割機(jī)工作精度的前提下，降低了設(shè)備部署難度和設(shè)備價(jià)格。該文通過Matlab+Simulink聯(lián)合平臺對模糊純追蹤模型和預(yù)適應(yīng)路徑追蹤模型進(jìn)行仿真分析，發(fā)現(xiàn)預(yù)適應(yīng)路徑追蹤模型追蹤效果更好，平均誤差得到大幅度降低。通過在收割機(jī)上使用RAC導(dǎo)航系統(tǒng)的實(shí)際測試發(fā)現(xiàn)，預(yù)適應(yīng)路徑追蹤模型平均誤差值為0.63 m，滿足設(shè)計(jì)要求。

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