基于语义先验和几何约束的动态场景SLAM算法

张皓诚，王晓华，王文杰

（西安工程大学 电子信息学院，陕西 西安 710048）

引言

将特征点的实时单目同步定位与地图构建(oriented fast and rotated BRIEF-simultaneous localization and mapping，ORB-SLAM)算法应用于真实的动态场景时，存在例如行人、车辆、动物等运动物体，也存在如行人携带的书包等移动性物体，该类运动物体和移动性物体的特征点会导致SLAM前端的帧间无法正常匹配，易出现特征跟踪丢失、运行轨迹漂移等问题[1-3]，影响系统的定位精度以及鲁棒性，使得构建的环境地图的精度大大降低。同时，在高动态场景中，物体的快速运动会导致图像区域模糊，低质量的特征点在相邻帧间不稳定。因此，如何提取高质量图像特征点以及如何区分动、静态点和剔除动态特征点，成为高精度SLAM 要解决的问题。

方琪[4]等针对快速鲁棒特征算法的描述符运算时间较长、匹配准确率低的问题，提出一种基于网格运动统计的改进快速鲁棒特征图像提取与匹配算法。在动、静态特征点区分方面，Tan 等[5]对关键帧和当前帧进行比较，将特征变化大的关键帧替换成新选择的关键帧，消除动态物体对SLAM系统的影响，但并未对动点处理，计算出来的相机位姿和实际位姿还是有较大偏差。Klappstein 等人[6]通过计算特征点是否违背光流法灰度不变假设为依据，判断是否为动态特征点，但通过图像分割理论只能粗略分割物体的轮廓，实际效果不佳。林付春等人[7]通过德洛内三角对动态和静态特征点进行分割，并利用几何约束计算出明显的外点，但由于受到环境中物体的纹理、亮度等因素影响，并不能完整地分割出运动物体的整体轮廓。Emanuele[8]等人将RGB-D 相机采集到动静态特征点信息映射到三维空间，依据显式建模来检测动态物体，使用颜色信息编码对相机进行位姿估计，并构建全局地图，但当运动特征点较多时，系统无法区分运动和静止的特征点，位姿估计会有极大误差。

语义分割方法[9]能够提取出动态物体的轮廓，可作为机器人识别环境中动态特征点的先验信息。Zhang[10]等人以ORB-SLAM2算法为基础，使用YOLO[11]（you only look once）算法对检测到的关键帧进行语义分割，同时剔除位于动态物体区域的特征点，以适应动态环境，但在检测目标时，由于YOLO算法给出的边界框含有大量的背景信息，会对算法的正常运行产生干扰。Yu 等人[12]提出DS-SLAM（dynamic environments-SLAM），该算法以ORB-SLAM 为框架，依据光流法对原始RGB图像进行运动一致性检测的同时，采用SegNet（segmentation network）[13]网络对该原始RGB 图像进行语义分割，然后删除异常值并估计位姿，在独立线程中构建语义八叉树地图。SegNet网络对物体的边缘轮廓分割较差，对于人体等存在局部运动现象。Berta 等人于2018年提出Dyna-SLAM（dynamic-SLAM）[14]算法，该算法结合实例分割网络Mask-RCNN（mask-region convolutional neural networks）[15]以及多视图几何对系统运行场景中的动态特征点进行筛选，对图像静态部分提取ORB（oriented fast and rotated BRIEF）特征，进行场景恢复和建图。将动态物体去掉之后检测到的特征点会处于较远或纹理较弱的区域，会导致跟踪精度下降，同时，Dyna-SLAM 时间开销比较大。不断有学者提出新的语义分割网络，力图兼顾分割的精度和运行速度，以便在实际应用中达到理想的效果。姜昊辰等人[16]使用轻量级的Light-weight RefineNet[17]语义分割网络作为独立线程，和ORBSLAM2[18]框架相结合，采用语义先验的加权极线和深度约束的运动一致性检测与动态特征点剔除的方法，虽然减少了位姿估计的误差，但语义分割对于动态物体边缘的感知效果较差，会对构建静态地图造成重影等影响。精度高且实时性能较好的语义分割网络以及高质量的静态特征点提取，为相机位姿估计和建图打下良好基础，也是动态SLAM 技术要解决的首要问题。

