面向动态环境的机器人同步定位与建图技术

赖尚祥，杨忠，姜遇红，张弛，方千慧

1. 南京航空航天大学 自动化学院，江苏 南京 211106

2. 南京航空航天大学 无人机研究所，江苏 南京 211106

同步定位与建图技术(simultaneous localization and mapping，SLAM)是提高机器人自主性和智能性的关键技术。近年来，它一直是机器人领域研究的热点。SLAM技术仅通过机载传感器的数据流，联合评估未知环境的地图和机器人在地图中的姿态。该地图允许机器人在相同的环境中不断地定位而不积累漂移。由于视觉传感器具有成本低、数据量大等优点，视觉SLAM越来越受到人们的关注[1]。常用的视觉传感器有单目相机、双目相机、RGB-D相机等。单目相机方案在尺寸、功率和成本方面有实际优势，但也面临一些挑战，如尺度、初始化困难问题。通过使用更复杂的视觉传感器可以避免这些问题，如双目相机和RGB-D相机。现代视觉SLAM系统的框架由特征提取、状态估计、建图、回环检测等几个基本部分组成。SVO[2]、LSD-SLAM[3]、ORB-SLAM2[4]等SLAM算法已经取得了令人满意的视觉效果。然而绝大多数方法和数据集都采用静态环境。这类方法通过将动态内容分类为静态模型的异常值，从而处理一小部分动态内容。但是在人口稠密的现实环境中，如行人、动物，这些均是不可避免的内容，同时这些环境正是服务机器人或自动驾驶汽车等相关应用的目标。

因此有效地解决动态环境下的机器人同步建图技术显得尤为重要。考虑到上述算法的局限性，动态SLAM算法的主要挑战为：

1)如何检测动态目标；

2)如何防止跟踪算法使用属于动态对象的特征点。

1 相关工作

在典型的SLAM系统中，动态目标被视为噪声数据，因此它既不包含在地图中，也不用于跟踪摄像机。在ORB-SLAM[4-5]中最具代表性的是RANSAC算法。RANSAC算法假设存在一个模型，所有的局内点符合模型，局外点不符合模型。只要符合模型的局内点足够多，则模型便足够合理。整个过程采用反复迭代、优化模型、更新内点集的方式寻找最优模型。但是，当动态对象的比例较大，甚至覆盖了相机的主要视场时，该方法将失效。

Kundu等[6]提出一种基于几何方法的动态目标检测。通过构造机器人运动的基本矩阵，定义对极几何约束。如果匹配的特征点离外极线太远，则最可能被认为是动态的。这种方法的关键点在于对基本矩阵的估计，如果能获得相对可靠的基本矩阵，则可以很容易地检测到大部分动态特征。由于RGB序列中包含大量动态特征，则对这种方法的初始基础矩阵计算造成很大的影响，从而影响整个系统的精度。

随着深度学习技术的发展，图像中的高级语义信息逐渐被用来处理场景中的动态对象，以提高SLAM的性能。在SLAM系统中，利用YOLO[7]、SSD[8]、SegNet[9]、Mask R-CNN[10]等先进的CNN架构，获得每一帧特征的语义标签。根据人类的先验知识，如人、动物被认为是动态物体，附属于该类对象上的特征点将被剔除。现实环境中存在一类物体，如椅子，它在常规情况下属于静态的，因此在卷积神经网络(convolutional neural networks,CNN)中的标签也为静态。但与人发生接触后，其三维空间位置大概率会发生刚性变换，这对后续相机位姿估计精度造成一定的影响。

Cui等[11]采用光流法追踪经过CNN去动态的RBD图，从而获得一个较为准确的基础矩阵，进而进行运动一致性检测，去除潜在的动态物体。由于算法采用了光流法，势必对系统的实时性造成影响。

