基于改进ORB-SLAM2的机器人同步定位与建图技术研究

阳霜，罗 堪，韦晓慧，周志伟，彭国超

(1.湖南机电职业技术学院，湖南长沙 410151；2.长沙师范学院，湖南长沙 410100)

0 引言

随着机器人技术的飞速发展，如何实现机器人在未知环境下的定位与导航，并自主地完成特定任务，是一个火热的研究方向。在深海、隧道、室内等应用场景，GPS等全球卫星定位系统的信号存在高衰减现象，机器人面临“在哪里”“去哪里”“怎么去”等问题。为了实现这些功能，让移动机器人能够同步定位与建图(SLAM)，需要让其能够明确自身的位置和位姿，感知周围环境的信息并创建地图。这一技术的发展，对于实现机器人的智能定位与导航具有非常重要的意义。

目前，SLAM技术主要分为两类：基于激光雷达的SLAM[1]和基于视觉的SLAM[2]，这两种技术在机器人定位和地图构建方面都有广泛的应用。其中，激光SLAM 是一种较为成熟的方法，已成功应用于多种商业产品。然而，激光SLAM 构建的地图信息过于简单，无法让机器人获取更丰富的环境信息[3]。视觉SLAM 是近十年来发展迅速的SLAM 解决方案，可以使用摄像头重建3D 地图。此外，图像中包含丰富的物体信息，可以帮助机器人完成各种基于视觉的智能任务[4]。

Cesar Cadena 等人将视觉SLAM 的发展，划分为三个代表性的时代[5]，第一个时代“经典时代”，主要解决SLAM 问题，凭借提出的数学公式，SLAM问题首次得到了有效应用。第二个时代“算法改进时代”，引入GPU、RGB-D、立体相机等新硬件，用于改进视觉SLAM 算法。第三个时代“提高鲁棒性时代”，主要是针对特定场景，提高系统鲁棒性。比较有代表的是基于稀疏直接法的视觉里程计(DSO, Direct Sparse Odometry)[5]，ORB-SLAM[7]等算法，这一时期SLAM 问题最大的挑战，是在控制计算量大小的情况下，构建一致收敛的轨迹和地图，保证移动机器人定位精度的同时，提高算法的运行效率。2017年Mur-Artal R等，在ORB-SLAM 算法基础上，开源了ORB-SLAM2[8]方案，新增了对双目相机、RGB-D 相机的源生支持，是目前市场上主流的具有实时性好、地图重用和动态管理关键帧等优点的完整视觉SLAM 系统。D3VO[9]是一种基于深度学习的视觉里程计框架，由Stumberg等人在2020年提出。该框架将深度学习的深度估计、位姿估计、不确定度估计应用于视觉里程计方法中，以优化前端追踪效果，并提高后端非线性优化的性能[10]，但目前的深度学习解决方案仍处于探索阶段，对数据和计算机设备要求较高，且现有的模型还不够成熟，运行效果有待提高。

综上所述，ORB-SLAM2 是目前视觉SLAM 中，最前沿的同步定位与建图算法之一，本文选择ORBSLAM2 算法，对机器人进行同步定位与建图，提出自适应降低阈值的方法提取特征点，当通过高响应阈值搜索，未搜索到特征点时，自适应逐级降低阈值搜索特征点，提高所提取特征点的响应值，从而提高移动机器人在未知环境下，进行同步定位与建图的精度。

1 ORB-SLAM2简介

ORB-SLAM2由跟踪、局部建图、闭环优化检测三大线程组成，如图1所示。

图1 ORB-SLAM2框架图

在跟踪线程中，首先从图像中提取ORB 特征点，并运用四叉树法对这些特征点进行筛选。然后，根据上一帧的位姿信息，进行位姿估计或进行重定位以初始化位姿。接下来，跟踪线程会跟踪重建局部地图，并进一步优化位姿以确定新的关键帧。

在局部建图线程中，机器人会将跟踪线程生成的新的关键帧插入，并验证最新获取的地图点，进而构建新的地图点。这些地图点通过局部捆集调整(Local BA)进一步优化，最后机器人会对插入的关键帧进行筛选，删除冗余关键帧，实现局部地图构建。

