1.   引言

同时定位与地图构建技术(Simultaneous Localization And Mapping, SLAM)指在没有环境先验信息的情况下，机器人可以估计自身位置和构建环境地图的技术已经成为机器人在未知领域内自主工作的核心技术。近年来，随着研究的深入，很多以相机为主传感器的视觉SLAM被广泛应用，比如Kimera^[1], Vins-mono^[2], ORB-SLAM3^[3]等都取得了良好的效果。

目前视觉SLAM技术仍然面临着许多问题亟待解决。首先是目前大部分视觉SLAM 系统都基于静态环境的假设^[2-4]，即基于静态的环境信息判断自身的位姿。然而在实际的应用场景中，不可避免地会存在动态物体，这就会大大降低机器人的定位精度，甚至导致SLAM系统无法正常运行；其次是目前的视觉SLAM系统大多专注于利用低维信息构建几何地图^[2-4]，这类地图不提供对周围环境的高层次理解，不利于机器人执行复杂任务，比如避障和目标抓取。

为了克服环境中动态物体的影响，近期的研究提出了许多解决方案，这些方案的核心思想都是在估计位姿之前将属于动态物体的特征点剔除。RigidFusion^[5]通过计算几何残差将动态物体分离；Ji等人^[6]通过分割深度图像并计算重投影误差来确定动态区域；FlowFusion^[7]引入了光流残差的概念，结合几何信息区分动态像素点。然而这些方法都对噪声非常敏感，在复杂场景下鲁棒性较差。深度学习的发展为这个问题带来了新的解决方案，利用目标检测^[8,9]或是语义分割^[10,11]结果可以更加准确地获取动态区域。MaskFusion^[12]通过引入Mask R-CNN^[10]获取带有语义标签的实例级分割来处理动态物体；DynaSLAM^[13]进一步将Mask R-CNN的结果与多视角几何相结合从而过滤所有的动态特征点；DS-SLAM^[14]则利用SegNet^[11]进行语义分割，加上运动一致性的检测，获取场景中的运动区域；RDS-SLAM^[15]则采用了与DS-SLAM较为一致的方法，也得到了不错的效果。基于深度学习的方法相较于传统方法更为鲁棒，但会消耗更多的时间和计算资源，同时对于被迫移动的物体，比如手上拿着的书本和正在被移动的键盘，也很难达到预期的效果。

在语义建图方面，早期的工作都是针对静态场景进行建图，SemanticFusion^[16]通过贝叶斯更新模型传递语义信息，提升了3维语义地图的重建精度。但这种方法在动态场景下就会完全失效，因此如何针对动态场景进行语义地图构建也成为近期的研究热点。Fan等人^[17]提出用神经网络剔除动态目标后进行建图，虽然建图质量尚可但地图并不包含语义信息；Cheng等人^[18]将语义信息和几何信息进行融合，构建出在动态场景中一样适用的3维语义地图。但是由于目前3维地图中的语义信息大多是由2维图像语义信息反投影得到的，因此必然会存在重叠，误匹配等现象，换言之，3维中的语义信息无法还原出真实比例的物体模型，这样的地图在真实场景的使用中会严重受限。

针对目前视觉SLAM遇到的问题以及现有方法的不足，本文提出一种简便且有效的视觉SLAM系统。与现有的绝大多数方法不同，本文考虑了被迫移动的物体给定位和建图带来的影响。本文首先利用YOLOv3 (You Only Look Once)^[19]目标检测网络的检测结果中被迫移动物体大概率被包含于潜在动态目标检测框范围内的事实，在所有潜在的动态目标框内进行光流特征点的计算，然后根据几何约束得到的动态特征点比例判断该目标是否是真正的动态目标，最后只有落在真正动态目标检测框内的特征点才会被剔除。在语义建图方面，本文结合了深度图像和YOLOv3目标检测网络带来的语义信息，并用前后多帧的图像来填充和补全当前帧动态目标遮挡的区域，构建了3维点云语义地图。同时为了让语义信息能够更好地还原出物体的真实模型，构建更精准的物体级语义地图，本文提出一种基于超体素的快速点云分割方案。最后为了让YOLOv3目标检测网络和语义SLAM能够更好地结合，进而让两者相辅相成形成良性循环，本文将得到的3维语义分割地图根据定位时获得的不同位姿投影至2维图像，根据2维图像上的语义信息重新生成目标检测的真值结果，构成新的数据集，包括高亮、模糊、旋转、遮挡等复杂样本用来微调YOLOv3目标检测网络，从而提升目标检测性能。

