李文華, 汪立新, 沈 強(qiáng), 李 燦, 吳宗收

(火箭軍工程大學(xué)導(dǎo)彈工程學(xué)院, 陜西 西安 710025)

0 引 言

隨著導(dǎo)航技術(shù)的不斷發(fā)展,各種導(dǎo)航手段和數(shù)據(jù)處理方法不斷涌現(xiàn),組合導(dǎo)航和信息融合技術(shù)的研究和發(fā)展方興未艾。多源傳感器信息融合系統(tǒng)相比于單一傳感器系統(tǒng)具有更高精度和穩(wěn)定性,尤其在各種復(fù)雜環(huán)境下仍可實(shí)現(xiàn)長(zhǎng)時(shí)間高精度導(dǎo)航。傳統(tǒng)慣導(dǎo)/衛(wèi)導(dǎo)組合導(dǎo)航在各導(dǎo)航鄰域已得到廣泛的應(yīng)用,但在衛(wèi)星信號(hào)受干擾無(wú)法使用時(shí)難以保證導(dǎo)航精度,在復(fù)雜環(huán)境下穩(wěn)定性較差。

微機(jī)械慣性導(dǎo)航系統(tǒng)(micro-electro-mechanical system, inertial navigation system, MEMS-INS)體積小、成本低,但精度較差;全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system,GNSS)定位精度高,實(shí)時(shí)性好;視覺(jué)里程計(jì)(visual odometry, VO)自主性強(qiáng),可以提供較高精度位置、姿態(tài)信息,近年來(lái)發(fā)展迅速。為此,構(gòu)建基于MEMS的INS/GNSS/VO組合導(dǎo)航方法以提升導(dǎo)航系統(tǒng)在復(fù)雜環(huán)境下的可靠性和穩(wěn)定性,且整個(gè)導(dǎo)航系統(tǒng)體積小、成本低、可適用性強(qiáng)。

目前,卡爾曼濾波(Kalman filter, KF)方法在組合導(dǎo)航中應(yīng)用廣泛,但是當(dāng)建立模型不準(zhǔn)確、外界擾動(dòng)較大時(shí),卡爾曼濾波算法容易發(fā)散,且標(biāo)準(zhǔn)KF僅適用于線性系統(tǒng)。針對(duì)非線性系統(tǒng),最常用和有效的方法是擴(kuò)展KF(extended KF, EKF),但EKF依然無(wú)法克服觀測(cè)量受噪聲干擾時(shí)對(duì)導(dǎo)航精度的影響,魯棒性較差。

1964年,Huber結(jié)合兩種范數(shù)提出了廣義極大似然估計(jì)方法,即Huber方法,其魯棒性優(yōu)于基于范數(shù)的估計(jì)方法。針對(duì)系統(tǒng)存在異常觀測(cè)值的問(wèn)題,研究者們將Huber方法或?qū)ζ涓倪M(jìn)后與KF結(jié)合起來(lái)進(jìn)行組合導(dǎo)航,以提升導(dǎo)航系統(tǒng)的魯棒性,但KF不適用于非線性系統(tǒng),而組合導(dǎo)航系統(tǒng)往往是非線性的。文獻(xiàn)[15-17]將Huber方法與非線性濾波方法結(jié)合起來(lái)進(jìn)行組合導(dǎo)航,取得不錯(cuò)的效果。

針對(duì)以上問(wèn)題,本文將Huber方法與EKF融合,提出一種基于魯棒EKF的MEMS-INS/GNSS/VO組合導(dǎo)航方法,以有效提升系統(tǒng)在復(fù)雜環(huán)境下以及觀測(cè)量受噪聲干擾時(shí)的可靠性和魯棒性。

1 組合導(dǎo)航濾波模型

本文構(gòu)建的MEMS-INS/GNSS/VO組合導(dǎo)航系統(tǒng)以INS為主,當(dāng)GNSS信號(hào)有效時(shí),采用MEMS-INS/GNSS組合導(dǎo)航,當(dāng)GNSS信號(hào)無(wú)效時(shí),采用MEMS-INS/VO組合導(dǎo)航。

建立組合導(dǎo)航系統(tǒng)的線性濾波模型如下:

(1)

