面向动态环境的视觉惯性SLAM算法

崔林飞，黄丹丹，王祎旻，耿振野，刘 智

(长春理工大学 电子信息工程学院，吉林 长春 130000)

0 引 言

动态环境中的自我运动估计是移动设备的基本问题，SLAM技术就是有效解决这个问题的常用方法[1]。视觉传感器因成本低廉和使用方便等优点，视觉SLAM得到了快速发展[2]，目前开源视觉SLAM具有代表性的有ORB-SLAM[3]、DSO[4]等。为了弥补单目视觉SLAM在快速移动下容易失效的缺点，添加了惯性测量单元(IMU)，以增强单目视觉SLAM系统的定位精度和鲁棒性。目前流行的开源视觉惯性SLAM解决方案包括：VINS-MONO[5]、VINS-Fusion[6]等。然而，绝大多数SLAM方法以及数据集都是基于静态环境下进行研究，而如何消除动态环境对SLAM算法性能影响的相关研究较少。同时视觉SLAM主要是根据两帧图像之间匹配的特征点估计出自身的旋转和平移(即相机的位姿)。在动态环境中由于存在大量移动对象，会造成提取的特征中包含部分动态特征点，使用这些动态特征点进行特征匹配，会降低系统位姿估计的准确性。

针对SLAM系统受到动态环境影响的问题，提出一种基于对极几何的特征检测算法，由于动态环境对视觉和惯性信息影响程度不同，设计了一种自适应调整因子用于后端调整视觉信息权重。改进后的视觉惯性SLAM系统可以提高相机的位姿估计精度以及系统鲁棒性。

1 相关研究

大多数基于视觉惯性SLAM的移动设备，通常应用在动态环境，经常会遇到移动的对象，如行人、汽车等。因此，研究SLAM系统如何在动态环境下稳定、精确地定位导航，对智能机器人以及增强现实、虚拟现实的普遍应用具有重要意义。

根据SLAM系统在动态环境中受到干扰的分析可知，去除提取特征中的动态特征，仅保留静态特征可以提高SLAM系统在高动态环境中位姿估计的准确性。现有针对动态环境的SLAM解决方案，主要可以分为：基于几何信息的动态特征检测；基于语义信息的动态目标检测；将几何信息和语义信息相结合的动态目标检测。

几何信息处理方案主要有Bescos等[7]提出的Dyna-SLAM通过结合多视图几何和深度学习的方法来检测动态目标。魏彤和李绪等[8]根据立体视觉几何约束判别场景中的稀疏特征点，然后根据场景深度和颜色进行场景分割。高成强等[9]通过建立图像块实时更新高斯模型，依据方差变化分割出动态对象，通过最小化重投影误差对相机位姿进行优化。张慧娟等[10]采取了直线特征的静态权重剔除动态直线特征的方法。

基于语义信息处理的方案主要有房立金等[11]通过深度学习进行语义信息分割，结合特征点光流对语义分割进一步优化，检测动态目标并去除不可靠的动态特征。Zhang等[12]使用光流残差来突出RGB-D点云中的动态特征。Brasch等[13]提出一种基于CNN(convolution neural networks)预测的语义先验信息结合概率模型以及时间运动信息判断动态特征。利用语义信息来判断动态目标主要是根据常识和经验对目标进行判断，但是可能会将处于静止的人、汽车等先验动态目标的全部特征去除，而运动的椅子、桌子等先验为静态的目标无法判断为移动物体。

几何信息与语义信息相融合的方案包括Cui和Ma等[14]提出的将语义分割和对极几何信息以紧耦合的方式融合在一起有效检测并去除动态特征。Yu和Liu等[15]通过将语义分割和运动一致性检测相结合检测动态特征。Bescos等结合多视图几何和MaskR-CNN[16]检测动态目标。虽然将几何信息和语义信息相结合会提升SLAM系统对动态目标的检测，但由于语义信息的获取需要算力较高的硬件支持，这不适合移动设备轻便的设计要求。

