基于李群的视觉/惯性组合导航算法

王景琪，刘海颖，王馨瑶，王晓龙

基于李群的视觉/惯性组合导航算法

王景琪，刘海颖，王馨瑶，王晓龙

（南京航空航天大学 航天学院，南京 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—），男，江苏盐城人，硕士研究生，研究方向为组合导航。