基于特征点法的自适应SLAM改进方法

陈继清, 桂海宁, 王志奎, 龙腾, 黄春林

( 1.广西大学 机械工程学院, 广西 南宁 530004；2.南宁正腾农机有限公司, 广西 南宁 530007)

0 引言

近些年，即时定位与地图构建(simultaneous localization and mapping，SLAM)作为自动驾驶领域的关键技术[1-5]，已经开始广泛的应用于航空航天、无人化驾驶和扫地机器人等领域。

对于SLAM[6]系统而言，高精度和低计算成本是视觉SLAM的核心的要求。目前主流的方法有基于光度不变性的直接方法DSO[7]和LSD(large scale direct monocular， SLAM )[8]，以及基于特征的间接方法。直接方法通过最小化光度误差[7-8]来恢复相机姿态位置，而间接方法则利用最小化重投影误差[9]来判别相机姿态位置，并使用回环检测实现重定位来消除全局基于特征点的漂移。这种连续跟踪相邻帧的关键点来恢复相机运动轨迹的方法具有鲁棒性，因为这种辨别出的关键点相对而言是不受视点和光照变化的影响的。最近，很多学者提出了许多用于实时的间接SLAM方法的应用。其中，ORB-SLAM2[10]被认为是一种先进的SLAM方法[11-14]，是在总结前人的基础上进一步开发的SLAM系统，是一种结合了实时视觉SLAM系统(PTAM[15])与一种快速位置的识别方法(Bow2[16])，以及一种高效的基于图像处理的光束法平算法的SLAM系统，因此，与现有的其他解决方案相比ORB-SLAM2有更好的精度和稳定性。在实验研究中发现，在一些低纹理场合中，ORB-SLAM2无法得到足够的描述子数量，使得ORB-SLAM2的跟踪失败，致使ORB-SLAM2无法正常运作。而LSD-SLAM因为其直接法使用的是像素梯度而不必是角点，所以可以在特征缺失的环境下运行得到了定位信息。

为了应对在低纹理环境下ORB-SLAM2失效的情况，本文中提出了一种对于环境进行预处理的检测方法，使得系统可以自主分辨出环境是否为低纹理环境，对于分辨出的低纹理环境由LSD-SLAM进行处理，而对于非低纹理环境的图片由ORB-SLAM2进行处理。最后把从LSD-SLAM和ORB-SLAM2中得到的位姿信息与惯性测量单元(inertial measurement unit，IMU)[17]相结合，使得整个算法得到更为准确的尺度信息，从而提高整个SLAM算法[18-20]的精度。该算法首先基于传统的ORB-SLAM2和LSD-SLAM算法，不仅使得跟踪可以在低纹理环境下运行，且保持着更快的速度比之于传统的SLAM算法。其次，对于针对单目相机对于尺度判断的不确定性，通过IMU数据[21]增强系统的鲁棒性，增加系统的精度。

1 图像预处理设计

对于环境的分析较为常见的是特征点的直接统计，但是在实验的过程中发现直接统计特征点的方法不能非常有效的区分低纹理环境。虽然有些环境下的图片的特征点分布较为集中，导致其特征点数量达到了要求[22]，但是分布的问题导致其并不能满足特征点法的需求。本文提出一种图像预处理方法来帮助SLAM系统可以识别当前环境。图像预处理流程图如图1所示。

首先，图1中的图像处理器使用Fast特征点[5]提取的方法对于图片的特征进行处理。而传统的对于特征点的判断是基于特征点的数量，图片中的特征点非常集中且为远点不利于特征点法的定位，而相对于特征点法，LSD-SLAM则体现出了相当强的捕捉能力。

图1 图像预处理流程图

系统首先用Bresenham四分法[23]对于整个图像进行处理。去除其不含特征点的区域，以此方法对此迭代5次，得到256个分割后的图像，对于保有特征点的图像进行保留并记录，并记录保留图像总数记为Num，K为计算所得当前图像的评价系数，n为图像的评价系数。

K=Num/256。

经过多次实验验证n取0.23时系统表现最好。具体流程如图2所示。

图2 图像处理器进程图

