CN111275748A - 动态环境下基于激光雷达的点云配准方法 - Google Patents

Abstract

本发明公开了动态环境下基于激光雷达的点云配准方法，包括如下步骤：（1）读取k时刻每个激光雷达线上的激光点云信息，并将每个激光雷达线上的激光点云进行平均划分，对每一段进行特征点筛选，从而得到k时刻对应的特征点集合；（2）基于先验信息的随机采样一致性方法迭代获取k时刻最优内点集以及转移矩阵；（3）根据最优内点集采用LM方法重新解算转移矩阵；（4）计算k时刻内点比重，根据内点比重计算得到k+1时刻解算模型参数的先验信息；进入k+1时刻，重复（1）‑（4）。在动态环境下，相对于传统ICP匹配模型，通过本发明公开的基于先验信息的配准方法能够在包含大量动态点的环境中有效提高点云配准精度和实时性。

Description

动态环境下基于激光雷达的点云配准方法

技术领域

本发明涉及动态环境下基于激光雷达的点云配准方法，属于机器人自主导航技术领域。

背景技术

同步定位与地图(SLAM，Simultaneous Localization and Mapping)技术是机器人自主导航技术领域中的一大研究热点，也是机器人实际应用中的关键技术。激光雷达不依赖于外界光照条件，是一种主动式探测传感器，且具备高精度的测距信息。激光雷达SLAM方法是机器人SLAM方法中应用最为广泛的方法之一。

目前大多激光雷达SLAM技术都针对于静态环境，即整个SLAM过程中，环境不会发生任何变化，而实际应用环境大多为动态环境，存在着移动物体的影响。机器人SLAM方法中，通常利用激光雷达扫描的点云数据进行匹配来求解位姿，而激光雷达探测到移动物体的扫描点云数据会对直接影响匹配的效果，造成较大的误差，不仅如此，移动物体会直接导致构图精度变差。

发明内容

本发明所要解决的技术问题是：提供动态环境下基于激光雷达的点云配准方法，以解决动态环境下已有的点云配准方法中容易受动态点导致定位与构图效果差的问题，从而提高激光雷达SLAM在动态环境下的点云匹配精度。

本发明为解决上述技术问题采用以下技术方案：

动态环境下基于激光雷达的点云配准方法，包括如下步骤：

步骤1，读取k时刻每个激光雷达线上的激光点云信息，并将每个激光雷达线上的激光点云平均划分为N_b段，对每一段进行特征点筛选，从而得到k时刻对应的特征点集合；

步骤2，基于先验信息的随机采样一致性方法迭代获取k时刻最优内点集以及[k+1,k]时刻之间的转移矩阵；具体过程如下：

步骤21，设置参数，包括最大迭代次数K_s，内点集

内点集内点个数

第l段激光点云B_k,l中的内点集内点个数

内点比重阈值T_better，一致集

一致集内点个数

第l段激光点云B_k,l中的一致集内点个数

一致集评分s_best，最优内点集

最优内点集内点个数

第l段激光点云B_k,l中的最优内点集内点个数

初始化K_s＝∞，

T_better＝60％，

s_best＝0，

步骤22，获取k时刻激光点云先验点集D_k，当k＝0时，每个B_k,l随机选取一个激光点，构成先验点集D₀；当k≠0时，根据k-1时刻计算得到解算模型参数的先验点集D_k；令迭代次数K＝0；

步骤23，在先验点集D_k中随机抽样3个点，记为a、b、c，通过kNN算法在P_k+1中搜索与a最邻近的点a′、与b最邻近的点b′、与c最邻近的点c′，组成3个点对，P_k+1为k+1时刻对应的特征点集合，利用SVD方法计算[k+1,k]时刻之间的转移矩阵T_k+1,k；

步骤24，对于k时刻对应的特征点集合P_k＝{p_k,r}，预测k+1时刻各特征点p_k,r的坐标

通过kNN算法在P_k+1中搜索与p_k,r最邻近的点，其坐标记为x_k+1,r，计算x_k+1,r到

的欧式距离d_k,r，设置距离阈值L₁，若d_k,r＞L₁，则将点p_k,r加入

并更新

步骤25，计算k时刻内点所占比重T_count，当T_count＞T_better时，更新一致集为

