视觉与惯性传感器融合的室内导航算法研究

杨志芳，潘忠运

（武汉工程大学 电气信息学院，武汉430200）

目前，室内移动机器人的导航定位算法主要采用的是SLAM（同步定位与建图）算法[1]，SLAM 算法大致可分为两大类，激光SLAM 和视觉SLAM[2]。激光SLAM 是通过采集不同时刻的两片点云信息（一系列分散的、具有准确角度和距离信息的点集）然后将两片点云图进行匹配，计算出激光雷达相对运动的距离和姿态变化，也就是对机器人的定位。激光SLAM 需要搭载成本较高的激光雷达，同时激光雷达获得的环境信息不如视觉传感器丰富。视觉SLAM 成本低廉、获得的环境信息丰富，然而为了提高移动机器人定位的准确度和稳定性，靠纯视觉SLAM 系统来对环境的感知是不够的，因为纯视觉SLAM 系统往往容易受到光照变化的影响，同时在低纹理和运动速度较快导致图像信息模糊的情况下定位精度很低，甚至无法使用。融合IMU[3]很好地克服了纯视觉SLAM 的缺点，在运动速度过快的情况下，图像会出现模糊，两帧图像之间的特征点匹配极少甚至没有，在遇到玻璃、白墙等低纹理的情况下同样特征点匹配很少。在这些情况下，IMU 可以提供数据，继续估算可靠的位姿。在动态环境中，当环境变化时，纯视觉SLAM 可能误以为自身在动，而IMU 可以感知自身运动，从而避免误判。

近几年IMU 与视觉融合的SLAM 研究越来越流行，有一些开源的VIO[4]（视觉惯导里程计）系统。如ROVIO 是基于扩展卡尔曼滤波的紧耦合方法，由Bloesch 等人在2015年提出的一种视觉里程计[5]，具有跟踪3D 路标点图像特征匹配的功能，但仅支持单目相机。OKVIS 也是一种基于扩展卡尔曼滤波的紧耦合方法，它是由苏黎世大学的Stefan Leutenegge 等人在2015年提出的[6]，在前端采用的是特征点法和BRISK 描述子[7]，后端使用滑动窗口模型的视觉惯导SLAM 系统，同时首次在系统中对IMU 进行了预积分处理[8]，解决了IMU 与相机帧率差异问题。支持基于单目和双目相机的滑动窗口位姿估计。VINS 是香港科技大学沈劭劼团队于2017年提出的通用单目视觉惯性估计的鲁棒方法[9]，它的前端使用Harise 角点检测，光流法来跟踪[10]，后端是基于优化和滑动窗口的VIO，该系统主要应用于增强现实领域，也可以在手持式设备或无人机上运行。

1 改进系统框架

本文是在经典的ORB-SLAM2 框架基础上进行改进的，设计了基于非线性优化的IMU 数据与RGB-D 图像数据紧耦合的方式来解决纯视觉SLAM所面对的问题。改进系统框架如图1所示，系统的前端包括图像信息和IMU 传感器信息，将获得的图像信息进行特征提取与匹配，然后进行动态特征点滤除和RANSAC 算法的误匹配剔除，将动态特征点和误匹配点滤除掉。IMU 数据进行预计分处理，使其频率与相机获取图像的频率保持一致，然后系统进行视觉惯导联合初始化，确定载体的初始状态（初始位置、初始速度、加速度和陀螺仪的偏差、重力向量、世界坐标与载体坐标系的变换关系等等），后端包括基于滑动窗口的关键帧筛选和紧耦合非线性优化，最小化重投影误差（相机的重投影误差和IMU 误差）求出载体的位姿信息。输出是载体的位姿和运动轨迹。

图1 本文算法流程Fig.1 Algorithm flow chart of this paper

2 IMU 与RGB-D 融合算法设计

2.1 动态特征点滤除

动态特征点滤除主要是利用图像帧间的几何约束方法来滤除图像中的动态特征点，为后面的RANSAC 算法剔除误匹配提供更好的数据样本。从而减少RANSAC 算法的迭代次数，提高系统运行效率，同时使其在动态场景中有更高的定位精度。

引入几何约束图像模型，如图2所示。F1和F2为相邻两帧图像，p1、p2、p3为F1图像帧的3 个特征点，q1、q2、q3是其在F2图像帧上对应的投影点，3 个特征点在F1上构成三角形Δ p1p2p3，对应的投影点构成三角形Δq1q2q3，其图像中特征点间的距离用三角形三边d 表示。

