CN112907491A - 一种适用于井下巷道的激光点云回环检测方法及系统 - Google Patents

Abstract

本发明提供了一种适用于井下巷道的激光点云回环检测方法及系统，包括：步骤S1：使用激光里程计算法得到定位系统的三维位姿信息；步骤S2：使用基于视点的特征直方图对点云关键帧进行描述；步骤S3：使用向量的相关性计算公式计算关键帧点云相似度；步骤S4：当相似度高于预设阈值时，则计算历史关键帧和当前关键帧的时间差，当大于预设阈值时，则执行步骤S5，否则结束当前回环检测，等待新的关键帧信息输入，重复执行步骤S1至步骤S4；步骤S5：根据点云的曲率信息计算关键帧点云的边角特征和平面特征，基于边角特征和平面特征的配准计算关键帧的位姿变换；步骤S6：根据位姿变化通过因子图优化方法更新当前的位姿信息和历史的轨迹信息。

Description

一种适用于井下巷道的激光点云回环检测方法及系统

技术领域

本发明涉及激光同步定位和建图(SLAM)领域，具体地，涉及一种适用于井下巷道的激光点云回环检测方法及系统。

背景技术

提高煤矿作业的自动化和智能化是国家煤矿业发展的重点方向。目前，为了保证矿山的生产安全和高效运行，工人们每日需要对井下巷道进行安全巡视检查，及时发现事故隐患和险情，该项工作劳动强度大且危险。针对此种现状，在矿井开采中应用巡检无人机替代人工，可以实时监督井下巷道的状况，对潜在的危险和隐患进行预警，保障工人的作业安全。其中，实现高精度井下环境的同步定位和建图功能是井下无人机巡检进行智能避障和路径规划的基础和要点。

井下活动范围广，巷道环境错综复杂，长时间运行后存在累积误差，需要通过回环检测方法校正累计误差。回环检测方法在定位系统第二次经过某一地点时，通过相似度匹配判断曾抵达过此处，计算相对位姿变换，将位姿信息作为约束添加到整体轨迹计算中，消除由于噪声而不断累积的定位误差。

一般以视觉相机作为传感器，使用ORB、SIFT等特征检测方法对图像进行特征提取，对提取的特征描述子进行聚类和训练，合并相似度高的特征描述子为一个单词，搭建包含多个单词的词袋模型，在实时定位时，使用每个单词在图像中出现的次数对图像进行描述，进而实现回环检测。但是井下环境黑暗，照度低，无纹理性，难以提取图像特征，基于机器视觉的算法具有较强的局限性，实际应用中精度较差。

为提高在井下巷道环境中的定位精度，需要同时保证相似度检测和位姿计算的准确性和传感器对井下环境的适应性。

专利文献CN110645998A(申请号：201910851167.1)公开了一种基于激光点云的无动态物体地图分段建立方法，涉及地图领域，包括以下步骤：1、用激光雷达进行数据采集，获得点云数据；点云数据为多帧数据；2、使用工控机播放采集好的点云数据，离线分段创建无动态物体地图；对点云数据的每一帧进行体素网格降采样后传递给地图创建方法；3、对地图进行初始化，创建无动态物体的初始地图；4、对子地图进行创建并更新，同时去除动态物体；5、对地图进行回环检测，并计算地图的误差；6、优化地图的误差；7、将经过误差优化的地图输出。该专利与本发明相比，本发明防止建图过程中效率过度下降，实现了实时定位和建图，同时进行了回环检测和回环误差的优化，在长时间运行过程中保持稳定，具有全局一致性。并且本申请没有滤除动态物体相关的功能，使用直方图描述一帧，内存占用低；直方图相似度计算，比一般方法利用距离搜索相似帧更为可靠；边角特征、平面特征配准，计算速度更快，在长时间的运行过程中通过回环检测在经过重复位置时进行位姿矫正。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种适用于井下巷道的激光点云回环检测方法及系统。

根据本发明提供的一种适用于井下巷道的激光点云回环检测方法，包括：

步骤S1：定位系统使用激光里程计算法得到定位系统的三维位姿信息；

步骤S2：基于三维位姿信息提取点云关键帧，使用基于视点的特征直方图对点云关键帧进行描述；

步骤S3：基于视点的特征直方图对点云关键帧的描述，使用向量的相关性计算公式计算关键帧点云相似度；

