LQIM制御/H∞IM制御

メインページ

●制御対象（赤）
$\left\{\begin{array}{l} \dot{\vec x}=A(T_w){\vec x}+B(T_w)\vec{u}+B_w(T_w,\beta){\vec w},\ {\vec x}(0)={\vec x}_0\\ {\vec y}_M=C_M{\vec x}\\ y=\underbrace{C_SC_M}_{C}{\vec x} \end{array}\right.$
●外乱信号（赤紫）
${\vec w}=\left[\begin{array}{l} H_w\sin\frac{2\pi}{T_w} t \\ H_w\cos\frac{2\pi}{T_w} t \end{array}\right]$
ここで、 $H_w=0.01[m]$ とする。
●外乱推定に用いる観測変数（緑）
$y_d=C_d{\vec x}$
これは主に制御点の縦変位を取る。
●外乱フィルタ（緑）
$\dot{\hat{\vec x}}_d=A_d(T_w)\hat{{\vec x}}_d+B_d(T_w){y}_d,\ \hat{x}_d(0)=0$
●安定化制御則（緑）
$\vec{u}=-F(T_w)\vec{x}-F_d(T_w)\hat{\vec{x}}_d$
●推力配分（橙）
$\vec{u}$ は制御目的を達成するための望ましい $F_z,F_\phi,F_\theta$ であるが、これは４つのスラスタに次式で配分する。
$\underbrace{\left[\begin{array}{c}F_1\\F_2\\F_3\\F_4\end{array}\right]}_{\vec F}= \underbrace{\frac{1}{4}\left[\begin{array}{rrr} 1 & 1 & -1 \\ 1 & 1 & 1 \\ 1 & -1 & 1\\1 & -1 & -1 \end{array}\right]}_{G^\dag} \underbrace{\left[\begin{array}{c}F_z\\F_\phi\\F_\theta\end{array}\right]}_{\vec\tau}$
●推力制限（橙）
その際、次の制約が課される。
$F_{min}\le F_k\le F_{max}\quad(k=1,2,3,4)$
上のシミュレータは、この制約を考慮したものとなっている。ここでは、 $F_{max}=5$ [kgf]、 $F_{min}=-5$ [kgf]を用いる（要チェック）。
●留意点
制約を受けた推力は次式で駆動力に変換されるが、推力制限がある場合は設計した駆動力が働かないので注意が必要である。
$\underbrace{\left[\begin{array}{c}F_z\\F_\phi\\F_\theta\end{array}\right]}_{\vec\tau}= \underbrace{\left[\begin{array}{rrrr} 1 & 1 & 1 & 1\\ 1 & 1 & -1 & -1\\ -1 & 1 & 1 & -1 \end{array}\right]}_{G} \underbrace{\left[\begin{array}{c}F_1\\F_2\\F_3\\F_4\end{array}\right]}_{\vec F}$

このとき、次の16ケースについて、上のシミュレータを用いてシミュレーションを行なう。ただし、短周期は $6/\sqrt{50}$ 、長周期は $10/\sqrt{50}$ とする。

モデル/制御	横波短周期	横波長周期	斜波短周期	斜波長周期
２次系/LQIM	Case 2.1	Case 2.2	Case 2.3	Case 2.4
４次系/LQIM	Case 2.5	Case 2.6	Case 2.7	Case 2.8
６次系/LQIM	Case 2.9	Case 2.10	Case 2.11	Case 2.12
６次系/H∞IM	Case 2.13	Case 2.14	Case 2.15	Case 2.16


%---LQIM制御（２次）
if iCON==2 & iTAR==0,  %phi
  is=[4,10]; ks=[2]; ns=length(is); ms=length(ks);
  A=AA(is,is,iTW); B1=BB2(is,:,iTW); B2=BB1(is,ks,iTW); 
  Cd=[CM(6,2) 0];
  AE=[A zeros(ns,2);Bd(:,:,iTW)*Cd Ad(:,:,iTW)];
  BE1=[B1;zeros(2)]; BE2=[B2;zeros(2,ms)];  
  CE=[1 0 0 0;0 0 1 0];
  Mphi=0.5/180*pi; Mu=5/2; 
  QE=diag(1./[Mphi Mphi].^2); RE=1/Mu^2;  
  [FE,pCLE]=opt(AE,BE2,CE,QE,RE);  %FE=FE*0   
end
%---LQIM制御（４次）
if iCON==3 & iTAR==0,  %phi
  is=[4,5,10,11]; ks=[2,3]; ns=length(is); ms=length(ks);
  A=AA(is,is,iTW); B1=BB2(is,:,iTW); B2=BB1(is,ks,iTW); 
  Cd=[CM(4,[2,3]) 0 0];
  AE=[A zeros(ns,2);Bd(:,:,iTW)*Cd Ad(:,:,iTW)];
  BE1=[B1;zeros(2)]; BE2=[B2;zeros(2,ms)]; 
  CE=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 0 0 1 0];
  Mphi=0.5/180*pi; Mu=5/2; 
  QE=diag(1./[Mphi Mphi/10 Mphi].^2); RE=1/Mu^2;  
  [FE,pCLE]=opt(AE,BE2,CE,QE,RE);    
