基于深度学习和边缘检测的动态场景下鲁棒SLAM*

李璐琪蔡成林

(湘潭大学自动化与电子信息学院，湖南 湘潭411105)

同时定位与地图创建(simultaneous localization and mapping，SLAM)是机器人研究领域的热点技术，是机器人实现环境感知与场景三维重建，自主避障与路径规划，自动驾驶与导航等应用的关键[1]。随着研究的不断发展，机器人搭载的传感器也更加多元化，包括视觉，激光，雷达，以及多传感器融合的方式，视觉SLAM 则是指以相机、摄像头等视觉传感器作为主要数据采集器，图像序列作为信息源的定位建图方法[2]。 现代视觉SLAM 系统已经相当成熟，并具有令人满意的性能，例如基于特征点法的ORB＿SLAM2[3]，利用ORB 特征[4]进行计算，支持单目、双目、RGB-D 三种模式，使用跟踪、局部建图、回环检测的三线程结构，构建稀疏特征点地图；基于半直接法的LSD-SLAM[5]，利用直接图像对齐进行高精度位姿估计，实时重建3D 环境作为与半稠密深度图相关的关键帧位姿图，该算法允许构建大规模半稠密的三维地图；基于直接法的DTAM[6]，依赖于单个像素的光度变化、纹理信息，通过直接最小化光度误差和全局空间正则化来估计出完全密集的重构。

但是以上经典SLAM 系统都是建立在场景中的物体均是静态的假设前提下，对动态物体的检测和处理十分有限，然而实际的室内外场景中，无法规避运动物体，比如行走的人或动物、行驶的自行车或汽车。 在这种情况下，周围环境的意外变化可能会严重影响相机位姿估计，增大轨迹误差甚至导致系统故障。 于是，运动物体的检测与动态区域的正确分割成为动态场景下视觉SLAM 的重要研究环节。 目前，有学者使用多传感器融合来检测运动物体，Chavez-Garcia 等[7]提出用雷达，激光雷达和摄像头构建环境感知模型来进行运动物体检测，Popov[8]等使用Kinect 传感器和2D 激光雷达对移动目标进行检测，有学者使用密集场景流来检测运动物体，Pan等[9]通过估计3D 场景流来分割多个运动对象从而恢复潜在的清晰图，Xiao 等[10]通过计算视差图和密集光流场获得超像素再输入到条件随机场模型进行动态静态分割，有学者使用深度学习来检测运动物体，Runz 等[11]使用MASK-RCNN[12]充分利用实例级语义分割的优势，以使语义标签能够融合到一个对象感知映射中，可以识别，检测，跟踪和重建多个移动的刚性物体，Brickwedde 等[13]提出的monostixels 基于逐像素语义分割来处理运动对象，并在KITTI 数据集[14]上实验，表明单像素可对场景的静态和运动部分进行紧凑而可靠的深度重建，Chen等[15]利用CNN 目标检测技术来检测当前视图中的可能移动对象，并让SLAM 的前端使用边界框以外的局部特征来实现连续的跟踪和映射，从而可以消除由移动物体引起的噪声，Bescos[16]等提出的DynaSLAM 通过使用多视几何、深度学习或者两者兼有的方法实现移动物体的检测，并且通过对动态物体遮挡的背景帧进行修复，生成静态场景地图。

但是，多传感器融合方案需要添加来自不同传感器源的信息，面临数据关联、信号同步，融合处理等多重困难，大大增加了系统的复杂性，密集场景流的方法计算量大，对实时计算是一个很大的挑战，深度学习的方法通过深度神经网络对图像强大的学习能力，容易标记出潜在运动物体，并提供语义信息可构建语义地图，丰富机器人对环境的理解从而获得高级感知，但是存在对于可移动物体误判以及动态物体检测框过大或边缘不对齐的问题。

针对以上问题，本文提出一种更加鲁棒的动态场景下的SLAM 算法，采用深度学习快速识别动态物体框与稀疏特征光流计算做进一步动态判断相结合，并利用边缘检测算法对动态物体的边缘进行有效分割，保证没有过多的静态特征点被误删除，构建无动态物体的静态环境三维点云地图以真正实现自主机器人的强大感知和导航能力。

1 系统整体框架和流程

本研究采用YOLOv4[17]实时目标检测算法快速获取输入的彩色三通道图像中潜在的语义动态物体粗略的矩形范围，同时提取ORB 特征点并计算光流场，仅计算所跟踪的ORB 特征点的光流场可很大程度减少计算全部像素点光流场的时间，通过语义信息与光流场计算筛选出的动态特征点相互结合可获得真正运动的物体，再针对动态物体采用canny 算子[18]进行边缘检测，得到动态物体的边缘信息，通过最小化动态物体以外静态特征点的重投影误差进行相机位姿估计。 最后，使用剔除了动态物体的关键帧构建地图。 整体流程如图1 所示。

