Matlab之雷达和激光雷达数据跟踪级融合（附源码）
此示例展示了如何从雷达和激光雷达传感器生成合成数据，并通过融合算法估计车辆状态。首先，场景中创建了四个2-D雷达传感器（前后各两个）和一个3D激光雷达传感器。雷达具有更高的分辨率但返回多个测量值，而激光雷达返回多个点云并需预处理以移除障碍物影响。传统跟踪算法如GNN和JPDA因无法直接处理高分辨率数据而需预聚类或扩展对象跟踪器（EOMT）进行处理。
热熔器用于融合来自两种传感器的轨迹列表，并结合状态协方差交集算法优化估计值。广义最佳子模式分配（GOSPA）指标用于评估追踪性能，并通过可视化工具展示各传感器的测量值与轨迹估计值。实验表明，在紧密间隔的目标中（如超车场景），融合估计值优于单独的雷达或激光雷达结果；而在远距离目标中（如超越障碍物后），激光雷达表现更优。最终程序通过Matlab实现了上述流程并可生成效果图。
目录
一、合成数据生成的设置方案
二、雷达跟踪算法
三、激光雷达跟踪算法
四、设置热熔器、指标和可视化
4.1 热熔器
4.2 指标
4.3 可视化
五、运行方案和跟踪器
5.1 轨道维护
5.2 轨道级精度
5.3 紧密间隔的目标
5.4 远距离目标
六、总结
七、程序
本示例阐述了基于雷达传感器及激光雷达传感器测量值的对象级轨迹列表生成方法，并通过轨迹级融合方案进一步整合这些轨迹。具体而言，在处理 radar 测量数据时可采用扩展目标跟踪器（Extended Object Tracker），而对于 lidar 数据则可应用联合概率数据关联（JPDA）追踪算法以实现精确匹配。随后，在获得各独立估计结果后需通过轨道融合模块完成最终的优化与整合工作。工作流程图如下所示。
一、合成数据生成的设置方案
此示例中的方案基于驾驶场景（自动驾驶工具箱）进行创建。其中来自雷达与激光雷达的数据分别采用驾驶雷达数据生成器以及激光雷达点云生成器（自动驾驶工具箱）对数据进行模拟。其中场景及传感器模型的构建嵌入到辅助程序函数中。
车辆搭载了四个二维（2-D）雷达探头装置。左右前后布置的 radar 探测器具有不同的覆盖范围：左右两个各拥有45度视场角范围；而前后布置的两个则可提供150度视场角范围。每个 radar 的方位分辨率设定为6度，并能有效覆盖距离至近达2.5米远的目标物。此外，在该车身上还配备了全方位（360度）扫描能力显著提升的一台三维（3D）激光雷达探测器系统。该系统不仅能够全方位扫描环境（仰角可达40度），还能精确测量了1.25度高程信息，并分割成32个层次（即具有分层测高能力）。为了直观展示探测器布局及其工作原理，在下面提供的动画中进行了详细模拟和数据可视化展示。特别指出的是，在与低复杂度多边形网格体及路面交互过程中所得数据更为丰富和可靠。
二、雷达跟踪算法
基于以往的讨论，在本研究背景下
在一般情况下, 高级的对象跟踪系统能够显著提升对目标的估计精度. 这种系统基于它们采用了时间序列的历史数据, 并协同处理目标分类与关联问题. 在本案例中, 雷达检测采用了一种基于高斯混合概率假设密度 (GM-PHD) 的追踪框架, 其包含两种变体形式——PHD和GMPHD追踪算法. 该算法框架下, 系统不仅能够有效识别多个目标的存在, 还能准确建立其空间定位关系.
基于雷达测得的值作为依据，在System对象™实现的一个辅助类中对算法进行封装。该帮助器类能够生成一个由多个目标组成的数组，并遵循如下规则确定其状态：
该帮助器类能够生成一个由多个目标组成的数组，并遵循如下规则确定其状态：
- 每个目标都会被单独处理
- 输出结果遵循特定的状态定义
- 数学公式...原样保留
三、激光雷达跟踪算法
在自动驾驶领域中使用感知技术时会遇到许多挑战，在实现车辆自动驾驶的过程中需要考虑的因素也相对较多
在此示例中，采用传统的联合概率数据关联（JPDA）跟踪系统对激光雷达数据进行处理，并配置了基于交互多模型（IMM）的滤波器进行目标跟踪。通过基于RANSAC算法的平面拟合方法对激光雷达获取的数据进行预处理以去除点云噪声，并利用欧几里得距离聚类算法生成相应的边界框用于限定物体区域。与参考案例相比，在同一场景帧内完成目标跟踪任务，并根据目标尺寸差异采用了不同的跟踪策略以适应不同大小的目标运动需求。此外，在变量状态定义方面进行了优化调整，在估计的目标飞行方向上限制目标运动轨迹以便于后续精确定位与预测
基于激光雷达数据的跟踪算法嵌入到辅助类模块中，并通过系统对象机制实现其功能。该系统将生成一个 objectTrack 对象数组序列，并按照以下规则定义状态信息：
雷达算法具备相似的状态定义特征。此外，在三维传感器体系中作为重要组成部分，在激光雷达跟踪器中增加了三个额外的状态变量：z, 点z和H。这些状态变量分别对应于被追踪物体的空间位置信息：包括沿Z轴的位置坐标（以米计），Z方向的速度（每秒米），以及高度值（以米为单位）。
四、设置热熔器、指标和可视化
4.1 热熔器
在本节中,我们将设计一种融合算法,用于整合来自雷达和激光雷达的数据轨迹,以提升定位精度。该方法与传统跟踪方法一致,其核心步骤包括确定中心参考轨迹的状态空间以及建立从中心参考轨迹到本地参考轨迹的状态转换关系。为了实现这一目标,我们将采用热熔器源配置对象来辅助完成数据整合过程。
请详细说明您需要对哪些技术指标进行优化及优化目标。
针对该系统的关键性能指标进行详细描述：
- 配置相关参数
- 输出通道数设定为一
- 作为每个目标检测器的独特标识
- 便于识别自不同数据源的目标
- 确保两者在属性上保持一致
- 无需额外设置即可自动启用
- 建立双向的数据映射关系
协助完成数据转换过程，并置于本示例结尾部分。
确定激光雷达源的配置方案。考虑到激光雷达轨迹的状态空间与中心轨迹具有相同的属性特征，在这种情况下无需建立任何变换关系即可实现目标定位任务。接下来将阐述状态信息的融合过程：该过程将用于状态信息的整合，并返回融合后的状态估计及其协方差矩阵。具体而言，在本例中将采用协方差交集算法作为辅助工具来进行数据融合运算。对于两个均值高斯分布估计量而言，其通用协方差交集算法ξ和对应的协方差矩阵Π可通过以下数学表达式进行定义：
\xi = \arg\max_{\mu} \min(\mathcal{N}(\mu, \Sigma_1), \mathcal{N}(\mu, \Sigma_2))
\Pi = \arg\min_{\Sigma} \max(\Sigma_1, \Sigma_2)
其中\mathcal{N}(\cdot)表示正态分布函数，
\Sigma_i = \Pi_i + R_i (i=1,2)
xf代表融合的状态向量与协方差矩阵；w1、w2分别作为各估计量的权重参数。通常情况下，在构建多源数据融合模型时会采用使融合后的协方差矩阵行列式的值最小的方法来确定各分量的数据量分配参数。在这个具体案例中，则主要依赖于激光雷达提供的二维信息来进行三维目标状态的推断。随后将所有采集到的信息进行综合分析处理。
4.2 指标
在此示例中使用广义最佳子模式分配指标（GOSPA）来评估各个算法的表现。为了实现这一目标，请为每个追踪器设定三个独立的追踪GOSPA指标。该指标旨在通过提供标量成本来量化跟踪系统的性能。具体而言，在数值越小的情况下表示追踪算法具有更好的性能表现。当将其与自定义运动模型结合时，请将属性设置为‘自定义’并定义轨迹与其相关地面特征之间的距离函数。
4.3 可视化
该可视化过程借助帮助程序类 实现。显示器被划分为四个独立的部分。具体展示的内容如下所示：
此外，在每个部分中，默认会标注曲目的信息，并通过前缀进行标识。
其中前缀‘R’代表雷达设备，‘L’代表激光雷达设备，并融合估计结果。
五、运行方案和跟踪器
接下来实施方案，在基于所有传感器的数据生成综合数据集后对这些数据进行预处理，则可从各个子系统中提取跟踪信息。同时，在评估各跟踪器性能时，则可依据该方案所提供的基础参数进行量化分析。
通过图像化呈现和数值化评估的方法对每个跟踪器的性能进行综合分析；深入研究各类动态过程以优化车辆状态的预测能力
5.1 轨道维护
该动画展示了每隔三个时间步的整体运行情况。特别值得一提的是，在该系统中设置了三个追踪模块：雷达、激光雷达以及轨道级融合追踪器。这些追踪模块在每一轮观测中均能够精确识别并记录下场景中所有四辆待监测车辆的状态信息，并且确保了追踪结果的高度准确性和可靠性。
该性能指标通过‘未命中目标’和‘错误跟踪’组件进行量化评估。特别地，请注意下图中所示的情况：由于系统建立延迟的存在，在初始阶段‘未命中目标’组件的数值较高；随后，在约5至10个时间步骤内，该数值逐渐降至零。此外，请注意观察到在所有系统中假轨道组件的数值均为零；这表明没有被识别出的错误轨迹。
|
|
|
|---|---|
5.2 轨道级精度
每个跟踪器的定位精度水平或定位准确度均可在每个时间段内通过GOSPA指标进行量化评估。数值越小则表明定位精度越高。鉴于未丢失目标或出现错误轨迹记录，在该评价标准下能够反映出每一辆车状态估计所造成的定位偏差。值得注意的是，在对各传感器提供的轨迹估计结果进行融合后所得出的结果对应的GOSPA指标数值低于单一传感器的情况（即其数值小于单一传感器对应的数值），这表明多传感器信息融合后的定位精度得到了显著提升。
5.3 紧密间隔的目标
如同前面所述，在车辆识别与行为预测系统中
这种影响也体现在GOSPA指标中。从上述GOSPA评价指标图中可以看出，在第65个时间步附近区域有明显的峰值。
由于存在两大关键因素，在此事件期间雷达信号未受到影响。首要因素是雷达扫描系统在每一次扫描周期内生成测距数据，并将这些数据与移动较慢的传统型乘用车进行比较分析；由于其超车行为产生的噪声特性与单一车辆存在差异；因此导致与其他单一车辆测距结果间的统计距离有所提升；其次，在多目标跟踪过程中扩展目标追踪算法依据预估运动轨迹对多组候选群落进行分析；通过评估各候选群落间的相似度指标实现错误群落的有效抑制；最终确定正确的群落归属；值得注意的是要想使该算法成功识别出最佳群落划分；需要确保系统的滤波器模块具备识别群落间特征差异的能力；否则若系统滤波器无法有效区分两组数据间的细微差别则可能导致误判现象的发生
5.4 远距离目标
随着目标逐渐远离雷达传感器时
六、总结
在此示例中，我们掌握了设置轨迹级融合算法的方法，并结合雷达传感器与激光雷达传感器的数据进行融合处理。此外，我们了解了利用广义最佳子模式度量及其相关组件来评估跟踪系统的性能。
七、程序
使用Matlab R2021a版本，点击打开。
请在以下位置打开该程序文件...并单击运行该文件后，请查看其运行效果。
程序下载链接：<>