基于区域映射深度优化的动态多特征RGBD-SLAM 算法

陈孟元，郭行荣，钱润邦，程 浩

（1.安徽工程大学 电气工程学院，芜湖 241000；2.高端装备先进感知与智能控制教育部重点实验室，芜湖 241000）

近年来，随着机器人技术与计算机技术的发展，同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）受到越来越多的机器人及计算机行业从事人员的关注，SLAM 主要研究机器人在未知环境中认知自身位姿以及构建周围环境地图[1]。根据搭载传感器的不同，SLAM 技术主要分为激光和视觉两大类，早期的SLAM 技术主要采用激光雷达来实现，但由于其高昂价格的局限性，很难大规模应用于各种工业场景下；随着技术的发展，视觉传感器逐步进入大众视野，由于视觉传感器具有使用场景广、价格便宜、能够获取丰富的信息等优点，使得视觉传感器逐渐成为SLAM 研究的主流[2,3]。

目前基于特征点法的视觉SLAM系统主要分为以单一点特征和以多特征为主两大类。Mur-Artal 等人[4]提出基于单一特征点的ORB-SLAM2 算法，通过提取角点信息完成特征匹配，并通过特征数据关联得到物体运动位姿，但此系统在稀疏纹理场景下极易出现因特征数量过少而跟踪丢失的问题。Campos 等人[5]提出的ORB-SLAM3 依赖最大后验估计，并利用所有先前信息进行重定位，使系统可以在特征稀少场景中长期运行，但依然无法有效解决稀疏纹理场景下跟踪易丢失的问题。Von Gioi 等人[6]提出了线段检测器（Line Segment Detector,LSD），该算法计算图像中所有像素点的梯度大小和方向，将梯度方向变化小且相邻的点作为一个连通域，并改善和筛选连通域为直线检测结果。基于此系统引入了多特征提取的方法，Gomez-Ojeda 等人[7]提出了PL-SLAM 算法，在图像点特征稀少或分布不均匀的情况下引入LSD 检测器，并在所有线程中使用两种特征，采用两种特征的组合描述能力作为闭环检测的依据，使得该算法在稀疏纹理场景下取得了不错的效果，但当环境中出现动态物体时容易出现特征提取紊乱的问题。Li 等人[8]针对稀疏结构化场景下传统SLAM无法有效提取特征点进行准确跟踪的问题，提出了基于点、线和面的解耦-精化方法，并在位姿估计模块中使用曼哈顿关系的RGBD-SLAM。该算法通过从周围环境中提取几何特征来提高跟踪和映射精度，但在特征提取环节由于深度相机得到的深度图在物体边界出现深度不连续以及场景中闯入动态物体的问题，使得RGBD-SLAM 算法无法运行。为了减少动态物体的影响，Zhang 等人[9]利用点和线特征计算摄像机姿态，并结合深度学习语义分割与深度信息K-均值聚类算法来检测潜在的动态特征，采用两种一致性检查策略，检测和过滤动态特征。该算法能够有效解决低动态稀疏场景下的动态物体，但易因深度信息的不准确以及几何信息提取过少而出现动态物体剔除不完全的问题。

针对上述问题，本文提出基于区域映射深度优化的动态多特征 RGBD-SLAM 算法（Dynamic Multi-feature RGBD-SLAM Algorithm based on Region Mapping Depth Optimization,DR-SLAM）。该算法在RGBD-SLAM 算法的基础上，引入区域映射深度优化以解决深度不连续的问题。通过联合深度图与多视图几何剔除动态物体，改善了现有RGBD-SLAM 算法深度信息不准确的问题，提高了动态场景下系统运行的稳定性。在公开数据集中对本文算法进行验证，测试结果表明DR-SLAM 算法与ORB-SLAM3 算法、RGBD-SLAM 算法以及DS-SLAM 算法相比，在抗干扰性、定位精度、轨迹精度等方面有较大优势，表现出了良好的构图能力。

1 系统整体框架

DR-SLAM 算法框架图如图1 所示。

图1 DR-SLAM 算法框架图Fig.1 DR-SLAM algorithm framework diagram

