动态场景下移动机器人视觉SLAM

仉新，郑飂默，谭振华，张雷，李锁

(1.中国科学院沈阳计算技术研究所有限公司，辽宁沈阳 110168；2.东北大学软件学院，辽宁沈阳110169；3.沈阳理工大学机械工程学院，辽宁沈阳 110159；4.63867部队，吉林白城 137000)

0 前言

同时定位与地图构建(Simultaneous Localization and Mapping, SLAM) 是移动机器人在未知环境中，利用自身传感器获取环境信息，构建环境地图，同时估计自身在地图中的位姿，实现机器人自主定位及导航[1]。SLAM广泛应用于自动驾驶、智能家居、工业自动化以及移动机器人等领域。视觉SLAM获取场景信息丰富，成本低廉，成为机器人领域研究热点问题[2]。

视觉SLAM分为特征点法和直接法。特征点法通过特征提取和匹配实现位姿估计，ORB-SLAM2是经典的基于特征点的VSLAM系统，适用于单目、双目及多目等视觉传感器[3]。直接法基于灰度不变假设，通过计算最小化灰度误差实现位姿估计，它对曝光和相机内参非常敏感，光照变化场景下容易出现失效或退化问题。视觉SLAM大多基于外部静态环境假设，然而实际环境包含车辆和行人等动态信息，严重影响移动机器人定位和建图的准确性，动态场景下SLAM的准确性和鲁棒性问题亟待解决[4]。

动态场景下SLAM的准确性和鲁棒性问题受到研究者们广泛关注。根据动态场景下SLAM系统的目标检测方法分为两类，分别为基于几何信息的动态目标检测和基于深度学习的动态目标检测。基于几何信息的动态目标检测方法大多无法获取场景语义信息，缺乏对场景的高层语义信息理解[5-7]。

随着深度学习的发展，学者们将深度学习引入SLAM系统。基于深度学习的动态目标检测方法中，ZHONG等[8]采用单帧多框检测器(Single Shot Multibox Detector, SSD)检测运动物体，剔除相关区域的全部特征。阮晓钢等[9]采用Mask R-CNN网络进行图像语义分割，剔除场景动态特征，依靠静态特征进行位姿估计，提升了SLAM系统的准确性[10]。YU等[11]采用光流法进行运动一致性检测，通过SegNet (Segmentation Network)[12]网络进行语义分割，实现位姿估计和地图构建。BESCOS等[5]采用Mask R-CNN(Region-based Convolutional Neural Network)方法进行目标检测，提升ORB-SLAM2系统定位和建图的准确性，但实时性较差。相比Mask R-CNN网络，目标检测模型YOLO(You Look Only Once)[13]检测速度提高8～30倍，有利于保障SLAM系统的实时性需求。

本文作者提出了融合目标检测和点线特征的SLAM系统：首先，通过四叉树算法均匀化特征分布，解决动态场景特征聚集问题；然后，采用融合语义信息的目标检测网络识别场景的动态特征，剔除动态物体对位姿估计的干扰；最后，结合点特征和线特征进行特征提取及闭环检测，充分提取场景空间结构信息，构建无动态物体影响的环境地图，实现动态场景下移动机器人定位及建图。

1 基础理论

1.1 ORB-SLAM2系统框架

ORB-SLAM2包含多线程结构，分别为特征跟踪线程、闭环检测及局部建图线程。跟踪线程提取图像ORB特征，通过帧间特征匹配计算相机位姿。闭环检测线程通过闭环约束实现全局图优化，消除系统累计误差。局部建图线程包含局部BA(Bundle Adjustment)优化、关键帧管理以及地图点过滤，通过BA可以实现对局部空间特征及相机位姿优化。

图1 ORB-SLAM2系统框架Fig.1 The ORB-SLIM2 system framework

1.2 YOLO目标检测

目标检测可自主识别图像中的各类物体，主流算法包含基于回归的单阶目标检测和基于候选区域的双阶目标检测。单阶目标检测通过网络预测物体的位置和类别，如YOLO、SSD、Retina Net等，网络训练及识别速度较高，小目标物体检测难，漏检率较高，准确性一般；双阶目标检测包含区域提取和类别预测两部分，如R-CNN、SPP-Net、Fast R-CNN、Faster R-CNN、R-FCN以及 NAS-FPN等，准确度有所提升但训练及识别速度较低[14]。

2016年REDMON等提出基于深度学习的目标检测YOLO v1算法[13]，采用卷积神经网络实现目标类别判定和边界预测，REDMON等采用Darknet53作为特征提取网络，将YOLO算法性能提升为YOLO v3。BOCHKOVSKIY等基于YOLO v3，2020年提出了YOLO v4[15]，包含主干、颈部和头部网络，采用CSPDarknet53作为特征提取网络，引入空间金字塔池化。YOLO v4提升了模型精度，广泛应用于目标检测领域，但存在网络结构复杂及计算量大等问题。

