攻击时间控制的动态逆三维制导律源代码

%参考文献：《攻击时间控制的动态逆三维制导律》，哈尔滨工程大学学报，2010
%编程难点：视线坐标系与参考坐标系间的转换关系
clear
clc
V=300;% 导弹速度，m/s
x=-5000;%进攻弹坐标，m
y=5000;
z=6000;
xt=0;%目标坐标，m
yt=0;
zt=0;
Rm=sqrt((x-xt)^2+(y-yt)^2+(z-zt)^2);
DRm=0;
theta_m=20*pi/180;
Dtheta_m=0;
fea_m=10*pi/180;
theta_s=atan((zt-z)/sqrt((xt-x)^2+(yt-y)^2));
fea_s=atan((yt-y)/(xt-x));
Dtheta_s=0;
Dfea_s=0;

g=9.8;%重力加速度
k1=10;
c1=0.7;
c2=0.9;
k3=5;
Td=45;

n=1;
t=0;
dt=0.001;
while DRm<=0 &&Rm>=0
    et=Td-t-Rm/V;
    k2=(1-cos(theta_m)+c1*et*cos(theta_m)/(et+c2))/et;
    Aym=k1*abs(DRm)*Dtheta_s/cos(theta_m);%增广比例导引律
    if Aym>8*g
        Aym=8*g;
    end
    if Aym<-8*g
        Aym=-8*g;
    end
    if fea_m>=0 && fea_m<pi/2
        Azm=(V*(1-k2*et)*sin(theta_m)*Dtheta_m/cos(theta_m)+k2*V-k2*V*cos(theta_m)*cos(fea_m))/sqrt(1-((1-k2*et)/cos(theta_m))^2)+k3*V*cos(theta_m)*(fea_m-acos((1-k2*et)/cos(theta_m)))+V^2/Rm*sin(fea_m)*(1-sin(theta_m)*cos(theta_m)*cos(fea_m)*tan(theta_s));
    end
    if fea_m>=-pi/2 && fea_m<0
        Azm=-(V*(1-k2*et)*sin(theta_m)*Dtheta_m/cos(theta_m)+k2*V-k2*V*cos(theta_m)*cos(fea_m))/sqrt(1-((1-k2*et)/cos(theta_m))^2)+k3*V*cos(theta_m)*(fea_m+acos((1-k2*et)/cos(theta_m)))+V^2/Rm*sin(fea_m)*(1-sin(theta_m)*cos(theta_m)*cos(fea_m)*tan(theta_s));
    end
   % Azm=-k1*abs(DRm)*Dfea_s/cos(fea_m);%方法2：偏航通道也采用增广比例导引律
    if Azm>8*g
        Azm=8*g;
    end
    if Azm<-8*g
        Azm=-8*g;
    end 
   
    DRm=-V*cos(theta_m)*cos(fea_m);
    Dtheta_m=Aym/V+V*cos(theta_m)*sin(fea_m)*sin(fea_m)*tan(theta_s)/Rm+V*sin(theta_m)*cos(fea_m)/Rm;
    Dfea_m=-Azm/(V*cos(theta_m))+V*sin(fea_m)/(Rm*cos(theta_m))-V*sin(theta_m)*cos(fea_m)*tan(theta_s)*sin(fea_m)/Rm;
    Dtheta_s=-V*sin(theta_m)/Rm;
    Dfea_s=-V*cos(theta_m)*sin(fea_m)/(Rm*cos(theta_s));
    
    Rm=Rm+DRm*dt;
    theta_m=theta_m+Dtheta_m*dt;
    fea_m=fea_m+Dfea_m*dt;
    theta_s=theta_s+Dtheta_s*dt;
    fea_s=fea_s+Dfea_s*dt;
    
    %视线坐标系转换到参考坐标系-
    Tran=[cos(theta_s)*cos(fea_s) sin(theta_s) -cos(theta_s)*sin(fea_s);
        -sin(theta_s)*cos(fea_s) cos(theta_s) sin(theta_s)*sin(fea_s);
        sin(fea_s) 0 cos(fea_s)];
    DP=Tran'*[V*cos(theta_m)*cos(fea_m);V*cos(theta_m)*sin(fea_m);V*sin(theta_m)];
        
    Dx=DP(1);
    Dy=DP(2);
    Dz=DP(3);
    x=x+Dx*dt;
    y=y+Dy*dt;
    z=z+Dz*dt;
    
    P_s(:,n)=[x;y;z];
    S_s(:,n)=[Rm;theta_m;fea_m;et];
    A_s(:,n)=[Aym/g;Azm/g];
    n=n+1;
    t=t+dt;
end
figure(1)%由于plot3函数默认采用美国坐标系，需要转化为我们习惯的俄式坐标系
plot3(P_s(2,:)/1000,P_s(1,:)/1000,-P_s(3,:)/1000);
xlabel('Y/km')
ylabel('X/km')
zlabel('Z/km')

figure(2)
plot((1:n-1)*dt,S_s(1,:))
xlabel('time/s')
ylabel('相对距离')

figure(3)
plot((1:n-1)*dt,S_s(2,:)*180/pi)
xlabel('time/s')
ylabel('俯仰通道前置角/deg')

figure(4)
plot((1:n-1)*dt,S_s(3,:)*180/pi)
xlabel('time/s')
ylabel('偏航通道前置角/deg')

figure(5)
plot((1:n-1)*dt,S_s(4,:))
xlabel('time/s')
ylabel('et/s')

figure(6)
plot((1:n-1)*dt,A_s(1,:))
xlabel('time/s')
ylabel('Amy/g')
title('俯仰通道控制量')

figure(7)
plot((1:n-1)*dt,A_s(2,:))
xlabel('time/s')
ylabel('Amz/g')
title('偏航通道控制量')