复杂交通环境下基于关键目标的视觉SLAM

连 静，皮家豪，李琳辉

(1.大连理工大学 汽车工程学院， 辽宁 大连 116024； 2.大连理工大学 工业装备结构分析国家重点实验室， 辽宁 大连 116024)

0 引言

实时定位是实现智能汽车自主导航的重要前提，而在GPS距止环境下，SLAM(simultaneous localization and mapping，同时定位及地图构建)技术作为实现实时定位的重要技术[1]，是智能驾驶行业的研究热点。根据传感器的不同，SLAM主要包括视觉SLAM[2]和激光SLAM[3]，而视觉传感器由于具有价格低、获得信息丰富等优点，成为学者们关注的重点。

根据前端的不同，视觉SLAM可以分为以SVO[4]、DSO[5]为代表的直接法和以ORB-SLAM3[6]为代表的特征法。特征法利用点特征的重投影误差进行位姿计算。相比基于光度误差的直接法，特征法在光照变化、相机剧烈运动等场景下的鲁棒性更好。特征点法依赖于场景中特征点的提取，在动态特征过多和近处纹理匮乏等复杂场景下会出现定位精度下降甚至定位失效的情况。

为提高动态场景中的定位算法的性能，面向动态环境的视觉SLAM算法相继被提出，如Berta[7]提出的DynaSLAM在ORB-SLAM2[8]基础上融合了由Mask-RCNN[9]构成的语义模块，对潜在的运动物体区域进行分割，并结合区域增长算法对动态特征点进行剔除。Yu等[10]提出的DS-SLAM则是通过SegNet[11]语义分割网络进行动态物体的识别，在去除环境中运动物体的影响的同时，构建八叉树语义地图。Han等[12]提出的MLC-SLAM在视觉词汇表中引入语义信息进行训练，而后通过定义静态速率的方式处理动态物体。尽管上述算法在大多数室外交通场景中表现优异，但是它们在一些近处纹理缺失和动态车辆同时存在的场景容易出现定位失效的问题。

针对复杂场景的另一种解决方法则是利用更高级的语义信息，在参数化语义目标后，通过语义目标之间的关联进行位姿求解，如Yang等[13]提出的CubeSLAM，利用目标检测出来的2D检测框，通过消失点采样生成3D立方体，继而基于立方体之间的关联进行位姿的求解和优化；Nicholson等[14]提出的QuadricSLAM通过目标检测后的边界框将物体表达成椭球体，通过椭球体的重投影误差求解位姿，Tian等[15]进一步完善了椭球体的初始化和椭球体之间的数据关联问题，提高了物体级SLAM系统的鲁棒性。但是它们在交通场景中主要是对静态和动态车辆进行参数化，在只有动态车辆或者没有车辆的场景下效果并不显著。Li等[16]提出的TextSLAM，通过文本检测提取出文本信息，通过融合文本信息之间的光度误差和文本内特征点的几何误差计算位姿，提高了定位算法在商场和室内环境下的定位精度和稳定性。还有学者通过结合道路结构化特征[17]，提高有效特征数量进而提高算法在近景特征不足场景的定位精度。但是该方法在路面特征被运动目标遮挡严重的场景下效果并不显著。

针对上述问题，提出一种基于关键目标的视觉SLAM算法。通过提取交通场景中普遍存在的交通标志和路牌等静止目标并在内部进一步完成特征的提取，经过筛选后构建出初始的关键目标。然后，在相连帧之间通过几何关系和特征之间的匹配完成关键目标的匹配，并在此基础上完成系统的初始化和关键目标的跟踪。最后，基于关键目标求解相机位姿，并在局部地图中对二者联合优化，以提高算法在复杂环境下的定位稳定性。

1 基于关键目标的SLAM算法框架

本文算法的整体流程如图1所示，主要包括跟踪、关键目标管理和局部建图3部分。跟踪部分的流程是在图像输入后，对图像进行静止目标检测，继而在静止目标范围内提取特征点。如果SLAM系统还没有初始化或者初始化失败，则尝试进行地图的初始化，在初始化成功后则进行基于关键目标的跟踪和位姿估计；关键目标管理部分主要是控制关键目标的生成以及关键目标内部特征点的生成和更新；局部建图部分则是控制关键目标在局部地图中的插入、更新和剔除，以及对相机位姿和关键目标特征的整体优化。

图1 算法整体流程框图

1.1 关键目标提取

