田間管理機路徑跟蹤系統(tǒng)設(shè)計及試驗

黃騰達，蔣蘋，胡文武，肖菲菲，吳帆，劉展名

（湖南農(nóng)業(yè)大學(xué)機電工程學(xué)院，長沙410128）

農(nóng)業(yè)機械導(dǎo)航技術(shù)有效地減輕了勞動強度，提高了生產(chǎn)效率，是精準農(nóng)業(yè)體系中的重要組成部分[1-3]。自動轉(zhuǎn)向是實現(xiàn)農(nóng)機導(dǎo)航駕駛的關(guān)鍵技術(shù)。目前，農(nóng)業(yè)機械自動轉(zhuǎn)向研究集中在機械轉(zhuǎn)向和全液壓轉(zhuǎn)向器自動化技術(shù)改進上，主要方法是在原有的轉(zhuǎn)向系統(tǒng)上加裝電機驅(qū)動轉(zhuǎn)向或者并聯(lián)電控轉(zhuǎn)向液壓系統(tǒng)[4-5]。現(xiàn)有的自動轉(zhuǎn)向方案技術(shù)成本較高，自動轉(zhuǎn)向系統(tǒng)和手動轉(zhuǎn)向系統(tǒng)相互獨立。

路徑控制算法是農(nóng)機導(dǎo)航駕駛的核心。近年來，國內(nèi)外導(dǎo)航路徑跟蹤控制算法有比例-積分-微分（proportion-integration-differentiation, PID）控制[6]、純追蹤算法[7-12]、模糊控制[13-19]、神經(jīng)網(wǎng)絡(luò)[20-21]等。其中，純追蹤算法應(yīng)用最廣泛，并且，為了改善單獨的純追蹤算法路徑跟蹤上線效果不佳問題，研究者對該算法進行了優(yōu)化改進。如：張聞宇等[22]建立雙切圓尋線模型，設(shè)置門限參數(shù)條件確保轉(zhuǎn)向相切上線，其中對有限的拖拉機門限參數(shù)進行設(shè)置，使得控制方法適應(yīng)性有限；陳康等[23]使用兩步航向校正模型，在車體已經(jīng)偏向規(guī)劃路徑時減小期望轉(zhuǎn)向角，對車體航向角進行校正，但試驗平均誤差較大，銜接過渡不平滑；王輝等[24]以預(yù)瞄追蹤模型采用二元線性控制律和極點配置方法構(gòu)建線性最優(yōu)控制算法，該算法可在小角度線性近似條件下取得較好的效果，但大角度偏差僅考慮了單因素航向偏差不合理問題。上述路徑跟蹤控制算法通過建立數(shù)學(xué)分析模型，解決了模型中農(nóng)機在靠近規(guī)劃路徑時車輛航向與規(guī)劃路徑偏差較大的問題。為了更好地解決這一問題，本文提出了一種基于四輪轉(zhuǎn)向前視距離在線調(diào)整路徑跟蹤算法，該算法根據(jù)純追蹤算法決策期望轉(zhuǎn)向角，并以橫向偏差、航向偏差作為輸入，以前視距離系數(shù)為輸出的模糊控制在線修整前視距離參數(shù)，不僅實現(xiàn)了不同速度前視距離自適應(yīng)調(diào)整，而且解決了上線位置不佳的問題，為農(nóng)業(yè)機械自動駕駛提供了一定的理論依據(jù)。

同時，本文設(shè)計了一種電控手柄液壓轉(zhuǎn)向方案：當(dāng)進行手動轉(zhuǎn)向操作時，控制器可根據(jù)手柄動作行程進行轉(zhuǎn)向角控制；當(dāng)進行自動轉(zhuǎn)向時，控制器可根據(jù)上位機期望轉(zhuǎn)向角完成自動轉(zhuǎn)向。同現(xiàn)有的自動轉(zhuǎn)向方案比較，該方案不需要安裝其他執(zhí)行元件，電控手柄操作靈活，轉(zhuǎn)向精度能滿足農(nóng)機轉(zhuǎn)向控制要求。

1 轉(zhuǎn)向結(jié)構(gòu)設(shè)計及線控轉(zhuǎn)向原理

