CN108445505A - 线环境下基于激光雷达的特征显著性检测方法 - Google Patents

Abstract

本发明公开了一种线环境下基于激光雷达的特征显著性检测方法，包括如下步骤：(1)周期采集k时刻激光雷达信息S(k)；(2)将激光雷达点云数据分群；(3)根据点云数据的空间几何关系，将激光雷达点云数据分割为两个点集合；(4)对步骤(3)中得到的两个点集合进行直线拟合；(5)构建环境特征显著性函数，对环境特征的显著性进行判断。本发明的有益效果为：通过本发明，可以完成对环境的特征显著性检测，准确地辨识出特征不显著的环境。

Description

线环境下基于激光雷达的特征显著性检测方法

技术领域

本发明涉及定位导航与控制技术领域，尤其是一种线环境下基于激光雷达的特征显著性检测方法。

背景技术

机器人进行自主导航时常采用同步定位与地图构建(SLAM，SimultaneousLocalization and Mapping)技术，根据探测机器人相对于环境的变化进行定位，并进行地图构建。激光雷达能够获取高精度的测距信息，且不依赖于外界环境的光照条件，常作为SLAM的传感器。

本发明中所指的特征不显著环境主要为类似于长廊、单面墙结构的环境，在该环境中几何特征大多为平行的线特征，沿线特征方向缺少参考的环境特征。在该类环境中采用激光雷达SLAM方法时，其沿线特征方向的定位误差大。因此，在利用激光雷达进行机器人自主导航时，需要辨识环境中几何特征不显著情况，若机器人处于几何特征不显著环境中，则应当增加其它传感器辅助激光雷达进行定位导航。

发明内容

本发明所要解决的技术问题在于，提供一种线环境下基于激光雷达的特征显著性检测方法，能够采用激光雷达探测周围环境，提取环境中的几何特征，构建环境特征显著性函数，对环境特征的显著性进行判断。

为解决上述技术问题，本发明提供一种线环境下基于激光雷达的特征显著性检测方法，包括如下步骤：

(1)周期采集k时刻激光雷达信息S(k)；

(2)将激光雷达点云数据分群；

(3)根据点云数据的空间几何关系，将激光雷达点云数据分割为两个点集合；

(4)对步骤(3)中得到的两个点集合进行直线拟合；

(5)构建环境特征显著性函数，对环境特征的显著性进行判断。

优选的，步骤(2)中，点云数据分群具体包括如下步骤：

(21)对激光雷达扫描的点云数据进行坐标转换，得到其在直角坐标系下的坐标；记p_i为S(k)的第i个激光点(i＝1,2,…,N₀)，N₀为S(k)中激光点的数量，定义直角坐标系原点为激光雷达所在位置，X轴为前向，Y轴为右向；极坐标系原点为激光雷达所在位置，前向为极轴，极角的角度取顺时针方向为正，极角的范围为[0°,360°)，和为p_i在激光雷达直角坐标系中的坐标；(ρ_i,θ_i)为p_i在激光雷达极坐标系中的坐标，其中为，和与(ρ_i,θ_i)的转换关系为：

(22)计算S(k)相邻序号激光点之间的距离若小于阈值E_t，则p_i和p_i+1为同一点群中的激光点，记点群集合为{s_g}，1≤g≤N_g，N_g为点群的数量。

优选的，步骤(3)中，将激光雷达点云数据分割成两个点集合具体包括如下步骤：

(31)筛选出{s_g}中激光点数量最多的点群，记为s_gd，根据s_gd的数据利用最小二乘法拟合直线fe₀，其在激光雷达直角坐标系中的直线方程为a₀x+b₀y+1＝0，a₀和b₀通过如下公式计算：

其中上标“T”表示转置，上标“-1”表示求逆，为的矩阵，的元素均为1，为s_gd中激光点的数量，α₀的表达式如下：

其中和表示s_gd中第i个激光点在激光雷达直角坐标系中的坐标，

(32)定义两个角度区间为和其求解方法如下：

求解直线a₀x+b₀y＝0与圆x²+y²＝l_d ²的两个交点，记其在激光雷达极坐标中的坐标分别为和其中l_d为所采用的激光雷达有效探测距离的最大值，角度区间和定义为：

(3.3)分割激光雷达数据S(k)得到的两个激光点集合分别记为s_B1和s_B2，求解方法如下：

