一种基于物体追踪的改进语义SLAM算法

杜小双，施 展，华云松

（上海理工大学 光电信息与计算机工程学院，上海 200093）

0 引言

随着机器人技术、无人驾驶、增强现实等领域的发展与普及，视觉SLAM 作为其应用的基础技术之一［1］，得到了学者们的广泛关注与研究，并成为机器人定位与建图研究领域的一个热点［2］。

视觉同步定位与建图（Simultaneous Localization and Mapping，SLAM）可以大致分为两类：直接法和特征点法。直接法假设相邻帧间光度值不变，用局部像素的梯度和方向进行计算并优化出相机位姿，如LSD-SLAM［3］、DSO［4］等。特征点法通过特征点提取，并使用描述子匹配特征点，通过匹配特征点解出帧间位姿并优化多帧位姿。由Mur-Artal 等［6-8］提出的ORB-SLAM 是一个适用于单目、双目和RGB-D 相机的SLAM 系统，因出色的性能而得到了广泛应用。

上述SLAM 算法都是基于静态场景假设无法解决动态场景的问题，而随着深度学习的快速发展，一些先进深度学习网络，如DeepLab［9］、SegNet［10］和YOLACT［11］等可以精确进行像素级的语义分割，语义SLAM 即利用语义神经网络分割出图像物体掩码，通过剔除动态物体掩码上的特征点以实现正确的跟踪。Yu 等［12］提出的DS-SLAM 在ORBSLAM2 的3 个线程基础上新增语义分割和稠密建图两个新线程，使用SegNet 网络分割出物体掩码并结合移动一致性检查方法以减少动态对象影响，从而提高动态环境下的跟踪精度。此外，Bescos 等［13］提出的DynaSLAM 利用mask R-CNN［14］神经网络和多视图几何检测和剔除运动物体从而实现更精准的轨迹估计。

针对语义SLAM 仅停留在剔除动态物体特征点层面并没有充分利用语义信息，无法解决分割错误对物体追踪的影响，本文提出了一种基于物体追踪的改进语义SLAM 算法，实现物体追踪与定位，并通过剔除运动物体影响提高轨迹估计精度。本文贡献如下：①提出一种物体动静态检测方法，对物体匹配特征点进行深度、重投影误差和极线约束三重检测实现精准的物体动静态判断；②提出一种物体追踪状态判别方法，通过物体的4 种追踪状态相互转换，实现稳健的环境物体追踪与定位；③在TUM RGB-D 数据集上进行实验，证明本文所提方法的有效性。

1 系统设计

1.1 系统框架

Fig.1 System architecture图1 系统框架

1.2 语义分割网络

针对室内动态环境中的物体分割，本文采用YOLACT［15］++网络。YOLACT++网络是一种简单的全卷积实时语义分割网络，使用Resnet101-FPN 作为主干网络，使用在MS COCO 数据集上训练得到的网络参数进行物体检测。假设YOLACT++网络的输入是一个x×y×3 的RGB 图像矩阵，则网络的输出就是一个x×y×n的掩码矩阵，其中n是图像中物体的数量。对于网络最后的每个输出通道m∈n，都会得到一个对象的掩码矩阵。

在实时性上，YOLACT++使用Resnet101-FPN 主干网络在MS COCO 2017 验证集上的检测速度可以达到27.3帧/s［15］。同样地，在TUM 数据集实验中，使用YOLACT++分割RGB 图像中的人物、椅子和屏幕3 种物体，速度也能够满足算法语义分割任务中的实时性需求。每帧图像分割完成后，将得到的掩码与定位框输入本文算法。在本帧图片提取特征点时，结合掩码得到每个特征点的物体类别。为了特征匹配的准确性，剔除位于物体掩码边缘的特征点。

1.3 物体追踪

物体追踪模块为本文核心算法，当出现新的物体时创建物体追踪对象，其包含物体追踪状态信息、物体相邻两帧的物体特征点（当前帧与上一帧）和由物体特征点所创建的物体地图点。再得到当前帧位姿后将上一帧追踪物体特征点投影到当前帧上进行匹配，对匹配成功的特征点进行三重动静态检测以判断其动静态，之后根据特征点匹配点数量与动静态点比例更新物体追踪状态。如果当前帧需要创建关键帧则将物体追踪信息输入关键帧中，在局部建图中根据关键帧中物体追踪信息新建替换与删除地图点。

1.4 三重动静态检测

在匹配物体特征点时，对匹配成功的物体特征点进行深度、重投影误差和极线约束三重检测以判断是否为动态点，其中深度检测为匹配特征点投影到当前帧上的深度值与当前帧的深度计算其差值后与深度阈值对比，差值超过深度阈值则认为该特征点为动态点。

1.4.1 重投影误差

重投影误差即将上一帧的匹配特征点投影到当前帧上与匹配点的直接平面距离差距。重投影误差如图2所示。

Fig.2 Schematic diagram of reprojection error图2 重投影误差示意图

