两轮自平衡系统俯仰角估算：EKF与Madgwick融合方案

1. 项目概述：两轮平台俯仰角估算的挑战与解决方案

在两轮自平衡系统（如Segway平衡车、两轮机器人）的开发中，俯仰角（Pitch Angle）的精确估计是维持系统稳定的关键。这个角度直接反映了平台相对于重力方向的倾斜程度，是控制算法做出平衡决策的核心输入。然而，工程实践中我们面临一个经典难题：加速度计在静态环境下精度很高，但会因平台振动或线性加速度产生严重干扰；陀螺仪能快速响应角度变化，却存在不可避免的积分漂移问题。

我在参与某仓储物流机器人项目时，曾遇到一个典型场景：当机器人载货通过不平整地面时，传统单一滤波器要么因振动导致角度跳变，要么因积分漂移产生控制滞后。这促使我深入研究EKF（扩展卡尔曼滤波）与Madgwick滤波器的融合方案。该方案通过动力学模型约束和梯度下降优化的优势互补，最终实现了：

• 静态误差<0.5度
• 动态延迟控制在5ms内
• 振动环境下角度波动幅度降低60%

2. 核心算法原理与实现架构

2.1 传感器特性与误差建模

加速度计的工作机理：
当平台静止时，加速度计测量的重力分量g在机体坐标系下的分布可表示为：

code复制[ax, ay, az] = [g*sinθ, 0, g*cosθ]

其中θ即为俯仰角。理论上通过atan2(ax, az)即可解算角度。但实际运动中，线性加速度a_linear会叠加在测量值上：

code复制ax_measured = g*sinθ + a_linear

这就是为什么急加速时会出现"角度虚高"现象。

陀螺仪的漂移问题：
陀螺仪输出角速度ω，通过积分得到角度变化：

code复制θ(t) = θ0 + ∫ω(t)dt

但实际传感器存在零偏bias（典型值1°/s）和随机游走噪声（0.1°/√Hz），导致积分误差随时间累积。实测数据显示，廉价MEMS陀螺仪10秒漂移可达8-10度。

关键经验：必须对陀螺仪进行温度校准。我们在恒温箱测试发现，温度每升高10℃，零偏变化达0.3°/s。建议在系统启动时执行静态校准流程。

2.2 EKF滤波器设计

扩展卡尔曼滤波的核心在于状态空间建模。我们定义状态向量：

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

状态方程（离散时间）：

code复制θ_k = θ_{k-1} + (ω_measu