基于魯棒EKF的MEMS-INS/GNSS/VO組合導航方法

李文華, 汪立新, 沈 強, 李 燦, 吳宗收

(火箭軍工程大學導彈工程學院, 陜西 西安 710025)

0 引 言

隨著導航技術的不斷發展,各種導航手段和數據處理方法不斷涌現,組合導航和信息融合技術的研究和發展方興未艾。多源傳感器信息融合系統相比于單一傳感器系統具有更高精度和穩定性,尤其在各種復雜環境下仍可實現長時間高精度導航。傳統慣導/衛導組合導航在各導航鄰域已得到廣泛的應用,但在衛星信號受干擾無法使用時難以保證導航精度,在復雜環境下穩定性較差。

微機械慣性導航系統(micro-electro-mechanical system, inertial navigation system, MEMS-INS)體積小、成本低,但精度較差;全球導航衛星系統(global navigation satellite system,GNSS)定位精度高,實時性好;視覺里程計(visual odometry, VO)自主性強,可以提供較高精度位置、姿態信息,近年來發展迅速。為此,構建基于MEMS的INS/GNSS/VO組合導航方法以提升導航系統在復雜環境下的可靠性和穩定性,且整個導航系統體積小、成本低、可適用性強。

目前,卡爾曼濾波(Kalman filter, KF)方法在組合導航中應用廣泛,但是當建立模型不準確、外界擾動較大時,卡爾曼濾波算法容易發散,且標準KF僅適用于線性系統。針對非線性系統,最常用和有效的方法是擴展KF(extended KF, EKF),但EKF依然無法克服觀測量受噪聲干擾時對導航精度的影響,魯棒性較差。

1964年,Huber結合兩種范數提出了廣義極大似然估計方法,即Huber方法,其魯棒性優于基于范數的估計方法。針對系統存在異常觀測值的問題,研究者們將Huber方法或對其改進后與KF結合起來進行組合導航,以提升導航系統的魯棒性,但KF不適用于非線性系統,而組合導航系統往往是非線性的。文獻[15-17]將Huber方法與非線性濾波方法結合起來進行組合導航,取得不錯的效果。

針對以上問題,本文將Huber方法與EKF融合,提出一種基于魯棒EKF的MEMS-INS/GNSS/VO組合導航方法,以有效提升系統在復雜環境下以及觀測量受噪聲干擾時的可靠性和魯棒性。

1 組合導航濾波模型

本文構建的MEMS-INS/GNSS/VO組合導航系統以INS為主,當GNSS信號有效時,采用MEMS-INS/GNSS組合導航,當GNSS信號無效時,采用MEMS-INS/VO組合導航。

建立組合導航系統的線性濾波模型如下:

(1)

式中:為狀態向量;為量測向量;-1為狀態一步轉移矩陣,可參考文獻[4]進行設置;為量測矩陣;是均值為0方差陣為的系統噪聲向量;是均值為0方差陣為的量測噪聲向量。

選取15維狀態向量如下:

(2)

當GNSS信號有效時,選取INS和GNSS的速度誤差和位置誤差作為組合導航系統的觀測量:

(3)

對應量測噪聲協方差陣由GNSS接收機速度量測噪聲和位置量測噪聲構成,對應的量測矩陣如下:

=[,,]

(4)

VO可以提供較為精確的姿態和位置信息,但VO提供的是以第一幀圖像為參考坐標系的世界坐標系下的結果,需轉換到地理導航坐標系(系)下。當GNSS信號無效時,選取INS和VO的姿態誤差和位置誤差作為組合導航系統的觀測量:

(5)

對應量測噪聲協方差陣由VO速度量測噪聲和位置量測噪聲構成,對應的量測矩陣如下:

(6)

(7)

2 基于Huber的魯棒EKF

