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

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

0 引言

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

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

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

1 車輛運動學模型建立

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

基于以上假設(shè),以X-O-Y為固定于地面的慣性坐標系建立農(nóng)用車輛兩輪模型,如圖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);

δ—前輪轉(zhuǎn)角(rad);

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

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

(3)

(4)

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

2 基于RTK-BDS的導(dǎo)航控制器設(shè)計

2.1 卡爾曼濾波器設(shè)計

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

設(shè)系統(tǒng)的狀態(tài)向量X為

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

(5)

(6)

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

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

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

φ—航向角(rad)。

經(jīng)過離散化后得

Xk=AXk-1+wk-1

(7)

式中Xk—車輛在k時刻被估計的狀態(tài)量;

A—狀態(tài)變換矩陣;

wk-1—協(xié)方差已知的零均值不相關(guān)白噪聲。

離散化后的量測方程為

Zk=HkXk+vk

(8)

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

Hk—量測矩陣;

vk—符合正態(tài)分布的觀測噪聲。

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

(9)

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

(10)

式中A—狀態(tài)轉(zhuǎn)移矩陣。

(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時刻的預(yù)測誤差協(xié)方差矩陣。

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

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

(15)

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

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

(16)

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

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

(17)

2.2 自適應(yīng)純追蹤導(dǎo)航控制器設(shè)計

2.2.1 純追蹤模型

純追蹤模型算法是一種基于幾何形學、具有仿生性的算法,通過模擬車內(nèi)駕駛員的視覺,確定合適的前視距離,計算出車輛到達指定位置所需走過的弧長,從而實現(xiàn)車輛的路徑追蹤[15]。建立純追蹤模型幾何表達示意圖,如圖2所示。其中,X-O-Y與二輪車運動學模型坐標系相同,即為滿足右手法則的完成坐標轉(zhuǎn)換后的大地直角坐標系;點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。

由幾何關(guān)系可得

(18)

由式(18)可得

(19)

結(jié)合二輪車模型可得前輪轉(zhuǎn)角δ公式為

(20)

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

(21)

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

θ—航向偏差(rad)。

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

(22)

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

2.2.2 模糊控制器設(shè)計

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

選用高斯型隸屬函數(shù),其表達式為

(23)

其中,參數(shù)σ用于決定曲線的寬度;參數(shù)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}表示。同時,規(guī)定車輛位于導(dǎo)航線右側(cè)時橫向偏差為負,左側(cè)為正;航向偏差順時針為正,逆時針為負;分為7個模糊集:負大(FB)、負中(FM)、負小(FS)、零(Z)、正小(ZS)、正中(ZM),正大(ZB)。最后,利用MatLab工具箱繪制出輸入輸出隸屬度函數(shù)曲線如圖3～圖5所示。

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

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

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

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

表1 模糊控制規(guī)則表Table 1 Fuzzy control regulation table。

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

通過模糊推理得到的結(jié)果是一個模糊集合,但在實際應(yīng)用中必須要有一個確定值,才能控制或者驅(qū)動執(zhí)行機構(gòu)。本文選用重心法進行反模糊化,即

(24)

式中v0—輸出值;

V—輸出論域;

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

3 仿真與田間試驗

3.1 MatLab仿真

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

圖7 導(dǎo)航系統(tǒng)Simulink仿真主程序Fig.7 Simulink model of navigation main program。

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

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

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

3.2 田間試驗

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

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

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

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

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

表3 橫向偏差統(tǒng)計結(jié)果Table 3 Statistical results of lateral deviation m。

續(xù)表3。

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

4 結(jié)論與展望

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

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