自动驾驶系统滤波（4）：目标跟踪与预测滤波——从状态估计到意图理解
一、引言：为什么“看到”不等于“看懂”？
在前三部分，我们花费了大量篇幅解决“我在哪？”（自车定位）的问题。这是自动驾驶的基础，但绝不是终点。自动驾驶的终极目标是安全、高效地通过交通流。这就要求系统必须回答一个更难的问题：“别人在干嘛？接下来要去哪？”
仅仅检测出前方有一辆车（Detection）是远远不够的。如果这辆车正在以80km/h的速度靠近，而你正在变道，你就必须预测它是否会减速避让，还是会加速逼停你。这不再是简单的几何问题，而是涉及心理学、博弈论的意图理解（Intent Understanding）问题。
本部分将深入自动驾驶的感知与预测核心——目标跟踪（Object Tracking）与轨迹预测（Trajectory Prediction）。我们将看到，滤波算法如何从单纯的“位置平滑器”，演变为“行为预言家”。我们将解析交互多模型（IMM）、联合概率数据关联（JPDA）在量产车中的应用，并探讨深度学习如何重塑预测滤波的边界。
二、目标跟踪的架构：从检测到轨迹
在自动驾驶系统中，感知模块（Perception）通常分为两层：
检测层（Detection）：每一帧独立工作。摄像头和雷达告诉你“哪里有什么”。这是离散的、帧与帧之间没有关联的。
跟踪层（Tracking）：跨帧工作。滤波算法在这里登场，将离散的检测点连接成连续的轨迹（Track）。
2.1 跟踪的核心任务
数据关联（Data Association）：解决“谁是谁”的问题。当前帧的检测点A，是上一帧的跟踪目标1，还是新出现的目标？
状态估计（State Estimation）：解决“在哪”的问题。利用滤波算法（如EKF），平滑检测点的抖动，输出稳定的位置和速度。
生命周期管理（Lifecycle Management）：解决“生老病死”的问题。一个新检测何时确立为一个新目标？一个老目标何时被判定为消失？
三、数据关联：跟踪算法的“七寸”
如果数据关联错了，后面的滤波做得再好也是徒劳。这是目标跟踪中最棘手的部分。
3.1 最近邻（Nearest Neighbor, NN）
最简单的算法。计算检测点与所有跟踪目标的距离（通常是马氏距离 Mahalanobis Distance），选最近的配对。
优点：快。
缺点：密集场景下完全失效。当十辆车挤在一起时，NN会乱点鸳鸯谱。
3.2 全局最近邻（Global Nearest Neighbor, GNN）
不仅看一对一的距离，还看全局总成本最低。这是一个二分图匹配问题，通常用匈牙利算法（Hungarian Algorithm）求解。
应用：这是目前量产车（如特斯拉、蔚来）视觉感知的标准配置。
3.3 JPDA（联合概率数据关联）
如前所述，当遮挡发生时，检测点可能来自多个目标。JPDA计算检测点属于每个目标的概率，然后进行加权更新。
实战：在拥堵路况下，前车被公交车遮挡一半，JPDA能利用雷达和摄像头的信息，维持对前车的跟踪，而不会误以为前车消失了。
四、交互多模型（IMM）：应对复杂的运动模式
一辆车可能直行，可能转弯，可能停车。如果我们只用一种运动模型（如匀速模型CV）去跟踪，一旦车辆转弯，滤波器就会滞后，导致预测的轨迹向外甩。
4.1 IMM的工作原理
IMM并行运行多个滤波器（模型），每个滤波器负责一种运动模式：
模型1：匀速直线运动（CV） —— 用于高速公路巡航。
模型2：匀加速运动（CA） —— 用于加减速。
模型3：协同转弯（CT） —— 用于变道和弯道。
交互（Interaction）是IMM的精髓：
输入交互：基于上一时刻各模型的概率，混合状态估计，作为当前时刻的输入。
模型滤波：每个滤波器独立预测和更新。
模型概率更新：根据当前观测的匹配程度，更新每个模型的权重（比如转弯模型现在权重很高）。
输出融合：加权平均所有滤波器的输出。
4.2 案例：AEB（自动紧急制动）中的IMM
在AEB触发前，系统必须准确预测前车是否会刹停。
如果只用CV模型，预测前车会一直撞上来，导致过早刹车（幽灵刹车）。
使用IMM，如果检测到前车刹车灯亮起（语义信息），CA模型的权重会增加，预测出前车会减速，从而更平滑地进行制动介入。
五、轨迹预测：从滤波到博弈
跟踪解决的是“现在在哪”，预测解决的是“未来在哪”。这是自动驾驶算法皇冠上的明珠。
5.1 基于物理学的预测（Physics-based）
这是滤波的直接延伸。
恒定速度/加速度模型：假设物体会继续当前状态。适用于高速公路，不适用于城市路口。
恒定转向速率和速度（CTRV）：假设车辆会以恒定的角速度转弯。这是目前最主流的预测模型，计算量小，效果尚可。
5.2 基于地图的预测（Map-based）
车辆受道路拓扑约束，不能开上人行道。
车道跟随（Lane Following）：将预测轨迹投影到高精地图上最近的车道线上。
路径规划启发式：如果车辆正在接近路口，且转向灯亮起，预测其会沿着对应车道行驶。
5.3 基于交互的预测（Interaction-aware）
这是最难的部分。路上的车不是孤立的，它们在互相博弈。
场景：你试图并入右侧车道，右侧车辆可能会减速让你，也可能会加速不让。
算法：使用社会力模型（Social Force Model）或博弈论（Game Theory）。
滤波视角：将周围车辆的状态也纳入你的状态向量，使用联合状态估计。但这会导致维度爆炸。
六、深度学习与滤波的融合：数据驱动的预测
传统滤波方法（CV、CTRV）无法理解人类驾驶的随机性。深度学习通过海量驾驶数据，学习人类的驾驶习惯。
6.1 RNN/LSTM 预测
使用循环神经网络处理历史轨迹序列，输出未来轨迹。
优点：能捕捉长期的时序依赖，比如前车刹车灯亮了之后通常会有什么动作。
缺点：物理合理性差，有时会预测出违反物理定律的轨迹（如瞬间变向）。
6.2 图神经网络（GNN）与Transformer
GNN：将车辆视为图中的节点，它们之间的交互视为边。GNN能很好地建模多车交互。
Transformer：利用自注意力机制（Self-Attention），捕捉场景中任意两辆车之间的关系。这是目前SOTA（State-of-the-Art）预测算法的标配。
6.3 混合架构：TNT/DETR
工业界的最新趋势是混合架构：
检测与跟踪：依然使用EKF/JPDA，保证稳定和实时。
预测头（Prediction Head）：在跟踪输出的稳定轨迹上，挂一个轻量级的深度学习网络（如MLP或小型Transformer），输出多条可能的未来轨迹及其概率。
七、占用栅格与势场滤波：不依赖检测的跟踪
在某些极端场景（如不规则障碍物、翻倒的卡车），我们无法用 bounding box（边界框）来描述目标。
7.1 占用栅格滤波（Occupancy Grid Filtering）
将车辆周围的空间划分为一个个小格子（Grid）。
算法：使用贝叶斯占用滤波（Bayesian Occupancy Filter）。每个格子维护一个概率值（被占用的概率）。
优势：不需要识别物体是什么，只需要知道那里“有东西”。这对于通用障碍物检测至关重要。
7.2 势场（Potential Field）与代价地图（Costmap）
预测不仅要知道别人去哪，还要知道哪里不能去。
滤波应用：使用高斯过程（Gaussian Process）对周围车辆的预测轨迹进行平滑，生成一个动态的势场。车辆离其他车越近，势场值越高（排斥力越大）。
用途：直接输入给规划模块，作为避障的约束条件。
八、量产落地的挑战：鬼影与误检
在实验室里，预测算法指标（ADE/FDE）可以刷得很高。但在实车上，有两个恶魔必须解决：
8.1 鬼影（Ghost Objects）
由于雷达多径效应，系统会“看到”路边护栏的影子变成了一辆车。
滤波对策：使用持久性检查（Persistence Check）。只有连续3-5帧都检测到的目标才被确认为真。但这会带来延迟。
8.2 漏检（Missed Detection）
前车被大货车完全遮挡，系统丢失了目标。
滤波对策：使用交互多模型（IMM）进行预测外推。即使没有观测，也要基于上一时刻的状态继续预测其轨迹，直到确认丢失。
九、总结与展望
目标跟踪与预测滤波，是自动驾驶从“感知”迈向“认知”的桥梁。我们已经看到：
关联算法（GNN/Hungarian）解决了“谁是谁”。
交互多模型（IMM）解决了“怎么动”。
深度学习解决了“为什么动”。
然而，即使我们能完美预测旁边车辆的轨迹，还有一个终极问题没有解决：如果传感器本身给出的数据就是错的怎么办？ 摄像头被强光致盲、激光雷达在暴雨中失效、IMU在高温下漂移。在这种情况下，任何基于错误输入的滤波算法都会输出错误的结果。
这就需要引入鲁棒滤波（Robust Filtering）和故障诊断（Fault Diagnosis）机制。我们需要算法不仅能估计状态，还能诊断传感器是否生病了，并在生病时自动切换到安全模式。
在下一部分中，我们将深入探讨鲁棒滤波与故障诊断。我们将解析M估计（M-Estimation）、Huber滤波、以及如何在ISO 26262功能安全框架下设计“永不宕机”的滤波系统。