交互式教学案例

无人机多传感器数据融合

深入理解加速度计与陀螺仪的信号特性，掌握互补滤波与卡尔曼滤波的核心原理

传感器基本原理

为什么需要数据融合？

在无人机飞行控制中，精确的姿态估计（俯仰角 Pitch、横滚角 Roll、偏航角 Yaw）是实现稳定飞行的基础。单一传感器都存在固有缺陷，因此需要将多个传感器的数据进行融合，取长补短。

拖动鼠标改变无人机姿态

θ = 0.0°

加速度计 Accelerometer

工作原理

基于 MEMS 技术，测量微小质量块在各轴方向上受到的惯性力（包括重力）。静止时测量重力加速度分量，从而计算出倾斜角度。

角度计算

θ acc = atan2(a y, a z) \times 180 / π

优势

长期稳定、无累积漂移，能提供绝对角度参考。

劣势

对振动和线性加速度非常敏感，短期噪声大，动态响应慢。

陀螺仪 Gyroscope

工作原理

基于科里奥利效应，测量角速度（°/s）。通过对角速度进行积分可以得到角度变化量。

角度计算

θ gyro (t) = θ gyro (t-1) + ω(t) \times Δt

优势

响应迅速、短期精度高，不受外力干扰。

劣势

积分累积误差（漂移），长期不可靠，存在零偏（bias）。

频域特性互补

这两个传感器的噪声特性恰好在频域上互补——这是数据融合的理论基础：

加速度计
低频可靠

陀螺仪
高频可靠

融合结果
全频段可靠

💡

核心直觉

加速度计像一个"长期记忆者"——慢慢变化的真实角度它记得很准，但容易被短期振动干扰；陀螺仪像一个"短期反应者"——快速运动它跟得很紧，但时间长了就会"忘记"起始位置（漂移）。融合就是让两者互相弥补。

信号与噪声特性

🔬 交互实验：观察原始传感器信号

下面的仿真展示了真实角度信号、加速度计测量值和陀螺仪积分值。调节参数观察不同噪声条件下的传感器表现。

加速度计噪声强度 0.50

陀螺仪漂移率 (°/s) 0.10

信号频率 (Hz) 0.20

传感器原始信号对比

真实角度

加速度计测量

陀螺仪积分

—

加速度计 RMSE

—

陀螺仪 RMSE

—

最终漂移量

⚠️

观察要点

注意加速度计信号（蓝色）围绕真实值上下剧烈抖动但不偏离——这是高频噪声；陀螺仪（粉色）短期平滑但逐渐偏离真实值——这是低频漂移。增大漂移率参数可以看到更明显的漂移效果。

低通滤波器与加速度计

为什么加速度计需要低通滤波？

加速度计的有用信息（重力方向 → 真实倾角）变化缓慢，属于低频分量。而振动、电机转动、空气扰动等引入的噪声主要是高频分量。低通滤波器可以保留低频有用信号，滤除高频噪声。

一阶低通滤波器（指数移动平均）

y(t) = α \cdot x(t) + (1 - α) \cdot y(t-1)

其中 α 是滤波系数，取值范围 [0, 1]。α 越小，滤波越强（越平滑），但延迟越大。

等效关系

α = Δt / (τ + Δt)，其中 τ = 1/(2π·f_c) 是时间常数，f_c 是截止频率

📐

物理直觉

可以把低通滤波器想象成一个"惰性系统"——它不愿意跟随快速变化，只愿意缓慢地跟踪信号。α越小，它就越"懒"，越不容易被高频噪声带偏，但也越跟不上真实的快速变化。

🎛️ 交互实验：调节低通滤波器参数

滤波系数 α 0.10

加速度计噪声 0.80

低通滤波效果

真实角度

加速度计原始（含噪声）

低通滤波后

—

原始 RMSE

—

滤波后 RMSE

—

估计延迟 (采样)

🔍

实验任务

① 将 α 从 0.01 逐步调到 1.0，观察滤波曲线如何从极度平滑变为完全跟随原始信号；② 找到一个使 RMSE 最小的 α 值——这就是噪声抑制与延迟之间的最优平衡点。

频率响应

低通滤波器的频率响应展示了不同频率分量的衰减程度。截止频率以下的信号基本通过，以上的信号被逐步抑制。

