融合毫米波雷达跟踪结果的自动驾驶决策目标筛选策略

1. 目标筛选在自动驾驶决策链路中的定位1.1 从感知到决策之间的一条“减负层”不少做自动驾驶仿真的朋友早期都会陷入一个误区以为感知模块输出的障碍物列表可以原封不动地喂给决策模块。我自己也走过这个弯路。有一次在仿真场景里跑高架路感知模块输出的障碍物数量直接到了上百个决策模块的计算耗时飙了好几倍车辆的跟车行为开始变得神经质时不时来一脚莫名其妙的刹车。排查到最后才意识到问题不出在感知精度也不在决策算法本身而是中间缺了一道“减负层”。感知层输出的原始目标列表里很多目标从决策角度来说根本不用管马路牙子、护栏、远处的桥墩、路牌立柱。你要是全喂给决策模块第一是算力撑不住第二是大量低优先级目标会干扰决策模块对关键目标的判断。目标筛选说白了就是把这道上百个原始障碍物压缩成决策真正关心的几十个、甚至十几个关键目标。但“压缩”这两个字说起来轻松做起来非常讲究。你不能简单按距离一刀切也不能只按目标类型过滤因为一个目标的“威胁程度”取决于它当前在哪条车道上、速度如何、有没有跟自车存在轨迹交叉可能性。这套综合判断逻辑就是目标筛选策略的核心。1.2 毫米波雷达跟踪结果的价值与不可替代性那为什么要单独把“毫米波雷达跟踪结果”拎出来说因为在多传感器组合里毫米波雷达的位置一直被低估但又无法被替代。讲个具体场景。摄像头在逆光、夜晚、雨雾天气下检测能力会明显下降尤其是对深色车辆和行人误检和漏检都会增加。激光雷达在雨雪天气下点云会被雨滴反射干扰产生大量噪点。但毫米波雷达波长长穿透性好受天气影响相对小对运动目标的检测能力很稳尤其能直接测出目标的径向相对速度。这里的关键词是“跟踪结果”不是“原始点迹”。雷达原始检测点信噪比低、稳定性差直接拿来做决策输入噪音太大了。但经过目标跟踪算法处理之后输出的是带目标ID、连续多帧状态估计的稳定目标列表这个数据质量完全可以用来做决策级的参考了。我在项目里把雷达跟踪结果当成一个“高置信度线索源”来用。用它来验证视觉目标是不是真的存在用它来给视觉目标补充速度信息也用它来发现视觉漏检的目标。打个比方视觉像是人眼信息丰富但偶尔会看花眼雷达则像是盲人手里的探路棍信息单调但老老实实。1.3 把雷达目标直接丢给决策模块会发生什么在做正式策略之前我做了一个对照实验把雷达跟踪目标不做任何处理直接并入感知障碍物列表喂给决策模块。结果非常惨烈。第一个问题是目标爆炸。雷达对金属物体的敏感性极高高速公路两侧的金属护栏、隔音屏、标识牌金属立柱都会被检出。单侧护栏就能形成几十个反射点经跟踪之后会形成十几个稳定目标两侧加起来三四十个。这些目标毫无决策价值却白占运算资源。第二个问题是坐标口径混乱。雷达输出的相对位置是传感器局部系下的如果直接并进去跟视觉目标的坐标不统一决策模块里的坐标转换代码会算出一堆奇怪的结果。最典型的表现就是目标横向位置跳变一会儿在左侧车道一会儿又出现在右侧护栏上。第三个问题是时间戳不同步。雷达跟踪结果和视觉结果如果各带各的时间戳决策模块拿到的就是不同时刻的目标做融合判断的时候两个目标明明对应的是同一个物理物体坐标却对不上。这三个问题其实都指向同一个结论雷达跟踪结果必须经过一个数据重构和筛选的中间层才能变成决策模块真正可用的输入。这就是这个项目的出发点。当然现实世界里雷达和视觉的坐标系统一、时间同步问题需要借助多传感器标定和时钟同步来进一步解决。2. 先搞清楚两边的数据口径——融合的前提2.1 雷达跟踪结果里到底有什么字段融合的第一课不是写代码而是读数据。我拿到雷达跟踪结果之后做的第一件事是画了张表把雷达输出的每一个字段意义、单位、坐标系都重新理了一遍。一个典型的毫米波雷达跟踪目标包含这些核心字段id跟踪器给目标分配的编号稳定跟踪时保持不变longitude_dist/lateral_dist目标相对雷达的纵向、横向距离单位米longitude_vel/lateral_vel目标的纵向、横向相对速度rcs目标雷达散射截面积单位dBsm反映目标反射强度prob_exist目标存在概率范围0到1status目标状态标记比如新生、稳定、遮挡、消失time_stamp该帧数据的时刻这里有个非常坑的细节很多雷达的“相对速度”默认指的是径向速度也就是目标相对于自车连线方向上的速度分量不是笛卡尔坐标系下的完整速度。如果你直接把longitude_vel当成目标的实际纵向速度判断结果会有偏差尤其是在目标横向切入的时候。还有一点雷达输出的lateral_dist正负号取决于雷达内部坐标系的定义有的雷达左正右负有的反过来。这个如果搞错了目标明明在左侧车道代码里判断出来却在右侧整个决策逻辑就全乱套了。我建议任何团队在做融合之前先花时间做一次数据可视化把雷达目标的坐标画出来跟实际场景叠加一下确认坐标系的定义再开始写逻辑。这个步骤能帮你避开后面很多莫名其妙的bug。2.2 Apollo决策模块在等什么样的目标再看另一边决策模块期望的输入是什么样子的。在Apollo的规划模块里决策部分拿到的障碍物信息来源是感知模块输出的目标列表每个目标是一个包含完整状态量的对象三维位置、三维速度、加速度、朝向角、目标尺寸长宽高、目标类型、置信度、跟踪时长等。这里面的几个关键字段雷达跟踪结果其实给不出来目标的长宽高和类型。一个雷达点目标本质上是“一个占据点位置的运动物体”它没有物理尺寸的概念。你让它区分一辆重型卡车和一辆自行车几乎不可能。毫米波雷达的点迹在雷达看来是“反射峰”而一个目标扩展了多少一方面由天线角度分辨率和距离分辨率决定另一方面和反射点聚类的算法有关。这就决定了融合方案的大方向融合不等于把雷达目标“升级”成完整感知目标而是利用雷达目标的高置信度速度和位置信息去做决策目标的筛选和验证。也就是说雷达目标更像是一个“参考线索”它帮你判定哪些区域存在值得关注的运动物体但最终给决策模块用的完整目标可能还是以视觉检测框为基础来构建再用雷达信息去校验和补充。2.3 融合的三个层次与这个项目的定位在我自己梳理的过程中我发现“融合”这个词被用滥了。很多人一提到融合就以为要做一个超级算法把雷达、摄像头、激光雷达的所有原始数据塞进一个网络里只要训练得当就能无所不能。先不说算力能不能扛住单是数据标注成本就够喝一壶。回归到工程实践融合至少可以分成三个层次第一层是数据级融合。把雷达原始点迹、激光点云、图像像素统一放到一个空间里或者做点级别的特征融合这是感知团队的范畴通常需要传感器时间硬同步。第二层是目标级融合。每个传感器先各自形成目标列表再做目标关联、匹配、合并输出一个统一的融合目标列表。这一层已经比较接近决策需求了但关联规则的设计需要做大量的场景适配。第三层是决策级融合。感知目标列表已经形成了决策模块在做行为决策的时候综合多路输入来做权重判断。比如视觉说前方有个目标雷达也确认那里有个目标那决策的可信度就高。这个项目的定位很有意思它夹在第二层和第三层之间。不是去做感知内部的融合而是把雷达的跟踪结果用一种巧妙的方式引入到决策目标筛选这个环节。雷达不直接参与目标的视觉特征生成但它的存在会直接影响哪些目标能进入决策模块的视野。我之所以选择这个定位核心考量是可移植性。感知融合跟传感器硬件耦合非常紧换一个雷达型号融合算法要重构。而决策级筛选策略只要你输出标准化的雷达目标字段无论底下是什么毫米波雷达硬件策略逻辑都能复用。3. 决策目标筛选策略的设计思路3.1 四步流水线从雷达目标到决策目标的筛选框架整个筛选策略我设计成了四步流水线。这个结构不是一次想出来的而是踩了几个坑之后反复调整得出的。每一道步骤都在解决某个我在仿真中实际遇到的痛点。第一步坐标系归一化。不管雷达目标原本在哪个坐标系下传感器局部系、自车系统一转换到一个基准坐标系下。在Apollo的生态里考虑到后续规划模块用Frenet关系最多我选择把坐标转换到Frenet坐标系即以参考线为基准、纵向用s表示、横向用l表示。同时把雷达的时间戳和对齐后的视觉目标时间戳统一到一个时间轴上解决时间戳不同步的问题。第二步车道相关区域初筛。有了Frenet坐标就可以直接判断目标相对本车车道的横向关系。这里不是简单地判断“目标是否在同一条车道”而是把目标在横向l方向的位置和横向速度趋势结合起来判断它是否属于“可能会影响本车行驶轨迹”的目标。第三步碰撞风险筛选。只看横向还不够。一个目标在隔壁车道但它跟我们同向同速并行那么即便是并排行驶也没有追尾碰撞风险。但如果一个目标在我们的正前方且相对速度较大碰撞时间很短那这就是高风险目标。第四步目标生命周期管理。雷达跟踪本身不是100%可靠的会有抖动、丢失、存在概率波动。如果每一帧都直接筛选决策决策模块看到的输入就会忽多忽少不稳定。所以我对每个目标挂了生命周期状态连续被观测多少帧才“激活”连续丢失多少帧才“消亡”这相当于做了一个时间维度的滤波。3.2 Frenet坐标系下的车道相关性判断整个策略里最值得展开讲的就是Frenet坐标系下的车道相关性判断。先用一个生活化的例子解释Frenet。你在一条弯曲的山路上开车直角坐标笛卡尔坐标告诉你前方目标在你的东南方向但你知道道路在这里拐了一个弯光看东南方向根本判断不了它是否在你的车道前方。而Frenet坐标把道路中心线当成一把拉直的尺子纵向距离s是离你沿道路走了多远横向距离l是偏离中心线多少。这样一看目标在不在当前车道一目了然。具体到我的筛选逻辑里对每一个雷达目标都会有它的Frenet坐标(s, l)。本车所在的车道中心线横向l方向上占据了大约[-1.75米, 1.75米]的范围以标准车道宽度3.5米计。那么我第一直觉是目标的l绝对值大于某个阈值就是非关键目标直接筛掉。但这个直觉我第一次用就翻车了。原因是有一种经典的切入场景目标在隔壁车道它的中心点l可能已经超过本车道边界但它正在朝本车道变道车头已经压到了车道线。如果你只看中心点位置会把它筛掉然后下一秒它切入本车道决策模块才发现前方多了一个目标晚了。所以我在做车道相关判断时除了看当前横向距离l还叠加了几个因素目标的横向速度分量l的变化率、目标的朝向角、目标与本车的纵向距离。如果目标横向速度在朝本车道方向变化即使当前l略大也保留为候选目标。相当于提前预测了切入意图。这里的本质是“横向距离表象”加上“横向运动趋势”的联合判断把静态几何约束变成了动态风险评估。3.3 碰撞时间筛选与目标置信度的融合使用车道相关性初筛完成之后剩下的目标数量少了很多但还不能直接全部送入决策模块。在高速场景下前面两三百米内仍然会有不少目标。这里我需要引入一个更细粒度的优先级排序碰撞时间TTC。碰撞时间的计算不复杂TTC 相对纵向距离 / 相对纵向速度。前提是把目标速度换算成纵向相对速度。注意雷达给的是径向速度跟纵向速度有个夹角差计算的时候要做一次投影。举例来说自车以36米/秒约130km/h巡航前方有一个目标以33米/秒约119km/h同向行驶那么相对纵向速度为3米/秒此时如果相对距离是60米TTC就是20秒风险并不高。但如果前方是一个静止目标比如事故车辆相对速度变成36米/秒TTC就只有大约1.7秒这是极高风险的场景必须立刻进入决策模块的候选列表。我设置了多级TTC阈值TTC低于5秒的目标标记为最高优先级TTC在5秒到15秒之间的标记为普通关注TTC大于15秒或为非关键车道目标直接候选剔除。这里有个细节不能只依赖单帧TTC。雷达目标存在速度波动单帧速度噪声对TTC计算影响很大。我用的是连续多帧TTC值的加权平滑降低单帧波动的影响。这也是为什么第4步“生命周期管理”要放在筛选循环里反复迭代的原因。其实TTC这个指标放到决策逻辑里时还是要结合具体工况做标定的并不能一个阈值套所有场景。城区低速跟车场景5秒阈值会导致大量误触发我后面做场景适配时针对不同运行区域做了参数分组。4. 仿真环境搭建与实操过程4.1 为什么选CARLA Apollo的组合项目标题里带“仿真”两个字意味着我要在一个安全可控的环境里验证这套筛选策略。仿真平台可选方案不少有CARLA、LGSVL也有Apollo自带的仿真器。我最终选了CARLA和Apollo的组合原因是两个第一CARLA的场景灵活度高。它支持自定义地图、自定义传感器配置、恶劣天气模拟这对雷达数据的场景多样性覆盖很重要。我可以很方便地搭建“高速多车道带护栏”的场景也可以在雨雾天测试雷达跟踪数据质量的变化。第二Apollo提供完整决策规划链路。我只需要在Apollo的感知和规划之间做定制化开发不需要从零构建一整套决策框架。毕竟项目的目标不是重新发明轮子而是在现有决策目标筛选框架中验证融合策略的有效性。在传感器配置上我用的是CARLA中模拟的毫米波雷达模型。CARLA的雷达传感器输出的其实是点云数据仿真环境里不存在真正的多普勒速度所以我在雷达点迹基础上做了一个简单的目标跟踪器用卡尔曼滤波把点迹聚合成稳定目标列表。这一步模拟了真实车辆上雷达跟踪模块的输出。实际项目里要接真实雷达考量肯定不一样。真实雷达的跟踪器通常由雷达厂商提供或者自研输出格式会因为协议版本不同而不同。好在CARLA模拟出来的数据字段基本对齐了真实雷达跟踪输出代码层面可以直接平移到后续真机实测阶段。4.2 核心实现从雷达目标订阅到目标筛选输出整个软件链路我按模块拆解成了四层每一层都有清晰的输入输出接口第一层是数据接入层。订阅CARLA仿真器里雷达传感器输出的目标列表同时订阅Apollo感知模块输出的障碍物列表并把两者的时间戳对齐。第二层是坐标转换层。将雷达目标从雷达局部坐标转换到自车坐标再进一步投影到Frenet坐标。坐标转换要用到车辆定位信息如果定位不准投影结果会跟着偏。第三层是筛选策略层。把前面说的车道关联、TTC计算、生命周期管理统一算一遍。第四层是输出适配层。把筛选后的目标转换成Apollo决策模块的障碍物格式注入到决策输入之前。代码上看起来是这种结构def filter_targets(radar_targets, reference_line, ego_state): frenet_targets [] for target in radar_targets: # 1. 坐标转换雷达局部坐标 - 自车坐标 - Frenet坐标 cartesian_pos radar_to_ego(target.position, ego_state) frenet_pos cartesian_to_frenet(cartesian_pos, reference_line) # 2. 车道相关性判断含横向速度趋势 lateral_velocity compute_lateral_velocity(target, ego_state) is_relevant is_in_relevant_lane(frenet_pos, lateral_velocity) if not is_relevant: continue # 3. 碰撞风险筛选 ttc compute_ttc(frenet_pos, target.velocity, ego_state.speed) priority get_priority_from_ttc(ttc) # 4. 生命周期状态更新 epoch lifecycle.update(target.id, priority) if epoch ACTIVE and priority ! LOW: frenet_targets.append(target) return frenet_targets这段代码不是项目完整代码但结构上已经把整套策略的骨架体现出来了。实际工程里reference_line是来自规划模块查出来的参考线ego_state里包含了自车的位置、速度和航向。4.3 关键参数与调参验证过程我直接把一组在仿真验证里调定的核心参数贴出来不代表所有场景适用但可以作为首发初始值参数取值说明车道宽度3.8m仿真地图实际宽度车道相关扩展宽度1.0m在车道边界上向外扩展的相关带横向速度切入阈值1.2m/s横向速度大于该值认为有变道趋势TTC高优先级阈值5s低于5s标记为最高优先级必须保留TTC普通关注阈值15s5s-15s之间标记为普通关注目标激活帧数3帧连续3帧观测到目标才激活进入决策列表目标消亡帧数5帧连续5帧丢失才从决策列表删除参数调试的过程其实就是一个不断用仿真场景压测的过程。我先搭了一个直线高速场景左右两边放护栏同一车道前后放静止事故车和慢速车。跑下来看筛选结果核心目标是保留正确率要高误筛率要低。调参的时候我会关注两个指标筛选精确率指最终进决策列表的目标里真正有决策意义的目标占比筛选召回率指场景里真正有关键目标被正确选出的比例。这两个指标在很多场景是此消彼长的调参就是找平衡点。4.4 一个值得注意的时序问题帧对齐与延迟补偿仿真里面还有个看不见但始终存在的隐形难题——时序。雷达目标的时间戳和视觉目标的时间戳几乎不可能精确对齐。CARLA仿真器里雷达传感器的发布频率通常在10Hz到20Hz而Apollo感知模块的输出可能跟着主循环走两者之间会有几十毫秒的相位差。如果忽略这个相位差照常做融合会出现目标位置在横向上的系统性偏差。自动驾驶时速36米/秒时50毫秒时间差就对应1.8米的空间错位足够让一个L2级辅助驾驶系统做出错误判断。我的处理办法是引入时间滤波在筛选策略内部维护一个小的状态缓存把每个雷达目标最近的几帧信息保存下来当视觉目标到来时用时间戳差值做插值预测出当前时刻雷达目标的位置。这个操作在工程上叫时间对齐是融合算法能不能稳定工作的基础。这个时序问题很多做融合前期调试的同行第一版代码里都忽略了直到目标位置对不上才回头来加。我建议你从第一天就把时间对齐纳入代码架构不要等到后面再补后面再补的成本高得多。5. 常见问题与排查技巧实录5.1 问题一目标闪烁导致的决策模块“神经质”我遇到的最早一个问题是目标闪烁。高速场景下雷达目标在连续帧之间时有时无导致筛选后的目标列表随之忽长忽短决策模块经常性做出撤销、重建、再撤销的判断车辆行为很不稳定。排查发现根本原因不在筛选策略而在雷达目标自身的置信度波动。雷达跟踪器输出的存在概率prob_exist在边缘场景下会在0.3到0.7之间跳我初版筛选逻辑直接把prob_exist低于0.5的目标删掉结果目标就在删除和恢复之间反复横跳。解决方案是把单帧硬判决改成多帧软判决。不直接删目标而是给目标累计一个消失帧计数器只有连续消失超过5帧才真正移除。同理目标激活也设了连续3帧的观测门限。这就是前面生命周期管理的由来。经过这样处理目标闪烁带来的决策抖动几乎消失。5.2 问题二慢速目标被TTC误杀第二个问题是低速目标误筛。有次跑城市道路场景一辆自行车在前面大概20米远的右侧车道缓缓移动。按TTC来算相对速度很低TTC很大被标记为低优先级直接筛掉了。但事实上这个目标很快就因为红绿灯变成横穿轨迹决策模块没有及时看到它造成潜在风险。这让我认识到TTC这个指标天然偏向“纵向追尾风险”对侧向冲突和路口目标关照不够。修复的办法是增加一个目标轨迹意图判断如果一个目标虽然TTC大但预测轨迹与本车行驶轨迹存在交点也就是存在侧向碰撞风险那也应该提升优先级。实际实现时我用的是基于目标横向位置和横向速度的轨迹外推预测未来1秒内目标的横向位移。如果预测结果显示目标会进入本车车道边界内就把它的优先级强制提升到最高级别不受TTC阈值限制。5.3 问题三多目标场景下的雷达目标ID切换这个坑是在一个多车并行场景下遇到的同时有五六个目标在本车周围其中两个目标短暂并行了十来秒。我注意到雷达跟踪器给他们分配的ID有时候会互相交换。直观表现是筛选策略好不容易给目标A积累了三帧历史突然那个track ID换了一个来源——视觉目标的历史轨迹就跟丢了而筛出的候选还带着错误的关联关系。ID切换的本质是雷达跟踪器在数据关联阶段产生了歧义。两个目标在雷达视场里靠得太近且运动模式相似跟踪器很可能把目标A的观测点关联到了目标B的轨迹上。解决思路上不能指望雷达跟踪器百分百不犯错而是要在筛选策略里接受ID可能变动的现实。我的做法是在筛选输出层增加一个“感知身份指纹”的概念除了目标ID之外把目标的位置范围、速度值、RCS值组成一个签名特征。当ID发生偏移时用签名特征重新关联历史状态。虽然不能完全消除ID切换的影响但会把切换带来的决策抖动降低很多。5.4 一个容易忽略的场景强反射干扰雷达有一个天生的软肋是金属强反射体比如货车侧面、隧道壁、高架下的金属梁。这类物体反射强度远超常规车辆可能会在雷达图像上形成一条“反射带”甚至让跟踪器在短时间内生成多个鬼影目标或者拉偏同一目标的测量位置。一次在高架路场景里我同时收到三辆物流车并排行驶的数据雷达目标列表里却出现了一大片反射点密集区跟踪器生成的临时目标数量增加了接近一倍。大量鬼影目标通过车道筛选后进入到候选列表决策模块开始频繁误判前方有密集障碍。排查之后发现是强反射造成的互反射和反射镜像误差。在策略层面我的临时对策是增加一个RCS阈值判断对于RCS异常高且尺寸飘忽不定的目标降低它的优先级不作为最高优先级决策参考。后续如果是真实道路场景再考虑结合视觉确认有效过滤强反射带来的轨道起伏。这里需要说明的是这种启发式算法只能缓解问题真正的解法还是要在感知层做点云质量和跟踪关联加固。6. 一点经验上的收尾这套融合毫米波雷达跟踪结果的决策目标筛选策略做下来前后大概花了一个多月的时间。其中最花时间也最磨人的可能不是我预想中的策略逻辑设计而是数据口径整理和参数反复调试。有个经验我特别想分享仿真里能复现的问题到了真实系统里往往只多不少。你在CARLA里跑出来的场景已经比真实道路简单很多了。仿真环境里雷达的数据格式干净、时间戳稳定、没有通信延迟真实车上则伴随着信号遮挡、多径、丢帧还有各种线束和驱动层的特殊波动。所以建议做类似项目的朋友尽量在设计阶段就把策略的容错能力做好。另外一个感受是决策目标筛选这种模块没有标准的评估体系很多效果只能靠场景来验证。我在项目里积累了一批仿真用例包括高速切入、慢速横穿、护栏强反射、多目标并行这些经典场景。每次改完策略参数就全部回归跑一遍。这套回归用例的价值甚至超过了策略本身。关于后续扩展方向我认为这套筛选策略做完了可以考虑把模型往端到端方向迁移或者把策略决定的过程从规则换成强化学习让它学会在复杂场景下自动调节阈值。不过这些都是后话先把手头的规则策略调稳了再谈算法升级。做自动驾驶这类系统工程稳定可靠永远是第一位的。