星核火花赛车科技

标签： 传感器融合

  • locationd 的四种健康位：20 Hz 输出为什么不能压缩成一个布尔值

    结论：locationd 的健康状态是四个独立维度——deviceMotion.valid、inputsOK、sensorsOK、posenetOK，它们不能压缩成一个「正常」布尔值。顶层 valid 只等于 filter_initialized && isfinite(kf.t)，不直接包含另外三项；而 filter_initialized 一旦置真，不会在消息异常时自动变回假。名义频率下，约第 2 个相机坏输入或第 10 个 IMU 坏输入才会让 critical_service_inputs_valid 变假。

    调度与启动门：先看名义频率

    main() 以 cameraOdometry 为 SubMaster 轮询主题，另开独立 socket 批量读取 IMU 消息。服务表配置 加速度计/陀螺仪名义 104 Hz，相机里程计和 deviceMotion 20 Hz。

    主题 名义频率 作用
    加速度计 / 陀螺仪 104 Hz IMU 观测源，经独立 socket 批量读取
    相机里程计 20 Hz 轮询主题，同时触发 deviceMotion 发布
    deviceMotion 20 Hz 仅在 sm.updated['cameraOdometry'] 时发布

    启动门的细节很关键：初始化阶段只在 sm.all_checks() 与 sensor_all_checks() 同时满足时把 filter_initialized 置真；之后此布尔值不会在消息异常时自动变回假。输出侧，deviceMotion 仅在 sm.updated['cameraOdometry'] 时发布——单有 IMU 更新不触发输出。

    循环把 IMU 与 SubMaster 更新的消息按 logMonoTime 排序，仅把顶层 valid=True 的消息交给 handle_log()。这是当前批次内排序，不能保证跨批次全局严格有序；历史观测由滤波器最大回绕窗口约束。进程还可读取持久的 LocationFilterInitialState 作为初始状态/协方差来源，但本快照常规运行代码未定位到写入该键的对应路径——不能假定每次启动都有历史姿态。

    十八维状态与四类观测

    States 把状态分为 NED 姿态、设备速度、角速度、陀螺偏置、设备加速度、加计偏置，共 18 维。generate_code() 定义姿态旋转、速度积分以及 IMU/相机四类观测方程，运行时通过 rednose 生成的 EKF 执行——不是简单对四种速度取平均。四类观测的门各不相同：

    观测 生产条件与变换 拒绝 / 降权边界
    加速度计 msg.which() == acceleration，硬件数组重排取负为 [-v[2], -v[1], -v[0]]，用传感器纳秒时间戳 时间戳不能为零，与 logMonoTime 差严格不超过 0.1 秒，不能早于滤波状态 0.8 秒以上；bmx055 作为第二 IMU 源被忽略；范数须 <100 m/s²
    陀螺仪 同样重排取负，先减滤波器陀螺偏置，再与相机偏航率分布交叉比较 除时间/来源门外，角速度范数须 <10 rad/s，偏航差须低于相机偏航标准差的 30 倍；只有通过才作为 PHONE_GYRO 观测
    相机旋转 以 timestampEof - 0.1 秒作为视觉姿态时刻，用 device_from_calib 旋到设备系 受 0.8 秒回绕门；旋转范数 ≤10 rad/s；标准差各分量须 >1e-5，范数不可超对应上限的 10 倍；入滤波前标准差乘 10
    相机平移 与旋转同一延迟时间和标定旋转；视觉平移写入 CAMERA_ODO_TRANSLATION 平移范数 ≤200 m/s；标准差须 >1e-5 且范数不超过 10*200，入滤波前标准差乘 4；方差旋到设备系后给 EKF

    两处时间语义差异必须记住：carState 在此只更新 abs(vEgo) 供 posenetOK 门使用，不是这只姿态滤波器的直接速度观测；extrinsicsCalibration 在 rpyCalib 非空且分量位于 ±0.5 rad 内时更新 device_from_calib。更关键的是：相机观测没有像 IMU 那样检查自身时间戳与 logMonoTime 的 0.1 秒差，而是按 timestampEof-0.1 与滤波器时刻比较——新硬件不能把两种时间语义混为一谈。

    坏输入计数：第 2 个相机、第 10 个 IMU

    HandleLogResult 区分成功、时间非法、值非法和传感器来源非法。主循环只对关键主题的时间/值非法增加坏输入计数，成功时乘以按频率计算的 calculate_invalid_input_decay()；SENSOR_SOURCE_INVALID 不进入这两个分支。

    名义频率下，相机 input_invalid_limit=2、IMU 各为 round(2*104/20)=10，输出门使用比它们各低 0.5 的阈值——所以约第 2 个相机坏输入或第 10 个 IMU 坏输入，才会让 critical_service_inputs_valid=False。源文档特别澄清：实际恢复取决于后续成功样本数量和消息速率，不是固定墙上 10 秒。

    四种健康含义各不相同

    输出字段 源码计算 不代表什么
    deviceMotion.valid filter_initialized && isfinite(kf.t)；同样设置四组 XYZMeasurement.valid 不直接包含当前 inputsOK/sensorsOK/posenetOK；某传感器掉线时顶层 valid 不一定同步变假
    inputsOK sm.all_valid() 与关键观测坏输入计数门同时成立 all_valid() 只检查消息有效位，不等于全部主题当前 alive/频率正常
    sensorsOK 对两路 IMU 各保存最近消息的 valid 位；非仿真要求距接收单调时间 <0.1 秒 不等于加速度/陀螺数值一定通过 handle_log() 的物理合理性与时间检查
    posenetOK 对相机平移标准差的前/后各 20 项均值作比，后均值是前均值 4 倍以上且 >7、车速 >5 m/s 时置假（低速放行） 它是模型运动估计的波动门，不是整条视觉链的真实性证明

    复位逻辑同样要分清：_finite_check() 在滤波返回的状态或协方差出现非有限值时调用 reset(t)；但它不自动重置 filter_initialized 或所有输入计数。上游 selfdrived 对 posenetOK/inputsOK 另产生事件，controlsd 消费设备姿态与车辆参数——健康位与事件门是分层契约。

    测试与硬件插件边界

    test_locationd_scenarios.py 使用路段回放构造 IMU 缺失、数值峰值和 150 ms 时间偏移场景；其测试依赖路段数据下载/缓存，本轮未执行，不据测试文件存在而宣称当前设备已验证。

    自制 IMU/相机插件至少要保持轴序、符号、单位、硬件时间戳、logMonoTime 对齐、噪声标准差及顶层 valid 语义；对异步/乱序观测要验证 0.1 秒传感器门、0.8 秒回绕窗口和视觉 0.1 秒模型延迟。算法层允许回退或过滤观测，独立执行器安全限制仍在 panda 固件。

    常见问题

    顶层 valid 为真，能否说明所有传感器都正常？

    不能。deviceMotion.valid 只等于 filter_initialized && isfinite(kf.t)，不直接包含 inputsOK/sensorsOK/posenetOK。由于 filter_initialized 一旦置真不会在消息异常时自动变回假，首次初始化后某传感器掉线，顶层 valid 不一定同步变假。

    为什么相机和 IMU 的坏输入阈值差这么多？

    因为阈值按名义频率换算：相机 20 Hz 对应 input_invalid_limit=2，IMU 104 Hz 对应 round(2*104/20)=10，输出门再各低 0.5。所以约第 2 个相机坏输入或第 10 个 IMU 坏输入才触发，恢复则取决于后续成功样本数量和消息速率，不是固定时长。

    posenetOK 变假是否等于视觉定位失效？

    不等于。它是对相机平移标准差前后各 20 项均值作比得到的波动门（后半均值是前半 4 倍以上且 >7、车速 >5 m/s 时置假，低速放行），只反映模型运动估计的波动，不是整条视觉链的真实性证明。

    相机观测和 IMU 观测的时间门一样吗？

    不一样。IMU 检查自身时间戳不能为零、与 logMonoTime 差严格不超过 0.1 秒、且不早于滤波状态 0.8 秒以上；相机观测没有检查自身时间戳与该 0.1 秒差，而是按 timestampEof-0.1 与滤波器时刻比较。两种时间语义不能混用。

    延伸阅读

    参考资料

    • 项目 02 —— openpilot-research/LOCATIOND_SENSOR_FUSION_HEALTH_DEEP_DIVE.md（取用了 18 维状态、四类观测门、坏输入计数与四种健康含义）。
    • 项目 02 —— openpilot-research/CAR_PROTOCOL_IMPLEMENTATIONS.md（仅用于确认 selfdrived/controlsd 与健康位事件的分层关系）。

    说明：本文为对固定源码快照的静态结构分析，未运行完整测试套件、未连接硬件、未验证目标车型的真实参数组合。文中所有阈值、行号索引与配置均出自源文档，标注为未验证的结论保留其原有边界。

    更新记录

    首次发布 2026-10-11 最近核对：2026-10-11

  • 1.52 米偏置与 3 m/s 门：雷达轨迹与视觉前车的融合边界

    结论：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。

    延伸阅读

    参考资料

    • 项目 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