基于改进关键帧选择的ORB-SLAM2 算法

2023-02-09张洪于源卓邱晓天

北京航空航天大学学报 2023年1期

张洪，于源卓，邱晓天

(1. 江南大学机械工程学院，无锡 214122； 2. 江苏省食品先进制造装备技术重点实验室，无锡 214122)

近年来，移动机器人逐渐走进社会为人们的生产生活提供便利[1-2]。在这种情况下，机器人的控制与路径规划显得尤为重要[3-4]。控制与路径规划需要定位与建图作为前提，因此，将机器视觉与同时定位与建图相结合(V-SLAM)[5-7]成为了机器人领域研究的热点[8]。相比于采用雷达的二维SLAM，V-SLAM 通过机器视觉获取三维信息，对物体空间的几何外形进行描述，对于机器人在未知领域的导航有着重要意义[9]。随着国内外学者的逐渐深入研究，V-SLAM 先后出现了LSD-SLAM、RGB-D SLAM、ORB-SLAM 及语义SLAM。

TUM[10-11]计算机视觉组采用了直接法的方式，通过灰度图的强度对相邻的2 帧进行匹配成功，实现大规模环境下的定位与建图。Endres 等[12]使用深度相机获取视觉信息，完成了RGB-D SLAM 算法，以提高建图鲁棒性。Mur-Artal 等[13]提出了经典的ORB-SLAM 算法，在跟踪、建图、重定位及闭环检测的过程中，全程采用ORB 特征完成，使其具有较好的实时性；随后，为解决ORB-SLAM 所用相机单一且精度低的问题，又提出了ORB-SLAM2[14]，该算法采用光速平差法进行优化使定位更加精确，并提出了一种轻量级的定位模式，使其能够更加有效地重用地图。Liu 等[15]对物体语义信息进行了总结，语义SLAM[16-17]也逐渐出现在人们的视野中，借用物体的语义信息，成功提高了SLAM 的人机交互能力，提高了算法精度。

相比于其他SLAM 算法，ORB-SLAM2 算法具有高效的实时性、相机使用的多样性及良好的精度,深受国内外研究学者的喜爱。关键帧作为决定ORB-SLAM2 算法精度的重要因素，广大学者针对其进行了大量创新优化，以提升算法性能。Yang 等[18]提出基于地图元素和关键帧数据库的离线地图构建算法，通过对关键帧与地图元素在线储存离线构图的方式提高建图精度。Xie 等[19]提出重复运动检测方法改进关键帧选择算法，剔除冗余关键帧以提高整个系统的实时性能及定位精度。Cui 等[20]在ORB-SLAM2 算法的基础上使用图像强度而非特征点匹配关键帧，提出了Direct-ORB-SLAM，该方法避免了计算冗余，使系统在效率与准确度上均有提高。综上所述，国内外针对ORB-SLAM2 关键帧的研究已取得大量成果，但不难发现：①在跟踪定位的过程中，跟踪失败情况经常发生，严重影响定位效果；②虽然对算法进行了优化，但精度不高，误差较大的问题依然存在。

基于此，本文首先采用ORB-SLAM2 算法对帧间相对位姿进行估计；其次在原有算法模型的基础上，增加旋转与平移量作为判定依据，决定是否插入新关键帧，解决了跟踪定位易失败的问题，并提高了定位精度；然后设计了劣质关键帧剔除算法，避免了因移动机器人所安装的相机与机器人机身产生相对运动而导致劣质关键帧生成的问题；最后通过RGB-D 数据集与移动机器人开展了实验验证。

1 ORB-SLAM2 算法

经典的V-SLAM 算法框架主要通过视觉传感器读取数据，完成前端视觉里程计，通过后端进行非线性优化，回环检测再次对整体地图进行优化更新，以完成地图的构建。而ORB-SLAM2 算法将经典的V-SLAM 算法划分为3 个线程：Tracking、Local Mapping、Loop Closing，并通过System 总线程完成对移动机器人的定位与建图。

