pidnlj.m

蚁群算法优化ＰＩＤ参数．．只运行ant.m这个文件就行了

%模型参数
a(1,1)=1;b(1,1)=1;c(1,1)=1;d(1,1)=1;
a(1,2)=1;b(1,2)=1;c(1,2)=2;d(1,2)=2;
a(1,3)=1;b(1,3)=1;c(1,3)=3;d(1,3)=5;
a(2,1)=1;b(2,1)=2;c(2,1)=2;d(2,1)=2;
a(2,2)=1;b(2,2)=2;c(2,2)=3;d(2,2)=6;
a(2,3)=1;b(2,3)=2;c(2,3)=4;d(2,3)=9;
%for i=1:1:2
   %for j=1:1:3
    %r(i,j)=0.5*a(i,j)*d(i,j);
    %t1(i,j)=(a(i,j)+0.5*b(i,j)*d(i,j))/r(i,j);
    %t2(i,j)=b(i,j)/r(i,j);
    %t3(i,j)=-(0.5*c(i,j)*d(i,j))/r(i,j);
    %t4(i,j)=c(i,j)/r(i,j);
    %end
    %end
Y=rand(500,20)-0.5*ones(500,20);%随机数
Y(1,:)=zeros(1,20);
P=[6.7310 6.355];
kp=[0.2,5;0.3,6;0.4,8];ti=[10,20;30,40;50,60];td=[1,2;3,4;5,6];
U=[kp,ti,td];
R=2.0*U;%搜索半径
T=1;%采样时间
h=0.1;
%设初值
%for i=1:1:3
   %for j=1:1:500
    %u(i,j)=0;
    %end
    %end
for i=1:1:2
   for j=1:1:2002
       e(i,j)=0;ef(i,j)=0;ef1(i,j)=0;
   end
end
%for i=1:3
    %for j=1:500
        %ytm1(i,j)=0;ytm2(i,j)=0;
       %yt1(i,j)=0;yt2(i,j)=0;ysm1(i,j)=0;ysm2(i,j)=0;ys1(i,j)=0;ys2(i,j)=0;
       %end
       %end
for i=1:1:2
    for j=1:501
        f(i,j)=0;%目标函数值
    end
end
[y]=simu(P(1),P(2));
[yp]=PIDsimu(kp,ti,td);
for m=1:200/h
    for i=1:2
        ef1(i,m+1)=yp(i,m)-y(i,m);
        f(i,1)=f(i,1)+abs((m*h)*ef1(i,m));
    end
end
for i=1:2
    fmin(i)=f(i,1);
end
%终于把初始目标函数值编完了

%以下开始nlj搜索啦，程序刚刚完成1/2不到，加油阿
for J=1:50
    for n=1:300
        for i=1:3
            for j=1:6
            g(i,j)=U(i,j)+Y(n,i)*R(i,j);
            end
        end
        %[y]=simu(a(1),a(2));
        kp=g(:,1:2);ti=g(:,3:4);td=g(:,5:6);
        %for i=1:3
            %for j=1:2
                if (kp(1,1)<0)| (kp(1,2)<0)|(kp(2,1)<0)|(kp(2,2)<0)|(kp(3,1)<0)|(kp(3,2)<0)|(ti(1,1)<0) | (ti(2,1)<0) |(ti(1,2)<0) |(ti(2,2)<0) |(ti(3,1)<0) |(ti(3,2)<0) |(td(1,1)<0)|(td(1,2)<0)|(td(2,1)<0)|(td(2,2)<0)|(td(3,1)<0)|(td(3,2)<0)
                    continue;
                end
                %end
                %end
       
        for i=1:2
            s(i)=0;
        end
       for i=1:3
           for j=1:2002
               u(i,j)=0;
           end
       end
       for i=1:2
           for j=1:2002
               e(i,j)=0;yp(i,j)=0;ef(i,j)=0;%e为控制器输入，ef为输出偏差；
           end
       end
       for i=1:2
           for j=1:2002
               u1(i,j)=0;u2(i,j)=0;u3(i,j)=0;
           end
       end
       for i=1:2
           for j=1:3
               x(i,j)=0;x1(i,j)=0;
           end
       end
       
       for j=1:3               
           for i=1:d(1,j)/h
                ys1(j,i)=0;
                ysm1(j,i)=0;
               end
           end
              for j=1:3               
             for i=1:d(2,j)/h
                ys1(j,i)=0;
                ysm1(j,i)=0;
               end
           end
