基于模糊Stanley模型的農(nóng)機(jī)全田塊路徑跟蹤算法研究

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

(1.江蘇大學(xué)農(nóng)業(yè)工程學(xué)院， 鎮(zhèn)江 212013； 2.江蘇大學(xué)現(xiàn)代農(nóng)業(yè)裝備與技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室， 鎮(zhèn)江 212013)

0 引言

農(nóng)業(yè)機(jī)械(簡(jiǎn)稱農(nóng)機(jī))智能化是無(wú)人農(nóng)場(chǎng)和智慧農(nóng)業(yè)的重要支撐，而自動(dòng)導(dǎo)航是智能化農(nóng)機(jī)的核心技術(shù)[1]。農(nóng)機(jī)自動(dòng)導(dǎo)航為疫情防控和糧食安全提供了重要保障。路徑跟蹤算法控制農(nóng)機(jī)沿預(yù)定軌跡實(shí)現(xiàn)自主行駛，是農(nóng)機(jī)田間無(wú)人化自主作業(yè)的重要環(huán)節(jié)[2-3]。

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

Stanley模型是一種基于橫向跟蹤誤差的非線性反饋函數(shù)，其橫向跟蹤誤差以指數(shù)收斂于零[11-12]，但其在農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航上的應(yīng)用報(bào)道較少。為提高農(nóng)業(yè)機(jī)械全田塊自主作業(yè)導(dǎo)航精度和穩(wěn)定性，本文提出一種基于改進(jìn)Stanley模型的路徑跟蹤控制算法，采用模糊算法實(shí)現(xiàn)控制增益系數(shù)的自適應(yīng)調(diào)整，提高農(nóng)機(jī)自動(dòng)轉(zhuǎn)彎路徑跟蹤精度，并減小初始誤差較大時(shí)的上線距離。以移動(dòng)小車為試驗(yàn)平臺(tái)，進(jìn)行聯(lián)合收獲機(jī)全田塊作業(yè)路徑跟蹤試驗(yàn)，以驗(yàn)證算法的有效性。

1 Stanley路徑跟蹤算法

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

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

(1)

式中δ(t)——期望轉(zhuǎn)角

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

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

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

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

(2)

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

v(t)——行駛速度

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

δθ(t)=θe(t)

(3)

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

綜合考慮這兩方面的影響，得出前輪期望轉(zhuǎn)角函數(shù)為

(4)

2 參數(shù)自適應(yīng)方法

常用的參數(shù)自適應(yīng)方法有智能搜索算法、神經(jīng)網(wǎng)絡(luò)算法和模糊算法等[13]。智能搜索算法主要包括遺傳算法、粒子群優(yōu)化算法以及差分進(jìn)化算法[14-16]。神經(jīng)網(wǎng)絡(luò)算法根據(jù)激勵(lì)函數(shù)的不同可分為BP神經(jīng)網(wǎng)絡(luò)、RBF神經(jīng)網(wǎng)絡(luò)、PNN神經(jīng)網(wǎng)絡(luò)以及GRNN神經(jīng)網(wǎng)絡(luò)等[17-20]。相較于其他參數(shù)自適應(yīng)方法，模糊算法具有計(jì)算量小、實(shí)時(shí)性好、魯棒性強(qiáng)、適合單片機(jī)系統(tǒng)等優(yōu)點(diǎn)[21]，本文采用模糊算法對(duì)Stanley模型增益系數(shù)進(jìn)行動(dòng)態(tài)調(diào)整。

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

考慮農(nóng)機(jī)實(shí)際工作情況，以橫向偏差和航向偏差為輸入變量構(gòu)建隸屬度函數(shù)，限定前輪最大轉(zhuǎn)角為35°，初步制定模糊推理規(guī)則。以橫向偏差最大值和標(biāo)準(zhǔn)差為觀測(cè)量，進(jìn)行多次試驗(yàn)，尋找不同誤差輸入下最優(yōu)的增益系數(shù)，修正模糊推理表。

2.1 隸屬度函數(shù)構(gòu)建

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

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

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

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

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

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

2.2 模糊推理表設(shè)計(jì)

模糊推理表具體規(guī)則為：航向偏差較小時(shí)，橫向偏差越大，增益系數(shù)越大；航向偏差與橫向偏差對(duì)車輪轉(zhuǎn)角影響方向相同，航向偏差越大，增益系數(shù)越小；航向偏差與橫向偏差對(duì)車輪轉(zhuǎn)角影響方向相反，航向偏差越大，增益系數(shù)越大；航向偏差較大時(shí)，與橫向偏差對(duì)前輪轉(zhuǎn)角影響方向相同，選較小增益系數(shù)；航向偏差較大時(shí)，與橫向偏差對(duì)前輪轉(zhuǎn)角影響方向相反，選較大增益系數(shù)。據(jù)此，共總結(jié)49條模糊推理規(guī)則，并通過試驗(yàn)進(jìn)行修正，制定如表1所示的模糊推理規(guī)則表。

