基于改进Vins-Mono的INS/视觉组合定位方法研究

Research on INS/Vision Combination Localization Method Based on Improved Vins-Mono

摘要: 针对现有Vins-Mono算法在惯性导航系统(Inertial Navigation System, INS)与视觉组合定位中存在图像特征点提取分布不均匀、特征点匹配准确率低和系统定位精度不高的问题，提出一种改进的Vins-Mono算法。首先，采用四叉树算法划分图像特征点疏密不同的区域，在各个区域内同时进行特征点提取，加快了提取速率；然后，采用RANSAC算法进行特征点匹配，提高了匹配准确率，并结合汉明距离加快了匹配时间；最后，采用LM算法取代高斯牛顿算法，添加信赖区域控制更新步长，提高了系统定位精度。相比于传统Vins-Mono算法，本文算法在特征点提取分布均匀度上提高了54%，在特征点匹配准确率上提高了28.4%，在不同难度数据集上的平均定位精度提高了34%。

Abstract: Aiming at the problems of the existing Vins-Mono algorithm in combined positioning of Inertial Navigation System (INS) and vision, such as uneven distribution of image feature points extraction, low matching accuracy, and low positioning accuracy, an improved Vins-Mono algorithm is proposed. Firstly, the quadtree algorithm is used to divide regions with different densities of feature points and extract feature points simultaneously in each region to speed up the extraction rate. Then, the RANSAC algorithm is used to match the feature points, which improves the matching accuracy and speeds up the matching time by combining the Hamming distance. Finally, the LM algorithm is used to replace Gauss Newton algorithm, and trust region control is added to update the step size, which improves the positioning accuracy of the system. Compared with the traditional Vins-Mono algorithm, the algorithm in this paper improves the distribution uniformity of feature point extraction by 54%, the accuracy of feature point matching by 28.4%, and the average positioning accuracy on different difficulty datasets by 34%.

1. 引言

随着自动驾驶技术的兴起，人们对定位精度的要求越来越高。传统的定位方法依靠全球卫星导航系统(Global Navigation Satellite System, GNSS)和惯性导航系统(Inertial Navigation System, INS)，二者组合后能弥补各自不足，可以满足大多数场景下的定位精度要求。然而，处于封闭环境下，GNSS信号受到屏蔽，组合定位系统退化为单一INS，误差会随时间累积，无法长时间高精度定位。由于相机成本低、体积小、受封闭环境影响小等优点，利用视觉进行定位的视觉里程计技术得到了快速发展，视觉里程计和INS组合后的视觉–惯性里程计系统抗干扰性强，定位精度高，成为当下研究热点。

Leutenegger [1] 等提出了一种基于IMU预积分处理办法的VIO算法OKVIS，相较于MSCKF，提高了系统运行速率，以非线性优化算法取代滤波算法，使得系统定位精度更高；我国香港科技大学沈劭劼 [2] 团队提出了Vins-Mono算法，在OKVINS的基础上加入回环检测大大提高了系统定位精度，减少了轨迹漂移。MSCKF、OKVIS、Vins-Mono这三种为目前主流的INS/视觉组合定位方法，其中Vins-Mono算法的定位精度最高。

Vins-Mono算法中对图像特征点提取、匹配采用的是ORB算法和汉明距离，存在特征点提取不均匀、匹配准确率低的问题。季莘翔 [3] 等在ORB算法中引入密度峰值聚类算法，通过对提取出的特征点的密度峰值计算，优化特征点提取的有效性；余正强 [4] 等先通过预设条件，使得特征点稀疏化，再引入特征点数量调整机制，改善了特征点分布过于集中的现象，提高了视觉里程计的定位精度；骆大森 [5] 等在特征点匹配之前，先用相位相关法求取连续图像间的像素偏移，再采用最速邻近算法进行特征点匹配，相位相关法计算出的像素偏移作为限制条件，提高了匹配的准确率；陈坚炜 [6] 等采用粗匹配和精匹配结合的策略，粗匹配中匹配限制阈值为0.6，精匹配中利用单位球的约束除去误匹配的特征点；赵磊 [7] 等在后端优化中，选用g2o图优化的方式进行位姿估计，提高了系统定位的精度；卢艳军 [8] 等在传统图优化算法的基础上，在代价函数中引入鲁棒核函数，对环境引起的传感器误差进行过滤，提高了系统的定位精度。以上这些方法都改进了INS/视觉组合定位的精度，但其结果仍有提升空间。

