1 引言（Introduction）

同时定位与地图构建（simultaneous localization and mapping，SLAM）是移动机器人实现自主导航和完成复杂环境交互任务的基础。RGB-D相机可以直接采集彩色和深度图像且成本较低，因此基于RGB-D相机的视觉SLAM系统被广泛应用于室内移动机器人等领域。室内场景常常包含各种运动物体，对SLAM系统会产生位姿估计和地图构建两方面的干扰^[1-2]。

现有的视觉SLAM系统大部分都假设场景为静态，并根据这一假设简化位姿估计。这些SLAM算法将动态特征点加入位姿计算，在稀疏点云地图中生成对应的错误地图点，导致位姿估计产生偏差。因此，需要一种有效方法区分动态特征或区域。运动物体还将影响SLAM系统的稠密建图，错误的位姿估计使得算法错误叠加多帧观测信息，造成地图扭曲。即便位姿估计正确，同一运动物体在不同时刻的信息被多次加入地图将形成物体运动的“残影”，无法正确反映场景的真实状态。

光流法是研究帧间运动变化的常用方法。Sun等^[3]通过稠密光流和CodeBook模型分割动态区域，排除动态特征干扰，建立稠密点云。Wang等^[4]对图像计算稠密光流，结合点轨迹聚类得到动态区域分割。场景流方法将2维光流的思想引入到3维空间中，计算3维空间位移。Lee等^[5]计算场景流，建立刚体运动模型，分割运动刚体区域。稠密光流和场景流方法对于动态区域的分割效果好，但对大量像素点进行光流计算将降低算法的实时性。

采用其他方法同样存在运算效率与动态分割、稠密建图之间的矛盾。Sun等^[6]对相邻图像帧进行背景配准，采用帧间差分法识别运动，利用深度图量化分割动态区域，不涉及稠密建图。高成强等^[7]使用单高斯模型（single Gaussian model，SGM）对背景进行建模，建立稠密TSDF（截断符号距离函数）地图，但仅能得出运动物体的矩形框选。魏彤等^[8]根据对极几何原理筛选动态特征点，并结合超像素分割算法标记动态区域，但分割计算量较大，降低了系统的实时性且不涉及稠密建图。张慧娟等^[9]利用环境中的直线特征代替点特征，采用计算静态权重方法剔除动态线特征，计算效率高但不适用于稠密建图。文[10]提出基于改进几何与运动约束的算法，同样基于稀疏特征，因此不具备运动区域分割与在线稠密建图的能力。

除此之外，越来越多的研究者将深度学习方法应用到动态场景的SLAM算法中。Bescos等^[11]利用Mask R-CNN网络^[12]分割动态物体并借助过去时刻的关键帧对当前画面中缺失的背景进行填充以便进行稠密建图，Yu等^[13]则利用SegNet网络^[14]对图像进行分割和稠密建图。也有研究者选择使用目标检测网络先确定动态物体位置，再使用其他方法进行分割。Zhong等^[15]通过目标检测网络和GrabCut算法^[16]分割动态物体。姚二亮等^[17]使用YOLOv3算法^[18]初步滤除动态区域后通过图像中边缘的距离变换误差和光度误差的一致性评估更加细致地分割和滤除动态区域。Liu等^[19]将YOLOv3与基于光流法的极线约束算法结合去除动态特征，位姿精度高但不涉及动态分割与稠密建图。基于深度学习的方法能够准确识别物体，通常表现良好，但无法识别不在网络先验范围内的动态物体且计算量大、依赖GPU硬件，难以应用在移动机器人等平台上。

综上所述，定位精度高且具有分割与稠密建图能力的方法往往计算效率不够理想。为解决这一问题，针对室内动态场景移动机器人，本文提出一种RGB-D SLAM方法，同时具有以下特点：

(1) 消除动态场景干扰，定位精度高；

(2) 计算效率高；

(3) 分割和标记动态物体；

(4) 在线稠密建图。

本文提出网格分割方法代替像素级分割，引入单应运动补偿与根据光流理论设计的双向补偿光流法，结合几何连通性处理与深度值聚类进行逐帧动态区域分割，避免动态特征点参与位姿优化，并大幅提升计算速度。同时构建稀疏点云地图和静态八叉树地图并进行耦合，在关键帧上使用基于网格分割和八叉树地图光线遍历的方法筛选静态地图点，更新稀疏点云地图，保障定位精度。

2 系统框架（System frame）

本文提出的SLAM算法是在ORB-SLAM3算法^[20]基础上进行改进，构建基于网格分割的位姿跟踪、稀疏点云建图与双地图耦合、八叉树建图、回环检测的四线程结构。图 1为系统流程图。

