CN114280626A - 一种基于局部结构信息扩展的激光雷达slam方法及系统 - Google Patents

Abstract

本发明提供了一种基于局部结构信息扩展的激光雷达SLAM方法，步骤如下：获取雷达点云数据；计算点云数据各点的曲率信息及相对反射强度信息提取特征点；1.通过点到直线和点到平面的配准方式对相邻的两帧特征点进行配准；2.筛选大于预设阈值的特征点，得到局部的结构化点云信息，采用正态分布变换法NDT对局部的结构化点云信息进行配准；通过对相邻两帧特征点的配准和拟合的正态分布配准，得到机器人每帧的位姿，同时将每帧的点云信息与地图点云进行配准建造增量式地图。本方法通过反射强度信息提取了额外的特征并且在特征点附近提取局部的结构化点云信息，通过正态分布的方式进行配准，仅增加了少量计算量，便使得方法更加的精准和鲁棒。

Description

一种基于局部结构信息扩展的激光雷达SLAM方法及系统

技术领域

本发明涉及计算机视觉技术领域，具体而言，涉及一种基于局部结构信息扩展的激光雷达SLAM方法及系统。

背景技术

在SLAM中，最常用的传感器是激光雷达(LiDAR)，激光雷达可以直接获取周围的距离信息，与视觉传感器相比，激光雷达几乎不受天气等因素的影响，非常地鲁棒。此外，激光雷达传感器的价格已经在过去十年中不断下降。因此，基于LiDAR的SLAM方法一直是一个热门的研究课题，吸引了很多的研究人员。

基于激光雷达的SLAM系统通常分为基于点的配准(Iterative Closest Point，ICP)、基于分布的配准(Normal Distribution Transform，NDT)和基于特征的配准。

基于点的配准方法和基于分布的配准方法在计算量都很大，因此难以应用于实时的SLAM中，而基于特征的点云配准方法，由于其轻量级特征提取和匹配过程，更有潜力用于实时地图构建；但是，当我们使用基于特征配准方法在非结构化场景中进行配准时，提取的特征太稀疏，使得配准结果不准确，从而影响构建地图的质量。

发明内容

本发明的目的在于提供一种基于局部结构信息扩展的激光雷达SLAM方法及系统，其通过额外提取反射强度特征和局部结构化信息，使得基于激光雷达的SLAM方法更加的精准和鲁棒。

本发明的实施例通过以下技术方案实现：一种基于局部结构信息扩展的激光雷达SLAM方法，包括如下步骤：

获取雷达传感器采集的点云数据；

针对每一帧点云数据，通过计算其每个点的曲率信息以及相对反射强度信息提取特征点；

通过点到直线和点到平面的配准方式对相邻的两帧特征点进行配准；

以大于预设阈值为条件进行特征点筛选，得到局部的结构化点云信息，采用正态分布变换法NDT对所述局部的结构化点云信息进行配准；

通过对相邻两帧特征点中拟合的正态分布进行配准，得到机器人每帧的位姿，同时将每帧局部的结构化点云信息进行配准建造增量式地图。

进一步地，所述曲率信息的计算公式表示为：

上式中，Δ_ci表示曲率，P_i表示待求目标点，P_j表示P_i相邻点，S表示以P_i为中心，同一圈周围点的点集；

所述相对反射强度信息的计算公式表示为：

上式中，Δ_Ii表示相对反射强度，I_i表示点i的反射强度，I_j表示点j的反射强度。

进一步地，所述提取特征点包括根据曲率信息提取角点特征和平面特征。

进一步地，所述通过点到直线和点到平面的配准方式对相邻的两帧特征点进行配准具体包括：

若P_i为角点上的边缘点或强度点，则通过点到直线的配准方式对相邻的两帧特征点进行配准，公式表示为：

若P_i为平面点，则通过点到平面的配准方式对相邻的两帧特征点进行配准，公式表示为：

上式中，

均为损失函数，P_i′表示当前帧转换到上一帧的点，P_j1,P_j2,P_j3为上一帧的点。

进一步地，所述采用正态分布变换法NDT对所述局部的结构化点云信息进行配准包括：

通过如下公式进行正态分布的拟合，公式表示为：

