数学建模

这一部分强烈建议看一篇知乎文章：zinghd的回答
其最终模型结果：

其中部分符号的定义可以参考上面给的知乎链接。下面给出角度的定义和飞行器的外观。

对于上图给出的四旋翼飞行器，其x轴为向量M3M1，y轴为M4M2，z轴指向上方。
观察之前给出的四旋翼飞行器的数学模型我们发现前三个式子是飞行器位置的计算，使用到了三个欧拉角，后三个式子为三个欧拉角的计算，与飞行器的位置无关，因此四旋翼飞行器是一个耦合的系统，位置与角度耦合。

开始仿真

其实对四旋翼的仿真就是对上面6个微分方程进行Simulink搭建，搭建微分方程事实上是一个很直观简单的事情，主要就是利用好积分器。

欧拉角模型搭建

整体外观：

其中fcn主要是对三个欧拉角取 2 π 2\pi 2π的余数，方便后面的控制算法的实现（因为后面用PD控制存在比较，需要把角度都规定在 2 π 2\pi 2π内）
点进模块后会是如下外观：

点进第一个红色模块（计算 ϕ \phi ϕ）：

之后的两个欧拉角同理可以搭建（根据第五个式子和第六个式子）。这里的每一个角都由两个积分器组成（系统是二阶微分方程）

位置模型搭建

总体外观：

以x为例，根据第一个公式搭建，点进子模块后：

如果x搭建成功，y和z也就易如反掌，搭建xyz比角度要简单一点，但是xyz的计算是需要三个欧拉角的。

控制算法搭建

这里强烈建议去看一篇知乎文章：串级PD控制四旋翼
其控制框图如下图所示：

具体每一步的计算公式上面那篇知乎文章已经写得十分详细了，接下来将根据文章中的公式进行控制器搭建。

给定值

这里psai（偏航角）需要先给定，因此整个控制器事实上是需要先给定4个参考量的。
外环采用PD控制：

这里输入用到的就是xd，yd，zd，输出为Ux，Uy，Uz，它们会被用来下一步计算，这里建议再去看看上面知乎文章的公式，对照着看很清晰。
接下来是一个函数计算模块：

function [U1,phid,thetad] = fcn(Ux,Uy,Uz,psaid,m)U1 = m*sqrt(Ux^2+Uy^2+(Uz+9.81)^2);phid = asin((Ux*sin(psaid)-Uy*cos(psaid))*m/U1);thetad = asin((Ux*m-U1*sin(psaid)*sin(phid))/(U1*cos(psaid)*cos(phid)));
end

这里公式也请移步知乎文章。

内环为角度控制，依旧使用PD控制：

现在我们已经得到了U1，U2，U3，U4，这四个量中U1对应四个螺旋桨力之和，后三个量时对欧拉角产生变化作用的力，其来源于公式4,5,6中的 C T ( . . . . . . ) C_T(......) CT(......)，这样根据这四个量就可以反解出四个螺旋桨的转动角速度 w i w_i wi了。

function [w1,w2,w3,w4] = fcn(U1,U2,U3,U4)CT = 1.116e-5; dCT = 0.225*CT;matrixA = [CT,CT,CT,CT;0,CT,0,-CT;-CT,0,CT,0;-dCT,dCT,-dCT,dCT];w_2 = inv(matrixA)*[U1,U2,U3,U4]';w1 = sqrt(abs(w_2(1))); w2 = sqrt(abs(w_2(2)));w3 = sqrt(abs(w_2(3))); w4 = sqrt(abs(w_2(4)));
end

得到了 w i w_i wi将其输入之前搭建的位置-角度模型即可形成闭环控制。

脚本绘制三维跟踪图

四旋翼飞行器的绘制

这里就采用两根线条组成飞行器，其中一个难点在于如何绘制欧拉角变换后的飞行器姿态，其实也很简单，就是将机体坐标乘以旋转矩阵就到了地球坐标，然后地球坐标的每一个分量加上飞行器的实际位置x，y，z即可绘制出三维空间任意欧拉角和位置下的飞行器外观。
绘制四旋翼的函数：

