Matlab之雷达和激光雷达数据跟踪级融合（附源码）

此示例演示如何根据雷达和激光雷达传感器的测量值生成对象级轨迹列表，并使用轨迹级融合方案进一步融合它们。可以使用扩展对象跟踪器处理雷达测量值，并使用联合概率数据关联 （JPDA） 跟踪器处理激光雷达测量值。可以使用轨道级融合方案进一步融合这些轨道。工作流程示意图如下所示。

 一、合成数据生成的设置方案

此示例中使用的方案是使用驾驶场景（自动驾驶工具箱）创建的。来自雷达和激光雷达传感器的数据分别使用驾驶雷达数据生成器（自动驾驶工具箱）和激光雷达点云生成器（自动驾驶工具箱）进行模拟。场景和传感器模型的创建包含在帮助程序函数 中。

自我车辆装有四个2-D雷达传感器。前后雷达传感器的视野为45度。左右雷达传感器的视场角为150度。每个雷达的方位角分辨率为6度，范围为2.5米。ego还安装了一个3D激光雷达传感器，其方位角视野为360度，仰角为40度。激光雷达的方位角分辨率为 0.2 度，高程为 1.25 度（32 个高程通道）。在下面的动画中可视化传感器的配置和模拟的传感器数据。请注意，雷达的分辨率高于物体，因此会返回每个物体的多个测量值。另请注意，激光雷达与Actor的低多边形网格以及路面进行交互，以从这些对象返回多个点。

 二、雷达跟踪算法

如前所述，雷达具有比物体更高的分辨率，并且每个物体返回多个检测。全球最近邻 （GNN） 和联合概率数据协会 （JPDA） 等传统跟踪器假定传感器每次扫描每个对象最多返回一个检测结果。因此，来自高分辨率传感器的检测必须在使用传统跟踪器处理之前进行聚类，或者必须使用扩展的对象跟踪器进行处理。扩展对象跟踪器不需要对检测进行预聚类，并且通常可以估计运动学状态（例如，位置和速度）和对象的范围。

通常，扩展的对象跟踪器可以更好地估计对象，因为它们使用跟踪的时间历史记录同时处理聚类和数据关联。在此示例中，雷达检测使用高斯混合概率假设密度 （GM-PHD） 跟踪器（跟踪器PHD 和 gmphd）和矩形目标模型进行处理。

使用雷达测量值跟踪对象的算法包装在作为 System 对象™实现的帮助器类 中。此类输出一个对象跟踪对象数组，并根据以下约定定义其状态：

 三、激光雷达跟踪算法

与雷达类似，激光雷达传感器也会返回每个对象的多个测量值。此外，传感器从道路上返回大量点，在用作对象跟踪算法的输入之前必须删除这些点。虽然来自障碍物的激光雷达数据可以通过扩展的目标跟踪算法直接处理，但传统的跟踪算法对于使用激光雷达数据的跟踪仍然更为普遍。这种趋势的第一个原因主要是由于大型数据集的扩展对象跟踪器的计算复杂性更高。第二个原因是对先进的基于深度学习的探测器的投资，如点螺旋体、体素网和PIXOR，它们可以分割点云并为车辆返回边界框检测。这些探测器可以帮助克服由于聚类不当而导致的传统跟踪器的性能下降。

在此示例中，使用传统的联合概率数据关联 （JPDA） 跟踪器处理激光雷达数据，该跟踪器配置了交互多模型 （IMM） 过滤器。使用基于 RANSAC 的平面拟合算法对激光雷达数据进行预处理以移除点云，并通过执行基于欧几里得的距离聚类算法形成边界框。与链接的示例相比，跟踪是在场景帧中执行的，并且跟踪器被以不同的方式调整以跟踪不同大小的对象。此外，变量的状态以不同的方式定义，以约束轨道在其估计的航向角方向上的运动。

使用激光雷达数据跟踪对象的算法包含在帮助器类中，作为 System 对象实现。此类输出一个 objectTrack 对象数组，并根据以下约定定义其状态：

 雷达算法的通用状态定义类似。此外，作为3D传感器，激光雷达跟踪器输出三个附加状态，z,{\点z}和H，分别指被跟踪对象的 z 坐标 （m）、z 速度 （m/s） 和高度 （m）。

 四、设置热熔器、指标和可视化

4.1 热熔器

接下来，将设置一个融合算法，用于融合来自雷达和激光雷达追踪器的轨迹列表。与其他跟踪算法类似，设置轨道级融合算法的第一步是为融合或中心轨道定义状态向量（或状态空间）的选择。在这种情况下，将选择融合轨迹的状态空间与激光雷达相同。选择中央轨道状态空间后，定义中央轨道状态到本地轨道状态的转换。在这种情况下，局部轨迹状态空间是指雷达和激光雷达轨迹的状态。为此，请使用热熔器源配置对象。

定义雷达源的配置。输出轨道设置为 1。作为每个跟踪器上的属性提供，以唯一标识它，并允许融合算法区分来自不同来源的跟踪。因此，可以设置雷达配置的属性与雷达轨迹的属性相同。设置为 允许该未分配的雷达轨迹启动新的中心轨迹。接下来，定义中心状态空间中的轨迹到雷达状态空间的转换，反之亦然。帮助程序函数并执行两个转换，并包含在此示例的末尾。

定义激光雷达源的配置。由于激光雷达轨迹的状态空间与中心轨迹相同，因此无需定义任何变换。下一步是定义状态融合算法。状态融合算法将中心状态空间中的多个状态和状态协方差作为输入，并返回状态和协方差的融合估计值。在此示例中，将使用由帮助器函数提供的协方差交集算法。两个均值高斯估计值的通用协方差交集算法xi和协方差Pi可以根据以下等式进行定义：