上式中，μ_i表示正态分布的均值，V_i表示筛选所得特征点周围的原始点云，∑_i表示正态分布的协方差。

进一步地，所述对相邻两帧特征点中拟合的正态分布进行配准，包括：通过分布式NDT进行无损检测，表达式为：

上式中，f_d2d表示损失函数值，R为旋转矩阵，P_k为某帧点云数据，P_k-1为P_k相邻帧点云数据，d₁和d₂为正则化因子。

进一步地，所述方法还包括：构建每一帧贝叶斯因子图，通过因子图优化的方式对机器人的位姿序列进行优化。

进一步地，所述同时将每帧局部的结构化点云信息进行配准建造增量式地图还包括：通过体素滤波器对每帧局部的结构化点云信息进行滤波处理。

进一步地，所述方法还包括：

对关键帧进行回环优化：分别计算三欧式距离和两帧之间的反射强度相似度，当所得值小于预设阈值时，通过ICP配准方法计算ICP的误差得分，当误差得分小于预设阈值时，将认定的回环边转化为因子图，通过因子图优化的方式进行优化。

本发明还提供一种基于局部结构信息扩展的激光雷达SLAM系统，应用到如上述所述的方法，包括：

输入模块，用于获取雷达传感器采集的点云数据；

特征提取模块，用于针对每一帧点云数据，通过计算其每个点的曲率信息以及相对反射强度信息提取特征点；

配准模块，用于通过点到直线和点到平面的配准方式对相邻的两帧特征点进行配准；

局部结构化信息提取和配准模块，用于以大于预设阈值为条件进行特征点筛选，得到局部的结构化点云信息，采用正态分布变换法NDT对所述局部的结构化点云信息进行配准；

地图构建模块，用于通过对相邻两帧特征点中拟合的正态分布进行配准，得到机器人每帧的位姿，同时将每帧局部的结构化点云信息进行配准建造增量式地图。

本发明实施例的技术方案至少具有如下优点和有益效果：本方法通过提取反射强度特征，因此在现实的场景中，尤其是在非结构化场景中，本方法比其他SLAM算法配准的结构会更精确；此外，本方法在一些具有代表性的特征点附近，提取了局部的结构化点云信息，通过正态分布的方式进行配准，使得本方法仅仅增加了少量计算量，便使得SLAM系统更加的鲁棒和精准，综上，本方法通过提出反射强度特征和局部结构化信息，使得基于激光雷达的SLAM方法更加的精准和鲁棒。

附图说明

图1为本发明实施例1提供的基于局部结构信息扩展的激光雷达SLAM方法流程示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。

实施例1

经申请人研究发现，基于激光雷达的SLAM系统通常分为基于点的配准(IterativeClosest Point，ICP)、基于分布的配准(Normal Distribution Transform，NDT)和基于特征的配准。

基于点的配准方法和基于分布的配准方法在计算量都很大，因此难以应用于实时的SLAM中，而基于特征的点云配准方法，由于其轻量级特征提取和匹配过程，更有潜力用于实时地图构建；但是，当我们使用基于特征配准方法在非结构化场景中进行配准时，提取的特征太稀疏，使得配准结果不准确，从而影响构建地图的质量。

因此，本发明实施例提供一种基于局部结构信息扩展的激光雷达SLAM方法。

参考图1，具体步骤包括：获取雷达传感器采集的点云数据，针对每一帧点云数据，通过计算其每个点的曲率信息以及相对反射强度信息提取特征点，所述提取特征点包括根据曲率信息提取角点特征和平面特征；在本发明实施例的一种实施方式中，给出一帧点云数据P_k，通过如下两个公式计算每一个点P_i的曲率信息Δ_ci和相对反射强度信息Δ_Ii，其中，所述曲率信息的计算公式表示为：

上式中，P_j表示P_i相邻点，S表示以P_i为中心，同一圈周围点的点集，P_i包含于其中；

所述相对反射强度信息的计算公式表示为：

上式中，I_i表示点i的反射强度，I_j表示点j的反射强度。

在计算完每一帧的特征点之后，进一步地对相邻的两帧特征点进行配准，方案具体如下：

