CN112665575B - 一种基于移动机器人的slam回环检测方法 - Google Patents

Abstract

本发明属于机器人定位技术领域，主要涉及一种基于移动机器人的SLAM回环检测方法，包括如下步骤：S1、对机器人运动产生的激光运动畸变进行激光预处理，并对距离图像中边缘点、平面点特征进行提取；S2、将连续帧的相对姿态变换获得雷达里程计，并通过关键帧的选择组成局部子图；S3、采用全新描述子EPSC代表局部子图特征进行回环检测。本发明方法采用全新描述子EPSC进行回环检测，可有效降低漂移误差建立一致性更优化的全局地图，提升了闭环检测精度、召回率及计算效率。

Description

一种基于移动机器人的SLAM回环检测方法

技术领域

本发明属于机器人定位技术领域，具体涉及一种基于移动机器人的SLAM回环检测方法。

背景技术

随着计算机科学技术的快速发展，轮式移动机器人自主导航的研究已是智能车交通系统的研究热点之一，其中，路径规划是智能车研究领域的重要技术。移动机器人执行各类任务，如导航、传递、搜索和救援等，一般都需要对其工作环境建构地图，并确定自己的在环境中的位置，这就需要同步定位与地图构建(simultaneous localization andmapping，以下简称SLAM)技术。其中，基于激光雷达的SLAM技术具有较多优势，成为目前最受关注的SLAM方向之一。目前，市面上有很多开源的激光SLAM整体解决方案，但大部分方案很难以保证全局一致。

发明内容

本发明的目的是：旨在提供一种基于移动机器人的SLAM回环检测方法，用于解决移动机器人的SLAM回环检测实现全局地图一致的问题。

为实现上述技术目的，本发明采用的技术方案如下：

一种基于移动机器人的SLAM回环检测方法，包括如下步骤：

S1、对机器人运动产生的激光运动畸变进行激光预处理，并对距离图像中边缘点、平面点特征进行提取；

S2、将连续帧的相对姿态变换获得雷达里程计，并通过关键帧的选择组成局部子图；

S3、采用全新描述子EPSC代表局部子图特征进行回环检测。

进一步，S1包括以下步骤：

S101、利用IMU预积分后的值去除因机器人运动产生的激光运动畸变；

S102、一帧激光点云数据集合记为P＝{p₁，p₂，p₃，...，p_n}，并投影到距离图像上；

S103、在距离图像上选取一行连续的点，其集合记为R，点p_i为中点，通过以下公式可以计算p_i的曲率值：

其中d_i为点p_i到激光雷达的欧式距离，设定一个值c_b，当c大于c_b时为边缘点，当c小于c_b时为平面点。

进一步，S2包括如下步骤：

S201、两个连续帧的相对姿态变换即雷达里程计通过点到平面和点到边缘的扫描匹配方法获得，其中边沿特征点到线的距离计算如下：

平面特征的点到面的计算如下：

其中，k和k+1是连续且相关联的两帧激光，i，j，l和m是两帧激光中的点；

S202、用LM非线性优化方法得到连续两帧的位姿变换即雷达里程计；

S203、选取关键帧，设置一个距离值l和角度值θ，当某一帧的距离比上一个关键帧大于l或者角度偏移大于θ时，将被选择为关键帧；

S204、关键帧组成局部子图，当以下条件满足其一时，一个局部子图被建立：现有子图关键帧数量超过设定值n；创建子图的时间超过设定值t；角度变化过于剧烈，超过设定值ω。

进一步，S3包括如下步骤：

S301、提取特征点，设一个局部子图所有的点云集合为S＝{s₁，s₂，…，s_n}，且s_k＝(x_k，y_k，z_k)，Se＝{s_e1，s_e2，…，s_em}和S_p＝{s_p1，s_p2，…，s_p3}分别代表子图中边缘特征点和平面特征点的集合；

S302、建立传感器坐标系，采用3D扫描的点云俯视图，并将地面区域划分为多个子空间；

S303、局部子图根据方位角和径向方向进行分割，并以不同颜色区分为若干环、扇区、子空间，点云中的每个点s_k都可用极坐标来表示：

s_k＝[ρ_k，θ_k，z_k]

其中，方位角具体为：在激光雷达帧中从0到2π，径向具体为：从中心到激光能打到的最大距离；

S304、通过在方位角和径向方向上等分极坐标，将点云分割成N_s个扇区和N_r个环，整个局部子图被划分为N_r×N_s个子空间，把第i个扇区和第j个环中的边缘特征数量和平面特征数量分别记作n_eij和n_pij；

S305、将局部子图映射到矩阵上，方位角映射为矩阵的列，径向映射为矩阵的行，表征边缘特征局部子图的矩阵表示为Ω_e，且Ω_e(i，j)＝n_eij；表征平面特征局部子图的矩阵表示为Ω_p，且Ω_p(i，j)＝n_pij；若矩阵元素为空，则把该元素记为0；

