【四旋翼飞行器】模拟四旋翼飞行器的平移和旋转动力学（Simulink仿真实现）

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

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

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

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

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、Simulink仿真及文献

---

💥1 概述

摘要-由于民用和军用无人机(Unmanned Aerial Vehicles, UAV)的兴趣日益增长，对自主微型飞行机器人的研究得到了显著加强。本文总结了OS4项目建模和控制部分的最终结果，该项目的重点是四旋翼设计和控制。文章介绍了一个考虑到飞行器运动引起的空气动力学系数变化的仿真模型。利用该模型找到的控制参数成功地应用于直升机上，无需重新调整。本文的最后部分描述了控制方法(积分反馈)和我们提出的四旋翼完全控制方案(姿态、高度和位置)。最后，介绍了自主起飞、悬停、降落和避障的结果。

飞行物体一直以来都对人类产生着巨大的吸引力，鼓励各种研究和发展。这个项目始于2003年，当时机器人学界对无人机(Unmanned Aerial Vehicles, UAV)的发展表现出越来越大的兴趣。在拥挤环境下设计和控制无人机的科学挑战以及缺乏现有解决方案的情况非常激发人们的积极性。另一方面，军事和民用市场上广泛的应用领域鼓励了与无人机相关项目的资金支持。从项目开始就决定专注于一种特定的配置：四旋翼飞行器。这种选择不仅源于其动力学特性，这代表了一个有吸引力的控制问题，还源于设计问题。将传感器、执行器和智能集成到一个轻量级的垂直飞行系统中，并保持良好的运行时间并不是一件简单的事情。

A. 现状
在过去几年中，四旋翼飞行器控制的现状发生了巨大的变化。解决这个问题的项目数量大幅增加。这些项目中的大多数是基于商用玩具，如Draganflyer [1]，后来进行了修改以增加更多的传感和通信功能。只有少数几个团队解决了MFR设计问题。论文[2]列举了过去10年中一些最重要的四旋翼项目。Mesicopter项目[3]始于1999年，于2001年结束。它旨在研究厘米级四旋翼的可行性。E. Altug在他的论文中介绍了2003年的双摄像头视觉反馈控制[4]。

详细讲解见第4部分。

📚2 运行结果

Matlab代码：

% KF_setup.m           19/11/2013
%Quadrotor Sim 
% 
%
% Purpose: to declare initial values
%
%%
%% sensor noises
position_uncertainty_var = (20/3600*pi/180)^2*ones(3,1);
%% simulation set up
step_time = 0.5;                       % simulation step time(sec)
end_time  = 1000;                   % simulation end time (sec)
%end_time  = 86400;
%% attitude estimator gains
Tatd  = 0.5;                          % attitude estimator update time (sec)
Tqint = 0.5;                         % discrete quaternion integration period (sec)
Tsen_out = 0.5;                      % sensor output period (sec)
TkfProp = 0.5;                       % Kalman filter propagation period (sec)
KfupdatePeriodInCycle = 1;           % Kalman filter update period (propagation cycle)
f_bw_atd = 0.02;                     % attitude determination bandwidth (hz)
%f_bw_atd = 0.005;
zeta = 0.7;
Krp =  (2*pi*f_bw_atd)^2 * eye(3);
Kpp =  2*zeta*2*pi*f_bw_atd*eye(3);
qest0 = [0*1e-4; 0; 0; 1];                          % initial estimator quaternion
delta_west0 = zeros(3,1);                           % initial deviation of estimator angular rate (rad/sec)
max_delta_w = 0.1*pi/180;
delta_w_lim = 2e-4; %0.1/pi/Tqint;
delta_th_lim= 1e-4; %0.1*pi/180/Tqint;
q0 = [0; 0; 0; 1];   %% for estimate error standard deviation prediction calculation
wn=sqrt(diag(Krp));
k=sqrt((wn.^4+4*zeta^2)./(4*zeta*wn));
%% for using Lyapunove equation to solve for expected estimation error
C=[1 0];  K=[Kpp(1,1);Krp(1,1)];      A=[0    1;0 0]-K*C; B=K; 
H=[1 0];  K=[Kpp(1,1);Krp(1,1)]*Tatd; F=[1 Tatd;0 1]-K*H; G=K;
%% Kalman filter setups
Fmat = [eye(3) TkfProp*eye(3);zeros(3,3) eye(3)];
Hmat = [eye(3)  zeros(3,3)];
therr0 = max([abs(qest0(1:3)); 5*1e-4]);  % initial error estimate, assuming q0=[0 0 0 1]
P0 = diag([therr0^2*ones(1,3) 3e-6^2*ones(1,3)]);
R = TkfProp*KfupdatePeriodInCycle*diag(position_uncertainty_var);%1e-3^2*eye(3)*
Q = diag([1e-5^2*ones(1,3), 1e-7^2*ones(1,3)])*TkfProp;
max_rate = pi/180;
P0 = diag([1e-32*ones(1,3) 1e-5^2*ones(1,3)]);
Q = diag([1e-5^2*ones(1,3), 5e-6^2*ones(1,3)])*TkfProp;
max_bias = 1*pi/180/3600;
%% start simulation
Tcapt = Tsen_out;                              % sim variable capture rate (sec)

