动态环境下的语义视觉SLAM算法研究

华春生，郭伟豪

(辽宁大学 信息学院，辽宁 沈阳 110036)

0 引言

同步定位与地图构建(Simultaneous localization and mapping，SLAM)是指载体利用传感器在未知环境移动的过程中估计自身位姿的同时，进行地图的构建.由于机器人的主要传感器为视觉SLAM，它具有性价比高、体积小、可获得丰富的环境信息等特点，所以近年来SLAM在机器人领域的应用得到广泛关注.研究者提出了很多优秀的视觉SLAM算法，例如MonoSLAM[1]、PTAM[2](Parallel tracking and mapping)、LSD-SLAM[3]、DSO[4]、ORB-SLAM2[5]等.但是这些算法都是建立在所遇到的场景是静态的假设基础上，即图像的信息变化只取决于相机本身的运动.然而在实际的应用场景中很难满足这种假设，会不可避免地出现动态物体，例如行走的人、移动的物体等.虽然可以采用随机抽样一致性检测(RANSAC)[6]来减少一些小物体的异常值，但是较大物体和移动较多的情况却很难处理.这不但会给系统带来误差还会影响到位姿估计和地图的构建.深度学习近年来发展迅速，将视觉SLAM和深度学习相结合的解决方案应运而生.利用深度学习技术来提取语义信息并融合到视觉SLAM中，不仅可以有效去除动态点，而且可以在几何和语义两个层面上对周围环境进行一些高层次的理解，建立具有语义信息的三维地图，为机器人执行高级任务做准备.Mclormac等[7]利用卷积神经网络和Whelan等[8]结合构建语义地图，都没有考虑动态物体移动较多的问题.Runz等[9]和He等[10]将MASK-RCNN和基于几何分割的算法相结合，实现了多物体跟踪识别和重建，但是对非刚性物体无法重建.Bescos等[11]利用MASK-RCNN和多视角几何相融合，能够剔除动态物体并进行背景修复，但是，该算法会把有一定概率移动的对象移除掉，例如静止的车.这会导致留下的特征点太少，进而影响到系统的位姿估计等.Lu等[12]提出了一种基于分布和局部的算法(DM-SLAM)，并设计了一种基于邻域互斥的候选地图选择机制，减少了动态物体的影响.这种算法局限性在于大量静止物体出现在场景中，此后在跟踪移动物体时，算法就会失效.Yang等[13]提出语义和几何约束算法提高了位姿估计精度，但是当动态对象占据当前帧的一半时，基础矩阵不能准确反映整个帧，会导致对极约束的错误.

由于在动态环境下收到动态对象的干扰(类似于行人等移动物体的出现)，一方面会导致定位精度的降低，累计误差不断增大，严重时会导致系统崩溃；另一方面保存在地图中的动态对象会严重影响地图的使用价值.针对现有的视觉SLAM在动态环境中存在的鲁棒性与稳定性的问题，本文在ORB-SLAM2系统的基础上进行改进，提出动态点检测和语义分割相结合的算法，剔除动态点，减少动态目标的影响，并构建出三维语义地图.

1 系统介绍

ORB-SLAM2算法是目前应用最广泛、系统最完整的视觉SLAM系统之一，同时支持单目、双目和RGB-G相机.因此本文将基于ORB-SLAM2算法进行改进，在提高精度的同时增加系统的稳定性.改进后的系统框架如图1所示，在ORB-SLAM2算法的跟踪线程里嵌入动态点检测，另外增加了语义分割线程和地图构建线程.系统的流程是先将RGB图像序列同时输入到跟踪线程和分割线程，通过动态点检测算法和语义分割获得的结果来检测动态物体，然后删除动态对象的特征点.最后通过稳定的静态特征点来估计姿态，并构建语义地图.

图1 系统框架

2 动态特征点检测与剔除

2.1 光流法

光流法是一种利用图像序列中的像素在时间域上的变化和相邻的图像帧之间的相关性来求解前一帧和当前帧之间的对应关系，进而求解对象运动信息的方法.光流法可以分为稀疏光流和稠密光流，稀疏光流只是针对图像中的部分像素进行计算，并且计算开销要比稠密光流小.其中较为典型的是LK光流[14].本文利用LK光流提取特征点和求解基础矩阵.

LK光流的使用需要遵循以下3个假设条件.

1)灰度不变假设：图像中相同空间点的像素灰度值在每个图像中是不变的.

2)小运动假设：相邻帧间的运动位移要小.

3)空间一致假设：相邻的像素要具有相似的运动.

空间中某个点，在t时刻，假设其像素坐标为(x，y)，在t+dt时刻设它运动到了(x+dx，y+dy)处.根据灰度不变假设，可以得到

I(x+dx，y+dy，t+dt)=I(x，y，t)

