張聞?dòng)?王 進(jìn) 張智剛 何 杰 胡 煉 羅錫文
(1.華南農(nóng)業(yè)大學(xué)南方農(nóng)業(yè)機(jī)械與裝備關(guān)鍵技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室,廣州 510642;2.雷沃重工股份有限公司,濰坊 261206)
針對(duì)旱地農(nóng)機(jī)的導(dǎo)航系統(tǒng)[1-4]、針對(duì)水田農(nóng)機(jī)的導(dǎo)航系統(tǒng)[5-9]和針對(duì)農(nóng)業(yè)機(jī)械的導(dǎo)航系統(tǒng)[10-11],大多以定位系統(tǒng)穩(wěn)定可靠為設(shè)計(jì)前提,但是實(shí)際作業(yè)中存在定位數(shù)據(jù)不可靠現(xiàn)象,由于差分信號(hào)的無線通信不穩(wěn)定、樹木引起的多路徑效應(yīng)和高壓線干擾等原因,導(dǎo)致北斗全球定位系統(tǒng)(Beidou Navigation Satellite System,BDS)差分解失鎖,系統(tǒng)丟失固定解類,定位誤差增大。該現(xiàn)象易導(dǎo)致系統(tǒng)突然失控,對(duì)導(dǎo)航作業(yè)影響較大,例如,棉花播種鋪膜作業(yè)時(shí)的失控將導(dǎo)致切膜現(xiàn)象,需要揭膜、揭滴灌帶和重播,費(fèi)時(shí)、費(fèi)力、費(fèi)料。因此,研究農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航系統(tǒng)BDS失鎖解決方法具有實(shí)際意義。
BDS失鎖問題存在于汽車、飛機(jī)和導(dǎo)彈等多個(gè)領(lǐng)域,國內(nèi)外研究者對(duì)其進(jìn)行了相關(guān)研究。曹娟娟等[12]提出一種基于徑向基(Radial basis function,RBF)神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)的MEMS-SINS誤差反饋校正方法,在4個(gè)50 s以內(nèi)的全球定位系統(tǒng)(Global positioning system,GPS)人為失鎖過程中,該方法導(dǎo)航結(jié)果與參考系統(tǒng)比較,平均位置誤差為3.8 m,平均速度誤差為0.6 m/s,平均姿態(tài)誤差為0.5°。譚紅力[13]利用神經(jīng)網(wǎng)絡(luò)標(biāo)定和補(bǔ)償陀螺儀誤差,結(jié)合Kalman濾波器設(shè)計(jì)了慣導(dǎo)快速傳遞對(duì)準(zhǔn)算法,5次跑車試驗(yàn)結(jié)果表明,誤差由40 m下降到10 m。鮑泳林等[14]針對(duì)飛行條件下慣性導(dǎo)航系統(tǒng)(Inertial navigation system,INS)/GPS 組合導(dǎo)航系統(tǒng)在 GPS 失鎖時(shí)解算精度下降甚至發(fā)散的問題,采用神經(jīng)網(wǎng)絡(luò)輔助組合導(dǎo)航算法,飛行試驗(yàn)數(shù)據(jù)仿真表明,能夠在一定程度上抑制解算發(fā)散。SHEN等[15]針對(duì)導(dǎo)航中GPS信號(hào)中斷問題提出了基于徑向基函數(shù)多層感知機(jī)的容積卡爾曼濾波,信號(hào)中斷500 s,推算均方誤差保持在23.11 m以內(nèi)。劉慶元等[16]針對(duì)組合導(dǎo)航中載體處于惡劣環(huán)境下,或者載體處于大機(jī)動(dòng)運(yùn)動(dòng)情況下導(dǎo)致的GPS失鎖問題,提出了一種GPS/INS組合導(dǎo)航系統(tǒng),通過模擬試驗(yàn)表明,經(jīng)緯度與速度精度均提高了65%左右。BROWN等[17]研究的組合GPS/微機(jī)電系統(tǒng)(Micro electro mechanical system, MEMS)慣性導(dǎo)航組件應(yīng)用于無人地面車輛,失鎖后20 s內(nèi)誤差超過10 m。
上述研究對(duì)農(nóng)機(jī)導(dǎo)航BDS失鎖問題有一定的借鑒價(jià)值,但這些研究對(duì)精度的要求為米級(jí),與農(nóng)機(jī)農(nóng)藝要求不匹配。農(nóng)機(jī)導(dǎo)航系統(tǒng)具有高精度、較低的運(yùn)動(dòng)速度和有限緩沖距離需求等特點(diǎn),上述研究無法直接應(yīng)用于農(nóng)業(yè)機(jī)械導(dǎo)航系統(tǒng)中。
本文根據(jù)農(nóng)機(jī)農(nóng)藝特點(diǎn),分析INS傳感器誤差對(duì)失鎖狀態(tài)導(dǎo)航定位精度的影響規(guī)律,研究適應(yīng)農(nóng)機(jī)農(nóng)藝要求的基于自校準(zhǔn)變結(jié)構(gòu)Kalman濾波器的農(nóng)機(jī)導(dǎo)航BDS失鎖續(xù)航方法,該方法能夠?qū)崿F(xiàn)常態(tài)與失鎖態(tài)無縫切換、在有限時(shí)間內(nèi)維持一定的直線跟蹤精度(以棉花播種為例,實(shí)際偏差小于20 cm將不會(huì)出現(xiàn)切膜現(xiàn)象),同時(shí)提示駕駛員,有足夠的緩沖時(shí)間和距離進(jìn)行停車處理,不影響重新獲得固定解后繼續(xù)進(jìn)行自動(dòng)導(dǎo)航作業(yè)。
融合BDS和INS的系統(tǒng)信息是克服BDS系統(tǒng)短時(shí)失鎖問題的可行方案之一。信息融合需要構(gòu)建農(nóng)機(jī)簡(jiǎn)化的運(yùn)動(dòng)學(xué)模型來表示不同維度信息間的物理關(guān)系,依據(jù)文獻(xiàn)[1]描述的關(guān)于橫滾俯仰對(duì)定位精度的影響原理,設(shè)計(jì)了4自由度農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型,該模型包含4個(gè)自由度,分別是前進(jìn)、轉(zhuǎn)向、橫滾和俯仰,農(nóng)業(yè)機(jī)械4自由度運(yùn)動(dòng)學(xué)模型如圖1所示。
圖1 4自由度農(nóng)機(jī)簡(jiǎn)化運(yùn)動(dòng)學(xué)模型
x′y′z′是農(nóng)機(jī)車身坐標(biāo)系,該坐標(biāo)系隨車身移動(dòng),x′方向與航向一致,x′y′平面為水平面,h為接收天線到底面的垂直距離(m),α、β和γ分別是橫滾角、俯仰角和偏航角(rad)。
在前進(jìn)和轉(zhuǎn)向兩個(gè)自由度上采用簡(jiǎn)化二輪車模型[4]。為將橫滾、俯仰自由度上的衛(wèi)星天線漂移統(tǒng)一到二維平面上來,依據(jù)4自由度運(yùn)動(dòng)學(xué)模型對(duì)橫滾、俯仰引起的定位漂移進(jìn)行定義。將三維的BDS天線定位投影到車身坐標(biāo)的二維平面(x′y′),并轉(zhuǎn)換到全局坐標(biāo)系(XY)中,用于消除橫滾、俯仰導(dǎo)致的等效定位誤差干擾[1]。投影和轉(zhuǎn)換的幾何關(guān)系如圖2所示。
圖2 衛(wèi)星天線平面投影與轉(zhuǎn)換幾何關(guān)系圖
XYZ為全局坐標(biāo)系,該坐標(biāo)系不隨車身移動(dòng),其中XY平面與x′y′平面平行,px為BDS的天線坐標(biāo)在局部坐標(biāo)系x′軸投影,py為BDS的天線坐標(biāo)在局部坐標(biāo)系y′軸投影,θ為全局坐標(biāo)系航向角(rad)。
由于農(nóng)機(jī)行駛時(shí)α和β均較小,所以將sinα和sinβ用α和β進(jìn)行等效替換,獲得橫滾俯仰引起的定位投影相對(duì)位置漂移微分方程
(1)
式中x″——全局坐標(biāo)系橫滾俯仰引起的定位相對(duì)位置X軸投影
y″——全局坐標(biāo)系橫滾俯仰引起的定位相對(duì)位置Y軸投影
Kalman濾波器是BDS/INS數(shù)據(jù)融合的常用方法[18-19]。針對(duì)BDS失鎖、固定解丟失定位誤差增大導(dǎo)致的BDS定位數(shù)據(jù)失效的問題,基于4自由度運(yùn)動(dòng)學(xué)模型設(shè)計(jì)了一種變參數(shù)Kalman濾波器,以同時(shí)適應(yīng)正常狀態(tài)下的數(shù)據(jù)融合功能和失鎖狀態(tài)下的INS定位功能。
首先依據(jù)簡(jiǎn)化二輪車模型和橫滾、俯仰相對(duì)位置漂移模型(式(1))構(gòu)建狀態(tài)估計(jì)方程和觀測(cè)方程[20-21]
(2)
zkal=Hkalxkal+vkal
(3)
(4)
(5)
(6)
(7)
式中xkal——濾波器狀態(tài)量
ukal——濾波器狀態(tài)輸入量
zkal——狀態(tài)觀測(cè)變量
wkal——過程激勵(lì)噪聲
Akal——系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣
Bkal——系統(tǒng)控制量矩陣
vkal——觀測(cè)噪聲t——離散時(shí)間間隔,s
Hkal——觀測(cè)增益矩陣,為6階單位矩陣
v——農(nóng)機(jī)行駛速度,m/s
ax′——車輛坐標(biāo)系x′軸加速度,m2/s
ay′——車輛坐標(biāo)系y′軸加速度,m2/s
(8)
(9)
當(dāng)BDS系統(tǒng)失鎖時(shí),減少BDS系統(tǒng)數(shù)據(jù)的權(quán)重,依賴AHRS系統(tǒng)數(shù)據(jù)進(jìn)行定位濾波觀測(cè),等待BDS系統(tǒng)恢復(fù)鎖定固定解時(shí)切換回式(8)、(9)。失鎖狀態(tài)BDS信息噪聲增大時(shí)減小BDS系統(tǒng)觀測(cè)值權(quán)重,使濾波器系統(tǒng)對(duì)衛(wèi)星定位的依賴降低,定位誤差由厘米級(jí)提高到米級(jí),定位方差提高100~200倍,所以對(duì)BDS觀測(cè)相關(guān)系數(shù)進(jìn)行調(diào)整,將BDS觀測(cè)權(quán)重降低至極低的水平,設(shè)定失鎖狀態(tài)觀測(cè)噪聲協(xié)方差矩陣R′為
(10)
BDS系統(tǒng)失鎖后需依賴INS系統(tǒng)實(shí)現(xiàn)有限直線導(dǎo)航行駛,這里有限直線行駛是指在直線作業(yè)過程中出現(xiàn)發(fā)散性跟蹤偏差,但是該偏差在作業(yè)要求的接受范圍內(nèi)。由于INS導(dǎo)航系統(tǒng)受零位漂移和噪聲等因素的影響,運(yùn)用慣性導(dǎo)航系統(tǒng)方程推算位置存在累計(jì)誤差??梢酝ㄟ^不確定度(理論正負(fù)最大誤差)來表示最大誤差最大累計(jì)程度。從而分析零位漂移和噪聲對(duì)農(nóng)機(jī)在GPS失鎖的狀態(tài)下保持有限直線行駛的時(shí)間和定位誤差影響。慣性導(dǎo)航系統(tǒng)方程見文獻(xiàn)[22]。
根據(jù)慣性導(dǎo)航系統(tǒng)方程可知角速率測(cè)量誤差以累計(jì)方式傳遞到定位誤差上,角速率測(cè)量誤差w為
w(t)=b+e(t)
(11)
式中b——零偏,主要受溫度和環(huán)境影響變化緩慢,(°)/s
e(t)——帶寬固定的高頻白噪聲,(°)/s
再根據(jù)慣性導(dǎo)航系統(tǒng)仿真模型[22]分析噪聲不確定度傳遞過程。設(shè)定行駛速度v為常規(guī)播種作業(yè)速度1.0 m/s,車輛初始位置為(0,0),初始航向?yàn)? rad,設(shè)角速率測(cè)量零偏b不確定度為±0.03(°)/s,角速率白噪聲e(t)幅值為±0.05(°)/s,通過仿真模型計(jì)算獲得100 s內(nèi)的系統(tǒng)累計(jì)誤差的不確定度如圖3所示。20 s時(shí)定位不確定度為±0.1 m,100 s時(shí)定位不確定度為±2.6 m。
圖3 INS導(dǎo)航不確定度仿真結(jié)果
為分析100 s時(shí)不確定度與轉(zhuǎn)速標(biāo)定殘差不確定度之間的關(guān)系,設(shè)定行駛速度v為1.0 m/s,車輛初始位置為(0,0),初始航向?yàn)? rad,白噪聲幅值為±0.05(°)/s,分別設(shè)定角速率測(cè)量零偏b的不確定度為±0.01(°)/s、±0.02(°)/s、±0.03(°)/s、±0.04(°)/s和±0.05(°)/s。計(jì)算獲得100 s時(shí)定位不確定度分別為±0.9 m、±1.7 m、±2.6 m、±3.5 m和±4.3 m,運(yùn)用Matlab中corrcoef函數(shù)進(jìn)行相關(guān)性分析,相關(guān)系數(shù)為0.99,顯著相關(guān)。以相同方法分析角速率白噪聲e(t),幅值分別為0.03、0.05、0.07、0.09、0.11(°)/s,零偏b的不確定度設(shè)為±0.03(°)/s。于100 s時(shí),定位不確定度分別為±2.606 m、±2.633 m、±2.656 m、±2.614 m和±2.580 m,相關(guān)系數(shù)為-0.392 5,相關(guān)性不顯著。
模型推導(dǎo)和仿真試驗(yàn)結(jié)果表明:零偏b對(duì)推算定位不確定度的影響遠(yuǎn)大于高頻噪聲e(t)。在導(dǎo)航系統(tǒng)校準(zhǔn)時(shí)著重校準(zhǔn)INS傳感器零偏b,將會(huì)有效提高農(nóng)機(jī)INS續(xù)航定位精度。
由于基于INS的導(dǎo)航系統(tǒng)跟蹤誤差受初始狀態(tài)信息和零偏b的影響較大,所以針對(duì)這兩類參數(shù)的校準(zhǔn)將有助于控制INS導(dǎo)航誤差。
2.3.1基于自回歸模型的航向校準(zhǔn)方法
INS導(dǎo)航需要定義初始航向,根據(jù)BDS報(bào)文可以實(shí)時(shí)獲得信號(hào)解類,失鎖時(shí)為非固定解類。以信號(hào)解類為失鎖標(biāo)志,在失鎖的同時(shí)進(jìn)行初始航向θ0校準(zhǔn),校準(zhǔn)方法為10階自回歸(Autoregressive,AR)模型,模型定義為
Xt=a1Xt-1+a2Xt-2+…+a10Xt-10+εt
(12)
式中a——AR模型參數(shù)
X——?dú)v史BDS航向數(shù)據(jù)序列
εt——白噪聲
本文采用伯格(Burg)算法,依據(jù)已知時(shí)間序列遞推計(jì)算AR模型參數(shù),該算法以前項(xiàng)預(yù)測(cè)誤差ef和后項(xiàng)預(yù)測(cè)誤差eb均方誤差之和最小為目標(biāo)求取反射系數(shù)Km,再利用反射系數(shù)Km遞推求解10階參數(shù)an,獲得AR模型具體形式。計(jì)算方法參見文獻(xiàn)[23]。
2.3.2INS傳感器角速率測(cè)量零偏實(shí)時(shí)校準(zhǔn)方法
BDS數(shù)據(jù)為正常固定解時(shí),利用定位信息通過時(shí)間窗進(jìn)行INS傳感器零速校準(zhǔn)和基于一次線性回歸最小二乘模型(Least squares matching,LSM)的零偏動(dòng)態(tài)校準(zhǔn)。
設(shè)計(jì)實(shí)時(shí)滑動(dòng)時(shí)間窗口為30 s,按隊(duì)列方式定義,新數(shù)據(jù)載入則刪除30 s前的數(shù)據(jù),先進(jìn)先出。時(shí)間窗口內(nèi)BDS的定位數(shù)據(jù)最遠(yuǎn)點(diǎn)之間的距離Lmax小于0.1 m且速度v最大值小于0.1 m/s,則判定當(dāng)前窗口內(nèi)導(dǎo)航系統(tǒng)為零速,由于該值遠(yuǎn)大于農(nóng)機(jī)最小轉(zhuǎn)彎半徑不可能旋轉(zhuǎn),根據(jù)式(11)可得MEMES傳感器零偏為
(13)
其中
式中n——時(shí)間窗內(nèi)數(shù)據(jù)集數(shù)量
k——零偏數(shù)組編號(hào)
農(nóng)機(jī)導(dǎo)航系統(tǒng)正常工作時(shí)處于直線跟蹤狀態(tài),該狀態(tài)的特點(diǎn)為行駛目標(biāo)航向不變。依據(jù)該特點(diǎn)設(shè)計(jì)動(dòng)態(tài)校準(zhǔn)方法,由于時(shí)間窗口內(nèi)軌跡近似直線,航向在有限范圍內(nèi)圍繞目標(biāo)航向小范圍波動(dòng),累計(jì)航向變化近似為零,所以同樣可以采用式(13)進(jìn)行轉(zhuǎn)向率傳感器的零偏b估計(jì)。
判斷車輛是否處于直線行駛狀態(tài)的標(biāo)準(zhǔn)為跟蹤軌跡的LSM擬合,采用最小二乘法獲得,前期非道路試驗(yàn)表明:直線跟蹤狀態(tài)下30 s時(shí)間窗口的高斯投影坐標(biāo)一次最小二乘擬合R2檢驗(yàn)結(jié)果均大于0.995,所以判斷標(biāo)準(zhǔn)定為R2檢驗(yàn)結(jié)果大于0.995時(shí)導(dǎo)航系統(tǒng)處于直線行駛狀態(tài),采用式(13)進(jìn)行零偏b實(shí)時(shí)校準(zhǔn),流程如圖4所示。
圖4 零偏實(shí)時(shí)校準(zhǔn)流程圖
由于農(nóng)機(jī)導(dǎo)航系統(tǒng)曲線行駛狀態(tài)的一般時(shí)間較短,衛(wèi)星天線獲取的航向存在誤差,用于零偏b校準(zhǔn)影響較大,所以非直線狀態(tài)不進(jìn)行校準(zhǔn)處理。
針對(duì)正常狀態(tài)和BDS失鎖狀態(tài)的位姿處理無縫切換的需求,設(shè)計(jì)了一種自校準(zhǔn)變結(jié)構(gòu)位姿信息處理方法。該方法包括變結(jié)構(gòu)參數(shù)、INS導(dǎo)航參數(shù)校準(zhǔn)模塊和信息融合Kalman濾波器。INS導(dǎo)航參數(shù)校準(zhǔn)模塊采用的方法如第4節(jié)所述。其中Kalman濾波器設(shè)計(jì)原理如2.1節(jié)所述。該方法輸入為解類、BDS和INS信號(hào),輸出為融合濾波后的位姿信息,具體輸入輸出信息參數(shù)為式(4)、(5)。
其中解類信號(hào)為變結(jié)構(gòu)判斷標(biāo)準(zhǔn),來源是BDS實(shí)時(shí)GPGGA報(bào)文,固定解(第4類)為衛(wèi)星定位正常狀態(tài),差分定位誤差為±(10+1×10-6D)mm,D為基站到移動(dòng)站的距離,其他解類均定義為失鎖狀態(tài),定位誤差從米級(jí)到亞米級(jí)不等,無法用于農(nóng)業(yè)高精度導(dǎo)航控制。當(dāng)解類為固定解時(shí)采用正常狀態(tài)參數(shù)(式(8)、(9)),否則采用失鎖狀態(tài)觀測(cè)噪聲協(xié)方差矩陣R′(式(10))。解類同時(shí)控制校準(zhǔn)模塊,正常狀態(tài)時(shí),為校準(zhǔn)零偏和初始航向模塊采集歷史數(shù)據(jù),在失鎖同時(shí)計(jì)算初始航向用于INS定位處理,同時(shí)啟動(dòng)零偏補(bǔ)償。總體結(jié)構(gòu)如圖5。
圖5 自校準(zhǔn)變結(jié)構(gòu)位姿信息處理方法結(jié)構(gòu)圖
為構(gòu)建機(jī)器人失鎖續(xù)航系統(tǒng),基于BDS失鎖變結(jié)構(gòu)位姿信息處理方法設(shè)計(jì)了BDS失鎖續(xù)航導(dǎo)航控制器,該控制器能夠依據(jù)解類信息無縫切換BDS/INS組合導(dǎo)航和INS導(dǎo)航模式,運(yùn)用自校準(zhǔn)變結(jié)構(gòu)位姿信息處理模型進(jìn)行無縫位姿信息處理,與目標(biāo)路徑相差獲得橫向偏差和航向偏差,輸入到基于深度強(qiáng)化學(xué)習(xí)網(wǎng)絡(luò)的導(dǎo)航控制模型中計(jì)算出目標(biāo)行駛曲率[24]。模型輸入為BDS、INS和目標(biāo)路徑信息,輸出為目標(biāo)曲率。控制器結(jié)構(gòu)如圖6。
圖6 BDS失鎖續(xù)航導(dǎo)航控制器結(jié)構(gòu)圖
為研究BDS失鎖續(xù)航方法的性能,搭建輪式差速機(jī)器人平臺(tái)。該平臺(tái)硬件部分包括輪式差速機(jī)器人(Husky Robotics Inc.)和衛(wèi)星定位系統(tǒng)(Swift company);定位信息獲取頻率為10 Hz,水平定位精度±(10+1×10-6D)mm,AHRS慣性傳感器(MicroStrain公司 3DM-GX5-25型),陀螺儀非線性度0.02%,橫滾/俯仰精度±0.25°,采樣頻率30 Hz;控制終端為NVIDIA公司的 Jetson TX1 嵌入式計(jì)算機(jī)和觸控顯示屏。軟件部分為基于BDS失鎖續(xù)航導(dǎo)航控制模型的機(jī)器人導(dǎo)航控制軟件系統(tǒng),該軟件系統(tǒng)運(yùn)用QT5.0編寫加載于Ubuntu 16.04 LTS系統(tǒng)中。RKT-GNSS(BDS)定位信息和AHRS(INS)慣性航姿信息通過串口輸入到導(dǎo)航控制系統(tǒng)。該系統(tǒng)將計(jì)算獲得目標(biāo)曲率和設(shè)定行駛速度,通過ROS機(jī)器人操作系統(tǒng)傳輸給Husky機(jī)器人,機(jī)器人自動(dòng)執(zhí)行目標(biāo)控制信號(hào)實(shí)現(xiàn)自動(dòng)導(dǎo)航行駛。系統(tǒng)結(jié)構(gòu)如圖7,試驗(yàn)平臺(tái)實(shí)物如圖8。
圖7 機(jī)器人失鎖續(xù)航導(dǎo)航試驗(yàn)系統(tǒng)結(jié)構(gòu)圖
圖8 機(jī)器人失鎖續(xù)航導(dǎo)航試驗(yàn)系統(tǒng)
圖9 BDS失鎖狀態(tài)續(xù)航性能對(duì)比試驗(yàn)實(shí)際橫向偏差
為研究BDS信號(hào)失鎖狀態(tài)下,控制器的無縫處理能力,在機(jī)器人系統(tǒng)直線跟蹤時(shí)人為軟件屏蔽BDS定位點(diǎn)信息的輸入和給出失鎖信號(hào)。只記錄BDS定位數(shù)據(jù)而不作為導(dǎo)航控制輸入。使用INS遞推軌跡作為導(dǎo)航控制器輸入進(jìn)行路徑跟蹤控制。同時(shí)記錄BDS數(shù)據(jù)獲得實(shí)際軌跡,分析失鎖續(xù)航導(dǎo)航系統(tǒng)性能。
試驗(yàn)設(shè)計(jì):規(guī)劃直線路徑,使用機(jī)器人導(dǎo)航系統(tǒng)在草地進(jìn)行直線導(dǎo)航跟蹤,行駛速度設(shè)置為1 m/s。當(dāng)系統(tǒng)進(jìn)入直線跟蹤的穩(wěn)定狀態(tài)時(shí),切換系統(tǒng)輸入為INS定位數(shù)據(jù)和非4類解信號(hào)。分別采用基于自校準(zhǔn)變結(jié)構(gòu)(Self-calibrating variable structure)Kalman濾波位姿信息處理方法(SCVS-K)的BDS失鎖續(xù)航導(dǎo)航控制器和未校準(zhǔn)的變結(jié)構(gòu)(Variable structure)Kalman濾波(VS-K)進(jìn)行信息處理,失鎖續(xù)航控制的時(shí)間為20 s,每組控制器進(jìn)行3次重復(fù)試驗(yàn)。
轉(zhuǎn)彎跟蹤時(shí)航向變化率較大,INS測(cè)量誤差增大,INS定位航位推算誤差累計(jì)較快,為測(cè)試矩形路徑(含有轉(zhuǎn)彎環(huán)節(jié))導(dǎo)航失鎖續(xù)航控制器性能,采用與3.3節(jié)直線續(xù)航試驗(yàn)相同的處理方法獲取BDS數(shù)據(jù)獲得實(shí)際軌跡與INS續(xù)航軌跡數(shù)據(jù),分析失鎖續(xù)航導(dǎo)航系統(tǒng)性能。
試驗(yàn)設(shè)計(jì):規(guī)劃矩形路徑,使用機(jī)器人導(dǎo)航系統(tǒng)在草地進(jìn)行矩形路徑導(dǎo)航跟蹤,直線行駛部分速度設(shè)置為1 m/s,轉(zhuǎn)彎上線部分速度設(shè)置為0.5 m/s。當(dāng)系統(tǒng)進(jìn)入直線跟蹤的穩(wěn)定狀態(tài)時(shí),切換系統(tǒng)輸入為INS定位續(xù)航狀態(tài)數(shù)據(jù)和非4類解信號(hào)。采用基于SCVS-K方法的BDS失鎖續(xù)航導(dǎo)航控制器進(jìn)行信息處理,失鎖續(xù)航控制對(duì)矩形路徑進(jìn)行續(xù)航,進(jìn)行3次重復(fù)試驗(yàn)。
表1 BDS失鎖狀態(tài)續(xù)航性能對(duì)比試驗(yàn)結(jié)果
圖10 機(jī)器人矩形路徑BDS失鎖續(xù)航試驗(yàn)1軌跡
圖11 續(xù)航試驗(yàn)BDS定位與INS定位差值變化曲線
圖12 續(xù)航試驗(yàn)實(shí)際橫向誤差變化曲線
表2 機(jī)器人矩形路徑BDS失鎖續(xù)航試驗(yàn)結(jié)果
為驗(yàn)證BDS失鎖續(xù)航控制方法田間工作效果,將BDS失鎖續(xù)航控制方法移植到雷沃重工AGCS-Ⅰ自動(dòng)導(dǎo)航系統(tǒng)中,搭載于雷沃重工M-1104型拖拉機(jī),在雷沃阿波斯智慧農(nóng)業(yè)示范基地的試驗(yàn)田進(jìn)行田間試驗(yàn)。RTK-GNSS定位模塊(司南公司的K728),定位信息獲取頻率為10 Hz,水平定位精度±(10+1×10-6D)mm;AHRS慣性傳感器(XSENS公司MTi-30 AHRS),陀螺儀非線性度0.01%,橫滾/俯仰精度±0.3°,采樣頻率25 Hz;前輪轉(zhuǎn)角傳感器為BEI-9902120CW型,非線性度為±2%,A/D采樣精度為12位;轉(zhuǎn)向執(zhí)行機(jī)構(gòu)為力士樂公司的HT801053;控制終端為AGCS-Ⅰ控制器和觸控顯示屏。運(yùn)用Metrowerks Code Warrior for ARM Developer Suite v1.2進(jìn)行軟件開發(fā)。系統(tǒng)結(jié)構(gòu)如圖13所示,試驗(yàn)平臺(tái)實(shí)物如圖14所示。
圖13 農(nóng)機(jī)導(dǎo)航系統(tǒng)試驗(yàn)平臺(tái)結(jié)構(gòu)圖
圖14 農(nóng)機(jī)導(dǎo)航系統(tǒng)田間試驗(yàn)
圖15 BDS失鎖狀態(tài)田間續(xù)航試驗(yàn)實(shí)際橫向偏差變化曲線
為研究BDS信號(hào)失鎖系統(tǒng)的田間導(dǎo)航性能,在農(nóng)機(jī)導(dǎo)航系統(tǒng)直線跟蹤時(shí)人為軟件屏蔽BDS定位點(diǎn)信息的輸入和給出失鎖信號(hào)。采用與機(jī)器人試驗(yàn)相同方法,記錄BDS數(shù)據(jù)和INS推算數(shù)據(jù)獲得實(shí)際軌跡和遞推軌跡,分析失鎖續(xù)航導(dǎo)航系統(tǒng)性能。
試驗(yàn)設(shè)計(jì):規(guī)劃直線路徑,使用機(jī)器人導(dǎo)航系統(tǒng)在試驗(yàn)田地進(jìn)行直線導(dǎo)航跟蹤,直線行駛速度設(shè)置為1 m/s。當(dāng)系統(tǒng)進(jìn)入直線跟蹤的穩(wěn)定狀態(tài)時(shí),切換系統(tǒng)輸入為INS定位續(xù)航狀態(tài)數(shù)據(jù)和非4類解信號(hào)。采用基于SCVS-K方法和未校準(zhǔn)的 VS-K方法分別對(duì)直線路徑進(jìn)行BDS失鎖續(xù)航處理,分別進(jìn)行3次重復(fù)試驗(yàn)。
試驗(yàn)分析:農(nóng)機(jī)田間試驗(yàn)中,3組SCVS-K方法失鎖續(xù)航過程的實(shí)際橫向偏差ed如圖15a所示,3組VS-K方法失鎖續(xù)航過程的實(shí)際橫向偏差如圖15b所示。對(duì)比試驗(yàn)結(jié)果如表3,SCVS-K處理方法平均L20 cm達(dá)到16.65 m,滿足棉花播種失鎖應(yīng)激處理的要求。
文獻(xiàn)[12]中采用基于神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)的誤差反饋矯正方法,在車輛行駛速度為16 m/s時(shí),失鎖時(shí)間25 s內(nèi)位置誤差為3.5 m。文獻(xiàn)[15]中采用基于立方Kalman濾波器和神經(jīng)網(wǎng)絡(luò)的失鎖定位方法,速度為1.48 m/s時(shí),500 s內(nèi)定位誤差為23.11 m。在文獻(xiàn)[17]的研究中使用MEMS慣性傳感器在GPS失鎖時(shí)進(jìn)行續(xù)航,20 s內(nèi)誤差超過10 m。上述研究沒有對(duì)分米級(jí)精度失鎖續(xù)航特性進(jìn)行細(xì)分研究。文獻(xiàn)[25]采用擴(kuò)展Kalman濾波器進(jìn)行拖拉機(jī)失鎖續(xù)航,橫向誤差達(dá)到20 cm時(shí),行駛距離小于10 m。使用本文方法續(xù)航,橫向誤差達(dá)到20 cm時(shí),行駛距離大于15 m。
表3 BDS失鎖狀態(tài)田間續(xù)航試驗(yàn)結(jié)果
(1)針對(duì)農(nóng)機(jī)導(dǎo)航作業(yè)過程中因存在BDS信號(hào)失鎖而導(dǎo)致突然失控的問題,提出了一種基于自校準(zhǔn)變結(jié)構(gòu)Kalman濾波的農(nóng)機(jī)導(dǎo)航BDS失鎖續(xù)航方法,使操作人員有足夠的時(shí)間和緩沖距離進(jìn)行進(jìn)一步處理,減輕由于突然失控導(dǎo)致的農(nóng)資損失和額外勞動(dòng)強(qiáng)度。該方法為棉花播種導(dǎo)航作業(yè)過程中BDS失鎖應(yīng)急續(xù)航處理提供了研究基礎(chǔ)。
(3)根據(jù)農(nóng)機(jī)導(dǎo)航系統(tǒng)低速、高精度的特點(diǎn),進(jìn)行了BDS失鎖續(xù)航方法的田間試驗(yàn),試驗(yàn)結(jié)果表明:在實(shí)際偏差小于20 cm的條件下,農(nóng)機(jī)在路徑上的行駛平均距離達(dá)到16.65 m,預(yù)留反應(yīng)距離能夠滿足農(nóng)機(jī)導(dǎo)航作業(yè)需求。
(4)BDS失鎖試驗(yàn)結(jié)果表明,INS導(dǎo)航航向角速率測(cè)量零偏對(duì)直線路徑失鎖續(xù)航影響較大,進(jìn)一步提高零偏的校準(zhǔn)效率和精度能夠提高直線路徑失鎖續(xù)航質(zhì)量和可靠性。
(5)機(jī)器人矩形路徑BDS失鎖續(xù)航試驗(yàn)表明,系統(tǒng)在轉(zhuǎn)彎環(huán)節(jié)續(xù)航誤差迅速增加,并影響后續(xù)續(xù)航效果,這主要是測(cè)量誤差、采樣率和系統(tǒng)延時(shí)等因素導(dǎo)致,進(jìn)一步研究導(dǎo)航航向和航向變化率的測(cè)量、采樣、預(yù)測(cè)和插值方法,能夠提高非直線續(xù)航質(zhì)量。