图2 中，P1、P2为匹配特征点，MP为点P1投影到3D 空间中的地图点和P'1在F2像素平面的投影，相机内参矩阵为K，F1到F2之间的旋转矩阵为R，平移向量为t。其中，P1点的像素坐标为P1=[u1，v1]，P1的像素坐标为P2=[u2，v2]。MP在空间中地图点的坐标为MP=[X1，Y1，Z1]，其中深度Z1通过RGB-D 相机的深度图获得。X1、Y1与像素坐标系的关系如下：

后来，朝廷下令，把抚顺关马市关闭了，这一关就是几个月。马市不开，女真人没盐，没铁器，没马匹，生活不了，他们就上书朝廷，要求开关，恢复交易。朝廷又重新开市，开市以后，地方上的守关明军照样勒索。王杲呢，再带女真人去打。这样，把朝廷又惹翻儿了，一面安抚，一面带兵去围剿女真人。李成梁受命带领几万明军，把古埒城团团围住。

F1相机坐标系中的MP点坐标通过变换矩阵到F2帧相机坐标系下的坐标[X2，Y2，Z2]T为：

得到P'1点的投影坐标后与匹配点P2坐标求得匹配后的重投影误差e为：

得到重投影误差后与设定的误差阈值对比则可进行判断，如果其重投影误差大于阈值该特征点为动态点。

1.4.2 极线约束

极线约束是摄像机运动学的重要组成部分，可用于检查特征点的运动状态。对极几何示意图如图3所示。

Fig.3 Schematic diagram of epipolar geometry图3 对极几何示意图

对于匹配特征点，相邻帧间基础矩阵F为：

其中，K为相机内参，R、t为帧间的旋转矩阵和平移向量。在得到基础矩阵F后对每个物体掩码的匹配特征点计算当前特征点的极线。上一帧的特征点通过基础矩阵映射到当前帧的区域即为极线。设前一帧和当前帧中的匹配特征点分别表示为[u1，v1]和[u2，v2]，u、v表示特征点的像素坐标值，而P1=[u1，v1，1]，P2=[u2，v2，1]表示匹配特征点的齐次坐标形式。匹配特征点的极线约束如下：

则F2图像平面上的匹配特征点P2对应极线L计算如下：

最后，当前帧的特征点到对应极线距离D如下：

判断匹配特征点极线之间的距离是否大于阈值，如果距离大于阈值，则该特征点被视为动态点。

1.5 局部建图

在局部建图中，本文算法只对静态地图点进行操作。由于动态地图点已经无法跟踪优化，因此将其设置为坏点不加入关键帧与地图点的局部优化，并且动态物体也不再新建地图点，仅仅依靠帧间的物体特征点进行追踪。如果动态物体在算法运行中由动态转换为静态，就可以正常新建地图点进行跟踪优化。对于消失在相机视野中的静态物体，其物体地图点得到保留，并且它们将被用于恢复物体跟踪状态。

2 基于语义分割与动静态检测的追踪状态转换

在本文算法中，物体都具有4 种追踪状态：①静态是匹配特征点、动静态点在阈值范围内；②动态是匹配特征点、动静点比例超过阈值范围；③即将丢失是当前帧未匹配到同类型掩码；④已丢失是连续多帧未匹配同类型掩码。

物体跟踪状态转换过程如图4所示。具体步骤如下：

Fig.4 Schematic diagram of object tracking state transition图4 物体追踪状态转换示意图

（1）当物体首次出现在相机视野时，本文算法会根据物体的类别创建跟踪对象，并在创建的初始帧中确定物体所处的运动状态。如果判断为静态物体特征点和地图点，加入后续匹配跟踪；反之，判断为动态则删除地图点，其特征点不参与后续匹配跟踪。

（2）正常追踪的物体会根据当前帧间物体特征点匹配情况更新当前帧中的跟踪状态。如果匹配成功，则使用动静态点比例判断物体当前帧的运动状态；反之，则判断物体追踪失败，将物体转为即将丢失状态，并储存物体信息用于丢失找回。

（3）当物体处于即将丢失状态时，会储存丢失前一帧的物体相关信息，在之后匹配过程中使用丢失帧的储存信息进行匹配。如果成功匹配上相同类型的掩码，则物体恢复追踪状态后正常追踪；而对于超过十帧依然没有匹配正确的物体掩码，将物体的追踪状态转为已丢失，不再对物体进行追踪，动态物体直接删除跟踪信息，静态物体则保留全局地图点等待之后的召回。

在算法物体追踪过程中，通过物体4 种追踪状态的相互转换，可以有效地避免遮掩与分割掩码错误导致的追踪失败，从而稳定地实现物体的追踪与定位。

3 实验与分析

对于本文所提出的SLAM 算法，将通过实验证明其算法的准确性与稳定性，在公共TUM RGB-D数据集上进行两个部分的实验：①对TUM RGB-D 数据集中的walking_halfsphere 序列进行实验，通过跟踪物体定位框对本文算法的物体追踪能力进行评估；②对TUM RGB-D 数据集中的8个sitting序列和walking序列进行定量分析。利用TUM数据集的官方评估工具对本文算法得到的轨迹进行评估，同时使用其他SLAM算法结果与本文算法结果进行比较。