(1)

左边进行泰勒级数展开并保留一阶项得到

(2)

由于灰度不变假设，故下一时刻的灰度等于之前的灰度，从而有

(3)

两边除以dt，得

(4)

其中令Vx=dx/dt，表示像素在x轴上的速度.令Vy=dy/dt，表示像素在y轴上的速度.令Ix=∂I/∂x，Iy=∂I/∂y，分别表示像素在该点处x方向的梯度，y方向上的梯度.图像灰度对连续时间的变化量可以记为It，上式可以写成如下形式：

IxVx+IyVy=-It

(5)

考虑一个窗口的大小为ω×ω，即含有ω2个数量的像素，因此可建立如下方程：

AV=-b

(6)

V=-(ATA)-1ATb

(7)

这样就可以求得像素在图像间的运动速度，进而求得像素在若干块图像中出现的位置.但以上3种假设在实际的应用中不能完全满足.当物体移动较快时往往会破坏这种假设，进而会导致计算的光流值出现较大的误差，为了改善此问题可以采用图像金字塔LK光流法.图像金字塔通过缩放等操作，获得不同分辨率的图像.计算光流时，可以首先从顶部的图像计算，得到结果后，将其作为下一层要计算的初始值.这种方法的好处在于，当原始图像的像素运动较大时，对于金字塔的顶层来说，运动仍然是在一个很小的范围内，进而满足了基本假设.

2.2 动态点检测

本文提出的动态点检测算法主要分为预检测和极线约束两个步骤.

图2 对极几何约束

第一步，预检测：①使用稀疏LK光流得到上一帧和当前帧图像相匹配的特征点；②根据以下方法处理匹配的特征点，如果匹配的特征点特别靠近图像的边缘，则丢弃该对特征点.

第二步，对极几何约束：①对预检测之后当前帧和上一帧中剩余的特征点进行特征匹配；②从匹配的特征点对中使用八点法来计算基础矩阵F；③利用基础矩阵和上一帧中的特征点计算出当前帧对应的极线；④计算当前帧中的匹配特征点到极线的距离，和预设阈值比较，如果大于阈值则将其视为动态点.

(8)

式中A，B，C为线向量.对于当前帧的每个匹配点，计算其到极线的距离，如果大于预设阈值dthr，则视为动态点.距离计算公式为

(9)

2.3 剔除动态特征点

通过动态点检测算法检测完动态点后，无法获得完整动态区域边界轮廓，而语义分割可以完整地分割出动态区域.考虑到本系统是为实际场景的应用而设计的，所以要在实时性和准确性之间取得平衡.本文采用SegNet[15]语义分割网络模型，并使用Pascal VOC数据集训练该模型.SegNet网络是一种适合实时应用的快速、小存储空间的网络模型，它可以将图像中的对象进行分割，能够达到像素的级别.本文将动态点检测结果和语义分割结果相结合，去除动态点的示意图如图3所示，圈出来的点为检测出的动态点，语义分割可以分割出精确的轮廓.如果检测出一定数量的动态点是在语义分割的动态对象区域内，则将该对象视为动态物体，然后将该对象内的特征点去除，而剩下的特征点可以用于后续的跟踪和建图.通过这种方式可以极大地提高鲁棒性和精度.

图3 剔除动态点

3 地图构建

3.1 单帧点云生成

单帧点云是构建全局地图的基础，每一个点都包含位置信息和深度信息，根据相机模型将二维图像上的点映射到三维空间，从而得到对应的点云.根据相机模型，空间点与对应的像素坐标之间变换关系如下：

(10)

其中f表示焦距，c表示相对于原点的偏移量，K表示相机内参.设图像中某一点的深度为d，则对应的空间点为

(11)

进而得到

(12)

将所有的像素按照式(12)进行转换即可得到相应的点云.

3.2 点云的拼接与滤波

图4 点云拼接示意图

将关键帧对应的点云生成后进行拼接即可形成局部点云地图.由于局部点云在拼接的过程中会出现关键帧重叠的情况，导致视野重叠区域存在很多位置相近的点，极大地浪费了存储空间，因此需要进行体素滤波.通过体素滤波后可以使相应的空间内只有一个点，从而节约了存储空间.点云拼接的示意图如图4所示.数学表达式为

(13)

其中：Ck表示单帧点云，Tk表示相机位姿，m表示局部点云.

4 实验结果与分析

4.1 实验环境

ORB-SLAM2算法作为视觉SLAM领域最为经典的算法之一，因其优秀的稳定性和实用性被广泛应用到各个领域，而本文的算法也是在其基础上进行改进.因此本文算法实验的各种数据将与ORB-SLAM2相比较，以此来验证在动态环境下本文算法的鲁棒性.本文使用TUM RGB-D公开数据集中的walking序列进行测试，分别是freiburg3_walking_static，freiburg3_walking_xyz，freiburg3_walking_halfsphere.walking序列中两名测试人员会围绕桌子来回走动，属于高动态场景.

