两轮自平衡系统姿态估算：EKF与Madgwick融合算法实践

1. 两轮平台俯仰角估算的核心挑战

两轮自平衡系统（如Segway、两轮机器人）的核心控制难题在于实时准确地估算平台的俯仰角度。这个看似简单的参数实际上牵涉到复杂的传感器融合和信号处理问题。我在实际项目中遇到过这样的情况：当自平衡车在粗糙路面上行驶时，原始传感器数据会产生剧烈波动，导致控制系统误判车身姿态而失控。

1.1 传感器特性与局限

现代两轮平台通常配备六轴IMU（惯性测量单元），包含三轴加速度计和三轴陀螺仪。加速度计通过测量重力加速度在各轴的分量来计算姿态角，其静态精度可达0.1°。但在实际运行中，平台加速度会产生额外的惯性力，与重力加速度叠加，导致动态误差可能超过10°。我曾测试过一款常见IMU，在1m/s²的线性加速度下，俯仰角误差达到8.7°。

陀螺仪测量角速度，通过积分得到角度变化。其优势在于动态响应快（延迟<1ms），但积分过程会累积误差。以典型的MEMS陀螺仪为例，即使经过校准，零偏不稳定性仍会导致约0.5°/min的漂移。这意味着运行10分钟后，单纯依赖陀螺仪的角度误差可能达到5°。

1.2 平台动态干扰

两轮平台的运动特性加剧了姿态估算的难度。在加速阶段，前向惯性力会产生虚假的俯仰角信号；转向时，向心加速度会干扰横滚角测量；遇到颠簸路面时，高频振动会导致加速度计数据严重失真。我在实验室环境下模拟过这些场景：

• 0.5g的急加速导致加速度计估算角度偏离真实值26°
• 2Hz的机械振动使原始角度数据出现±15°的波动
• 连续转向操作30秒后，纯陀螺积分角度漂移达7.2°

2. 融合算法架构设计

2.1 EKF滤波器实现

扩展卡尔曼滤波(EKF)通过状态空间模型来处理非线性系统。针对两轮平台，我设计的系统状态向量为：

code复制x = [θ, θ_bias]^T

其中θ是俯仰角，θ_bias是陀螺仪零偏。状态转移模型为：

code复制θ_k = θ_{k-1} + (gyro - θ_bias)*Δt
θ_bias_k = θ_bias_{k-1}

观测模型使用加速度计数据：

code复制z = atan2(acc_y, acc_z)

EKF的预测和更新过程如下：

1. 预测阶段：

   matlab复制F = [1 -Δt; 0 1];  % 状态