CN114839620A - 一种自动驾驶系统多激光雷达点云去重的快速标定方法 - Google Patents

Abstract

本发明提供了一种自动驾驶系统多激光雷达点云去重的快速标定方法，包括以下后步骤：S1、针对父雷达以及子雷达的数据解析，生成点云数据，所述父雷达指的是最终发布融合点云数据的激光雷达，子雷达指的是需要将点云数据进行转化的激光雷达；S2、粗略计算父雷达以及子雷达的位姿；S3、计算父雷达以及子雷达之间的配准关系。本发明有益效果：一种自动驾驶系统多激光雷达点云去重的快速标定方法，通过点云数据解析以及点云数据特征提取配准，以及ICP算法的精细配准，最后将不同点云数据进行时空同步以及邻阈法进行重复点去除，实现了多个激光雷达的外参精细标定，将不同的激光雷达数据在同一视角下进行表达，极大的提高了点云数据的密度以及雷达的视野。

Description

一种自动驾驶系统多激光雷达点云去重的快速标定方法

技术领域

本发明属于雷达技术领域，尤其是涉及一种自动驾驶系统多激光雷达点云去重的快速标定方法。

背景技术

在无人驾驶技术领域中，环境感知作为无人车于周边环境的交互模块具有非常重要的作用，在当前技术领域中，常用的传感器类型大致有激光雷达以及单目相机。由于激光雷达具有不受光照影响的特性可以实现全天候工作，其测距方式可以为无人车提供精确的障碍物位置信息，为无人车后续的决策、控制提供可靠的依据。

目前由于高线束的雷达价格高昂，低线束的雷达点云密度低，针对这种情况，为了降低无人车传感器的设备价格，将多个低线束雷达组合成一个高线束雷达是一个可靠的解决方案，这样不仅可以提供高密度的点云，还可以提高雷达的感受视野；

在目前的标定方法中，大多数方法都是基于坐标系转化进行实现的，通过不同传感器之间的坐标系关系，将点云按照坐标系之间的转化关系进行处理，这样操作的缺点是点云数据只是进行了简单的叠加，存在一定位置偏差以及大量的重复点云。

发明内容

有鉴于此，本发明旨在提出一种自动驾驶系统多激光雷达点云去重的快速标定方法，基于点云特征匹配的方法进行不同激光雷达之间的外参标定。

为达到上述目的，本发明的技术方案是这样实现的：

一种自动驾驶系统多激光雷达点云去重的快速标定方法，包括以下步骤：

S1、针对父雷达以及子雷达的数据解析，生成点云数据，所述父雷达指的是最终发布融合点云数据的激光雷达，子雷达指的是需要将点云数据进行转化的激光雷达，因为每个雷达都是相互独立，所以需要对每个雷达进行数据解析，数据解析前，接收到的是雷达的数据帧，里面包括一些数据帧头、数据内容以及数据校验位等，经过数据解析后，会按照点云数据类型进行发布；

S2、粗略计算父雷达以及子雷达的位姿；

S3、计算父雷达以及子雷达之间的配准关系；

S4、针对父雷达以及子雷达的数据帧进行时空同步，所述数据帧是步骤S1中父子雷达解析后产生的数据帧，是经过原始数据解析后形成的点云数据帧；

S5、去除同视角下的冗余数据。

进一步的，在步骤S1中，父雷达以及子雷达产生的数据经过驱动程序解析后，以话题的形式进行消息发布。

进一步的，在步骤S2中，需要计算的内容包括子雷达相对于父雷达的空间位置差异以及姿态差异。

进一步的，计算各个子雷达相对于父雷达的空间位置关系时，需要分别计算各个子雷达以及父雷达的空间坐标X,Y,Z的值。

进一步的，在步骤S3中，利用点云特征匹配算法计算不同传感器之间的配准关系，包括基于点云聚类的动态网格滤波法、点云特征提取方法、点云特征配准方法、计算不同雷达数据帧之间的外参关系方法。