低通滤波器频率响应（幅度 vs 频率）

高通滤波器与陀螺仪

为什么陀螺仪需要高通滤波？

陀螺仪积分后的角度信号在短期内非常准确（高频分量可靠），但会累积低频漂移误差。高通滤波器可以保留高频有用信号，滤除低频漂移。

一阶高通滤波器

y(t) = (1 - α) \cdot [y(t-1) + x(t) - x(t-1)]

高通滤波器是低通滤波器的"镜像"——它让快速变化通过，阻止缓慢漂移。

📐

物理直觉

高通滤波器像一个"只关注变化"的观察者——它只对信号的快速变化感兴趣，对缓慢的偏移视而不见。这恰好能消除陀螺仪的积分漂移。

🎛️ 交互实验：调节高通滤波器参数

滤波系数 (1−α) 0.95

陀螺仪漂移率 0.20

高通滤波效果

真实角度

陀螺仪积分（含漂移）

高通滤波后

—

原始 RMSE

—

滤波后 RMSE

🔍

观察要点

注意高通滤波能有效消除漂移，但会丢失直流分量（绝对角度参考）。滤波后的信号围绕零值波动而非围绕真实角度——这就是为什么单独使用高通滤波的陀螺仪无法给出准确的绝对角度。

互补滤波器

互补滤波的核心思想

既然加速度计在低频段可靠、陀螺仪在高频段可靠，我们能不能用一个低通滤波器处理加速度计数据，用一个高通滤波器处理陀螺仪数据，然后把两者加在一起？答案是：可以！这就是互补滤波。

θ fused (t) = α \cdot θ acc (t) + (1 - α) \cdot [θ fused (t-1) + ω(t) \cdot Δt]

展开理解

低通部分：α · θ_acc(t) ← 加速度计提供长期参考
高通部分：(1−α) · [θ_prev + ω·Δt] ← 陀螺仪提供短期动态
权重之和：α + (1−α) = 1 ← 这就是"互补"的含义

① 读取传感器

→

② 陀螺仪积分

→

③ 加权融合

→

④ 输出角度

📖

步骤 ①：读取传感器数据

每个采样周期，从加速度计获取当前加速度值 (a_x, a_y, a_z)，从陀螺仪获取当前角速度 ω(t)。加速度计数据通过 atan2 计算出角度 θ_acc。

🎛️ 交互实验：互补滤波参数调节

调节 α 值观察融合效果。α 越大，越信任加速度计（抗漂移好，但噪声大）；α 越小，越信任陀螺仪（平滑好，但漂移大）。

互补滤波系数 α 0.05

加速度计噪声 0.80

陀螺仪漂移率 0.15

互补滤波融合结果

真实角度

加速度计

陀螺仪积分

互补滤波结果

—

加速度计 RMSE

—

陀螺仪 RMSE

—

融合 RMSE

—

RMSE 改善率

伪代码实现

// 互补滤波器 — 每个采样周期执行一次
const alpha = 0.05;  // 典型值 0.02~0.1
let angle = 0;

function complementaryFilter(acc_angle, gyro_rate, dt) {
  // 高通：陀螺仪积分提供短期动态
  const gyro_part = (1 - alpha) * (angle + gyro_rate * dt);
  // 低通：加速度计提供长期参考
  const acc_part = alpha * acc_angle;
  // 互补融合
  angle = gyro_part + acc_part;
  return angle;
}
    

卡尔曼滤波器

卡尔曼滤波的核心思想

卡尔曼滤波是一种最优状态估计器，它通过以下两步迭代过程来融合传感器数据：

预测（Predict）— 基于陀螺仪

利用系统动力学模型（陀螺仪数据）预测下一时刻的状态和不确定性。

状态预测：x̂⁻(t) = A \cdot x̂(t-1) + B \cdot u(t) 协方差预测：P⁻(t) = A \cdot P(t-1) \cdot Aᵀ + Q

其中 Q 是过程噪声协方差，表示对模型预测的不确定程度。Q 越大，表示越不信任陀螺仪的预测。

更新（Update）— 基于加速度计

利用观测数据（加速度计）修正预测值，根据噪声特性自动计算最优权重（卡尔曼增益）。