若则

若则

其中为S(k)中激光点的极角位于角度区间的点组成的集合，为点集的激光点数量，记为点集中第i个激光点，的极坐标记为有且

为S(k)中激光点的极角位于角度区间的点组成的集合，为点集的激光点数量，记为点集中第i个激光点，的极坐标记为有且

优选的，步骤(4)中，拟合直线特征具体包括如下步骤：

(41)利用点集s_B1的激光点数据拟合直线fe₁，其在激光雷达直角坐标系中的直线方程为a₁x+b₁y+1＝0，通过最小二乘法求解a₁和b₁：

上式中为的矩阵，的元素均为1，为s_B1中激光点的数量,α₁的表达式如下：

其中和表示s_B1中第i个激光点在激光雷达直角坐标系中的坐标，

(42)记为点集s_B2的激光点数量，若大于阈值E_N，则利用s_B2拟合直线fe₂，其在激光雷达直角坐标系中的直线方程记为a₂x+b₂y+1＝0，且fe₂与fe₁的斜率相同，记为s_B2中第i个激光点，和表示在激光雷达直角坐标系中的坐标，通过最小二乘法方法求解a₂和b₂：

若b₁≠0，b₂可通过如下公式计算：

a₂的表达式如下：

a₂＝a₁/b₁*b₂

若b₁＝0，a₂可通过如下公式计算：

b₂的表达式如下：

b₂＝b₁/a₁*a₂。

优选的，步骤(5)中，构建环境特征显著性函数具体包括如下步骤：

(51)结合S(k)构建点云数据

记为的第i个激光点为采用的激光雷达能够获取激光点数量的最大值，的极坐标为：

其中为S(k)中激光点的极角的集合，λ为大于1的常数，则的直角坐标和为

(52)构建环境特征显著性函数χ_f：

χ_f＝χ₁+χ₂

其中χ₁计算方法如下：

上式中为激光点集合中的第j个激光点，和为的直角坐标，为激光点数量，为：

上式中点集由中激光点的极角位于角度区间中的激光点组成，其中为：

上式中和为直线a₁x+b₁y+1＝0与圆x²+y²＝l_d ²的两个交点的极角，其在激光雷达极坐标系中的坐标分别为和其中

若χ₂＝0；若χ₂为：

上式中为点集中的点，和为的直角坐标，为激光点数量，为：

上式中点集由中激光点的极角位于角度区间中的激光点组成，其中为：

上式中和为直线a₂x+b₂y+1＝0与圆x²+y²＝l_d ²的两个交点的极角，其在激光雷达极坐标系中的坐标分别为和其中

本发明的有益效果为：通过本发明，可以完成对环境的特征显著性检测，准确地辨识出特征不显著的环境。

附图说明

图1为本发明的方法流程示意图。

具体实施方式

本发明方法的流程如图1所示，其具体步骤如下：

步骤一：周期采集k时刻激光雷达信息S(k)；

步骤二：将激光雷达点云数据分群：

(2.1)对激光雷达扫描的点云数据进行坐标转换，得到其在直角坐标系下的坐标。记p_i为S(k)的第i个激光点(i＝1,2,…,N₀)，N₀为S(k)中激光点的数量。定义直角坐标系原点为激光雷达所在位置，X轴为前向，Y轴为右向；极坐标系原点为激光雷达所在位置，前向为极轴，极角的角度取顺时针方向为正，极角的范围为[0°,360°)。和为p_i在激光雷达直角坐标系中的坐标；(ρ_i,θ_i)为p_i在激光雷达极坐标系中的坐标，其中为，和与(ρ_i,θ_i)的转换关系为：

(2.2)计算S(k)相邻序号激光点之间的距离若小于阈值E_t，则p_i和p_i+1为同一点群中的激光点。记点群集合为{s_g}，1≤g≤N_g，N_g为点群的数量；

步骤三：根据点云数据的空间几何关系，将激光雷达点云数据分割为两个点集合：

(3.1)筛选出{s_g}中激光点数量最多的点群，记为s_gd，根据s_gd的数据利用最小二乘法拟合直线fe₀，其在激光雷达直角坐标系中的直线方程为a₀x+b₀y+1＝0，a₀和b₀通过如下公式计算：

其中上标“T”表示转置，上标“-1”表示求逆，为的矩阵，的元素均为1，为s_gd中激光点的数量,α₀的表达式如下：