针对上述情况，提出一种动态SLAM系统，通过结合语义分割和几何分割来检测动态对象。采用Mask R-CNN去除先验动态物体，使用惯性模型实现帧间基础矩阵求解，根据对极约束进行运动一致性检测。最后，使用区域生长法标记动态对象。相对其他方法，本文方法提升了定位精度。

图1 系统跟踪线程框架图

2 系统跟踪线程框架

2.1 系统跟踪线程

图1为本系统的跟踪线程框图。首先，RGB序列经过一个语义分割网络，将所有先验的动态对象逐像素分割出来。其次，我们使用几何分割的方法来标记在Mask R-CNN阶段未被检测到的其他动态物体。为此，有必要估计两帧间的相机姿态变化。因此我们设计了一种基于惯性模型快速求解相对可靠的相机位姿方法。值得注意的是，CNN无法检测到的其他动态对象将通过几何分割进行检测。剩余的静态特征被保留，以便用于后续跟踪线程新帧的姿态估计。动态物体去除模块流程如图2所示。

图2 动态物体去除模块流程

2.2 语义分割先验动态物体

为了检测动态对象，本文采用了一种通用的实例分割结构Mask R-CNN。Mask R-CNN可以同时获得实例标签和像素级语义分割。

根据掩码的标签，可以获得一些动态对象的先验信息，如人、猫、狗等。如果对象被标记为“人”，那么我们假设落在该对象上的所有像素都是动态的可能性非常大，因为一个人在常规情况下倾向于在移动。同样地，如果像素的标签是“桌子”，我们将假设这些像素所在的物体是静态的可能性非常大。如果像素的标签是“椅子”，这种情况就不同了，因为在人的影响下，椅子是可以移动的。所以我们需要使用其他方法来检测这些潜在的动态对象。

有了先验知识，我们可以将物体上的像素大致分为3类：动态的、潜在的动态的、静态的。为了减少计算量，本文只考虑特征点，而不考虑像素点。这样，落在动态对象上的特征点将被移除，而静态特征点将被保留。如果将潜在的动态点简单地看作动态点或静态点，将对SLAM系统的定位精度产生不可逆转的影响。因此，本文采用了一种将语义分割和几何分割相结合的动态目标检测方法。

2.3 惯性模型

几何分割根据相机姿态进一步筛选特征点中的动态点。因此，需要一种快速准确的姿态估计方法。本文采用惯性模型对图像中的静态点进行跟踪，这些图像的静态点由语义分割去动态之后获得。由于帧率较高，假设帧之间的速度不变，因而相机将以上一帧的速度移动。相机姿态变换矩阵应为

式中：Tcur、Tlast表示当前帧与上一帧的姿态变换矩阵；Min表示惯性系数，当上一帧跟踪失败时，该值等于0，否则为1。

将地图上的特征点投影到图像帧中，在投影点附近搜索相应的匹配点。如果搜索到的匹配点对数量不够，则增大搜索范围。如果依然搜索不到足够数量的匹配点对，则认为模型失效。若模型失效，将对上一个关键帧采用惯性模型求解，或者进行重定位。

在模型成功的情况下，将观测方程抽象记为

式中：x代指此刻相机位姿，即相机变换矩阵T；y代指路标点，即三维特征点P；z为观测值。获得匹配特征点对与初始位姿可以得到此次观测误差为

式中：err 为误差项，z为观测数据z≅[us,vs]。将具有共视关系的几帧观测量加入方程，设zij为在位姿Ti处观察点Pj产生的数据，则整体的代价函数为

为求得投影误差最小时对应的待优化变量Ti，采用图优化[12]的方法，从初始估计值开始，寻找目标函数下降方法，对Ti进行修正，找到最优解。此时Pj不参与被优化，以减少计算量。

惯性模型求解相机位姿使用的特征点包含潜在动态物体，如与人发生互动的椅子、书本等物体。这些潜在动态物体对应的特征点三维空间坐标发生了变化，相关的投影误差项失效。因此需要通过进一步使用几何分割检测动态点。

2.4 几何分割

