四旋翼无人机轨迹跟踪matlab仿真

编写位置模型

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;

位置控制器

u1x=-c1*dxe+K1*dx1/m+ddxd-k1*s1-eta1*sat_s1;
u1y=-c2*dye+K2*dy/m+ddyd-k2*s2-eta2*sat_s2;
u1z=-c3*dze+g+K3*dz/m+ddzd-k3*s3-eta3*sat_s3;

姿态控制器

u2=-c4*dthetae+l*K4*dtheta/I1+ddthetad-k4*s4-eta4*sat_s4;
u3=-c5*dpsie+l*K5*dpsi/I2+ddpsid-k5*s5-eta5*sat_s5;
u4=-c6*dphie+l*K6*dphi/I3+ddphid-k6*s6-eta6*sat_s6;

matlab仿真图

Figure1(无人机跟踪轨迹)

Figure2(无人机位置跟踪误差)

Figure3(无人机姿态角变化)