基于Matlab在城市环境中多激光雷达网格跟踪仿真（附源码）

此示例演示如何使用基于格网的跟踪器跟踪具有多个激光雷达的移动对象。基于网格的跟踪器可以早期融合来自高分辨率传感器（如雷达和激光雷达）的数据，以创建全局对象列表。

一、介绍

大多数多对象跟踪方法将环境表示为一组离散且数量未知的对象。跟踪器的工作是使用传感器测量来估计物体的数量及其相应的状态，例如位置，速度和尺寸。对于雷达或激光雷达等高分辨率传感器，可以使用点-目标跟踪器或扩展对象跟踪器来配置跟踪算法。

点-目标跟踪器

点-目标跟踪器假定每个对象最多可以为每个传感器产生一个检测。因此，当使用点-目标跟踪器跟踪扩展对象时，首先从对象级别的传感器测量中提取边界框检测等功能。然后，这些对象级特征与跟踪器中的对象级假设融合在一起。因此，传感器级别的不良对象级提取算法（例如不完美的聚类）会极大地影响跟踪器的性能。

扩展对象跟踪器

另一方面，扩展的对象跟踪器处理检测，而不会在传感器级别提取对象级假设。扩展的对象跟踪器将传感器测量值与跟踪器维护的对象级假设直接相关联。为此，一类算法通常需要特定于每个传感器模态的对象范围的复杂测量模型。

基于网格的跟踪器可以被视为一种扩展对象跟踪算法，它使用动态占用网格图作为环境的中间表示。在动态占用网格图中，使用一组 2-D 网格单元离散化环境。动态地图表示由网格像元表示的空间的占用率和运动学。使用动态映射估计并将单元格进一步分类为静态和动态可作为预处理步骤，以过滤掉静态对象的测量值并降低计算复杂性。

在此示例中，将使用跟踪器GridRFS 系统对象来配置基于网格的跟踪器。该跟踪器使用具有登普斯特-谢弗近似的随机有限集 （RFS） 公式来估计动态映射。此外，它使用最近邻像元到跟踪关联方案来跟踪场景中的动态对象。为了初始化新磁道，跟踪器使用 DBSCAN 算法对未分配的动态网格单元进行聚类。

二、设置场景和激光雷达传感器模型

此示例中使用的方案是使用驾驶方案设计器（自动驾驶工具箱）应用程序创建的，并已导出到 MATLAB 函数。该方案表示一个城市交叉口场景，并包含各种对象，包括行人、骑自行车的人、汽车和卡车。

ego车辆配备了6个均匀的激光雷达，每个激光雷达的水平视野为90度，垂直视野为40度。激光雷达是使用激光雷达PointCloud生成器（自动驾驶工具箱）系统对象模拟的。每个激光雷达都有 32 个高程通道，方位角的分辨率为 0.16 度。在此配置下，每个激光雷达传感器每次扫描输出大约 18000 个点。此处显示了每个传感器的配置。

场景和来自不同激光雷达的数据可以在下面的动画中可视化。为简洁起见，为了使示例更易于可视化，通过将属性指定为 ，将激光雷达配置为不从地面返回点云。使用真实数据或使用来自道路的模拟数据时，必须使用点云预处理来消除来自地面和其他环境的回波。

 三、设置基于网格的跟踪器

可以定义基于网格的跟踪器，用于跟踪场景中的动态对象。定义跟踪器的第一步是将传感器配置设置为对象。传感器配置允许您指定每个传感器相对于跟踪坐标系的安装。传感器配置还允许指定每个传感器的检测限 - 视场和最大范围。在此示例中，将使用模拟激光雷达传感器的属性来定义这些属性。

在此示例中，使用车辆的模拟姿势在全球坐标系或世界坐标系中跟踪目标。该信息通常通过惯性导航系统获得。当传感器在场景系统中移动时，每次都必须通过将配置指定为跟踪器的输入来更新其配置。