%龙格库塔求控制作用
for m=1:200/h
        %if m==1
            %for l=1:2
            %e(l,m)=1;
            %end
            %else
            %for l=1:2
               %e(l,m)=1-yp(l,m-1);
               %end%求e1 and e2
               %end
    %end
    for i=1:2
           u1(i,m+2)=u1(i,m+1)+kp(1,i)*(e(i,m+2)-e(i,m+1))+T/ti(1,i)*e(i,m+2)+td(1,i)/T*(e(i,m+2)-2*e(i,m+1)+e(i,m));
           u2(i,m+2)=u2(i,m+1)+kp(2,i)*(e(i,m+2)-e(i,m+1))+T/ti(2,i)*e(i,m+2)+td(2,i)/T*(e(i,m+2)-2*e(i,m+1)+e(i,m));
           u3(i,m+2)=u3(i,m+1)+kp(3,i)*(e(i,m+2)-e(i,m+1))+T/ti(3,i)*e(i,m+2)+td(3,i)/T*(e(i,m+2)-2*e(i,m+1)+e(i,m));
           %ut=u(m+2);
           % u(i,m)=u(i,m)+f4(i,j)*xu(i,j)+f2(i,j)*e(j,m);%u1/2=g1*e1+g2*e2
    end
    for i=1:2
        u(1,m+2)=u(1,m+2)+u1(i,m+2);u(2,m+2)=u(2,m+2)+u2(i,m+2);u(3,m+2)=u(3,m+2)+u3(i,m+2);
    end
    %龙格库塔求输出          
    %真实对象状态
  for i=1:2
      for j=1:3
            for k=1:1
                k1(i,j)=h*(-b(i,j)*x(i,j)+u(j,m+2));
                k2(i,j)=h*(-b(i,j)*(x(i,j)+0.5*k1(i,j))+u(j,m+2));%为什么是0.5？
                k3(i,j)=h*(-b(i,j)*(x(i,j)+0.5*k2(i,j))+u(j,m+2));%为什么是0.5？
                k4(i,j)=h*(-b(i,j)*(x(i,j)+k3(i,j))+u(j,m+2));
                x(i,j)=x(i,j)+(k1(i,j)+2*k2(i,j)+2*k3(i,j)+k4(i,j))/6;
            end
        end
    end
            %switch i
            %case '1'

            for j=1:3
                ys1(j,m+d(1,j)/h)=c(1,j)*x(1,j);

                ys2(j,m+d(2,j)/h)=c(2,j)*x(2,j);
            end
            
                yp(1,m)=ys1(1,m)+ys1(2,m)+ys1(3,m);
                yp(2,m)=ys2(1,m)+ys2(2,m)+ys2(3,m);
                
            for i=1:2
                e(i,m+1)=1-yp(i,m);
                ef(i,m+1)=yp(i,m)-y(i,m);
                if yp(i,m)>1.1
                    s(i)=0;%break;
                else
                    s(i)=1;
                    f(i,n+1)=f(i,n+1)+abs((m*h)*ef(i,m));
                end
            end
            
           if (yp(1,m)>1.1) | (yp(2,m)>1.1)%s(1)==0 | s(2)==0
                break;
            end
        end

        %for i=1:1:2
            %if (s(1)==1) & (s(2)==1)
                %if f(1,n+1)<fmin(1)
                    %P(1)=g(1);fmin(1)=f(1,n+1);
                    %end
                 %if (f(2,n+1)<fmin(2)) & (f(2,n+1)>0)
                     %P(2)=g(2);fmin(2)=f(2,n+1);
                     %end
                 if (f(1,n+1)<fmin(1))&(f(2,n+1)<fmin(2))&(f(1,n+1)>0)&(f(2,n+1)>0)
                    U=g;
               % continue;
                end
                %end
      
        n=n+1;
    end
    if J==1
        z=1.0;
    elseif 1<J & J<=15
        z=0.95;
    elseif 16<=J & J<=30
        z=0.76;
    elseif J>31
        z=0.475;
    end
    R=z*R;
end

[yp]=PIDsimu(U(:,1:2),U(:,3:4),U(:,5:6));
plot(yp(1,:),'r');
hold on;
plot(yp(2,:));