1.1 轉(zhuǎn)向結(jié)構(gòu)設(shè)計

3WPZ-750田間管理機采用高地隙通用作業(yè)底盤設(shè)計[25]，結(jié)構(gòu)如圖1所示。其轉(zhuǎn)向系統(tǒng)主要由4個獨立轉(zhuǎn)向支腿組成，其中轉(zhuǎn)向支腿、轉(zhuǎn)向軸安裝固定在支腿套管中，由轉(zhuǎn)向馬達提供轉(zhuǎn)向動力。支腿套管上的垂直立板鉸接平行四邊形結(jié)構(gòu)一端，另一端立板焊接固定在作業(yè)底盤上。車輪轉(zhuǎn)向角度傳感器旋轉(zhuǎn)軸與轉(zhuǎn)向軸平行布置，安裝固定在支腿垂直立板上，通過柔性齒輪與轉(zhuǎn)向軸上的齒輪嚙合，實時監(jiān)測車輪轉(zhuǎn)向角度。車輪轉(zhuǎn)向力矩通過支腿直角支撐架傳遞至車輪。支腿直角支撐架上部安裝行走馬達，通過行走鏈輪箱向車輪傳遞行走動力。

1.2 自動轉(zhuǎn)向

田間管理機液壓轉(zhuǎn)向系統(tǒng)如圖2所示。田間管理機液壓轉(zhuǎn)向系統(tǒng)由前后輪轉(zhuǎn)向電磁閥串聯(lián)相應(yīng)轉(zhuǎn)向馬達，每個液壓馬達并聯(lián)轉(zhuǎn)向旁通閥，當(dāng)在兩側(cè)車輪出現(xiàn)因負載不同導(dǎo)致轉(zhuǎn)向不同步情況時，控制器控制轉(zhuǎn)向旁通閥進行轉(zhuǎn)向同步性調(diào)整。控制器采用線控技術(shù)實時監(jiān)測轉(zhuǎn)向電控手柄目標轉(zhuǎn)向角、轉(zhuǎn)向角度傳感器實際轉(zhuǎn)向角，構(gòu)成轉(zhuǎn)向閉環(huán)控制。在轉(zhuǎn)向過程中，控制器控制轉(zhuǎn)向電磁閥上的轉(zhuǎn)向電磁閥動作，當(dāng)一側(cè)車輪轉(zhuǎn)向至目標角度時，對應(yīng)的兩位三通旁通閥導(dǎo)通該側(cè)車輪，直至兩側(cè)車輪轉(zhuǎn)向同步。

圖1 田間管理機機械結(jié)構(gòu)Fig.1 Mechanical structure of field management machine

圖2 液壓轉(zhuǎn)向系統(tǒng)示意圖Fig.2 Schematic diagram of hydraulic steering system

為了提高自動液壓轉(zhuǎn)向精度，采用卡爾曼濾波方法，減小工作過程中噪聲等干擾對角度傳感器的影響，試驗結(jié)果如圖3所示。其中，靜態(tài)無濾波角度傳感器電壓值標準差為0.03 V，啟動后振動噪聲干擾角度傳感器電壓值標準差為0.028 V，啟動后卡爾曼濾波角度傳感器電壓值標準差為0.02 V。表明該方法有效地抑制了工作噪聲干擾，角度傳感器采樣電壓值標準差降低了33%左右，提高了自動轉(zhuǎn)向精度。

另外，使用電腦向控制器發(fā)送單個車輪轉(zhuǎn)向命令后，記錄目標轉(zhuǎn)向角和實際車輪轉(zhuǎn)向角，供轉(zhuǎn)向分析。將目標轉(zhuǎn)向角和實際車輪轉(zhuǎn)向角進行線性擬合，得出的左前、右前、左后、右后轉(zhuǎn)向輪線性擬合關(guān)系式如下。

圖3 角度傳感器去噪效果Fig.3 Denoising effect of angle sensor

轉(zhuǎn)向角誤差如圖4 所示：角度傳感器實際轉(zhuǎn)向角與目標轉(zhuǎn)向角構(gòu)成的轉(zhuǎn)向閉環(huán)控制誤差范圍為－0.5°～0.5°，滿足農(nóng)業(yè)機械自動轉(zhuǎn)向要求。