其中和表示s_gd中第i个激光点在激光雷达直角坐标系中的坐标，

(3.2)定义两个角度区间为和其求解方法如下：

求解直线a₀x+b₀y＝0与圆x²+y²＝l_d ²的两个交点，记其在激光雷达极坐标中的坐标分别为和其中l_d为所采用的激光雷达有效探测距离的最大值，角度区间和定义为：

(3.3)分割激光雷达数据S(k)得到的两个激光点集合分别记为s_B1和s_B2，求解方法如下：

若则

若则

其中为S(k)中激光点的极角位于角度区间的点组成的集合，为点集的激光点数量，记为点集中第i个激光点，的极坐标记为有且

为S(k)中激光点的极角位于角度区间的点组成的集合，为点集的激光点数量，记为点集中第i个激光点，的极坐标记为有且

步骤四：对步骤三中得到的两个点集合进行直线拟合：

(4.1)利用点集s_B1的激光点数据拟合直线fe₁，其在激光雷达直角坐标系中的直线方程为a₁x+b₁y+1＝0，通过最小二乘法求解a₁和b₁：

上式中为的矩阵，的元素均为1，为s_B1中激光点的数量,α₁的表达式如下：

其中和表示s_B1中第i个激光点在激光雷达直角坐标系中的坐标，

(4.2)记为点集s_B2的激光点数量，若大于阈值E_N，则利用s_B2拟合直线fe₂，其在激光雷达直角坐标系中的直线方程记为a₂x+b₂y+1＝0，且fe₂与fe₁的斜率相同，记为s_B2中第i个激光点，和表示在激光雷达直角坐标系中的坐标，通过最小二乘法方法求解a₂和b₂：

若b₁≠0，b₂可通过如下公式计算：

a₂的表达式如下：

a₂＝a₁/b₁*b₂

若b₁＝0，a₂可通过如下公式计算：

b₂的表达式如下：

b₂＝b₁/a₁*a₂

步骤五：构建环境特征显著性函数，对环境特征的显著性进行判断：

(5.1)结合S(k)构建点云数据

记为的第i个激光点为采用的激光雷达能够获取激光点数量的最大值，的极坐标为：

其中为S(k)中激光点的极角的集合，λ为大于1的常数，则的直角坐标和为

(5.2)构建环境特征显著性函数χ_f：

χ_f＝χ₁+χ₂

其中χ₁计算方法如下：

上式中为激光点集合中的第j个激光点，和为的直角坐标，为激光点数量，为：

上式中点集由中激光点的极角位于角度区间中的激光点组成，其中为：

上式中和为直线a₁x+b₁y+1＝0与圆x²+y²＝l_d ²的两个交点的极角，其在激光雷达极坐标系中的坐标分别为和其中

若χ₂＝0；若χ₂为：

上式中为点集中的点，和为的直角坐标，为激光点数量，为：

上式中点集由中激光点的极角位于角度区间中的激光点组成，其中为：

上式中和为直线a₂x+b₂y+1＝0与圆x²+y²＝l_d ²的两个交点的极角，其在激光雷达极坐标系中的坐标分别为和其中

通过本发明，可以完成对环境的特征显著性检测，准确地辨识出特征不显著的环境。

Claims (5)

1.线环境下基于激光雷达的特征显著性检测方法，其特征在于，包括如下步骤：

(1)周期采集k时刻激光雷达信息S(k)；

(2)将激光雷达点云数据分群；

(3)根据点云数据的空间几何关系，将激光雷达点云数据分割为两个点集合；

(4)对步骤(3)中得到的两个点集合进行直线拟合；

(5)构建环境特征显著性函数，对环境特征的显著性进行判断。

2.如权利要求1所述的线环境下基于激光雷达的特征显著性检测方法，其特征在于，步骤(2)中，点云数据分群具体包括如下步骤：

(21)对激光雷达扫描的点云数据进行坐标转换，得到其在直角坐标系下的坐标；记p_i为S(k)的第i个激光点(i＝1,2,…,N₀)，N₀为S(k)中激光点的数量，定义直角坐标系原点为激光雷达所在位置，X轴为前向，Y轴为右向；极坐标系原点为激光雷达所在位置，前向为极轴，极角的角度取顺时针方向为正，极角的范围为[0°,360°)，和为p_i在激光雷达直角坐标系中的坐标；(ρ_i,θ_i)为p_i在激光雷达极坐标系中的坐标，其中为，和与(ρ_i,θ_i)的转换关系为：