2 动态SLAM系统框架

2.1 四叉树算法

以SLAM系统中广泛采用的ORB(Oriented FAST and Rotated BRIEF)特征为例，ORB分为oFAST(Oriented FAST)特征点提取和rBRIEF(Rotated BRIEF)特征描述符计算。图像外观、视角、纹理变化及遮挡等动态场景下，提取的ORB特征通常比较集中，影响SLAM前端特征提取和位姿估计的准确性。四叉树算法广泛应用于图像处理、空间数据索引等方向，文中选用四叉树算法实现特征均匀分布。

四叉树算法流程如下：

(1)初始化，顶点分别是TL、TR、BL和BR，将当前节点特征划分至n1、n2、n3、n4区域；

(2)判断节点内特征数量是否等于设定的数量或者达到设定的深度，是则不再生成新的子节点，否则继续划分，执行步骤(1)；

(3)计算节点Harris响应值，保留响应值最大的特征。

图2为传统ORB特征提取，图3为基于四叉树的特征提取。如图2和图3所示：四叉树算法均匀化了特征分布，解决了动态场景特征聚集问题，为SLAM前端运动估计提供稳定的特征。

图2 ORB特征提取Fig.2 Feature extraction of ORB

图3 基于四叉树的特征提取Fig.3 Feature extraction based on quadtree

2.2 PL-SLAM系统

传统点特征方法难以在动态场景下追踪充足的特征，依赖于场景中的纹理信息，容易造成初始化失败以及相机跟踪失败问题，影响SLAM系统定位的准确性。线特征可以表示图像边缘信息，体现了结构化空间几何约束关系，提高数据关联的准确性。本文作者采用融合点特征和线特征的PL-SLAM(Points and Line segments SLAM, PL-SLAM)系统，最小化点特征和线特征约束函数，实现相机位姿估计。

(1)点特征重投影误差

(1)

点特征约束函数为

fp(x,u)=‖π(Tcw·x)-u‖Σp

(2)

式中：‖x‖Σ=xTΣ-1x表示马氏距离；Σ为相应的协方差矩阵，可以体现数据间的相似度。

(2)线特征重投影误差

假设空间直线Lw由世界坐标系转换成相机坐标系直线Lc，直线Lc的Plucker坐标表示为

(3)

(4)

其中：(fu,fv,cu,cv)为相机内参，fu和fv分别表示相机在x轴及y轴焦距，cu和cv分别表示相机的光圈中心；K′表示直线投影矩阵。

空间直线Lw与相机位姿重投影误差为

(5)

第k帧观测第j条空间直线Lw,j重投影误差为

(6)

因此，线特征的约束函数为

(7)

(3)PL-SLAM系统的重投影误差模型

Huber核函数为

(8)

采用Huber函数降低代价函数异常项，当误差e大于阈值δ时，目标函数化简为一次函数，有效抑制误匹配对，实现优化结果稳定性。最小化目标函数(9)，求解相机位姿，实现SLAM系统位姿估计。

(9)

2.3 融合目标检测的动态SLAM系统

本文作者在ORB-SLAM2系统基础上增加了四部分内容，分别为YOLO v4目标检测模块、线特征提取与匹配跟踪及闭环检测以及地图构建模块。图4为融合目标检测的动态SLAM系统。

图4 融合目标检测的动态SLAM系统Fig.4 Dynamic SLAM system with fused target detection

YOLO v4采用CSPDarknet53作为特征提取网络，存在网络结构复杂及计算量大等问题。考虑到SLAM系统实时性需求，文中引入深度可分离卷积对主干网络进行改进，深度可分离卷积包含逐点和逐通道卷积，有效减小计算量，提升目标检测效率。

(1)目标检测模块：通过YOLO v4网络检测场景中动态目标，剔除场景动态信息，将结果输入跟踪以及地图构建模块。

(2)特征跟踪模块：结合ORB点特征和LSD线特征，建立点特征和线特征的重投影误差模型，通过非线性优化估计相机位姿。

(3)闭环检测模块：构建基于ORB点特征和LSD线特征的视觉词袋模型，度量图像相似性。

(4)地图构建模块：剔除场景动态信息，构建关键帧，构建外部环境静态点云地图。

3 实验结果与分析

文中选用的实验平台为履带式机器人和Kinect v1相机传感器。运行环境为64位Ubuntu16.04系统、处理器为i7-8750H、主频2.2 GHz和16 GB内存的便携式计算机，不使用GPU加速。

3.1 相机标定

传统相机标定法准确性高，但标定过程复杂，所需成本高。张正友标定法利用棋盘格模型进行相机标定，通过拍摄不同视角下的棋盘格，提取图像特征，计算单应性矩阵得出相机内参。具有成本低、准确性高和易于操作等优点，被广泛应用于相机标定。张正友标定法步骤如下：首先，将Kinect v1相机位置固定；其次，打印8×6棋盘格，将其固定于标定板；最后，调整标定板的角度和距离，获取多组彩色图像与深度图像。标定结果如表1所示。

