3. 陀螺仪积分姿态解算

任务说明

本案例用一段模拟的陀螺仪角速度数据，通过欧拉积分得到俯仰角（pitch），并对比真实角度，直观看到积分漂移。核心公式：angle += gyro_rate * dt。

环境与依赖

• Python 3.8+，仅用标准库 math、random，无需安装第三方库。
• 运行：python gyro_integrate.py。

核心代码

import math, random

DT = 0.01          # 采样周期 10ms
N  = 1000          # 采样点数，共 10s
BIAS = 0.02        # 陀螺仪零偏 (rad/s)，模拟真实器件误差
NOISE = 0.005      # 角速度噪声标准差

def true_rate(t):
    # 真实角速度：0.5 rad/s 的正弦摆动
    return 0.5 * math.sin(2 * math.pi * 0.2 * t)

def true_angle(t):
    # 真实角度 = 角速度积分（解析解）
    return -0.5 / (2 * math.pi * 0.2) * math.cos(2 * math.pi * 0.2 * t) \
           + 0.5 / (2 * math.pi * 0.2)

def main():
    random.seed(0)
    angle = 0.0
    max_err = 0.0
    print(f"{'t(s)':>6} {'true':>9} {'est':>9} {'err':>9}")
    for k in range(N):
        t = k * DT
        # 模拟陀螺仪读数 = 真实角速度 + 零偏 + 噪声
        gyro = true_rate(t) + BIAS + random.gauss(0, NOISE)
        # 欧拉积分
        angle += gyro * DT
        ta = true_angle(t)
        err = angle - ta
        max_err = max(max_err, abs(err))
        if k % 200 == 0:
            print(f"{t:6.2f} {ta:9.4f} {angle:9.4f} {err:9.4f}")
    print(f"\n最大误差: {max_err:.4f} rad")
    print(f"零偏导致的漂移速率 ≈ {BIAS:.4f} rad/s")

if __name__ == "__main__":
    main()

运行与验证

执行 python gyro_integrate.py，观察输出：

• 每 2s 打印一次真实角、积分角与误差，误差随时间单调增大。
• 末尾打印最大误差与漂移速率。由于零偏 BIAS=0.02，10s 后误差约 0.02×10=0.2 rad，与输出一致即验证成功。
• 把 BIAS 改为 0，误差只剩噪声引起的随机游走，量级明显减小，说明漂移主因是零偏。

漂移成因

陀螺仪输出含零偏，积分时零偏被不断累加，误差随时间线性增长，这就是积分漂移。抑制方法（如零速修正、互补滤波、卡尔曼滤波）将在后续案例展开。