CN113655498B - 一种基于激光雷达的赛道中锥桶信息的提取方法及系统 - Google Patents

Abstract

本发明涉及一种基于激光雷达的赛道中锥桶信息的提取方法及系统。所述提取方法包括步骤：根据赛道中的锥桶位置将赛道区域分割成锥桶区域，以使所需锥桶均位于锥桶区域内；对锥桶区域进行水平校准，以使锥桶区域在雷达坐标系中保持水平；对锥桶区域的地面点云进行提取处理，得到非地面点云；对非地面点云进行半径滤波处理，以滤除悬空点或无效点，得到精确点云；对精确点云进行聚类，滤除锥桶外的非地面点云，得到锥桶的位置信息。对锥桶点云进行重建优化，优化显示效果；本发明通过过滤掉赛道区域内远处无用的扫描点，同时过滤掉激光雷达附近噪点的干扰以及赛车本身发射导致的干扰点，从而能够减少对扫描点的运算强度。

Description

一种基于激光雷达的赛道中锥桶信息的提取方法及系统

技术领域

本发明涉及智能车辆环境感知领域，特别是涉及一种基于激光雷达的赛道中锥桶信息的提取方法及系统。

背景技术

无人驾驶方程式赛车比赛的项目有四项，分别为直线加速、8字环绕、高速避障以及耐久测试。这需要考验赛车的环境感知、导航定位、路径规划以及车辆控制的综合能力。赛车在行驶时需要依据相机和激光雷达识别锥桶的位置和颜色信息，以便赛车能够平稳、安全地通过赛道。

目前赛车在对赛道上的锥桶信息进行提取时存在不足，例如赛车在行驶时因其抖动导致识别的锥桶位置出现误差，周围障碍物的干扰会影响锥桶信息的提取准确率等，这些因素均会造成提取锥桶信息时产生一定的误差。

发明内容

基于此，为了能够解决传统赛车难以精确提取赛道上的锥桶信息的技术问题，提供一种基于激光雷达的赛道中锥桶信息的提取方法及系统。

本发明采用以下技术方案实现：一种基于激光雷达的赛道中锥桶信息的提取方法，其用于提取赛道上的锥桶的位置信息，所述锥桶信息的提取方法包括步骤：

步骤S1，对赛道区域实时扫描，得到赛道点云，且对所述赛道点云根据不同赛场情况设定的截取范围，截取一个锥桶区域；

步骤S2，对所述锥桶区域根据一个提取平面的平面方程进行水平校准，以使所述锥桶区域在雷达坐标系中保持水平；所述水平校准的方法包括以下步骤：

步骤S21，定义所述提取平面的平面方程：ax+by+cz+d＝0，其中，x、y、z分别是所述锥桶区域的x轴、y轴、z轴，a、b、c分别是x、y、z的法向量，d为所述提取平面相对于坐标原点的偏移量，所述坐标原点指所述提取平面所在的坐标系的原点；

步骤S22，对激光雷达坐标系中的竖直向量进行定义，将所述提取平面的法向量通过所述竖直向量进行旋转，得到旋转矩阵；

步骤S23，将所述旋转矩阵通过点云旋转函数应用到所述提取平面，完成所述锥桶区域的水平校准过程；

步骤S3，对所述锥桶区域内的点云进行过滤，分离所述锥桶区域内的点云中的地面点云和非地面点云；

步骤S4，对所述非地面点云进行半径滤波处理，过滤所述非地面点云中的悬空点和无效点，得到精确点云；

步骤S5，通过聚类所述精确点云得到所述锥桶区域内的所有锥桶的位置信息；

其中，所述精确点云在聚类前或者后，对所述精确点云进行重建优化，优化显示效果，所述重建优化的方法包括以下步骤：

步骤S61，将所述精确点云通过任意法线投影到一个与所述任意法线相对应的投影平面上，得到投影点云；

步骤S62，对所述投影点云作平面内的三角化处理，得到各个投影点之间的连接关系；

步骤S63，根据各个投影点之间的连接关系确定各原始三维点之间的拓扑连接关系，根据所述拓扑连接关系形成三角网格曲面，所得的三角网格曲面即为重建得到的曲面模型，优化显示效果；其中，所述原始三维点指投影前的所述精确点云。

本发明通过对锥桶区域进行水平校准，能够及时纠正赛车受到道路因素的干扰时产生的偏差，利用提取的平面的法向量和雷达坐标系中的竖直向量来进行水平校准，提高了锥桶位置信息的准确性，提供稳定的锥桶信息。通过半径滤波处理，能够滤去非地面点云内部的干扰点，对非地面点云中存在的一些悬空的孤立点或无效点去除，能够进一步提升锥桶位置的精确度。通过对锥桶点云进行重建优化处理，能够优化显示效果。

作为上述方案的进一步改进，所述半径滤波处理的方法包括步骤：

步骤S41，选取所述非地面点云中的一个点，并将其定义为点A，以点A作为圆心以一个距离阈值作为半径，形成一个滤波圆，判断所述滤波圆内是否包含至少两个点：若为是，则将点A保留；若为否，则将点A删除；

步骤S42，重复执行步骤S41，直至所有的非地面点云中的点均完成处理为止，将所有保留的点A组合，得到精确点云。

作为上述方案的进一步改进，所述截取范围以赛道内所需锥桶均位于所述锥桶区域内作为标准而设定。

优选地，所述锥桶区域的截取方法包括以下步骤：

步骤S11，对所述赛道区域中的锥桶位置进行预估计，将所述赛道区域分割成初始区域，所需锥桶均位于所述初始区域内；