本文算法包含深度图优化和动态物体剔除两个环节。深度图优化环节通过轻量级的AFFormer[10]网络提取语义信息，并将提取的语义信息映射到其对应的深度图上进行近邻修复，从而完成深度图的优化。动态物体剔除环节包含点特征剔除动态物体线程和点线面特征提取线程。首先利用点特征信息，联合优化图像序列与多视图几何完成动态物体的剔除，并利用双向映射背景修复模型补全剔除区域的静态信息，然后进行点线面特征的提取，为SLAM 系统定位与建图提供准确的信息。

2 深度图优化

良好的深度图像是特征信息准确跟踪的前提。在点线面SLAM 系统中，使用D435i 相机，用红外结构光进行深度测量时，容易受到环境和设备的影响。由于RGB-D 相机得到的深度图在物体边界处有深度不连续的先天性弱势，将特征根据深度值和相机内参投影至相机坐标系时，会出现由于深度不连续导致投影不准确的情况，进而降低帧与帧之间的特征跟踪精度。因此，高质量的深度图像是提高SLAM 系统性能的重要因素。为了提高深度图像的准确度，本文算法采用了AFFormer 语义分割网络。AFFormer 语义分割网络是采用Transformer 架构的新型语义分割网络，具有全视野、高效准确的优点。通过实时轻量级的网络对RGB 图像进行检测，得到图像中物体的类别集合。

其中，Clxi表示的第i个类别；n表示算法中检测到当前帧中的物体类别总数；C是类别集合。对于每个Clxi∈C，网络对其进行特征纹理的提取，得到各种特征纹理的集合。

其中，Rdepth表示的是RGB 图像深度特征纹理信息；ClV表示的是颜色值信息，以及一些其他的特征信息。

AFFormer 语义分割网络对RGB 图像中的类别进行标定，使得Clxi在RGB 图中生成对应的掩码Mxi。本文将掩码映射到深度图上对应区域，此时RGB 图像中所有类别的深度特征纹理信息通过掩码映射输送到对应的深度图像区域，得到单个类别对应的优化区域，数学表达式为：

其中，Ddepth表示深度图像；Ri表示单个类别对应的优化区域。

通常认为由深度相机生成的深度图像同一类别的深度值相差不大，为了更好地解决物体边界处出现深度不连续现象，本文采用物体内修复方法。根据随机抽样一致[11]（Random Sample Consensus,RANSAC）算法和Ri检测出正常深度值点以及异常深度值点并剔除掉异常深度值，最后通过近邻值替代法完成深度图的优化。具体做法如下：

1）通过深度相机得到在Ri的深度值Dpv，其中Dpv={Dpv1,Dpv2,Dpv3…}，其中包含正常深度值点和异常深度值点。

2）运用RANSAC 算法剔除异常值。通过步骤1）得到单个类别的所有深度值，为了得到最准确的深度值，本文选择优化区域的所有深度值作为模型的数据点。若任意两个数据点满足式(5)，则可以认为是正常深度值点，反之为异常的深度值点，并将异常值剔除。

3）近邻值替代法完成深度图的优化。通过步骤2）得到异常值点，以此点为原点四周索引，直至索引到深度正常值，用其替代异常值点，即完成深度图优化。

深度相机在不同实验场景下得到的深度值一般都是不相同的，本文经实验得出在TUM[12]数据集上的阈值X通常取0.8。

3 动态物体剔除

3.1 联合深度图与多视图几何剔除动态物体

动态物体对SLAM算法的定位精度和地图的构建有着重要影响，当前大多数点线面SLAM 系统都在静态环境中实现了高精度的定位与构图，但当环境中闯入动态物体时极易造成轨迹误差增大问题，使得整体算法缺乏克服机器人在动态环境下稳定运行的能力，从而影响整个SLAM 系统的定位与建图。因此，本文提出一种联合深度图与多视图几何剔除动态物体的方法，剔除动态物体示意图如图2 所示，其中Ck-1和Ck表示前后两帧的相机中心。

图2 剔除动态物体Fig.2 Culling dynamic objects

SLAM 系统在运行过程中，在已知相机内参和深度图像深度值的前提下，将经过深度优化的深度图像转化为点云，计算公式见式(6)。

其中，(x,y,z)是点云坐标；(u,v)是图像坐标；fx、fy为RGB-D 相机的内参。