(1) 基于网格分割的位姿跟踪：提取灰度图像的ORB（oriented brief）特征点^[21]，引入单应运动补偿方法和根据光流理论设计的双向补偿光流法，结合几何连通性处理方法和深度值聚类方法，形成网格化的运动分割图像，剔除动态特征点，通过最小化重投影误差计算相机位姿。位姿计算完成后，选取部分帧为新的关键帧。

(2) 稀疏点云建图与双地图耦合：根据关键帧还原特征点在3维空间中的位置，在稀疏点云地图中插入地图点。在每个关键帧上，首先根据观测状况剔除低质量的已有地图点，然后利用网格化运动分割图像、结合八叉树地图的双地图耦合技术来筛选并插入新地图点，最后进行局部BA（bundle adjustmen）优化，剔除冗余关键帧。

(3) 八叉树建图：区别于一般的SLAM方法，本文算法基于光线遍历法，根据关键帧上的相机位姿、RGB图像、深度图像、网格化运动分割图像构建场景的静态八叉树地图。

(4) 回环检测与校正：采用ORB-SLAM3中的方法，利用词袋模型进行回环检测。

3 基于网格分割的位姿估计（Pose estimation based on gridding segmentation） 3.1 单应运动补偿与双向补偿光流法

单应矩阵描述了2个平面之间的映射关系，单应变换前后点坐标的对应关系由式(1) 计算，$ {{\mathit{\boldsymbol{H}}}}_{t+1, t} $为第$ t $帧到第$ t+1 $帧之间的单应矩阵。在图像包含明确前景和背景且运动前景不占据画面绝大部分像素的情形下，单应变换可以补偿由于相机自身运动引起的图像背景变化。其中式(1) 可以表示为式(2) 的形式，该等式为齐次坐标等式。

$ \begin{align} {{\mathit{\boldsymbol{x}}}}_{t+1} & ={{\mathit{\boldsymbol{H}}}}_{t+1, t} {{\mathit{\boldsymbol{x}}}}_{\rm t} \end{align} $

(1)

$ \begin{align} \begin{bmatrix} {x_{t +1 }} \\ {y_{t +1 }} \\ 1 \end{bmatrix} & = \begin{bmatrix} {h_{11}} & {h_{12}} & {h_{13}} \\ {h_{21}} & {h_{22}} & {h_{23}} \\ {h_{31}} & {h_{32}} & {h_{33}} \end{bmatrix} \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} \end{align} $

(2)

两帧图像的单应矩阵计算依赖两帧之间对应点的匹配。由于存在误匹配和运动特征点等，两帧图像中的特征点对不可能全部满足同一单应变换。因此，使用LMedS（least median of squares）方法确定参与单应矩阵计算的内点，使用Levenberg-Marquardt方法迭代求解单应矩阵。

$ \begin{align} \mathop{\text{arg min}}\limits_{{{\mathit{\boldsymbol{H}}}}_{t+1, t}} \rm{Median}& \left\{ \left({x_{t+1} -\frac{h_{11} x_{\rm t} +h_{12} y_{\rm t} +h_{13}} {h_{31} x_{\rm t} +h_{32} y_{\rm t} +h_{33}}}\right)^{2} +\right. \\ & \left. \left({y_{t+1} -\frac{h_{21} x_{\rm t} +h_{22} y_{\rm t} +h_{23}} {h_{31} x_{\rm t} +h_{32} y_{\rm t} +h_{33}}}\right)^{2} \right\} \end{align} $

(3)

式(3) 将变换后坐标和实际对应坐标之间的像素距离作为误差项，利用LMedS方法进行求解，使得选出点集误差的中值最小化。与RANSAC方法相比，LMedS方法无需手动选择阈值，同时LMedS方法在连续帧序列中得到的单应变换有很好的连续性，较少发生跳变。

两帧之间的对应点信息通过光流法求得。提取第$ t $帧中的ORB特征点，利用Lucas-Kanade稀疏光流法向第$ t+1 $帧进行追踪，得到对应点位置。

本文提出了基于光流原理的双向补偿光流法，如图 2所示，根据上文原理对前后两帧$ t $与$ t+1 $进行前向光流计算，其中$ F_t $表示第$ t $帧图像，$ F_{t, t+1} $表示第$ t $帧到第$ t+1 $帧的单应补偿图像。根据对应点信息计算单应矩阵，对第$ t $帧图像应用该单应变换得到补偿图像，此时从第$ t+1 $帧向补偿图像作反向光流追踪得到消除干扰的反向光流场。

首先讨论补偿图像的含义，将补偿图像与第$ t+1 $帧图像进行叠加，如图 3所示。由图 3可知，图像背景吻合得很好，而在运动人物的区域可以辨别出人物的前后2个位置（描边区域），运动前景被突出显示出来了。

