1. 项目背景与核心需求
在移动机器人控制领域，姿态估计算法一直是实现稳定运动控制的关键技术。特别是对于两轮平衡车这类倒立摆系统，实时准确的俯仰角（pitch angle）测量直接决定了系统的控制性能和稳定性。传统方案通常采用惯性测量单元（IMU）结合卡尔曼滤波进行姿态解算，但在实际应用中面临着传感器噪声、动态扰动等挑战。
本项目提出了一种融合扩展卡尔曼滤波（EKF）与Madgwick滤波器的混合算法架构，通过MATLAB实现了一套完整的姿态估计解决方案。这种组合方案的优势在于：
- EKF擅长处理非线性系统的状态估计问题
- Madgwick滤波器在计算效率上具有明显优势
- 两者互补可以平衡精度与实时性的需求
2. 系统建模与算法原理
2.1 两轮平台动力学模型
建立准确的系统模型是滤波算法的基础。对于两轮平衡平台，我们采用经典的倒立摆模型：
code复制ẍ = (m*l*sinθ*θ̇² - m*g*l*sinθ + u) / (M + m - m*cos²θ)
其中：
- M为车体质量
- m为摆杆质量
- l为摆杆长度
- θ为俯仰角
- u为控制输入
2.2 扩展卡尔曼滤波设计
EKF的实现包含以下关键步骤：
-
状态空间建模：
- 状态变量：x = [θ, θ̇, bias]ᵀ
- 过程模型：xₖ = f(xₖ₋₁, uₖ₋₁) + wₖ₋₁
- 观测模型：zₖ = h(xₖ) + vₖ
-
线性化处理：
- 计算雅可比矩阵F = ∂f/∂x
- 计算观测矩阵H = ∂h/∂x
-
预测-更新循环：
matlab复制% 预测步骤 x_pred = f(x_est, u); P_pred = F * P_est * F' + Q; % 更新步骤 K = P_pred * H' / (H * P_pred * H' + R); x_est = x_pred + K * (z - h(x_pred)); P_est = (eye(3) - K * H) * P_pred;
2.3 Madgwick滤波器原理
Madgwick算法是一种基于梯度下降的姿态解算方法，其核心迭代公式为：
code复制q̇ = 0.5 * q ⊗ [0, ω] - β * ∇f/||∇f||
其中：
- q为四元数
- ω为陀螺仪角速度
- β为滤波增益系数
- ⊗表示四元数乘法
3. 混合算法实现方案
3.1 系统架构设计
我们采用级联式架构组合两种滤波器：
code复制IMU数据 → Madgwick滤波器 → 粗估计值 → EKF → 精估计值
这种设计使得：
- Madgwick先进行快速但粗略的姿态解算
- EKF在此基础上进行精细优化
- 计算负载被合理分配
3.2 MATLAB实现关键代码
传感器数据预处理
matlab复制function [acc, gyro] = readIMU(data)
% 加速度计数据归一化
acc = data(1:3) / norm(data(1:3));
% 陀螺仪数据去偏置
gyro = data(4:6) - calib_offset;
end
Madgwick滤波器实现
matlab复制function q = madgwickUpdate(q, gyro, acc, dt)
beta = 0.1; % 调参经验值
% 梯度计算
J = [2*q(3), -2*q(4), 2*q(1), -2*q(2);
-2*q(2), -2*q(1), -2*q(4), -2*q(3);
2*q(1), 2*q(4), 2*q(3), 2*q(2)];
% 四元数更新
q_dot = 0.5 * quatmultiply(q, [0 gyro]) - beta * J' * acc;
q = q + q_dot * dt;
q = q / norm(q);
end
EKF实现核心
matlab复制function [theta, P] = ekfUpdate(theta_prev, P_prev, z, Q, R)
% 状态转移矩阵
F = [1 dt 0;
0 1 -dt;
0 0 1];
% 预测步骤
theta_pred = F * theta_prev;
P_pred = F * P_prev * F' + Q;
% 观测更新
H = [1 0 0];
K = P_pred * H' / (H * P_pred * H' + R);
theta = theta_pred + K * (z - H * theta_pred);
P = (eye(3) - K * H) * P_pred;
end
4. 参数调优与实验验证
4.1 关键参数设置
-
EKF参数：
- 过程噪声协方差Q = diag([0.01, 0.1, 0.001])
- 观测噪声协方差R = 0.05
- 初始协方差P0 = diag([0.1, 0.1, 0.1])
-
Madgwick参数：
- β = 0.1（动态场景可设为0.2）
- 采样周期dt = 0.01s
4.2 性能对比实验
我们在MATLAB中设计了三种测试场景：
| 测试场景 | 纯EKF误差(°) | 纯Madgwick误差(°) | 混合方案误差(°) |
|---|---|---|---|
| 静态测试 | 0.8 | 1.2 | 0.5 |
| 慢速运动 | 1.5 | 2.1 | 0.9 |
| 快速动态变化 | 3.2 | 1.8 | 1.3 |
实验数据表明：
- 静态场景下EKF表现更优
- 动态场景中Madgwick响应更快
- 混合方案在所有场景下均保持稳定性能
5. 工程实践中的经验总结
5.1 传感器校准要点
-
陀螺仪偏置校准：
matlab复制% 静止状态下采集100个样本求均值 offset = mean(gyro_data(1:100,:)); -
加速度计校准：
- 六面法校准：每个轴向正反方向各采集数据
- 使用最小二乘法求解标定矩阵
5.2 实时性优化技巧
-
矩阵运算简化：
- 预先计算常数矩阵
- 利用对称性减少乘法次数
-
定点数优化：
matlab复制% 将关键变量转换为定点数 q = fi(q, 1, 16, 12); % 符号位1，总位宽16，小数位12
5.3 常见问题排查
-
角度漂移问题：
- 检查陀螺仪积分误差
- 增加加速度计权重
-
动态响应滞后：
- 调整Madgwick的β参数
- 检查传感器采样率是否足够
-
发散问题处理：
matlab复制% 增加协方差重置逻辑 if trace(P) > threshold P = P0; end
6. 扩展应用与改进方向
6.1 多传感器融合方案
可进一步融合：
- 编码器里程计信息
- 视觉姿态估计
- 超声波测距数据
6.2 自适应参数调整
实现β和Q/R矩阵的动态调整：
matlab复制function beta = adaptiveBeta(acc_norm)
% 根据加速度计振动幅度调整
dynamic_level = abs(acc_norm - 9.8);
beta = max(0.05, min(0.3, 0.1 + dynamic_level/2));
end
6.3 嵌入式移植优化
-
内存优化：
- 使用查表法替代实时矩阵运算
- 降低四元数计算精度
-
定点化实现：
c复制typedef struct { int16_t w; int16_t x; int16_t y; int16_t z; } Quaternion;
这套算法方案已经成功应用于我们的两轮平衡车项目，在8MHz的STM32F103上实现了1kHz的更新频率，角度估计误差长期稳定在±1°以内。实际测试表明，即使在剧烈晃动条件下，系统也能保持可靠的姿态输出。