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

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

0 引言

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

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

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

1 車輛運(yùn)動(dòng)學(xué)模型建立

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

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

圖1 車輛運(yùn)動(dòng)學(xué)模型Fig.1 Vehicle kinematics model。

由圖1可得表達(dá)式為

(1)

(2)

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

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

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

φ—航向角(rad);

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

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

把式(1)中vp代入到式(2),可以得出二輪車的運(yùn)動(dòng)學(xué)模型,即

(3)

(4)

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

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

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

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

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

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

(5)

(6)

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

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

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

φ—航向角(rad)。

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

Xk=AXk-1+wk-1

(7)

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

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

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

離散化后的量測(cè)方程為

Zk=HkXk+vk

(8)

式中Zk—車輛在k時(shí)刻的觀測(cè)值;

Hk—量測(cè)矩陣;

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

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

(9)

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

(10)

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

(11)

(12)

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

Zk+1—k+1時(shí)刻的實(shí)際觀測(cè)值。

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

(13)

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

(14)

式中P(k+1|k)—k時(shí)刻對(duì)k+1時(shí)刻的預(yù)測(cè)誤差協(xié)方差矩陣。

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

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

(15)

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

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

(16)

式中R—觀測(cè)時(shí)的高斯白噪聲協(xié)方差對(duì)角陣,定義為均值0m、標(biāo)準(zhǔn)差1m的對(duì)角陣。

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

(17)

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

2.2.1 純追蹤模型

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

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

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

(18)

由式(18)可得

(19)

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

(20)

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

(21)

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

θ—航向偏差(rad)。

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

(22)

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

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

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

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

(23)

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

對(duì)橫向偏差、航向偏差和前視距離進(jìn)行模糊化,橫向偏差模糊論域?yàn)閇-0.5,0.5](單位為m),航向偏差模糊論域?yàn)閇-π/2,π/2](單位為rad),前視距離的模糊論域?yàn)閇0,6](單位為m),三者均劃分為13個(gè)等級(jí),用{-6、-5、-4、-3、-2、-1、0、+1、+2、+3、+4、+5、+6}表示。同時(shí),規(guī)定車輛位于導(dǎo)航線右側(cè)時(shí)橫向偏差為負(fù),左側(cè)為正;航向偏差順時(shí)針為正,逆時(shí)針為負(fù);分為7個(gè)模糊集:負(fù)大(FB)、負(fù)中(FM)、負(fù)小(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)驗(yàn)建立49條模糊規(guī)則如表1所示,利用MatLab工具箱繪制了模糊控制曲面如圖6所示。

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

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

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

(24)

式中v0—輸出值;

V—輸出論域;

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

3 仿真與田間試驗(yàn)

3.1 MatLab仿真

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

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

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

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

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

3.2 田間試驗(yàn)

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

圖9 田間試驗(yàn)場景Fig.9 Field trial scene。

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

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

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

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

續(xù)表3。

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

4 結(jié)論與展望

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

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