基于李群的視覺/慣性組合導航算法

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

基于李群的視覺/慣性組合導航算法

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

（南京航空航天大學 航天學院，南京 210016）

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

視覺信息；慣性導航；李群；無跡卡爾曼濾波；不變擴展卡爾曼濾波

0 引言

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

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

1 整體框架

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

如圖1所示，構建視覺慣性緊耦合模型，慣性傳感器輸出3軸加速度和角速度，相機提供對3維路標點的觀測，將相機觀測值和慣導測量值輸入耦合模型，對系統狀態量進行估計。同時為了保證系統的實時性，每當下1幀圖像數據輸入進來，自動從圖像幀上提取路標觀測值并更新相機觀測量。

圖1 視覺慣導緊耦合模型

其中，3維路標的選取以及從圖像幀中的提取對應觀測值流程如下：

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

此時相當于求解最小二乘問題，也就是求出矩陣的最小奇異值對應的單位奇異向量，再經過縮放得到點在3維空間的坐標。為保證估計特征點空間坐標值的準確性，利用線性三角形法恢復路標3維空間坐標所采用的視圖數量為7。

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

2.1 李群介紹

2.2 系統狀態空間模型

1）系統的動力模型為

2）系統的觀測模型

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

傳統UKF算法是對sigma點采樣，然后利用狀態模型投影形成了1個高斯分布來處理均值和協方差的非線性傳遞；而L-UKF算法是在李代數空間上進行sigma點的采樣投影到李群空間，并基于狀態模型以及李群李代數之間的映射關系，在李代數空間上形成1個高斯分布來處理協方差的非線性傳遞。

1）狀態預測

2）量測更新

①計算采樣點為

3）濾波更新

①計算增益為

2.4 不變擴展卡爾曼濾波算法

普通EKF對動態方程進行線性化估計狀態與狀態的協方差，但是在誤差傳遞過程中，誤差傳遞矩陣依賴當前狀態量的估計值。在有噪聲引入時，狀態估計量是無法預測的，可能會導致濾波器發散[11]。InEKF則是通過改變狀態誤差的定義方法，作用于李群空間實現誤差傳遞矩陣與狀態估計值的相對獨立。

1)狀態預測

2）量測更新

根據InEKF的誤差定義，更新后誤差為

則誤差狀態更新方程為

簡化式（8）得

因而狀態誤差更新方程為

3）濾波更新

3 實驗與結果分析

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

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

圖2 模擬飛行路徑與真實飛行路徑

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

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

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

表1 第1組數據集下導航位姿的均方根誤差

表2 第2組數據集下導航位姿的均方根誤差

表1和表2記錄了2組數據集下基于3種濾波方法估計導航位姿的RMSE結果，可以看出L-UKF和InEKF濾波效果優于傳統UKF濾波效果，驗證了本文算法的有效性。如表1所示，3種濾波算法對位置誤差估計效果類似，而姿態誤差估計方面，L-UKF算法解算效果優于InEKF算法，且2種改進濾波算法解算效果都優于傳統UKF算法。如表2所示，第2組數據集解算結果顯示3種濾波算法對位置誤差估計效果類似，而姿態誤差估計方面，InEKF算法解算結果優于L-UKF算法，且2種改進濾波算法效果都優于傳統UKF算法。

4 結束語

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

[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]于永軍, 徐錦法, 張梁, 等. 慣導/雙目視覺位姿估計算法研究[J]. 儀器儀表學報, 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]程傳奇, 郝向陽, 李建勝, 等. 基于非線性優化的單目視覺/慣性組合導航算法[J]. 中國慣性技術學報, 2017, 25(5): 643-649.

[7]蔡迎波. 基于多狀態約束的視覺/慣性組合導航算法研究[J]. 光學與光電技術, 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]王晨琳, 劉海穎, 蔣鑫. 一種單目視覺信息輔助慣性導航算法[J]. 導航定位學報, 2018, 6(4): 19-23, 28.

[10]白龍, 董志峰, 戈新生. 基于李群的水下航體動力學建模及最優控制[J]. 系統仿真學報, 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

王景琪，劉海穎，王馨瑤，等. 基于李群的視覺/慣性組合導航算法[J]. 導航定位學報, 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

中央高校基本科研業務專項（NS2019047）。

王景琪（1995—），男，江蘇鹽城人，碩士研究生，研究方向為組合導航。