5.2 多传感器数据融合

5.2 多传感器数据融合

本节摘要：单个传感器各有盲区，融合的精髓是让不同传感器在各自擅长的时段说标准话。本节先给滤波器选型四分法（滑动平均、中值、一阶低通、卡尔曼各管什么噪声），再以姿态解算为完整案例讲透互补滤波——为什么加速度计信得过低频、陀螺仪信得过高频，以及卡尔曼滤波如何把这个"分段信任"变成连续的自动调节。

5.1 节保证了单路信号采得准；但准的单路信号依然有盲区——加速度计静止时可信、运动时被惯性加速度污染；陀螺仪积分姿态短期极稳、长期漂移。融合不是"多买几个传感器求平均"，而是按噪声的频段与场景分配信任。这一节放在链路中段：它吃 5.1 的干净单源数据，产出 5.3 决策需要的可信估计。

滤波器选型四分法

滤波不是玄学，先给噪声画像再选工具。四种最常用的数字滤波各管一类噪声：

选型口诀：毛刺用中值，抖动用低通，两样都有先中值后低通——这个组合几乎覆盖 80% 的场景，卡尔曼留给"系统有明确运动模型、且噪声统计特性已知"的场合，别把它是万金油。

图：三种典型噪声与各自的克星

互补滤波：把两个缺陷拼成一个优点

姿态估计是融合的教科书案例。加速度计测重力方向：静止时给出准确的倾角，但设备一动，运动加速度混进来，读数瞬间失真——它擅长低频（静态），害怕高频（动态）。陀螺仪测角速度，积分得角度：运动再剧烈也不受惯性加速度影响，但零偏经积分持续累积，几分钟后漂出量程——它擅长高频（动态），害怕低频（长期）。

两者的缺陷恰好互补，互补滤波用一行公式完成拼接：

// 一阶互补滤波：融合加速度计与陀螺仪的俯仰角
float pitch = 0.0f;

void fuse(float gyroRateDeg, float accelAngleDeg, float dt) {
  const float alpha = 0.98f;                 // 高通给陀螺仪的权重
  pitch = alpha * (pitch + gyroRateDeg * dt)  // 高频段：信陀螺仪
        + (1.0f - alpha) * accelAngleDeg;     // 低频段：信加速度计
}
​

alpha 取 0.98 的含义是：这一帧的角度 98% 来自陀螺仪积分（管动态）、2% 来自加速度计校正（管漂移）。加速度计的"慢而准"不断把陀螺仪的漂移往回拉，陀螺仪的"快而稳"不断替加速度计挡住运动噪声。系数怎么定？看应用对哪个失真更敏感：姿态显示取 0.95 到 0.98，自平衡车取 0.99 以上——倒了的代价远大于漂移。

卡尔曼滤波：把"分段信任"变成"自动调节"

互补滤波的 alpha 是拍脑袋定的常数；卡尔曼滤波做的是同一件事，但 alpha 不再是常数——它按每一帧的噪声实测自动调节：加速度计这帧抖得厉害就多信陀螺仪，陀螺仪这帧漂得多就多信加速度计。这是一维卡尔曼的直觉版：预测（陀螺仪积分）、观测（加速度计）、按双方方差加权，五个公式完成一次信任分配。

微控制器上不必敬畏它。一维标量的卡尔曼就是十几行代码加几个需整定的参数（过程噪声、观测噪声），整定的思路也朴素：滤波结果追得太紧就加大观测噪声，太迟钝就减小。真正该警惕的是多维状态加协方差矩阵的完整版——矩阵运算在无浮点的 AVR 上可能占掉整个算力预算，此时互补滤波的性价比碾压一切。

案例：云台稳像的滤波链实战

背景：相机云台要输出平滑的俯仰角驱动电机，原始数据是 1kHz 陀螺仪加 100Hz 加速度计。需求：稳像延迟低于 20 毫秒，长时间不漂。

操作：第一步分频段处理——陀螺仪 1kHz 全速积分，保证动态响应；加速度计 100Hz 低通后参与校正。第二步互补滤波融合，alpha 取 0.992（云台怕抖不怕漂）。第三步对融合输出再串一个轻微的一阶低通压残余高频，截止频率取 30 赫兹——低于电机谐振点。第四步加"零速校正"：检测到连续 2 秒无运动时，把加速度计均值直接写入角度（静止时它最准），漂移被周期性归零。

结果：静止漂移从每小时 4 度降到 0.2 度，动态延迟实测 8 毫秒，稳像画面无"果冻感"。

解读：这条链路的每个环节都对应本节一个知识点：分频段是信任分配，互补是融合骨架，末级低通是给执行器留余量，零速校正是利用"静止时加速度计王座"的场景知识。融合设计的实质是把应用约束翻译成滤波结构——没有万能参数，只有贴着需求的组合。

变式：三轴姿态（加偏航角）时互补滤波不够用——偏航没有重力可参照，需要磁力计加入构成航向参考，矩阵化的姿态估计（马哈尼滤波或四元数卡尔曼）登场。那是另一层复杂度，方法论的根还是本节的"按时段分配信任"。

⚠️ 常见坑：融合前不做时间对齐。1kHz 的陀螺仪与 100Hz 的加速度计数据错开一帧融合，动态下姿态误差可达数度——每个传感器打上自己的采样时刻戳，融合时按最近时刻配对，是融合代码的第一行纪律。

💡 关键直觉：每个传感器都有"它说这话时最有底气的场合"。融合不是平均，是排班表——把每个传感器排进它最有底气的那个时段。

滤波参数的整定套路

选型定了，参数怎么定？三个常用套路。滑动平均窗口：窗口长度除以采样率等于等效延迟，按"应用能容忍多少延迟"倒推——人机界面容忍 100 毫秒，控制环只容忍一两个周期。一阶低通系数：alpha 约等于采样间隔除以时间常数，想要"抑制抖动但跟得上 1 秒级变化"，时间常数取 0.3 到 0.5 秒起步，再按实测响应微调。互补滤波权重：先看陀螺仪漂移速率——每小时漂 2 度的器件，校正强度要保证漂移在两次校正间不累积出可见误差；再看加速度计受扰程度——运动越剧烈，alpha 越往 1 推。

整定的通用心法：一次只动一个参数，动完立刻看阶跃响应——给系统一个已知的真实变化（比如把设备从桌面拿起），观察输出追得是否及时、稳得是否干净。参数整定没有玄学，只有"改一个、看一次"的笨功夫；把每次改动的参数与响应记录下来，两三轮之后你对该平台的参数直觉就有了。

本节要点回顾

• 先画像再选型：白噪用平均低通、尖刺用中值、漂移靠融合校准，滤波管不了系统性偏差；
• 互补滤波一行公式：高频信陀螺仪、低信加速度计，系数按应用敏感点定；
• 卡尔曼是自动调 alpha 的互补滤波，一维版平易近人，多维版先算算力账；
• 时间对齐是融合第一纪律，错帧融合的误差比不融合还大；
• 融合设计就是把应用约束翻译成滤波结构，没有万能参数。