3. 传感器噪声与滤波：加速度计与陀螺仪的噪声特性、低通滤波与互补滤波在俯仰角估计中的应用

做飞控这么多年，我踩过最大的坑之一，就是传感器噪声。你想想看，一个抖动的加速度计数据，直接拿去算俯仰角，飞机会是什么反应？嗯，轻则高频抖动，重则直接炸机。所以这一节，咱们就专门聊聊噪声和滤波。

3.1 加速度计与陀螺仪的噪声特性

先说加速度计。这东西在静态时很准，但一有振动就完蛋。我曾在某款固定翼上遇到过，电机启动后，加速度计输出的Z轴数据直接跳了0.3g。说白了，加速度计对高频振动特别敏感。

陀螺仪呢？正好相反。它短时间内的角速度测量很稳，但时间一长就会漂移。我记得有一次做长航时测试，陀螺仪的零偏漂移让姿态角在10分钟内偏了5度。这就是典型的低频噪声问题。

我把它们的特性总结成一张表，方便你对照：

传感器	优势	劣势	典型噪声频段
加速度计	静态精度高，无长期漂移	对振动敏感，高频噪声大	10Hz以上
陀螺仪	动态响应快，短时稳定	存在零偏漂移，低频误差累积	0.1Hz以下

为什么会这样？因为加速度计本质上是一个质量块-弹簧系统，外部振动会直接激励它。而陀螺仪是测量科里奥利力，长时间积分后，微小的零偏误差会被放大。

3.2 低通滤波：干掉高频噪声

对付加速度计的高频噪声，最直接的办法就是低通滤波。我个人习惯用一阶RC低通滤波，简单有效。

公式长这样：

output = alpha * input + (1 - alpha) * output_prev

其中alpha = dt / (RC + dt)，RC是时间常数。alpha越小，滤波越狠，但延迟也越大。

我在项目中遇到过一个问题：滤波太强，飞机俯仰响应变得迟钝；滤波太弱，噪声又压不住。后来我总结出一个经验——对于常见的固定翼，截止频率设在5-10Hz比较合适。

代码实现也很简单：

// 一阶低通滤波
float lowpass_filter(float input, float alpha, float *output_prev) {
    *output_prev = alpha * input + (1.0f - alpha) * (*output_prev);
    return *output_prev;
}

// 使用示例
float accel_filtered;
float alpha = 0.2f;  // dt=0.01s, fc≈8Hz
accel_filtered = lowpass_filter(raw_accel, alpha, &accel_filtered);

3.3 互补滤波：融合加速度计与陀螺仪

低通滤波只能解决加速度计的问题，但陀螺仪的漂移怎么办？这时候就需要互补滤波了。说白了，就是把加速度计的低频信号和陀螺仪的高频信号拼在一起。

互补滤波的核心思想：

• 加速度计：信任它的低频部分（长期稳定）
• 陀螺仪：信任它的高频部分（短期准确）

公式如下：

angle = tau / (tau + dt) * (angle + gyro * dt) + dt / (tau + dt) * accel_angle

其中tau是时间常数，决定了融合的权重。tau越大，越信任陀螺仪；tau越小，越信任加速度计。

我曾经在调试一架飞翼布局的飞机时，发现互补滤波的tau值设成0.5秒效果最好。太大，飞机在转弯时俯仰角会滞后；太小，悬停时角度会跟着振动跳。

完整的互补滤波代码：

// 互补滤波俯仰角估计
float complementary_filter(float accel_angle, float gyro_rate, float dt, float tau, float *angle) {
    float alpha = tau / (tau + dt);
    *angle = alpha * (*angle + gyro_rate * dt) + (1.0f - alpha) * accel_angle;
    return *angle;
}

// 使用示例
float pitch_angle = 0.0f;
float tau = 0.5f;  // 时间常数0.5秒
float dt = 0.01f;  // 采样周期10ms

// 每次传感器更新时调用
pitch_angle = complementary_filter(accel_pitch, gyro_pitch_rate, dt, tau, &pitch_angle);

3.4 核心逻辑：传感器融合流程

为了让你更直观地理解整个流程，我画了一张图。这张图展示了从原始传感器数据到最终俯仰角估计的完整路径。

这张图其实就三件事：

1. 加速度计路径：先低通滤波，再算俯仰角
2. 陀螺仪路径：直接积分角速度
3. 互补滤波：把两条路径的结果融合在一起

最后说一句，互补滤波虽然简单，但在实际工程中非常实用。它不需要复杂的矩阵运算，计算量小，适合在低成本MCU上跑。我见过不少商业飞控，其实内部用的就是互补滤波，只是参数调得比较精细而已。

---

公众号:蓝海资料掘金营，微信deep3321