开放科学（资源服务）标志码（OSID）：

0 概述

SLAM（Simultaneous Localization and Mapping）在机器人位姿估计及环境感知过程中起着至关重要的作用，主要应用于机器人的导航和避障任务以及自动驾驶^[1-3]和增强现实^[4-5]等领域。视觉传感器体积小、使用方便，并且图像存储了丰富的环境纹理和色彩信息，从而使得V-SLAM（Visual-Simultaneous Localization and Mapping）得到广泛关注和研究。虽然目前的V-SLAM系统框架比较成熟，解决了V-SLAM任务中的很多问题，许多先进的SLAM算法^[6-8]也相继被提出，但是，这些算法均在静态的场景或数据集上进行实验测试，可以达到很好的定位精度与鲁棒性，但并未解决场景中存在动态物体的问题。当场景中存在动态物体，如人、车等，会导致V-SLAM出现定位误差较大或跟踪失败的问题，因此，在动态环境中进行位姿估计与地图构建依然是一个挑战。目前用于消除动态物体影响的方法主要有直接筛选剔除法和语义分割法2种。

直接筛选剔除法可以分为2类，一类是根据图像帧提取的关键点，如Shi-Tomasi^[9]、FAST^[10]和ORB^[11]等，直接进行关键点筛选，最具代表性的算法是RANSAC^[12]，该算法针对低动态场景具有较好的鲁棒性，但是，当场景中存在运动的车或人时，依然会导致位姿估计错误或跟踪失败；另一类方法根据连续图像帧之间的运动关系，采用光流法进行动态物体检测，如文献[13]结合最优估计和均匀采样的方式进行动态物体检测，相比于传统的光流法，其速度快，但是精度较低，文献[14]采用RGB光流法进行场景中的动态物体分割，文献[15]根据双目立体的场景流进行动态物体检测，但是上述2个系统均需要提供动态物体的先验信息，难以适用于长期的位姿跟踪。

语义分割法使用深度学习的方式，检测场景中的动态物体，并对其所处区域进行标注与分割，基于该方法已取得诸多研究成果^[16-18]。文献[18]在ORB-SLAM2的基础上，采用Segnet^[19]网络进行语义分割，获得图像中的语义信息，同时结合运动一致性检查剔除动态物体上的特征点，该算法构建了带有语义信息的八叉树地图，具有较好的实时性，但是定位精度不高。文献[20]采用Mask R-CNN^[21]进行图像的实例分割，对于算法中的RGB-D相机部分，采用多视角几何检测动态特征点并将其移除，同时结合图像修复技术实现静态三维稠密地图构建，该算法具有较高的定位精度，但是系统实时性不高。文献[22]采用稀疏场景流和语义分割进行动态目标识别，采用双目立体匹配构建静态的稠密三维地图，但是该算法没有对定位精度进行评价，并且系统运行速度仅为2.5 Hz。

现有的SLAM系统在动态环境下普遍存在实时性较差或定位误差较大的问题，导致其无法应用于现实场景中。针对以上问题，本文结合自适应窗隔匹配模型及深度学习算法，将两者应用于SLAM系统中。在本文系统前端，采用自适应隔窗匹配模型进行匹配点筛选，得到静态区域的特征匹配点对，实现V-SLAM系统的初始化。在位姿跟踪部分，通过最小化重投影误差进行当前帧位姿跟踪，并且仅对关键帧实现特征匹配，同时向局部地图中添加静态三维点，更新稀疏点云地图。在系统后端，将前端获取的关键帧位姿信息以及RGB-D相机获得的图像信息传送至深度学习模块进行语义分割，以消除场景中的动态物体，实现静态三维稠密地图构建。

1 算法框架

动态场景下的RGB-D SLAM算法结构框图如图 1所示，该算法主要包括初始化、位姿跟踪、局部地图构建、闭环检测、深度学习语义分割及三维地图构建等模块。

在RGB-D SLAM算法中，首先进行图像帧的构建，图像帧的构建方式和ORB-SLAM2^[6]一致，将每一帧的图像信息和提取的ORB特征点保存在系统中，用于初始化、位姿跟踪和地图优化等。

若系统未初始化，则采用自适应隔窗匹配的方式实现动态场景中静态特征点的匹配，然后根据BA（Bundle Adjustment）优化得到最终的初始化静态3D地图点。在初始化成功之后，将当前帧作为初始位位姿，根据构建的静态局部地图实现连续图像帧的位姿估计。

