一种融合ORB闭环检测的激光SLAM方法

技术领域

本发明涉及移动机器人领域，特别涉及一种融合ORB闭环检测的激光SLAM方法。

背景技术

未知环境下移动机器人同时定位与建图(Simultaneous Localization andMapping—SLAM)是当前移动机器人研究的热点。随着移动机器人探索规模的扩大，机器人在大规模环境下同时定位与建图的鲁棒性显得尤为重要，而可靠的闭环检测是构建鲁棒SLAM中最重要最关键的问题之一。闭环检测是机器人判断自己当前位置是否位于己访问过的环境区域，并以此作为地图是否需要更新校正的依据，对于提高大规模环境下的SLAM鲁棒性有重大意义。激光传感器是SLAM中应用最广的传感器，大规模环境下基于激光的SLAM采用算法本身产生的位姿估计，存在累计误差的问题，从而可能导致闭环检测失败，而视觉特征包含的信息丰富，在多视点匹配上更适合用于闭环检测。视觉SLAM中在不需要位姿估计的前提下可通过视觉特征的匹配实现闭环检测，但该闭环检测方法与视觉SLAM过程联系紧密，只能应用于视觉SLAM。

激光雷达和视觉传感器各有优缺点，激光雷达测量精度高，计算量少、但是激光的信息单一，视觉传感器信息丰富，但是测量误差大，而且无法保证实时性。单独使用任意的传感器都会引入各自的缺点，但是我们可以结合两者的优点，来实现准确度高而且能保证实时性的SLAM算法。

目前国内存在一些与机器人SLAM相关的一些专利，但是其中大多数方法都是基于单一激光传感器或者视觉传感器的，从现有的研究结果来看，在小型环境中，基于图优化方法的激光SLAM即使没有闭环检测也能拥有很高的建图精度，而到了大型环境中，比如大型的室内工厂，机器人运行一段时间后，由于累积误差的存在使得地图会有较大程度的形变。现有的激光SLAM算法对于这个问题的解决是引进闭环检测来消除累积的误差。但是由于单线激光雷达所采集的信息量比较少，所以在判断闭环时从实际闭环点到检测出来的闭环点之间会有一个较大的误差，有时激光SLAM甚至难以检测到闭环。而且当移动机器人处在几何环境特征重复率较高的环境中时，单独根据激光雷达所采集到的信息来检测闭环会有一定概率出现错误的匹配。而单目相机采集到的图像信息比较丰富，这一特点恰好弥补了激光SLAM的不足。

综上所述，已有的SLAM算法主要是基于单一距离传感器的：纯激光SLAM方法在几何结构重复的环境中容易产生错误匹配，而视觉SLAM难以保证实时性，这些方案没有普遍应用的潜力。

发明内容

本发明的目的是根据上述现有技术的不足，提供了一种融合ORB闭环检测的激光SLAM方法。

本发明目的实现由以下技术方案完成：

一种融合ORB闭环检测的激光SLAM方法，其包括并行运行的激光SLAM过程以及视觉闭环检测过程；

在激光SLAM过程中，持续检测位姿信息，当上一个关键时间点与当前时间点的位姿信息的变化率大于变化率阈值时，将当前时间点作为关键时间点，并触发一次视觉闭环检测过程；

在视觉闭环检测过程中，从环境图像中提取图像关键帧，提取当前的图像关键帧的ORB特征，用词袋模型表示当前的图像关键帧，得到当前的图像关键帧的词向量；根据当前的图像关键帧的词向量在历史图像关键帧集合中进行搜索，以检测视觉闭环，并将当前的图像关键帧的词向量加入到历史图像关键帧集合中；

检测到视觉闭环后，激光SLAM过程进行视觉辅助激光闭环检测。

本发明的进一步改进在于，所述位姿信息为三维向量P(x，y，θ)，其中(x，y)表示机器人在二维空间中的位置，θ表示机器人绕自身的旋转角；判断上一个关键时间点与当前时间点的位姿信息的变化率是否大于变化率阈值采用的公式为：

P

其中，P

|θ

其中，xy

本发明的进一步改进在于，用词袋模型表示当前的图像关键帧，得到图像关键帧的词向量具体包括以下步骤：

提取图像关键帧的ORB特征，并将ORB特征点周围的像素使用高斯滤波平滑；

利用平滑后的图像计算描述子；描述子为一个长度为L

其中B

选取多个图像关键帧，将选取的图像关键帧的特征点的描述子通过kmeans++算法聚类，得到词袋模型的词语；利用词语的译频率-逆文档频率作为该词语的权重w

其中1.w