步骤S12，判断所述初始区域内的赛道点云中的任一点与所述激光雷达之间的间距是否位于一个预设的距离范围内：若为是，则将相应的点保留；若为否，则剔除相应的点；

步骤S13，重复执行步骤S12，直至所有的赛道点云中的点均完成处理为止，将所有保留的赛道点组合，得到一个锥桶区域，且所需锥桶均位于所述锥桶区域内。

作为上述方案的进一步改进，所述点云中的地面点云和非地面点云的分离方法包括以下步骤：

步骤S31，加载所述锥桶区域内的点云，划分为规则网格，在每个网格中选取高程最低点，构建最小高程表面；其中，如果网格中没有高程最低点，则根据最近邻点进行插值；

步骤S32，使用一个宽度尺寸为w₁的窗口以及设定的高程差阈值dh_T来对原始点云进行开操作：地形特征中小于w₁的部分为局外点，反之，地形特征中大于w₁的部分为局内点，其中，所述原始点云指所述锥桶区域内的点云数据；并假设dh_p,1代表原始点云与在任意给定p1点处第一次迭代表面之间的高程差，dh_T代表高程差阈值，则如果dh_p,1≤dh_T点p1就被认为是局内点，反之如果dh_p,1>dh_T就认为点p1是一个局外点；此后，令dh_max(t),1为当前迭代中初始地面点与滤波表面之间差值的最大值，则如果选取的dh_T>dh_max(t),1则所有的测量值都会保留为局内点；

步骤S33，进行下一次迭代，计算新的滤波窗口尺寸，新的滤波窗口的宽度尺寸增大为w₂，对于上一次的处理结果进行开操作处理，以此迭代，通过逐渐增加窗口尺寸来去除不同大小的局外点，其中逐渐增加的窗口的宽度尺寸w_k＝2w₁ ^k+1，式中k为迭代次数，w₁是初始窗口大小；重复执行步骤S32，直至窗口尺寸达到设定的最大窗口的宽度尺寸w_max为止；其中，所述局内点的集合为地面点云，所述局外点的集合为非地面点云。

作为上述方案的进一步改进，在步骤S5中，所述聚类的方法包括步骤：

步骤S51，定义一个扩张出的三维空间中，任意一点的投影点为点P2，利用Kd树搜索方法得到k个距离点P2最近的投影点；

步骤S52，判断k个投影点与点P2之间的间距是否均小于一个半径阈值：选择满足条件的点构成集合Q；

步骤S53，选取点P2以外的投影点为新的投影点，重复执行步骤S51，直至所述三维空间中所有的投影点均完成判断为止；

步骤S54，根据锥桶形状特征获得锥桶模型边缘在投影平面内的最大值与最小值，并根据所述最大值与最小值，并滤除锥桶外的投影点，得到锥桶的投影点；

步骤S55，根据所述锥桶的投影点，得到锥桶的位置信息。

优选地，在步骤S41中，所述距离阈值为0.1m。

优选地，在步骤S12中，所述距离范围为0.1～15m；

优选地，在步骤S52中，所述半径阈值为0.3m。

优选地，在步骤S54中，通过点云库中的getMinMax3D函数提取出集合Q在空间位置边缘的最大值与最小值。

本发明还提供一种基于激光雷达的赛道中锥桶信息的提取系统，其采用上述任意基于激光雷达的赛道中锥桶信息的提取方法对赛道上的锥桶信息进行提取；所述提取系统包括：

扫描模块，其用于对赛道区域实时扫描，得到赛道点云，且对所述赛道点云根据不同赛场情况设定的截取范围，截取一个锥桶区域；

水平校准模块，其用于对所述锥桶区域根据一个提取平面的平面方程进行水平校准，以使所述锥桶区域在雷达坐标系中保持水平；

过滤模块，其用于对所述锥桶区域内的点云进行过滤，分离所述锥桶区域内的点云中的地面点云和非地面点云；

滤波模块，其用于对所述非地面点云进行半径滤波处理，过滤所述非地面点云中的悬空点和无效点，得到精确点云；以及

聚类模块，其用于通过聚类所述精确点云得到所述锥桶区域内的所有锥桶的位置信息；

重建优化模块，其用于在聚类前或者后，对所述精确点云进行重建优化，优化显示效果。

作为上述方案的进一步改进，所述扫描模块包括激光雷达。

本发明的有益效果如下：

1、通过对锥桶区域进行水平校准，能够及时纠正赛车受到道路因素的干扰时产生的偏差，利用提取的平面的法向量和雷达坐标系中的竖直向量来进行水平校准，提高了锥桶位置信息的准确性，提供稳定的锥桶信息。通过半径滤波处理，能够滤去非地面点云内部的干扰点，对非地面点云中存在的一些悬空的孤立点或无效点去除，能够进一步提升锥桶位置的精确度。

2、通过过滤掉赛道区域内远处无用的扫描点，同时过滤掉激光雷达附近噪点的干扰以及赛车本身发射导致的干扰点，从而能够减少对扫描点的运算强度，提升了运算效率。通过对锥桶扫描点进行聚类，能够准确得出锥桶的边缘坐标，其与锥桶中心坐标相互配合，能够准确得到锥桶的位置信息，通过对锥桶点云进行重建优化处理，优化显示效果。本发明方法精简，效率高，能够适应不同赛道场景中锥桶信息的提取，稳定性强。

附图说明

图1为本发明实施例1提供的一种基于激光雷达的赛道中锥桶信息的提取方法的流程图。

