基于自适应惯导辅助特征匹配的视觉SLAM 算法

贾晓雪，赵冬青，张乐添，肖国锐，徐 青

（信息工程大学 地理空间信息研究所，河南 郑州 450001）

1 引言

视觉同步定位与建图技术（Simultaneous Localization and Mapping，SLAM）同时包含定位与建图两个问题，提高SLAM 系统的定位精度是构建精确稠密三维点云地图的前提。视觉里程计作为SLAM 的关键部件，其主要任务是根据相邻图像的匹配信息估计相机运动，其中对图像进行特征匹配是最关键环节之一，匹配的准确度直接影响视觉里程计的精度［1-3］。常用的特征匹配算法有暴力匹配算法和快速最近邻匹配算法［4-5］。然而，当相机运动过快时，图像模糊、场景动态、光线变化和图像特征本身的局部特性，使特征误匹配广泛存在，并成为制约视觉里程计性能提升的一大瓶颈［6-7］。

为解决图像特征误匹配率高的问题，国内外很多学者提出基于惯性测量单元（Inertial Measurement Unit，IMU）辅助的视觉SLAM 系统［8-9］。文献［10］提出了一种仿射尺度不变特征变换（Affine Scale-invariant Feature Transform，ASIFT）特征匹配方法，利用IMU 提供的运动信息虚构相机视图并估计本质矩阵，以提高特征匹配的稳健性。文献［11］利用图像的对极约束建立了前后两帧图像像素所在极线的数学关系，并结合IMU 预测相机位姿获取特征点所在极线，有效地剔除误匹配的特征点。文献［12-14］提出了一种全局搜索和局部搜索交替操作并自动切换的特征匹配算法，利用相机投影模型和IMU预积分结果预测特征点可能出现的区域，并对局部固定区域进行搜索匹配。同样地，文献［15］提出的视觉惯性SLAM 系统ORB-SLAM3 在特征匹配阶段也引入了惯导约束，利用IMU 预测相机位姿，将双目相机左右匹配或三角化得到的地图点投影至图像，预测像素位置。

然而IMU 测量值本身存在误差，在惯导辅助图像特征匹配时，通常将全图像的特征匹配转换为以预测位置为原点，某一固定值为半径的局部固定区域特征匹配。为了适配不同精度IMU，普遍做法是设置一个较大的搜索半径值，以避免漏掉特征点，这在IMU 精度较高时易造成计算资源浪费，增加图像特征的误匹配率，使SLAM系统的位姿估计精度降低。为此，本文提出了一种自适应的惯导辅助视觉特征匹配方法，以惯性测量单元误差为基础，根据误差传播定律自动调整图像特征匹配搜索半径，不仅能提升特征点匹配的准确度，还能提高双目视觉惯性SLAM 的定位精度，并构建更高精度的稠密地图。

2 惯导辅助图像特征匹配

为提高视觉惯性里程中图像特征匹配的准确度，ORB-SLAM3 利用IMU 预积分预测相机位姿，将图像预处理获取的地图点三维坐标投影至当前帧，预测地图点在当前帧图像的像素位置，从而将全图像特征匹配转换为局部区域的特征匹配，具体流程如图1 所示。

图1 惯导辅助图像特征匹配流程Fig.1 Flow chart of IMU aided image feature matching

2.1 双目图像预处理

双目相机一般由左眼和右眼两个水平放置的相机组成，经过校正的左右相机只在x轴上有位移，所以地图点在左右相机的像素坐标也只在x轴（对应图像的u轴）上有差异。通过计算双目图像对应像素点的视差，可以估计出像素的深度。记地图点P的左侧像素坐标为pl，右侧像素坐标为pr，几何关系如图2 所示。根据相似三角形原理可得地图点在相机系下的深度值z为：

图2 双目相机的成像模型Fig.2 Imaging model of stereo camera

其中：f为相机的焦距，b为双目相机的基线。

将特征点在相机坐标系下的深度值z代入针孔相机投影模型，可得地图点在相机系下的坐标Pc为：

其中：(u，v)是地图点P在左目相机图像平面坐标系下的像素坐标，cx和cy是相机内参。