本文以BiseNet（bilateral segmentation network）[19]实时轻量级双边语义分割网络为基础，设计了基于分支空洞卷积的实时轻量级双边语义分割网络，对环境中的潜在运动区域和静态区域进行分割，作为语义先验信息。同时对ORB算法中图像金子塔各层网络进行网格划分，使得特征均匀化分布，解决环境中特征点在部分区域过于集中、部分区域过于分散而导致的图像局部特征信息丢失和动态物体剔除后纹理较弱区域缺乏特征点而跟踪精度下降问题；应用几何约束运动一致性检测算法，采用对潜在运动区域像素块间相似度判断的策略，对动态特征点剔除，进而应用静态特征点估计系统的位姿，更新环境地图。

1 动态环境下基于语义先验的SLAM系统

1.1 ORB 图像的特征点均匀化

物体的快速运动导致图像像素点间灰度差较小，图像特征的质量不高。本文将图像的每层金字塔图像划分为K个区域，在每个区域内进行ORB特征点提取和检测，对于检测不到特征点的区域，调整阈值，使得ORB 图像特征在各个区域可以均匀分布，改变由动态物体去掉之后特征点处于较远或纹理较弱的区域而导致的跟踪精度下降的现象。对于机器人实时采集的环境图像，采用ORB算法提取特征点，图像金字塔各层的缩放尺度表达式为

式中：n为 图像金子塔层数；α是金子塔各层的尺度因子；Si是第i层图像的缩放尺度。

若图像中能够提取的特征点总数为N，则每层金字塔图像需要提取的特征点数目为

式中：Ni是第i层所需的特征点数目；1/Si是尺度因子的倒数。

对每层金字塔图像进行区域划分，定义Sirow为第i层图像的行分割数，Sicol为 第i层图像的列分割数，设定预提取的数目为

在划分区域中，设置提取特征点的初始化阈值Tini=25，记提取的数目为Nini。判断Nini与预期提取的特征点数目的大小，若判定此时有区域提取的特征点为空，调整阈值Tmin，完成区域内特征点的提取，保证特征点分布均匀。改进前后的ORB算法特征提取效果如图1所示。

由图1 可以看出，图像特征点均匀化改进后，极大地改善了ORB-SLAM 原算法中图像特征点过于集中的情况，改进算法保证了每帧图像中特征信息的完整性，有助于相机位姿估计和地图精度的提升。

1.2 轻量级高精度语义分割及运动先验

SLAM 系统增加语义分割线程，会增加实际运行的时间，因此设计轻量级高分割精度的网络尤为重要。BiseNet 网络精度高，在大目标的分割上较为精准，缺点是网络感受野大，造会成图像中部分物体的空间信息丢失，致使网络高精度的优势不复存在。因此，本文改进BiseNet 结构，设计了基于分支空洞卷积的双边语义分割网络，结构如图2所示。

在原有的主干分支上下文路径后添加分支空洞卷积为3 次3×3的深度可分离卷积提取特征，并设置6、12、18 不同比例的空洞卷积扩张率，调节网络感受野，获取上下文特征信息。

在图中原有的Xception 模型中添加多尺度特征融合模块，为了避免Xception 提取特征冗余或导致模型过拟合，将重复残差结构的数量调整为2（即stride=2）。将不同分辨率分支提取的特征先经过3×3卷积，其次融合2 倍上采样特征，再经过1×1卷积进行多尺度融合，实现特征的反复提取与交换，丰富上下文信息。

将上下文路径获取的融合特征与原BiseNet 空间路径获取的特征使用双边引导的聚合层进行融合，通过不同的尺度去指导捕获那些层次不同且固定编码的特征，使得图像分割效果更好。图3 显示了语义分割算法结果。

室内场景中每个物体的运动可能性的先验是不同的，针对PASCALVOC2012 数据集中的20 类物体，将人、动物、自行车作为潜在运动物体，椅子等为静态物体，应用改进BiseNet 语义分割网络，获得图像的运动掩膜，即物体的运动先验。

1.3 运动一致性检测

地图中不能存在运动的人或动物等，真实环境中会存在没有先验动态标记而具有移动性的物体（如人手里拿着的书），因此需要进一步判定静态区域物体是否真正静态，本文结合图像的几何约束进行静态特征的判断。空间中的点P，在双目视觉中，满足如图4所示的几何约束。

