比例導(dǎo)引法Matlab仿真

%三維制導(dǎo)模型，比例導(dǎo)引法求解%源代碼作者不詳，注釋人：lylogn%Modifiedbylylogn，2012年4月17日clearall;closeall;clc

dt=0.1;%仿真時間步長alpha=pi/6;v_t=0.42;s_t=v_t*dt;%目標(biāo)以0.42的速度沿alpha的角方向勻速飛行，s_t為目標(biāo)在單位仿真步長前進(jìn)的距離v_m=0.60;s_m=v_m*dt;%s_m為導(dǎo)彈在單位仿真步長沿目前速度方向前進(jìn)的距離x(1)=0;y(1)=1.0;z(1)=0;

pmr(:,1)=[x(1);y(1);z(1)];

%導(dǎo)彈初始位置，在坐標(biāo)原點(diǎn)ptr(:,1)=[25;5;7];

%目標(biāo)初始位置K=3;

%比例導(dǎo)引系數(shù)q(1)=0;

%初始的視線角，設(shè)定參考線為t和m初始位置的連線o(1)=0;

%初始導(dǎo)彈速度向量方向角a(1)=0;

%初始導(dǎo)彈相對目標(biāo)的運(yùn)動速度向量的方向角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];

%目標(biāo)運(yùn)行軌跡方程，勻速直線運(yùn)動r(k-1)=sqrt((ptr(1,k-1)-pmr(1,k-1))^2+(ptr(2,k-1)-pmr(2,k-1))^2+(ptr(3,k-1)-pmr(3,k-1))^2);%k-1時刻導(dǎo)彈與目標(biāo)在三維空間中的歐氏距離c=sqrt((ptr(1,k)-pmr(1,k-1))^2+(ptr(2,k)-pmr(2,k-1))^2+(ptr(3,k)-pmr(3,k-1))^2);

%目標(biāo)k時刻位置與導(dǎo)彈k-1時刻位置間的距離b=acos((r(k-1)^2+s_t^2-c^2)/(2*r(k-1)*s_t));

%%%此處參見公式一%%%dq=acos((r(k-1)^2-s_t^2+c^2)/(2*r(k-1)*c));%k-1時刻到k時刻的視線角變化量（假設(shè)導(dǎo)彈不動，目標(biāo)移動）%%%此處參見圖一%%%ifabs(imag(b))>0

%如果acos的值出現(xiàn)虛數(shù)，則說明該角度一定很小，對其進(jìn)行近似操作b=0.0000001;

end

ifabs(imag(dq))>0

%同上dq=0.0000001;

end

q(k)=q(k-1)+dq;

%更新視線角o(k)=o(k-1)+K*dq;

%更新導(dǎo)彈速度向量方向角a(k)=o(k)-q(k);

%更新導(dǎo)彈相對目標(biāo)的運(yùn)動速度向量的方向角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)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b));

%計算k時刻導(dǎo)彈m與目標(biāo)t之間的距離（在導(dǎo)彈不動，目標(biāo)移動的假設(shè)條件下），為假值dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));

%k-1時刻到k時刻的視線角變化量（假設(shè)導(dǎo)彈移動，目標(biāo)也移動），以下代碼重復(fù)以上過程，為假值%%%此處參見圖二%%%ifabs(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));

%計算k時刻導(dǎo)彈m與目標(biāo)t之間的距離（在導(dǎo)彈移動，目標(biāo)也移動的假設(shè)條件下），逼近真值，以下計算使之更加精確dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));

%k-1時刻到k時刻的視線角變化量（假設(shè)導(dǎo)彈移動，目標(biāo)也移動），以下代碼重復(fù)以上過程，為真值ifabs(imag(dq))>0

dq=0.0000001;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%?dedbylylogn2012.04.17，Tomake'dq'getclosertoitstruevalueq(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));

%計算k時刻導(dǎo)彈m與目標(biāo)t之間的距離（在導(dǎo)彈移動，目標(biāo)也移動的假設(shè)條件下），逼近真值，以下計算使之更加精確dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));

%k-1時刻到k時刻的視線角變化量（假設(shè)導(dǎo)彈移動，目標(biāo)也移動），以下代碼重復(fù)以上過程，為真值ifabs(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));

%計算k時刻導(dǎo)彈m與目標(biāo)t之間的距離（在導(dǎo)彈移動，目標(biāo)也移動的假設(shè)條件下），逼近真值，以下計算使之更加精確dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));

%k-1時刻到k時刻的視線角變化量（假設(shè)導(dǎo)彈移動，目標(biāo)也移動），以下代碼重復(fù)以上過程，為真值ifabs(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));

%計算k時刻導(dǎo)彈m與目標(biāo)t之間的距離，終于近似為真值了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));

%計算出角b所對邊與目標(biāo)運(yùn)動軌跡的交點(diǎn)：（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));

%計算出導(dǎo)彈k時刻所運(yùn)動到的位置：（x,y,z）%%%參見公式三%%%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;

end

sprintf('遭遇時間：%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比例導(dǎo)引律制導(dǎo)下的導(dǎo)彈運(yùn)動軌跡');

text(ptr(1,280),ptr(2,280),ptr(3,280),'\rightarrow