跟踪器使用二维网格作为环境的中间表示。网格由 3 个属性定义：其长度、宽度和分辨率。长度和宽度分别描述了网格在自我车辆的局部X和局部Y方向上的跨度。分辨率定义了网格中每米的像元数。在此示例中，您使用了一个 120 米 x 120 米的格网，每米有 2 个像元。除了定义网格之外，还可以通过指定网格的原点（左角）相对于自向量器的原点来定义自营车辆的相对位置。在这个例子中，自我车辆位于网格的中心。

跟踪器使用基于粒子的方法估计每个网格单元的状态，并进一步将它们分类为动态或静态。它在网格上使用固定数量的持久性粒子，该网格定义了现有目标的分布。它还使用固定数量的颗粒来采样新生儿靶标的分布。这些出生粒子根据出生概率在不同的网格细胞中进行采样。此外，使用使用先验限值提供的先验信息对粒子的速度和其他未知状态（如转弯率和加速度（当跟踪器不适用时适用）进行均匀采样。重采样步骤可确保网格上的粒子数量保持不变。

跟踪器使用邓普斯特-谢弗方法来定义每个像元的占用率。动态网格估计网格的占用和自由状态的信念质量。在预测期间，网格单元的占用信念质量由于粒子分布的预测而更新。控制生存概率（DeathRatePs）的粒子，并导致在预测期间占用信念质量的衰减。由于自由信念质量与粒子无关，自由信念质量使用预先指定的恒定折扣因子衰减。此折扣系数指定免费区域在预测期间保持空闲的概率。

在估计每个网格单元的状态后，跟踪器使用其估计的速度和相关不确定性将每个网格单元分类为静态或动态。此外，跟踪器使用以下技术使用动态单元格提取对象级假设：

每个动态网格单元都被视为与现有轨道进行分配。如果网格像元和轨迹之间的负对数似然低于分配阈值，则会将动态网格像元分配给其最近的轨道。超出分配阈值的动态网格单元格被视为未分配。跟踪器在每个步骤中使用未分配的网格单元格来启动新轨道。由于多个未分配的网格单元可以属于同一个对象跟踪，因此使用 DBSCAN 聚类分析算法来协助此步骤。由于在将单元格分类为静态或动态时存在误报，因此跟踪器以两种方式过滤这些误报。首先，只有形成具有超过指定点数的聚类的未分配单元才能创建新轨迹。其次，每个轨道首先初始化为暂定轨道，并且只有在 N 次中检测到 M 个时才确认。还可以通过指定跟踪器的属性，通过在 GPU 上执行动态地图估计来加速模拟。

可视化

此示例使用的可视化效果是使用与此示例一起附加的帮助器类 定义的。可视化包含三个部分。

地面真相 - 前视图：此面板使用来自自我车辆的追逐图显示地面真相的前视图。为了强调场景中的动态参与者，静态对象以灰色显示。

激光雷达视图：这些面板显示来自每个传感器的点云返回。

基于网格的跟踪器：此面板显示基于网格的跟踪器输出。轨道显示为框，每个框都由其标识注释。轨迹将叠加在动态格网地图上。动态网格单元格的颜色是根据色轮定义的，色轮表示场景帧中的运动方向。静态网格像元根据其占用情况使用灰度表示。灰度表示网格单元所占用空间为自由的概率。轨道的位置显示在 ego 车辆坐标系中，而速度矢量对应于场景帧中轨道的速度。

运行方案和跟踪动态对象

接下来，运行方案，模拟来自每个激光雷达传感器的激光雷达传感器数据，并使用基于网格的跟踪器处理数据。

四、结果

接下来，使用此示例中使用的可视化效果分析跟踪器的性能。基于网格的跟踪器使用估计网格映射中的动态单元格来提取对象轨迹。下面的动画显示了此方案中跟踪器的结果。“基于网格的跟踪器”面板显示估计的动态地图以及对象的估计轨迹。它还将安装在自我车辆上的传感器配置为蓝色圆形扇区。请注意，这些传感器封装的区域在动态地图中估计为“灰色”，表示任何传感器都不会观察到该区域。该补丁还可以指示自我车辆在动态网格上的位置。

