基于单目/IMU/里程计融合的SLAM算法

张福斌， 张炳烁， 杨玉帅

(1.西北工业大学 航海学院, 陕西 西安 710129; 2.天津航海仪器研究所, 天津 300130)

0 引言

轮式机器人在室内反恐防爆、探测、搜救等领域发挥着重要作用，而实现高精度的导航定位是轮式机器人执行各项任务的重要基础。近年来，基于视觉和惯性测量元件(IMU)融合的同时定位与地图构建(SLAM)[1]技术受到了广泛关注。IMU和视觉传感器之间具有良好的互补性，视觉SLAM定位精度高且漂移量小，但受工作环境影响较大，如在快速运动情况下估计效果较差；IMU稳定性高且能够对短时间的快速运动提供良好估计，但存在明显的累积误差[2-3]。因此，视觉与IMU组合的视觉惯性里程计(VIO)，可以很好地解决系统的稳定性和精度问题[4]。

相比于双目相机和深度相机，单目相机具有操作简单、计算方便、使用范围广等优点[5]，更能满足轮式机器人导航定位的实时性和多场景适用性要求。香港科技大学提出的单目视觉- 惯性导航系统(VINS-Mono)[6-7]是目前性能较为优秀的VIO系统，具有良好的鲁棒性与较高的精度。该系统实现了单目视觉和惯性测量信息的紧耦合处理，并具有完整的回环检测和重定位功能。但在轮式机器人上应用时，由于载体主要进行平面运动，且经常处于匀速或匀加速运动状态，系统会产生额外的不可观测度，从而导致定位精度下降[8-9]。

为进一步提高单目视觉- 惯性SLAM系统的定位精度，国内外研究人员进行了大量的研究工作，部分学者对前端光流跟踪部分进行了改进[10]。其中，Hwangbo等[11-12]将陀螺仪信息用于视觉点特征跟踪，有效提高了金字塔LK光流法对相机快速旋转运动的鲁棒性。Zhang等[13]基于VINS-Mono框架，提出了一种基于点- 线特征跟踪的单目VIO算法，提高了在地下车库等场景中的定位精度。本文通过引入陀螺仪和里程计导航信息，进一步优化了光流追踪求解效率。

多传感器融合能够充分实现各传感器间优势互补，提高位姿估计的精度，同样受到了广泛关注[14-16]。部分研究人员提出基于EKF的单目视觉与轮式里程计融合位姿估计模型[17-18]，可以方便地融合多种传感器信息，适用于轮式机器人应用场景。但基于松耦合的算法难以充分利用各传感器信息，精度有待进一步提高。Quan等[19]提出了一种基于概率的紧耦合单目/IMU/轮式里程计SLAM算法，利用陀螺仪的角速度和里程计的线速度信息计算预积分，加入视觉和平面约束因子进行非线性优化，同样提高了定位精度。但该算法抛弃了加速度量测，因此只能适用于水平面的运动。

结合轮式机器人特点，本文改进了单目视觉- 惯性SLAM系统，提出一种单目/IMU/里程计紧耦合SLAM算法。在视觉前端部分，对原始图像金字塔光流法进行改进。通过MEMS IMU/里程计导航算法，将陀螺仪数据得到的帧间角增量信息和里程计数据得到的位移增量信息作为先验，改进光流初值选择，并优化像素偏导数的计算方法，提高特征匹配的速度和准确率。针对预积分环节，推导IMU/里程计预积分公式和误差传递函数，将轮式机器人的里程计测量信息加入单目视觉/惯性SLAM系统。初始化阶段中，利用预积分结果改进对速度、重力矢量、尺度因子的初始化过程。在后端非线性优化部分，推导了单目/IMU/里程计紧耦合SLAM系统的目标优化函数和优化状态变量，构建了新的模型。最后，进行公开数据集仿真测试并设计实验，验证本文所提算法的有效性。

1 单目/IMU/里程计紧耦合SLAM算法设计

1.1 算法框架

本文针对轮式机器人导航定位问题，基于VINS-Mono算法框架，引入车轮里程计信息，提出了单目/IMU/里程计紧耦合的SLAM算法。算法的工作流程如图1所示。

图1 单目/IMU/里程计紧耦合算法框架

