CN110531618B - 基于有效关键帧的闭环检测机器人自定位误差消除方法 - Google Patents

Abstract

本发明公开了一种基于有效关键帧的机器人闭环检测自定位误差消除方法，包括基于偏移度的关键帧粗提取，以偏移度为另一选取原则融入ORB‑SLAM的关键帧提取之中；针对区域特征相似的关键帧部分采用基于深度学习的精提取，利用Alex‑NET进行学习，完成关键帧精提取；最后基于上述的有效关键帧集进行历史闭环检测，以判断自身是否进入历史同一状况。本发明以一个多角度的有效关键帧选取策略，能很好的处理有大偏移的运动情况；利用深度学习网络，避免了相似区域关键帧提取模糊的问题；而基于历史关键帧数据集的闭环检测则避免了为发生闭环或少发生闭环情况下的时间浪费问题，提高了系统的运行速度，降低了整个过程的算法复杂度。

Description

基于有效关键帧的闭环检测机器人自定位误差消除方法

技术领域

本发明涉及基于有效关键帧的机器人闭环检测自定位误差消除方法，尤其涉及偏移动态情况下的闭环检测方法。

背景技术

长久以来，人们使用机器人对复杂环境的测量及构图一直不够理想，动态情况下的特征量的跟随效果差、环境纹理稀疏、计算负荷大以及实时性差等问题都影响着测量效果，因此，一系列的机器人SLAM构图方案被提出。

机器人SLAM算法起初是采用滤波方式，例如基于卡尔曼滤波的方法，但其必须基于高斯假设的前提之下，极大程度上限制了应用场合。而2007年，Montemerlo利用粒子滤波器实现FastSLAM算法，尽管脱离了高斯分布的噪声模型但也面临着粒子退化的问题，其效果也不如人意。随着研究发展，优化算法的效果要好于滤波算法已成为共识，因此一大批优化算法应运而生。优化算法在系统上将整个SLAM分为前后端两个部分，而这两个部分包含了以下三个步骤：(1)特征提取：作为SLAM系统的前端，对地图进行关键帧的获取，将地图信息以关键帧的形式，进行记录；(2)数据匹配：同样作为前端，完成短期内的特征跟踪，长期内的闭环检测的工作；(3)地图建立及优化：对前端信息进行地图构建并优化。在这之中，步骤(3)的研究发展最为全面，在既得的关键帧信息前提下的地图构建算法都有着不错的效果，例如基于SIEF的子地图连接以及语义地图的构建。然而对于关键帧的提取与闭环检测则显得较为不足，尤其是动态且环境纹理信息不足的情况下，特别地，较好的关键帧提取结果集也会对最后地图构建部分起到不错的帮助效果。

为采集到较为全面的关键帧，我们需要对其提取过程进行优化。而传统的提取过程中，由于动态偏移角度过大，很容易遗失许多有效的关键帧。而随着深度学习的发展，其与SLAM的结合应用使得整个系统对动态环境的适应力越来越强，取得了不错的效果。例如Konda和Memisevic提出的基于端到端的深度神经网络架构，其用于预测摄像机位姿变换进行帧间估计，取得了不错的效果。Costanta利用卷积神经网络学习图像数据的最优特征表示进行视觉里程估计，并展示了其算法在应对图像运动模糊、光照变化方面的鲁棒性。然而，深度学习需要基于大量的训练材料才能得到一个较好的网络模型，且随着目标范围的增大，训练模式与训练周期并非简单的线性增长。因此，对于全局地图为目标的深度学习容易导致整个算法的复杂度偏高，训练周期过长。

发明内容

发明目的：为了解决现有技术中对复杂动态环境的SLAM有效关键帧丢失问题以及由逐帧闭环检测导致的算法复杂度过高的问题，本发明提供一种基于有效关键帧的机器人闭环检测自定位误差消除方法。

技术方案：为达成上述目的，本发明所述的一种基于有效关键帧的闭环检测机器人自定位误差消除方法，包括以下步骤：

(1)以偏移度为选取原则融入ORB-SLAM的关键帧提取之中，对关键帧进行粗提取；

(2)经上述粗提取后，区别度较大区域所提取出来的关键帧有较好的特征性，相似区域提取的关键帧则较为模糊，将相似区域的关键帧作为待定部分，作为深度学习的输入，以卷积神经网络，实行关键帧的精提取，获取关键帧集合；

