基于单目视觉的动态环境同步定位与多地图构建算法

齐咏生 陈培亮 刘利强 董朝轶

(1.内蒙古工业大学电力学院,呼和浩特 010051;2.内蒙古机电控制重点实验室,呼和浩特 010051)

0 引言

随着人工智能和大数据等技术的不断发展，智能机器人技术将提升到一个新的阶段，智能机器人的自主定位已成为机器人研究领域的热点问题[1]。智能机器人涉及控制理论、人工智能技术、多传感器数据融合技术、电子信息与计算机技术等多学科交叉，在很多领域得到了广泛应用[2]。同步定位与建图(SLAM)技术是一种为解决机器人在位置环境中自主定位问题而提出的，也是移动机器人的一项基本能力。移动机器人在未知环境中，利用板载传感器尝试构建周围环境地图，并利用这幅地图来确定自身的位置[3]。

在视觉SLAM算法中所用的相机主要有：单目相机、双目相机和RGB-D相机。而单目相机具有价格低廉、功耗低、体积小、安装方便、获取信息量大、适用于大规模场景等优点，因此基于单目相机的单目SLAM(V-SLAM)算法被广泛应用。

迄今为止，已有一些比较经典的V-SLAM算法问世，如：MonoSLAM[4]、PTAM[5]、ORB-SLAM[6]、DSO[7]、SVO[8]等。其中ORB-SLAM是目前众多V-SLAM算法中较为完善的一种开源算法，但其本身仍然存在一些问题，如：在纹理不明显的场景无法初始化且容易跟丢，跟丢之后到重定位之前都不能建图，丢失了这段地图信息，如果重定位失败则无法建图；ORB-SLAM的建图是基于场景的刚性不变的前提下完成的，因此在动态环境下往往无法正常跟踪和建图。对于V-SLAM算法跟丢后无法建图的问题，有些学者曾经提出建立多个地图的想法来解决此问题，但效果不理想。

由于V-SLAM算法能够在动态环境下正常工作的需求日益增长，近几年研究人员提出了一些能够工作于动态环境下的V-SLAM算法。如2017年LI等[9]提出一种动态环境下的V-SLAM算法，该算法测量边缘点的深度，这些点有其对应的权重，这些权重表示对应的点属于动态物体的概率，而这些概率是通过地图上所有边缘的投影点之间的欧氏距离来计算。SUN等[10]在2016年也提出一种基于直接法的动态环境V-SLAM算法，该算法计算了连续RGB图像之间的强度差异，对量化后的深度图像进行了像素分类，以此来实现对环境中的动态物体进行检测。但是上述两种V-SLAM算法都只能检测正在移动的物体，不能检测出不属于静态场景，但暂时未运动的物体(如：人、汽车、狗等)。

经过前期研究和深入分析发现，目前所研究的V-SLAM算法主要存在2个缺陷：①V-SLAM算法在跟丢后无法继续建图(即类似于机器人的“绑架劫持”)。②无法在动态环境下建立环境真实地图。针对缺陷，本文引入一种多地图构建思想。传统算法中(如ORB-SLAM2)重定位是一个贪婪搜索过程，它要用当前帧与之前的所有关键帧进行匹配，费时费力且容易陷入死循环。为此本文提出采用局部多地图构建代替传统算法中的重定位环节，即当发生算法跟丢时，直接建立新的局部地图，而不是进入重定位模式。同时引入基于区域卷积神经网络的深度学习与多视图几何相结合的动态物体检测机制，利用两种方法各有优势，解决不同目标情况。深度学习对于先验动态目标有非常好的检测精度，但对于一些偶然性或未学习动态目标检测率较低。而多视图几何技术刚好可弥补这个缺失，由于它是利用空间几何尺度进行位姿计算，对正在移动的目标比较敏感。不过，多视图几何技术也有缺陷，对缓变或暂时静止目标无法检测，如一个暂时不动的动态目标人，多视图几何技术几乎不能检测出它是动态目标。最后，针对地图构建中背景修复问题，本文提出一种基于时间加权滤波的多帧融合算法，对动态物体遮挡部分的地图进行背景修复。

综上，本文提出一种基于单目视觉的动态环境同步定位与多地图构建(Dynamic environment simultaneous location and multi mapping,DE-SLAMM)算法。该算法分为5个线程，包含跟踪、局部建图、回环检测、多地图构建和动态目标检测与背景修复的完整过程。最后采用实际场景对算法的有效性进行验证，并与3种经典的V-SLAM算法进行比较。

