搜救机器人视觉SLAM 回环检测方法研究

2020-12-24谢鹏发苏卫华李世国张世月

医疗卫生装备 2020年12期

谢鹏发，苏卫华，*，李世国，张世月

（1.军事科学院国防科技创新研究院，北京100071；2.天津（滨海）人工智能创新中心，天津300457）

0 引言

在视觉同时定位与地图构建（simultaneous localization and mapping，SLAM）中，机器人位姿估计是一个递推的过程，由于算法本身的局限，对一帧位姿估计产生的误差将在后一帧位姿估计中延续放大，因而随着建图时间的增加，机器人位姿估计的累计误差会越来越大，对机器人定位精度与地图构建精度将产生严重的影响。回环检测是指机器人在进行SLAM 的过程中判断自己是否到达之前已经访问过的某个场景[1]，从而根据前面误差较小的位姿修正后面的位姿，消除累计误差，提高机器人定位与建图精度。对于搜救机器人而言，在复杂的灾后环境中执行搜救任务时[2]需要有更加精准的定位与环境地图，因此研究搜救机器人视觉SLAM 回环检测方法对于提升搜救任务的完成效率具有重大的意义。

现有的视觉SLAM 系统中，回环检测方法主要有基于词袋模型（bag of words，BoW）的回环检测、基于随机蕨法的回环检测和基于深度学习的回环检测等。基于深度学习的回环检测方法目前是SLAM 研究的热门方向之一，PoseNet[3]为该种方法的代表算法，Gao 等[4-5]、何元烈等[6]、Lategahn 等[7]对深度学习用于SLAM 回环检测方向进行了大量研究。该方法提高了SLAM 算法的运行效率以及定位与建图的精度，但是对个人计算机的性能要求较高，需要图形处理器（graphics processing unit，GPU）加速方能实现。对于搜救平台而言，因工作环境复杂，要自主完成搜救任务需要占用大量计算资源，若SLAM 算法本身需要大量的计算资源则无法保证搜救机器人的实时工作性能。目前SLAM 的另一个发展方向是轻量化，力求在较少的计算资源下实现SLAM 算法的实时运行。相较于基于随机蕨法和基于深度学习的回环检测方法，基于BoW 的回环检测方法通过词袋对算法的关键帧进行相似度计算，不需要GPU 加速，在CPU 上即可实现SLAM 算法的实时运行，因此成为搜救机器人SLAM 回环检测算法的主流方法。

ORB-SLAM2[8]是运用基于BoW 的回环检测方法的经典算法之一，该算法能够有效消除累计误差，且在相机位姿跟踪丢失后还能迅速找回，相较于其他SLAM 算法具有很大的优势。但是该算法的回环检测模块没有考虑到相似场景对回环准确率的影响，在相似场景中容易产生错误回环，严重影响算法的定位与建图精度。因此，本文在ORB-SLAM2 算法的基础上，对其回环检测模块进行改进，对相似场景的错误回环进行剔除，以提高回环准确率和算法的定位与建图精度，进而提高搜救机器人在复杂搜救环境中执行任务的效率。

1 相似场景回环帧剔除算法设计

1.1 回环检测算法整体框架

基于BoW 的方法是预先加载一个词袋字典树，通过这个预加载的字典树将图像中的每一局部特征点的描述子转换为一个单词，字典里包含着所有的单词，将整幅图像的单词统计为一个词袋向量，词袋向量间的距离代表了2 幅图像之间的差异[9]。ORBSLAM2 算法的回环检测模块使用BoW 对当前帧与历史关键帧进行单词聚类，通过计算2 帧的相似性得到相似性得分较高的关键帧，对该关键帧进行连续性检测后得到回环候选帧，之后通过计算当前帧与回环候选帧的sim3 变换以及重投影匹配等步骤得到最终的回环帧。这种方法主要利用了图像的色彩（RGB）信息，而图像的深度（Depth）信息并没有得到充分利用，因此搜救机器人在建图过程中遇到相似场景时容易产生错误回环，对定位与建图结果产生严重影响。本文在ORB-SLAM2 算法得到回环候选帧之后，添加相似场景回环候选帧剔除模块，通过索引当前帧与回环候选帧的深度信息对其进行比较，当结果达到设定阈值后将其剔除。改进的回环检测算法流程如图1 所示。

