基于自校準(zhǔn)變結(jié)構(gòu)Kalman的農(nóng)機(jī)導(dǎo)航BDS失鎖續(xù)航方法

張聞?dòng)?王 進(jìn) 張智剛 何 杰 胡 煉 羅錫文

(1.華南農(nóng)業(yè)大學(xué)南方農(nóng)業(yè)機(jī)械與裝備關(guān)鍵技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，廣州 510642;2.雷沃重工股份有限公司，濰坊 261206)

0 引言

針對(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è)。

1 4自由度農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型

融合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軸投影

2 自校準(zhǔn)變結(jié)構(gòu)Kalman濾波器設(shè)計(jì)

2.1 基礎(chǔ)Kalman濾波器設(shè)計(jì)

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)

2.2 INS導(dǎo)航系統(tǒng)定位誤差不確定度分析

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ù)航定位精度。

2.3 INS導(dǎo)航參數(shù)自校準(zhǔn)方法

由于基于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)處理。

2.4 自校準(zhǔn)變結(jié)構(gòu)位姿信息處理方法

針對(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)圖

3 BDS失鎖續(xù)航機(jī)器人平臺(tái)試驗(yàn)

3.1 BDS失鎖續(xù)航導(dǎo)航控制器

為構(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)圖

3.2 機(jī)器人導(dǎo)航試驗(yàn)系統(tǒng)

為研究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í)際橫向偏差

3.3 BDS失鎖狀態(tài)續(xù)航性能對(duì)比試驗(yàn)

為研究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)。

3.4 機(jī)器人矩形路徑(含有轉(zhuǎn)彎環(huán)節(jié))BDS失鎖狀態(tài)續(xù)航試驗(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é)果

4 農(nóng)機(jī)導(dǎo)航系統(tǒng)BDS失鎖續(xù)航田間試驗(yàn)

4.1 農(nóng)機(jī)導(dǎo)航系統(tǒng)

為驗(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í)際橫向偏差變化曲線

4.2 BDS失鎖狀態(tài)田間續(xù)航試驗(yàn)

為研究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é)果

5 結(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ì)量。