3.1 物体追踪实验

在实验中使用walking_halfsphere 序列集，此场景中人与相机既有运动又有静止，物体间存在相互遮盖和重合，相机自身运动包含平移和旋转，这些要素的结合对于物体的分割、跟踪都是一项较大的挑战，因此选择此序列集进行实验能够较为准确地评估基于本文算法的物体追踪性能。图5 是基于本文算法运行walking_halfsphere 序列时的图像（彩图扫OSID 码可见）。

Fig.5 Operation diagram of the object tracking system图5 物体追踪系统运行图

从图5（a）可以看出，在实验开始时，人和屏幕都被成功追踪，其中动态物体为人，静态物体为屏幕。图5（b）中相机和人都在运动，先前追踪的人已经丢失，此时删除该追踪信息，等到人再出现在相机视野中时，重新创建新的追踪对象并追踪。图5（c）和图5（d）中，两个椅子进入相机视野，被成功追踪并编号为一号椅子和二号椅子。之后，图5（e）、图5（f）与图5（g）中两人重新回到相机视野且其中一人坐回椅子，该人从动态转变为静态。图5（h）是相机与人运动后的图像。由此证明，本文所提出的算法可正常追踪物体，同时具有更新物体追踪状态的能力。

3.2 轨迹估计误差实验

实验中将对TUM RGB-D 数据集中的siting 和walking两种类型共8 条序列进行实验并将结果与ORB-SLAM3、DS-SLAM 和DynaSLAM 的轨迹估计误差数据进行比较。

图6 为本文算法和ORB-SLAM3 在高动态场景中轨迹估计误差对比图，其中黑色线段为真实轨迹，蓝色线段为估计轨迹，而红色表示真实轨迹与估计轨迹间的差值，红色范围越大表示误差越大（彩图扫OSID 码可见）。

Fig.6 Schematic diagram of estimated trajectory error图6 ORB-SLAM3系统的估计轨迹误差示意图

图6 与图7 为本文算法与ORB-SLAM3 在walking_halfsphere 和walking_xyz 两图像序列集下的估计轨迹误差示意图，可以看出本文算法与ORB-SLAM3 相比在动态环境中的轨迹误差存在较大差距。因此，以ORB-SLAM3 为代表的传统视觉SLAM 在处理动态环境上存在不足，同时表明对动态物体进行去除可使视觉SLAM 跟踪精度得到极大提高。

Fig.7 Schematic diagram of the estimated trajectory error of the algorithm in this paper图7 本文算法的估计轨迹误差示意图

在该部分实验中，将本文算法与ORB-SLAM3、DSSLAM、DynaSLAM 的结果进行比较，使用TUM 官方提供的误差分析工具对估计轨迹与真实轨迹进行对比。其测量值为绝对轨迹误差（Absolute Trajectory Error，ATE）与相对姿态误差（Relative Pose Error，RPE），使用均方根误差（Root Mean Square Error，RMSE）和标准差（Standard Deviation，SD）作为评估指标。结果如表1—表3 所示（“—”表示相关论文中没有提供结果）。

Table 1 Comparison of ATE result of TUM RGB-D dataset表1 TUM RGB-D 数据集的绝对轨迹误差结果比较

Table 2 Comparison of results of relative pose translation error of TUM RGB-D dataset表2 TUM RGB-D 数据集的相对姿态平移误差结果比较

Table 3 Comparison of relative pose rotation error results of TUM RGB-D dataset表3 TUM RGB-D数据集的相对姿态旋转误差结果比较

从表1—表3 可以看出，与原版ORB-SLAM3 系统相比，语义SLAM 算法可以极大减少在动态场景下的绝对轨迹误差与相对姿态误差。本文算法与DS-SLAM 和DynaSLAM 相比，在4 个动态序列中的3 个（walking_xyz，walking_rpy 和walking_half）中的绝对轨迹误差和相对姿态误差都有不同程度的下降；而在静态序列中，由于DS-SLAM与DynaSLAM 缺少相关数据，因而本文算法与ORBSLAM3 进行对比，在4 个静态序列中由于本文算法在跟踪中剔除了类别匹配错误的特征点，故在静态场景下的估计误差比ORB-SLAM3 更小，但当前视觉SLAM 算法在静态场景中已经有较好的性能，因此可提升的空间有限。

4 结语

本文提出了一种基于ORB-SLAM3 的语义SLAM 算法，该算法使用YOLACT++获取图像中掩码与定位框，对物体特征点匹配后通过三重检测判断其动静状态，再根据数量与动静态比例更新当前物体跟踪状态，实现稳定的物体追踪，同时剔除动态物体的影响以实现精准的轨迹估计。本文算法的实验结果与其他视觉SLAM 算法的实验结果相比，可以更加准确地估计相机运动轨迹并正确地追踪到物体的运动状态与位置，具有广泛应用价值。