2.2 IMU 预积分

惯性测量单元是由三轴陀螺仪和三轴加速度计组成的，陀螺仪和加速度计分别以一定的频率采集载体系(b)相对于惯性系(i)的三轴角速度ω和加速度a。为解决视觉惯性里程计中采样频率不一致和重复积分问题，文献［16］首次提出了欧式空间中的预积分方法；随后，文献［17］在前者的基础上提出了IMU 在流形面的预积分方法，得到了与初始状态无关的相对运动增量，从而显著减少了重复计算。

IMU 采样频率远高于相机，在两个连续图像 帧i，j之间有多帧IMU 数 据，通过式（3）对IMU 量测值预积分可得到两帧图像的相对运动增量。

2.3 预测像素坐标

将当前图像帧j时刻载体在世界系w的位姿转换为世界系w相对j时刻载体系bj的位姿为：

利用视觉惯性里程计标定的相机系c与载体系b变换矩阵，预测当前图像帧j时刻相机相对世界系w的位姿为

其中：Rcb和tcb分别为相机系c与载体系b的旋转矩阵和平移向量，Rcw和tcw分别为利用IMU 预积分预测出的世界系w相对于j时刻相机系c的旋转矩阵和平移向量。

当相机模型为针孔相机模型时，利用双目相机模型获取的地图点在当前帧图像上对应像素坐标为：

其中：fx，fy，cx，cy为相机的内参数；Pw为地图点P在世界系下的三维坐标；Pc=[Xc，Yc，Zc]T为 地图点在当前帧相机系的坐标；(u，v)为地图点对应像素坐标。

式（7）表明前一帧图像像素点对应地图点在当前帧图像的位置。相机的内参数可以通过相机标定获取，因此只需通过IMU 量测值预测相机位姿便可确定地图点在当前帧图像的像素位置。但IMU 预积分预测相机位姿存在一定误差，因此在图像特征匹配时没有直接匹配预测特征点，而是在以预测位置为原点，某一阈值为半径的局部区域内搜索特征点并匹配，以防出现无法匹配的情况。为适配不同精度的惯性测量单元，目前普遍的做法是设置一个较大的固定值作为搜索半径，以避免漏掉特征点。但这种惯例在IMU 精度较高时易造成计算资源浪费，同时增加了图像特征的误匹配率，使SLAM 系统的位姿估计精度降低。

3 自适应惯导辅助特征匹配

针对惯导辅助图像特征匹配方法中固定搜索半径过大的问题，本文提出了一种搜索半径自适应算法。根据误差传播定律和惯导辅助图像特征匹配原理计算预测像素位置的中误差，确定地图点最有可能出现的区域半径，将局部固定区域特征匹配转化为局部自适应区域的特征匹配，确定自适应特征匹配搜索半径的流程如图3所示。

图3 确定自适应特征匹配搜索半径流程Fig.3 Flow chart of determining adaptive feature matching search radius

3.1 预积分误差分析

由于预积分量与IMU 噪声存在复杂的依赖关系，难以直接利用误差传播定律递推出预积分的信息矩阵，因此可将IMU 噪声从预积分量中分离出来，得到预积分测量噪声与IMU 噪声关系为：

三个预积分状态量误差表达式较为复杂，直接计算会给计算平台带来资源的浪费，经推导与简化可得预积分量误差的递推公式为：

3.2 自适应阈值的确定

由IMU 预积分误差和误差传播定律可得图像间相对位姿的协方差，利用惯性导航辅助特征匹配原理可将其传递至每个地图点对应的像素，获取预测像素位置的中误差。由于特征匹配过程中的函数与位姿有关，因此误差传递时从李代数的角度对函数进行线性化。

将IMU 预积分相对位姿误差传递至当前图像帧时刻载体相对世界系的位姿误差过程中，需要对式（4）线性化，得到其关于相对姿态和位置的导数为：

则当前图像帧时刻载体相对世界系位姿的协方差为：

同理，依据式（5）和（6）由当前图像帧时刻载体系相对世界系位姿协方差可得相机相对世界系位姿的协方差为：