本发明的进一步改进在于，进行闭环检测的过程包括以下步骤：

在历史图像关键帧集合中搜索与当前的图像关键帧的相似度大于相似度阈值的历史图像关键帧；

根据当前的图像关键帧以及搜索到的历史图像关键帧进行组匹配、时间一致性检验以及几何一致性检验，若通过时间一致性检验以及几何一致性检验表示检测到视觉闭环。

本发明的进一步改进在于，计算两个图像关键帧i和j的相似度η(v

其中，v

其中，S(v

本发明的进一步改进在于，寻找与当前的图像关键帧的相似度大于相似度阈值的历史图像关键帧的过程中，需要剔除搜索结果中不满足时间间隔限定的历史图像关键帧；时间间隔限定的判断公式为：

t

其中，t

本发明的进一步改进在于，在激光SLAM过程中，在每个关键时间点使用激光雷达采集一次激光关键帧，并进行一次激光闭环检测或者所述视觉辅助激光闭环检测；在所述视觉辅助激光闭环检测的过程中，闭环判定参数的范围大于所述激光闭环检测采用的闭环判定参数的范围。

本发明的有益效果是：

(1)采用ORB词袋提取图像关键帧的词向量，与现有的SURF词袋、BRIEF词袋模型相比，ORB词袋具有更高的效率以及更好的准确性；

(2)通过消除冗余闭环检测结果，可以输出更少的闭环，提高了闭环检测的准确性，在保证建图过程效果不变的情况下，使得计算时间减少，提升了SLAM过程的实时性；

(3)将视觉闭环检测方法与激光SLAM融合之后，激光SLAM可以利用视觉信息判断是否存在闭环，从而提高了建图的准确度。

附图说明

图1为本发明融合ORB闭环检测的激光SLAM方法的流程图；

图2为本发明中的视觉闭环检测的流程图。

具体实施方式

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

如图1、2所示，本实施例公开了一种融合ORB闭环检测的激光SLAM方法，该方法应用在机器人领域。该融合ORB闭环检测的激光SLAM方法包括并行运行的激光SLAM过程以及视觉闭环检测过程。

具体的，如图1和图2所示，在激光SLAM过程中，机器人持续检测位姿信息，当上一个关键时间点与当前时间点的位姿信息的变化率大于变化率阈值时，将当前时间点作为关键时间点，并触发一次视觉闭环检测过程。

在视觉闭环检测过程中，从环境图像中提取图像关键帧，提取当前的图像关键帧的ORB特征，用词袋模型表示当前的图像关键帧，得到当前的图像关键帧的词向量；并根据当前的图像关键帧的词向量在历史图像关键帧集合中进行搜索，以检测视觉闭环，并将当前的图像关键帧的词向量加入到历史图像关键帧集合中。

检测到视觉闭环后，激光SLAM过程进行视觉辅助激光闭环检测；在激光SLAM过程中，在每个关键时间点使用激光雷达采集一次激光关键帧，并进行一次激光闭环检测或者视觉辅助激光闭环检测。在视觉辅助激光闭环检测的过程中，闭环判定参数的范围大于普通的激光闭环检测采用的闭环判定参数的范围。检测到激光闭环之后，激光SLAM过程对位姿图进行全局优化。

本实施例中，机器人采用单目摄像头获取视觉图像，并根据视觉图像进行视觉闭环检测。视觉图像仅用于闭环检测，不参与SLAM过程中的位姿计算，减小了图像处理的运算量，保证了实时性。激光SLAM过程会依据视觉闭环检测的结果，当检测出视觉闭环时，表明很可能遇到了闭环，此时激光SLAM过程可采用视觉辅助激光闭环检测的方式进行闭环检测，在视觉辅助激光闭环检测的过程中，可采用更加宽松的判断标准，以便激光SLAM过程检测出激光闭环。视觉特征相比于单线激光雷达的特征更为丰富，可以给激光SLAM的闭环检测提供一个可靠的参考检测结果。

本实施例中，采用激光雷达、惯性传感器编码装置相结合的方式检测机器人的位姿信息。位姿信息是三维向量P(x，y，θ)，其中(x，y)表示机器人在二维空间中的位置，θ表示机器人绕自身的旋转角。激光雷达获得的位姿信息是不精确的位姿信息，同时由机器人提供的编码器是不准确的，累积误差同样非常大。因此本文利用机器人的编码器与惯性测量单元(Inertial measurement unit，IMU)结合给SLAM算法作为机器人的初始化位姿。其中机器人编码器的输出为粗略的P(x

P(x，y，θ)＝EKF(P(x

其中，P(x，y，θ)表示输入给激光SLAM的初始化里程计，P(x

在激光SLAM过程以及视觉闭环检测过程中，均根据位姿信息选取关键帧。具体的，当上一个关键时间点与当前时间点的位姿信息的变化率大于变化率阈值时，表示机器人移动了一定的距离，因此将当前时间点作为关键时间点，并触发一次视觉闭环检测过程。判断上一个关键时间点与当前时间点的位姿信息的变化率是否大于变化率阈值采用的公式为：

P

其中，P

|θ

其中，xy

视觉闭环检测的过程中，需要使用词袋模型表示图像关键帧，以得到图像关键帧的词向量。其具体包括以下步骤：

首先，提取图像关键帧的ORB特征，并将ORB特征点周围的像素使用高斯滤波平滑；提取图像关键帧的ORB特征为本领域的现有技术；

随后，利用平滑后的图像计算描述子；描述子为一个长度为L

其中B

选取多个图像关键帧，将选取的图像关键帧的特征点的描述子通过kmeans++算法聚类，得到词袋模型的词语；利用词语的译频率-逆文档频率作为该词语的权重w

其中1.w

通过使用词向量v(B)表示图像关键帧，使得图像关键帧可以方便地进行相似性对比，从而便于检测视觉闭环。用ORB特征的词向量来描述一幅图像，该算法的优越性在于其具有比较良好的描述能力、快速检测能力以及比较鲁棒的特征匹配能力，使用ORB特征能够帮助我们在闭环检测方面减少大量的计算时间。

在视觉闭环过程中，每获得一幅图像关键帧就进行一次视觉闭环检测。闭环检测过程中，在历史图像关键帧集合中搜索与当前的图像关键帧的相似度大于相似度阈值α的历史图像关键帧。搜寻完成后，根据当前的图像关键帧以及搜索到的历史图像关键帧进行组匹配、时间一致性检验以及几何一致性检验，若通过时间一致性检验以及几何一致性检验表示检测到视觉闭环。闭环检测完成后，将当前的图像关键帧的词向量以及时间戳存储至历史图像关键帧集合中。历史图像关键帧集合可采用数据库进行实现，历史图像关键帧集合中保存有每一个图像关键帧的词向量以及时间戳。

本实施例中，通过图像关键帧的词向量计算图像关键帧的相似度。计算两个图像关键帧i和j的相似度η(v

其中，v

其中，S(v

根据当前的图像关键帧以及匹配的历史图像关键帧进行组匹配的过程中，对于具有不同时间戳t

一个正确的闭环对应的查询图像和匹配图像在时间上应该具有一定的时间连续性，这便是时间一致性检验。对于正确的闭环来说，当(v

几何一致性检验在满足时间一致性检验之后考虑正确的闭环对应的两张图应具有相似的场景，根据ORB特征点的分布，应能通过图像匹配算法计算出一个合理的本质矩阵。当这一条件满足时，认为产生了正确的闭环。

初始闭环匹配算法采用时间约束与几何约束提高匹配结果的鲁棒性，但由于机器人行驶过程中存在长时间停留在某一地点或行进过程中图像非常相似的情况，因此需要剔除搜索结果中与当前的图像关键帧间隔时间过短的历史图像关键帧。

关键帧的概念在一定程度上可以帮助我们去除因停留在某一地点而产生的错误闭环，但对于二维空间位置不变，但角度有变化的情况仍然会匹配出错误的闭环如机器人在同一个地点旋转，在不同时刻的角度分别为0°，15°，3°，14°，这时仍会出现错误的闭环结果，这是因为得出的闭环结果不满足人的直观感受：闭环检测结果得到的两帧图像应该相距一定的时时间。

具体的，在搜索与当前的图像关键帧的相似度大于相似度阈值的历史图像关键帧的过程中，需要剔除搜索结果中不满足时间间隔限定的历史图像关键帧；时间间隔限定的判断公式为：

t

其中，t

闭环检测的作用是减少机器人在定位与建图中的累积误差，这一过程通常由图优化算法完成，然而图优化算法是一项很耗时的过程，为了提高建图的时间效率，闭环检测的结果应保证具有正确性与合理性。为了避免出现冗余的闭环检测结果，检测到视觉闭环时，记录视觉闭环的闭合点的图像关键帧的编号，图像关键帧的编号逐帧累加，相邻两个视觉闭环的闭合点的图像关键帧之间需要间隔一定数量的图像关键帧：

loop

其中，loop

视觉闭环检测过程检测到视觉闭环后，通过ROS的消息机制将闭环检测成功的结果发送至激光SLAM过程。本实施例中，激光SLAM过程与现有的激光SLAM过程基本相同，例如开源项目ROS(https：//github.com/ros-perception/open_karto)中实现的激光SLAM算法。与该开源项目相比，本实施例的激光闭环检测过程除了普通的激光闭环检测之外，还包括视觉辅助激光闭环检测，视觉辅助激光闭环检测应用在检测到视觉闭环的情况下。

与普通的激光闭环检测相比，在视觉辅助激光闭环检测的过程中，闭环判定参数的范围大于所述激光闭环检测采用的闭环判定参数的范围，使得视觉辅助激光闭环检测更容易检测到激光闭环，这使得激光SLAM过程可以利用视觉闭环检测的信息。

激光雷达与视觉传感器的方法各有优劣，激光雷达包含信息量少，在复杂或者几何结构相似的大型环境中检测闭环存在问题，而好的闭环检测能够在位姿图中添加新的连接，从而进行全局优化来消除累计误差，进而可以更好的优化地图。考虑到图像信息丰富，通过视觉方法检测闭环可以有效的检测复杂或重复环境中的闭环，

在几何结构很相似的重复环境中，激光SLAM很容易判断出错误的闭环，而图像信息丰富，一般不会受相似的几何环境的影响，能够更检测出更准确的闭环，在判断激光闭环之前先判断视觉部分有没有检测到闭环，相当于给激光的闭环检测加入了一个先验条件，在视觉成功检测闭环的时候放宽激光闭环检测的条件，而在没有检测到视觉闭环时，采用相对严格的条件进行激光闭环检测，避免了激光闭环检测过程中因几何结构重复的环境中检测到错误的闭环。

本实施例中的激光SLAM算法大多是基于图优化的方法，图优化的SLAM主要分为前端和后端，前端通过观测信息与系统内部约束来建立图的节点和边，实现图的建立；后端主要优化前端建立的图。基于图优化的SLAM结合全部观测信息来优化并估计出机器人的整体运动轨迹，并且基于该估计，进一步给出环境地图。这里，激光SLAM是利用位姿信息来建立位姿图的节点和边。

本实施例中，每个关键时间点使用激光雷达采集一次激光关键帧，各历史激光关键帧按照时间序列存储在数据库中。进行激光闭环检测或者视觉辅助激光闭环检测的过程包括以下步骤：

(S01)将当前的激光关键帧与历史激光关键帧进行匹配，找到可能的闭环链；

(S02)验证该闭环链能否和当前的激光关键帧构成闭环。

在上述的步骤(S01)中，从第0个历史激光关键帧进行搜寻匹配，直到当前的激光关键帧，这个过程为现有技术。闭环链的判定条件为：

a.找到的候选闭环链中相邻激光关键帧是连续的；上述连续指的是空间上的连续；

b.找到的候选闭环链中每一个激光关键帧到当前的激光关键帧的距离小于一个值alpha；

c.找到的候选闭环链长度大于beta；

d.该链中的激光关键帧不能是当前激光关键帧的指定距离alpha2内的邻居。

在步骤(S02)中，对满足上述条件的候选闭环链进行验证，以判断其是否能与当前激光关键帧组成闭环。该验证过程为现有技术，在验证过程中，使用该可能的闭环链和当前帧进行扫描匹配，当匹配得分大于gama1时，并且协方差小于cov1矩阵时，进行更高分辨率的匹配，当匹配得分大于gama2时，认为有闭环，进行闭环优化。

在步骤(S01)和(S02)中涉及的参数beta、alpha2、gama1、cov1、gama2均为闭环判定参数。激光闭环检测与视觉辅助激光闭环检测的区别在于闭环判定参数的取值不同，以是的视觉辅助激光闭环检测更容易检测到激光闭环。在视觉辅助激光闭环检测的过程中，上述闭环判定参数可部分放宽，也可全部放宽。在一个具体实施例中，视觉辅助激光闭环检测与激光闭环检测采用的闭环判定参数的比值如表-1所示。

表-1激光闭环检测与视觉辅助激光闭环检测采用的闭环判定参数的比值

本发明的实施例具有以下有益技术效果：

(1)采用ORB词袋提取图像关键帧的词向量，与现有的SURF词袋、BRIEF词袋模型相比，ORB词袋具有更高的效率以及更好的准确性；

(2)通过消除冗余闭环检测结果，可以输出更少的闭环，提高了闭环检测的准确性，在保证建图过程效果不变的情况下，使得计算时间减少，提升了SLAM过程的实时性；

(3)将视觉闭环检测方法与激光SLAM融合之后，激光SLAM可以利用视觉信息判断是否存在回环，从而提高了建图的准确度。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。