在双向补偿光流过程中记第$ t $帧的时刻为$ t $，$ t $时刻位于$ ( x, y) $处的像素在$ t + {\rm d} t $时刻运动到$ ( x + $ $ {\rm d} x, y + {\rm d} y) $处。根据光流法中的灰度不变假设，有前向光流灰度关系式(4)，其中$ I(x, y, t) $表示$ t $时刻$ ( x, y) $处的像素灰度。前向光流的意义主要在于建立前后两帧之间的对应点联系，从而计算单应矩阵$ {{\mathit{\boldsymbol{H}}}}_{t+1, t} $。

$ \begin{gather} I( x+{\rm d}x, y+{\rm d}y, t+{\rm d}t )=I(x, y, t) \end{gather} $

(4)

$ \begin{gather} I(x_{\rm b}, y_{\rm b}, t)=I({x+{\rm d}x, y+{\rm d}y, t+{\rm d}t}) \end{gather} $

(5)

反向光流过程中，进行第$ t+1 $帧到经过单应补偿的第$ t $帧图像的光流计算，找到$ t+{\rm d}t $时刻像素位置$ ( x + $ $ {\rm d}x, y+ {\rm d}y) $在$ t $时刻的对应点位置，有反向光流灰度关系式(5)。

式(6) 为前向光流的光流矢量公式，为方便比较，反向光流的光流矢量定义为该过程中原本光流矢量的相反矢量，如式(7) 所示。

$ \begin{align} \mathit{\boldsymbol{v}}_{\rm{forward}} & =({\rm d}x, {\rm d}y) \end{align} $

(6)

$ \begin{align} \mathit{\boldsymbol{v}}_{\rm{backward}} & =-(x_{\rm b} -x-{\rm d}x, y_{\rm b} -y-{\rm d}y) \\ & =({\rm d}x+x-x_{\rm b}, {\rm d}y+y-y_{\rm b}) \end{align} $

(7)

2个矢量的差值$ ( x - x_{\rm b}, y - y_{\rm b}) $表征了单应运动补偿对于相机自身运动引起的背景运动的补偿，使得$ \mathit{\boldsymbol{v}}_{\rm{backward}} $接近于运动前景的位移。

如图 4光流场显示，和前向光流的结果相比，反向光流计算后静止背景中特征点的移动得到了有效抵消，从而反向光流矢量可以在一定程度上表征一个特征点的运动方向和速度。

从多帧图像序列角度考虑，如图 5所示，为了提高双向补偿光流法的可靠性，在按图 2中的方法计算$ t $帧和$ t + 1 $帧之间的反向光流场1的基础上，同理计算$ t $帧和$ t + 2 $帧之间的反向光流场2，并对比分析2个光流场中的光流矢量。

图 6(a)为反向光流场1，图 6(b)为反向光流场2。除了一些显著错误的点，反向光流场2的时间跨度更长，运动前景的移动更为显著，按以下条件判断特征点是否做稳定的运动：

$ \begin{align} \begin{cases} \| {{\mathit{\boldsymbol{v}}}}_{2} \|>\| {{\mathit{\boldsymbol{v}}}}_{1} \|>\delta\\ {{\mathit{\boldsymbol{v}}}}_{1} \cdot {{\mathit{\boldsymbol{v}}}}_{2} >0 \end{cases} \end{align} $

(8)

式(8) 中，$ {{\mathit{\boldsymbol{v}}}}_{1} $和$ {{\mathit{\boldsymbol{v}}}}_{2} $分别为2个光流场中对应的光流矢量，$ \delta $为一个极小的值，受运动程度影响小。式(8) 最后一个式子将运动方向一致性转化为高效的内积运算。经过筛选，可以得到图像中的动态特征点集。与基于对极几何原理的检测方法相比，双向补偿光流法减少了运动检测中对阈值选择的依赖，提高了检测稳定性，同时考虑了短时间内物体运动方向和速度的一致性，去除了光流法错误跟踪和相机高频抖动的干扰。

3.2 网格化运动分割

图 7(a)为按3.1节方法得到的动态特征点集合，动点可以表征运动要素所在画面中的位置，但分布较为稀疏，需要进一步得到动态物体在画面中的区域划分。

将输入图像划分为20$ \times $20的矩形网格区域。通过网格划分将动态特征点转换为动态区域。图 7(b)将含有动态特征点的区块标记为疑似动态区块，同时对每个矩形区域内动态特征点数量进行计数。

在得到的动态区域结果基础上，基于几何连通性处理孤立区块与被包围区块。如图 8所示，若某动态区块孤立且动态特征点计数少于一定阈值（取为3），则该区块标记为静止。若某静态区块被较多个动态区块包围，则该区块标记为运动。

3.3 基于深度值聚类的动态区域优化

