基于自校準變結構Kalman的農機導航BDS失鎖續航方法

張聞宇 王 進 張智剛 何 杰 胡 煉 羅錫文

(1.華南農業大學南方農業機械與裝備關鍵技術教育部重點實驗室，廣州 510642;2.雷沃重工股份有限公司，濰坊 261206)

0 引言

針對旱地農機的導航系統[1-4]、針對水田農機的導航系統[5-9]和針對農業機械的導航系統[10-11]，大多以定位系統穩定可靠為設計前提，但是實際作業中存在定位數據不可靠現象，由于差分信號的無線通信不穩定、樹木引起的多路徑效應和高壓線干擾等原因，導致北斗全球定位系統(Beidou Navigation Satellite System，BDS)差分解失鎖，系統丟失固定解類，定位誤差增大。該現象易導致系統突然失控，對導航作業影響較大，例如，棉花播種鋪膜作業時的失控將導致切膜現象，需要揭膜、揭滴灌帶和重播，費時、費力、費料。因此，研究農業機械自動導航系統BDS失鎖解決方法具有實際意義。

BDS失鎖問題存在于汽車、飛機和導彈等多個領域，國內外研究者對其進行了相關研究。曹娟娟等[12]提出一種基于徑向基(Radial basis function，RBF)神經網絡預測的MEMS-SINS誤差反饋校正方法，在4個50 s以內的全球定位系統(Global positioning system，GPS)人為失鎖過程中,該方法導航結果與參考系統比較,平均位置誤差為3.8 m,平均速度誤差為0.6 m/s,平均姿態誤差為0.5°。譚紅力[13]利用神經網絡標定和補償陀螺儀誤差，結合Kalman濾波器設計了慣導快速傳遞對準算法，5次跑車試驗結果表明，誤差由40 m下降到10 m。鮑泳林等[14]針對飛行條件下慣性導航系統(Inertial navigation system，INS)/GPS 組合導航系統在 GPS 失鎖時解算精度下降甚至發散的問題，采用神經網絡輔助組合導航算法，飛行試驗數據仿真表明，能夠在一定程度上抑制解算發散。SHEN等[15]針對導航中GPS信號中斷問題提出了基于徑向基函數多層感知機的容積卡爾曼濾波，信號中斷500 s，推算均方誤差保持在23.11 m以內。劉慶元等[16]針對組合導航中載體處于惡劣環境下，或者載體處于大機動運動情況下導致的GPS失鎖問題，提出了一種GPS/INS組合導航系統，通過模擬試驗表明，經緯度與速度精度均提高了65%左右。BROWN等[17]研究的組合GPS/微機電系統(Micro electro mechanical system, MEMS)慣性導航組件應用于無人地面車輛，失鎖后20 s內誤差超過10 m。

上述研究對農機導航BDS失鎖問題有一定的借鑒價值，但這些研究對精度的要求為米級，與農機農藝要求不匹配。農機導航系統具有高精度、較低的運動速度和有限緩沖距離需求等特點，上述研究無法直接應用于農業機械導航系統中。

本文根據農機農藝特點，分析INS傳感器誤差對失鎖狀態導航定位精度的影響規律，研究適應農機農藝要求的基于自校準變結構Kalman濾波器的農機導航BDS失鎖續航方法，該方法能夠實現常態與失鎖態無縫切換、在有限時間內維持一定的直線跟蹤精度(以棉花播種為例，實際偏差小于20 cm將不會出現切膜現象)，同時提示駕駛員，有足夠的緩沖時間和距離進行停車處理，不影響重新獲得固定解后繼續進行自動導航作業。

1 4自由度農機運動學模型

融合BDS和INS的系統信息是克服BDS系統短時失鎖問題的可行方案之一。信息融合需要構建農機簡化的運動學模型來表示不同維度信息間的物理關系，依據文獻[1]描述的關于橫滾俯仰對定位精度的影響原理，設計了4自由度農機運動學模型，該模型包含4個自由度，分別是前進、轉向、橫滾和俯仰，農業機械4自由度運動學模型如圖1所示。

圖1 4自由度農機簡化運動學模型

x′y′z′是農機車身坐標系，該坐標系隨車身移動，x′方向與航向一致，x′y′平面為水平面，h為接收天線到底面的垂直距離(m)，α、β和γ分別是橫滾角、俯仰角和偏航角(rad)。

在前進和轉向兩個自由度上采用簡化二輪車模型[4]。為將橫滾、俯仰自由度上的衛星天線漂移統一到二維平面上來，依據4自由度運動學模型對橫滾、俯仰引起的定位漂移進行定義。將三維的BDS天線定位投影到車身坐標的二維平面(x′y′)，并轉換到全局坐標系(XY)中，用于消除橫滾、俯仰導致的等效定位誤差干擾[1]。投影和轉換的幾何關系如圖2所示。