初始化处理20张图像后，通过记录K值的大小，分辨出图片是否为低纹理图片[24]，再流向不同的SLAM系统对于图像信息进行进一步的处理。与此同时，FAST提取特征点ORB-SLAM2的Tracking的一个进程，所以对于处理后的结果无需重复进行，直接将图导入ORB-SLAM2中，由于没有进行匹配，因此运行时间仅为20 ms。特征点处理时间为150 ms。当图片导入到LSD-SLAM算法中，由于直接法使得算法的运行速度大大快于原有的特征点法，因此整个运行速度提升。同时，低纹理状态下的图片导入到了LSD-SLAM中，使得整个系统得以对低纹理环境继续进行追踪，弥补了ORB-SLAM的失效问题。

自适应改进SLAM整体框架如图3所示，图3中的系统A与系统B分别为ORB-SLAM2与LSD-SLAM系统。深色的方框是自适应改进SLAM的贡献。

为了实现运动的连贯性，采用了速度滑冰接力赛的方法：预留一个缓冲区，使得系统的切换变得更加紧凑，由于ORB-SLAM2的处理速度远远慢于LSD-SLAM的处理速度，因此当判定符号发出以后，LSD-SLAM的线程继续运行30张图片后处理公布出姿态位置信息，ORB-SLAM2同时也对这30张图进行处理，并公布姿态位置信息，2个姿态位置信息融合得到新的姿态位置信息，随后LSD-SLAM停止运作，在新的姿态信息的基础上ORB-SLAM2对于后面的信息进行运作。为了保持系统的稳定性，对于LSD-SLAM线程切换到ORB-SLAM2线程也是运用这种方式。

图3 改进的SLAM的框架

2 IMU融合设计

首先在通常情况下IMU[25]的更新频率远远快于系统输出的位姿估计，使得对于IMU融合的时候要同步二者的更新频率，保证二者在时域上进行统一，以惯性数据为核心, 视觉测量数据修正惯性测量数据的累积误差，从而得到更准确的位姿信息。

系统采用的是多传感器融合(multi-sensor fusion，MSF)[26]的方法，方法的进程图如图4所示。

图4 IMU融合进程图

图4中位姿预测信息来源于之前系统中生成的坐标信息，IMU信息[27]来自于传感器和数据集中提供的IMU信息。IMU主要是加速度信息和速度信息，位姿信息为系统生成的坐标信息，由于单目相机中生成的图片信息很容易产生尺度的突变。为了应对这种现象同时计算双方的相对位置[28]，这样尺度就可以过滤的比较准确。然后，再把优化后的值加入到原来的状态中，整合出新的位姿[29]的均值和扰动，进而优化了整个IMU系统。具体关于IMU与姿态的整合公式说明如下：

IMU初始状态表示：

(1)

理想运动模型：

(2)

(3)

(4)

(5)

(6)

实际运动模型：

(7)

(8)

(9)

(10)

(11)

误差状态表示：

(12)

误差运动状态模型：

(13)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

误差状态导数的转移矩阵：

(21)

二阶泰勒展开(忽略二阶以上微小量)：

(22)

误差状态的转移矩阵：

(23)

预测误差状态和预测误差协方差矩阵：

(24)

因为系统与IMU的融合是建立在位姿估算已经完成的情况下，所以在这里系统已经完成了回环检测以及建图的工作，因此系统无需与常规的IMU融合一样进行全局BA。系统以标定的位姿估计为起点，把视觉的相对测量值与IMU的相对测量值进行融合，随后融合得到相对的测量状态，再把相对状态的均值和扰动合并进主状态以及主状态扰动，最后主状态再与位姿估计融合出新的主状态。

对于p的位置进行测算：

(25)

(26)

得到误差测量参数矩阵Hp：

(27)

再对于q位姿进行测算：

(28)

(29)

得到误差测量参数矩阵Hq:

(30)

最后得到位置纠正矩阵H:

(31)

最后更新位置信息：

(32)

完成IMU融合，同时更新地图信息，输出跟踪轨迹信息。

3 实验

3.1 实验设计

