Skip to content
机器人系统与感知阅读约 6 分钟
Sensor FusionTime SyncOutlier GatingRobotics

梳理高频 IMU 与低频相机/毫米波雷达在异步采样、传输延迟与外参标定下的时空对齐框架,以及基于卡方检验的野值门控机制。

多传感器时空对齐与异步融合流水线设计 ​

在真实机器人或自动驾驶边缘平台上,不同传感器的工作频率、曝光时间与总线传输延迟往往完全不同:IMU 通常以 200 Hz – 1000 Hz 连续输出角速度与比力,而相机、激光雷达或毫米波雷达往往只有 10 Hz – 30 Hz,且存在数十毫秒的驱动层延迟。

如果直接把“当前时刻刚收到的一帧雷达观测”硬塞进“当前时刻的 IMU 滤波状态”,就会引入严重的时间错位误差(Time-Offset Error)。

异步融合流水线结构 ​

工程上最稳健的做法是维护一个定长的滑动历史状态环形缓冲区(Ring Buffer),在高频传播的同时支持延迟观测的回滚重传播(Out-of-Sequence Measurement, OOSM):

Asynchronous Sensor Fusion Pipeline
高频 IMU (400 Hz)状态预测 / 预积分雷达 / 相机 (20 Hz)硬件时间戳 + 延迟 τ状态环形缓冲区1. 按 t_obs 查找历史状态2. 卡方门控剔除野值3. 更新后向重传播至 t_now外参空间对齐T_base_sensor · SE(3)平滑状态x̂_now, P_now

三个关键工程细节 ​

  1. 硬件时间同步优先于软件打标:尽可能使用 PPS 秒脉冲或硬件触发信号统一各传感器时钟源;若仅能依赖主机接收时间,需在状态向量中增广一个慢变的时间偏移量 td 进行在线联合标定。
  2. 基于马氏距离的卡方检验门控(χ² Gating): 多传感器环境下,雷达多径鬼影或视觉误匹配会导致瞬间野值(Outlier)。在执行状态更新前,先计算新息向量 νk = zk − h(x̂k|k−1) 的归一化马氏距离平方:
Mahalanobis Innovation Gating
dk2 = νkT (Hk Pk|k−1 HkT + Rk)−1 νk < χα, m2(1) 99% 置信门控阈值

当 dk2 超出自由度为 m 的卡方分布阈值时,说明观测与先验严重冲突,应当降权(放大 Rk)或直接拒收该帧观测,防止单帧野值污染整条状态轨迹。 3. 平滑与响应的权衡:你可以通过下面的交互实验直观感受当观测噪声协方差 R 与过程噪声 Q 变化时,滤波器在面对阶跃突变时的收敛速度差异:

INTERACTIVE LAB · 1D KALMAN FILTER

过程噪声 Q 与观测噪声 R 对滤波增益 K 的动态影响

414233342k=0k=15k=30k=45k=59
步骤 k=59真实值 39.70观测值 39.45滤波估计 35.84增益 K 0.221
信任物理预测 (78%)信任传感器观测 (22%)
当前增益 K0.221
滤波 RMSE2.82
原始观测 RMSE2.05

讨论区开启了自动与人工双重审核,文明交流的评论会在短时间内展示。

Built with VitePress