否则不更新；

步骤26，计算内点分布的权重协方差矩阵C，以及内点分布评分s，当s＞s_best时，更新s_best＝s，并更新最优内点集为

步骤27，若K＜K_s，则K＝K+1并返回步骤23，否则迭代结束，输出最优内点集

及转移矩阵T_k+1,k；

步骤3，根据最优内点集

采用LM方法重新解算转移矩阵；

步骤4，计算k时刻B_k,l中内点比重，根据B_k,l中内点比重计算得到k+1时刻解算模型参数的先验信息，包括先验点集D_k+1和先验采样概率p_k+1,l；

步骤5，进入k+1时刻，重复步骤1-步骤4。

作为本发明的一种优选方案，所述步骤1的具体过程如下：

步骤11，记s_i为k时刻激光点云信息S_k的第i线激光点云，S_k＝{s_i}，1≤i≤N_a，N_a为激光雷达线数，将s_i平均划分为N_b段，则S_k被划分为N_a×N_b段，记B_k,l为S_k的第l段激光点云，l＝1,2,...,N_a×N_b；

步骤12，计算第i线激光点云中每个激光点的曲率，筛选出B_k,l中曲率最大的2个点和曲率最小的4个点作为B_k,l的特征点，则k时刻对应的特征点集合为P_k＝{p_k,r}，1≤r≤N_k，N_k为特征点总数，N_k＝6×N_a×N_b。

作为本发明的一种优选方案，步骤12所述第i线激光点云中每个激光点的曲率，计算公式为：

其中，c_i,d为第i线激光点云中第d个激光点的曲率，第i线激光点云中第d个激光点的曲率由第d个激光点的前5个点和后5个点的坐标得到，S＝10，x_i,d为第i线激光点云中第d个激光点的坐标，x_i,e为第d个激光点的前5个点和后5个点中任意一个点的坐标。

作为本发明的一种优选方案，步骤24所述预测k+1时刻各特征点p_k,r的坐标，公式为：

其中，

为k+1时刻特征点p_k,r的预测坐标，T_k+1,k(1:3)为转移矩阵T_k+1,k前三列向量，x_k,r为k时刻特征点p_k,r的坐标，R_k+1,k是由罗德里格斯公式定义的一个旋转矩阵：

其中，I表示单位矩阵，θ＝||T_k+1,k(4:6)||，

是ω的反对称矩阵，T_k+1,k(4:6)为转移矩阵T_k+1,k第4到6列向量。

作为本发明的一种优选方案，所述步骤3的具体过程如下：

步骤31，记

为k时刻最优内点集

中的每个点通过kNN算法寻找的对应k+1时刻最邻近点集，获得

与[k+1,k]时刻之间的转移矩阵T_k+1,k之间的几何关系d：

计算d关于T_k+1,k的雅可比矩阵J：

步骤32，通过非线性迭代，最小化d为0，根据以下公式更新T_k+1,k：

T_k+1,k←T_k+1,k-(J^TJ+λdiag(J^TJ))^-1J^Td

其中，λ为由Levenberg-Marquardt方法确定的因子。

作为本发明的一种优选方案，所述步骤4的具体过程如下：

步骤41，将k时刻第l段激光点云B_k,l的内点比率

映射到[δ,1]：

其中，

n_k,l为B_k,l中激光点个数，

为B_k,l中的最优内点集内点个数，0＜δ＜0.5；

步骤42，根据

计算k+1时刻解算模型参数的先验信息：

D_k+1,l＝N_a×N_b×p_k+1,l

其中，p_k+1,l为k+1时刻从B_k+1,l中采样一个点的先验采样概率，N_a×N_b为k时刻激光点云信息S_k被划分的段数，D_k+1,l为k+1时刻第l段激光点云的先验点集，D_k+1为k+1时刻先验点集。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明提供一种动态环境下的激光雷达点云配准方法，解决了动态环境下已有的点云配准方法中容易受动态点导致定位与构图效果差的问题。通过本发明方法，可以提高激光雷达SLAM在动态环境下的点云匹配精度。

2、通过本发明基于先验信息的配准方法能够在包含大量动态点的环境中有效提高点云配准精度和实时性。

