1. 从“概率性”三个字说起为什么确定性跟踪在自动驾驶里不够用做自动驾驶感知的人绕不开多目标跟踪MOT这个环节。检测模型在每一帧告诉你“这里有辆车、那里有个行人”但下一帧它可能漏检、可能框飘了、可能把两个靠近的目标合并成一个。跟踪模块的任务就是把这些离散的、带噪声的观测串成一条条有身份、有连续轨迹的目标流。传统做法大多走的是“确定性”路线卡尔曼滤波配匈牙利算法算出一个最优关联然后更新状态。这套东西在结构化场景、目标稀疏、遮挡不严重的时候跑得挺好但一旦进入城市复杂路口、密集车流、频繁遮挡的场景问题就暴露了。核心矛盾在于数据关联本身就是一个不确定的决策过程。你用一个检测框和一条已有轨迹做匹配本质上是在问“这个观测属于这条轨迹的概率有多大”。确定性方法把这个概率问题硬生生压成了一个“是或否”的二值判断中间的不确定性信息全丢了。一旦关联错了误差会沿着轨迹传播后面几帧可能连续跟错甚至发生身份跳变ID Switch。而概率性跟踪的思路是不急着做硬判决而是维护每个目标状态的概率分布以及观测与轨迹之间关联的概率分布让不确定性在系统里显式地表达和传递。我最早接触概率性跟踪是在做激光雷达点云序列处理的时候。当时用传统的IoU匹配加卡尔曼在高速场景还行但一到城区路口行人、电动车、车辆混在一起ID Switch 频繁到没法看。后来换成基于概率假设密度PHD滤波的思路把多目标状态建模成一个随机有限集RFS关联的不确定性用概率权重来表达效果立竿见影。这也是为什么现在越来越多的自动驾驶跟踪方案开始强调“概率性”这三个字——它不是噱头而是对真实感知不确定性的尊重。那“3D多模态”又意味着什么纯视觉做3D跟踪深度信息靠估计误差大纯激光雷达做跟踪纹理和语义信息弱对远距离小目标不友好。多模态融合就是把激光雷达点云、相机图像、毫米波雷达甚至IMU的数据在跟踪层面结合起来用不同传感器的互补性来对抗单一传感器的退化。而“多目标”则意味着系统要同时维护几十甚至上百条轨迹每条轨迹都有自己的生命周期、置信度和运动模型。这三者叠加就是标题里“概率性3D多模态多目标跟踪”的完整含义。这篇文章我会从实际工程落地的角度把这条技术路线拆开讲清楚概率性建模到底怎么建、多模态融合在跟踪层怎么融、3D空间里的状态估计有哪些坑、数据关联的概率框架怎么设计以及我在实测中踩过的那些坑。适合有一定感知基础、正在做自动驾驶跟踪模块或者准备复现相关论文的工程师参考。2. 概率性跟踪的数学骨架从随机有限集到概率假设密度2.1 为什么用随机有限集而不是固定维度向量传统多目标跟踪把状态写成一个大向量比如[x1, v1, x2, v2, ...]维度固定。但真实场景里目标数量是变化的车开走了、新人出现了、目标被遮挡后消失又重现。固定维度向量处理不了这种“集合”性质的变化。随机有限集Random Finite Set, RFS的核心思想是把多目标状态本身建模成一个随机集合集合的元素个数和每个元素的值都是随机的。这样一来目标的出现、消失、分裂、合并都可以用集合的运算来统一描述。具体到工程实现最常用的是概率假设密度Probability Hypothesis Density, PHD滤波。PHD 是 RFS 的一阶矩它在一个空间点上对目标密度的积分就是该区域目标数量的期望。你不需要显式地给每个目标分配 IDPHD 峰值所在位置就对应目标最可能的状态。这对于处理目标数量未知、检测漏报和虚警并存的场景非常自然。我自己的理解是PHD 滤波像是把“目标在哪里”这个问题从“点估计”变成了“密度估计”。传统方法告诉你“目标在 (x, y)”PHD 告诉你“目标在 (x, y) 附近的概率密度是 0.8在 (x, y) 附近的密度是 0.3”。后者显然携带了更多信息尤其是在目标靠近、遮挡、检测不稳定的时候。2.2 PHD 滤波的预测与更新一个可落地的推导PHD 滤波分两步预测和更新。预测步把上一时刻的 PHD 通过运动模型传播到当前时刻同时考虑新生目标和衍生目标。更新步用当前帧的观测来修正 PHD。预测公式简化形式D_{k|k-1}(x) ∫ p_s(x) * f(x|x) * D_{k-1|k-1}(x) dx γ_k(x)其中p_s(x)是目标存活概率f(x|x)是状态转移密度γ_k(x)是新生目标 PHD。更新公式D_{k|k}(x) (1 - p_d(x)) * D_{k|k-1}(x) Σ_{z∈Z_k} [ p_d(x) * g(z|x) * D_{k|k-1}(x) ] / [ κ(z) ∫ p_d(x) * g(z|x) * D_{k|k-1}(x) dx ]这里p_d(x)是检测概率g(z|x)是观测似然κ(z)是杂波密度。这个公式看起来复杂但工程上通常用高斯混合GM-PHD或者序贯蒙特卡洛SMC-PHD来近似。GM-PHD 把 PHD 表示成一组高斯分量的加权和每个分量有均值、协方差和权重。预测和更新就变成对高斯分量做解析运算计算效率高适合实时系统。SMC-PHD 用粒子来近似精度更高但计算量大适合离线分析或者算力充裕的平台。我在实际项目里用的是 GM-PHD 的变体因为激光雷达点云经过聚类后每个簇的质心和协方差天然就是高斯分量。把检测框的均值和协方差直接映射成高斯分量预测用匀速模型CV或者匀加速模型CA更新用观测似然做权重修正。整个流程跑在 ROS 节点里单帧处理时间控制在 30ms 以内满足 10Hz 激光雷达的实时性要求。2.3 高斯分量的剪枝与合并工程上必须做的“瘦身”GM-PHD 有个绕不开的问题高斯分量会随着时间不断增殖。每一帧的预测和更新都会产生新的分量如果不加控制几百帧之后分量数量会爆炸。所以必须做剪枝pruning和合并merging。剪枝就是丢掉权重低于阈值的高斯分量。权重代表该分量对应真实目标的概率低于阈值的基本可以认为是杂波或者已经消失的目标。合并是把距离很近、协方差重叠度高的分量合并成一个避免同一个目标被多个分量重复表示。这里有个经验参数剪枝阈值一般设在1e-5到1e-3之间合并阈值用马氏距离或者欧氏距离通常取 1 到 4 米。阈值太小分量删不干净计算量下不来阈值太大真实目标可能被误删导致轨迹断裂。我一般会先用仿真数据扫一遍参数找到漏检率和计算量的平衡点再在实车上微调。还有一个细节新生目标 PHD 的设计。新生目标不能随便撒否则虚警会很多。常见做法是用当前帧的检测结果来初始化新生分量但只对那些没有被现有分量“解释”的检测才初始化。判断“是否被解释”可以用检测与现有分量的马氏距离超过一定阈值就认为是新生目标。这个策略在城区场景里很有效能显著降低虚警轨迹的数量。3. 多模态融合在跟踪层的落地方式不是简单拼接3.1 激光雷达与相机的互补性到底体现在哪激光雷达点云的优势是深度精度高、不受光照影响、能直接给出 3D 位置和尺寸。但它的弱点也很明显远距离点稀疏小目标比如远处的行人可能只有几个点聚类都聚不出来对纹理和颜色不敏感分类能力弱。相机的优势是分辨率高、纹理丰富、语义信息强但深度估计依赖标定和模型误差随距离增长。在跟踪层融合不是把两个传感器的检测框简单拼在一起而是要让它们在同一套概率框架下互相修正。具体来说激光雷达提供高精度的 3D 位置和速度观测相机提供类别置信度和外观特征。当激光雷达漏检时相机的 2D 检测可以通过标定关系投影到 3D 空间生成一个“弱观测”来维持轨迹当相机分类不确定时激光雷达的几何特征可以帮助区分车辆和行人。我做过一个对比实验纯激光雷达跟踪在夜间和逆光场景下表现稳定但在远距离小目标上漏检严重纯视觉跟踪在白天纹理丰富时 ID Switch 少但一到夜间或者强光下就崩。融合之后两个场景的指标都有提升尤其是 ID Switch 降低了约 40%。这个提升不是来自某个传感器变强了而是来自不确定性在融合框架里被正确表达了。3.2 观测似然的建模不同传感器用不同的概率分布在 PHD 框架里每个传感器对应一个观测似然函数g(z|x)。激光雷达的观测通常是 3D 包围盒的中心点、尺寸和朝向可以用高斯分布建模均值和协方差从点云聚类结果里估计。相机的观测是 2D 框投影到 3D 空间后变成一个锥形区域可以用均匀分布或者高斯分布来近似。关键问题是不同传感器的观测似然怎么组合。最直接的做法是假设传感器独立把各自的似然相乘。但实际中激光雷达和相机可能同时漏检同一个目标或者同时被同一个遮挡物影响独立性假设不成立。更稳妥的做法是用协方差交叉Covariance Intersection或者概率融合规则在不知道相关性的情况下给出保守的融合结果。我在工程里用的是一种加权融合策略激光雷达观测的权重设为 0.7相机观测的权重设为 0.3权重根据当前场景的光照条件和目标距离动态调整。白天近距离时相机权重可以提到 0.4夜间或者远距离时降到 0.1。这个权重不是拍脑袋定的而是用标注数据做最大似然估计得到的。具体做法是在训练集上对每个传感器单独跑跟踪统计其在不同距离和光照下的检测概率和定位误差然后反推出似然函数的参数。3.3 时间同步与空间标定融合的地基不能歪多模态融合最容易被忽视但最致命的问题是时间同步和空间标定。激光雷达和相机的帧率不同曝光时刻不同如果时间戳对齐误差超过 20ms在高速场景下目标位移可能超过 0.5 米融合出来的位置就是错的。空间标定误差哪怕只有 0.5 度在 50 米外投影误差就超过 0.4 米足以让关联失败。我的做法是硬件上尽量用同一时钟源触发传感器软件上用插值对齐时间戳。空间标定用棋盘格加激光雷达靶标联合标定标定完必须做验证在远处放一个已知尺寸的标定物看投影误差是否在允许范围内。每次车辆震动或者传感器拆装后都要重新标定不能偷懒。还有一个坑激光雷达和相机的坐标系定义可能不同。有的激光雷达是右手系相机是左手系转换的时候容易搞错符号。我建议在代码里统一用一套坐标系比如车辆后轴中心所有传感器数据先转到这套坐标系再做融合避免混乱。4. 3D 状态估计与数据关联的概率化设计4.1 3D 运动模型的选择CV、CA 还是 CT3D 空间里的目标运动比 2D 复杂得多。车辆有横摆运动行人可能突然变向这些都不是简单的匀速模型能描述的。常用的运动模型有三种匀速CV、匀加速CA和匀速转弯CT。CV 模型最简单状态向量是[x, y, z, vx, vy, vz]转移矩阵是线性的。它在高速直线行驶时很准但一到转弯就滞后。CA 模型加了加速度项状态变成 9 维能捕捉加减速但对转弯还是不够。CT 模型引入横摆角速度能描述转弯但非线性需要用扩展卡尔曼滤波EKF或者无迹卡尔曼滤波UKF来处理。我的经验是不要用一个模型打天下。城区场景里车辆大部分时间在直行或者缓慢转弯CV 加一个过程噪声就够用行人运动随机性强用 CA 或者交互式多模型IMM更合适。IMM 的做法是同时跑多个模型根据观测似然动态调整每个模型的权重。我在行人跟踪上用过 IMM-CV-CA 组合相比单一 CV 模型位置误差降低了约 25%。过程噪声的设置也很关键。噪声太小滤波器对机动反应慢噪声太大轨迹抖动严重。我一般用自适应噪声根据目标当前的速度和转向角速度动态调整过程噪声协方差。速度高、转向大时噪声调大反之调小。4.2 数据关联的概率化从匈牙利算法到概率数据关联传统跟踪用匈牙利算法做关联本质是找一个使总代价最小的二分匹配。代价通常是检测框和预测框之间的 IoU 或者马氏距离。这个方法在目标稀疏时没问题但在密集场景下一个检测可能同时靠近多条轨迹硬匹配容易出错。概率数据关联PDA的思路是不急着做硬匹配而是计算每个检测属于每条轨迹的概率然后用这些概率加权更新轨迹状态。对于单目标PDA 很有效对于多目标联合概率数据关联JPDA同时考虑所有检测和所有轨迹的联合概率但计算量随目标数指数增长实时性差。在 PHD 框架里关联的概率化是天然内置的。更新步里的权重计算本质上就是在做软关联每个观测对每个高斯分量的贡献由似然决定不需要显式的匹配矩阵。这也是我倾向于用 PHD 而不是“检测匈牙利”的原因之一。如果非要用匹配框架我建议用概率数据关联的近似版本先计算检测-轨迹的关联概率矩阵然后用贪心或者匈牙利算法在概率矩阵上做匹配但保留概率信息用于后续更新。这样比硬匹配鲁棒计算量又比 JPDA 小。4.3 轨迹管理新生、确认、删除的逻辑多目标跟踪系统必须有一套轨迹生命周期管理机制。新生轨迹来自未被解释的检测确认轨迹需要连续多帧被关联上删除轨迹则是因为长时间没有观测更新。我的参数设置供参考新生轨迹的初始权重设为 0.3连续 3 帧关联上后权重升到 0.8 并标记为确认轨迹连续 5 帧没有关联则权重衰减低于 0.1 时删除。这些数字不是固定的要根据传感器帧率和场景动态调整。激光雷达 10Hz 时3 帧确认对应 0.3 秒行人突然出现时可能来不及确认相机 30Hz 时3 帧只有 0.1 秒又太敏感。我一般用时间而不是帧数来定义确认时间 0.3 秒删除时间 0.5 秒这样不同帧率的传感器可以统一。还有一个容易被忽视的点轨迹的“休眠”与“唤醒”。目标被遮挡后消失不应该立刻删除而是进入休眠状态保留其运动状态和外观特征。当类似观测再次出现时优先与休眠轨迹关联避免 ID Switch。这个机制在城区遮挡场景里非常关键我实测能减少约 30% 的 ID Switch。5. 实测中的坑与调参经验从仿真到实车5.1 仿真数据很美好实车数据教你做人仿真环境里检测框干净、时间戳完美、标定无误差PHD 滤波跑出来的轨迹平滑得像教科书。但一上实车问题全来了点云聚类不稳定有时一个目标裂成两个簇有时两个目标粘成一个簇相机检测在逆光时漏检在夜间噪声大时间同步有抖动标定有残差。我踩过最大的坑是点云聚类参数固定。仿真里目标距离变化不大聚类半径设 0.5 米就行。实车场景里近处目标点密0.5 米可能把一个目标切成几块远处目标点稀0.5 米可能把相邻目标合并。后来改成自适应聚类根据距离动态调整聚类半径近处用小半径远处用大半径。这个改动让聚类准确率提升了约 20%。另一个坑是相机检测的置信度阈值。仿真里检测置信度都很高阈值设 0.5 没问题。实车里夜间检测置信度普遍偏低阈值 0.5 会漏掉大量真实目标。我后来把阈值降到 0.3同时用激光雷达的几何信息做二次验证虚警没有明显增加漏检减少了很多。5.2 参数调优的顺序先关联再运动最后融合调参不能乱调要有顺序。我的习惯是先调数据关联的参数关联阈值、杂波密度再调运动模型的参数过程噪声、观测噪声最后调融合权重。因为关联错了后面运动模型再准也没用运动模型不准融合权重再优化也白搭。关联阈值用马氏距离一般取 3 到 5。太小真实关联被拒太大错误关联增多。我通常从 4 开始看漏检率和 ID Switch 的曲线找拐点。杂波密度κ(z)根据场景调整高速场景杂波少设小一点城区场景杂波多设大一点。这个参数对虚警轨迹数量影响很大需要仔细调。运动模型的过程噪声用Q矩阵表示。CV 模型下Q通常是对角阵位置项小速度项大。我一般设位置噪声 0.1 米速度噪声 1 米/秒然后根据实测轨迹的平滑度和滞后程度微调。观测噪声R从传感器标定报告里拿激光雷达位置噪声约 0.05 米相机投影后的位置噪声约 0.3 米。5.3 评估指标不要只看 MOTA跟踪评估常用 MOTA、MOTP、ID Switch 等指标。MOTA 综合了漏检、虚警和 ID Switch但它是全局指标掩盖了很多细节。我建议同时看分距离段的指标近处0-30 米、中距30-60 米、远处60-100 米分别统计。很多时候整体 MOTA 不错但远处 ID Switch 很高这说明融合权重或者关联阈值在远距离需要单独优化。还有一个指标是轨迹碎片化程度Fragmentation。一条真实轨迹被跟踪系统切成多段每段都有不同 ID这就是碎片化。碎片化高说明轨迹管理有问题可能是删除太激进或者新生太保守。我一般用平均轨迹长度和轨迹数量的比值来衡量比值越低碎片化越严重。5.4 计算资源分配别让跟踪模块吃掉所有算力PHD 滤波的计算量主要在高斯分量的预测和更新上。分量数量多的时候单帧计算可能超过 100ms满足不了实时性。我的优化策略是限制高斯分量的最大数量比如 200 个超过就按权重排序保留权重最高的。同时用矩阵运算替代循环把高斯分量的均值和协方差组织成批量矩阵用 BLAS 库加速。另一个优化点是只在感兴趣区域ROI内做跟踪。激光雷达全场景点云很大但真正需要跟踪的目标通常集中在车辆前方一定范围内。把 ROI 限制在 100 米 x 50 米的矩形内计算量能减少一半以上。ROI 之外的检测直接忽略或者用轻量级逻辑做简单跟踪。如果算力实在紧张可以考虑降频跟踪激光雷达 10Hz跟踪模块跑 5Hz中间帧用运动模型预测。这样计算量减半精度损失在可接受范围内。我在一个低算力平台上用过这个策略MOTA 只下降了约 3%但帧率翻倍。6. 多模态时序融合的进阶思路与代码复现要点6.1 时序融合不只是“上一帧和这一帧”多模态时序融合的常见做法是把当前帧的观测和上一帧的跟踪结果融合。但更有效的做法是维护一个时间窗口内的观测序列用序列信息来做关联和状态估计。比如过去 5 帧的激光雷达观测和相机观测一起参与当前帧的更新这样对瞬时漏检和噪声更鲁棒。具体实现上可以用一个滑动窗口缓存最近 N 帧的观测每帧的观测都带时间戳。更新时对窗口内的每个观测计算其对当前 PHD 的贡献按时间衰减加权。时间衰减因子一般取 0.8 到 0.95越近的观测权重越高。这个方法在我实测中能提升约 15% 的跟踪精度代价是内存和计算量增加。另一个进阶思路是用外观特征做长期关联。激光雷达和相机的几何观测在目标被遮挡时失效但外观特征比如车辆的颜色、纹理可以在遮挡期间保持。用孪生网络提取外观嵌入在轨迹休眠时保存嵌入向量唤醒时用嵌入相似度做关联。这个思路在行人跟踪上效果很好但需要额外的计算资源。6.2 代码复现GM-PHD 的核心实现片段如果你要复现 GM-PHD核心是三个函数预测、更新、剪枝合并。下面是我用 Python 写的一个简化版核心逻辑依赖 NumPy 和 SciPy。import numpy as np from scipy.linalg import block_diag class GaussianComponent: def __init__(self, mean, cov, weight): self.mean mean # 状态均值shape (state_dim,) self.cov cov # 状态协方差shape (state_dim, state_dim) self.weight weight # 分量权重标量 def predict(components, F, Q, p_survival): 预测步对每个高斯分量做线性变换 predicted [] for comp in components: new_mean F comp.mean new_cov F comp.cov F.T Q new_weight p_survival * comp.weight predicted.append(GaussianComponent(new_mean, new_cov, new_weight)) return predicted def update(components, measurements, H, R, p_detect, clutter_intensity): 更新步用观测修正高斯分量 updated [] # 漏检部分权重乘以 (1 - p_detect) for comp in components: new_weight (1 - p_detect) * comp.weight if new_weight 1e-6: updated.append(GaussianComponent(comp.mean.copy(), comp.cov.copy(), new_weight)) # 检测部分对每个观测计算对每个分量的似然 for z in measurements: total_likelihood 0.0 temp_comps [] for comp in components: # 计算观测似然 innovation z - H comp.mean S H comp.cov H.T R K comp.cov H.T np.linalg.inv(S) new_mean comp.mean K innovation new_cov (np.eye(len(comp.mean)) - K H) comp.cov likelihood np.exp(-0.5 * innovation.T np.linalg.inv(S) innovation) / \ np.sqrt((2 * np.pi) ** len(z) * np.linalg.det(S)) weight p_detect * comp.weight * likelihood total_likelihood weight temp_comps.append(GaussianComponent(new_mean, new_cov, weight)) # 归一化权重 if total_likelihood 1e-6: for comp in temp_comps: comp.weight / (clutter_intensity total_likelihood) if comp.weight 1e-6: updated.append(comp) return updated def prune_and_merge(components, prune_threshold, merge_threshold): 剪枝与合并控制分量数量 # 剪枝 components [c for c in components if c.weight prune_threshold] # 按权重排序 components.sort(keylambda c: c.weight, reverseTrue) # 合并简单起见只合并距离很近的分量 merged [] used [False] * len(components) for i, comp_i in enumerate(components): if used[i]: continue total_weight comp_i.weight weighted_mean comp_i.mean * comp_i.weight for j in range(i 1, len(components)): if used[j]: continue comp_j components[j] dist np.linalg.norm(comp_i.mean - comp_j.mean) if dist merge_threshold: total_weight comp_j.weight weighted_mean comp_j.mean * comp_j.weight used[j] True if total_weight 0: new_mean weighted_mean / total_weight merged.append(GaussianComponent(new_mean, comp_i.cov.copy(), total_weight)) return merged这段代码是简化版实际工程里还需要处理状态维度、观测维度、矩阵维度匹配等细节。但核心逻辑就是这三步预测、更新、剪枝合并。你可以先用仿真数据跑通再逐步替换成真实传感器数据。6.3 多模态数据接入的工程细节多模态数据接入最大的问题是数据格式和坐标系不统一。激光雷达点云通常是sensor_msgs/PointCloud2相机图像是sensor_msgs/Image检测结果是自定义消息。我建议在跟踪模块入口处做一层适配把所有传感器数据统一转成内部表示包括时间戳、3D 位置、协方差、类别、置信度。这样跟踪核心逻辑不需要关心数据来源只处理统一格式。时间同步用 ROS 的message_filters做近似时间同步允许的时间偏差根据传感器帧率设定。激光雷达 10Hz相机 30Hz时间偏差容忍度设 30ms 左右。如果偏差超过阈值丢弃该帧或者只用单传感器数据。坐标系转换用 TF 树管理所有传感器坐标系到车辆坐标系的变换通过 TF 查询。注意 TF 查询有时间戳要用数据的时间戳而不是当前时间否则会有插值误差。7. 一些个人体会和后续可以折腾的方向概率性 3D 多模态多目标跟踪这个方向我做了两年多最大的体会是理论优雅和工程落地之间有一条很宽的鸿沟。PHD 滤波的数学推导很漂亮但真正跑在车上你需要花 80% 的时间处理数据质量问题点云聚类不稳定、标定漂移、时间戳抖动、检测置信度不可靠。剩下的 20% 时间才是调滤波参数。另一个体会是不要迷信单一指标。MOTA 高不代表系统好用ID Switch 少不代表轨迹连续。我见过 MOTA 很高但轨迹碎片化严重的系统实际用起来体验很差。评估的时候一定要多看几个维度最好把轨迹可视化出来肉眼看看跟得对不对。后续可以折腾的方向我觉得有几个一是端到端的概率跟踪用神经网络直接学习关联概率和状态分布替代手工设计的似然函数二是多传感器异步融合不同传感器帧率不同、延迟不同如何在不损失精度的情况下做异步更新三是跟踪与预测的联合优化跟踪输出的轨迹直接喂给预测模块预测的误差反馈回来修正跟踪参数。这些方向都有论文在做但工程落地还早有兴趣的可以提前布局。最后分享一个小技巧用仿真数据做回归测试。每次改完跟踪代码先在仿真数据集上跑一遍看指标有没有退化。仿真数据虽然不真实但可复现、可标注适合做快速验证。实车测试成本高不能每次改代码都上路。我一般仿真和实车按 7:3 的时间分配仿真做迭代实车做验证。