一、引言为什么“看到”不等于“看懂”在前三部分我们花费了大量篇幅解决“我在哪”自车定位的问题。这是自动驾驶的基础但绝不是终点。自动驾驶的终极目标是安全、高效地通过交通流。这就要求系统必须回答一个更难的问题“别人在干嘛接下来要去哪”仅仅检测出前方有一辆车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与TransformerGNN将车辆视为图中的节点它们之间的交互视为边。GNN能很好地建模多车交互。Transformer利用自注意力机制Self-Attention捕捉场景中任意两辆车之间的关系。这是目前SOTAState-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功能安全框架下设计“永不宕机”的滤波系统。