圖4 目標轉(zhuǎn)向角與實際轉(zhuǎn)向角誤差Fig.4 Error between target steering angle and actual steeringangle

另外，為了評價該自動液壓轉(zhuǎn)向系統(tǒng)的響應(yīng)性，對該液壓系統(tǒng)響應(yīng)時間進行試驗。具體試驗操作步驟：1）啟動田間管理機；2）通過上位機發(fā)送“車輪轉(zhuǎn)向30°”的命令；3）使用雷諾液壓測試儀實時監(jiān)測轉(zhuǎn)向馬達流量大??；4）從使用的雷諾液壓測試儀中導(dǎo)出轉(zhuǎn)向馬達運行時間和流量，為自動轉(zhuǎn)向試驗結(jié)果分析提供參考。測得土槽中土壤濕度為72%時，車輪轉(zhuǎn)向至最大轉(zhuǎn)向角的時間為3.8 s；在水泥路面所需時間為1.9 s。根據(jù)轉(zhuǎn)向馬達流量、壓力數(shù)據(jù)計算分析，土槽中轉(zhuǎn)向馬達進出口壓差達8.111 MPa，轉(zhuǎn)向扭矩為145.3 N·m；水泥地面上轉(zhuǎn)向馬達進出口壓差達1.224 MPa，轉(zhuǎn)向扭矩為21.927 N·m。總體來看，復(fù)雜土壤地況所需轉(zhuǎn)向扭矩比水泥平坦地面大，轉(zhuǎn)向響應(yīng)時間更長，說明自動轉(zhuǎn)向液壓系統(tǒng)轉(zhuǎn)向響應(yīng)和穩(wěn)定性有效。

2 模型與分析

2.1 路徑跟蹤技術(shù)路線

為了實現(xiàn)路徑跟蹤功能，本研究在原有車載電子控制單元（electronic control unit, ECU）上設(shè)計了一套基于微軟集成開發(fā)環(huán)境（visual studio）的上位機路徑跟蹤控制界面。上位機接收來自北斗定位系統(tǒng)的車體位姿、速度信息后，通過純追蹤算法搜索規(guī)定路徑上的預(yù)瞄點，經(jīng)算法處理獲得期望的車輪轉(zhuǎn)向角、車速信號，通過串口通信傳遞到車載控制器，從而控制執(zhí)行相應(yīng)轉(zhuǎn)向和行走動作，其技術(shù)路線如圖5所示。

圖5 路徑跟蹤技術(shù)路線Fig.5 Technical route of path tracking

2.2 四輪轉(zhuǎn)向模型分析

2.3 純追蹤算法改進

純追蹤模型被廣泛應(yīng)用在車輛和機器人的路徑跟蹤研究上[26-28]。如圖7所示：車體轉(zhuǎn)向中心N（x，y）在規(guī)劃路徑T 上的目標點為M（gx，gy）；O 為轉(zhuǎn)向圓心；弧NM 半徑為R；L0為橫向偏差；θ 為航向偏差；2θ為沿弧NM到目標點M的圓心角；L為車身軸距；E為前視距離。

根據(jù)圖7，由三角形MNO正弦定理可得：

由四輪轉(zhuǎn)向模型分析已知轉(zhuǎn)向角與轉(zhuǎn)向半徑的關(guān)系如下：

圖6 四輪轉(zhuǎn)向模型Fig.6 Model of four-wheel steering

圖7 純追蹤模型Fig.7 Pure tracking model

由式（5）～（7）可得車輪轉(zhuǎn)向角與橫向偏差、前視距離的關(guān)系式：

式（8）闡明了期望車輪轉(zhuǎn)向角與前視距離參數(shù)取值問題。固定前視距離不能滿足快速上線要求，實時動態(tài)最優(yōu)前視距離參數(shù)難以確定。因此，純追蹤模型前視距離應(yīng)根據(jù)橫向偏差、航向偏差和車速合理地進行在線動態(tài)調(diào)整。

前視距離的取值與當(dāng)前位置橫向偏差、航向偏差和車速有直接關(guān)系。參考車速與前視距離研究文獻[29]，車速（v）和前視距離（E）的關(guān)系可以表示為：