表1 Kinect v1标定参数Tab.1 Calibration parameters of Kinect v1

3.2 动态物体检测

实验数据集选择COCO数据集，包含了行人等动态目标，如图5所示。数据集分为训练集、验证集和测试集。原始网络在检测动态物体的同时，会检测出图5所示的静态物体。通过对YOLO v4网络进行训练，如图6所示，可以有效检测出场景中的动态物体，准确识别出场景中动态物体信息。

图5 动态目标检测结果(训练前)

图6 动态目标检测结果(训练后)

3.3 动态场景下特征提取及匹配

SLAM前端加入优化后的YOLO v4网络，实现动态场景下特征的有效识别，通过传感器获取彩色图像和深度图像，一分支采用优化后的YOLO v4网络实现动态目标检测，同时对动态物体进行掩码处理，二分支对图像进行点特征和线特征提取。

对图像提取ORB点特征和LSD线特征，采用随机采样一致性算法RANSAC(Random Sample Consensus)剔除误匹配，点特征和线特征匹配结果分别如图7和图8所示。可以看出有效剔除了动态物体的干扰，去除了行人的特征，实现静态特征提取和匹配，避免动态场景对位姿估计的影响。

图7 特征点匹配Fig.7 Feature point matching

图8 特征线匹配Fig.8 Feature line matching

3.4 SLAM系统实验

采用SLAM常用TUM数据集验证文中方法(IPL-SLAM)的有效性。TUM数据集为慕尼黑工业大学通过Kinect传感器采集的RGB-D数据集，包含彩色和深度图像，分为低动态sitting序列和高动态walking序列，sitting序列包含肢体或小物体的轻微移动，walking序列包含行走及大范围运动[16]。数据集参数如表2所示。

表2 TUM数据集参数Tab.2 Parameters of TUM dataset

与经典的ORB-SLAM2系统进行对比，采用绝对轨迹误差(Absolute Trajectory Error, ATE)和相对位姿误差(Relative Pose Error, RPE)评估SLAM系统的准确性。ATE表示真实轨迹坐标与估计坐标之间的误差，其均方根误差为

(10)

RPE表示真实位姿变换与估计位姿变换之前的误差，其均方根误差为

(11)

图9为针对TUM数据集的低动态场景及高动态场景下，IPL-SLAM与经典ORB-SLAM2绝对轨迹误差对比。TUM数据集提供了真实轨迹(Ground Truth)，用于评估SLAM系统定位准确性。

图9 TUM数据集下ORB-SLAM2和IPL-SLAM绝对轨迹误差对比

如图9(a)和图9(b)所示：在低动态场景下IPL-SLAM与ORB-SLAM2位姿估计结果与真实值均较为接近，定位准确性较高。如图9(c)和图9(d)所示：在高动态场景下，ORB-SLAM2位姿估计结果与真实值轨迹误差较大，ORB-SLAM2系统受动态场景影响；IPL-SLAM仍然保持着较高的定位准确率，是由于文中采用YOLO v4剔除了场景中的动态特征，采用稳定的ORB特征点和LSD线特征共同进行SLAM前端及闭环检测模块，点特征和线特征相互辅助，实现了动态场景下SLAM系统的准确定位。表3为ORB-SLAM2和IPL-SLAM在数据集上的ATE和RPE均方根误差对比，结果表明IPL-SLAM在低动态及高动态场景均保持较高的定位准确性。

表3 ATE和RPE汇总 单位：m

3.5 真实场景实验

实验平台为搭载Kinect传感器的履带式机器人，如图10所示，处理器i7-8750H，内存16 GB，系统Ubuntu16.04。控制机器人在学校走廊内运动采集环境信息，光线较差的走廊内有行人随意行走，场景相似且纹理单一。

利用文中IPL-SLAM方法得到走廊点云地图如图11所示。移动机器人运动轨迹与实际机器人运动轨迹基本一致，纹理单一的走廊场景下，建立的场景环境地图不包含动态点无重影，剔除了动态行人特征，且与走廊实际情况相一致，表明文中方法在动态场景下鲁棒性较好。

图10 履带式机器人 图11 点云地图

4 结论

为提升动态场景下移动机器人SLAM的准确性和鲁棒性，提出了融合目标检测的IPL-SLAM方法。采用YOLO v4提取检测场景动态信息，剔除动态特征；利用四叉树算法实现特征均匀分布；在经典的ORB-SLAM2系统基础上，结合图像静态特征点和特征线进行移动机器人位姿估计，建立无动态特征的环境地图，适用于结构化及低纹理等复杂场景。实验结果表明：相比ORB-SLAM2系统，改进方法解决了动态场景下SLAM系统位姿估计准确性低和鲁棒性差的问题，且满足SLAM系统的实时性需求。