利用相机的针孔投影模型将预测相机位姿协方差传递到每个地图点对应像素，得到像素的协方差为：

自适应特征匹配搜索半径与像素协方差的对应关系为：

其中：σ0为单位权中误差；σu和σv分别为像素坐标p=(u，v)T的中误差；m为特征匹配的搜索半径，取像素坐标的极限误差。

4 实验结果与分析

为验证所提方法的可行性和有效性，分别使用EuRoC 和TUM 两个公共数据集中的不同场景序列进行实验分析［18-19］。其中EuRoC 数据集由搭载MT9V034 摄像机和ADIS16448 惯性测量单元的微型飞行器采集，同时使用激光跟踪仪或Vicon 动作捕捉系统实时捕捉飞行时位姿，作为飞行器的真实轨迹；TUM 数据集由搭载uEye 相机和BMI160 惯性测量单元的移动小车采集，真实轨迹由OptiTrack 动作捕捉系统提供。

本文选择两个公开数据集中MH01、V101、V201、Corridor1、Magistrale1、Room1 和Slides1这7 个代表性序列，每个序列都给出了距离、加速度计和陀螺仪的噪声，如表1 所示。

表1 实验所选序列Tab.1 Sequences chosen for the experiment

4.1 自适应特征匹配结果分析

首先，利用本文的自适应算法在不同图像序列上进行测试，计算特征匹配搜索半径，得到各序列自适应特征匹配搜索半径累积分布函数，如图4 所示。图中F(x)表示搜索半径小于等于x的比例。

图4 各序列自适应特征匹配搜索半径的累积分布函数Fig.4 Cumulative distribution function of search radius for adaptive feature matching of sequences

从图中可以看出，各图像序列依据本文算法获得的特征匹配搜索半径绝大部分小于ORBSLAM3 中的固定值，这是因为ORB-SLAM3 设定了一个较大的搜索半径以适配不同规格的设备，避免IMU 精度过低时漏掉特征点，导致系统跟踪失败。而且不同图像序列对应的特征匹配搜索半径并不完全与IMU 精度相关，这是由于不同图像序列使用的相机模型不同，EuRoC 采用的是针孔相机，而TUM 采用的是鱼眼相机；另外，每个地图点所处位置也不同，进而说明特征匹配搜索半径应该设定为自适应阈值，综合考虑相机模型、惯性测量单元噪声和每个地图点位置等因素自适应调节，以提高特征匹配的准确度。

然后利用改进前后的特征匹配算法分别计算各图像序列特征匹配所需时间，得到平均每个特征点匹配所用时间。如图5 所示，改进后算法在EuRoC 数据集对应的三个序列中特征匹配时间有所降低，而在TUM 数据集对应的序列中有所增加，这是由于自适应特征匹配算法在缩小特征匹配搜索半径的同时，增加了函数线性化及误差传递的计算工作量；TUM 数据集采用鱼眼相机，相机投影模型较为复杂，占用了一些计算资源，但增加的时间属于微秒级，在可以接受的范围内。

图5 改进前后每个特征点匹配的平均耗时Fig.5 Average time-consuming of each feature point matching before and after improvement

4.2 局部地图跟踪结果分析

IMU 初始化后，ORB-SLAM3 跟踪线程利用局部地图点跟踪建立图像帧间的多视图几何关系，并通过最小化重投影误差估计载体位姿。本文提出的自适应阈值算法直接作用于局部地图跟踪的特征匹配阶段，因此可通过评估跟踪线程位姿精度间接验证本文特征匹配方法的有效性，同时使用不同数据集中不同实验场景序列验证本文算法的鲁棒性。

以V101 序列为例，改进的跟踪线程位姿估计算法与原算法定位轨迹如图6 所示。选用的数据为该序列在10 次评测中均方根误差最接近均值的两组。可以看出，本文算法输出的位姿精度有所提升，这说明本文提出的自适应算法能够降低图像特征的误匹配率，因为基于特征点法视觉里程计精度主要取决于图像特征点匹配的准确度。

图6 改进前后V101 序列跟踪线程定位轨迹Fig.6 V101 sequence tracking thread location before and after improvement