若P_i为角点上的边缘点或强度点，则通过点到直线的配准方式对相邻的两帧特征点进行配准，公式表示为：

若P_i为平面点，则通过点到平面的配准方式对相邻的两帧特征点进行配准，公式表示为：

上式中，

均为损失函数，P_i′表示当前帧转换到上一帧的点，P_j1,P_j2,P_j3为上一帧的点。

在对相邻帧的特征点进行配准之后，以大于预设阈值为条件进行特征点筛选，选取具有代表性的特征点(Δ_c大于预设阈值的点)，然后获取其附近的原始点云，例如其附近0.4m立方体内的原始点云，由此得到局部的结构化点云信息；进一步地，采用正态分布变换法NDT对所述局部的结构化点云信息进行配准，具体包括如下：

通过如下公式进行正态分布的拟合，公式表示为：

上式中，μ_i表示正态分布的均值，∑_i表示正态分布的协方差。

然后对相邻两帧特征点中拟合的正态分布进行配准，通过分布式NDT进行无损检测，表达式为：

上式中，f_d2d表示损失函数值，R为旋转矩阵，P_k为某帧点云数据，P_k-1为P_k相邻帧点云数据，d₁和d₂为正则化因子。需要说明的是，D2D-NDT(分布式NDT)过程是NDT的扩展，它对原点云和目标点云进行体素化，并计算原点云与目标点云之间的距离，其用于累计距离误差的方案与GICP相似，即通过协方差进行权重的控制。

在点云配准的过程中，即可得到机器人每帧的位姿，本发明实施例同时将每帧局部的结构化点云信息配准在一起，经过体素滤波器对每帧局部的结构化点云信息进行滤波处理之后建造增量式地图。

考虑到随着SLAM过程的推移，机器人的位姿序列会不可避免地产生累积误差，因此需要优化机器人一段时间后的位姿序列，本实施例所采用的方案如下：在本实施例中，通过构建每一关键帧贝叶斯因子图，然后通过因子图优化的方式进行统一的后端优化，以此实现对机器人位姿序列的优化目的。

此外，考虑到在激光雷达SLAM中，长时间的建图会产生累积误差，进而导致所构建的地图质量下降，为避免该问题的出现，本实施还对关键帧进行回环优化处理。

本实施例回环优化具体方案如下：分别计算三欧式距离和两帧之间的反射强度相似度，当所得值小于预设阈值时，即可得知有潜在的回环；进一步地，通过ICP配准方法计算ICP的误差得分，当误差得分小于预设阈值时，则认定潜在的回环为真正的回环，将回环边转化为因子图加入到后端优化中，通过因子图优化的方式进行优化。

此外，本发明实施例还提供一种基于局部结构信息扩展的激光雷达SLAM系统，应用到如上述所述的方法，包括：

输入模块，用于获取雷达传感器采集的点云数据；

特征提取模块，用于针对每一帧点云数据，通过计算其每个点的曲率信息以及相对反射强度信息提取特征点；

配准模块，用于通过点到直线和点到平面的配准方式对相邻的两帧特征点进行配准；

局部结构化信息提取和配准模块，用于以大于预设阈值为条件进行特征点筛选，得到局部的结构化点云信息，采用正态分布变换法NDT对所述局部的结构化点云信息进行配准；

地图构建模块，用于通过对相邻两帧特征点中拟合的正态分布进行配准，得到机器人每帧的位姿，同时将每帧局部的结构化点云信息进行配准建造增量式地图。

综上所述，本方法通过提取反射强度特征，因此在现实的场景中，尤其是在非结构化场景中，本方法比其他SLAM算法配准的结构会更精确；此外，本方法在一些具有代表性的特征点附近，提取了局部的结构化点云信息，通过正态分布的方式进行配准，使得本方法仅仅增加了少量计算量，便使得SLAM系统更加的鲁棒和精准，综上，本算法通过提出反射强度特征和局部结构化信息，使得基于激光雷达的SLAM方法更加的精准和鲁棒。

以上仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于局部结构信息扩展的激光雷达SLAM方法，其特征在于，包括如下步骤：

获取雷达传感器采集的点云数据；

针对每一帧点云数据，通过计算其每个点的曲率信息以及相对反射强度信息提取特征点；