该系统中，对于新输入的IMU、里程计测量值，计算IMU/里程计预积分得到帧间姿态、速度、位置增量；对于新获得的图像帧，进行Shi-Tomasi角点特征提取，并用改进金字塔LK光流法进行追踪。如果系统还未初始化，则执行初始化操作，完成视觉、IMU、里程计对齐。内容包括恢复尺度，估计陀螺仪零偏和重力向量，确定世界坐标系，并且得到姿态、速度及位置的初值。最后进行基于滑动窗口和边缘化的非线性优化，对滑窗中的各个变量进行实时优化。同时，进行回环检测工作，如果检测到正确回环，则执行重定位和四自由度位姿图优化，保证全局轨迹一致性。

1.2 基于陀螺仪和里程计改进的图像金字塔光流法

VINS-Mono系统前端采用图像金字塔LK光流法追踪帧间运动，该方法精度较高但效率有待提高。本文SLAM系统在视觉前端部分将陀螺仪和里程计的旋转和平移增量作为先验信息，对图像金字塔光流法迭代过程进行改进，来提高光流追踪速度。

1.2.1 低精度陀螺仪- 里程计导航算法

轮式机器人中通常使用的惯性器件为陀螺零偏及重复性精度低于0.1°/s量级的MEMS IMU，此时进行导航解算无需考虑与地球相关参数。因此，本节将推导忽略地球参数的简洁惯性导航算法。

基于四元数的姿态更新算法算法如下

(1)

式中：

(2)

(3)

(4)

当载体速度不超过100 m/s时，可以不考虑地球自转带来的速度影响，此时位置更新微分方程将简化为

(5)

离散化后，得到

(6)

以上旋转和平移增量可用于改进本文SLAM系统前端所用图像金字塔光流法的光流初值选择。

1.2.2 改进的图像金字塔光流法

由于特有的基于图像金字塔分层的迭代操作，基于图像金字塔的LK光流法相比于传统的LK光流法在追踪精度和鲁棒性上性能有所提升，但追踪速度却略有不足。为尽量弥补追踪速度上的缺陷及进一步提升追踪精度，在原始的图像金字塔光流法基础上引入陀螺仪和里程计数据分别作为旋转和平移先验信息。

传统的图像金字塔LK光流法中，首先找到第I帧图像中像素点u=(ux,uy)和第J帧图像中的匹配像素点v=(vx,vy)，然后对第I帧图像和第J帧图像建立m层图像金字塔：{IL,JL},L=0,…,Lm。随后设定图像金字塔光流初值：

(7)

在本文的SLAM系统中，由于存在陀螺仪和里程计，且帧间间隔较短，累积漂移小，可以获得两帧图像间旋转和平移的先验值。设图像I与图像J中的匹配像素点对u和v的归一化相机坐标分别为p1和p2，则根据对极几何约束关系，有

p2=Rp1+t

(8)

式中：R表示陀螺仪的旋转先验信息；t表示里程计的平移先验信息，根据(2)式、(6)式以及相机-IMU外部参数求得。由于相机内参矩阵K已知，根据p2可以求得像素点v的先验像素坐标，从而优化基于图像金字塔的LK光流法的光流初值选择。并且在初值有先验值的情况下，可以适当减少图像金字塔层数，从而达到减小耗时且提高匹配精度的目的。

在计算图像光流的过程中，首先找到第I帧图像中像素点u=(ux,uy)和第J帧图像中的匹配像素点v=(vx,vy)，然后对第I帧图像和第J帧图像建立m层图像金字塔。则第J帧图像中的匹配像素点v的预测值的计算过程为

(9)

改进后图像金字塔的光流初值为

(10)

传统的图像金字塔LK光流法中，图像IL对像素点u在x轴方向和Y轴方向的偏导数Ix和Iy的计算分别采用了该像素点左右和上下像素点的差分，而这种近似在实际的计算中效果并不如意。因此，在本文改进的光流法中，采用(11)式所示的两个3×3的卷积核：

(11)

(12)

通过以上改进，可以有效提高SLAM系统前端光流追踪的速度和精度。

1.3 IMU/里程计预积分

由于IMU通常具有比摄像机更高的量测速率，为避免在优化过程中重复积分，节省计算资源，视觉惯性SLAM系统中通常采用IMU预积分[20]方式来处理高速率IMU测量值。本文在IMU预积分理论的基础上，引入里程计数据，推导IMU/里程计预积分的基本原理与公式。