经初步处理的网格分割结果已经能够在一定程度上表示画面中的动态区域，但在实际的室内动态场景中还有2种经常出现的情况：(1) 行人在图像中并不会表现出近似刚体的性质。人在走动时，通常各部分的运动情况不一，导致以上的运动分割只覆盖物体的部分区域。(2) 对于弱纹理的运动物体，只在边缘有较强的运动检测响应，初步处理存在不足。因此，按以下步骤对图像的深度值进行K均值聚类，并据此对动态区域进行进一步优化。

将深度图像降采样至20$ \times $20分辨率，对应所划分的图像网格，降低了聚类算法的计算量。以深度值为特征，对像素点进行K均值聚类操作。K均值聚类算法的结果很大程度上依赖初始聚类中心的选择，因此使用K-Means$ ++ $方法^[22]进行深度值聚类。第一个初始聚类中心从所有数据中随机选取，计算每个样本$ x_{i} $与最近聚类中心的距离$ D({x_{i}}) $，深度值聚类是一个1维聚类问题，距离为深度值之差的绝对值。按照式(9) 计算每个样本被选为聚类中心的概率$ P({x_{i}}) $，选择$ P({x{}_{i}}) $值最大的样本为下一个聚类中心，重复该步骤直到选出$ K $个聚类中心。

$ \begin{align} P({x_{i}})=\frac{D^{2}({x_{i}})}{\sum \limits_i {D^{2}({x_{i}})}} \end{align} $

(9)

K-Means$ ++ $方法着重优化初始聚类中心的选择，使得初始聚类中心的分布尽可能均匀，符合聚类结果能够区分不同深度层次物体的期望。聚类结果如图 9(a)所示，使用不同颜色标注不同的深度层次，运动人体部分在画面中有显著的表示。

如图 10所示，参照深度值聚类结果扩展和填充动态区域。若动态区域中的一个连通区域与某聚类簇内的一个连通区域重合度高于阈值$ \varepsilon $，则将动态区域周围属于同一聚类簇的非动态区域也标记为动态区域，最终动态区域如图 9(b)所示，运动人体所在区域得到了很好的覆盖。重合度计算方法为

$ \begin{align} r=\frac{s}{s_{1} +s_{2} -s} \end{align} $

(10)

其中，$ s $为重合区域面积，$ s_{1} $为聚类簇连通区域面积，$ s_{2} $为动态区域面积。

由于结合3帧连续图像进行运动分割，算法每隔一帧得出一次运动分割。因为两帧之间时间较短、运动量极小，所以没有分割的帧可以继承前一帧的运动分割结果。

3.4 消除动态影响的位姿估计

结合RGB图像和深度图像得出动态区域分割结果后，根据网格分割结果对当前帧中的特征点的运动性质作出判断，落在动态区域内的特征点为动态特征点，记为集合$ \chi_{\rm s} $。利用最小化重投影误差方式计算当前帧的相机位姿$ \mathit{\boldsymbol{T}}_{\rm{cw}} $，其中，不对动态特征点计算误差项，不进行优化处理。

$ \begin{align} \mathit{\boldsymbol{T}}_{\rm{cw}} =\text{arg min} \sum _{i\in \chi_{\rm s}} \rho \left({\| {\mathit{\boldsymbol{x}}_{i} -\boldsymbol \pi ( \mathit{\boldsymbol{T}}_{\rm{cw}} \mathit{\boldsymbol{P}}_{i} )} \|_{\mathit{\boldsymbol{\varSigma}}}^{2}}\right) \end{align} $

(11)

其中：

$ \begin{align} \boldsymbol \pi \left(\begin{bmatrix} X \\ Y \\ Z \end{bmatrix}\right) & =\begin{bmatrix} f_{ x} \dfrac{X}{Z}+c_{ x} \\[6pt] f_{ y} \dfrac{Y}{Z}+c_{ y} \end{bmatrix} \end{align} $

(12)

$ \begin{align} \mathit{\boldsymbol{\varSigma}} & =n\cdot \mathit{\boldsymbol{E}} \end{align} $

(13)

$ \rho (\cdot) $为Huber鲁棒损失函数，$ \mathit{\boldsymbol{x}}_{i} $为静态特征点坐标，$ \mathit{\boldsymbol{P}}_{i} $为对应的3维点坐标。$ \boldsymbol \pi (\cdot) $为相机的投影函数，其中所涉及变量为相机内参。$ \mathit{\boldsymbol{\varSigma}} $为信息矩阵，$ n $为当前特征点所在图像金字塔的层数，$ \mathit{\boldsymbol{E}} $为3$ \times $3的单位矩阵。

利用高斯—牛顿方法对最小化重投影误差问题进行求解，优化得到相机的位姿估计。

4 双地图构建与耦合（Construction and coupling of the dual maps） 4.1 基于光线遍历法的八叉树地图构建

