基于双目相机的SLAM回环检测改进算法研究

2023-02-13王宇杰

测控技术 2023年1期

朱晨，王宇杰，郭杭

(南昌大学信息工程学院，江西南昌 330031)

随着社会与经济快速发展，定位和导航在大众生活和国家重要战略中发挥着越来越重要的作用。例如灾难救援[1-3]、工业生产[4-5]、家庭服务[6-8]等。但是由于卫星导航系统在室内等场景下，无法进行高精度定位，对于室内和室外卫星信号较弱的场景，同步定位与建图(Simultaneous Localization and Mapping,SLAM)技术能够有效地解决这一问题[9]。随着近些年计算机视觉技术的不断发展，以相机为传感器的视觉SLAM技术受到了越来越多的关注[10]。视觉SLAM具有成本低、精度较高、采集的图像信息丰富等优点，在室内外复杂和未知的环境下具有很强的适应性[10]，因为视觉定位技术利用丰富的信息在定位的同时还能够进行环境感知(即为建图)，可以将未知环境变成已知环境进行进一步处理。视觉定位技术系统具有图像处理、跟踪(Tracking)、建图(Mapping)和回环检测(Loop Closing)4个模块[11]。

回环检测是视觉定位技术中较为关键的一个部分[9]。视觉定位技术虽然能够进行当前载体的实时位姿估计，但是从本质上来说是一种航迹推算算法，存在位姿误差累积的问题。所以需要回环检测算法消除与抑制累计误差[10]。基于词袋模型(Bag of Words，BoW)的回环检测技术，在目前各种开源的视觉SLAM算法中被广泛使用，其本质上是基于外观的回环检测算法[12]，通过图像信息判断当前场景是否是之前到达过的地点，实现位姿纠正。

随着视觉SLAM在小型移动载体上应用越来越广泛，近年来回环检测出现了一些新的问题。主要问题是小型移动载体对视角变化的要求较高[13]。以地面移动机器人为例，如果机器人在原地旋转一定的角度，虽然还是在原地，但是却很难正确匹配。为解决这一问题，部分学者提出使用全景相机来代替普通的相机[14]、利用激光SLAM来辅助视觉SLAM[15]、直接基于RGB-D相机来解决回环检测的问题[16]。虽然这些方法能够解决一些视角变换带来的问题，但是却增加了额外的成本。

针对传统视觉SLAM闭环检测过程中，因视角变化导致难以正确闭环匹配的问题，本文对传统的基于BoW闭环检测算法提出改进，采用低成本的双目相机进行环境感知。具体方法是：①利用透视变换算法对当前图像关键帧进行视角变换；②采用基于迭代最近点(Iterative Closest Point，ICP)算法求出当前关键帧与闭环候选关键帧的空间信息，进行几何检验。在算法耗时要求不甚严格的前提下，采用本文的算法，可以提高回环检测的准确率和召回率。

1 回环检测算法

图1为本文回环检测算法流程。判断当前图像关键帧是否满足回环条件，首先，对该关键帧进行左右各30°视角的透视变换，目的是减少因视角变化过大导致误匹配和漏匹配的影响；然后，提取变换后该关键帧中的特征点与描述子，将提取到的每一个特征点和描述子通过多级聚类的方式转化为词汇，计算出该关键帧的词袋向量。相似度计算是计算当前关键帧与历史关键帧的词袋向量的相似情况，如果小于设定的相似度阈值，历史关键帧中的关键帧即为闭环候选关键帧，进行下一步的操作。基于ICP算法与空间信息几何检验可以判断闭环候选关键帧是否真的构成回环，如果满足距离阈值，则认为是正确回环帧，从而纠正位姿，消除累积误差。

图1 本文回环检测算法流程图

2 基于词袋模型的回环检测方法

基于词袋模型的方法忽略单词出现的顺序和语法规则，只需要统计图像中的文本单词出现的次数，能够极大地提高单词的检索速度并减少内存开销。实现过程是将每张图片关键帧转为单词向量，即为词袋，然后计算当前关键帧与历史关键帧的词袋向量相似度，确定是否经过同一地点[12]。