附图说明

图1是本发明动态环境下基于激光雷达的点云配准方法的流程示意图。

图2是本发明基于先验信息的随机采样一致性方法迭代获取k时刻最优内点集及转移矩阵的流程示意图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

如图1所示，为本发明动态环境下基于激光雷达的点云配准方法的流程示意图，其具体步骤如下：

步骤1：进行激光雷达点云划分和特征提取，具体方法如下：

步骤11，周期读取k时刻的激光点云信息S_k并进行点云划分：

记s_i为S_k的第i线激光点云，S_k＝{s_i}，1≤i≤N_a，i按照激光雷达线垂直角度从小到大依次递增，N_a为激光雷达线数。将s_i平均分为N_b段，S_k将被划分为N_a×N_b段，记B_k,l为S_k的第l段激光点云，l＝1,2,...,N_a×N_b。

步骤12，检测S_k中每个B_k,l的特征点：

计算每个激光点的曲率c：

其中，x_i,d,x_i,e分别表示k时刻第i线上不同两点的坐标，S选取10，表示该激光点位于同一激光线根据时间戳顺序前后各5个点。筛选出B_k,l曲率最大的2个点(作为角特征点)和曲率最小的4个点(作为平面特征点)为特征点。记特征点的集合为P_k＝{p_k,r}，r＝1,2,...,N_k，N_k为特征点总数，N_k＝6×N_a×N_b，记x_k,r为特征点p_k,r在全局系下的坐标。记初始时刻雷达坐标系为全局系，记雷达坐标系的X,Y,Z轴分别为激光雷达的前向、右向、下向。

步骤2：基于先验信息的随机采样一致性方法迭代获取最优内点集，具体方法如图2所示：

步骤21，设置参数并进行初始化：

记K_s为最大迭代次数，K为迭代次数。记

为内点集(认定为静态点)，

为内点集内点个数，

为B_k,l中的内点集内点个数，T_better为内点比重阈值。记

为一致集，

为一致集内点个数，

为B_k,l中的一致集内点个数，s_best为一致集评分。记

为最优内点集，

为最优内点集内点个数，

为B_k,l中的最优内点集内点个数。

初始化K_s＝∞，K＝0，

T_better＝60％，

s_best＝0，

步骤22，筛选k时刻雷达先验采样数据集：

k＝0时，每个B_k,l随机选取一个激光点，构成点集D₀。k≠0时，根据上一时刻步骤4计算得到解算模型参数的先验点集D_k。

步骤23，根据先验采样数据集解算模型参数：

在点集D_k随机抽样3个点，通过k最近邻算法(k-NearestNeighbor，kNN)在P_k+1中搜索与之邻近的3个点组成3个点对，利用奇异值分解(Singular Value Decomposition，SVD)方法计算[k+1,k]时刻采样之间的转换矩阵记为T_k+1,k＝[t_x,t_y,t_z,θ_x,θ_y,θ_z]^T。其中，t_x,t_y,t_z分别是沿着雷达系的x,y,z轴的平移，θ_x,θ_y,θ_z是旋转角度，旋转方向遵循右手法则。

步骤24，根据激光点与模型的欧氏距离筛选内点集：

预测k+1时刻特征点p_k,r坐标

其中，T_k+1,k(1:3)是T_k+1,k前三列向量，R_k+1,k是由罗德里格斯公式定义的一个旋转矩阵：

在上式中，I表示单位矩阵，θ＝||T_k+1,k(4:6)||，

是ω的反对称矩阵，T_k+1,k(4:6)是T_k+1,k后三列向量。

通过kNN算法在P_k+1中搜索与x_k,r最邻近的激光点坐标记为x_k+1,r，计算x_k+1,r到

的欧式距离d_k,r：

距离阈值为L₁＝2δ，δ为k+1时刻特征点集P_k+1到投影

距离的标准差，记

为

的集合。若d_k,r＞L₁则将p_k,r加入

并更新

步骤25，根据内点比重筛选一致集：

记k时刻内点所占比重为T_count，当T_count＞T_better时，更新一致集

根据下式计算内点比重T_count：

步骤26，根据内点分布情况筛选最优内点集：

记内点分布的权重协方差矩阵为C，计算公式如下：

