王景琪，劉海穎，王馨瑤，王曉龍

基于李群的視覺/慣性組合導(dǎo)航算法

王景琪，劉海穎，王馨瑤，王曉龍

（南京航空航天大學(xué) 航天學(xué)院，南京 210016）

為了進(jìn)一步研究高性能導(dǎo)航，提出基于李群的視覺慣性組合導(dǎo)航算法：利用單目相機(jī)對全局地圖上一定數(shù)量的3維固定路標(biāo)進(jìn)行跟蹤觀測，并采用多速率融合方法解決視覺慣導(dǎo)工作頻率不一致問題；構(gòu)建視覺慣性緊耦合模型，然后采用基于李群的無跡卡爾曼濾波方法和不變擴(kuò)展卡爾曼濾波方法對觀測信息與慣導(dǎo)數(shù)據(jù)進(jìn)行融合并相互比較。通過數(shù)值仿真將2種濾波結(jié)果與采用傳統(tǒng)無跡卡爾曼濾波算法的濾波結(jié)果進(jìn)行對比，結(jié)果表明2種濾波方法都可以有效抑制系統(tǒng)誤差，提升導(dǎo)航精度。

視覺信息；慣性導(dǎo)航；李群；無跡卡爾曼濾波；不變擴(kuò)展卡爾曼濾波

0 引言

傳統(tǒng)的導(dǎo)航方法中，衛(wèi)星導(dǎo)航處于不可動搖的地位。隨著導(dǎo)航技術(shù)的發(fā)展，人們開始著眼于解決在衛(wèi)星導(dǎo)航信號較弱甚至缺失的情況下選擇合適的導(dǎo)航方法來保證系統(tǒng)正常工作的問題。慣導(dǎo)輸出速率高，且不受環(huán)境變化的影響，但是慣導(dǎo)會隨著時(shí)間積累誤差。視覺傳感器輸出速率低，但是經(jīng)過圖像處理后能提供準(zhǔn)確的圖像信息。2者組合后導(dǎo)航質(zhì)量會有較大的提升，可以為智能體提供自主權(quán)，在陌生雜亂的環(huán)境中進(jìn)行導(dǎo)航[1]。因而視覺慣性導(dǎo)航系統(tǒng)（visual inertial navigantion system, VINS）因其巨大的潛力而成為熱門的研究方向[2-3]。文獻(xiàn)[4]提出1種視覺/慣性融合的艦載著陸引導(dǎo)方法，并采用擴(kuò)展卡爾曼濾波（extended Kalman filter, EKF）算法對無人機(jī)和船舶的運(yùn)動進(jìn)行估計(jì)；文獻(xiàn)[5]提出了1種基于視覺和慣性測量信息融合的無人機(jī)自主導(dǎo)航方案，通過幀間圖像特征檢測和跟蹤，對無人機(jī)相對平移和旋轉(zhuǎn)運(yùn)動參數(shù)進(jìn)行估計(jì)。文獻(xiàn)[6]基于全局3維點(diǎn)約束下的視覺慣性緊耦合模型對系統(tǒng)實(shí)時(shí)運(yùn)動狀態(tài)進(jìn)行估計(jì)。同時(shí)關(guān)于更好地融合這2種傳感器的測量結(jié)果，文獻(xiàn)[7]介紹了1種基于多狀態(tài)約束的視覺/慣性組合導(dǎo)航算法。文獻(xiàn)[8]介紹了1種應(yīng)用于流形空間的無跡卡爾曼濾波算法，并簡單提及不變擴(kuò)展卡爾曼濾波概念。

以文獻(xiàn)[8]為基礎(chǔ)，本文構(gòu)建了視覺慣性緊耦合模型，詳細(xì)推導(dǎo)了基于李群空間下的無跡卡爾曼濾波算法（unscented Kalman filter on Lie groups, L-UKF）和不變擴(kuò)展卡爾曼濾波算法（invariant extended Kalman filter, InEKF）并進(jìn)行對比。

1 整體框架

慣導(dǎo)數(shù)據(jù)的輸出速率和圖像數(shù)據(jù)的輸出速率差異較大，出現(xiàn)了視覺慣導(dǎo)工作頻率不一致的問題[9]。故以慣導(dǎo)周期為采樣濾波周期，采取多速率數(shù)據(jù)融合方法完成視覺與慣導(dǎo)數(shù)據(jù)的融合。即當(dāng)前時(shí)刻只有慣導(dǎo)數(shù)據(jù)輸入時(shí)，視覺慣性緊耦合模型只對慣導(dǎo)測量數(shù)據(jù)進(jìn)行解算并更新系統(tǒng)狀態(tài)量；當(dāng)同時(shí)檢測到圖像數(shù)據(jù)輸入時(shí)，視覺慣性緊耦合模型將視覺觀測信息和慣導(dǎo)數(shù)據(jù)融合并更新系統(tǒng)狀態(tài)量。