文本针对Vins-Mono算法中存在图像特征点提取不均匀、匹配准确率低、定位精度低等问题，提出了一种改进的INS/视觉组合定位方法。主要从上述三个方面进行改进，在特征点提取中引入四叉树算法，改善特征点提取后不均匀的情况；采用RANSAC算法进行特征点匹配，提高匹配正确率，并结合汉明距离加快了匹配时间；使用LM优化算法提高了系统定位精度。

2. Vins-Mono算法原理

Vins-Mono算法结构 [9] 如图1所示，由输入端、前端视觉里程计、回环检测、后端非线性优化、输出端组成。

Figure 1. Vins-Mono structure

图1. Vins-Mono结构图

输入端通过相机采集图片，前端视觉里程计接受到图片后，采用ORB算法对图像进行特征点提取，以汉明距离作为阈值，匹配出连续两帧图像中相同的特征点，根据匹配后的特征点的像素坐标变化，采用对极几何算法估计出相机实际中的位姿变化。前端视觉里程计完成对相机位姿估计后进入回环检测模块，基于词袋模型判断采集的图像帧是否为关键帧，如果连续的图像帧都是关键帧，则触发了轨迹回环，将作为约束条件引入到后端非线性优化中。在后端非线性优化中，INS通过惯性测量单元获取数据，将IMU数据进行预积分处理减少计算冗余，根据前端视觉里程计中相机位姿的变化结果、IMU预积分解算结果和回环检测的约束条件，以紧耦合的方式构建目标函数，再通过高斯牛顿算法进行优化，最后将优化后的位姿结果输出。

3. Vins-Mono算法改进

针对传统Vins-Mono算法中存在图像特征点提取不均匀、特征点匹配准确率低和定位精度不高的问题，本文分别在前端视觉里程计中的特征点提取、特征点匹配和后端非线性优化中的优化环节进行了改进。

3.1. 基于四叉树ORB算法的特征点提取

Vins-Mono算法中特征点提取采用了ORB算法，ORB算法将FAST关键点BRIEF描述子结合。FAST关键点是比较像素间的差异得出来的点 [10] ，对于图中某一像素点p，其亮度为 $I_p$ ，将p与以p为圆心某一半径内的所有像素的亮度进行对比，像素的点亮度与点p的亮度的差超过阈值且区域内满足条件的点的数量超过N，则点p被认定为特征点，一般情况下，圆半径为3，N为12。接着使用BRIEF对这些特征点进行描述，BRIEF是二进制编码描述子，BRIEF描述向量由0和1组成，0和1表示的是相邻两个像素点的灰度值的大小关系。

不经处理提取出的特征点在整个图像中会出现“扎堆”现象，特征点冗余度过高加大了计算量，降低了系统定位精度，因此本文引入四叉树算法 [11] 。四叉树算法将图像帧划分为2 × 2的网格，在每个网格中，如果检测到的特征点数量过多，则可以根据设置的阈值判断是否需要继续分割网格，通过这种方式，可以减少区域内特征点的密集程度，最后将图像拼接从而均匀分布特征点。但是如果特征点数量过多，会导致四叉树的分割次数过多，降低了检索效率，因此本文将分割最大次数限制为4次，不仅提高了特征点提取的速度，还通过这种方式提高了特征点质量，四叉树ORB算法流程如图2所示。

Figure 2. Quadtree ORB algorithm structure