通过式(6)获取当前图像帧与后一帧的点云地图点X={x1,x2···xn}和Y={y1,y2···yn}，并通过迭代最近点[13]（Iterative Closest Point,ICP）得到相机前后的位姿差，从而求解出相机位姿。得到相机前后的位姿差，采用最小二乘法求解旋转矩阵R∈R3×3和平移向量t∈R3，最终得到Hc，计算公式见式(7)。

其中，xi∈R3为前一帧点云；yi∈R3为后一帧点云；Mr为最小残差；Hc∈SE(3)为相机姿态相对变换矩阵；hc∈ SE(3)为相机位姿相对变换向量；Ny为匹配的点数。

其中，hc*∈R6为最小二乘解；ρh为惩罚因子；ΣP∈R4×4为R、t协方差矩阵；j为残差运算所需3D点投影至2D 点数量；e(hc)∈R4×4为hc的分解矩阵。

由相机位姿和多视图几何确定物体位姿，将所有潜在动态物体建模为位姿变换矩阵Ho∈R4×4。通过对应的RGB 相邻帧间几何关系将空间中的点从参考帧Fk-1关联到后一帧Fk，计算公式见式(9)。

其中，mk是Fk中的平面坐标；ε和ε-1分别为投影函数和反向投影函数；Ik-1是空间点投影到Fk-1中的三维点；mk-1为Ik-1在Fk-1中的平面坐标；mz-1为Ik-1在Fk-1中的深度值。

多视图几何得到的物体变换矩阵存在误差，本文通过重投影误差与最小二乘法对物体姿态变换矩阵Ho进行优化处理。同理，通过∈R6从SE(3)到 R6的映射运算，得到准确的物体运动估计矩阵ho*。

其中，e(Ho)为位姿变换矩阵优化解（最小化重投影误差）；ho为位姿变换向量，e(ho)为ho的分解矩阵；为Fk-1中mk-1的坐标投影到Fk中的坐标。

SLAM 系统在运行过程中受环境影响较大，仅通过物体运动估计矩阵难以准确判断物体是否运动。本文采用平面几何映射判断物体状态，空间中的点投影到Fk-1帧中得到2D像素坐标mk-1，此时假设mk-1通过几何关系投影到Fk中的点为，与Fk中真实投影点mk的像素距离d1为动态视觉误差权重。为了减少误差，同时将Fk中的2D像素坐标mk也投影到Fk-1中，与Fk-1中真实投影点mk-1形成动态视觉误差权重d2。由于动态物体具有丰富的像素点，故物体的动态视觉误差可由像素动态视觉误差代替。为了得到准确的物体动态视觉误差，本文采用深度值类比权重法，计算公式见式(11)。

其中，mz为空间中的点到Fk中mk的深度值；θ为物体动态视觉误差。

姿态不确定性是判断物体是否运动的重要指标，姿态不确定性可以通过微分熵值从光度残差最小化导出，高姿态不确定性的三维空间点运动观测将导致图像上某物体产生较大的位移，反之低姿态不确定性将产生较小图像差异，因此本文采用运动熵作为判断物体运动的依据，计算公式见式(12)。

其中，H(θ)为运动熵；e为自然指数；Pθ为不确定误差，服从ω维高斯分布，通常Pθ=（JT∑-1J）-1，∑-1为对角矩阵，J为雅各比矩阵。

基于此，将物体动态视觉误差与一个由运动微分熵f(H(θ))引导并随熵缓慢变大的动态阈值Δθ=f(H(θ))进行对比，其中f(·) 为求微分。若θ＞Δθ，则判断为动态物体并将其剔除。动态物体剔除方法如表1 所示。

表1 动态物体剔除算法Tab.1 Dynamic object elimination algorithm

3.2 背景修复与点线面特征提取

剔除动态物体后用于特征提取的信息减少，使得本算法在多动态物体场景中位姿估计精度降低，继而影响回环检测和全静态场景地图构建。因此，本文提出一种基于双向映射模型的背景修复方法，借助前后帧中的静态信息修复剔除区域RGB 图像和深度图静态背景。完成静态背景修复后，进行点线面特征的提取以完成SLAM 定位与建图的任务。