2.   系统设计

2.1   系统框架

本文选用ORB-SLAM2^[4]作为基础框架，因为ORB-SLAM2是目前最为完整、稳定的开源视觉SLAM系统之一。图1是本文提出的SLAM系统的完整框架，在ORB-SLAM2的基础上引入了动态特征点剔除和语义建图线程。首先原始的彩色图像被同时送入特征点提取和目标检测模块，光流约束模块确认真正的动态目标，属于真正动态目标的特征点被剔除。其次目标检测模块获取的语义信息关联到跟踪模块生成的关键帧中静态区域。接着处理后的关键帧会被送入语义建图线程，生成3维稠密语义点云地图(地图1)，3维语义分割地图(地图2)，物体级语义地图(地图3)。最后在地图2的基础上，根据定位获得的位姿投影至2维图像，生成针对复杂场景的数据样本，进而微调目标检测网络，形成良性互补。

图 1  系统总体框架

下载: 全尺寸图片 幻灯片

图 2  关键帧处理示意图

下载: 全尺寸图片 幻灯片

图 3  地图1和地图2的结果

下载: 全尺寸图片 幻灯片

2.2   动态特征点剔除

为了兼顾速度和准确度，本文采用YOLOv3^[19]作为目标检测网络，并在COCO (Common Objects in COntext)^[20]数据集上进行训练，目标检测的结果可以被分为静态目标包括键盘、书本等，和潜在的动态目标，比如人和车。选择目标检测网络而非语义分割网络主要是考虑到了在现实情况中许多静态目标都会随着人为因素而运动，而这些被迫移动的物体都有极大概率会与动态目标相邻，目标检测网络恰好能够针对这样的特性，更加适用于一些被迫移动物体影响定位精度的情况。因为一些潜在的动态目标也会保持静止，比如停在原地的汽车，直接剔除这些属于潜在动态目标的特征点又会丧失很多信息。因此本文在目标检测模块后又加入了光流约束模块，简单来说就是通过计算符合几何约束的光流特征点的比例来判断潜在动态目标框内的目标是否真确实产生移动。

首先本文对连续两帧图像内的潜在动态目标区域计算稀疏LK (Lucas-Kanade)光流金字塔，获得若干对无需描述子的光流匹配点对，根据这些光流匹配点对恢复出两帧之间的相对变换关系，即基础矩阵，之后，可以利用基础矩阵和上一帧中的ORB关键点，得到当前帧中的极线。按照对极几何理论，在当前帧中与上一帧匹配的ORB特征点应该坐落在极线上，因此可以根据当前帧上ORB关键点到极线的距离来判断该ORB关键点是否为动态特征点。一旦距离大于1，则认为该特征点为动态特征点。最后遍历潜在动态目标区域内的所有特征点，根据其中动态特征点的占比来判断该潜在动态目标是否真的在运动，如果被判定为真正的动态目标，那么该区域内的所有特征点都会被剔除，而剩余的特征点将会被保留至后续跟踪和建图时使用。

2.3   语义地图构建

2.3.1   3维稠密语义点云地图(地图1)

首先本文将ORB-SLAM2中生成的关键帧作为语义地图构建的基础，并结合YOLOv3目标检测结果，对这些关键帧进行处理。本文对置信度大于0.8的目标检测框进行填充，每个类对应不同的颜色，但考虑到物体间可能存在遮挡的情况，本文又采用了当前关键帧所对应的深度图(图2(b))，根据目标检测框的平均深度来判断物体的前后关系，如图2(a)中电脑屏幕的颜色就会遮挡一部分泰迪熊的颜色。其次所有被判定为潜在动态目标的检测框，无论是否真的在运动也不考虑与其他物体的遮挡关系，都会被直接填充为白色，如图2(a)中白色会直接覆盖键盘、水杯和鼠标的颜色。在语义建图线程中，白色就被认为是丢弃的像素点，不会参与建图任务，而由白色带来的空缺的部分会由前后多张相邻的关键帧进行弥补。