图2. 四叉树ORB算法流程图

3.2. 基于RANSAC算法的特征点匹配

Vins-Mono算法中特征点通过设置汉明距离的阈值并将计算结果与之比较来进行匹配，设连续两帧图像特征点集合为 $\left\{m_1,m_2,\cdots \right\}$ 、 $\left\{n_1,n_2,\cdots \right\}$ ，将特征点二进制描述子不同位的数量作为汉明距离，汉明距离 $D\left(k_1,k_2\right)$ 反应了特征点间的相似关系，该算法的评价指标单一且计算量大，匹配的准确率低，汉明距离的具体表达式为：

$D\left(k_1,k_2\right)={\displaystyle \underset{i=1}{\overset{n}{\sum }}{x}_i\oplus }{y}_i$ (1)

本文采用结合汉明距离的RANSAC算法。RANSAC [12] 算法通过随机选择部分数据子集来估计模型参数，并使用估计的模型对数据进行测试。RANSAC算法主要过程为：首先，从特征点集中随机选取一定数量的点，然后使用最小二乘法拟合模型并得到模型的参数，再计算他特征点与拟合模型的拟合度，如果小于阈值则为内点。不断重复上过程，当迭代达到一定次数后，或者最佳模型出现时，选取保存内点数最多的那一组数据作为最终结果。其中迭代次数s应满足：

$s=\frac{\ln \left(1-p\right)}{\ln \left(1-{\alpha }^n\right)}$ (2)

其中， $\alpha$ 表示外点的比例， $p$ 表示置信度。为了抑制因误差而导致迭代次数过大，收敛过程耗时较长的情况。将两倍的汉明距离作为约束条件，提前对特征点进行质量排序，每次迭代只在高质量特征点中进行模型估计，大大提高了匹配的准确性和速度。

3.3. 基于LM算法的非线性优化

Vins-Mono算法在后端优化中，非线性优化算法采用高斯牛顿算法。紧耦合将视觉的残差项和IMU预积分残差项组合作为目标函数：

$T\arg et=\arg \min \left(E_{proj}+E_{INS}\right)$ (3)

视觉重投影误差指的是相机观测得到的投影位置与按估计位姿变换得到的投影之间的像素误差，IMU预积分误差主要由零偏引起的测量值与观测值的误差，将其目标函数简写为 $f\left(x\right)$ ，高斯牛顿法将其进行一节泰勒展开：

$f\left(x+\Delta x\right)\approx f\left(x\right)+J\left(x\right)\Delta x$ (4)

其中， $J\left(x\right)$ 是 $f\left(x\right)$ 关于x的导数，即雅克比矩阵，为了求 $\Delta x$ 使得 $\Vert f{\left(x+\Delta x\right)}^2\Vert$ 达到最小，构建一个线性的最小二乘方程：

$\begin{array}{c}\Delta x^{*}=\arg {\min }_{\Delta x}\frac{1}{2}{\Vert f\left(x\right)+J\left(x\right)\Delta x\Vert }^2\\ =\frac{1}{2}{\left(f\left(x\right)+J\left(x\right)\Delta x\right)}^T\left(f\left(x\right)+J\left(x\right)\Delta x\right)\\ =\frac{1}{2}\left({\Vert f\left(x\right)\Vert }^2+2f{\left(x\right)}^{T}J\left(x\right)\Delta x+\Delta x^{T}J{\left(x\right)}^{T}J\left(x\right)\Delta x\right)\end{array}$ (5)

将上式对 $\Delta x$ 求导，令导数为零，则：

$\begin{array}{l}2J{\left(x\right)}^{T}f\left(x\right)+2J{\left(x\right)}^{T}J\left(x\right)\Delta x=0\\ J{\left(x\right)}^{T}J\left(x\right)\Delta x=-J{\left(x\right)}^{T}f\left(x\right)\end{array}$ (6)

把左边系数定义为H，右边定义为g，则上式为：

$H\Delta x=g$ (7)