综上，现有的动态SLAM算法大多结合语义信息进行动态目标检测，但要实现精确的动态目标检测需要构建高性能的目标检测网络，但这需要大量的算力，难以应用在嵌入式移动平台上；同时这些解决方案大多只采用单一的视觉传感器，已知单一的视觉传感器容易受到环境的干扰。综合考虑后，本文提出了一种适用于动态环境的DVI-SLAM系统，它通过在VINS-MONO前端特征提取阶段添加动态特征检测器，同时根据当前帧动态点的数量，自适应调整后端优化中视觉信息的权重，从而提高视觉惯性SLAM系统在动态环境中的位姿的准确性以及系统的鲁棒性。

2 DVI-SLAM概述

在真实环境中，精确的位姿估计以及系统的鲁棒性是评价智能机器人的一个重要标准。VINS-MONO在目前公开的视觉惯性数据集和真实环境中均有良好表现，因此本文将VINS-MONO作为DVI-SLAM的基本框架，在此基础上添加了动态目标检测并改进了后端优化方式，以更好适应动态环境的挑战。

DVI-SLAM系统的动态特征检测器总览如图1所示，对先前图像帧和当前帧的特征点进行KLT(Kanade-Lucas-Tomasi)稀疏光流[17]跟踪，然后，通过对极几何约束检测并去除动态特征点，只保留静态特征点用于位姿估计。

图1 DVI-SLAM动态检测器总览

本文提出的DVI-SLAM系统的整体结构如图2所示。其中测量预处理模块对新采集的图像帧提取亚像素级特征点，然后，通过动态特征检测器将特征点进行分类，去除动态特征，仅保留真正的静态特征，采用KLT稀疏光流法对静态特征点进行跟踪；其中IMU数据的处理与VINS-MONO相同。在后端非线性优化部分，通过添加自适应视觉信息权重因子，来降低动态环境对系统的影响。初始化模块、重定位模块以及全局位姿图优化模块与原始VINS-MONO相同。位姿图优化模块接受几何验证的重定位结果，并进行全局优化以消除漂移。回环检测模块是为了判断是否到达过以前经过的位置，消除轨迹漂移。

图2 DVI-SLAM系统的整体结构

3 动态点检测与后端优化

3.1 动态特征点检测

本文采用对极几何约束来判断当前关键帧中的特征是否为动态。图3为静态特征点在相邻图像帧的对极几何约束，其中X是静态三维特征点，x1是空间点X在I1帧上的投影，x2是空间点X在I2帧上的投影。C1和C2是相机的光学中心，连接C1和C2的线称为基线，与两个关键帧相交的点称为极点e1、e2。C1和C2以及空间点X组成的平面称为对极平面，对极平面与帧I1、I2相交的直线L1，L2称为极线。

假设在静态环境中，已知像素点的像素坐标p1，三维空间点X位于从x1的反投影线C1x上，通过对极几何约束得到X在关键帧I2中的投影点x2的像素坐标范围，如图3所示。若三维空间点X为静态特征点，在已知相机的旋转和平移后，可以通过对极几何约束判断x2的像素坐标。两幅图像的对极几何约束可用相邻两图像间的基础矩阵来表示

(1)

式中：p1和p2是对应投影x1和x2的像素坐标。I2帧中的极线L2可以通过帧I1中相对应的p1和基础矩阵F计算得到

L2=Fp1

(2)

图3 对极几何约束

如果三维点X是静态的，根据对极几何约束，x2投影点一定会在极线L2上。如果三维点X是动态点，X可能会在对极平面之外运动如图4(a)所示，会导致投影点x′2和极线L2之间有一定的距离d；若是在对极平面内运动如图4(b)所示，则像素点x′2依然会落在极线L2上。由此可以看出，只有特征点在对极平面之外运动才不满足对极几何约束。