轨迹仅从动态网格单元格中提取，因此跟踪器能够过滤掉静态对象。另请注意，车辆进入网格区域后，其轨道建立只需很少的时间步长。这是由于两个主要原因。首先，在将细胞分类为动态时存在建立延迟。其次，对象的确认阈值需要一些步骤才能将跟踪建立为已确认对象。

 接下来，可以查看几个轨道的历史记录，以了解动态网格的估计如何影响轨道的状态。

4.1 纵向移动轨道

以下快照显示了由 T1 表示的轨道的历史记录。T1 赛道表示在模拟的最初几秒钟内通过左侧的 ego 车辆的黄色汽车。请注意，此轨道占用的网格单元格以红色显示，表示它们在正 X 方向上的运动。轨道使用分配的网格单元的速度分布来获取轨道的速度和航向信息。它还使用所分配的格网像元的空间分布来获取其长度、宽度和方向。的默认值从每一步关联格网像元的空间分布中提取新的长度、宽度和方向信息。在下面的快照中可以看到这种效果，其中轨道的长度和宽度根据关联网格单元格的边界框进行调整。通过使用自定义 ，可以使用轨道的预测长度、宽度和方向添加其他过滤方案。

 接下来，仔细看看T4的历史。T4轨道表示卡车向自我车辆相反的方向移动。请注意，表示此轨道的网格单元格显示为蓝色，表示网格单元格的估计运动方向。另请注意，轨道中存在网格单元格，这些单元格被跟踪器错误地分类为静态（白色）。当传感器报告物体先前被遮挡的区域时，这些错误分类的网格细胞经常发生，因为跟踪器具有对这些细胞属性进行分类的建立延迟。

请注意，在时间 = 4 时，当卡车和车辆彼此靠近时，网格单元保持各自的颜色，表示它们估计的速度方向之间的明显差异。这也会导致网格像元与 T1 和 T4 的预测轨迹之间的正确数据关联，这有助于跟踪器将它们解析为单独的对象。

4.2 横向移动的轨道

以下快照表示由 T7 表示的轨道。该轨道表示当自我车辆停在十字路口时，车辆沿横向移动。请注意，此轨迹的网格单元格以紫色表示负 Y 方向的运动方向。与其他轨道类似，轨道使用所分配的格网像元的空间分布来保持其长度和宽度。

 4.3 轨道改变方向

在此示例中，对跟踪器使用了“恒定速度”模型。此运动模型假设目标以恒定的速度移动，这意味着恒定的速度和方向。然而，在城市场景中，这种假设通常并不准确。为了补偿物体的未知加速度，在跟踪器上指定了过程噪声。以下快照显示了轨道 T2 的历史记录。该轨道代表自我车辆正前方的车辆。请注意，在地面事实中，这辆车在十字路口右转。

 请注意，与此轨道关联的网格单元格的颜色从红色变为紫色。此外，颜色的转换会导致一些错误分类的单元格，这可能导致对车辆长度和宽度的估计不佳。跟踪器能够保持该车辆上的轨道是由于三个主要原因的耦合效应。首先，跟踪器允许指定分配阈值。即使预测的轨迹与动态网格单元格不对齐，它也可以与它们关联到某个阈值。其次，要从保持在阈值之外的网格单元格创建新轨道，需要满足最小单元格数标准。第三，跟踪器具有删除阈值，它允许在删除跟踪之前滑行几个步骤。如果在转弯期间网格单元的分类非常差，则轨道可以存活几步，并且可以与网格单元重新关联。请注意，错误分类的网格像元在 Track T8 中更容易观察到，如下面的历史记录所示。T8 赛道代表在十字路口右转之前沿正 Y 方向行驶的浅蓝色汽车。这辆车在转弯前被部分遮挡，在转弯时有另一辆行驶的车辆。

五、总结

在此示例中，学习了基于网格的跟踪器的基础知识，以及如何在复杂的城市驾驶环境中跟踪动态对象。还学习了如何配置追踪器以使用来自多个激光雷达传感器的点云来追踪对象。

六、程序

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

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

关注下面公众号，后台回复关键词：在城市环境中多激光雷网格跟踪仿真，发送源码链接。