CN115661255B - 一种激光slam回环检测与校正方法 - Google Patents

Abstract

本发明涉及SLAM系统中机器人的回环检测领域，公开了一种激光SLAM回环检测与校正方法，可以更加鲁棒地检测回环并更好地解算出回环之间的相对位姿变换；通过利用扫描点云的几何中心作为极坐标系原点建立改进型Scan Context全局描述子，增强了Scan Context方法对平移偏差的鲁棒性；并通过设计基于特征点的Scan Context局部描述子集，使得可以同时通过描述子解算出回环相对位姿的旋转分量和平移分量，为后续的迭代最近点操作提供了更优的位姿初始解，因此可以减小迭代最近点操作陷入局部最优解的概率，提高最终解算出的位姿的准确性。本发明整体方案鲁棒性和适应性强、效率高、可移植性好。

Description

一种激光SLAM回环检测与校正方法

技术领域

本发明涉及SLAM系统中机器人的回环检测领域，具体涉及一种激光SLAM回环检测与校正方法。

背景技术

随着科技的发展，通过机器人进行导航的应用越来越广泛。为了实现导航的目的，在许多机器人应用中都需要实时定位和建图系统（SLAM系统），SLAM系统又以激光SLAM和视觉SLAM为主。回环检测是SLAM系统的一个重要模块，它有助于减小位姿累积误差和构建环境的拓扑结构。在激光SLAM系统中，回环检测的关键问题是如何对三维结构信息进行有效编码，以使得机器人系统能够根据激光雷达的感知信息识别曾经访问过的地点和区分模糊地点。一般来说，现有的方法可以分为两类，包括基于传统描述子的方法和基于深度学习的方法。基于传统描述子的方法旨在将无序的3D点云转换为结构化数据表示。为了实现视点变化鲁棒性（这对于循环闭合很重要），大多数方法将场景信息编码为描述子的过程中丢失了详细的几何结构。这一限制使得描述子在位置识别问题上区分度不够，从而导致潜在的误判。如何将稳定的、有区别的几何结构信息保存到全局描述子中，仍然是传统描述子设计的一个挑战。

现有技术中存在以下技术方案试图解决上述问题：

（1）使用由形函数组成的直方图的串联作为描述子，或者采用双阈值方案在3D点云上使用两个距离函数来生成法向量的直方图作为描述子；但这两种方法均在KITTI数据集上表现不佳。

（2）利用点云的法线和质心方向之间的角度构建直方图；但是这种方法舍弃了点的位置信息，这可能会导致描述子的可区分性不足，且计算法线过程耗时较长。

（3）通过节点匹配和几何验证来执行扫描匹配；然而由于此类方法对于对象和平面的特殊性要求，它们通用性不如基于全局描述子的方法。

此外，还存在一种扫描上下文（Scan Context）方法；扫描上下文（Scan Context）方法在对环境信息进行简化编码的同时有着较强的描述能力，与其他传统的描述子相比，性能有了较大提升，且它对点云密度变化、噪声和超参数具有更强的鲁棒性。另外，ScanContext方法的主要优点之一是具有旋转不变性。但是该方法也存在以下不足：

（1）Scan Context方法对平移敏感。具体来说，因Scan Context方法以激光雷达所在位置为极坐标原点来构建描述子，如果存在回环关系的两个位姿之间存在较大的平移偏移，两者对应的描述子中心位置也会有较大平移偏差，而Scan Context描述子之间的相似性是基于严格的逐列对比来计算的，因此会导致应有的闭环检测失败。Scan Context方法试图通过在周围扩大描述范围来解决该问题，但是该方法的实际效果有限且会消耗过多内存。

（2）因Scan Context被设计为一种全局描述子，但它同样可被用于局部特征描述的潜力并没有被重视和利用，且用于全局描述子的Scan Context方法对于回环相对位姿的估计只能提供旋转分量，而无法提供平移分量的估计。

本发明旨在增强Scan Context方法对平移偏差的鲁棒性，通过改进的ScanContext全局描述来进行鲁棒的回环检测，并在原始Scan Context方法中逐列匹配获得相对位姿的旋转分量估计的基础上，设计基于Scan Context的局部描述子，以通过局部描述子匹配提供相对位姿的平移分量估计。

发明内容

