APPLICATION

多模态传感器融合目标跟踪

本文深入介绍LiDAR、雷达和无迹卡尔曼滤波器，并从零开始用C++实现。

admin

• 18 min read
多模态传感器融合目标跟踪
“我坚决认为不存在所谓的模糊概念……我们谈论模糊的东西，但它们不是科学概念。过去有些人发现了一些有趣的东西，并以非模糊的方式表述他们的发现，因此我们推动了科学的进步。”
— 鲁道夫·E·卡尔曼（1972年）

在自动驾驶汽车快速发展的世界中，准确且安全的导航取决于汽车如何感知和理解其周围环境。在特斯拉、Cruise和NASA等公司工作十年后，我亲身体验了强大的感知对于现实世界自主性的重要性。

现代自动驾驶系统依赖于一种复杂的传感器组合——每种传感器都有其独特的优点和局限性——来构建对环境的连贯理解。立体相机捕捉视觉深度和纹理。雷达穿透雨和雾。LiDAR生成车辆周围世界的精确3D点云。这些传感器共同为感知、定位和决策做出贡献。

让我们简要探讨每种传感器的作用。

相机 vs LiDAR vs 雷达性能比较图
雷达：具有抗干扰能力的速度感知传感

通常安装在汽车保险杠上，雷达擅长利用多普勒效应测量直接速度——这是相机和LiDAR无法原生做到的。这使得雷达能够更快更准确地收敛到物体运动，使其非常适合跟踪车辆、检测盲点以及在恶劣天气条件下进行地图绘制。

然而，雷达的空间分辨率较低，可能导致误报。例如，路面上的一个汽水罐可能被误认为是更大的危险物。

LiDAR：高分辨率3D映射

LiDAR使用脉冲红外激光扫描环境并生成详细的3D点云。其垂直扫描层和旋转设计提供了丰富的空间数据，对于精确的目标检测和定位至关重要。

但LiDAR也有其局限性。它不能直接测量速度，更容易受到灰尘和天气的影响，并且其尺寸可能使集成到现代车辆设计中变得复杂。

为什么需要传感器融合？

每个传感器都提供世界的一部分视图。单独使用，它们都不够。但通过智能融合，它们形成了对驾驶环境的完整且可靠的了解。

这就是无迹卡尔曼滤波器（UKF）发挥作用的地方。

与假设状态和测量之间线性关系的标准卡尔曼滤波器不同，UKF专门设计用于具有非线性动态和多个传感器模式的测量模型的系统——如自动驾驶中遇到的那样。

不过，与普通的或扩展卡尔曼滤波器相比，UKF能更好地将雷达的极坐标测量（距离、角度、速度）与LiDAR的笛卡尔坐标（x, y）结合起来。这是因为UKF不通过导数对方程进行线性化，而是使用一组精心选择的sigma点来近似状态分布。这些点通过系统的非线性函数传播，使滤波器能够保持对不确定性的更准确估计。

通过结合LiDAR的空间精度和雷达的动态运动数据，UKF实现了更快的收敛和更准确的目标跟踪——这对自动驾驶汽车在现实交通中运行至关重要。

现在，让我们深入探讨如何从零开始实现一个UKF。为了使本教程易于访问和聚焦，我们将使用模拟的LiDAR和雷达测量来估计简单2D环境中物体的位置和速度。

请注意，现实中的自动驾驶系统要复杂得多——通常会整合摄像头视觉、先进的传感器融合管道，甚至视觉语言模型以实现对世界的丰富语义理解。但从这个简化的案例开始，将为我们理解多模态感知及其扩展性奠定坚实的基础。

那么，我们开始吧，好吗？

1、教程概览

本教程分为三个关键部分。首先，我们初始化所有必要的参数以设置我们的系统。接下来，我们实现无迹卡尔曼滤波器，使用带有噪声的LiDAR和雷达测量来估计高速公路上的多个车辆的状态。最后，我们通过计算与真实数据的均方根误差（RMSE）来评估滤波器的准确性。本项目中使用的所有代码都在下面的章节中包含并解释。

为了启动该项目，我首先将原始代码库适配到现代库和工具链上——更新构建系统，解决依赖问题，并确保与最新版本的PCL和C++编译器兼容。

