结论:radard 把硬件雷达轨迹与视觉前车融合成两个前车,但它的匹配是「先选最高分、再验合理性门」:一旦最高分目标被距离或速度门拒绝,不会改试次高分目标,即使后者能通过。它依赖三个固定几何/速度常量——距离减 1.52 m 的相机-雷达偏置、距离差门 max(视觉距离×0.25, 5 m)、速度门「差 <10 m/s 或雷达前车绝对速度 >3 m/s」。自制安装位姿或坐标符号不同时,不能只改 CAN 解码而不重验这些常量。以下基于源码快照 c8fb906,未做回放或台架测试;这些阈值是该提交的实现,不等于传感器硬件规格。
消息链与调度:20 Hz 与可能的缓存复用
card 在雷达接口返回数据时发布 radarTracks;若雷达错误结构有任一真值,顶层 valid=False,但错误与点仍在消息内。radard.main() 等待 CarParams,以 modelV2 为 SubMaster 轮询主题,读取 radarTracks/carState 后调用 RadarD.update() 与 publish()。这里有个细节:循环没有再用 sm.updated['modelV2'] 包住更新,因此轮询超时也可能沿用缓存数据再发 radarState,不能把每条输出都当作一帧新视觉预测。
组件与关键函数
| 组件/函数 | 实现功能 | 边界 |
|---|---|---|
KalmanParams |
根据固定表插值 KF1D 增益,建立速度/加速度模型 | 构造要求雷达步长严格在 (0.01,0.2) s;实际传入 DT_MDL,不是逐帧读取雷达硬件时间差 |
Track.update() |
保存相对距离/横向位置/相对速度,并更新前车绝对速度及加速度滤波 | 新 track 第一帧不做 kf.update();track ID 消失即删除对象,重现会重置滤波历史 |
Track.potential_low_speed_lead() |
判定低速近距离同车道候选 | 要求 vEgo<4 m/s、|yRel|<1 m、0.75<dRel<25 m;不依赖视觉概率 |
match_vision_to_track() |
对每条雷达轨迹算纵向/横向/速度拉普拉斯评分并取最大者 | 选中后还查距离与速度合理性;最高分也可能被拒绝 |
get_lead() |
优先匹配雷达;高视觉概率但无匹配时回退视觉;可再被低速雷达目标覆盖 | leadOne 启用低速覆盖,leadTwo 不启用;视觉入门为滤波概率 >0.5 |
RadarD.update() |
对齐车速、维护轨迹 ID、平滑视觉概率、组装两个前车和错误 | 即使输入 sm.all_checks() 不通过,仍执行轨迹/前车组包 |
几何、速度与来源选择
RadarD.__init__() 用 CP.radarDelay 构造自车速度历史队列;RadarD.update() 仅在 carState 接收帧变化时推进队列,以历史首元素加雷达相对速度得到前车速度。原类方法探针确认:0.1 s 延迟配置下队列在启动时先含 0,未收到新 carState 帧即使当前对象的 vEgo 改变也不推进;第三个不同接收帧才取到此前的非零车速。这是按接收帧计数的延迟,不是按每个雷达点的硬件时间戳做精确插值。radarTracks.points 按 trackId 转字典,本轮未出现的 ID 被删;若上游对同一物体频繁换 ID,速度/加速度估计会反复从初值开始。
match_vision_to_track() 把视觉距离减固定 RADAR_TO_CAMERA=1.52 m,横向位置取负号,速度比较 vRel+vEgo 与视觉预测 lead.v[0]。评分尺度使用模型给的 xStd/yStd/vStd,但最终还要求距离差小于 max(视觉距离×0.25, 5 m),速度差 <10 m/s 或雷达前车绝对速度 >3 m/s。探针确认:速度差即使很大,只要雷达前车绝对速度严格大于 3 m/s,速度合理性门仍放行;且它先选最高分再验门,最高分目标被拒时不会改试次高分目标。
| 常量/门 | 数值 | 作用 |
|---|---|---|
RADAR_TO_CAMERA |
1.52 m |
视觉距离减去的固定相机-雷达偏置 |
| 距离合理性门 | max(视觉距离×0.25, 5 m) |
限制视觉与雷达的距离不一致幅度 |
| 速度合理性门 | 差 <10 m/s 或绝对速度 >3 m/s |
后者会大幅放行不匹配的速度差 |
| 低速覆盖门槛 | vEgo<4 m/s、|yRel|<1 m、0.75<dRel<25 m |
仅 leadOne 启用,等距不覆盖 |
有效性、故障与下游
RadarD.update() 的顶层有效位取 sm.all_checks(),包含 modelV2、carState、radarTracks 的存活/频率/消息有效检查。ready 只取 sm.seen['modelV2'],不是「当前模型仍新鲜有效」的证明;radarErrors 直接来自上游。探针确认:输入检查失败时仍能组装 leadOne.present=True,而 publish() 将顶层 valid=False 随前车和错误一起交给发布器——硬件错误不应靠 present 掩盖。
模型概率采用上升立即、下降经一阶滤波的非对称策略:下降按 dt=0.05 s、rc=0.2 s 逐次衰减,探针从 0.8 突降到 0 时观察到 0.64→0.512→0.4096,因此低于原始视觉阈值后的两轮仍可保留视觉前车。若 leadsV3 少于两条,原函数会跳过两个前车字段的赋值。下游 selfdrived 在允许纵向控制的车型上,把 canError、临时不可用与其它雷达错误分别映射为事件;LongitudinalPlanner.update() 把 radarState 交给 MPC,计划输出的 hasLead 使用首个前车的 present。雷达数据最终仍要过计划、使能状态机、车辆控制限幅与独立执行网关——radard 不直接发 CAN。
插件化时应保留什么
雷达输入插件应先只读地影子发布候选轨迹,保留唯一且不复用的 ID、硬件时间戳、雷达延迟、车辆坐标与安装偏置、单位及四种错误位。权威 radarTracks 与 radarState 各只能有一个生产者;输入边界须拒绝或隔离 NaN/Inf,缺席、无效或过期轨迹须明确失效,不得用最近一次 present=True 无限延续。ID 连续性会影响滤波历史,候选排序与合理性门的组合也会影响回退,因此设备跟踪策略与前车选择语义都属于插件契约。应按路段回放、误报/漏报故障注入与目标车型 SIL/HIL 顺序验证,再考虑上车。
常见问题
视觉前车和雷达前车匹配不上时会怎样?
get_lead() 优先匹配硬件雷达;高视觉概率但无匹配时回退视觉前车,标 radar=False、radarTrackId=-1,之后还可能被低速雷达目标覆盖。但仅 leadOne 启用低速覆盖,leadTwo 不启用。
为什么最高分匹配目标被拒绝后,不会换下一个?
因为函数先对每条轨迹评分并取最大者,再验证距离与速度合理性门;一旦最高分目标被拒,实现不会改试次高分目标,即使它能通过门。这是该提交的确定性行为。
radarState 的顶层 valid 和前车 present 是一回事吗?
不是。valid 取输入检查的聚合结果,present 与 radarErrors 各自独立。探针确认输入检查失败时仍能组装 leadOne.present=True,由发布器带着 valid=False 一起发出。
低速下第一个前车一定会被雷达覆盖吗?
不一定。低速候选要求 vEgo<4 m/s、|yRel|<1 m、0.75<dRel<25 m。首个前车即使 ready=False 也可被选中,但已有前车时只允许更近的低速点覆盖,等距不覆盖,此时 modelProb 会回到默认 0。
延伸阅读
- 传感器融合的健康位 —— 融合输出为什么要把「数据有效」与「目标存在」分开
- 车载信号为什么会说谎:TTL、源时间与 500 ms 新鲜度阈值 —— 接收帧计数延迟与时间戳新鲜度
- 横向延迟估计 —— 另一处按帧对齐与延迟补偿的工程取舍
- 从使能到车轮:执行链上的八道门,以及它们各自不保证什么 —— 为什么
radard不直接发 CAN
参考资料
- 项目 02 ——
openpilot-research/RADARD_LEAD_FUSION_DEEP_DIVE.md(雷达轨迹与视觉前车融合,含源码行号索引) - 项目 02 ——
openpilot-research/probes/test_radar_lead_selection_contract.py(坐标/速度匹配与回退的隔离探针) - 项目 02 ——
openpilot-research/PLUGIN_REFACTOR.md(源文档指向的插件化蓝图)
说明:本文为对固定源码快照的静态结构分析,未运行完整测试套件、未连接硬件、未验证目标车型的真实参数组合。文中所有阈值、行号索引与配置均出自源文档,标注为未验证的结论保留其原有边界。
更新记录
首次发布 2026-10-11 最近核对:2026-10-11