基于Matlab使用激光雷达从点云到跟踪列表跟踪车辆仿真（附源码）

​这些示例显示了如何使用安装在自我车辆顶部的激光雷达传感器的测量值来跟踪车辆。激光雷达传感器将测量结果报告为点云。该示例说明了 MATLAB中用于处理点云和跟踪对象的工作流。此示例中使用的激光雷达数据是从高速公路驾驶方案中记录的。在此示例中，将使用记录的数据通过联合概率数据关联 （JPDA） 跟踪器和交互式多模型 （IMM） 方法跟踪车辆。 ​

与文章《基于Simulink使用激光雷达数据跟踪车辆仿真》不同之处，在于该文章基于Matlab的m文件实现激光雷达数据跟踪车辆仿真的功能，而《基于Simulink使用激光雷达数据跟踪车辆仿真》是基于 Simulink实现跟踪车辆仿真的功能。

一、3-D 边界盒探测器模型

由于激光雷达传感器的高分辨率功能，来自传感器的每次扫描都包含大量点，通常称为点云。必须预处理此原始数据以提取感兴趣的对象，例如汽车、骑自行车的人和行人。在此示例中，您将使用经典的分割算法和基于距离的聚类分析算法。有关将激光雷达数据分割为地平面和障碍物等对象的更多详细信息，请参阅使用激光雷达进行地平面和障碍物检测（自动驾驶工具箱）示例。有关深度学习分割工作流，请参阅使用激光雷达检测、分类和跟踪车辆（激光雷达工具箱）示例。在此示例中，使用该函数将属于障碍物的点云进一步分类为聚类，并且每个聚类都转换为具有以下格式的边界框检测：

pcsegdist​

参考边界框的 x、y 和 z 位置，指其偏航角和,和分别指其长度、宽度和高度。​ pcfitcuboid（激光雷达工具箱）函数使用 L 形拟合算法来确定边界框的偏航角。探测器由一个支持类实现，它围绕着点云分割和聚类功能。此类的对象接受输入并返回具有边界框度量值的对象列表。 ​

该图显示了边界框检测器模型和用于实现每个过程的激光雷达工具箱™函数中涉及的过程。它还显示控制每个进程的支持类的属性。

​

将数据文件下载到临时目录中，其位置由 MATLAB 的函数指定。如果要将文件放在其他文件夹中，请在后续说明中更改目录名称

二、目标状态和传感器测量模型

跟踪对象的第一步是定义其状态，以及定义状态转换和相应度量的模型。这两组方程统称为目标的状态空间模型。为了使用激光雷达对车辆的状态进行建模以进行跟踪，此示例使用具有以下约定的长方体模型：

​

​

测量模型描述了传感器如何分别感知恒定速度和恒定转弯速率状态，并返回边界框测量值。由于状态包含有关目标大小的信息，因此测量模型包括传感器感知到的中心点偏移和边界框收缩的影响，这是由于自遮挡[1]等效应造成的。这种效应是通过收缩因子建模的，该收缩因子与从被跟踪车辆到传感器的距离成正比。

下图显示了在不同状态空间样本下工作的测量模型。请注意当对象在 ego 车辆周围移动时，边界框收缩和中心点偏移的建模效果。

​

三、设置跟踪器和可视化

下图显示了从点云输入获取轨道列表的完整工作流程。

​

现在，设置示例中使用的跟踪器和可视化效果。 

在此示例中，联合概率数据关联跟踪器 与 IMM 筛选器 结合使用来跟踪对象。IMM 滤波器使用恒定速度和恒定转弯率模型，并使用本例中包含的支持函数 进行初始化。IMM方法有助于轨道在运动模型之间切换，从而在机动或车道变换等事件中实现良好的估计精度。下面的动画显示了在IMM滤波器的预测阶段混合恒定速度和恒定转弯率模型的效果。

​

IMM 筛选器在使用来自对象的检测进行校正时更新每个模型的概率。下面的动画显示了车道变换事件期间车辆的估计轨迹以及每个模型的相应估计概率。

​

将跟踪器的属性设置为 ，这样可以指定与状态相关的检测概率。跟踪的检测概率由本示例末尾列出的函数计算。

可视化分为以下主要类别：

1. 激光雷达预处理和追踪 - 此显示显示原始点云、分割地面和障碍物。它还显示了探测器模型的结果检测以及跟踪器生成的车辆轨迹。

2. Ego 车辆显示屏 - 此显示屏显示场景的 2-D 鸟瞰图。它显示了障碍物点云、边界框检测以及跟踪器生成的轨迹。作为参考，它还显示从安装在自我车辆上的摄像头记录的图像及其视野。

3. 跟踪详细信息 - 此显示围绕自我车辆缩放的场景。它还显示更精细的跟踪详细信息，例如每个跟踪的估计位置的误差协方差及其运动模型概率（用 cv 和 ct 表示）。

四、循环访问数据

循环遍历记录的激光雷达数据，使用检测器模型从当前点云生成检测结果，然后使用跟踪器处理检测结果。

​

上图显示了时间 = 18 秒时的三个显示器。轨道由绿色边界框表示。边界框检测由橙色边界框表示。检测器内部还具有橙色点，表示被分割为障碍物的点云。分割的地面以紫色显示。裁剪或丢弃的点云以蓝色显示。

五、结果

现在，分析场景中的不同事件，并了解激光雷达测量模型、联合概率数据关联和交互多模型过滤器的组合如何帮助实现对车辆轨迹的良好估计。

轨道维护

​

上面的动画显示了时间 = 3 秒和时间 = 16 秒之间的模拟。请注意，T10 和 T6 等轨道在时间跨度内保持其 ID 和轨迹。但是，轨道T9丢失，因为跟踪的车辆被传感器长时间错过（未检测到）。另外，请注意，被跟踪的物体能够通过将检测定位到车辆的可见部分来保持其形状和运动学中心。例如，当轨道 T7 向前移动时，边界框检测开始落在其可见的后部，并且轨道保持车辆的实际大小。这说明了在测量函数中建模的偏移和收缩效应。

捕捉演习

​

动画显示，使用 IMM 过滤器有助于跟踪器保持机动车辆上的轨迹。请注意，T4 跟踪的车辆在 ego 车辆后面改变车道。跟踪器能够在此机动事件期间保持车辆上的轨迹。另请注意，在车道变换机动期间，其遵循恒定转弯模型（用 ct 表示）的概率会增加。

 联合概率数据

​

使用联合概率数据关联跟踪器有助于在模棱两可的情况下维护跟踪。在这里，T43和T73跟踪的车辆由于与传感器的距离较远，因此检测概率较低。请注意，当未检测到其中一辆车时，跟踪器能够在事件期间保持跟踪。在事件期间，轨道首先合并，这是JPDA中的已知现象，然后在再次检测到车辆后立即分离。

六、总结

此示例显示了如何使用带有 IMM 过滤器的 JPDA 跟踪器来跟踪使用激光雷达传感器的对象。您 了解 了 如何 对 原始 点 云 进行 预 处理 以 生成 传统 跟踪 器 的 检测， 传统 跟踪 器 假定 每个 传感器 扫描 每个 对象 一 个 检测。您还学习了如何定义长方体模型来描述 JPDA 跟踪器跟踪的扩展物体的运动学、尺寸和测量值。

七、程序

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

​

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

​

 关注下面公众号，后台回复关键词：激光雷达从点云到跟踪列表跟踪车辆仿真，发送源码链接。