(13)

(14)

在此基础上，推导IMU/里程计预积分在连续形式下的误差传递方程，t时刻误差项的导数为

(15)

式中：na、nω和nv分别为加速度计、陀螺仪和里程计的加性噪声；nba和nbω为加速度计和陀螺仪偏置的导数，均为白噪声。根据以上公式进一步得到误差协方差传递矩阵，用于后端非线性优化。

1.4 系统初始化

视觉/惯性SLAM系统初始化包括纯视觉SFM和视觉/惯性对准，能够为系统提供初始化估计。经过视觉/惯性对准，可以实现对陀螺仪偏差的估计，并恢复速度、重力矢量和尺度因子信息。VINS-Mono系统在车辆匀速运动或机动较小的情况下，容易出现初始化误差过大的问题，通过加入轮式里程计信息，可以有效改善初始化过程稳定性，提高SLAM系统在匀速运动场景下的适用性。

首先，通过视觉获得的帧间旋转值和IMU预积分的旋转值，计算得到陀螺仪误差。该过程通常只需要几秒钟时间，在此期间陀螺仪零偏变化极小，可认为是一个常值。陀螺仪零偏估计的目标函数为

(16)

随后，根据(14)式中IMU/里程计预积分结果，对速度、重力矢量、尺度因子和相关外参进行估计：

(17)

(18)

由于重力矢量的模值已知，在空间中只具有两个自由度。可以将重力矢量重新进行分解，代入(17)式中进行计算，求得精细化分解的重力矢量，并与世界坐标系w坐标系的z轴对齐。据此可以将初始化的速度、位置及姿态等状态量均旋转至w坐标系中，完成系统初始化。

1.5 非线性优化

(19)

单目/IMU/里程计紧耦合SLAM系统非线性优化的目标函数为：

(20)

(21)

IMU和里程计测量残差部分所影响的优化变量为bk帧和bk+1帧对应的位置、速度、姿态、加速度计零偏、陀螺仪零偏和IMU-里程计对应的外参，这些优化量可表示为

(22)

采用扰动的方式对以上变量计算雅可比矩阵，并利用Ceres库进行求解，实现对滑窗中各个变量的实时非线性优化，得出位姿估计的最优结果。如果检测到回环，则执行重定位和四自由度位姿图优化，对位姿进行进一步优化，减小累积误差影响。

2 实验结果与分析

2.1 基于陀螺仪和里程计改进的图像金字塔光流法实验验证

为验证本文提出的基于陀螺仪和里程计改进的图像金字塔光流法的可靠性，以Euroc开源数据集作为实验数据，进行仿真实验。Euroc数据集中包含图像数据和陀螺仪数据，根据数据集中标准轨迹的帧间位姿增量，加入方差(0.013 m/s)2的高斯白噪声，得到模拟的里程计数据。然后从数据集中选出11帧连续关键帧对应的图片，用改进的图像金字塔光流法和原始的图像金字塔光流法对这些图片进行追踪，分别记录下这两种算法在追踪过程中的角点数量、迭代次数和像素匹配误差。表1为两算法在剔除误匹配后追踪到的角点数量对比，表2为两算法在计算光流过程中的迭代次数对比，表3为两算法最终的像素匹配误差对比。

表1 角点数量对比

表2 迭代次数对比

表3 匹配误差对比

从表1、表2和表3中可以看出，在两种算法追踪匹配角点数量相当的情况下，基于陀螺仪和里程计改进的图像金字塔光流法的迭代次数均值要明显少于原始的图像金字塔光流法，减少了32.5%，证明了改进光流法的有效性。而像素匹配误差均值只是略小于原始的图像光流法，这是因为改进的图像金字塔光流法中先验信息起到了补偿作用，使得算法的光流匹配速度有所提升，匹配误差有轻微下降。

2.2 单目/IMU/里程计紧耦合SLAM算法实验验证

为验证本文提出的验证本文所提的单目/IMU/里程计紧耦合的SLAM算法有效性，搭建实验平台，进行室内和室外实验。

