基于同心圆扇区划分的点云动态物搜索及其去除方法与流程

本发明涉及自动驾驶激光雷达点云数据领域，具体地说，本发明涉及同心圆扇区划分的点云动态物搜索及其去除方法。

背景技术：

1、在结构化道路动态环境建图时，不可避免地会遇到动态物的干扰，如：车辆、行人等。这些动态物对前端配准的精度和稳定性的影响是不可忽视的，同时，若slam生成的地图中含有大量的动态点，会对路径规划和轨迹规划产生不利影响，故搜索和去除动态物至关重要。

2、现有技术中常用的动态物体去除方法按策略可以分为两类：(1)slam过程中考虑前后帧间的局部信息匹配，在线去除动态点云，(2)slam过程后再进行后处理，考虑全局地图信息，去除动态点云；单独使用一种策略去除动态点云算法的鲁棒性不强、准确率不高。

技术实现思路

1、本发明提供同心圆扇区划分的点云动态物搜索及其去除方法，能有效去除建图过程以及建图结束后存在的动态物体，很大程度上减少了前端点云误匹配概率，给路径规划和轨迹规划提供了可靠的地图支持。

2、为了实现上述目的，本发明采取的技术方案为：同心圆扇区划分的点云动态物搜索及其去除方法，具体包括以下步骤：

3、步骤s1，为保持点云的整体轮廓和形状，采用改进的体素滤波方式，对激光雷达采集的每帧点云去除噪音点和离群点；

4、步骤s2，对步骤s1生成的点云数据的前后帧点进行栅格化处理，并划分同心圆扇区，通过地面拟合，并计算点云到地面的距离，分割出地面与非地面点云，高亮显示非地面区域点云；

5、步骤s3，相邻帧间非地面拼接，非地面点云聚类，计算前后帧非地面区域点云的相似性，求出动态点，进行欧式聚类，提取出点云动态物；

6、步骤s4，为预防因外界因素干扰导致的slam过程中前后帧动态物去除不彻底问题，提出在slam建图后，利用局部先验地图和点云结合的当时，采用改进的erasor算法去除动态点，准确地去除动态物。

7、优选的，所述步骤s1的具体步骤为，

8、步骤s1.1：为一帧点云数据创建一个3d体素栅格，此过程采用pcl的voxelgrid类处理；

9、步骤s1.2：在每个体素中，将距离体素重心点最近的点作为体素中心，用体素中心来替代体素内的所有点云，设置体素大小为5cm*5cm*5cm，对于所有体素处理后得到过滤后的点云；

10、步骤s1.3：保存滤波后的点云数据。

11、优选的，所述步骤s2的具体步骤为，

12、步骤s2.1：根据当前帧的距离和时间戳信息，找到距离当前帧20米半径范围内并且时间间隔2s的关键帧作为候选帧，并将前后两帧点云转化为2d的扇形栅格地图，将每帧的点云划分同心圆；

13、步骤s2.2：对每个扇区内的点云进行平面拟合，选取高度最低的10个点作为种子点向上进行5次区域生长，找出地面上的点，并基于点云的法向量利用主成分分析法(pca)计算地面点云的特征值和特征向量，设置阈值，剔除噪点，将特征值最小的特征向量作为地面的法向量，估计每个扇区的地面方程；

14、步骤s2.3：判断每个点到平面的高度时，为减少物体距传感器距离不同导致的高度不准确问题，对距离越远的点乘以越小的系数(按照距离的远近，设置特定的系数，每5米距离系数减小0.05)，若高度小于阈值，则该点为路面点，否则为非路面点；

15、步骤s2.4：将同心圆内每个扇区的地面拼接在一起，将多个同心圆的扇区进行拼接，生成当前帧的地面点集，并将非地面区域的点云进行高亮显示。

16、优选的，所述步骤s2.2中，假设平面方程为ax+by+cz＝d,具体过程如下：

17、步骤s2.21：计算某个扇区内所有点云坐标的均值

18、步骤s2.22：利用pca算法求出法向量；

19、步骤s2.23：求出d，求均值点到平面的距离为

20、步骤s2.24：计算该扇区内所有点到平面的距离：

21、

22、步骤s2.25：计算出距离di的标准差：

23、其中为

24、步骤s2.26：当σ>2σ是，认为是噪点，去掉这些点，反之则保留；

