环境的感知和理解能力。在特斯拉、Cruise 和 NASA 等公司从事安全关键系统工作十年后，我亲眼见证了强大的感知能力对现实世界自动驾驶的重要性。

现代自动驾驶系统依赖于复杂的传感器组合——每种传感器都有其独特的优势和局限性——来构建连贯的环境图像。立体摄像头可以捕捉视觉深度和纹理。雷达可以穿透雨雾。激光雷达可以生成车辆周围环境的精确 3D 点云。这些传感器共同作用，共同促进感知、定位和决策。

让我们简单探讨一下每个传感器的作用。

摄像头、激光雷达和雷达性能对比图

雷达：弹性、速度感知传感

雷达通常嵌入汽车保险杠中，擅长利用多普勒效应测量直接速度——这是摄像头和激光雷达无法做到的。这使得雷达能够更快、更准确地捕捉物体运动，使其成为车辆追踪、盲点探测以及在恶劣天气条件下测绘的理想选择。

然而，雷达的低空间分辨率使其容易出现误报。高反射性的物体，例如路上的汽水罐，可能会被误认为是更大的危险物。

LiDAR：高分辨率3D测绘

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

但激光雷达也有局限性。它不能直接测量速度，更容易受到灰尘和天气的影响，而且其尺寸使其难以集成到现代车辆设计中。

为什么要进行传感器融合？

每个传感器都只能提供部分的世界视角。单独使用，不够全面。但当它们智能地融合在一起时，就能形成对驾驶环境完整而可靠的理解。

这就是无迹卡尔曼滤波器 (UKF) 的作用所在。

与假设状态和测量之间存在线性关系的标准卡尔曼滤波器不同，UKF 专门针对具有非线性动态的系统和具有多种传感器模式的测量模型（例如自动驾驶中的系统）而设计。

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

通过将 LiDAR 的空间精度与雷达的动态运动数据相结合，UKF 实现了更快的收敛和更准确的物体跟踪——这两者对于现实世界交通中的自动驾驶汽车至关重要。

现在让我们深入研究如何从头开始实现 UKF。为了使本教程通俗易懂且重点突出，我们将使用模拟的 LiDAR 和雷达测量来实现，以估计简单二维环境中物体的位置和速度。

请记住，现实世界的自主系统要复杂得多——通常集成摄像头视觉、先进的传感器融合流程，甚至视觉语言模型，以实现对世界的丰富语义理解。但从这个简化的案例入手，将为我们理解多模态感知的工作原理及其扩展方式奠定坚实的基础。

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

教程概述

本教程由三个关键部分组成。首先，我们将初始化所有必要的参数来设置我们的系统。接下来，我们将实现无迹卡尔曼滤波器，利用噪声激光雷达和雷达测量数据来估计高速公路上多辆汽车的状态，例如位置和速度。最后，我们将通过计算与地面真实数据的均方根误差 (RMSE) 来评估滤波器的准确性。本项目使用的所有代码都包含在以下部分中，并进行

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

环境设置

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

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

为了直观地展示传感器数据，红色球体表示激光雷达探测结果（在二维 x/y 平面），紫色线条表示雷达测量结果，包括探测到的物体的角度和速度大小。由于我们只在二维（x 和 y）上进行跟踪，因此本项目忽略了 z 轴。您还可以尝试tools.cpp更改这些测量结果的获取方式，例如，通过扫描 PCD 环境并执行聚类，可以将激光雷达标记设为边界框的 (x, y) 中心。

自定义模拟

该highway.h文件提供了几个可以调整的参数来控制模拟的行为——这对于调试或测试不同的配置尤其有用。以下是一些关键参数的细分：

• trackCars列表决定了高速公路上哪些车辆将被 UKF（无迹卡尔曼滤波器）跟踪。向量中的每个值代表一辆交通车辆（将值设置为 false 表示该车辆不会被 UKF 跟踪）：

<span style="color:rgba(0, 0, 0, 0.8)"><span style="background-color:#ffffff"><span style="background-color:#f9f9f9"><span style="color:#242424">std::vector< <span style="color:#5c2699">bool</span> > trackCars = { <span style="color:#aa0d91">true</span> , <span style="color:#aa0d91">true</span> , <span style="color:#aa0d91">true</span> };</span></span></span></span>

• visualize_lidar和开关控制可视化的传感器数据类型。您可以随意启用或禁用每个选项，以便在模拟过程中专注于特定的传感器类型visualize_radar。visualize_pcd

<span style="color:rgba(0, 0, 0, 0.8)"><span style="background-color:#ffffff"><span style="background-color:#f9f9f9"><span style="color:#242424"><span style="color:#5c2699">bool</span> visualize_lidar = <span style="color:#aa0d91">true</span> ;    <span style="color:#007400">// 显示 LiDAR 测量值（红色球体）</span>
<span style="color:#5c2699">bool</span> visualize_radar = <span style="color:#aa0d91">true</span> ;    <span style="color:#007400">// 显示雷达测量值（紫色线）</span>
<span style="color:#5c2699">bool</span> visualize_pcd = <span style="color:#aa0d91">false</span> ;     <span style="color:#007400">// 显示原始 LiDAR 点云数据</span></span></span></span></span>