采用改装的Autolabor Pro轮式遥控车作为实验平台，搭载的相机为OV2710单目相机，相机图像采集频率为20 Hz，图像分辨率为640×480。同时，在车上安装MEMS IMU传感器，型号为法国SBG公司的Ellipse2-A，采样频率100 Hz。轮式编码器为增量型欧姆龙E6B2-CWZ3E，采样频率为100 Hz。主控为一台mini工控机，处理器为Intel i7 4560U 4核，运行内存为8 GB，运行系统为Ubuntu16.04。室外实验使用差分GPS设备来提供基准轨迹，其定位精度可达厘米级。

室内开环实验在实验室走廊内进行，遥控小车沿地板上预定的轨迹行驶，从起点运动到终点。运动轨迹包括2段直行和1个转弯的过程，大致为“L”形状，总长度大概为25 m。本文算法实时运行后在RVIZ工具下得到的轨迹图2所示。

图2 室内实验运行轨迹图

该实验中，将本文单目/IMU/里程计紧耦合SLAM算法和VINS-Mono算法进行对比，通过判断实验轨迹和预定轨迹的终点误差来评判各定位算法的性能优劣。

从表4和表5中可以观察到，在室内进行的10次开环实验中，本文算法(单目/IMU/里程计紧耦合SLAM算法)的终点误差为0.168～0.290 m，占总里程的0.67%～1.16%。VINS-Mono算法的终点误差为0.212～0.308 m，占总里程的0.85%～1.23%。对比得出，本文算法相比VINS-Mono算法的终点定位精度略有提高。室内开环实验在一定程度上验证了本文算法的有效性，但实时基准轨迹的缺失使得室内实验不能全面地体现出算法的性能，在此基础上，进一步应用差分GPS设备和Autolabor Pro小车进行了室外场景实验。

表4 室内开环实验终点间距离误差

表5 室内开环实验距离偏差占总里程的百分比

室外环境相比于室内更为复杂，存在各种动态或未知的干扰。为进一步验证本文单目/IMU/里程计紧耦合算法的有效性，进行了室外场景下的开环和闭环实验验证。控制Autolabor Pro轮式小车在校园内道路上运动，将速度控制在1 m/s左右。开环实验轨迹总长度为100 m，包含两个90°转弯。闭环实验运动轨迹包括4段直行和4个转弯的过程，大致为一个闭合的矩形形状，总长度大概为600 m。路线分别如图3和图4中差分GPS轨迹所示。

图3 室外开环实验轨迹对比

图4 室外闭环实验轨迹对比

将实验轨迹(包括本文的单目/IMU/里程计紧耦合算法和VINS-Mono算法)与差分GPS提供的轨迹通过Evo工具进行对齐，然后将实验轨迹与差分GPS轨迹进行对比得到误差均值、误差最大值和均方根误差(RMSE)，并对其误差进行分析。

实验结果如图3、图4和图5所示，图3和图4为实验轨迹(本文的单目/IMU/里程计紧耦合算法和VINS-Mono算法)与差分GPS轨迹的对比图，图5为闭环实验中，实验运行轨迹与差分GPS轨迹之间距离误差的对比图。详细的定位误差对比如表6所示。

图5 室外闭环实验距离误差对比

表6 室外实验定位误差对比

对比以上实验结果可以得出，由于本文SLAM算法加入了里程计信息作为紧耦合约束，在室外长距离运动下，定位误差均值相比VINS-Mono算法减少近40%，导航定位精度相比VINS-Mono有明显提高。

3 结论

本文针对室内轮式机器人高精度导航定位的需求，在VINS-Mono算法框架的基础上，引入里程计数据，提出了一种基于改进光流法的单目/IMU/里程计紧耦合SLAM算法。首先，在前端图像金字塔LK光流法基础上，加入陀螺仪和里程计信息对算法进行改进。通过将陀螺仪和里程计得到的帧间位置和姿态增量作为先验信息，改进图像金字塔光流法的光流初值，并且减少了一层图像分层，从而减少光流追踪计算量，减小泰勒展开的误差以提高匹配精度。在Euroc数据集下进行了仿真测试，验证了改进光流法对于加快光流匹配速度的有效性。推导了IMU/里程计预积分公式，并对初始化、非线性优化等过程进行改进，加入了里程计信息约束，进一步提高位姿估计精度。进行小车实验，验证了本文SLAM算法的有效性，该算法定位精度相比VINS-Mono具有明显提高。