对于一对匹配好的静态特征点p1、p2，两点满足极线约束，即E=t^R。二者在空间中的关系如图3所示。

图3 多视图几何中特征的对极约束

I1、I2分别代表2帧图像；p1、p2是一对匹配好的 点，2个 相 机 中 心 分 别 为O1、O2；连 线在三维空间中相交于P。 这时O1O2P三点可以确定一个平面，称为极平面。O1O2连线与像平面I1、I2的交点分别为e1、e2。e1、e2称为极点，O1O2被称为基线。

极平面O1O2P与I1、I2两帧的交线l1、l2称为极线。极线l2可以视为在帧I2上的投影线。在三维点P(P0,P00)不确定深度的情况下，可以知道它在I2上的投影必定在极线l2上。这种对极几何约束实际上描述了从一帧中的一个点到另一帧中相应的极线的投影。投影关系可以用基本矩阵F表示：

式中：E为 本质矩阵，t为平移向量，R为旋转矩阵，K为相机内参矩阵。

由于特征提取和基本矩阵F估计中存在不可避免的误差，实际上特征点p2并没有完全落在极线l2上，如图2中p¯2所示。此时，由匹配点对可知坐标：P点 三维坐标：

根据RGB-D相机测量的特征点深度值，恢复

P=dK−1p1

其中d为该点深度值，P点在I2上的投影为

p2=RP+t

其中R、t由基础矩阵F分解得到。位于I2上的实际匹配点为则误差项定义为

通过运动一致性检测，可以知道哪些特征点落在动态对象上。为了避免遗漏动态对象上的特征点，我们通过区域生长获得几何分割掩码。区域生长算法的基本思想是将具有相似属性的像素合并在一起。RGB图包含过多的冗余信息，而深度图上一个对象的像素分布始终是连贯的，因此本文选择在深度图上采用区域生长算法[13]。

然而，一些位于动态对象边界上的特征点可能会引起问题。因为掩码的边缘会大致覆盖整个对象，边缘特性不是很明显，而边缘又属于容易被检测出特征点的区域。为了弥补该缺点，本文对每个掩码进行了形态计算以优化掩模的边界。

3 实验分析

本文系统在公共TUM RGB-D数据集中进行了评估，并与其他动态SLAM系统进行了比较。首先，将本文系统与原始的ORB-SLAM2系统进行比较，该系统被认为是相对更出色且稳定的SLAM系统之一。所有实验均在Intel i5-9400F、Nvidia GTX-1660、16 GB RAM的计算机上执行。

TUM RGB-D数据集[14]包含多个动态环境序列。包含深度图像和RGB图像，以及由高精度运动捕捉系统记录的地面真实轨迹。由于序列中移动的人可能会占据图像的很大一部分，因此序列中的动态对象给SLAM系统的定位精度带来了极大的挑战。我们将本系统与ORB-SLAM2进行了比较，以说明此动态SLAM系统的作用。

根据语义分割以及几何分割去除动态特征点，效果如图4所示。图4(a)、(c)、(e)为本系统结果，图4(b)、(d)、(f)为ORB-SLAM2的结果。图中红色框为先验动态物体，蓝色框为潜在动态物体。图4(a)、(c)、(e)中红色框内由Mask R-CNN获得相关掩码并去除对应的特征点，蓝色框内由几何分割检测得到的动态物体并去除相应的特征点。由图可知，动态物体对应的特征点均被有效剔除，从而减少其对系统的影响。

图4 动态环境下本系统与ORB-SLAM2特征点检测结果比较

在SLAM系统中，误匹配点对，动态点对参与图优化的整个过程。由于他们的数据是错误的，即误差项很大，意味着算法将试图调整这条边所连接的所有节点的估计值，以缓和错点带来的大误差项。这将导致优化方向偏离真实方向，因此一般采用鲁棒核函数削减这些点的影响：