Tracking 线程主要包含相机定位、关键帧选取与插入、ORB 特征点提取及相机位姿估计。ORBSLAM2 算法采用了ORB 特征点：由FAST 角点与角点位置的BRIEF 描述子组成，这种特征点有着较快的计算速率及较高的精度，其采用opencv 库中的BruteForceMatcher 结合汉明距离进行匹配，采用随机采样一致性算法(RANSAC)减少误匹配对，抑制噪声，剔除外点。

同时，ORB-SLAM2 之所以能够拥有较好的实时性，是因为在跟踪线程运行的过程中，不对相机传来的所有帧进行处理，而是如图1 所示根据关键帧生成规则生成关键帧，仅对选取的关键帧进行处理，减少处理的帧数，节约运算资源，保证系统的实时性。

图1 关键帧选取示意图Fig. 1 Schematic of key frame selection

2 关键帧选择算法

2.1 常用关键帧选择算法

常用的关键帧选择算法首先要保证当前帧的内点数超过设定的最小阈值，并且重叠度不高，既保证关键帧跟踪的指令，同时又避免引入过多的信息冗余。上述条件符合时，线程开始对下述条件进行判定，当有一条判定成功时，插入关键帧。

1) 为避免通过帧数过多造成跟踪丢失，在距离上次关键帧插入过去MAX 帧(预设的最大值)时，插入关键帧。

2) 为防止图像重叠度过高，在距离上次插入关键帧至少过去MIN 帧(预设的最小值)且局部建图线程处于空闲状态时，插入关键帧。

3) 局部建图线程处理能力有限，在该线程中需要保持关键帧队列的关键帧不超过3 个。

但是由于此关键帧筛选条件只与线程中通过帧的数量有关，当机器人在转弯或者移动速度过快的情况下，就会导致关键帧信息采集不足，漏掉重要信息，导致轨迹造成较大偏差，甚至跟踪中断。针对这一问题，本文新增帧间相对运动量作为关键帧选择条件，并设计劣质关键帧剔除算法，以提高跟踪定位精度。

2.2 改进关键帧选择算法

2.2.1 关键帧的选择

为提高定位精度，本文增加帧间相对运动量作为关键帧选择条件之一。帧间相对运动量表示帧与帧之间的相对运动程度，主要由2 部分组成，分别是旋转和平移，其中将旋转作为主要变化量，平移作为次要变化量，计算方式如下：

通过视觉里程计得到相匹配的特征点，计算2 组点的质心位置p'和p，计算每个点的去质心坐标：

对式（3）进行化简，去除与R无关项，实际优化函数化简为

式中：E为奇异值组成的对角矩阵，对角线元素从大到小排列；U、V为对角矩阵。

当W为满秩时，即可根据式（6）计算得出R：

通过计算出的R计算平移向量t：

式中：R表示当前帧与最新关键帧之间的帧间旋转矩阵；t表示二者之间的平移向量。

通过R和t求出D，并在ORB-SLAM2 系统中为其设定阈值Dk：

式中： α为位移与转角的平衡权值系数，取值大小随转角增大呈指数形式增加。由于本文算法在原有以帧数为判定依据的关键帧选择算法基础上增加了转角作为主要变化量，转角所采用的平衡权值系数，在存在角度转变时远大于位置移动的平衡权值系数，采用式(9)的计算公式进行计算，设ω=min(2π-‖R‖,‖R‖)，取值范围为0°～90°（当旋转角度大于90°时，将彻底失去视角，无法特征点匹配）：

对计算所得相对运动量与其阈值进行比较：当D≥Dk时， Framekey=Framecur， FrameRef=Framecur；当D

