【四旋翼】基于Matlab的四旋翼飞行器建模仿真综合实验，定点悬停、航路跟踪、编队飞行

✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、算法改进、程序设计科研仿真。

🍎完整代码获取 定制创新 论文复现私信

🍊个人信条：做科研，博学之、审问之、慎思之、明辨之、笃行之，是为：博学慎思，明辨笃行。

1. 相关介绍

1.1 四旋翼飞行器建模思路

对四旋翼飞行器建模其实就是在 Simulink 中模仿一架实际四旋翼的输入输出特性，能够正确反映飞行器在给定转速指令（如 PWM 波）下的输出情况，比如角速度、速度等，如果给定初始状态，还应能获得姿态、位置等信息。

1.2 广义电机模型

广义电机模型输入为使能转速，输出为每个电机产生的力与力矩，包括两大部分：

（1）电机动力学模型：输入为转速指令（实际系统中应为机载控制器发出的 PWM波或其他调节电调输入占空比的信号），输出为推力与力矩。

（2）使能转速-期望转速转换模块：输入为“使能转速”，输出为四个电机的期望转速。“使能转速”包括一个常量𝜔௛ 与三个变量𝛥𝜔ி , 𝛥𝜔ఏ , 𝛥𝜔ట ，使能转速的每个分量描述其使飞行器运动状态产生变化的效果，下文将详细阐释。在整个四旋翼飞行器控制系统中，使能转速是姿态控制器的输出。为使整个系统模块化程度更高，将此转换模块与电机动力学模型整合为了广义电机模型。

2. 运行效果展示

3. 部分代码呈现

function [sys,x0,str,ts,simStateCompliance] = double_atti(t,x,u,flag,dt,J,K)

%

switch flag,

%%%%%%%%%%%%%%%%%%

% Initialization %

%%%%%%%%%%%%%%%%%%

case 0,

[sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(dt);

%%%%%%%%%%%%%%%

% Derivatives %

%%%%%%%%%%%%%%%

case 1,

sys=mdlDerivatives(t,x,u);

%%%%%%%%%%

% Update %

%%%%%%%%%%

case 2,

sys=mdlUpdate(t,x,u,dt);

%%%%%%%%%%%

% Outputs %

%%%%%%%%%%%

case 3,

sys=mdlOutputs(t,x,u,J,K);

%%%%%%%%%%%%%%%%%%%%%%%

% GetTimeOfNextVarHit %

%%%%%%%%%%%%%%%%%%%%%%%

case 4,

sys=mdlGetTimeOfNextVarHit(t,x,u);

%%%%%%%%%%%%%

% Terminate %

%%%%%%%%%%%%%

case 9,

sys=mdlTerminate(t,x,u);

%%%%%%%%%%%%%%%%%%%%

% Unexpected flags %

%%%%%%%%%%%%%%%%%%%%

otherwise

DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));

end

% end sfuntmpl

%

%=============================================================================

% mdlInitializeSizes

% Return the sizes, initial conditions, and sample times for the S-function.

%=============================================================================

%

function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes(dt)

%

% call simsizes for a sizes structure, fill it in and convert it to a

% sizes array.

%

% Note that in this example, the values are hard coded. This is not a

% recommended practice as the characteristics of the block are typically

% defined by the S-function parameters.

%

sizes = simsizes;

sizes.NumContStates = 0;

sizes.NumDiscStates = 15;%Rd wd dwd

sizes.NumOutputs = 7;% M notfullrank

sizes.NumInputs = 21;%R w Rd

sizes.DirFeedthrough = 1;

sizes.NumSampleTimes = 1; % at least one sample time is needed

sys = simsizes(sizes);

%

% initialize the initial conditions

%

x0 =[1 0 0 0 1 0 0 0 1 0 0 0 0 0 0]';

%

% str is always an empty matrix

%

str = [];

%

% initialize the array of sample times

%

ts = [dt 0];

% Specify the block simStateCompliance. The allowed values are:

% 'UnknownSimState', < The default setting; warn and assume DefaultSimState

% 'DefaultSimState', < Same sim state as a built-in block

% 'HasNoSimState', < No sim state

% 'DisallowSimState' < Error out when saving or restoring the model sim state

simStateCompliance = 'UnknownSimState';

% end mdlInitializeSizes

%

%=============================================================================

% mdlDerivatives