1 理论基础

1.1 ORB-SLAM2算法

ORB-SLAM2算法[11]建立在ORB-SLAM算法的基础上，是当前主流的V-SLAM算法之一。该算法主要包含3个线程：跟踪、建图、回环检测。跟踪线程负责从图像中提取ORB特征，根据上一帧进行姿态估计，或者通过全局重定位初始化位姿，然后跟踪已经构建的局部地图，优化位姿，再根据一些规则确定新的关键帧。建图线程主要完成局部地图构建，包括对关键帧的插入，验证最近生成的地图点并进行筛选，然后生成新的地图点，再使用局部捆绑调整(Local BA)进行优化，最后再对插入的关键帧进行筛选，去除多余的关键帧。回环检测线程主要分为闭环探测和闭环校正。闭环探测先使用词袋(BOW)进行探测，然后通过Sim3算法计算相似变换；闭环校正主要是闭环融合和对本质图(Essential graph)的图优化。

分析可知，ORB-SLAM2算法常常存在算法跟丢后无法继续跟踪和建图，以及无法在动态环境下建图的问题。因为ORB-SLAM2算法在跟丢后会直接进入重定位模式，并在重定位模式中进行无限的重定位直到回到之前重定位的位置才能跳出，然而很多情况达不到跳出条件而陷入长循环，导致算法失效。此外ORB-SLAM2无法在动态环境下建立真实地图，是因为它缺乏对动态目标的实时有效检测，导致地图失效。为此，本文在ORB-SLAM2算法基础上，对其原有3个线程进行改进，并增加了2个新的线程，提出了DE-SLAMM算法。

1.2 多视图几何技术

多视图几何[12](Multiple view geometry)主要研究用几何的方法，通过若干幅二维图像，来恢复物体在三维空间的位置。

多视图几何技术有2种测量方法：

(1)用对极几何计算相机的位姿，如图1所示。

图1 对极几何测量方法示意图Fig.1 Diagram of polar geometry measurement method

计算公式为

E=R

(1)

F=(K-1)TEK-1

(2)

式中R——当前帧相对于前一帧图像的旋转矩阵

K——相机内参矩阵

E——本质矩阵

F——基本矩阵

之后利用八点法对式(2)求解，即可求出相机相对于前一帧图像的位姿。

(2)用三角测量方法计算空间点的深度s1、s2，如图2所示。

图2 三角测量方法示意图Fig.2 Schematic of triangulation method

设x1、x2为点p1、p2在图像中的坐标，则

s2x2=s1Rx1+t

(3)

式中t——前一帧到当前帧的平移向量

又因为

s22x2=0

(4)

则

s12Rx1+2t=0

(5)

最后通过式(4)和式(5)求出点p1、p2的深度s1、s2。

1.3 Mask R-CNN神经网络框架

Mask R-CNN[13]是一种简单、灵活、通用的目标实例分割框架，它能够有效检测图像中的对象，同时生成每个先验实例的高质量分割掩膜，其实例分割的实现是在Faster R-CNN的基础上加了一个可以预测目标掩膜(Mask)的分支，它很容易拓展到其他任务，如：关键点的检测，该算法在COCO数据集中的目标检测、实例分割、人体关键点检测都取得了较好的效果。

由于Mask R-CNN能够有效检测图像中的对象且能够对先验实例对象进行分割，因此这种实例分割算法可用于V-SLAM中，用来提取环境中的动态实例对象，从而达到对环境中先验动态对象检测的目的。

2 DE-SLAMM算法

DE-SLAMM算法主要由跟踪、局部建图、回环检测、多地图构建、动态目标检测与背景修复5部分组成。

2.1 DE-SLAMM算法架构

DE-SLAMM算法基本框架如图3所示。跟踪、局部建图和回环检测线程是以ORB-SLAM2算法为基础，本文算法对其进行了一定的改进。首先，考虑到大部分环境都是特征比较明显的场景，因此，本算法对跟踪线程中的初始化部分进行修正，去除了对单应性矩阵的计算，在不影响算法性能的基础上提升算法初始化的速度；其次，在局部建图线程中增加任务转移功能，该功能由跟踪线程发起，实现将建图任务向新地图的转移；最后，在回环检测线程中增加任务转移功能和将局部关键帧数据库添加到全局关键帧数据库中的功能。

图3 DE-SLAMM框架示意图Fig.3 Schematic of DE-SLAMM framework

