CN115453559A - 一种基于动态数据进行多激光雷达时空同步的方法 - Google Patents

Abstract

本发明涉及一种基于动态数据进行多激光雷达时空同步的方法，包括一种二阶段的轨迹数据匹配在阶段1中，基于轨迹的局部运动状态相似性得到的配对关系，计算转换参数，初步求得需要时空同步的两个雷达之间的转换参数，实现雷达的粗配准；在阶段2中，对阶段1得到的所述两个雷达之间的转换参数采用一种基于轨迹的最小迭代算法进一步调整，减少参数误差，得到最终的转换参数，转换参数包括空间的旋转和平移，以及时间的偏移。

Description

一种基于动态数据进行多激光雷达时空同步的方法

技术领域

本发明涉及一种基于动态数据进行多激光雷达时空同步的方法，属于自动驾驶、车路协同领域相关技术。

背景技术

近年来，尤其是车路协同领域，使用路侧设备辅助车辆自动驾驶，激光雷达由于可以提供准确的3d信息，成为了一种广泛使用的传感器。但是，由于激光的物理特性，雷达容易被遮挡，通常需要使用多个雷达进行联合感知的方案。而只有对雷达在时间和空间维度均进行同步，才能对雷达的感知结果进行融合。然而，在车路协同场景下，路测的多激光雷达隶属于不同子系统，而且雷达之间位置变换大，时间和空间的同步成为了一个重要的问题。

传统的同步算法中，ICP[1]是一种基于交替迭代达到最优值的算法，是点云配准工作中最常用的算法之一，而且有很多的变种，然而ICP算法容易收敛于局部最优值，对初值有很大的依赖。NDT算法将点云切分成cell，并基于点云的高斯分布进行配准，但是和ICP相同，非常依赖于初值。显然，ICP和NDT并不适合用于路侧雷达的配准，因为点云之间的高度和视角都有很大差异。

基于特征的方法，先提取特征，然后确定特征之间的对应关系来进行点云配准，例如SAC-IA[3],通过提取fast point feature histograms进行近似匹配。但是这类型的方法并不适合交通场景下，因为交通场景存在很多重复的结构，比如楼房，路沿石等，导致找到特征之间的准确对应变得很困难。

综上可以看到，目前的雷达标定算法存在依赖初值、特征匹配模糊性高等问题，均不能很好地解决路侧多激光雷达的标定问题，本发明提出了一种将时间同步和空间同步问题归一为一个问题的自动化时空同步算法，而且算法对初始参数没有依赖。

[1]P.Besl and N.D.McKay,“A method for registration of 3-d shapes,”IEEE Transactions on Pattern Analysis and Machine Intelligence,vol.14,no.2,pp.239–256.

[2]Peter Biber and Wolfgang Straβer.2003.The normal distributionstransform:A new approach to laser scan matching.In Proceedings 2003 IEEE/RSJInternational Conference on Intelligent Robots and Systems(IROS 2003)(Cat.No.03CH37453),Vol.3.IEEE,2743–2748.

[3]R.B.Rusu,N.Blodow,and M.Beetz,“Fast point feature histograms(fpfh)for 3d registration,”in 2009IEEE International Conference on Robotics andAutomation,2009,pp.3212–3217.

发明内容

本发明技术解决问题：本算法实现了对多个激光雷达之间时空同步的问题。克服现有技术的什么不足，提供一种基于动态数据进行多激光雷达时空同步的方法，具有不依赖于初始值，具备更小的计算量和迁移性，而且达到了厘米级的精度。

本发明技术解决方案：

一种基于动态数据进行多激光雷达时空同步的方法，包括一种二阶段的轨迹数据匹配在阶段1中，基于轨迹的局部运动状态相似性得到的配对关系，计算转换参数，初步求得需要时空同步的两个雷达之间的转换参数，实现雷达的粗配准；在阶段2中，对阶段1得到的所述两个雷达之间的转换参数采用一种基于轨迹的最小迭代算法进一步调整，减少参数误差，得到最终的转换参数，转换参数包括空间的旋转和平移，以及时间的偏移；