图4 空间三维点的运动可能性

在实际操作中由于特征点的提取和基础矩阵F的计算均存在误差，即使三维点X是静态的，投影点x′2也不一定位于对应的极线L2上，而是距离相应极线有一定距离(即使距离非常小)，如图4(a)所示。因此可以通过这个约束来判断特征点是动态点还是静态点。通过判断投影点x2到对应极线L2的距离d是否大于预定义的阈值，从而判断该特征点是否为动态特征点。在具体的实现过程中，本文选择1个像素作为阈值，即当前帧中的投影点距离相应极线的距离超过1个像素则认为此特征点为动态特征。

假设有

P1=(x1,y1,1)P2=(x2,y2,1)

(3)

式中：P1和P2是投影点的归一化坐标。极线方程可以通过以下公式计算

(4)

式中：X,Y,Z是极线方程的系数向量，F代表基础矩阵。投影点到对应的极线的距离方程如下

(5)

通过式(5)可以看出，基础矩阵F的选取对于距离d的精度非常重要。在相邻两帧之间通常可以通过5点法[18]、8点法[19]以及RANSAC(random sample consensus)[20]计算基础矩阵。最常用的是8点法推导基础矩阵F。假设

(6)

根据式(1)和式(3)可以得出

Af=0

(7)

式中：A=(x1x2,x1y2,x1,y1x2,y1y2,y1,x2,y2,1),f=(f11,f12,f13,f21,f22,f23,f31,f32,f33)T， 此时的A矩阵只是由一对匹配点组成的矩阵，想要计算出基础矩阵F则只需找到两帧之间8对匹配特征点。8点法是计算基础矩阵F最简单的方法，为了增强其计算的精度和稳定性，一般先对输入的匹配点进行归一化处理，具体过程在本文不作讨论。

为了得到两帧间正确匹配的特征点，采用可以跟踪特征点运动方向的光流法，在一定程度上也可以对动态物体进行判断，但考虑到一般光流法计算量较大，因此采取了KLT稀疏光流法对特征点进行跟踪。采用RANSAC方法简单去除跟踪过程中误匹配的特征点，以得到更加精确的基础矩阵。因此系统需要在一个相对静态的环境下完成初始化，并通过RANSAC去除误匹配的点，得到一个相对精确的初始基础矩阵。

特征点的提取、动态特征点检测以及动态特征分离的算法如算法1所示。

算法1： 动态特征点检测算法

输入： 先前关键帧I1； 当前关键帧I2；

输出： 动态特征点集合D；

(1)先前帧的特征点P1=goodFeaturesToTrack(I1);

(2)亚像素级特征点P1=cornerSubPix(I1,P1);

(3)当前帧特征点P2=CalcOpticakFlowPyrLK(I1,I2,P1);

(4)去除P2中的外点；

(5)F=FindFundamentalMatrix(P1,P2);

(6)forP1,P2中每对匹配的点p1,p2do

L1=ComputeCorrespondEpilines(p1,F);

d=getDist_P2L(p2,L1);

ifd> 1 then

Appendp2to D;

end if

end for

算法1中用到的表达式均来源于OPENCV库中的相关函数。

图5表示原始VINS-MONO在3个动态序列中提取的特征点的结果。图6显示了本文提出的DVI-SLAM系统去除动态特征后的效果。可以看到与原始VINS-MONO相比DVI-SLAM提取的特征几乎没有来自于移动的行人身上。

图5 VINS-MONO在动态环境中提取的特征点

图6 DVI-SLAM系统在3个动态序列中的特征提取

3.2 后端非线性优化

当SLAM系统在动态环境中运行时，打破了传统SLAM静态环境的基本假设，影响了SLAM系统位姿估计的准确性。经过分析发现，移动目标对视觉惯性系统的影响主要是发生在视觉前端，即在图像帧提取关键特征的过程中使用动态特征点进行特征点匹配。惯性传感器是由陀螺仪和加速度计构成，因此测量数据并不会受到移动对象的影响，因此可以调整视觉信息的权重来提高系统在动态环境中的鲁棒性。

