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

一、四旋翼无人机姿态估计背景与挑战

四旋翼无人机在飞行过程中需实时感知自身姿态（滚转角、俯仰角、偏航角），以实现稳定控制与路径跟踪。传统姿态估计方法依赖惯性测量单元（IMU），但存在以下问题：

1. 传感器噪声：加速度计与陀螺仪受高斯白噪声和随机游走噪声影响，直接积分会导致误差累积。
2. 非线性特性：姿态动力学方程为强非线性系统，线性卡尔曼滤波（KF）无法直接应用。
3. 计算效率：无人机嵌入式系统资源有限，算法需兼顾精度与实时性。

扩展卡尔曼滤波（EKF）通过局部线性化处理非线性问题，成为无人机姿态估计的主流方案。其核心思想是在预测步使用非线性模型，在更新步通过雅可比矩阵线性化观测方程，从而递归估计系统状态。

二、EKF姿态估计原理与数学建模

1. 系统状态定义

将无人机姿态表示为四元数 ( \mathbf{q} = [q0, q_1, q_2, q_3]^T )，避免欧拉角的奇异性问题。状态向量扩展为：
[
\mathbf{x} = [\mathbf{q}^T, \mathbf{\omega}_b^T]^T
]
其中 ( \mathbf{\omega}_b = [\omega_x, \omega_y, \omega_z]^T ) 为陀螺仪测量的机体角速度，附加偏置项 ( \mathbf{b}\omega ) 可进一步优化。

2. 状态转移方程

基于刚体运动学，四元数更新方程为：
[
\dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \otimes \begin{bmatrix} 0 \ \mathbf{\omega}m - \mathbf{b}\omega \end{bmatrix}
]
其中 ( \otimes ) 为四元数乘法，( \mathbf{\omega}_m ) 为测量角速度。离散化后得到状态转移矩阵 ( \mathbf{F}_k )。

3. 观测方程

利用加速度计测量重力方向 ( \mathbf{a}_m )，与估计的重力方向 ( \mathbf{h}(\mathbf{x}) ) 构建观测模型：
[
\mathbf{z}_k = \mathbf{h}(\mathbf{x}_k) + \mathbf{v}_k = \begin{bmatrix} 0 \ 0 \ g \end{bmatrix} - \mathbf{R}(\mathbf{q})^T \mathbf{a}_m + \mathbf{v}_k
]
其中 ( \mathbf{R}(\mathbf{q}) ) 为四元数对应的旋转矩阵，( \mathbf{v}_k ) 为观测噪声。

4. EKF算法流程

1. 预测步：
   • 计算状态转移矩阵 ( \mathbf{F}_k ) 和过程噪声协方差 ( \mathbf{Q}_k )。
   • 更新先验状态估计 ( \hat{\mathbf{x}}_k^- ) 和协方差 ( \mathbf{P}_k^- )。
2. 更新步：
   • 计算雅可比矩阵 ( \mathbf{H}_k ) 和卡尔曼增益 ( \mathbf{K}_k )。
   • 融合加速度计观测值，更新后验状态 ( \hat{\mathbf{x}}_k ) 和协方差 ( \mathbf{P}_k )。

三、Matlab代码实现与关键步骤

1. 初始化参数

% 系统参数
dt = 0.01;          % 采样时间
g = 9.81;           % 重力加速度
sigma_w = 0.01;     % 陀螺仪噪声标准差
sigma_a = 0.1;      % 加速度计噪声标准差
% EKF初始状态
q0 = [1; 0; 0; 0];  % 初始四元数（水平姿态）
b_w0 = [0; 0; 0];   % 初始陀螺仪偏置
x0 = [q0; b_w0];    % 状态向量
P0 = eye(7) * 0.1;  % 初始协方差矩阵

2. 预测步实现