{2≤E ＜4 m，0＜v ＜10 km/h；4≤E ＜5m，10≤v ＜20 km/h；5≤E ＜6 m，20≤v ＜30 km/h.

田間管理機在作業(yè)過程中車速較低且變化不大，通常認為是勻速作業(yè)，車速為前視距離取值提供了參考區(qū)間。所以，前視距離在線動態(tài)調(diào)整時應(yīng)以橫向偏差、航向偏差作為模糊控制輸入，獲取該車速區(qū)間內(nèi)前視距離值。

根據(jù)實際駕駛操作，前視距離在線調(diào)整模糊控制器的規(guī)則是：當(dāng)航向偏差和橫向偏差較大時，減小前視距離來消除偏差，使農(nóng)機航向偏差和橫向偏差迅速得到糾正；當(dāng)航向偏差和橫向偏差較小時，以增大前視距離來使農(nóng)機穩(wěn)定地沿規(guī)劃路徑行駛[30-32]。前視距離在線調(diào)整主要是指在某一車速區(qū)間內(nèi)調(diào)整橫向偏差和航向偏差的權(quán)重，以得到較好的在線前視距離[33]，前視距離在線調(diào)整模糊控制規(guī)則如表1 所示。通過田間管理機實際操作，定義車體橫向偏差、航向偏差相對規(guī)劃路徑左偏差為負值，右偏差為正值；車輪轉(zhuǎn)向角相對車輪零度角左偏差為正值，右偏差為負值，最大轉(zhuǎn)向角為30°。

表1 模糊控制規(guī)則表Table 1 Regulation table of fuzzy control

橫向偏差L0基本論域為［－0.6 m，0.6 m］，量化等級為{－6，－5，－4，－3，－2，－1，0，1，2，3，4，5，6}，量化因子KL0為0.1。航向偏差θ 基本論域［－90°，90°］，量化等級為{－6，－5，－4，－3，－2，－1，0，1，2，3，4，5，6}，量化因子Kθ為1/15。前視距離控值系數(shù)k 基本論域為［0，1 m］，量化等級為{－6，－5，－4，－3，－2，－1，0，1，2，3，4，5，6}，量化因子Kk為6。

由前視距離模糊控制系數(shù)與實時車速前視距離區(qū)間得到的前視距離表達式為：

式中：k為前視距離模糊控制系數(shù)；SE為實時車速前視距離區(qū)間長度值；E0為實時車速前視距離基值。

使用MATLAB軟件對該算法模型進行仿真，設(shè)定相關(guān)初始條件：橫向偏差為2 m，航向偏差為53°，初始車輪轉(zhuǎn)向角度為0，仿真車速為2 m/s，k取值為1，SE為1 m，E0為2 m。仿真結(jié)果如圖8所示。從中可知，在橫向偏差2 m 的條件下，車體橫向偏差在2 s 靠近規(guī)劃路徑，其中航向偏差、轉(zhuǎn)向角度突然變化是根據(jù)模糊控制規(guī)則為了穩(wěn)定完成上線操作對前視距離動態(tài)調(diào)整引起的，4 s 后車體橫向偏差、航向偏差滿足田間管理機上線穩(wěn)定要求。從整體上看，該路徑跟蹤方法能迅速消除橫向偏差，振蕩較小，上線曲線平滑，具有較好的動態(tài)特性。表明該前視距離在線調(diào)整路徑跟蹤算法對提高田間管理機上線響應(yīng)性和穩(wěn)定性是可行的。

3 驗證與討論

3.1 試驗平臺信息

圖8 仿真效果Fig.8 Simulation effect

田間管理機試驗平臺如圖9 所示，其搭載了北斗定位系統(tǒng)、上位機、控制器、液壓執(zhí)行元件等。北斗定位系統(tǒng)采用“合眾思壯壁虎”北斗高精度全球?qū)Ш叫l(wèi)星系統(tǒng)差分接收機，差分定位精度為2 cm；采用Robobrain公司的霍爾型角度傳感器，基本參數(shù)為工作電壓DC-24 V，輸出0～5 V 模擬量信號，精度為0.5%FS。試驗平臺上位機技術(shù)路線設(shè)計思路：上位機先將北斗定位的經(jīng)緯度投影轉(zhuǎn)換至平面坐標，其中高斯投影采用我國2000 國家大地坐標系橢球參數(shù)；然后利用純追蹤算法模型搜索規(guī)劃路徑上的預(yù)瞄點，以獲取當(dāng)前位置下的橫向偏差、航向偏差和車速，經(jīng)前視距離在線調(diào)整后得出期望四輪轉(zhuǎn)向角。