图2 几何约束模型Fig.2 Geometric constraint model

pi和pj的距离表示为d（pi，pj），qi和qj的距离表示为d（pi，pj），则三角形对应边的差即为

定义几何约束函数：

A（i，j）表示两对应边的平均距离，即：

假设q1为运动目标上的点，由q1运动到q1′的位置，则构成新的三角形Δq1′q2q3，可知计算出的几何约束函数q（i，j）会很大，但是由于几何约束函数涉及的是一对特征点，仍然无法确定qi点是动态点，还是qj点是动态点，因此采用计分的方法来确定最终的特征点，即若q（i，j）大于阈值α，则将特征点qi和qj的得分分别加1，这样不断地将不同的特征点构成三角形进行计算，最终动态特征点会比静止特征点的得分大很多，若某特征点得分大于阈值β，则认为该特征点为动态特征点。

2.2 RANSAC 算法

RANSAC（Random Sample Consensus，随机抽样一致）算法用来估算表示帧间图像的位姿变换关系的基本矩阵H，得到一个最优的基本矩阵H 模型，然后根据该模型剔除掉误匹配的点，使得特征匹配更加的准确，从而提高系统的定位精度。

RANSAC 算法基本流程如图3所示。

图3 RANSAC 算法流程Fig.3 Flow chart of RANSAC algorithm

（1）设样本集为S，其有N 个数据点，内点的比例为w，从样本集S 中随机选取4 个数据点，根据该数据点计算出单应矩阵H。

（2）将数据集中所有的数据按单应矩阵H 进行投影，得到所有数据点的投影误差，将误差小于阈值d 的数据点添加到内点集Si中。

（3）如果内点集Si超过了某个阈值m，就用Si重新估计单应矩阵H 并结束，否则重新选取新的4 个数据点，计算单应矩阵H，然后重复步骤2。如果迭代的次数大于T，选取最大的内点集Si，用它来估计单应矩阵H，得到最后的估计模型，然后退出。

由上述的计算流程可以看出，RANSAC 算法的运行效率取决于迭代次数，进一步取决于样本集的内点比例，因此在进行RANSAC 之前，尽量提高样本集的内点比例，这样可以提高RANSAC 的运行效率。在之前进行的动态特征点滤除，有效地滤除了样本集中的部分外点，在很大程度上降低了RANSAC 的迭代次数，提高了RANSAC 的运行效率。

2.3 IMU 预积分

IMU 传感器是测量移动机器人自身的姿态角和加速度的装置，由三轴陀螺仪和三轴加速度计组成。由于IMU 和相机的采样频率相差较大，在相同的时间内IMU 采集的数据比相机更多，本文使用的IMU 采样频率为200 Hz，相机的采样频率为20 Hz。所以需要采用预积分方法来解决IMU 与相机频率不同的问题。

在两帧图像之间有多帧IMU 数据，假设IMU的第i 时刻数据与图像的第k 帧对齐，IMU 的第j时刻数据与图像的第k+1 帧对齐，然后连续两图像之间的IMU 数据进行预积分处理，如图4所示。

图4 IMU 预积分示意图Fig.4 Schematic diagram of IMU pre-integration

在某一离散时刻t 采集数据，可得到t=i 和t=j两帧之间的位姿与速度关系：

2.4 非线性优化估计位姿

在系统完成初始化之后，本文采用滑动窗口的方法来控制优化的规模，用紧耦合的方式对相机的位姿、速度以及IMU 的偏置进行估计优化。滑动窗口是增加新的变量进入到最小二乘系统优化，维持窗口关键帧的数量，数量达到一定个数就移除旧的关键帧。然后不断地重复这一过程。在传统的视觉SLAM 中，是通过最小化图像帧中特征点的重投影误差来求得相机的位姿以及三维路标点。但增加IMU 传感器之后，增加了要估计的系统状态变量，需要同时考虑重投影误差和IMU 误差。系统的状态向量表示如下：

根据相机的重投影误差和IMU 误差构建目标函数，通过高斯牛顿法求得目标函数最小时的位姿信息，如图5所示。

图5 非线性优化流程Fig.5 Flow chart of nonlinear optimization

目标函数为

式中：V 表示滑动窗口中被观测到两次以上的特征点集；I 表示滑动窗口中IMU 预积分增量方程的集合；Ecam（k，j）为地图点k 在第j 帧图像时的重投影误差，其误差方程表示为