步骤S4：当相似度高于预设阈值时，则计算历史关键帧和当前关键帧的时间差，当时间差大于预设阈值时，则执行步骤S5，否则结束当前回环检测，等待新的关键帧信息输入，重复执行步骤S1至步骤S4；

步骤S5：根据点云的曲率信息计算关键帧点云的边角特征和平面特征，配准边角特征和平面特征，当满足配准要求的边角特征和平面特征数量达到预设值时，则执行步骤S6，否则结束当前回环检测，等待新的关键帧信息输入，重复执行步骤S1至步骤S5；

步骤S6：基于边角特征和平面特征的配准计算关键帧的位姿变换，当位姿变换小于等于预设值时，则执行步骤S7；否则结束当前回环检测，等待新的关键帧信息输入，重复执行步骤S1至步骤S6；

步骤S7：根据计算得到的关键帧的位姿变换，通过因子图优化方法更新当前的位姿信息和历史的轨迹信息，保证位姿和点云地图的全局一致性。

优选地，所述步骤S1包括：

步骤S1.1：通过激光雷达扫描周围环境点云，使用强度滤波和距离筛选方法对原始点云数据进行预处理；

步骤S1.2：对预处理后的相邻帧点云进行配准，计算相对位姿变换，输出里程计位姿信息；

所述里程计位姿信息以世界坐标系下的三维坐标t_{w_l[i]}＝(x_i,y_i,z_i)和姿态旋转的四元数q_{w_l[i]}(q_{w_i},q_{x_i},q_{y_i},q_{z_i})表示。

优选地，所述步骤S2包括：

步骤S2.1：根据里程计位姿信息，每隔预设时间或距离阈值保存当前帧点云为关键帧，并在保存关键帧时将当前关键帧相对雷达坐标系的点云坐标p_l[i,n]转换成世界坐标系下的点云坐标p_{w_l[i,n]}：

p_{w_l[i,n]}＝q_{w_l[i]}×p_l[i,n]+t_{w_l[i]}

步骤S2.2：关键帧点云通过PCA主成分分析方法计算关键帧点云中所有点的法向量n_t；

步骤S2.3：对关键帧点云中所有点的坐标求和并取平均值，计算三维世界坐标系下的点云中心p_s，以三维世界坐标系的原点到点云中心作为视点向量n_s；

步骤S2.4：以点云中心p_s为坐标系原点，视点向量n_s方向为x轴，视点向量n_s与中心点p_s到点云任意一点p_t的叉乘方向为y轴，建立视点坐标系；

步骤S2.5：计算点云中每一点法向量n_t相对于视点坐标系三个坐标轴的角度作为特征分量，将三个坐标轴上每个角度值分为多个区间，统计整幅点云中所有点的角度区间分布情况，构建特征直方图，对点云关键帧进行描述。

优选地，所述步骤S3包括：

步骤S3.1：将视点特征直方图进行归一化，计算特征向量H_i,H_j的相似度；

其中，I表示向量H中个元素的集合；H_i(I)表示向量H_i中任一元素；H_j(I)表示H_j中任一元素；N表示向量H中元素的个数；

表示第k帧的均值；

表示；

表示。

优选地，所述步骤S5包括：

步骤S5.1：选取相似度最高且高于设定阈值的关键帧点云，根据点云的曲率计算特征点，当曲率大于设定阈值时，则点云为边角特征；当曲率小于等于设定阈值时，则点云为平面特征；

步骤S5.2：基于边角特征点云和平面特征点云，使用KD树查找进行最邻近点搜索，查找匹配帧点云中相对于特征距离最近的点p及点p邻近的k个点，对邻近的k个点云进行PCA主成分分析，计算相邻的k个点相对于点p坐标的协方差矩阵，对协方差矩阵进行SVD分解，获得特征值和特征向量；根据特征值的分布情况判断是否为对应的边角特征和平面特征，进行几何关系验证；当满足要求的边角特征和平面特征数量少于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行。

优选地，所述步骤S6包括：

步骤S6.1：以验证后的边角特征和平面特征之间的距离作为优化代价函数，使用列文伯格-马夸尔特算法求解，得到代价函数残差最小的位姿变换；

步骤S6.2：根据得到的代价函数的最小位姿变换对点云进行坐标变换，重新配准边角特征和平面特征，执行步骤S6.1至步骤S6.2，直至残差小于预设值或达到预设的迭代次数，当迭代次数达到预设值，当前残差大于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行。