为了减小之前步骤所积累产生的误差，闭环优化线程的主要任务是通过词袋库判别闭环帧。然后，利用Sim3算法计算相似变换，对地图进行矫正及优化，从而减小之前步骤中，所积累产生的误差。

系统的地图由4个部分组成，即地图点、关键帧、一致图和生成树。其中，一致图是由节点和边构成的数据结构，节点指的是关键帧，边是关键帧之间的联系。生成树则是按照地图点和关键帧之间的关系，以树状结构进行存储，用于查找相邻的关键帧。

位置识别模块包括视觉词袋和识别数据库两部分。视觉词袋利用ORB特征信息对特征点进行分类，实现位置的快速匹配。识别数据库是利用关键帧所匹配的特征点，与视觉词袋进行关联，将相似度得分最高的，形成关键帧数据库，运用到重定位及回环检测。

2 ORB特征点提取优化

原算法在跟踪线程中提取ORB特征点时，争取特征点在图像的所有部分分布均匀，将图像分为30*30大小的CELL 区域，然后对每个CELL 搜索特征点，优先使用高响应阈值搜索特征点，若使用高响应阈值没找到特征点，则将搜索阈值调整为低响应阈值，重新对该CELL 进行搜索，直至将所有CELL 搜索完毕后，特征点提取工作结束。

由于原算法，在ORB 特征点提取环节，若通过高响应阈值没有搜索到特征点，直接采用低响应阈值进行搜索，由于高响应阈值与低响应阈值之间的跨度较大，因此在搜索特征点时，提取了较多低响应值特征点，将增大后续特征点筛选及匹配误差，从而影响机器人在进行同步定位与建图的精度。

本文对提取ORB特征点算法进行改进，若通过高响应阈值搜索，未搜索到特征点，则将搜索响应值在原搜索响应值的基础上，降低t来搜索特征点。如果此时搜索到特征点，则记录特征点后，来到下一CELL进行特征点搜索；若未搜索到特征点，且此时搜索响应值大于低响应阈值，则在此搜索响应值的基础上，再次降低t来搜索特征点。由此，通过将响应值逐次降低t来搜索特征点，直至遍历完所有的CELL，如图2所示。通过对响应值进行自适应调整，在特征点提取时，将有效减少低响应值的特征点，在降低后续对特征点进行筛选与匹配的误差的同时，还可以减少特征点匹配的工作量，提高系统运行效率。

图2 特征点提取优化流程图

3 实验与分析

3.1 数据准备

EuRoC MAV 数据集是微型飞行器(MAV)上收集的视觉惯性数据集。数据集包含立体图像、同步IMU测量以及准确的运动和结构地面实况。本文采用Eu-RoC MAV 数据集中的01、03、05 号场景，各数据集特点如表1所示。通过对三个数据集进行比较，得知厂房01的数据拍摄条件最好，飞行器运行速度低，拍摄场景纹理及光照均较好，厂房03 的数据较厂房01 而言，运行速度有所加快，而厂房05的数据最难，不仅运行速度快，其场景的纹理及光照条件均较差。因此，笔者利用这三个数据集，来运行原始ORB-SLAM2 算法及改进后的ORB-SLAM2 算法，有助于分析算法对高速运行、弱纹理、光照较差的不良条件的鲁棒性。

表1 数据集介绍

3.2 评价标准

针对改进算法和原始算法，分别计算相对定位误差(RPE，Relative Positioning Error) 和绝对定位误差(APE，Absolute Positioning Error) 的 均 方 根 误 差(RMSE)、标准差(σ)。

APE是估计位置(2D 或3D)与真实位置之间的欧几里得距离，用于检验算法的全局准确性和稳定性。其RMSE计算如式(1)，RMSE越小，估计轨迹越接近真实轨迹，标准差计算如式(2):

式中，n表示关键帧数目，i表示第i个关键帧，xobs.i表示第i个关键帧姿态真实值，xmodel.i为第i个关键帧姿态的估计值。

式中，n表示关键帧数目，i表示第i个关键帧，xmodel.i表示第i个关键帧姿态真实值，μ为xmodel.i为的平均值。

RPE是连续位置估计(2D 或3D)之间的欧几里得距离，用于计算漂移，即每米的平移或旋转误差。第i帧的RPE定义如下：