具体实现如下：

阶段1：

(1)将激光雷达获取点云数据输入检测器进行检测，得到每帧点云中存在物体的包围框(bounding box)作为检测结果，物体的包围框包括物体的长宽高，转向角，类别信息；将检测结果输入追踪器，将前后帧的不同物体进行关联，得到获得雷达视野中物体的历史运动轨迹；历史轨迹是一系列轨迹点的有序序列，轨迹点包含物体ID，轨迹点对应时间戳，对应空间坐标信息；

(2)采用卡尔曼滤波器去除步骤(1)中物体的历史运动轨迹的噪声，以减少轨迹误差；计算物体的历史运动轨迹在每个轨迹点的附近运动信息，运动信息包含速度均值、方差以及局部曲率；具体计算方法为：

局部速度均值：

局部速度方差：

局部曲率：

其中，v_i表示第i帧的速度，L_i为第i帧的位置向量；m为数据选取的帧数间隔，根据不同数据集的误差而进行条件，当m越小，计算结果越能表征局部运动特征，但是容易受到数据误差影响；L_i-m及L_i+m分别表示第i-m及i+m帧的位置向量；N⁺为正整数。

(3)将步骤(2)得到的每个轨迹点附近运动信息作为轨迹点特征，将运动信息相似的轨迹点进行配对，得到不同雷达追踪到物体历史轨迹点的配对关系，并建立物体历史轨迹点配对关系的集合；其中轨迹点相似性度量公式为：

轨迹点相似性度量公式：

α₁+α₂+α₃＝1

其中，

表示局部速度均值，

表示局部速度方差，cur为局部曲率，α₁,α₂,α₃为各指标权重。

(4)对物体历史轨迹点配对关系的集合中的物体历史轨迹点的配对关系进行过滤，删除步骤(3)中的物体历史轨迹点配对关系的集合中不符合规则的配对关系，以减小对结果带来的误差，得到删除后的得到的轨迹点配对关系；

(5)采用步骤(4)的轨迹点配对关系构成约束条件，基于最优化的方法，初步求解两个雷达之间的转换参数，实现雷达的粗配准；

阶段2中：所述一种基于轨迹的最小迭代算法流程如下：

(6)对于两个激光雷达，选取其中的一激光雷达称为源激光雷达，另一雷达为目标激光雷达，基于两个雷达之间的转换参数对源雷达轨迹的数据进行更新，修正源雷达轨迹点的时间戳以及三维空间坐标；

(7)遍历源激光雷达感知得到的所有轨迹，针对每一物体的历史运动轨迹，寻找目标雷达中距离最近的物体的历史运动轨迹，若这两物体的历史运动轨迹之间距离小于设定阈值，则认为这两物体的历史运动轨迹能够匹配，将加入物体历史轨迹配对关系集，最终得到源雷达和目标雷达之间物体历史轨迹的配对关系的集合；

(8)遍历步骤(7)中得到的源雷达和目标雷达之间轨迹配对关系的集合，遍历轨迹配对关系，寻找轨迹中具体轨迹点的配对关系集合，具体方法如下：

称配对的两条轨迹分别为轨迹A与轨迹B,轨迹A属于源激光雷达感知到物体的历史运动轨迹，轨迹B属于目标激光雷达感知到物体历史运动轨迹集合，逐一遍历轨迹配对关系集合，并进行如下操作：

a)寻找轨迹A与轨迹B距离最近的轨迹点,分别记为a’和b’，并将这一配对关系添加至轨迹点的配对关系集；

b)以a’和b’为起始位置，以固定的时间差t向时间增序遍历，将相同时间差的点添加到轨迹点的配对关系集；

c)以a’和b’为起始位置，以固定的时间差t向时间减序遍历，将相同时间差的点添加到轨迹点的配对关系集；