在跟踪过程中同时进行关键帧的选择，以保存机器人定位过程中的关键信息。通过关键帧之间的特征匹配，向局部地图中添加三维点，用于位姿跟踪、闭环检测及重定位等。同时，对关键帧中的图像信息进行语义分割，消除场景中动态物体的影响，构建静态的三维稠密地图，从而实现地图的可重复使用。

2 系统前端设计

在本文算法的前端，首先对每幅图像进行均匀分布的ORB特征提取，然后使用自适应窗隔匹配模型消除场景中动态物体的影响，得到静态区域的特征匹配点对，最后进行位姿跟踪与关键帧筛选。

2.1 ORB特征提取

首先将原始图像按照尺度为1.2的大小构成层数为8的图像金字塔，将每层图像分成30×30的网格区域；然后设置FAST^[10]角点提取的阈值为9，在每层图像上提取FAST角点，并且生成256维的BRIEF^[23]特征点描述子。如果当前网格区域没有提取到特征点，减少FAST角点提取的阈值继续提取特征点，阈值最小值为5。将所有层提取到的特征点进行保存得到当前图像帧的ORB特征点。

2.2 自适应窗隔匹配模型

普通的特征点法SLAM系统一般采用连续的两帧图像进行特征匹配，以实现系统的初始化和位姿跟踪。但是，当场景中存在动态物体时，由于相机帧率较高，对于连续的两帧图像，运动物体并没有发生明显的移动，难以消除动态物体的影响。另外，对于非连续的两帧图像，传统的匹配方式会造成大量的误匹配。连续图像帧及非连续图像帧的匹配区别如图 2所示。

在图 2中，A和B为场景中的部分运动区域，C为场景中的静态区域。图 2（a）描述了连续图像帧之间的特征匹配，由于动态区域移动较小，因此A、B和C区域均能得到较好的特征匹配，但是会造成SLAM系统累计误差持续增大的问题，影响定位精度。图 2（b）所示为非连续图像帧之间的特征匹配，由于动态区域移动较大，使A和B区域的特征匹配发生错误，同样会影响系统的定位精度。

针对以上问题，为方便理论分析，本文定义如下的集合关系式，记图像中全局特征点集合为$ U $，静态背景及物体上的特征点集合为$ S $，动态物体上的特征点集合为$ D $，三者的关系为：

$ U=D\bigcup S $

(1)

所有图像帧集合表示为$ F=\left\{{f}_{1}, {f}_{2}, \cdots , {f}_{N}\right\} $，固定窗口图像帧集合为$ W=\left\{{f}_{1}, {f}_{2}, \cdots , {f}_{8}\right\} $，其中，$ W\subset F $。

为消除动态物体的影响，引入自适应窗隔匹配模型，该模型通过帧间筛选策略将窗口中首帧图像与第x（$ x\le 8 $）帧图像进行特征匹配，x-1为窗隔大小，窗口大小设置为固定值8。自适应窗隔匹配模型算法步骤如下：

1）选取窗口$ W $中两帧图像$ {f}_{1} $、$ {f}_{x} $，提取均匀分布的ORB特征点，并使用暴力匹配法获得初步的匹配关系，然后进行图像区域划分。图像区域划分主要流程如图 3所示。

将图像帧划分为16×16大小的像素块，以$ {f}_{x} $图像左上角像素块为起点，在$ {f}_{1} $图像中执行匹配搜索，搜索步长设置为8，优先选择含匹配角点的像素块。匹配正确之后以该像素块为基准，在横向和纵向依次进行匹配扩展，逐渐扩大区域范围，若该区域块能够占据图像区域的绝大部分，则视为静态区域块，记作T区域。匹配搜索过程中检测到不连续区块，则将其单独标记，并对该区域进行局部扩展，若该区域占据图像范围较小，视其为动态区域块，记为F区域。在实际应用中，设定T区域必定出现且唯一，F区域可为0个或者多个。

2）图像中的场景随着相机运动而不断更新，两帧图像间的变换可视为仿射变换。记第一帧图像中某区域第i个像素块坐标的向量形式为$ {\mathit{\boldsymbol{v}}}_{iI} $，第x帧（$ x\le 8 $）图像中与之对应的像素块坐标向量为$ {\mathit{\boldsymbol{v}}}_{iP} $，则有如下关系：