2、环境设置

我们通过构建一条直的三车道道路来模拟一个动态的高速公路环境，其中包含三辆交通车和一辆自车位于中央车道。整个场景设计为跟随自车（显示为绿色），而周围的交通车辆（显示为蓝色）通过加速和转向输入进行真实移动。坐标系以自车为中心，所有传感器数据和跟踪相对于其位置。此实现定义在highway.h中并通过main.cpp运行。

在这里，每辆交通车都配备了自己的UKF实例。这些滤波器在每个时间步独立更新，使我们能够在车辆行驶在路上时实时跟踪每辆车的位置和速度。

为了可视化传感器数据，红色球体表示LiDAR检测（在2D x/y平面上），而紫色线条表示雷达测量，包括检测对象的角度和速度大小。由于我们只在两个维度（x和y）上进行跟踪，该项目中忽略了z轴。您还可以查看tools.cpp以更改这些测量方式，例如LiDAR标记可以是边界框的(x, y)中心，通过扫描PCD环境并执行聚类。

3、自定义模拟

highway.h文件提供了几个您可以调整的参数，以控制模拟的行为——这对调试或测试不同的配置非常有用。以下是关键参数的概述：

  • trackCars列表确定高速公路上哪些车辆将使用UKF（无迹卡尔曼滤波器）进行跟踪。向量中的每个值代表一辆交通车（将值设为false意味着该特定车辆不会被UKF跟踪）：
std::vector<bool> trackCars = {true, true, true};
  • visualize_lidar、visualize_radar和visualize_pcd切换控制显示哪种类型的传感器数据。随意启用或禁用每个选项，以便在模拟过程中专注于特定的传感器类型。
bool visualize_lidar = true;   // 显示LiDAR测量（红色球体）  
bool visualize_radar = true;   // 显示雷达测量（紫色线条）  
bool visualize_pcd = false;    // 显示原始LiDAR点云数据
  • projectedTime和projectedSteps控制使用UKF预测车辆估计未来路径的预测范围。如果您想根据当前状态看到车辆的去向，请增加这些值。
double projectedTime = 0;  // 要投影的总时间（以秒为单位）  
int projectedSteps = 0;    // 要预测的未来步骤数

4、UKF实现

与使用数学近似（如线性化）不同，UKF方法通过精心选择的“sigma点”采样不确定性空间，将每个样本通过精确的非线性运动模型运行，并统计地组合结果以获得预测的均值和协方差。这比传统的卡尔曼滤波器为非线性系统提供了更准确的预测。

在以下步骤中，我们将学习ProcessMeasurement函数接收新的传感器数据（LIDAR或RADAR测量），并启动Prediction()函数。Prediction使用运动模型预测当前时间的对象应在哪里，且没有涉及任何传感器数据——纯粹基于物理/运动的预测。最后，它调用适当的更新函数（UpdateLidar或UpdateRadar）。这是完整的测量处理周期。

5、卡尔曼滤波器完整流程

将UKF想象成预测一辆车将要去哪里：
  1. 我们有一个“最佳猜测”当前汽车的位置（状态）
  2. 我们对这个猜测不确定（协方差）
  3. 我们预测它下一步会去哪里（预测）
  4. 我们得到传感器测量并更新我们的猜测（更新）

同样，ProcessMeasurement()函数捕获两个主要步骤：Prediction(dt)（使用物理预测“对象现在应该在哪里？”）然后UpdateLidar/Radar()（“用传感器数据纠正预测”）。这个函数的实现与标准卡尔曼滤波器模式非常吻合：

  1. 预测：使用运动模型预测状态
  2. 更新：使用传感器测量来纠正预测

6、UKF参数初始化

为了构建我们的无迹卡尔曼滤波器（UKF），我们首先设置几个核心变量，这些变量定义了滤波器如何跟踪和预测物体运动。

  • 状态向量 (x_)：我们对5个事物的“最佳猜测”：[x_pos, y_pos, speed, heading, turn_rate]
  • 协方差 (P_)：我们对每个猜测的不确定性
  • Sigma点：围绕我们的猜测的15个样本点，以处理曲线运动
  • 权重：信任每个样本点的程度