圖9 試驗平臺Fig.9 Experimental platform

3.2 直線路徑跟蹤試驗

為驗證路徑跟蹤效果，分別在水泥路面和田間不同作業(yè)環(huán)境下進行直線跟蹤試驗。具體試驗過程如下：1）在試驗場地預(yù)先使用北斗定位系統(tǒng)規(guī)劃好跟蹤路徑；2）將田間管理機放置在規(guī)劃路徑起點；3）啟動田間管理機，置油門開度于1/2 處；4）田間管理機從規(guī)劃路徑起點處開始行走，上位機導(dǎo)航控制界面通過實時監(jiān)控四輪轉(zhuǎn)向角實現(xiàn)田間管理機沿規(guī)劃路徑直線跟蹤，至規(guī)劃路徑終點；5）從上位機導(dǎo)航控制界面導(dǎo)出路徑跟蹤過程參數(shù)，為后續(xù)試驗結(jié)果分析提供參考依據(jù)。路徑跟蹤過程參數(shù)主要有實時行駛軌跡定位坐標、橫向偏差、航向偏差等，實驗數(shù)據(jù)處理結(jié)果均以一次路徑跟蹤所得。水泥路面場地為湖南農(nóng)業(yè)大學(xué)西籃球場，水田試驗場地為湖南農(nóng)業(yè)大學(xué)耘園水稻試驗田，水稻生長處于灌漿結(jié)實期，土質(zhì)較黏。

田間管理機在水泥路面直線路徑跟蹤時車輛擺放初始橫向偏差、航向偏差較大，橫向偏差實際測量值為0.1 m，但上線后行駛軌跡與規(guī)劃路徑基本一致，水泥路面試驗中行駛軌跡如圖10所示。在水田直線路徑跟蹤時，由于場地條件有限，車輛擺放初始橫向偏差、航向偏差較小，橫向偏差實際測量值為0.05 m，且車輪下陷量較大，實際測量值為25 cm，晃動較小，水田試驗中行駛軌跡如11 圖所示。試驗結(jié)果表明：在上線過程中水泥路面路徑跟蹤誤差均值為6.92 cm，標準差為4.84 cm；水田路徑跟蹤誤差均值為2.25 cm，標準差為1.35 cm。在不同環(huán)境下，路徑跟蹤控制偏差基本符合設(shè)計要求，但在水泥路面試驗效果較水田試驗效果差。分析原因可知：田間管理機在水泥路面和水田中的車輪附著力不相同，水田環(huán)境下因土壤較黏，車輪下陷，車輪附著力大于水泥路面。且當(dāng)在水泥路面行走時液壓系統(tǒng)中分流集流閥未達到最佳分流狀態(tài)，4個車輪轉(zhuǎn)速不一致，致使田間管理機不停地進行轉(zhuǎn)向上線調(diào)整，跟蹤誤差較大；而在水田環(huán)境下田間管理機車輪同步誤差較小，雖然在開始進行跟蹤時位置誤差較大，但靠近規(guī)劃路徑后，車體會沿著以前的車轍繼續(xù)行駛，跟蹤誤差明顯減小。

3.3 曲線路徑跟蹤試驗