当算法跟丢后，跟踪线程会建立一个新的局部地图插入到多地图构建线程中。并且在回环检测时，该线程将插入的新地图与之前的地图进行融合。

动态检测与背景修复线程实现对动态环境目标进行自动检测和背景修复的功能。该线程利用Mask R-CNN对单目相机获取的图像进行像素语义分割即可分割出先验动态对象；再利用多视图几何技术检测出正在移动的动态对象，之后在建图中去除动态对象，实现动态环境建图。在动态检测线程中被剔除的动态对象在地图中显示为空白部分，为此提出一种时间加权滤波的多帧融合背景修复算法，对空白部分的静态地图进行修复。

2.2 跟踪、局部建图和回环检测线程改进

2.2.1跟踪线程改进

跟踪线程的任务主要是对新地图进行初始化和对环境进行跟踪。本文算法跟踪线程中的初始化部分只要计算非平面场景的基本矩阵，不需要计算平面场景的单应性矩阵。由于一般场景都是特征较为明显的场景，而且本文算法包含多地图构建线程，无需考虑算法跟丢之后无法继续建图的问题，因此，去掉单应性矩阵计算，整体算法性能并不受影响，且可以显著加快初始化速度。跟踪线程初始化方法如下：

计算场景的基本矩阵Fcr为

(6)

式中xc——当前帧的ORB特征点

xr——参考帧的ORB特征点

再由相机内参矩阵K和基本矩阵Fcr计算本质矩阵Erc为

Erc=KTFcrK

(7)

之后，对本质矩阵进行分解得到4个位姿，再通过三角测量的方法来验证这4个位姿以获取相机正确的位姿，即可完成地图的初始化，并建立初始地图。

在初始化工作完成之后，先以1.2倍的比例因子提取8个金字塔级别的加速分段测试特征(Features from accelerated segment test,FAST)角点，保留角点的数量取决于图像尺寸和图像内角点的分布，然后再计算ORB描述符，检测是否初始化，如果未初始化则重新初始化，若已完成初始化，则用运动模型来计算当前位姿继续跟踪局部地图中的所有点，计算公式为

T′cw=MTcw

(8)

其中

式中T′cw——相机当前帧位姿

ΔR——旋转矩阵变化量

Δt——平移向量变化量

Tcw——相机上一帧位姿

若上一帧的地图点与当前帧地图点匹配点数小于等于设定的阈值时，就会将匹配模块放在更宽的范围内匹配；若匹配点数大于阈值，则优化当前位姿，丢弃离群点。

本文算法中，若跟踪成功，则算法会检查是否需要一个新的关键帧，再通知局部建图线程处理该关键帧，并将其加入到地图中。若跟踪失败，则跟踪线程会直接重新初始化建立一个新的局部地图，再将新的局部地图及其关键帧数据库加入到多地图构建线程中，并通知局部建图线程和回环检测线程将建图任务和回环检测任务转移到新的局部地图中进行，而不是进入重定位模式进行重定位。

2.2.2局部建图线程改进

局部建图线程在地图初始化完成后，首先插入第一个关键帧作为参考关键帧(Key frames,KFS)KFSr，且将此时相机位置设为世界坐标的坐标原点Ow，根据初始化中计算的特征点的深度信息计算出每个特征点在世界坐标中的位置，最后创建初始地图的地图点，如图4所示。世界坐标系计算公式为

图4 地图点计算示意图Fig.4 Map point calculation schematic

Pw=sT-1K-1p1

(9)

式中Pw——世界坐标

s——特征点的深度

T——相机位姿，在参考帧中相机位姿为1(相机坐标系与世界坐标系重合)

另外，局部建图线程还会删除初始地图中一部分可见性低或不可跟踪的地图点，最后对局部地图进行局部捆绑调整(BA)优化。之后不断地接受新关键帧，利用式(9)计算地图点位置并建图。

本文算法中，当跟踪失败后，跟踪线程创建一个新的局部地图，局部建图线程接收到跟踪线程发出的任务转移消息后，将建图任务转移到新的局部地图中，重复上述建图过程。

2.2.3回环检测线程改进

回环检测线程在接收到一个关键帧时，先将其转换为视觉词汇，描述如下：

设不同类型特征点的视觉词汇分别为w1、w2、…、wn，再用一组向量表示一个关键帧

A′=a1w1+a2w2+…+anwn

(10)

A=(a1,a2,…,an)

(11)

式中A——接收到的关键帧的视觉词汇向量

A′——接收到的关键帧的视觉词汇

