一种剔除点云地图中动态物体点云的方法与流程

1.本发明涉及辅助驾驶技术，尤其涉及一种剔除点云地图中动态物体点云的方法。


背景技术：

2.随着近几年来汽车产业的飞速发展，辅助驾驶技术特别是高级辅助驾驶技术成为了各大车企争相抢占的技术制高点。各大车企竞相推出搭载l2 和l2.99级别的辅助驾驶技术的车型。
3.目前实现高级辅助驾驶主要面临定位，感知，规划和控制这几个细分领域的技术难题。其中定位技术解决的不仅仅是车辆在哪条路，行驶方向是朝哪个方向的简单问题，还要负责高速高级辅助驾驶系统车辆当前所处的车道以及车辆在车道中的准确横向和纵向位置。
4.目前解决定位问题最为普遍的方法是使用先验地图作为参考，结合车辆传感器扫描到的车辆周边的环境信息进行匹配，得出车辆在先验地图中的位置。地图的正确性直接影响到了后期的定位精度，这就要求见图过程中要尽量多的还原静态场景信息，尽可能减少动态物体对地图的影响。
5.目前减少动态物体对地图影响的方法主要有以下几种：使用栅格地图过滤动态物体；通过计算占据概率过滤动态物体；通过最近点迭代算法过滤动态物体；还有近几年出现的使用深度学习训练出的模型过滤动态物体。
6.区别于以上算法，本发明提出一种剔除点云地图中动态物体点云的方法，对地图中的动态目标点云的剔除更彻底，不会误删地图中静态点。


技术实现要素：