通过设置一个比率M，即当前帧中去除动态特征后剩余静态特征点的数量与当前帧中特征点的总数的比值，作为调整视觉信息权重的参考条件

(8)

式中：当前帧的特征点的数量，本文采用VINS-MONO中采用的150个特征点。

在滑动窗口内n个IMU帧以及m个特征点的状态向量为

(9)

通过最小二乘非线性优化方法，最小化滑动窗口内得到的边缘化先验信息、视觉残差以及惯性残差的平方和。目标函数如下

(10)

(11)

其中自适应调整因子α被定义为

(12)

θ为确定是否舍弃当前帧的视觉测量信息的阈值。阈值θ在实验过程中根据环境中动态对象的数量进行自适应调整。若当前关键帧中的静态特征点占的比重M小于阈值θ时，则舍弃当前的视觉信息，只保留惯性信息进行位姿估计，以此来保证SLAM系统在动态环境中的鲁棒性。

4 实验评估

在这一部分，本文在现有的公共视觉惯性数据集TUM-VI[21]上对提出的DVI-SLAM系统进行相关实验。TUM-VI数据集中包含动态的运动场景，这有利于对提出的DVI-SLAM系统在动态环境中的性能进行评估。为了实验的公平比较，将提出的DVI-SLAM系统与原始VINS-MONO系统在多个动态场景序列中进行了比较，以验证其在动态环境中的改进效果。算法的实验平台为：Intel i5 5200U处理器(2.2 GHz)，8 GB RAM，64位Ububtu16.04操作系统。

4.1 在TUM-VI数据集的评估

TUM-VI数据集提供了许多序列，这些序列提供了20 Hz帧率，分辨率为1024×1024的高动态范围光度较准图像。此外还提供了频率为200 Hz的IMU惯性单元，测量3个轴上的加速度和角速度。相机和IMU在硬件中进行时间同步。同时在数据序列的开始和结束处，给出使用运动捕捉系统以120 Hz的高频率获取的精确标准位姿数据，并与相机和IMU测量值对准作为参考标准的地面真实值。为了验证提出的DVI-SLAM系统在动态环境中的性能，本文选择具有挑战性并且包含动态元素的序列来进行相关实验，即序列Slides1、Slides2、Slides3，这3个序列是在学校的中央大厅采集的，光照明暗变化较大，并且有突然进入的行人，这样的环境会对SLAM系统构成巨大的挑战。在TUM-VI数据集的这3个动态序列中，人是主要的动态对象。其中Slides1序列动态对象相对较少称为低动态环境，其余两个序列动态对象较多称为高动态环境。

在选定的3个动态序列上分别运行VINS-MONO和本文提出的DVI-SLAM系统。并把这两个系统运行估计的运动轨迹与地面真实值绘制在一个图中。为了更清晰直观展示对比效果，将三维轨迹投影到二维平面中，以定性和直观的方式展示两个系统的定位精度。两个系统估计的运动轨迹对比结果如图7所示。通过图7可以看出，在低动态的场景下VINS-MONO和DVI-SLAM都可以回到出发原点；而在高动态的场景下，VINS-MONO的轨迹会发生偏移而无法正常回到出发原点；DVI-SLAM则偏移较小。

图7 在3个动态序列中两个系统的测试结果对比

由于TUM-VI数据集中的Slides1、Slides2和Slides3这3个动态序列，只给出了开始和结束的地面真实轨迹，位置如图7中的虚线框处，两个系统的轨迹与真实轨迹对比如图8所示，其中粗虚线表示真实轨迹，点虚线表示

图8 3个序列开始和结束阶段地面真实轨迹的对比