为了测试自适应改进SLAM的性能，系统将在Ubuntu18.04和ROS Melodic中进行运行通用数据集EuRoC，实验所用的电脑配置为低功耗的Intel Core i7-10750H CPU@2.60GHz。其中n值取0.23，为提高ORB-SLAM2运行速度及提高描述子采集频率描述子关联数量取5。

采用的数据集为EuRoC，其中数据集名称中的MH代表工厂(多纹理环境)，V代表室内环境(多纹理和低纹理共存环境)，easy代表相机的移动速度为慢，med代表相机的移动速度为中等，dif代表相机的移动速度为快。

为了展示实验的效果图片，实验收集了改进的SLAM的位姿信息和ORB-SLAM2位姿信息，并用Evo做出其与真实轨迹的对比图片。

为了评估系统与其他系统之间的差异性，实验使用绝对轨迹误差(APE)和相对位姿误差(RPE)的均方根误差(RMSE)进行评估。

最后为了评估系统与其他系统运行时间方面的差异性，实验统计了平均执行时间，以此对于系统的运行速度进行评估。

3.2 实验结果分析

首先分别使用ORB-SLAM2和改进的SLAM运行数据集EuRoC 中的数据包，使得生成位姿信息，并分别使用Evo把位姿信息与真实轨迹进行对比，结果如图5-16所示。

(a) ORB-SLAM2相对位置轨迹跟踪图

(b) 改进的SLAM系统相对位置轨迹跟踪图

(c) ORB-SLAM2相对位置误差分析表

(d) 改进的SLAM系统相对位置误差分析表

(a) ORB-SLAM2绝对位置轨迹跟踪图

(b) 改进的SLAM系统绝对位置轨迹跟踪图

(c) ORB-SLAM2绝对位置误差分析表

(d) 改进的SLAM系统绝对位置误差分析表

(a) ORB-SLAM2相对位置轨迹跟踪图

(b) 改进的SLAM系统相对位置轨迹跟踪图

(c) ORB-SLAM2相对位置误差分析表

(d) 改进的SLAM系统相对位置误差分析表

(a) ORB-SLAM2绝对位置轨迹跟踪图

(b) 改进的SLAM系统绝对位置轨迹跟踪图

(c) ORB-SLAM2绝对位置误差分析表

(d)改进的SLAM系统绝对位置误差分析表

(a) ORB-SLAM2相对位置轨迹跟踪图

(b) 改进的SLAM系统相对位置轨迹跟踪图

(c) ORB-SLAM2相对位置误差分析表

(d) 改进的SLAM系统相对位置误差分析表

(a) ORB-SLAM2绝对位置轨迹跟踪图

(b) 改进的SLAM系统绝对位置轨迹跟踪图

(c) ORB-SLAM2绝对位置误差分析表

(d) 改进的SLAM系统绝对位置误差分析表

(a) ORB-SLAM2相对位置轨迹跟踪图

(b) 改进的SLAM系统相对位置轨迹跟踪图

(c) ORB-SLAM2相对位置误差分析表

(d) 改进的SLAM系统相对位置误差分析表

(a) ORB-SLAM2绝对位置轨迹跟踪图

(b) 改进的SLAM系统绝对位置轨迹跟踪图

(c) ORB-SLAM2绝对位置误差分析表

(d) 改进的SLAM系统绝对位置误差分析表

(a) ORB-SLAM2相对位置轨迹跟踪图

(b) 改进的SLAM系统相对位置轨迹跟踪图

(c) ORB-SLAM2相对位置误差分析表

(d) 改进的SLAM系统相对位置误差分析表

(a) ORB-SLAM2绝对位置轨迹跟踪图

(b) 改进的SLAM系统绝对位置轨迹跟踪图

(c) ORB-SLAM2绝对位置误差分析表

(d) 改进的SLAM系统绝对位置误差分析表

(a) ORB-SLAM2相对位置轨迹跟踪图

(b) 改进的SLAM系统相对位置轨迹跟踪图

(c) ORB-SLAM2相对位置误差分析表

(d) 改进的SLAM系统相对位置误差分析表

(a) ORB-SLAM2绝对位置轨迹跟踪图

