概要

PID制御は現代の制御の９割を締めているといわれています。
現代制御の状態フィードバックはPD制御、サーボ系を入れるとPIDになる
と考える事ができるため、現代制御もモデルベースによってゲイン決定をしたPID制御といえます。これらは目標値に状態量を追従させる事を目的としています

今回は制御入力を0にする事を目的とした場合のPID制御方法を提案します。
その方法とは
入力を積分してフィードバックに追加
これを提案します。

シミュレーション

\dot{x}=Ax+Bu

という、状態方程式に対し

u=Fx

という状態フィードバックする事で制御します。これは、状態ベクトルｘが

x=[x_1 x_2]^T

の場合、$x_2$は$x_1$の微分したものです。

u=F_1 x_1 + F_2 x_2

これは、PD制御と考えられます。

ここで、状態フィードバックを行うためには、状態量$x$を計測する必要がありますが、定常偏差が乗る場合が多いです。
定常偏差ベクトルを$\alpha$とすると

u=F(x+\alpha)

というフィードバックを行う事になります。

--> AD
 AD  = 

   0.9768239   1.2834701
   0.0098728   1.0064424

--> BD
 BD  = 

   0.0296185
   0.0001487

--> alpha
 alpha  = 

   0.
   0.001

--> F
 F  = 

  -6.8504172  -82.277427

--> x0
 x0  = 

   0.
   0.005

--> spec(AD)　　：　不安定　制御なしでは状態量は無限大に発散する
 ans  =
   0.8780954  
   1.1051709

という条件でシミュレーションします。

偏差$\alpha$が乗らなかった場合

入力、状態量ともに0に収束しています

偏差が乗った場合

入力は0.1の偏差、状態量は緑の方に偏差が残ってしまっています。
緑の状態量の方の計測データに偏差（誤差）が乗っているのでずれたと予想できます。

ここでサーボ系を組む場合、状態量を積分しフィードバックします（I制御）。

    uu=F*(xx+alpha) + KI*sumX;    //制御入力決定     
    sumX = sumX + (xx+alpha);     //計測状態量積分

の形で制御します。

5s後の状態量は

--> xx
 xx  =
  -0.0000008
  -0.0009991

--> uu
 uu  = 
   0.0432931

に状態量は最終的に収束しています。これでもやはり、緑側の状態量を0に収束させる事ができません。

入力積分型

積分器は、積分する対象をずれなく完全に0に収束させる効果があります。
入力を積分すれば、入力を完全に0に収束させることが可能です。

    uu=F*(xx+alpha) + sumU * kpI;
    sumU = sumU + uu;

の形で制御します。

5s後の状態量は

--> xx
 xx  = 
   0.0000044
  -0.0000028

--> uu
 uu  = 
   0.0001224

計測状態量に偏差が乗っているという面倒なシステムに対し、入力を0に収束させながらシステムを安定がさせる事ができました。

シミュレーションコード

入力積分方式（入力PID）

IntegerI

clear();
for i = 1:10 do
    close
end

/*　
A = [-6     0         //mg/J由来が倍大きくなったとすると
      1     0 ];
      
B=[8713
   0];
*/
/*  //自然振り子
A = [-1.825675  -81.68         //mg/J由来が倍大きくなったとすると
      1     0 ];
      
B=[-0.001778742
   0];
*/
//倒立
/*
 A = [-1.825675  81.68         //mg/J由来が倍大きくなったとすると
          1     0 ];
      
B=[+0.001778742
   0];
   */
A = [-3  130         //mg/J由来が倍大きくなったとすると
     1     0 ];
      
B=[3
   0];
V=[B, A*B ];
rank(V)

C=[1 0];

VO=[C
    C*A];
rank(VO)

D=[0];

x0=[0.00
    0.005]
sl = syslin('c',A,B,C,D,x0);
sl2= dscr(sl,0.01);              //離散化

//-----------離散化したＡ，Ｂ，Ｃ，Ｄについてプログラム用に書き下す
AD=sl2(2);
BD=sl2(3);
CD=sl2(4);
for i=1:2 do
    for j = 1:2 do
       printf("volatile float a%1d%1d = %10f;\n",i,j,AD(i,j)); 
    end
end
for i=1:2 do
    printf("volatile float b%1d = %10f;\n",i,BD(i,1)); 
end

poles = [0.5         
         0.5];
//spec( AD + H*CD ) = poles
H=(ppol(sl2(2)',-1*sl2(4)',poles))';  //オブザーバゲインH決定
for i=1:2 do
    printf("volatile float h%1d = %10f;\n",i,H(i,1)); 
end

t=0:0.01:5;
for    i = 1:501  do
    u(i)=0
    ,end