VINS-MONO的轨迹，实线表示DVI-SLAM的轨迹。若轨迹与给出的地面真实轨迹重合度越高，表示SLAM系统的位姿估计更准确。通过对图8分析可知，在低动态序列Slides1中两个系统的轨迹都非常接近地面真实值。而在高动态的Slides2和Slides3序列中，本文提出的DVI-SLAM系统与地面真实值更接近；而VINS-MONO估计的轨迹与地面真实值之间的差距较大，轨迹的偏移较大。这是因为 VINS-MONO仅仅通过使用RANSAC方法粗略的将误匹配特征点去除，并没有针对性的考虑移动对象对SLAM系统的影响。相反，新提出的DVI-SLAM系统针对性的对动态特征进行检测与去除，并改进了后端非线性优化，提高了系统的定位精度，减小了轨迹的偏移。

为了对提出的DVI-SLAM系统做进一步的定性比较，以验证在动态环境中的性能。本文使用绝对轨迹误差ATE(absolute trajectory error)的RMSE(root mean square error)作为本文实验的定性评估指标。绝对轨迹误差直接计算系统所估计的相机位姿于数据集提供的真实轨迹之间的差。首先，通过时间戳将估计位置与真实位置进行对齐，然后，分别计算每对位姿的差值。绝对轨迹误差的定义如下

(13)

式中：Qi为真实的位姿，Pi为估计位姿，并通过变换矩阵T将两者对齐。获得每帧的绝对轨迹误差之后，求其均方根误差

(14)

对于每种方法，都会在相应序列运行5次，以获得RMSE的中位数、均值、最小和最大值，以降低偶然因素对实验定性测试的影响。结果见表1。为了更直观展示DVI-SLAM较VINS-MONO在相应序列的改进情况，在表2中展现了提升的百分比。

根据表1和表2，可以看到在低动态序列Slides1中，这两种方法的结果非常接近，VINS-MONO的性能要稍好于DVI-SLAM。而在高动态的两个序列中，DVI-SLAM系统与原始的VINS-MONO相比大大提高了系统的准确性，测试的所有参数，中位数、均值、最小和最大RMSE都有很大幅度得提升。这说明了本文改进后的视觉惯性SLAM算法，在动态环境中展现了更好的性能。因此验证了提出的DVI-SLAM系统在高动态的环境中实现了较高的定位精度。

4.2 与主流视觉惯性SLAM的对比

为了更全面验证本文提出系统在动态环境中的性能，将本文的方法与已经发表论文中的实验结果进行比较。进行对比的系统均在TUM-VI动态序列中测试过的视觉惯性SLAM：OKVIS、ROVIO[22]，这两个系统没有对动态物体进行针对处理。选择DVI-SLAM在测试实验中RMSE的中位数作为对比值。比较结果见表3。

通过表3，可以看出本文提出的系统在动态环境中的轨迹的误差远优于OKVIS、ROVIO。可以验证提出的DVI-SLAM系统在动态环境中的性能要优于上述两种视觉惯性SLAM系统。

5 结束语

本文提出一个适用于动态场景的视觉惯性SLAM——DVI-SLAM。通过在视觉前端添加动态检测模块，用于检测动态特征，同时统计每帧静态特征点所占的比重，用于后端非线性优化过程中自适应调整视觉信息的权重。在高动态的环境中，由于添加了惯性传感器，当相机提取的有效特征点不足的情况下，也可以通过IMU的测量信息在短时间内进行有效定位，这提高了系统在动态环境中的鲁棒性。为了验证提出系统的有效性，在公开的TUM-VI数据集中进行了相关实验，并与最新的视觉惯性SLMA做了对比分析。实验结果表明，在高动态序列中，DVI-SLAM相对于VINS-MONO的定位精度提升了平均47.34%。

表1 RMSE实验数据对比

表2 较VINS-MONO提升的百分比

表3 与最新视觉惯性SLAM系统的RMSE数据对比