a1、a2、…、an——每一种特征点在关键帧中出现的次数

将一个关键帧转换成了一个视觉词汇向量。再将视觉词汇插入到局部关键帧数据库中，并计算当前帧的视觉词汇与局部关键帧数据库中的其他关键帧的视觉词汇的相似性评分S为

(12)

式中n——视觉词汇数

ai——第i种特征点在关键帧数据库中出现次数

bi——第i种特征点在局部关键帧数据库中出现次数

由式(12)可知，S越小表示相似度越高，因此需要找到S小于设定阈值Smin(本文中Smin为0.05)且与当前关键帧没有直接链接的关键帧，将这些关键帧作为候选帧，再计算每个候选帧的相似变换来确定回环关键帧，若存在回环关键帧，则将回环的地图点进行融合，并对相机的位姿进行修正，否则继续在局部地图中做回环检测。

在本文算法中，当跟踪失败后，跟踪线程创建1个新的局部地图，回环检测线程接收到跟踪线程发出的任务转移消息后，将回环检测的任务转移到新的局部地图中，将局部关键帧数据库添加到全局关键帧数据库中，再重复上述回环检测过程。

2.3 多地图构建线程

传统的V-SLAM算法在跟丢之后就会进入重定位模式，由于重定位模式本质上是一种贪婪搜索过程，需要进行无限的重定位直到回到之前重定位的位置，若无法回到该位置，则会导致算法失效。为解决该问题，本文引入了多地图构建思想，并设计 了一种多地图构建线程，算法示意图如图5所示。

图5 多地图构建线程算法示意图Fig.5 Schematic of multi-map builder thread algorithm

多地图构建线程主要功能是将基于局部建图线程建立的多个局部地图及其关键帧数据库保存下来，同时检测正在跟踪的当前局部地图与保存的全局地图是否有回环现象，若有回环则进行地图融合，并同时优化相机位姿。多地图构建线程具体流程如下：

(1)当算法开始运行后，跟踪线程会创建第1个地图M0及其局部关键帧数据库KFS0，并将其传入到多地图构建线程中，再由跟踪线程、局部建图线程和回环检测线程在地图M0中进行跟踪和建图，只要跟踪线程没有跟丢，多地图构建线程就处于空闲状态。

(2)到第n次跟踪线程跟丢后，跟踪线程将创建一个新的地图Mn及其关键帧数据库KFSn，并将其传递给多地图构建线程中的全局地图M和全局关键帧数据库。

(3)跟踪线程尝试重新初始化，一旦初始化成功，就会通知各线程将跟踪和建图的任务转移到局部地图Mn中运行，此时多地图构建线程会扫描新地图的局部关键帧数据库KFSn中存储的关键帧，并与之前的全局关键帧数据库中存储的关键帧进行匹配。

(4)匹配方法与回环检测过程类似，多地图构建线程遍历之前所有的局部关键帧数据库(KFS0～KFS(n-1))，并计算之前所有关键帧与当前关键帧之间的最小相似性评分来查询之前的地图中是否有与当前关键帧Kc匹配的关键帧。

(5)对于每一个局部地图Mi(i=1,2,…,n)中的关键帧Kj，若它与Kc有超过15个匹配点，则让求解器来计算它们之间的相似变换，或者对每个Kj执行随机采样一致性(RANSC)迭代，直到找到具有足够匹配点的Kj，或者所有的候选帧都失败。若RANSC迭代后能够返回一个相似变换，则可对这个相似变换进行优化，若优化之后仍有足够的匹配点，则Kj被认为是一个回环关键帧。而所有在Kj及其相邻帧中看到的地图点都会在Kc被检测和重投影，再利用计算出的相似变换搜索更多的匹配帧。若所有的匹配帧对应的点数超过阈值，则认为此处为回环。

(6)多地图构建线程检测到回环的地图与当前地图进行地图融合，并对相机位姿进行优化，最后生成新的全局地图M′。地图融合的方法如下：多地图构建线程先计算出Kc与Kj的相似变换矩阵Scw，再利用Scw将局部地图Mn与产生回环的地图Mi通过回环融合(同回环检测中的回环融合)连接在一起，若这两个地图是第1次融合，则检索Mn的所有关键帧和地图点，否则就只检索Kc的相邻帧及其地图点，之后再将Kc的位姿设置为Scw进行校正，并将检测到的每个关键帧的位姿转换为地图Mi坐标系下的坐标

Tic=TiwTwc

(13)