高斯牛顿法用J矩阵的转置乘以J矩阵作为牛顿法中二阶H矩阵的近似，从而省略了计算H矩阵的过程。而实际计算中，J矩阵的转置乘以J矩阵并非是正定的，会出现奇异值，即使H非奇异值，如果求得的 $\Delta x$ 非常大，也会导致局部近似不够准确，不能保证收敛，还有可能让目标函数更大，导致系统的定位精度降低，因此本文采用LM算法取代高斯牛顿法。

LM算法 [13] 引入了信赖半径，在给定初始值 $x_0$ 和信赖半径u后的每一次迭代时，求解最小二乘方程：

${\min }_{\Delta x_k}\frac{1}{2}{\Vert f\left(x_k\right)+J\left(x_k\right)\Delta x_k\Vert }^2,s.t.{\Vert D\Delta x_k\Vert }^2\le u$ (8)

上式为带不等式约束条件的优化问题，采用拉格朗日法将其转化为一个无条件约束的问题：

${\min }_{\Delta x_k}\frac{1}{2}{\Vert f\left(x_k\right)+J\left(x_k\right)\Delta x_k\Vert }^2+\frac{1}{2}\lambda {\Vert D\Delta x_k\Vert }^2$ (9)

上式中对左半边和右半边分别对 $\Delta x$ 求梯度，则有：

$\left(H+\lambda D^{T}D\right)\Delta x=g$ (10)

相比于高斯牛顿算法，LM算法多了一个阻尼系数 $\lambda$ ， $D^{T}D$ 可简写为单位矩阵I，当 $\lambda >0$ 时， $\left(H+\lambda I\right)$

正定，保证了梯度下降的方向；当 $\lambda$ 很大时， $\Delta x\approx \frac{1}{\lambda }g$ 也是最速下降方向，当 $\lambda$ 较小时，LM算法接近

于高斯牛顿算法。利用增益p来选取阻尼系数，p的表达式为：

$p=\frac{f\left(x+\Delta x\right)-f\left(x\right)}{J\left(x\right)\Delta x}$ (11)

当p变大时，则说明实际目标函数变化超过了预期变化，此时应当减小 $\lambda$ ，反之则提高 $\lambda$ 。信赖半径u的变化与p的变化呈正相关，由此控制更新步长，加快目标函数的收敛速度，使得系统的定位精度更高。

4. 实验与分析

本文的实验运行环境为Ubuntu18.04，CPU为Intel (R) Core (TM) i5-11400H。用于实验验证的数据集为EuRoc，EuRoc数据集通过无人机在特定室内场景采集完成，包含了多种场景下的轨迹信息和图像，能有效验证算法的可靠性。

4.1. 特征点提取对比

为验证本文图像特征点提取算法的有效性，选取EuRoc数据集中一组连续图像，图像1和图像2是同一场景下的连续图像。将运用本文算法提取的特征点结果与运用传统ORB算法的结果进行对比。

从图3、图4可以看出，每种算法在连续帧中提取的特征点数量都一致，两种算法特征点提取的准确性得到验证。本文算法由于引入四叉树分割法，提高了特征点提取的质量，在图像1和图像2中提取到的特征点数量为134，而ORB算法在两张图像中提取到的特征点数量为500。本文算法的提取速度分别为0.48 s、0.52 s，ORB算法的提取速度为0.47 s、0.53 s，由于四叉树最大分裂次数为4，所以本文算法的提取速度没有收到影响，二者相差不大。

图3(b)、图4(b)可以看出，图像里红框中提取出的特征点出现了“扎堆”现象，这会影响后续对相机位姿的估计，加大了计算量容易造成系统定位精度的降低，本文采用分布均匀度U [14] 作为特征点分布均匀性的客观评价指标，计算方法如式(12)所示。

$U=101\log \left(v\right)$ (12)