end
%---LQIM制御（６次）
if iCON==4 & iTAR==0,  %phi
  is=[3,4,5,9,10,11]; ks=[1,2,3]; ns=length(is); ms=length(ks);
  A=AA(is,is,iTW); B1=BB2(is,:,iTW); B2=BB1(is,ks,iTW); 
  Cd=[CM(4,[1 2,3]) 0 0 0];
  AE=[A zeros(ns,2);Bd(:,:,iTW)*Cd Ad(:,:,iTW)];
  BE1=[B1;zeros(2)]; BE2=[B2;zeros(2,ms)]; 
  CE=[eye(3,ns+2);0 0 0 0 0 0 1 0];
  Mz=0.002; Mphi=0.5/180*pi; Mu=5/2; 
  QE=diag(1./[Mz Mphi Mphi/10 Mphi].^2); RE=1/Mu^2;  
  [FE,pCLE]=opt(AE,BE2,CE,QE,RE);    
end
%---H∞IM制御（６次）
if iCON==5 & iTAR==0,  %phi
  is=[3,4,5,9,10,11]; ks=[1,2,3]; ns=length(is); ms=length(ks);
  A=AA(is,is,iTW); B1=BB2(is,:,iTW); B2=BB1(is,ks,iTW); 
  Cd=[CM(4,[1,2,3]) 0 0 0];  
  AE=[A zeros(ns,2);Bd(:,:,iTW)*Cd Ad(:,:,iTW)];
  BE1=[B1;zeros(2)]; BE2=[B2;zeros(2,ms)];
  CE=[eye(3,ns+2);0 0 0 0 0 0 1 0];  
  Mz=0.002; Mphi=0.5/180*pi; Mu=5/2;   
  C1=[1/Mz*[Cd 0 0];zeros(3,6+2)]; 
  D11=zeros(4,2); 
  D12=[zeros(1,3);1/Mu*eye(3)];
  alpha=0.1; r=20; th=pi/4;
  LMIs=sf_synlmi6(AE,BE1,BE2,C1,D11,D12,alpha,r,th);
  cobj=zeros(1,decnbr(LMIs)); 
  cobj(1)=1; 
  [cost,xopt]=mincx(LMIs,cobj); 
  YE=dec2mat(LMIs,xopt,2);  
  ZE=dec2mat(LMIs,xopt,3);  
  FE=ZE/YE;  
  pCLE=eig(AE-BE2*FE)
end
  figure(1) 
  CdE=[Cd 0 0];
  ol=ltisys(AE,BE1,CdE); 
  cl=ltisys(AE-BE2*FE,BE1,CdE); 
  om=logspace(-1,2,200); 
  splot(cl,'sv',om),hold on 
  splot(ol,'sv',om),grid

以下に、各ケースについてLQ制御系/H∞制御系の応答とゲイン線図を示す。前者の上段は制御点の応答[cm]、中段は操作入力[kgf]、下段は波浪外力[kgf]を示している。閉ループ系では
$\dot{\vec x}=(A(T_w)-B(T_w)F(T_w)){\vec x}\underbrace{-B(T_w)F_d(T_w)\hat{\vec{x}}_d+B_w(T_w,\beta){\vec w}}_{相殺?}$
が成り立つと思われる。左辺第２項（外乱推定項）と第３項（外乱項）が相殺するかどうかを下段で確認している。４秒までは制御なしで、４秒以降が制御ありの応答。制御点は横波・斜波とも右舷船首S1を取っている（横波の場合、右舷制御点の区別はない）。

●留意点　
外乱から着目する制御点までのゲイン線図は、LQIM制御系の場合、波周波数においてピンポイントで低減され、H∞IM制御系の場合、波周波数近傍で低減されている。

２次モデルに対するLQIM制御
Case 2.1（横波、短周期規則波）　　　　Case 2.2（横波、長周期規則波）
　
　