视觉惯性融合定位的室内SLAM研究

刘大鹏

（浙江农林大学光机电工程学院，浙江 杭州 311300）

0 引言

随着人工智能技术飞跃式的发展，室内移动机器人开始广泛进入人们的日常生活中，智能家居、工业辅助生产、物流便捷式仓储与服务型机器人等热点内容引起人们的关注与研究。而VI-SLAM（Visual And Inertial Simultaneous Localization And Mapping，视觉惯性即时定位与地图构建）[1-4]中定位技术在智能移动机器人执行任务过程中发挥着举重若轻的作用，使得移动机器人在未知场景中，通过识别障碍物的相对位置和方向，防止本体与物体接触碰撞；通过感知且估计本体所在空间位姿信息，保证移动机器人构建地图与场景环境信息全局的一致性。因此，研究VI-SLAM定位技术有助于增强移动机器人在室内环境中的空间信息感知与定位的功能，并为移动机器人自主导航提供先验信息。

目前，VI-SLAM根据视觉传感器与IMU数据融合方式分为松耦合和紧耦合。其中，松耦合[5]是指相机和IMU分别进行各自的位姿估计，然后对它们各自的结果共同构建运动方程和观测方程进行位姿估计。Lynen等[6]提出MSF-EKF框架，建立按时间顺序的视觉测量位姿状态缓存区，当IMU状态数据获取时插入到相应的时间戳的状态缓存区，更新视觉惯性融合后的状态，以提高系统定位的精确性与一致性。

与松耦合相比，视觉惯性系统采用IMU的状态数据与相机的观测数据联合后进行位姿估计优化的紧耦合[7]关联方式，系统更加健壮，且定位更精准。Leutenegger等[8]提出采用紧耦合的方式将视觉与IMU测量数据关联，构建基于关键帧的全概率的视觉与惯性误差项，并以关键帧的相对不确定性来替代全局不确定性，最后通过滑动窗口的形式对其误差项进行位姿优化。

1 双目视觉惯性系统框架

本文构建的VI-SLAM框架系统如图1所示：在系统前端的环境探索中，采用双目相机以特征法提取场景特征，并以改进光流法匹配跟踪，IMU使用流形预积分感知移动机器人运动位姿；在初始化阶段，移动机器人提供视觉惯性初始运动参数；采用基于关键帧的紧耦合方式对视觉惯性数据进行定位融合，并在后端采取滑动窗口对运动位姿进行非线性优化得到移动机器人的轨迹姿态。

图1 视觉惯性系统框架图

2 双目视觉里程计

在前端特征检测器选择的过程中，为了平衡系统的实时性与定位精度。对此，提出双目特征点法和改进光流结合的视觉前端里程计用于对场景特征的检测与跟踪。Fast角点是根据图片像素点亮度差异提取环境局部特征，且在图像特征点提取速度和数量上表现优异，故选择其作为特征提取器[9]。为提升系统实时性和提高系统适应动态场景的鲁棒性，在特征点匹配跟踪时采取改进光流算法。

在图像特征点提取与正向跟踪匹配后，特征点跟踪由于噪声存在误匹配，错误匹配将影响下一帧中目标位置的判断。针对特征存在误匹配，对于图像帧采用逆向光流[10]跟踪，如图2所示，通过当前帧追踪上一帧的角点，筛除图像像素距离过大的外点，提高特征点匹配质量。

图2 正向和逆向光流跟踪图

稀疏LK光流法在实际场景下，相机获取的图像特征点相对速度过快，导致像素运动偏移量过大，造成跟踪失败，因此，选取图像金字塔来追踪特征点偏移量d。

如图3所示，图像金字塔指对同一图像进行尺寸缩放得到不同分辨率下的图像，光流跟踪时自顶层到原始图像由粗到细依次优化得到最佳光流，在最优解处，此时前后相邻帧图像对应特征点相似度最高。为进一步提升结果的精确性，金字塔光流跟踪时在金字塔的每一层采取多次迭代来求得精确的最优解，最终获得领域内匹配误差最小的光流。

图3 金字塔光流法示意图

相机位姿的运动估计，以左目图像为基准，在经过左右立体特征提取、光流跟踪和前后帧间特征正逆向光流追踪后，采用三角化获得特征点深度信息，进而解算出上一帧与当前帧的特征点的三维坐标。并且已知相应的图像特征点二维坐标，采用PNP（Perspective-n-Point，透视N点投影）构建BA最小二乘法优化相机帧间位姿（R，T）。

3 视觉惯性数据初始化

在移动机器人视觉惯性数据融合位姿估计之前，需要对视觉传感器与IMU数据进行对齐校准，为状态变量的精准求解与系统快速收敛提供必要的初始化参数。且良好的初始化在系统丢失时能够快速重启进入工作状态。