本文算法在跟踪相机位姿的同时会建立场景稀疏点云地图进行定位，但是稀疏点云地图不足以充分描述场景细节，也不能用于导航和路径规划。系统设计了八叉树地图模块，基于光线遍历法，接收了关键帧的相机位姿、RGB图像、深度图像、网格分割图像信息，建立和更新了场景的静态地图。

八叉树地图将3维空间划分为立方体节点，其中叶子节点通过占据和置空2种状态来表示该位置是否存在障碍物。为了降低计算量、减少冗余，更新地图时对图像进行1/4降采样，在行和列上每4个像素选取1个像素进行更新。将动态区域掩模信息应用到八叉树地图的建立当中，算法只根据静态区域内的图像信息更新八叉树地图，不在八叉树地图当中插入动态区域对应节点，这样可以一定程度上避免动态物体的信息被记录到地图当中。

更新地图时采用一种光线遍历方法，如图 11所示，从相机光心$ O $向平面像素点$ p_{i} $对应的空间点$ P_{i} $投射一道光线，遍历光线穿越的所有节点。对应图 11中3束光线的例子，且存在以下3种情况：

(1) 光线穿过若干数量的被占据节点后将路径上所有被占据节点置为空，将末端节点置为占据。

(2) 光线未穿过被占据节点，但末端落在被占据节点上，则不更新地图。

(3) 光线未穿过被占据节点，且末端不落在被占据节点上，则将末端节点置为占据。

八叉树中的光线遍历计算通过3维数值微分算法实现。动态掩模和光线遍历更新消除了地图中由于同一动态物体运动到不同位置时的信息被多次记录导致的“残影”。

4.2 稀疏点云地图与静态八叉树地图耦合

算法在关键帧上会通过RGB-D相机的深度测量结果还原部分ORB特征点在3维空间中的坐标，并将其加入到稀疏点云地图中用于特征匹配和位姿计算，如图 12所示。属于动态物体的地图点被加入地图后，将导致在该位置上后期进行位姿跟踪时会产生错误匹配，影响位姿估计精度。为了避免这种现象，使用运动分割生成的动态区域掩模帮助稀疏点云地图筛选一部分动态地图点。除此之外，还将采用一种基于八叉树地图光线遍历法的方法进一步剔除一部分动态地图点，实现2种地图之间的耦合，通过先前对场景3维结构的认知来指导稀疏点云建图。

根据特征点的2维坐标可以在深度图中查询对应的深度值，记为$ z_{\rm c} $。进一步将特征点的2维坐标$ {{\mathit{\boldsymbol{P}}}} =[u \; \; v \; \; 1 ]^{\rm T} $反投影为相机坐标系中的3维坐标$ {{\mathit{\boldsymbol{P}}}}_{\rm c} =[{x_{\rm c}} \; \; {y_{\rm c}} \; \; {z_{\rm c}} ]^{\rm T} $，$ {{\mathit{\boldsymbol{K}}}} $为相机内参矩阵：

$ \begin{align} {{\mathit{\boldsymbol{P}}}}_{\rm c} =z_{\rm c} {{\mathit{\boldsymbol{K}}}}^{-1}{{\mathit{\boldsymbol{P}}}} \end{align} $

(14)

根据相机位姿变换$ {{\mathit{\boldsymbol{T}}}}_{\rm{cw}} $得到该点在世界坐标系中的坐标$ {{\mathit{\boldsymbol{P}}}}_{\rm w} =[x \; \; y \; \; z ]^{\rm T} $：

$ \begin{align} {{\mathit{\boldsymbol{P}}}}_{\rm w} ={{\mathit{\boldsymbol{T}}}}_{\rm{cw}}^{-1}{{\mathit{\boldsymbol{P}}}}_{\rm c} ={{\mathit{\boldsymbol{R}}}}^{-1}({{{\mathit{\boldsymbol{P}}}}_{\rm c} -{{\mathit{\boldsymbol{t}}}}}) \end{align} $

(15)

其中，相机位姿变换$ {{\mathit{\boldsymbol{T}}}}_{\rm{cw}} $可以分解为旋转矩阵$ {{\mathit{\boldsymbol{R}}}} $和平移向量$ {{\mathit{\boldsymbol{t}}}} $，相机在世界坐标系中的坐标为

$ \begin{align} {{\mathit{\boldsymbol{P}}}}_{\rm{cam}} ={{\mathit{\boldsymbol{t}}}} =[{x_{\rm{cam}}} \; \; {y_{\rm{cam}}} \; \; {z_{\rm{cam}}} ]^{\rm T} \end{align} $

(16)

