动态环境下基于深度学习的语义SLAM算法

郑思诚，孔令华，游通飞，易定容

（1.福建工程学院机械与汽车工程学院，福州 350118；2.数字福建工业制造物联网实验室（福建工程学院），福州 350118；3.华侨大学机电及自动化学院，福建厦门 361021）

0 引言

同步定位与地图构建（Simultaneous Localization And Mapping，SLAM）是指在没有环境先验信息的情况下，载体通过自身搭载的传感器收集环境信息，在估计自身位姿的同时构建环境地图，该技术被认为是移动机器人智能化的先决条件。基于视觉信息的SLAM（visual SLAM），因其所使用的相机性价比高、体积小、功耗较低，并且可以获得丰富的环境信息的特点，成为机器人领域的研究热点［1］。

目前，基于视觉的SLAM 算法研究取得了很大的进展，例如ORB-SLAM2（Orient FAST and Rotated BRIEF SLAM2）［2］、LSD-SLAM（Large Scale Direct monocular SLAM）［3］等。但这些算法通常都基于一个强烈的假设，即工作环境是静态或低动态的，对应用环境有严格的限制。这种假设影响了视觉SLAM 系统在实际场景中的适用性，当环境中存在动态物体时，例如走动的人、反复开关的门窗等，都会给系统带来错误的观测数据，降低系统的精度和鲁棒性。

在移动机器人使用相机进行定位与建图的过程中相机通常处于运动状态。这就使得经典的运动分割方法，例如背景去除（Background Subtraction）［4］无法使用在视觉SLAM 中。早期的SLAM 系统大多采用数据优化的方法降低动态物体的影响。文献［5］使用随机抽样一致性检测（RANdom SAmple Consensus，RANSAC）算法粗略地估计出两帧之间的基础矩阵，再通过迭代最近点（Iterative Closest Point，ICP）算法将两帧图像的点云数据进行拟合，动态物体的点云数据将作为噪声或离散点被删除，这与三维重建中的点云拼接类似。文献［2］将动态物体上的帧间特征点匹配对使用RANSAC 算法剔除，一定程度上降低动态物体对SLAM 系统的影响。这些方法都隐性地假设图像中的物体大部分是静态的，当动态物体产生的数据超过一定阈值时，这些方法将失效。

随着深度学习技术的发展和硬件计算能力的提升，获取图像中语义信息的成本逐渐降低。将深度学习和视觉SLAM结合，使SLAM 系统从几何和语义两个层次上理解环境信息，生成的语义地图使得机器人更好地认知周围环境和进行任务规划，提升机器人的人机交互能力［6］。Salas-Moreno等［7］提出的SLAM++将采集到的点云信息与使用KinectFusion［8］提前构建的高质量的3D模型数据库对比，将匹配的物体模型插入地图中，但语义地图只能构建数据库中存在的物体。Bescos等［9］提出的DynaSLAM 中，使用RGB-D 相机作为传感器将MASKRCNN（MASK Region-based Convolutional Neural Network）［10］与多视图几何结合，实现对没有先验知识的动态目标检测和剔除，同时对剔除动态物体的图像进行修复获得静态场景用于三维重建。Yu 等［11］利用帧间图像的光流跟踪，进行一致性检验的同时结合SegNet［12］剔除动态物体上的特征点，提高位姿估计精度并使用八叉树建立带有语义信息的地图。

针对现有视觉SLAM 系统在动态环境中普遍存在精度较低或实时性较差的问题。本文在ORB-SLAM2 的基础上进行改进，提出基于特征点和语义信息的SLAM 算法，消除动态目标对SLAM系统的影响并在室内动态环境中进行测试。

1 系统框架

1.1 ORB-SLAM2

Mur-Artal 等［2］在2015 年提出的ORB-SLAM2（Orient FAST and Rotated BRIEF SLAM2）是前端视觉里程计基于特征点法的代表工作之一，开创了3个实时运行的平行线程框架：特征点提取和匹配的跟踪线程、局部光束法平差优化线程以及全局回环检测和优化的回环线程。这个框架被后来的很多视觉SLAM系统沿用或在其基础上进行改进，其算法流程如图1所示。

