📚 课程目录第2章倾转角度传感器标定与滤波

2. 倾转角度传感器标定与滤波

任务定义

倾转机构的角度测量是闭环控制的基础。本案例针对eVTOL倾转机构中常见的MEMS加速度计与陀螺仪组合，完成两项核心工作：（1）对加速度计原始输出进行两点标定，消除零偏与比例因子误差；（2）设计并实现一维卡尔曼滤波器，融合标定后的加速度计倾角（低频准确但含噪声）与陀螺仪角速度积分（高频稳定但会漂移），输出融合后的高精度倾角估计。

传感器模型与标定原理

加速度计在静止时测量重力分量，倾角θ与输出关系为：a_out = K·sin(θ) + b，其中K为比例因子，b为零偏。通过将传感器分别置于已知角度θ₁和θ₂（本案例取0°和90°），记录输出a₁和a₂，即可解算：

K = (a₂ - a₁) / (sinθ₂ - sinθ₁)
b = a₁ - K·sinθ₁

标定后倾角由θ_acc = arcsin((a_out - b) / K)计算。陀螺仪输出为角速度ω，通过积分获得角度增量，但积分会累积漂移。

卡尔曼滤波器设计

建立一维状态空间模型（状态量为倾角θ，角速度作为控制输入）：

状态方程: θ_k = θ_{k-1} + (ω_k - bias_gyro)·Δt + w,  w~N(0, Q)
观测方程: θ_acc,k = θ_k + v,  v~N(0, R)

其中Q为过程噪声方差（反映陀螺仪积分漂移），R为观测噪声方差（反映加速度计测量噪声）。陀螺仪零偏bias_gyro在静止时通过取均值估计。卡尔曼滤波递推公式为标准五式：预测（先验估计与协方差）、更新（卡尔曼增益、后验估计与协方差）。

Python实现

import numpy as np
import matplotlib.pyplot as plt

# ---------- 1. 标定数据（模拟静止采集，实际应取多帧平均） ----------
theta_cal = np.array([0.0, 90.0])          # 已知角度（度）
a_cal = np.array([0.02, 1.01])             # 对应加速度计原始输出（g）
sin_val = np.sin(np.deg2rad(theta_cal))
K = (a_cal[1] - a_cal[0]) / (sin_val[1] - sin_val[0])
b = a_cal[0] - K * sin_val[0]
print(f"标定结果: K={K:.4f}, b={b:.4f}")

# ---------- 2. 生成模拟动态数据（真实角度+传感器噪声） ----------
fs = 100.0          # 采样率100Hz
dt = 1.0 / fs
t = np.arange(0, 10, dt)
N = len(t)
# 真实角度：1Hz正弦摆动，幅度30度
true_theta = 30.0 * np.sin(2 * np.pi * 1.0 * t)
true_omega = np.gradient(true_theta, dt)   # 真实角速度（度/秒）

# 模拟加速度计输出（含噪声），经标定反算倾角
a_sim = K * np.sin(np.deg2rad(true_theta)) + b + np.random.normal(0, 0.02, N)
theta_acc = np.rad2deg(np.arcsin((a_sim - b) / K))

# 模拟陀螺仪输出（含噪声与零偏）
gyro_bias = 0.5    # 度/秒
gyro_noise_std = 0.1
omega_sim = true_omega + gyro_bias + np.random.normal(0, gyro_noise_std, N)

# ---------- 3. 陀螺仪零偏估计（静止前0.5秒） ----------
bias_est = np.mean(omega_sim[:50])
print(f"陀螺仪零偏估计: {bias_est:.4f} 度/秒")

# ---------- 4. 卡尔曼滤波 ----------
Q = 0.01           # 过程噪声方差（调参）
R = 0.04           # 观测噪声方差（调参）
theta_est = np.zeros(N)
P = 1.0            # 初始协方差
theta_est[0] = theta_acc[0]

for k in range(1, N):
    # 预测
    theta_pred = theta_est[k-1] + (omega_sim[k] - bias_est) * dt
    P_pred = P + Q
    # 更新
    K_gain = P_pred / (P_pred + R)
    theta_est[k] = theta_pred + K_gain * (theta_acc[k] - theta_pred)
    P = (1 - K_gain) * P_pred

# ---------- 5. 误差对比 ----------
err_acc = np.abs(theta_acc - true_theta)
err_filt = np.abs(theta_est - true_theta)
print(f"加速度计倾角MAE: {np.mean(err_acc):.4f} 度")
print(f"卡尔曼滤波MAE:   {np.mean(err_filt):.4f} 度")

# ---------- 6. 绘图 ----------
plt.figure(figsize=(10, 6))
plt.plot(t, true_theta, 'k-', lw=2, label='真实角度')
plt.plot(t, theta_acc, 'r-', alpha=0.6, label='加速度计标定后')
plt.plot(t, theta_est, 'b-', lw=1.5, label='卡尔曼滤波输出')
plt.xlabel('时间 (s)'); plt.ylabel('角度 (度)')
plt.legend(); plt.grid(True); plt.title('倾转角度传感器标定与卡尔曼滤波')
plt.savefig('kalman_result.png', dpi=100)
print("结果图已保存为 kalman_result.png")

运行与验证

运行方式：将上述代码保存为kalman_angle.py，在已安装NumPy和Matplotlib的Python环境中执行python kalman_angle.py。

验证方法：观察控制台输出的标定参数K和b是否接近设定值（K≈1.0，b≈0.0）；陀螺仪零偏估计是否接近0.5度/秒；对比加速度计倾角MAE与卡尔曼滤波MAE，滤波后误差应显著减小。查看生成的kalman_result.png，蓝色曲线应比红色曲线更贴近黑色真实角度曲线。

判定标准：卡尔曼滤波输出的MAE小于加速度计直接解算的MAE，且图中滤波曲线平滑、无明显滞后，即判定滤波有效。若效果不理想，可调整Q和R：Q增大使滤波更信任观测（响应快但噪声大），R增大使滤波更信任预测（平滑但滞后）。