基于领导跟随法的四旋翼无人机编队matlab仿真

参数设置

m=2;l=0.2;g=9.8;
K1=0.01;K2=0.01;K3=0.01;K4=0.012;K5=0.012;K6=0.012;
I1=1.25;I2=1.25;I3=2.5;

c1=2;c2=2;c3=2;
c4=10;c5=10;c6=10;

k1=2;k2=2;k3=2;
k4=20;k5=20;k6=20;

eta1=0.10;eta2=0.10;eta3=0.10;
eta4=0.10;eta5=0.10;eta6=0.10;
phid=pi/3;

位置模型

ddx=u1*(cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi))-K1*dx1/m;
ddy=u1*(sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi))-K2*dy/m;
ddz=u1*(cos(phi)*cos(psi))-g-K3*dz/m;

姿态模型

ddtheta=u2-l*K4*dtheta/I1+d4;
ddpsi=u3-l*K5*dpsi/I2+d5;
ddphi=u4-K6*dphi/I3+d6;

无人机编队模型

dex1 = -(nanmdayd-ey1)*dpsi+vLx-vF1x*cos(epsi1)+vF1y*sin(epsi1);
dey1 = (nanmdaxd-ex1)*dpsi+v