卡尔曼增益：K(t) = P⁻(t) \cdot Hᵀ \cdot (H \cdot P⁻(t) \cdot Hᵀ + R)⁻¹ 状态更新：x̂(t) = x̂⁻(t) + K(t) \cdot [z(t) - H \cdot x̂⁻(t)] 协方差更新：P(t) = (I - K\cdotH) \cdot P⁻(t)

其中 R 是测量噪声协方差，表示加速度计测量的不确定程度。R 越大，越不信任加速度计。

完整迭代流程

初始化 x̂, P

→

预测 x̂⁻, P⁻

→

计算 K

→

更新 x̂, P

→

循环 ↩

🎯

与互补滤波的关系

卡尔曼滤波可以看作"自适应的互补滤波"——互补滤波的 α 是手动固定的，而卡尔曼滤波的增益 K 是根据噪声统计特性自动最优计算的。当噪声参数恒定时，卡尔曼增益会收敛到一个常值，此时它等价于一个特定 α 值的互补滤波。

🎛️ 交互实验：卡尔曼滤波参数调节

Q 和 R 是卡尔曼滤波最关键的调参。理解它们的物理含义是掌握卡尔曼滤波的关键。

过程噪声 Q 0.010

测量噪声 R 0.500

加速度计噪声 0.80

陀螺仪漂移 0.15

卡尔曼滤波融合结果

真实角度

加速度计

陀螺仪积分

卡尔曼滤波

卡尔曼增益收敛过程

—

加速度计 RMSE

—

陀螺仪 RMSE

—

卡尔曼 RMSE

—

稳态增益 K∞

🔍

实验任务

① 固定 R，增大 Q：观察卡尔曼增益升高，滤波器更信任测量值（加速度计），结果更跟随加速度计但更noisy。② 固定 Q，增大 R：增益降低，更信任预测（陀螺仪），结果更平滑但响应慢。③ 观察增益收敛曲线——卡尔曼增益从初始值逐步收敛到稳态值。

伪代码实现

// 简化一维卡尔曼滤波器
let x_hat = 0;   // 状态估计（角度）
let P = 1;       // 估计协方差
const Q = 0.01;  // 过程噪声协方差
const R = 0.5;   // 测量噪声协方差

function kalmanFilter(acc_angle, gyro_rate, dt) {
  // === 预测步骤 ===
  x_hat = x_hat + gyro_rate * dt;  // 状态预测
  P = P + Q;                        // 协方差预测

  // === 更新步骤 ===
  const K = P / (P + R);   // 卡尔曼增益
  x_hat = x_hat + K * (acc_angle - x_hat);  // 状态更新
  P = (1 - K) * P;        // 协方差更新

  return { angle: x_hat, gain: K };
}
    

综合对比与总结

🏆 四种方法同台对比

在相同的噪声条件下，对比原始传感器数据、互补滤波和卡尔曼滤波的效果。

加速度计噪声 0.80

陀螺仪漂移率 0.15

互补滤波 α 0.05

所有方法融合结果对比

真实角度

加速度计

陀螺仪

互补滤波

卡尔曼滤波

—

加速度计 RMSE

—

陀螺仪 RMSE

—

互补滤波 RMSE

—

卡尔曼 RMSE

方法对比总结

特性	互补滤波	卡尔曼滤波
计算复杂度	极低（一行公式）	较高（矩阵运算）
参数调节	1 个参数（α）	2 个参数（Q, R）
最优性	非最优，但实用	在高斯噪声下最优
自适应性	固定权重	增益自动收敛
适用场景	嵌入式、资源受限	精度要求高的系统
实现难度	⭐ 简单	⭐⭐⭐ 中等
实际无人机应用	入门级飞控	专业级飞控（PX4/ArduPilot）

🎓

关键总结

① 加速度计 = 低通特性（低频可靠，高频噪声大）；陀螺仪 = 高通特性（高频可靠，低频漂移）。
② 互补滤波 = 低通（加速度计）+ 高通（陀螺仪），简单高效。
③ 卡尔曼滤波 = 最优版的互补滤波，能自动计算最佳融合权重。
④ 在噪声为高斯分布且参数已知的理想条件下，卡尔曼滤波是理论最优的；在工程实践中，调参良好的互补滤波往往已经足够好。