0 概述

同时定位与地图构建(Simultaneous Localization and Mapping, SLAM)问题是机器人和计算机视觉领域的研究热点。研究人员已设计了不同的传感器模式, 包括2D激光扫描仪、3D扫描仪、单目相机、双目相机和RGB-D传感器。现有SLAM方法按照输入数据类型的不同, 可分为基于深度相机的方法和基于单目相机的方法, 按照采用方法的不同, 也可以分为依靠特征点进行匹配并构建稀疏地图的特征点法和最小化光度误差并构建稠密地图的直接法。本文的研究对象是基于ORB(ORiented Brief)特征点的SLAM方法^[1-2]。ORB特征点^[3]可以在CPU上进行实时计算, 相比Harris等简单角点特征, 其具有良好的旋转和缩放不变性。此外, ORB定义的描述子在运动范围较大时也可以实现良好的回环检测和重定位效果。为了消除应用场景中移动目标和潜在移动目标的影响, 本文引入基于深度学习的对象检测算法以检测场景中的移动目标和潜在移动目标。基于非潜在动态目标的运动模型对特征点全集进行筛选, 利用其中的静态特征点实现位姿跟踪, 通过静态特征点中的非潜在动态特征点进行地图构建。

1 相关工作

目前, 多数SLAM方法的一个基本假设是环境为静态的。然而, 像人类这样的活动对象存在于许多真实场景中。因此, 最初设计用于在静态环境中执行SLAM的方法都无法处理复杂的动态场景。为解决该问题, 需要从环境中识别出移动目标, 然后在进行姿态估计之前丢弃它们。在稠密SLAM中, 许多运动目标检测方法往往基于光流方法。如果图像中存在运动目标就会产生光流, 通过计算光流的不一致性可以区分静态背景和运动对象。例如, 文献[4]使用最优估计和均匀采样的方法来检测动态对象, 与其他导数光流方法相比, 该方法具有较高的时效性, 但精度较低且计算量较大, 尤其是在采样量较大的情况下。文献[5]基于点轨迹对图像进行聚类, 将动态对象排除在最小化能量函数之外, 该方法具有健壮性但不具备实时性。近年来, 在动态场景中出现了许多SLAM方法, 它们大多使用语义分割或目标检测算法来识别环境中的动态目标。

dynaSLAM^[6]方法中设计了动态对象检测系统, 其使用MASK-RCNN^[7]实现实例分割, 并对具有移动性的物体进行分割, 将之前的20个关键帧的RGB信息和深度信息映射投影到当前帧上, 在没有动态对象的情况下完成背景修复。该系统能够较好地处理动态场景, 但无法实现实时操作, 而实时性能对于SLAM相关研究而言不容忽视。在DS-SLAM^[8]方法中, 通过将语义分割网络与光流法相结合以提供八叉树地图的语义表示, 从而降低基于视觉的SLAM中动态对象的影响。VSO^[9]的主要思想是使用语义作为不变的场景表示, 其假设视角、尺度和光照等方面的变化只影响物体的低层外观而不影响其语义。基于这一思想, 文献[9]提出了一种将语义约束集成到姿态优化和地图优化中的视觉语义测距方法。

近年来, 驾驶环境中的目标检测问题得到广泛关注。自动检测道路上的车辆和行人等物体有助于驾驶员了解道路状况、交通信息等, 因此, 目标检测可以用于各种应用, 如自动车辆^[10]和自动监视系统^[11]等。然而, 在开发驾驶环境下可靠目标检测方法的过程中, 面临着如目标遮挡^[12]、尺度的大方差^[13]等问题, 尤其是由停放的汽车、过往车辆和行人引起的道路阻塞, 这种遮挡会使道路上的目标检测难度升高。在文献[14-15]中, 提出了一种基于HOG和线性SVM的行人检测方法, 其主要思想是利用HOG特征和线性SVM学习正、负样本模板。文献[16]提出了一种基于多尺度变形分量检测模型的可变形零件模型。DPM是除深度学习之外的最优对象检测模型, 其通常采用滑动窗口检测方法, 通过构造一个尺度金字塔对每个尺度进行搜索。区域卷积神经网络(RCNN)^[17-19]是较早的CNN之一, 在检测模型中应用网络进行目标检测。RCNN在传统选择性搜索的基础上选择候选集, 建立CNN特征提取网络从而检测目标。YOLO是一种基于神经网络的目标检测系统^[20], 其将整个图像作为网络模型的输入, 将图像划分为S×S网格, 如果一个物体的中心落在网格上, 网络将检测这个物体然后输出该区域的物体以及置信度。

2 体系架构 2.1 主要框架