$ {\mathit{\boldsymbol{v}}}_{iI}=\mathit{\boldsymbol{A}}{\mathit{\boldsymbol{v}}}_{iP}+\mathit{\boldsymbol{t}}, i=\mathrm{1, 2}, \cdots , m $

(2)

其中，$ \mathit{\boldsymbol{A}} $为二阶可逆矩阵，$ \mathit{\boldsymbol{t}} $为二维平移向量，m为对应区域中像素块的总数。可以通过最小化均方根误差来求解$ \mathit{\boldsymbol{A}} $、$ \mathit{\boldsymbol{t}} $，其公式为：

$ \phi (\mathit{\boldsymbol{A}}, \mathit{\boldsymbol{t}})=\underset{\mathit{A}, \mathit{t}}{\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{i}\mathrm{n}}{\left(\frac{1}{m}\sum\limits _{i=1}^{m}{‖{\mathit{\boldsymbol{v}}}_{iI}-(\mathit{\boldsymbol{A}}{\mathit{\boldsymbol{v}}}_{iP}+\mathit{\boldsymbol{t}})‖}_{2}^{2}\right)}^{\frac{1}{2}} $

(3)

考虑到区域中所有的像素块，将式（2）写成矩阵形式可得：

$ \left[{\mathit{\boldsymbol{v}}}_{1I}, {\mathit{\boldsymbol{v}}}_{2I}, \cdots , {\mathit{\boldsymbol{v}}}_{mI}\right]=\mathit{\boldsymbol{A}}\left[{\mathit{\boldsymbol{v}}}_{1P}, {\mathit{\boldsymbol{v}}}_{2P}, \cdots , {\mathit{\boldsymbol{v}}}_{mP}\right]+\mathit{\boldsymbol{t}} $

(4)

记$ {\mathit{\boldsymbol{V}}}_{I}=\left({\mathit{\boldsymbol{v}}}_{1I}, {\mathit{\boldsymbol{v}}}_{2I}, \cdots , {\mathit{\boldsymbol{v}}}_{mI}\right) $，$ {\mathit{\boldsymbol{V}}}_{P}=\left({\mathit{\boldsymbol{v}}}_{1P}, {\mathit{\boldsymbol{v}}}_{2P}, \cdots , {\mathit{\boldsymbol{v}}}_{mP}\right) $，则式（4）可以记作$ {\mathit{\boldsymbol{V}}}_{I}=\mathit{\boldsymbol{A}}{\mathit{\boldsymbol{V}}}_{P}+\mathit{\boldsymbol{t}}{\mathit{\boldsymbol{L}}}^{\mathrm{T}} $，$ \mathit{\boldsymbol{L}} $为m维单位列向量。因此，对式（3）的求解可以等价为无约束极值问题，如式（5）所示：

$ \begin{array}{l}\psi (\mathit{\boldsymbol{A}}, \mathit{\boldsymbol{t}})=m{\left(\frac{1}{m}\sum\limits _{i=1}^{m}{‖{\mathit{\boldsymbol{v}}}_{iI}-(\mathit{\boldsymbol{A}}{\mathit{\boldsymbol{v}}}_{iP}+\mathit{\boldsymbol{t}})‖}_{2}^{2}\right)}^{\frac{1}{2}}=\\ \mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{ }\mathrm{t}\mathrm{r}\left(({\mathit{\boldsymbol{V}}}_{I}-(\mathit{\boldsymbol{A}}{\mathit{\boldsymbol{V}}}_{P}+\mathit{\boldsymbol{t}}\left)\right)({\mathit{\boldsymbol{V}}}_{I}-(\mathit{\boldsymbol{A}}{\mathit{\boldsymbol{V}}}_{P}{+\mathit{\boldsymbol{t}}\left)\right)}^{\mathrm{T}}\right)\end{array} $

(5)

其中，$ \psi (\mathit{\boldsymbol{A}}, \mathit{\boldsymbol{t}}) $是关于$ \mathit{\boldsymbol{A}} $和$ \mathit{\boldsymbol{t}} $的矩阵函数，$ \mathrm{t}\mathrm{r}\left(\mathrm{ }\mathrm{ }\right) $运算符表示求矩阵的迹。在函数$ \psi (\mathit{\boldsymbol{A}}, \mathit{\boldsymbol{t}}) $取极小值时，对应的参数$ {\mathit{\boldsymbol{A}}}^{\mathit{\boldsymbol{*}}} $、$ {\mathit{t}}^{\mathrm{*}} $满足如下关系式：

