飞行器控制实验报告剖析

dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3)); if abs(imag(dq))>0 dq=0.0000001; end

q(k)=q(k-1)+dq; o(k)=o(k-1)+K*dq; a(k)=o(k)-q(k);

c1=r(k-1)*sin(b)/sin(a(k)+b); c2=r(k-1)*sin(a(k))/sin(a(k)+b);

c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b)); x1(k)=ptr(1,k-1)+c2/s_t*(ptr(1,k)-ptr(1,k-1)); y1(k)=ptr(2,k-1)+c2/s_t*(ptr(2,k)-ptr(2,k-1)); z1(k)=ptr(3,k-1)+c2/s_t*(ptr(3,k)-ptr(3,k-1)); x(k)=pmr(1,k-1)+s_m/c1*(x1(k)-pmr(1,k-1)); y(k)=pmr(2,k-1)+s_m/c1*(y1(k)-pmr(2,k-1)); z(k)=pmr(3,k-1)+s_m/c1*(z1(k)-pmr(3,k-1)); pmr(:,k)=[x(k);y(k);z(k)];

r(k)=sqrt((ptr(1,k)-pmr(1,k))^2+(ptr(2,k)-pmr(2,k))^2+(ptr(3,k)-pmr(3,k))^2); if r(k)<0.06; break; end; end

figure(1);

plot3(pmr(1,1:k),pmr(2,1:k),pmr(3,1:k),'k',ptr(1,:),ptr(2,:),ptr(3,:)); axis([0 25 0 5 0 25]); grid on

当K=3时 仿真图像如下图所示：