式中:為狀態(tài)向量;為量測(cè)向量;-1為狀態(tài)一步轉(zhuǎn)移矩陣,可參考文獻(xiàn)[4]進(jìn)行設(shè)置;為量測(cè)矩陣;是均值為0方差陣為的系統(tǒng)噪聲向量;是均值為0方差陣為的量測(cè)噪聲向量。

選取15維狀態(tài)向量如下:

(2)

當(dāng)GNSS信號(hào)有效時(shí),選取INS和GNSS的速度誤差和位置誤差作為組合導(dǎo)航系統(tǒng)的觀測(cè)量:

(3)

對(duì)應(yīng)量測(cè)噪聲協(xié)方差陣由GNSS接收機(jī)速度量測(cè)噪聲和位置量測(cè)噪聲構(gòu)成,對(duì)應(yīng)的量測(cè)矩陣如下:

=[,,]

(4)

VO可以提供較為精確的姿態(tài)和位置信息,但VO提供的是以第一幀圖像為參考坐標(biāo)系的世界坐標(biāo)系下的結(jié)果,需轉(zhuǎn)換到地理導(dǎo)航坐標(biāo)系(系)下。當(dāng)GNSS信號(hào)無(wú)效時(shí),選取INS和VO的姿態(tài)誤差和位置誤差作為組合導(dǎo)航系統(tǒng)的觀測(cè)量:

(5)

對(duì)應(yīng)量測(cè)噪聲協(xié)方差陣由VO速度量測(cè)噪聲和位置量測(cè)噪聲構(gòu)成,對(duì)應(yīng)的量測(cè)矩陣如下:

(6)

(7)

2 基于Huber的魯棒EKF

在工程實(shí)踐中遇到的導(dǎo)航系統(tǒng)往往是非線性的,即系統(tǒng)方程和量測(cè)方程均是非線性的。EKF可解決系統(tǒng)方程和量測(cè)方程均為非線性時(shí)的估計(jì)。EKF是對(duì)原系統(tǒng)和量測(cè)先進(jìn)行泰勒級(jí)數(shù)展開(kāi),略去高階項(xiàng)后近似為線性系統(tǒng),再應(yīng)用第一節(jié)所建模型作線性Kalman濾波估計(jì)。

假設(shè)系統(tǒng)非線性模型如下:

(8)

則EKF方程如下:

(9)

基于KF或EKF的導(dǎo)航系統(tǒng)在復(fù)雜環(huán)境下易受到干擾,異常觀測(cè)值會(huì)嚴(yán)重影響導(dǎo)航精度,魯棒性較差。而Huber濾波方法通過(guò)重新構(gòu)造量測(cè)噪聲協(xié)方差陣可以有效降低異常觀測(cè)值對(duì)系統(tǒng)的影響,提升系統(tǒng)的魯棒性。

Huber代價(jià)函數(shù)結(jié)合了和范數(shù)的特性,是Huber所提出的M估計(jì)中最常用的代價(jià)函數(shù),其表達(dá)式如下:

(10)

Huber權(quán)值函數(shù)表達(dá)式如下:

(11)

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

(12)

利用式(12)對(duì)量測(cè)噪聲協(xié)方差矩陣進(jìn)行修正,得到修正后的量測(cè)噪聲協(xié)方差陣如下:

(13)

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

由式(12)構(gòu)造權(quán)值矩陣,并根據(jù)式(13)計(jì)算修正后的量測(cè)噪聲協(xié)方差陣;

3 試驗(yàn)與分析

為驗(yàn)證所提方法可靠性和魯棒性,采用仿真試驗(yàn)和KITTI數(shù)據(jù)集中一段行車軌跡進(jìn)行分析。

3.1 仿真試驗(yàn)

設(shè)計(jì)一個(gè)持續(xù)1 300 s的運(yùn)動(dòng)軌跡,如圖1所示,包含加減速、直線、轉(zhuǎn)彎、爬坡、下坡等機(jī)動(dòng)動(dòng)作。

圖1 運(yùn)動(dòng)軌跡

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

=(1-)+

(14)

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

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

式(14)所示的分布被稱為污染高斯分布,當(dāng)干擾分布為具有較大標(biāo)準(zhǔn)偏差的高斯分布時(shí),污染高斯分布也被稱為混合高斯分布。本文中設(shè)置干擾分布的標(biāo)準(zhǔn)差為GNSS信號(hào)的20倍,污染分布的比值為01。

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