圖2 衛星天線平面投影與轉換幾何關系圖

XYZ為全局坐標系，該坐標系不隨車身移動，其中XY平面與x′y′平面平行，px為BDS的天線坐標在局部坐標系x′軸投影，py為BDS的天線坐標在局部坐標系y′軸投影，θ為全局坐標系航向角(rad)。

由于農機行駛時α和β均較小，所以將sinα和sinβ用α和β進行等效替換，獲得橫滾俯仰引起的定位投影相對位置漂移微分方程

(1)

式中x″——全局坐標系橫滾俯仰引起的定位相對位置X軸投影

y″——全局坐標系橫滾俯仰引起的定位相對位置Y軸投影

2 自校準變結構Kalman濾波器設計

2.1 基礎Kalman濾波器設計

Kalman濾波器是BDS/INS數據融合的常用方法[18-19]。針對BDS失鎖、固定解丟失定位誤差增大導致的BDS定位數據失效的問題，基于4自由度運動學模型設計了一種變參數Kalman濾波器，以同時適應正常狀態下的數據融合功能和失鎖狀態下的INS定位功能。

首先依據簡化二輪車模型和橫滾、俯仰相對位置漂移模型(式(1))構建狀態估計方程和觀測方程[20-21]

(2)

zkal=Hkalxkal+vkal

(3)

(4)

(5)

(6)

(7)

式中xkal——濾波器狀態量

ukal——濾波器狀態輸入量

zkal——狀態觀測變量

wkal——過程激勵噪聲

Akal——系統狀態轉移矩陣

Bkal——系統控制量矩陣

vkal——觀測噪聲t——離散時間間隔,s

Hkal——觀測增益矩陣，為6階單位矩陣

v——農機行駛速度,m/s

ax′——車輛坐標系x′軸加速度,m2/s

ay′——車輛坐標系y′軸加速度,m2/s

(8)

(9)

當BDS系統失鎖時，減少BDS系統數據的權重，依賴AHRS系統數據進行定位濾波觀測，等待BDS系統恢復鎖定固定解時切換回式(8)、(9)。失鎖狀態BDS信息噪聲增大時減小BDS系統觀測值權重，使濾波器系統對衛星定位的依賴降低，定位誤差由厘米級提高到米級，定位方差提高100～200倍，所以對BDS觀測相關系數進行調整，將BDS觀測權重降低至極低的水平，設定失鎖狀態觀測噪聲協方差矩陣R′為

(10)

2.2 INS導航系統定位誤差不確定度分析

BDS系統失鎖后需依賴INS系統實現有限直線導航行駛，這里有限直線行駛是指在直線作業過程中出現發散性跟蹤偏差，但是該偏差在作業要求的接受范圍內。由于INS導航系統受零位漂移和噪聲等因素的影響，運用慣性導航系統方程推算位置存在累計誤差。可以通過不確定度(理論正負最大誤差)來表示最大誤差最大累計程度。從而分析零位漂移和噪聲對農機在GPS失鎖的狀態下保持有限直線行駛的時間和定位誤差影響。慣性導航系統方程見文獻[22]。

根據慣性導航系統方程可知角速率測量誤差以累計方式傳遞到定位誤差上，角速率測量誤差w為

w(t)=b+e(t)

(11)

式中b——零偏，主要受溫度和環境影響變化緩慢，(°)/s

e(t)——帶寬固定的高頻白噪聲，(°)/s

再根據慣性導航系統仿真模型[22]分析噪聲不確定度傳遞過程。設定行駛速度v為常規播種作業速度1.0 m/s，車輛初始位置為(0，0)，初始航向為0 rad，設角速率測量零偏b不確定度為±0.03(°)/s，角速率白噪聲e(t)幅值為±0.05(°)/s，通過仿真模型計算獲得100 s內的系統累計誤差的不確定度如圖3所示。20 s時定位不確定度為±0.1 m，100 s時定位不確定度為±2.6 m。

圖3 INS導航不確定度仿真結果