xf和Pf是融合状态和协方差，以及w1和w2是每个估计值的混合系数。通常，这些混合系数是通过最小化熔融协方差的行列式或迹线来估计的。在此示例中，混合权重是通过最小化每个估计的位置协方差行列式来估计的。此外，由于雷达不估计3D状态，因此3D状态仅与激光雷达融合。接下来，使用对象组合所有信息。

4.2 指标

在此示例中，将使用广义最佳子模式分配指标 （GOSPA） 指标评估每个算法的性能。可以使用跟踪GOSPAMetric 为每个跟踪器设置三个单独的指标。GOSPA 指标旨在通过提供标量成本来评估跟踪系统的性能。指标值越低，表示跟踪算法的性能越好。 若要将 GOSPA 指标与自定义运动模型（如本示例中使用的模型）结合使用，请将属性设置为“自定义”，并定义轨迹与其关联的地面实况之间的距离函数。

4.3 可视化

此示例的可视化是使用帮助程序类 实现的。显示器分为4个面板。显示屏绘制了每个传感器的测量值和轨迹，以及融合轨迹估计值。显示的图例如下所示。此外，曲目通过其唯一标识以及前缀进行注释。前缀“R”、“L”和“F”分别代表雷达、激光雷达和融合估计。

 五、运行方案和跟踪器

接下来，推进方案，从所有传感器生成合成数据，并对其进行处理以从每个系统生成跟踪。还可以使用方案中提供的基本事实来计算每个跟踪器的衡量指标。

使用可视化和定量指标评估每个跟踪器的性能。分析场景中的不同事件，并了解轨道级融合方案如何帮助更好地估计车辆状态。

5.1 轨道维护

下面的动画显示了每三个时间步的整个运行过程。请注意，三种跟踪系统（雷达、激光雷达和轨道级融合）中的每一个都能够跟踪场景中的所有四辆车，并且没有确认任何错误的轨迹。

 还可以使用 GOSPA 指标的“未命中目标”和“错误跟踪”组件来定量衡量性能的这一方面。请注意，在下图中，由于建立延迟，未命中的目标组件从较高的值开始，并且对于每个跟踪系统，在大约5-10个步骤中下降到零。另请注意，所有系统的假轨道组件均为零，这表示未确认任何错误轨道。

 5.2 轨道级精度

 每个跟踪器的跟踪级别或定位精度也可以在每个时间步通过GOSPA指标进行定量评估。值越低，表示跟踪精度越高。由于没有错过目标或错误轨迹，该指标捕获由每辆车的状态估计引起的定位误差。 请注意，融合估计值的 GOSPA 指标低于单个传感器的指标，这表示在融合来自每个传感器的轨迹估计值后，轨道精度提高。

 5.3 紧密间隔的目标

如前所述，此示例使用基于欧氏距离的聚类和边界框拟合将激光雷达数据馈送到传统的追踪算法。聚类分析算法通常在对象间距较近时受到影响。使用本例中使用的探测器配置，当过往车辆接近自向车辆前方的车辆时，探测器将来自每辆车的点云聚集成一个更大的边界框。可以在下面的动画中注意到，轨道偏离了车辆中心。由于在估计几步时，轨道的确定性更高，因此最初也影响了融合估计值。然而，随着不确定性的增加，它与融合估计值的关联变得越来越弱。这是因为协方差交集算法根据每个估计值的确定性为每个分配的轨迹选择混合权重。

这种影响也反映在 GOSPA 指标中。可以在上面的 GOSPA 衡量指标图中注意到，激光雷达指标显示了第 65 个时间步长附近的峰值。

由于两个主要原因，雷达轨迹在此事件期间不受影响。首先，雷达传感器在每次检测中输出测距率信息，与移动较慢的汽车相比，超车的噪声水平不同。这导致与单个汽车的检测之间的统计距离增加。其次，扩展对象跟踪器根据预测的轨迹评估多个可能的聚类假设，这导致拒绝不正确的聚类并接受正确的聚类。请注意，要使扩展对象跟踪器正确选择最佳聚类，磁道的过滤器必须具有能够捕获两个聚类之间的差异的鲁棒性。例如，具有高过程噪声和高度不确定尺寸的轨道可能无法正确声明集群，因为它的年龄过早，并且具有更大的灵活性来解释不确定的事件。

 5.4 远距离目标

当目标远离雷达传感器时，由于探测器的信噪比降低以及传感器的分辨率有限，测量的准确性会降低。这导致测量中的高不确定性，从而降低轨道精度。请注意，在下面的特写显示中，雷达的轨迹估计值与雷达传感器的地面实况相差甚远，并且报告的不确定性更高。但是，激光雷达传感器在点云中报告足够的测量值以生成“缩小”边界框。激光雷达跟踪算法的测量模型中建模的收缩效应允许跟踪器保持具有正确尺寸的轨迹。在这种情况下，激光雷达混合重量高于雷达，并允许融合估计值比雷达估计值更准确。

六、总结

在此示例中，学习了如何设置轨迹级融合算法，以融合来自雷达和激光雷达传感器的轨迹。还学习了如何使用广义最佳子模式度量及其关联组件来评估跟踪算法。

七、程序

使用Matlab R2021a版本，点击打开。

 打开下面的“RadarAndLidarTrackFusionExample.m”文件，点击运行，就可以看到上述效果。 

 关注下面公众号，后台回复关键词：雷达和激光雷达数据跟踪级融合，发送源码链接。