第一次在实车数据回放里看到障碍物 ID 反复横跳我几乎把锅全甩给了检测算法。后来把精力从 detection 挪到 perception 链路的 tracker 逻辑才明白问题的根子经常不在网络输出框准不准而在于后端有没有把同一辆车的身份在帧间稳定接住。这篇文章是 Apollo 开源项目里 perception 模块的一个源码解读范围锁在 lidar 分支的 tracker 部分也就是负责把每帧检测出的障碍物列表变成携带稳定track_id、速度、方向和类型的轨迹集合的组件。我会用一条虚拟目标从出现到消失的一生为主线来拆代码逻辑适合正在读感知源码的人、在自己数据集上做多目标跟踪的人以及负责激光雷达感知工程参数调试的人。1. 先搞清楚 Tracker 在感知链路里的位置吃进去什么吐出来什么1.1 从原始点云到稳定轨迹的最小数据链很多读者打开感知源码时习惯直接从检测模型开始看看到segmented_objects或类似的目标列表输出觉得障碍物已经框出来了再往后就不太愿意碰。但真实系统里检测输出到最终决策之间还隔着关键的一层也就是 tracker。整体数据链大致是这样的原始激光雷达点云先进 ROI 区域过滤去掉自车周围范围外和地图上已知的静态区域再做地面分离和点云分割然后进入检测网络或聚类算法得到单帧目标框。这一层拿到的是这一帧有哪些障碍物每个目标通常包含中心点、长宽高、朝向角、类别概率和置信度。到这里为止目标与目标之间是没有时间关系的。下一帧同一个障碍物可能因为点云视角变化而产生完全不同的框如果没有跟踪模块它就会被当成一个新目标。Apollo 的 lidar tracker 就是在这一层工作拿到当前帧检测出的障碍物列表结合内部保存的历史轨迹输出一份带身份信息的tracked_objects。这份结果会被继续送往多传感器融合、预测和规划模块。也就是说segmented_objects解决的是这一帧哪里有东西tracker 解决的是这个东西从哪来、往哪去、是不是之前那个东西。1.2 输入与输出的对象字段差异从障碍物到轨迹多了哪几位身份信息如果把segmented_objects和tracked_objects的字段放在一起对比能非常直观地看到 tracker 做了什么。检测输出侧常见的字段是center、size、theta、category和confidence它们描述的是空间占有情况。而跟踪输出侧除了保留这些空间信息还会补上track_id、track_age、consecutive_invisible_count、velocity、direction、type_probs等时间维度的状态量。track_id是全局递增的身份编号删掉的轨迹不会复用这是为了避免旧 ID 干扰下游逻辑。track_age表示这个轨迹自创建以来关联成功的帧数它和连续可见帧数并不是一回事中间可能有遮挡帧。consecutive_invisible_count专门记录连续多少帧没有匹配到观测这个值是判断轨迹要不要被删除的关键。velocity和direction是跟踪器通过滤波估计出来的不是检测器直接给的检测器能给出的只是两帧中心差的一阶粗略推算噪声很大。type_probs是类别概率的历史累积用于平滑单帧分类的抖动。字段差异已经能说明问题检测器负责看见tracker 负责记住。没有后面这一层规划模块拿到的障碍物就不知道下一秒还在不在预测模块也没有历史轨迹可以做意图判断。1.3 为什么检测已经出了框还要再跟踪一个很常见的疑问是既然每帧都能检测到目标为什么不能直接拿检测结果用原因有三层。第一激光雷达点云是稀疏且视角依赖的。一辆车在一帧里可能只被扫到侧面中心点落在车身中段下一帧扫到车头中心点整体前移如果没有滤波和平滑障碍物位置会出现明显跳变。第二单帧检测必然有虚警和漏检跟踪器可以通过连续多帧的一致性来过滤孤立的假目标也可以在短暂遮挡时用运动模型把轨迹外推撑过去。第三自动驾驶下游需要知道目标的身份连续性比如一个障碍物是始终静止还是刚刚减速停下这只有跨帧关联之后才能回答。所以tracker 在感知链路里并不是一个可选项而是把单帧感知升级成时序感知的必经环节。理解了这一层再回头看源码里的各种阈值和状态机就不会觉得它们只是在堆参数。2. Tracker 源码骨架主入口如何拆活三个子模块怎么协作2.1 主入口负责装配不负责算法打开 Apollo 的 lidar tracker 源码第一印象往往不是算法复杂而是类的层次非常多。主入口通常表现为一个类似LidarObstacleTracking的类它继承一个基类接口Process方法是整个跟踪器的门面。这个函数本身没有太多数学逻辑它做的事情更像一个调度器先从配置里读取当前使用的算法实现名然后用工厂创建关联器、状态更新器和滤波器最后把当前帧的segmented_objects转换成内部观测结构依次喂给这几个子模块。这一段代码的阅读建议是不要陷在工厂和注册的细节里。全局搜索tracker_config或*.pt配置文件找到tracking字段下配置了哪几个实现名。不同发布版本里默认实现的名字可能不同例如某组常见实现是MlfDataAssociation、MlfTracker、MlfFilter换一个版本可能叫HmttAssociation、HmttTracker、HmttFilter但职责是稳定的。看到create_algorithm之类的调用时只要知道它根据配置字符串返回了对应对象即可先跳过去看算法实现比看框架代码有价值得多。2.2 三个子模块的分工边界Apollo 这类跟踪器把逻辑拆成三个子模块我认为是这套设计中比较值得借鉴的一点。关联器负责回答当前帧观测和历史轨迹之间谁和谁是一对它输出匹配对、未匹配的新观测和未匹配的旧轨迹。状态更新器只处理匹配上的轨迹用当前帧观测去修正轨迹的位置、速度、方向等运动状态。滤波器负责轨迹生命周期的维护包括新轨迹的创建、确认、删除以及类别概率和置信度的平滑更新。三个模块的边界非常干净数据关联不会去修改轨迹状态状态更新不会去创建轨迹生命周期管理也不会插手匹配。这样的拆分在工程上最大的好处是每个模块可以独立替换和调参。你不需要为了调整关联阈值而重写滤波逻辑也不需要为了改变轨迹存活时长而去动状态更新代码。在实际调试时这个特性非常有用我可以只把关联器的日志打开观察它给出的匹配结果而不影响其他模块的运行。2.3 数据流向从观测列表到轨迹列表的文字路线如果要用一段文字把数据流画出来大概是下面这条链路segmented_objects ↓ 转为内部观测列表中心、尺寸、朝向、类别、置信度 ↓ Association构建代价矩阵 → 门控 → 匹配 ↓ 匹配对 / 未匹配观测 / 未匹配轨迹 ↓ Tracker对匹配轨迹做卡尔曼滤波更新 ↓ Filter新轨迹确认、丢失计数、轨迹删除 ↓ tracked_objects在代码里分别给这个链路的每个阶段加日志比直接看几十个类之间的调用关系要直观得多。我看到很多读者第一次读这段源码时会在某个Update函数里迷路因为函数内部会继续调用下一层的Update。我的习惯是先在主入口打印每种集合的大小比如matched.size()、unmatched_obs.size()、unmatched_tracks.size()看一帧数据在哪个环节数量变化再去对应的子模块里断点。这样可以快速把问题定位到某一个具体的算法策略上。2.4 读源码的三个实用步骤我给第一次接触这套代码的人建议一个三步走的路径。第一步找到主入口的Process函数在上面打一个断点尽可能把当前帧segmented_objects的数量先打印出来。第二步单步走到关联器之后打印匹配对和未匹配集合数量。如果你的场景里检测出了 5 个目标但是匹配对只有 2 个说明关联阈值或代价权重设置有问题要么是新观测和旧轨迹的差异超过了门控范围要么是代价矩阵里某个特征权重压倒了其余特征。第三步走到 filter 之后看最终输出轨迹的track_id是否有变化再结合关联阶段的日志基本就能定位 ID 跳变的根源。这个办法不需要改太多代码成本很低但能极大缩短理解源码的时间。不要一开始就去啃卡尔曼滤波矩阵推导先把数据流向和模块边界盘清楚后面看数学公式时才有落点。3. 核心逻辑一条虚拟目标的诞生、成长与消亡3.1 关联之前的准备从匹配对到代价矩阵关联器是整个 tracker 里对最终效果影响最大的部分。它首先要构建一个代价矩阵矩阵的行是当前帧新观测的索引列是历史轨迹的索引每个单元格表示这个观测和这条轨迹有多像的代价代价越小越可能是同一个目标。代价通常由几个特征距离加权求和得到。位置距离是最基础的一项一般取中心点的欧氏距离或经过协方差加权的马氏距离。尺寸距离描述的是长宽高的差异因为检测框在遮挡和视角变化下会抖动不能只靠它区分目标。朝向距离则比较两个框主方向之间的夹角点云框的朝向对车辆这类目标比较稳定对行人类目标常常漂移权重需要区分目标类型。除了这三项有些版本还会加入速度差。最终代价会做归一化避免某一个特征因为数值量级大而主导整个匹配过程。门控机制在这个阶段非常重要。所谓门控就是给每个配对设一个上限如果观测和轨迹的某个距离指标超过阈值直接把对应单元格置为无穷大或不可匹配。这样做有两个目的一是避免把明显不同的目标强行配在一起二是缩小匹配算法的搜索空间。实际调参时如果门控太紧短暂遮挡后重新出现的目标会因为预测位置偏差过大而变成新目标门控太松又容易把相邻的两辆车搞混。这个度需要结合传感器频率和场景特点反复试。3.2 匈牙利匹配与先易后难的策略代价矩阵构建好之后下一步是求解最优匹配。经典的做法是把它转换成一个最大化分数的问题然后用匈牙利算法或带门控的二分图匹配求解。算法本身并不神秘它解决的就是在满足一对一约束的前提下让整体匹配代价最小的组合优化问题。因为目标数量和轨迹数量往往不相等矩阵不是方阵代码里通常会给缺失的行或列补零值或者分别处理多出来的观测和轨迹。值得注意的是一种先易后难的匹配策略。第一轮先对高置信度观测和已经确认存活很久的轨迹做匹配因为这些配对通常比较可靠匹配结果可以直接锁定。第二轮再处理低置信度观测、新轨迹和没有在第一轮被匹配上的对象。这种做法和近年多目标跟踪里常见的思想是一致的先把可靠的锁定再处理模糊情况能显著减少误匹配。源码中可能体现为按score或按轨迹确认状态排序后分段匹配阅读时如果有条件建议把每轮匹配的输入集合和输出结果都打出来看看。匹配结果被分为三类匹配对、未匹配观测和未匹配轨迹。匹配对会进入状态更新阶段未匹配观测成为新轨迹候选未匹配轨迹则增加不可见计数等待生命周期管理模块决定是继续保留还是删除。3.3 状态预测与更新卡尔曼滤波在激光雷达里的简化状态更新器负责维护轨迹的运动信息核心手段通常是卡尔曼滤波器或它的扩展形式。在激光雷达目标跟踪里状态向量一般取位置加速度例如(x, y, z, vx, vy, vz)有些实现还会把长宽高和朝向放进去。预测阶段使用匀速模型也就是认为目标在相邻两帧之间速度不变位置按照上一刻的速度线性外推。观测模型相对简单因为激光雷达检测器能直接给出目标的中心位置所以观测矩阵只取位置分量。实际代码里容易让人困惑的是滤波并不负责所有字段。尺寸、朝向和类别往往不走卡尔曼滤波因为它们在点云中并不满足线性高斯假设。尺寸和朝向通常用历史值与当前观测值做加权平滑类别则通过类别概率的累积更新来处理。卡尔曼滤波的两个噪声矩阵也对效果影响很大观测噪声太大轨迹会变得迟钝跟不上突然加速的目标观测噪声太小轨迹中心就会紧贴检测框中心跳动。我一般先从日志里看轨迹速度曲线的波动如果速度在每帧之间大幅跳变基本就是过程噪声或观测噪声的均衡没有调好。没有匹配到观测的轨迹在可见帧里只执行预测不做更新同时累计不可见帧数。这里要控制外推的时间长度如果目标被遮挡或漏检太多帧而继续外推轨迹的不确定性会膨胀位置预测会偏离真实目标反而增加后续错误关联的风险。3.4 轨迹生命周期诞生、确认和消亡每一条轨迹都会经历三个状态候选、确认、消亡。当前帧出现了一个没有匹配到任何历史轨迹的观测关联器会把它标记为未匹配观测filter 随之创建一个新轨迹候选。这个候选会分配一个新的全局递增track_id初始track_age是 0consecutive_invisible_count也是 0score初始化为当前观测的置信度。关键点在于候选轨迹不会立刻被输出到tracked_objects。它需要连续若干帧都被关联到观测或者累计可见帧数达到阈值才会被确认为正式轨迹。这个确认机制是过滤单帧虚警的核心手段。一个真正的目标通常会在连续多帧里反复出现而一次性的误检很难在同一个位置连续出现多帧所以确认阈值设得越高虚警越少但新目标的响应越慢。工程上一般取 2 到 5 帧之间具体要看下游对延迟的容忍度。消亡条件通常有两个方向一是连续不可见帧数超过阈值二是轨迹的score降到设定值以下。前者用于处理遮挡和漏检后者用于处理置信度持续偏低的轨迹。轨迹被删除后它的track_id不会还给全局计数器新轨迹会继续取新的编号。这样能避免下游因为 ID 复用而产生混淆虽然代价是 ID 数值会单调增长但对系统稳定性来说更值得。3.5 track_id 不抖的秘密身份累积与关联鲁棒性理解 track_id 为什么稳定不能只看 id 分配逻辑更关键的是关联环节是否在每一帧都做出了正确选择。只要一个目标在连续帧间都被匹配到同一条轨迹它的track_id自然不变一旦某帧发生了错误匹配ID 就会跳变。所以工程上真正要做的是提高关联的鲁棒性。一个常见优化是给轨迹维护累积分数。匹配成功时score增加匹配失败时score减少只有分数足够高时才确认轨迹分数跌到阈值以下才删除。这和单纯的可见帧计数不同它能表达最近一段时间整体质量如何而不是仅仅看连续帧。另一个优化是给刚创建的目标一个较短的确认窗口在窗口期间即使短暂丢失也不要立刻删除等它再出现时仍然可以接回原 ID。这些策略在 Apollo 源码中或多或少都有体现读代码时可以特别关注score、age和consecutive_invisible_count这几个变量是如何互动的。4. 实战中 Tracker 最容易暴露问题的四个瞬间4.1 目标一靠近自车就断线我遇到过一种很典型的现象目标在 30 米外跟踪得好好的一旦进入自车周围 10 米范围内轨迹就断掉重新分配一个新 ID。表面上看像是检测漏了几帧实际上问题往往出在关联环节。近处点云密集传感器对目标的扫描角度变化很快检测框的中心、尺寸和朝向在相邻两帧之间会有较大波动。如果代价矩阵里尺寸差的权重太大或者门控阈值设置过紧旧轨迹和当前观测就可能被判定为不是同一个目标。排查这个问题时先不要急着去调检测参数。打开关联日志打印匹配对数量和未匹配轨迹数量再看断线的轨迹是不是连续多帧不可见。如果是先把门控半径适度调大或者把尺寸代价权重降低观察轨迹是否恢复。另外还要考虑自车周围的盲区比如目标进入车顶上方或车侧近处时激光雷达可能一时扫不到这种情况属于物理遮挡需要靠轨迹外推和存活阈值来扛。4.2 同一辆车 ID 反复横跳ID 反复横跳是跟踪问题里最常见的抱怨现象是记录的 track_id 序列像 12、12、13、12、14 这样不稳定。这个问题的本质是目标的观测短暂中断后旧轨迹没有成功接住重新出现的观测于是生成了一条新轨迹下一帧旧轨迹又和新观测匹配成功于是又换回旧 ID给人造成反复横跳的观感。解决方案通常有三个方向。一是适当延长轨迹在不可见状态下的存活时间让它能在遮挡结束后继续参与匹配。二是检查运动外推如果卡尔曼滤波估计的速度过快或过慢重新出现时的预测位置会偏离真实位置门控就把这次匹配拒绝了。三是采用二次匹配策略第一轮先配高置信度观测和已确认轨迹第二轮再处理低置信度观测给那些短暂丢失的轨迹一个低级复活的机会。需要提醒的是这些手段都有副作用每个参数都有度不能为了追求 ID 连续就把门控放到无限大否则相邻目标会开始互相串 ID。4.3 类别抖动行人和车辆之间的反复切换有时候轨迹的track_id是稳定的但目标类型却在行人和车辆之间来回跳。这个现象说明关联没出问题问题出在类别信息的历史累积策略上。激光雷达单帧分类本身就存在歧义比如行人抱着大箱子、骑车人侧向点云形态近似车辆模型输出概率很容易在阈值附近摇摆。如果 tracker 直接采纳最新一帧的类别那么类别就会跟着单帧分类结果剧烈抖动。正确处理方式是历史概率平滑保留每个轨迹的type_probs向量每次关联成功后把当前观测的类别概率与历史概率做加权平均历史权重通常取 0.7 到 0.9。这样单帧的分类波动只会轻微拉动概率不会立刻改变最终类别。如果还需要更稳可以在输出层做滞后切换要求新类别连续若干帧的概率都超过旧类别才真正切换。注意这个逻辑不一定在默认源码里全部实现有时需要自己补一个极小的补丁。4.4 离线调参的正确姿势调跟踪参数最忌讳的做法是改一个数就去跑一次实车或用在线数据试既浪费时间又没法保证复现。正确的方式是把原始点云或者检测结果录制成离线数据在回放环境下反复调参。每次只改一个变量记录下同一段场景里的跟踪指标比如 ID 切换次数、轨迹中断次数、漏检率再回来对比。我自己会在关联器入口加一个临时日志输出一帧的代价矩阵和匹配结果在 filter 出口加另一个日志输出每条轨迹的删除原因。两段日志拼在一起基本能还原 ID 跳变的完整现场。可视化方面可以打开 DreamView 或者直接用 cyber_visualizer 看 track_id 在画面上的分布ID 跳变在视觉上非常直观。离线调参时还要注意版本管理把不同阈值对应的结果文件放在一起不然一周后你就忘了当前这套参数是从哪组实验里来的。5. 从 Apollo Tracker 带走一套通用多目标跟踪方法论5.1 在一个自定义检测器上复用这套逻辑的最小配置读 Apollo 的 tracker 源码收获不应该只停留在我懂了它的代码而应该能提炼出一套可迁移的多目标跟踪方案。如果你手里有任意一种检测器想给它加上稳定跟踪最小配置只需要四块轨迹数据结构、代价度量、匹配算法和生命周期状态机。轨迹数据结构至少要包含track_id、state_vector、age、score、visible_count、invisible_count、type_probs和last_update_time。代价度量最简版可以使用中心点欧氏距离加尺寸距离有朝向信息就再加一个角度差项。匹配算法在目标数量不多、帧率稳定的场景下可以先做门控再用贪心或匈牙利算法求解。生命周期状态机就是我在第 3 章讲的候选、确认、消亡三态模型。下面给出一段极简的伪代码思路def associate(pred_tracks, detections): cost build_cost_matrix(pred_tracks, detections) mask gating(cost, threshold) matched, unmatched_dets, unmatched_tracks hungarian_with_gate(cost, mask) return matched, unmatched_dets, unmatched_tracks def update_track(track, detection): track.predict() # 匀速模型预测位置与协方差 track.correct(detection) # 卡尔曼滤波更新位置与速度 track.update_type_probs(detection.category_probs) def manage_lifecycle(tracks, matched, unmatched_dets, unmatched_tracks): for det in unmatched_dets: create_new_candidate(det) for track in unmatched_tracks: track.invisible_count 1 if track.invisible_count max_lost_frames: delete_track(track) for track in matched: track.invisible_count 0 if track.age confirm_threshold: track.confirmed True这套最小系统跑起来之后再逐步加入朝向平滑、速度约束、类别累积这些细节。它和 Apollo 里真实代码的差距只是工程复杂度和边界处理核心思想是一致的。5.2 想做得更好三个可落地的扩展方向如果你不满足于复现一个最小 tracker想进一步降低 ID Switch 和轨迹断裂可以从三个方向扩展。第一个方向是引入外观特征也就是给每个目标提取一个 embedding让关联代价里多一项外观距离。点云场景可以用 BEV 特征或前景点云特征。这个方案对遮挡后重识别效果显著但会引入额外算力部署到嵌入式平台前要评估。第二个方向是改进运动模型车辆在转弯时匀速直线模型误差很大可以针对车辆用 CTRV 或更复杂的运动模型行人和骑行者则保留匀速模型。第三个方向是把 tracker 的结果作为局部轨迹送入多传感器融合框架和相机、毫米波雷达的目标做全局关联这样即使激光雷达有短暂遮挡视觉信息也能帮忙维持 ID。这三个方向做起来都不简单但它们的共同点在于数据关联、状态估计、生命周期管理这套核心框架基本不用推倒重来。你在 Apollo 源码里读到的每个模式都能在这些扩展里继续复用。最后补一条我自己的心得读这套源码时与其一次把所有类读完不如先在关联器入口打一个日志打印出最近一帧的代价矩阵和匹配结果再把 filter 里删除轨迹的日志打开两段日志拼起来几乎就能还原所有 ID 跳变的现场。我自己就靠这个办法定位过好几次问题。另外不要轻易把门控半径调到很大它确实能让漏检率好看一点但 ID Switch 会变得很难看这个分寸需要针对你的传感器和场景各调一轮。希望这篇从轨迹一生视角去拆的逻辑推演能让你下次打开代码时少走几层弯路。