图1 算法流程

1.1 基于深度学习的目标检测

YOLOv4 是YOLO 系列的目标检测算法，是目前速度和精度平衡得最好的算法，因此本研究选取该网络进行SLAM 过程中图像语义信息处理。YOLOv4 网络采用CSPDarkNet53[19]作为主干网络提取图像特征，CSPDarkNet53 是在Darknet53 的每个大残差块上加上CSP，共有5 个大残差块，每个大残差块所包含的小残差单元个数为1、2、8、8、4。 在特征金字塔部分，使用SPP[20]和PANet[21]，可以有效增加主干特征的感受野，分离最显著的上下文特征，SPP 对CSPdarknet53 的最后一个特征层进行三次DarknetConv2D＿BN＿Leaky 卷积后，再进行最大池化处理，最大池化的池化核大小分别为13×13、9×9、5×5，完成池化后进行张量拼接并通过1×1 降维到512 个通道，PANet 实现特征的反复提取，在完成特征金字塔从下到上的特征提取后，还需要实现从上到下的特征提取。 分类回归层采用YOLOv3[22]头部进行预测输出，提取三个特征层获得预测结果，采用MS COCO 数据集进行训练，输入图片大小为608×608，包含80 种不同类别，三个特征层的形状为(19，19，255)，(38，38，255)，(76，76，255)，最后的维度255 ＝3×85，85 包含了4＋1＋80，分别代表x＿offset、y＿offset、height 和width、置信度、分类结果，网络结构如图2 所示。 在预训练中，训练步数为500，500，采用阶跃衰减学习率调度策略，初始学习率为0.01，在400 000 步和450 000 步分别乘以因子0.1，动量衰减和权重衰减分别设置为0.9 和0.000 5，训练曲线如图3 所示。

图2 YOLOv4 网络结构

图3 训练曲线图

首先将图像输入至YOLOv4，获取先验语义信息，即得到目标类别，置信度，所在矩形框的位置及大小，如图4(a)所示。 根据输出结果，可大致得到图像的语义动态区域和语义静态区域。 语义动态区域即传统意义上运动目标区域，比如人，动物，汽车等具有主动运动能力的物体类别区域，语义静态区域即为非语义动态区域。 实际上语义动态区域包含动态物体以外的静态场景，如图4(b)所示。 如果在SLAM 算法中直接去除语义动态区域，不利于精确的相机位姿估计，甚至当过多的静态环境被包含在语义动态区域时，会造成相机定位失败。 因此有必要进一步对动态物体进行精确判定与划分。 另外，如果场景中的人并没有移动就不属于动态物体或者人坐着的时候转动椅子，那椅子就也变成运动物体。总之，不能简单地认为语义动静态区域就是真实的动静态区域，也就是说通过YOLOv4 算法得到的语义信息能够提供先验知识以及丰富的物体类别信息，但在实际的动静态判断上是模糊的，需要结合接下来的特征点光流信息进一步做出准确判断。

图4 YOLOv4 目标检测获取的语义动静态区域信息

1.2 ORB 特征点提取与光流计算

为了进行物体动静态分析同时节约计算成本保证实时性，本研究针对提取的ORB 特征点对其进行光流场计算估计其运动状态。 ORB 特征点的提取主要分为FAST 角点提取和BRIEF 描述子计算两部分，具体步骤如下。

①构造图像金字塔，对金字塔的每一层使用基于四叉树的均匀提取策略提取FAST 角点，如图5所示，具体计算过程如下:

第1 步 在图像中选取像素p，获取它的亮度，假设为Ip；

第2 步 设置阈值T＝Ip×0.2；

第3 步 遍历以像素p为中心的半径为3 的圆上的16 个像素点；

第4 步 设每个遍历点的亮度为Icp，若存在连续N个点，其Icp＞IP＋T或者Icp＜Ip-T，则认定该点为特征点，本研究中N为12；

第5 步 对图像中每个像素点执行上述操作。

图5 FAST 角点计算

②利用灰度质心法，计算FAST 角点旋转角度，定义图像的矩为:

式中:I(x，y)FAST 角点(x，y)灰度值，a，b为矩的阶次，图像的质心坐标为:

旋转角度为:

③计算旋转后的BRIEF 描述子，选取S×S的窗口W，定义:

式中:I(x)是x处的灰度值，随机选取n对特征点，生成n维BRIEF 描述子向量:

引入方向因子θ，则特征点的描述子为:

式中:Sθ为定义的一个2×n的矩阵:

由方向因子θ的旋转矩阵Rθ变换所得，即Sθ＝RθS。

