一种结合区域信息和角点检测算子的线性特征拟合方法

1.本发明涉及激光检测技术领域，特别涉及一种结合区域信息和角点检测算子的线性特征拟合方法。


背景技术：

2.环境感知是移动机器人智能化的一种体现，在移动机器人的应用中起着决定性作用。激光雷达是当前移动机器人上进行环境感知的常用设备，具有效率高、价格适中、适用性强等特点。但激光雷达易受光照影响，在单一环境中，会由于特征退化，而导致定位精度降低或定位缺失。通过对特征点进行加工，提取出具有一定几何构型的环境特征，能够有效增强系统的定位能力和鲁棒性。
3.线性特征被公认的作为移动机器人环境感知的重要特征。线性特征提取主要分为两个步骤，区域分割和特征提取。区域分割阶段主要完成特征模式的分类及识别确定，特征模式包括直线，圆弧等，并确定属于该特征模式的区域及区域内的激光数据点集。特征提取主要完成各类特征模式参数的确定以及特征点的提取。
4.常用的线特征提取算法有hough变换、lt(line-tracting)、pdbs(point distance based methods)、iepf(iterative end point fit)等。hough变换首先将线特征从笛卡尔坐标系转换到极坐标系，然后统计极坐标系中交点处的曲线条数，只有达到一定的阀值才标记为是线特征，该方法的缺点是运算量较大。lt算法原理是按照直线追踪准则判断后续探测到的点与前面探测到的点是否在同一直线上，缺点是提取出的线段缺乏完整性且误差较大。pdbs主要根据激光雷达两个相邻点之间在直角坐标下的距离与设定的阈值相比，距离大于阈值，则认为两个点属于不同的区域。该方法中设定的阈值对线段的分割具有重要的影响，阈值设定太小容易过分割，阈值设定过大会出现欠分割，并且在角点处会出现分割失败的情况。iepf算法首先将点集拟合为一条直线，然后判断点到直线的距离与其设定的阀值之间的关系对点集进行分割和直线拟合。该方法在性能上较优越，但其对阈值的选取敏感，线特征提取也会产生过分割或欠分割。
5.综上所述，目前对于从激光雷达扫描点集中提取特征的方法较多，但存在低效率、复杂和不准确的问题，本技术通过改进使得激光雷达的识别更加高效和准确。


技术实现要素：