在工程實踐中遇到的導航系統往往是非線性的,即系統方程和量測方程均是非線性的。EKF可解決系統方程和量測方程均為非線性時的估計。EKF是對原系統和量測先進行泰勒級數展開,略去高階項后近似為線性系統,再應用第一節所建模型作線性Kalman濾波估計。

假設系統非線性模型如下:

(8)

則EKF方程如下:

(9)

基于KF或EKF的導航系統在復雜環境下易受到干擾,異常觀測值會嚴重影響導航精度,魯棒性較差。而Huber濾波方法通過重新構造量測噪聲協方差陣可以有效降低異常觀測值對系統的影響,提升系統的魯棒性。

Huber代價函數結合了和范數的特性,是Huber所提出的M估計中最常用的代價函數,其表達式如下:

(10)

Huber權值函數表達式如下:

(11)

式中:()由()求導所得,即()=′()。

(12)

利用式(12)對量測噪聲協方差矩陣進行修正,得到修正后的量測噪聲協方差陣如下:

(13)

用式(13)替代式(9)中的量測噪聲協方差陣,即為基于Huber的魯棒EKF算法(robust extended Kalman filter based on huber method, HREKF)。HREKF本質上是對量測噪聲協方差陣進行重加權,對不同大小的觀測殘差構造不同的權重,進而重構量測噪聲協方差陣,以克服異常觀測值對導航精度的影響,達到提升系統魯棒性的目的。HREKF的詳細步驟如下:

由式(12)構造權值矩陣,并根據式(13)計算修正后的量測噪聲協方差陣;

3 試驗與分析

為驗證所提方法可靠性和魯棒性,采用仿真試驗和KITTI數據集中一段行車軌跡進行分析。

3.1 仿真試驗

設計一個持續1 300 s的運動軌跡,如圖1所示,包含加減速、直線、轉彎、爬坡、下坡等機動動作。

圖1 運動軌跡

為驗證所提方法在傳感器有效性改變情況下的可靠性以及存在異常觀測值情況下的魯棒性,設置大角度的初始姿態誤差為[1°,1°,5°];設置GNSS信號在630～650 s、700～720 s期間不可用;在400～500 s、800～900 s期間加入混合高斯噪聲:

=(1-)+

(14)

式中:為污染分布的比值;為污染高斯噪聲的概率分布;為GNSS輸出信息的概率分布;為干擾分布。

在1 100～1 200 s期間異常,即將量測噪聲的幅值調大4倍;并采用Sage-Husa量測噪聲自適應KF (adaptive KF, AKF)、EKF和所提HREKF算法的濾波結果進行對比分析。

式(14)所示的分布被稱為污染高斯分布,當干擾分布為具有較大標準偏差的高斯分布時,污染高斯分布也被稱為混合高斯分布。本文中設置干擾分布的標準差為GNSS信號的20倍,污染分布的比值為01。

基于3種濾波方法的MEMS-INS/GNSS/VO組合導航系統仿真結果如圖2～圖4所示。由圖2～圖4可以看出,在整個時間段內3種濾波方法都能保持較高精度,且當GNSS信號在630～650 s、700～720 s期間不可用時,VO的引入仍可約束慣導的累積誤差,輸出較高精度的導航結果。其中,水平位置誤差在15 m以內,水平速度誤差在1.5 m/s以內,水平姿態誤差在130′以內,驗證了INS/GNSS/VO組合導航系統的可靠性。

圖2 仿真實驗位置誤差

圖3 仿真實驗速度誤差

圖4 仿真實驗姿態誤差

對比圖2～圖4中3種濾波方法誤差結果,可以看出,HREKF算法融合結果的精度都高于AKF和EKF,且當存在大角度的初始姿態誤差時,HREKF的濾波結果相較于AKF和EKF可以更快更準確的收斂,說明HREKF算法能夠較好地處理非線性系統。當GNSS輸出信號受到污染以及在4倍初始誤差情形下,AKF和EKF濾波效果變差,對導航精度影響較大,但HREKF通過重構量測噪聲協方差陣,降低了誤差的發散程度,仍能輸出較高精度結果,表明了所提HREKF算法有較強的魯棒性。