根据事先训练好的词汇树，计算出关键帧之间的相似度。根据关键帧的词袋，使用 TF-IDF 算法计算当前关键帧Ii以及所有历史关键帧Ij中每个单词的权值，每张关键帧中的所有词汇信息用一个词袋向量vt表示；最后将当前关键帧在之前的所有关键帧中检索，计算当前关键帧与候选关键帧的相似度分数，并对其进行归一化处理，如式(1)和式(2)所示。

(1)

式中：s(vi,vj)为关键帧i和关键帧j的词袋向量vi和vj的相似度值。

(2)

式中：η为归一化的相似度分数；vj-1为和该地点最接近的另一个关键帧词袋向量。若η高于相似度阈值t，则关键帧j是当前关键帧i的闭环候选帧，在几何检验步骤中通过检验，则确认为正确回环。

3 利用透视变换对图片进行视角变换

传统的视觉SLAM中主要采用的是基于词袋模型的图像检索方式，但是在视角差异较大情况下难以匹配到正确的图像。

在视觉SLAM过程中，地面机器人重新回到闭环地点处，相较于初始姿态，其当前姿态难免会发生变化。根据参考文献[13]，当相机的视角旋转30°时，其相似度将衰减至原来的20%，当相似度为20%时则不可避免会产生错误的回环检测。所以本文提出采用透视变换的方式对当前关键帧的视角进行变换，从而减少因视角变化而产生的影响，提高算法召回率。

在实际生活中的景物，由于观察距离与方位的不同，导致在视觉上引起不同的反映，即为透视现象。透视变换的本质是将在原来平面上的图像向一个新的平面投影，得到图像在新的平面上的坐标[17]，如式(3)所示。透视变换原理示意图如图2所示。

图2 透视变换原理示意图

(3)

透视变换后相机视角示意图如图3所示。Z轴方向的视角为当前相机视角，通过透视变换矩阵对相机的当前视角下获取的关键帧图片进行左右各30°的视角变换，获得相机在发生向左(正方向)或向右(负方向)旋转30°的视角下拍摄到的当前场景的图片关键帧。在室内环境下实际相机视角拍摄的图片如图4所示。经过向左和向右透视变换30°后视角的图片如图5和图6所示。

图3 透视变换后相机视角示意图

图4 实际相机视角拍摄图片

图5 向左透视变换30°视角图片

图6 向右透视变换30°视角图片

4 基于ICP算法空间信息的几何检验

通过相似度阈值筛选出的闭环候选关键帧仍然存在误匹配、当前关键帧对应到多个闭环候选关键帧的情况，所以需要几何验证进一步去筛选。

由于本文回环检测改进算法是基于双目相机的，在相机坐标系中，通过双目相机模型可以计算出当前关键帧和闭环候选关键帧中单词相对应的特征点和描述子的三维点云信息，基于ICP算法就可以求出关键帧与闭环候选关键帧的相对位姿转换关系，即为空间信息，用于几何检验判断是否构成回环，然后再纠正位姿，消除累积误差。

ICP算法是3D空间点配准算法,该算法思想简单、需要的信息少、容易实现并能够获得高精度的配准结果。ICP算法的实质是基于最小二乘最优匹配思想的匹配算法。ICP算法的核心思想是：首先，基于点到点的欧式距离最近原则，在当前点云和目标点云中寻找多组对应的点对，组成对应点集；然后，在这个对应点集的基础上，计算出这2个点云间的变换矩阵，更新当前点云；最后，通过迭代的方式，直至误差小于某一阈值或者达到迭代次数，确定出最终的变换关系[18-19]。

假设当前点云为P={p1,p2,…，pm}；目标点云为Q={q1,q2,…，qn}；m和n为当前点云和目标点云的点数；设最大迭代次数为kmax；单次迭代的均方差阈值为ξ；两次迭代的均方误差差值的阈值为ε。ICP算法的实现过程如下。

② 求解变换参数。求出如式(4)所示的最小值，然后使用四元数法计算出变换参数Rk、Tk。

