使用HGS算法调整PD控制器增益的无人机动态性能数据——基于启发式的无人机路径跟踪优化（Matlab代码实现）

clear all
close all
load('HGS20_20.mat');
x_2020      = xa_HGS;
y_2020      = ya_HGS;
z_2020      = za_HGS;

load('HGS30_20.mat');
x_3020      = xb_HGS;
y_3020      = yb_HGS;
z_3020      = zb_HGS;

 load('HGS40_20.mat');
x_4020      = x_HGS;
y_4020      = y_HGS;
z_4020      = z_HGS;

load('xi.mat');
T=0.05;
t = [0:T:40];
x_d     = xi_d(1,:)';
y_d     = xi_d(2,:)';
z_d     = xi_d(3,:)';
phi_d   = xi_d(4,:)';
theta_d = xi_d(5,:)';
psi_d   = xi_d(6,:)';

figure
set(gcf,'position', [150,150,800,500])

x=x_HGS; y=y_HGS; z=z_HGS;

plot3(x_d,y_d,z_d,'k--',x_4020,y_4020,z_4020,'r-',x_2020,y_2020,z_2020,'b-.',x_3020,y_3020,z_3020,'g.','LineWidth',2);
xlabel('X [m]','Fontsize',18);
ylabel('Y [m]','Fontsize',18);
zlabel('Z [m]','Fontsize',18);
set(gca,'Fontsize',18)
legend('Desired trajectory ','40ind, 20 iter','30ind, 20 iter','20ind, 20 iter','AutoUpdate','off','Fontsize',18)
hold on
grid on

%%
clear xi_d
load('xiCircle.mat');
x_dC     = xi_d(1,:)';
y_dC     = xi_d(2,:)';
z_dC     = xi_d(3,:)';
x_HGS_C     = xi(:,4);
y_HGS_C     = xi(:,5);
z_HGS_C     = xi(:,6);

figure
set(gcf,'position', [150,150,800,500])

%x=x_HGS; y=y_HGS; z=z_HGS;

plot3(x_dC,y_dC,z_dC,'k--',x_HGS_C,y_HGS_C,z_HGS_C,'r','LineWidth',2);
xlabel('X [m]','Fontsize',18);
ylabel('Y [m]','Fontsize',18);
zlabel('Z [m]','Fontsize',18);
set(gca,'Fontsize',18)
legend('Desired trajectory ','20ind, 20 iter HGS','Fontsize',18)
hold on
grid on