实验环境为Ubuntu 18.04，实验平台为一台配备Intel Xeon CPU E5-2678 v3，12GB内存和一个NVIDIA Tesla K80 GPU的计算机.绝对轨迹误差(Absolute trajectory error，ATE)和相对位姿误差(Relative pose error，RPE)[16]是用来评价算法优劣的最常用的指标.本文将用以上两项指标作为算法精度评估的指标.绝对轨迹误差用于衡量估计轨迹与真实轨迹的一致程度，即轨迹准确度.相对位姿误差用于评估在一定时间间隔内的局部轨迹精度，并可用于评价位姿估计时产生的漂移量.

4.2 误差分析

如图5(a)所示，在以活动的人为对象的动态场景中，从表1和图6、图7可以观察出原始的ORB-SLAM2算法估计轨迹存在很大的误差.而经过本文的改进，在加入了语义分割之后可以精确地分割出动态目标，把动态对象(即人身上)的特征点去除.不仅消除了动态对象对位姿估计的影响，而且定位精度也取得了很大的提升，图6最能直观地看出增加了语义分割后定位精度的提升效果.

图5 动态环境测试图

实验的结果如表1所示，其中第一列有3种相机运动类型：①static表示相机处于静止状态；②xyz表示相机会沿着xyz轴运动；③halfsphere表示的是相机会按照半球面来运动.其中提升率计算公式为

(10)

式中α表示ORB-SLAM2算法，β表示本文算法.在实验结果中计算了均方根误差(Root mean squared error，RMSE)，平均值(Mean)和标准差(Standard deviation，S.D.)这3个参数，平均值反映估计位姿的精度，均方根误差和标准差更能体现系统的稳定性和鲁棒性.

表1 绝对轨迹误差(ATE)

由表1可知，在高动态场景下，本文算法的RMSE数值相比较于ORB-SLAM2算法平均下降了95.32%，反映系统鲁棒性的标准差S.D.则下降了94.00%.实验结果表明了在高动态环境中，本文算法比ORB-SLAM2算法具有更好的鲁棒性和稳定性.

图6(a)是ORB-SLAM2算法在数据集walking_xyz下的相机轨迹图，图6(b)是本文算法所估计的相机轨迹图.图中虚线表示的是数据集的真值，实线表示的是各个算法的相机估计轨迹值.通过观察图中两种线条的重合度，明显可以看出本文算法估计的相机轨迹和真值基本重合，相比较于ORB-SLAM2算法，准确度有很大的提升.图7(a)和(b)展示了ORB-SLAM2算法和本文算法的绝对轨迹误差图.通过图7(a)可以观察出ORB-SLAM2算法的绝对轨迹误差主要分布在0.3 m到0.5 m之间，还有一些时间的误差快达到了1.0 m，误差在0.1 m内的时间则极短.而从图7(b)中可以看出本文算法估计的轨迹误差基本都是在0.015 m到0.028 m的区间内，最小的误差能达到0.005 m，最大的误差都不超过0.08 m.从图6和图7中都能非常直观地看出本文算法相较ORB-SLAM2算法，在动态环境中的轨迹误差有很大幅度的降低，大大提高了位姿估计的准确度.

图6 轨迹真实值与估计轨迹

图7 绝对轨迹误差图

4.3 语义建图实验

在语义建图实验中，本文选择了walking_static数据集.在此序列中会有两个人坐在椅子上，或者围绕着桌子进行走动，属于高动态场景.图8(a)展示了ORB-SLAM2算法构建的地图，从图中可以看出ORB-SLAM2没有去除动态目标，会影响机器人执行路径规划等任务.图8(b)展示了经过语义分割和动态点检测算法结合去除动态特征点之后的语义地图效果，从图中可以看出本文算法去除了场景中大部分动态特征点，建立了点云地图，可以用机器人执行高级任务.

图8 ORB-SLAM2算法与本文算法构建地图效果

5 结束语

本文为了解决环境中出现动态对象而影响视觉SLAM算法的精度和缺乏语义信息的问题，基于ORB-SLAM2算法提出了一个动态点检测算法，嵌入到ORB-SLAM2算法的追踪线程中，并增加一个语义分割线程和稠密建图线程.通过动态点检测算法与语义分割相结合去除动态对象上的特征点，以减少动态对象对系统位姿估计的影响，并建立了语义地图.在公开数据集上的实验结果表明，在动态场景下本文算法比ORB-SLAM2算法大幅度提升了位姿估计的准确度，提高了系统的鲁棒性和稳定性.在未来的工作中将改进稠密语义建图的功能，使之用于机器人执行高级任务.