圖2 仿真實(shí)驗(yàn)位置誤差

圖3 仿真實(shí)驗(yàn)速度誤差

圖4 仿真實(shí)驗(yàn)姿態(tài)誤差

對(duì)比圖2～圖4中3種濾波方法誤差結(jié)果,可以看出,HREKF算法融合結(jié)果的精度都高于AKF和EKF,且當(dāng)存在大角度的初始姿態(tài)誤差時(shí),HREKF的濾波結(jié)果相較于AKF和EKF可以更快更準(zhǔn)確的收斂,說(shuō)明HREKF算法能夠較好地處理非線性系統(tǒng)。當(dāng)GNSS輸出信號(hào)受到污染以及在4倍初始誤差情形下,AKF和EKF濾波效果變差,對(duì)導(dǎo)航精度影響較大,但HREKF通過(guò)重構(gòu)量測(cè)噪聲協(xié)方差陣,降低了誤差的發(fā)散程度,仍能輸出較高精度結(jié)果,表明了所提HREKF算法有較強(qiáng)的魯棒性。

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

表1 仿真實(shí)驗(yàn)各參數(shù)RMSE

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

3.2 KITTI數(shù)據(jù)集驗(yàn)證

為驗(yàn)證本文所提組合導(dǎo)航方法在實(shí)際工程應(yīng)用中的可靠性和有效性,采用KITTI數(shù)據(jù)集對(duì)其進(jìn)行驗(yàn)證。KITTI數(shù)據(jù)集是目前國(guó)際上最大的自動(dòng)駕駛場(chǎng)景評(píng)測(cè)數(shù)據(jù)集,該數(shù)據(jù)集提供多種傳感器數(shù)據(jù),其中包含基于MEMS的OXTS RT 3003慣性導(dǎo)航系統(tǒng),2個(gè)灰度相機(jī),2個(gè)彩色相機(jī)。

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

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

圖5 drive_0036運(yùn)動(dòng)軌跡

圖6 drive_0033運(yùn)動(dòng)軌跡

對(duì)兩個(gè)序列采用3種濾波方法進(jìn)行組合導(dǎo)航的水平位置和速度誤差結(jié)果如圖7～圖10所示。從圖中可以看出,當(dāng)衛(wèi)星信號(hào)失效時(shí),VO的加入仍可約束慣導(dǎo)的累積誤差,輸出較高精度的導(dǎo)航結(jié)果,在衛(wèi)星信號(hào)失效20 s的情形下,水平定位誤差保持在12.5 m以內(nèi)。

圖7 drive_0036水平位置誤差

圖8 drive_0036水平速度誤差

圖9 drive_0033水平位置誤差

圖10 drive_0033水平速度誤差

為定量分析不同濾波方法在實(shí)際場(chǎng)景下的導(dǎo)航精度,計(jì)算了2011_09_30_drive_0033序列的水平位置和速度RMSE,如表2所示。從表2中可以看出,HREKF的水平位置和速度RMSE結(jié)果均優(yōu)于AKF和EKF,而EKF將量測(cè)噪聲協(xié)方差陣設(shè)置為固定值,其導(dǎo)航精度有時(shí)會(huì)低于AKF,但HREKF通過(guò)修正量測(cè)噪聲協(xié)方差陣,可以輸出較高精度導(dǎo)航結(jié)果,驗(yàn)證了基于HREKF的INS/GNSS/VO組合導(dǎo)航方法擁有較高的可靠性和魯棒性。

表2 drive_0033中各參數(shù)RMSE

4 結(jié) 論

本文研究了基于魯棒EKF的INS/GNSS/VO組合導(dǎo)航方法,將EKF算法與Huber方法結(jié)合,并在傳統(tǒng)衛(wèi)星/慣導(dǎo)組合導(dǎo)航方式基礎(chǔ)上加入了VO。仿真和KITTI數(shù)據(jù)集實(shí)驗(yàn)結(jié)果表明,組合導(dǎo)航系統(tǒng)在GNSS信號(hào)失效時(shí)仍能輸出較高精度的導(dǎo)航結(jié)果,且HREKF濾波結(jié)果相較于AKF和EKF擁有更高的精度和魯棒性,驗(yàn)證了提出組合導(dǎo)航方法的可靠性和魯棒性,非常適用于成本低、體積小的工程應(yīng)用背景。