通过点到直线和点到平面的配准方式对相邻的两帧特征点进行配准；

以大于预设阈值为条件进行特征点筛选，得到局部的结构化点云信息，采用正态分布变换法NDT对所述局部的结构化点云信息进行配准；

通过对相邻两帧特征点中拟合的正态分布进行配准，得到机器人每帧的位姿，同时将每帧局部的结构化点云信息进行配准建造增量式地图。

2.如权利要求1所述的基于局部结构信息扩展的激光雷达SLAM方法，其特征在于，所述曲率信息的计算公式表示为：

上式中，Δ_ci表示曲率，P_i表示待求目标点，P_j表示P_i相邻点，S表示以P_i为中心，同一圈周围点的点集；

所述相对反射强度信息的计算公式表示为：

上式中，Δ_Ii表示相对反射强度，I_i表示点i的反射强度，I_j表示点j的反射强度。

3.如权利要求2所述的基于局部结构信息扩展的激光雷达SLAM方法，其特征在于，所述提取特征点包括根据曲率信息提取角点特征和平面特征。

4.如权利要求3所述的基于局部结构信息扩展的激光雷达SLAM方法，其特征在于，所述通过点到直线和点到平面的配准方式对相邻的两帧特征点进行配准具体包括：

若P_i为角点上的边缘点或强度点，则通过点到直线的配准方式对相邻的两帧特征点进行配准，公式表示为：

若P_i为平面点，则通过点到平面的配准方式对相邻的两帧特征点进行配准，公式表示为：

上式中，

均为损失函数，P′_i表示当前帧转换到上一帧的点，P_j1,P_j2,P_j3为上一帧的点。

5.如权利要求4所述的基于局部结构信息扩展的激光雷达SLAM方法，其特征在于，所述采用正态分布变换法NDT对所述局部的结构化点云信息进行配准包括：

通过如下公式进行正态分布的拟合，公式表示为：

上式中，μ_i表示正态分布的均值，V_i表示筛选所得特征点周围的原始点云，∑_i表示正态分布的协方差。

6.如权利要求5所述的基于局部结构信息扩展的激光雷达SLAM方法，其特征在于，所述对相邻两帧特征点中拟合的正态分布进行配准，包括：

通过分布式NDT进行无损检测，表达式为：

上式中，f_d2d表示损失函数值，R为旋转矩阵，P_k为某帧点云数据，P_k-1为P_k相邻帧点云数据，d₁和d₂为正则化因子。

7.如权利要求6所述的基于局部结构信息扩展的激光雷达SLAM方法，其特征在于，所述方法还包括：构建每一帧贝叶斯因子图，通过因子图优化的方式对机器人的位姿序列进行优化。

8.如权利要求7所述的基于局部结构信息扩展的激光雷达SLAM方法，其特征在于，所述同时将每帧局部的结构化点云信息进行配准建造增量式地图还包括：通过体素滤波器对每帧局部的结构化点云信息进行滤波处理。

9.如权利要求8所述的基于局部结构信息扩展的激光雷达SLAM方法，其特征在于，所述方法还包括：

对关键帧进行回环优化：分别计算三欧式距离和两帧之间的反射强度相似度，当所得值小于预设阈值时，通过ICP配准方法计算ICP的误差得分，当误差得分小于预设阈值时，将认定的回环边转化为因子图，通过因子图优化的方式进行优化。

10.一种基于局部结构信息扩展的激光雷达SLAM系统，应用到如权利要求1至9任一项所述的方法，其特征在于，包括：

输入模块，用于获取雷达传感器采集的点云数据；

特征提取模块，用于针对每一帧点云数据，通过计算其每个点的曲率信息以及相对反射强度信息提取特征点；

配准模块，用于通过点到直线和点到平面的配准方式对相邻的两帧特征点进行配准；

局部结构化信息提取和配准模块，用于以大于预设阈值为条件进行特征点筛选，得到局部的结构化点云信息，采用正态分布变换法NDT对所述局部的结构化点云信息进行配准；

地图构建模块，用于通过对相邻两帧特征点中拟合的正态分布进行配准，得到机器人每帧的位姿，同时将每帧局部的结构化点云信息进行配准建造增量式地图。

Images