• projectedTime并projectedSteps控制预测范围，以便使用 UKF 可视化车辆的未来路径估计。如果您想根据车辆的当前状态查看其行驶方向，请增加这些值。

<span style="color:rgba(0, 0, 0, 0.8)"><span style="background-color:#ffffff"><span style="background-color:#f9f9f9"><span style="color:#242424"><span style="color:#5c2699">double</span> projectedTime = <span style="color:#1c00cf">0</span> ;   <span style="color:#007400">// 预计的总时间（以秒为单位）</span>
<span style="color:#5c2699">int</span> projectedSteps = <span style="color:#1c00cf">0</span> ;     <span style="color:#007400">// 预测的未来步数</span></span></span></span></span>

UKF实现

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

在接下来的步骤中，我们将了解该ProcessMeasurement函数如何接收新的传感器数据（激光雷达或雷达测量）并启动该Prediction()函数。该函数Prediction使用运动模型来估计物体当前的位置，并且不涉及任何传感器数据——纯粹基于物理/运动的预测。最后，它调用适当的更新函数（UpdateLidar或UpdateRadar）。这是完整的测量处理周期。

卡尔曼滤波器全周期

把 UKF 想象成预测汽车的位置：

1. 我们对汽车现在的位置有一个“最佳猜测”（状态）
2. 我们不确定这个猜测（协方差）
3. 我们预测它下一步会去哪里（预测）
4. 我们获得传感器测量值并更新我们的猜测（更新）

类似地，该ProcessMeasurement()函数捕获两个主要步骤：（Prediction(dt)“利用物理原理判断物体现在应该在哪里？”）以及UpdateLidar/Radar()（“利用传感器数据进行正确预测”）。此函数实现与标准卡尔曼滤波器模式非常吻合：

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

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_
 

了解过程测量

该UKF::ProcessMeasurement()函数负责管理滤波器如何处理来自激光雷达和雷达传感器的输入数据。其行为会根据处理的是首次测量还是后续测量而变化。

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

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

再次强调，对于所有后续测量，滤波器都会经历两个主要阶段：

1. 预测：此步骤使用运动模型（无需任何传感器输入）来估计物体在当前时间戳的位置。时间差 (delta_t) 是根据当前时间戳和上一个时间戳计算出来的，并将其从微秒转换为秒。
2. 更新：预测完成后，滤波器会结合实际的传感器测量值来修正其预测。根据传感器类型，它会调用 UpdateLidar() 或 UpdateRadar()。

这个循环——预测，然后更新——会随着每个新的传感器读数而持续进行，从而对物体随时间的运动进行平滑一致的估计。激光雷达提供精确的位置数据，而雷达即使在没有视线的情况下也能提供速度和角度信息。结合来自不同模态的数据，可以稳健地跟踪移动物体，尤其是在嘈杂或非线性环境中。

预测步骤

函数的主要功能UKF::Prediction()是预测物体在下一个时间步的位置，同时适当考虑不确定性和非线性运动。也就是说，给定当前状态估计值和已用时间 ( delta_t)，预测物体的未来位置、速度、航向和转弯速率。激光雷达和雷达测量的预测过程相同。

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

这些步骤是处理非线性运动（与线性卡尔曼滤波器不同）所必需的，例如转动的物体（弯曲轨迹）。它不仅预测单个“最佳猜测”，还会通过追踪不确定性随时间的增长来判断我们对该预测的不确定性程度。预测步骤还考虑了我们无法精确预测的随机加速度和扰动。

激光雷达和雷达数据更新

UKF::UpdateLidar()更新激光雷达和雷达测量值（在和 中）的步骤UKF::UpdateRadar()相似，只是激光雷达点位于笛卡尔坐标系中，而雷达点位于极坐标系中。因此，它们在测量维度n_z、矩阵维度和变换方程上有所不同。通常，它们遵循相同的步骤来更新测量值。

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

传感器融合评估：RMSE 预测准确度

在物体追踪应用中，尤其是在自动驾驶中，不仅要知道物体的位置，还要知道它的移动方式。因此，我们计算了四个关键状态变量x、y、vx 和 vy 的 RMSE（均方根误差）。变量x 和 y 分别表示物体在二维空间中的估计位置，而 vx 和 vy 分别表示沿 x 轴和 y 轴的速度分量。

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

上面的截图取自我们的模拟帧。目标车辆为绿色，其他交通车辆为蓝色。车辆上方的红色球体表示激光(x,y)雷达探测，紫色线条表示雷达测量结果以及沿探测角度的速度大小。车辆上方的绿色球体表示车辆在不久的将来的预测路径。

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

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

结论

该项目展示了无迹卡尔曼滤波器 (UKF) 在弥合传感器融合理论与实际自动驾驶汽车应用之间的差距方面的强大功能。通过成功融合 LiDAR 和雷达数据来跟踪模拟高速公路上的多辆车辆，我们展示了 UKF 的 sigma 点方法如何优雅地处理实际驾驶场景中固有的非线性动态特性。RMSE 值在我们的目标阈值范围内实现——位置和速度分量分别为 0.30、0.16、0.95 和 0.70——这证明了精确的多目标跟踪不仅可行，而且对于安全关键型应用而言足够可靠。