【四旋翼】六自由度四旋翼动力学仿真与PID控制系统设计Matlab实现
✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。
🍎完整代码获取 定制创新 论文复现点击：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：做科研，博学之、审问之、慎思之、明辨之、笃行之，是为：博学慎思，明辨笃行。
🔥 内容介绍
一、引言
四旋翼飞行器因其结构简单、机动性强，在航拍、物流、监测等领域得到广泛应用。深入理解其六自由度动力学特性，并设计有效的 PID 控制系统，对于实现四旋翼飞行器的稳定、精准飞行至关重要。通过动力学仿真，我们能够模拟飞行器在各种情况下的运动状态，为 PID 控制器的设计与优化提供依据。
二、六自由度四旋翼动力学模型
（一）基本假设与坐标系定义
- 基本假设
：假设四旋翼飞行器为刚体，忽略其弹性变形；旋翼产生的升力和扭矩与旋翼转速呈线性关系；不考虑空气压缩性等复杂空气动力学效应。
⛳️ 运行结果
📣 部分代码
function dydt = Motion_ABC(t, y, Omega_hover, I, m, g, l, k, b)
% Get phi, theta, psi, p, q, and r
phi = y(7); theta = y(8); psi = y(9);
p = y(10); q = y(11); r = y(12);
% Create omega for rotors
Omega = Omega_hover * ones(4,1);
% If statements to find Omega
if t < 1
Omega = Omega_hover + 70 * sin(2*pi*t/4);
Omega = ones(4,1) * Omega;
elseif t < 2
Omega = Omega_hover - 77 * sin(2*pi*t/4);
Omega = ones(4,1) * Omega;
elseif t < 3
Omega(2) = sqrt(Omega_hover^2 - 70^2 * sin(2*pi*(t-2)/4));
Omega(4) = sqrt(Omega_hover^2 + 70^2 * sin(2*pi*(t-2)/4));
elseif t < 4
Omega(2) = sqrt(Omega_hover^2 + 70^2 * sin(2*pi*(t-2)/4));
Omega(4) = sqrt(Omega_hover^2 - 70^2 * sin(2*pi*(t-2)/4));
elseif t < 5
Omega(1) = sqrt(Omega_hover^2 - 70^2 * sin(2*pi*(t-4)/4));
Omega(3) = sqrt(Omega_hover^2 + 70^2 * sin(2*pi*(t-4)/4));
else
Omega(1) = sqrt(Omega_hover^2 + 70^2 * sin(2*pi*(t-4)/4));
Omega(3) = sqrt(Omega_hover^2 - 70^2 * sin(2*pi*(t-4)/4));
end
% Get T and Tau
T = k * sum(Omega.^2);
L = k * l * (-Omega(2)^2 + Omega(4)^2);
M = k * l * (-Omega(1)^2 + Omega(3)^2);
N = b * (Omega(1)^2 - Omega(2)^2 + Omega(3)^2 - Omega(4)^2);
Tau = [L; M; N];
% Rotation matrix (inertial to body)
C_IB = [ cos(theta)*cos(psi), cos(theta)*sin(psi), -sin(theta);
sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi), sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi), sin(phi)*cos(theta);
cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi), cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi), cos(phi)*cos(theta)];
% Body to inertial rotation
C_BI = C_IB';
Thrust = C_BI(:,3);
% Get r double dot
r_dd = [0;0;-g] + (T/m) * Thrust;
% Get w and Omega_dot
w = [p; q; r];
Omega_dot = I \ ( - cross(w, I*w) + Tau);
% Euler angle rates
A = [1, sin(phi)*tan(theta), cos(phi)*tan(theta);
0, cos(phi), -sin(phi);
0, sin(phi)/cos(theta), cos(phi)/cos(theta)];
Euler = A \ w;
if t > 4
Euler(1) = 0;
end
% dydt
dydt = zeros(12,1);
dydt(1:3) = y(4:6);
dydt(4:6) = r_dd;
dydt(7:9) = Euler;
dydt(10:12) = Omega_dot;
end