【无人机】基于EKF的四旋翼无人机姿态估计（Matlab实现）

  💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

---

💥1 概述

基于扩展卡尔曼滤波（EKF）的四旋翼无人机姿态估计是一种常用的技术，用于推断无人机在空间中的姿态（即俯仰、横滚和偏航角度）。该方法结合了惯性测量单元（IMU）和其他传感器的数据，例如陀螺仪和加速度计，以及可能的外部信息，如磁力计或视觉信息，以提高姿态估计的准确性和稳定性。在这种方法中，IMU提供了关于无人机的角速度和加速度的测量值，但由于积分误差和传感器噪声，单独使用IMU可能会导致姿态估计的漂移。为了解决这个问题，EKF结合了IMU的测量值和四旋翼动力学模型，以及其他传感器提供的信息，通过状态估计和卡尔曼滤波技术，来不断地校正和更新对无人机姿态的估计。具体来说，EKF首先根据四旋翼动力学模型预测下一个时刻的姿态状态，并将这个预测值与来自IMU的实际测量进行比较。然后，根据测量与模型之间的残差，以及预测姿态的不确定性，EKF通过卡尔曼滤波算法计算出对姿态的最优估计，并更新估计的不确定性。通过持续地整合IMU数据和其他传感器的信息，并使用EKF来进行状态估计和滤波，可以实现对四旋翼无人机姿态的高精度和稳定的实时估计，从而为飞行控制和导航提供了可靠的基础。

📚2 运行结果

主函数部分代码：

%动态仿真实验结果
%无人机在实际飞行过程中姿态角会不断改变，所以仿真过程中使无人机的姿态角，角速度随机游走
%即每次循环对 4 维输入 u1，u2，u3，u4随机赋值

%俯仰角、俯仰角速度、横滚角、横滚角速度、偏航角、偏航角速度
%状态量：X = [x, vx, y, vy, z, vz]
%控制量：U = [u1, u2, u3, u4]        u2,u3,u4:无人机在坐标系{B}三个轴上的升力矩分量[廖坤男公式(2-22)(2-27)]

%正态分布的随机变量通过非线性系统后就不再是正态的了

close all;
clear all;

%常系数
L= 0.3875;      %无人机质心到电机转轴的距离 单位(m) 
Ix = 0.05887;   %绕机体坐标系三个轴的转动惯量 单位(kg·m^2) 
Iy = 0.05887;
Iz = 0.13151;
g = 9.81;       %重力加速度 单位(N/kg)

%动力学方程的常系数
a1 = -(Iy - Iz)/Ix;
a2 = -(Iz - Ix)/Iy;
a3 = -(Ix - Iy)/Iz;  
b1 = L/Ix;
b2 = L/Iy;
b3 = 1/Iz;

Ts = 0.1;               %采样时间
t = 5;                  %仿真时间
len = fix(t/Ts);        %仿真步数 50
n = 6;                  %状态维度
w = 0.1;                %过程标准差 状态转移方程的w                                     这几个参数？？？
v = 0.5;                %测量标准差 观测方程的v
Q = w^2*eye(n);         %过程方差
R = v^2;                %测量值的方差

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

[1]岳鹍,张月新,宋光坤.基于虚拟力分析的全柔性共轴旋翼无人机路径跟踪方法[J/OL].实验技术与管理:1-11[2024-04-27].http://kns.cnki.net/kcms/detail/11.2034.T.20240425.1624.002.html.

[2]王清华,胡永兵,李迎松.基于四旋翼无人机的嵌入式系统实践教学设计[J/OL].实验室研究与探索:1-5[2024-04-27].http://kns.cnki.net/kcms/detail/31.1707.T.20240418.0954.026.html.

🌈4 Matlab代码实现