具有角度和时间约束的导弹最优全弹道设计（源代码）

最新推荐文章于 2022-04-17 15:39:24 发布

51星系

最新推荐文章于 2022-04-17 15:39:24 发布

阅读量9.3k

点赞数 35

分类专栏：科研航空航天仿真算法文章标签： matlab 编程

本文链接：https://blog.csdn.net/stk10/article/details/52814917

版权

科研同时被 2 个专栏收录

118 篇文章 37 订阅

订阅专栏

航空航天仿真算法

92 篇文章 183 订阅

订阅专栏

%具有角度和时间约束的导弹最优全弹道设计
%根据论文，初制导功能在于控制导弹快速爬升
%指令高度：Hc=1000+35*t，以35m/s快速爬升
%指令偏航角为-45deg
clear
clc
%---------------------导弹参数设置-----------------------
V=260; % 单位：m/s，导弹飞行速度
theta=45*pi/180;%单位：rad，初始弹道倾角
fea=-45*pi/180;%单位：rad，初始弹道偏角
T_1=14.2;%单位：s，初制导时间
H=1000;%单位：m，飞行高度
g=9.6;%单位：m/s^2，重力加速度
X=0; %单位：m，拦截弹位置坐标，X轴
Y=1000; %单位：m，拦截弹位置坐标，Y轴
Z=0; %单位：m，拦截弹位置坐标，Z轴

%------------------变结构控制律参数设置-----------------------
c1=30;   %纵向通道
epu1=.1;%纵向通道
k1=.1;%纵向通道
c2=30;%偏航通道
epu2=.0005;%偏航通道
k2=.5;%偏航通道

dH=0;
dfea=0;
t=0; %单位：s，仿真时刻
dt=0.001;%单位：s，仿真时间步长
n=1;%计数用

for i=1:14200   %循环14200次，对应初制导时间14.2s
    
    %-------------------------纵向通道------------------------------
    Hc=1000+35*t;  %高度指令
    dHc=35;%高度指令的导数
    ddHc=0;%高度指令的二阶导数
    
    e1=Hc-H;  %高度误差
    de1=dHc-dH; %高度误差的导数
    S1=c1*e1+de1;  %切换函数（论文中式（4））
    dS1=-epu1*sign(S1)-k1*S1;%指数趋近律（论文中式（5））
    ny=1/g*(c1*(dHc-dH)+ddHc+(epu1+10)*sign(S1)+k1*S1);  %控制输入（论文中式（6））
    
    dH=V*sin(theta); %纵向通道：高度变化函数（论文中式（1））
    dtheta=g/V*(ny-cos(theta)); %纵向通道：弹道倾角变化函数（论文中式（1））
    H=H+dH*dt;
    theta=theta+dtheta*dt;
    
    %-------------------------偏航通道------------------------------
    feac=-45*pi/180;  %弹道偏角指令
    dfeac=0;%弹道偏角指令的导数
    
    e2=feac-fea;  %弹道偏角误差
    de2=dfeac-dfea; %弹道偏角误差的导数
    S2=c2*e2+de2;  %切换函数
    dS2=-epu2*sign(S2)-k2*S2;%指数趋近律
    nz=-V/g*(c2*(feac-fea)+dfeac+(epu2)*sign(S2)+k2*S2);  %控制输入（论文中式（8））
    
    dfea=-g/(V*cos(theta))*nz;%偏航通道：弹道偏角变化函数（论文中式（7））
    fea=fea+dfea*dt;
    
    %----------------------------计算坐标----------------------------
    X=X+V*cos(theta)*cos(fea)*dt;
    Y=Y+V*sin(theta)*dt;
    Z=Z-V*cos(theta)*sin(fea)*dt;
    
    %-------------------------保存结果------------------------------
    H_store(:,n)=[H;Hc]; %第一行为实际飞行高度，第二行为指令高度   
    theta_store(n)=theta;  %保存弹道倾角
    fea_store(:,n)=[fea;feac];   %第一行为实际弹道偏角，第二行为指令弹道偏角   
    ny_store(n)=ny;   %保存纵向过载
    nz_store(n)=nz;   %保存偏航过载
    P_store(:,n)=[X;Y;Z];
    n=n+1;  %每循环一次，计数加一
    t=t+dt; %往前推进一个仿真步长
end

figure(1)
plot((1:n-1)*dt,H_store(1,:),(1:500:n-1)*dt,H_store(2,1:500:end),'+')
xlabel('time/s')
ylabel('Altitude/m')
legend('实际飞行高度','指令高度')
title('初制导：高度变化情况')