最终，得到源激光雷达物体历史轨迹点和目标激光雷达物体历史轨迹点配对关系的集合；

(9)采用步骤(8)得到的轨迹点的配对关系集合构建约束，基于最小二乘最优化的方法求解雷达之间新的转换参数；再基于雷达之间新的转换参数，对源激光雷达感知的轨迹数据进行更新，并清空步骤(7)中得到的轨迹匹配关系的集合以及步骤(8)中获得的轨迹点的匹配关系集合；

(10)跳转至步骤(6)重复执行，直至连续两次对雷达转换参数的更新小于设定阈值或者运行次数超过设定阈值时，阶段2结束，返回最近一次循环中步骤(8)得到的转化参数。

所述阶段1的步骤(4)中，删除步骤(3)中的物体历史轨迹点配对关系的集合中不符合规则的配对关系中采用如下两个规则，如果不符合如下规则任意之一的，则该配对关系：

规则1：邻域相似过滤

当物体历史轨迹点的配对关系中配对的历史轨迹点所在时域周围运动和分布相似，则认为是最近邻配对，两个配对点的邻域也是相似的，将采用邻域相似过滤规则进行删除；

规则2：属性相似性过滤

当物体历史轨迹点的配对关系中配对的历史轨迹点所在时域周围运动和分布相似，应该属于同一物体的轨迹，即该配对关系对应物体的属性相似，将采用属性相似性过滤规则进行删除。

本发明与现有技术相比的优点在于：本发明直接使用了雷达追踪任务的结果来进行路侧多激光雷达的同步，与现有基于原始点云进行同步的方法不同，不再专门为同步任务对原始点云进行处理，提高了信息的利用率，进一步节约了算法。使用雷达感知信息这一动态数据进行配准，与基于原始点云提取特征进行同步的方法不同，算法效果不再依赖于环境中特征的丰富程度，在系统的实际部署中具有更好的适用性。与传统的ICP，NDT等算法不同，本方法不依赖于初始结果，即使时路侧多激光雷达之间具具有很大偏移的情况也能取得很好的结果。

附图说明

图1为本发明方法整体图；

图2为本发明实施的路侧多激光雷达系统模块图。

具体实施方式

下面结合附图及实施例对本发明进行详细说明。应当理解，此处所描述的具体实施仅仅是以解释本发明，并不用于限定本发明。

如图2所示，多激光雷达协系统模块中，各激光雷达独立采集数据，然后经由各边缘MEC进行检测/追踪。边缘云模块负责数据及信息的汇总及融合，并负责设备之间的时空同步，具体流程如本发明所示。

如图1所示，本发明方法的整体流程如图，分为4个步骤：

1)步骤1：激光雷达按照固定的频率采集数据，得到点云数据，传输至MEC进行处理，主要包括检测/追踪。不对相关算法进行限制，检测得到物体的bounding box,追踪得到物体的历史轨迹，用于后续的处理。

2)步骤2：在边缘云部分接收激光雷达数据，包括但不限于雷达追踪和检测结果，具体传输信息可有不同算法策略修改。本发明中，将时间和空间同步问题设定为同一个问题进行同时求解，即同时找到不同雷达之间的时间偏移量以及空间坐标转换关系。

步骤2中对多激光雷达进行时空同步的算法主要包含两个阶段：

阶段一主要基于物体运动的相似性，得到历史轨迹数据点的配对，计算转换关系。这一理论基础在于，物体的历史运动具有时空唯一性，同一运动被不同激光雷达观察到理想条件下应完全相同。这一阶段中，主要考虑了局部信息对时空同步进行约束。在这一阶段中，先对局部运动状态进行提取，包括但不限于速度均值

方差δ²以及局部曲率cur，根据上述轨迹点度量公式：

s.t.α₁+α₂+α₃＝1

计算隶属于轨迹i与轨迹j的轨迹点之间的相似性,其中α₁,α₂,α₃为对每一指标对应的权重，通常情况下，若数据没有明显的误差使用(1/3,1/3,1/3)，参数视情况而调节。基于该相似性程度，每一轨迹点与其最相似的轨迹点建立匹配关系。基于建立的匹配关系，形成几何约束，使用最优化的方法，初步求解两个雷达之间的转换参数。