图1 ORB-SLAM2算法流程Fig.1 Flowchart of ORB-SLAM2 algorithm

跟踪线程对每一帧图像提取ORB（Oriented FAST and Rotated BRIEF）特征进行帧间特征点匹配，并通过最小化重投影误差计算相机位姿。局部建图线程负责对得到的关键帧进行管理，根据关键帧中所包含的地图点信息确定与其他关键帧之间的共视关系，剔除冗余关键帧以及不合格的地图点，随后对一个滑动时间窗口内的一系列存在共视关系的关键帧组及其所观测到的地图点进行局部BA（Bundle Adjustment）优化。当回环线程检测到回环发生时，利用回环约束对关键帧进行融合和图优化，消除系统运行过程中累计的误差。本文算法在该系统的基础上进行后续开发。

1.2 YOLOv4目标检测算法

YOLO（You Only Look Once）是一种基于深度神经网络的单阶段目标检测算法，它将目标检测作为回归问题求解，直接从一张输入图片获得预测物体的边界框（bounding box）位置及其分类，在保证准确率的同时兼顾了实时性。

YOLOv4［13］算法是在YOLOv3［14］算法的基础上对网络结构进行改进，将深度学习的优化方法分为免费包（Bag of freebies）和特色包（Bag of specials），对其中不增加目标检测器计算成本而只改变模型训练技巧和训练成本的方法成为“免费包”，而对于那些只增加少量计算成本但能显著提高对象检测精度的模块和处理方法称之为“特色包”。文献［13］对近年来提出的深度学习优化方法进行测试，最终选取其中适合YOLO 的部分相结合，相比YOLOv3 在训练数据处理、骨干网络结构、激活函数、边界框损失函数等各方面均有所改进。

文献［13］中认为一个标准的目标检测器由输入、骨干网络（Backbone）、颈部（Neck）和头部（Head）四个部分组成。YOLOv4 的主干网络在YOLOv3 的DarkNet53 的基础上，将其与CSPNet［15］相结合，在DarkNet 的每个残差块加上CSPNet 将基础层的特征映射划分为两部分，再通过一个跨阶段层级将这2 个部分融合起来，在降低内存占用和计算量的同时提升精度。在骨干网络中使用中，使用Mish 激活函数代替了原来的Leaky ReLU。引入空间金字塔池化（Spatial Pyramid Pooling，SPP）［16］利用四个不同尺度的最大池化对上层输出进行处理，显著增加了感受野。PANet（Path Aggregation Network）［17］作为颈部代替YOLOv3 中的FPN 作为参数聚合的方法，并对其进行了修改，使用张量连接（Concat）代替了原来的捷径连接（shortcut connection）。头部部分继承了YOLOv3的网络设计进行多尺度检测，采用3 个不同层级的特征图进行融合，提高对不同尺寸目标的检测能力。YOLOv4 的网络结构如图2所示。

图2 YOLOv4网络结构Fig.2 Network structure of YOLOv4

在实际工程应用中，需要根据使用需求以及搭载平台的硬件性选择合适的网络结构或并对其进行调整。考虑到视觉SLAM 系统通常安装在移动终端上，这类载体如移动机器人、手机等受到体积、能耗、价格等因素的限制，需要在有限的算力和内存下保证系统的稳定运行。Wang 等［18］对YOLOv4 网络模型进行调整缩放，使其满足部署在不同平台的需求。

YOLOv4-tiny便是针对嵌入式平台，考虑到其内存访问速度、内存带宽、运算能力等因素改进后的网络结构。在其骨干网络中，为了更快的运算速度将激活函数改为了Leaky ReLU。在颈部与原版相比其只提取两个特征层，使用特征金字塔对最后一个有效特征层卷积后进行上采样，和另一个特征层进行张量（Concat）融合。原版YOLOv4 共有162 层，缩放后则只剩38 层。在使用GTX1070 图像处理器的条件下，YOLOv4 检测视频的平均速度为20 FPS（Frames Per Second），而YOLOv4-tiny 能够达到180 FPS 的处理速度，检测速度显著提升。改进后的YOLOv4-tiny网络结构如图3所示。

