安波福从记录的传感器数据中获取驾驶场景
作者 Krishna Koravadi, 安波福和 Seo-Wook Park, MathWorks
此工作流是使用 MATLAB 和 RoadRunner 产品完成的，它提供了一种结构化的方法来创建虚拟场景，使安波福的工程师能够通过仿真严格验证 ADAS/AD 闭环算法。
对于开发下一代汽车的工程师，具备在逼真的驾驶场景下测试高级驾驶辅助系统 (ADAS) 和自动驾驶 (AD) 系统算法的能力至关重要。但是，在真实交通中测试新开发算法的性能和稳健性是不切实际的。理想的替代方案是根据实际道路测试期间所捕获的传感器数据生成虚拟场景。这些场景随后可以经济高效的方式，在虚拟环境中安全、可重复地测试和微调 ADAS/AD 算法。
安波福的工程师通过我们实现的工作流从记录的车辆数据中获取驾驶场景。此工作流是使用 MATLAB® 和 RoadRunner 产品分三步完成的。第一步是，基于来自车载 GPS、惯性测量单元 (IMU)、相机和地图数据的输入，重建自主车辆的轨迹。第二步是，使用雷达数据和非因果联合集成概率数据关联 (JIPDA) 跟踪算法重建非自主车辆的轨迹，以创建周围交通的动态表示。最后一步是，使用 RoadRunner 生成场景，根据需要进行编辑，并将其导出为 ASAM OpenSCENARIO® 格式，该格式可用于其他场景编辑和可视化工具。此工作流是使用 MATLAB 和 RoadRunner 产品完成的，它提供了一种结构化的方法来创建虚拟场景，使 安波福的工程师能够通过仿真严格验证 ADAS/AD 闭环算法（图 1）。
图 1. 从传感器数据中获取场景的工作流。
重建自主车辆的轨迹
在工作流的第一阶段，我们会创建一个虚拟道路场景，并添加一个自主车辆，其轨迹是根据记录的传感器数据重建的。
为了创建虚拟场景，我们使用 RoadRunner Scene Builder 从导入的高清地图创建三维道路模型，该地图以通过 GPS 获得的车辆位置为中心。我们从地图上删除不需要的道路并保存场景（图 2）。
图 2. 使用 RoadRunner Scene Builder 从高清地图生成场景。
此时，我们可以使用 GPS 数据在地图上叠加自主车辆的轨迹。然而，完全依赖 GPS 输入进行自主车辆跟踪是有问题的，原因有如下几个：确定车辆行驶在哪个车道通常不够准确；GPS 信号可能会临时丢失；其采样时间（通常为数百毫秒）相对较长。为了弥补这些缺陷，我们使用 Navigation Toolbox™ 中的 insfilterAsync 对象，将 GPS 数据与来自 IMU 的加速度计和陀螺仪数据融合在一起。IMU 数据每 10 毫秒采样一次。通过融合该数据，我们能够更精确地估计自主车辆的轨迹（图 3）。
图 3. 根据单独的 GPS 数据（蓝色）和融合的 GPS 和 IMU 数据（红色）估计的自主车辆轨迹。
尽管 GPS 和 IMU 数据融合提高了准确度，但结果仍不足以可靠地确定自主车辆行驶在哪个车道上。为了提高准确度，我们利用相机数据跟踪自主车辆及其相邻车道的车道标线检测情况。然后，我们校正自主车辆轨迹，使基于高清地图计算的车道数和偏移量与基于车道检测计算的车道数和偏移量相符（图 4）。
图 4. 通过车道检测定位之前（蓝色）和之后（红色）的自主车辆轨迹。
这样，我们就有了车道级别准确度的自主车辆轨迹，该轨迹可导出到逗号分隔值 (CSV) 文件。
然后，我们可将该 CSV 文件导入 RoadRunner Scenario 以创建一个场景，以供我们可以用来运行自主车辆的仿真（图 5）。
图 5. 通过车道检测定位之前（白色）和之后（蓝色），在 RoadRunner Scenario 中运行的自主车辆仿真。
重建非自主车辆的轨迹
工作流中的第二步是将非自主车辆的轨迹添加到场景中。在此步骤中，我们评估了几种不同方法，包括使用雷达、激光雷达以及雷达和相机的组合。最终，我们决定继续使用基于雷达的方法，因为我们记录的大多数车辆数据都包括雷达测量值，而激光雷达数据只能从专门安装仪器的车辆获得。
为了准备用于跟踪的雷达数据，我们需要运行一系列信号处理操作来区分静态检测（如护栏）和动态检测（如其他车辆），以及过滤由多径反射引起的重影。然后，我们应用基于密度的含噪空间聚类 (DBSCAN) 算法来生成目标级检测结果。
接下来，我们将目标级检测结果馈送到非因果联合集成概率数据关联 (JIPDA) 跟踪器。这种跟踪算法是非因果的，因为在每个时间步，它都使用该时间步之前和之后直到记录数据结束的所有检测结果。在线多目标跟踪算法（例如，在行驶的车辆中运行的算法）只能依赖过去和当前的测量值，而非因果算法可以离线运行，并使用从场景开始到结束的所有雷达测量值。由于事先获得了这些测量值，离线 JIPDA 跟踪算法可比在线跟踪算法更准确地解决数据关联中的多义性（这种多义性会导致轨迹切换、分段和错误轨迹）问题。
为了实现这种改进的性能，JIPDA 算法会运行迭代数据关联（图 6）。在每个时间步，它都使用截至时间步 k-1 的测量值执行前向跟踪，然后使用从时间步 k+1 到 N 的测量值执行后向跟踪。该算法将这两项操作的融合预测与检测相关联，并移至下一个时间步。最后，该算法会执行 Rauch-Tung-Striebel 平滑处理。虽然该算法本身可能看似很复杂，但在 MATLAB 中实现它只需使用 Sensor Fusion and Tracking Toolbox™ 中的 smootherJIPDA 对象编写几行代码即可。
图 6. JIPDA 算法的图形化表示。
使用 JIPDA 算法计算每个非自主车辆的轨迹后，我们将雷达跟踪结果与相机的视图进行比较（图 7）。
图 7. 雷达跟踪结果与同步相机视图。
场景生成和验证
作为工作流的第三步也是最后一步，我们将非自主车辆轨迹与自主车辆轨迹相结合来完成场景的创建。为此，我们使用 Automated Driving Toolbox™ 中的 actorprops 函数将非自主车辆轨迹变换为世界坐标系。
完成这些操作后，我们将非自主车辆轨迹导出到 CSV 文件，并将该文件导入 RoadRunner Scenario，以将这些轨迹添加到场景中。然后，我们运行仿真来检查结果。
我们可以使用 RoadRunner Scenario 编辑该场景或将其导出为 ASAM OpenSCENARIO 格式。该场景随后可用于任何兼容 ASAM OpenSCENARIO 的仿真器和播放器，包括 esmini（图 8）。
图 8. RoadRunner Scenario（左）和 esmini（右）中运行的同步仿真。
作为最后的验证步骤，我们将获得的场景与挡风玻璃相机的视图进行比较，以确认车道变更、车辆超车和其他事件的匹配情况（图 9）。
图 9. 同步的 RoadRunner Scenario 仿真（左）和相机视图（右）。
一旦此工作流得到验证，我们就会应用它从更多的记录传感器数据集自动生成虚拟场景。这些虚拟场景随后用于测试 ADAS/AD 算法，作为我们仿真工作流的一部分。
2023 年发布
