比例导引法Matab仿真.docxVIP

%三维制导模型，比例导引法求解 %源代码作者不详，注释人：lylogn %Modified by lylogn，2012年4月17日 clear all; close all; clc? dt=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];?%目标初始位置 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))^2+(ptr(2,k-1)-pmr(2,k-1))^2+(ptr(3,k-1)-pmr(3,k-1))^2);%k-1时刻导弹与目标在三维空间中的欧氏距离 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);?%目标k时刻位置与导弹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时刻的视线角变化量（假设导弹不动，目标移动） %%%此处参见图一%%% if abs(imag(b))0?%如果acos的值出现虚数，则说明该角度一定很小，对其进行近似操作 b=0.0000001;? end? 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);?%计算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时刻导弹m与目标t之间的距离（在导弹不动，目标移动的假设条件下），为假值 dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));?%k-1时刻到k时刻的视线角变化量（假设导弹移动，目标也移动），以下代码重复以上过程，为假值 %%%此处参见图二%%% 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));?%计算k时刻导弹m与目标t之间的距离（在导弹移动，目标也移动的假设条件下），逼近真值，以下计算使之更加精确 dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3));?%k-1时刻到k时刻的视线角变化量（假设导弹移动，目标也移动），以下代码重复以上过程，为真值 if abs(imag(dq))0? dq=0.0000001;? end? %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ?ded by lylogn 2012.04.17，To make dq get closer to its true value q(k)=q(k-1