同时对提取的ORB 特征点计算光流场，筛选动态特征点，再将动态特征点与语义信息结合选取动态物体。 如图6(a)所示，特征点集合Φ＝{λ1，λ2，…，λn}，其中每个λi＝{xi，yi，mi}，(xi，yi)为像素坐标，mi为物体类别信息。 假设灰度不变，运动方式一致且幅度小，任意第i个特征点λi＝(xi，yi)在t时刻的灰度为Ii(xi，yi，ti)，在t＋δt时刻的灰度为Ii(xi＋δxi，yi＋δyi，ti＋δti)，则:

用泰勒公式展开可得:

利用Locas-Kanade 算法，对光流值局部进行调整，假设光流在λi为中心的空间小领域内基本相同，取7×7 个特征点联立方程进行计算:

这是一个超定线性方程:

利用最小二乘法来求解，可得λi的光流信息为:

如图6(b)所示。 设[Uj，Vj]T为mj所在矩形区域的背景光流场，根据

计算此区域背景运动速度，式中，k＝1，2，…N为此区域内所有特征点。 利用背景运动速度进行动态特征点筛选，若λk的运动速度满足

则λk为动态点，本研究中在时间t取1 的情况下ε为13，如图6(c)所示，进一步地，mj含有的动态特征点的数目大于一定阈值，则mj为动态物体，本研究中提取的特征点数目为1 000，阈值为20，如图6(d)所示。

图6 特征提取与光流计算

1.3 基于canny 算子的动态物体边缘检测

直接去除动态物体矩形区域则去掉了过多的静态场景，不利于精确的相机定位与地图构建。 为了更准确地提取出动态物体的轮廓，本研究采用canny 算子对筛选出的动态物体(如图7(a)所示)进行边缘检测，如图7(b)所示。 canny 算子属于二阶微分算子，通过给定图像边缘处的二阶导数零点来提取图像的边缘，相比于一阶微分算子提取的边缘更细，连续性更好，不容易受噪声的干扰，而且使用两种不同的阈值分别检测强边缘和弱边缘，能够检测到真正的弱边缘。 具体步骤如下。

①消除图像噪声。 首先利用高斯函数对图像进行平滑处理，设f(x，y)为输入图像，g(x，y)为输出图像，G(x，y)为高斯函数，定义为:

进行卷积降噪，则:

②计算梯度幅值和方向。 利用高斯滤波之后的图像，选用合适的梯度算子计算相邻像素之间一阶偏导的有限差分来计算各像素点梯度的大小和方向。

式中:Ax，Ay为Sobel 梯度算子，Ex，Ey为水平和垂直方向的差分，则梯度幅值和方向为

图7 动态物体边缘检测

③过滤非极大值。 在高斯滤波过程中，边缘有可能被放大了，采用非极大值抑制方法来过滤不是边缘的点。 若当前计算出的梯度幅值在该点所在领域内，大于沿该点梯度方向上其他相邻2 个像素点的梯度幅值，则该点属于可能的边缘点，否则不是，采取抑制手段，将灰度值设为0。

④双阀值检测和连接边缘。 经过以上步骤的处理仅得到候选边缘点，再采用上、下阈值的检测处理来剔除伪边缘点。 大于上阈值的点都被检测为边缘点，小于下阈值的点都被检测为非边缘点，介于两个值之间的点则被检测为弱边缘点，如果与确定为边缘点的像素点邻接，则判定为边缘点；否则为非边缘点。

1.4 基于动态物体剔除的位姿估计与点云地图构建

1.4.1 基于静态特征点的位姿估计

在确定了动态物体的准确轮廓之后，剔除分布在动态物体内的动态特征点，仅利用非动态区域的稳定特征点进行更为精确的相机位姿求解，如图8 所示。设为当前帧c中静态特征点的像素坐标，利用深度值可得3D空间点坐标

式中:fx，fy为相机焦距，(cx，cy)为相机主点坐标，本研究中fx＝535.4，fy＝539.2，cx＝320.1，cy＝247.6。设参考关键帧k，当前帧c和参考关键帧k之间相机的位姿变换矩阵:

图8 位姿估计过程运行图

式中:为旋转矩阵，为平移向量，则空间坐标点对应参考关键帧的空间坐标点

将投影至参考关键帧k的图像平面得到其投影的像素坐标

式中:ω(·)为投影函数。 通过特征匹配知道在参考关键帧k中匹配点，对关键帧中所有特征点构造重投影误差函数:

利用迭代非线性优化算法进行求解，每次迭代使用李代数ζ∈se(3)作为位姿的扰动量进行优化，左乘对位姿进行更新:

采用列文伯格-马夸尔特方法作为梯度下降策略，求解过程中重投影误差对于位姿增量的雅可比矩阵为:

式中:ex和ey分别为重投影误差在横纵方向的梯度值，求解所得的(R，t)即为相机位姿变换。