在八叉树地图中进行从$ { P}_{\rm{cam}} $向$ { P}_{\rm w} $方向的光线遍历，如图 13所示。与之前不同，投射的光线为一条射线，而不仅仅遍历2点之间的节点。记第1个被穿越节点的坐标为$ {{\mathit{\boldsymbol{P}}}}_{\rm w}' = [x' \; \; y' \; \; z' ]^{\rm T} $，变换到相机坐标系得到该点的深度值$ z_{\rm c}' $。由于八叉树地图中的信息是根据先前关键帧进行更新，在八叉树中进行光线遍历得到的深度值与相机测量得到的深度值有较大出入的地图点很可能在关键帧之间发生运动。

RGB-D相机在测量较远物体的深度时误差较大，因此只对4 m内的地图点作深度值检查。图 13中，如果$ z_{\rm c}' $在$ z_{\rm c} $允许的误差范围内，则保留地图点并将其插入到稀疏点云地图中。将该过程表示为

$ \begin{align} \begin{cases} z_{\rm c} <4 \\ z_{\rm c}' >z_{\rm c} +\varDelta \\ z_{\rm c}' <z_{\rm c} -\varDelta \end{cases} \end{align} $

(17)

其中$ \varDelta $为误差范围大小，这一过程的误差主要来源于采用的传感器Kinect相机的测量误差、位姿估计带来的误差、八叉树建模的精度。根据Kinect v1相机的三角测量模型^[23]可以构建相机测量深度$ z_{\rm k} $与量化视差$ {d}' $的关系，如式(18) 所示，其中$ m $和$ n $为视差$ d $量化的线性变化参数，$ z_{0} $为参考深度，$ f $为相机焦距，$ b $为相机基线长度，深度值的倒数与量化视差之间存在线性关系：

$ \begin{align} z_{\rm k}^{-1} = {\frac{m}{fb}}{d}'+ {z_{0}^{-1} +\frac{n}{fb}} \end{align} $

(18)

Kinect v1相机误差主要来源于视差量化，假设$ {d}' $为服从高斯分布的随机变量，通过式(18) 得到深度值的方差为