接着本文将处理后的关键帧上的每个像素点${{{p_i}}}$根据对应的深度值${{{d_{{p_i}}}}}$和相机内参$K$投影到相机坐标系下形成3维点${{{P_i}}}$，所有的3维点就构成了当前关键帧对应的3维点云${\rm{P{C_{kf}}}}$，具体公式为

其中，${\pi }^{-1}(\cdot)$表示2～3维的逆投影变换。考虑到深度图中存在的噪点，以及后续拼接中可能出现大面积重叠等资源过剩的问题，采用体素滤波的形式对每一帧构成的点云进行降采样，即构建一个3维体素栅格，用体素中所有点的重心点来近似代替体素中的其他点，滤波后的点云用${\rm{PC}}'_{{\rm{kf}}}$表示。接着按照当前关键帧的位姿${T_K}$，将滤波后的点云投影至世界坐标系下，生成${\rm{P}}{{\rm{C}}_{{\rm{cur}}}}$。各关键帧所对应的点云集合就形成了3维稠密点云地图${\rm{P}}{{\rm{C}}_{{\rm{MAP}}}}$，如式(2)所示

由于本文所采用的关键帧已经是经过上色处理的关键帧，因此生成的地图1不会受到潜在的动态目标干扰并且已经携带了颜色信息，同时该颜色要么对应2维图像上检测到的语义信息，要么对应物体原有的颜色。相较于ORB-SLAM2生成的稀疏地图，地图1携带更丰富的信息，并且可以更加直观地展现场景内容，如图3(a)所示。

2.3.2   3维语义分割地图(地图2)

由于目标检测网络只能用一个粗略的矩形框去包围目标，再将这样的2维图像投影至3维空间生成的地图1必然会存在语义信息的误匹配和无法对齐等现象，如图3(a)中黑色框出部分所示。因此为了能够优化地图1，本文采用了一种简便快速的点云分割方法来生成地图2。

受到Pham等人^[21]提出的方法的启发，本文首先将地图1分割成多个用形心和法向量表示的超体素，并且根据超体素的相邻关系构造一张邻接图，而所谓的分割任务就等价于对邻接图的划分。接着本文利用随机采样一致(RANdom SAmple Consensus, RANSAC)寻找点云中的底层平面，并计算每个超体素到所有底层平面的最短距离，根据最短距离确定每个超体素的所属底层平面，这是划分邻接图的前提。之后根据超体素的法向量以及超体素与底层平面之间的关系，利用式(3)为邻接图的每条边计算一个权重

其中，${W_{ij}}$表示两个超体素${s_i}$和${s_j}$之间边上的权重，${{\boldsymbol{n}}_{{i}}}$和${{\boldsymbol{n}}_{{j}}}$分别是他们的法向量。在同一个类别上的超体素之间的法向量应该接近平行，这就使得他们之间的权重相对更小；相反，不同类别上的超体素间的法向量应该有更大的夹角，他们之间的权重也会变得更大。接着本文将权重大于0.3的边进行切割，从而将点云分割成不同的类别。最后本文将地图1上每个3维点的语义信息一一映射，如果某一点的语义信息在该点所处的类别中超过一定占比，则用该语义表示这个类别，否则抛弃这一语义信息。最终结果如图3(b)所示。

2.3.3   物体级语义地图(地图3)

尽管地图2让每个3维地图点都属于一个类别，并且让一些带有语义信息的类别更接近真实尺寸，但地图2仍然没有物体的概念。因此在地图3中本文用独立的3维模型来表示每个类别。

首先本文根据地图2精确的边界，如图4(a)所示，获取每个类别的中心点云的3维坐标和对应的长宽高，其次根据主成分分析法(Principal Component Analysis, PCA)获得每个类别基于世界坐标系的特征向量，这样就可以为每个类别构建一个方向包围盒(Oriented Bounding Box, OBB)，通过这样一个具有方向性的3维矩形框表征物体。最终结果如图4(b)所示，由于在目标检测时有些类的置信度太低，比如盆栽、瓶子和餐桌，这些类即使在地图2中被分割出来，也并没有携带语义信息，所以在地图3中就全部用红色框表示，而其他框的颜色则用其原本的语义信息表示。

