4. 互补滤波融合加速度计与陀螺仪

任务说明

本案例实现一阶互补滤波器，将陀螺仪积分得到的高频角度与加速度计解算得到的低频角度融合，输出稳定的俯仰角（pitch）。核心公式：

angle = α · (angle + gyro · dt) + (1 − α) · accel_angle

其中 α 由时间常数 τ 与采样周期 dt 决定：α = τ / (τ + dt)。τ 越大越信任陀螺仪，越小越信任加速度计。

数据流与滤波结构

完整实现

import math

class ComplementaryFilter:
    """一阶互补滤波：融合陀螺仪高频与加速度计低频姿态。"""

    def __init__(self, tau=0.5, dt=0.01):
        # tau: 时间常数(秒)，越大越信任陀螺仪
        # dt : 采样周期(秒)
        self.tau = tau
        self.dt = dt
        self.alpha = tau / (tau + dt)
        self.angle = 0.0          # 融合后的俯仰角(弧度)
        self.initialized = False

    def update(self, gyro_rate, ax, ay, az):
        """输入: 陀螺仪角速度(rad/s), 加速度计三轴(g)。返回融合角度(弧度)。"""
        # 加速度计解算俯仰角(低频，长期稳定但有噪声)
        accel_angle = math.atan2(-ax, math.sqrt(ay * ay + az * az))

        if not self.initialized:
            # 首次用加速度计初始化，避免从0开始收敛
            self.angle = accel_angle
            self.initialized = True
            return self.angle

        # 陀螺仪积分(高频，短期精确但会漂移)
        gyro_angle = self.angle + gyro_rate * self.dt

        # 互补融合
        self.angle = self.alpha * gyro_angle + (1 - self.alpha) * accel_angle
        return self.angle

if __name__ == "__main__":
    import random
    random.seed(42)

    filt = ComplementaryFilter(tau=0.5, dt=0.01)
    true_pitch = 0.3          # 真实俯仰角 0.3 rad
    gyro_bias = 0.02          # 陀螺仪零偏 rad/s

    for i in range(500):
        # 模拟传感器：陀螺仪带零偏+噪声，加速度计带噪声
        gyro = 0.0 + gyro_bias + random.gauss(0, 0.01)
        ax = -math.sin(true_pitch) + random.gauss(0, 0.02)
        ay = 0.0 + random.gauss(0, 0.02)
        az = math.cos(true_pitch) + random.gauss(0, 0.02)

        angle = filt.update(gyro, ax, ay, az)

        if i % 100 == 0:
            print(f"step {i:3d}  angle={math.degrees(angle):7.3f} deg  "
                  f"true={math.degrees(true_pitch):7.3f} deg")

    print(f"final angle = {math.degrees(filt.angle):.3f} deg")

运行方式

python complementary_filter.py

无需第三方依赖，仅用标准库 math 与 random。

验证方法与通过条件

• 脚本每 100 步打印一次融合角度与真实角度，观察两者是否接近。
• 通过条件：最终 final angle 与真实值 0.3 rad（约 17.19°）的偏差在数度以内，且不随步数持续发散。
• 对比实验：把 tau 改为 0.01（更信任加速度计），输出会更抖但无漂移；改为 5.0（更信任陀螺仪），短期平滑但受零偏影响会缓慢偏移。这验证了 α 对高低频权重的调节作用。

关键点

• atan2(-ax, sqrt(ay²+az²)) 是俯仰角的标准解算，符号约定需与陀螺仪轴向一致。
• 首次更新用加速度计初始化，避免滤波器从 0 缓慢收敛。
• α 由 τ 和 dt 共同决定，改变采样率时需同步更新 dt 以保持等效时间常数。