基于RTK-BDS的果園農用車輛自主導航系統研究

郭成洋,張 碩,趙 健,陳 軍

(西北農林科技大學 機械與電子工程學院,陜西 楊凌 712100)

0 引言

目前,我國的水果生產在世界上位居前列,截至2016年水果種植面積約為1 725萬hm2,各類水果產量約為28 319萬t[1]。2019年,中央一號文件對“三農”工作進行了全新的部署,也對果園全程機械化發展提出了更高、更新的要求。自動導航技術作為現代精準農業的一個重要組成部分[2],可以減輕果農勞動強度,提高果園作業效率和質量[3],也是實現果園全程機械化管理的重要途徑[4]。

目前,能夠提供商業服務的GNSS包括美國的GPS、俄羅斯的GLONASS和中國的BDS系統[5]。Michael O’Connor[6]等人以John Deere 7800拖拉機為研究平臺,設計了一套基于四天線RTK-GPS的自主導航系統,試驗結果表明:當車輛行駛速度為0.9m/s時,直線跟蹤標準差不大于2.5cm。Zachary Pezzementi[7]等人將RTK-GPS應用于蘋果園和橘子園的導航定位,實現了果園作業機械的自主行走和自動收獲。日本國家農業研究中心[8]將久保田SPU650型水稻收割機進行改裝,通過MS750GPS和IMUJCS-7401A提供定位信息,試驗結果表明:平均橫向偏差小于3cm,最大橫向偏差不大于4cm。國內羅錫文[9]等人較早對RTK-GPS導航控制系統進行了研究,將東方紅X-804拖拉機進行改裝,田間試驗表明:控制系統具有較好的精度和可靠性。黎永鍵[10]等設計了一套基于RTK-DGPS和雙閉環轉向控制的自動導航定位系統,并以東方紅X-804型拖拉機為平臺,可將定位平均偏差降低至0.031m。劉軍[11]等人設計了一套基于GPS/INS的農用機械自動導航系統,試驗表明:該系統定位精度可達0.1m,直線路徑跟蹤偏差不大于0.27m。

BDS導航系統可在全球范圍內為用戶提供全天候、全天時的高精度、高可靠性的即時定位服務[12],且該系統完全由我國自主研制,在使用成本和安全性上均優于GPS定位系統,從而得到了廣泛關注。為此,結合卡爾曼濾波技術,設計了一套基于RTK-BDS的自適應純追蹤導航控制器,并進行了路徑仿真和田間試驗,證明導航系統具有良好的精度和可靠性。

1 車輛運動學模型建立

建立整車運動學模型是研究車輛自動導航控制的基礎[13]。在實際應用中,果園農用車輛常以低速行駛完成作業工作,因此可以將問題簡化為二維平面上的運動描述,并統一考慮農用車輛為前輪轉向、后輪驅動形式。為使模型的建立不失一般性及簡單性,將輪胎看作剛性輪,在建模中不考慮輪胎由于壓力產生的變形和側向滑移。

基于以上假設,以X-O-Y為固定于地面的慣性坐標系建立農用車輛兩輪模型,如圖1所示。其中,X-O-Y滿足右手法則。

圖1 車輛運動學模型Fig.1 Vehicle kinematics model。

由圖1可得表達式為

(1)

(2)

式中v—車輛行駛速度(m/s);

vx、vy—車輛沿x軸和y軸方向的速度(m/s);

vp—車輛前輪速度(m/s);

φ—航向角(rad);

δ—前輪轉角(rad);

L—前后兩輪軸中心的距離(m)。

把式(1)中vp代入到式(2),可以得出二輪車的運動學模型,即

(3)

(4)

式中R—車輛轉彎移動半徑(m)。

2 基于RTK-BDS的導航控制器設計

2.1 卡爾曼濾波器設計

卡爾曼濾波是一種時域方法,對于具有高斯分布噪聲的線性系統,應用該方法可以得到系統狀態的遞推最小均方差估計[14]。將控制系統視為線性系統,同時將獲取的第1個RTK-BDS點作為跟蹤軌跡的起點(即平面坐標系的原點)?？刂栖囕v前進運動速度為恒定值,RTK-BDS接收機的采樣頻率f=5Hz。

設系統的狀態向量X為

X=(x,y,vx,vy)T

(5)

(6)

式中x—車輛在平面坐標系內的橫坐標值(m);

y—車輛在平面坐標系內的縱坐標值(m);