当判定通过后，将当前帧设为关键帧，同时更新参考关键帧，再次计算当前帧与新关键帧之间的相对运动量，算法流程如图2 所示。图中：Tracking线程中存在成员变量指针mCurrentFrame，指向当前帧成员变量，其中包括图像、深度信息、相对位姿等信息。

图2 关键帧选择算法流程Fig. 2 Flowchart of key frame selection algorithm

步骤 1 判断插入关键帧的前置条件是否满足：系统不处于定位模式且不在全局闭环的条件下，同时距离上一次重定位的距离较远，内点数必须超过设定的最小阈值，保证关键帧跟踪质量。

步骤 2 在原有关键帧筛选条件的基础上，增加了帧间相对运动量作为筛选条件之一，System 主线程中将Tracking 线程中的第1 帧作为初始关键帧，根据式(8)计算出当前帧与关键帧的帧间相对运动量D。

步骤 3 对帧间相对运动量进行比较。如果当前帧的相对运动大于或等于一定的阈值，说明相机相对于之前跟踪定位的位置有了较大的相对运动，相机的视角发生了较大改变，此时不生成新的关键帧进行定位，极有可能在跟踪定位时产生误差，甚至因检测不到地图点造成跟踪中断。

因此，当相对运动较大时，添加关键帧可以有效改善这种情况。当关键帧生成后，用新产生的关键帧作为参考关键帧继续计算下一帧的相对位姿，直到整个系统循环结束。

2.2.2 劣质关键帧剔除

本文自主设计研发的移动机器人，在运行过程中与机械臂共用相机，因此地面轮胎的打滑及机器人移动过程中的震动，会导致相机与机器人本身发生相对运动从而改变相机的视角，产生误拍摄，此时所得关键帧会对跟踪定位产生误导，从而使定位精度降低，本文将其定义为劣质关键帧。针对这一问题，本文设计了劣质关键帧剔除算法，剔除由于误拍摄造成的劣质关键帧，提高跟踪定位精度。

在ORB-SLAM2 跟踪定位线程中，存在冗余关键帧剔除模块，该模块只对冗余关键帧进行剔除，并不对精度产生影响，其目的是为了节约资源，减轻工作线程负担。本文基于冗余关键帧剔除模块，设计了劣质关键帧剔除算法，剔除过程如下：

步骤 1 在关键帧生成时，临时存储彩色图像及时间戳。

步骤 2 选取相邻3 关键帧进行ORB 特征匹配，分别保存了2 次相邻关键帧特征点匹配数量NUMAdj1、NUMAdj2及一次相隔关键帧匹配数NUMSep。

步骤 3若相邻2 帧的特征点匹配数量小于相隔2 帧，说明3 帧中的中间帧大概率是由于误拍摄而产生的关键帧。

步骤 4激活关键帧剔除函数对劣质关键帧进行剔除，继续线程的运行，直到整个线程结束，释放内存。

伪代码如下：

这种方法区别于传统SLAM 算法中只对相邻2帧的关键帧进行比较，而是采用了比较相邻3 帧的方法。由于本文所设计的移动机器人平台不存在视角突变，算法运行过程中，如果第1 帧和第3 帧差异较小，而第2 帧与两者之间图像差异较大，说明第2 帧在拍摄的过程中，因震动或其他原因导致相机本身与移动机器人之间发生了相对运动，从而使视角发生了改变，产生了劣质关键帧。

3 实验

3.1 公开数据集实验

在Ubuntul 16.04 环境下进行了公开数据集实验，计算机所用的CPU 为2.0 GHz i7，运行内存为4 GB。实验采用TUM RGB-D Benchmark 数据集，选取fr1中的desk、xyz 和room 进行。依次对比了常用关键帧选择算法、改进关键帧选择算法(包含劣质关键帧)、改进关键帧选择算法(剔除劣质关键帧)的实验效果。

3.1.1 ORB-SLAM2 实时性分析

SLAM 的实时性是评价其优劣程度重要的性能指标，表1 对各个数据集平均跟踪时间进行了记录。