$ {\left.\frac{\partial \psi (\mathit{\boldsymbol{A}}, \mathit{\boldsymbol{t}})}{\partial \mathit{\boldsymbol{A}}}\right|}_{\mathit{A}={\mathit{A}}^{\mathrm{*}}}=0 $

(6)

$ {\left.\frac{\partial \psi (\mathit{\boldsymbol{A}}, \mathit{\boldsymbol{t}})}{\partial \mathit{\boldsymbol{t}}}\right|}_{\mathit{t}={\mathit{t}}^{\mathrm{*}}}=0 $

(7)

令$ \mathit{\boldsymbol{J}}=\mathit{\boldsymbol{I}}-\frac{1}{m}\mathit{\boldsymbol{L}}{\mathit{\boldsymbol{L}}}^{\mathrm{T}} $，$ \mathit{\boldsymbol{I}} $为单位矩阵，则有$ \mathit{\boldsymbol{J}}{\mathit{\boldsymbol{J}}}^{\mathrm{T}}={\mathit{\boldsymbol{J}}}^{\mathrm{T}}\mathit{\boldsymbol{J}}=\mathit{\boldsymbol{J}}={\mathit{\boldsymbol{J}}}^{\mathrm{T}} $，对式（6）、式（7）求解可得：

$ {\mathit{\boldsymbol{t}}}^{\mathrm{*}}=\frac{1}{m}({\mathit{\boldsymbol{V}}}_{I}-{\mathit{\boldsymbol{V}}}_{I}\mathit{\boldsymbol{J}}{\mathit{\boldsymbol{V}}}_{P}^{\mathrm{T}}({\mathit{\boldsymbol{V}}}_{P}\mathit{\boldsymbol{J}}{\mathit{\boldsymbol{V}}}_{P}^{\mathrm{T}}{)}^{-1}{\mathit{\boldsymbol{V}}}_{P})\mathit{\boldsymbol{L}} $

(8)

$ {\mathit{\boldsymbol{A}}}^{\mathrm{*}}={\mathit{\boldsymbol{V}}}_{I}\mathit{\boldsymbol{J}}{\mathit{\boldsymbol{V}}}_{P}^{\mathrm{T}}({\mathit{\boldsymbol{V}}}_{P}\mathit{\boldsymbol{J}}{\mathit{\boldsymbol{V}}}_{P}^{\mathrm{T}}{)}^{-1} $

(9)

由式（8）、式（9）可以得到T区域及F区域对应的仿射变换参数。由于相机运动是平滑的，为简化运算，对窗口中的图像只分析平移向量$ \mathit{\boldsymbol{t}} $的变化。

在窗口图像帧集合$ W $中，将图像帧$ {f}_{1} $与图像帧$ {f}_{2}\sim {f}_{8} $逐一进行匹配搜索来确定窗隔的大小。定义$ {\mathit{\boldsymbol{t}}}_{T} $为T区域的平移向量，$ {\mathit{\boldsymbol{t}}}_{F} $为F区域的平移向量。存在如下3种情况：

（1）图像帧中仅存在T区域，则将其视为静态场景，若$ {‖{\mathit{\boldsymbol{t}}}_{T}‖}_{2}\ge 20 $，表示场景中在某段时间内没有出现动态物体且相机已经发生明显移动，则认定两帧图像符合筛选条件。

（2）当图像帧中存在一个F区域，判断$ {\mathit{\boldsymbol{t}}}_{T} $与$ {\mathit{\boldsymbol{t}}}_{F} $的大小，若$ 0\le {‖{\mathit{\boldsymbol{t}}}_{F}‖}_{2}\le 0.2{‖{\mathit{\boldsymbol{t}}}_{T}‖}_{2} $或$ 1.8{‖{\mathit{\boldsymbol{t}}}_{T}‖}_{2}\le {‖{\mathit{\boldsymbol{t}}}_{F}‖}_{2}\le 3{‖{\mathit{\boldsymbol{t}}}_{T}‖}_{2} $，表示图像帧中动态物体区域发生明显变化，则认为两帧图像符合筛选条件。

（3）当图像帧中出现多个F区域，仅对平移向量较小的T区域做出判断，若其满足$ 0\le {‖{\mathit{\boldsymbol{t}}}_{F}‖}_{2}\le 0.4{‖{\mathit{\boldsymbol{t}}}_{T}‖}_{2} $或$ 1.2{‖{\mathit{\boldsymbol{t}}}_{T}‖}_{2}\le {‖{\mathit{\boldsymbol{t}}}_{F}‖}_{2}\le 2{‖{\mathit{\boldsymbol{t}}}_{T}‖}_{2} $，则认为两帧图像符合筛选条件。