為分析100 s時不確定度與轉速標定殘差不確定度之間的關系，設定行駛速度v為1.0 m/s，車輛初始位置為(0，0)，初始航向為0 rad，白噪聲幅值為±0.05(°)/s，分別設定角速率測量零偏b的不確定度為±0.01(°)/s、±0.02(°)/s、±0.03(°)/s、±0.04(°)/s和±0.05(°)/s。計算獲得100 s時定位不確定度分別為±0.9 m、±1.7 m、±2.6 m、±3.5 m和±4.3 m，運用Matlab中corrcoef函數進行相關性分析，相關系數為0.99，顯著相關。以相同方法分析角速率白噪聲e(t)，幅值分別為0.03、0.05、0.07、0.09、0.11(°)/s，零偏b的不確定度設為±0.03(°)/s。于100 s時，定位不確定度分別為±2.606 m、±2.633 m、±2.656 m、±2.614 m和±2.580 m，相關系數為-0.392 5，相關性不顯著。

模型推導和仿真試驗結果表明：零偏b對推算定位不確定度的影響遠大于高頻噪聲e(t)。在導航系統校準時著重校準INS傳感器零偏b，將會有效提高農機INS續航定位精度。

2.3 INS導航參數自校準方法

由于基于INS的導航系統跟蹤誤差受初始狀態信息和零偏b的影響較大，所以針對這兩類參數的校準將有助于控制INS導航誤差。

2.3.1基于自回歸模型的航向校準方法

INS導航需要定義初始航向，根據BDS報文可以實時獲得信號解類，失鎖時為非固定解類。以信號解類為失鎖標志，在失鎖的同時進行初始航向θ0校準，校準方法為10階自回歸(Autoregressive,AR)模型，模型定義為

Xt=a1Xt-1+a2Xt-2+…+a10Xt-10+εt

(12)

式中a——AR模型參數

X——歷史BDS航向數據序列

εt——白噪聲

本文采用伯格(Burg)算法，依據已知時間序列遞推計算AR模型參數，該算法以前項預測誤差ef和后項預測誤差eb均方誤差之和最小為目標求取反射系數Km，再利用反射系數Km遞推求解10階參數an，獲得AR模型具體形式。計算方法參見文獻[23]。

2.3.2INS傳感器角速率測量零偏實時校準方法

BDS數據為正常固定解時，利用定位信息通過時間窗進行INS傳感器零速校準和基于一次線性回歸最小二乘模型(Least squares matching,LSM)的零偏動態校準。

設計實時滑動時間窗口為30 s，按隊列方式定義，新數據載入則刪除30 s前的數據，先進先出。時間窗口內BDS的定位數據最遠點之間的距離Lmax小于0.1 m且速度v最大值小于0.1 m/s，則判定當前窗口內導航系統為零速，由于該值遠大于農機最小轉彎半徑不可能旋轉，根據式(11)可得MEMES傳感器零偏為

(13)

其中

式中n——時間窗內數據集數量

k——零偏數組編號

農機導航系統正常工作時處于直線跟蹤狀態，該狀態的特點為行駛目標航向不變。依據該特點設計動態校準方法，由于時間窗口內軌跡近似直線，航向在有限范圍內圍繞目標航向小范圍波動，累計航向變化近似為零，所以同樣可以采用式(13)進行轉向率傳感器的零偏b估計。

判斷車輛是否處于直線行駛狀態的標準為跟蹤軌跡的LSM擬合，采用最小二乘法獲得，前期非道路試驗表明：直線跟蹤狀態下30 s時間窗口的高斯投影坐標一次最小二乘擬合R2檢驗結果均大于0.995，所以判斷標準定為R2檢驗結果大于0.995時導航系統處于直線行駛狀態，采用式(13)進行零偏b實時校準，流程如圖4所示。

圖4 零偏實時校準流程圖

由于農機導航系統曲線行駛狀態的一般時間較短，衛星天線獲取的航向存在誤差，用于零偏b校準影響較大，所以非直線狀態不進行校準處理。

2.4 自校準變結構位姿信息處理方法

針對正常狀態和BDS失鎖狀態的位姿處理無縫切換的需求，設計了一種自校準變結構位姿信息處理方法。該方法包括變結構參數、INS導航參數校準模塊和信息融合Kalman濾波器。INS導航參數校準模塊采用的方法如第4節所述。其中Kalman濾波器設計原理如2.1節所述。該方法輸入為解類、BDS和INS信號，輸出為融合濾波后的位姿信息，具體輸入輸出信息參數為式(4)、(5)。

其中解類信號為變結構判斷標準，來源是BDS實時GPGGA報文，固定解(第4類)為衛星定位正常狀態，差分定位誤差為±(10+1×10-6D)mm，D為基站到移動站的距離，其他解類均定義為失鎖狀態，定位誤差從米級到亞米級不等，無法用于農業高精度導航控制。當解類為固定解時采用正常狀態參數(式(8)、(9))，否則采用失鎖狀態觀測噪聲協方差矩陣R′(式(10))。解類同時控制校準模塊，正常狀態時，為校準零偏和初始航向模塊采集歷史數據，在失鎖同時計算初始航向用于INS定位處理，同時啟動零偏補償。總體結構如圖5。