figure(2)
plot((1:n-1)*dt,theta_store*180/pi)
xlabel('time/s')
ylabel('弹道倾角/deg')
title('初制导：弹道倾角变化情况')

figure(3)
plot((1:n-1)*dt,ny_store)
xlabel('time/s')
ylabel('纵向过载ny')
title('初制导：纵向过载变化情况')

figure(4)
plot((1:n-1)*dt,fea_store(1,:)*180/pi,(1:500:n-1)*dt,fea_store(2,1:500:end)*180/pi,'+')
xlabel('time/s')
ylabel('弹道偏角/deg')
legend('实际弹道偏角','指令弹道偏角')
title('初制导：弹道偏角变化情况')

figure(5)
plot((1:n-1)*dt,nz_store)
xlabel('time/s')
ylabel('偏航过载nz')
title('初制导：偏航过载变化情况')

figure(6)%注意matlab三维坐标画图X、Y、Z轴方向与导弹飞行力学坐标系不同
plot3(P_store(1,:),P_store(3,:),P_store(2,:))
xlabel('X/m')
ylabel('-Z/m')
zlabel('Y/m')
title('拦截弹位置坐标')

%具有角度和时间约束的导弹最优全弹道设计
%根据论文，中制导功能在于控制偏航角，纵向通道论文中没有明确说明
%我们认为在中制导段，纵向高度保持在初制导结束时的状态，此处仍采用算法一来控制纵向通道
%采用算法二控制偏航通道

clear
clc
%---------------------导弹参数设置-----------------------
V=260; % 单位：m/s，导弹飞行速度
theta=10*pi/180;%单位：rad，初始弹道倾角
fea=-45*pi/180;%单位：rad，初始弹道偏角
T_2=85;%单位：s，中制导时间
H=1500;%单位：m，飞行高度
g=9.6;%单位：m/s^2，重力加速度
X=0; %单位：m，拦截弹位置坐标，X轴
Y=H; %单位：m，拦截弹位置坐标，Y轴
Z=0; %单位：m，拦截弹位置坐标，Z轴

%------------------纵向通道变结构控制律参数设置-----------------------
c1=5;   %纵向通道
epu1=.1;%纵向通道
k1=.1;%纵向通道

%----------------------最优制导边界条件--------------------------
    fea_A=fea;   %中制导初始弹道偏角
    fea_B=120*pi/180;     %中制导结束时期望的弹道偏角
    %x_B=18000*cos(pi/3)/V;
    x_B=18000/V;
    x_A=0;
    %z_B=-22100*sin(pi/3)/V;
    z_B=-12822/V;
    z_A=0;
    t_B=85;
    t_A=0;
    
%------------------------一般初始条件---------------------------    
dH=0;
dfea=0;
t=0; %单位：s，仿真时刻
dt=0.01;%单位：s，仿真时间步长
n=1;%计数用
for i=1:8500   %循环8500次，对应中制导时间85s
    
    %-------------------------纵向通道------------------------------
    Hc=1500;  %高度指令
    dHc=0;%高度指令的导数
    ddHc=0;%高度指令的二阶导数
    
    e1=Hc-H;  %高度误差
    de1=dHc-dH; %高度误差的导数
    S1=c1*e1+de1;  %切换函数（论文中式（4））
    dS1=-epu1*sign(S1)-k1*S1;%指数趋近律（论文中式（5））
    ny=1/g*(c1*(dHc-dH)+ddHc+(epu1+10)*sign(S1)+k1*S1);  %控制输入（论文中式（6））
    
    dH=V*sin(theta); %纵向通道：高度变化函数（论文中式（1））
    dtheta=g/V*(ny-cos(theta)); %纵向通道：弹道倾角变化函数（论文中式（1））
    H=H+dH*dt;
    theta=theta+dtheta*dt;
    
    %-------------------------偏航通道------------------------------
    
    A=[(fea_B-fea_A)/2+(sin(2*fea_B)-sin(2*fea_A))/4   -1/2*((sin(fea_B))^2-(sin(fea_A))^2)              sin(fea_B)-sin(fea_A);
        -1/2*((sin(fea_B))^2-(sin(fea_A))^2)           (fea_B-fea_A)/2-(sin(2*fea_B)-sin(2*fea_A))/4     cos(fea_B)-cos(fea_A);
        sin(fea_B)-sin(fea_A)                          cos(fea_B)-cos(fea_A)                             fea_B-fea_A];
    ANS=inv(A)*[-(x_B-x_A);  -(z_B-z_A);  -(t_B-t_A)];
    lam1=ANS(1);
    lam2=ANS(2);
    lam3=ANS(3);
    nz=-V/(lam1*cos(fea)-lam2*sin(fea)+lam3)/g;  %控制输入（论文中式（17），（17）式算出来的是加速度，需要除以g得到过载）
    
    dfea=-g/(V*cos(theta))*nz;%偏航通道：弹道偏角变化函数（论文中式（7））
    fea=fea+dfea*dt;
    
    %----------------------------计算坐标----------------------------
    X=X+V*cos(theta)*cos(fea)*dt;
    Y=Y+V*sin(theta)*dt;
    Z=Z-V*cos(theta)*sin(fea)*dt;
    
    %-------------------------保存结果------------------------------
    H_store(:,n)=[H;Hc]; %第一行为实际飞行高度，第二行为指令高度   
    theta_store(n)=theta;  %保存弹道倾角
    fea_store(n)=fea;   %第一行为实际弹道偏角，第二行为指令弹道偏角   
    ny_store(n)=ny;   %保存纵向过载
    nz_store(n)=nz;   %保存偏航过载
    P_store(:,n)=[X;Y;Z];
    n=n+1;  %每循环一次，计数加一
    t=t+dt; %往前推进一个仿真步长
