方案弹道法向过载

2545985604

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