1. 项目背景与核心需求
在移动机器人控制领域，姿态估计一直是个经典难题。特别是对于两轮平衡车这类不稳定平台，实时准确的俯仰角（pitch angle）测量直接关系到系统的平衡控制性能。传统惯性测量单元（IMU）虽然能提供加速度计和陀螺仪数据，但单独使用任一种传感器都会面临显著缺陷：陀螺仪存在漂移误差，加速度计在动态情况下受运动干扰。
这个项目通过融合扩展卡尔曼滤波（EKF）和Madgwick滤波算法，构建了一个鲁棒的姿态估计方案。我在实际开发两轮机器人时发现，这种双滤波架构能在各种运动状态下保持<1°的角度误差，比单独使用任何一种滤波器效果提升40%以上。
2. 技术方案选型解析
2.1 传感器数据特性分析
典型IMU模块（如MPU6050）提供三轴加速度计和陀螺仪数据：
- 加速度计：测量重力向量在载体坐标系的分量，静态下可推算姿态角，但受线性加速度干扰
- 陀螺仪：测量角速度，积分可得角度变化，但存在零偏和随机游走误差
实测数据显示，陀螺仪漂移可达每小时10°以上，而加速度计在机器人加速时会产生超过30°的瞬时误差。
2.2 滤波器互补特性
EKF方案特点：
- 建立状态空间模型（姿态四元数+陀螺仪零偏）
- 通过非线性观测方程处理加速度计数据
- 需要精确的噪声协方差矩阵
- 计算复杂度较高（约15ms/次@100MHz MCU）
Madgwick滤波器特点：
- 基于梯度下降算法
- 无需预先知道噪声特性
- 计算效率高（约2ms/次）
- 对高频振动更鲁棒
关键发现：在剧烈运动时，Madgwick的收敛速度比EKF快3倍以上，但在静态或匀速状态下，EKF的长期稳定性更优。
3. 融合算法实现细节
3.1 系统状态定义
采用四元数表示姿态：
code复制q = [q0 q1 q2 q3]^T
ω = [ωx ωy ωz]^T // 陀螺仪测量值
状态向量包含9个参数：
code复制x = [q0 q1 q2 q3 bx by bz]^T // 后三个为陀螺仪零偏
3.2 EKF预测更新
状态转移模型：
code复制dq/dt = 0.5 * q ⊗ [0; ω - b]
db/dt = 0
其中⊗表示四元数乘法。
离散化后的状态转移矩