vx、vy—車輛沿x軸和y軸方向的速度(m/s);

φ—航向角(rad)。

經過離散化后得

Xk=AXk-1+wk-1

(7)

式中Xk—車輛在k時刻被估計的狀態量;

A—狀態變換矩陣;

wk-1—協方差已知的零均值不相關白噪聲。

離散化后的量測方程為

Zk=HkXk+vk

(8)

式中Zk—車輛在k時刻的觀測值;

Hk—量測矩陣;

vk—符合正態分布的觀測噪聲。

vk的值可由式(9)計算得出,即

(9)

式中γk—均值為零的白噪聲。

(10)

式中A—狀態轉移矩陣。

(11)

(12)

式中Kk+1—k+1時刻的卡爾曼增益;

Zk+1—k+1時刻的實際觀測值。

定義觀測矩陣Zk+1可由式(13)得到,即

(13)

k+1時刻的卡爾曼增益可通過式(14)得到,即

(14)

式中P(k+1|k)—k時刻對k+1時刻的預測誤差協方差矩陣。

P(k+1|k)的值可通過式(15)得到,即

P(k+1|k)=APkAT+Q

(15)

由于RTK-BDS的定位精度已經可以達到厘米級,因此Q是均值為0的過程噪聲協方差對角矩陣。

Sk+1為中間變量,可通過式(16)得到,即

(16)

式中R—觀測時的高斯白噪聲協方差對角陣,定義為均值0m、標準差1m的對角陣。

Pk+1=(I-Kk+1Hk+1)P(k+1|k)

(17)

2.2 自適應純追蹤導航控制器設計

2.2.1 純追蹤模型

純追蹤模型算法是一種基于幾何形學、具有仿生性的算法,通過模擬車內駕駛員的視覺,確定合適的前視距離,計算出車輛到達指定位置所需走過的弧長,從而實現車輛的路徑追蹤[15]。建立純追蹤模型幾何表達示意圖,如圖2所示。其中,X-O-Y與二輪車運動學模型坐標系相同,即為滿足右手法則的完成坐標轉換后的大地直角坐標系;點P(x2,y2)為路徑上的一點,前視距離Ld為連接車輛重心點A(x1,y1)和點P(x2,y2)弧段的弦長;R為該弧段的半徑;Δx為A、P兩點的橫坐標差值;Δy為A、P兩點的縱坐標差值。

圖2 純追蹤模型幾何表達示意圖Fig.2 Geometric expression of pure pursuit model。

由幾何關系可得

(18)

由式(18)可得

(19)

結合二輪車模型可得前輪轉角δ公式為

(20)

在直線追蹤情況下,Δx的計算公式為

(21)

式中d—橫向偏差(m);

θ—航向偏差(rad)。

結合式(20)～式(21)式可得

(22)

由式(22)可知:運用純追蹤模型算法進行路徑追蹤時,其前視距離是固定不變的。Conlter[16]等在研究中發現:當前視距離取值過大時,會使車輛沿小弧曲線逼近設定路線,耗時較長;前視距離取值過小時,會使車輛沿大弧度曲線逼近設定路線,使系統產生較大振蕩。為此,本文提出采用模糊自適應純追蹤算法來實現車輛的路徑追蹤。

2.2.2 模糊控制器設計

模糊控制模仿人腦對不確定性概念進行判斷和推理,適宜解決非精確數學模型的復雜控制系統的控制問題[17]。由于前視距離的大小受橫向偏差和航向偏差兩因素雙重影響,故本文將橫向偏差與航向偏差作為模糊控制器的輸入,前視距離作為輸出。當橫向偏差與航向偏差較大時,使用較小的前視距離使車輛快速逼近跟蹤路徑,減小系統調節時間,提高系統響應速度;當橫向偏差與航向偏差較小時,使用較大的前視距離,防止系統超調,提高系統穩定性。

選用高斯型隸屬函數,其表達式為

(23)

其中,參數σ用于決定曲線的寬度;參數c用于決定曲線的中心。

對橫向偏差、航向偏差和前視距離進行模糊化,橫向偏差模糊論域為[-0.5,0.5](單位為m),航向偏差模糊論域為[-π/2,π/2](單位為rad),前視距離的模糊論域為[0,6](單位為m),三者均劃分為13個等級,用{-6、-5、-4、-3、-2、-1、0、+1、+2、+3、+4、+5、+6}表示。同時,規定車輛位于導航線右側時橫向偏差為負,左側為正;航向偏差順時針為正,逆時針為負;分為7個模糊集:負大(FB)、負中(FM)、負小(FS)、零(Z)、正小(ZS)、正中(ZM),正大(ZB)。最后,利用MatLab工具箱繪制出輸入輸出隸屬度函數曲線如圖3～圖5所示。