Tcorr=TicTwc

(14)

式中Tiw——校正前检索到的关键帧位姿

Twc——校正前Kc的逆位姿

Tcorr——在Mi坐标系中对检索到的关键帧进行校正后的位姿

综上，每个检索到的关键帧及其相邻帧的地图点都被校正为地图Mi坐标系下的坐标，之后将Kj及其相邻帧的地图点投影到Kc及其相邻帧中，这样就完成了地图之间的融合。

2.4 动态目标检测与背景修复线程

传统的V-SLAM算法是基于刚性环境设计的，不适用于实际的动态环境。为此，本文利用Mask R-CNN神经网络框架与多视图几何技术互补结合的方法，实现了一种动态目标实时检测算法来解决动态环境跟踪问题，算法的基本框架如图6所示。

图6 动态检测与背景修复线程算法示意图Fig.6 Schematic of dynamic detection and background repair thread algorithm

如图6所示，动态检测与背景修复线程主要包括：Mask R-CNN神经网络模块、低成本跟踪模块、多视图几何模块和背景修复模块。

Mask R-CNN神经网络框架是一种对象实例分割算法，它既可以获得像素级语义分割图像，也可以获取实例标签，而本文只使用Mask R-CNN获取的像素级语义分割图像，并将分割图像传给低成本跟踪模块。即先利用Pycharm平台的TensorFlow训练出Mask R-CNN需要获取的动态对象的模型(如：人、牛、马、羊、自行车、汽车和公交车等)，然后再将此模型放入本文算法文件夹中，应用此模型即可检测到环境中的相应动态对象(先验对象)。

显然，先验对象不可能涵盖所有，一定存在一些动态对象不能被Mask R-CNN算法分割出来，例如一个人拿着一个椅子在走动，人可以被分割出来，但是椅子却不能，但椅子也属于动态物体却未被分割。为此，引入多视图几何技术，前人的研究已表明，该技术能够很好地实现移动对象的检测[14]。因此，将其与Mask R-CNN算法相结合，可实现动态目标检测的优势互补。

低成本跟踪模块是对跟踪线程进行简化后得到的一个模块，采用低成本的跟踪模块将相机定位在已创建的场景地图中，并为检测潜在的动态对象做铺垫(如上述中的椅子)。首先，获取Mask R-CNN处理后的图像帧，将相机定位到已建立的地图场景中；其次从该地图场景将局部建图线程生成的地图点重投影到分割的图像帧中，在图像帧中搜索特征点，保留静态区域特征点，删除动态区域的特征点；最后，将该图像帧传入到多视图几何模块。

利用多视图几何模块对正在移动的目标进行检测。具体步骤如下：

(1)输入帧要选择重叠度最高的关键帧，为了充分考虑新帧与每个关键帧之间的距离和旋转要求，将重叠关键帧的数量设置一个阈值(本文中设置为5)，这个阈值表征了计算损失和检测动态对象准确性之间的权衡，其目标是在保证检测到的动态对象准确的前提下，最大限度的降低计算带来的损失，若阈值过大会使地图更容易跟丢，而阈值太小则会使计算量过大，从而降低算法的执行效率。

(2)利用三角测量方法计算上一帧的像素点x投影到当前帧像素点x′的深度zproj，计算公式为

zproj′x′=0

(15)

计算x′和′的反向投影之间的视差角[8]α为

α=arccos〈n·n′〉

(16)

〈·〉——向量积运算符号

判断α是否大于设定阈值β(本例中取30°)。阈值β若偏大则检测不到微弱运动的物体，阈值β若偏小则可能会将不动的物体检测为运动物体。若大于β，则认为该关键点可能被遮挡，之后忽略此关键点。但有时静态点也有可能会大于β，因此，需要增加一个限制条件，即先获取当前帧像素点x′的深度z′，再计算其重投影误差zproj，最后比较z′与zproj的差值Δz，计算式为

Δz=zproj-z′

(17)

若Δz超过阈值τz，则认为像素点x′属于动态对象，否则为静态。因此，只要满足Δz>τz，则认为像素点为动态点，后续就忽略对应的动态点。图7是静态对象与动态对象的检测示意图。

图7 静态对象与动态对象检测示意图Fig.7 Static object and dynamic object detection schematics

(3)在正确判断动态对象之后，将动态对象中包含的特征点去除，生成一个分割帧，将分割帧传递给跟踪线程进行跟踪。