为了改进Scan Context方法存在的对平移的鲁棒性差、只用作全局描述子、只能提供回环相对位姿的旋转分量估计而无法提供平移分量估计的问题，本发明提供一种激光SLAM回环检测与校正方法，可增强原始Scan Context方法对平移偏差的鲁棒性，并进一步设计基于Scan Context局部描述子，以基于局部描述子匹配提供的平移分量进行相对位姿估计。

为解决上述技术问题，本发明具体采用如下技术方案：

一种激光SLAM回环检测与校正方法，包括以下步骤：

步骤一、改进型Scan Context全局描述子构建：

将三维激光雷达当前扫描的激光雷达点云的所有点，投影到x-y平面，并计算激光雷达点云的几何中心：其中，代表激光雷达点云中的一个点，为激光雷达点云中点的数量，分别为点的x坐标、y坐标、z坐标，并以几何中心为原点建立极坐标系；

在极坐标系的基础上对激光雷达点云进行分割和编码，得到改进型ScanContext全局描述子；

步骤二、回环检测：

通过Scan Context方法中的回环检测方法，将当前扫描的激光雷达点云与历史扫描的激光雷达点云进行回环关系检测，检测到的回环帧为；

步骤三、局部描述子构建：

去除当前扫描的激光雷达点云中的地面点，并提取特征点，记为特征点集合，为中的特征点的数量，将去除地面点后的激光雷达点云所有点投影到x-y平面，得到投影点；设定一个距离阈值r，将激光雷达点云对应的投影点中与特征点的平面距离小于r的所有的投影点，构成的集合记作；以特征点为极坐标原点，通过点集构建特征点的局部Scan Context描述子；

特征点完整的局部描述子为：

；

其中和分别为点集中的最高点和最低点，为的中心点；形成当前扫描的激光雷达点云的局部描述子集，为局部描述子集中局部描述子的数量：

；

步骤四、基于局部Scan Context描述子的扫描匹配和相对位姿解算：

对应的回环帧的局部描述子集设为，使用改进型Scan Context全局描述子以及Scan Context方法中的匹配方法对局部描述子集和进行逐列匹配，获得初始的列对齐结果，对应的列位移值记作；对于局部描述子，通过中心点在全局Scan Context描述子矩阵中获得局部描述子对应的行索引和列索引；然后将局部描述子的匹配范围限定在：行索引和列索引满足条件一的局部描述子；，为中局部描述子的数量；其中条件一为：

；

其中Δ为固定值，表示搜索的范围；

计算局部描述子和满足条件一的局部描述子的ScanContext距离，取Scan Context距离最小值对应的局部描述子与局部描述子建立对应关系，进而获得具有对应关系的和之间的最高点的点对匹配关系和最低点的点对匹配关系；基于多组点对匹配关系、，使用奇异值分解计算当前扫描的激光雷达点云和回环帧之间的全局变换，即获得相对位姿的初始解；在相对位姿的初始解的基础上将相对位姿精确化，以获得回环相对位姿的最终结果，实现回环检测的校正。

进一步地，步骤一中，在极坐标系的基础上对激光雷达点云进行分割和编码得到改进型Scan Context全局描述子时：通过个等圆心角间隔的径向线和个同心圆将x-y平面分为*个小格子，z轴方向上不做分割，进而对每个小格子进行编码，将激光雷达点云投影在每个小格子中的点的集合记作，则点集的编码为：

；

其中指点集中点的z坐标值，为按圆心角排列的索引号，，为按同心圆排列的索引号，；

激光雷达点云所有点集的编码形成的大小的矩阵，即为所述的改进型Scan Context全局描述子。

进一步地，步骤三中，通过点集构建特征点的局部Scan Context描述子时，如果某一特征点被计算了局部Scan Context描述子，则对与特征点的平面距离小于r的特征点不再计算对应的局部Scan Context描述子；因为这些特征点彼此接近，他们的局部描述子会相似，对有效的特征匹配不利。

进一步地，步骤一中，计算激光雷达点云的几何中心时：

。

进一步地，步骤三中，利用LeGO-LOAM算法去除当前扫描的激光雷达点云中的地面点，并提取特征点。

进一步地，使用迭代最近点方法在相对位姿的初始解的基础上将相对位姿精确化。

与现有技术相比，本发明的有益技术效果是：