优选地，所述步骤S7包括：将位姿变换作为回环约束添加至历史轨迹的因子图中，以代价函数的残差作为协方差矩阵的值，通过图优化方法更新全部历史轨迹，实现回环检测功能。

根据本发明提供的一种适用于井下巷道的激光点云回环检测系统，包括：

模块M1：定位系统使用激光里程计算法得到定位系统的三维位姿信息；

模块M2：基于三维位姿信息提取点云关键帧，使用基于视点的特征直方图对点云关键帧进行描述；

模块M3：基于视点的特征直方图对点云关键帧的描述，使用向量的相关性计算公式计算关键帧点云相似度；

模块M4：当相似度高于预设阈值时，则计算历史关键帧和当前关键帧的时间差，当时间差大于预设阈值时，则触发模块M5执行，否则结束当前回环检测，等待新的关键帧信息输入，重复触发模块M1至模块M4执行；

模块M5：根据点云的曲率信息计算关键帧点云的边角特征和平面特征，配准边角特征和平面特征，当满足配准要求的边角特征和平面特征数量达到预设值时，则触发模块M6执行，否则结束当前回环检测，等待新的关键帧信息输入，重复触发模块M1至模块M5执行；

模块M6：基于边角特征和平面特征的配准计算关键帧的位姿变换，当位姿变换小于等于预设值时，则触发模块M7执行；否则结束当前回环检测，等待新的关键帧信息输入，重复触发模块M1至模块M6执行；

模块M7：根据计算得到的关键帧的位姿变换，通过因子图优化方法更新当前的位姿信息和历史的轨迹信息，保证位姿和点云地图的全局一致性。

优选地，所述模块M5包括：

模块M5.1：选取相似度最高且高于设定阈值的关键帧点云，根据点云的曲率计算特征点，当曲率大于设定阈值时，则点云为边角特征；当曲率小于等于设定阈值时，则点云为平面特征；

模块M5.2：基于边角特征点云和平面特征点云，使用KD树查找进行最邻近点搜索，查找匹配帧点云中相对于特征距离最近的点p及点p邻近的k个点，对邻近的k个点云进行PCA主成分分析，计算相邻的k个点相对于点p坐标的协方差矩阵，对协方差矩阵进行SVD分解，获得特征值和特征向量；根据特征值的分布情况判断是否为对应的边角特征和平面特征，进行几何关系验证；当满足要求的边角特征和平面特征数量少于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行。

优选地，所述模块M6包括：

模块M6.1：以验证后的边角特征和平面特征之间的距离作为优化代价函数，使用列文伯格-马夸尔特算法求解，得到代价函数残差最小的位姿变换；

模块M6.2：根据得到的代价函数的最小位姿变换对点云进行坐标变换，重新配准边角特征和平面特征，触发模块M6.1至模块M6.2执行，直至残差小于预设值或达到预设的迭代次数，当迭代次数达到预设值，当前残差大于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行；

所述模块M7包括：将位姿变换作为回环约束添加至历史轨迹的因子图中，以代价函数的残差作为协方差矩阵的值，通过图优化方法更新全部历史轨迹，实现回环检测功能。

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

1、本发明选择激光雷达为传感器，在黑暗、纹理性弱的井下巷道环境中仍能捕捉到足够有效信息，保证定位系统的基础精度；

2、本发明通过对巷道墙壁的几何结构信息提取特征，采用视点特征直方图方法作为点云的全局特征描述子，匹配准确率高，内存占用小；

3、本发明使用边角特征和平面特征配准的方法，减小了算法的计算量，提高了运算效率，实现了实时的回环检测功能，维持了良好的全局一致性；

4、本发明的技术关键点在于提出了一种适用于井下巷道的激光点云回环检测方法，以点云全局描述子进行点云相似度指标的计算，并通过边角特征、平面特征和因子图优化方法计算和更新定位信息，计算实时性高，可以有效地消除定位系统的累积误差。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明提出的适用于井下巷道的激光点云回环检测方法的算法流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例1

针对现有技术的不足，本发明提出一种以激光雷达作为传感器的回环检测系统，以巷道墙壁的几何特征对点云进行描述，使用边角特征和平面特征配准计算位姿变换，提高了定位系统的精度和计算效率。