窗隔图像帧的更新策略为：若图像帧$ {f}_{1} $在初始窗口图像帧中没有匹配到符合筛选条件的图像帧，则保留$ {f}_{1} $，将$ {f}_{2}\sim {f}_{8} $帧移出，再向窗口中移入7帧图像，重新进行匹配搜索。若图像帧$ {f}_{1} $与窗口中第x帧图像符合筛选条件，则窗隔大小即为x-1，然后将窗口中前x-1帧图像移出，并将第x帧图像设置为$ {f}_{1} $帧，再向窗口中补充x-1帧图像，并开启新一轮匹配搜索。

3）在筛选出符合条件的图像帧之后，本文使用改进的GMS^[24]特征匹配算法来获得高质量的匹配点对，该算法是一种高效且实时的匹配点筛选算法，其基于运动网格统计模型对原始的特征匹配进行筛选，得到静态区域的特征匹配。

根据GMS特征匹配算法的原理，得出如下关系：

$ s=\left|X\right|-1 $

(10)

其中，$ s $表示两帧图像中对应区域得分，$ \left|X\right| $表示该区域中特征匹配点对的数量，如果一个特征匹配是正确的，那么它的周围依然存在其他特征匹配点对，此时$ s $的值较大。

对于连续的2个图像帧，如图 2（a）所示，区域A、B、C中均可得到较多的匹配点对，从而使得动态区域A、B造成假得分的情况。假设A、B和C区域中的特征匹配点对得分为$ {s}_{\mathrm{A}} $、$ {s}_{\mathrm{B}} $和$ {s}_{\mathrm{C}} $，那么有如下关系：

$ {s}_{\mathrm{A}}\approx {s}_{\mathrm{B}}\approx {s}_{\mathrm{C}} $

(11)

在这种情况下，无法通过设定合适的阈值将动态区域（区域A和B）和静态区域（区域C）的特征匹配区分开。因此，GMS算法不适用于场景中动态物体移动较小的情况。

本文系统采用自适应窗隔匹配模型，得到场景中动态物体发生明显移动的两帧图像。以图 2（b）为例，静态区域C有较高的匹配得分，但是，区域A和B受到动态物体的影响发生了明显变化，使得这些区域的特征匹配出现较大的错误，$ {s}_{\mathrm{A}} $和$ {s}_{\mathrm{B}} $的值很小，则有如下关系：

$ {s}_{\mathrm{A}}\approx {s}_{\mathrm{B}}\ll {s}_{\mathrm{C}} $

(12)

这样就可以通过设置合适的阈值，消除动态物体上特征匹配点的影响，解决假得分的问题，获得静态区域的特征匹配点集$ S $。

为提高特征匹配点对的筛选速度，将图像进行网格划分。由网格的可分性度量值$ P\propto \sqrt{KM} $可知，$ P $正比于网格数量$ K $及网格区域中特征点数量$ M $，且$ K $与$ M $互补。因此，综合考虑特征匹配点对的筛选速度和可分精度，采用增大网格数量$ K $，即缩小网格大小的方式，将网格大小设置为10×10。然后，直接判断每个网格中所有特征匹配点对的正确性，如图 4所示。

图 4为经过窗隔匹配筛选的两帧图像，图 4（a）为窗隔模型中的首帧图像，图 4（b）为第x帧图像，在以网格A为中心的3×3网格中，统计左右图像对中区域A的匹配得分$ {s}_{ij} $，其定义如下：

$ {s}_{ij}=\sum\limits _{k=1}^{9}\left|{X}_{{i}_{k}{j}_{k}}\right| $

(13)

其中，$ \left|{X}_{{i}_{k}{j}_{k}}\right| $表示网格对$ \left\{{i}_{k}, {j}_{k}\right\} $中对应的特征点匹配数量，根据运动的平滑性及特征点匹配的一致性，$ {s}_{ij} $值越大，表示网格A中特征匹配的正确性越高，反之，则正确性越低。此时，设置阈值$ t $来对网格A中的特征匹配是否正确进行判断：

$ t=\alpha \sqrt{n} $

(14)

其中，$ \alpha $为常量（一般设置为6），$ n $表示以A为中心的3×3网格中特征匹配的数量。