图 4  地图3的生成过程

下载: 全尺寸图片 幻灯片

2.4   微调目标检测网络

受到最近一些工作^[22]的启发，本文认为语义地图不仅可以帮助机器人更好地认识周围环境，同时可以帮助提升目标检测网络的检测效果，这也使得目标检测网络和语义SLAM两者互相受益，从而形成一个良性循环。相较于通过训练后直接对单张图片进行检测，机器人可以在行进过程中反复地从多个角度观察不同种类的物体。本文通过将不同视角的连续图片进行整合，这样即使在某一张图片中无法给出一个很好的检测结果，也可以利用相近的图片提供的信息给出一个更加准确的目标检测结果，这也就是SLAM与其语义地图的优势所在。

为了确保获得的结果能够更加贴近真实值，本文首先根据在跟踪阶段获得的不同时刻的相机位姿将地图2投影回成像平面，这样就得到了多张2维的语义分割图，如图5(b)所示，并且每个像素点都有一个与3维点云对应的类别标签，接着用最小包围矩形框涵盖具有同一个类别标签的像素点，并且只保留那些带有语义信息的类别，如图5(c)所示，这样就生成了基于语义地图的目标检测结果。

图 5  用地图2生成目标检测结果的示意图

下载: 全尺寸图片 幻灯片

用语义地图恢复单张图像的目标检测结果不仅可以在给定的序列影像中提高现有类别上的检测成功率，如图5(a)和图5(c)所示，原始YOLOv3网络将泰迪熊错检为人，并且显示器的置信度只有0.74，而通过本文的方法生成的检测结果可以正确地将所有目标框出。同时生成的结果还可以作为复杂样本下的真值用来微调YOLOv3，使其能够在高、模糊、旋转和遮挡等复杂情况下提升性能。具体微调网络的方法比如训练所用数据集、训练集与验证集的划分在3.4节中详细介绍。

3.   实验与分析

3.1   实验设置

针对提出的方法，本文在TUM数据集^[23]和ICL-NUIM数据集^[24]上进行实验测试，主要包含3个方面的任务：第一是对定位精度的测评，第二是对语义建图质量的评估，第三是对微调网络可靠性的验证。

TUM数据集提供彩色和深度图像序列，同时提供由高速运动捕捉系统确定的相机的轨迹真值，因此被广泛运用于测试SLAM算法在动态场景下的定位准确性和鲁棒性。数据集的后缀static, xyz, half和rpy分别代表相机帧间运动的4种类型。ICL-NUIM数据集模拟了对同一个合成的室内静态场景在多种不同拍摄轨迹下的数据，提供了相机运行轨迹的真值以及正确的3维重建模型，因此可以用于对RGB-D3维重建结果进行评估。

本测试运行在一台PC上，CPU为Intel-i5，内存为64 GB，显卡为NVIDIA Geforce GTX-1080TI。系统环境为Ubuntu 16.04。YOLOv3目标检测网络基于darknet框架，SLAM主程序使用C++编写。

3.2   定位精度评估

本文挑选了TUM数据集中一些具有代表性的序列来验证本文提出的算法在不同场景下的有效性和鲁棒性。本文采用在视觉SLAM验证中最常用的绝对轨迹误差(Absolute Trajectory Error,ATE)和相对位姿误差(Relative Pose Error,RPE)作为评价指标。其中ATE通过对真实值的位置偏移求和来评估轨迹的整体质量，而RPE则考虑局部运动误差，因此可以用来测量漂移。为了能够提供更加完善的实验结果，本文对每个数据集进行连续5次的数据采集，记录每次的均方根误差(Root Mean Square Error, RMSE)，并将这5次的均值作为最终数据样本，数据结果如表1所示。

表 1  绝对轨迹误差(Absolute Trajectory Error, ATE)和相对位姿误差(Relative Pose Error, RPE)在不同方法下的数据对比

数据集	ATE(m)
	现有方法							本文方法
	ORB-SLAM2	BaMVO	SPW	LC-CRF	DynaSLAM	DS-SLAM		目标检测	光流约束	最终结果