在交通环境中分布着丰富的静止目标，如交通指示灯、交通标志等。它们不仅能提供车辆周围的环境信息，为智能车辆的行驶提供方向上的指引，还能给车辆的定位系统提供稳定的参考。因为它们的安装位置较高，遍布在道路中央和两旁，不容易被前方车辆遮挡，容易识别，具有鲜明的标识作用。同时它们纹理丰富，适合特征的提取。

得益于深度学习在近年来的迅速发展，对交通场景中静止目标的检测方案也不断被提出[18-20]。如图2所示，交通场景中的交通灯、交通标志和路牌分别被目标检测网络检出。

图2 静止目标检测结果

当静止目标检测完成后，在目标区域内提取ORB[21]特征，特征点提取的数量根据静止目标区域在图像上的占比确定。为了避免特征点出现过度扎堆的现象，采用ORB-SLAM3[6]中的四叉树方法均匀化特征点，特征提取结果如图3所示。广告牌在图像中占比较大，因此广告牌上特征点分布更多。接着，通过静止目标中特征点数目筛选关键目标，确保筛选出的关键目标有足够的特征用于后续基于特征的匹配和基于关键物之间的跟踪。至此，完成关键目标的初步构建，并根据其检测类别和检测顺序对其分类和编号。

图3 静止目标特征提取

1.2 系统初始化

在完成关键目标的构建且关键目标的数量大于初始化阈值后，则将当前帧设置为待初始化帧，开始尝试初始化过程。当连续两帧都满足初始化的基本条件时，对两帧之间的关键目标进行匹配并实施初始化，具体的过程如下：

1) 关键目标匹配。以关键目标区域的中心为圆心，设置关键目标的匹配半径，根据关键目标类别在搜索范围内确定候选关键目标，通过关键目标的几何信息筛选候选关键目标。筛选条件如式(1)和式(2)所示。

(1)

(2)

筛选条件中对于关键目标高度的变化要求更严格。因为智能车辆在道路上的行驶可以近似为平面运动，只存在直行或转弯的情况，相较于偏航角度的变化，俯仰角度并不会出现较大幅度的变化。

当多个关键目标区域满足几何筛选条件时，通过内部特征关联完成最后的筛选。筛选方式是对待匹配目标区域与筛选目标区域的特征点依次进行匹配，并记录匹配成功的特征点数目，特征点匹配数目最多且大于阈值条件的即视为匹配成功的关键目标区域，否则匹配失败，并将该关键目标从当前跟踪的局部地图中剔除。关键目标的匹配结果如图4所示，图中连线两端对应着关键目标内匹配成功的特征点。

图4 关键目标匹配

2) 初始化条件判定。在关键目标匹配步骤完成后，根据匹配成功的关键目标数量判定是否满足初始化条件，满足阈值条件则开始初始化位姿计算，否则初始化失败。

3) 初始化特征点对选择。单目初始化过程中使用RANSAC[22]算法随机选择特征点，筛选过程与ORB-SLAM3[6]不同，分两步进行。首先确定关键目标内待选择特征点数量，当关键目标数量不满足8个时，则根据关键目标内匹配成功的特征点个数进行排序，排序靠前的选择更多的特征点参与初始化位姿计算，同时也保证每个关键目标都有特征点参与初始化计算。然后在特定关键目标内按照指定特征点数目随机选择特征点。

4) 关键目标和特征点三角化。根据特征点三角化的比例判断初始化是否成功以及关键目标区域三角化是否成功，如果初始化成功但是关键目标区域三角化失败，则剔除对应的关键目标区域并删除其包含的所有三角化路标点。

5) 初始地图创建。当关键目标区域三角化成功后，将关键目标中心的三维坐标和对应的路标点坐标插入到地图中，完成初始的地图构建，同时为后续关键目标的跟踪和相机位姿计算建立基础。

1.3 关键目标跟踪

借鉴ORB-SLAM3[6]的方式，采用恒速模型计算出关键目标中心在当前帧的像素坐标，并基于此进行跟踪。

(3)

计算得到关键目标中心点位置后，设置搜索半径，在目标区域内搜索对应的候选关键目标区域，通过目标类别和目标区域的几何信息进行筛选，筛选方式与1.2节方法相似。

在关键目标和关键目标的匹配过程中，对其内部特征点匹配前，先确定待匹配点在当前帧中的搜索中心，该中心通过特征点在关键目标区域内的相对位置确定。在指定半径内搜索特征点，然后依据BRIEF描述子之间的汉明距离筛选相似度最高的特征点。接着根据旋转一致性检验，剔除在方向角度上不符合角度分布规律的特征点，完成关键目标内特征点的最终匹配，并依次记录各候选区域匹配成功的特征点数量。特征数量最多且大于匹配阈值的候选关键目标即为匹配成功的关键目标。否则，匹配失败并剔除局部地图中对应的关键目标。