如圖1所示，構(gòu)建視覺慣性緊耦合模型，慣性傳感器輸出3軸加速度和角速度，相機(jī)提供對3維路標(biāo)點(diǎn)的觀測，將相機(jī)觀測值和慣導(dǎo)測量值輸入耦合模型，對系統(tǒng)狀態(tài)量進(jìn)行估計(jì)。同時(shí)為了保證系統(tǒng)的實(shí)時(shí)性，每當(dāng)下1幀圖像數(shù)據(jù)輸入進(jìn)來，自動從圖像幀上提取路標(biāo)觀測值并更新相機(jī)觀測量。

圖1 視覺慣導(dǎo)緊耦合模型

其中，3維路標(biāo)的選取以及從圖像幀中的提取對應(yīng)觀測值流程如下：

對于估計(jì)補(bǔ)充3維路標(biāo)點(diǎn)的3維空間坐標(biāo)問題，可以通過反向追蹤路標(biāo)在前序圖像幀投影點(diǎn)，建立與前面圖像幀之間的約束。采用線性三角形法（linear triangulation methods），通過多個視圖匹配的2維特征點(diǎn)來恢復(fù)3維路標(biāo)空間坐標(biāo)。為了方便表述，問題可以簡化成：得到2個視圖的1組匹配點(diǎn)時(shí)，如何使用線性三角形法來恢復(fù)點(diǎn)在3維空間的結(jié)構(gòu)。

此時(shí)相當(dāng)于求解最小二乘問題，也就是求出矩陣的最小奇異值對應(yīng)的單位奇異向量，再經(jīng)過縮放得到點(diǎn)在3維空間的坐標(biāo)。為保證估計(jì)特征點(diǎn)空間坐標(biāo)值的準(zhǔn)確性，利用線性三角形法恢復(fù)路標(biāo)3維空間坐標(biāo)所采用的視圖數(shù)量為7。

2 基于李群的視覺慣性測量信息融合算法

2.1 李群介紹

2.2 系統(tǒng)狀態(tài)空間模型

1）系統(tǒng)的動力模型為

2）系統(tǒng)的觀測模型

2.3 基于李群的無跡卡爾曼濾波算法

傳統(tǒng)UKF算法是對sigma點(diǎn)采樣，然后利用狀態(tài)模型投影形成了1個高斯分布來處理均值和協(xié)方差的非線性傳遞；而L-UKF算法是在李代數(shù)空間上進(jìn)行sigma點(diǎn)的采樣投影到李群空間，并基于狀態(tài)模型以及李群李代數(shù)之間的映射關(guān)系，在李代數(shù)空間上形成1個高斯分布來處理協(xié)方差的非線性傳遞。

1）狀態(tài)預(yù)測

2）量測更新

①計(jì)算采樣點(diǎn)為

3）濾波更新

①計(jì)算增益為

2.4 不變擴(kuò)展卡爾曼濾波算法

普通EKF對動態(tài)方程進(jìn)行線性化估計(jì)狀態(tài)與狀態(tài)的協(xié)方差，但是在誤差傳遞過程中，誤差傳遞矩陣依賴當(dāng)前狀態(tài)量的估計(jì)值。在有噪聲引入時(shí)，狀態(tài)估計(jì)量是無法預(yù)測的，可能會導(dǎo)致濾波器發(fā)散[11]。InEKF則是通過改變狀態(tài)誤差的定義方法，作用于李群空間實(shí)現(xiàn)誤差傳遞矩陣與狀態(tài)估計(jì)值的相對獨(dú)立。

1)狀態(tài)預(yù)測

2）量測更新

根據(jù)InEKF的誤差定義，更新后誤差為

則誤差狀態(tài)更新方程為

簡化式（8）得

因而狀態(tài)誤差更新方程為

3）濾波更新

3 實(shí)驗(yàn)與結(jié)果分析

使用Euroc數(shù)據(jù)集[12]驗(yàn)證算法，數(shù)據(jù)集選擇微型飛行器作為載具，并配置了視覺慣性傳感器采集數(shù)據(jù)。校正后的圖像分辨率為752×480個像素，采集頻率為20 Hz，IMU型號為ADIS16448，采樣率為200 Hz。同時(shí)飛行器的真實(shí)路徑以及位姿信息由Lecia MS50激光雷達(dá)以及Vicon運(yùn)動捕捉系統(tǒng)提供。選擇數(shù)據(jù)集中的Vicon Room 201和Vicon Room 202數(shù)據(jù)集作為第1、2組數(shù)據(jù)集，對L-UKF算法、InEKF算法和傳統(tǒng)UKF算法進(jìn)行仿真對比，并從位姿估計(jì)角度誤差驗(yàn)證算法魯棒性與準(zhǔn)確度。