本发明涉及一种适用于煤矿井下巷道环境的激光里程计回环检测方法。该方法使用点云的视点特征直方图检测点云的相似度，具有旋转不变性、计算效率高和特征区分性好的特点，在环境特征不明显的井下巷道可以实现实时的相似场景检测功能；通过计算点云的曲率，确定边角特征和平面特征，进行相似帧的边角特征和平面特征配准，计算相似帧之间的相对位姿变换；更新位姿信息，采用因子图优化的方法，将回环检测过程中计算得到的三维坐标和旋转信息作为约束添加到因子图中，对全部历史轨迹进行优化。这种回环检测方法使得系统能够适应特征稀疏的巷道环境，在定位过程中进行有效的回环检测，消除里程计的累积误差。

根据本发明提供的一种适用于井下巷道的激光点云回环检测方法，如图1所示，包括：

步骤S1：定位系统使用激光里程计算法得到定位系统的三维位姿信息；

具体地，所述步骤S1包括：

步骤S1.1：通过激光雷达扫描周围环境点云，使用强度滤波和距离筛选方法对原始点云数据进行预处理；

步骤S1.2：对预处理后的相邻帧点云进行配准，计算相对位姿变换，输出里程计位姿信息；

所述里程计位姿信息以世界坐标系下的三维坐标t_{w_l[i]}＝(x_i,y_i,z_i)和姿态旋转的四元数q_{w_l[i]}(q_{w_i},q_{x_i},q_{y_i},q_{z_i})表示。

步骤S2：基于三维位姿信息提取点云关键帧，使用基于视点的特征直方图对点云关键帧进行描述；

具体地，所述步骤S2包括：

步骤S2.1：根据里程计位姿信息，每隔预设时间或距离阈值保存当前帧点云为关键帧，并在保存关键帧时将当前关键帧相对雷达坐标系的点云坐标p_l[i,n]转换成世界坐标系下的点云坐标p_{w_l[i,n]}：

p_{w_l[i,n]}＝q_{w_l[i]}×p_l[i,n]+t_{w_l[i]}

步骤S2.2：关键帧点云通过PCA主成分分析方法计算关键帧点云中所有点的法向量n_t；

步骤S2.3：对关键帧点云中所有点的坐标求和并取平均值，计算三维世界坐标系下的点云中心p_s，以三维世界坐标系的原点到点云中心作为视点向量n_s；

步骤S2.4：以点云中心p_s为坐标系原点，视点向量n_s方向为x轴，视点向量n_s与中心点p_s到点云任意一点p_t的叉乘方向为y轴，建立视点坐标系；

步骤S2.5：计算点云中每一点法向量n_t相对于视点坐标系三个坐标轴的角度作为特征分量，将三个坐标轴上每个角度值分为多个区间，统计整幅点云中所有点的角度区间分布情况，构建特征直方图，对点云关键帧进行描述。

步骤S3：基于视点的特征直方图对点云关键帧的描述，使用向量的相关性计算公式计算关键帧点云相似度；

具体地，所述步骤S3包括：

步骤S3.1：将视点特征直方图进行归一化，计算特征向量H_i,H_j的相似度；

其中，I表示向量H中个元素的集合；H_i(I)表示向量Hi中任一元素；H_j(I)表示H_j中任一元素；N表示向量H中元素的个数；

表示第k帧的均值；

表示第i帧均值；

表示第j帧均值。

步骤S4：当相似度高于预设阈值时，则计算历史关键帧和当前关键帧的时间差，当时间差大于预设阈值时，则执行步骤S5，否则结束当前回环检测，等待新的关键帧信息输入，重复执行步骤S1至步骤S4；

步骤S5：根据点云的曲率信息计算关键帧点云的边角特征和平面特征，配准边角特征和平面特征，当满足配准要求的边角特征和平面特征数量达到预设值时，则执行步骤S6，否则结束当前回环检测，等待新的关键帧信息输入，重复执行步骤S1至步骤S5；

具体地，所述步骤S5包括：

步骤S5.1：选取相似度最高且高于设定阈值的关键帧点云，根据点云的曲率计算特征点，当曲率大于设定阈值时，则点云为边角特征；当曲率小于等于设定阈值时，则点云为平面特征；

步骤S5.2：基于边角特征点云和平面特征点云，使用KD树查找进行最邻近点搜索，查找匹配帧点云中相对于特征距离最近的点p及点p邻近的k个点，对邻近的k个点云进行PCA主成分分析，计算相邻的k个点相对于点p坐标的协方差矩阵，对协方差矩阵进行SVD分解，获得特征值和特征向量；根据特征值的分布情况判断是否为对应的边角特征和平面特征，进行几何关系验证；当满足要求的边角特征和平面特征数量少于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行。