end

figure(1)
plot((1:n-1)*dt,H_store(1,:),(1:500:n-1)*dt,H_store(2,1:500:end),'+')
xlabel('time/s')
ylabel('Altitude/m')
legend('实际飞行高度','指令高度')
title('中制导：高度变化情况')

figure(2)
plot((1:n-1)*dt,theta_store*180/pi)
xlabel('time/s')
ylabel('弹道倾角/deg')
title('中制导：弹道倾角变化情况')

figure(3)
plot((1:n-1)*dt,ny_store)
xlabel('time/s')
ylabel('纵向过载ny')
title('中制导：纵向过载变化情况')

figure(4)
plot((1:n-1)*dt,fea_store*180/pi)
xlabel('time/s')
ylabel('弹道偏角/deg')
title('中制导：弹道偏角变化情况')

figure(5)
plot((1:n-1)*dt,nz_store)
xlabel('time/s')
ylabel('偏航过载nz')
title('中制导：偏航过载变化情况')

figure(6)%注意matlab三维坐标画图X、Y、Z轴方向与导弹飞行力学坐标系不同
plot3(P_store(1,:),P_store(3,:),P_store(2,:))
xlabel('X/m')
ylabel('-Z/m')
zlabel('Y/m')
title('拦截弹位置坐标')

%全弹道仿真
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轴

%------------------算法一：变结构控制律参数设置-----------------------
c1=30;   %纵向通道
epu1=.1;%纵向通道
k1=.1;%纵向通道
c2=30;%偏航通道
epu2=.0005;%偏航通道
k2=.5;%偏航通道
dH=0;
dfea_m=0;
t=0; %单位：s，仿真时刻
dt=0.01;%单位：s，仿真时间步长
n=1;%计数用
%===========================初制导阶段============================
for i=1:1420   %循环1420次，对应初制导时间14.2s
    
    %-------------------------纵向通道------------------------------
    Hc=1000+35*t;  %高度指令
    dHc=35;%高度指令的导数
    ddHc=0;%高度指令的二阶导数
    
    e1=Hc-H;  %高度误差
    de1=dHc-dH; %高度误差的导数
    S1=c1*e1+de1;  %切换函数（论文中式（4））
    dS1=-epu1*sign(S1)-k1*S1;%指数趋近律（论文中式（5））
    ny=1/g*(c1*(dHc-dH)+ddHc+(epu1+10)*sign(S1)+k1*S1);  %控制输入（论文中式（6））
    
    dH=V_m*sin(theta_m); %纵向通道：高度变化函数（论文中式（1））
    dtheta_m=g/V_m*(ny-cos(theta_m)); %纵向通道：弹道倾角变化函数（论文中式（1））
    H=H+dH*dt;
    theta_m=theta_m+dtheta_m*dt;
    
    %-------------------------偏航通道------------------------------
    feac=-45*pi/180;  %弹道偏角指令
    dfeac=0;%弹道偏角指令的导数
    
    e2=feac-fea_m;  %弹道偏角误差
    de2=dfeac-dfea_m; %弹道偏角误差的导数
    S2=c2*e2+de2;  %切换函数
    dS2=-epu2*sign(S2)-k2*S2;%指数趋近律
    nz=-V_m/g*(c2*(feac-fea_m)+dfeac+(epu2)*sign(S2)+k2*S2);  %控制输入（论文中式（8））
    
    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;
    
    %-------------------------保存结果------------------------------
    H_store(:,n)=[H;Hc]; %第一行为实际飞行高度，第二行为指令高度
    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];
    n=n+1;  %每循环一次，计数加一
    t=t+dt; %往前推进一个仿真步长