function [point1_trans,point2_trans,point3_trans,point4_trans] = drone(phi,theta,psai)
Cbn = [cos(psai)*cos(theta),cos(psai)*sin(theta)*sin(phi)-sin(psai)*cos(phi),...sin(theta)*cos(phi)*cos(psai)+sin(phi)*sin(psai);...sin(psai)*cos(theta),cos(phi)*cos(psai)+sin(phi)*sin(theta)*sin(psai),...sin(theta)*cos(phi)*sin(psai)-sin(phi)*cos(psai);...-sin(theta),cos(theta)*sin(phi),cos(theta)*cos(phi)];x0 = 0; y0 = 0; z0 = 0;
point1 = [x0-1,y0,z0]; point2 = [x0+1,y0,z0];
point3 = [x0,y0-1,z0]; point4 = [x0,y0+1,z0];
point1_trans = Cbn*point1';point2_trans = Cbn*point2';
point3_trans = Cbn*point3';point4_trans = Cbn*point4';
end

Cbn就是旋转矩阵。

三维轨迹仿真

这一部分就较为简单了，位置信息和角度信息Simulink传到workspace后利用这些数据绘制就行了，另外Simulink的仿真步长设置为定步长且设置小一些，这样可以仿真出实际时间流逝的情况。

%仿真轨迹与绘制
clf
len = length(tout);
xmax = 0; ymax = 0; zmax = 0;
xmin = 0; ymin = 0; zmin = 0;
for i = 1:lenfigure(1);if(x(i)>=xmax)xmax = x(i);endif(y(i)>=ymax)ymax = y(i);endif(z(i)>=zmax)zmax = z(i);endif(x(i)<=xmin)xmin = x(i);endif(y(i)<=ymin)ymin = y(i);endif(z(i)<=zmin)zmin = z(i);endlimitmin = min(xmin,ymin); limitmax = max(xmax,ymax);xlim([limitmin-2,limitmax+2]),ylim([limitmin-2,limitmax+2]),zlim([-1,zmax+5])grid on;[point1_trans,point2_trans,point3_trans,point4_trans]=drone(phi(i),theta(i),psai(i));%绘制四旋翼trydelete(h1);delete(h2);delete(point);plot3([x(i-1),x(i)],[y(i-1),y(i)],[z(i-1),z(i)],"LineWidth",2)catchendh1 = plot3([x(i)+point1_trans(1),x(i)+point2_trans(1)],[y(i)+point1_trans(2),y(i)+point2_trans(2)],...[z(i)+point1_trans(3),z(i)+point2_trans(3)],"LineWidth",3,"Color","r");hold on;h2 = plot3([x(i)+point3_trans(1),x(i)+point4_trans(1)],[y(i)+point3_trans(2),y(i)+point4_trans(2)],...[z(i)+point3_trans(3),z(i)+point4_trans(3)],"LineWidth",3,"Color","b");point = scatter3(xd(i),yd(i),zd(i),100,"filled","g");set(gca,'ztick',0:20:z(i)+5)
%     pause(0.1)
end

跟踪曲线的绘制

%绘制位置图像
figure(2);clf;
subplot(1,3,1)
h1 = plot(tout,xd,"r","LineWidth",2);hold on;
h2 = plot(tout,x,"g","LineWidth",2);
legend([h1,h2],["参考位置xd","实际位置x"])
grid on;
subplot(1,3,2)
h3 = plot(tout,yd,"b","LineWidth",2);hold on;
h4 = plot(tout,y,"m","LineWidth",2);
legend([h3,h4],["参考位置yd","实际位置y"])
grid on;
subplot(1,3,3)
h5 = plot(tout,zd,"y","LineWidth",2);hold on;
h6 = plot(tout,z,"k","LineWidth",2);
legend([h5,h6],["参考位置zd","实际位置z"])
grid on;
%绘制角度图像
figure(3);clf;
h7 = plot(tout,phi,"LineWidth",2); hold on;
h8 = plot(tout,psai,"LineWidth",2);
h9 = plot(tout,theta,"LineWidth",2);hold off;
legend([h7,h8,h9],["phi","psai","theta"]),grid on;