进一步的，基于点云聚类的动态网格滤波法包括：采用基于K-Means的动态网格法进行点云滤波操作的方法，包括以下步骤：

A1、首先将父雷达和子雷达的点云数据通过KD树的点云数据结构进行表示，生成组织好的KD树；

A2、将步骤A1中组织好的KD树点云利用K-Means算法进行空间聚类划分，将聚类划分后的点云簇进行记录，存放在一个点云容器中；

A3、将每个点云簇的点云网格划分，划分规则为当点云簇内的点云包含的点云数目超过设定的阈值时，则使用固定阈值的体素块质心的点云代表该点云区域内的点云，如果某个点云簇的点云簇数量较少时，则将此点云簇作为整个点云块进行处理，不进行降采样操作。

进一步的，针对步骤A3中进行点云网格划分后的点云簇进行点云空间特征提取，用于提取点云角点以及平面点，包括：

采用Harris特征点检测方法，首先设定点云角点阈值以及点云平面点阈值，检测点云数据在XYZ三轴方向上斜率的变化率，如果点云在XYZ三轴方向上斜率的变化率大于设定的阈值，则属于角点，小于设定的阈值，则判定为平面点，最后将提取到的点云特征进行存储，即大于设定的阈值，则将此特征点归类于角点特征点，如果小于此阈值，则将此特征点归类于平面特征点。

进一步的，计算不同雷达数据帧之间的外参关系方法包括：

在点云特征配准过程中，对每个激光雷达提取到的点云数据进行步骤(2)计算，得到相同视角下的点云数据特征，进而对这些特征进行特征描述，将点云特征按照向量的方式进行描述，然后对每个激光雷达存储的点云特征描述子进行相似度检测，本专利采用余玹距离度量方式，其计算方式如(1)所示；

其中，cosθ表示两个点之间的余玹距离，取值范围为[-1,1]；

通过计算各个雷达(包括父雷达以及子雷达)下的特征描述子的余玹距离，所述余玹距离表征的是两个特征描述子的相似程度，将大于设定余玹距离阈值的点云数据利用PCL点云库进行粗略位置的计算，将粗略得到的点云位姿变化矩阵进行点云数据转化，将子雷达的点云数据通过矩阵转化到父雷达的坐标系下，完成点云数据帧的粗配准。经过点云特征粗配准之后，将当前的位姿变换信息作为ICP算法迭代的开始初值，再利用ICP算法进行多次迭代，可以进一步提高配准的精度，当ICP配准收敛且小于设定的收敛阈值后，输出多个子雷达到父雷达的位姿变换矩阵，即标定的外参矩阵。

进一步的，去除同视角下的冗余数据方法为：

采用半径邻阈法进行重复点云的去除，首先将同步后的不同雷达数据帧的点云进行接收，接收后的点云通过KD树的数据结构形式进行组织，接下来对点云数据按照设定的检测半径阈值以及相邻点的阈值进行距离计算，若两点之间的距离小于设定的检测阈值则认为是重合点，将该重复点的索引值进行排序以及记录，最后通过PCL中的Extract类根据点云的索引值进行删除操作，通过此方法去除不同传感器在同视角下的冗余数据。

相对于现有技术，本发明所述的一种自动驾驶系统多激光雷达点云去重的快速标定方法具有以下有益效果：

本发明所述的一种自动驾驶系统多激光雷达点云去重的快速标定方法，通过点云数据解析以及点云数据特征提取配准，以及ICP算法的精细配准，最后将不同点云数据进行时空同步以及邻阈法进行重复点去除，实现了多个激光雷达的外参精细标定，将不同的激光雷达数据在同一视角下进行表达，极大的提高了点云数据的密度以及雷达的视野。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

下面将结合实施例来详细说明本发明。

在目前的标定方法中，大多数方法都是基于坐标系转化进行实现的，通过不同传感器之间的坐标系关系，将点云按照坐标系之间的转化关系进行处理，这样操作的缺点是点云数据只是进行了简单的叠加，存在一定位置偏差以及大量的重复点云。

