基于扩展卡尔曼滤波(EKF)的四旋翼无人机姿态估计附Matlab代码

引言

四旋翼无人机作为当前最主流的无人机形态，其稳定飞行高度依赖精确的姿态估计。传统传感器（如陀螺仪、加速度计、磁力计）存在噪声和漂移问题，直接使用原始数据会导致姿态解算误差累积。扩展卡尔曼滤波（EKF）作为一种非线性状态估计方法，通过融合多传感器数据并建立动态模型，可有效提升姿态估计的精度和鲁棒性。本文将详细阐述EKF在四旋翼无人机姿态估计中的应用，并提供完整的Matlab代码实现。

四旋翼无人机运动模型

姿态表示方法

四旋翼无人机的姿态通常用欧拉角（滚转角φ、俯仰角θ、偏航角ψ）或四元数表示。欧拉角直观但存在万向节锁问题，四元数无奇异性且计算效率高，本文采用四元数作为状态变量。

状态空间模型

EKF的核心是建立系统的状态方程和观测方程。对于四旋翼无人机，状态变量定义为：
[ \mathbf{x} = [q_0, q_1, q_2, q_3, \omega_x, \omega_y, \omega_z]^T ]
其中 ( q_0, q_1, q_2, q_3 ) 为四元数，( \omega_x, \omega_y, \omega_z ) 为角速度。

状态方程

状态方程描述系统动态，由四元数运动学和角速度模型组成：
[ \dot{\mathbf{q}} = \frac{1}{2} \begin{bmatrix} 0 & -\omegax & -\omega_y & -\omega_z \ \omega_x & 0 & \omega_z & -\omega_y \ \omega_y & -\omega_z & 0 & \omega_x \ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} \mathbf{q} ]
[ \dot{\boldsymbol{\omega}} = \mathbf{n}\omega ]
其中 ( \mathbf{n}_\omega ) 为过程噪声。

观测方程

观测方程融合加速度计和磁力计数据。加速度计测量重力方向，磁力计测量地磁场方向，通过最小化测量值与估计值的误差构建观测模型。

扩展卡尔曼滤波原理

EKF是卡尔曼滤波的非线性扩展，通过线性化处理非线性系统。其核心步骤包括：

1. 预测步骤：根据状态方程预测下一时刻状态和协方差。
2. 更新步骤：利用观测数据修正预测值，更新状态和协方差。

预测步骤

1. 状态预测：
   [ \hat{\mathbf{x}}{k|k-1} = f(\hat{\mathbf{x}}{k-1|k-1}, \mathbf{u}_k) ]
2. 协方差预测：
   [ \mathbf{P}{k|k-1} = \mathbf{F}_k \mathbf{P}{k-1|k-1} \mathbf{F}_k^T + \mathbf{Q}_k ]
   其中 ( \mathbf{F}_k ) 为状态方程的雅可比矩阵，( \mathbf{Q}_k ) 为过程噪声协方差。

更新步骤