具体地，几何关系验证包括：

实际判断条件如下，λ₁>λ₂>λ₃为协方差矩阵的三个特征值。

步骤S6：基于边角特征和平面特征的配准计算关键帧的位姿变换，当位姿变换小于等于预设值时，则执行步骤S7；否则结束当前回环检测，等待新的关键帧信息输入，重复执行步骤S1至步骤S6；

具体地，所述步骤S6包括：

步骤S6.1：以验证后的边角特征和平面特征之间的距离作为优化代价函数，使用列文伯格-马夸尔特算法求解，得到代价函数残差最小的位姿变换；

步骤S6.2：根据得到的代价函数的最小位姿变换对点云进行坐标变换，重新配准边角特征和平面特征，执行步骤S6.1至步骤S6.2，直至残差小于预设值或达到预设的迭代次数，当迭代次数达到预设值，当前残差大于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行。

步骤S7：根据计算得到的关键帧的位姿变换，通过因子图优化方法更新当前的位姿信息和历史的轨迹信息，保证位姿和点云地图的全局一致性。

具体地，所述步骤S7包括：将位姿变换作为回环约束添加至历史轨迹的因子图中，以代价函数的残差作为协方差矩阵的值，通过图优化方法更新全部历史轨迹，实现回环检测功能，在重复经过同一位置时消除累积误差，达到更高的精度。

根据本发明提供的一种适用于井下巷道的激光点云回环检测系统，包括：

模块M1：定位系统使用激光里程计算法得到定位系统的三维位姿信息；

具体地，所述模块M1包括：

模块M1.1：通过激光雷达扫描周围环境点云，使用强度滤波和距离筛选方法对原始点云数据进行预处理；

模块M1.2：对预处理后的相邻帧点云进行配准，计算相对位姿变换，输出里程计位姿信息；

所述里程计位姿信息以世界坐标系下的三维坐标t_{w_l[i]}＝(x_i,y_i,z_i)和姿态旋转的四元数q_{w_l[i]}(q_{w_i},q_{x_i},q_{y_i},q_{z_i})表示。

模块M2：基于三维位姿信息提取点云关键帧，使用基于视点的特征直方图对点云关键帧进行描述；

具体地，所述模块M2包括：

模块M2.1：根据里程计位姿信息，每隔预设时间或距离阈值保存当前帧点云为关键帧，并在保存关键帧时将当前关键帧相对雷达坐标系的点云坐标p_l[i,n]转换成世界坐标系下的点云坐标p_{w_l[i,n]}：

p_{w_l[i,n]}＝q_{w_l[i]}×p_l[i,n]+t_{w_l[i]}

模块M2.2：关键帧点云通过PCA主成分分析方法计算关键帧点云中所有点的法向量n_t；

模块M2.3：对关键帧点云中所有点的坐标求和并取平均值，计算三维世界坐标系下的点云中心p_s，以三维世界坐标系的原点到点云中心作为视点向量n_s；

模块M2.4：以点云中心p_s为坐标系原点，视点向量n_s方向为x轴，视点向量n_s与中心点p_s到点云任意一点p_t的叉乘方向为y轴，建立视点坐标系；

模块M2.5：计算点云中每一点法向量n_t相对于视点坐标系三个坐标轴的角度作为特征分量，将三个坐标轴上每个角度值分为多个区间，统计整幅点云中所有点的角度区间分布情况，构建特征直方图，对点云关键帧进行描述。

模块M3：基于视点的特征直方图对点云关键帧的描述，使用向量的相关性计算公式计算关键帧点云相似度；

具体地，所述模块M3包括：

模块M3.1：将视点特征直方图进行归一化，计算特征向量H_i,H_j的相似度；

其中，I表示向量H中个元素的集合；H_i(I)表示向量Hi中任一元素；H_j(I)表示H_j中任一元素；N表示向量H中元素的个数；

表示第k帧的均值；

表示第i帧均值；

表示第j帧均值。

模块M4：当相似度高于预设阈值时，则计算历史关键帧和当前关键帧的时间差，当时间差大于预设阈值时，则触发模块M5执行，否则结束当前回环检测，等待新的关键帧信息输入，重复触发模块M1至模块M4执行；