本专利基于点云特征匹配的方法进行不同激光雷达之间的外参标定，具体流程如下：

多激光雷达数据解析；

粗略计算父雷达以及子雷达的位姿；

利用点云特征匹配算法计算不同传感器之间的配准关系；

数据帧进行时空同步；

去除同视角下的冗余数据。

具体实施方法如下：

多激光雷达数据解析

由于激光雷达的数据通信为UDP通信方式，需要按照激光雷达的相关技术协议进行数据解析以及发布，本专利的研究平台为Ubuntu18.04以及ROS(Robot OperatingSystem)机器人操作系统，经过解析后的雷达数据最终以话题的形式进行消息发布。

粗略计算父雷达以及子雷达的位姿

在多雷达的外参标定中，其根本目的是将不同雷达视角下的点云标定到同一个激光雷达视角下，故需要确定父雷达以及子雷达，父雷达指的是最终发布融合点云数据的激光雷达，子雷达指的是需要将点云数据转化的激光雷达。在计算父雷达以及子雷达位姿的过程中，需要计算的内容包括子雷达相对于父雷达的空间位置差异以及姿态差异，由于步骤2是一个粗略计算位姿的过程，这里只需要计算各个子雷达相对于父雷达的空间位置关系，即空间坐标X,Y,Z的值，姿态估计使用初始值即可，通过粗略的计算子雷达和父雷达的之间关系，可以极大的加速后续配准过程中的精度以及速度。

利用点云特征匹配算法计算不同传感器之间的配准关系

在利用点云特征匹配算法进行不同传感器之间位姿计算的过程中，主要包括以下步骤：

分别为基于点云聚类的动态网格滤波法、点云特征提取、点云特征配准、计算不同雷达数据帧之间的外参关系。

(1)基于点云聚类的动态网格滤波法

目前主流的点云滤波方法主要有高斯滤波、体素滤波等方法，在无人车点云处理领域中，大多数采用固定阈值的点云体素滤波法，但是当点云密度较低时，采用固定阈值的滤波法会出现破坏点云结构的现象出现，而且不同的体素阈值也需要适配不同的应用场景，不具有普适性。

本专利采用基于K-Means的动态网格法进行点云滤波操作，首先将点云数据通过KD树的点云数据结构进行表示，这样做的目的是为了后续步骤中进行不同点的距离划分以及加快点云的搜索速度。接下来将组织好的KD树点云利用K-Means算法进行空间聚类划分，将聚类后的点云簇进行记录，存放在一个点云容器中，这样做的目的是为了后续进行点云空间网格划分以及后续点云特征比对。最后将每个聚类簇的点云网格划分，划分规则为当聚类簇内的点云包含的点云数目超过设定的阈值时，则使用固定阈值的体素块质心的点云代表该点云区域内的点云。如果某个点云簇的点云簇数量较少时，即低于设定的阈值时，则将此点云簇作为整个点云块进行处理，不进行降采样操作。通过点云聚类的动态网格滤波法既可以保证点云的空间结构不会被破坏，保留更多的点云特征，有利用后续的点云特征提取，又可以极大的减少空间点云的数据量，加快整体的计算速度。

(2)空间点云特征提取

通过步骤(1)得到了点云数据网格，接下来需要对点云数据进行空间特征提取，这里提取的点云特征点主要为点云角点以及平面点。

其中检测方法采用的是Harris特征点检测，主要关注的是点云数据在三轴方向上斜率的变化率，通过设定的角点阈值以及平面点阈值，如果斜率在三轴上斜率变化比较明显，则属于角点，小于设定的阈值，则判定为平面点。最后将提取到的特征点进行存储，方便后续进行特征相似度度量，进行点云数据帧的匹配。

点云特征匹配以及获取标定外参