S306、将两个矩阵的对应元素合并成新矩阵的对应位置元素，得到新的矩阵记作Ω_ep即获得当前局部子图的EPSC；

其中：

进一步，本发明方法还包括如下步骤：

位置重新识别，将当前位置P_n与数据库中的历史位置D＝{P₁，P₂，...，P_n-1}进行比对，确认机器人是否回环，随着数据库D的规模不断增加，采用两阶段分级位置再识别策略提高计算效率，包括基于二进制运算的快速几何检索和特征结构匹配进行回环检测。

进一步，基于二进制运算的快速几何检索的具体步骤如下：

给定EPSC矩阵Ω_ep，其在局部坐标上的几何分布可以表示为二值矩阵Γ：

对于当前EPSC矩阵候选EPSC矩阵/>及其二进制变换/>可以导出几何相似性：

其中XOR(x，y)是指矩阵x和y的异或运算，|x|是指矩阵x中的元素数量，消除视角带来的误差，最终得分计算如下：

其中是/>移动i列，通过移动列来获得一个最大的相似度得分，就可以认定为最佳匹配，以纠正激光视角带来的变化几何检索完成。

进一步，特征结构匹配的具体步骤如下：

通过列式比较识别两个EPSC矩阵和/>之间的特征相似性，假设/>和/>分别是/>和/>的第i列，则得分可以通过如下得到：

由于激光视角的影响，假设当移动k列时得分最多，那么最终得分如下计算得到：

通过两部分的得分相加确定总的得分，得分越高，子图越匹配；

当得分大于一个阈值c的时候，即可确定当前位置是机器人之前经过的历史位置，即完成回环检测。

采用上述技术方案的发明，具有如下优点：

1、本发明方法采用全新描述子EPSC进行回环检测，可有效降低漂移误差，建立一致性更优化的全局地图，提升了闭环检测精度、召回率及计算效率；

2、本发明方法通过局部子图的建立，以及关键帧的条件选择，进一步优化数据结构与计算效率；

3、本发明方法采用两阶段分级位置再识别策略，包括基于二进制运算的快速几何检索和特征结构匹配，有效提升计算效率。

附图说明

本发明可以通过附图给出的非限定性实施例进一步说明；

图1表示局部子图的原始点云；

图2是基于平面特征点的子地图；

图3是基于边缘特征点的局部子图；

图4是全新EPSC描述子矩阵。

具体实施方式

以下将结合附图和具体实施例对本发明进行详细说明，需要说明的是，在附图或说明书描述中，相似或相同的部分都使用相同的图号，附图中未绘示或描述的实现方式，为所属技术领域中普通技术人员所知的形式。另外，实施例中提到的方向用语，例如“上”、“下”、“顶”、“底”、“左”、“右”、“前”、“后”等，仅是参考附图的方向，并非用于限制本发明的保护范围。

如图1-4所示，一种基于移动机器人的SLAM回环检测方法，包括如下步骤：

S1、对机器人运动产生的激光运动畸变进行激光预处理，并对距离图像中边缘点、平面点特征进行提取；

S2、将连续帧的相对姿态变换获得雷达里程计，并通过关键帧的选择组成局部子图；

S3、采用全新描述子EPSC进行回环检测。

实施例1：激光预处理

S101、利用IMU预积分后的值去除因机器人运动产生的激光运动畸变；

S102、一帧激光点云数据集合记为P＝{p₁，p₂，p₃，...，p_n}，并投影到距离图像上；

S103、在距离图像上选取一行连续的点，其集合记为R，点p_i为中点，通过以下公式可以计算p_i的曲率值：

其中d_i为点p_i到激光雷达的欧式距离，设定一个值c_b，当c大于c_b时为边缘点，当c小于c_b时为平面点。

实施例2：局部子图建立

S201、两个连续帧的相对姿态变换即雷达里程计通过点到平面和点到边缘的扫描匹配方法获得，其中边沿特征点到线的距离计算如下：

平面特征的点到面的计算如下：

其中，和k+1是连续且相关联的两帧激光，i，j，l和m是两帧激光中的点；

S202、用LM非线性优化方法得到连续两帧的位姿变换即雷达里程计；

S203、选取关键帧，设置一个距离值l和角度值θ，当某一帧的距离比上一个关键帧大于l或者角度偏移大于θ时，将被选择为关键帧；

S204、关键帧组成局部子图，当以下条件满足其一时，一个局部子图被建立：现有子图关键帧数量超过设定值n；创建子图的时间超过设定值t；角度变化过于剧烈，超过设定值ω。