1.4.2 剔除动态物体的静态环境点云地图构建

构建环境3D 点云地图，能够更好地提供环境可视化。 利用点云携带的语义信息则能为机器人导航避障提供依据。 在构建点云时，若位姿存在较大误差，会造成地图出现明显的交错重叠，不利于导航。 利用剔除了动态物体之后的点云进行叠加，能有效解决这个问题。

利用ORB＿SLAM2 算法获得关键帧，将所有关键帧的点云进行叠加，太过繁复冗余，可通过精简关键帧数量进一步提高建图的效率。 在关键帧筛选的过程中，考虑以下两个策略:①关键帧有效性判断。若被剔除的点云面积超过当前关键帧面积的一半，则认为该关键帧包含的有效信息不足，不参与叠加。②关键帧冗余性判断。 能被多个关键帧观测到的特征点，称为多个关键帧的共视地标点。 对当前关键帧观测到的共视地标点做检测，假设已确定的绘图关键帧集合为F，观测到的地标点集合为L，当前关键帧观测到的地标点集合Lc，若L∩Lc的数量超过Lc的一半，则认为当前关键帧包含过多共视地标点，信息冗余，不参与叠加。

满足以上两个条件，则更新F以及L，这样既能保证引入新的点云信息又能保证有足够多的静态环境信息，运行过程中跟踪的关键帧如图9 所示。

图9 跟踪的关键帧

2 实验与分析

本节采用TUM-RGBD 数据集以及其提供的测评工具从位姿估计误差和点云地图构建两方面对系统综合能力进行评估，并与ORB＿SLAM2 以及DynaSLAM(N)仅使用MASK-RCNN 进行动态物体分割的方法对比。 为了检验动态物体的处理对SLAM算法的影响，选取dynamic objects 分类下的walking序列进行实验。 此序列为两个人在办公桌周围运动行走，halfsphere、rpy、xyz、static 分别表示Asus Xtion传感器在拍摄过程中的运动方式。

2.1 位姿估计误差实验

本实验首先在数据集的四个高动态序列上进行测试，获取本文系统的估计轨迹，再使用位姿估计评估工具计算估计轨迹与真实轨迹之间的绝对轨迹误差(absolute trajectory erro，ATE)来进行评估，并与ORB＿SLAM2 以及DynaSLAM(N)的运行结果进行比较。 ATE 统计对比结果如表1 所示，其中:RMSE为比对数据之间的均方根误差，Mean 为平均误差，Median 为中值误差，std 为标准偏差。

由表1 可以看出，由于本文算法增加了动态物体处理，并能较为准确的检测出动态物体区域，大大减小了轨迹误差，定位精度明显优于ORB＿SLAM2，与DynaSLAM 对比在半球面运动、rpy 运动和静止序列中稍有提升，但是DynaSLAM 是采用MASKRCNN 实例分割算法提取场景中动态物体的语义信息，运行速度较慢，实时性不是太好。 ORB＿SLAM2与本文算法得到的估计轨迹与真实轨迹之间的误差如图10 所示，图中黑线为真实轨迹，蓝线为估计轨迹，红线为两者之间的误差。 从图中可以看出，本文算法的误差明显减少。

表1 动态环境下的ATE 统计对比

图10 ORB＿SLAM2 算法与本文算法的轨迹对比

2.2 点云地图构建实验

本实验采用ORB＿SLAM2 和本文算法在以上四个高动态序列中运行得到的位姿估计数据以及关键帧进行点云地图叠加，叠加过程中考虑关键帧的冗余性和有效性。 如图11 所示，实验表明剔除动态物体的点云地图能有效改善地图重叠交错的现象，构建动态场景中较为准确的静态环境图，大大提高地图可读性。

图11 ORB＿SLAM2 算法与本文算法的点云地图对比

本实验在配置为Intel Xeon E5-2600 V3 CPU，RTX2080TI GPU，16G 内存，11G 显存的服务器上进行，ORB＿SLAM2 平均速度为32 帧/s，目标检测线程平均速度为51 帧/s，slam 与目标检测并行进行，增加光流信息计算以及边缘检测后总平均速度为25帧/s，可以到达实时性要求。

3 结论

本文利用目标检测网络YOLOv4 从语义上获取图像的动静态信息，再联合特征点光流信息识别真正运动的物体，并使用canny 算子提取动态物体的轮廓边缘，从而获得图像较为准确的动态区域，仅利

用动态区域以外的静态特征点构造重投影误差函数进行相机位姿求解，在构建环境点云地图时，对关键帧的冗余性和有效性加以判断，叠加其点云信息，构建动态环境下剔除了动态物体的点云地图，保证实时性的同时提高精度。 在TUM 数据集中，本文系统与ORB＿SLAM2 进行对比，定位精度大大提升，点云地图可读性明显增强。