在现有面向动态环境的SLAM改进方法中, 实时性是一种重要的评价指标。首先, 本文考虑将基于像素的语义分割方法与SLAM相结合, 但实验发现基于像素的语义分割方法的运行速度和准确率成反比。因此, 本文引入基于深度学习的目标检测方法来检测环境中潜在的动态对象, 该方法较高的运行速度满足了SLAM系统的实时性要求。如图 1所示, 本文在基于特征点的SLAM方法中增加了基于深度学习的目标检测方法, 将输入图像中提取出的特征点分为两类, 一类是潜在动态特征点, 这一类特征点具有可移动性, 在场景中并非长时间固定存在(如车、人等目标上提取的特征点), 在重定位和闭环检测时可能会导致场景匹配失败; 另一类是非潜在动态特征点, 这一类特征点在场景中往往不能移动或者在足够长的时间内不会移动(如建筑物、树木等目标上提取的特征点)。

本文首先利用基于深度学习的目标检测将特征点集合U分为潜在动态特征点P(人、车等目标)和非潜在动态特征点P(建筑、道路和植被等); 然后使用特征匹配一致性评估跟踪的车辆位姿运动模型, 将特征点集合U分为动态特征点集合D(场景中实际移动了的特征点, 如从正在移动的车和人上提取的特征点)和静态特征点集合S(场景中未移动的特征点, 如从建筑物、停在路边的车上提取的特征点)。上述集合之间的关系为:

$ U = D \cup S = P \cup \bar P $

(1)

在位姿跟踪模块中, 只基于场景中的静态特征点进行跟踪, 排除动态特征点的影响, 可以有效提高跟踪精度。在建图模块中, 只基于场景中的非潜在动态特征点中的静态特征点S∩P进行建图, 可以提高地图的适用性, 防止在重定位和闭环检测时受到场景中动态目标的干扰。

2.2 ORB特征点提取

ORB特征点由关键点和描述符2个部分组成。ORB特征点提取主要分为2个步骤:

1) FAST角点提取:找到图像中的角点, 计算特征点的主方向并为后续的简要描述符添加旋转不变特征。

2) BRIEF描述子:描述上一步提取的特征点周围的图像区域。

FAST角点提取如图 2所示, 其主要依据是:如果一个像素与其周围像素显著不同(太亮或太暗), 则它可能就是角点。

ORB添加了尺度和旋转的描述。对于任意一个特征点p而言, 其邻域像素的矩为:

$ {m_{pq}} = \sum\limits_{x,y} {{x^p}} {y^q}I(x,y) $

(2)

其中, I(x, y)为点(x, y)处的灰度值。可以得到图像的质心为:

$ C = \left( {\frac{{{m_{10}}}}{{{m_{00}}}},\frac{{{m_{01}}}}{{{m_{00}}}}} \right) $

(3)

则特征点与质心的夹角定义为FAST特征点的方向:

$ \theta = {\rm{arctan}}({m_{01}},{m_{10}}) $

(4)

为了提高方法的旋转不变性, 需要确保x和y在半径为r的圆形区域内, 即x, y∈[-r, r], r为邻域半径。在提取有向FAST关键点后, 计算每个点的描述符。ORB选择BRIEF作为特征描述方法, BRIEF采用随机选取点的方法, 选择特征点周围S×S大小的像素块, 随机选取n对像素点, 定义如下:

$ \tau (p;x,y): = \left\{ {\begin{array}{*{20}{l}} {1,p(x) < p(y)}\\ {0,{\rm{其他 }}} \end{array}} \right. $

(5)

其中, p(x)是点x处的灰度值, 则特征点p的描述子定义为:

$ {f_n}(p): = \sum\limits_{i = 1}^n {{2^{i - 1}}} \tau (p;{x_i},{y_i}) $

(6)

2.3 基于深度学习的目标检测算法

随着深度学习技术的快速发展, 目标检测从基于手工特征的传统算法转化为基于深度神经网络的检测技术。本文采用由残差块构成的全卷积网络作为网络主体, YOLOv3中将其命名为Darknet-53网络, 结构如图 1所示, 在不同尺度上进行特征提取获得最终的目标检测结果。如图 3所示, 定义检测框集合为R={r₁, r₂, …}, 单个检测框定义为r(x, y, w, h)∈R, 其中, (x, y)为检测框左上角在帧中的坐标, (w, h)为检测框的宽和高。对特征点集合U中的每个特征点p(u, v)进行如下判断:

$ p \in \left\{ {\begin{array}{*{20}{l}} {\bar P,x \le u \le x + w{\rm{ 且 }}y \le v \le y + h}\\ {P,u < x{\rm{ 或 }}u > x + w{\rm{ 或 }}v < y{\rm{ 或 }}v > y + h} \end{array}} \right. $

(7)

经过上述操作, 特征点全集U被分为潜在动态特征点集合P和非潜在动态特征点集合P。

2.4 特征点匹配

在提取特征点后将其和参考帧中的特征点进行粗匹配, 由前文定义可知, P中的特征点都是非潜在动态特征点, 因此, 它们多数都是静态特征点或者运动范围非常小的特征点。本文针对属于集合P的特征点对, 采用文献[21]算法获得能够符合场景中静态特征点的最优运动模型。特征点匹配算法框架如图 4所示。

特征点匹配算法具体分为以下4个步骤:

1) 根据特征点的描述子对U中的特征点进行粗匹配, 比较特征点描述向量之间的Hamming距离, 距离越小表明2个特征点之间相似度越高, 若Hamming距离小于一定的阈值, 则表示2个特征点匹配成功。记匹配点对集合为U_m:

$ {U_m} = \left\{ {\{ {u_c},{u_r}\} ,\sum\limits_{i = 1}^n {f_n^i} ({u_c}) \oplus f_n^i({u_r}) < \delta } \right\} $

(8)

其中, u_c为当前帧中的一个特征点, u_r为参考帧中的匹配特征点, δ为定义的距离阈值。定义U_P为:

$ {U_{\bar P}} = \{ \{ {u_c},{u_r}\} ,\{ {u_c},{u_r}\} \in {U_m}{\rm{ 且 }}{u_c} \in \bar P\} $

(9)

2) 对匹配点对集合U_P进行顺序抽样, 标记每对点对为内点的概率η, 通过验证假设模型来更新η值。假设对于当前抽取的样本集I^*的假设模型为M^*, 当前的前3个最优模型为M₁、M₂和M₃, 对应的样本集为I₁、I₂和I₃, 若I_k∈I^*, k∈{1, 2, 3}, 则更新概率η_i为:

$ \begin{array}{l} {\eta _i} = \left\{ {\begin{array}{*{20}{l}} {{\eta _i} + {b_1},{u_i} \in {I^*}{\rm{ 且 }}{u_i} \in I}\\ {{\eta _i} + {b_2},{u_i} \notin {I^*}{\rm{ 且 }}{u_i} \in I}\\ {{\eta _i} - {b_1},{u_i} \in {I^*}{\rm{ 且 }}{u_i} \notin I}\\ {{\eta _i},{u_i} \notin {I^*}{\rm{ 且 }}{u_i} \notin I} \end{array}} \right.\\ I = \left\{ {\begin{array}{*{20}{l}} {{I_1},{I_1} \in {I^*}}\\ {{I_2},{I_2} \in {I^*}}\\ {{I_3},{I_3} \in {I^*}} \end{array}} \right. \end{array} $

(10)

若更新了模型M₁, 则M₃被删除, 模型M₁、M₂变成新的M₂和M₃。对M₂和M₃的更新策略可依此类推。

3) 在顺序抽取完匹配点对中的所有点对并更新η值后, 根据各个点对的η值进行重新排序, 重复执行第2步的操作, 直到在某次操作后对匹配点对重排序时并未改变匹配点对的顺序。分别计算M₁、M₂和M₃相对应的运动模型, 如下:

$ f({M_k}) = ({q_k},{t_k}),k = \{ 1,2,3\} $

(11)

其中, q_k、t_k为模型M_k对应的旋转四元数和平移距离, 定义:

$ \mathop {{\rm{argmin}}}\limits_k ({\rm{dif}}{{\rm{f}}_k}) = {\lambda _1}(q_k^{ - 1} \cdot q) + {\lambda _2}({t_k} - t),k = \{ 1,2,3\} $

(12)

其中, q、t为上一帧的旋转四元数和平移距离, λ₁、λ₂为常数。选择diff值最小的模型作为最优模型M的输出, 对应的匹配点对集合作为I的输出。

4) 根据最优模型M, 计算出相机运动的本质矩阵。设匹配点对{i_c, i_r}属于集合I, 其在当前帧中的特征点为i_c, 在参考帧中的匹配特征点为i_r。i_c和i_r的归一化坐标为:

$ \begin{array}{*{20}{l}} {{i_c} = ({a_c},{b_c},1)}\\ {{i_r} = ({a_r},{b_r},1)}\\ {\{ {i_c},{i_r}\} \in S} \end{array} $

(13)

根据对极约束:

$ ({a_r},{b_r},1)\left( {\begin{array}{*{20}{c}} {{e_1}}&{{e_2}}&{{e_3}}\\ {{e_4}}&{{e_5}}&{{e_6}}\\ {{e_7}}&{{e_8}}&{{e_9}} \end{array}} \right)\left( {\begin{array}{*{20}{c}} {{a_c}}\\ {{b_c}}\\ 1 \end{array}} \right) = 0 $

(14)

求得本质矩阵:

$ \mathit{\boldsymbol{E}} = \left( {\begin{array}{*{20}{l}} {{e_1}}&{{e_2}}&{{e_3}}\\ {{e_4}}&{{e_5}}&{{e_6}}\\ {{e_7}}&{{e_8}}&{{e_9}} \end{array}} \right) $

根据E可以计算出两帧之间的旋转矩阵R和平移向量t:

$ \mathit{\boldsymbol{E}} = \mathit{\boldsymbol{t}}{^ \wedge }\mathit{\boldsymbol{R}} $

(15)

其中, t^{^}是t的反对称矩阵。

2.5 位姿跟踪与建图

对于集合U中的特征点筛选分为以下2个步骤:

1) 如图 5所示, 当前帧中提取出的潜在动态特征点p_c(p_c∈U), 在参考帧中的对应匹配点为p_r, 根据上文计算出的本质矩阵E, 可以获得空间点P(X, Y, Z)在参考帧中的投影点为p, p和p_r的齐次坐标为:

$ \begin{array}{*{20}{l}} {p = (u,v,1)}\\ {{p_r} = ({u_r},{v_r},1)} \end{array} $

(16)

若满足式(17)则保留特征点p并加入到集合S中; 否则, 丢弃特征点p。

$ \left\{ {\begin{array}{*{20}{l}} {\sqrt {{{({u_r} - u)}^2} + {{({v_r} - v)}^2}} \le d}\\ {\sum\limits_{i = 1}^n {f_n^i} (p) \oplus f_n^i({p_r}) < \delta /2} \end{array}} \right. $

(17)

其中, d为设置的距离阈值。

2) 对集合S中的特征点进行跟踪。

对于建图模块的操作分为以下2个步骤:

1) 对跟踪模块输入的关键帧中的特征点集合进行筛选, 基于特征点集合L实现建图:

$ L = \{ l,l \in S{\rm{ 且 }}l \notin \bar P\} $

(18)

2) 插入该关键帧到地图中, 具体操作本文不做赘述。

图 6所示为本文方法和ORB_SLAM2系统的特征点提取效果对比, 图 6(a)为输入图像, 图 6(b)为ORB_SLAM2系统提取特征点后的图像帧, 图 6(c)为本文方法对特征点进行筛选后传入跟踪模块的图像帧。

3 实验结果与分析 3.1 KITTI数据集

在KITTI数据集中, 一些移动的车辆和行人对跟踪和定位精度产生影响, 本文方法将环境中的车辆和行人作为动态对象进行检测和处理。由于数据集中动态对象较少, 跟踪模块性能提升不明显, 但是可以有效减小没有闭环情况下的累积误差。从图 7可以看出, 在没有闭环的情况下, 因为本文方法在地图中滤除了所有移动的物体, 所以其轨迹误差相对ORB_SLAM2系统明显降低。

3.2 TUM数据集

如图 8所示, 在TUM数据集上, 本文方法的性能明显优于ORB_SLAM2系统。ORB_SLAM2在序列walking_xyz和walking_halfsphere上的轨迹跟踪误差较高, 原因是该方法主要针对的是场景中的动态对象。

表 1所示为TUM数据集中本文方法和ORB_SLAM2系统的绝对路径误差(ATE)对比结果, 表 2、表 3所示分别为2种方法相对位姿误差(RPE)在平移和旋转方面的对比结果。其中, RMSE为均方根误差, MEAN为平均误差, STD为标准差, Improvement定义为:

$ {\rm{Improvement}} = \frac{{{\rm{ our}} - {\rm{ori }}}}{{{\rm{ ori }}}} \times 100\% $

(19)

其中, our为本文方法的运行结果, ori为ORB_SLAM2系统的运行结果。从表 1~表 3可以看出, 相对ORB_SLAM2系统, 本文方法性能提升明显。

4 结束语

为了降低动态对象对跟踪定位结果的影响, 本文提出一种基于视觉特征的实时SLAM方法。该方法引入基于深度学习的目标检测算法, 将特征点进行分类处理, 降低动态特征点对位姿跟踪与建图造成的误差。实验结果表明, 与ORB_SLAM2系统相比, 该方法的跟踪性能得到明显提升, 其运行速度可以满足实时性的要求, 且在多数情况下能达到较高的精度。下一步将使用像素级语义分割算法处理动态对象区域, 以在保证图像实时性的前提下提高其处理效率。