实施例3：全新描述子，Edge-Planar Scan Context(EPSC)建立

S301、提取特征点，设一个局部子图所有的点云集合为S＝{s₁，s₂，…，s_n}，且s_k＝(x_k，y_k，z_k)，S_e＝{s_e1，s_e2，…，s_em}和S_p＝{s_p1，s_p2，…，s_p3}分别代表子图中边缘特征点和平面特征点的集合；

S302、建立传感器坐标系，采用3D扫描的点云俯视图，并将地面区域划分为多个子空间；

S303、局部子图根据方位角和径向方向进行分割，并以不同颜色区分为若干环、扇区、子空间，点云中的每个点s_k都可用极坐标来表示：

s_k＝[ρ_k，θ_k，z_k]

其中，方位角具体为：在激光雷达帧中从0到2π，径向具体为：从中心到激光能打到的最大距离；

S304、通过在方位角和径向方向上等分极坐标，将点云分割成N_s个扇区和N_r个环，整个局部子图被划分为N_r×N_s个子空间，把第i个扇区和第j个环中的边缘特征数量和平面特征数量分别记作n_eij和n_pij；

S305、将局部子图映射到矩阵上，方位角映射为矩阵的列，径向映射为矩阵的行，表征边缘特征局部子图的矩阵表示为Ω_e，且Ω_e(i，j)＝n_eij；表征平面特征局部子图的矩阵表示为Q_p，且Ω_p(i，j)＝n_pij；若矩阵元素为空，则把该元素记为0；

S306、将两个矩阵的对应元素合并成新矩阵的对应位置元素，得到新的矩阵记作Ω_ep即获得当前局部子图的EPSC；

其中：

实施例4：EPSC的具体建立

首先用ring(环)和sector(扇形)把局部子图划分，环上的方位角从0到2π，这里均匀划分20等份，扇形的径向范围从中心点到激光能打到的最大距离，均匀划分为60等份。利用局部子图的俯视图把地面区域划为多个子空间。把方位角映射为列，扇形径向映射为行，即可把传感器坐标系映射为一个矩阵，以方便计算，把矩阵分别记为Ω_e(边缘特征矩阵)和Ω_p(平面特征矩阵)。统计每个子空间的边缘特征点数和平面特征点数(区分边缘特征和平面特征的方法和激光预处理的方法一致)，分别记为n_eij和n_pij，与Ω_e(i，j)和Ω_p(i，j)对应，这样就可以把局部子图中的特征用矩阵表示。但是用两个矩阵表示一个子图的特征显得过于冗余，因此我们融合两个矩阵得到一个新矩阵Ω_ep来表示，

其中，a可以为任意常数，取a＝100。得到矩阵即ESSC，元素为空的位置我们设为0。

实施例5：回环检测完成

位置重新识别，将当前位置P_n与数据库中的历史位置D＝{P₁，P₂，...，P_n-1}进行比对，确认机器人是否回环，随着数据库D的规模不断增加，采用两阶段分级位置再识别策略提高计算效率，包括基于二进制运算的快速几何检索和特征结构匹配进行回环检测。

实施例6：回环检测具体实现一

给定EPSC矩阵Ω_ep，其在局部坐标上的几何分布可以表示为二值矩阵Γ：

对于当前EPSC矩阵候选EPSC矩阵/>及其二进制变换/>可以导出几何相似性：

其中XOR(x，y)是指矩阵x和y的异或运算，|x|是指矩阵x中的元素数量，消除视角带来的误差，最终得分计算如下：

其中是/>侈动i列，通过移动列来获得一个最大的相似度得分，就可以认定为最佳匹配，以纠正激光视角带来的变化，这样几何检索完成。

实施例7：回环检测具体实现二

通过列式比较识别两个EPSC矩阵和/>之间的特征相似性，假设/>和/>分别是/>和/>的第i列，则得分可以通过如下得到：

由于激光视角的影响，假设当移动k列时得分最多，那么最终得分如下计算得到：

通过两部分的得分相加确定总的得分，得分越高，子图越匹配；

当得分大于一个阈值c的时候，即可确定当前位置是机器人之前经过的历史位置，即完成回环检测。

本发明对实现一种基于移动机器人的SLAM回环检测方法进行了详细介绍。具体实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，改进和修饰也落入本发明权利要求的保护范围内。

Claims (5)

1.一种基于移动机器人的SLAM回环检测方法，其特征在于，包括如下步骤：

S1、对机器人运动产生的激光运动畸变进行激光预处理，并对距离图像中边缘点、平面点特征进行提取；

S2、将连续帧的相对姿态变换获得雷达里程计，并通过关键帧的选择组成局部子图；

S3、采用全新描述子EPSC代表局部子图特征进行回环检测；

其中，S2包括如下步骤：