本发明可以更加鲁棒地检测回环并更好地解算出回环之间的相对位姿变换。本发明通过利用扫描点云的几何中心作为极坐标系原点建立改进型Scan Context全局描述子，增强了Scan Context方法对平移偏差的鲁棒性；并通过设计基于特征点的Scan Context局部描述子集，使得可以同时通过描述子解算出回环相对位姿的旋转分量和平移分量，为后续的迭代最近点操作提供了更优的位姿初始解，因此可以减小迭代最近点操作陷入局部最优解的概率，提高最终解算出的位姿的准确性。整体方案鲁棒性和适应性强、效率高、可移植性好，且占用计算资源少。

附图说明

图1为本发明中的激光SLAM回环检测与校正方法的流程图；

图2为本发明中点云划分的示意图。

具体实施方式

下面结合附图对本发明的一种优选实施方式作详细的说明。

如图1所示，本发明中的激光SLAM回环检测与校正方法，包括以下步骤：

步骤一、改进型Scan Context全局描述子构建：

将三维激光雷达当前扫描得到的激光雷达点云的所有点，投影到x-y平面；并计算激光雷达点云的几何中心：

；

其中，代表激光雷达点云中的一个点，为激光雷达点云中点的数量，分别为点的的x坐标、y坐标、z坐标，并以几何中心为原点建立极坐标系。

在上述极坐标系的基础上，对当前激光雷达点云进行分割和编码，具体包括：

按照等间隔的圆心角和径向距离将x-y平面分为多个小格子，z轴方向上不做分割，如图2所示。按圆心角分为份，即得到个扇形，按径向距离分为份，即得到个同心圆，被扇形和同心圆分割得到的小格子总数为*。进而对每个小格子进行编码，若将投影在每个小格子中的点的集合记作，为按圆心角排列的索引号，，为按同心圆排列的索引号，，则点集的编码为：

；

其中指点集中点的z坐标值。

全局描述子为激光雷达点云所有点集的编码形成的大小的矩阵。

步骤二、回环检测：

通过Scan Context方法中的回环检测方法，将当前扫描的激光雷达点云与历史扫描的激光雷达点云进行回环关系检测。记当前扫描的激光雷达点云为，记检测到的回环帧为。

步骤三、局部描述子构建：

利用LeGO-LOAM算法去除当前扫描的激光雷达点云中的地面点，并提取特征点，特征点包括边缘点集（edge points）和表面点集（surface points），记为特征点集合。将去除地面点后的激光雷达点云所有点投影到x-y平面，得到投影点；设定一个距离阈值r，将激光雷达点云对应的投影点中与特征点的平面距离小于r的所有的投影点构成的集合记作。通过以特征点为极坐标原点，基于点集构建特征点的局部Scan Context描述子，记作。

另外，若点被计算了局部Scan Context描述子，则与特征点的平面距离小于r的特征点将不再计算这些特征点对应的描述子，因为这些点彼此接近，他们的局部描述子会相似，对有效的特征匹配不利。特征点完整的局部描述子：

；

其中和分别为点集中的最高点和最低点，在随后的扫描匹配过程中可用于提供刚性位姿约束，为的中心点。

通过上述方法，形成当前扫描的激光雷达点云的局部描述子集，为局部描述子集中局部描述子的数量：

。

步骤四、基于局部Scan Context描述子的扫描匹配和相对位姿解算：

当前扫描的激光雷达点云的局部描述子集为，对应的回环帧的局部描述子集设为，使用改进型Scan Context全局描述子以及Scan Context方法中的匹配方法进行逐列匹配获得初始的列对齐结果，对应的列位移值记作。对于局部描述子，根据中心点在的全局Scan Context描述子矩阵中获得对应的行索引、列索引，记作。然后将的匹配范围限定在中行索引、列索引满足以下条件一的局部描述子中；，为中局部描述子的数量；条件一：

；

其中Δ为固定值，表示搜索的范围。若上述索引超出描述子矩阵的范围，则忽略超出的区域。