(4)由于动态目标被动态检测线程分割出来，局部建图线程建图时就不会对动态物体建图，因此静态地图中出现动态部分的空缺，需要对动态空缺部分进行修复。为此，本文提出了一种基于时间加权滤波的多帧融合算法来修复空缺部分的地图，如图8所示。

图8 背景修复示意图Fig.8 Background restoration diagram

从当前时刻，回溯前n个时刻的关键帧图像来进行背景修复，令不同时间的关键帧所占的融合权重不同，越靠近当前帧的关键帧权重越大，则有

(18)

式中KFSi——ti时刻的关键帧

KFSc——当前关键帧

KFS——修复关键帧

即可利用n个关键帧合成一个关键帧用于背景修复。不过，合成后的关键帧仍然有可能会有少量空缺部分未被修复，此时可对合成关键帧的像素进行平滑滤波来修复少量未被修复的部分。若像素位置第i1～i2行、第j1～j2列未被修复，则设置平滑滤波阈值k，进行平滑滤波，计算公式为

(19)

式中ui,j——修复前图像第i行第j列的像素值

u′i,j——修复后图像第i行第j列的像素值

即可实现图像修复。

3 实验验证

为验证本文算法的有效性，采用TUM RGB-D Benchmark公共数据集[14-19]的多个典型视频序列对算法进行测试,包括初始化及跟踪速度、算法跟丢后多地图构建、动态环境下检测效果和有效性问题等。最后将本文提出的DE-SLAMM算法与ORB-SLAM2算法[11]、ORBSLAMM算法[3]和DynaSLAM算法[20]进行了性能比较。实验平台的硬件环境为Intel Core i7-10750 CPU,2.6 GHz,x6 cores,16 GB RAM，实验结果表明该算法能以每个序列的帧速率实时运行。

此外，在实验前，算法会对先验对象进行训练，以先验对象“人”为例简要说明训练过程。首先，在COCO数据集中提取约1 000幅“人”的图像及其标签文件，利用Mask R-CNN神经网络框架训练出先验动态目标“人”的模型文件；之后，将此模型文件加入本文算法的文件夹中；最后，在实验运行阶段，程序会自动调用训练好的Mask R-CNN模型文件，检测环境中的先验动态目标。

3.1 初始化及跟踪速度性能测试

首先，本文利用TUM RGB-D Benchmark公共数据集中的fr1-xyz、fr1-desk、fr3-nostr-tex-far和fr3-nostr-tex-near-loop这4个视频序列对本文算法的初始化及跟踪速度进行测试，并与上述3种算法的初始化速度及跟踪时间进行了比较，测试结果如表1所示。

表1 DE-SLAMM算法与其他3种SLAM算法的初始化及跟踪速度比较Tab.1 Comparison of initialization and tracking speed between DE-SLAMM and other three SLAM algorithms

其中，fr1-xyz和fr1-desk视频序列所用相机内参矩阵为

fr3-nostr-tex-far和fr3-nostr-tex-near-loop视频序列所用的相机内参矩阵为

由表1可知，正常情况下在这几个序列中，ORB-SLAM2算法的初始化时间平均为11 s左右；DynaSLAM算法的初始化时间平均为16 s左右；ORBSLAMM算法的初始化时间平均为6 s左右；而本文算法的初始化时间平均为7 s左右。分析可知，在ORB-SLAM2和DynaSLAM算法初始化时，都需要计算一个单应性矩阵和一个基本矩阵，导致其初始化时间较长；而在ORBSLAMM和本文算法中，只需要计算一个基本矩阵，因此初始化速度明显加快。

在平均跟踪时间上，由于DynaSLAM算法跟踪线程的计算较复杂，而且还需要等待Mask R-CNN的处理结果，因此其跟踪速度较慢；DE-SLAMM算法和ORB-SLAM2算法跟踪速度近似，跟踪速度较快；而ORBSLAMM算法的跟踪线程最简单，因此其跟踪速度最快。

3.2 多地图构建性能测试

在测试DE-SLAMM多地图构建性能时，为了能够更好地展现出DE-SLAMM算法在跟丢后能够继续跟踪和建图，采用了两个比较典型的跟丢测试视频序列fr2-360-kidnap、fr1-floor进行实验，仍与上述3种算法进行了性能比较。

实验1使用fr2-360-kidnap视频序列，这个视频序列是在工厂里先让相机运动十几秒，再用物体将相机遮挡几秒，并移动到其他位置，让相机无法捕捉到被盖住这段时间的环境图像，3 s后再去掉遮挡物，让相机继续在环境中跟踪建图，以此来模拟机器人的“绑架劫持”问题。其中，fr2-360-kidnap视频序列所用的相机内参矩阵为

