比例导引(PNG)-Matlab 程序

news2024/9/24 20:02:41

本文提供比例导引的matlab程序，想要看理论的可以看书《导弹飞行力学》或者我的博客比例导引详解

代码

%% 三维比例导引末制导

clc;clear;
close all;

%% 设置导弹初始参数和目标参数

% 总步长
length = 1000000;
x_m = zeros(length,1);
y_m = zeros(length,1);
z_m = zeros(length,1);
v_m = zeros(length,1);
theta_m = zeros(length,1);
phi_m = zeros(length,1);
r = zeros(length,1);
gamma_m = zeros(length,1);
psi_m = zeros(length,1);
dr = zeros(length,1);
d_gamma_m = zeros(length,1);
d_psi_m = zeros(length,1);
a_vz_save = zeros(length,1);
a_vy_save = zeros(length,1);

% 初始化
u_max = 200;
t = 0;
dt = 0.001;
% 目标位置(m)
x_T = 30000;
y_T = 0;
z_T = 10000;
% 导弹初始位置(m)
x_m(1) = 0;
y_m(1) = 20000;
z_m(1) = 0;
v_m(1) = 200;
theta_m(1) = 0;
phi_m(1) = 0;
% 比例系数
N = 4;


% 循环
for i = 1:length

    % 计算导弹位置、视线距离、视线倾角、视线偏角更新量
    dx     = v_m(i).*cos(theta_m(i)).*cos(phi_m(i));
    dy     = v_m(i).*sin(theta_m(i));
    dz     = -v_m(i).*cos(theta_m(i)).*sin(phi_m(i));
    delta_x = x_T - x_m(i);
    delta_y = y_T - y_m(i);
    delta_z = z_T - z_m(i);
    delta_vx = - dx;
    delta_vy = - dy;
    delta_vz = - dz;

    r(i)       = sqrt(delta_x .^ 2 + delta_y .^ 2 + delta_z .^ 2);
    gamma_m(i) = atan(delta_y ./ sqrt(delta_x .^ 2 + delta_z .^ 2));
    psi_m(i)   = atan(- delta_z ./ delta_x);
    d_gamma_m(i) = ((delta_x .^ 2 + delta_z .^ 2) .* delta_vy - delta_y .* (delta_x .* delta_vx + delta_z .* delta_vz)) ./ (delta_x .^ 2 + delta_y .^ 2 + delta_z .^ 2) ./ (sqrt(delta_x .^ 2 + delta_z .^ 2));
    d_psi_m(i)   = (delta_z .* delta_vx - delta_x .* delta_vz) ./ (delta_x .^ 2 + delta_z .^ 2); 

    % 求解制导指令
    a_vy   = N.*v_m(i).*d_gamma_m(i);
    a_vz   = -N.*v_m(i).*d_psi_m(i);
    
    % 限幅
    if abs(a_vz) > u_max
        a_vz = sign(a_vz) .* u_max;
    end
    if abs(a_vy) > u_max
        a_vy = sign(a_vy) .* u_max;
    end

    dtheta = a_vy./v_m(i);
    dphi   = -a_vz./(v_m(i).*cos(theta_m(i)));

    % 更新状态
    x_m(i+1)     = x_m(i)+dx.*dt;
    y_m(i+1)     = y_m(i)+dy.*dt;
    z_m(i+1)     = z_m(i)+dz.*dt;
    v_m(i+1)     = v_m(i);
    theta_m(i+1) = theta_m(i)+dtheta.*dt;
    phi_m(i+1)   = phi_m(i)+dphi.*dt;
    t            = t+dt;

    % 保存数据
    a_vy_save(i) = a_vy;
    a_vz_save(i) = a_vz;

    % 导弹高度低于目标高度时表示击中或脱靶
    if abs(r(i)) < 10 
        break
    end

    if i > 1 && abs(r(i)) > abs(r(i-1))
        break
    end

end

len = t ./ dt;
fig1 = figure(1);
plot3(x_m(1:len,:),z_m(1:len,:),y_m(1:len,:));
view([145.8,32.4]);
title('3D trajectory')
xlabel('x');
ylabel('z');
zlabel('y');

fig2 = figure(2);
plot(dt:dt:len.*dt,a_vy_save(1:len,:));
xlabel('t(sec)');
ylabel('U_y(m/s^2)');

fig3 = figure(3);
plot(dt:dt:len.*dt,a_vz_save(1:len,:));
xlabel('t(sec)');
ylabel('U_z(m/s^2)');

fig4 = figure(4);
plot(dt:dt:len.*dt,r(1:len,:));
xlabel('t(sec)');
ylabel('r(m)');

print(fig1, './fig1.png', '-dpng', '-r300');
print(fig2, './fig2.png', '-dpng', '-r300');
print(fig3, './fig3.png', '-dpng', '-r300');
print(fig4, './fig4.png', '-dpng', '-r300');