(22)计算S(k)相邻序号激光点之间的距离若小于阈值E_t，则p_i和p_i+1为同一点群中的激光点，记点群集合为{s_g}，1≤g≤N_g，N_g为点群的数量。

3.如权利要求1所述的线环境下基于激光雷达的特征显著性检测方法，其特征在于，步骤(3)中，将激光雷达点云数据分割成两个点集合具体包括如下步骤：

(31)筛选出{s_g}中激光点数量最多的点群，记为s_gd，根据s_gd的数据利用最小二乘法拟合直线fe₀，其在激光雷达直角坐标系中的直线方程为a₀x+b₀y+1＝0，a₀和b₀通过如下公式计算：

其中上标“T”表示转置，上标“-1”表示求逆，为的矩阵，的元素均为1，为s_gd中激光点的数量，α₀的表达式如下：

其中和表示s_gd中第i个激光点在激光雷达直角坐标系中的坐标，

(32)定义两个角度区间为和其求解方法如下：

求解直线a₀x+b₀y＝0与圆x²+y²＝l_d ²的两个交点，记其在激光雷达极坐标中的坐标分别为和其中l_d为所采用的激光雷达有效探测距离的最大值，角度区间和定义为：

(3.3)分割激光雷达数据S(k)得到的两个激光点集合分别记为s_B1和s_B2，求解方法如下：

若则

若则

其中为S(k)中激光点的极角位于角度区间的点组成的集合，为点集的激光点数量，记为点集中第i个激光点，的极坐标记为有且

为S(k)中激光点的极角位于角度区间的点组成的集合，为点集的激光点数量，记为点集中第i个激光点，的极坐标记为有且

4.如权利要求1所述的线环境下基于激光雷达的特征显著性检测方法，其特征在于，步骤(4)中，拟合直线特征具体包括如下步骤：

(41)利用点集s_B1的激光点数据拟合直线fe₁，其在激光雷达直角坐标系中的直线方程为a₁x+b₁y+1＝0，通过最小二乘法求解a₁和b₁：

上式中为的矩阵，的元素均为1，为s_B1中激光点的数量,α₁的表达式如下：

其中和表示s_B1中第i个激光点在激光雷达直角坐标系中的坐标，

(42)记为点集s_B2的激光点数量，若大于阈值E_N，则利用s_B2拟合直线fe₂，其在激光雷达直角坐标系中的直线方程记为a₂x+b₂y+1＝0，且fe₂与fe₁的斜率相同，记为s_B2中第i个激光点， 和表示在激光雷达直角坐标系中的坐标，通过最小二乘法方法求解a₂和b₂：

若b₁≠0，b₂可通过如下公式计算：

a₂的表达式如下：

a₂＝a₁/b₁*b₂

若b₁＝0，a₂可通过如下公式计算：

b₂的表达式如下：

b₂＝b₁/a₁*a₂。

5.如权利要求1所述的线环境下基于激光雷达的特征显著性检测方法，其特征在于，步骤(5)中，构建环境特征显著性函数具体包括如下步骤：

(51)结合S(k)构建点云数据

记为的第i个激光点为采用的激光雷达能够获取激光点数量的最大值，的极坐标为：

其中为S(k)中激光点的极角的集合，λ为大于1的常数，则的直角坐标和为

(52)构建环境特征显著性函数χ_f：

χ_f＝χ₁+χ₂

其中χ₁计算方法如下：

上式中为激光点集合中的第j个激光点，和为的直角坐标，为激光点数量，为：

上式中点集由中激光点的极角位于角度区间中的激光点组成，其中为：

上式中和为直线a₁x+b₁y+1＝0与圆x²+y²＝l_d ²的两个交点的极角，其在激光雷达极坐标系中的坐标分别为和其中

若χ₂＝0；若χ₂为：

上式中为点集中的点，和为的直角坐标，为激光点数量，为：

上式中点集由中激光点的极角位于角度区间中的激光点组成，其中为：

上式中和为直线a₂x+b₂y+1＝0与圆x²+y²＝l_d ²的两个交点的极角，其在激光雷达极坐标系中的坐标分别为和其中