基于激光点云杆状物特征提取和匹配的定位方法及系统与流程

1.本发明涉及激光定位的技术领域，具体地，涉及基于激光点云杆状物特征提取和匹配的定位方法及系统，尤其涉及一种使用树干等杆状物特征进行激光定位的方法。


背景技术：

2.基于激光雷达点云地图的特征定位已经成为自动驾驶高精定位极其重要的一部分，尤其是gps信号长期不稳定的场景，该定位方法常使用环境的线、面特征，深度学习方法提取的特征与人工放置的固定特征。特征的鲁棒性和复杂度会直接影响定位的性能，线、面特征依赖于环境的结构性，深度学习方法会增加定位的复杂度，人工放置固定特征会增加人工复杂度而稀疏特性却无法明显提升定位的性能。
3.在公告号为cn113313081b的专利文献中公开了融合车载三维激光点云与影像的道路交通杆状物分类方法，包括如下步骤，获取点云和影像数据；剔除点云中的地面点得到预分割体，进行切片并对连续切片集合进行特征约束，将符合特征约束的连续切片集合作为杆状物原始种子点；通过生长算法对杆状物原始种子点进行生长，得到完整的杆状物点云，以获得杆状物点云的精确位置信息；根据点云与全景影像的对应关系，将杆状物点云投射到全景影像上得到杆状物点云相应的影像范围；利用训练后的mask-rcnn对影像进行实例分割，得到杆状物的实例分割信息，利用杆状物点云在全景影像中的范围对其进行精细分类。
4.因此，需要提出一种新的技术方案以改善上述技术问题。


技术实现要素：