25、步骤s2.27：将保留的计算平面方程，重复执行步骤s2.23到步骤s2.26；

26、步骤s2.28：直到所有的点到平面的距离都在阈值(0.5m)以内，结束迭代，取到地面方程。

27、优选的，所述步骤s3具体步骤为，

28、步骤s3.1：将下一帧的点云也采用步骤s2的方式进行地面与非地面的分割，同时将相邻两帧非地面点云进行拼接；

29、步骤s3.2：利用八叉树对非地面点云建立拓扑模型，根据k近邻搜索，通过计算欧式聚类，对非地面点云进行聚类，通过设置距离阈值，确定为前后帧点云中可能的动态点，不断的验证后续帧，若这些点能被后续的激光束穿过，则为动态点；

30、步骤s3.3：利用欧式聚类算法，将距离小于设定阈值的聚类到动态物点集。

31、优选的，所述步骤s4的具体步骤为：

32、步骤s4.1：使用先验地图作为参考，将t时刻原始点云和局部先验地图转换为鸟瞰图；

33、步骤s4.2：选取步骤3中去除动态点后的一帧点云的中心作为圆心，将原有的扇形划分方式进行改进，按激光线扫描的光束作为同心圆(同心圆的数量为15)，最内部有的圆采用均匀划分成n个扇区，之后激光束每外扩一圈，同心圆均匀划分的扇区越少；

34、步骤s4.3：对每个扇区内的点云，计算z坐标最大的前5个点的中位数作为最大值，再取z坐标最小的5个点的中位数作为最小值，计算最大值与最小值之差作为该扇区的特征值，对比原始点云和局部地图对应的扇区，若原始点云扇区的高度差远小于0.2倍的局部地图的高度差，则原始点云中的扇区被认为存在潜在的动态区域；

35、步骤s4.4：在潜在的动态区域中，弃用pca算法，采用最小二乘法拟合平面，将距离地面0.5m以上的动态点剔除。

36、采用以上技术方案的有益效果是：

37、1、本发明能有效去除建图过程以及建图结束后存在的动态物体，很大程度上减少了前端点云误匹配概率，给路径规划和轨迹规划提供了可靠的地图支持。

38、2、本发明在去除动态物的同时，有效分割出地面与非地面点云，为后期地面交通标志的识别与检测，提供了准确的感兴趣区域，减少了冗余数据，有利于提高识别与检测精度，提高工作效率。

技术特征：

1.同心圆扇区划分的点云动态物搜索及其去除方法，其特征在于：具体包括以下步骤：

2.根据权利要求1所述的同心圆扇区划分的点云动态物搜索及其去除方法，其特征在于：所述步骤s1的具体步骤为，

3.根据权利要求1所述的同心圆扇区划分的点云动态物搜索及其去除方法，其特征在于：所述步骤s2的具体步骤为，

4.根据权利要求3所述的同心圆扇区划分的点云动态物搜索及其去除方法，其特征在于：所述步骤s2.2中，假设平面方程为ax+by+cz＝d,具体过程如下：

5.根据权利要求1所述的同心圆扇区划分的点云动态物搜索及其去除方法，其特征在于：所述步骤s3具体步骤为，

6.根据权利要求1所述的同心圆扇区划分的点云动态物搜索及其去除方法，其特征在于：所述步骤s4的具体步骤为：

技术总结
本发明公开了基于同心圆扇区划分的点云动态物搜索及其去除方法，具体包括以下步骤：步骤S1，为保持点云的整体轮廓和形状，采用改进的体素滤波方式，对激光雷达采集的每帧点云去除噪音点和离群点；步骤S2，对步骤S1生成的点云数据的前后帧点进行栅格化处理，并划分同心圆扇区，通过地面拟合，并计算点云到地面的距离，分割出地面与非地面点云，高亮显示非地面区域点云；步骤S3，相邻帧间非地面拼接，非地面点云聚类，计算前后帧非地面区域点云的相似性，求出动态点。本发明能有效去除建图过程以及建图结束后存在的动态物体，很大程度上减少了前端点云误匹配概率，给路径规划和轨迹规划提供了可靠的地图支持。

技术研发人员：徐秀芝,徐礼成,杨德森,肖杨,管延刚
受保护的技术使用者：浙江飞碟汽车制造有限公司
技术研发日：
技术公布日：2024/1/15