因为UKF通过模拟多个可能的状态结果（称为sigma点）来工作，我们使用一个增广状态向量，额外的维度用于考虑过程噪声（加速度和偏航加速度的不确定性）。这个更大向量的大小存储在n_aug_中，并用于创建预测的sigma点矩阵X_sig_pred_，这代表了运动模型的不同可能结果。

每个sigma点都被加权以贡献最终预测，这些权重存储在weights_向量中。这些权重确保从sigma点集中正确重建预测均值和协方差。

关键参数 解释：

  1. n_x_ = 5：我们跟踪移动物体的5个方面
  2. n_aug_ = 7：添加2个噪声项以处理预测不确定性
  3. lambda_：控制样本点的分散程度，这会影响滤波器对系统中非线性的敏感度
  4. std_a_：加速度可能变化的不可预测性——纵向加速度的标准偏差，表示意外的速度变化（如突然刹车或加速）
  5. std_yawdd_：转向可能变化的不可预测性——偏航加速度的标准偏差，捕捉意外的转向或方向变化

调优指南：

位置误差太大？→ 减少 std_a_, std_yawdd_速度误差太大？→ 增加 std_a_，或改进初始值 滤波器适应太慢？→ 增加过程噪声 滤波器太跳跃？→ 减少过程噪声

7、理解过程测量

UKF::ProcessMeasurement()函数负责管理滤波器如何处理来自LiDAR和雷达传感器的输入数据。其行为取决于它是在处理第一个测量还是后续的测量。

在第一个测量时，滤波器执行初始化。它设置状态向量x_，其中包括五个关键元素：x和y位置（px, py）、速度（v）、偏航角（yaw）和偏航率（yawd）。它还初始化协方差矩阵P_，这反映了每个状态组件的不确定性，并记录初始时间戳time_us_。

测量的来源决定了初始化的方式。对于LiDAR，它提供直接的笛卡尔坐标，我们使用原始(x, y)位置。对于雷达，它提供极坐标（范围、角度和范围率），我们在初始化状态之前将其转换为笛卡尔坐标。

再次强调——对于所有后续测量，滤波器进入两个主要阶段：

  1. 预测：此步骤使用运动模型（没有任何传感器输入）来预测对象在当前时间戳处应在哪里。时间差（delta_t）从当前和之前的时刻计算得出，并从微秒转换为秒。
  2. 更新：预测之后，滤波器结合实际的传感器测量来校正其预测。根据传感器类型，它调用UpdateLidar()或UpdateRadar()。

这个循环——预测，然后更新——随着每个新的传感器读数继续，随着时间的推移创建了对象运动的平滑且一致的估计。LiDAR提供准确的位置数据，而雷达即使没有视线也能提供速度和角度信息。结合不同模态的数据可以稳健地跟踪移动对象，特别是在嘈杂或非线性环境中。

8、预测步骤

UKF::Prediction()函数的主要功能是预测对象在下一步的时间位置，同时正确考虑不确定性和非线性运动。也就是说，给定当前状态估计和经过的时间（delta_t），预测对象的未来位置、速度、方向和转弯率。预测过程对于LiDAR和雷达测量都是相同的。

  • 创建一个增广均值向量 x_aug 和增广状态协方差矩阵 P_aug
  • 生成先前估计状态向量的sigma点矩阵 Xsig_aug
  • 预测当前状态向量的sigma点矩阵 Xsig_pred_
  • 使用权重和预测的sigma点预测状态均值 x_ 和协方差 P_

这些步骤是处理非线性运动（不同于线性卡尔曼滤波器）所必需的，例如物体转弯（曲线轨迹）。它不仅仅预测一个“最佳猜测”，而是通过跟踪不确定性随时间的增长来表明我们应该对预测有多大的不确定性。预测步骤还考虑了我们无法准确预测的随机加速度和扰动。

9、对LiDAR和雷达数据进行更新

