运动曲率自适应V-SLAM算法研究

赵荣亮，王红旗，刘群坡，张 岩

运动曲率自适应V-SLAM算法研究

赵荣亮，王红旗，刘群坡，张 岩

（河南理工大学电气工程与自动化学院，河南 焦作 454000）

传统室内视觉同时定位与建图（V-SLAM）存在特征点误匹配剔除耗时长、较大转弯时定位精度低等问题。为了提升匹配速度，提出一种基于特征点的余弦相似度直方图剔除误匹配方法。为了避免大幅度转弯时关键帧丢失，提出一种基于运动曲率的关键帧自适应提取V-SLAM算法（MSAV-SLAM）。该算法通过高效多点透视（EPnP）计算帧间位姿，以融合了运动曲率、帧间位移和旋转矩阵的复合运动量为参考变量，自适应选择关键帧。室内数据集实验表明，所提出的算法与基于固定时间、空间和原ORB-SLAM2算法相比，平均每帧追踪时间减少为原算法的57.62%，且绝对轨迹误差为改进前的81.45%。

视觉同步定位与地图创建；关键帧；特征点匹配；运动曲率；室内定位

0 引言

近年来，室内同时定位与建图（simultaneous localization and mapping，SLAM）进展很大，在室内服务机器人、智慧工厂和智能仓储[1]等领域发挥着重要作用。根据室内定位SLAM系统的传感器类型可分为视觉同时定位与建图（visual-SLAM，V-SLAM）和激光SLAM 2大类，与激光传感器相比，相机获取的信息丰富，便于提取环境中深层次的信息，因此V-SLAM成为当前的研究热点[2]。而在V-SLAM中，依据视觉前端对相机图像的处理方式，一般可分为特征点法和直接法。直接法通过最小化光度误差的方法计算相机的运动轨迹，计算速度很快，但光照变化对其影响很大。特征点法需要计算每帧图像中的特征点与描述子，对环境中光照强度和尺度等因素的变化不敏感[3]。

文献[4]提出了首个基于单目相机的单目同时定位与建图算法（monocular simultaneous localization and mapping, MonoSLAM），利用扩展卡尔曼滤波器（extended Kalman filter, EKF）实时计算出相机的运动轨迹。但随着地图规模变大，系统状态变量维数不断增多，计算复杂度会急速增加。文献[5]提出的并行追踪与建图（parallel tracking and mapping,PTAM）通过光束法平差（bundle adjustment, BA）优化关键帧，避免对每帧图像进行全局优化，并将追踪线程与建图线程分离，可以进行较大规模建图。2015年，文献[6]在PTAM算法的基础上进行大量改进，提出了尺度不变特征变换同时定位建图算法（oriented fast and rotated brief-SLAM，ORB-SLAM），采用宽松的关键帧选择和剔除机制，以减少追踪丢失。文献[7]增加了双目相机和深度相机的接口，优化了特征点的存储方式，为减少误匹配点对位姿求解的干扰，使用随机采样一致法（random sample consensus, RANSAC）计算最优位姿。文献[8]在其基础上使用渐进采样一致算法（progressive sample consensus，PROSAC），缓解了前端的计算压力，但对特征点要求较高。

V-SLAM中的关键帧机制是影响系统精度的重要因素，吸引了许多研究者的注意。传统关键帧选择算法如基于固定时间、空间等，这2种算法都属于在时域或空域的均匀采样[9]，计算速度快，在相机运动轨迹简单时表现良好，但忽略了关键帧之间的共视关系，定位效果很差。ORB-SLAM2结合时间和空间等特征设置多个判断条件选择关键帧[10]，避免了上述问题；但在相机旋转角度大时易漏选关键帧，定位精度降低，甚至造成追踪失败。文献[11]根据非关键帧滑动窗口帧间约束关系对关键帧筛选机制进行优化，减少了后端优化的计算量。文献[12]通过同时维护直接法与特征点法选择的2种关键帧，保证了系统的全局一致性。文献[13]对关键帧再次筛选，解决了往返运动存在重复关键帧的问题。