end
% X_m
% Y_m
% Z_m
% theta_m
% fea_m
% pause    
%===========================中制导阶段============================
%----------------------最优制导边界条件--------------------------
    fea_A=fea_m;   %中制导初始弹道偏角
    fea_B=120*pi/180;     %中制导结束时期望的弹道偏角
    x_B=(18000+X_m)/V_m;
    x_A=X_m/V_m;
    z_B=(Z_m-12423)/V_m;
    z_A=Z_m/V_m;
    t_B=85;
    t_A=0;
for i=1:8500%循环8500次，对应中制导时间85s
    
    %-------------------------纵向通道------------------------------
    Hc=1497;  %高度指令
    dHc=0;%高度指令的导数
    ddHc=0;%高度指令的二阶导数
    
    e1=Hc-H;  %高度误差
    de1=dHc-dH; %高度误差的导数
    S1=c1*e1+de1;  %切换函数（论文中式（4））
    dS1=-epu1*sign(S1)-k1*S1;%指数趋近律（论文中式（5））
    ny=1/g*(c1*(dHc-dH)+ddHc+(epu1+10)*sign(S1)+k1*S1);  %控制输入（论文中式（6））
    
    dH=V_m*sin(theta_m); %纵向通道：高度变化函数（论文中式（1））
    dtheta_m=g/V_m*(ny-cos(theta_m)); %纵向通道：弹道倾角变化函数（论文中式（1））
    H=H+dH*dt;
    theta_m=theta_m+dtheta_m*dt;
    
    %-------------------------偏航通道------------------------------
    
    A=[(fea_B-fea_A)/2+(sin(2*fea_B)-sin(2*fea_A))/4   -1/2*((sin(fea_B))^2-(sin(fea_A))^2)              sin(fea_B)-sin(fea_A);
        -1/2*((sin(fea_B))^2-(sin(fea_A))^2)           (fea_B-fea_A)/2-(sin(2*fea_B)-sin(2*fea_A))/4     cos(fea_B)-cos(fea_A);
        sin(fea_B)-sin(fea_A)                          cos(fea_B)-cos(fea_A)                             fea_B-fea_A];
    ANS=inv(A)*[-(x_B-x_A);  -(z_B-z_A);  -(t_B-t_A)];
    lam1=ANS(1);
    lam2=ANS(2);
    lam3=ANS(3);
    nz=-V_m/(lam1*cos(fea_m)-lam2*sin(fea_m)+lam3)/g;  %控制输入（论文中式（17）,（17）式除以g得到过载）
    
    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;
    
    %-------------------------保存结果------------------------------
    H_store(:,n)=[H;Hc]; %第一行为实际飞行高度，第二行为指令高度   
    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];
    n=n+1;  %每循环一次，计数加一
    t=t+dt; %往前推进一个仿真步长
end

X_m
Y_m
Z_m
theta_m
fea_m
%===========================末制导阶段============================
%---------------------目标弹参数设置-----------------------
V_t=150; %单位：m/s，目标弹飞行速度
theta_t=10*pi/180;%单位：rad，初始弹道偏角
fea_t=80*pi/180;%单位：rad，初始弹道偏角
X_t=-14000; %单位：m，目标弹位置坐标，X轴
Y_t=2500; %单位：m，目标弹位置坐标，Y轴
Z_t=-8500; %单位：m，目标弹位置坐标，Z轴
%------------------算法三：比例导引律参数设置-----------------------
k11=10; %纵向通道导引系数
k12=10;%偏航通道导引系数
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))  %视线偏角
pause
n2=1;
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=k11*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=k12*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(:,n2)=[X_t;Y_t;Z_t]; %保存目标弹坐标
    
    n=n+1;  %每循环一次，计数加一
    n2=n2+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')

%具有角度和时间约束的导弹最优全弹道设计
%算法三：比例导引末制导
%说明：在仿真中，下标"_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')