从表1 可知，由于对关键帧选择算法进行了改进，增加了关键帧选择的计算量，导致运行时间稍有增加，但由于相对整个系统而言计算并不复杂，增加时间极少，对系统的实时性并没有造成影响。

表1 平均跟踪时间对比结果Table 1 Comparison of mean tracking time

TUM 计算机视觉组数据集中包含利用运动视觉捕捉系统记录的摄像头在该场景下真实且连续的轨迹数据，图3(a)～图3(c)分别为3 种算法所得到的估计轨迹与真实轨迹的对比轨迹曲线。可以明显看出：①在图3(a)的轨迹交叉处有较大视角突变，原有关键帧选择算法未能检测到此信息，导致跟踪失败，采用线段直接相连的方式进行轨迹估计；而改进后的算法如图3(b)、图3(c)所示，跟踪失败并未发生，说明改进算法可以将转弯信息较好地采集，并用于关键帧选择，有效解决了跟踪失败的问题。②图3(a)中轨迹最右侧转弯后估计轨迹明显偏离真实值，是由于原有关键帧选择算法检测转弯信息出现了滞后，造成较大偏差；改进后的算法增加了相对变化量，缩减了该处的定位误差。③由于劣质关键帧仅在移动机器人实验中产生，图3(b)与图3(c)相差不大。

图3 三种关键帧选择算法运动轨迹对比Fig. 3 Comparison of motion trajectories of three key frame selection algorithms

对于定位精度，采用绝对轨迹误差（absolute trajectory error，ATE）进行比较。ATE 越小，精度越高。第i帧的ATE 定义如下：

3.1.2 ORB-SLAM2 跟踪定位精度分析

式中： Δ为真实位置与估计位置之间的误差；Fi为第i帧的ATE；Qi为真实位姿；Pi为算法估计位姿；S为从估计位姿到真实位姿的相似旋转矩阵。

将估计轨迹与真实轨迹进行对比，绘制出绝对轨迹误差曲线，如图4 所示。图4(a)为常用关键帧选择算法所得到的估计轨迹与真实曲线的误差，图4(b)为2 种改进关键帧选择算法后所得到估计轨迹与真实轨迹的误差，是否添加劣质关键帧剔除算法对误差结果并没有太大影响，说明劣质关键帧剔除算法对数据集的定位精度并没有太大影响。由图4可以看出，相较于常用关键帧选择算法，改进后的算法有明显提高。

图4 定位轨迹误差曲线Fig. 4 Curve of track positioning error

选取最大绝对轨迹误差作为评判标准，表2 为3 种算法下的最大绝对轨迹误差。

由表2 可知：①改进关键帧选择算法相比于常用关键帧选择算法，在3 个数据集中误差均明显降低，最优结果改进后的算法误差仅为改进前误差的51.9%；②剔除了劣质关键帧的改进算法与包含劣质关键帧的选择算法轨迹误差相差较小，仅存在合理波动，这是由于以数据集作为实验对象时，数据集内部并不包含劣质关键帧。

表2 最大绝对轨迹误差对比Table 2 Maximum absolute trajectory error

综上，改进后的关键帧选择算法在保证实时性的情况下，能够有效地提高算法估计轨迹精度，同时还能有效避免跟踪失败的情况发生。

3.1.3 ORB-SLAM2 关键帧数量分析

图5 比较了不同数据集下3 种关键帧选择算法计算的关键帧数量。显然，改进后的关键帧选择算法的关键帧数量明显大于改进前，说明改进后的算法选择了更多的关键帧以提升估计定位精度。

图5 数据集实验关键帧数量比较Fig. 5 Comparison of key frame numbers in dataset experiments

3.2 移动机器人实验

