做过几年激光雷达感知的朋友都清楚检测只是第一步真正让自动驾驶系统“看懂”动态世界的是检测之后的多目标跟踪与轨迹预测。这是《智能驾驶之激光雷达算法详解》系列的第4章重点讲点云多目标跟踪MOT的全链路数据关联怎么做、卡尔曼滤波怎么调、轨迹什么时候该生什么时候该灭、预测结果怎么喂给下游规划。适合正在做感知算法落地、被ID Switch搞到头秃、或者刚入行想知道跟踪模块内部到底在跑什么的同学。我会把工程上的坑和参数经验一并写出来能少走不少弯路。1. 为什么激光雷达感知离不开多目标跟踪1.1 单帧检测解决不了“连续理解”的问题激光雷达检测模型再强它输出的也只是当前帧的一组框位置、尺寸、朝向、类别、置信度。单帧检测有三个天然缺陷任何一个都会让下游模块很难受。第一个是帧间不稳定。同一个目标这一帧被检出来了下一帧因为遮挡、距离太远或者点云太稀疏直接漏检。检测框的尺寸和朝向也会抖动哪怕目标静止输出的中心点可能几厘米几厘米地跳。第二个是没有速度信息。两帧检测结果放在那里你只能知道目标“在这里”和“刚才在那里”但不知道它将要到哪里去。规划模块需要的是速度、加速度、朝向变化率这些单帧给不了。第三个是没有身份概念。你无法回答“左边第二辆车是不是刚才那辆”——这在并道、超车、路口博弈场景里是致命的。多目标跟踪要做的就是三件事把相邻帧属于同一个目标的检测框关联起来保持ID稳定用滤波估计出目标更平滑的运动状态包括速度和加速度维护轨迹的完整生命周期包括目标出现、短暂遮挡、最终消失。这套输出才是规划模块真正能用的“动态目标列表”。我见过不少入门同学觉得跟踪只是“检测的补丁”随便写个最近邻匹配就行。实际上量产级跟踪系统的难度不亚于检测模型它要处理漏检、误检、遮挡、目标交错、传感器噪声还要在嵌入式平台上跑得足够快。检测模型的competition都在刷点跟踪的competition刷的是你能不能接住真实世界的混乱。1.2 一个完整跟踪系统的模块划分一套基于激光雷达点云的跟踪系统输入是检测模块输出的3D框通常是[x, y, z, w, l, h, theta, class, score]输出是带ID、带速度、带预测轨迹的目标列表。中间通常串着四个模块数据关联把当前帧的检测框和已有轨迹做匹配确定“哪个框对哪条轨迹”。状态估计对匹配成功的轨迹用滤波算法更新目标的运动状态。轨迹管理维护轨迹的状态机决定何时新建轨迹、何时确认轨迹、何时删除轨迹。轨迹预测基于滤波输出的运动状态外推出未来几秒的目标位置供规划使用。这四步里数据关联是“最容易出错但最不引起注意”的环节状态估计是“看起来有公式但调参全靠玄学”的环节轨迹管理是“逻辑简单但边界条件极多”的环节轨迹预测在量产方案里则越来越被重视。与视觉跟踪不同点云跟踪有个很大的特点检测框是3D的天然包含目标的几何尺寸和朝向这给数据关联提供了更丰富的特征但也带来了新的问题——点云稀疏时目标远端的框会变形3D IOU会突然变得不可靠。点云的另一个特点是没有颜色和纹理目标外观特征几乎用不上所以激光雷达跟踪更依赖几何和运动特征。这也导致纯点云跟踪在目标交错时的ID Switch比视觉更严重这是后文要重点聊的问题。2. 数据关联点云多目标跟踪的第一步棋2.1 匈牙利算法与代价矩阵的构建数据关联的核心是把当前帧的N个检测框和已有的M条轨迹做配对使总代价最小。工程上最常用的算法是匈牙利算法解决的是“二分图最大权匹配”问题。听起来高级本质就是在做一个最优分配每条轨迹最多匹配一个检测框每个检测框最多匹配一条轨迹目标是让总关联代价最小。关联代价矩阵是关键。矩阵的行是轨迹列是检测框每个元素表示“这条轨迹和这个检测框”的匹配代价。代价怎么算点云场景常用三种3D中心点欧氏距离最简单直接用检测框中心坐标和轨迹预测中心坐标算距离。好处是快、直观坏处是完全没有考虑目标尺寸和朝向。马氏距离在欧氏距离基础上引入协方差矩阵把“不同方向上的不确定性”也考虑进去。目标运动方向上的不确定性通常比横向上大马氏距离能自动对这个方向惩罚更小。3D IOU或BEV IOU距离算两个3D框的交并比代价1-IOU。IOU匹配在目标密集、交错多的场景表现好因为它同时关联了几何形状。但点云稀疏时远处框的IOU会很不稳定。我实际用下来的经验是近处用BEV IOU远处用马氏距离。近处点云密、框准IOU区分度高远处框不稳定IOU经常算出来是0这时候距离匹配反而更可靠。还有一种做法是同时用几个距离加权比如“中心点欧氏距离 形状尺寸差异惩罚”效果也不错但参数又多了一堆没有那个必要。注意3D IOU计算时BEVBird‘s Eye View鸟瞰图下的2D IOU往往比完整3D IOU更实用。原因很简单点云框在高度方向的稳定性通常较差垂直方向误差会放大IOU的波动而BEV下的矩形框更能反映目标的水平占位。2.2 距离度量背后的“门控”逻辑比代价矩阵更重要的一步是门控Gating在计算关联代价之前先通过一个距离阈值把明显不可能的匹配过滤掉。这样做有两个好处一是减少计算量二是不让“远距离的误匹配”污染全局最优解。门控通常有两层。第一层是粗门控轨迹预测位置与检测框中心点的距离超过某个物理阈值比如10米直接丢弃不进入代价矩阵。这个阈值可以按类别和速度动态调整比如高速上的车可能一帧跑出去2米阈值就得比市区的行人大一些。第二层是细门控对于可能匹配的候选对计算马氏距离时设置卡方检验阈值超过阈值的丢弃。为什么门控很关键举个实际例子两个目标在十字路口交错轨迹A和轨迹B在空间上挨得很近检测框也经常重叠。如果没有门控匈牙利算法可能为了全局最优把轨迹A与检测框B_cur匹配、轨迹B与检测框A_cur匹配一瞬间两个ID就换了。加上门控后虽然两条轨迹在几何上都接近但运动方向差异很大马氏距离对“运动方向不一致”的惩罚很重大概率不会被误配。除了单帧匹配还有业界常用的级联匹配思路对不同寿命的轨迹分优先级。刚确认的轨迹、已经跟踪很久的高置信轨迹、以及正在“等待确认”的新轨迹匹配顺序不同。高置信度的老轨迹优先匹配这样能大幅减少ID Switch。这个技巧在检测质量不稳定的时候尤其有效——它能确保“跟了很久的目标”不会被新检测顶掉ID。2.3 匹配失败之后的处置策略一轮匈牙利匹配跑完总有一部分轨迹和检测框落空unmatched track 和 unmatched detection。这两类情况的处置策略直接决定了系统的鲁棒性。对unmatched track常见原因有三个目标被遮挡、目标已经驶出雷达视野、检测模型漏检。处置逻辑通常是“先不急着杀轨迹”而是给轨迹一个缓冲期用预测框暂时维持。缓冲期内连续没匹配上再把轨迹删除。这里有个细节缓冲期内轨迹速度一定要可信否则预测框会飘等目标重新出现时关联就断掉了。如果速度不可靠宁可把预测框的置信度降下来也别让它乱飘。对unmatched detection原因一般是目标刚进入视野、目标之前被完全遮挡现在重新出现、或者就是误检。处置逻辑基本只有两条一是为没有匹配上的检测框生成一条“候选轨迹”进入待确认状态二是若检测置信度很低直接当作误检丢弃。要不要为低置信度检测生成轨迹是个取舍题。我的习惯是“宁可多建不可漏建”但候选轨迹必须经过连续多帧确认才能转正误检轨迹会在确认阶段自然淘汰。3. 运动滤波与状态估计卡尔曼滤波在点云跟踪中的实战3.1 卡尔曼滤波解决了什么痛点检测框本身就是有噪声的直接拿检测框中心点当目标位置下游规划会非常难受。我见过一个项目车停在路边检测框中心点在3D空间里每帧跳5到10厘米规划模块算出的碰撞时间也跟着跳最后不得不靠滤波把输出抹平。卡尔曼滤波做的事情可以这么理解它维护目标状态的“预测值”和“置信度”每来一帧新检测就在“预测”和“测量”之间做一个加权平均权重取决于两者的不确定性。检测噪声大就多相信预测模型不准确就多相信测量。这个过程有严谨的数学推导但工程直觉更重要——它本质上是“用历史运动规律约束当前测量”的平滑器。在激光雷达跟踪里滤波的直接收益有三个输出位置更稳速度和加速度变得可用遮挡期间轨迹仍然有“预测位置”可以顶住。最后一点在工程上价值巨大——没有滤波目标短暂消失在点云里之后轨迹直接断掉再出现时只能新建一条IDID Switch会高到没法看。3.2 状态向量与运动模型怎么选卡尔曼滤波里的“运动模型”决定了状态怎么随时间演化。点云跟踪里最常用的是恒定速度CV模型和恒定转弯率与速度CTRV模型。CV模型的状态向量一般是[x, y, z, vx, vy, vz, w, l, h, theta]x、y、z是目标中心位置vx、vy、vz是三个方向的速度w、l、h是检测框的宽度、长度、高度theta是朝向角。CV模型的假设是速度恒定适用于短时间间隔内的近似——10Hz的雷达相邻帧间隔只有0.1秒大部分目标在这么短时间内的运动确实可以近似为匀速。这个模型的优势是状态向量简单、计算量小、数值稳定。CTRV模型则增加了一个偏航角速度omega[x, y, vx, vy, theta, omega]这个模型能描述转弯中的车辆对弯道场景更友好但状态更新方程是非线性的需要配合扩展卡尔曼滤波EKF或无损卡尔曼滤波UKF使用运算量会上升一个档次调参难度也更高。量产项目里很多团队的做法是“低速复杂场景用CTRV高速简单场景用CV”或者干脆CV加一个过程噪声补偿——让过程噪声协方差Q大一点给模型不确定性留出余地。我个人的习惯是在城市复杂路况下使用CTRV高速封闭路段用CV效果比单模型通用好不少。注意状态向量里要不要放目标尺寸w、l、h我的建议是放但给它们一个很小的过程噪声。尺寸本身不会突变但检测框的尺寸估计很不稳定如果过程噪声太小滤波器会把尺寸“锁死”在初始值上导致后续跟踪框和实际目标越来越不匹配。3.3 五步公式与代码骨架卡尔曼滤波在代码里其实就五步预测两步更新三步。预测阶段状态外推用运动模型把上一帧的后验状态推演到当前帧。协方差外推状态的不确定性也同步增长增长量由过程噪声决定。更新阶段 3. 计算卡尔曼增益权衡预测不确定性和测量噪声决定“信预测多还是信测量多”。 4. 更新状态用检测框的测量值修正预测状态。 5. 更新协方差得到更“紧”的后验不确定性。以CV模型和一个简单的四维状态x, y, vx, vy为例代码骨架长这样import numpy as np def predict(x, P, F, Q): x_pred F x P_pred F P F.T Q return x_pred, P_pred def update(x_pred, P_pred, z, H, R): S H P_pred H.T R K P_pred H.T np.linalg.inv(S) y z - H x_pred # 测量残差 x_new x_pred K y P_new (np.eye(len(x_pred)) - K H) P_pred return x_new, P_new # 状态: [x, y, vx, vy] F np.array([[1, 0, 0.1, 0], [0, 1, 0, 0.1], [0, 0, 1, 0], [0, 0, 0, 1]]) # 测量: [x, y] H np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) # 过程噪声与测量噪声 Q np.eye(4) * 0.1 R np.eye(2) * 0.5这一小段就是跟踪系统“运动估计”的地基。真正的工程代码里F矩阵要根据雷达帧间隔动态填写Q和R要按目标类别和距离分段设置但基本思想完全一致。关于协方差初始化一个常见误区是新轨迹的协方差给太小滤波器觉得自己“很确定”结果后续检测稍有波动更新反而跟不上。我建议新轨迹的协方差给一个较大的初值比如速度协方差给10甚至更大让它经过几帧的收敛再“定下来”。这个“让子弹飞一会儿”的策略能显著减少初始阶段的轨迹抖动。3.4 EKF和UKF什么时候才需要上线性卡尔曼滤波要求运动模型和测量模型都是线性的CTRV模型和某些非线性测量比如从检测框角点反推朝向就不满足这个条件。这时候有两个选择扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF。EKF的核心是“对非线性函数做一阶泰勒展开”实现相对简单但遇到强非线性时会有一阶截断误差。UKF则是用一组精心选取的Sigma点来近似概率分布不需要算雅可比矩阵精度更高尤其适合强非线性系统。代价是运算量比EKF大实现也更繁琐。对于点云跟踪我的实际经验是城区场景的转弯用UKF比EKF好。EKF在目标突然大转弯时预测框经常会滞后又抖动UKF对曲率变化的适应性明显更强。但UKF也不是没有代价它对过程噪声参数更敏感调参工作量会明显增加。如果CTRVEKF已经够用不建议盲目上UKF——在嵌入式平台上每帧多花0.1毫秒可能都是浪费。4. 轨迹生命周期管理起轨、维持、消亡与遮挡处理4.1 轨迹的三种状态轨迹管理的本质是一个状态机。最常见的划分是三种状态候选tentative、确认confirmed、消亡dead。候选状态是轨迹的“试用期”。每次出现一个没有匹配到任何轨迹的检测框就为他创建一条候选轨迹状态是tentative。连续若干帧比如3帧都成功匹配候选轨迹转正状态变为confirmed。反之如果在试用期内有一帧没匹配上这条候选轨迹就直接删除。确认状态是轨迹的稳定期。它会被持续跟踪数据关联时优先匹配也会输出给下游模块使用。确认轨迹连续多帧失配后也不会立刻删除而是进入“缓冲期”或“濒死期”等一段时间后仍无法重新关联才转入dead。为什么这么设计两个原因。第一避免误检污染。单独一帧的高分误检如果直接建confirmed轨迹下游会看到一个幽灵目标规划可能因此急刹。多帧确认就是“眼见为实”的过程。第二避免漏检打断跟踪。真实目标被短暂遮挡一帧如果立刻删轨迹重新出现时就要建新IDID Switch爆炸。缓冲期就是“容忍短暂消失”的窗口。4.2 生命周期参数应该怎么给生命周期管理这块没有统一公式参数高度依赖场景。我给出一个默认起点大家可以根据自己的数据调整参数推荐初值说明候选确认帧数3帧连续匹配成功且置信度达标候选死亡帧数1帧试用期失配即删避免误检拖沓确认轨迹失配删帧数5到10帧取决于遮挡场景严重程度确认轨迹重新关联窗口3帧缓冲期内用预测框参与匹配速度合理性阈值按类别和场景动态设防止静止误检建出离谱轨迹再补充几个细节。候选轨迹也不是只看连续帧数还要看匹配质量如果每一帧的关联代价都很高哪怕连续匹配成功也要打个折扣。另外置信度阈值在确认阶段可以比检测阶段更高因为候选阶段样本很多宁缺毋滥。最后不同类别的轨迹生命参数最好分开设置行人和车辆的遮挡行为完全不同——行人容易被车身挡住消失几帧很常见车辆如果消失几帧往往是驶离视野了长期保留反而会和远处的误检搅在一起。4.3 遮挡处理与预测框维持的边界目标被遮挡时轨迹并没有消失而是“暂时失明”。这时候系统会用预测位置生成一个“预测框”继续参与下一帧的数据关联。这个策略能救回大量短暂被挡的目标但也有一个明显的副作用预测框只是滤波外推的结果随着时间推移误差会越来越大不能真的把它当成可靠检测框。工程上的做法通常是“三个区别”。第一预测框不参与某些高置信度检测的匹配避免目标被挡后同一位置来了一个新检测直接把ID带跑。第二预测框的置信度要逐步衰减不是每帧都一样。第三若遮挡帧数超过阈值预测框退出关联池轨迹进入“休眠”等目标重新出现后视作“重新建轨但尽量延续原ID”。这里有个特别常见的坑遮挡恢复后检测框往往出现在预测框附近但中心点偏差可能超过一个车身宽度。如果关联门控设得太死目标既没被重新匹配上也没被删除反复横跳最后ID Switch爆炸。我的处理经验是恢复阶段的关联阈值要比正常阶段放宽20%到30%并且优先匹配“同类别、尺寸相近”的轨迹用类别和尺寸信息补足位置不确定性的不足。关于休眠轨迹到底该不该保留不同团队分歧很大。有的方案完全不做休眠遮挡超过阈值就删简单直接ID稳定性差一点有的方案保留休眠轨迹几十帧为的是尽量延续ID但内存开销和误匹配风险都会上升。我个人的倾向是量产项目里保留休眠轨迹的时间不要超过10帧。超过这个时间重新关联成功率和误匹配概率都非常高收益远小于风险。5. 轨迹预测从运动模型到交互感知5.1 短时预测为什么能靠运动模型顶上轨迹预测要看预测时长。预测500毫秒姿态基本就是“把当前速度接着滑”预测3秒就需要考虑车道、红绿灯、其他目标的交互。点云跟踪系统输出的预测主要是给规划模块做“未来500毫秒到1秒”的安全校验和纵向控制参考这个时间尺度上运动模型外推是完全够用的。但“把速度接着滑”有个前提速度估计必须准。这就要回到上一章说的卡尔曼滤波。速度估计的精度取决于两个因素检测噪声水平和滤波器参数。检测噪声大滤波器的速度估计会滞后因为系统不敢轻易相信“位移/时间”算出来的瞬时速度。滞后表现为目标其实已经在刹停但滤波输出的速度还是很大预测轨迹还在往前冲。这个现象在实车上非常危险。减小滞后有三个手段。第一合理设置过程噪声Q——太大则速度跟随太快易抖太小则滞后严重。第二用更高阶的测量模型比如把加速度也放进状态向量。第三针对静止目标做特殊处理连续多帧位置变化不超过阈值的轨迹直接强制速度归零。这个“静止锁定”在量产项目里几乎是标配能明显改善下游控制的舒适性。5.2 预测框和真实目标中心的偏差问题点云检测框的中心点并不等于目标车体的真实中心。原因有很多目标部分被遮挡时点云只覆盖了可见部分框会偏向可见侧大卡车经过传感器近旁时高反射表面会让点云分布不均框中心被拉到一侧还有一个常见情况是两辆车离得很近时点云分割不干净框会“拟合”成一个更大的假目标。这个偏差对滤波的影响很大。如果直接把检测框中心当作准确测量值卡尔曼滤波会把偏差当成“目标的真实位置变化”结果产生虚假的速度分量。我做过一个场景测试一辆车静止在路边但因为遮挡导致框中心偏移滤波输出的速度居然一直在0.3到0.5米/秒之间浮动预测轨迹也跟着漂。后来加了“静止检测位置锁定”逻辑这个问题才被按住。解决思路有几个层面第一在测量模型里把“框中心偏移”建模为附加噪声让滤波器知道中心点不可全信第二输出下游之前把轨迹速度做一个低通滤波减少脉冲式波动第三对轨迹的连续多帧位置做一致性校验如果位置几乎不动就强制把速度状态归零。最后这个“硬规则”虽然不优雅但在工程里极其有效。5.3 从单目标外推到场景级预测运动模型外推只能解决“目标自己会怎么走”。真实交通场景里车和车、车和路之间有交互前车减速后车大概率也要减速路边有静止障碍目标轨迹往往不会穿过它。这些信息运动模型是推不出来的。更完整的轨迹预测通常分为三档。第一档就是纯运动模型外推0到1秒够用方案简单可靠。第二档是“运动模型道路约束”用地图信息把预测轨迹投影到车道中心线上或者用人行横道、路口边界做约束修正预测时长可以扩展到3秒。第三档是“交互感知预测”用一个轻量网络编码所有目标的历史轨迹和道路结构输出每个目标未来多条候选轨迹的概率分布像生成式模型那样给出多种可能。在量产方案里我见过最多的是“第一档为主、第二档做后处理、第三档备选”。这和算力成本、模型迭代周期都有关系。第三档虽然效果上限高但要跟检测模型、跟踪系统联合标定工程链路过长。如果团队资源有限先把第一档做扎实配合交通规则的后处理已经能覆盖大部分封闭路段和部分城市场景的需求。注意轨迹预测的输出格式也影响下游使用。有的规划模块要的是“目标未来N帧的预测位置列表”有的要的是“目标的意图类别置信度”有的只要“未来的可达域多边形”。不同下游预测模块的接口设计完全不同。建议在系统设计阶段就和规划团队对齐输出格式这比后面反复改接口省力得多。6. 评价指标与工程调优实战6.1 指标怎么看才不被数字骗评估跟踪系统不能只看一个MOTA。多目标跟踪的经典指标有指标含义注意点MOTA综合跟踪精度结合漏报、误报和ID Switch数值高不代表轨迹稳定MOTP跟踪框与真值框的重合精度受检测质量影响大IDSID Switch次数ID发生跳变的次数密集场景和交错场景的“照妖镜”MT长时间跟踪成功的轨迹占比反映系统对稳定目标的保持能力ML过早丢失的轨迹占比反映系统对遮挡、漏检的容忍度HOTA更高阶的评估指标兼顾检测和关联比MOTA更能反映关联质量做工程评估时单独看MOTA很容易被“高检测质量”掩盖。比如检测模型很准漏检很少MOTA自然高但ID Switch可能很多轨迹质量一塌糊涂。所以我建议至少同时看MOTA和IDS并且把“长时间保持ID不跳变”作为量产验收的硬指标。还有一个容易被忽略的细节指标要用加权方式算不能按帧平均。不同场景下目标数量差异很大拥挤路口的几百个目标帧和野外的一条直道按帧平均会把拥挤场景的ID Switch稀释掉。按目标时长加权或者分场景单独统计才能暴露真实问题。6.2 高频问题排查速查表下面这张表是我调过多个跟踪系统之后总结的“症状-原因-解法”速查表遇到问题可以直接对着排查症状可能原因解决思路轨迹抖动严重运动模型不匹配、Q过大/过小、检测噪声大先校准测量噪声R检查模型是否符合目标运动特征ID Switch频繁检测漏检严重、门控太宽、级联匹配优先级不对放宽缓冲期、收紧门控、优先匹配高置信轨迹目标短暂遮挡后丢失轨迹缓冲期太短、预测速度不准增加缓冲帧数检查遮挡期间速度是否发散静止目标有速度漂移框中心偏移、滤波器未收敛加静止锁定对连续低速轨迹做速度置零虚警轨迹过多候选确认条件太宽松提高确认帧数和置信度阈值预测轨迹“冲进”障碍物纯运动模型外推无法感知交互加道路约束或场景级交互预测6.3 调优的顺序和原则调跟踪系统的参数千万不要一上来就调卡尔曼滤波的Q和R。调参顺序很重要我的习惯是先外部后内部先是数据层。检查检测模型在不同距离、不同遮挡程度下的输出质量。如果检测框本身就不稳定跟踪系统再努力也是白搭。这个阶段重点确认瞬时漏检率、误检率、框的尺寸抖动方差。数据层不达标先去优化检测而不是堆跟踪参数。再是关联层。打开数据关联的log看每一帧的代价矩阵、匹配结果和unmatched列表。这个阶段重点调的是门控阈值、级联匹配的优先顺序、以及匹配代价的权重。很多ID Switch问题在关联层就能解决掉大半。最后才轮到状态估计层。在关联稳定之后再去调卡曼滤波的Q、R、协方差初始值。这时候调参的目标不是“让跟踪跟得更紧”而是“在噪声和滞后之间找到平衡点”让轨迹既平滑又不迟钝。还有一条很重要的原则每次只动一个参数。跟踪系统的各个模块强耦合你同时改了门控阈值和Q矩阵出了问题你根本分不清是谁闯的祸。改完一个参数回归一版测试集记录指标变化再动下一个。这听起来很笨但却是量产项目里效率最高的做法。最后再分享一个我自己常用的小技巧在调试阶段给可视化工具加一个“显示轨迹历史轨迹点”的开关。轨迹的历史点能直观暴露很多问题——轨迹是否抖动、是否弯曲、是否在目标停下后还在原地画圈。我发现很多参数问题不是看指标看出来的而是看可视化“一眼看出不对劲”的。先把可视化做到位再开始调参手里有图心里不慌。