比例导引(PNG)-Matlab 程序

本文主要是介绍比例导引(PNG)-Matlab 程序，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

本文提供比例导引的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_maxa_vz = sign(a_vz) .* u_max;endif abs(a_vy) > u_maxa_vy = sign(a_vy) .* u_max;enddtheta = 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 breakendif i > 1 && abs(r(i)) > abs(r(i-1))breakendendlen = 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');