综上所述，ORB-SLAM2使用RANSAC算法多次迭代确立最优运动模型，特征点的计算复杂，耗时较长；当相机旋转角度大时易漏选关键帧。本文提出的运动状态自适应的视觉同时定位与建图算法（motion state adaptive V-SLAM，MSAV-SLAM），在特征追踪阶段先计算出特征点方向向量，构建余弦相似度直方图，对误匹配进行初步剔除，以降低迭代次数，减少特征点的计算量。在局部优化阶段，使用运动曲率自适应的关键帧选取策略，可以避免关键帧漏失，提升室内定位精度。

1 V-SLAM算法框架

本文在ORB-SLAM2算法框架的基础上进行改进，主要由追踪模块、局部建图模块和回环检测模块3部分组成。

追踪模块先提取出每帧图像中的ORB特征点，再根据对应帧之间匹配成功的特征点分别计算基础矩阵与单应矩阵，并选择运动追踪模型求解初始位姿与特征点在地图中的投影点坐标。本文算法先计算出特征点的主方向向量，再建立余弦相似度直方图进行误匹配剔除。局部建图模块使用运动曲率自适应的关键帧选取机制。在相机旋转幅度大时可准确选取关键帧，再根据关键帧进行局部优化和地图创建。回环检测模块通过词袋模型获取当前帧与关键帧之间的共视关系，检测是否发生回环。若发生回环，则根据该帧与地图的约束关系进行全局BA优化修正整体运动轨迹，减少累积漂移。

2 误匹配剔除与关键帧选取

2.1 余弦相似度直方图误匹配剔除

特征点匹配是室内V-SLAM算法实现图像帧之间数据关联的关键步骤，准确的特征点匹配对前端求解位姿以及后端的全局优化十分重要；但是在特征点匹配时常常会出现错误的匹配，需要进行误匹配剔除。一般的，匹配正确的特征点远多于误匹配的点，且在发生旋转时对应特征点的方向一致；而在追踪过程中相邻二帧的相机旋转角度小于90°，否则视角会完全丢失，无法进行定位。余弦相似度可以衡量2个向量方向的相似程度，因此，本文利用相邻图像帧之间的特征点方向向量的余弦相似度直方图来剔除误匹配的特征点。

图1 未剔除误匹配特征点

图2 剔除误匹配特征点

2.2 运动曲率自适应关键帧选择策略

V-SLAM中关键帧直接影响系统的性能，一般从以下二方面选取关键帧：一方面是关键帧必须是清晰的图像，且拥有分布均匀的高质量特征点，

能满足建图和位姿优化的要求；另一方面是关键帧必须能够充分代表相邻普通帧，且要与其他关键帧有一定的共视关系。

代入针孔相机模型为

图3 运动曲率自适应选取关键帧

3 实验与结果分析

本文所有实验在处理器为AMD Ryzen74800U，内存4 GB，操作系统为Ubuntu 16.04.6 LTS 64-bit的计算机上进行。为验证算法在室内定位的性能，先确立评价算法定位精度与实时性的指标。在欧盟机器人挑战赛（European robotics challenge，EuEoC）的室内数据集上进行实验，并与多种关键帧选择算法进行对比。

3.1 评估标准