图1 改进后的回环检测算法流程图

ORB-SLAM2 算法在回环检测模块前端得到与当前帧相似性得分较高的历史关键帧后，将其共视帧中相连的帧分为一组，计算组内最高得分的同时得到每组中得分最高的关键帧，通过比较组最高得分得到回环候选帧所在组，组中得分最高的关键帧记为回环候选帧。由此计算了回环检测帧的连续性，避免了将单幅图像相似但场景并不相同的关键帧作为回环候选帧，但是在相似场景中，其相似的图像帧具有连续性，此处并不能将其剔除。因此，在得到回环候选帧之后添加相似场景剔除模块，对相似场景产生的错误候选回环进行剔除。

1.2 相似场景回环剔除

由于ORB-SLAM2 算法自带的字典是大量的室内外场景训练所得，字典内容包含了常见的各种物体信息，所以本文延用ORB-SLAM2 算法的字典，对属于同一节点（特定层）的特征点通过计算描述子距离进行匹配。采用这种BoW 加速匹配的方法分别对当前帧与每一个候选回环帧进行单词查找匹配得到对应的地图点。

本文提出的相似场景回环剔除算法防伪代码如下：

算法1：相似场景回环剔除算法输入：当前帧与回环候选帧输出：是否剔除回环候选帧

For i=1 到n

vpMapPointMatches = SearchByBoW（Current

KF，Loop Candidates）

vpKeyFrameMP = mpCurrentKF->GetMapPoint－

Matches()

Fora=1：m

If(vpMapPointMatches(a))

pMP1=vpMapPointMatche(a)

pMP2=vpKeyFrameMP(a)

pMP1->GetWorldPos()

pMP2->GetWorldPos()

If(z1/z2>1.18||z1/z2<0.85)

相似点数量num 加1

Endif

If(num>20)

剔除当前候选回环帧

Endif

Endfor

算法1 中，n 为候选回环帧的数量，m 为当前帧与候选回环帧地图匹配点的数量。对于经过相似度筛选后的回环候选帧的每一帧，通过BoW 加速匹配得到对应地图点（vpMapPointMatches，vpKeyFrameMP）后，对于2 帧对应匹配的地图点索引各自在世界坐标系中的z 值（z1，z2），将计算得到的比值与设定的阈值进行比较。考虑到相似场景深度值不同，但是不排除场景距离较近、深度差值小的情况，因此本文以15%的比值差值为判定条件，将阈值边界设定为0.85 和1.18，如果比值小于0.85 或者大于1.18，则将此地图点标记为相似点，若相似点累计超过20 个，则证明当前的回环候选帧为相似场景关键帧，将其剔除。

本文的改进算法是利用深度信息对相似场景的回环关键帧进行剔除，阈值设定的前提是ORBSLAM2 算法具有较好的定位精度与鲁棒性。相较于ElasticFusion[10]和RGBD SLAM[11]等SLAM 领域知名的开源算法，ORB-SLAM2 算法具有更好的定位精度与鲁棒性[8]，其在相机位姿跟踪丢失后也能迅速找回，为本文改进算法提供了理论保障。

2 数据集制作

2.1 相机标定

由于每个相机的几何模型参数不同，为得到精确的相机几何模型参数，保证后续实时建图的算法精度，本文使用张正友标定法[12]，在Ubuntu 16.04 系统中，对Kinect1.0 相机进行标定，求取相机参数。该方法克服了传统标定法需要的高精度标定物的缺点，仅需要一个打印出来的棋盘格就可完成标定，因而又称棋盘法。本文标定相机使用的棋盘格如图2 所示。同时相对于自标定而言，棋盘法提高了精度，便于操作，因此被广泛应用。

