比例導引法Matlab仿真

1、%三維制導模型，比例導引法求解%源代碼作者不詳，注釋人：lylogn%Modifiedbylylogn,2012年4月17日clearall;closeall;clcdt=0.1;%仿真時間步長alpha=pi/6;v_t=0.42;s_t=v_t*dt;%目標以0.42的速度沿alpha的角方向勻速飛行，s_t為目標在單位仿真步長前進的距離v_m=0.60;s_m=v_m*dt;%s_m為導彈在單位仿真步長沿目前速度方向前進的距離x(1)=0;y(1)=1.0;z(1)=0;pmr(:,1)=x(1);y(1);z(1);%導彈初始位置，在坐標原點ptr(:,1)=25;5;7;%目標初始位

2、置K=3;%比例導引系數q(1)=0;%初始的視線角，設定參考線為t和m初始位置的連線o(1)=0;%初始導彈速度向量方向角a(1)=0;%初始導彈相對目標的運動速度向量的方向角for(k=2:600)ptr(:,k尸ptr(1,1)-v_t*cos(alpha)*dt*k;ptr(2,1);ptr(3,1)+v_t*sin(alpha)*k*dt;%目標運行軌跡方程，勻速直線運動r(k-1)=sqrt(ptr(1,k-1)-pmr(1,k-1)A2+(ptr(2,k-1)-pmr(2,k-1)A2+(ptr(3,k-1)-pmr(3,k-1)A2);%k-1時刻導彈與目標在三維空間中的歐氏距

3、離c=sqrt(ptr(1,k)-pmr(1,k-1)A2+(ptr(2,k)-pmr(2,k-1)A2+(ptr(3,k)-pmr(3,k-1)A2);%目標k時刻位置與導彈k-1時刻位置間的距離b=acos(r(k-1)A2+s_tA2-cA2)/(2*r(k-1)*s_t);%此處參見公式一%dq=acos(r(k-1)A2-s_tA2+cA2)/(2*r(k-1)*c);%k-1時刻到k時刻的視線角變化量(假設導彈不動，目標移動)%此處參見圖一%ifabs(imag(b)0%如果acos的值出現(xiàn)虛數，則說明該角度一定很小，對其進行近似操作b=0.0000001;endifabs(ima

4、g(dq)0%同上dq=0.0000001;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);%計算k-1時刻角b所對邊的長度%此處參見公式二%c2=r(k-1)*sin(a(k)/sin(a(k)+b);%計算k-1時刻角a(k)所對邊的長度c3=sqrt(c1-s_m)A2+(c2-s_t)A2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b);%計算k時刻導彈m與目標t之間的距離(在導彈不動，目

5、標移動的假設條件下)，為假值dq=a(k)-acos(c1-s_m)A2+c3A2-(c2-s_t)A2)/(2*(c1-s_m)*c3);%k-1時刻到k時刻的視線角變化量(假設導彈移動，目標也移動)，以下代碼重復以上過程，為假值%此處參見圖二%ifabs(imag(dq)0dq=0.0000001;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)A2+(c2-s_t)A2+2*(c1-s_m

6、)*(c2-s_t)*cos(a(k)+b);%計算k時刻導彈m與目標t之間的距離(在導彈移動，目標也移動的假設條件下)，逼近真值，以下計算使之更加精確dq=a(k)-acos(c1-s_m)A2+c3A2-(c2-s_t)A2)/(2*(c1-s_m)*c3);%k-1時刻到k時刻的視線角變化量(假設導彈移動，目標也移動)，以下代碼重復以上過程，為真值ifabs(imag(dq)0dq=0.0000001;end%?dedbylylogn2012.04.17,Tomakedqgetclosertoitstruevalueq(k)=q(k-1)+dq;o(k)=o(k-1)+K*dq;a(k)

7、=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)A2+(c2-s_t)A2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b);%計算k時刻導彈m與目標t之間的距離(在導彈移動，目標也移動的假設條件下)，逼近真值，以下計算使之更加精確dq=a(k)-acos(c1-s_m)A2+c3A2-(c2-s_t)A2)/(2*(c1-s_m)*c3);%k-1時刻到k時刻的視線角變化量(假設導彈移動，目標也移動)，以下代碼重復以上過程，為真值ifabs(imag(dq

8、)0dq=0.0000001;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)A2+(c2-s_t)A2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b);%計算k時刻導彈m與目標t之間的距離(在導彈移動，目標也移動的假設條件下)，逼近真值，以下計算使之更加精確dq=a(k)-acos(c1-s_m)A2+c3A2-(c2-s_t)A2)/(2*(c1-s_m)*c3);%k-1時

9、刻到k時刻的視線角變化量(假設導彈移動，目標也移動)，以下代碼重復以上過程，為真值ifabs(imag(dq)0dq=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)A2+(c2-s_t)A2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b);%計算k時刻導彈m與目標t之間的距離，終于近似為真值了x1(k)=ptr(1,k-1)+c2/s_t*(ptr(1,k

10、)-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);%計算出角b所對邊與目標運動軌跡的交點：(x1,y1,z1)%參見公式三%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);%計算出導彈k時刻所運動到的位置：(x,y,z)%參見公式三%pmr(:,

11、k)=x(k);y(k);z(k);r(k)=sqrt(ptr(1,k)-pmr(1,k)A2+(ptr(2,k)-pmr(2,k)A2+(ptr(3,k)-pmr(3,k)A2);ifr(k)0.06;break;end;endsprintf(遭遇時間：%3.1f,0.1*k);figure(1);plot3(pmr(1,1:k),pmr(2,1:k),pmr(3,1:k),k,ptr(1,:),ptr(2,:),ptr(3,:);axis(02505025);text(x(180),y(180),z(180),rightarrow比例導引律制導下的導彈運動軌跡,);text(ptr(1,280),ptr(2,280),ptr(3,280),rightarrow目標運動軌跡,);gridon之后，鑒于程序中很多地方不結合模型圖也很難理解，將其中關鍵的圖例與公式提取如下：3味航上航天上B曰JINGUNIVERSITYOFAERONAUTICSANDASTRONAUTICS比例身和，三地剃與曼道畫蜂因梅是名覲叔理力）-GfexM切啞科訓2*n團-：的時刻事假設導彈不論目標制動工七卮飾現(xiàn)囪國二:k-L時卻倡資與彝詁廿仁