飞行器控制实验报告

Harbin Institute of Technology飞行器制导与控制实验报告专 业： 自动化 班 级： 学 号： 姓 名： 设计时间： 2015/12/12 上机实验1：使用四阶龙格库塔法求解微分方程先定义参数，初值条件可以自己任取。1. 源程序：function x,y = M1(fun,x0,xt,y0,PointNum)if nargin4 | PointNum=0 PointNum=100;endif nargin0b=0.;endifabs(imag(dq)0dq=0.;endq(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);dq=a(k)-acos(c1-s_m)2+c32-(c2-s_t)2)/(2*(c1-s_m)*c3);ifabs(imag(dq)0dq=0.;endq(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);dq=a(k)-acos(c1-s_m)2+c32-(c2-s_t)2)/(2*(c1-s_m)*c3);ifabs(imag(dq)0dq=0.;endq(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);dq=a(k)-acos(c1-s_m)2+c32-(c2-s_t)2)/(2*(c1-s_m)*c3);ifabs(imag(dq)0dq=0.;endq(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);dq=a(k)-acos(c1-s_m)2+c32-(c2-s_t)2)/(2*(c1-s_m)*c3);ifabs(imag(dq)0dq=0.;endq(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);ifr(k)0.06;break;end;endfigure(1);plot3(pmr(1,