(4)

③ 更新源点云。使用上一步计算出的变换参数Rk、Tk对当前点云Pk进行变换得到新的当前点云Pk+1。

Pk+1=RkPk+Tk

(5)

④ 进行迭代计算判定。根据式(6)计算本次迭代变换的均方误差dk，若迭代达到最大次数kmax、dk+1-dk<ε或dk<ξ，则停止迭代，否则重复上述步骤。

(6)

(7)

⑤ 配准点云。以最后一次迭代计算的变换参数为最终的变换参数，通过此变换参数将当前点云配准到目标点云，从而完成点云匹配。

根据已知的双目相机的参数和数学模型，由当前关键帧和闭环候选关键帧中的单词对应的特征点，可以求出这两个关键帧图片特征点对应的点云。根据ICP算法可以求出当前关键帧和候选关键帧的变换参数。

在求出变换参数之后，还需要进一步筛选和确认候选关键帧，求出当前关键帧的正确闭环候选关键帧。本文采取的方法是设置当前关键帧的距离半径阈值，如图7所示。若当前关键帧与当前关键帧闭环候选关键帧的之间的距离小于设定的阈值，则判定闭环检测成功，由于基于双目相机的视觉SLAM位姿漂移不严重[11]，可以通过此方法进行验证。

图7 空间信息的几何验证

5 回环检测评价指标与实验分析

5.1 回环检测评价指标

对于基于外观的回环检测算法，存在可能在同一地点因为光线、天气或动态物体的影响导致两张图片在外观上存在较大的差异，算法可能会无法检测出其中存在的闭环匹配关系。如果拍摄的场景过于单一，导致整个场景中的景物非常相像，极有可能将不是回环地点的场景认作同一场景。

对于前一种实际上构成闭环但是算法认为不是闭环的情况，称之为感知变异(Perceptual Variability)或者假阴性(False Negative,FN)。而实际上不构成闭环但是算法认为是闭环的情况，称之为感知偏差(Perceptual Aliasing)，或者假阳性(False Positive)。回环检测结果分类如表1所示。

表1 回环检测结果分类

在回环检测算法中，分别使用回环检测的准确率(Precision)和召回率(Recall)这两个性能指标[12-20],如式(8)所示。

(8)

在实际过程中，应该尽可能地同时提高回环检测算法的准确率和召回率。但二者存在矛盾性，即回环检测的召回率和准确率是反比变化的，随着准确率的降低，召回率变得越来越高，这是不可避免的。当选取的相似度评分阈值较高时，准确率就会更高，同时召回率则会由于通过筛选的图像数量降低而下降；而当阈值设置得较低时，条件放宽后可以得到更高的召回率，但是准确率则会下降。所以在多数情况下，回环检测算法在保证准确率的同时要尽可能地提高召回率。

5.2 实验分析

本文实验中的词汇树，采用室内环境下采集的Bicocca_2009-02-26a数据集训练生成。本文使用双目相机在室内环境下采集3个数据集，图8～图10分别为3个数据集的真实轨迹，其中数据集traj_1、traj_2、traj_3的轨迹长度分别为338.249 m、175.861 m、56.178 m，然后通过高精度的光学捕捉系统获取到当前实际位姿作为参考值。

图8 traj_1数据集的真实轨迹

图9 traj_2数据集的真实轨迹

图10 traj_3数据集的真实轨迹

利用该数据集进行实验，通过调节关键帧相似度阈值t=[0,1]，得到准确率和召回率的变化结果如图11～图13所示。

图11 traj_1的召回率和准确率实验结果

图12 traj_2的召回率和准确率实验结果

图13 traj_3的召回率和准确率实验结果

由图11～图13可以看出，3个数据集在准确率都为100%的情况下，本文算法的召回率比DBoW分别高出(0.247-0.183)/0.183 = 34.9%，(0.098-0.086)/0.086 = 13.9%，(0.351-0.292)/0.292= 20.2%。本文的算法在准确率相同的基础上，获得比DBoW算法更好的召回率，效果比传统基于DBoW算法更好。