图2为图1中提取方法的截取方法的流程示意图。

图3为本发明实施例1提供的一种基于激光雷达的赛道中锥桶信息的提取方法中截取锥桶区域后的效果图。

图4为本发明实施例1提供的一种基于激光雷达的赛道中锥桶信息的提取方法中对锥桶区域进行水平校准后的效果图。

图5为图1中提取方法的分离方法的流程示意图。

图6为本发明实施例1提供的一种基于激光雷达的赛道中锥桶信息的提取方法中过滤掉地面点云后的效果图。

图7为图1中提取方法的半径滤波处理的流程示意图。

图8为本发明实施例1提供的一种基于激光雷达的赛道中锥桶信息的提取方法中半径滤波处理后的效果图。

图9为本发明实施例1提供的一种基于激光雷达的赛道中锥桶信息的提取方法中聚类处理后的效果图。

图10为图1中提取方法的重建优化方法的流程示意图。

图11为本发明实施例2提供的一种基于激光雷达的赛道中锥桶信息的提取系统中未对锥桶扫描点进行处理的原始扫描图。

图12为本发明实施例2提供的一种基于激光雷达的赛道中锥桶信息的提取系统中对锥桶扫描点进行处理后的对比扫描图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，当组件被称为“安装于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“设置于”另一个组件，它可以是直接设置在另一个组件上或者可能同时存在居中组件。当一个组件被认为是“固定于”另一个组件，它可以是直接固定在另一个组件上或者可能同时存在居中组件。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“或/及”包括一个或多个相关的所列项目的任意的和所有的组合。

实施例1

请参阅图1，本实施例介绍了一种基于激光雷达的赛道中锥桶信息的提取方法，其用于提取赛道上的锥桶的位置信息。信息提取方法包括步骤：

步骤S1，锥桶区域的截取。

步骤S2，使所述锥桶区域在雷达坐标系中保持水平。

步骤S3，分离所述锥桶区域内的点云中的地面点云和非地面点云。

步骤S4，过滤所述非地面点云中的悬空点和无效点，得到精确点云。

步骤S5，通过聚类所述精确点云得到所述锥桶区域内的所有锥桶的位置信息。

在步骤S1中，对赛道区域实时扫描，得到赛道点云，且对所述赛道点云根据不同赛场情况设定的截取范围，截取一个锥桶区域。赛道点云包括地面点云与非地面点云。在对赛道区域进行扫描时，预先在Ubuntu系统中安装ROS库和PCL库，配置电脑ip与激光雷达进行连接，通过激光雷达对赛道区域进行扫描，加载并显示实时点云。

请结合图2，所述锥桶区域的截取方法包括以下步骤：

步骤S11，对所述赛道区域中的锥桶位置进行预估计，将所述赛道区域分割成初始区域，所需锥桶均位于所述初始区域内。

请结合图3，在本实施例中，根据激光雷达探测周围环境提取锥桶并与摄像头进行联合标定与数据融合，标明锥桶颜色，输出其位置坐标供路径规划板块使用，从而扫描出赛道区域。使用点云库(PCL)库中的pcl::PassThrough直通滤波函数，截取赛道区域点云的部分点云，即分割出一个锥桶分布的区域。该区域可以为矩形，可以为圆形，也可以为其他形状，只要能够满足提取条件即可。例如针对不同赛道情况，在雷达坐标系内直接设定某坐标轴上的某范围，例如在x轴(0.1，15)，y轴(-3，3)，z轴(0.4，1.5)区域，其中，x轴设置该范围的原因是激光雷达探测锥桶的硬件极限(垂直角分辨率为0.33°)，y轴设置该范围的原因是赛道的宽度为3m，z轴设置该范围是根据雷达安装高度和大、小锥桶高度确定。将该区域以内或以外的区域删除，从而得到了一个锥桶分布的区域。

步骤S12，判断所述初始区域内的赛道点云中的任一点与所述激光雷达之间的间距是否位于一个预设的距离范围内：若为是，则将相应的点保留；若为否，则剔除相应的点。

在本实施例中，判断初始区域内赛道点云中的任一点与激光雷达之间的间距是否位于一个预设的距离范围内：若为是，则将相应的点保留；若为否，则剔除相应的点；距离范围为0.1～15m。在本实施例中，利用欧式距离过滤函数剔除掉过近和过远的点(例如剔除掉距离雷达半径小于0.1m和大于15m的点)，以减少雷达附近噪点以及较远无用点云的干扰，从而在一定程度上减少了对点云的运算强度，提升了运算效率。

步骤S13，重复执行步骤S12，直至所有的赛道点云中的点均完成处理为止，将所有保留的赛道点组合，得到一个锥桶区域，且所需锥桶均位于所述锥桶区域内。

在本实施例中，将多个保留的点组成一个锥桶区域，且所需的锥桶均位于锥桶区域内。此时，能够将赛道区域分割成一个锥桶分布的锥桶区域，其他没有锥桶分布的区域的赛道点云被剔除，从而能够有效减少了所需处理的项目，提升了运算效果。

在步骤S2中，对所述锥桶区域根据一个提取平面的平面方程进行水平校准，以使所述锥桶区域在雷达坐标系中保持水平。激光雷达在采集数据时，由于赛车在行驶时不可避免的产生颠簸，或者激光雷达在安装时其安装位置可能会倾斜，又或者赛道的路面本身会有坡度，从而导致激光雷达在对赛道进行扫描时，造成扫描出来的地面不是水平的，从而会影响后续对扫描点的处理，因此需要对扫描的锥桶区域进行水平校准，以提高处理结果的精度。

