两轮自平衡平台俯仰角估算：EKF与Madgwick融合算法实践

1. 两轮平台俯仰角估算的核心挑战与解决方案

两轮自平衡平台（如平衡车、两轮机器人）的姿态控制中，俯仰角（Pitch Angle）的精确估算一直是个棘手问题。我在实际项目中多次遇到这样的场景：当平台在粗糙路面行驶或突然加速时，传统单一传感器的角度读数会出现明显偏差，导致控制系统产生振荡甚至失稳。究其根源，问题出在传感器特性与平台动态特性的矛盾上。

加速度计通过测量重力分量可以计算倾角，其静态精度可达0.1°以内。但实际运行中，平台振动和线性加速度会产生额外的惯性力，使加速度计输出失真。我曾实测过，当平衡车以0.5g加速度启动时，仅用加速度计计算的俯仰角会产生超过8°的误差。另一方面，陀螺仪虽然对动态响应灵敏（延迟通常小于1ms），但其固有的零偏稳定性问题会导致积分误差随时间累积。在常温环境下，消费级MEMS陀螺仪的漂移可达10°/min，这意味着即使平台静止，3分钟后角度误差就会达到30°。

2. 融合算法架构设计

2.1 扩展卡尔曼滤波(EKF)实现

EKF的核心在于状态空间模型的建立。对于两轮平台，我通常采用以下状态向量：

code复制x = [θ, ω_b]^T

其中θ为俯仰角，ω_b为陀螺仪零偏。状态方程考虑刚体旋转动力学：

code复制θ_k = θ_{k-1} + (ω_m - ω_b)Δt + q_θ
ω_b_k = ω_b_{k-1} + q_b

这里ω_m是陀螺仪测量值，q_θ和q_b是过程噪声。观测方程则融合加速度计数据：

code复制z = [a_y, a_z]^T = [-gsinθ, -gcosθ]^T + r

g为重力加速度，r为观测噪声。EKF的预测和更新步骤通过雅可比矩阵线性化：

matlab复制% EKF预测步骤示例
A = [1, -dt; 0, 1]; % 状态转移雅可比
P_pred = A*P_prev*A' + Q; % 协方差预测
x_pred = [x_prev(1) + (gyro_m - x_prev(2))*dt; x_prev(2)];

% EKF更新步骤示例
H = [-g*cos(x_pred(1)), 0; g*sin(x_pred(1)), 0]; % 观测雅可比
K = P_pred*H'/(H*P_pred*H' + R); % 卡尔曼增益
x_es