式中：xk为像素平面坐标；π（·）表示三维空间点投影到像素平面的投影变换；WXk为世界坐标系下的三维坐标点；Eimu（i，i+1）为IMU 预积分中连续两帧i和i+1 之间的增量方程误差，其误差方程表示为

3 实验结果与分析

本文使用EuRoC 和TUM 数据集对本算法进行了测试，将其测试的结果与ORB-SLAM2 算法进行了对比和分析。EuRoC 数据集是用微型飞行器在1个工业厂房和2 个不同的房间内采集，分为easy、medium 和difficult 3 个不同的等级。TUM 数据集中包含大量动态场景的序列，采集了办公室中同事聊天互动、手势和起身等场景。本数据的实验平台系统为Ubuntu18.04，CPU 为Intel i5-7200U 处理器，主频为2.7 GHz，内存为8 GB。

本算法的动态特征滤除效果如图6所示，图像为TUM 数据集freiburg3_walking 序列中的一帧，动态目标正走向座椅。由图6可以看出本算法对动态特征点滤除有很好的效果，将人身上的大部分动态点滤除掉了。

图6 动态特征点滤除对比图Fig.6 Comparison diagram of dynamic feature points filtering

本文在评估定位精度时，采用算法跟踪过程中的位姿估计值与数据集提供的真实轨迹的均方根误差（RMSE）来对算法的定位精度来进行评估。分别比较本算法和ORB-SLAM2 算法中的双目模式在各种环境下的均方根误差，所有结果都为3 次实验结果的均值。

如表1所示，本算法和ORB_SLAM2 在EuRoC数据集和TUM 数据集实验轨迹精度比较，可以看出本文提出的惯导与双目视觉融合算法在各个场景中比双目视觉模式的ORB_SLAM2 算法的定位精度更高。

表1 在EuRoC、TUM 数据集上实验的均方根误差对比Tab.1 Comparison of RMS errors of experiments on EuRoC and TUM datasets

图7为本算法和ORB_SLAM2 算法在MH_05_difficult 序列的绝对轨迹误差，从MH_05_difficult 序列可以看出，当相机快速运动导致图像模糊和弱纹理时，ORB_SLAM2 的定位精度有所下降，这是由于在这些序列中ORB_SLAM2 所提取的特征点少，且比配质量差导致的。本算法在ORB_SLAM2 的基础上增加了IMU 信息，在各场景中不仅仅只用视觉信息进行定位，在提取特征点较少时，可以结合IMU的运动信息进行计算，这样在弱纹理，快速运动导致图像模糊等场景时，本算法同样有很好的定位精度。同时本算法还增加了动态特征点滤除，可以较好的滤除动态特征点，提高特征点的匹配质量，freiburg3_sitting（低动态）和freiburg3_walking（高动态）是含有动态物体的序列，从上表可以看出在freiburg3_walking 序列中，ORB_SLAM2 算法的定位精度急剧下降，大量在动态物体上提取出来的特征点没有被有效的剔除，而是参与了匹配和进一步的计算，从而对系统的定位精度产生影响，而本算法的动态特征点滤除模块能有效剔除动态特征点，降低动态特征点对系统的影响，所以在含有动态目标时本算法的定位精度比ORB_SLAM2 更高。综上本算法比ORB_SLAM2 算法对场景的适应性更好。

4 结语

根据纯视觉的SLAM 系统对环境的适应性不强，本文在ORB-SLAM2 的基础上进行了改进，增加了IMU 传感器的数据，采用了基于非线性优化的紧耦合方式，这种融合方式系统的运行效率会更高。同时在特征匹配时增加了动态特征点滤除算法，先将动态特征点滤除后，再进行RANSAC 算法误匹配剔除，使得特征匹配的更加准确，提高了系统的定位精度，同时使系统能够更好的适应含有动态目标的场景。EuRoC 数据集和TUM 数据集实验表明，本文改进的算法在弱纹理、快速运动、图像模糊和含有动态目标影响的场景中定位精度上比ORBSLAM2 表现更好，在快速运动导致图像模糊时也有很高的定位精度。因为融合了IMU 传感器的数据，使其算法更为复杂，整个系统的运行效率上略有降低。由于系统在特征点提取上消耗的时间较多，后续可以采用FPGA 对图像的特征点提取进行硬件加速，这样可以提升系统的运行速度，同时采用FPGA可以大幅降低功耗。