zuiyoukongzhi.m

最优制导律攻击机动目标弹道仿真

%最优制导率仿真,初始化参数+10*pi*rand(1)/180
clear all;
clear;
global signvc;
g=9.81;
pi=3.14159265;
vm=1000;vt=-300;%导弹和目标的速度
headerror=-15*pi/180; %指向角误差
thetat=0*pi/180;    %目标的速度方向
rmx=0;rmy=0;

rtx=3000;rty=1000;
ptr(:,1)=[rty;rtx];
pmr(:,1)=[0;0];
at=5*g;%目标法向加速度
vtx=vt*sin(thetat);%目标的速度分量
vty=vt*cos(thetat);
rtmx=rtx-rmx;%弹目相对距离
rtmy=rty-rmy;
ammax=100*g;%导弹的最大机动能力为15g
rtm=sqrt(rtmx^2+rtmy^2);
sightangle=atan(rtmx/rtmy);%视线角
leadangle=asin(vt*sin(sightangle-thetat)/vm);%指向角
vmx=vm*sin(sightangle-leadangle+headerror);%导弹的速度分量
vmy=vm*cos(sightangle-leadangle+headerror);
vtmx=vtx-vmx;
vtmy=vty-vmy;
%弹目的相对运动速度
vc=-(rtmx*vtmx+rtmy*vtmy)/rtm;
signvc=sign(vc); %vc的符号
time=0;timestep=0.01;%时间和时间步长

par(:,1)=[time;0];
pvr(:,1)=[time;vm];
tgo=rtm/abs(vc);
dsightangle=(rtmy*vtmx-rtmx*vtmy)/(rtm^2);
D(:,1)=[time;rtm*dsightangle*tgo];
%file=fopen('output.dat','wt');%将数据写入文件
k=1;

%循环
while(1)
    k=k+1;
    %vc改变符号仿真结束
    if(sign(vc)~=signvc)
    break;
else 
    %if(rtm<100)%弹目距离小于100，步长变为0.005
     %  timestep=0.001;
     % end

signvc=sign(vc);
tgo=rtm/abs(vc);
%视线角速率
dsightangle=(rtmy*vtmx-rtmx*vtmy)/(rtm^2);
%dtheta=3*vc*dsightangle/vm;%最优比例导引
%dtheta=3*vc*dsightangle/vm+1*cos(thetat)/vm;       %目标航迹倾角修正项 +k5*cos(thetat)/vm
%k1=1.45;k2=1.5;k3=0.3;k5=3;
%dtheta=k1*vc*dsightangle/(vm*(k2+k3*tgo));
%dtheta=3*dsightangle+0.1*cos(thetat);%比例导引
%dtheta=(vc*rtm^3/(vc^3+3*rtm^3))*dsightangle;
%dtheta=3*(vc*dsightangle+0.5*at)/vm;       %目标加速度修正项
dtheta=3*dsightangle;

theta=atan(vmx/vmy);
%plot(time,dtheta),hold on,
%导弹加速度
am=vm*dtheta;
%am=k1*vc*dsightangle/(k2+k3*tgo);
%am=3*(vc*dsightangle+0.5*at);%与dtheta=3*(vc*dsightangle+0.5*at)/vm效果相同,
%解释：am=dtheta/vm
%限制机动能力
if(abs(am)>=ammax)
    am=sign(am)*ammax;
end

%加速度分量
amx=am*cos(sightangle);
amy=-am*sin(sightangle);
amfa=am*cos(sightangle-theta)/g;
time=time+timestep;
%if(time>=5)
  %  at=50*g*sign(sin(pi*(time-5))/3);      %目标作开关机动每3秒一次
  %end

%目标位置
rtx=rtx+timestep*vtx;
rty=rty+timestep*vty;
dthetat=at/vt;
thetat=thetat+timestep*dthetat;

vtx=vt*sin(thetat);%目标的速度分量
vty=vt*cos(thetat);
%导弹位置
rmx=rmx+timestep*vmx;
rmy=rmy+timestep*vmy;
rm=sqrt(rmx^2+rmy^2);
ptr(:,k)=[rty;rtx];
pmr(:,k)=[rmy;rmx];
par(:,k)=[time;amfa];

%导弹速度
vmx=vmx+timestep*amx;
vmy=vmy+timestep*amy;
vm=sqrt(vmx^2+vmy^2);
pvr(:,k)=[time;vm];
%弹目相对位移
rtmx=rtx-rmx;
rtmy=rty-rmy;

%上一步的脱靶量
rtm0=rtm;
rtm=sqrt(rtmx^2+rtmy^2);      %弹目距离
D(:,k)=[time;rtm*dsightangle*tgo];
sightangle=atan(rtmx/rtmy);%视线角
%弹目相对速度
vtmx=vtx-vmx;
vtmy=vty-vmy;
vc=-(rtmx*vtmx+rtmy*vtmy)/rtm;
   if(rtm<=20)%||(time>=1)%弹目距离小于100，步长变为0.005
       break;
   end
%数据写入文件
%x(:,k)=3*vc/vm;
%fprintf(file,'%.1f %.1f %.1f %.1f %.1f 不%.1f %.1f %.1f %.1f %.1f %.1f\n',time/0.01,rty,0,rtx,0,0,rmy,0,rmx,0,0);
%figure(1);plot(rmy,rmx,'g',rty,rtx,'r'),hold on,
%figure(2);plot(time,dtheta*57.3,'k'),hold on,%画出过载随时间的变化

%figure(3);plot(time,D,'b'),hold on,
end
end

%status=fclose(file);
%legend('比例导引');
figure(1);plot(pmr(1,:),pmr(2,:),'k',ptr(1,:),ptr(2,:),'r');xlabel('x(m)');ylabel('y(m)');legend('导弹','目标');grid on;
%axis([0 15000 0 6000]);
%figure(2);plot(pvr(1,:),pvr(2,:),'k'),hold on,
%figure(3);plot(D(1,:),D(2,:),'b'),hold on,
figure(2);plot(par(1,:),par(2,:),'k');xlabel('t(s)');ylabel('法向过载(g)');
grid on;
rtm,
time,
amfa,
pmr(:,k),
%sqrt(pmr(1,k)^2+pmr(2,k)^2),