其中，x_k,l为B_k,l中心，

为激光雷达特征点云的权重中心，

为B_k,l中候选地面点比重，n_k,l为B_k,l中特征点个数。

记s为内点分布评分，当s＞s_best，更新s_best＝s，更新最优内点集

s计算公式如下：

步骤27，更新迭代次数并判断是否满足终止条件：

迭代的同时不断更新迭代次数K，如果K＜K_s，则跳转至步骤23，直到满足终止条件，如果K＞K_s，将直接停止迭代。K_s的计算公式如下：

其中，记μ为采样K_s次都至少包含一个非地面点的概率。记p_k,l为k时刻从B_k,l中采样一个点的概率，k＝0时

k≠0时，p_k,l根据步骤(4)得到先验采样概率。记

为k时刻B_k,l中内点比率，计算公式如下：

步骤28，输出最优内点集

及对应转移矩阵T_k+1,k。

步骤3：根据最优内点集采用LM(列文伯格-马夸尔特算法)算法重新解算转移矩阵，具体方法如下：

步骤31，记

为k时刻最优内点集

通过kNN算法寻找的所对应k+1时刻最近点集

获得

与T_k+1,k之间的几何关系：

其中，f中的每一行对应一个特征点坐标信息，d包含了所有的对应距离。计算d关于T_k+1,k的雅可比矩阵，记为J：

步骤32，通过非线性迭代，最小化d为0，根据以下公式更新T_k+1,k。

T_k+1,k←T_k+1,k-(J^TJ+λdiag(J^TJ))^-1J^Td (12)

其中，λ是由Levenberg-Marquardt方法确定的因子。

步骤4：计算k时刻内点比重，得到k+1时刻解算模型参数的先验信息，具体方法如下：

步骤41，将k时刻B_k,l的内点比重映射到[δ,1]，以确保每一个B_k,l均有采样的几率：

其中，0＜δ＜1。

步骤42，根据内点分布获得k+1时刻采样数据集：

记p_k+1,l为k+1时刻从B_k+1,l中采样一个点的概率，记D_k+1为根据p_k+1,l从B_k+1,l中取整抽样数据集。

D_k+1,l＝N_a×N_b×p_k+1,l (15)

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (6)

1.动态环境下基于激光雷达的点云配准方法，其特征在于，包括如下步骤：

步骤1，读取k时刻每个激光雷达线上的激光点云信息，并将每个激光雷达线上的激光点云平均划分为N_b段，对每一段进行特征点筛选，从而得到k时刻对应的特征点集合；

步骤2，基于先验信息的随机采样一致性方法迭代获取k时刻最优内点集以及[k+1,k]时刻之间的转移矩阵；具体过程如下：

步骤21，设置参数，包括最大迭代次数K_s，内点集

内点集内点个数

第l段激光点云B_k,l中的内点集内点个数

内点比重阈值T_better，一致集

一致集内点个数

第l段激光点云B_k,l中的一致集内点个数

一致集评分s_best，最优内点集

最优内点集内点个数

第l段激光点云B_k,l中的最优内点集内点个数

初始化K_s＝∞，

T_better＝60％，

s_best＝0，

步骤22，获取k时刻激光点云先验点集D_k，当k＝0时，每个B_k,l随机选取一个激光点，构成先验点集D₀；当k≠0时，根据k-1时刻计算得到解算模型参数的先验点集D_k；令迭代次数K＝0；

步骤23，在先验点集D_k中随机抽样3个点，记为a、b、c，通过kNN算法在P_k+1中搜索与a最邻近的点a′、与b最邻近的点b′、与c最邻近的点c′，组成3个点对，P_k+1为k+1时刻对应的特征点集合，利用SVD方法计算[k+1,k]时刻之间的转移矩阵T_k+1,k；

步骤24，对于k时刻对应的特征点集合P_k＝{p_k,r}，预测k+1时刻各特征点p_k,r的坐标

通过kNN算法在P_k+1中搜索与p_k,r最邻近的点，其坐标记为x_k+1,r，计算x_k+1,r到

的欧式距离d_k,r，设置距离阈值L₁，若d_k,r＞L₁，则将点p_k,r加入

并更新