对于静态区域，如图 4中白色方框所示，网格A可以获得较高的匹配得分$ {s}_{ij} $，而对图 4中黑色方框所示的动态区域，由于存在运动物体，使得左图像提取的特征点无法在右图像中找到对应的匹配点，甚至会在右图像中找到错误的匹配点，因此该区域的匹配得分值$ {s}_{ij} $较低。由式（14）能够看出，可以根据区域内匹配特征点的数量设置自适应阈值。若$ {s}_{ij}\ge t $，则区域匹配正确，保留匹配点；若$ {s}_{ij}<t $，则区域匹配错误，舍弃匹配点。通过自适应阈值判断匹配点对，可以剔除动态区域的误匹配，得到静态区域的匹配点对，从而提高全局的匹配正确率。

2.3 位姿跟踪

使用自适应窗隔匹配模型进行系统的初始化，初始化成功后得到当前帧的位姿信息，将当前帧设置为关键帧和参考帧。当新的图像帧创建后，得到$ n $组匹配成功的静态点对，其对应于三维空间点$ {P}_{i} $，$ {p}_{i1} $、$ {p}_{i2} $（$ i\in n $）为该点在参考帧和当前帧中的投影，$ {\widehat{p}}_{i2} $为在初始位姿下空间点$ {P}_{i} $在当前帧中的投影，$ {e}_{i} $为重投影误差，如图 5所示。然后，可通过BA优化方式，即最小化重投影误差求解相机位姿。引入李代数中$ \mathit{\xi } $表示相机位姿，有如下关系：

$ {\widehat{p}}_{i2}=\mathrm{e}\mathrm{x}\mathrm{p}\left({\mathit{\xi }}^{\wedge }\right){p}_{i1}, \mathrm{ }\forall i\in n $

(15)

其中，“$ \stackrel{\wedge }{} $”运算符号表示将李代数元素转换为变换矩阵。该点重投影误差可表示为：

$ {e}_{i}={p}_{i2}-\mathrm{e}\mathrm{x}\mathrm{p}\left({\mathit{\xi }}^{\wedge }\right){p}_{i1} $

(16)

将n组匹配点的误差项进行平方求和，构建最小二乘问题，并使其最小化：

$ {\mathit{\xi }}^{\mathrm{*}}=\underset{\mathit{\xi }}{\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{i}\mathrm{n}}\frac{1}{2}\sum\limits _{i=1}^{n}{‖{p}_{i2}-\mathrm{e}\mathrm{x}\mathrm{p}\left({\mathit{\xi }}^{\wedge }\right){p}_{i1}‖}_{2}^{2} $

(17)

使用列文伯格-马夸尔特优化算法求解式（17），可求得最优相机位姿$ {\mathit{\xi }}^{\mathrm{*}} $。

在进入跟踪模块后，使用参考帧模型或恒速模型对当前帧位姿进行估计。对于参考帧模型，设置最新的关键帧作为参考帧，对参考帧和当前帧进行窗隔匹配，如果参考帧匹配的特征点在局部地图中存在对应的三维点，就将这些三维点投影到当前帧。将上一帧的位姿信息作为当前帧位姿的初始值，根据BA优化得到当前帧的位姿信息与观测到的三维地图点。与参考帧模型不同，恒速模型假设两帧之间的运动与之前两帧相同，以此来设定BA优化的位姿初始值。

关键帧的筛选策略不同于ORB-SLAM2，本文首先采用自适应窗隔匹配法得到两图像帧，再对静态区域的匹配点数进行判断，若匹配点数大于50且距离上一次所加入的关键帧超过8帧，则加入新的关键帧。该方法既可确保图像中的动态物体出现显著运动，又可避免引入过多的冗余信息，而且在为局部地图添加新的地图点时，能够消除场景中动态物体的影响。

3 系统后端设计

为消除场景中的动态物体，构建可供实际应用的静态稠密地图，本文系统采用Mask R-CNN进行图像的语义分割，然后将分割得到的动态物体对应的深度信息从该帧的深度图中移除，从而达到仅对静态场景进行稠密地图构建的目的。

3.1 图像语义分割

本文采用Mask R-CNN对图像帧进行语义分割，使用在COCO^[25]数据集上训练的深度模型，该模型可以分割常见的运动物体（如人、自行车、汽车、火车、卡车、鸟、马、羊等）。Mask R-CNN算法的输入是原始的RGB图像，输出图像中包含矩形选择框、类别标签以及实例的掩膜区域。然而，在本文系统的应用中，仅需用到实例的掩膜信息，因此，在算法中移除了关于类别预测的模块，分割结果如图 6所示，该图来自COCO数据集。