在点云特征配准过程中，对每个激光雷达提取到的点云数据进行步骤(2)计算，得到相同视角下的点云数据特征，对每个各个激光雷达存储的点云特征进行相似度检测，这里采用的余玹距离度量，计算方式如公式1所示。

其中，cosθ表示两个点之间的余玹距离，取值范围为[-1,1]。通过计算各个雷达下的特征点余玹距离，将大于设定阈值的点云数据进行粗略位置的计算，这里的计算方式是利用PCL点云库进行，将粗略得到的点云位姿变化矩阵进行点云数据转化，即将子雷达的点云数据通过矩阵转化到父雷达的坐标系下，这就完成了点云数据帧的粗配准，为了提高配准的精度，提高多雷达外参标定的精度，上一步的粗配准的主要作用是为后续的精准配准ICP算法提供较好的迭代初始位置。因为ICP算法的缺点是需要一个较好的初始迭代位置，如果没有精确的粗配准，会使用后续的ICP算法迭代效率以及收敛速度变慢。经过点云特征粗配准之后，再利用ICP算法多次迭代，可以进一步提高配准的精度，当ICP配准收敛且小于设定的阈值后，输出多个子雷达到父雷达的位姿变换矩阵，即标定的外参矩阵。

4、数据帧进行时空同步

不同传感器之间的数据具有不同的坐标系以及时间戳，多雷达传感器数据标定过程包括时间同步以及空间同步，其中时间同步的实现方式是利用ROS操作系统的时间戳同步机制，本专利采用的方式是相邻帧数据同步的方式。空间同步的方法是利用步骤(3)得出的旋转平移矩阵进行点云坐标数据的转化。

5、去除同视角下的冗余数据

由于不同激光雷达之间存在相同的检测视角，需要去除同视角下的重复点云数据，这样可以保证空间位置与点云数据一对一的关系，本专利采用半径邻阈法进行重复点云的去除。首先将同步后的不同雷达数据帧的点云进行接收，接收后的点云通过KD树的数据结构形式进行组织，加快点云的搜索速度。接下来对点云数据按照设定的检测半径阈值以及相邻点的阈值进行距离计算，若两点之间的距离小于设定的检测阈值则认为是重合点，将该重复点的索引值进行排序以及记录，最后通过PCL中的Extract类根据点云的索引值进行删除操作，通过此方法去除不同传感器在同视角下的冗余数据。

本专利通过点云数据解析以及点云数据特征提取配准，以及ICP算法的精细配准，最后将不同点云数据进行时空同步以及邻阈法进行重复点去除，实现了多个激光雷达的外参精细标定，将不同的激光雷达数据在同一视角下进行表达，极大的提高了点云数据的密度以及雷达的视野。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本申请所提供的几个实施例中，应该理解到，所揭露的方法和系统，可以通过其它的方式实现。例如，以上所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。上述单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本发明实施例方案的目的。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围，其均应涵盖在本发明的权利要求和说明书的范围当中。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种自动驾驶系统多激光雷达点云去重的快速标定方法，其特征在于，包括以下步骤：

S1、针对父雷达以及子雷达的数据解析，生成点云数据，所述父雷达指的是最终发布融合点云数据的激光雷达，子雷达指的是需要将点云数据进行转化的激光雷达；

S2、粗略计算父雷达以及子雷达的位姿；

S3、计算父雷达以及子雷达之间的配准关系；

S4、针对父雷达以及子雷达的数据帧进行时空同步；

S5、去除同视角下的冗余数据。

2.根据权利要求1所述的一种自动驾驶系统多激光雷达点云去重的快速标定方法，其特征在于，在步骤S1中，父雷达以及子雷达产生的数据经过驱动程序解析后，以话题的形式进行消息发布。

3.根据权利要求1所述的一种自动驾驶系统多激光雷达点云去重的快速标定方法，其特征在于，在步骤S2中，需要计算的内容包括子雷达相对于父雷达的空间位置差异以及姿态差异。