步骤25，计算k时刻内点所占比重T_count，当T_count＞T_better时，更新一致集为

否则不更新；

步骤26，计算内点分布的权重协方差矩阵C，以及内点分布评分s，当s＞s_best时，更新s_best＝s，并更新最优内点集为

步骤27，若K＜K_s，则K＝K+1并返回步骤23，否则迭代结束，输出最优内点集

及转移矩阵T_k+1,k；

步骤3，根据最优内点集

采用LM方法重新解算转移矩阵；

步骤4，计算k时刻B_k,l中内点比重，根据B_k,l中内点比重计算得到k+1时刻解算模型参数的先验信息，包括先验点集D_k+1和先验采样概率p_k+1,l；

步骤5，进入k+1时刻，重复步骤1-步骤4。

2.根据权利要求1所述动态环境下基于激光雷达的点云配准方法，其特征在于，所述步骤1的具体过程如下：

步骤11，记s_i为k时刻激光点云信息S_k的第i线激光点云，S_k＝{s_i}，1≤i≤N_a，N_a为激光雷达线数，将s_i平均划分为N_b段，则S_k被划分为N_a×N_b段，记B_k,l为S_k的第l段激光点云，l＝1,2,...,N_a×N_b；

步骤12，计算第i线激光点云中每个激光点的曲率，筛选出B_k,l中曲率最大的2个点和曲率最小的4个点作为B_k,l的特征点，则k时刻对应的特征点集合为P_k＝{p_k,r}，1≤r≤N_k，N_k为特征点总数，N_k＝6×N_a×N_b。

3.根据权利要求2所述动态环境下基于激光雷达的点云配准方法，其特征在于，步骤12所述第i线激光点云中每个激光点的曲率，计算公式为：

其中，c_i,d为第i线激光点云中第d个激光点的曲率，第i线激光点云中第d个激光点的曲率由第d个激光点的前5个点和后5个点的坐标得到，S＝10，x_i,d为第i线激光点云中第d个激光点的坐标，x_i,e为第d个激光点的前5个点和后5个点中任意一个点的坐标。

4.根据权利要求1所述动态环境下基于激光雷达的点云配准方法，其特征在于，步骤24所述预测k+1时刻各特征点p_k,r的坐标，公式为：

其中，

为k+1时刻特征点p_k,r的预测坐标，T_k+1,k(1:3)为转移矩阵T_k+1,k前三列向量，x_k,r为k时刻特征点p_k,r的坐标，R_k+1,k是由罗德里格斯公式定义的一个旋转矩阵：

其中，I表示单位矩阵，θ＝||T_k+1,k(4:6)||，

是ω的反对称矩阵，T_k+1,k(4:6)为转移矩阵T_k+1,k第4到6列向量。

5.根据权利要求1所述动态环境下基于激光雷达的点云配准方法，其特征在于，所述步骤3的具体过程如下：

步骤31，记

为k时刻最优内点集

中的每个点通过kNN算法寻找的对应k+1时刻最邻近点集，获得

与[k+1,k]时刻之间的转移矩阵T_k+1,k之间的几何关系d：

计算d关于T_k+1,k的雅可比矩阵J：

步骤32，通过非线性迭代，最小化d为0，根据以下公式更新T_k+1,k：

T_k+1,k←T_k+1,k-(J^TJ+λdiag(J^TJ))^-1J^Td

其中，λ为由Levenberg-Marquardt方法确定的因子。

6.根据权利要求1所述动态环境下基于激光雷达的点云配准方法，其特征在于，所述步骤4的具体过程如下：

步骤41，将k时刻第l段激光点云B_k,l的内点比率

映射到[δ,1]：

其中，

n_k,l为B_k,l中激光点个数，

为B_k,l中的最优内点集内点个数，0＜δ＜0.5；

步骤42，根据

计算k+1时刻解算模型参数的先验信息：

D_k+1,l＝N_a×N_b×p_k+1,l

其中，p_k+1,l为k+1时刻从B_k+1,l中采样一个点的先验采样概率，N_a×N_b为k时刻激光点云信息S_k被划分的段数，D_k+1,l为k+1时刻第l段激光点云的先验点集，D_k+1为k+1时刻先验点集。

Images