基于双向映射的背景修复示意图如图3 所示，以待修复关键帧Ft为起点，向前后箭头方向共移动8 帧的时间窗口。窗口内的关键帧图像依次与待修复关键帧图像对齐，当所有图像对齐后，将待修复关键帧图像的剔除区域沿着黑色虚线箭头方向索引所对应的像素。若索引到一个对应像素，直接进行缺失区域像素补全，若索引到多个对应像素值，则对索引到的像素取平均值，然后再进行缺失区域的像素补全。

图3 基于双向映射的背景修复Fig.3 Bidirectional mapping based background repair

4 实验结果与分析

本文实验所用电脑设备：CPU 为 Inter Core i7-10080F、8 核处理器、主频2.9 GHz、内存16 GB；系统为Ubuntu18.04。为了验证本文算法的有效性，使用TUM 公开数据集进行验证。

4.1 深度优化

深度优化效果如图4 所示，其中图4(a)～图4(d)为TUM 数据集中四帧原始图像帧，图4(e)～图4(h)为四帧经过图像对比度处理的原始深度图像帧，图 4(i)～ 图4(l)为四帧采用本文算法优化后得到的深度图像帧。由于深度相机在运行过程中易受环境中噪音影响，使产生的深度图像存在深度缺失，尤其是在动态场景及两个物体的连接处。从图4(i)-图4(l)可以看出，采用本文算法优化得到的深度图像中存在的黑洞值大多数都被修复，而且从图4(e)和图4(i)中能够看出本文算法能够粗略重现深度图像中一部分深度完全丢失的地方，如图4(a)中的手臂和后背就被重现出来，深度优化算法提高了特征跟踪的精度，为后续动态物体剔除提供了良好的深度依据。

由于RGB-D 相机自身的原因，得到的深度图会出现深度不连续的问题，影响特征跟踪精度。当特征点根据深度值和相机内参投影至相机坐标系时，存在由于深度错误影响投影精度的情况，进而降低帧与帧之间的特征跟踪的准确率。为了体现深度优化算法与SLAM 系统的联系以及在SLAM 线程中的优势，本文采用特征跟踪准确率（Feature Tracking Accuracy）作为深度优化算法的评价指标。特征跟踪准确率指的是当前帧中提取的特征点投影到下一帧的成功率，特征跟踪的准确率越高表示跟踪精度越高。本文在TUM数据集上选取了四个高动态序列（fr3_walking_xyz、f r3_walking_static、fr3_walking_rpy、fr3_walking_half sphere）以及两个无动态序列（fr2_xyz、fr2_rpy），并将每个序列图像帧数据的中位数（Median）与平均值（Mean）作为实验数据指标。表2 给出ORB-SLAM3、RGBD-SLAM、不加优化的本文算法（DR-SLAM wit hout Optimization,DR-SLAM-WO）和本文DR-SLA M 算法在TUM 数据集下的特征跟踪准确率，表中加粗黑色字体表示最优值。从表2 可以看出，在四个高动态序列中，DR-SLAM 的特征跟踪准确率是最高的，DR-SLAM-WO 的特征跟踪准确率是次高的；在两个无动态序列中，DR-SLAM 的特征跟踪准确率与ORB-SLAM3 几乎相同且都高于DR-SLAM-WO 和RGBDSLAM，验证了本文深度优化算法能够提高动态SLA M 系统特征跟踪精度，从而提升动态SLAM 系统的位姿估计准确率，为减少轨迹误差提供良好基础。

表2 四种算法在TUM 数据集下的特征跟踪准确率（单位：%）Tab.2 Feature tracking accuracy of four algorithms in TUM dataset (Unit: %)

4.2 点特征剔除动态物体效果图

点特征动态物体剔除效果如图5 所示，其中图5(a)～图5(d)为在TUM 数据集上任意选取的四个动态原始帧；图5(e)～图5(h)为四个动态物体剔除图像帧，从图中可以看出，DR-SLAM 在动态物体场景下能有效剔除动态物体；图5(i)～图5(l)为与动态物体剔除帧对应的稀疏地图点帧。如图5(j)所示，其中红色点为静态特征点，黑色点为本文标记的动态特征点。随着动态物体的减少，黑色点的数量也逐渐减少，验证了DR-SLAM 能够有效区分动态特征点，为后续的多特征信息提取减少了动态物体的影响。