//フィードバッグゲイン計算
Q=eye(2,2);
R=10;
X=riccati(sl2(2),sl2(3)*inv(R)*sl2(3)',Q,'d','eigen');
K=inv(R)*sl2(3)'*X;
F=sl2(2)-sl2(3)*K;


B4=[0 0]';
x2=ltitr(F,B4,u',x0);    //離散シミュレーション
y2=sl2(4)*x2;
//plot(t,y2);
//scf();

ure=-1*K*x2;
//plot(t,ure);
//scf();
//plot(t,x2);
//scf();



AD=sl2(2);
BD=sl2(3);
CD=sl2(4);

//-------------離散フィードバックゲイン
R = [1];
Q = [1 0
     0 1]*10;
B_ric = BD*inv(R)*BD'
[X1,X2] = riccati(AD,B_ric,Q,'d')
X = X1*inv(X2)
F = -1*inv(R+BD'*X*BD)*(BD'*X*AD)
for i=1:2 do
    printf("volatile float f%1d = %10f;\n",i,F(1,i)); 
end

disp(spec(AD+BD*F))


uu=K*x0;
if uu> 1 then
      uu=1;
elseif uu<-1 then
      uu=-1;
end        
xx=x0;
//uに上限がある場合のシミュレーション
xxx=x0;
uuu=uu;
alpha = [0
         0.001]
         
sumU = 0;
kpI = 0.01;
for    i = 1:500  do
    yy=CD*xx;
    uu=F*(xx+alpha) + sumU * kpI;
    sumU = sumU + uu;
    if uu> 1 then
        uu=1;
    elseif uu<-1 then
        uu=-1;
    end        
    xx=AD*xx+BD*uu;
    xxx=cat(2,xxx,xx);  //どんどん列方向に　xxx　に　xx　を繋げていく
    uuu=cat(2,uuu,uu);
,end

plot(t,xxx);
hl=legend(['x1';'x2']);
scf();
plot(t,uuu);
hl=legend(['u']);
printf("sumU = %f\n", sumU);
printf("sumU*kpI = %f", sumU*kpI);

状態量積分方式

IntegerX

clear();
for i = 1:10 do
    close
end

/*　
A = [-6     0         //mg/J由来が倍大きくなったとすると
      1     0 ];
      
B=[8713
   0];
*/
/*  //自然振り子
A = [-1.825675  -81.68         //mg/J由来が倍大きくなったとすると
      1     0 ];
      
B=[-0.001778742
   0];
*/
//倒立
/*
 A = [-1.825675  81.68         //mg/J由来が倍大きくなったとすると
          1     0 ];
      
B=[+0.001778742
   0];
   */
A = [-3  130         //mg/J由来が倍大きくなったとすると
          1     0 ];
      
B=[3
   0];
V=[B, A*B ];
rank(V)

C=[1 0];

VO=[C
    C*A];
rank(VO)

D=[0];

x0=[0.00
    0.005]
sl = syslin('c',A,B,C,D,x0);
sl2= dscr(sl,0.01);              //離散化

//-----------離散化したＡ，Ｂ，Ｃ，Ｄについてプログラム用に書き下す
AD=sl2(2);
BD=sl2(3);
CD=sl2(4);
for i=1:2 do
    for j = 1:2 do
       printf("volatile float a%1d%1d = %10f;\n",i,j,AD(i,j)); 
    end
end
for i=1:2 do
    printf("volatile float b%1d = %10f;\n",i,BD(i,1)); 
end

poles = [0.5         
         0.5];
//spec( AD + H*CD ) = poles
H=(ppol(sl2(2)',-1*sl2(4)',poles))';  //オブザーバゲインH決定
for i=1:2 do
    printf("volatile float h%1d = %10f;\n",i,H(i,1)); 
end
T=10
DT =0.01
t=0:DT:T;
for    i = 1:int(T/DT)+1  do
    u(i)=0
    ,end


//フィードバッグゲイン計算
Q=eye(2,2);
R=10;
X=riccati(sl2(2),sl2(3)*inv(R)*sl2(3)',Q,'d','eigen');
K=inv(R)*sl2(3)'*X;
F=sl2(2)-sl2(3)*K;


B4=[0 0]';
x2=ltitr(F,B4,u',x0);    //離散シミュレーション
y2=sl2(4)*x2;
//plot(t,y2);
//scf();

ure=-1*K*x2;
//plot(t,ure);
//scf();
//plot(t,x2);
//scf();



AD=sl2(2);
BD=sl2(3);
CD=sl2(4);

//-------------離散フィードバックゲイン
R = [1];
Q = [1 0
     0 1]*10;
B_ric = BD*inv(R)*BD'
[X1,X2] = riccati(AD,B_ric,Q,'d')
X = X1*inv(X2)
F = -1*inv(R+BD'*X*BD)*(BD'*X*AD)
for i=1:2 do
    printf("volatile float f%1d = %10f;\n",i,F(1,i)); 
end

disp(spec(AD+BD*F))


uu=K*x0;
if uu> 1 then
      uu=1;
elseif uu<-1 then
      uu=-1;
end        
xx=x0;
//uに上限がある場合のシミュレーション
xxx=x0;
uuu=uu;
alpha = [0
         0.001]
         
sumX = [0
        0];
KI = [-2 -2]
for    i = 1:int(T/DT) do
    yy=CD*xx;
    uu=F*(xx+alpha) + KI*sumX;
    if uu> 1 then
        uu=1;
    elseif uu<-1 then
        uu=-1;
    end        
    sumX = sumX + (xx+alpha);
    xx=AD*xx+BD*uu;
    xxx=cat(2,xxx,xx);  //どんどん列方向に　xxx　に　xx　を繋げていく
    uuu=cat(2,uuu,uu);
,end

plot(t,xxx);
hl=legend(['x1';'x2']);
scf();
plot(t,uuu);
hl=legend(['u']);

積分器のフィードバックゲインについて

フィードバックゲインを上げると発散します。

どれくらいが良いかは難しいですが、この値（sumU）は偏差$\alpha$によって決定する量なので（偏差がなければsumU = 0）、一度これくらいが丁度良いとわかればゲインは低い値で初期sumUを調整するのが良い方法だと思っています。

実際に、3次元倒立振子に対し適用した記事を投稿するつもりなのでそちらで、実践の個人的なコツを書きたいと思います。

追記

P制御入力を積分しても偏差は解消しない。

    uu=F*(xx+alpha) + sumU * kpI;
    sumU = sumU + F(2)*(xx(2)+alpha(2));

これでは、解消しない

sumU = sumU + F*(xx+alpha);

これでも解消しない。
sumUを含めて、実際に入力したデータを積分しないと偏差解消にはならないのである。

sumU = sumU + F*(xx+alpha)+ sumU * kpI;

としないといけないのである．