1.4 位姿求解

完成关键目标之间的数据关联后，基于关键目标内的特征点和关键目标中心坐标估计相机位姿。该估计通过最小化下列的代价函数实现。

(4)

(5)

式中：ρ代表Huber鲁棒核函数；Pij代表第i个关键目标内第j个特征对应的三维坐标。

1.5 局部BA

通过关键目标的数量和新关键目标的比例筛选当前图像帧，并完成关键帧的创建，然后在局部建图线程中，对关键帧位姿及关键目标进行联合优化，优化方法选择鲁棒的LM(Levenberg-Marquardt，列文伯格-马夸尔特)方法。

(6)

式中：x为局部优化窗口中所有关键帧位姿，关键目标中心坐标和关键目标内特征点的三维坐标。

完成局部BA后，对相应的关键帧位姿、关键目标中心坐标和关键目标内地图点进行更新。更新结果用于最新的关键目标跟踪和位姿计算，并等待下次局部BA的开始。

2 实验

2.1 数据集

选择ApolloScape[23]的自定位数据集作为算法测试的数据集。该数据集记录了北京、广州和成都等地的城市交通场景，并且每一个区域都在不同的天气以及光照条件下进行重复扫描，因此数据集的场景复杂多样，更具有挑战性。自定位数据集中对于记录的每个片段都提供了相应的轨迹真值，便于实验部分的定位误差的定量比较。

2.2 实验结果

为了验证算法的有效性，选择Road11中具有挑战性的Record001序列场景和Road12中动态物体遮挡严重的Record001序列场景进行验证。场景如图5所示。场景a中，车辆刚从高楼和树木环立的阴影中驶出，由于外部强烈的光照和地面的反光，近处地面上的纹理几乎没有。近处的树叶由于角度的变化其轮廓也会跟随改变，没有区分度，不适合特征的提取和匹配。稍远处则是正在横向行驶的车辆，马路对面则是商铺以及建筑等。远处可以看见清晰的交通信号灯，以及广告牌等标志物。场景b中，车辆正处于转弯路口，前方都是动态的车辆，路面遮挡严重，不适合路面特征的提取。对面灰色建筑上分布着重复纹理，不利于特征点之间准确的匹配。

图5 Apollo数据集中的复杂场景

在图5所示的典型场景中，当前主流的视觉SLAM算法——ORB-SLAM3[6]和DynaSLAM[7]都出现了定位失效的问题，而本文算法则成功完成了场景的跟踪，算法轨迹和真实轨迹的对比如图6所示。

图6 平面轨迹

从图6(a)和图6(b)可以看出，本文算法的整体轨迹与真实轨迹基本一致。其中图6(a)中的轨迹只在中间部分和真实轨迹有些许偏差，图6(b)中的轨迹则是在弯道中央处的弧度要稍大于真实轨迹，但并不妨碍整体的定位精度。因此，所提算法通过关键目标能有效地完成复杂环境下的定位任务。使用绝对轨迹误差作为SLAM算法定位精确性的评价指标。从误差最大值、误差均值和均方根误差等方面进行评价，结果如表1所示。

表1 绝对轨迹误差 m

表1中的ORB-SLAM3是当前最先进的特征点法SLAM，DynaSLAM则是面向动态环境的先进SLAM算法，“x”表示对应算法在该场景中定位失效，无法获得轨迹误差。本文算法在避免定位失效的同时，在场景a中轨迹的平均误差在0.228 m，场景b中轨迹的平均误差在0.119 m，满足智能车辆车道级定位精度的要求。在上述典型场景中的实验证明了本文算法在复杂场景下仍能保持较高的定位精度，具有较高的定位鲁棒性。

3 结论

提出了一种基于关键目标的视觉SLAM算法，以解决因近处纹理匮乏、动态车辆干扰等复杂环境下的定位问题。首先，利用目标检测算法提取出静止目标，然后提取静止目标上的特征，根据特征数目判断能否生成关键目标。在单目初始化阶段，根据关键目标的数目以及求解结果判断初始化结果。在跟踪阶段，利用关键目标中心和恒速模型进行跟踪，完成关键目标之间的数据关联，通过最小化关键目标中心和关键目标特征的重投影误差求解位姿，并对关键目标的状态进行更新。经实验验证，所提算法能在当前视觉SLAM出现定位失效的复杂交通场景保持定位的稳定性，且能满足智能车辆在道路中行驶时的定位精度，对智能车辆的定位系统具有重要意义。