然后计算和满足条件一的的Scan Context距离，取ScanContext距离最小值对应的与建立对应关系。然后就获得了两者之间的最高点的点对匹配关系和最低点的点对匹配关系。基于点对匹配关系，使用奇异值分解（SVD）计算当前扫描的激光雷达点云和回环帧之间的全局变换，即获得相对位姿的初始解。并在该相对位姿的初始解的基础上使用迭代最近点（ICP）方法将相对位姿精确化，以获得回环相对位姿的最终结果，实现回环检测的校正。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内，不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立技术方案，说明书的这种叙述方式仅仅是为了清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (5)

1.一种激光SLAM回环检测与校正方法，包括以下步骤：

步骤一、改进型Scan Context全局描述子构建：将三维激光雷达当前扫描的激光雷达点云的所有点，投影到x-y平面，并计算激光雷达点云的几何中心；其中，代表激光雷达点云中的一个点，为激光雷达点云中点的数量，分别为点的x坐标、y坐标、z坐标，并以几何中心为原点建立极坐标系；在极坐标系的基础上对激光雷达点云进行分割和编码，得到改进型Scan Context全局描述子；

步骤二、回环检测：通过Scan Context方法中的回环检测方法，将当前扫描的激光雷达点云与历史扫描的激光雷达点云进行回环关系检测，检测到的回环帧为；

步骤三、局部描述子构建：去除当前扫描的激光雷达点云中的地面点，并提取特征点，记为特征点集合，为中的特征点的数量，将去除地面点后的激光雷达点云所有点投影到x-y平面，得到投影点；设定一个距离阈值r，将激光雷达点云对应的投影点中与特征点的平面距离小于r的所有的投影点，构成的集合记作；以特征点为极坐标原点，通过点集构建特征点的局部Scan Context描述子；

特征点完整的局部描述子为：；

其中和分别为点集中的最高点和最低点，为的中心点；形成当前扫描的激光雷达点云的局部描述子集，为局部描述子集中局部描述子的数量：；

步骤四、基于局部Scan Context描述子的扫描匹配和相对位姿解算：对应的回环帧的局部描述子集设为，使用改进型Scan Context全局描述子以及Scan Context方法中的匹配方法对局部描述子集和进行逐列匹配，获得初始的列对齐结果，对应的列位移值记作；对于局部描述子，通过中心点在的全局Scan Context描述子矩阵中获得局部描述子对应的行索引和列索引；然后将局部描述子的匹配范围限定在：行索引和列索引满足条件一的局部描述子；，为中局部描述子的数量；其中条件一为：

；

其中Δ为固定值，表示搜索的范围；计算局部描述子和满足条件一的局部描述子的Scan Context距离，取Scan Context距离最小值对应的局部描述子与局部描述子建立对应关系，进而获得具有对应关系的和之间的最高点的点对匹配关系和最低点的点对匹配关系；基于点对匹配关系、，使用奇异值分解计算当前扫描的激光雷达点云和回环帧之间的全局变换，即获得相对位姿的初始解；在相对位姿的初始解的基础上将相对位姿精确化，以获得回环相对位姿的最终结果，实现回环检测的校正；

步骤一中，在极坐标系的基础上对激光雷达点云进行分割和编码得到改进型ScanContext全局描述子时：通过个等圆心角间隔的径向线和个同心圆将x-y平面分为*个小格子，z轴方向上不做分割，进而对每个小格子进行编码，将激光雷达点云投影在每个小格子中的点的集合记作，则点集的编码为：

；

其中指点集中点的z坐标值，为按圆心角排列的索引号，，为按同心圆排列的索引号，；

激光雷达点云所有点集的编码形成的大小的矩阵，即为所述的改进型Scan Context全局描述子。

2.根据权利要求1所述的激光SLAM回环检测与校正方法，其特征在于，步骤三中，通过点集构建特征点的局部Scan Context描述子时，如果某一特征点被计算了局部Scan Context描述子，则对与特征点的平面距离小于r的特征点不再计算对应的局部Scan Context描述子。

3.根据权利要求1所述的激光SLAM回环检测与校正方法，其特征在于，步骤一中，计算激光雷达点云的几何中心时：

。

4.根据权利要求1所述的激光SLAM回环检测与校正方法，其特征在于，步骤三中，利用LeGO-LOAM算法去除当前扫描的激光雷达点云中的地面点，并提取特征点。

5.根据权利要求1所述的激光SLAM回环检测与校正方法，其特征在于，步骤四中，使用迭代最近点方法在相对位姿的初始解的基础上将相对位姿精确化。

Images