图3 YOLOv4-tiny网络结构Fig.3 Network structure of YOLOv4-tiny

1.3 动态特征点剔除

为解决传统视觉SLAM 算法在动态场景下相机位姿估计和建图不准确的问题，本文在ORB-SLAM2 的基础上进行改进。在原先三线程并行的框架下增加一个目标检测线程。当图像帧传入系统后，跟踪线程和语义分割线程并行对图像进行处理。本文设计的动态特征剔除算法框架如图4所示。

图4 动态物体剔除算法框架Fig.4 Framework of dynamic object elimination algorithm

本文将传感器获取的RGB 图像输入到追踪线程中对每一帧图像提取ORB 特征点并与相邻帧进行帧间匹配，利用匹配的特征点对求解相机运动是一个PNP（Perspective-N-Point）问题。本文使用BA 非线性优化算法求解相机位姿。根据投影的像素坐标ui=[ui，vi]T，RGB-D 相机可以直接获取对应特征点在3D空间点坐标Pi=[Xi，Yi，Zi]T。两者之间关系如下：

式中：K为相机的内参数矩阵，T为变换矩阵。

由于特征点误匹配以及观测的噪声的存在，式（1）存在一个误差项。构建最小二乘问题，误差项如下：

该问题的误差项是将观测到的特征点像素坐标与其对应的3D 空间点根据计算得出的位姿投影到像素平面的坐标计算差值，故称为重投影误差。重投影误差模型如图5所示。

图5 重投影误差模型Fig.5 Reprojection error model

在语义分割模块中，首先对接收到的图像数据进行预处理，在不失真的前提下使其达到尺寸要求。随后将图像输入到CSPDarkNet53 骨干网络中，在一系列卷积和残差网络中提取特征信息，最终获得3 个大小为（52×52×256）、（26×26×512）、（13×13×1 024）的有效特征层。这3个输出结果将图片划分成不同大小的网格，以适应不同尺寸的物体。每个网格上预设有3个先验框，卷积神经网络会判断先验框中是否存在物体并对先验框尺寸进行调整。对先验框进行调整的过程被称为解码，调整后的先验框被称为预测框由x、y、w、h以及置信度和分类结果6个参数组成：x、y为预测框的中心与网格原点的相对值；w和h表示预测框的长和宽；置信度代表该预测框内包含某物体的可信度以及预测框位置的准确度；分类结果则根据训练时使用的数据集所包含的物体类别的数量决定。

由于每个网格都进行独立的预测，因此会出现某个区域多个预测框指向同一个物体的情况，需要根据各个预测框的得分使用非极大值抑制的方法筛选出得分最高的预测框。随后根据预设好的动态物体类别筛选预测结果，对其中被标记为动态物体的区域添加掩码，并根据掩码区域剔除ORB 特征检测模块中图像对应区域上的特征点。

在动态环境中，位于动态目标上的特征点匹配对会使重投影的误差显著增大，相机位姿无法收敛从而导致位姿估计精度下降。为此，在跟踪线程提取特征点后需等待语义分割线程传来的目标检测信息，根据预设的动态目标类型剔除动态目标上的特征点。在后续的局部地图跟踪、相机位姿估计和局部全局BA优化中只使用处理后的特征点，增强系统在动态环境下的精度和鲁棒性。

图6（a）是为原版ORB-SLAM2，可以看出大部分特征点分布在人身上，由于总的特征点数目是固定的，因此静态物体只有少量的特征点被提取。图6（b）是添加了动态目标检测线程的特征点提取图，人身上的特征点被剔除，取而代之在静态的背景上提取特征点，增强了系统的精度和鲁棒性。

图6 动态特征点剔除效果Fig.6 Effect of dynamic feature point elimination

2 实验与结果分析

本章使用TUM RGB-D 数据集评估系统在动态环境中的定位精度和实时性，并与其他先进的视觉SLAM 进行对比。主要将本文系统与ORB-SLAM2 进行比较以评估其在动态场景中的改进效果，开源算法中ORB-SLAM2在静态场景中实现了精度和实时性的平衡，同时其作为本文算法的改进基础适合作为比较对象。