ORB-SLAM2算法的实验结果如图9所示，蓝色矩形框表示跟踪过程的关键帧，红色与黑色的点表示建立的地图点，ORB-SLAM2算法初始化后提取环境中的特征点开始跟踪和建图，当“绑架劫持”发生(被遮挡3 s)时，ORB-SLAM2算法进入重定位模式，但由于无法回到重定位位置，导致算法陷入重定位的死循环中，使后续无法继续跟踪和建图，即关键帧与地图点不再增加。

图9 ORB-SLAM2在fr2-360-kidnap序列中运行结果Fig.9 Processing results of ORB-SLAM2 in fr2-360-kidnap sequence

DynaSLAM算法的实验结果如图10所示，它和ORB-SLAM2算法类似，初始化后提取环境中的特征点开始跟踪和建图，当“绑架劫持”发生时，该算法也是进入重定位模式，无法回到重定位位置，导致后续无法继续跟踪和建图。

图10 DynaSLAM在fr2-360-kidnap序列中运行结果Fig.10 Processing results of DynaSLAM in fr2-360-kidnap sequence

ORBSLAMM算法的实验结果如图11所示，算法初始化后提取环境中的特征点开始跟踪和建图，建立出地图1后，“绑架劫持”发生，ORBSLAMM算法重新初始化提取环境中的特征点继续跟踪和建图，建立地图2，该算法在跟丢后可以在遇到“绑架劫持”问题后继续跟踪和建图，优于前两种算法，但这是2个独立的地图，无法融合为一个完整的全局地图，影响后续跟踪效率和定位精度。

图11 ORBSLAMM在fr2-360-kidnap序列中运行结果Fig.11 Processing results of ORBSLAMM in fr2-360-kidnap sequence

本文算法的实验结果如图12所示，算法初始化后开始跟踪和建图，建立第1个局部地图M1，在相机被“绑架劫持”后，本文算法能够重新初始化继续建立第2个局部地图M2，继续对当前环境进行跟踪和建图，并在检测到回环帧时，将地图M1和M2融合为一个整体地图，所以本文算法在遇到“绑架劫持”问题后继续跟踪和建图，优于ORB-SLAM2算法和DynaSLAM算法，还能将建立的两个局部地图融合为一个完整的全局地图，因此，在遇到“绑架劫持”问题时本文算法优于ORBSLAMM算法、ORB-SLAM2算法和DynaSLAM算法。

图12 DE-SLAMM在fr2-360-kidnap序列中运行结果Fig.12 Processing results of DE-SLAMM in fr2-360-kidnap sequence

实验2使用fr1-floor视频序列，该视频序列是相机在一个房间中快速运动，本实验主要测试算法在遇到跟丢问题时是否能够继续跟踪和建图。该视频序列所用相机内参矩阵为K1。图13给出4种算法的实验结果，由结果可知，得到了与实验1相似的结论，本文算法在跟丢之后能够重新初始化建立一个新地图继续跟踪和建图，在回环时能将2个局部地图融合为一个完整的全局地图，因此，本文算法在遇到跟丢问题时优于其他3种算法。

图13 不同算法在fr1-floor序列中运行结果Fig.13 Processing results of different algorithms in fr1-floor sequence

3.3 动态目标检测和背景修复性能测试

为了测试本文算法对动态目标检测和背景修复的性能，利用动态环境数据集中fr3-sitting-xyz、fr3-walking-xyz和fr3-walking-static这3个视频序列进行了实验，仍与上述3种算法进行了比较。上述3个视频序列所用相机内参矩阵均为K3。

实验1采用fr3-sitting-xyz视频序列，该视频是相机在X、Y、Z3个方向上移动，fr3-sitting-xyz视频序列中的人仅坐在椅子上运动，动作幅度较小，因此，fr3-sitting-xyz视频序列所属环境属于弱动态环境。

