粒子群优化pid

close all;
clear;
clc;
%惯性因子
wmax=0.9;
wmin=0.4;
%自我、种群学习因子
c1=0.8;
c2=0.5;
%迭代次数、种群大小、维度
gens=20;
np=20;
nd=3;
%范围
kpmax=50;
kpmin=0;
kimax=10;
kimin=0;
kdmax=50;
kdmin=0;
%速度
vmin=-1;
vmax=1;
v=ones(np,nd);
%初始化种群
x=ones(np,nd);
xp_best=1e2*ones(np,nd);
xg_best=[1e2 1e2 1e2];
%种群、全局最优
zp_best=1e6*ones(np,1);
zg_best=1e6;
zz=ones(1,gens);
K_p=ones(1,gens);
K_i=ones(1,gens);
K_d=ones(1,gens);
for i=1:np
    %初始化粒子位置
    x(i,1)=kpmin+(kpmax-kpmin)*rand;
    x(i,2)=kimin+(kimax-kimin)*rand;
    x(i,3)=kpmin+(kpmax-kpmin)*rand;
    %初始化粒子速度
    v(i,:)=vmin+(0.5*rands(1,nd)+0.5)*(vmax-vmin);
    z=pso_pid(x(i,1),x(i,2),x(i,3));
    %更新个体最优
    if z<zp_best(i)
       zp_best(i)=z;
       xp_best(i,:)=x(i,:);
    end
    %更新全局最优
    if z<zg_best
       zg_best=z;
       xg_best=x(i,:);
    end    
end
%开始迭代
iter=0;
for k=1:gens
    for i=1:np
      v(i,:)=wmax*v(i,:)+c1*rand*(xp_best(i,:)-x(i,:))+c2*rand*(xg_best-x(i,:));
      %速度限幅
      for j=1:nd
          if v(i,j)>vmax
              v(i,j)=vmax;
          end
          if v(i,j)<vmin
              v(i,j)=vmin;
          end
      end
      x(i,:)=x(i,:)+v(i,:);
      %位置限幅
      for j=1:nd
          if x(i,1)>kpmax
              x(i,1)=kpmax;
          end
          if x(i,1)<kpmin
              x(i,1)=kpmin;
          end
          
          if x(i,2)>kimax
              x(i,2)=kimax;
          end
          if x(i,2)<kimin
              x(i,2)=kimin;
          end
          
          if x(i,3)>kdmax
              x(i,3)=kdmax;
          end
          if x(i,3)<kdmin
              x(i,3)=kdmin;
          end  
      end
      z=pso_pid(x(i,1),x(i,2),x(i,3));
      %更新个体最优
      if z<zp_best(i)
          zp_best(i)=z;
          xp_best(i,:)=x(i,:);
      end
      %更新全局最优
      if z<zg_best
          zg_best=z;
          xg_best=x(i,:);
      end
    end 
    iter = iter+1;                % 迭代次数更新
    zz(1,iter) = zg_best;         % 为绘图做准备
    K_p(1,iter) = xg_best(1,1);
    K_i(1,iter) = xg_best(1,2);
    K_d(1,iter) = xg_best(1,3);
end
figure     % 绘制性能指标ITAE的变化曲线
plot(zz,'LineWidth',2)
title('最优个体适应值','fontsize',18);
xlabel('迭代次数','fontsize',18);ylabel('适应值','fontsize',18);
set(gca,'Fontsize',18);
 
figure      % 绘制PID控制器参数变化曲线
plot(K_p)
hold on
plot(K_i,'k','LineWidth',3)
plot(K_d,'--r')
title('Kp、Ki、Kd 优化曲线','fontsize',18);
xlabel('迭代次数','fontsize',18);ylabel('参数值','fontsize',18);
set(gca,'Fontsize',18);
legend('Kp','Ki','Kd');
zg_best
xg_best