阶段二中本发明提出了一种针对轨迹数据的迭代收敛的算法，对在阶段一取得的转换参数的基础上，进一步转换的误差。在阶段二中，本发明主要基于1)不同轨迹在时间和空间位置及状态的相对信息；2)轨迹自身连续性约束等全局信息对时空同步进行建立约束，基于整体距离最小化的目标进行参数迭代更新。

3)上述方法阶段一中，本发明考虑实际中存在的感知等误差，以及交通场景下不同物体运动同样具有一定的相似性，即仅基于运动相似性进行匹配会包含大量错误匹配。本发明提出了一种鲁棒且有效的过滤算法，主要包含(1)物体属性的相似性，(2)历史轨迹点邻域的相似性，邻域相似性主要指某一时刻或在某一时间段内，形成配对的轨迹点，其周围轨迹点应具有相似的空间分布。

4)上述方法阶段二中，本发明针对轨迹数据自身特性，在经典的ICP算法基础上进行了优化，在鲁棒性和准确率方面均有提升。其一，经典的ICP算法中对于源点云中的每一个点寻找目标数据集中最近的点构成集合约束求解问题。本发明提出，针对轨迹数据，先对每一轨迹求解距离最近的轨迹，能很好利用轨迹之间的几何相关性，很好地考虑了数据的全局性，能够更好地约束算法收敛于全局最优解。其二，利用轨迹时序性的特性约束轨迹点之间的配对关系，减少错误匹配，提升算法的正确率。

上述方法，通过同步多个激光雷达感知结果，从而得到多个雷达之间的时间和空间转换关系，保证良好的数据融合和数据同步。

以上虽然描述了本发明的具体实施方法，但是本领域的技术人员应当理解，这些仅是举例说明，在不背离本发明原理和实现的前提下，可以对这些实施方案做出多种变更或修改，因此，本发明的保护范围由所附权利要求书限定。

Claims (2)

1.一种基于动态数据进行多激光雷达时空同步的方法，其特征在于：包括一种二阶段的轨迹数据匹配在阶段1中，基于轨迹的局部运动状态相似性得到的配对关系，计算转换参数，初步求得需要时空同步的两个雷达之间的转换参数，实现雷达的粗配准；在阶段2中，对阶段1得到的所述两个雷达之间的转换参数采用一种基于轨迹的最小迭代算法进一步调整，减少参数误差，得到最终的转换参数，转换参数包括空间的旋转和平移，以及时间的偏移；

具体实现如下：

阶段1：

(1)将激光雷达获取点云数据输入检测器进行检测，得到每帧点云中存在物体的包围框bounding box作为检测结果，物体的包围框包括物体的长宽高，转向角，类别信息；将检测结果输入追踪器，将前后帧的不同物体进行关联，得到获得雷达视野中物体的历史运动轨迹；历史轨迹是一系列轨迹点的有序序列，轨迹点包含物体ID，轨迹点对应时间戳，对应空间坐标信息；

(2)采用卡尔曼滤波器去除步骤(1)中物体的历史运动轨迹的噪声，以减少轨迹误差；计算物体的历史运动轨迹在每个轨迹点的附近运动信息，运动信息包含速度均值、方差以及局部曲率；具体计算方法为：

局部速度均值：

局部速度方差：

局部曲率：

其中，v_i表示第i帧的速度，L_i为第i帧的位置向量；m为数据选取的帧数间隔，根据不同数据集的误差进行选择；L_i-m及L_i+m分别表示第i-m及i+m帧的位置向量；N⁺为正整数；

(3)将步骤(2)得到的每个轨迹点附近运动信息作为轨迹点特征，将运动信息相似的轨迹点进行配对，得到不同雷达追踪到物体历史轨迹点的配对关系，并建立物体历史轨迹点配对关系的集合；其中轨迹点相似性度量公式为：

