基于扩展卡尔曼滤波的四旋翼无人机姿态估计及Matlab实现

引言

四旋翼无人机因其灵活性和机动性，在航拍、物流、农业监测等领域得到广泛应用。姿态估计作为无人机自主飞行的核心环节，直接决定了控制系统的稳定性和精度。传统的姿态解算方法（如互补滤波、梯度下降法）在动态环境下易受噪声干扰，导致姿态角发散。扩展卡尔曼滤波（EKF）作为一种非线性状态估计方法，通过迭代更新状态估计值和协方差矩阵，能够有效处理系统噪声和观测噪声，成为四旋翼无人机姿态估计的理想选择。

EKF算法原理

1. 系统模型构建

四旋翼无人机的姿态通常用欧拉角（滚转角φ、俯仰角θ、偏航角ψ）或四元数表示。本文采用四元数描述姿态，避免欧拉角的奇异性问题。系统状态向量定义为：
[ \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} \mathbf{q} \otimes \begin{bmatrix} 0 \ \omega_x \ \omega_y \ \omega_z \end{bmatrix} ]
离散化后得到状态转移矩阵(F_k)，用于预测步骤。

2. EKF迭代流程

EKF分为预测和更新两步：

• 预测步骤：

  • 状态预测：(\hat{\mathbf{x}}{k|k-1} = f(\hat{\mathbf{x}}{k-1|k-1}, \mathbf{u}_k))
  • 协方差预测：(P{k|k-1} = F_k P{k-1|k-1} F_k^T + Q_k)
    其中，(Q_k)为过程噪声协方差。

• 更新步骤：

  • 计算卡尔曼增益：(Kk = P{k|k-1} Hk^T (H_k P{k|k-1} H_k^T + R_k)^{-1})
  • 状态更新：(\hat{\mathbf{x}}{k|k} = \hat{\mathbf{x}}{k|k-1} + Kk (\mathbf{z}_k - h(\hat{\mathbf{x}}{k|k-1})))
  • 协方差更新：(P{k|k} = (I - K_k H_k) P{k|k-1})
    其中，(H_k)为观测矩阵，(R_k)为观测噪声协方差，(\mathbf{z}_k)为加速度计和磁力计的观测值。

Matlab代码实现

1. 初始化参数

% 系统参数
dt = 0.01; % 采样时间
Q = diag([0.01, 0.01, 0.01, 0.01, 0.1, 0.1, 0.1]); % 过程噪声
R = diag([0.1, 0.1, 0.1, 0.05, 0.05, 0.05]); % 观测噪声
% 初始状态
q0 = [1; 0; 0; 0]; % 初始四元数
omega0 = [0; 0; 0]; % 初始角速度
x0 = [q0; omega0]; % 初始状态向量
P0 = eye(7); % 初始协方差矩阵

2. EKF主函数

function [q_est, omega_est] = EKF_AttitudeEstimation(acc, mag, gyro, x0, P0, Q, R, dt)
    % 初始化
    x_est = x0;
    P_est = P0;
    N = length(acc); % 数据长度
    q_est = zeros(4, N);
    omega_est = zeros(3, N);
    for k = 1:N
        % 预测步骤
        % 状态转移
        omega = x_est(5:7);
        q = x_est(1:4);
        omega_skew = [0, -omega(3), omega(2);
                      omega(3), 0, -omega(1);
                      -omega(2), omega(1), 0];
        dqdt = 0.5 * [q(1)*eye(3) - q(2:4)'*omega_skew;
                      q(2)*eye(3) + omega_skew*q(1) + cross(omega, q(2:4))'];
        q_pred = q + dqdt * dt;
        q_pred = q_pred / norm(q_pred); % 归一化
        omega_pred = gyro(:, k); % 假设陀螺仪直接输出角速度
        % 线性化状态转移矩阵（简化版，实际需数值近似）
        F = eye(7);
        % 此处省略F的详细推导，实际需根据雅可比矩阵计算
        x_pred = [q_pred; omega_pred];
        P_pred = F * P_est * F' + Q;
        % 更新步骤
        % 观测模型（加速度计和磁力计）
        h_acc = [2*(q_pred(1)*q_pred(3) + q_pred(2)*q_pred(4));
                 2*(q_pred(2)*q_pred(3) - q_pred(1)*q_pred(4));
                 q_pred(1)^2 - q_pred(2)^2 - q_pred(3)^2 + q_pred(4)^2];
        h_mag = [2*q_pred(1)*q_pred(2) + 2*q_pred(3)*q_pred(4);
                 q_pred(1)^2 - q_pred(2)^2 + q_pred(3)^2 - q_pred(4)^2;
                 -2*q_pred(1)*q_pred(3) + 2*q_pred(2)*q_pred(4)];
        z_pred = [h_acc; h_mag];
        % 观测矩阵（简化版，实际需数值近似）
        H = [jacobian_h_acc(q_pred), zeros(3,3);
             jacobian_h_mag(q_pred), zeros(3,3)];
        % 卡尔曼增益
        K = P_pred * H' / (H * P_pred * H' + R);
        % 观测值（加速度计和磁力计数据需归一化）
        z_real = [acc(:, k); mag(:, k)];
        % 状态更新
        x_est = x_pred + K * (z_real - z_pred);
        x_est(1:4) = x_est(1:4) / norm(x_est(1:4)); % 归一化四元数
        % 协方差更新
        P_est = (eye(7) - K * H) * P_pred;
        % 保存结果
        q_est(:, k) = x_est(1:4);
        omega_est(:, k) = x_est(5:7);
    end
end

3. 辅助函数（雅可比矩阵计算）

function J = jacobian_h_acc(q)
    % 加速度计观测模型的雅可比矩阵
    q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4);
    J = [2*q3, 2*q4, 2*q1, 2*q2;
         2*q4, -2*q3, 2*q2, -2*q1;
         2*q1, -2*q2, -2*q3, 2*q4];
end
function J = jacobian_h_mag(q)
    % 磁力计观测模型的雅可比矩阵
    q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4);
    J = [2*q2, 2*q1, 2*q4, 2*q3;
         2*q1, -2*q2, 2*q3, -2*q4;
         -2*q3, -2*q4, -2*q1, -2*q2];
end

实际应用建议

1. 传感器校准：使用前需对IMU和磁力计进行标定，消除零偏和比例误差。
2. 噪声参数调整：根据实际传感器特性调整(Q)和(R)矩阵，提升滤波效果。
3. 实时性优化：对于嵌入式实现，可采用定点运算或简化雅可比矩阵计算。
4. 异常值处理：加入磁力计干扰检测机制，避免地磁异常导致的姿态发散。

结论

本文提出的基于EKF的四旋翼无人机姿态估计方法，通过融合IMU和磁力计数据，实现了高精度、鲁棒的姿态解算。Matlab代码提供了完整的实现框架，可进一步扩展至嵌入式平台。未来工作可探索自适应EKF或无迹卡尔曼滤波（UKF）以提升非线性系统的估计性能。