function [x_pred, P_pred] = ekf_predict(x, P, omega_m, dt)
    % 提取状态
    q = x(1:4);
    b_w = x(5:7);
    % 预测角速度（扣除偏置）
    omega_true = omega_m - b_w;
    % 四元数更新（一阶近似）
    q_dot = 0.5 * [0; omega_true] * q' - 0.5 * q * [0; omega_true]';
    q_pred = q + q_dot * dt;
    q_pred = q_pred / norm(q_pred); % 归一化
    % 状态转移矩阵（简化版）
    F = eye(7);
    F(1:4, 5:7) = -0.5 * dt * [q(2:4)'; -q(1)*eye(3) + skew(q(2:4))];
    % 过程噪声协方差
    Q = diag([sigma_w^2*dt*ones(3,1); sigma_w^2*dt*ones(3,1); 0.001*ones(1,3)]);
    % 预测协方差
    P_pred = F * P * F' + Q;
    x_pred = [q_pred; b_w];
end
function S = skew(w)
    % 构造斜对称矩阵
    S = [0, -w(3), w(2);
         w(3), 0, -w(1);
         -w(2), w(1), 0];
end

3. 更新步实现

function [x_upd, P_upd] = ekf_update(x_pred, P_pred, acc_m)
    % 提取状态
    q_pred = x_pred(1:4);
    % 计算预期重力方向（机体坐标系）
    R = quat2rotm(q_pred');
    h_x = R * [0; 0; g];
    % 观测雅可比矩阵
    H = [gradient_h(q_pred, acc_m), zeros(3,3)];
    % 观测噪声协方差
    R_obs = sigma_a^2 * eye(3);
    % 卡尔曼增益
    K = P_pred * H' / (H * P_pred * H' + R_obs);
    % 更新状态
    z = [0; 0; g] - h_x; % 观测残差
    x_upd = x_pred + K * z;
    x_upd(1:4) = x_upd(1:4) / norm(x_upd(1:4)); % 重新归一化四元数
    % 更新协方差
    P_upd = (eye(7) - K * H) * P_pred;
end
function H = gradient_h(q, acc_m)
    % 计算观测函数h(q)的雅可比矩阵（简化版）
    epsilon = 1e-6;
    H = zeros(3,4);
    for i = 1:4
        q_pert = q;
        q_pert(i) = q_pert(i) + epsilon;
        R_pert = quat2rotm(q_pert');
        h_pert = R_pert * [0; 0; g];
        q_neg = q;
        q_neg(i) = q_neg(i) - epsilon;
        R_neg = quat2rotm(q_neg');
        h_neg = R_neg * [0; 0; g];
        H(:,i) = (h_pert - h_neg) / (2*epsilon);
    end
end

4. 主循环示例

% 模拟传感器数据
N = 1000;
omega_true = [0.1; 0.05; 0.02]; % 真实角速度
acc_true = [0; 0; -g];          % 真实加速度
omega_m = omega_true + sigma_w * randn(3,N);
acc_m = acc_true + sigma_a * randn(3,N);
% EKF主循环
x_est = x0;
P_est = P0;
q_est = zeros(4,N);
for k = 1:N
    % 预测步
    [x_pred, P_pred] = ekf_predict(x_est, P_est, omega_m(:,k), dt);
    % 更新步
    [x_est, P_est] = ekf_update(x_pred, P_pred, acc_m(:,k));
    % 保存估计结果
    q_est(:,k) = x_est(1:4);
end

四、性能优化与工程实践建议

1. 数值稳定性：

   • 定期对四元数和协方差矩阵进行归一化与正定化处理。
   • 使用QR分解替代直接矩阵求逆，避免病态矩阵问题。

2. 传感器融合扩展：

   • 加入磁力计数据以估计偏航角，构建完整姿态观测方程。
   • 采用自适应EKF动态调整过程噪声 ( \mathbf{Q} ) 和观测噪声 ( \mathbf{R} )。

3. 实时性优化：

   • 固定点数实现替代浮点运算，适配嵌入式处理器。
   • 简化雅可比矩阵计算，利用解析表达式替代数值差分。

五、结论与展望

本文通过扩展卡尔曼滤波实现了四旋翼无人机的高精度姿态估计，Matlab代码验证了算法在噪声环境下的鲁棒性。未来工作可探索以下方向：

1. 结合深度学习模型处理非高斯噪声。
2. 研究多传感器紧耦合融合方案。
3. 开发基于ROS的实时仿真平台，加速算法落地。

（全文约1500字，完整代码与仿真数据见附件）