圖3 橫向偏差隸屬函數Fig.3 Lateral deviation membership function。

圖4 航向偏差隸屬函數Fig.4 Heading deviation membership function。

圖5 前視距離隸屬度函數Fig.5 Forward vision distance membership function。

利用專家經驗建立49條模糊規則如表1所示,利用MatLab工具箱繪制了模糊控制曲面如圖6所示。

表1 模糊控制規則表Table 1 Fuzzy control regulation table。

圖6 模糊控制曲面Fig 6 Fuzzy control surface。

通過模糊推理得到的結果是一個模糊集合,但在實際應用中必須要有一個確定值,才能控制或者驅動執行機構。本文選用重心法進行反模糊化,即

(24)

式中v0—輸出值;

V—輸出論域;

μv(v)—輸出的模糊集合。

3 仿真與田間試驗

3.1 MatLab仿真

將簡化二輪車模型、模糊控制模型和純追蹤模型,跟蹤路徑在MatLab/Simulink中搭建,仿真主程序如圖7所示。

圖7 導航系統Simulink仿真主程序Fig.7 Simulink model of navigation main program。

在仿真過程中設定起始點為(0,-5)(單位為m)處,初始航向角為0°,行駛速度為0.5m/s,固定前視距離取車輛2～3s走過的距離為1.3m,車輪距為1.6m。該參數來自實驗室SH-GL型電動車,直線跟蹤路徑為y=x。直線追蹤不同算法對比如圖8所示。

圖8 直線追蹤不同算法誤差對比圖Fig.8 Straight line tracking different algorithm error comparison chart。

仿真結果表明:直線跟蹤時,固定前視距離算法的橫向偏差最大超調量為0.399m,航向偏差最大超調量為0.092rad,達到穩態的時間為47s;自適應純追蹤算法的橫向偏差最大超調為0.314m,航向偏差最大超調為0.106rad,達到穩態的時間為36s。超調量總體較大的原因是由于起始點位置選取與跟蹤路徑起始點位置相差較遠造成。綜上所述,自適應純追蹤算法具有較快的響應速度和較好的調節精度,可以滿足果園農用車輛的實際作業需要。

3.2 田間試驗

為了驗證自適應純追蹤算法的精度,以雙輝車業有限公司生產的SH-GL型電動車作為試驗平臺,試驗選擇在西北農林科技大學校內的水泥路面上進行。為了模擬果園環境,道路兩旁種有株間距為3m,行間距為5.5m的16株樹木,試驗場景圖像如圖9所示。

圖9 田間試驗場景Fig.9 Field trial scene。

在田間試驗開始前,調整車輛行駛速度為0.5m/s,并連結由RTK-BDS導航定位系統靜止采集得到的起點和終點轉換坐標,從而形成試驗所需的直線目標追蹤路徑。試驗中選用的傳感器型號如表2所示。

表2 主要傳感器選型Table 2 Main sensor selection。

選取不同的起始點進行5次試驗,以RTK-BDS實時采集到的車輛位姿坐標來計算橫向偏差,統計結果如表3所示。

表3 橫向偏差統計結果Table 3 Statistical results of lateral deviation m。

續表3。

由表3可知:經過5次試驗后,最大橫向偏差為0.086m,最大平均偏差為0.05m,最大標準差為0.04m;最大偏差平均值為0.063m,平均偏差平均值為0.036m,平均標準差為0.03m。試驗表明:基于RTK-BDS的自適應純追蹤算法具有較好的導航精度,可以滿足果園農用車輛的實際作業需要。

4 結論與展望

使用RTK-BDS接收機實時提供的導航定位數據,采用卡爾曼濾波算法和自適應純追蹤控制方法,實現了果園農用車輛的直線跟蹤導航控制。試驗表明:在前進速度為0.5m/s時,直線跟蹤的最大誤差不大于0.086m,平均跟蹤誤差不大于0.036m。

本文中只考慮了果園農用車輛定速直線路徑追蹤的情況,未考慮地頭轉彎和不同工況下不同行駛速度對路徑追蹤效果的影響,還需要進一步的研究。