5.针对现有技术中的缺陷，本发明的目的是提供一种基于激光点云杆状物特征提取和匹配的定位方法及系统。
6.根据本发明提供的一种基于激光点云杆状物特征提取和匹配的定位方法，包括：
7.步骤s1：利用带有激光雷达的数据采集车对环境进行扫描，得到激光点云地图；
8.步骤s2：对所述激光点云地图进行高度过滤、聚类、特征粗提取和特征精提取，得到地图杆状物特征；
9.步骤s3：利用车辆的激光雷达对车辆周围环境进行扫描，得到实时激光点云；
10.步骤s4：对所述实时激光点云进行高度过滤、雷达线数特性过滤、聚类、特征粗提取和特征精提取，得到实时杆状物特征；
11.步骤s5：对所述地图杆状物特征和所述实时杆状物特征进行特征匹配和非线性优化，得到车辆位姿。
12.优选地，所述高度过滤包括：在激光点云中选取距离地面高度大于第一高度值的所有点，过滤掉其他点，得到高度过滤后点云；
13.所述雷达线数特性过滤包括：针对所述高度过滤后点云，利用激光雷达单线扫描点云的特性，将单线点云中的各个点按照扫描的先后顺序依次排列，再根据点与点的间距
分割出多个点云段，选取长度在第一长度值以内的所有点云段，过滤掉其他点云段，得到线数特性过滤后点云；
14.所述聚类包括：对所述高度过滤后点云或者所述线数特性过滤后点云进行欧氏聚类，然后选取聚类中心周围第一半径值范围内的所有点，构成对应的聚类点云簇；
15.所述特征粗提取包括：对所述聚类点云簇进行主成分分析，提取出最大特征值对应特征向量的z轴分量fz，判断其是否大于第一特征阈值f
th
，若是，则选取所述聚类点云簇作为备选点云簇，若否，则滤除所述聚类点云簇。
16.优选地，所述特征精提取包括：
17.步骤t1：将备选点云簇中的所有点按照z坐标从小到大排序，设置一个高度为d的滑动窗口，然后在备选点云簇的z轴方向上，针对滑动窗口自下而上设置m个停留位置，最底部对应第1个停留位置，最顶部对应第m个停留位置；
18.步骤t2：将滑动窗口放置在第一个停留位置上，k＝0，高度差值h1＝0；
19.步骤t3：当滑动窗口位于第i个停留位置时(i＝1,2,
…
,m)，根据z坐标截取处于滑动窗口内的点云，然后对窗口内点云进行主成分分析，提取出最大特征值对应特征向量的z轴分量g
zi
；
20.步骤t4：判断g
zi
是否大于第二阈值g
th
，若是，进入步骤t5，若否，令高度差值hi＝0且令k＝0，然后进入步骤t6；
21.步骤t5：k的值加1，然后判断k值的大小，若k＝1，记录此时滑动窗口的起点位置h
l1
和终点位置h
u1
，计算高度差值hi＝h
u1
–hl1
，若k》1，记录此时滑动窗口的终点位置h
ek
，计算高度差值hi＝h
uk
–hl1
；
22.步骤t6：判断i是否等于m，若否，则将滑动窗口向上移动到下一个停留位置，即i的值加1，然后跳回步骤t3，若是，则进入步骤t7；
23.步骤t7：从所有高度差值hi(i＝1,2,
…
,m)中选出最大的高度差值h
max
，确定h
max
对应的上边界hu和下边界h
l
，然后选取备选点云簇中位于hu和h
l
之间的点，得到目标点云簇，即杆状物特征。
24.优选地，所述滑动窗口的m个停留位置，根据所述备选点云簇在z轴方向上的高度，通过固定设置相邻两个停留位置的间距得到，或者根据所述备选点云簇中的点在z轴方向上的分布，自下而上每固定数量的点或每个高度的点设置一个停留位置得到。
25.优选地，在步骤s4的特征精提取中，采用多帧拼接或者降采样的方法维持点云密集度，其中多帧拼接使用帧与帧之间的里程计位姿，将前几帧提取的杆状物特征全部转换到当前帧的坐标系下，共同作为当前帧的杆状物特征。
26.本发明还提供一种基于激光点云杆状物特征提取和匹配的定位系统，包括：
27.激光点云地图构建模块：利用带有激光雷达的数据采集车对环境进行扫描，得到激光点云地图；
28.地图杆状物特征提取模块：对所述激光点云地图进行高度过滤、聚类、特征粗提取和特征精提取，得到地图杆状物特征；
29.实时激光点云获取模块：利用车辆的激光雷达对车辆周围环境进行扫描，得到实时激光点云；
30.实时杆状物特征提取模块：对所述实时激光点云进行高度过滤、雷达线数特性过
滤、聚类、特征粗提取和特征精提取，得到实时杆状物特征；
31.位姿获取模块：对所述地图杆状物特征和所述实时杆状物特征进行特征匹配和非线性优化，得到车辆位姿。
32.优选地，所述地图杆状物特征提取模块和所述实时杆状物特征提取模块进行高度过滤的单元包括在激光点云中选取距离地面高度大于第一高度值的所有点，过滤掉其他点，得到高度过滤后点云；
33.所述实时杆状物特征提取模块进行雷达线数特性过滤的单元包括针对所述高度过滤后点云，利用激光雷达单线扫描点云的特性，将单线点云中的各个点按照扫描的先后顺序依次排列，再根据点与点的间距分割出多个点云段，选取长度在第一长度值以内的所有点云段，过滤掉其他点云段，得到线数特性过滤后点云；
34.所述地图杆状物特征提取模块和所述实时杆状物特征提取模块进行聚类的单元包括对所述高度过滤后点云或者所述线数特性过滤后点云进行欧氏聚类，然后选取聚类中心周围第一半径值范围内的所有点，构成对应的聚类点云簇；
35.所述地图杆状物特征提取模块和所述实时杆状物特征提取模块进行特征粗提取的单元包括对所述聚类点云簇进行主成分分析，提取出最大特征值对应特征向量的z轴分量fz，判断其是否大于第一特征阈值f
th
，若是，则选取所述聚类点云簇作为备选点云簇，若否，则滤除所述聚类点云簇。
36.优选地，所述地图杆状物特征提取模块和所述实时杆状物特征提取模块进行特征精提取的单元包括：
37.部件m1：将备选点云簇中的所有点按照z坐标从小到大排序，设置一个高度为d的滑动窗口，然后在备选点云簇的z轴方向上，针对滑动窗口自下而上设置m个停留位置，最底部对应第1个停留位置，最顶部对应第m个停留位置；
38.部件m2：将滑动窗口放置在第一个停留位置上，k＝0，高度差值h1＝0；
39.部件m3：当滑动窗口位于第i个停留位置时(i＝1,2,
…
,m)，根据z坐标截取处于滑动窗口内的点云，然后对窗口内点云进行主成分分析，提取出最大特征值对应特征向量的z轴分量g
zi
；
40.部件m4：判断g
zi
是否大于第二阈值g
th
，若是，进入部件m5，若否，令高度差值hi＝0且令k＝0，然后进入部件m6；
41.部件m5：k的值加1，然后判断k值的大小，若k＝1，记录此时滑动窗口的起点位置h
l1
和终点位置h
u1
，计算高度差值hi＝h
u1
–hl1
，若k》1，记录此时滑动窗口的终点位置h
ek
，计算高度差值hi＝h
uk
–hl1
；
42.部件m6：判断i是否等于m，若否，则将滑动窗口向上移动到下一个停留位置，即i的值加1，然后跳回部件m3，若是，则进入部件m7；
43.部件m7：从所有高度差值hi(i＝1,2,
…
,m)中选出最大的高度差值h
max
，确定h
max
对应的上边界hu和下边界h
l
，然后选取备选点云簇中位于hu和h
l
之间的点，得到目标点云簇，即杆状物特征。
44.优选地，所述滑动窗口的m个停留位置，根据所述备选点云簇在z轴方向上的高度，通过固定设置相邻两个停留位置的间距而得到，或者根据所述备选点云簇中的点在z轴方向上的分布，自下而上每固定数量的点或每个高度的点设置一个停留位置而得到。
45.优选地，所述实时杆状物特征提取模块在进行特征精提取时，采用多帧拼接或者降采样的系统维持点云密集度，其中多帧拼接使用帧与帧之间的里程计位姿，将前几帧提取的杆状物特征全部转换到当前帧的坐标系下，共同作为当前帧的杆状物特征。
46.与现有技术相比，本发明具有如下的有益效果：
47.1、综合应用高度过滤、聚类、特征粗提取和特征精提取方法对激光点云地图进行杆状物特征提取，提高提取精度；
48.2、综合应用高度过滤、雷达线数特性过滤、聚类、特征粗提取和特征精提取方法对实时激光点云进行杆状物特征提取，提高提取精度和效率；
49.3、雷达线数特性过滤方法可以快捷地过滤掉墙面、路牌等大面积物体，提高杆状物特征提取效率；
50.4、特征精提取中的滑动窗口方法可以方便地剔除树木的枝杈和底部支撑物，提高树干提取精度。
附图说明
51.通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
52.图1为本技术实施例所提供方法的整体流程图；
53.图2为本技术实施例所提供方法中步骤s2的流程图；
54.图3为本技术实施例所提供方法中步骤s4的流程图；
55.图4为本技术实施例中特征粗提取的流程图；
56.图5为本技术实施例中特征精提取的示意图；
57.图6为本技术实施例中特征精提取的流程图；
58.图7为本技术实施例中特征匹配的流程图；
59.图8为本技术实施例中非线性优化的流程图；
60.图9为本技术实施例的激光点云地图，包含树干点云；
61.图10为本技术实施例的实时激光点云，包含树干点云；
62.图11为本技术实施例中杆状物特征定位误差对比图；
63.图12为本技术实施例中杆状物特征定位轨迹图。
具体实施方式
64.下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
65.根据本发明提供的一种基于激光点云杆状物特征提取和匹配的定位方法，如图1～3所示，包括：
66.步骤s1：利用带有激光雷达的数据采集车对环境进行扫描，得到激光点云地图；
67.步骤s2：对激光点云地图进行高度过滤、聚类、特征粗提取和特征精提取，得到地图杆状物特征；
68.步骤s3：利用车辆的激光雷达对车辆周围环境进行扫描，得到实时激光点云；
69.步骤s4：对实时激光点云进行高度过滤、雷达线数特性过滤、聚类、特征粗提取和特征精提取，得到实时杆状物特征；
70.步骤s5：对地图杆状物特征和实时杆状物特征进行特征匹配和非线性优化，得到车辆位姿。
71.高度过滤包括：在激光点云中选取距离地面高度大于第一高度值的所有点，过滤掉其他点，得到高度过滤后点云。在一个具体实施例中，选取距离地面高度大于1.2m的所有点，过滤掉其他点。
72.雷达线数特性过滤包括：针对高度过滤后点云，利用激光雷达单线扫描点云的特性，将单线点云中的各个点按照扫描的先后顺序依次排列，再根据点与点的间距分割出多个点云段，选取长度在第一长度值以内的所有点云段，过滤掉其他点云段，得到线数特性过滤后点云。在一个具体实施例中，选取长度在60cm以内的所有点云段，过滤掉其他点云段。
73.本技术采用雷达线数特性过滤方法可以快捷地过滤掉墙面、路牌等大面积物体，提高杆状物特征提取效率。
74.聚类包括：对高度过滤后点云或者线数特性过滤后点云进行欧氏聚类，然后选取聚类中心周围第一半径值范围内的所有点，构成对应的聚类点云簇。在一个具体实施例中，选择聚类中心70cm范围内的点云作为聚类点云簇。
75.如图4所示，特征粗提取包括：对聚类点云簇进行主成分分析，提取出最大特征值对应特征向量的z轴分量fz，判断其是否大于第一特征阈值f
th
，若是，则选取聚类点云簇作为备选点云簇，若否，则滤除聚类点云簇。
76.如图6所示，特征精提取包括：
77.步骤t1：将备选点云簇中的所有点按照z坐标从小到大排序，设置一个高度为d的滑动窗口，然后在备选点云簇的z轴方向上，针对滑动窗口自下而上设置m个停留位置，最底部对应第1个停留位置，最顶部对应第m个停留位置；
78.步骤t2：将滑动窗口放置在第一个停留位置上，k＝0，高度差值h1＝0；
79.步骤t3：当滑动窗口位于第i个停留位置时(i＝1,2,
…
,m)，根据z坐标截取处于滑动窗口内的点云，然后对窗口内点云进行主成分分析，提取出最大特征值对应特征向量的z轴分量g
zi
；
80.步骤t4：判断g
zi
是否大于第二阈值g
th
，若是，进入步骤t5，若否，令高度差值hi＝0且令k＝0，然后进入步骤t6；
81.步骤t5：k的值加1，然后判断k值的大小，若k＝1，记录此时滑动窗口的起点位置h
l1
和终点位置h
u1
，计算高度差值hi＝h
u1
–hl1
，若k》1，记录此时滑动窗口的终点位置h
ek
，计算高度差值hi＝h
uk
–hl1
；
82.步骤t6：判断i是否等于m，若否，则将滑动窗口向上移动到下一个停留位置，即i的值加1，然后跳回步骤t3，若是，则进入步骤t7；
83.步骤t7：从所有高度差值hi(i＝1,2,
…
,m)中选出最大的高度差值h
max
，确定h
max
对应的上边界hu和下边界h
l
，然后选取备选点云簇中位于hu和h
l
之间的点，得到目标点云簇，即杆状物特征。
84.滑动窗口的m个停留位置，根据备选点云簇在z轴方向上的高度，通过固定设置相
邻两个停留位置的间距得到，或者根据备选点云簇中的点在z轴方向上的分布，自下而上每固定数量的点或每个高度的点设置一个停留位置得到。
85.在一个具体实施例中，设置滑动窗口的高度为45cm，针对滑动窗口自下而上设置10个停留位置，最底部对应第1个停留位置，最顶部对应第10个停留位置。
86.在特征精提取中采用滑动窗口方法，可以方便地剔除树木的枝杈和底部支撑物，提高树干提取精度。
87.在步骤s4的特征精提取中，采用多帧拼接或者降采样的方法维持点云密集度，其中多帧拼接使用帧与帧之间的里程计位姿，将前几帧提取的杆状物特征全部转换到当前帧的坐标系下，共同作为当前帧的杆状物特征。
88.本技术综合应用高度过滤、聚类、特征粗提取和特征精提取方法对激光点云地图进行杆状物特征提取，提高提取精度。
89.在一个具体实施例中，如图7所示，步骤s5中的特征匹配包括：
90.将地图树干特征集合所有树干中心点插入kd-tree中；
91.遍历实时帧所有树干的中心点，在kd-tree中找出地图在某个范围内的树干中心并得到对应地图树干点云；
92.判断查找结果是否唯一，对于唯一结果直接构造地图
‑‑‑
实时树干的匹配对；若结果不唯一，则比较维度相似性(长宽比与长高比乘积的差值)，选择最佳得分作为最佳匹配对。
93.在一个具体实施例中，如图8所示，步骤s5中的非线性优化包括：
94.设置迭代次数，每一次迭代的初始位姿是上一次迭代优化后的位姿，第一个初始位姿是实时帧带有噪声的车辆位姿；
95.每次迭代都将实时树干点云{p
lidar
}转化到地图坐标系下{p
map
}，并将匹配的地图树干所有点插入kd-tree中；
96.对于{p
map
}中的每个点，在kd-tree中找到匹配地图树干点云的最近点p
map’(需要确保地图树干点云的点不被重复查找)，构造匹配点对{p
lidar
,p
map’}；
97.利用所有匹配点对{p
lidar
,p
map’}与位姿(其中x、y、yaw为优化变量而roll、pitch、z为常量)，构造残差：
[0098][0099]
对于每个构造的匹配对，如果误差e值小于阈值或达到迭代次数，则终止迭代，输出位姿，否则以优化后的位姿重新开始。
[0100]
在一个具体实施例中，从激光点云地图中提取出的杆状物特征如图9所示，从实时激光点云从提取出的杆状物特征如图10所示，通过杆状物特征进行定位的误差对比如图11所示，通过杆状物特征进行定位的轨迹对比如图12所示。其中，误差是位姿与真值的欧式距离，真值是建图的位姿，输入位姿在真值基础上添加了20～45cm的随机噪声。从图中可以看出，使用杆状物特征进行定位可以将误差控制在10cm以内。
[0101]
本发明还提供一种基于激光点云杆状物特征提取和匹配的定位系统，包括：
[0102]
激光点云地图构建模块：利用带有激光雷达的数据采集车对环境进行扫描，得到
激光点云地图；
[0103]
地图杆状物特征提取模块：对激光点云地图进行高度过滤、聚类、特征粗提取和特征精提取，得到地图杆状物特征；
[0104]
实时激光点云获取模块：利用车辆的激光雷达对车辆周围环境进行扫描，得到实时激光点云；
[0105]
实时杆状物特征提取模块：对实时激光点云进行高度过滤、雷达线数特性过滤、聚类、特征粗提取和特征精提取，得到实时杆状物特征；
[0106]
位姿获取模块：对地图杆状物特征和实时杆状物特征进行特征匹配和非线性优化，得到车辆位姿。
[0107]
地图杆状物特征提取模块和实时杆状物特征提取模块进行高度过滤的单元包括在激光点云中选取距离地面高度大于第一高度值的所有点，过滤掉其他点，得到高度过滤后点云。
[0108]
实时杆状物特征提取模块进行雷达线数特性过滤的单元包括针对高度过滤后点云，利用激光雷达单线扫描点云的特性，将单线点云中的各个点按照扫描的先后顺序依次排列，再根据点与点的间距分割出多个点云段，选取长度在第一长度值以内的所有点云段，过滤掉其他点云段，得到线数特性过滤后点云。
[0109]
地图杆状物特征提取模块和实时杆状物特征提取模块进行聚类的单元包括对高度过滤后点云或者线数特性过滤后点云进行欧氏聚类，然后选取聚类中心周围第一半径值范围内的所有点，构成对应的聚类点云簇。
[0110]
地图杆状物特征提取模块和实时杆状物特征提取模块进行特征粗提取的单元包括对聚类点云簇进行主成分分析，提取出最大特征值对应特征向量的z轴分量fz，判断其是否大于第一特征阈值f
th
，若是，则选取聚类点云簇作为备选点云簇，若否，则滤除聚类点云簇。
[0111]
地图杆状物特征提取模块和实时杆状物特征提取模块进行特征精提取的单元包括：
[0112]
部件m1：将备选点云簇中的所有点按照z坐标从小到大排序，设置一个高度为d的滑动窗口，然后在备选点云簇的z轴方向上，针对滑动窗口自下而上设置m个停留位置，最底部对应第一个停留位置，最顶部对应第m个停留位置；
[0113]
部件m2：将滑动窗口放置在第一个停留位置上，k＝0，高度差值h1＝0；
[0114]
部件m3：当滑动窗口位于第i个停留位置时(i＝1,2,
…
,m)，根据z坐标截取处于滑动窗口内的点云，然后对窗口内点云进行主成分分析，提取出最大特征值对应特征向量的z轴分量g
zi
；
[0115]
部件m4：判断g
zi
是否大于第二阈值g
th
，若是，进入部件m5，若否，令高度差值hi＝0且令k＝0，然后进入部件m6；
[0116]
部件m5：k的值加1，然后判断k值的大小，若k＝1，记录此时滑动窗口的起点位置h
l1
和终点位置h
u1
，计算高度差值hi＝h
u1
–hl1
，若k》1，记录此时滑动窗口的终点位置h
ek
，计算高度差值hi＝h
uk
–hl1
；
[0117]
部件m6：判断i是否等于m，若否，则将滑动窗口向上移动到下一个停留位置，即i的值加1，然后跳回部件m3，若是，则进入部件m7；
[0118]
部件m7：从所有高度差值hi(i＝1,2,
…
,m)中选出最大的高度差值h
max
，确定h
max
对应的上边界hu和下边界h
l
，然后选取备选点云簇中位于hu和h
l
之间的点，得到目标点云簇，即杆状物特征。
[0119]
滑动窗口的m个停留位置，根据备选点云簇在z轴方向上的高度，通过固定设置相邻两个停留位置的间距而得到，或者根据备选点云簇中的点在z轴方向上的分布，自下而上每固定数量的点或每个高度的点设置一个停留位置而得到。
[0120]
实时杆状物特征提取模块在进行特征精提取时，采用多帧拼接或者降采样的系统维持点云密集度，其中多帧拼接使用帧与帧之间的里程计位姿，将前几帧提取的杆状物特征全部转换到当前帧的坐标系下，共同作为当前帧的杆状物特征。
[0121]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
[0122]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。