模块M5：根据点云的曲率信息计算关键帧点云的边角特征和平面特征，配准边角特征和平面特征，当满足配准要求的边角特征和平面特征数量达到预设值时，则触发模块M6执行，否则结束当前回环检测，等待新的关键帧信息输入，重复触发模块M1至模块M5执行；

具体地，所述模块M5包括：

模块M5.1：选取相似度最高且高于设定阈值的关键帧点云，根据点云的曲率计算特征点，当曲率大于设定阈值时，则点云为边角特征；当曲率小于等于设定阈值时，则点云为平面特征；

模块M5.2：基于边角特征点云和平面特征点云，使用KD树查找进行最邻近点搜索，查找匹配帧点云中相对于特征距离最近的点p及点p邻近的k个点，对邻近的k个点云进行PCA主成分分析，计算相邻的k个点相对于点p坐标的协方差矩阵，对协方差矩阵进行SVD分解，获得特征值和特征向量；根据特征值的分布情况判断是否为对应的边角特征和平面特征，进行几何关系验证；当满足要求的边角特征和平面特征数量少于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行。

具体地，几何关系验证包括：

实际判断条件如下，λ₁>λ₂>λ₃为协方差矩阵的三个特征值。

模块M6：基于边角特征和平面特征的配准计算关键帧的位姿变换，当位姿变换小于等于预设值时，则触发模块M7执行；否则结束当前回环检测，等待新的关键帧信息输入，重复触发模块M1至模块M6执行；

具体地，所述模块M6包括：

模块M6.1：以验证后的边角特征和平面特征之间的距离作为优化代价函数，使用列文伯格-马夸尔特算法求解，得到代价函数残差最小的位姿变换；

模块M6.2：根据得到的代价函数的最小位姿变换对点云进行坐标变换，重新配准边角特征和平面特征，触发模块M6.1至模块M6.2执行，直至残差小于预设值或达到预设的迭代次数，当迭代次数达到预设值，当前残差大于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行。

模块M7：根据计算得到的关键帧的位姿变换，通过因子图优化方法更新当前的位姿信息和历史的轨迹信息，保证位姿和点云地图的全局一致性。

具体地，所述模块M7包括：将位姿变换作为回环约束添加至历史轨迹的因子图中，以代价函数的残差作为协方差矩阵的值，通过图优化方法更新全部历史轨迹，实现回环检测功能，在重复经过同一位置时消除累积误差，达到更高的精度。

实施例2

实施例2是实施例1的优选例

本实施例中，一种适用于井下巷道的激光点云回环检测方法，包括以下步骤：

步骤1：通过激光雷达扫描周围环境点云，使用滤波方法对原始点云数据进行预处理。对处理后的相邻帧点云进行配准，计算相对位姿变换，输出激光里程计信息。

步骤2：按照距离或时间提取点云的关键帧，使用主成分分析方法计算点云中点的法向量。

确定点云的中心和视点坐标系。对关键帧点云求和并取平均，计算三维世界坐标系下的点云中心p_s，以原点到点云中心作为视点向量n_s。以点云中心p_s为坐标系原点，视点向量n_s方向为x轴，n_s与中心点p_s到点云任意一点p_t的叉乘方向为y轴，建立视点坐标系。

以点云中每一点相对于视点坐标系三个坐标轴的角度作为特征分量。将每个角度值平均分为多个区间，统计点云中所有点的角度区间分布情况，构建特征直方图，以特征直方图对点云进行全局描述。

步骤3：以相关性系数公式计算点云的相似度，寻找相似度最高的点云。相似度需高于设定阈值，且和当前时刻存在一定时间差。

步骤4：根据点云的曲率大小，提取边角特征和平面特征，对特征进行配准。对配准后的点云，使用协方差矩阵SVD分解的方法获取特征值，进行几何关系验证。以特征之间的距离作为优化函数，迭代求解，使得代价函数残差最小，解得位姿变换。

步骤5：将步骤4中计算得到的位姿变换作为回环约束添加至历史轨迹的因子图中，以步骤4中代价函数的残差作为协方差矩阵的值，通过图优化方法更新全部历史轨迹。实现回环检测功能，在重复经过同一位置时消除累积误差，达到更高的精度。

本发明的技术关键点在于提出了一种适用于井下巷道的激光点云回环检测方法，以点云全局描述子进行点云相似度指标的计算，并通过边角特征、平面特征和因子图优化方法计算和更新定位信息，计算实时性高，可以有效地消除定位系统的累积误差。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种适用于井下巷道的激光点云回环检测方法，其特征在于，包括：