本文采用绝对轨迹误差（absolute trajectory error，ATE）的平均值评价定位精度，在慕尼黑工业大学（Technische Universität München，TUM）数据集的基准测试中首次使用该指标。使用评估里程计(evaluation of odometry and SLAM，EVO）工具计算不同关键帧算法的ATE，并绘制轨迹图。SLAM算法在追踪阶段计算相机位姿耗时较长，是影响系统整体效率的关键因素。因此选用SLAM系统在追踪阶段处理每帧图像的平均时间作为实时性指标。为避免参数调整的影响，在不同关键帧选取算法中，每帧图像的特征点提取数量等参数都使用默认值。

3.2 结果与分析

EuRoc数据集由苏黎世联邦理工学院的无人驾驶系统实验室发布，包含微型无人机在真实厂房环境和2个不同房间内采集并对齐后的双目图像。相机使用全局快门曝光，采集帧率为20 Hz，分辨率为752×480个像素。无人机的真实运动轨迹由动作捕捉系统提供，误差在1 mm左右。根据无人机飞行速度和环境等因素，将不同序列分为简单、中等和困难3个等级。

为避免系统波动的影响，每种算法运行10次以上，取中位数进行对比。基于时间与空间的算法在追踪模块与原ORB-SLAM2算法相同，因此只对比本文算法与原ORB-SLAM2算法在追踪模块每帧图像的处理时间。2种算法在MH-05序列上每帧图像的追踪时间对比如图4所示。可以看出，本文算法耗时整体上低于原算法。

图4 追踪时间对比

为验证本文算法在特征提取模块的速度优势，使用EuRoC数据集中不同场景下的序列进行实验，包括光线较暗的厂房内的MH系列和2个不同房间内的V1、V2系列，结果如表1所示。

表1 不同序列平均每帧追踪时间 ms

从表1可知：在不同场景下，本文算法比ORB-SLAM2算法速度都有提升，平均追踪时间为原算法的57.62%；而MH-05序列的光线较暗，提取的特征点数量少，因此速度提升较小。

为验证本文算法在相机转弯幅度较大时的性能，选取运动速度较快、轨迹复杂、困难等级的MH-05序列进行实验。该序列飞行距离为97.8 m，时间为111 s，平均速度为0.88 m/s。分别使用时域均匀、空域均匀、原ORB-SALM2和融合曲率 4种不同的关键帧机制实验。为避免关键帧数量的影响，保证基于时域和空域算法所选取的关键帧数量与融合曲率后选取的数量大致相等，约占总帧数的20%。融合曲率后选取的关键帧数量略大于原关键帧策略，不影响系统整体。4种算法的估计轨迹与轨迹真值的对比如图5所示。

图5中虚线GroundTruth代表数据集提供的轨迹真值；曲线D-SLAM为基于固定距离选择关键帧算法的轨迹；曲线T-SLAM为基于固定时间选择关键帧算法的轨迹；以及本文MSAV-SLAM的轨迹曲线，和原ORB-SLAM2的轨迹曲线都有表示。整体来看，基于固定时间与空间的算法定位精度较低。无人机在和处的旋转幅度较大；从局部放大图中可以直观地看出，本文算法比其他3种算法更接近轨迹真值。

图5 不同关键帧选择策略的运动轨迹

通过EVO工具计算4种不同关键帧选取算法的ATE，如表2所示，T-SLAM和D-SLAM分别为固定时间和空间的关键帧选择算法，加粗表示该项最低值。

表2 不同关键帧选择策略的绝对轨迹误差（ATE） mm

从表2可以看出：由于无人机的速度变化大，因此基于时间选择关键帧的T-SLAM算法精度最低；基于空间的D-SLAM算法受速度影响较小，精度高于T-SLAM算法；本文MSAV-SLAM与其他3种算法相比绝对轨迹误差的平均值最小，最优结果仅为原ORB-SLAM2算法误差的81.45%。

综合各项指标，本文的MSAV-SLAM算法在不同的室内场景中，在特征追踪模块的耗时优于ORB-SLAM2，且相机轨迹更加符合真值，可以满足V-SLAM系统实时性和定位精度的要求。

4 结束语

本文的MSAV-SLAM算法根据余弦相似度剔除误匹配点，可以减少特征点的运算量，提升室内定位系统效率。使用运动曲率自适应的关键帧选择机制，解决了ORB-SLAM2算法室内在转弯幅度较大时定位精度降低的问题。选取室内EuRoC数据集上相机旋转角度大、运动轨迹复杂的序列进行实验。结果表明，本文的算法定位精度和追踪速度均优于原ORB-SLAM2算法。

[1] 王雅仪, 余萌, 朱锋. 线面特征结合的三维激光SLAM算法及性能评估[J]. 导航定位学报. 2021, 9(1): 122-129.

[2] TAKETOMI T, UCHIYAMA H, IKEDA S. Visual SLAM algorithms: a survey from 2010 to 2016[J]. Ipsj Transactions on Computer Vision & Applications. 2017, 32(1): 1-19.

[3] 方琪, 王晓华, 苏杰. 基于分组策略的点线特征融合同步定位与地图构建算法[J]. 激光与光电子学进展, 2021, 58(14): 405-413.

[4] DAVISON A J, REID I D, MOLTON N D, et al. MonoSLAM: real-time single camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence. 2007, 29(6): 1052-1067.

[5] KLEIN G, MURRAY D. Parallel tracking and mapping for small AR workspaces[EB/OL]. [2022-05-29]. http: //www. robots. ox. ac. uk/～gk/publications/KleinMurray2007ISMAR. pdf.

[6] MUR-ARTAL R, MONTIEL J M M, TARDOS J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.

[7] MUR-ARTAL R, TARDOS J D. ORB-SLAM2: an open-source SLAM system for monocular, stereo and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262.

[8] 曹蜜, 胡凌燕, 熊彭文, 等. 基于PROSAC算法与ORB-SLAM2的RGB-D相机室内视觉定位研究[J]. 传感技术学报, 2019, 32(11): 1706-1712.

[9] 张新良, 李阳, 赵运基. 一种基于时空切片的SLAM关键帧提取方法[J]. 系统仿真学报. 2018, 30(5): 1724-1729.

[10] ZIELIŃSKI K, STASZAK R, NOWACZYK M, et al. 3D dense mapping with the graph of keyframe-based and view-dependent local maps[J]. Journal of Intelligent & Robotic Systems. 2021, 103(2): 28.

[11] 张小国, 刘启汉, 李尚哲, 等. 基于双步边缘化与关键帧筛选的改进视觉惯性SLAM方法[J]. 中国惯性技术学报, 2020, 28(5): 608-614.

[12] 谷晓琳, 杨敏, 张燚, 等. 一种基于半直接视觉里程计的RGB-D SLAM算法[J]. 机器人, 2020, 42(1): 39-48.

[13] XIE P, SU W, LI B, et al. Modified keyframe selection algorithm and map visualization based on ORB-SLAM2[C]//TheInstitute of Electrical and Electronic Engineers (IEEE). Proceedings of the 20204th International Conference on Robotics and Automation Sciences. Wuhan, China: IEEE, 2020: 142-147[2022-05-29].

Motion curvature adaptive V-SLAM algorithm research

ZHAO Rongliang, WANG Hongqi, LIU Qunpo, ZHANG Yan

（School of Electrical Engineering and Automation, Henan Polytechnic University, Jiaozuo, Henan 454000，China）

Traditional indoor vision simultaneous localization and mapping (V-SLAM) has many problems, such as time-consuming feature point mismatch elimination and low localization accuracy in large turns. In order to improve the matching speed, a method of eliminating false matching based on cosine similarity histogram of feature points is proposed. In order to avoid the loss of key frames in large turns, a motion state adaptive key frame extraction V-SALM (MSAV-SLAM) algorithm was proposed. This algorithm calculates the interframe pose by efficient perspective-n-point (EPnP), and adaptively selects key frames by taking the combined motion curvature, interframe displacement and the compound motion of rotation matrix as reference variables. Laboratory data set experiments show that compared with the original ORB-SLAM2 algorithm based on fixed time and space, the average tracking time per frame of the proposed algorithm is reduced to 57.62%, and the absolute trajectory error is 81.45%.

visual simultaneous localization and mapping; key frame; feature point match; motion curvature; indoor localisation

TP242

A

2095-4999(2022)06-0053-06

赵荣亮，王红旗，刘群坡，等. 运动曲率自适应V-SLAM算法研究[J]. 导航定位学报, 2022, 10(6): 53-58.（ZHAO Rongliang, WANG Hongqi, LIU Qunpo, et al. Motion curvature adaptive V-SLAM algorithm research[J]. Journal of Navigation and Positioning, 2022, 10(6): 53-58.）

10.16547/j.cnki.10-1096.20220607.

2022-05-25

赵荣亮（1997—），男，河南南阳人，硕士研究生，研究方向为基于视觉的移动机器人定位。

王红旗（1973—），男，河南周口人，博士，副教授，硕士研究生导师，研究方向为机器人控制。