实验所使用的TUM 数据集是由德国慕尼黑大学制作，使用Kinect 传感器以30 Hz 的速率采集信息，图像分辨率为640×480，在采集图像数据的同时，使用高精度运动捕捉系统VICON 与惯性测量系统实时获取相机位置与姿态的数据，可近似作为RGB-D 相机的真实位姿数据。因此该数据集被大多数视觉SLAM研究者所采用，作为基准评估视觉SLAM算法的标准数据集之一。

文献［19］提出了两种用以评估视觉SLAM 算法精度的评价指标，分别是相对位姿误差（Relative Pose Error，RPE）和绝对轨迹误差（Absolute Trajectory Error，ATE）。作者认为直接评估建图精度是较为困难的，因此通过计算SLAM 系统估计位姿的误差，间接评价建图的精度。文中使用均方根误差（Root Mean Squared Error，RMSE）作为评估这两个指标的参数，计算所有时刻估计位姿的误差，计算公式如下。

其中：trans(Ei)表示i时刻的绝对或相对位姿估计误差，E1：n表示从1到n的一系列相机位姿，Δ表示相隔固定时间差。

相对位姿误差是描述在一个固定时间段Δ内估计位姿和真实位姿的差值，其中包含平移误差和旋转误差两个部分。绝对轨迹误差是直接计算估计位姿和真实位姿的差值，能直观地反映算法估计轨迹的精度。

在TUM数据集中的walking序列中，两个人从椅子上站起并围绕办公桌行走最后坐下，同时相机也根据预先设定的运动模式移动，在某些时间节点移动的人占据了图像一半以上的区域，对于评估视觉SLAM 算法在动态环境下的性能极具挑战性，将该数据集作为高动态实验环境。本文还测试了ORB-SLAM2以及改进后的系统在静态及低动态场景的表现，对照说明本系统的精度。在sitting 序列中两个人坐在办公桌前的椅子上小幅度运动，在desk 数据集中相机围绕静止的办公桌运动。不同动态环境下的数据集场景如图7所示。

图7 不同动态环境下的TUM数据集场景Fig.7 TUM dataset scenes in different dynamic environments

本文分别选用walking_xyz、walking_halfsphere、walking_static 作为高动态场景序列；sitting_static 作为低动态场景序列分别对ORB-SLAM2 和本文算法进行测试。为了验证本文算法在动态环境下实现的精度，使用fr2_desk 静态场景数据集对两种算法进行测试。同时实验中还将本文算法与其他针对动态环境设计的视觉SLAM 算法进行对比，分析其在精度和实时性上的优劣。

2.1 相机位姿误差分析

4个序列的定量比较结果如表1～3所示。在实验中，使用RPE 和ATE 评估位姿真实值与估计值的差异，计算其RMSE和标准偏差（Standard Deviation，SD），其中均方根误差描述观测值与真实值的偏差，易受到较大或偶发错误的影响，因此能更好地反映系统的鲁棒性［20］。标准差评价估计轨迹相较于真实轨迹的离散程度，能够反映系统的稳定性。

表1 绝对轨迹误差对比结果（ATE）Tab.1 Absolute trajectory error comparison results（ATE）

数据集的xyz、halfsphere、static 分别代表三种不同的相机运动方式：相机沿X、Y、Z轴移动，相机在直径为1 m 的半球面移动以及相机基本保持静止。

表格中计算本文算法相比ORB-SLAM2 的改进值（Improvement），评估其优化效果，公式如下：

式中：η表示算法的改进程度，α表示ORB-SLAM2算法轨迹误差，β表示本文算法轨迹误差。