圖5 自校準變結構位姿信息處理方法結構圖

3 BDS失鎖續航機器人平臺試驗

3.1 BDS失鎖續航導航控制器

為構建機器人失鎖續航系統，基于BDS失鎖變結構位姿信息處理方法設計了BDS失鎖續航導航控制器，該控制器能夠依據解類信息無縫切換BDS/INS組合導航和INS導航模式，運用自校準變結構位姿信息處理模型進行無縫位姿信息處理，與目標路徑相差獲得橫向偏差和航向偏差，輸入到基于深度強化學習網絡的導航控制模型中計算出目標行駛曲率[24]。模型輸入為BDS、INS和目標路徑信息，輸出為目標曲率。控制器結構如圖6。

圖6 BDS失鎖續航導航控制器結構圖

3.2 機器人導航試驗系統

為研究BDS失鎖續航方法的性能，搭建輪式差速機器人平臺。該平臺硬件部分包括輪式差速機器人(Husky Robotics Inc.)和衛星定位系統(Swift company)；定位信息獲取頻率為10 Hz，水平定位精度±(10+1×10-6D)mm，AHRS慣性傳感器(MicroStrain公司 3DM-GX5-25型)，陀螺儀非線性度0.02%，橫滾/俯仰精度±0.25°，采樣頻率30 Hz；控制終端為NVIDIA公司的 Jetson TX1 嵌入式計算機和觸控顯示屏。軟件部分為基于BDS失鎖續航導航控制模型的機器人導航控制軟件系統，該軟件系統運用QT5.0編寫加載于Ubuntu 16.04 LTS系統中。RKT-GNSS(BDS)定位信息和AHRS(INS)慣性航姿信息通過串口輸入到導航控制系統。該系統將計算獲得目標曲率和設定行駛速度，通過ROS機器人操作系統傳輸給Husky機器人，機器人自動執行目標控制信號實現自動導航行駛。系統結構如圖7，試驗平臺實物如圖8。

圖7 機器人失鎖續航導航試驗系統結構圖

圖8 機器人失鎖續航導航試驗系統

圖9 BDS失鎖狀態續航性能對比試驗實際橫向偏差

3.3 BDS失鎖狀態續航性能對比試驗

為研究BDS信號失鎖狀態下，控制器的無縫處理能力，在機器人系統直線跟蹤時人為軟件屏蔽BDS定位點信息的輸入和給出失鎖信號。只記錄BDS定位數據而不作為導航控制輸入。使用INS遞推軌跡作為導航控制器輸入進行路徑跟蹤控制。同時記錄BDS數據獲得實際軌跡，分析失鎖續航導航系統性能。

試驗設計：規劃直線路徑，使用機器人導航系統在草地進行直線導航跟蹤，行駛速度設置為1 m/s。當系統進入直線跟蹤的穩定狀態時，切換系統輸入為INS定位數據和非4類解信號。分別采用基于自校準變結構(Self-calibrating variable structure)Kalman濾波位姿信息處理方法(SCVS-K)的BDS失鎖續航導航控制器和未校準的變結構(Variable structure)Kalman濾波(VS-K)進行信息處理，失鎖續航控制的時間為20 s，每組控制器進行3次重復試驗。

3.4 機器人矩形路徑(含有轉彎環節)BDS失鎖狀態續航試驗

轉彎跟蹤時航向變化率較大，INS測量誤差增大，INS定位航位推算誤差累計較快，為測試矩形路徑(含有轉彎環節)導航失鎖續航控制器性能，采用與3.3節直線續航試驗相同的處理方法獲取BDS數據獲得實際軌跡與INS續航軌跡數據，分析失鎖續航導航系統性能。

試驗設計：規劃矩形路徑，使用機器人導航系統在草地進行矩形路徑導航跟蹤，直線行駛部分速度設置為1 m/s，轉彎上線部分速度設置為0.5 m/s。當系統進入直線跟蹤的穩定狀態時，切換系統輸入為INS定位續航狀態數據和非4類解信號。采用基于SCVS-K方法的BDS失鎖續航導航控制器進行信息處理，失鎖續航控制對矩形路徑進行續航，進行3次重復試驗。

表1 BDS失鎖狀態續航性能對比試驗結果

圖10 機器人矩形路徑BDS失鎖續航試驗1軌跡

圖11 續航試驗BDS定位與INS定位差值變化曲線

圖12 續航試驗實際橫向誤差變化曲線