(3)在上述关键帧集合基础上进行历史闭环检测，确定机器人在当前时刻所得到的图案提取到的关键帧，是否进入历史同一状况，若不是，则没发生闭环；若是，则在当前时刻，发生闭环，针对存在误差的地方，在历史地图上进行自定位的更新修正，消除误差。

所述步骤(1)的包括以下步骤：

(11)提取数据帧：参考ORB-SLAM提取关键帧策略，使用FAST算法找到关键点，用BRIEF作为描述符；

(12)判断匹配点是否大于15个点，是则到(13)，否则返回(11)；

(13)判断自上个关键帧过去20或与关键帧之间的偏移大于一固定阈值，记为n，是则到(14)，否则返回(11)；

(14)判断与关键帧之间偏移度大于n，是则到(15)，否则执行(17)；

(15)判断跟踪参考帧的25％以上，是则到(16)，否则返回(11)；

(16)判断跟踪是否大于15个点，是则到(18)，否则返回(11)；

(17)判断跟踪是否大于50个点，是则到(18)，否则返回(11)；

(18)判断当前帧跟踪到的点是否小于参考帧的90％，是则作为插入关键帧，否则返回(11)。

所述步骤(13)的实现过程如下：结合李群李代数，以李群定义关键帧流形，在该流形上，以对应李代数上的直线距离作为偏移情况，如式(1)所示为第1帧到第2帧的偏移程度，其中，δ为关键帧的位置，d为不同关键帧处的偏角状态，Δd则反应了偏移程度：

同时，对于两个有间隔的图像帧a帧和a'帧之间的偏移度的大小定义为从第a到第a'之间的累计邻近图像帧偏移度和，如式(2)所示：

所述步骤(2)包括以下步骤：

(21)获取待定部分的特征向量，作为输入端，进入深度学习模型Alex-NET；

(22)经模型得到深度卷积网络特征，生成特征向量；

(23)结合支持向量机分类器对关键帧提取出来的特征向量完成类识别。

步骤(21)所述的Alex-NET模型具有5层的卷积层、3层的池化层和3层的全连接层，包含有60M个参数和650个神经元，类识别上限为1000类；激活函数为线性整流函数。

所述步骤(3)包括以下步骤：

(31)进入闭环检测，提取记录当前关键帧；

(32)判断当前关键帧所处区间为低概率区间还是高概率区间；

(33)低概率区间的采用跳跃式检测法，高概率区间的采用逐帧式检测法；

(34)判断是否检测到闭环，是则到(35)，否则到(31)；

(35)判断是否在低概率区间检测到闭环，是则到(36)，否则到(38)；

(36)获取上一检测关键帧；

(37)重置当前区间为高概率区间，并回到(31)，提取关键帧；

(38)进行自定位信息更新，消除误差。

所述步骤(32)中的判断过程如下：关键帧序列为有序序列，则随着序号的增加，发生闭环的概率也增大，取前半部分为低概率区间，后半部分为高概率区间，如式(3)、式(4)所示：

基于历史闭环检测结果，在相邻两次发生闭环的关键帧(帧号为a、b，a<b)之间，提取并记录一关键帧m(a<m<b)，

其中低概率区间L为：

高概率区间H为：

有益效果：与现有技术相比，本发明的有益效果：1、本发明提出了基于偏移度的关键帧粗提取方法，有效地使算法能够跟踪到偏移度较大的图像帧，同样适用于环境纹理信息缺乏的情况；2、提出了基于深度学习的关键帧精提取，有效地使算法在相似区域也能进行很好的关键帧提取工作，考虑并结合了深度学习的高效性，有效避免了相似区域下的关键帧选取模糊问题；3、基于历史关键帧集的闭环检测则解决了传统闭环检测过程中，尤其是实际运动过程中不发生闭环或者很少发生闭环的情况下浪费时间，拖慢系统运行速度耗时导致的系统运行速度慢的问题。

附图说明

图1为本发明的流程图；

图2为基于偏移度的关键帧粗提取流程图；

图3为基于关键帧集的历史闭环检测。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

如图1所示，基于有效关键帧的闭环检测，包括基于偏移度的关键帧粗提取，结合深度学习的关键帧精提取，生成有效关键帧集，然后基于该有效关键帧集进行历史闭环检测，具体包括以下步骤：

1、基于偏移度的关键帧粗提取，如图2所示。