更新LiDAR和雷达测量的步骤（在UKF::UpdateLidar()和UKF::UpdateRadar()中）是相似的，除了LiDAR点是笛卡尔坐标，而雷达点是极坐标。因此，它们在测量维度 n_z、矩阵的维度和变换方程上有所不同。一般来说，它们遵循相同的步骤来更新测量。

  • 根据传感器类型，将预测的sigma点 Xsig_pred_ 转换到测量空间 Zsig
  • 计算带噪声的均值状态 z_ 和协方差矩阵 S
  • 计算状态空间和测量空间之间的交叉相关矩阵 Tc
  • 计算卡尔曼增益 K
  • 更新状态向量 x_ 和协方差 P_

10、传感器融合评估：RMSE预测准确性

在目标跟踪应用中——尤其是在自动驾驶中——不仅要知道物体在哪里，还要知道它是如何移动的。这就是我们计算RMSE（均方根误差）的原因，针对四个关键状态变量：x、y、vx 和 vy。变量x和y代表物体在2D空间中的估计位置，而vx和vy分别代表沿x和y轴的速度分量。

模拟收集了上述算法输出的位置和速度值，并与真实数据进行比较。在模拟器运行超过1秒后，px、py、vx和vy的RMSE分别为小于或等于0.30、0.16、0.95和0.70。如果RMSE值超过阈值，模拟器也会显示出来。

项目输出

上面的截图来自我们的模拟帧。自车是绿色的，其他交通车是蓝色的。汽车上方的红色球体代表(x,y) LiDAR检测，紫色线条显示雷达测量，沿着检测角度的速度大小。汽车上方的绿色球体代表汽车在不久的将来可能移动的预测路径。

测量位置（x, y）的准确性确保跟踪系统知道每个物体在道路上的位置——这对于车道保持、障碍物避让和碰撞检测等任务至关重要。另一方面，评估速度准确性（vx, vy）告诉我们物体移动的速度和方向。即使物体的当前位置估计准确，速度估计不佳也可能导致错误的未来预测。例如，如果系统错误地认为一辆车正在减速，而实际上它正在加速，它可能会在路径规划或制动时做出不安全的决定。

通过计算所有四个组件的RMSE，我们可以全面了解滤波器的性能。RMSE将偏差和方差合并成一个分数，提供估计状态与真实数据之间平均偏差的清晰视图。这使我们能够评估无迹卡尔曼滤波器的整体性能，并确保系统不仅在当下精确，而且在预测未来状态时也可靠。

11、结束语

这个项目展示了无迹卡尔曼滤波器在理论传感器融合概念和实际自动驾驶应用之间的强大作用。通过成功融合LiDAR和雷达数据以跟踪模拟高速公路上的多辆车，我们展示了UKF的sigma点方法如何优雅地处理现实驾驶场景中的非线性动力学。达到RMSE值在我们的目标阈值内——分别为0.30、0.16、0.95和0.70的位置和速度组件——验证了准确的多目标跟踪不仅是可能的，而且足够可靠，可用于安全关键的应用。

我们传感器模态的互补性证明了系统成功的关键。虽然LiDAR通过其高分辨率3D点云提供了精确的空间意识，但雷达提供了LiDAR无法匹敌的直接速度测量和环境弹性。这种传感器融合方法突显了自动驾驶中的一个基本原则：没有单一的传感器技术足以实现安全导航。UKF框架使结合这些多样数据源既数学严谨又计算高效，创建了一个即使在挑战性条件下也能良好运行的鲁棒感知系统。

也许最重要的是，这个实现超越了学术练习，展示了现实世界的应用价值。完整的动态高速公路模拟，包括变道、多车辆交互和真实的运动模式，表明UKF可以处理实际驾驶环境的复杂性。模块化架构允许轻松调整参数并扩展到不同的场景，使其成为生产自动驾驶系统的一个坚实基础。

随着我们接近完全自动驾驶的车辆，UKF展示了传感器融合如何将嘈杂的数据转化为清晰的见解。通过结合LiDAR和雷达的优势，UKF提供了一种精确而自信地解释复杂、嘈杂世界的方法。希望这篇讲解帮助你揭开UKF的工作原理及其重要性。有了这种更深入的理解，我希望我们都能朝着构建更安全、更可靠的自动驾驶系统迈出一小步。

原文链接：Building Multimodal Sensor Fusion for Real-Time Object Tracking

汇智网翻译整理，转载请标明出处