圖2直觀描述了L-UKF算法、InEKF算法和UKF算法在使用數(shù)據(jù)集上的運(yùn)行結(jié)果，深灰色線表示真實(shí)運(yùn)動軌跡，其他線表示各算法處理估計(jì)的運(yùn)行軌跡。從圖2中各算法解算得到的飛行路徑可以看出，算法估計(jì)的運(yùn)動軌跡和真實(shí)軌跡基本符合，主要誤差存在于飛行起始處以及飛行器急轉(zhuǎn)彎處。由于本文算法不包含閉環(huán)檢測，因此解算得到的位置誤差存在不斷累積的趨勢。圖2中Real表示真實(shí)運(yùn)動軌跡。

圖2 模擬飛行路徑與真實(shí)飛行路徑

為驗(yàn)證L-UKF算法和InEKF算法在提高位姿估計(jì)性能方面的有效性，采用3方向位置和姿態(tài)角在各個時(shí)刻的誤差以及整個運(yùn)行軌跡的3方向上的位置和姿態(tài)角誤差的均方根誤差（root mean square error, RMSE）作為評價(jià)指標(biāo)。圖3、圖4表示在第1、2組數(shù)據(jù)集下基于3種濾波方法估計(jì)導(dǎo)航位姿誤差的對比結(jié)果。

圖3描述的是在第1組數(shù)據(jù)集下3種濾波方法解算得到各個時(shí)刻的姿態(tài)角和位置量與真實(shí)值之間的誤差。圖中可以看到在3種算法估計(jì)性能相似，其中L-UKF算法估計(jì)值相對最優(yōu)。

圖4描述的是在第2組數(shù)據(jù)集下3種濾波方法解算得到各個時(shí)刻的姿態(tài)角和位置量與真實(shí)值之間的誤差。圖4中可以看到由于第2組數(shù)據(jù)集中飛行器急轉(zhuǎn)彎次數(shù)較多，UKF算法解算得到的姿態(tài)角誤差明顯相對較差，而InEKF和L-UKF算法仍能得到相對較好且穩(wěn)定的估計(jì)值。

表1 第1組數(shù)據(jù)集下導(dǎo)航位姿的均方根誤差

表2 第2組數(shù)據(jù)集下導(dǎo)航位姿的均方根誤差

表1和表2記錄了2組數(shù)據(jù)集下基于3種濾波方法估計(jì)導(dǎo)航位姿的RMSE結(jié)果，可以看出L-UKF和InEKF濾波效果優(yōu)于傳統(tǒng)UKF濾波效果，驗(yàn)證了本文算法的有效性。如表1所示，3種濾波算法對位置誤差估計(jì)效果類似，而姿態(tài)誤差估計(jì)方面，L-UKF算法解算效果優(yōu)于InEKF算法，且2種改進(jìn)濾波算法解算效果都優(yōu)于傳統(tǒng)UKF算法。如表2所示，第2組數(shù)據(jù)集解算結(jié)果顯示3種濾波算法對位置誤差估計(jì)效果類似，而姿態(tài)誤差估計(jì)方面，InEKF算法解算結(jié)果優(yōu)于L-UKF算法，且2種改進(jìn)濾波算法效果都優(yōu)于傳統(tǒng)UKF算法。

4 結(jié)束語

為了滿足系統(tǒng)在衛(wèi)星導(dǎo)航信號較弱甚至缺失時(shí)正常工作的需要，本文提出了基于李群的視覺/慣性組合導(dǎo)航算法。利用單目相機(jī)對全局地圖上的3維固定路標(biāo)進(jìn)行跟蹤觀測獲得觀測信息，并采用多速率融合方法解決視覺、慣性傳感器工作頻率不一致的問題；構(gòu)建視覺慣性緊耦合模型，模型采用L-UKF和InEKF 2種不同的方法分別對視覺觀測信息與慣導(dǎo)數(shù)據(jù)融合以實(shí)現(xiàn)導(dǎo)航狀態(tài)的濾波估計(jì)。并通過仿真將2種濾波結(jié)果與采用傳統(tǒng)UKF方法的濾波結(jié)果進(jìn)行對比，驗(yàn)證了2種濾波方法都可以有效抑制系統(tǒng)誤差，提升導(dǎo)航性能。

