基于目标检测的室内动态场景定位与建图
时间：2023-02-12 14:50:06 来源：千叶帆 本文已影响人
席志红，温家旭
（哈尔滨工程大学 信息与通信工程学院，哈尔滨 150001）
同时定位与地图构建（Simultaneous Localization And Mapping，SLAM）是移动机器人在未知环境下实现自主定位导航的核心技术［1］，视觉SLAM（Visual SALM，VSLAM）主要采用视觉传感器作为数据源，相比激光雷达等其他SLAM 技术具有一定价格优势，近年来成为SLAM 研究领域的热点方向之一［2］。
室内场景中存在运动物体时，动态物体内不稳定的特征点会严重影响相机位姿估计的准确性，因此视觉SLAM 在室内动态场景中仍然具有很大的挑战性。随着深度学习的快速发展，基于深度学习的室内动态场景SLAM 逐渐受到人们的关注［3］。
ORB-SLAM2［4］被认为是目前最完整的SLAM 框架之一，但是在动态场景中ORB-SLAM2 系统定位与建图效果并不理想，并且仅创建稀疏点云地图，无法用于移动机器人后续导航工作。针对以上问题研究学者们做了大量尝试，Yu 等［5］提出的DS-SLAM 将SLAM 与SegNet 语义分割网络相结合，利用语义信息和运动特征点检测并滤除每一帧图像中的动态物体，从而极大提高了动态环境中相机的定位精度，同时建立语义八叉树地图。Bescos 等［6］提出的DynaSLAM 将深度学习和多视几何结合，在ORB-SLAM2 系统的基础上，增加了动态物体检测和背景修补功能，同时建立静态场景地图。房立金等［7］提出一种结合实例分割的动态场景SLAM 方法，利用MaskRCNN［8］对关键帧进行实例分割，计算特征点光流场筛选出动态特征点，识别出场景中真正运动的物体并将属于动态物体的特征点去除，最后进行语义关联，建立无动态物体干扰的语义地图；
Wen 等［9］提出了一种利用MaskRCNN 检测环境中动态对象并构建包含语义地图的SLAM 方法，在该方法中重投影误差、光度误差和深度误差被用来给每个关键点分配一个鲁棒的权重，从而将动态点和静态点分开，实现动态对象的几何分割；
Ren 等［10］提出了一种基于图像信息熵的视觉SLAM 方法，根据图像的信息熵确定信息量，筛选对比度低、梯度变化小的图像，对图像进行增强处理，尽可能多地提取出能够代表图像信息的特征点，作为相邻帧匹配和关键帧匹配的相关基础。
本文针对室内场景中动态对象影响相机位姿估计准确性的问题，提出了一种基于目标检测的室内动态场景SLAM系统，主要工作如下：
1）将ORB-SLAM2 系统与YOLOv4 目标检测网络［11］相结合，检测并滤除动态对象边界框的掩膜区域内部的特征点，利用静态特征点估计相机位姿；
2）在ORB特征匹配时，结合GMS（Grid-based Motion Statistics）稳健匹配算法［12］进一步有效剔除误匹配，提高特征匹配质量；
3）建立静态稠密点云地图［13］以及八叉树地图［14］，可用于后续移动机器人的导航工作；
4）在TUM RGB-D 公开数据集［15］上，将本文系统与ORBSLAM2 系统和YOLOv4+ORB-SLAM2［16］系统进行对比测试，评估本文系统相机位姿估计精度。
1.1 SLAM系统框架
本文SLAM 系统在ORB-SLAM2 系统框架的基础上进行改进，采用RGB-D 相机获取场景图像，并行运行5 个线程，分别为目标检测、跟踪、局部地图、局部回环以及地图构建。本文SLAM 系统框架如图1 所示，首先图像经过目标检测网络处理，提取场景中的动态物体信息；
跟踪线程提取图像ORB特征点，将动态物体检测框掩膜内部的特征点剔除后，再经过GMS 特征匹配算法剔除误匹配［17］；
利用质量较好的匹配进行跟踪，局部地图和局部回环可对关键帧的位姿和地图点的位置进行优化，避免产生尺度漂移问题；
最后在地图构建线程中建立静态稠密点云地图以及八叉树地图。
图1 本文SLAM系统框架Fig.1 Framework of SLAM system in this paper
1.2 动态目标检测
本文系统使用YOLOv4 网络进行动态目标检测，YOLOv4 目标检测网络是一种高效且强大的目标检测模型，YOLOv4 算法在实时目标检测算法中精度很高，实现了精度和速度的最佳平衡。
YOLOv4 目标检测网络结构采用CSPDarknet53 作为主网络，保留了YOLOv3 的head 部分，采用空间金字塔池化思想增加感受野，能够分离出最重要的上下文特征而且没有降低网络运行的速度，同时使用PANet 中的路径聚合模块，将融合的方法由加法改为乘法。
在实际室内场景中，人、狗、猫被认为是主要动态对象，本系统利用YOLOv4 网络对室内场景进行动态目标检测，若目标检测后的结果中含有上述动态对象，则生成对应边界框的掩膜区域，结合掩膜区域和ORB 特征提取后的结果，将掩膜区域内的ORB 特征点剔除，这样可以显著降低动态对象的影响。本文系统输入图像经过YOLOv4 目标检测网络后的结果如图2 所示（测试图像来源于freiburg3_walking_xyz 数据集）。
图2 YOLOv4目标检测结果Fig.2 YOLOv4 target detection results
1.3 GMS算法剔除误匹配
GMS 是一种基于网格运动统计的快速鲁棒的特征匹配方法，可以快速剔除误匹配，提高匹配稳定性，核心思想是利用相邻像素之间的一致性约束，并将这种约束封装成区域对之间的统计特性，误匹配点和正确匹配点在统计特性上会服从不同的分布，可通过设定阈值的方法来区分正确匹配和误匹配。
首先将图片进行网格化处理，任意网格中的匹配点为xi，xi所在网格中的匹配点数量记为Si，则Si服从二项分布，如式（1）所示：
其中：K为xi周围的网格数，n为xi所在网格内特征点的数量，pt表示xi为正确匹配的概率，pf表示xi为误匹配的概率，T代表匹配正确，F代表匹配错误。该分布的均值和标准差为：
通常来说，正确匹配点要比误匹配点多，设定合适的阈值，利用Si的均值和方差就可以划分误匹配点和正确匹配点，定义划分指标为P，使用划分指标P来剔除误匹配点，其数学表达式如式（3）所示：
这可以转化为一个优化问题，即最大化划分指标，由式（3）可得：
从式（4）可以看到，P值和特征点的邻域格网数量以及特征点数量呈正相关，设阈值为τ，那么
其中：α设为6，网格内的特征点个数若大于阈值则认为是正确匹配，否则为误匹配。
本文系统在ORB 特征点匹配后利用GMS 算法来剔除误匹配，这样不仅可以使后续RANSAC（RANdom SAmple Consensus）算法［18］更快收敛，而且可以得到更加准确的相机位姿，利用GMS 算法进行ORB 特征点匹配的效果如图3 所示，其中ORB 特征点提取数量设置为1 000，最终得到的匹配数量为137 对（注：测试图像来源于freiburg3_walking_xyz 数据集）。
图3 GMS算法特征点匹配效果Fig.3 Feature point matching effect by GMS algorithm
1.4 地图构建
1.4.1 静态稠密点云地图
点云地图即一组离散点表示的地图，包含三维坐标以及彩色信息，利用RGB-D 相机提供的彩色图和深度图，结合相机内参便可计算点云，根据相机位姿将点云进行加和，就可以得到全局点云，ORB-SLAM2 系统仅创建稀疏点云地图，缺乏环境信息，无法用于移动机器人后续导航工作。本文系统首先利用目标检测结果滤除场景中的先验动态对象，然后构建无动态对象干扰的静态稠密点云地图，能够更好地展示场景环境信息，并且在建图时加入外点去除滤波器和降采样滤波器对点云进行滤波处理，利用外点去除滤波器可去除孤立的噪声点，利用降采样滤波器对三维空间进行降采样，从而节省存储空间。
1.4.2 八叉树地图
八叉树地图（octomap）是通过八叉树结构进行信息存储的地图，是一种灵活的、压缩的、又能随时更新的地图形式，可用于移动机器人导航与避障。
八叉树的节点存储该节点是否被占据的信息，用概率形式表达某节点是否被占据，定义一个浮点数x∈[0，1]，初始值取0.5，如果不断观测到该节点被占据，那么不断增大x取值；
如果不断观测到该节点是空白，便不断减小x取值，这样就可以动态建模地图中的障碍物信息，设y∈R 为概率对数值，那么
这样可保证x∈[0，1]，设某节点为m，观测数据为z，那么从开始到t时刻某节点的概率对数值为L(n|z1：t)，t+1 时刻为：
这样就可以根据RGB-D 数据更新整个八叉树地图，相对于稠密点云地图，八叉树地图能够显著减小地图存储空间，可用于后续移动机器人的导航工作。
2.1 数据集与测试平台
本文选用TUM RGB-D 公开数据集中的动态序列freiburg3_walking_xyz、freiburg3_walking_halfsphere和freiburg3_walking_static 进行测试，测试数据集均为高动态图像序列，freiburg3_walking_xyz 图像序列相机沿(x，y，z)三个方向移动，freiburg3_walking_halfsphere 图像序列中相机在大约1 m 直径的小半球上移动，freiburg3_walking_static 图像序列相机保持在适当位置，本实验测试运行平台为配备Intel Core i7 处理器、GeForce GTX 1050Ti 型号GPU，内存为8 GB的台式电脑。
2.2 相机位姿估计
2.2.1 位姿估计精度
本文进行多次实验测试对比了ORB-SLAM2 系统、GCNv2_SLAM 系统、YOLOv4+ORB-SLAM2 系统与本文系统在上述3 个数据集序列中的实验结果如表1～3 所示，利用绝对轨迹误差（Absolute Trajectory Error，ATE）和相对位姿误差（Relative Pose Error，RPE）作为评价指标，ATE 代表轨迹的全局一致性，RPE 测量平移和旋转漂移，其中均方根误差（Root Mean Square Error，RMSE）反映估计值与真实值之间的偏差，平均误差（Mean）反映所有估计误差的平均水平。这几种客观评价指标表现出系统估计的轨迹与真实值之间的差距，反映了系统的稳定性和可靠性。
由表1、2 可以看出，相对于ORB-SLAM2 系统和GCNv2_SLAM 系统来说，本文系统相机位姿估计的绝对轨迹误差和相对位姿误差明显降低；
相对于YOLOv4+ORBSLAM2 系统，本文系统利用GMS 算法剔除误匹配，同时也提高了相机位姿估计的精度，为后续地图创建提供更加准确的相机位姿，提高建图精度与效果。
表1 四个系统的绝对轨迹误差结果对比单位：cm Tab.1 Absolute trajectory error results comparison of four systems unit：cm
本文利用evo 工具分别对ORB-SLAM2 系统、GCNv2_SLAM 系统、YOLOv4+ORB-SLAM2 系统与本文系统绘制相机的轨迹，并评估估计轨迹与真值的误差，在freiburg3_walking_xyz 数据集下的实验结果如图4 所示，其中虚线代表相机真实轨迹，实线代表相机估计轨迹，不同颜色代表不同的误差。
表2 四个系统的相对位姿误差结果对比 单位：cmTab.2 Relative pose error results comparison of four systems unit：cm
表3 四个系统的旋转漂移结果对比 单位：（°）Tab.3 Rotational drift results comparison of four systems unit：（°）
图4 四个系统的相机轨迹与误差对比Fig.4 Camera trajectory and error comparison of four systems
由以上实验测试结果可以发现，本文系统在相机位姿估计准确性方面表现良好，在freiburg3_walking_xyz、freiburg3_walking_halfsphere 和freiburg3_walking_static 数据集中，本文系统相对于ORB-SLAM2 系统来说，绝对位姿误差的均方根误差分别降低了97.47%、93.13% 和97.21%，相对于GCNv2_SLAM 系统分别降低了97.42%、93.30%和87.07%，相对于YOLOv4+ORB-SLAM2 系统分别降低了4.93%、9.26%和10.72%，其中本文系统相机估计轨迹与真实轨迹整体误差最小，相机位姿估计精度最高，其原因在于本文系统增加了对动态物体的处理，利用目标检测方法筛选出静态特征点进行位姿估计，并且利用GMS 特征匹配方法有效剔除误匹配，从而提升动态场景中相机位姿估计精度。
2.2.2 运行时间
本文测试对比了ORB-SLAM2 系统、DynaSLAM 系统以及本文系统在freiburg3_walking_xyz 数据集上的平均跟踪时间，结果如表4 所示。
表4 三种系统的运行时间结果对比 单位：sTab.4 Running time results comparison of three systems unit：s
由表4 可以看出本文系统在ORB-SLAM2 系统的基础上添加目标检测和GMS 特征匹配，同时也增加了运行时间，但相对于DynaSLAM 系统来说，运行时间较短，并且能够保证较高的精度。
2.3 地图创建
为了滤除动态对象的干扰以及后续移动机器人的导航与路径规划，本文在ORB-SLAM2 的基础上构建静态稠密点云地图和八叉树地图，在freiburg3_walking_xyz 数据集上创建的静态稠密点云地图与八叉树地图如图5 所示。
由图5 可以看出，静态稠密点云地图可以比较完整地保留环境信息，较清楚地显示细节。八叉树地图所占存储空间（1.6 MB）约为静态稠密点云地图（11.3 MB）的14%，同时八叉树地图可应用于移动机器人导航任务中，对于移动机器人避障与路径规划有着重要的意义。
图5 静态稠密点云地图和八叉树地图Fig.5 Static dense point cloud map and octomap
本文针对室内动态场景下动态对象影响相机位姿估计准确性的问题，在ORB-SLAM2 系统的基础上，结合YOLOv4目标检测网络检测场景中的动态对象，剔除分布在动态对象检测框掩膜内部的特征点；
同时结合GMS 特征匹配算法剔除误匹配，利用质量较高的匹配计算相机位姿，这样可以显著降低动态对象的影响，提高相机位姿估计的准确性；
最后实现静态稠密点云地图和八叉树地图的构建。然而本文SLAM 系统在减小误差的同时，也增加了运行时间，在接下来的工作中，将进一步提升目标检测网络性能以减小相机位姿估计误差，并且保证系统的实时性。