為定量分析不同濾波方法、不同情形對導航系統精度的影響,分別計算了在全時段(0～1 300 s)、混合高斯噪聲(800～900 s)和4倍初始誤差(1 100～1 200 s)下3種濾波方法各導航參數結果的均方根誤差(root mean square error, RMSE),如表1所示。

表1 仿真實驗各參數RMSE

從表1中可以看出,HREKF各導航參數的RMSE在全時段都優于AKF和EKF,尤其在混合高斯噪聲和4倍初始誤差情形下,HREKF的導航精度明顯優于AKF和EKF,進一步表明了基于HREKF的MEMS-INS/GNSS/VO組合導航方法的可靠性和魯棒性。

3.2 KITTI數據集驗證

為驗證本文所提組合導航方法在實際工程應用中的可靠性和有效性,采用KITTI數據集對其進行驗證。KITTI數據集是目前國際上最大的自動駕駛場景評測數據集,該數據集提供多種傳感器數據,其中包含基于MEMS的OXTS RT 3003慣性導航系統,2個灰度相機,2個彩色相機。

實驗中采用KITTI數據集中的兩段行車軌跡(“2011_09_26_drive_0036”和“2011_09_30_drive_0033”)對所提方法進行驗證。原始IMU數據為100 Hz,矯正后灰度圖像為10 Hz。采用ORB SLAM2框架對圖像處理后的結果作為VO輸出。陀螺漂移為36°/h,加速度計零偏為1 mg。以數據集中衛星信號提供的經緯度信息和水平速度信息作為參考基準。

KITTI數據集中2011_09_26_drive_0036序列衛星信號軌跡如圖5所示,為驗證本文方法性能,假設衛星信號在20～30 s、50～60 s期間無效;2011_09_30_drive_0033序列的衛星信號軌跡如圖6所示,假設衛星信號在50～60 s、125～145 s期間無效;并采用Sage-Husa量測噪聲AKF,EKF和HREKF算法的濾波結果進行對比分析。

圖5 drive_0036運動軌跡

圖6 drive_0033運動軌跡

對兩個序列采用3種濾波方法進行組合導航的水平位置和速度誤差結果如圖7～圖10所示。從圖中可以看出,當衛星信號失效時,VO的加入仍可約束慣導的累積誤差,輸出較高精度的導航結果,在衛星信號失效20 s的情形下,水平定位誤差保持在12.5 m以內。

圖7 drive_0036水平位置誤差

圖8 drive_0036水平速度誤差

圖9 drive_0033水平位置誤差

圖10 drive_0033水平速度誤差

為定量分析不同濾波方法在實際場景下的導航精度,計算了2011_09_30_drive_0033序列的水平位置和速度RMSE,如表2所示。從表2中可以看出,HREKF的水平位置和速度RMSE結果均優于AKF和EKF,而EKF將量測噪聲協方差陣設置為固定值,其導航精度有時會低于AKF,但HREKF通過修正量測噪聲協方差陣,可以輸出較高精度導航結果,驗證了基于HREKF的INS/GNSS/VO組合導航方法擁有較高的可靠性和魯棒性。

表2 drive_0033中各參數RMSE

4 結 論

本文研究了基于魯棒EKF的INS/GNSS/VO組合導航方法,將EKF算法與Huber方法結合,并在傳統衛星/慣導組合導航方式基礎上加入了VO。仿真和KITTI數據集實驗結果表明,組合導航系統在GNSS信號失效時仍能輸出較高精度的導航結果,且HREKF濾波結果相較于AKF和EKF擁有更高的精度和魯棒性,驗證了提出組合導航方法的可靠性和魯棒性,非常適用于成本低、體積小的工程應用背景。