所述水平校准的方法包括以下步骤：

步骤S21，定义所述提取平面的平面方程：ax+by+cz+d＝0，其中，x、y、z分别是所述锥桶区域的x轴、y轴、z轴，a、b、c分别是x、y、z的法向量，d为所述提取平面相对于坐标原点的偏移量，所述坐标原点指所述提取平面所在的坐标系的原点。

在本实施例中，对锥桶区域的范围进行定义，得到平面：ax+by+cz+d＝0，其中，x、y、z分别是锥桶区域的x轴、y轴、z轴，a、b、c分别是x、y、z的法向量，d为平面方程相对于坐标原点的偏移量。在本实施例中，先设定提取平面的范围，其根据具体赛道场景而定，例如x轴方向为(0.3，6)，y轴方向为(-1，1)，该平面通过PCL中基于Progressive MorphologicalFilter形态学滤波平面检测方法得到。

步骤S22，对激光雷达坐标系中的竖直向量进行定义，将所述提取平面的法向量通过所述竖直向量进行旋转，得到旋转矩阵。

在本实施例中，对雷达坐标系中的竖直向量进行定义，将平面的法向量通过竖直向量进行旋转，得到旋转矩阵。在本实施例中，雷达坐标系中的竖直向量为(0，0，1)。

步骤S23，将所述旋转矩阵通过点云旋转函数应用到所述提取平面，完成所述锥桶区域的水平校准过程。

请结合图4，在本实施例中，将旋转矩阵通过点云旋转函数应用到平面，完成锥桶区域的水平校准过程。在本实施例中，旋转矩阵利用PCL库pcl::transformPointCloud点云旋转函数应用到点云，点云即可得到旋转，从而实现了对锥桶区域的水平校准。

在步骤S3中，激光雷达的一些线束会扫描到地面，由于在对赛道进行扫描时，会在地面和非地面的位置分别扫描出点云，而地面点云会对非地面点云的后续处理造成一定的影响。对于地面点云，应该预先进行过滤，仅保留非地面点云即可，以提高后续的处理效率。

请结合图5及图6，所述点云中的地面点云和非地面点云的分离方法包括以下步骤：

步骤S31，加载点云，划分为规则网格，在每个网格中选取高程最低点(如果网格中没有点则根据最近邻点进行插值)，构建最小高程表面。

步骤S32，使用一个宽度尺寸为w₁的窗口以及设定的高程差阈值dh_T来对原始点云数据进行开操作，地形特征中小于w₁的部分为局外点，反之，地形特征中大于w₁的部分为局内点。并假设dh_p,1代表原始点云与在任意给定p1点处第一次迭代表面之间的高程差，dh_T代表高程差阈值，则如果dh_p,1≤dh_T点p就被认为是局内点，反之如果dh_p,1>dh_T就认为点p1是一个局外点。此后，令dh_max(t),1为当前迭代中初始地面点与滤波表面之间差值的最大值，则如果选取的dh_T>dh_max(t),1则所有的测量值都会保留为局内点。本实例中，高程差阈值dh_T选取0.1m，w₁初始窗口宽度尺寸选取0.2m。

步骤S33，进行下一次迭代，计算新的滤波窗口宽度尺寸，窗口的宽度尺寸增大为w₂，对于上一次的处理结果进行开操作处理，以此迭代，通过逐渐增加窗口宽度尺寸来去除不同大小的局外点，其中窗口的宽度尺寸w_k＝2w₁ ^k+1，式中k为迭代次数，w₁是初始窗口大小。重复执行步骤S32，直至窗口宽度尺寸达到设定的最大窗口的宽度尺寸w_max为止。其中，所述局内点的集合为地面点云，所述局外点的集合为非地面点云。本实例中，最大窗口的宽度尺寸w_max选取15m。

在步骤S4中，对非地面点云进行半径滤波处理，对其中的悬空点或无效点进行过滤，得到精确点云。由于非地面点云包含一些悬空的点或者无效的点，这部分点会对后续的计算产生一定的影响，造成误差，因此需要将这些点进行处理。

请结合图7及图8，所述半径滤波处理的方法包括步骤：

步骤S41，选取所述非地面点云中的一个点，并将其定义为点A，以点A作为圆心以一个距离阈值作为半径，形成一个滤波圆，判断所述滤波圆内是否包含至少两个点：若为是，则将点A保留；若为否，则将点A删除。本实施例中，距离阈值优选为0.1m。

步骤S42，重复执行步骤S41，直至所有的非地面点云中的点均完成处理为止，将所有保留的点A组合，得到精确点云。此时，悬空点或无效点已经被处理完全，并得到精确点云。

请结合图9，在步骤S5中，对精确点云进行聚类，滤除锥桶外的非地面点云，得到锥桶的位置信息。在完成了非地面点云的半径滤波处理后，剔除了非地面点云中的悬空点或无效点，得到了精确点云。此时需要对精确点云进行聚类，以将锥桶外的非地面点云剔除，仅保留锥桶内的非地面点云，从而获得锥桶的位置信息。在本实施例中，采用欧式聚类算法进行聚类，首先创建一个Kd树对象作为提取点云时所用的搜索方法，再创建一个点云索引向量cluster_indices，用于存储实际的点云索引信息，每个检测到的点云聚类被保存在这里。聚类方法包括步骤：