步骤S1：定位系统使用激光里程计算法得到定位系统的三维位姿信息；

步骤S2：基于三维位姿信息提取点云关键帧，使用基于视点的特征直方图对点云关键帧进行描述；

步骤S3：基于视点的特征直方图对点云关键帧的描述，使用向量的相关性计算公式计算关键帧点云相似度；

步骤S4：当相似度高于预设阈值时，则计算历史关键帧和当前关键帧的时间差，当时间差大于预设阈值时，则执行步骤S5，否则结束当前回环检测，等待新的关键帧信息输入，重复执行步骤S1至步骤S4；

步骤S5：根据点云的曲率信息计算关键帧点云的边角特征和平面特征，配准边角特征和平面特征，当满足配准要求的边角特征和平面特征数量达到预设值时，则执行步骤S6，否则结束当前回环检测，等待新的关键帧信息输入，重复执行步骤S1至步骤S5；

步骤S6：基于边角特征和平面特征的配准计算关键帧的位姿变换，当位姿变换小于等于预设值时，则执行步骤S7；否则结束当前回环检测，等待新的关键帧信息输入，重复执行步骤S1至步骤S6；

步骤S7：根据计算得到的关键帧的位姿变换，通过因子图优化方法更新当前的位姿信息和历史的轨迹信息，保证位姿和点云地图的全局一致性。

2.根据权利要求1所述的适用于井下巷道的激光点云回环检测方法，其特征在于，所述步骤S1包括：

步骤S1.1：通过激光雷达扫描周围环境点云，使用强度滤波和距离筛选方法对原始点云数据进行预处理；

步骤S1.2：对预处理后的相邻帧点云进行配准，计算相对位姿变换，输出里程计位姿信息；

所述里程计位姿信息以世界坐标系下的三维坐标t_{w_l[i]}＝(x_i,y_i,z_i)和姿态旋转的四元数q_{w_l[i]}(q_{w_i},q_{x_i},q_{y_i},q_{z_i})表示。

3.根据权利要求1所述的适用于井下巷道的激光点云回环检测方法，其特征在于，所述步骤S2包括：

步骤S2.1：根据里程计位姿信息，每隔预设时间或距离阈值保存当前帧点云为关键帧，并在保存关键帧时将当前关键帧相对雷达坐标系的点云坐标p_l[i,n]转换成世界坐标系下的点云坐标p_{w_l[i,n]}：

p_{w_l[i,n]}＝q_{w_l[i]}×p_l[i,n]+t_{w_l[i]}

步骤S2.2：关键帧点云通过PCA主成分分析方法计算关键帧点云中所有点的法向量n_t；

步骤S2.3：对关键帧点云中所有点的坐标求和并取平均值，计算三维世界坐标系下的点云中心p_s，以三维世界坐标系的原点到点云中心作为视点向量n_s；

步骤S2.4：以点云中心p_s为坐标系原点，视点向量n_s方向为x轴，视点向量n_s与中心点p_s到点云任意一点p_t的叉乘方向为y轴，建立视点坐标系；

步骤S2.5：计算点云中每一点法向量n_t相对于视点坐标系三个坐标轴的角度作为特征分量，将三个坐标轴上每个角度值分为多个区间，统计整幅点云中所有点的角度区间分布情况，构建特征直方图，对点云关键帧进行描述。

4.根据权利要求1所述的适用于井下巷道的激光点云回环检测方法，其特征在于，所述步骤S3包括：

步骤S3.1：将视点特征直方图进行归一化，计算特征向量H_i,H_j的相似度；

其中，I表示向量H中个元素的集合；H_i(I)表示向量H_i中任一元素；H_j(I)表示H_j中任一元素；N表示向量H中元素的个数；

表示第k帧的均值；

表示；

表示。

5.根据权利要求1所述的适用于井下巷道的激光点云回环检测方法，其特征在于，所述步骤S5包括：

步骤S5.1：选取相似度最高且高于设定阈值的关键帧点云，根据点云的曲率计算特征点，当曲率大于设定阈值时，则点云为边角特征；当曲率小于等于设定阈值时，则点云为平面特征；