[1]LOIANNO G, WATTERSON M, KUMAR V. Visual inertial odometry for quadrotors on SE(3)[EB/OL]. [2019-08-28].https://www.seas.upenn.edu/~wami/papers/icra2016-1.pdf.

[2]ECKENHOFF K, GENEVA P, HUANG G Q.Direct visual-inertial navigation with analytical preintegration[EB/OL]. [2019-08-28]. http://udel.edu/~pgeneva/downloads/papers/c02.pdf.

[3]于永軍, 徐錦法, 張梁, 等. 慣導(dǎo)/雙目視覺位姿估計(jì)算法研究[J]. 儀器儀表學(xué)報(bào), 2014, 35(10): 2170-2176.

[4]MENG Y, WANG W, HAN H,et al. A visual/inertial integrated landing guidance method for UAV landing on the ship[J]. Aerospace Science and Technology, 2019, 85: 474-480.

[5]LI J G, LIU Q, LIU Q B, et al. Fusion of visual and inertial measurement information for unmanned aerial vehicles autonomous navigation in unknown environment[C]//The Institute of Electrical and Electronic Engineers(IEEE). Proceedings of 201720th International Conference on Information Fusion: Fusion. Xi'an, China: IEEE Inc., 2017: 1-6.

[6]程傳奇, 郝向陽, 李建勝, 等. 基于非線性優(yōu)化的單目視覺/慣性組合導(dǎo)航算法[J]. 中國慣性技術(shù)學(xué)報(bào), 2017, 25(5): 643-649.

[7]蔡迎波. 基于多狀態(tài)約束的視覺/慣性組合導(dǎo)航算法研究[J]. 光學(xué)與光電技術(shù), 2015, 13(6): 58-62.

[8]BROSSARD M, BONNABEL S, BARRAU A. Unscented Kalman filtering on Lie groups for fusion of IMU and monocular vision[C]//The Institute of Electrical and Electronic Engineers(IEEE). Proceedings of 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems: IROS. Madrid, Spain: IEEE Inc. , 2018: 649-655.

[9]王晨琳, 劉海穎, 蔣鑫. 一種單目視覺信息輔助慣性導(dǎo)航算法[J]. 導(dǎo)航定位學(xué)報(bào), 2018, 6(4): 19-23, 28.

[10]白龍, 董志峰, 戈新生. 基于李群的水下航體動力學(xué)建模及最優(yōu)控制[J]. 系統(tǒng)仿真學(xué)報(bào), 2016, 28(5):1150-1157.

[11]BARRAU A.Non-linear state error based extended Kalman filters with applications to navigation[D]. Paris: Paris Institute of Technology, 2015: 1-8.

[12]BURRI M, NIKOLIC J, GOHL P, et al. The EuRoC micro aerial vehicle datasets[J]. International Journal of Robotics Research, 2016, 35(10): 1157-1163.

Visual/inertial integrated navigation algorithm on Lie groups

WANG Jingqi, LIU Haiying, WANG Xinyao, WANG Xiaolong

（College of Astronautics, Nanjing University of Aeronautics and Astronautics, Nanjing 210016, China）

In order to further study on the efficient navigation, the paper proposed visual/inertial integrated navigation algorithm based on Lie groups: the monocular camera was used to track and observe a certain number of 3D fixed landmarks on the global map, and the multi-rate fusion method was used to solve the inconsistency of the visual and inertial working frequency; the visual and inertia tight coupling model was constructed, then two filtering methods of unscented Kalman filtering method on Lie groups and invariant extended Kalman filtering method were used to fuse and compare between the observed information with the INS data. Finally, the numerical simulation was used to compare the two different kinds of filtering results with those using the traditional unscented Kalman filtering algorithm, and result showed that the two methods could both effectively suppress system errors and improve the navigation accuracy.

visual information; inertial navigation; Lie groups; unscented Kalman filter; invariant extended Kalman filter

P228.4

A

2095-4999(2020)02-0036-07

王景琪，劉海穎，王馨瑤，等. 基于李群的視覺/慣性組合導(dǎo)航算法[J]. 導(dǎo)航定位學(xué)報(bào), 2020, 8(2): 36-42.（WANG Jingqi, LIU Haiying, WANG Xinyao, et al. Visual/inertial integrated navigation algorithm on Lie groups[J]. Journal of Navigation and Positioning, 2020, 8(2): 36-42.）

10.16547/j.cnki.10-1096.20200207.

2019-09-02

中央高?；究蒲袠I(yè)務(wù)專項(xiàng)（NS2019047）。

王景琪（1995—），男，江蘇鹽城人，碩士研究生，研究方向?yàn)榻M合導(dǎo)航。