6.本发明针对现有技术中的不足，提供一种结合区域信息和角点检测算子的线性特征拟合方法；以解决当前激光雷达识别的低效率、不准确的问题。首先，利用两点间距离判断区域断点以及区域外噪点。然后根据点到首尾连线距离筛选初选角点，根据点到弦、点到切线累加距离计算角点检测算子并确定候选角点，根据角点张开度剔除伪角点确定最终角点。最后，过滤噪声点，并在区域断点、角点处线性分割，利用加权最小二乘法拟合出激光扫描点线性特征。该方法能有效避免噪声点和固定分割阈值对角点提取的影响，拟合激光扫描数据特征更接近实际环境。
7.为实现上述目的，本发明采用以下技术方案：
8.一种结合区域信息和角点检测算子的线性特征拟合方法，包括以下步骤：
9.s1：通过激光雷达扫描周围环境数据，采集多个点数据，并将点数据从极坐标系转化到平面直角坐标系下；
10.s2：通过步骤s1平面直角坐标系下的各点坐标，计算相邻两点间距离，利用两点间距离判断区域断点以及区域外噪声点；
11.s3：确定多个初选角点coni；
12.s4：计算各个初选角点coni到各自弦长l的垂直距离累加值，进而确定对应初选角点coni到对应弦的角点响应函数；其中弦长l确定的过程为：针对对应的初选角点coni，找到其附近的多个激光数据点，连接多个激光数据点中的首尾两点的连线确定弦长；
13.针对各个初选角点coni，计算其支持域半径内多个激光点到对应初选角点coni的切线距离，得到切线距离累加值，进而确定对应初选角点coni到其切线的角点响应函数；
14.s5：根据步骤s4所确定的对应初选角点coni到对应弦的角点响应函数、以及对应初选角点coni到其切线的角点响应函数，计算对应初选角点coni的角点检测算子，进而确定候选角点；
15.s6：对所确定的所有候选角点，分别计算出其角点张开度θ，根据角点张开度剔除伪角点确定最终角点；
16.s7：根据步骤s2所确定的区域断点和区域外噪声点，以及步骤s6确定的最终角点；将区域外噪声点进行过滤，并在区域断点和最终角点处线性分割，利用加权最小二乘法拟合出激光扫描点线性特征。
17.为优化上述技术方案，采取的具体措施还包括：
18.进一步地，步骤s1的具体内容为：
19.通过sick tim571激光雷达扫描周围环境数据，其角度分辨率为0.33
°
，扫描角度160
°
，测量起始角度为-80
°
，终止角度为80
°
，共采集481组点数据，将点数据从极坐标系转化到平面直角坐标系下；
20.其中，采集到点的激光雷达极坐标数据表示为：
[0021][0022]
式中，r为激光雷达返回的障碍物距离，为每个环境距离对应角度；
[0023]
根据所选sick tim571雷达的参数配置，的计算方式如下：
[0024][0025]
从极坐标系转化到平面直角坐标系的转换方式如下：
[0026][0027]
式中，x
l
为激光扫描点横坐标长度，y
l
为激光扫描点纵坐标长度值，r、分别为极坐标中激光扫描点的长度和角度，i为激光扫描点中的任意一个。
[0028]
进一步地，步骤s2中计算相邻两点间距离，利用两点间距离判断区域断点以及区域外噪声点的具体内容为：
[0029]
两个相邻测量点之间的距离d的计算方式如下：
[0030][0031]
式中，分别为第i点的横、纵坐标值，分别为第i 1点的横、纵坐标值；
[0032]
在直角坐标下，将激光雷达两个相邻点之间的距离d与设定的阈值ε相比，其中阈值ε＝0.015，若d＞ε，则认为当前这两个点属于不同的区域，该点为激光扫描数据点的区域断点；若d＜ε，则该点为正常激光点云点，以此完成所有相邻点之间的距离判断；
[0033]
其中，若任意一点i到前一点的距离d和到后一点的距离d均大于阈值ε，则认为该点为激光雷达扫描数据点中的区域外噪声点，用噪声点附近十个雷达数据点的平均值代替该噪声点值。
[0034]
进一步地，步骤s3确定多个初选角点coni的具体的内容为：
[0035]
s3.1：连接首尾两个数据点之间的连线，计算除去首尾两点的其他数据点到该线的距离并进行由大到小顺序排列，选取序列第一个点作为第一个初选角点con1；
[0036]
s3.2：连接首个数据点和第一个初选角点con1之间连线，计算除去首个数据点和第一个初选角点con1的其他数据点到该线的距离并进行由大到小顺序排列，选取序列第一个点作为第二个初选角点con2；
[0037]
s3.3：连接第一个初选角点con1和第二个初选角点con2之间连线，计算除去点con1和点点con2之外的其他数据点到该线的距离并进行由大到小顺序排列，选取序列第一个点作为第三个初选角点con3；
[0038]
s3.4：同理，在进行下一次的初选角点筛选时，连接前两次确定的初选角点直线，计算其他数据点到该线的距离并进行由大到小顺序排列，选取序列第一个点作为当前所要确定的初选角点；以此得出多个初选角点coni。
[0039]
进一步地，步骤s4中，“计算各个初选角点coni到各自弦长l的垂直距离累加值，进而确定对应初选角点coni到对应弦的角点响应函数；其中弦长l确定的过程为：针对对应的初选角点coni，找到其附近的多个激光数据点，连接多个激光数据点中的首尾两点的连线确定弦长”的具体内容为：
[0040]
针对对应的初选角点coni，选择长度为l＝10个激光扫描点的弦，其中对应的初选角点coni需在确定弦l的两个端点以上及以内，因此所确定的弦具有多种情况；在弦l具有多种情况的条件下，计算初选角点coni到弦l的垂直距离累加值：
[0041][0042]
式中，d
j，k
为coni点到l＝10个激光扫描点的弦的垂直距离；j表示激光扫描点，k表示弦；
[0043]
对h
l
(j)进行归一化处理，得到对应初选角点coni到对应弦的角点响应函数h
l
(j)。
[0044]
进一步地，步骤s4中，“针对各个初选角点coni，计算其支持域半径内多个激光点到对应初选角点coni的切线距离，得到切线距离累加值，进而确定对应初选角点coni到其切线的角点响应函数”的具体内容为：
[0045]
针对一个对应的初选角点coni，其切线方程为：
[0046][0047]
式中，α1、β1为点coni处切线的方向向量，xi、yi为点coni的横纵坐标值，t表示方向向量的计算系数，r表示实数；
[0048]
针对一个对应的初选角点coni，其支持域半径内的多个激光点组合成点列点列上任一点(x
i j
，y
i j
)到切线l的距离为：
[0049][0050]
式中，α1、β1为点coni处切线的方向向量，xi、yi为点coni的横纵坐标值，x
i j
、y
x j
为距离点coni长度为支持域半径内附近点的横纵坐标值；
[0051]
则有点列到切线l的距离累加和，即对应初选角点coni到对应切线的角点响应函数表示为：
[0052][0053]
式中，d
i j
为点列上任一点(x
i j
，y
i j
)到切线l的距离，n表示支持域半径的部分。
[0054]
进一步地，步骤s5中计算对应初选角点coni的角点检测算子，进而确定候选角点的具体内容为：
[0055]
计算对应初选角点coni的角点检测算子：
[0056]
h(j)＝h
l
(j)
·hq
(j)
[0057]
式中，h
l
(j)为弦的角点响应函数，hq(j)为切线的角点响应函数；
[0058]
当对应初选角点coni的角点检测算子h(j)大于阈值t＝0.66时候，则该初选角点coni判定为候选角点。
[0059]
进一步地，步骤s6的具体内容为：
[0060]
在一定尺度下提取候选角点集以保证能够检测到所有的候选角点，然后根据角点张开度来筛出候选角点集中的伪角点，从而获取激光扫描点中的有效点；
[0061]
其中，角点张开度将当前待判断候选角点的局部动态支撑区定义为前一个候选角点与后一个候选角点之间边缘上所有的激光扫描点；当角点张开度θ满足160
°
≤θ≤200
°
，则该候选角点为伪角点，从候选角点中删除，以此确定最终角点。
[0062]
进一步地，其中角点张开度θ的计算公式为：
[0063][0064]
[0065][0066]
式中，δx1、δx2、δy1、δy2均表示为中间量，u、u1、u2分别当前候选角点、当前候选角点的前、后角点，l1表示u1与u之间的激光扫描点数，l2表示u2与u之间的激光扫描点数，n为当前候选角点的下标。
[0067]
本发明的有益效果是：
[0068]
1、本发明能有效避免噪声点和固定分割阈值对角点提取的影响，在对透明障碍物以及多层次障碍物环境中，能够大幅度减少反射、遮挡对激光雷达提取环境信息带来的影响，先将不同层次障碍物划分在不同区域，同时过滤反射噪点，然后结合角点特征，拟合出环境信息的不同层次，从而避免了将不同层次障碍物已经反射噪点划分在同一层次导致拟合失效的情形。
[0069]
2、本发明有较高的角定位和线定位精度，能够准确地获得噪声点和角点位置，拟合特征线段精度有显著提升，适用于使用嵌入式开发的自主机器人slam中。
附图说明
[0070]
图1是本发明整体流程步骤示意图。
[0071]
图2是本发明实施例中激光扫描仪角度定义示意图。
[0072]
图3是本发明噪声点建模及预处理示意图。
[0073]
图4是本发明初选角点确定示意图。
[0074]
图5是本发明点到弦累加距离计算示意图。
[0075]
图6是本发明点到切线累加距离计算示意图。
[0076]
图7是本发明实施例中平均重复率折线示意图。
具体实施方式
[0077]
现在结合附图对本发明作进一步详细的说明。
[0078]
如图1所示，结合区域信息和角点检测算子的雷达线性特征拟合，包括：
[0079]
步骤s1：sick tim571激光雷达扫描周围环境数据，角度分辨率为0.33
°
，扫描角度160
°
，测量起始角度为-80
°
，终止角度为80
°
，共采集481组点数据，将点数据从极坐标系转化到平面直角坐标系下；
[0080]
步骤s2：计算相邻两点间距离，利用两点间距离判断区域断点以及区域外噪声点；
[0081]
步骤s3：计算除去首尾两个点数据点之外到首尾连线的距离，根据点到首尾连线距离筛选初选角点，将其定义为初选角点coni；
[0082]
步骤s4：以步骤s3中找到的初选角点作为下一次迭代的终点及起点(重新定义首尾点，以前两次找到的角点作为新的首尾点)，找出第二个距离最远点con2，以此类推，循环操作，直到点到首尾连线距离最大值小于阈值ε，结束循环，找出所有初选角点coni；
[0083]
步骤s5：分别计算初选角点coni到弦长l为10个激光数据点的弦的垂直距离累加值，以及支持域半径为6个激光数据点的pi点到切线累加距离之和，根据点到弦、点到切线
累加距离计算角点检测算子并确定候选角点，记为ui；
[0084]
步骤s6：对所找出的所有候选角点ui分别计算出其角点张开度θ，根据角点张开度剔除伪角点确定最终角点；
[0085]
步骤s7：过滤噪声点，并在区域断点、角点处线性分割，利用加权最小二乘法拟合出激光扫描点线性特征。
[0086]
实施例中，步骤s1所述将点数据从极坐标系转化到平面直角坐标系下，具体为：
[0087]
每一组采集数据的极坐标信息表示方式如下：
[0088][0089]
公式(1)中r为激光雷达返回的障碍物距离；为每个环境距离对应角度。
[0090]
如图2所示，从第一个角度开始，每隔0.33
°
输出一个值，每个光束角度表示如下：
[0091][0092]
公式(2)中i为激光雷达返回数据序号。
[0093]
激光传感器每一个测量距离在激光传感器直角坐标x
l
oy
l
下坐标为：
[0094][0095]
公式(3)中x
l
为激光扫描点横坐标长度，y
l
为激光扫描点纵坐标长度值，r、分别为极坐标中激光扫描点的长度和角度，i为激光扫描点的个数。
[0096]
所有测量点x
l
和y
l
坐标集合用xz和yz表示：
[0097][0098]
实施例中，所述步骤s2中，区域断点判断方法如下：
[0099]
将激光雷达两个相邻点之间的距离di与设定的阈值ε相比，若两个相邻测量点之间的距离di大于阈值ε，则认为两个点属于不同的区域，该点为激光扫描数据点的区域断点。
[0100]
所述步骤s2中，对区域外噪声点判断方法如下：
[0101]
如果任意一点到前一点的距离di和到后一点的距离d
i 1
均大于阈值ε，则认为该点为激光雷达扫描数据点中的区域外噪声点。
[0102]
两个相邻测量点之间的距离di的计算公式如下：
[0103][0104]
公式(5)x
l
、y
l
中为第l点的横、纵坐标值，x
l 1
、y
l 1
为l 1点的横、纵坐标值。
[0105]
如图3所示，点p
i-1
和点p
i 1
为别为直线l1的末点和直线l2的起点，中间出现噪声点pi，且在距离分布图中，该点与前后两点的距离均大于阈值ε＝0.15，所以该点为噪声点。
[0106]
当检测到噪声点后，取噪声点前后各五个雷达数据点，利用均值滤波的方法，用噪声点前后十个雷达数据点的平均值代替噪声点值，雷达数据点经过过滤后，相邻两数据点间的距离均小于阈值ε＝0.15。
[0107]
实施例中，所述步骤s3中，初选角点的判断方式如下：
[0108]
如图4所示，将数据中的一个点p1和另一个点pn连接形成一条直线l1，然后计算从p2点到p
n-1
点之间所有点到l1的距离di。找出距离最远的con1点，定义该点则为初选角点。再
以con1点为下一次迭代的终点及起点，找出第二个距离最远点con2，以此类推，直到激光扫描数据点中的所有初选角点con都被寻找出来。
[0109]
其中，di的计算公式如下：
[0110][0111][0112][0113]
公式(6)(7)(8)中，分别为起点p1的坐标值，分别为终点pn的坐标值，分别为中间任意一点pi的坐标值，t
x
、ty分别为点在直线上的投影点的的坐标值(投影点指的是首尾连线内部所有点到首尾连线的投影点)。
[0114]
实施例中，所述步骤s5中，根据点到弦、点到切线累加距离计算角点检测算子并确定候选角点的判断方式如下：
[0115]
如图5所示，点到弦累加距离，计算由p1，p2，
…
，pn组成的弦上pi点的角点响应值时，选择长度为l＝10个激光扫描点的弦，计算pi点到l的垂直距离。要求计算pi在弦的两个端点内(包括pi与两个端点重合)的所有情况，并将所有情况下计算求得的点到弦垂直距离相加得到。(参考图5说明一个具体的实施例：例如初选角点的左边有1个点，右边有9个点，将左边第1个点和右边第9个点连线，计算距离，这是一种弦l的情况；初选角点的左边有2个点，右边有8个点，将左边的第2个点和右边的第8个点连线，计算距离，这也是一种弦l的情况。以此类推进行累加。具体的图5中：pi是初选角点，其两侧中p
j-l-pj是10个点，两点之间有条连线，这就弦l的一种情况)。
[0116]
累加距离计算公式如下：
[0117][0118]
公式(9)中，d
j，k
为pi点到l＝10个激光扫描点的弦的垂直距离。
[0119]
对该段轮廓线上的点到弦垂直距离和做归一化处理，点到弦的角点响应函数的计算公式如下：
[0120][0121]
公式(10)中，h
l
为点到l＝10个激光扫描点的弦的垂直累加距离之和。
[0122]
如图6所示，点到切线累加距离，图中一条实线表示函数y＝5x2，x∈[-1，1]的曲线，另一条实线表示函数y＝x2，x∈[-1，1]的曲线，函数y＝5x2与函数y＝x2相交于原点且它们在原点(0，0)处的切线均为y＝0。
[0123]
设x0＞0，(x0，0)为切线上原点附近的一点，则过点(x0，0)且与切线y＝0垂直的直线x＝x0分别与函数y＝5x2和函数y＝x2相交于点(x0，5x
02
)及点(x0，x
02
)。易知点(x0，5x
02
)及点(x0，x
02
)到切线y＝0的距离分别为d＝5x
02
及d＝x
02
，有d＞d。定义切点左右两侧雷达扫描数据点个数为支持域半径。(简单的就是对每个角点，计算其切线；同时找到其附近的6个激
光点到切线的距离，进行累加。具体的实施例：图6中的原点是角点，x轴是切线，计算其他点到其距离)。
[0124]
经过点pi(xi，yi)的切线方程表示方式如下：
[0125][0126]
公式(11)中α1、β1为pi处的切线的方向向量，xi、yi为pi的横纵坐标值。
[0127]
点列上任一点(x
i j
，y
i j
)到切线l的距离计算公式如下：
[0128][0129]
公式(12)中α1、β1为pi处的切线的方向向量，xi、yi为pi的横纵坐标值，x
i j
、y
x j
为距离pi点长度为支持域半径的点的的横纵坐标值。
[0130]
则有点列到切线l的距离累加和，即该点到切线的角点响应函数的计算公式如下：
[0131][0132]
公式(13)中，d
i j
为点列上任一点(x
i j
，y
i j
)到切线l的距离。
[0133]
最终初选角点的角点监测算子的计算公式如下：
[0134]
h(j)＝h
l
(j)
·hq
(j)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0135]
公式(14)中h
l
(j)为l＝10个激光扫描点时弦的角点响应函数，hq(j)为支持域半径为6个雷达扫描点时切线的角点响应函数。
[0136]
如图7(a)所示，平均重复率(变换雷达数据方向后重复识别角点数与实际角点比值的平均值)随着支持域半径大小不断增大，当支持域半径为6时，平均重复率达到极大值，随后开始降低，可以看出过大的支持域半径会使得距离相近的成对角点产生角点融合现象。
[0137]
如图7(b)所示，平均重复率在角点检测算子阈值t＝0.66处达到极大值，并且在极大值点处振荡变化幅度不大，说明了利用点到弦和点到切线累加距离的乘积求得的角点检测算子的鲁棒性大。通过以上讨论，最终确定支持域半径选择6和阈值大小取为t＝0.66。
[0138]
实施例中，所述步骤s6中，根据角点张开度剔除伪角点确定最终角点的判断方式如下：
[0139]
在小尺度下提取候选角点集以保证能够检测到所有的确定角点，然后根据角点张开度来筛出候选角点集中的伪角点，从而获取激光扫描点中的有效点。
[0140]
点张开度将当前待判断候选角点的局部动态支撑区定义为前一个候选角点与后一个候选角点之间边缘上所有的激光扫描点。
[0141]
角点张开度的计算公式如下：
[0142]
[0143][0144][0145]
公式(15)(16)(17)中u为当前待判断候选角点的位置，l1表示u1与u之间的激光扫描点数，l1表示u2与u之间的激光扫描点数。
[0146]
实施例中，所述步骤s7中，过滤噪声点，并在区域断点、角点处线性分割，利用加权最小二乘法拟合出激光扫描点线性特征，具体为：
[0147]
已知二维数据点列(x1，y1)，(x2，y2)，...，(xn，yn)，则设直线拟合的数学模型为y＝kx b，再通过残差ei＝y
i-kx
i-b(i＝1，2，3...，n)求出模型中的参数，考虑残差的平方和最小的关于参数k，b的方程。
[0148]
加权最小二乘法中参数k，b的计算公式如下：
[0149][0150][0151][0152][0153]
需要注意的是，发明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。
[0154]
以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。