表2 機器人矩形路徑BDS失鎖續航試驗結果

4 農機導航系統BDS失鎖續航田間試驗

4.1 農機導航系統

為驗證BDS失鎖續航控制方法田間工作效果，將BDS失鎖續航控制方法移植到雷沃重工AGCS-Ⅰ自動導航系統中，搭載于雷沃重工M-1104型拖拉機，在雷沃阿波斯智慧農業示范基地的試驗田進行田間試驗。RTK-GNSS定位模塊(司南公司的K728)，定位信息獲取頻率為10 Hz，水平定位精度±(10+1×10-6D)mm；AHRS慣性傳感器(XSENS公司MTi-30 AHRS)，陀螺儀非線性度0.01%，橫滾/俯仰精度±0.3°，采樣頻率25 Hz；前輪轉角傳感器為BEI-9902120CW型，非線性度為±2%，A/D采樣精度為12位；轉向執行機構為力士樂公司的HT801053；控制終端為AGCS-Ⅰ控制器和觸控顯示屏。運用Metrowerks Code Warrior for ARM Developer Suite v1.2進行軟件開發。系統結構如圖13所示，試驗平臺實物如圖14所示。

圖13 農機導航系統試驗平臺結構圖

圖14 農機導航系統田間試驗

圖15 BDS失鎖狀態田間續航試驗實際橫向偏差變化曲線

4.2 BDS失鎖狀態田間續航試驗

為研究BDS信號失鎖系統的田間導航性能，在農機導航系統直線跟蹤時人為軟件屏蔽BDS定位點信息的輸入和給出失鎖信號。采用與機器人試驗相同方法，記錄BDS數據和INS推算數據獲得實際軌跡和遞推軌跡，分析失鎖續航導航系統性能。

試驗設計：規劃直線路徑，使用機器人導航系統在試驗田地進行直線導航跟蹤，直線行駛速度設置為1 m/s。當系統進入直線跟蹤的穩定狀態時，切換系統輸入為INS定位續航狀態數據和非4類解信號。采用基于SCVS-K方法和未校準的 VS-K方法分別對直線路徑進行BDS失鎖續航處理，分別進行3次重復試驗。

試驗分析：農機田間試驗中，3組SCVS-K方法失鎖續航過程的實際橫向偏差ed如圖15a所示，3組VS-K方法失鎖續航過程的實際橫向偏差如圖15b所示。對比試驗結果如表3，SCVS-K處理方法平均L20 cm達到16.65 m，滿足棉花播種失鎖應激處理的要求。

文獻[12]中采用基于神經網絡預測的誤差反饋矯正方法，在車輛行駛速度為16 m/s時，失鎖時間25 s內位置誤差為3.5 m。文獻[15]中采用基于立方Kalman濾波器和神經網絡的失鎖定位方法，速度為1.48 m/s時，500 s內定位誤差為23.11 m。在文獻[17]的研究中使用MEMS慣性傳感器在GPS失鎖時進行續航，20 s內誤差超過10 m。上述研究沒有對分米級精度失鎖續航特性進行細分研究。文獻[25]采用擴展Kalman濾波器進行拖拉機失鎖續航，橫向誤差達到20 cm時，行駛距離小于10 m。使用本文方法續航，橫向誤差達到20 cm時，行駛距離大于15 m。

表3 BDS失鎖狀態田間續航試驗結果

5 結論

(1)針對農機導航作業過程中因存在BDS信號失鎖而導致突然失控的問題，提出了一種基于自校準變結構Kalman濾波的農機導航BDS失鎖續航方法，使操作人員有足夠的時間和緩沖距離進行進一步處理，減輕由于突然失控導致的農資損失和額外勞動強度。該方法為棉花播種導航作業過程中BDS失鎖應急續航處理提供了研究基礎。

(3)根據農機導航系統低速、高精度的特點，進行了BDS失鎖續航方法的田間試驗，試驗結果表明：在實際偏差小于20 cm的條件下，農機在路徑上的行駛平均距離達到16.65 m，預留反應距離能夠滿足農機導航作業需求。

(4)BDS失鎖試驗結果表明，INS導航航向角速率測量零偏對直線路徑失鎖續航影響較大，進一步提高零偏的校準效率和精度能夠提高直線路徑失鎖續航質量和可靠性。

(5)機器人矩形路徑BDS失鎖續航試驗表明，系統在轉彎環節續航誤差迅速增加，并影響后續續航效果，這主要是測量誤差、采樣率和系統延時等因素導致，進一步研究導航航向和航向變化率的測量、采樣、預測和插值方法，能夠提高非直線續航質量。