当误差项e大于阈值 δ时，函数由二次增长变为一次形式，限制了梯度的最大值。当SLAM系统面对一个高动态环境时，动态特征点对成为优化函数中具有分量的一类数据，则鲁棒核函数收效甚微。包含了大量动态点的误差项整体数值偏大，算法优化方向偏向于错点，导致估计位姿偏离真值，对跟踪效果造成不可逆的影响。因此去除动态点对能够使得优化函数准确表达真实三维场景约束。由图5、6可看出，没有进行动态点提出的ORBSLAM2系统轨迹与真实轨迹大相径庭，而本文提出的算法很好地对相机位姿进行了跟踪。

图5 本系统与ORBSLAM2轨迹误差对比

图6 本系统与ORBSLAM2平移误差对比

本实验采用绝对平移误差(absolute trajectory error, ATE)定量评估系统轨迹的整体一致性。绝对平移误差是估计位姿和真实位姿的直接差值，可以非常直观地反应算法精度和轨迹全局一致性。第i帧的ATE定义为

式中：Gi为相机位姿真值，Fi为估计值，S为相机估计位姿坐标系与真值坐标系的转换矩阵，由最小二乘法估计得到。通常使用均方根误差(root mean squared error, RMSE)统计前n帧的ATE值[14]：

为获得高度可信的实验数据，对每个序列运行5次，分别获得均方根误差、误差标准差(standard deviation, SD)、误差中位数(median)、误差平均值(mean)，取5次数据的中位数作为实验结果。

序列名称中，第一个字母表示序列中人的状态，“s”表示人的状态为静坐(弱动态场景)，“w”表示人的状态为移动(强动态场景)。后续字母表示相机运动方式。该系统的改进值通过式（1）计算：

式中：η表示提升值，εorb表示ORB-SLAM2的误差值，εours表示本系统的误差值。

由表1可以看出，与ORB-SLAM2相比，本系统在准确性上有了很大的提高，并且几乎所有结果都提高了一个数量级。在“w.half”序列中，RMSE、SD、平均误差、中值误差的准确度分别提高了95.59%、92.55%、95.80%和96.18%。在其他序列中也会发生类似情况。这是因为语义分割与几何分割相结合有助于去除动态特征，从而大大提高系统的准确性。

值得注意的是，在弱动态序列“s.half”中，系统改进并不明显。这是因为人在这些序列中的位置总是非常靠前。在本文方法中，即使它们没有移动，它们也会被标记为动态的。在这种情况下，ORB特征点在图像中的分布将不均匀，这将影响系统姿态确定的准确性。并且，原始的ORBSLAM2系统可以通过非线性优化来处理一些低动态情况。但是，这不会影响该系统的优越性，它的整体准确性仍然得到提高。

表1 本系统与ORB-SLAM2系统绝对轨迹误差的RMSE[m]对比结果

为体现本文算法优越性，将其与近代较为先进的动态环境RGB-D SLAM系统DS-SLAM[15]、Detect-SLAM[16]进行对比。表2的实验结果表明，与这些系统相比，本文方法在动态序列定位方面具有更好的性能。

表2 本系统与先进动态SLAM的RMSE[m]对比

4 结束语

本文提出了一种基于RGB-D传感器的方法，该方法能够一致地映射包含多个动态元素的场景。通过结合语义分割和几何分割来检测动态对象。跟踪序列以获得相机姿势，其中先验的动态对象由Mask R-CNN移除。移动一致性检查用于检测其他动态特征点。最后，使用区域生长法标记动态对象。实验结果表明，该方法减少了运动物体的影响。在动态序列中，本文系统在定位精度方面比原始的ORB-SLAM2有了很大的提高。此外，在动态场景中与其他最新动态SLAM系统的比较表明，本文系统在大多数情况下可以达到最高的精度。

本系统依然存在需要完善的工作，未来的工作包括：1)使用其他对象的语义标签，这将会在地图中提供更丰富的直观信息。2)使用系统计算得到的信息建立一个更加直观的语义稠密地图。