图中：O1和O2表示相机的光心；空间点P在Image1 和Image2 中的投影点用P1和P2表示，归一化坐标分别为p1=[u1,v1,1]和p2=[u2,v2,1]；极线l1和l2用平面方程Ax+By+C=0表示，则P1到极线l2的映射为

式中F为基础矩阵。

根据点P2和极线l2的关系，可判断特征点状态。当特征点P2不在极线l2上，空间点P可能为动态特征点，但也不排除静态点的系统误差原因。因而，计算点到直线之间的距离D：

如果距离D的数值大于经过实验和统计分析得到的距离设定值ds，则将这些特征点确定为动态特征点。

若点P2在极线l2上，也并不能确定点P2为静态点，该点可能来自潜在运动区域（如该点提取自人手中的书本）。由于对极约束和运动一致性检测比较耗时，只需对当前帧与关键帧进行检测，即取P1和P2周围5×5大小的像素块进行块匹配，使用去均值的归一化互相关（normalized cross correlation，NCC）计算像素块之间的相似度ρ。

式中：F、T分别表示特征点P1和P2周边5×5的像素块；、分别表示特征点P1和P2周边5×5像素块的均值。

若像素块间相似度|ρ|≥0.9，就认为这些特征点是静态特征点；否则是动态特征点。通过对特征点与极线关系的判断，确定动态特征点并剔除，图5所示为动态点剔除效果图。

对得到的当前帧中静态特征点与参考帧静态特征点进行关联，通过最小化误差函数可以获取相机的最优位姿估计，然后对相邻关键帧所有静态特征点构建最小二乘，从而得到系统位姿，并根据语义分割网络提取的语义信息剔除动态特征点，对剩余的静态特征点结合深度信息构建三维静态环境地图。

2 实验结果与分析

2.1 数据集实验

TUM[20]数据集有大量的室内环境图像。为了验证本文算法在动态环境下的鲁棒性和定位精度，基于上述数据集对提出的算法在特征点的均匀度、语义分割的效果、相机轨迹误差以及建图效果方面进行实验评估。针对测试实验的计算机配置如表1所示，实验所需环境中设计的视觉SLAM系统使用C++语言。

表1 实验计算机所需配置Table 1 Required configuration of experimental computer

2.1.1 图像特征点提取测试

特征点提取的时间决定了系统能否满足实时性的需求。采用牛津大学VGG（visual geometry group）标准数据测试各算法在不同实验场景特征点提取，表2 给出了分别使用本文提出的特征点提取算法与SIFT(scale-invariant feature transform)、SURF(speeded up robust features)、ORB 和AKAZE(accelerated-KAZE)算法对标准数据集进行特征点提取的用时比较。

表2 特征点提取用时比较Table 2 Comparison of feature point extraction time ms

从表2 可以看出，ORB算法提取特征点用时最短，SIFT算法提取特征点所需时间最长。本文ORB 特征均匀化方法带来的耗时与原ORB算法基本相当，对ORB-SLAM2算法的实时性能不会产生本质影响。

2.1.2 各语义分割算法分割效果测试

为了验证改进的语义分割网络具有高效的分割性能，选择Nyu-V2 数据集进行实验，并与当前常提及的语义分割算法进行比较，实验结果如图6所示。

如图6所示，第1 列为原始图，第2 列到第5列分别为SegNet、Mask-RCNN、BiseNet 和本文提出算法在Nyu-V2 数据集上的分割效果图。可知SegNet算法对于门和橱柜等大目标的分割出现了混淆，分割效果不佳。虽然Mask-RCNN、BiseNet在大目标的分割上较为精准，但物体边缘细节和遮挡处的分割不够准确。

为了更好地评估算法性能，各语义分割算法在Nyu-V2 数据集进行实验，得到平均像素准确率（MPA）和平均交并比（MIoU），结果如表3所示。

表3 各算法在Nyu-V2 的平均像素准确率（MPA）和平均交并比（MIoU）Table 3 MPA and MIoU of each algorithm in Nyu-V2

可见，相比于其他3 种语义分割算法，本文算法的平均像素准确率达到了74.83%，平均交并比达到了74.91%。这是因为在上下文路径模型中本文算法首先采用深度卷积神经网络提取图像的初始特征，然后在该模型后添加分支空洞卷积模型，与此同时嵌入多尺度融合增强网络，充分保存高分辨率信息，使物体边缘分割更加细致，有效提高了图像语义分割的精度。