图5 点特征动态物体剔除效果Fig.5 Point feature dynamic object elimination effect

4.3 背景修复与点线面特征提取

背景修复与点线面特征提取效果如图6 所示，其中图6(a)～图6(d)为在TUM 数据集上任意选取的四个动态原始帧；图6(e)～图6(h)为四个原始图像帧经修复后的图像帧，可以看出双向映射模型能有效地进行剔除区域静态背景的修复；图6(i)～图(l)为静态背景修复后的点线面特征提取帧。本文通过剔除动态物体后进行背景修复，并进行点线面特征的提取，能够有效降低动态物体对构建地图的影响，提供了良好的位姿估计精度，从而减少轨迹误差。

图6 背景修复与点线面特征提取效果Fig.6 Effect of background restoration and point,line and surface feature extraction

4.4 TUM 数据集验证与评估

为了验证DR-SLAM 剔除动态物体的能力，本文选取TUM 数据集的三个动态序列fr3_walking_xy（z图中为W_xyz：相机原地左右上下转动拍摄的数据集）、fr3_walking_rpy（图中为W_rpy：相机上下左右移动拍摄的数据集）、fr3_walking_halfsphere（图中为W_half：相机左右上下以及翻转拍摄的数据集）进行验证评估。图7 为ORB-SLAM3、RGBD-SLAM、DS-SLAM[21]和DR-SLAM 四种算法在这三个序列上运行的轨迹对比，图中黑色线为相机的真实运动轨迹，蓝色线为系统依据算法估计出的相机运动轨迹，红色线为相机运行轨迹和真实轨迹之间的误差。ORB-SLAM3 和RGBD-SLAM 为静态环境中的SLAM算法，DS-SLAM 和DR-SLAM 为动态环境中的SLAM算法。从图7(a)和图7(b)可以看出，由于ORB-SLAM3和RGBD-SLAM 无法识别环境中存在的动态物体，因此所生成的轨迹与真实轨迹相比存在较大的轨迹误差；从图7(d)可以看出，由于DR-SLAM 减小了深度不连续对动态物体的影响，所以DR-SLAM 生成的轨迹误差小于其他三种算法。图7(e)～图7(l)为四种算法在另两组动态序列的轨迹对比，从图中可以看出ORB-SLAM3、RGBD-SLAM 和DS-SLAM 算法所生成的轨迹误差依旧较大，而DR-SLAM 所生成的轨迹误差还是小于其他三种算法，验证了DR-SLAM 对于环境中的动态物体具有良好的剔除能力，能够有效减少轨迹误差。

图7 TUM 数据集验证评估Fig.7 TUM data set validation evaluation

为了进一步验证DR-SLAM 在动态环境以及静态环境中轨迹精度高、误差小的性能，在上述实验的三个序列上增加fr3_walking_static（相机拍摄场景中的人物是静止的，没有动态物体）序列来评测算法的定位与建图能力。为了消除偶然误差，本文在每个序列上进行5 次实验，并且取均方根误差（Root Mean Square Error,RMSE）、平均值（Mean）、标准差（Standard Deviation,SD）作为衡量单位。表3 给出ORB-SLAM3、RGBD-SLAM、DS-SLAM 和本文DR-SLAM 四种算法的绝对轨迹误差（Absolute Trajectory Error,ATE），表中加粗黑体表示最优数值。由于ORB-SLAM3、RGBD-SLAM 和DS-SLAM 三种算法都是直接通过深度相机获得深度图，存在深度值丢失的问题，易出现跟踪精度下降，导致无法准确剔除动态点，从而无法有效完整地剔除动态物体。而DR-SLAM 通过深度图优化以及多视图几何双向投影进行动态物体剔除并进行背景修复，既避免了深度黑洞值的影响又保留了大量的特征信息，该算法四个序列的所有衡量指标都达到最好。通常SLAM 系统中RMSE 指标最能反映轨迹精度优劣，本文选取RMSE指标做定量分析。在 fr3_walking_xyz 序列上，DR-SLAM 相较于ORB-SLAM3、RGBD-SLAM 以及DS-SLAM 的绝对轨迹误差RMSE 分别减少92.6%、87.5%和 28.2%；在 fr3_walking_static 序列上，DR-SLAM 相较于ORB-SLAM3、RGBD-SLAM 以及DS-SLAM 的绝对轨迹误差RMSE 分别减少91.5%、91.8%和37.5%；在fr3_walking_rpy 序列上，DR-SLAM相较于ORB-SLAM3、RGBD-SLAM 以及DS-SLAM的绝对轨迹误差RMSE 分别减少76.4%、65.3%和3.3%；在fr3_walking_halfsphere 序列上，DR-SLAM相较于ORB-SLAM3、RGBD-SLAM 以及DS-SLAM的绝对轨迹误差RMSE 分别减少52.3%、80.9%和1.4%。总的来说，DR-SLAM 相较于ORB-SLAM3、RGBD-SLAM 以及DS-SLAM 的平均绝对轨迹误差RMSE 分别减少78.2%、81.4%和17.6%，验证了本文动态剔除算法能够有效剔除环境中的动态物体，为SLAM 系统的定位与建图提供了良好基础。