(b) 改进的SLAM系统绝对位置轨迹跟踪图

(c) ORB-SLAM2绝对位置误差分析表

(d) 改进的SLAM系统绝对位置误差分析表

其中图5-16中的图(a)与图(c)是原有的ORB-SLAM2系统运行结果，图(b)与图(d)是自适应改进SLAM的运行结果。

由于与IMU的融合，使得整个SLAM有了尺度信息，因此自适应改进SLAM系统的RPE要远远优于ORB-SLAM2系统的RPE。在实验数据中可以发现MH_01_easy、MH_02_easy、V1_01_easy和V2_01_easy数据集中改进的SLAM的RPE要比ORB-SLAM2 的RPE小10倍，但是在MH_05_difficult和V2_03_difficult中发现，改进的SLAM的APE反倒不如ORB-SLAM2，经研究发现是因为这2个数据集是在较快运动速度的情况下运行，由于改进的SLAM系统的图片分选，系统的反应较慢，因此在这2个数据集中的APE表现不理想，但是数据上改进的SLAM系统在低速的环境下运行的表现要比现有的ORB-SLAM2系统误差减少1/2。

而后在搭建的环境中，分别运行ORB-SLAM2、LSD-SLAM、自适应改进SLAM，得到各自的实际轨迹数据。对于实际轨迹数据与真实轨迹数据进行RMSE运算得到的结果见表1。

表1 RMSE 误差Tab.1 RMSE error

由表1可知，仅仅使用特征点法或者仅仅使用直接法的结果都不如改进的SLAM算法，且误差只有LSD的1/2,精度方面提高了2倍，而ORB-SLAM2因为关键点跟踪容易丢失在某些序列中失败。相反，自适应改进SLAM成功地估计了所有序列中的摄像机轨迹。

由于实时性为SLAM的重要性能指标，因此各系统在不同模块的执行时间进行统计汇总，得到结果见表2。

表 2 平均执行时间对比 ORB-SLAM2,LSD-SLAM 和 自适应改进SLAM Tab 2 Average execution time comparison of ORB-SLAM2, LSD-SLAM and adaptive improved SLAM

由表2可得ORB-SLAM2、LSD-SLAM和自适应改进SLAM的比较，其中最佳值为粗体。时间方面因为是融合系统，所以速度与ORB-SLAM2运行速度相似。由于系统融合了IMU使得自适应改进SLAM的刷新频率是远远高于LSD-SLAM和ORB-SLAM2的，因此得到的位姿信息更为丰富准确。

在低纹理环境中，仅仅使用特征点法或者仅仅使用直接法的方法产生的结果都比自适应改进SLAM差，其中ORB-SLAM因为关键点跟踪容易丢失，所以容易在某些序列中失败。相反，自适应改进SLAM成功地估计了所有序列中的摄像机轨迹。由于使用IMU融合，因此低纹理环境下的自适应改进SLAM比LSD-SLAM更精确。

综上所述，自适应改进SLAM的RPE只有ORB-SLAM2的1/10，且在大部分情况下自适应改进SLAM的RPE都比ORB-SLAM2的APE要小16%～20%，表明适应改进SLAM方法是一种更好的定位系统。

4 结论

本文介绍了自适应改进SLAM方法，这是第一种基于实时优化的自适应单目VINS方法，它选择了用ORB算法或LSD算法处理图像的方法。通过实验，证明了LSD算法可以用于任何其他与直线对应的姿态估计相关的工作。此外，在系统中有效地利用了位姿信息与IMU信息融合的多传感器融合(multi-sensor fusion，MSF)方法，因此，在相同的姿态更新率下，融合自适应改进SLAM可以比ORB-SLAM2和LSD-SLAM产生更精确的定位。

通过实验证实了改进的SLAM系统中的分选机构可以成功地分选出低纹理环境，使得改进的SLAM系统可以在ORB-SLAM2失效的环境下正常运作，同时，对比于LSD-SLAM在纹理环境清晰的条件下有更好的精度。最后使用位姿信息与IMU的融合可以提高系统的刷新频率，并且增强了系统的鲁棒性。