步骤S51，定义扩张出的三维空间中任意一点的投影点为点P2，利用Kd树搜索方法得到k个距离点P2最近的投影点。其中Kd树是K-dimension tree的缩写，是对数据点在k维空间中划分的一种数据结构。Kd树是一种平衡二叉树。为了能有效的找到最近邻，Kd树采用分而治之的思想，即将整个空间划分为几个小部分。Kd树算法的应用可以分为两方面，一方面是有关Kd树本身这种数据结构建立的算法，另一方面是在建立的Kd树上如何进行最邻近查找的算法。Kd树是每个节点均为k维数值点的二叉树，其上的每个节点代表一个超平面，该超平面垂直于当前划分维度的坐标轴，并在该维度上将空间划分为两部分，一部分在其左子树，另一部分在其右子树。即若当前节点的划分维度为d，其左子树上所有点在d维的坐标值均小于当前值，右子树上所有点在d维的坐标值均大于等于当前值，对其任意子节点均成立。

步骤S52，判判断k个投影点与点P2之间的间距是否均小于一个半径阈值：选择满足条件的点构成集合Q。其中，半径阈值选取为0.3m，这是由于体积较小的锥桶的最大边缘距离为0.3m。对于欧式聚类来说，距离判断准则为欧氏距离。对于空间某点P2，通过Kd树近邻搜索算法找到k个离p2点最近的点，这些点中距离小于设定阈值的便聚类到集合Q中。如果Q中元素的数目不在增加，整个聚类过程便结束；否则须在集合Q中选取p2点以外的点，重复上述过程，直到Q中元素的数目不再增加为止。

步骤S53，选取点P2以外的投影点为新的投影点，重复执行步骤S51，直至三维空间中所有的投影点均完成判断为止。

步骤S54，根据锥桶形状特征获得锥桶模型边缘在投影平面内的最大值与最小值，并根据最大值与最小值，并滤除锥桶外的投影点，得到锥桶的投影点。

步骤S55，根据锥桶的投影点，得到锥桶的位置信息。

在步骤S6中，采用贪婪投影三角化算法对非地面点云进行重建优化。贪婪投影三角化算法的原理为：将三维点通过法线投影到某一平面，然后对投影得到的点云作平面内的三角化，从而得到各个点的连接关系。在平面区域的三角化过程中用到了基于Delaunay的空间区域增长算法，该方法通过选取一个样本三角片作为初始曲面，不断扩张曲面边界，最后形成一张完整的三角网格曲面，最后根据投影点云的连接关系确定各原始三维点间的拓扑连接，所得的三角网格即为重建得到的曲面模型。

请结合图10，重建优化方法包括步骤：

步骤S61，将所述非地面点云通过法线投影到一个平面上，得到投影点。

步骤S62，对所述平面上的投影点作平面内的三角化处理，得到各个投影点之间的连接关系。

步骤S63，根据各个投影点之间的连接关系确定各原始三维点之间的拓扑连接关系，根据所述拓扑连接关系形成三角网格曲面，所得的三角网格曲面即为重建得到的曲面模型，优化显示效果；其中，所述原始三维点指投影前的所述精确点云。

在本实施例中，利用欧式距离过滤算法过滤函数能够过滤掉赛道区域内远处无用的扫描点，同时过滤掉激光雷达附近噪点的干扰以及赛车本身发射导致的干扰点，从而能够减少对扫描点的运算强度，提升了运算效率。通过对锥桶区域进行水平校准，能够及时纠正赛车受到道路因素的干扰时产生的偏差，利用提取的平面的法向量和雷达坐标系中的竖直向量来进行水平校准，提高了锥桶位置信息的准确性，提供稳定的锥桶信息。通过半径滤波处理，能够滤去非地面点云内部的干扰点，对非地面点云中存在的一些悬空的孤立点或无效点去除，能够进一步提升锥桶位置的精确度。通过对锥桶点云进行重建优化处理，能够优化显示效果。

实施例2

本实施例介绍了一种基于激光雷达的赛道中锥桶信息的提取系统，其采用实施例1中介绍的信息提取方法对赛道的锥桶信息进行提取。信息提取系统包括：扫描模块、水平校准模块、过滤模块、滤波模块、重建优化模块以及聚类模块。

其中，扫描模块用于对赛道区域实时扫描，得到赛道点云，且对所述赛道点云根据不同赛场情况设定的截取范围，截取一个锥桶区域。

赛道点云包括地面点云与非地面点云。在本实施例中，根据激光雷达探测赛道区域扫描点，提取锥桶扫描点并与摄像头进行联合标定，标明锥桶颜色，输出其位置坐标供路径规划板块使用。激光雷达的型号不加限制，本实施例中，优选的，激光雷达的型号为RS-LiDAR-32型号激光雷达。

水平校准模块用于对所述锥桶区域根据一个提取平面的平面方程进行水平校准，以使所述锥桶区域在雷达坐标系中保持水平。

过滤模块用于对所述锥桶区域内的点云进行过滤，分离所述锥桶区域内的点云中的地面点云和非地面点云。

滤波模块，其用于对所述非地面点云进行半径滤波处理，过滤所述非地面点云中的悬空点和无效点，得到精确点云。

聚类模块，其用于通过聚类所述精确点云得到所述锥桶区域内的所有锥桶的位置信息。

重建优化模块，其用于在聚类前或者后，对所述精确点云进行重建优化，优化显示效果。对非地面点云进行重建优化，丰富非地面点云中点的数量，对非地面点云进行半径滤波处理，以对其中的悬空点或无效点进行过滤，得到精确点云。