表3 四种算法在TUM 数据集下的ATE 对比（单位：米）Tab.3 Comparison of ATE of four algorithms in TUM dataset (Unit: m)

图8 为ORB-SLAM3、RGBD-SLAM、DS-SLAM和DR-SLAM 四种算法在TUM 数据集运行时间的对比。

图8 四种算法在TUM 数据集下运行时间对比Fig.8 Comparison of running time of four algorithms in TUM dataset

从图8 可以看出，四种算法运行时间的平均值分别为105 s、101 s、537 s 和458 s。由于DR-SLAM 减少了深度不连续对SLAM 系统的影响，使DR-SLAM运行时间平均值与DS-SLAM 相比减少了15%；与RGBD-SLAM 和ORB-SLAM3 相比，由于DR-SLAM增加了语义分割网络映射对深度图的优化以及动态物体的剔除，使得DR-SLAM 运行时间较RGBD-SLAM有较大的增加，但仍可满足SLAM系统正常运行需求。

4.5 真实场景实验

实验平台为Husky 轮式机器人，其硬件配置为：CPU 为i7-10875H 处理器，内存8 GB；GPU 为GTX1080，操作系统为Ubuntu18.04。真实环境如图9(a)所示，场地大小为12 m×5 m。图9(b)为真实场景布局，图中黄色和蓝色部分为实验室工作台。

图9 真实场景设置Fig.9 Real scene settings

图10 为真实场景下 RGBD-SLAM 算法与DR-SLAM 算法的定位轨迹对比。本文算法是针对RGBD-SLAM 的问题进行研究的，而ORB-SLAM3 一般是在静态场景中运行，无法有效地在动态场景中工作，又因为DS-SLAM 的分割算法只能分割简单的物体，无法在实验室复杂的环境中运行（易分割失败），故本文只与RGBD-SLAM 作对比。从图中可以看出，DR-SLAM 与RGBD-SLAM 的绝对轨迹误差分别为0.078 m 和0.635 m，这是由于RGBD-SLAM 算法无法有效剔除动态物体，导致算法运行轨迹误差较大，而本文算法加入深度优化修复深度不连续以及联合多视图几何剔除动态物体，提高了特征匹配精度，减小了轨迹误差，因此本文算法在动态场景下能稳定运行，具有较高的鲁棒性。

图10 轨迹对比Fig.10 Trajectory comparison

5 结论

本文提出了一种DR-SLAM 算法，该算法具有以下创新点：1）提出一种基于语义信息的深度图优化，改善了点线面SLAM在特征提取环节由于深度相机得到的深度图易出现深度不连续影响特征跟踪的问题，提高了SLAM 系统的位姿估计精度。2）针对突然出现动态物体造成轨迹误差增大的问题，提出一种联合深度信息与多视图几何完成动态物体剔除的方法，并利用双向映射模型进行静态背景的修复，提高了定位的精度并减少轨迹误差。本文算法在公开数据集TUM上进行测试，结果表明，由于DR-SLAM 采用区域映射深度优化获取了良好的深度信息，以及联合多视图几何剔除动态物体和背景修复，使得本文算法与RGBD-SLAM 和DS-SLAM 算法相比，在位姿和轨迹精度方面具有良好的优势，体现了良好的抗干扰能力与动态物体剔除能力。