參考農(nóng)藝農(nóng)機作業(yè)規(guī)范要求和路徑規(guī)劃轉(zhuǎn)向方式，田間管理機在大田進行植保作業(yè)時完成一行后需轉(zhuǎn)向180°才能進入另一行繼續(xù)作業(yè)。因作業(yè)寬幅為12 m，故采用牛耕法弓形轉(zhuǎn)向換行作業(yè)操作。所以，田間管理機曲線路徑跟蹤試驗采用最小轉(zhuǎn)彎半徑定曲率曲線機進行跟蹤試驗，跟蹤車速設(shè)置為0.2 m/s，具體試驗步驟參考直線路徑跟蹤試驗。如圖12～13所示：在水泥路面定曲率曲線路徑跟蹤誤差均值為12.49 cm，標準差為9.16 cm；在水田路徑跟蹤誤差均值為8.72 cm，標準差為5.59 cm。對水泥路面和水田曲線路徑跟蹤試驗結(jié)果進行分析，水田曲線跟蹤試驗車輪在泥土中有25 cm 左右沉陷，前后車輪轉(zhuǎn)向附著力比水泥路面大，因此在進行轉(zhuǎn)向時，行走液壓系統(tǒng)中分流集流閥達到最佳分流狀態(tài)，車輪速度同步誤差小，致使水田曲線跟蹤效果優(yōu)于水泥路面曲線跟蹤效果。

4 結(jié)論

目前，我國農(nóng)業(yè)機械自動導(dǎo)航技術(shù)多在原來的全液壓或機械轉(zhuǎn)向上進行自動轉(zhuǎn)向改裝，采用純追蹤算法作路徑跟蹤；其純追蹤算法多以兩因素進行分析。這種方法雖然能達到自動駕駛目的，但人工駕駛、自動駕駛轉(zhuǎn)向系統(tǒng)不能共用一種設(shè)計方案，成本較高，不利于廣泛應(yīng)用。

圖10 水泥路面試驗效果Fig.10 Experimental effect on cement pavement

本研究采用四輪獨立液壓轉(zhuǎn)向設(shè)計方案，不同于前輪轉(zhuǎn)向模型路徑跟蹤，并以車載控制器作為中控單元來有效地將人工駕駛、自動駕駛轉(zhuǎn)向系統(tǒng)結(jié)合，并以速度、橫向偏差、航向偏差三因素優(yōu)化純追蹤算法前視距離參數(shù)，解決了四輪獨立自動轉(zhuǎn)向設(shè)計方案和純追蹤算法多因素影響問題，減少了相應(yīng)成本。并通過微軟集成開發(fā)環(huán)境（visual studio）建立了上位機軟件控制系統(tǒng)，以及通過MATLAB軟件進行上線仿真，得到4 s穩(wěn)定上線結(jié)果，能夠滿足田間管理機自動導(dǎo)航的功能，為實際應(yīng)用提供了一定的依據(jù)。

圖11 水田試驗效果Fig.11 Experimental effect of paddy field

圖12 水泥路面曲線跟蹤Fig.12 Curve tracking on cement pavement

圖13 水田曲線跟蹤Fig.13 Curve tracking in paddy field

試驗結(jié)果表明：在0.5 m/s的車速下，田間管理機在水泥路面環(huán)境下，路徑跟蹤誤差均值為6.92 cm，其標準差為4.84 cm，定曲率曲線路徑跟蹤誤差均值為12.49 cm，其標準差為9.16 cm；在0.2 m/s 的車速下，田間管理機在南方水田環(huán)境下，路徑跟蹤誤差均值為2.25 cm，其標準差為1.35 cm，定曲率曲線路徑跟蹤誤差均值為8.72 cm，其標準差為5.59 cm。通過MATLAB 軟件仿真、路面與田間試驗驗證，其自動導(dǎo)航效果能夠滿足田間管理機行駛和田間作業(yè)需要。所以，本研究的路徑跟蹤系統(tǒng)符合農(nóng)業(yè)機械自動導(dǎo)航的設(shè)計要求。

在試驗過程中，發(fā)現(xiàn)影響田間作業(yè)環(huán)境的因素較多，其中，水田土壤堅實度不同對田間管理機四輪自動轉(zhuǎn)向的同步性也不同，后續(xù)有待進一步研究水田不同土壤堅實度對四輪轉(zhuǎn)向同步性的影響，以調(diào)整相應(yīng)轉(zhuǎn)向液壓設(shè)計參數(shù)，保證水田四輪轉(zhuǎn)向同步性和響應(yīng)時間。此外，還需進一步研究該路徑跟蹤算法響應(yīng)時間、誤差精度以及轉(zhuǎn)向支腿結(jié)構(gòu)的設(shè)計。