在本实施例中，使用PCL库pcl::PassThrough直通滤波函数，截取赛道区域扫描点的部分扫描点，即切割出一个锥桶分布的区域，并利用根据欧式距离过滤函数剔除掉过近和过远的点(例如剔除掉距离雷达半径小于0.1m和大于15m的点)，减少雷达附近噪点以及较远无用扫描点的干扰。由于激光雷达的一些线束会扫射到地面，地面的点对于桩桶扫描点的聚类容易产生影响，故而需要过滤地面，将地面上的扫描点和非地面的扫描点进行分离。过滤模块的主要任务就是将一帧扫描点数据中的地面点和非地面点区分开，进行分类处理，才能进行后期的重建与聚类。

在本实施例中，对锥桶的扫描点进行处理时，先对其进行半径滤波，之后进行聚类，最后对其进行重建处理。

对非地面点云进行半径滤波处理，以对其中的悬空点或无效点进行过滤，得到精确点云。由于非地面点云包含一些悬空的点或者无效的点，这部分点会对后续的计算产生一定的影响，造成误差，因此需要将这些点进行处理。半径滤波处理方法为：选取非地面点云中的一个点，并将其定义为点A，判断在一个距离阈值内点A的外侧是否包含至少两个点：若为是，则将点A保留；若为否，则将点A删除。其中，在本实施例中，距离阈值优选为0.1m。重复执行上述步骤，直至所有的非地面点云中的点均完成处理为止，将所有保留的点进行集合，得到精确点云。此时，悬空点或无效点已经被处理完全，并得到得到精确点云。

在半径滤波处理完成后，需要对精确点云进行聚类处理。在本实施例中，采用欧式聚类算法对其进行聚类。首先创建一个Kd树对象作为提取扫描点时所用的搜索方法，再创建一个扫描点索引向量cluster_indices，用于存储实际的扫描点索引信息，每个检测到的扫描点聚类被保存在这里。因为扫描点是PointXYZ类型的，所以这里用扫描点类型PointXYZ创建一个欧氏聚类对象，并设置提取的参数和变量。根据锥桶形状特征设置一个合适的聚类搜索半径Cluster-Tolerance，如果搜索半径取一个非常小的值，那么一个实际的对象就会被分割为多个聚类；如果将值设置得太高，那么多个对象就会被分割为一个聚类，所以需要进行测试找出最适合的ClusterTolerance(例如按场景测试选取适当的距离0.3)。用两个参数来限制找到的聚类：用set-MinClusterSize()来限制一个聚类最少需要的点数目(根据具体激光雷达探测的点数而定，例如此值为5)，用setMaXClusterSize()来限制最多需要的点数目(根据具体激光雷达探测的点数而定，例如此值为1000)。接下来从扫描点中提取聚类，并将扫描点索引保存在cluster_indices中，便于后续遍历聚类扫描点。聚类之后，对每个分割出聚类进行循环判断处理，用PCL库pcl::getMinMax3D函数计算出障碍物的边缘。对聚类高度、宽度(高度、宽度根据具体赛道锥桶的形状而定)进行判断，滤去除锥桶外的障碍物，筛选出符合锥桶特征信息的障碍物，利用循环容器遍历锥桶扫描点，提取出锥桶位置信息(X、Y坐标)，发送给决策层。

在对锥桶扫描点进行重建时，采用的是贪婪投影三角化算法(假设曲面光滑,扫描点密度变化均匀)对扫描点进行曲面重建。其基本思想是：将三维点通过法线投影到某一平面，然后对投影得到的扫描点作平面内的三角化，从而得到各点的连接关系。在三维平面内根据各点的连接关系扩张为由有限的三角面构成的模型，即得到重建后的曲面模型，优化显示效果。

在本实施例还公开了基于激光雷达的赛道中锥桶信息的提取系统的具体工作流程。信息提取系统通过Ubuntu-16.04系统实现。Ubuntu-16.04系统安装ROSKinetic版本对激光雷达的扫描点进行提取。在工作时，首先接通硬件激光雷达，例如本次方案采取RS-LiDAR-32型号激光雷达，RS-LiDAR-32与电脑之间的通信采用以太网介质，使用UDP协议，使用设备的时候，需要把电脑IP设置为与设备同一网段上，例如192.168.1.x(x的选取值范围为1～254)，子网掩码为255.255.255.0。若不知设备网络配置信息，连接设备并使用wireshark抓取设备输出包进行分析。连接的具体操作为在Ubuntu-16.04系统IP地址在终端通过ifconfig命令配置，配置代码为：$sudo ifconfig eno1(本机网络端口名，可通过ifconfig命令查看)192.168.20.102(视激光雷达说明书而定)，此时激光雷达硬件与主机系统接通，可获取到激光雷达360°扫描得到的周围场景的扫描点数据。

为使扫描点提取流程能够在ROS系统中运行，将上述提取锥桶扫描点的流程方案编写为C/C++或Python文件，作为ROS系统的源文件，放于src源文件目录下，首先cd到工作空间catkin_ws所在的路径，然后在终端使用：$catkin_make命令对其进行编译，编译通过后source刷新环境：$source～/catkin_ws/devel/setup.bash，此时即可在ROS系统中运行。

接下来在ROS系统中启动上一步编译好的node节点，首先启动ROS系统，在新的终端窗口输入：$roscore命令启动ROS系统。此时打开一个新的终端窗口，cd到工作空间catkin_ws所在的路径，在终端使用：$source～/catkin_ws/devel/setup.bash命令来刷新环境，然后在终端中输入命令：$rosrun package(源文件所在package的名称，视具体名称而定)node(源文件的node节点名称，视具体情况而定)，此时即可运行对锥桶扫描点的提取流程，其具体提取流程如下：

