%具有角度和时间约束的导弹最优全弹道设计

%算法三：比例导引末制导

%说明：在仿真中，下标"_m"表示拦截弹参数，下标"_t"表示目标弹参数

clear

clc

%---------------------拦截弹参数设置-----------------------

V_m=260; % 单位：m/s，导弹飞行速度

theta_m=45*pi/180;%单位：rad，初始弹道倾角

fea_m=10*pi/180;%单位：rad，初始弹道偏角

H=1000;%单位：m，飞行高度

g=9.6;%单位：m/s^2，重力加速度

X_m=0; %单位：m，拦截弹位置坐标，X轴

Y_m=1000; %单位：m，拦截弹位置坐标，Y轴

Z_m=0; %单位：m，拦截弹位置坐标，Z轴

%---------------------目标弹参数设置-----------------------

V_t=150; %单位：m/s，目标弹飞行速度

theta_t=60*pi/180;%单位：rad，初始弹道偏角

fea_t=0*pi/180;%单位：rad，初始弹道偏角

X_t=1000; %单位：m，目标弹位置坐标，X轴

Y_t=2000; %单位：m，目标弹位置坐标，Y轴

Z_t=10*pi/180; %单位：m，目标弹位置坐标，Z轴

%------------------比例导引律参数设置-----------------------

k1=5; %纵向通道导引系数

k2=5;%偏航通道导引系数

t=0; %单位：s，仿真时刻

dt=0.001;%单位：s，仿真时间步长

n=1;%计数用

R=sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2); % 计算弹目相对距离

theta_L=atan((Y_t-Y_m)/sqrt((X_t-X_m)^2)+(Z_t-Z_m)^2);

fea_L=atan((Z_t-Z_m)/(X_t-X_m));

while(R>1) %当弹目相对距离小于阈值时，仿真停止

%-------------------------纵向通道------------------------------

% dR=V_t*cos(theta_L)*cos(fea_L-fea_t)-(cos(theta_m)*cos(theta_L)*cos(fea_L-fea_m)+sin(theta_m)*sin(theta_L))*V_m;

% dtheta_L=-1/R*V_t*sin(theta_L)*cos(fea_L-fea_t)-1/R*(sin(theta_m)*cos(theta_L)-sin(theta_L)*cos(theta_m)*cos(fea_L-fea_m))*V_m;

dX_m=V_m*cos(theta_m)*cos(fea_m);

dY_m=V_m*sin(theta_m);

dZ_m=-V_m*cos(theta_m)*sin(fea_m);

dX_t=V_t*cos(theta_t)*cos(fea_t);

dY_t=V_t*sin(theta_t);

dZ_t=-V_t*cos(theta_t)*sin(fea_t);

dR=((X_m-X_t)*(dX_m-dX_t)+(Y_m-Y_t)*(dY_m-dY_t)+(Z_m-Z_t)*(dZ_m-dZ_t))/sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);

dtheta_L=((dY_t-dY_m)*sqrt((X_t-X_m)^2+(Z_t-Z_m)^2)-(Y_t-Y_m)*((X_t-X_m)*(dX_t-dX_m)+(Z_t-Z_m)*(dZ_t-dZ_m))/sqrt((X_t-X_m)^2+(Z_t-Z_m)^2))/((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);

ny=k1*abs(dR)*dtheta_L/g; %控制输入(论文中式(18)第一式)

dtheta_m=g/V_m*(ny-cos(theta_m)); %纵向通道：弹道倾角变化函数(论文中式(1))

theta_m=theta_m+dtheta_m*dt;

%-------------------------偏航通道------------------------------

% dfea_L=1/(R*cos(theta_L))*V_t*sin(fea_L-fea_t)-1/(R*cos(theta_L))*V_m*cos(theta_m)*sin(fea_L-fea_m);

dfea_L=((dZ_t-dZ_m)*(X_t-X_m)-(Z_t-Z_m)*(dX_t-dX_m))/((X_t-X_m)^2+(Z_t-Z_m)^2);

nz=k2*abs(dR)*dfea_L/g; %控制输入(论文中式(18)第二式)

dfea_m=-g/(V_m*cos(theta_m))*nz;%偏航通道：弹道偏角变化函数(论文中式(7))

fea_m=fea_m+dfea_m*dt;

%----------------------------计算拦截弹坐标----------------------------

X_m=X_m+V_m*cos(theta_m)*cos(fea_m)*dt;

Y_m=Y_m+V_m*sin(theta_m)*dt;

Z_m=Z_m-V_m*cos(theta_m)*sin(fea_m)*dt;

%----------------------------计算目标弹坐标----------------------------

X_t=X_t+V_t*cos(theta_t)*cos(fea_t)*dt;

Y_t=Y_t+V_t*sin(theta_t)*dt;

Z_t=Z_t-V_t*cos(theta_t)*sin(fea_t)*dt;

%计算相对运动学参数

R=sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);

theta_L=atan((Y_t-Y_m)/sqrt((X_t-X_m)^2)+(Z_t-Z_m)^2);

fea_L=atan((Z_t-Z_m)/(X_t-X_m));

%-------------------------保存结果------------------------------

theta_m_store(n)=theta_m; %保存弹道倾角

fea_m_store(n)=fea_m; %第一行为实际弹道偏角，第二行为指令弹道偏角

ny_store(n)=ny; %保存纵向过载

nz_store(n)=nz; %保存偏航过载

P_m_store(:,n)=[X_m;Y_m;Z_m]; %保存拦截弹坐标

P_t_store(:,n)=[X_t;Y_t;Z_t]; %保存目标弹坐标

n=n+1; %每循环一次，计数加一

t=t+dt; %往前推进一个仿真步长

end

disp('脱靶量为(m)：')

R

figure(1)

plot((1:n-1)*dt,theta_m_store*180/pi)

xlabel('time/s')

ylabel('弹道倾角/deg')

title('末制导：弹道倾角变化情况')

figure(2)

plot((1:n-1)*dt,ny_store)

xlabel('time/s')

ylabel('纵向过载ny')

title('末制导：纵向过载变化情况')

figure(3)

plot((1:n-1)*dt,fea_m_store(1,:)*180/pi)

xlabel('time/s')

ylabel('弹道偏角/deg')

title('末制导：弹道偏角变化情况')

figure(4)

plot((1:n-1)*dt,nz_store)

xlabel('time/s')

ylabel('偏航过载nz')

title('末制导：偏航过载变化情况')

figure(5)%注意matlab三维坐标画图X、Y、Z轴方向与导弹飞行力学坐标系不同

plot3(P_m_store(1,:),P_m_store(3,:),P_m_store(2,:),P_t_store(1,:),P_t_store(3,:),P_t_store(2,:))

legend('拦截弹运动轨迹','目标弹运动轨迹')

xlabel('X/m')

ylabel('-Z/m')

zlabel('Y/m')