S201、两个连续帧的相对姿态变换即雷达里程计通过点到平面和点到边缘的扫描匹配方法获得，其中边沿特征点到线的距离计算如下：

平面特征的点到面的计算如下：

其中，k和k+1是连续且相关联的两帧激光，i,j,l和m是两帧激光中的点；

S202、用LM非线性优化方法得到连续两帧的位姿变换即雷达里程计；

S203、选取关键帧，设置一个距离值l和角度值θ，当某一帧的距离比上一个关键帧大于l或者角度偏移大于θ时，将被选择为关键帧；

S204、关键帧组成局部子图，当以下条件满足其一时，一个局部子图被建立：现有子图关键帧数量超过设定值n；创建子图的时间超过设定值t；角度变化过于剧烈，超过设定值ω；

S3包括如下步骤：

S301、提取特征点，设一个局部子图所有的点云集合为S＝{s₁，s₂，...，s_n}，且集合中某一点记为s_k＝(x_k,y_k,z_k)，S_e＝{s_e1,s_e2,...,s_em}和S_p＝{s_p1,s_p2,...,s_pn}分别代表子图中边缘特征点和平面特征点的集合；

S302、建立传感器坐标系，采用3D扫描的点云俯视图，并将地面区域划分为多个子空间；

S303、局部子图根据方位角和径向方向进行分割，并以不同颜色区分为若干环、扇区、子空间，点云中的每个点s_k都可用极坐标来表示：

s_k＝[ρ_k，θ_k，z_k]

其中，方位角具体为：在激光雷达帧中从0到2π，径向具体为：从中心到激光能打到的最大距离L_max；

S304、通过在方位角和径向方向上等分极坐标，将点云分割成N_s个扇区和N_r个环，整个局部子图被划分为N_r×N_s个子空间，把第i个扇区和第j个环中的边缘特征数量和平面特征数量分别记作n_eij和n_pij；

S305、将局部子图映射到矩阵上，方位角映射为矩阵的列，径向映射为矩阵的行，表征边缘特征局部子图的矩阵表示为Ω_e，且Ω_e(i,j)＝n_eij；表征平面特征局部子图的矩阵表示为Ω_p，且Ω_p(i,j)＝n_pij；若矩阵元素为空，则把该元素记为0；

S306、将两个矩阵的对应元素合并成新矩阵的对应位置元素，得到新的矩阵记作Ω_ep即获得当前局部子图的EPSC；

其中：

2.根据权利要求1所述的一种基于移动机器人的SLAM回环检测方法，其特征在于，S1包括以下步骤：

S101、利用IMU预积分后的值去除因机器人运动产生的激光运动畸变；

S102、一帧激光点云数据集合记为P＝{p₁，p₂，p₃，...，p_n}，并投影到距离图像上；

S103、在距离图像上选取一行连续的点，其集合记为R，点p_i为中点，通过以下公式可以计算p_i的曲率值：

其中d_i为点p_i到激光雷达的欧式距离，设定一个值c_b，当c大于c_b时为边缘点，当c小于c_b时为平面点。

3.根据权利要求1所述的一种基于移动机器人的SLAM回环检测方法，其特征在于，还包括如下步骤：

位置重新识别，将当前位置P_n与数据库中的历史位置D＝{P₁,P₂,...,P_n-1}进行比对，确认机器人是否回环，随着数据库D的规模不断增加，采用两阶段分级位置再识别策略，包括基于二进制运算的快速几何检索和特征结构匹配进行回环检测。

4.根据权利要求3所述的一种基于移动机器人的SLAM回环检测方法，其特征在于，所述基于二进制运算的快速几何检索的具体步骤如下：

给定EPSC矩阵Ω_ep，其在局部坐标上的几何分布可以表示为二值矩阵Γ：

对于当前EPSC矩阵候选EPSC矩阵/>及其二进制变换/>可以导出几何相似性：

其中XOR(x,y)是指矩阵x和y的异或运算，|x|是指矩阵x中的元素数量，消除视角带来的误差，最终得分计算如下：

其中是/>移动i列，通过移动列来获得一个最大的相似度得分，就可以认定为最佳匹配，以纠正激光视角带来的变化几何检索完成。

5.根据权利要求3所述的一种基于移动机器人的SLAM回环检测方法，其特征在于，所述特征结构匹配进行回环检测的具体步骤如下：

通过列式比较识别两个EPSC矩阵和/>之间的特征相似性，假设/>和/>分别是/>和/>的第i列，则得分可以通过如下得到：

由于激光视角的影响，假设当移动k列时得分最多，那么最终得分如下计算得到：

通过两部分的得分相加确定总的得分，得分越高，子图越匹配；

当得分大于一个阈值c的时候，即可确定当前位置是机器人之前经过的历史位置，即完成回环检测。