7.为解决现有技术中存在的不足，本发明的目的在于，提供一种剔除点云地图中动态物体点云的方法，使用地图关键帧的时间维度的数据对地图中残留的动态目标点云簇进行跟踪，同时在动态目标跟踪的目标关联阶段引入点云数量属性。
8.为实现本发明的目的，本发明所采用的技术方案是：
9.一种剔除点云地图中动态物体点云的方法，包括步骤：
10.(1)车辆正常行驶，数据采集单元同步采集激光雷达扫描得到的周边环境信息的点云数据，以及组合导航的3d位姿信息；
11.(2)动态目标剔除单元根据激光雷达扫描得到的周边环境信息，识别跟踪动态目标点云的运动并且剔除动态目标；
12.(3)地图生成单元根据组合导航的3d位姿和剔除动态目标的点云帧生成周围环境的高精度点云地图。
13.进一步地，步骤(1)中，同步采集组合导航输出的3d位姿和激光雷达输出的扫描数据，得到组合导航位姿序列position_queue_rtk和激光雷达扫描数据帧序列pointcloud_queue_lidar。
14.进一步地，步骤(2)中，具体包括步骤：
15.(2.1)从激光雷达扫描数据帧序列pointcloud_queue_lidar的第一帧开始，提取连续5帧激光雷达扫描数据到子序列pointcloud_queue_submap；
16.(2.2)从子序列pointcloud_queue_submap的第一帧开始，提取1帧激光雷达扫描数据并且对这帧点云执行非地面点提取算法，得到该帧点云中的非地面点存储到非地面点序列pointcloud_none_ground；
17.(2.3)利用组合导航位姿序列position_queue_rtk和子序列pointcloud_queue_submap激光雷达扫描数据同步采集的位姿信息，结合组合导航到激光雷达的外参矩阵拼接除了当前点云帧以外的相邻点云帧，得到局部点云地图submap_lidar；
18.(2.4)遍历非地面点序列pointcloud_none_ground中的每一个点，计算这个点在局部点云地图submap_lidar中的最邻近点，及到最邻近点的距离；存储距离小于阈值的非地面点序列pointcloud_none_ground中的点到可能运动点簇pointcloud_moving；
19.(2.5)在可能运动点簇pointcloud_moving中执行欧式聚类算法米，删除掉聚类中点的数量小于阈值的聚类簇，得到运动目标聚类簇cluster_moving[i]；
[0020]
(2.6)重复步骤2.2到步骤2.5直到子序列pointcloud_queue_submap中的所有点云都遍历得到运动目标聚类簇cluster_moving[i]，i的范围0-4；
[0021]
(2.7)重复步骤2.1到步骤2.6直到激光雷达扫描数据帧序列pointcloud_queue_lidar中的所有点云都遍历得到运动目标聚类簇cluster_moving[i]，i的范围0
‑‘
n-1’；
[0022]
(2.8)从i＝0开始，跟踪运动目标聚类簇cluster_moving[i]中的动态目标聚类簇的运动轨迹，得到运动目标点云的速度和运动方向；
[0023]
(2.9)提取步骤2.8中数据关联成功且速度大于阈值的运动目标点云聚类簇到待剔除点云聚类簇custer_moving_verified[i]；
[0024]
(2.10)遍历激光雷达扫描数据帧序列pointcloud_queue_lidar，剔除点云帧pointcloud_queue_lidar[i]中的动态目标点云聚类簇custer_moving_verified[i]，得到剔除动态目标点云后的点云帧pointcloud_queue_lidar_none_moving[i]。
[0025]
进一步地，步骤(2.8)中，从i＝0开始取cluster_moving[i]和cluster_moving[i 1]，结合cluster_moving[i]和cluster_moving[i 1]各自所在点云帧采集时的时间信息，组合导航位姿信息和组合导航到激光雷达的外参矩阵计算出cluster_moving[i]中的一个运动物体聚类簇运动到cluster_moving[i 1]中的对应位置的速度和运动方向。
[0026]
进一步地，使用数据关联判断cluster_moving[i]和cluster_moving[i 1]哪些点云簇是两帧点云簇中的同一个运动目标的点云；该数据关联属性为，聚类簇cluster_moving[i 1]中的一个运动目标的聚类簇中的点的数量与聚类簇cluster_moving[i]中的一个运动目标的聚类簇中的点的数量的比值。
[0027]
进一步地，步骤(3)中，利用位姿序列position_queue_rtk和剔除动态目标点云后的点云帧pointcloud_queue_lidar_none_moving[i]激光雷达扫描数据同步采集的位姿信息，结合组合导航到激光雷达的外参矩阵拼接从i＝0到i＝n-1的相邻点云帧，得到不包含动态目标点云的静态点云地图map_lidar_static。
[0028]
一种剔除点云地图中动态物体点云的系统，包括安装于车辆顶部的激光雷达和组合导航，以及数据采集单元、动态目标剔除单元和地图生成单元；用以实现所述的剔除点云地图中动态物体点云的方法。
[0029]
本发明的有益效果在于，与现有技术相比，本发明利用点云帧在时间和空间两个维度上的信息，跟踪动态目标点云的运动(速度和运动方向)，减少对静态目标的误删除。
[0030]
本发明在使用数据关联跟踪运动目标所在点云的运动时，引入相邻点云帧中目标聚类簇的点的数量的比值作为数据关联时的一个新属性，提升了单纯使用激光雷达输出的点云跟踪运动目标的效果(同一个目标被关联的正确性提升)。
附图说明
[0031]
图1是本发明所述的剔除点云地图中动态物体点云的方法流程图。
具体实施方式
[0032]
下面结合附图和实施例对本发明的技术方案作进一步的说明。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本技术的保护范围。
[0033]
本发明所述的剔除点云地图中动态物体点云的方法，需要用到的系统硬件架构包括安装于车辆顶部的机械激光雷达和安装于车辆顶部的组合导航。激光雷达扫描得到周边环境信息，组合导航采集3d位姿信息。
[0034]
组合导航发出1hz的pps卫星同步信号到安装于车辆顶部的激光雷达，激光雷达接收到pps卫星同步信号后，同步激光雷达的采集时间到组合导航时间(也叫gps时间)。
[0035]
本发明所述的剔除点云地图中动态物体点云的方法，需要用到的系统软件架构包括数据采集单元、动态目标剔除单元和地图生成单元。
[0036]
数据采集单元，同步采集激光雷达扫描得到的周边环境信息的点云数据，以及同步采集组合导航的3d位姿信息。
[0037]
动态目标剔除单元，使用激光雷达扫描得到的周边环境信息，识别、跟踪并且剔除运动目标。
[0038]
地图生成单元，使用组合导航输出的3d位姿和激光雷达采集得到的点云帧(剔除动态物体)，生成周围环境的高精度点云地图。
[0039]
如图1所示，本发明所述的剔除点云地图中动态物体点云的方法，包括步骤：
[0040]
(1)车辆在待测绘的道路上正常行驶，数据采集单元同步采集激光雷达和组合导航数据；
[0041]
其中，将组合导航到激光雷达的外参矩阵，记做
[0042]
(2)连续的同步采集组合导航输出的3d位姿和激光雷达输出的扫描数据帧，得到组合导航位姿序列position_queue_rtk和激光雷达扫描数据帧序列pointcloud_queue_lidar；
[0043]
(3)从激光雷达扫描数据帧序列pointcloud_queue_lidar的第一帧开始，提取连续5帧激光雷达扫描数据到子序列pointcloud_queue_submap；
[0044]
(4)从子序列pointcloud_queue_submap的第一帧开始，提取1帧激光雷达扫描数据并且对这帧点云执行非地面点提取算法，得到的该帧点云中的非地面点存储到非地面点
序列pointcloud_none_ground；
[0045]
(5)利用组合导航位姿序列position_queue_rtk和子序列pointcloud_queue_submap激光雷达扫描数据同步采集的位姿信息，结合组合导航到激光雷达的外参矩阵拼接除了当前点云帧以外的相邻点云帧，得到局部点云地图submap_lidar；
[0046]
(6)遍历非地面点序列pointcloud_none_ground中的每一个点，计算这个点在局部点云地图submap_lidar中的最邻近点，及到最邻近点的距离；存储距离小于阈值(例如0.1米)的pointcloud_none_ground中的点到可能运动点簇pointcloud_moving；
[0047]
(7)可能运动点簇pointcloud_moving中执行欧式聚类算法(欧氏距离小于0.1米)，并且删除掉聚类中点的数量小于阈值(10个点)的聚类簇，得到运动目标聚类簇cluster_moving[i]；
[0048]
(8)重复步骤4到步骤7直到子序列pointcloud_queue_submap中的所有点云都被遍历得到了运动目标聚类簇cluster_moving[i](i的范围0-4)；
[0049]
(9)重复步骤3到步骤8直到激光雷达扫描数据帧序列pointcloud_queue_lidar中的所有点云都被遍历得到了运动目标聚类簇cluster_moving[i](i的范围0
‑‘
n-1’)；
[0050]
(10)从i＝0开始，跟踪运动目标聚类簇cluster_moving[i]中的动态目标聚类簇的运动轨迹，得到运动目标点云的速度和运动方向；
[0051]
a、cluster_moving[i]代表了当前时刻点云帧中的动态目标在3d空间中的位置信息；
[0052]
b、从i＝0开始取cluster_moving[i]和cluster_moving[i 1]，结合cluster_moving[i]和cluster_moving[i 1]各自所在点云帧采集时的时间信息，组合导航位姿信息和组合导航到激光雷达的外参矩阵可计算出cluster_moving[i]中的一个运动物体聚类簇运动到cluster_moving[i 1]中的对应位置的速度和运动方向；
[0053]
c、步骤b中需要使用数据关联来判断cluster_moving[i]和cluster_moving[i 1]哪些点云簇是两帧点云簇中的同一个运动目标的点云；
[0054]
在传统的数据关联属性(速度，运动方向)之外，本发明引入一个新的数据关联属性，聚类簇cluster_moving[i 1]中的一个运动目标的聚类簇中的点的数量与聚类簇cluster_moving[i]中的一个运动目标的聚类簇中的点的数量的比值；若动态目标的运动方向和采集车辆的运动方向相反则采用这个比值代入数据关联计算，若动态目标的运动方向和采集车辆的运动方向相同则采用这个比值的倒数代入数据关联计算。
[0055]
(11)提取步骤10中数据关联成功且速度大于阈值(0.1m/s)的运动目标点云聚类簇到待剔除点云聚类簇custer_moving_verified[i]；
[0056]
速度小于阈值和未数据关联成功的点，被认为是由于雷达从不同角度扫描到同一个静态物体造成的伪动态目标，不加入待剔除点云聚类簇custer_moving_verified[i]。
[0057]
(12)遍历激光雷达扫描数据帧序列pointcloud_queue_lidar，剔除点云帧pointcloud_queue_lidar[i]中的动态目标点云聚类簇custer_moving_verified[i]，得到剔除动态目标点云后的点云帧pointcloud_queue_lidar_none_moving[i]。
[0058]
(13)利用位姿序列position_queue_rtk和pointcloud_queue_lidar_none_moving[i]激光雷达扫描数据同步采集的位姿信息，结合组合导航到激光雷达的外参矩阵
拼接从i＝0到i＝n-1的相邻点云帧得到不包含动态目标点云的静态点云地图map_lidar_static。
[0059]
本发明的有益效果在于，与现有技术相比，本发明利用点云帧在时间和空间两个维度上的信息，跟踪动态目标点云的运动(速度和运动方向)，减少对静态目标的误删除。
[0060]
本发明在使用数据关联跟踪运动目标所在点云的运动时，引入相邻点云帧中目标聚类簇的点的数量的比值作为数据关联时的一个新属性，提升了单纯使用激光雷达输出的点云跟踪运动目标的效果(同一个目标被关联的正确性提升)。
[0061]
本发明申请人结合说明书附图对本发明的实施示例做了详细的说明与描述，但是本领域技术人员应该理解，以上实施示例仅为本发明的优选实施方案，详尽的说明只是为了帮助读者更好地理解本发明精神，而并非对本发明保护范围的限制，相反，任何基于本发明的发明精神所作的任何改进或修饰都应当落在本发明的保护范围之内。