由于Mask R-CNN是在Faster R-CNN^[26]的框架下进行设计的，因此相对于其他语义分割算法具有较快的预测速度，在GPU设备上预测COCO数据集上一张图片的语义信息仅需200 ms。

V-SLAM系统均有很高的实时性要求，为了提高系统运行速度，将语义分割部分作为一个单独的线程运行。假设输入图像大小为$ m\times n\times 3 $，则输出矩阵的大小为$ m\times n\times l $，其中，$ l $表示图像中目标的数目。对图像的每一个通道进行语义分割，最终得到单帧图像中所有的动态目标。

3.2 静态三维地图构建

利用深度学习算法得到动态物体的掩膜信息，根据对应关系可移除动态物体在深度图中的深度数据，然后结合SLAM系统得到的关键帧位姿信息进行静态三维稠密地图构建。

为了保证系统的运行速度，将可视化点云信息界面作为一个单独的线程运行，当图像帧为关键帧时，将此时的深度图和RGB图像结合相机内部参数恢复得到相机坐标系下的三维点云坐标$ \left(x, y, z\right) $，如式（18）所示：

$ \left\{\begin{array}{l}z=d\\ x=z\left(u-{c}_{x}\right)/{f}_{x}\\ y=z\left(v-{c}_{y}\right)/{f}_{y}\end{array}\right. $

(18)

其中，$ d $与深度图在像素坐标$ \left(u, v\right) $的值成正相关，其余参数均为相机内参，由相机自身决定，可以通过相机标定的方式得到。

4 实验结果与分析

本文使用TUM^[27] RGB-D动态序列数据集进行特征匹配、定位精度及跟踪时间的对比分析，并在动态数据集及实际环境中进行场景稠密重建测试。实验环境配置为4核i7处理器，8 GB内存，GTX 1060，Ubuntu16.04操作系统。

4.1 特征匹配效果分析

基于TUM的fr3_w_xyz动态数据集，对本文系统的自适应窗隔匹配策略与BF、FLANN匹配方式作对比分析，在前后两帧中均提取1 000个特征点进行匹配，匹配结果如图 7所示。

从图 7（a）、图 7（b）可以看出，BF匹配和FLANN匹配不能有效移除动态物体上的匹配点对，同时导致静态区域部分出现较多的误匹配。图 7（c）为自适应窗隔匹配算法的匹配效果，该算法不仅移除了动态物体上的匹配点对，也使静态区域特征匹配的正确率得到提升。

对窗隔匹配算法、BF算法及FLANN算法的匹配耗时及匹配点对个数进行对比分析，结果如表 1所示。

由表 1可知，FLANN算法的匹配耗时最长，且匹配点对数量最少，BF算法匹配耗时最短，且匹配点对个数最多，但是该算法会带来大量的误匹配。因此，综合考虑匹配速度及匹配正确性，自适应窗隔匹配算法达到最优。

4.2 定位精度及实时性分析

使用TUM RGB-D数据集评估本文算法的定位精度和实时性，分析包含多动态对象的低动态（fr3_s_*）和高动态（fr3_w_*）场景序列的测试结果，并与ORB-SLAM2及DynaSLAM系统进行对比。以下均以OUR_SYSTEM来指代本文系统，实验结果如表 2和表 3所示。在表 2中使用绝对轨迹误差（ATE）中的平方根误差（RMSE）对定位精度进行评估，表 3中使用图像帧平均跟踪时间进行实时性能评估，图像帧跟踪时间为从构建图像帧到获取其位姿的耗时，图像帧平均跟踪时间为所有图像帧跟踪时间总和取平均值。其中，“*”表示跟踪丢失且重定位失败。

由表 2可得，在静态场景（fr1_*）及单一动态对象序列（fr2_*）中，本文系统（OUR_SYSTEM）定位精度最高。在低动态的场景序列（fr3_s_*）中，OUR_SYSTEM以及其他SLAM系统定位误差均较小。但是，在多数数据集上，OUR_SYSTEM达到了最高的定位精度。在高动态场景序列（fr3_w_*）中，OUR_SYSTEM的定位精度不仅远高于ORB-SLAM2系统，而且也优于DynaSLAM系统。

由表 3可得，在静态场景序列及全部动态场景序列中，OUR_SYSTEM的跟踪速度快于ORB-SLAM2及DynaSLAM，并且跟踪速度比DynaSLAM系统快10倍以上。计算表中OUR_SYSTEM跟踪时间的均值，可得平均时间为0.025 s，即跟踪速率为40 Hz，满足视觉SLAM的实时性要求。

综合以上关于定位精度和跟踪速度的分析，OUR_SYSTEM在静态及动态场景中均可实现较高的定位精度，而且具有较好的实时性，总体效果优于ORB-SLAM2和DynaSLAM系统。

为进一步验证OUR_SYSTEM的定位精度，绘制绝对位姿误差APE（Absolute Pose Error）的分布示意图，如图 8所示。

图 8所示为位姿跟踪过程中OUR_SYSTEM、ORB-SLAM2及DynaSLAM在动态数据集fr3_s_hs、fr3_s_xyz、fr3_w_hs及fr3_w_xyz上的APE分布，其中，矩形区域表示3/4的APE数据分布，其余部分表示剩余的APE数据分布。从图 8可以看出，在低动态数据集（fr3_s_*）的定位过程中，这3种SLAM算法均可达到较小的绝对位姿误差，并且OUR_SYSTEM绝对位姿误差最小。在高动态场景数据集（fr3_w_*）中，原始ORB-SLAM2系统的绝对位姿误差最大，而OUR_SYSTEM在最大限度上消除了运动物体的影响，达到了最高的定位精度。

OUR_SYSTEM在数据集fr3_s_hs、fr3_s_xyz、fr3_w_hs及fr3_w_xyz中实际运动轨迹与真实轨迹的热力对比如图 9所示。

从图 9可以看出，OUR_SYSTEM在动态环境下能够实现对相机位姿的精确估计。

4.3 静态地图构建及算法有效性分析

在TUM动态数据集和实际场景中分别进行三维场景重建效果测试，并在实际环境下测试OUR_SYSTEM特征提取及定位的效果。TUM动态数据集上的实验结果如图 10所示。图 10（a）为原始RGB图像，对原始图像通过深度学习算法进行动态目标剔除，得到图 10（b），同时，将对应的深度图中动态目标的深度信息也剔除，得到图 10（c）。使用不含动态目标的RGB图像及深度图像，结合图像帧的位姿信息进行静态三维稠密地图构建，结果如图 10（d）所示，该地图不含动态物体，可以重复使用，有较高的实用性。

为进一步验证算法的有效性，使用RGB-D相机（乐视Letv）在动态的实验室环境中进行场景三维重建。在场景中存在移动人员的情况下，通过RGB-D相机连续采集图像进行实验，使相机足够平稳且出现闭环。图 11所示为ORB-SLAM2与OUR_SYSTEM提取ORB特征的对比效果，图 11（a）为ORB_SLAM2算法效果，其中有大量的特征点分布在行走的人员身上，图 11（b）为OUR_SYSTEM效果，其中绝大多数的特征点分布在背景中。

图 12所示为ORB_SLAM2与OUR_SYSTEM相机预测轨迹对比结果，图中实线轨迹比较平滑，且有闭环出现，这与相机的真实运动轨迹相符，表明OUR_SYSTEM在实际动态场景中精度较高。而黑色虚线轨迹由于受到动态物体的影响，出现了大幅度的偏差，定位误差较大，虽然有闭环出现，但未能实现全局轨迹矫正。

与图 10类似，图 13（a）、图 13（b）、图 13（c）为视频序列中某一帧图像的处理效果，图 13（d）为最终的重建效果。可以看出，最终效果消除了实际场景中的动态物体，构建动态场景的静态三维稠密地图能够实现地图的可重复使用。

5 结束语

本文针对现有RGB-D视觉SLAM系统在动态环境下存在的精度不足、实时性较差等问题，提出一种动态场景下的RGB-D SLAM算法，该算法采用自适应窗隔匹配模型来实现动态环境下的运动估计。在定位过程中，相比于ORB-SLAM2及DynaSLAM方法，本文算法在实时性和定位精度上均有所提升。在场景重建过程中，本文算法使用Mask R-CNN检测运动物体，根据所提供的掩膜信息消除动态目标。实验结果表明，该算法可以对场景进行完整的三维重建。但是，当相机移动过快或者处于低纹理场景中时，本文算法容易出现跟踪丢失的情况。因此，下一步将考虑添加IMU传感器，使用多传感器融合的方法来实现更加鲁棒的位姿跟踪。