2.2 相似场景数据集制作

为了测试改进的回环检测算法在相似场景中的工作性能，本文采用Kinect1.0 相机采集相似场景数据制作相似场景数据集。通过场景布置，本研究采集了场景单一且相似的会议室（如图3 所示），通过交叉行走增加回环数量，为之后的回环性能测试提供基础。以10 Hz 的帧率采集场景的色彩图和深度图，并将每一幅图像的时间戳和图像名对应保存，以便之后对深度图像和色彩图像进行配准建图。最终生成相似场景数据集similar_boardroom_loop。

3 实验与分析

3.1 准确率与召回率曲线对比

准确率和召回率是回环检测的典型性能指标。准确率是指算法提取的所有回环中确实是真实回环的概率，召回率指的是在所有真实回环中被正确检测出来的概率。回环检测的结果分类见表1。

准确率与召回率的计算公式如下：

图2 相机标定使用的棋盘格（7×5）

图3 会议室场景

表1 回环检测的结果分类

为了评价算法的有效性，本研究通过设置阈值r0（0

图4 准确率与召回率对比曲线图

图4中本文改进算法剔除了ORB-SLAM2 算法在相似场景中产生的错误回环，得到的准确率与召回率曲线相较于ORB-SLAM2 算法整体偏右上方，在100%准确率下仍有50%的召回率，而ORB-SLAM2算法在100%准确率下召回率最高为30%，表明本文改进算法在准确率较高的同时还具有很好的召回率，验证了其有效性。图5 为2 种算法对相似场景数据集的建图结果，可以看出，ORB-SLAM2 算法在相似场景中产生错误回环导致场景重复出现或者产生重影，而本文改进算法在剔除错误回环后消除了这些建图误差，建图精确度更高。

3.2 定位精度

图5 相似场景建图结果对比

本文采用德国慕尼黑工业大学公布的TUM RGBD[13]标准数据集中的fr1-room 序列对ORBSLAM2 算法和本文改进算法进行性能测试，通过evo 评价工具对2 种算法的定位精度进行比较。

图6 为2 种算法运行数据集的轨迹与真实轨迹（groudtruth）的对比图，可以看出，相比于ORB-SLAM2算法，本文改进算法的运行数据集的轨迹整体接近真实轨迹，算法的定位误差更小。分别将2 种算法的轨迹与真实轨迹进行对比，将绝对误差可视化，如图7、8 所示，图中右边是不同误差数值对应的彩色样条，由深蓝色到深红色，误差数值逐渐变大。可以看出，ORB-SLAM2 算法运行数据集得到的轨迹误差数值整体偏浅蓝色和绿色，而本文改进算法运行数据集得到的轨迹误差数值整体偏深蓝色，整体误差明显偏小。

图6 轨迹对比图

结合图9、10 和表2 可知，ORB-SLAM2 算法运行数据集得到的绝对位姿误差的均方根误差为0.141 m、最大误差为0.242 m，本文改进算法运行数据集得到的绝对位姿误差的均方根误差为0.107 m、最大误差为0.208 m。相较于ORB-SLAM2 算法，本文改进算法的均方根误差减小了24.1%，最大误差减小了14.0%，提高了算法的定位精度。

4 结论

图7 ORB-SLAM2 算法绝对轨迹误差可视化图

图8 本文改进算法绝对轨迹误差可视化图

图9 ORB-SLAM2 算法绝对位姿误差算法

本文结合图像深度与色彩信息，对ORB-SLAM2算法的回环检测模块进行改进，剔除因相似场景产生的错误回环，提高了算法的回环准确率。通过场景布置，使用Kinect1.0 相机采集数据制作相似场景数据集。基于自制相似场景数据集和TUM RGBD 标准数据集对改进算法和ORB-SLAM2 算法进行测试对比，实验结果表明改进算法能够剔除因相似场景产生的错误回环，将改进算法应用于搜救机器人将提高搜救机器人的定位与建图精度，进而提高搜救机器人的自主行动能力和搜救效率。