由表1 可知，在高动态场景下绝对轨迹误差RMSE 和SD两者平均下降了96.7%，说明本文算法相比ORB-SLAM2在高动态场景下大幅度提升了定位精度和稳定性，且达到了与ORB-SLAM2 在静态环境下相同量级的精度。在低动态场景下本文算法相比ORB-SLAM2 没有明显的改进，分析原因是ORB-SLAM2 所使用的RANSAC（RANdom SAmple Consensus）算法能将部分小幅度移动的动态特征点作为噪声剔除，一定程度上降低了运动物体对算法精度的影响。表2 和表3 分别列出了两种算法在不同场景序列下的相对位姿和相对旋转误差并计算改进值，在高动态场景下，相对位姿和相对旋转的均方根误差分别平均降低96.82%和96.21%，标准差分别降低97.57%和96.63%。两者显示出与绝对轨迹误差相同的趋势，本文算法在高动态场景性能提升较大。

表2 相对位姿误差对比结果（RPE）Tab.2 Relative pose error comparison results（RPE）

表3 相对旋转误差对比结果Tab.3 Relative rotation error comparison results

如图8～10所示，分别绘制了ORB-SLAM2和本文算法在3个高动态数据集上的估计相机轨迹和真实轨迹对比图以及相对位姿误差曲线，在轨迹对比图中以不同线段分别代表由高精度运动传感器、本文SLAM 算法计算得到的相机运动轨迹以及两者之间的误差。

图8 walking_xyz数据集上的绝对轨迹和相对位姿误差Fig.8 Absolute trajectory and relative pose error on walking_xyz dataset

从图中可以更直观地体现与ORB-SLAM2 相比本文算法的绝对轨迹误差和相对轨迹误差均大幅度降低，位姿估计更加精确，证实了该算法的有效性。

为进一步验证本文算法的有效性，将本文算法与其他基于动态环境设计的视觉SLAM算法进行比较，对比各算法在高动态场景数据集walking_xyz、walking_halfsphere、walking_static的表现。实验主要对比各算法在相同数据集下的绝对轨迹误差，实验结果采用原始论文中提供的实验数据，结果如表4 所示。由表4 可知，本文算法在高动态环境下达到了与DynaSLAM、DS-SLAM 相近的精度并且远高于Mask-Fusion 以及ElasticFusion（EF）［20］的精度，证明了本文算法的有效性。

表4 基于动态环境的视觉SLAM算法精度比较Tab.4 Accuracy comparison of visual SLAM algorithms based on dynamic environment

图9 walking_halfsphere数据集上的绝对轨迹和相对位姿误差Fig.9 Absolute trajectory and relative pose error on walking_halfsphere dataset

图10 walking_static数据集上的绝对轨迹和相对位姿误差Fig.10 Absolute trajectory and relative pose error on walking_static data set

2.2 算法实时性测试

在实际应用中，实时性是评估一个SLAM 系统的重要指标。在本实验中，主要测试跟踪线程处理每一帧所需的时间，并与其他动态环境下的SLAM 系统进行比较，结果如表5 所示。在使用YOLOv4-tiny 网络架构的情况下，本系统跟踪线程处理一帧平均耗时63 ms，反映到系统整体上能达到每秒15帧的图像处理速度。与其他基于动态环境的视觉SLAM 算法相比，本文算法实现了精度与速度的平衡。

表5 基于动态环境的视觉SLAM算法实时性比较Tab.5 Real-time performance comparison of visual SLAM algorithms based on dynamic environment

3 结语

针对现有视觉SLAM 系统在动态环境下精度不足或实时性较差的问题，本文基于ORB-SLAM2 进行改进，增加一个语义分割线程。利用YOLOv4 目标检测算法获取图像的语义信息，去除动态物体上的特征点，使用处理后的特征点进行帧间匹配和位姿估计。

在公开数据集上进行的实验结果表明，改进后的算法能大幅度提高SLAM 系统在动态环境下的精度和鲁棒性且仍能保证实时性。但本文算法仍存在局限性，当相机移动过快或大幅度旋转，会出现跟踪丢失的情况导致定位建图失败。在先验知识中属于静态目标的物体，例如椅子、书本等被人为移动时，本文算法也无法生效。为克服这些局限性，在未来的工作中将考虑与惯性测量单元（Inertial Measurement Unit，IMU）融合，增强系统在极端环境下的鲁棒性；使用多视图几何等方法增强动态物体检测的性能，使系统具有更强的适用性。