在弱动态环境中，人仅坐在椅子上，动作幅度较小，相机在X、Y、Z3个方向上移动，ORB-SLAM2算法和ORBSLAMM算法在fr3-sitting-xyz视频序列中的实验结果如图14a、14b所示，ORB-SLAM2算法和ORBSLAMM算法在提取特征点进行跟踪和建图时都会提取到动态物体(人和椅子)上的点，将动态物体当作地图的一部分建入地图中，因此，ORB-SLAM2算法和ORBSLAMM算法在弱动态环境无法分割出环境中的动态目标；而DynaSLAM算法在fr3-sitting-xyz视频序列中的实验结果如图14c所示，它能检测出视频中的先验动态目标(人)，并能将其分割出来。由于该算法不对先验动态目标提取特征点进行跟踪和建图，导致其无法检测出非先验动态目标(正在移动的椅子)，且有时会出现误检测的情况，将静态物体也误检测为动态物体，因此，弱动态环境中，DynaSLAM算法无法分割出环境中所有的动态目标。而本文算法运行时会自动调用训练好的Mask R-CNN模型文件，对环境中的先验动态目标(人)检测并将其视为先验动态目标分割出来，不对其进行跟踪和建图；之后，再利用多视图几何技术检测并分割出正在摇动的椅子，进而不对椅子提取特征点进行跟踪和建图，如图14d所示。因此，本文算法在弱动态环境中几乎可以分割出环境中所有动态目标，本例中本文算法的动态目标检测性能优于其他3种SLAM算法。

图14 不同算法在fr3-sitting-xyz序列中运行结果Fig.14 Processing results of different algorithm in fr3-sitting-xyz sequence

实验2采用fr3-walking-xyz视频序列，该视频是相机在X、Y、Z3个方向上移动，fr3-walking-xyz视频序列中的人来回走动，动作幅度较大。因此，fr3-walking-xyz视频序列所属环境属于强动态环境。在强动态环境中，人来回走动，动作幅度较大，相机在X、Y、Z方向上移动，ORB-SLAM2算法和ORBSLAMM算法在fr3-walking-xyz视频序列中的实验结果如图15a、15b所示，ORB-SLAM2算法和ORBSLAMM算法在动态物体(人和椅子)上提取的特征点较少，但仍然会对动态物体提取特征点进行跟踪和建图，因此，ORB-SLAM2算法在强动态环境中无法分割出环境中的动态目标，也无法建立环境的真实地图；而DynaSLAM算法在fr3-walking-xyz视频序列中的实验结果如图15c所示。它能检测出视频中的先验动态目标(人)，并能将其分割出来，不对先验动态目标提取特征点进行跟踪和建图，但其无法检测出非先验动态目标(正在移动的椅子)，且有时会出现误检测的情况，将静态物体也误检测为动态物体，此外，该算法没有对分割出的先验动态目标遮挡的背景进行修复，所以其建立的稀疏点云地图中地图点较少有很大的空缺部分。因此，强动态环境中DynaSLAM算法无法建立环境的真实地图。而本文算法利用Mask R-CNN神经网络框架对环境中的先验动态目标(人)检测并分割出来，再利用多视图几何技术检测并分割出正在摇动的椅子，不对分割出的动态目标(人和正在移动的椅子)提取特征点进行跟踪和建图，如图15d所示。此外，本文算法还对第k帧中被动态目标遮挡的背景利用多帧融合的背景修复算法进行修复，所以本文算法建立的稀疏地图明显比图15c中建立的稀疏地图的地图点更多。综上所述，本文算法在强动态环境中几乎可以分割出环境中所有的动态目标，还能修复被动态目标遮挡的背景地图，因此，本例中本文算法优于其他3种V-SLAM算法。

图15 不同算法在fr3-walking-xyz序列运行结果Fig.15 Processing results of different algorithms in fr3-walking-xyz sequence

实验3采用fr3-walking-static视频序列，该视频中的相机静止不动，人来回走动，动作幅度较大。图16分别为4种算法的实验结果，得到了与实验2相同的结论，本文算法具有更好的动态目标检测性能和背景修复能力。

图16 不同算法在fr3-walking-static序列运行结果Fig.16 Processing results of different algorithms in fr3-walking-static sequence

4 结束语

针对传统V-SLAM算法无法实现动态环境建图，以及无法克服因环境特征不明显或机器人被“绑架劫持”而导致场景跟丢的问题，本文对ORB-SLAM算法中的3个线程做出改进，显著提高了DE-SLAMM算法的初始化速度。其次，本文提出的多地图构建线程能够有效克服传统V-SLAM算法在遇到“绑架劫持”或跟丢后无法继续建图的问题。本文所提出的深度学习与多视图几何技术相结合的动态目标检测方法能够更准确地检测出环境中存在的各类动态物体。最后，本文提出的基于时间加权滤波的背景修复算法能够有效地修复被动态物体遮挡的背景地图。