在跟踪线程中，由于本文算法采用多线程并行操作，因此实际的系统消耗时间大致由系统时间消耗最长的线程决定。本文中语义分割线程的平均耗时为49 ms。另外，本文算法还增加了对动态区域的判定与剔除（平均耗时5 ms），最终跟踪线程中每帧的平均运行时间为54 ms，即系统整体上能达到15 帧/s 的图像处理速度，对改进前的BiseNet分割的实时性影响较小，能够满足算法的实时性要求。

2.1.3 各算法相机轨迹误差实验

TUM 数据集中的fr3 系列是典型的动态场景序列集。数据集提供了应用KinectV1 相机拍摄的RGB 图像以及真实运动轨迹（ground truth）。其中，walking 序列是人在桌子旁边走动的场景图像。为体现本文算法在动态场景下的定位效果，通过TUM数据集中的walking_rpy 序列进行实验对比不同算法，采用相机运动轨迹和真实值之间的绝对轨迹误差（absolute trajectory error，ATE）进行系统性能评价。对比的目标包括ORB-SLAM2算法、DSSLAM算法以及Dyna-SLAM算法，实验结果如图7所示。

从图7 可以看出，在walking 系列数据集的高动态场景下，ORB-SLAM2算法由于应用大量动态特征点进行位姿优化，因而系统误差偏大。DSSLAM 中使用了语义分割网络SegNet，其对物体的边缘轮廓分割较差，因而存在人体等非刚性物体的局部运动特征点，这些动态点没有彻底删除而造成位姿估计精度不高。DynaSLAM算法中语义分割网络Mask-RCNN 性能好于SegNet，但静态特征点分布不均匀，导致跟踪精度和位姿估计下降。相较于其他3 种算法，本文算法中的语义分割网络分割精度高，且特征点均匀分布，因而，轨迹误差最小，位姿估计更精确。

2.1.4 各算法构建地图的效果对比

为了验证提出算法在动态环境中的建图效果，仍采用fr3 数据集中的高动态walking_rpy 序列构建环境地图，实验结果如图8所示。

从图8 可以看出，ORB-SLAM2算法构建的地图会有很多重影。DynaSLAM 中，语义分割网络分割效果不及本文改进的基于分支空洞卷积的双边语义分割网络，同时特征点分布不均匀，部分区域中静态特征点的稀疏性最终影响了填补效果。

2.2 实际场景实验

2.2.1 稀疏点云地图构建

在实验室环境中，应用ORB-SLAM2算法和本文算法建立稀疏点云图，图9 为建图效果，其中下方蓝色四棱锥代表关键帧位姿。

在图9(a)中，由于ORB-SLAM2算法不分辨特征点的运动状态，引起位姿计算错误，进而导致3D 坐标计算错误，最终地图点叠加在一块区域中，无法区分。图9(b)中，动态物体被检测到，剔除后建立静态环境地图。

2.2.2 稠密点云地图构建

取出人在视野内变化的2 帧图像，使用本文算法计算相机位姿，基于TUM 提供的稠密点云生成脚本，利用RGB-D 图像和对应的相机位姿拼接成稠密三维点云，应用常见算法与本文算法构建稠密地图，结果如图10所示。

从图10 可以看出，本文算法建立的点云图几乎不包含动态点，可以更好地表现静态场景。

3 结论

针对现有视觉SLAM 系统在动态环境下定位精度不足或实时性较差的问题，本文基于ORBSLAM2 进行改进，增加一个语义分割线程。通过设计分支空洞卷积的双边语义分割网络获取图像的语义信息，与运动一致性检测方法相结合，确定运动物体，剔除对应动态区域的特征点，使用处理后的静态特征点进行帧间匹配和位姿估计。在公开数据集上进行的实验结果表明，改进后的算法能大幅度提高SLAM 系统在动态环境下的精度和鲁棒性，且能保证实时性。但本文算法仍存在局限性，当相机移动过快或潜在运动物体出现突发的不连续运动时，会出现跟踪丢失的情况，导致定位建图失败。在未来的工作中将考虑与惯性测量单元（inertial measurement unit，IMU）融合，增强系统在极端环境下的鲁棒性，使系统具有更强的适用性。