这一部分强烈建议看一篇知乎文章：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'; endCbn就是旋转矩阵。
这一部分就较为简单了，位置信息和角度信息Simulink传到workspace后利用这些数据绘制就行了，另外Simulink的仿真步长设置为定步长且设置小一些，这样可以仿真出实际时间流逝的情况。
%仿真轨迹与绘制 clf len = length(tout); xmax = 0; ymax = 0; zmax = 0; xmin = 0; ymin = 0; zmin = 0; for i = 1:len figure(1); if(x(i)>=xmax) xmax = x(i); end if(y(i)>=ymax) ymax = y(i); end if(z(i)>=zmax) zmax = z(i); end if(x(i)<=xmin) xmin = x(i); end if(y(i)<=ymin) ymin = y(i); end if(z(i)<=zmin) zmin = z(i); end limitmin = 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));%绘制四旋翼 try delete(h1);delete(h2);delete(point); plot3([x(i-1),x(i)],[y(i-1),y(i)],[z(i-1),z(i)],"LineWidth",2) catch end h1 = 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四旋翼跟踪绿色标记目标点：
姿态角曲线，这里设置的psai = 0.2 rad 可以看到，整个控制系统是稳定的，轨迹跟踪也能实现，PD控制器搭建成功。
simulink模型：https://download.csdn.net/download/weixin_43145941/13712261
我已经写好本文的详细教程并将pdf上传至如下网址，有需要的同学可以根据文中内容一步步做下去，搭建自己的控制系统： 四旋翼飞行器的Simulink仿真与PD串级控制轨迹跟踪仿真pdf教程下载地址
