模拟四旋翼飞行器的平移和旋转动力学（Matlab、Simulink仿真实现）-CSDN博客

本文链接：https://blog.csdn.net/weixin_61181717/article/details/143001641

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

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

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

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

目录

⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、Simulink仿真实现

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

飞行物体一直对人类具有极大的吸引力，促进了各种研究和发展。这个项目始于2003年，那时机器人社区对无人机（UAV）的发展表现出越来越浓厚的兴趣。在拥挤环境中设计和控制无人机的科学挑战以及缺乏现有解决方案的情况非常激励人。另一方面，无人机在军事和民用市场上的广泛应用领域鼓励了与无人机相关项目的资金投入。从这个项目的开始就决定专门研究一种特定配置：四旋翼机。该兴趣不仅来自于其动力学，这代表了一个有吸引力的控制问题，还来自于设计问题。将传感器、执行器和智能集成到一个重量轻、垂直飞行系统中并保持良好运行时间并不简单。

A. 现状
四旋翼机控制领域的现状在过去几年里发生了巨大变化。解决这一问题的项目数量显著增加。这些项目中大多数是基于像Draganflyer [1]这样的商用玩具，后来进行修改以提升感知和通信能力。只有少数几个团队解决了四旋翼机设计问题。论文[2]列出了过去十年中一些最重要的四旋翼项目。Mesicopter项目[3]始于1999年，于2001年结束。它旨在研究厘米尺度四旋翼的可行性。E. Altug在他的论文中提出了2003年双摄像头视觉反馈控制 [4]。Lozano教授的团队在四旋翼机设计和控制方面也有着较强的活动[5]。来自CEA（法国）的N. Guenard也正在研究室内四旋翼机的自主控制[6]。另一个有趣的项目是Starmac，它旨在展示大约1公斤四旋翼机的多智能体控制。

这个模拟器模拟了四旋翼飞行器的平移和旋转动力学。包括以下内容的建模：
- 四旋翼动力学
- 电机动力学
- 用于状态估计的卡尔曼滤波器
- 简单传感器模型/ADC转换
以下内容未被建模：
- 螺旋桨动力学
- 控制规律
- 电力子系统

这个模拟器可以用于：
- 系统可行性研究
- 系统性能评估和权衡
- 控制规律性能评估

请运行KF_setp.m来初始化卡尔曼滤波器变量。在模拟器中，请不要忘记声明系统输入变量和初始条件。打开“电机动力学”块的掩模来声明电机变量。

📚2 运行结果

部分代码：

%% 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]