一、利用设计的欧式距离过滤函数过滤掉过远处无用的扫描点，减少对扫描点的运算强度，提升运算效率。

二、利用设计的欧式距离过滤函数过滤掉激光雷达附近噪点的干扰以及车辆本身反射导致的干扰点。

三、利用Progressive Morphological Filters渐进形态学滤波算法提取出地面，并利用设计的函数计算出从平面法向量旋转到竖直向量的旋转矩阵，利用PCL库pcl::transformPointCloud旋转函数再把此旋转矩阵应用到扫描点，扫描点即可得到旋转。使得校准后地面扫描点和系统坐标轴平面保持水平。

四、利用PCL库pcl::PassThrough直通滤波过滤函数，切割出感兴趣区域，根据赛场情况，切割出一个锥桶分布的矩形区域。

五、由于激光雷达的一些线束会扫射到地面，采用Progressive MorphologicalFilters渐进形态学滤波算法根据不同道路选取的合适的窗口尺寸和高程差阈值，保留非地面扫描点，从而将地面点滤去。

六、然后利用PCL半径滤波函数，根据具体场景设置合理的阈值滤去离群点，达到去除个别小的障碍物的目的。

七、接着采用欧式聚类，使用KD Tree数据结构来组织点，将符合半径阈值的球体内的点聚类成一个扫描点簇，根据激光雷达的实际探测到的扫描点数量设定最大和最小聚类点数阈值，保存符合锥桶范围的扫描点簇。

八、随后根据锥桶特征对聚类的高度、宽度进行限制(高度、宽度根据具体赛道锥桶的形状而定)，利用PCL库pcl::getMinMax3D函数提取出目标物在空间位置边缘的最大值与最小值，根据锥桶形状特征限制其最大值与最小值，从而滤去除锥桶外的障碍物，筛选出符合特征信息的锥桶，最终提取出锥桶扫描点和其位置信息，发送给决策层。

九、最后采用贪婪投影三角化算法扩张扫描点曲面边界，对扫描点进行曲面重建，优化显示效果。

十、若要可视化具体的扫描点提取效果，可打开RViz机器人可视化工具来查看提取效果，具体操作步骤为打开一个新的终端，输入：$rviz命令即可打开，在工具中选取对应的Topic即可查看效果。

请参阅图11和图12，根据上述流程进行多次实验，通过对比可以看出，经过过滤后，能够有效去除环境中的障碍物、地面噪声等影响，从而有效提取出锥桶的扫描点信息。

本实施例的有益效果为：通过过滤掉赛道区域内远处无用的扫描点，同时过滤掉激光雷达附近噪点的干扰以及赛车本身发射导致的干扰点，从而能够减少对扫描点的运算强度，提升了运算效率。通过对锥桶扫描点进行聚类，能够准确得出锥桶的边缘坐标，其与锥桶中心坐标相互配合，能够准确得到锥桶的位置信息。本发明方法精简，效率高，能够适应不同赛道场景中锥桶信息的提取，稳定性强。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明申请范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明申请的保护范围应以所附权利要求为准。

Claims (8)

1.一种基于激光雷达的赛道中锥桶信息的提取方法，其用于提取赛道上的锥桶的位置信息，其特征在于，所述锥桶信息的提取方法包括步骤：

步骤S1，对赛道区域实时扫描，得到赛道点云，且对所述赛道点云根据不同赛场情况设定的截取范围，截取一个锥桶区域；

步骤S2，对所述锥桶区域根据一个提取平面的平面方程进行水平校准，以使所述锥桶区域在雷达坐标系中保持水平；所述水平校准的方法包括以下步骤：

步骤S21，定义所述提取平面的平面方程：ax+by+cz+d=0，其中，x、y、z分别是所述锥桶区域的x轴、y轴、z轴，a、b、c分别是x、y、z的法向量，d为所述提取平面相对于坐标原点的偏移量，所述坐标原点指所述提取平面所在的坐标系的原点；

步骤S22，对激光雷达坐标系中的竖直向量进行定义，将所述提取平面的法向量通过所述竖直向量进行旋转，得到旋转矩阵；

步骤S23，将所述旋转矩阵通过点云旋转函数应用到所述提取平面，完成所述锥桶区域的水平校准过程；

步骤S3，对所述锥桶区域内的点云进行过滤，分离所述锥桶区域内的点云中的地面点云和非地面点云；其中，所述点云中的地面点云和非地面点云的分离方法包括以下步骤：

步骤S31，加载所述锥桶区域内的点云，划分为规则网格，在每个网格中选取高程最低点，构建最小高程表面；其中，如果网格中没有高程最低点，则根据最近邻点进行插值；

步骤S32，使用一个宽度尺寸为w₁的窗口以及设定的高程差阈值dh_T来对原始点云进行开操作：地形特征中小于w₁的部分为局外点，反之，地形特征中大于w₁的部分为局内点，其中，所述原始点云指所述锥桶区域内的点云数据；并假设dh_p,1代表原始点云与在任意给定p1点处第一次迭代表面之间的高程差，dh_T代表高程差阈值，则如果dh_p,1≤dh_T点p1就被认为是局内点，反之如果dh_p,1>dh_T就认为点p1是一个局外点；此后，令dh_max(t),1为当前迭代中初始地面点与滤波表面之间差值的最大值，则如果选取的dh_T>dh_max(t),1则所有的测量值都会保留为局内点；