将改进前后ORB-SLAM3 系统跟踪线程位姿估计结果分别与真实轨迹对比，并计算其绝对位姿误差，对比结果如图7 所示。可以看出，本文算法整体位姿精度都有所提升，其中系统刚开始的16 秒内精度提升尤为明显，这是因为初始时刻IMU 预测位姿精度不高，为避免漏掉特征点，原算法设置了一个较大的特征匹配搜索半径。而本文自适应特征匹配方法通过误差传播定律获取了每个地图点对应的搜索半径，使其搜索区域显著减小，因此初始一段时间特征点匹配的准确率会有明显提升，跟踪线程位姿估计也会更接近真值。

图7 改进前后V101 序列绝对位姿误差Fig.7 Absolute position and pose errors of V101 sequence before and after improvement

表2 是改进前后ORB-SLAM3 帧间跟踪位姿在7 个不同场景序列中的绝对位姿误差对比结果，这里的数据为各序列10 次评测绝对位姿误差均方根的平均值。由表2 可知，在测试的7 个不同场景序列中，改进后算法输出的位姿精度平均提升38.09%，说明自适应算法能够提高特征点匹配的准确度，而且在不同设备和不同场景的序列中精度均有所提升，具有较强的鲁棒性。

表2 各序列跟踪线程绝对位姿误差的均方根Tab.2 Root-mean-square of absolute pose error of each sequence tracking thread

4.3 ORB-SLAM3 系统跟踪结果分析

为验证算法的可用性，本文对比了不同数据集中不同图像序列利用改进前后ORB-SLAM3系统处理输出位姿的绝对位姿误差，如表3 所示，这里的数据为各序列10 次评测绝对位姿误差均方根的平均值。

由表3 可知，在选用的图像序列中，依据本文特征匹配方法改进后的ORB-SLAM3 系统的绝对轨迹误差均优于原算法，改进后系统整体位姿精度平均提高16.38%。证明改进后的ORBSLAM3 系统能够在不同场景序列中良好运行，具有较强的鲁棒性，同时定位精度也有所提升。这是因为ORB-SLAM3 系统后端采用非线性优化法，即使最终输出位姿经历了三次优化，但其仍与给定的初始位姿具有较强的相关性。而这里的初始位姿正是由跟踪线程提供的，因此本文提出的自适应阈值惯性导航辅助视觉SLAM 特征匹配方法直接提升了跟踪线程位姿估计的精度，为SLAM 系统提供了较为准确的初值，进而使SLAM 系统整体定位精度提高。

表3 各序列整体位姿的绝对位姿误差的均方根Tab.3 Root-mean-square of the absolute pose error of the global pose of each sequence

4.4 稠密地图的构建

为验证算法在地图构建中的有效性，本文利用改进后ORB-SLAM3 系统估计的位姿和ELAS（Efficient Large Scale Stereo Matching）算法恢复出的深度数据进行三维点云拼接。以V101 序列为例，构建的稠密地图，如图8 所示。从图中可以看出虽然还有些不规整的地方，但该算法能够完整地表述物体轮廓，几乎没有重影，且周围的离散点云也比较少，墙面、地面近乎在一个平面。这说明本文提出的自适应惯导辅助视觉里程计特征匹配方法不仅能够提升视觉惯性SLAM 系统的定位精度，在稠密建图方面也有较好的效果，能够为三维地形重建提供新思路。

图8 V101 序列的稠密地图Fig.8 Dense map of the V101 sequence

5 结论

为解决ORB-SLAM3 中图像特征匹配失误率高的问题，本文提出了自适应阈值惯导辅助的图像特征匹配算法。以惯性测量单元误差为基础，根据误差传播定律自动调整图像特征匹配搜索半径，从而利用不同精度的惯性测量数据高效辅助视觉SLAM 的特征匹配。实验结果表明，该方法可有效缩小特征匹配的搜索半径，为特征点提供区域约束，显著提高图像特征匹配的准确度，使跟踪线程位姿精度提高约 38.09%，系统整体位姿精度提高约16.38%，并且可以构建更精确的稠密三维点云地图。