pidmac.m

来自「将PID算法与MAC算法相结合」· M 代码 · 共 130 行

%MATLAB仿真程序及仿真曲线,PIDMAC
T=3;
num=[1,2];den=[15,2,1];
g=tf(num,den);
[h,t]=impulse(g,1:T:90);
N=30;P=10;m=3;rs=0.7;%模型的的长度 N,预测步数 P， 优化步数 m,柔化因子 r
z=5;%系数namda
Kp=0*eye(m);
Ki=10*eye(m);
Kd=0*eye(m);
I=eye(m);
Q=0.3*eye(P);%误差加权矩阵
R=0.3*eye(m);%控制加权矩阵
% 系统的状态空间形式,A,B,C
A=eye(N);
A=A(2:N,:);
A(N,:)=0;
B=h;
C=[1,zeros(1,N-1)];
A2i=zeros(P,m);
for(i=1:P)
    for(j=1:m)
        if(i<j)
            A2i(i,j)=0;
        elseif(j~=m)
            A2i(i,j)=h(i+1-j);
        else
            A2i(i,j)=h(i+1-j)+A2i(i-1,j);
        end
    end
end
A2p=zeros(P,m);
for i=1:P
    for j=1:m-1
        if(i>j)
            A2p(i,j)=h(i+1-j)-h(i-j);
        elseif(i==j)
            A2p(i,j)=h(1);
        elseif (j==m||i-j>=0)
            A2p(i,j)=h(i+1-j);
        end
    end
end
A2d=zeros(P,m);
for i=1:P
    for j=1:m
        if(i>j+1)
            A2d(i,j)=h(i+1-j)-2*h(i-j)+h(i-j-1);
            if(i==j+1)
                A2d(i,j)=h(i+1-j)-2*h(i-j);
            elseif(i==j)
                A2d(i,j)=h(1);
            elseif(i>m||j==m)
                A2d(i,j)=h(i+1-j)-h(i-j);
            end
        end
    end
end
gp=inv(z*I*R+Kp*A2p'*Q*A2p+Ki*A2i'*Q*A2i+Kd*A2d'*Q*A2d)*Kp*A2p'*Q;
gi=inv(z*I*R+Kp*A2p'*Q*A2p+Ki*A2i'*Q*A2i+Kd*A2d'*Q*A2d)*Ki*A2i'*Q;
gd=inv(z*I*R+Kp*A2p'*Q*A2p+Ki*A2i'*Q*A2i+Kd*A2d'*Q*A2d)*Kd*A2d'*Q;
Gp=gp(1,:);
Gi=gi(1,:);
Gd=gd(1,:);%取矩阵K1第一行
%参考输入值
if (t<4)
    ys=1;
else ys=0;
end
% 初始值
Ap=A(1:P,1:N);
X=zeros(N);
Yd=zeros(P,N);%期望输出初始矩阵
for k=1:N
    Yp(:,k+1)=Ap*X(:,k);%系统在k时刻的未来p步预测
   %将传递函数分部分开，%系统在k时刻的实际输出 
    [r,p,k1]=residue(num,den);
    bz(1)=r(1)+r(2);
    bz(2)=-(r(1)*exp(p(2)*T)+r(2)*exp(p(1)*T));
    az(1)=1;
    az(2)=exp(p(2)*T)+exp(p(1)*T);
    az(3)=-exp((p(1)+p(2))*T);
     if (k==1)
       yv(k)=0; %系统初始的实际输出
      elseif(k==2)
      yv(k)=az(2)*yv(k-1)+az(3)*yv(1)+bz(2)*u(k-1);  
      else yv(k)=az(2)*yv(k-1)+az(3)*yv(k-2)+bz(2)*u(k-1);
      end
for j=1:P
    yd(k)=yv(k);
    yd(k+j)=rs*yd(k+j-1)+(1-rs)*ys; %第k步时未来P步的期望输出
end
    Yd(:,k+1)=yd(k+1:k+P)';
    if (k==1)
        u(k)=Gi*[Yd(:,k+1)-Yp(:,k+1)]+Gp*[Yd(:,k+1)-Yp(:,k+1)-Yd(:,k)+Yp(:,k)]+Gd*[Yd(:,k+1)-Yp(:,k+1)-Yd(:,k)-Yd(:,k)+Yp(:,k)+Yp(:,k)];
    else u(k)=Gi*[Yd(:,k+1)-Yp(:,k+1)]+Gp*[Yd(:,k+1)-Yp(:,k+1)-Yd(:,k)+Yp(:,k)]+Gd*[Yd(:,k+1)-Yp(:,k+1)-Yd(:,k)-Yd(:,k)+Yp(:,k)+Yp(:,k)+Yd(:,k-1)-Yp(:,k-1)];
    end
%设定控制增量和控制量的限制
  Umin=-2;
  Umax=2;
  %检验增量是否超限
  if (u(k)>Umax)
      u(k)=Umax;
  elseif(u(k)<Umin)
      u(k)=Umin;
  end
if(k-1==0)
   u(k)=1;
end
% 闭环预测状态观测器，根据k时刻系统的状态预测k+1时刻系统的状态
F=0.3*ones(N,1); %预测观测器增益
Ak=A-F*C;  %预测状态观测器的系数
X(:,k+1)=Ak*X(:,k)+B*u(k)+F*yv(k);
yp(k+1)=X(1,k);
end
t=1:T:90;
figure(1)
hold on;
stairs(t,u,'b-');
hold off;
axis([1,30*T,-0.5,1.5])
xlabel('t'),ylabel('u')
grid on
title('系统的最优控制序列u(t)')
figure(2)
plot(t,yv,'b-')
axis([1,90,-0.5,0.5])
xlabel('t'),ylabel('yv')
grid on
title('系统的输出y(t)')