步骤S33，进行下一次迭代，计算新的滤波窗口的宽度尺寸，新的滤波窗口的宽度尺寸增大为w₂，对于上一次的处理结果进行开操作处理，以此迭代，通过逐渐增加窗口宽度尺寸来去除不同大小的局外点，其中逐渐增加的窗口的宽度尺寸w_k=2w₁ ^k+1，式中k为迭代次数，w₁是初始窗口大小；重复执行步骤S32，直至窗口尺寸达到设定的最大窗口的宽度尺寸w_max为止；其中，所述局内点的集合为地面点云，所述局外点的集合为非地面点云

步骤S4，对所述非地面点云进行半径滤波处理，过滤所述非地面点云中的悬空点和无效点，得到精确点云；其中，所述半径滤波处理的方法包括步骤：

步骤S41，选取所述非地面点云中的一个点，并将其定义为点A，以点A作为圆心以一个距离阈值作为半径，形成一个滤波圆，判断所述滤波圆内是否包含至少两个点：若为是，则将点A保留；若为否，则将点A删除；

步骤S42，重复执行步骤S41，直至所有的非地面点云中的点均完成处理为止，将所有保留的点A组合，得到精确点云；

步骤S5，通过聚类所述精确点云得到所述锥桶区域内的所有锥桶的位置信息；其中，所述聚类的方法包括步骤：

步骤S51，定义一个扩张出的三维空间中，任意一点的投影点为点P2，利用Kd树搜索方法得到k个距离点P2最近的投影点；

步骤S52，判断k个投影点与点P2之间的间距是否均小于一个半径阈值：选择满足条件的点构成集合Q；

步骤S53，选取点P2以外的投影点为新的投影点，重复执行步骤S51，直至所述三维空间中所有的投影点均完成判断为止；

步骤S54，根据锥桶形状特征获得锥桶模型边缘在投影平面内的最大值与最小值，并根据所述最大值与最小值，并滤除锥桶外的投影点，得到锥桶的投影点；

步骤S55，根据所述锥桶的投影点，得到锥桶的位置信息

其中，所述精确点云在聚类前或者后，对所述精确点云进行重建优化，优化显示效果，所述重建优化的方法包括以下步骤：

步骤S61，将所述精确点云通过任意法线投影到一个与所述任意法线相对应的投影平面上，得到投影点云；

步骤S62，对所述投影点云作平面内的三角化处理，得到各个投影点之间的连接关系；

步骤S63，根据各个投影点之间的连接关系确定各原始三维点之间的拓扑连接关系，根据所述拓扑连接关系形成三角网格曲面，所得的三角网格曲面即为重建得到的曲面模型，优化显示效果；其中，所述原始三维点指投影前的所述精确点云。

2.根据权利要求1所述的基于激光雷达的赛道中锥桶信息的提取方法，其特征在于，所述截取范围以赛道内所需锥桶均位于所述锥桶区域内作为标准而设定。

3.根据权利要求2所述的基于激光雷达的赛道中锥桶信息的提取方法，其特征在于，所述锥桶区域的截取方法包括以下步骤：

步骤S11，对所述赛道区域中的锥桶位置进行预估计，将所述赛道区域分割成初始区域，所需锥桶均位于所述初始区域内；

步骤S12，判断所述初始区域内的赛道点云中的任一点与所述激光雷达之间的间距是否位于一个预设的距离范围内：若为是，则将相应的点保留；若为否，则剔除相应的点；

步骤S13，重复执行步骤S12，直至所有的赛道点云中的点均完成处理为止，将所有保留的赛道点组合，得到一个锥桶区域，且所需锥桶均位于所述锥桶区域内。

4.根据权利要求1所述的基于激光雷达的赛道中锥桶信息的提取方法，其特征在于，在步骤S41中，所述距离阈值为0.1m。

5.根据权利要求3所述的基于激光雷达的赛道中锥桶信息的提取方法，其特征在于，在步骤S12中，所述距离范围为0.1~15m。

6.根据权利要求3所述的基于激光雷达的赛道中锥桶信息的提取方法，其特征在于，在步骤S52中，所述半径阈值为0.3m。

7.根据权利要求1所述的基于激光雷达的赛道中锥桶信息的提取方法，其特征在于，在步骤S54中，通过点云库中的getMinMax3D函数提取出集合Q在空间位置边缘的最大值与最小值。

8.一种基于激光雷达的赛道中锥桶信息的提取系统，其特征在于，其采用如权利要求1-7任意一项所述的基于激光雷达的赛道中锥桶信息的提取方法对赛道上的锥桶信息进行提取；所述提取系统包括：

扫描模块，其用于对赛道区域实时扫描，得到赛道点云，且对所述赛道点云根据不同赛场情况设定的截取范围，截取一个锥桶区域；

水平校准模块，其用于对所述锥桶区域根据一个提取平面的平面方程进行水平校准，以使所述锥桶区域在雷达坐标系中保持水平；

过滤模块，其用于对所述锥桶区域内的点云进行过滤，分离所述锥桶区域内的点云中的地面点云和非地面点云；

滤波模块，其用于对所述非地面点云进行半径滤波处理，过滤所述非地面点云中的悬空点和无效点，得到精确点云；以及

聚类模块，其用于通过聚类所述精确点云得到所述锥桶区域内的所有锥桶的位置信息；

重建优化模块，其用于在聚类前或者后，对所述精确点云进行重建优化，优化显示效果。