% Return the derivatives for the continuous states.

%=============================================================================

%

function sys=mdlDerivatives(t,x,u)

sys = [];

% end mdlDerivatives

%

%=============================================================================

% mdlUpdate

% Handle discrete state updates, sample time hits, and major time step

% requirements.

%=============================================================================

%

function sys=mdlUpdate(t,x,u,dt)

%内部状态读取

pre_Rd=reshape(x(1:9),[3,3]);

pre_wd=reshape(x(10:12),[3,1]);

%反馈信息处理

Rd=reshape(u(13:21),[3,3]);

if trace(pre_Rd)==0

wd=[0;0;0];

else

Wd=inv(Rd)*(Rd-pre_Rd)/dt;

wd=vee(Wd);

end

if norm(wd)==0

dwd=[0;0;0];

else

dwd=(wd-pre_wd)/dt;

end

% wd=reshape(u(16:18),[3,1]);

% dwd=[0;0;0];

if norm(wd)>20

wd=wd/norm(wd)*20;

end

%

if norm(dwd)>100

dwd=dwd/norm(dwd)*100;

end

sys = [reshape(Rd,[9,1]);wd;dwd];

% end mdlUpdate

%

%=============================================================================

% mdlOutputs

% Return the block outputs.

%=============================================================================

%

function sys=mdlOutputs(t,x,u,J,K)

%内部状态读取

Rd=reshape(x(1:9),[3,3]);

wd=reshape(x(10:12),[3,1]);

dwd=reshape(x(13:15),[3,1]);

%反馈信息处理

R=reshape(u(1:9),[3,3]);

w=reshape(u(10:12),[3,1]);

Rr=Rd'*R;

eR=0.5*vee(Rr-Rr');

ew=w-R'*Rd*wd;

hat_ew=hat(ew);

tmp1=Rr*hat_ew*hat(w)-hat(wd)*Rr*hat_ew- hat(dwd)*Rr;

Ad=tmp1-tmp1';

RB=[Rr(2,2)+Rr(3,3) -Rr(2,1) -Rr(3,1);

-Rr(1,2) Rr(1,1)+Rr(3,3) -Rr(3,2);

-Rr(1,3) -Rr(2,3) Rr(1,1)+Rr(2,2)];

e=vee(Rr-Rr');

de=vee(Rr*hat_ew+hat_ew*Rr');

if rank(RB)==3

B=RB*inv(J);

M=inv(B)*(-vee(Ad)-K*[e;de])+cross(w,J*w);

unfullrank=0;

else

M=-8.81/10*eR-2.54/10*ew+cross(w,J*w)-J*(hat(w)*Rr'*wd-Rr'*dwd);

unfullrank=1;

end

M(1:3)=-8.81/10*eR-2.54/10*ew+cross(w,J*w)-J*(hat(w)*Rr'*wd-Rr'*dwd);

unfullrank=1;

% q = dcm2quat(R');

% qd = dcm2quat(Rd');

% q_inv = quatinv(q); % 计算q的逆

% qe = quatmultiply(q_inv, qd); % 计算q的逆与qd的乘积

% w_cmd = 8 * sign(qe(1)) * qe(2:4);

% we = w_cmd' - w;

% sys =[we;unfullrank];

th1=0.3;

th2=0.1;

M(1)=deadzone(-th1,M(1),th1);

M(2)=deadzone(-th1,M(2),th1);

M(3)=deadzone(-th2,M(3),th2);

sys =[M;unfullrank;wd];

% end mdlOutputs

%

%=============================================================================

% mdlGetTimeOfNextVarHit

% Return the time of the next hit for this block. Note that the result is

% absolute time. Note that this function is only used when you specify a

% variable discrete-time sample time [-2 0] in the sample time array in

% mdlInitializeSizes.

%=============================================================================

%

function sys=mdlGetTimeOfNextVarHit(t,x,u)

sampleTime = 1; % Example, set the next hit to be one second later.

sys = t + sampleTime;

% end mdlGetTimeOfNextVarHit

%

%=============================================================================

% mdlTerminate

% Perform any end of simulation tasks.

%=============================================================================

%

function sys=mdlTerminate(t,x,u)

sys = [];

% end mdlTerminate

4. 参考文献

🍅更多免费数学建模和仿真教程关注领取

如果觉得内容不错，那就请分享和点个“在看”呗！