% KF_setup.m           19/11/2013
%Quadrotor Sim 
% 
%
% Purpose: to declare initial values
%
%%
%% sensor noises
position_uncertainty_var = (20/3600*pi/180)^2*ones(3,1);
%% simulation set up
step_time = 0.5;                       % simulation step time(sec)
end_time  = 1000;                   % simulation end time (sec)
%end_time  = 86400;
%% attitude estimator gains
Tatd  = 0.5;                          % attitude estimator update time (sec)
Tqint = 0.5;                         % discrete quaternion integration period (sec)
Tsen_out = 0.5;                      % sensor output period (sec)
TkfProp = 0.5;                       % Kalman filter propagation period (sec)
KfupdatePeriodInCycle = 1;           % Kalman filter update period (propagation cycle)
f_bw_atd = 0.02;                     % attitude determination bandwidth (hz)
%f_bw_atd = 0.005;
zeta = 0.7;
Krp =  (2*pi*f_bw_atd)^2 * eye(3);
Kpp =  2*zeta*2*pi*f_bw_atd*eye(3);
qest0 = [0*1e-4; 0; 0; 1];                          % initial estimator quaternion
delta_west0 = zeros(3,1);                           % initial deviation of estimator angular rate (rad/sec)
max_delta_w = 0.1*pi/180;
delta_w_lim = 2e-4; %0.1/pi/Tqint;
delta_th_lim= 1e-4; %0.1*pi/180/Tqint;
q0 = [0; 0; 0; 1];   

%% for estimate error standard deviation prediction calculation
wn=sqrt(diag(Krp));
k=sqrt((wn.^4+4*zeta^2)./(4*zeta*wn));
%% for using Lyapunove equation to solve for expected estimation error
C=[1 0];  K=[Kpp(1,1);Krp(1,1)];      A=[0    1;0 0]-K*C; B=K; 
H=[1 0];  K=[Kpp(1,1);Krp(1,1)]*Tatd; F=[1 Tatd;0 1]-K*H; G=K;
%% Kalman filter setups
Fmat = [eye(3) TkfProp*eye(3);zeros(3,3) eye(3)];
Hmat = [eye(3)  zeros(3,3)];
therr0 = max([abs(qest0(1:3)); 5*1e-4]);  % initial error estimate, assuming q0=[0 0 0 1]
P0 = diag([therr0^2*ones(1,3) 3e-6^2*ones(1,3)]);
R = TkfProp*KfupdatePeriodInCycle*diag(position_uncertainty_var);%1e-3^2*eye(3)*
Q = diag([1e-5^2*ones(1,3), 1e-7^2*ones(1,3)])*TkfProp;
max_rate = pi/180;
P0 = diag([1e-32*ones(1,3) 1e-5^2*ones(1,3)]);
Q = diag([1e-5^2*ones(1,3), 5e-6^2*ones(1,3)])*TkfProp;
max_bias = 1*pi/180/3600;
%% start simulation
Tcapt = Tsen_out;                              % sim variable capture rate (sec)

🎉3 参考文献

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

[1]杨维,周冰倩,吴志刚,等.四旋翼飞行器的设计与仿真分析[J].计算机光盘软件与应用, 2014, 17(16):2.DOI:CNKI:SUN:GPRJ.0.2014-16-154.

[2]高燕,虞旦.四旋翼飞行器的建模及控制算法仿真[J].工业控制计算机, 2014(9):3.DOI:10.3969/j.issn.1001-182X.2014.09.045.

[3]乔维维.四旋翼飞行器飞行控制系统研究与仿真[D].中北大学,2012.DOI:10.7666/d.D316360.

🌈4 Matlab代码、Simulink仿真及文献