fr3-W-static	0.3516	0.0082	0.0235	0.0111	0.0072	0.0081		0.0094	0.0112	0.0069
fr3-W-xyz	0.4563	0.2233	0.0551	0.0158	0.0164	0.0247		0.0152	0.0231	0.0132
fr3-W-half	0.4673	0.1940	0.0563	0.0318	0.0240	0.0303		0.0276	0.0259	0.0228
fr3-W-rpy	0.7946	0.1477	0.1593	0.0516	0.0367	0.0442		0.1325	0.0789	0.0488
	RPE(m/s)
fr3-W-static	0.0153	0.0068	0.0084	0.0062	0.0091	0.0066		0.0112	0.0094	0.0059
fr3-W-xyz	0.0261	0.0248	0.0086	0.0094	0.0217	0.0233		0.0238	0.0189	0.0116
fr3-W-half	0.0411	0.0244	0.0375	0.0158	0.0148	0.0297		0.0223	0.0386	0.0145
ffr3-W-rpy	0.0647	0.0155	0.0198	0.0423	0.0314	0.0503		0.0352	0.0279	0.0248

下载: 导出CSV 

| 显示表格

首先，本文针对动态特征点剔除的方法进行了消融实验，实验结果为表1最后3列，其中“目标检测”是指只依赖目标检测的结果来剔除动态特征点而不依靠光流约束，“光流约束”则恰恰相反，最后一列是将两者融合后得到的最终结果。从中可以看出，相较于原始的ORB-SLAM2^[4]，单独使用目标检测和光流约束都可以得到一个非常明显的提升；将两者结合后，与单独使用某一模块相比，误差又会明显减小。相较于原始的ORB-SLAM2，本文提出的最终方案在高动态场景下无论是ATE还是RPE的评价指标，都至少有60%的提升，效果非常明显。通过数据表明本文针对动态特征点剔除而采用的两个模块都是切实有效的。

接着，本文将提出的方法与现有针对动态场景的SLAM方法进行了比较。其中BaMVO^[25]，SPW^[26]和LC-CRF^[27]是目前无需使用神经网络中效果较为出色的算法，而DynaSLAM^[13]和DS-SLAM^[14]则是目前最为经典和被用来广泛比较的算法。从表1可以看出，无论是ATE还是RPE的评价指标，本文的方法在大部分数据集下都优于现有的方法。

最后，图6直观展示了ORB-SLAM2和本文算法在高动态场景下的轨迹比较，其中黑色是轨迹真值，红色是本文算法的轨迹，而蓝色是ORB-SLAM2得到的轨迹。从结果中可以非常明显地看出本文提出的算法与真实轨迹非常贴近，而原始的ORB-SLAM2算法则漂移严重，与真实轨迹相距甚远。

图 6  高动态场景估计轨迹对比图

下载: 全尺寸图片 幻灯片

3.3   地图质量评估

3.3.1   定量实验

为了验证本文方法在多种数据集下的可适用性以及对建图质量进行有效评估，本文在ICL-NUIM的kt0和kt1两个数据集上进行了点云地图重建并完成了定量实验。首先用本文方法重建数据集kt0的点云地图与真实3维模型进行了对齐，图7(a)展示了将kt0的点云地图与真实3维模型对齐后生成的热图，其中蓝绿色代表了重合度很高的点，而黄红色代表了重合度不够高的点。接着本文对在两个数据集下生成的热图结果进行了分析和统计，并与ElasticFusion^[28]进行了对比，ElasticFusion是目前最好的具有全局一致性的实时3维重建方案并且是大多数fusion类建图方法的基础。对比结果如图7(b)和图7(c)所示，其中距离接近0的点越多意味着越好的重建质量，从中可以看出，相较于ElasticFusion，在两个数据集下，在相同的距离下本文重建的地图都有更多的热点占比，这说明本文构建的稠密点云地图与真实模型的重合度更高，是对真实场景更加准确的重建结果。

图 7  背景重建质量的定量分析效果图

下载: 全尺寸图片 幻灯片

3.3.2   定性实验