本文自主设计的移动机器人如图6 所示，采用大唐工控机新创云i7mini 主机作为平台的嵌入式主控机，搭配一款15.6 寸触控屏作为人机交互硬件，并选用奥比中光深度相机作为视觉传感器为机器人传送视觉信息数据。实验比较了3 种算法在实时运行过程中的定位精度。

图6 移动机器人Fig. 6 Mobile robot

实验内容主要包括机器人直线运动、转弯运动及回环运动3 种运动方式。对机器人设计搭建、相机标定等操作后，通过对3 种运动的实际轨迹和ORB-SLAM2 跟踪估计的定位轨迹进行测量，分别进行了6 次实验，并取误差平均值进行比较。图7为3 种关键帧选择算法对应的回环运动轨迹对比。由于数据为机器人实时输出，真实轨迹存在差异，并不能在同一坐标系内进行绘制。

由图7 可知：①图7(b)相比于图7(a)有一部分路程未能检出，发生了跟踪失败的情况，在转弯处，明显因检测信息滞后而导致估计转角过小增大误差；②由图7(c)、(d)轨迹曲线可知，算法未发生大规模跟踪失败，且转角处定位精度明显提高；③图7(e)、(f)为剔除了劣质关键帧的改进算法，由于剔除了劣质关键帧，图7(f)的轨迹更接近真实轨迹曲线，抖动明显减少，在一定程度上提高了定位精度。

图7 回环运动轨迹对比Fig. 7 Comparison of loop trajectories

表3 为不同关键帧选择算法所得到的轨迹与真实轨迹平均位置误差。可知：①包含劣质关键帧的改进算法相较于改进前算法，成功提高了定位精度，由于算法改进主要针对转向，直线时误差减小并不明显，但转弯与回环运动时定位精度显著提高，误差仅为原误差的58.1%；②剔除劣质关键帧的改进算法轨迹精度与未剔除相比明显提高，尤其在直行时，误差为包含劣质关键帧算法误差的82.1%。

表3 平均位置误差Table 3 Average position error

图8 比较了机器人不同运行方式下的3 种关键帧选择算法计算的关键帧数量。显然，机器人实时运行的过程中，改进后的算法除直线外同样选择更多的关键帧以保证算法精度，而直线过程未发生角度转变，因此关键帧数量在误差范围内合理波动；剔除劣质关键帧后，关键帧数量相对减少，在直线过程中最为明显，所剔除的劣质关键帧数量约达到关键帧总数的25%；回环运动的直线行驶路程为直线运动的2 倍，但多次实验所剔除的劣质关键帧数量均未达到直线运动的2 倍。造成这种现象的原因有3 点：①角度的转变会对劣质关键帧的判断造成干扰；②劣质关键帧是否生成与机器人所处环境的优劣特别是路面的平滑度有着极大的关联；③实时运行的过程不能严格保证机器人运行轨迹完全相同。

图8 移动机器人实验关键帧数量比较Fig. 8 Comparison of key frame numbers in mobile robot experiments

3.3 实验参数分析

实验过程中应对某些参数进行调整，参数调整不当会造成定位错误，系统运行资源浪费，甚至使系统崩溃。对于不同的数据集，通过计算及多次实验调整的方式确定算法所用参数。

最大帧间相对位移量Dk是通过计算并进行多次实验后，依据算法最小绝对估计误差确定。Dk越大，表明选取关键帧时通过的相对位移越大，可能导致重要关键帧被忽略而导致精度降低；Dk越小，表明选取时通过的相对位移越小，选取的关键帧就会随之增多，产生冗余，导致系统实时性降低，对后续的建图工作产生较大影响。本文中Dk取转角为45°、位移为0 时计算值，并通过实验进行微调。此转角下相邻2 关键帧在匹配到足够特征点进行计算的同时，有效防止过多关键帧的产生浪费系统内存。在不同的实验条件下，相机的采样频率、机器人的运行速度及环境的规模大小均对Dk的最佳值产生影响。一般情况，Dk取30～50 为最佳。