仿真结果

四旋翼跟踪绿色标记目标点：

姿态角曲线，这里设置的psai = 0.2 rad

可以看到，整个控制系统是稳定的，轨迹跟踪也能实现，PD控制器搭建成功。

模型下载

simulink模型：https://download.csdn.net/download/weixin_43145941/13712261
模型2020a的

Tutorial撰写

我已经写好本文的详细教程并将pdf上传至如下网址，有需要的同学可以根据文中内容一步步做下去，搭建自己的控制系统：
四旋翼飞行器的Simulink仿真与PD串级控制轨迹跟踪仿真pdf教程下载地址

四个翅膀的飞机Simulink与轨迹跟踪仿真相关推荐

四旋翼飞行器轨迹跟踪仿真MATLAB simulink/simscape
四旋翼飞行器轨迹跟踪仿真MATLAB simulink/simscape ID:6929669117089277
四足机器人并联腿足端轨迹Matlab仿真
四足机器人并联腿足端轨迹Matlab仿真轨迹计算轨迹是分成两部分:摆线方程+水平线段首先,设置大腿关节轴心坐标为(0,0),根据机械设计的大腿小腿长度设置参数.此处设置大腿 l1=100mm,小 ...
差速机器人的纯轨迹跟踪仿真（Matlab）
差速机器人的纯轨迹跟踪仿真(Matlab) 刚入门,有的地方不对,烦请大家指正. 目录差速机器人的纯轨迹跟踪仿真(Matlab) 1 差速机器人运动模型 1.1 运动学分析建模 1.2 差速机器人的 ...
二轮车平衡车轨迹跟踪的Simulink仿真
最近做一些二轮平衡车的相关研究,做了一些分析,记录如下. 文章目录一.二轮平衡车模型分析? 二.小车初始状态分析三.Simulink模型建立四.轨迹追踪四.总结一.二轮平衡车模型分析? 二轮 ...
基于阻抗控制的工业机器人轨迹跟踪系统 Simulink/Simscape 仿真
本文是对文献 [1] 的复现. 文章目录更新日志关于阻抗控制离散化阻抗控制控制器的实现基于阻抗控制的工业机器人轨迹跟踪系统仿真 ■ 环境说明 ■ 系统说明 ■ 结果展示后记项目开源参 ...
空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂（双臂）轨迹跟踪控制matlab仿真程序
空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂(双臂)轨迹跟踪控制matlab仿真程序,含空间机器人动力学模型,PD控制程序,带仿真结果,可供二次开发学习 ID:672006146 ...
【Carsim Simulink自动驾驶仿真】基于MPC的轨迹跟踪控制
如果对Carsim的基础使用还不了解,可以参考:[Carsim Simulink自动驾驶仿真]基于MPC的速度控制如果对MPC算法原理不清楚,可以参考:如何理解MPC模型预测控制理论项目介绍: 教 ...
carsim+simulink联合仿真实现变道包含路径规划算法+mpc轨迹跟踪算法可选simulink版本和c++版本算法可以适用于弯道道路，弯道车道保持，弯道变道
carsim+simulink联合仿真实现变道包含路径规划算法+mpc轨迹跟踪算法可选simulink版本和c++版本算法可以适用于弯道道路,弯道车道保持,弯道变道 carsim内规划轨迹可视化 ...
carsim+simulink联合仿真实现变道包含路径规划算法+mpc轨迹跟踪算法
carsim+simulink联合仿真实现变道包含路径规划算法+mpc轨迹跟踪算法可选simulink版本和c++版本算法可以适用于弯道道路,弯道车道保持,弯道变道 carsim内规划轨迹可视化 ...

四个翅膀的飞机Simulink与轨迹跟踪仿真