1. 计算卡尔曼增益：
   [ \mathbf{K}k = \mathbf{P}{k|k-1} \mathbf{H}k^T (\mathbf{H}_k \mathbf{P}{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k)^{-1} ]
2. 状态更新：
   [ \hat{\mathbf{x}}{k|k} = \hat{\mathbf{x}}{k|k-1} + \mathbf{K}k (\mathbf{z}_k - h(\hat{\mathbf{x}}{k|k-1})) ]
3. 协方差更新：
   [ \mathbf{P}{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}{k|k-1} ]
   其中 ( \mathbf{H}_k ) 为观测方程的雅可比矩阵，( \mathbf{R}_k ) 为观测噪声协方差。

Matlab代码实现

代码结构

代码分为以下几个部分：

1. 初始化：设置初始状态、协方差、噪声参数等。
2. 主循环：模拟传感器数据，执行EKF预测和更新。
3. 可视化：绘制姿态估计结果与真实值的对比。

核心代码

% 参数设置
dt = 0.01; % 采样时间
T = 10; % 总时间
steps = T / dt;
% 初始化状态和协方差
x = [1; 0; 0; 0; 0; 0; 0]; % [q0; q1; q2; q3; wx; wy; wz]
P = eye(7) * 0.1;
% 噪声协方差
Q = diag([0.01, 0.01, 0.01, 0.01, 0.1, 0.1, 0.1]); % 过程噪声
R_acc = 0.1 * eye(3); % 加速度计噪声
R_mag = 0.1 * eye(3); % 磁力计噪声
% 存储结果
est_q = zeros(4, steps);
true_q = zeros(4, steps);
for k = 1:steps
    % 模拟真实状态（此处简化为固定角速度）
    wx = 0.1; wy = 0.05; wz = 0.02;
    omega = [wx; wy; wz];
    % 真实四元数更新（简化模型）
    q_dot = 0.5 * [0, -wx, -wy, -wz; wx, 0, wz, -wy; wy, -wz, 0, wx; wz, wy, -wx, 0] * x(1:4);
    x_true = x(1:4) + q_dot * dt;
    x_true = x_true / norm(x_true); % 归一化
    % 模拟传感器数据
    % 加速度计测量（重力方向）
    g = [0; 0; 9.81];
    acc_meas = rotate_vector(g, x_true) + sqrt(R_acc) * randn(3,1);
    % 磁力计测量（地磁场方向，简化）
    mag_ref = [1; 0; 0]; % 假设地磁场方向
    mag_meas = rotate_vector(mag_ref, x_true) + sqrt(R_mag) * randn(3,1);
    % EKF预测
    % 状态预测（简化，实际需数值积分）
    x_pred = x;
    x_pred(1:4) = x(1:4) + 0.5 * dt * [0, -x(5), -x(6), -x(7); x(5), 0, x(7), -x(6); x(6), -x(7), 0, x(5); x(7), x(6), -x(5), 0] * x(1:4);
    x_pred(1:4) = x_pred(1:4) / norm(x_pred(1:4));
    % 协方差预测
    F = eye(7); % 简化雅可比矩阵
    F(1:4,5:7) = 0.5 * dt * [0, -1, -1; 1, 0, 1; 1, -1, 0; 1, 1, -1]; % 近似
    P_pred = F * P * F' + Q;
    % EKF更新（加速度计）
    % 观测函数雅可比矩阵（简化）
    H_acc = zeros(3,7);
    H_acc(1:3,1:4) = 2 * [0, -acc_meas(3), acc_meas(2); acc_meas(3), 0, -acc_meas(1); -acc_meas(2), acc_meas(1), 0];
    % 卡尔曼增益
    K_acc = P_pred * H_acc' / (H_acc * P_pred * H_acc' + R_acc);
    % 观测残差（简化）
    acc_est = rotate_vector(g, x_pred(1:4));
    z_acc = acc_meas - acc_est;
    % 状态更新
    x_upd = x_pred + K_acc * z_acc;
    x_upd(1:4) = x_upd(1:4) / norm(x_upd(1:4));
    % 协方差更新
    P_upd = (eye(7) - K_acc * H_acc) * P_pred;
    % 合并磁力计更新（简化）
    % 实际需重复类似加速度计的更新步骤
    % 更新状态和协方差
    x = x_upd;
    P = P_upd;
    % 存储结果
    est_q(:,k) = x(1:4);
    true_q(:,k) = x_true;
    % 更新真实状态（用于下一步）
    x(1:4) = x_true;
    x(5:7) = omega;
end
% 辅助函数：向量旋转
function v_rot = rotate_vector(v, q)
    q_conj = [q(1); -q(2); -q(3); -q(4)];
    v_quat = [0; v];
    temp = quat_multiply(quat_multiply(q, v_quat), q_conj);
    v_rot = temp(2:4);
end
% 辅助函数：四元数乘法
function q_prod = quat_multiply(q1, q2)
    q_prod = [q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3) - q1(4)*q2(4);
              q1(1)*q2(2) + q1(2)*q2(1) + q1(3)*q2(4) - q1(4)*q2(3);
              q1(1)*q2(3) - q1(2)*q2(4) + q1(3)*q2(1) + q1(4)*q2(2);
              q1(1)*q2(4) + q1(2)*q2(3) - q1(3)*q2(2) + q1(4)*q2(1)];
end

代码说明

1. 初始化：设置初始状态、协方差和噪声参数。
2. 主循环：
   • 模拟真实状态和传感器数据。
   • 执行EKF预测步骤，更新状态和协方差。
   • 利用加速度计数据执行更新步骤。
   • 存储估计结果用于可视化。
3. 辅助函数：实现四元数旋转和乘法运算。

实际应用建议

1. 噪声参数调优：实际应用中需通过实验调整 ( \mathbf{Q} ) 和 ( \mathbf{R} ) 以获得最佳性能。
2. 传感器校准：确保加速度计和磁力计的零偏和比例因子已校准。
3. 实时性优化：对于嵌入式实现，需优化雅可比矩阵计算和矩阵运算效率。
4. 故障检测：加入传感器故障检测机制，提升系统鲁棒性。

结论

本文详细阐述了基于EKF的四旋翼无人机姿态估计方法，并通过Matlab代码实现了从传感器数据融合到姿态解算的全过程。EKF通过动态模型和非线性滤波，有效解决了传统传感器数据的噪声和漂移问题，为无人机稳定控制提供了可靠依据。实际应用中需结合具体硬件平台进行参数调优和性能优化。