4.根据权利要求3所述的一种自动驾驶系统多激光雷达点云去重的快速标定方法，其特征在于，计算各个子雷达相对于父雷达的空间位置关系时，需要分别计算各个子雷达以及父雷达的空间坐标X,Y,Z的值。

5.根据权利要求1所述的一种自动驾驶系统多激光雷达点云去重的快速标定方法，其特征在于，在步骤S3中，利用点云特征匹配算法计算不同传感器之间的配准关系，包括基于点云聚类的动态网格滤波法、点云特征提取方法、点云特征配准方法、计算不同雷达数据帧之间的外参关系方法。

6.根据权利要求5所述的一种自动驾驶系统多激光雷达点云去重的快速标定方法，其特征在于，基于点云聚类的动态网格滤波法包括：采用基于K-Means的动态网格法进行点云滤波操作的方法，包括以下步骤：

A1、首先将父雷达和子雷达的点云数据通过KD树的点云数据结构进行表示，生成组织好的KD树；

A2、将步骤A1中组织好的KD树点云利用K-Means算法进行空间聚类划分，将聚类划分后的点云簇进行记录，存放在一个点云容器中；

A3、将每个点云簇的点云网格划分，划分规则为当点云簇内的点云包含的点云数目超过设定的阈值时，则使用固定阈值的体素块质心的点云代表该点云区域内的点云，如果某个点云簇的点云簇数量较少时，则将此点云簇作为整个点云块进行处理，不进行降采样操作。

7.根据权利要求6所述的一种自动驾驶系统多激光雷达点云去重的快速标定方法，其特征在于，针对步骤A3中进行点云网格划分后的点云簇进行点云空间特征提取，用于提取点云角点以及平面点，包括：

采用Harris特征点检测方法，首先设定点云角点阈值以及点云平面点阈值，检测点云数据在XYZ三轴方向上斜率的变化率，如果点云在XYZ三轴方向上斜率的变化率大于设定的阈值，则属于角点，小于设定的阈值，则判定为平面点，最后将提取到的点云特征进行存储。

8.根据权利要求7所述的一种自动驾驶系统多激光雷达点云去重的快速标定方法，其特征在于，计算不同雷达数据帧之间的外参关系方法包括：

在点云特征配准过程中，对每个激光雷达提取到的点云数据进行步骤(2)计算，得到相同视角下的点云数据特征，进而对这些特征进行特征描述，将点云特征按照向量的方式进行描述，然后对每个激光雷达存储的点云特征描述子进行相似度检测，本专利采用余玹距离度量方式，其计算方式如(1)所示；

其中，cosθ表示两个点之间的余玹距离，取值范围为[-1,1]；

通过计算各个雷达下的特征描述子的余玹距离，将大于设定余玹距离阈值的点云数据利用PCL点云库进行粗略位置的计算，将粗略得到的点云位姿变化矩阵进行点云数据转化，将子雷达的点云数据通过矩阵转化到父雷达的坐标系下，完成点云数据帧的粗配准，经过点云特征粗配准之后，将当前的位姿变换信息作为ICP算法迭代的开始初值，再利用ICP算法进行多次迭代，可以进一步提高配准的精度，当ICP配准收敛且小于设定的收敛阈值后，输出多个子雷达到父雷达的位姿变换矩阵，即标定的外参矩阵。

9.根据权利要求1所述的一种自动驾驶系统多激光雷达点云去重的快速标定方法，其特征在于，在步骤S5中，去除同视角下的冗余数据方法为：

采用半径邻阈法进行重复点云的去除，首先将同步后的不同雷达数据帧的点云进行接收，接收后的点云通过KD树的数据结构形式进行组织，接下来对点云数据按照设定的检测半径阈值以及相邻点的阈值进行距离计算，若两点之间的距离小于设定的检测阈值则认为是重合点，将该重复点的索引值进行排序以及记录，最后通过PCL中的Extract类根据点云的索引值进行删除操作，通过此方法去除不同传感器在同视角下的冗余数据。