$ \begin{align} \sigma_{\rm z}^{2} = {\frac{\partial z}{\partial d}}\sigma_{\rm d'}^{2} \end{align} $

(19)

简化得到深度值的标准差：

$ \begin{align} \sigma_{\rm z} = {\frac{m}{fb}}z^{2}\sigma_{\rm d'} \end{align} $

(20)

由式(20) 可知，深度值测量的随机误差与物体实际深度的平方成正比。Zennaro等^[24]测量了各个距离上Kinect v1相机的测量精度，误差与深度成二次函数关系，印证了式(20) 的误差模型。将Kinect v1相机测量误差$ \varDelta_{\rm z} $建模为二次函数：

$ \begin{align} \varDelta_{\rm z} =\beta_{1} z^{2}+\beta_{2} z+\beta_{3} \end{align} $

(21)

通过最小二乘法拟合文中的测量结果得到$ \beta_{1} $ $ = $ $ 0.02 $、$ \beta_{2} =-0.0749 $、$ \beta_{3} =0.0942 $。位姿估计带来的误差主要指关键帧之间的位姿漂移，根据下文中测得的相对位姿误差的数量级取$ \varDelta_{\rm t} =0.1 $ m，设定的八叉树建模精度为$ \varDelta_{\rm o} =0.04 $ m，合成误差表示为

$ \begin{align} \varDelta =\varDelta_{\rm z} +\varDelta_{\rm t} +\varDelta_{\rm o} \end{align} $

(22)

本文提出的运动分割方法基于ORB特征点与稀疏光流计算，对纹理和垂直于深度方向的运动比较敏感。光线遍历方法对低纹理和沿深度方向运动的物体也具有很好的敏感性。基于八叉树光线遍历的双地图耦合方法对基于运动分割的地图点筛选形成了很好的补充。

5 实验与分析（Experiment and analysis）

本文实验使用的计算机硬件参数为：AMD Ryzen 5 2600X CPU，主频3.6 GHz，16 GB内存，不使用GPU加速。软件环境为Ubuntu 16.04系统及ROS（robot operating system）Kinetic框架。通过公开数据集实验验证算法在动态场景中的定位精度，通过移动机器人实验验证算法在实际机器人平台上工作时运动分割、抵抗运动干扰、在线建图的多种能力以及运行效率。

5.1 TUM RGB-D数据集定位精度与时效实验

TUM RGB-D数据集是由慕尼黑工业大学发布的SLAM评测数据集，主要针对纹理丰富的办公室场景，包含手持RGB-D相机拍摄的图像序列及使用高精度运动捕捉设备获取的相机轨迹真值。图像帧率30帧/s，分辨率为640$ \times $480。使用其中的“sitting”序列与“walking”序列来测试算法，2组序列都在同一办公室场景中拍摄，区别在于画面中人物的运动程度不同。“sitting”是典型的低动态场景，而“walking”属于高动态场景，2组序列分别包含4种不同的相机运动类型。

5.1.1 定位精度分析

为了评价SLAM系统定位的精确性，使用绝对轨迹误差（absolute trajectory error，ATE）和相对位姿误差（relative pose error，RPE）作为评价指标。ATE表示每一时刻位姿估计与位姿真值之间的绝对误差，反映了轨迹的全局一致性。由于在一个图像序列上的不同时刻可以计算得到多个ATE值，统计ATE的均方根误差（root mean squared error，RMSE）来评价SLAM位姿估计的精度。RPE每隔一段相同时间（取为1帧对应的时间）分别计算位姿估计和位姿真值的变化量，然后计算这2个变化量的差值，该差值能反映定位的漂移情况。

为说明双地图耦合方法的有效性，分别在关闭和开启双地图耦合的情况下运行算法。关闭双地图耦合时算法无法通过对场景3维结构的认知筛选动态地图点，仅能通过网格化运动分割进行筛选。

表 1列出了各种算法在8个序列下运行得到的绝对误差的RMSE值，其中组别“本文算法-A”为关闭双地图耦合的本文算法，“本文算法-B”为开启双地图耦合的本文算法。同时，选择4种动态SLAM方案进行对比，分别采用了帧差法^[6]、稠密光流法^[3]、深度学习方法^[13]以及几何与运动约束法^[10]，前3种方法都能提供像素级的动态分割。

可以看出，本文算法在低动态的“sitting”序列中与ORB-SLAM3算法有相似的定位精度，但在所有的高动态“walking”序列中，相较ORB-SLAM3算法其定位精度都有较大的提升，ORB-SLAM3算法在多个高动态场景都受到了动态物体的严重干扰，发生了位姿漂移，产生了很高的定位误差。进一步地，本文算法在不进行双地图耦合的情况下，已经能够在大部分场景中取得相对可靠的定位结果。但开启八叉树建图和基于八叉树地图的地图点筛选之后，高动态“walking”序列中算法的定位精度均有一定程度的提高，与无法适应动态场景的ORB-SLAM3算法相比，本文算法在这些序列上的定位精度误差平均下降了91.5%。

对比“本文算法-B”与其他几种动态SLAM方案，所有方案都可以有效抵抗动态场景的干扰。除去相机激烈转动导致大部分算法表现不佳的“walking_rpy”序列，本文算法的定位精度整体优于帧差法与稠密光流法，而与几何与运动约束法、DS-SLAM算法在同一数量级上，在“walking_xyz”序列上甚至具有最低的绝对轨迹误差。但是其中几何与运动约束法基于稀疏地图点滤除动态特征，对图像的动态物体区域没有感知，且不提供在线的稠密建图。本文算法的精度与DS-SLAM算法接近，其中在“rpy”“xyz”序列上更高。说明本文算法在动态场景中具有良好的定位精度。

特别地，在“walking_xyz”序列中，开关八叉树遍历的地图点筛选方法前后定位精度有较大的提升，如图 14所示。图 14(a)为ORB-SLAM3算法、关闭双地图耦合的本文算法（本文算法-A）、开启双地图耦合的本文算法（本文算法-B）在该序列上的ATE图像，将对应时间戳上的相机坐标误差用红色连线表示。图 14(b)为这3种算法对应的RPE曲线。由图 14可知，未开启双地图耦合时，轨迹与真值基本吻合但轨迹起点处存在较大的漂移，反映在RPE曲线中RPE值在前3 s较大，3 s之后减小。开启双地图耦合后，轨迹的吻合程度进一步提升，对应的RPE值整体降低到更低水平。

分析该序列的特点可以发现，该序列的前几秒画面中有一显著的人物远离镜头运动。本文中的运动分割方法基于稀疏光流，依赖2维的特征信息，对于深度方向运动的敏感性较低，单纯的运动分割或将不足以处理这类运动目标。而基于八叉树地图的地图点筛选方法依赖深度值变化，2种方法互相补充，能够应对更多的情况，因此能够进一步提高动态地图点的滤除效果，提升定位精度。

5.1.2 运行效率分析

为了验证本文算法的快速性，对算法的运行效率进行分析，并与ORB-SLAM3算法、DS-SLAM算法进行对比。对于多线程架构的SLAM算法，实时跟踪相机位姿的跟踪线程的计算效率能够反映算法的整体运行效率。

对照实验在fr3/walking_xyz序列上进行，统计跟踪线程在每一帧上消耗的时间，如图 15所示。3种算法的耗时呈现显著带状分布，ORB-SLAM3算法与本文算法的耗时都集中在30~60 ms区间，DS-SLAM算法耗时集中在70~100 ms区间。本文算法耗时显著低于DS-SLAM算法，与ORB-SLAM3算法在同一水平，但ORB-SLAM3算法只能用于静态场景，不适用于动态场景。

计算算法的平均耗时，由表 2可知，DS-SLAM算法耗时相对ORB-SLAM3算法超出78.07%，而本文算法相对ORB-SLAM3算法仅引入12.90% 的额外耗时。

DS-SLAM算法的耗时主要来源于图像分割网络，且图像分割需要GPU硬件支持。本文采用的基于稀疏光流法设计的双向补偿光流法可以快速而有效地识别运动特征，因此具有很高的计算效率。

5.2 实际动态场景分割与建图实验

为了验证算法，在实际的移动机器人平台上进行室内动态场景定位、建图的可行性分析，使用TurtleBot2移动机器人平台进行实验，如图 16所示。TurtleBot2搭载Kinect v1相机，在实验室动态场景中按照设定的路径移动，移动的同时运行本文SLAM算法进行实时的定位和在线八叉树建图。

所设定的路径为以0.2 m/s的速度直线行驶1.75 m，向左旋转90$ ^{\circ} $，之后以同样的速度直线行驶2.5 m，最后向右旋转180$ ^{\circ} $。场景中的动态物体为一个走动的人，在相机视野内经过、闪避机器人。同时使用ORB-SLAM3算法和本文算法得出定位结果，由于TurtleBot2移动机器人的车轮编码器也存在一定误差，机器人的最终运动路径与设定轨迹存在少量误差，但远小于ORB-SLAM3算法由于定位漂移产生的误差。

图 17为ORB-SLAM3算法与本文算法的定位轨迹，以及TurtleBot2机器人的设定运动轨迹。当走动的人进入相机视野时，ORB-SLAM3算法的轨迹随着人的运动发生了较大的漂移，而本文算法的轨迹未受到运动物体的显著影响。

5.2.1 运动分割

在实验的图像序列中截取包含和不包含运动物体的画面，如图 18所示。下方为算法运动分割得到的动态区域掩模图像，上方为成功跟踪的特征点，其中红色为动态特征点，绿色为静态特征点。由图 18可以发现，动态区域掩模基本覆盖了画面中运动的人物，能够粗略分割物体轮廓，也能很好地区分特征点，较少将静态特征点判为动态的噪声，误判程度在合理范围内。

5.2.2 建图效果

如图 19所示，由于位姿估计未受动态物体干扰，地图未出现扭曲，房间结构清晰，与实际环境一致。所得到的八叉树地图分辨率高，包含节点的颜色信息，还原了场景的原貌。图 19(b)中地图记录了多个关键帧上运动物体的信息，形成了“残影”现象。图 19(c)中地图根据运动分割和后续的观察更新有效地剔除了“残影”，仅包含场景中的静态要素，适用于移动机器人导航等。

为进一步验证八叉树建图在大场景中建图的稳定性，先后对大楼中的房间、走廊1、门厅、走廊2场景进行建图，如图 20所示。图 20为大场景地图与其中各个场景的八叉树地图。其中走廊宽度2.40 m，高度2.80 m，走廊1长度29.25 m，走廊2长度15.25 m。实验结果表明，实测结果与八叉树地图中数据非常吻合，房间场景中人物未产生重影，地图连续、完整，对场景结构还原到位，体现了本文算法在大场景中建图具有较强的稳定性。

6 结论（Conclusion）

提出了一种室内动态环境下基于网格分割与双地图耦合的RGB-D SLAM算法。算法首先引入单应运动补偿与根据光流理论设计的双向补偿光流法来发现图像中的动态特征点。将动态特征点的位置反映到网格图像中并根据几何连通性和深度值聚类结果优化分割结果，得到图像的网格化运动分割。通过提出的双向补偿光流法和网格化处理保障了运动分割的计算效率。之后利用静态区域中的特征点最小化重投影误差，得到相机的准确位姿估计。基于位姿估计，使用网格分割和光线遍历方法构建场景的静态八叉树地图，使地图中不包含动态物体的“残影”。此外，提出双地图耦合方法，同时利用网格化运动分割图像和八叉树地图光线遍历的方法剔除稀疏点云地图中的动态地图点，提升位姿估计精度。分别在TUM数据集和实际场景中对本文算法进行了验证。相比ORB-SLAM3算法，本文算法在动态场景中的定位进度和鲁棒性有了很大提升，相比其他适用于动态场景的SLAM算法，本文算法执行效率高，定位精度高，在大场景中建图稳定性强。

在下一步的工作当中，将讨论在室内动态环境下RGB-D SLAM中使用多目标追踪算法对动态物体进行持续跟踪。同时还将融入IMU（惯性测量单元）进一步提高SLAM系统的精度和鲁棒性。