步骤S5.2：基于边角特征点云和平面特征点云，使用KD树查找进行最邻近点搜索，查找匹配帧点云中相对于特征距离最近的点p及点p邻近的k个点，对邻近的k个点云进行PCA主成分分析，计算相邻的k个点相对于点p坐标的协方差矩阵，对协方差矩阵进行SVD分解，获得特征值和特征向量；根据特征值的分布情况判断是否为对应的边角特征和平面特征，进行几何关系验证；当满足要求的边角特征和平面特征数量少于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行。

6.根据权利要求1所述的适用于井下巷道的激光点云回环检测方法，其特征在于，所述步骤S6包括：

步骤S6.1：以验证后的边角特征和平面特征之间的距离作为优化代价函数，使用列文伯格-马夸尔特算法求解，得到代价函数残差最小的位姿变换；

步骤S6.2：根据得到的代价函数的最小位姿变换对点云进行坐标变换，重新配准边角特征和平面特征，执行步骤S6.1至步骤S6.2，直至残差小于预设值或达到预设的迭代次数，当迭代次数达到预设值，当前残差大于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行。

7.根据权利要求1所述的适用于井下巷道的激光点云回环检测方法，其特征在于，所述步骤S7包括：将位姿变换作为回环约束添加至历史轨迹的因子图中，以代价函数的残差作为协方差矩阵的值，通过图优化方法更新全部历史轨迹，实现回环检测功能。

8.一种适用于井下巷道的激光点云回环检测系统，其特征在于，包括：

模块M1：定位系统使用激光里程计算法得到定位系统的三维位姿信息；

模块M2：基于三维位姿信息提取点云关键帧，使用基于视点的特征直方图对点云关键帧进行描述；

模块M3：基于视点的特征直方图对点云关键帧的描述，使用向量的相关性计算公式计算关键帧点云相似度；

模块M4：当相似度高于预设阈值时，则计算历史关键帧和当前关键帧的时间差，当时间差大于预设阈值时，则触发模块M5执行，否则结束当前回环检测，等待新的关键帧信息输入，重复触发模块M1至模块M4执行；

模块M5：根据点云的曲率信息计算关键帧点云的边角特征和平面特征，配准边角特征和平面特征，当满足配准要求的边角特征和平面特征数量达到预设值时，则触发模块M6执行，否则结束当前回环检测，等待新的关键帧信息输入，重复触发模块M1至模块M5执行；

模块M6：基于边角特征和平面特征的配准计算关键帧的位姿变换，当位姿变换小于等于预设值时，则触发模块M7执行；否则结束当前回环检测，等待新的关键帧信息输入，重复触发模块M1至模块M6执行；

模块M7：根据计算得到的关键帧的位姿变换，通过因子图优化方法更新当前的位姿信息和历史的轨迹信息，保证位姿和点云地图的全局一致性。

9.根据权利要求8所述的适用于井下巷道的激光点云回环检测系统，其特征在于，所述模块M5包括：

模块M5.1：选取相似度最高且高于设定阈值的关键帧点云，根据点云的曲率计算特征点，当曲率大于设定阈值时，则点云为边角特征；当曲率小于等于设定阈值时，则点云为平面特征；

模块M5.2：基于边角特征点云和平面特征点云，使用KD树查找进行最邻近点搜索，查找匹配帧点云中相对于特征距离最近的点p及点p邻近的k个点，对邻近的k个点云进行PCA主成分分析，计算相邻的k个点相对于点p坐标的协方差矩阵，对协方差矩阵进行SVD分解，获得特征值和特征向量；根据特征值的分布情况判断是否为对应的边角特征和平面特征，进行几何关系验证；当满足要求的边角特征和平面特征数量少于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行。

10.根据权利要求8所述的适用于井下巷道的激光点云回环检测系统，其特征在于，所述模块M6包括：

模块M6.1：以验证后的边角特征和平面特征之间的距离作为优化代价函数，使用列文伯格-马夸尔特算法求解，得到代价函数残差最小的位姿变换；

模块M6.2：根据得到的代价函数的最小位姿变换对点云进行坐标变换，重新配准边角特征和平面特征，触发模块M6.1至模块M6.2执行，直至残差小于预设值或达到预设的迭代次数，当迭代次数达到预设值，当前残差大于预设值时，则结束当前回环检测，等待新的关键帧信息输入重新执行；

所述模块M7包括：将位姿变换作为回环约束添加至历史轨迹的因子图中，以代价函数的残差作为协方差矩阵的值，通过图优化方法更新全部历史轨迹，实现回环检测功能。

Images