式中，Pi表示第i帧的估计位姿，Qi表示第i帧的真实位姿，Δt表示间隔时间。

式中，n表示观测次数，Δt表示时间间隔t，m=n- Δt×RPE，Ei表 示i时 刻 相 对 位 姿 估 计 误 差，trans(Ei)代表相对位姿误差中的平移部分。

相对定位误差、绝对定位误差的具体计算，可运用开源的EVO Python计算包进行计算。

最后运用式(5)计算估计轨迹与真实轨迹长度的相对误差，用于评价算法对轨迹尺度估计的能力。

3.3 误差分析

运用原算法及改进算法，对厂房01、03、05号数据集进行同步定位与建图仿真，其绝对定位误差及相对定位误差比较，分别如表2、表3所示。

表2 不同数据集下APE对比表

表3 不同数据集下RPE对比表

从表2、表3 可知，改进算法的相对定位误差的RMSE与σ值，略小于原算法，而改进算法的绝对定位误差的RMSE与σ值，远优于原算法，三个场景中，其绝对定位误差的RMSE的平均值下降了15.87%。由此可知，改进算法在进行机器人同步定位与建图时，其漂移误差略微减小，而其全局准确性和稳定性得到大幅提升。

对改进算法在厂房01、03、05，三个不同场景下的APE、RPE进行比较，其中厂房01 和厂房03 的APE、RPE相差较小，而厂房05 的APE、RPE较另两种相差略大，说明改进算法对高速运动的鲁棒性较好，而对于弱纹理的鲁棒性有待提高。

运用ORB-SLAM2改进算法，对厂房01、03、05数据集进行同步定位与建图仿真，其轨迹仿真如图3所示，图中X、Y、Z 为三维空间坐标轴，图例data 为真实轨迹，图例KeyFrameTrajectory 为估计轨迹。通过对图3进行观察，可分析估计轨迹与真实轨迹的拟合程度，其中厂房01与03的轨迹仿真图中，估计轨迹与真实轨迹无较大轨迹错位，几乎重合，而厂房05的估计轨迹与真实轨迹在某一区域内，存在一定偏差。再次验证了，改进算法对高速运动的鲁棒性较好，而对于弱纹理的鲁棒性有待提高。

图3 轨迹仿真图

ORB-SLAM2 改进算法对厂房01、03、05 数据集仿真，其真实轨迹与估计轨迹长度，及轨迹长度误差计算，如表4所示。从表中数据分析可知，估计轨迹长度均与真实轨迹长度相近，且误差较小。结合轨迹仿真图及轨迹长度比较表，总体来说，对ORB-SLAM2算法进行ORB特征点提取优化后，能够较好地实现机器人同步定位与建图。

表4 轨迹长度比较表

4 结论与展望

通过上述实验分析，在提取ORB 特征点时，通过对响应值进行自适应调整，对ORB-SLAM2 算法进行改进，能够较好地应用于未知环境下，机器人同步定位与建图。通过实验数据对比分析可知，改进算法的相对定位误差的RMSE与σ值，略小于原算法，而改进算法的绝对定位误差的RMSE与σ值，远优于原算法，三个场景中，其APE的RMSE的平均值下降了15.87%。由此可知，通过对ORB-SLAM2算法进行改进，在进行同步定位与建图时，每米的平移或旋转误差略微有所下降，准确性及稳定性得到较大提高，估计轨迹更接近于真实轨迹。因此，运用改进算法使机器人在未知环境下，进行同步定位与建图的精度得到提高。

通过对厂房01 和厂房03 的绝对定位误差、相对定位误差、轨迹仿真图进行对比，得知改进算法，对于高速运动的鲁棒性较好，但对厂房05与厂房01、03场景的绝对定位误差、相对定位误差、轨迹仿真图进行对比，得知后续对于环境光照变化、低纹理环境等问题，如何提升系统鲁棒性，提高智能移动机器人定位与导航精度，仍有待加强研究。

另外，视觉SLAM 主要用于机器人或无人机等嵌入式平台的场景识别、深度学习、3D重建等应用。如何在提高机器人或无人机进行实时定位与导航精度的同时，加快运行速度，还有待进一步探索。