2.3 解模糊化方法確定

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

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

(5)

式中u0——輸出系數(shù)m——級(jí)數(shù)

uk——各級(jí)系數(shù)μk(uk)——隸屬度

3 試驗(yàn)與結(jié)果分析

3.1 試驗(yàn)平臺(tái)搭建

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

移動(dòng)小車試驗(yàn)平臺(tái)結(jié)構(gòu)如圖5所示，接收機(jī)通過天線和電臺(tái)同時(shí)接收衛(wèi)星信號(hào)和基站差分改正數(shù)，進(jìn)行RTK定位解算，實(shí)時(shí)輸出高精度位置與航向信息至導(dǎo)航上位機(jī)。上位機(jī)通過計(jì)算，輸出當(dāng)前位置與規(guī)劃路徑間的橫向偏差、航向偏差。下位機(jī)根據(jù)導(dǎo)航偏差計(jì)算出期望轉(zhuǎn)向角，通過CAN總線以頻率5 Hz將控制信息發(fā)送至電機(jī)驅(qū)動(dòng)器。電機(jī)驅(qū)動(dòng)器將控制信息轉(zhuǎn)換為電壓，并實(shí)時(shí)接收編碼器的反饋，組成閉環(huán)控制系統(tǒng)，以頻率100 Hz控制電機(jī)。

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

3.2 路徑規(guī)劃方案

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

圖6 聯(lián)合收獲機(jī)全田塊路徑規(guī)劃示意圖Fig.6 Field path planning schematic of combine harvester

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

3.3 試驗(yàn)結(jié)果分析

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

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

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

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

采用模糊算法改進(jìn)前后的路徑跟蹤橫向誤差如圖9所示。直線跟蹤時(shí)，模糊Stanley模型減小了路徑跟蹤的上線距離，10 m以后2種方法的穩(wěn)態(tài)誤差無(wú)明顯差異，最大橫向誤差小于3 cm，標(biāo)準(zhǔn)差小于1 cm。曲線跟蹤階段的穩(wěn)態(tài)誤差改善明顯，路徑跟蹤誤差最大值由28 cm下降為3 cm，滿足轉(zhuǎn)彎對(duì)行精度要求。試驗(yàn)過程發(fā)現(xiàn)直線路徑跟蹤存在明顯的穩(wěn)態(tài)誤差，其可能原因包括轉(zhuǎn)向系統(tǒng)機(jī)械間隙和天線基線偏角引起的系統(tǒng)誤差。前者在農(nóng)業(yè)機(jī)械上影響可以忽略，后者可以通過作業(yè)現(xiàn)場(chǎng)標(biāo)定進(jìn)一步補(bǔ)償[24]。

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

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

3.4 聯(lián)合收獲機(jī)田間試驗(yàn)

以約翰迪爾C230型輪式聯(lián)合收獲機(jī)為試驗(yàn)對(duì)象進(jìn)一步驗(yàn)證算法有效性，在其上安裝雙天線RTK定位定向系統(tǒng)并進(jìn)行電控化改造。C230型收獲機(jī)作業(yè)幅寬為5 m，轉(zhuǎn)彎半徑約為8 m，其收獲作業(yè)時(shí)導(dǎo)航系統(tǒng)配置與小車試驗(yàn)平臺(tái)一致，全田塊路徑規(guī)劃采用圖6所示方案。行駛控制系統(tǒng)包括電動(dòng)方向盤、轉(zhuǎn)角傳感器、電動(dòng)油門以及卸糧、脫粒相關(guān)控制系統(tǒng)等，如圖11所示。其中電動(dòng)方向盤為上海聯(lián)適EMS2型轉(zhuǎn)向驅(qū)動(dòng)單元，工作電流可達(dá)10 A，具有扭矩大、精度高等特點(diǎn)，IP65級(jí)防塵防水，適合農(nóng)機(jī)惡劣作業(yè)環(huán)境。轉(zhuǎn)角傳感器型號(hào)為北京天?？乒镜腄WQCAB-V-CH，其線性度為0.02%FS，角分辨率0.022°，轉(zhuǎn)角傳感器安裝在收獲機(jī)轉(zhuǎn)向立軸正上方，與轉(zhuǎn)向中心立軸直連。

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

圖12 聯(lián)合收獲機(jī)路徑跟蹤軌跡Fig.12 Path tracking trajectory of combine harvester

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

4 結(jié)論

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

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

(3)聯(lián)合收獲機(jī)田間試驗(yàn)表明，天線安裝精度影響路徑跟蹤穩(wěn)態(tài)誤差，且導(dǎo)航系統(tǒng)位姿更新率和穩(wěn)定性制約著農(nóng)業(yè)機(jī)械自主作業(yè)的可靠性和作業(yè)效率。