1）建立IMU预计分离散模型，根据视觉与惯性旋转量一致性建立最小二乘法方程迭代优化陀螺仪偏置bg；

2）由位姿的平移转换关系线性推算系统尺度S和重力g的近似值；

3）IMU坐标系与重力旋转构建线性方程，获取加速度计偏置ba，并对尺度和重力进一步计算精确数值；

4）利用IMU速度预计分模型求得速度v参数。

4 滑动窗口紧耦合的非线性优化

随着移动机器人在各种场景的不断探索，视觉惯性系统获取的环境路标、特征点的观测量和状态估计量的规模剧增，系统的实时性与鲁棒性受到约束，系统在衡量精度与计算效率后，采取基于关键帧的滑动窗口来控制系统状态量的参数优化。

滑动窗口：在窗口内固定关键帧数量N=10，在加入一个新的关键帧以及观测到的路标点时，对一个旧的关键帧进行“剔除”。基于滑动窗口状态量紧耦合的非线性优化中，通过边缘化把旧关键帧的状态量从当前固定窗口的状态估计中移除，但移除的状态量作为先验信息保留在当前滑动窗口中，避免窗口中其他帧和路标点信息的丢失。

为了达到视觉惯性系统的定位准确性和全局一致性，在滑动窗口中构造边缘化残差、IMU残差和视觉误差项来非线性优化移动机器人的状态变量，如下所示：

边缘化残差为滑动窗口移除旧关键帧的位姿与特征时，将与其关联的约束关系转化为先验（prior）参与系统优化；IMU残差为相邻帧间预积分观测值与估计值的残差；视觉误差是场景特征点在相机下的重投影误差，即特征点在当前帧中光流跟踪的归一化坐标与特征点在当前帧的重投影归一化坐标的残差[11-14]。

5 实验测试与结果

对构建的视觉惯性SLAM算法（简称为Ours）采用公共EuRoc数据集进行测试评价，并与当前开源的OKVIS、VINS-Fusion对比，获取实验测试现象与结果，如表1所示。EuRoc数据集共五个序列（采用简写表示），MH_01至MH_05数据集背景为大型机器厂房。数据集含有丰富的信息，并设置纹理、光照和速度组合的对比场景。

1）定位精度的检验：由表1可知，在公开的EuRoc的五个数据集中，Ours与VINS-Fusion、OKVIS实验测试中，性能最好的算法，并以加粗字体突出显示。在MH_01良好的纹理、场景明亮和低速运动的状况下，系统保持着优异的位姿估计精准性。与MH_01相比MH_03在快速运动时，移动机器人系统稳健性良好。在MH_04与MH_05中，移动机器人快速运动中，降低环境光线强度，数据表明VINS-Fusion定位性能最优，Ours其次，OKVIS表现最差。然而，Ours在五个场景数据的均方根误差的平均值最小，可知Ours在各种变化场景中保持良好的定位准确性。

表1 双目视觉惯性SLAM算法的均方根误差

2）累计误差的检验：移动机器人对场景的空间信息进行感知，在噪声的干扰和位姿的错误恢复下，系统的轨迹定位在长时间的运行中产生累计误差，对三种算法进行轨迹定量剖析。

随着运动距离的增加，移动机器人视觉惯性的误差逐渐累积，采用折线图来比较绝对位姿中的平移偏量。如图4所示，通过MH_01、MH_03和MH_05三种不同条件场景下，观察RMSE的中位数，对比三种算法在长时间运行下系统累积平移误差的大小，并观测三种算法箱体波动程度的走势图。结果表明，Ours算法随着距离的增加系统累计误差在箱体上下波动幅值最小，VINS-Fusion中等，OKVIS波动最大，体现出Ours算法系统的稳定性。

图4 OKVIS、VINS-Fusion和Ours绝对位姿的平移误差对比图

3）系统的稳健性测试：在教学楼实际环境下，手持与装载Ubuntu18.04电脑连接的Realsense T265，行走路线从一楼至四楼，中途经走廊和带有落地窗的楼梯平台，测试期间在光照不足和弱纹理的走廊快速移动，上楼时设置相机抖动，转弯时快速旋转变换前进方向。

构建的视觉惯性SLAM系统数据解耦后生成的轨迹路线如图5所示，结果表明，算法能够在弱纹理、光线不足、快速旋转和移动的挑战场景中实现连贯的位姿追踪，验证了系统良好的稳健性。

图5 教学楼轨迹路线图

6 结束语

通过IMU数据与视觉图像帧信息融合，在兼顾系统精度的前提下，通过构建改进LK光流法跟踪Fast角点的轻量型双目视觉里程计，并与IMU流形预计分数据紧耦合实现数据关联，经基于关键帧的滑动窗口内非线性优化移动机器人位姿，实现移动机器人在室内定位的稳健性与鲁棒性。