为了验证本文方法在动态场景下的建图可靠性，本文在TUM的fr3-W-xyz数据集上完成定性实验。图8展示了本文重建的3维点云地图与现有的针对动态场景建图方法的对比。其中图8(a)是在ORB-SLAM2的基础上加入了本文提出的地图构建方案，但是由于ORB-SLAM2算法本身无法处理动态目标，所以其构建的地图完全被人占据，效果最差；而StaticFusion^[29]专注于动态场景下的场景重建任务，虽然其建图速度很快，但由于没有考虑语义信息，最终仍然将坐在椅子上的人融入了地图之中，并且忽略了很多深度值较大的背景，丢失了很多信息；而本文重建的地图如图8(c)所示，不仅去除了动态目标带来的影响，还将其遮挡的部分通过相邻帧进行补全，并且提供了更多的细节，得到了最好的效果。

图 8  背景重建效果对比图

下载: 全尺寸图片 幻灯片

接着图9展示了本文在TUM的fr3-W-xyz数据集上生成多层次地图的最终结果，可以看出3张地图都没有受到数据集中大幅度走动的人的影响，并且都携带了一致的语义信息，其中绿色代表显示器，黄色为键盘，粉色为鼠标。而生成的地图3作为物体级语义地图不仅可以为避障和抓取等高级任务提供有效帮助，还节省了更多的存储资源。

图 9  fr3-W-xyz数据集上生成的多层次地图

下载: 全尺寸图片 幻灯片

3.4   微调网络效果

通过将地图2投影至2维图像，本文在多个TUM数据集下生成了有关显示屏、椅子、盆栽、水瓶和鼠标这5个类的共1000张训练图像，如图10所示，包括旋转、遮挡、模糊等多种复杂场景。

图 10  生成复杂样本的真值

下载: 全尺寸图片 幻灯片

为了验证微调目标检测网络的可行性，本文首先分别用VOC数据集和COCO数据集训练YOLOv3网络，接着对获得的1000张训练图片用地图2反投影后得到的信息进行标注，其中800张图片与原始两个数据集中随机挑选的4200张图片，共5000张图片组成新的训练集重新训练网络，剩余的200张与原始的两个数据集中随机挑选的800张图片，共1000张图片构成验证集来计算网络关于5个类的准确率和召回率，结果如表2所示，其中准确率为平均准确率，而召回率指最高召回率。可以明显看出相较于用原始数据集训练的网络，微调后的网络在准确率和召回率上都至少有10%的提升，这可以理解为微调后的网络更加适应模糊、高亮等复杂情况，对旋转和遮挡的内容也有了更好的理解和判断。同时从表中可以看出，对于那些原本相对来说更容易检测出的目标比如显示屏、鼠标等，加入新样本后再次训练的提升并不明显；而对于那些容易与其他物体混淆且易被遮挡的目标比如盆栽和水瓶等，加入新样本再次训练后，准确率和召回率都有了较大的提升，这样的提升在实际应用中是更为重要的。本文相信利用微调后的网络对输入图像进行目标检测又可以进一步提升本文提出的SLAM系统的定位精度和建图质量，从而达到一个良性循环。

表 2  不同数据训练YOLOv3后准确率和召回率的对比

类别	准确率/召回率
	显示屏	椅子	盆栽	水瓶	鼠标
VOC	61.7/63.8	56.5/60.7	35.6/51.7	–	–
VOC+本文样本	71.2/76.1	68.6/73.2	54.1/62.9	–	–
COCO	65.3/66.4	49.9/52.3	27.7/36.8	23.6/34.5	69.9/71.1
COCO+本文样本	72.4/75.8	61.2/63.9	53.1/66.2	51.5/60.8	77.4/78.9

下载: 导出CSV 

| 显示表格

4.   结论

本文提出了一个针对动态场景的语义SLAM系统，不仅可以解决动态目标影响定位精度的问题，还可以为动态场景重建多层次的3维语义地图。通过将目标检测网络与光流约束结合，并利用动态物体与静态物体在空间中的位置关系，从而将动态特征点完全剔除。引入的基于超体素的快速点云分割方法又可以将3维语义地图进行优化，进一步生成物体级地图。同时重建的3维语义分割地图又可以投影至2维图像，生成复杂样本微调目标检测网络。实验表明无论在定位精度和建图质量上，本文的算法都展现了出色的效果，鲁棒性和稳定性也可见一斑；微调目标检测网络也有一定成效，可以有效提升网络的平均准确率。实验充分证明本文系统可以真正做到神经网络和SLAM的有机结合，相辅相成。