(1)提取数据帧：采用ORB-SLAM(Oriented FAST and Rotated BRIEF-Simultaneous Localization And Mapping，基于ORB的同步定位与地图构建)提取关键帧策略。ORB-SLAM是一种采用FAST算法寻找关键点，选择BRIEF作为描述符，进行关键帧提取的SLAM算法。

(2)判断匹配点是否大于15个点，是则到(3)，否则返回(1)。

(3)判断自上个关键帧过去20或与关键帧之间的偏移大于一固定阈值(记为n)，是则到(4)，否则返回(1)。

这里对偏移度的确定，结合了李群李代数，以李群定义关键帧流形，在该流形上，以对应李代数上的直线距离作为偏移情况。如式(1)所示为第1帧到第2帧的偏移程度，其中，δ为关键帧的位置，d为不同关键帧处的角状态，Δd则反应了偏移程度。

同时，对于两个有间隔的图像帧(分别设为a帧、a'帧)之间的偏移度的大小定义为从第a到第a'之间的累计邻近图像帧偏移度和，如式(2)所示。

(4)判断与关键帧之间偏移度大于n，是则到(5)，否则执行(7)。

(5)判断是否跟踪参考帧的25％以上，是则到(6)，否则返回(1)。

(6)判断跟踪是否大于15个点，是则到(8)，否则返回(1)，在跟踪参考帧25％以上的同时，也保证跟踪大于15个点。合理地增加关键帧选取时候的灵活性。

(7)判断跟踪是否大于50个点，是则到(8)，否则返回(1)。原ORB-SLAM要求大于50个点，在偏移度未满足阈值情况下，这里也按照跟踪50个以上的点为标准。

(8)判断当前帧跟踪到的点是否小于参考帧的90％，是则作为插入关键帧，否则返回(1)；以90％为阈值区分两个参考帧，若高于90％则当作冗余帧剔除。

2、基于深度学习的关键帧精提取。经上述粗识别，区别度较大区域所提取出来的关键帧有很好的特征性，而少许相似区域提取的关键帧则较为模糊。将这些区域作为待定部分，作为深度学习的输入，进入卷积神经网络。

(1)构建Alex-NET网络，模型有5层的卷积层、3层的池化层和3层的全连接层，总体来说，有60M个参数和650神经元，类识别上限为1000类。

(2)提取相似待定区域的特征向量，作为输入图像，进入深度学习模型。

(3)经过五层卷积操作和相匹配的最大池化处理，激活函数采用线性整流函数ReLU，加快收敛速度，提取出深度卷积网络特征，生成特征向量。

(4)结合关键帧提取器的SVM分类器完成特征向量的类识别，对相似区域的关键帧进行分类处理，解决其模糊问题。

3、闭环检测。在上述已得的关键帧数据集基础上，判断自身是否进入历史同一地点，若不是，则没发生闭环；若是，则说明在当前时刻，发生闭环，即能将机器人此刻所处的真实位置与根据历史地图自定位所得到的位置进行比较，针对存在误差的地方，在历史地图上进行更新修正，达到误差消除的效果。在地图构建过程中对基于关键帧的视觉SLAM，一个解决其累积误差的手段是闭环检测，而闭环检测的效率以及精度也依赖于上述关键帧的选择。

如图3所示，本发明的闭环检测主要利用整个地图发生闭环的关键帧，结合历史闭环检测的结果来预测待检测关键帧所处位置，进而通过差异化的检测策略来进行闭环检测。包括：

(1)基于历史闭环检测结果，在相邻两次发生闭环的关键帧(帧号为a、b，a<b)之间，提取并记录一关键帧m(a<m<b)。

(2)预测当前关键帧所处区间。预测当前关键帧m处于低概率区间L还是高概率区间H，其代表了在当前关键帧处发生闭环的概率。特别地，该概率的表征意义是将关键帧与历史关键帧集合相比较，类似程度与数值呈正相关，取最大值并经归一化操作，作为概率。

考虑到关键帧序列为有序序列，则随着序号的增加，发生闭环的概率也增大(即闭环产生后立刻又产生闭环的几率很小)。因此，取前半部分为低概率区间，后半部分为高概率区间，判断m所在范围即可预测关键帧所处区间。

其中低概率区间L为:

高概率区间H为:

(3)根据所处区间选择检测策略。低概率区间采用跳跃式检测的方法和高概率区间采用逐帧式检测的方法；跳跃式检测是指在闭环检测过程中采用隔帧选取关键帧检测闭环，降低计算复杂度，提高系统运行速度。

(4)判断是否检测到闭环，是则到(5)，否则到(1)。

(5)判读是否在低概率区间检测到闭环，是则到(6)，否则到(8)。若在低概率区间检测到闭环说明，该区间不可靠，需修正更新。

(6)获取上一检测关键帧。对不可靠区间定位，为后续区间重置准备。

(7)重置当前区间为高概率区间。对不可靠的低概率区间进行修正更新。

(8)进行自定位信息更新，消除误差。

综上，采用基于有效关键帧的机器人闭环检测自定位误差消除方法能有效地跟踪环境纹理信息缺乏或偏移度较大的情况，解决了动态情况下的关键帧提取问题，同时，针对相似区域，基于Alex-NET的深度学习也能得到较精确的关键帧。而结合上述的有效关键帧集也为历史闭环检测缩小了计算范围，减少了检测时间，提高了运行效率。

Claims (3)

1.一种基于有效关键帧的机器人闭环检测自定位误差消除方法，其特征在于，包括以下步骤：

(1)以偏移度为选取原则融入ORB-SLAM的关键帧提取之中，对关键帧进行粗提取；

(2)经上述粗提取后，区别度较大区域所提取出来的关键帧有较好的特征性，相似区域提取的关键帧则较为模糊，将相似区域的关键帧作为待定部分，作为深度学习的输入，以卷积神经网络，实行关键帧的精提取，获取关键帧集合；

(3)在上述关键帧集合基础上进行历史闭环检测，确定机器人在当前时刻所得到的图案提取到的关键帧，是否进入历史同一状况，若不是，则没发生闭环；若是，则在当前时刻，发生闭环，针对存在误差的地方，在历史地图上进行自定位的更新修正，消除误差；

所述步骤(1)的包括以下步骤：

(11)提取数据帧：参考ORB-SLAM提取关键帧策略，使用FAST算法找到关键点，用BRIEF作为描述符；

(12)判断匹配点是否大于15个点，是则到(13)，否则返回(11)；

(13)判断自上个关键帧过去20或与关键帧之间的偏移大于一固定阈值，记为n，是则到(14)，否则返回(11)；

(14)判断与关键帧之间偏移度大于n，是则到(15)，否则执行(17)；

(15)判断跟踪参考帧的25％以上，是则到(16)，否则返回(11)；

(16)判断跟踪是否大于15个点，是则到(18)，否则返回(11)；

(17)判断跟踪是否大于50个点，是则到(18)，否则返回(11)；

(18)判断当前帧跟踪到的点是否小于参考帧的90％，是则作为插入关键帧，否则返回(11)；

所述步骤(2)包括以下步骤：

(21)获取待定部分的特征向量，作为输入端，进入深度学习模型Alex-NET；

(22)经模型得到深度卷积网络特征，生成特征向量；

(23)结合支持向量机分类器对关键帧提取出来的特征向量完成类识别；

所述步骤(3)包括以下步骤：

(31)进入闭环检测，提取记录当前关键帧；

(32)判断当前关键帧所处区间为低概率区间还是高概率区间；

(33)低概率区间的采用跳跃式检测法，高概率区间的采用逐帧式检测法；

(34)判断是否检测到闭环，是则到(35)，否则到(31)；

(35)判断是否在低概率区间检测到闭环，是则到(36)，否则到(38)；

(36)获取上一检测关键帧；

(37)重置当前区间为高概率区间，并回到(31)，提取关键帧；

(38)进行自定位信息更新，消除误差。

2.根据权利要求1所述的一种基于有效关键帧的机器人闭环检测自定位误差消除方法，其特征在于，所述步骤(13)的实现过程如下：结合李群李代数，以李群定义关键帧流形，在该流形上，以对应李代数上的直线距离作为偏移情况，如式(1)所示为第1帧到第2帧的偏移程度，其中，δ为关键帧的位置，d为不同关键帧处的偏角状态，Δd则反应了偏移程度：

同时，对于两个有间隔的图像帧a帧和a'帧之间的偏移度的大小定义为从第a到第a'之间的累计邻近图像帧偏移度和，如式(2)所示：

3.根据权利要求1所述的一种基于有效关键帧的机器人闭环检测自定位误差消除方法，其特征在于，步骤(21)所述的Alex-NET模型具有5层的卷积层、3层的池化层和3层的全连接层，包含有60M个参数和650个神经元，类识别上限为1000类；激活函数为线性整流函数。

Images