对轨迹中轨迹点i,j的相似性度量公式：

α₁+α₂+α₃＝1

其中，

表示局部速度均值，

表示局部速度方差，cur为局部曲率，α₁,α₂,α₃为各指标权重；

(4)对物体历史轨迹点配对关系的集合中的物体历史轨迹点的配对关系进行过滤，删除步骤(3)中的物体历史轨迹点配对关系的集合中不符合规则的配对关系，以减小对结果带来的误差，得到删除后的得到的轨迹点配对关系；

(5)采用步骤(4)的轨迹点配对关系构成约束条件，基于最优化的方法，初步求解两个雷达之间的转换参数，实现雷达的粗配准；

阶段2中：所述一种基于轨迹的最小迭代算法流程如下：

(6)对于两个激光雷达，选取其中的一激光雷达称为源激光雷达，另一雷达为目标激光雷达，基于两个雷达之间的转换参数对源雷达轨迹的数据进行更新，修正源雷达轨迹点的时间戳以及三维空间坐标；

(7)遍历源激光雷达感知得到的所有轨迹，针对每一物体的历史运动轨迹，寻找目标雷达中距离最近的物体的历史运动轨迹，若这两物体的历史运动轨迹之间距离小于设定阈值，则认为这两物体的历史运动轨迹能够匹配，将加入物体历史轨迹配对关系集，最终得到源雷达和目标雷达之间物体历史轨迹的配对关系的集合；

(8)遍历步骤(7)中得到的源雷达和目标雷达之间轨迹配对关系的集合，遍历轨迹配对关系，寻找轨迹中具体轨迹点的配对关系集合，具体方法如下：

称配对的两条轨迹分别为轨迹A与轨迹B,轨迹A属于源激光雷达感知到物体的历史运动轨迹，轨迹B属于目标激光雷达感知到物体历史运动轨迹集合，逐一遍历轨迹配对关系集合，并进行如下操作：

a)寻找轨迹A与轨迹B距离最近的轨迹点,分别记为a’和b’，并将这一配对关系添加至轨迹点的配对关系集；

b)以a’和b’为起始位置，以固定的时间差t向时间增序遍历，将相同时间差的点添加到轨迹点的配对关系集；

c)以a’和b’为起始位置，以固定的时间差t向时间减序遍历，将相同时间差的点添加到轨迹点的配对关系集；

最终，得到源激光雷达物体历史轨迹点和目标激光雷达物体历史轨迹点配对关系的集合；

(9)采用步骤(8)得到的轨迹点的配对关系集合构建约束，基于最小二乘最优化的方法求解雷达之间新的转换参数；再基于雷达之间新的转换参数，对源激光雷达感知的轨迹数据进行更新，并清空步骤(7)中得到的轨迹匹配关系的集合以及步骤(8)中获得的轨迹点的匹配关系集合；

(10)跳转至步骤(6)重复执行，直至连续两次对雷达转换参数的更新小于设定阈值或者运行次数超过设定阈值时，阶段2结束，返回最近一次循环中步骤(8)得到的转化参数。

2.根据权利要求1所述的基于动态数据进行多激光雷达时空同步的方法，其特征在于：所述阶段1的步骤(4)中，删除步骤(3)中的物体历史轨迹点配对关系的集合中不符合规则的配对关系中采用如下两个规则，如果不符合如下规则任意之一的，则该配对关系：

规则1：邻域相似过滤

当物体历史轨迹点的配对关系中配对的历史轨迹点所在时域周围运动和分布相似，则认为是最近邻配对，两个配对点的邻域也是相似的，将采用邻域相似过滤规则进行删除；

规则2：属性相似性过滤

当物体历史轨迹点的配对关系中配对的历史轨迹点所在时域周围运动和分布相似，应该属于同一物体的轨迹，即该配对关系对应物体的属性相似，将采用属性相似性过滤规则进行删除。

Images