1. 项目背景与核心需求
在惯性导航和运动追踪领域,准确估计物体的三维姿态(即俯仰角、横滚角和偏航角)是一个基础但极具挑战性的问题。这个项目通过融合IMU(惯性测量单元)和磁力计的数据,采用扩展卡尔曼滤波(EKF)算法实现高精度的姿态估计。
IMU通常包含三轴加速度计和三轴陀螺仪,能够测量物体的角速度和线性加速度。磁力计则提供地球磁场信息,用于确定绝对方向。单独使用IMU会遇到积分漂移问题,而单独使用磁力计则容易受到磁场干扰。EKF作为一种非线性状态估计方法,能够有效地融合这些传感器的优势,同时补偿各自的不足。
2. 系统建模与EKF原理
2.1 状态空间模型
在姿态估计问题中,我们通常选择四元数作为状态变量,因为它在计算效率和避免万向节锁问题上优于欧拉角表示。系统的状态向量可以表示为:
x = [q0 q1 q2 q3 ωx ωy ωz]^T
其中q0-q3是四元数的四个分量,ωx-ωz是陀螺仪的偏置误差。
状态转移方程基于四元数运动学:
q̇ = 0.5 * q ⊗ [0; ω]
其中⊗表示四元数乘法,ω是角速度测量值减去偏置。
2.2 观测模型
系统有两类观测:
- 加速度计测量:在静止状态下,加速度计主要测量重力方向
- 磁力计测量:提供相对于地磁北极的方向信息
观测方程可以表示为:
z_acc = R(q)^T * [0 0 g]^T + v_acc
z_mag = R(q)^T * [m_n 0 m_d]^T + v_mag
其中R(q)是旋转矩阵,g是重力加速度,m_n和m_d是地磁场的水平和垂直分量。
3. EKF实现细节
3.1 预测步骤
预测步骤包括状态预测和协方差预测:
x̂_k|k-1 = f(x_k-1, u_k)
P_k|k-1 = F_k P_k-1 F_k^T + Q_k
其中F_k是状态转移矩阵的雅可比矩阵:
F_k = ∂f/∂x |_
对于四元数部分,雅可比矩阵需要考虑四元数归一化约束。
3.2 更新步骤
当获得新的观测数据时,执行EKF更新:
K_k = P_k|k-1 H_k^T (H_k P_k|k-1 H_k^T + R_k)^-1
x̂_k = x̂_k|k-1 + K_k (z_k - h(x̂_k|k-1))
P_k = (I - K_k H_k) P_k|k-1
其中H_k是观测模型的雅可比矩阵:
H_k = ∂h/∂x |_
4. Matlab实现关键代码解析
4.1 初始化参数
matlab复制% 初始化状态向量(单位四元数)
x = [1; 0; 0; 0; 0; 0; 0];
% 过程噪声协方差矩阵
Q = diag([0.01 0.01 0.01 0.01 0.001 0.001 0.001]);
% 观测噪声协方差矩阵
R_acc = diag([0.1 0.1 0.1]);
R_mag = diag([0.05 0.05 0.05]);
% 初始状态协方差
P = eye(7) * 0.1;
4.2 EKF预测步骤
matlab复制function [x_pred, P_pred] = ekf_predict(x, P, gyro, dt, Q)
% 提取四元数和陀螺仪偏置
q = x(1:4);
b = x(5:7);
% 计算角速度(减去偏置)
omega = gyro - b;
% 四元数更新
Omega = [0 -omega(1) -omega(2) -omega(3);
omega(1) 0 omega(3) -omega(2);
omega(2) -omega(3) 0 omega(1);
omega(3) omega(2) -omega(1) 0];
q_pred = (eye(4) + 0.5*Omega*dt) * q;
q_pred = q_pred / norm(q_pred); % 归一化
% 构建状态转移雅可比矩阵F
F = zeros(7,7);
% ...(具体雅可比矩阵计算代码)
% 协方差预测
P_pred = F * P * F' + Q;
% 更新状态预测
x_pred = [q_pred; b];
end
4.3 EKF更新步骤(加速度计)
matlab复制function [x_updated, P_updated] = acc_update(x_pred, P_pred, acc, R_acc)
% 重力向量(全局坐标系)
g = [0; 0; 9.81];
% 当前姿态四元数
q = x_pred(1:4);
% 计算旋转矩阵
R = quat2rotm(q');
% 预测的加速度计测量
h_acc = R' * g;
% 计算观测雅可比矩阵H
H = zeros(3,7);
% ...(具体雅可比矩阵计算代码)
% 卡尔曼增益计算
K = P_pred * H' / (H * P_pred * H' + R_acc);
% 状态更新
x_updated = x_pred + K * (acc - h_acc);
% 四元数归一化
x_updated(1:4) = x_updated(1:4) / norm(x_updated(1:4));
% 协方差更新
P_updated = (eye(7) - K * H) * P_pred;
end
5. 传感器数据处理与校准
5.1 IMU数据预处理
原始IMU数据通常需要以下处理步骤:
- 单位转换:将原始ADC值转换为物理单位(deg/s, m/s²)
- 轴对齐:确保各传感器轴与定义坐标系一致
- 温度补偿:部分IMU需要根据温度修正偏差
matlab复制% 陀螺仪数据处理示例
gyro_calib = (gyro_raw - gyro_bias) .* gyro_scale;
% 加速度计数据处理示例
acc_calib = (acc_raw - acc_bias) .* acc_scale;
5.2 磁力计校准
磁力计易受硬铁和软铁干扰,需要进行椭球拟合校准:
matlab复制% 磁力计校准参数(通过校准程序获得)
hard_iron = [10.5; 20.3; -15.2];
soft_iron = [1.02 0.05 -0.01;
0.05 0.98 0.03;
-0.01 0.03 1.05];
% 磁力计校准
mag_calib = soft_iron \ (mag_raw - hard_iron);
6. 姿态表示与可视化
6.1 四元数转欧拉角
虽然内部使用四元数计算,但最终常以欧拉角形式输出:
matlab复制function euler = quat2euler(q)
% 提取四元数分量
q0 = q(1); q1 = q(2); q2 = q(3); q3 = q(4);
% 计算欧拉角(ZYX顺序)
roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1^2 + q2^2));
pitch = asin(2*(q0*q2 - q3*q1));
yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2^2 + q3^2));
euler = [roll; pitch; yaw];
end
6.2 实时可视化
使用MATLAB的3D动画工具展示姿态变化:
matlab复制figure;
h = plot3(0,0,0);
axis([-1 1 -1 1 -1 1]);
grid on;
while running
% 获取当前姿态
R = quat2rotm(x(1:4)');
% 绘制坐标系
x_axis = R * [1;0;0];
y_axis = R * [0;1;0];
z_axis = R * [0;0;1];
set(h, 'XData', [0 x_axis(1)], 'YData', [0 x_axis(2)], ...
'ZData', [0 x_axis(3)], 'Color', 'r');
% ... 类似更新y和z轴
drawnow;
end
7. 性能优化与调试技巧
7.1 滤波器调参经验
-
过程噪声Q:反映模型不确定性
- 四元数部分:0.01-0.1
- 陀螺仪偏置:0.001-0.01
-
观测噪声R:反映传感器精度
- 加速度计:0.1-1.0(取决于振动环境)
- 磁力计:0.01-0.1(取决于磁场干扰)
调试建议:从较大噪声参数开始,逐步减小直到性能不再提升
7.2 常见问题排查
-
姿态发散:
- 检查传感器校准
- 验证坐标系定义一致性
- 检查时间同步(特别是IMU和磁力计数据)
-
响应迟缓:
- 减小过程噪声Q
- 增加观测噪声R
-
磁力计干扰处理:
- 实现磁场强度检测
- 动态调整磁力计观测噪声
- 在强干扰时暂时禁用磁力计更新
8. 扩展与改进方向
8.1 自适应EKF
根据运动状态动态调整噪声参数:
- 高动态:增大加速度计噪声
- 静止状态:减小陀螺仪噪声权重
matlab复制% 运动检测示例
acc_norm = norm(acc - [0;0;9.81]);
if acc_norm > 0.5 % 运动阈值
R_acc = diag([1 1 1]); % 增大噪声
else
R_acc = diag([0.1 0.1 0.1]);
end
8.2 多传感器融合
加入GPS或视觉数据可进一步提高精度:
- GPS提供绝对位置约束
- 视觉里程计提供相对运动信息
融合框架可扩展为多速率EKF或联邦滤波结构。