数学建模

这一部分强烈建议看一篇知乎文章：zinghd的回答
其最终模型结果：
在这里插入图片描述

其中部分符号的定义可以参考上面给的知乎链接。下面给出角度的定义和飞行器的外观。
在这里插入图片描述

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

开始仿真

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

欧拉角模型搭建

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

点进第一个红色模块（计算 $\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(......)$ ，这样根据这四个量就可以反解出四个螺旋桨的转动角速度 $w_i$ 了。
在这里插入图片描述

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$ 将其输入之前搭建的位置-角度模型即可形成闭环控制。

脚本绘制三维跟踪图

四旋翼飞行器的绘制

这里就采用两根线条组成飞行器，其中一个难点在于如何绘制欧拉角变换后的飞行器姿态，其实也很简单，就是将机体坐标乘以旋转矩阵就到了地球坐标，然后地球坐标的每一个分量加上飞行器的实际位置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: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

跟踪曲线的绘制

%绘制位置图像
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;