其中，v为区域统计分布向量的方差，区域统计分布向量指各区域特征点数量值形成的向量。在特征点提取准确性得到保证的前提下，U值越小说明提取特征点分布性越好，从图3、图4可以看出，本文算法计算出的分布均匀度为141、144，ORB算法计算出的分布均匀度为311、313，证明本文算法在提取特征点的分布均匀度上提高了54%。

(a) 本文算法 (b) ORB算法

Figure 3. Image 1 results comparison diagram

图3. 图像1特征点提取结果对比图

(a) 本文算法 (b) ORB算法

Figure 4. Image 2 results comparison diagram

图4. 图像2特征点提取结果对比图

4.2. 特征点匹配对比

为了直观感受本文特征点匹配算法的有效性，将4.1中图像1和图像2采用ORB算法提取后分别用本文特征点匹配算法和汉明距离匹配算法进行对比，两种算法的匹配结果如表1所示。

Table 1. The two algorithms matched the results

表1. 两种算法匹配结果

表1可以看出，本文采用的RANSAC算法在结合汉明距离后，加快了匹配时间，用两倍汉明距离作为阈值，减少了冗余匹配且RANSAC算法在不断迭代过程中，提高了匹配的准确率，本文算法匹配的准确率为85.8%，比只采用汉明距离的匹配算法提高了28.4%。两种算法匹配的结果如图5所示。

(a) 本文算法 (b) 汉明距离算法

Figure 5. Comparison of the matching results of two algorithms

图5. 两种算法特征点匹配结果对比图

4.3. 定位精度对比

本文采用LM算法取代了原Vins-Mono中的高斯牛顿算法，对目标函数进行非线性优化，并结合4.1和4.2的改进，在EuRoc中不同场景下的数据集与原Vins-Mono算法进行对比，场景选为实验室(MH)和工厂(VH)且分别选取两种场景内不同难度的数据集进行验证，两种算法在不同场景下的不同难度上数据集的轨迹对比如图6、图7所示。

(a) MH_01_easy (b) MH_03_medium

Figure 6. Comparison of two difficulty trajectories in the laboratory environment

图6. 实验室环境下两种难度轨迹对比图

(a) VH_01_easy (b) VH_02_medium

Figure 7. Comparison of two difficulty trajectories in a factory environment

图7. 工厂环境下两种难度轨迹对比图

从图6和图7看出，本文改进后的Vins-Mono算法在不同数据集上的轨迹更加接近真实值，图中红框中可以看出，在部分轨迹段传统Vins-Mono算法出现了明显的轨迹漂移，而本文改进后算法的定位精度更高

为了进一步量化本文算法的定位精度，选择均方根误差(RMSE) [15] 作为定位精度的评价标准，对不同场景下各轨迹精度进行量化对比，具体参数如表2所示。

Table 2. Comparison of positioning accuracy of different data sets

表2. 不同数据集定位精度对比

从表2可以看出，本文算法在不同难度数据集上的定位精度分别提高了34%、42%、48%、12%，平均定位精度提高了34%，验证了本文改进算法在INS/视觉组合定位中的有效性。

5. 结论

本文针对现有Vins-Mono算法在INS/视觉组合定位中，存在图像特征点提取不均匀、匹配准确率低、定位精度不高的问题，提出了一种改进的Vins-Mono算法。在EuRoc公开数据集上进行验证，采用基于四叉树法的ORB特征点提取算法，使得特征点分布均匀化，将RANSAC算法结合汉明距离进行特征点匹配，提高了匹配的准确性，将Vins-Mono算法后端优化中原高斯牛顿法替换为LM算法，提高了定位精度。实验结果表明，改进后的Vins-Mono算法在图像特征点提取均匀度上提高了54%、特征点匹配准确率上提高了28.4%、不同难度数据集上的平均定位精度提高了34%，验证了本文改进算法在INS/视觉组合定位中的有效性。

基金项目

常州市5G+工业互联网融合应用重点实验室(CM20223015)。

NOTES

^*通讯作者。