制御系CAD

制御を意識したモノつくりを目指して

LQ制御/H∞制御

投稿日時: 2025年8月2日投稿者: cacsd

●制御対象（赤）
$\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]$ とする。
●安定化制御則（緑）
$\vec{u}=-F(T_w)\vec{x}$
●推力配分（橙）
$\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}=2.5$ [kgf]、 $F_{min}=-2.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}$ とする。

モデル/制御	横波短周期	横波長周期	斜波短周期	斜波長周期
２次系/LQ	Case 1.1	Case 1.2	Case 1.3	Case 1.4
４次系/LQ	Case 1.5	Case 1.6	Case 1.7	Case 1.8
６次系/LQ	Case 1.9	Case 1.10	Case 1.11	Case 1.12
６次系/H∞	Case 1.13	Case 1.14	Case 1.15	Case 1.16


%---LQ制御（２次）
if iCON==2 & iTAR==0, 
  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]; 
  Mphi=0.5/180*pi; Mu=5/2; 
  Q=1/Mphi^2; R=1/Mu^2;  
  C=[1 0]; 
  [F,pCL]=opt(A,B2,C,Q,R);    
end  
%---LQ制御（４次）
if iCON==3 & iTAR==0, 
  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]; 
  Mphi=0.5/180*pi; Mu2=5/2;
  Q=diag(1./[Mphi Mphi/10].^2); R=1/Mu2^2;  
  C=eye(2,ns); 
  [F,pCL]=opt(A,B2,C,Q,R);    
end    
%---LQ制御（６次）
if iCON==4 & iTAR==0,  
  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]; 
  Mz=0.002; Mphi=0.5/180*pi; Mu=5/2; 
  Q=diag(1./[Mz Mphi Mphi/10].^2); R=1/Mu^2; 
  C=eye(3,ns); 
  [F,pCL]=opt(A,B2,C,Q,R);  
end
%---H∞制御（６次）
if iCON==5 & iTAR==0,  
  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];    
  Mz=0.002; Mphi=0.5/180*pi; Mu=5/2;   
  C1=[1/Mz*Cd;zeros(3,6)]; 
  D11=zeros(4,2); 
  D12=[zeros(1,3);1/Mu*eye(3)]; 
  alpha=0.1; r=20; th=pi/4;
  LMIs=sf_synlmi6(A,B1,B2,C1,D11,D12,alpha,r,th);
  cobj=zeros(1,decnbr(LMIs)); 
  cobj(1)=1; 
  [cost,xopt]=mincx(LMIs,cobj); 
  Y=dec2mat(LMIs,xopt,2);  
  Z=dec2mat(LMIs,xopt,3);  
  F=Z/Y;  
  pCL=eig(A-B2*F)  
end
%---sigma plot
  figure(1) 
  ol=ltisys(A,B1,Cd); 
  cl=ltisys(A-B2*F,B1,Cd); 
  om=logspace(-1,2,200); 
  splot(cl,'sv',om),hold on 
  splot(ol,'sv',om),grid

以下に、各ケースについてLQ制御系/H∞制御系の応答とゲイン線図を示す。前者の上段は制御点の応答[cm]、中段は操作入力[kgf]、下段は波浪外力[kgf]を示している。４秒までは制御なしで、４秒以降が制御ありの応答。制御点は横波・斜波とも右舷船首S1を取っている（横波の場合、右舷制御点の区別はない）。

２次モデルに対するLQ制御
２次モデルの状態変数はロールとその微分。ここではロールに浮体の半幅長さを掛けた制御点の縦変位の低減を図っている。
Case 1.1（横波、短周期規則波）　　　Case 1.2（横波、長周期規則波）

Case 1.1コメント
●応答図上段：横波を受けて4秒までは制御点の縦変位は2cm程度まで大きく出る。4秒になると制御が始まり、1/10の0.2㎝程度まで低減される。
●応答図下段：浮体が受ける波浪外力を表している。計算式は
$B_{w,5,1}H_w\sin\frac{2\pi}{T_w}t+B_{w,5,2}H_w\cos\frac{2\pi}{T_w}t$ を用いている。4秒までは5つの山がありますが、これを受けて制御点にも５つの山があることに注意。そして、4秒付近で、波浪外力は次の山に向かって負から正の方向に働いている。
●応答図中段：この外力を打ち消すように、4秒から制御動作が始まる。
●波浪外力から制御点までのゲイン線図を、制御前の開ループ系（橙）と制御前の閉ループ系（青）について示している。波周期に対応する角周波数は $2\pi/(6/\sqrt{50})=7.4$ [rad/sec]です。閉ループ系のゲイン線図から、開ループ系のピークが抑制されていることが分かる。-20dBは1/10を意味する。
Case 1.2コメント
●波周期に対応する角周波数は $2\pi/(10/\sqrt{50})=4.4$ [rad/sec]。閉ループ系のゲイン線図から、開ループ系のピークが抑制されていることが分かる。

Case 1.3（斜波、短周期規則波）　　　Case 1.4（斜波、長周期規則波）
　
　
Case 1.3コメント
●斜波のときは必ずピッチが生じるが、ここではロールだけに注目している。浮体が受ける波浪外力はCase1.1に比べて小さくなっている。
Case 1.4コメント
●斜波のときは必ずピッチが生じるが、ここではロールだけに注目している。浮体が受ける波浪外力はCase1.2に比べて小さくなっている。

４次モデルに対するLQ制御
４次モデルの状態変数はロールとピッチ、およびそれらの微分。ここではロールとピッチから計算される制御点の縦変位（ヒーブは入らないことに注意）の低減を図っている。

Case 1.5（横波、短周期規則波）　　　Case 1.6（横波、長周期規則波）
　
　

Case 1.5コメント
●横波の場合はピッチは誘起されないので、Case1.1と同じになる。
●応答図中段：ピッチが誘起されないので、ピッチ駆動力(橙)は零。
Case 1.6コメント
●横波の場合はピッチは誘起されないので、Case1.2と同じになる。
●応答図中段：ピッチが誘起されないので、ピッチ駆動力(橙)は零。

Case 1.7（斜波、短周期規則波）　　　Case 1.8（斜波、長周期規則波）
　
　

Case 1.7コメント
●応答図下段：ピッチ運動を誘起する波浪外力（橙）が生じている。計算式は $B_{w,6,1}H_w\sin\frac{2\pi}{T_w}t+B_{w,6,2}H_w\cos\frac{2\pi}{T_w}t$ を用いています。
●応答図中段：ピッチ運動を抑制するピッチ駆動力(橙)が生じている。
Case 1.8コメント
●長周期斜波の場合の波浪外力は、短周期の場合に比べて、大き目に出る模様。

６次モデルに対するLQ制御
６次モデルの状態変数はヒーブとロール、ピッチ、およびそれらの微分。ここではヒーブとロール、ピッチから計算される制御点の縦変位（ヒーブが加わことに注意）の低減を図っている。

Case 1.9（横波、短周期規則波）　　　Case 1.10（横波、長周期規則波）
　
　

Case 1.9コメント
●横波の場合はピッチは誘起されないが、ヒーブが現れる。Case1.5との違いは、それに起因すると考えられる。
●応答図中段：青、赤、橙の順にヒーブ、ロール、ピッチに働く駆動力を表している。
●応答図下段：青、赤、橙の順にヒーブ、ロール、ピッチに働く波浪外力を表している。計算式は、それぞれ次を用いる。
$B_{w,4,1}H_w\sin\frac{2\pi}{T_w}t+B_{w,4,2}H_w\cos\frac{2\pi}{T_w}t$
$B_{w,5,1}H_w\sin\frac{2\pi}{T_w}t+B_{w,5,2}H_w\cos\frac{2\pi}{T_w}t$
$B_{w,6,1}H_w\sin\frac{2\pi}{T_w}t+B_{w,6,2}H_w\cos\frac{2\pi}{T_w}t$
Case 1.10コメント
●横波の場合はピッチは誘起されないが、ヒーブが現れます。Case1.6との違いは、それに起因すると考えられる。

Case 1.11（斜波、短周期規則波）　　　Case 1.12（斜波、長周期規則波）
　
　

Case 1.11コメント
●斜波の場合はピッチが誘起され、ヒーブが現れる。Case1.7との違いは、それに起因すると考えらる。
Case 1.12コメント
●斜波の場合はピッチが誘起され、ヒーブが現れる。Case1.8との違いは、それに起因すると考えらる。

６次モデルに対するH∞制御
波浪外力から制御点までのゲイン線図は、LQ制御と比べて、波周期に対応する角周波数でより低減されていて、制御点縦変位の抑制も効いています。

Case 1.13（横波、短周期規則波）　　　Case 1.14（横波、長周期規則波）
　
　
Case 1.13コメント
Case 1.14コメント

Case 1.15（斜波、短周期規則波）　　　Case 1.16（斜波、長周期規則波）
　
　
Case 1.15コメント
Case 1.16コメント

メインページ

Marine Craft

投稿日時: 2022年2月10日投稿者: cacsd

Marine Craft

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{\eta}=J_\Theta(\eta)\nu\\ M\dot{\nu}+C(\nu)\nu+D(\nu)\nu+g(\eta)=\tau\\ \tau=Bu \end{array}\right. }$

$1^\circ$ $M=M^T$ , $\dot{M}=0$
$2^\circ$ $C(\nu)=-C^T(\nu)$
$3^\circ$ $D(\nu)>0$
$4^\circ$ $BB^T$ は正則
$5^\circ$ $J_\Theta(\eta)$ はオイラー角変換行列

$\displaystyle{(1')\quad \left\{\begin{array}{l} M^*(\eta)\ddot{\eta}+C^*(\nu,\eta)\dot{\eta}+D^*(\nu,\eta)\dot{\eta}+g^*(\eta)=J^{-T}_\Theta(\eta)\tau\\ M^*(\eta)=J^{-T}_\Theta(\eta)MJ^{-1}_\Theta(\eta)\\ C^*(\nu,\eta)=J^{-T}_\Theta(\eta)(C(\nu)-MJ^{-1}_\Theta(\eta)\dot{J}_\Theta(\eta))J^{-1}_\Theta(\eta)\\ D^*(\nu,\eta)=J^{-T}_\Theta(\eta)D(\nu)J^{-1}_\Theta(\eta)\\ g^*(\eta)=J^{-T}_\Theta(\eta)g(\eta) \end{array}\right. }$

●Step 1

$\displaystyle{(2)\quad \boxed{\tilde{\eta}=\eta-\eta_d,\ \tilde{\nu}=\nu-\nu_d}\quad\Rightarrow\quad\dot{\tilde{\eta}}=J_\Theta(\eta)\tilde{\nu} }$

$\displaystyle{(3)\quad \dot{\eta}=s+\alpha_1 }$

$\displaystyle{(4)\quad \boxed{s=\dot{\tilde{\eta}}+\Lambda\tilde{\eta}}=\dot{\eta}-\dot{\eta}_d+\Lambda\tilde{\eta}}\quad(\Lambda={\rm diag}\{\lambda_1,\dots,\lambda_m\}) }$

$\displaystyle{(5)\quad \boxed{\alpha_1=\dot{\eta}-s=\dot{\eta}_d-\Lambda\tilde{\eta}} }$

$\displaystyle{(6)\quad \dot{\tilde{\eta}}=\dot{\eta}-\dot{\eta}_d=-\Lambda\tilde{\eta}+s }$

$\displaystyle{(7)\quad \boxed{V_1=\frac{1}{2}\tilde{\eta}^TK_p\tilde{\eta}}\quad(K_p>0) }$

$\displaystyle{(8)\quad \dot{V}_1=\tilde{\eta}^TK_p\dot{\tilde{\eta}}=\tilde{\eta}^TK_p(-\Lambda\tilde{\eta}+s) \quad\Rightarrow\quad \boxed{\dot{V}_1=-\tilde{\eta}^TK_p\Lambda\tilde{\eta}+s^TK_p\tilde{\eta}} }$

●Step 2

$\displaystyle{(9)\quad s=\dot{\tilde{\eta}}+\Lambda\tilde{\eta}=\dot{\eta}-\dot{\eta}_d+\Lambda\tilde{\eta} \quad\Rightarrow\quad\dot{\eta}=s+\dot{\eta}_d-\Lambda\tilde{\eta} }$

$\displaystyle{(10)\quad \dot{s}=\ddot{\tilde{\eta}}+\Lambda\dot{\tilde{\eta}}=\ddot{\eta}-\ddot{\eta}_d+\Lambda\dot{\tilde{\eta}} \quad\Rightarrow\quad\ddot{\eta}=\dot{s}+\ddot{\eta}_d-\Lambda\dot{\tilde{\eta}} }$

$\displaystyle{(11)\quad \begin{array}{l} M^*(\eta)(\dot{s}+\ddot{\eta}_d-\Lambda\dot{\tilde{\eta}})\\ +C^*(\nu,\eta)(s+\dot{\eta}_d-\Lambda\tilde{\eta}) +D^*(\nu,\eta)(s+\dot{\eta}_d-\Lambda\tilde{\eta}) +g^*(\eta)=J^{-T}_\Theta(\eta)\tau \end{array} }$

$\displaystyle{(1')\quad \left\{\begin{array}{l} M^*(\eta)=J^{-T}_\Theta(\eta)MJ^{-1}_\Theta(\eta)\\ C^*(\nu,\eta)=J^{-T}_\Theta(\eta)(C(\nu)-MJ^{-1}_\Theta(\eta)\dot{J}_\Theta(\eta))J^{-1}_\Theta(\eta)\\ D^*(\nu,\eta)=J^{-T}_\Theta(\eta)D(\nu)J^{-1}_\Theta(\eta)\\ g^*(\eta)=J^{-T}_\Theta(\eta)g(\eta) \end{array}\right. }$

$\displaystyle{(12)\quad \begin{array}{l} M^*(\eta)\dot{s}=-C^*(\nu,\eta)s-D^*(\nu,\eta)s+J^{-T}_\Theta(\eta)\tau\\ -M^*(\eta)(\ddot{\tilde{\eta}}_d-\Lambda\dot{\tilde{\eta}})-C^*(\nu,\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})-D^*(\nu,\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})-g^*(\eta)\\ =-C^*(\nu,\eta)s-D^*(\nu,\eta)s\\ +J^{-T}_\Theta(\eta)(\tau -\underbrace{J^{T}_\Theta(\eta)M^*(\eta)J_\Theta(\eta)}_{M} \underbrace{J^{-1}_\Theta(\eta)(\ddot{\tilde{\eta}}_d-\Lambda\dot{\tilde{\eta}})}_ {\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}}\\ -\underbrace{J^{T}_\Theta(\eta)C^*(\nu,\eta)J_\Theta(\eta)}_{C(\nu)-MJ^{-1}_\Theta(\eta)\dot{J}_\Theta(\eta)} \underbrace{J^{-1}_\Theta(\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})}_ {\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}}\\ -\underbrace{J^{T}_\Theta(\eta)D^*(\nu,\eta)J_\Theta(\eta)}_{D(\nu)} \underbrace{J^{-1}_\Theta(\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})}_ {\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}}\\ -\underbrace{J^{T}_\Theta(\eta)g^*(\eta)}_{g(\eta)})\\ =-C^*(\nu,\eta)s-D^*(\nu,\eta)s\\ +J^{-T}_\Theta(\eta)(\tau-M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}})-C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta)) \end{array} }$

$\displaystyle{(13)\quad \boxed{V_2=V_1+\frac{1}{2}s^TM^*(\eta)s} }$

$\displaystyle{(14)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+s^TM^*(\eta)\dot{s}+\frac{1}{2}s^T\dot{M}^*(\eta)s\\ =-\tilde{\eta}^TK_p\Lambda\tilde{\eta}+s^TK_p\tilde{\eta}\\ +s^T(-C^*(\nu,\eta)s-D^*(\nu,\eta)s)\\ +s^TJ^{-T}_\Theta(\eta)(\tau-M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}) -C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu}) -D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta))\\ +\frac{1}{2}s^T\dot{M}^*(\eta)s\\ =s^TJ^{-T}_\Theta(\eta)(\tau-M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}})-C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta)\\+J^{T}_\Theta(\eta)K_p\tilde{\eta}) +\frac{1}{2}\underbrace{s^T(\dot{M}^*(\eta)-C^*(\nu,\eta))s}_{0}\\ -s^TD^*(\nu,\eta)s-\tilde{\eta}^TK_p\Lambda\tilde{\eta} \end{array} }$

$\displaystyle{(15)\quad \boxed{\begin{array}{l} \tau=M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}})+C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta)\\ -J^{T}_\Theta(\eta)K_p\tilde{\eta}-J^{T}_\Theta(\eta)K_ds \end{array}} }$

$\displaystyle{(14')\quad \boxed{\dot{V}_2=-s^T(D^*(\nu,\eta)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} }$

MIMO Backstepping

投稿日時: 2022年2月1日投稿者: cacsd

MIMO Mass-Damper-Spring System

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{x}=v\\ M\dot{v}+D(v)v+K(x)x=Bu \end{array}\right. }$

●Step 1

$\displaystyle{(2)\quad \boxed{\tilde{x}=x-x_d,\ \tilde{v}=v-v_d}\quad\Rightarrow\quad\dot{\tilde{x}}=\tilde{v} }$

$\displaystyle{(3)\quad v=s+\alpha_1 }$

$\displaystyle{(4)\quad \boxed{s=\tilde{v}+\Lambda\tilde{x}}=v-v_d+\Lambda\tilde{x}\quad(\Lambda={\rm diag}\{\lambda_1,\dots,\lambda_m\}) }$

$\displaystyle{(5)\quad \boxed{\alpha_1=v-s=v_d-\Lambda\tilde{x}} }$

$\displaystyle{(6)\quad \dot{\tilde{x}}=\tilde{v}=v-v_d=-\Lambda\tilde{x}+s }$

$\displaystyle{(7)\quad \boxed{V_1=\frac{1}{2}\tilde{x}^TK_p\tilde{x}}\quad(K_p>0) }$

$\displaystyle{(8)\quad \dot{V}_1=\tilde{x}^TK_p\dot{\tilde{x}}=\tilde{x}^TK_p(-\Lambda\tilde{x}+s) \quad\Rightarrow\quad \boxed{\dot{V}_1=-\tilde{x}^TK_p\Lambda\tilde{x}+s^TK_p\tilde{x}} }$

●Step 2

$\displaystyle{(9)\quad \dot{s}=\dot{\tilde{v}}+\Lambda\dot{\tilde{x}}=\dot{v}-\dot{v}_d+\Lambda\dot{\tilde{x}} }$

$\displaystyle{(10)\quad \begin{array}{l} M\dot{s}+D(v)s=M(\dot{v}-\dot{v}_d+\Lambda\dot{\tilde{x}})+D(v)(v-v_d+\Lambda\tilde{x})\\ =M\dot{v}+D(v)v-M(\dot{v}_d-\Lambda\dot{\tilde{x}})-D(v)(v_d-\Lambda\tilde{x})\\ =Bu-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x \end{array} }$

$\displaystyle{(11)\quad \boxed{V_2=V_1+\frac{1}{2}s^TMs} }$

$\displaystyle{(12)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+s^TM\dot{s}\\ =-\tilde{x}^TK_p\Lambda\tilde{x}+s^TK_p\tilde{x}\\ +s^T(Bu-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x-D(v)s)\\ =s^T(Bu-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x-D(v)s+K_p\tilde{x})\\ -\tilde{x}^TK_p\Lambda\tilde{x} \end{array} }$

$\displaystyle{(13)\quad \boxed{u=B^\dag(M(\dot{v}-\dot{s})+D(v)(v-s)+K(x)x-K_p\tilde{x}-K_ds)\quad(K_d>0) }$

$\displaystyle{(12')\quad \boxed{\dot{V}_2=-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} }$

with Actuator Dynamics

$\displaystyle{(14)\quad \left\{\begin{array}{l} \dot{x}=v\\ M\dot{v}+D(v)v+K(x)x=Bu\\ T\dot{u}+u=u_c \end{array}\right. }$

●Step 3

$\displaystyle{(15)\quad Bu=z+\alpha_2\quad\Rightarrow\quad\dot{z}=B\dot{u}-\dot{\alpha}_2=BT^{-1}(u_c-u)-\dot{\alpha}_2 }$

$\displaystyle{(16)\quad \alpha_2=M(\dot{v}-\dot{s})+D(v)(v-s)+K(x)x-K_p\tilde{x}-K_ds }$

$\displaystyle{(12'')\quad \begin{array}{l} \dot{V}_2=s^T(z+\alpha_2-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x-D(v)s+K_p\tilde{x})\\ -\tilde{x}^TK_p\Lambda\tilde{x}\\ =s^Tz-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} \end{array} }$

$\displaystyle{(17)\quad \boxed{V_3=V_2+\frac{1}{2}z^Tz} }$

$\displaystyle{(18)\quad \begin{array}{l} \dot{V}_3=\dot{V}_2+z^T\dot{z}\\ =s^Tz-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x}+z^T(B\dot{u}-\dot{\alpha}_2)\\ =z^T(BT^{-1}(u_c-u)-\dot{\alpha}_2+s)-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} \end{array} }$

$\displaystyle{(19)\quad \boxed{u_c=u+TB^\dag(\dot{\alpha}_2-s-K_zz)} }$

$\displaystyle{(18')\quad \boxed{\dot{V}_3=-z^TK_zz-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} }$

MIMO Backstepping of Robots

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{q}=v\\ M(q)\dot{v}+C(q,v)v+g(q)=\tau \end{array}\right. }$

●Step 1

$\displaystyle{(2)\quad \boxed{\tilde{q}=q-q_d,\ \tilde{v}=v-v_d}\quad\Rightarrow\quad\dot{\tilde{q}}=\tilde{v} }$

$\displaystyle{(3)\quad v=s+\alpha_1 }$

$\displaystyle{(4)\quad \boxed{s=\tilde{v}+\Lambda\tilde{q}}=v-v_d+\Lambda\tilde{q}}\quad(\Lambda={\rm diag}\{\lambda_1,\dots,\lambda_m\}) }$

$\displaystyle{(5)\quad \boxed{\alpha_1=v-s=v_d-\Lambda\tilde{q}} }$

$\displaystyle{(6)\quad \dot{\tilde{q}}=\tilde{v}=v-v_d=-\Lambda\tilde{q}+s }$

$\displaystyle{(7)\quad \boxed{V_1=\frac{1}{2}\tilde{q}^TK_q\tilde{q}}\quad(K_q>0) }$

$\displaystyle{(8)\quad \dot{V}_1=\tilde{q}^TK_q\dot{\tilde{q}}=\tilde{q}^TK_q(-\Lambda\tilde{q}+s) }$

●Step 2

$\displaystyle{(9)\quad \dot{s}=\dot{v}-\dot{\alpha}_1 }$

$\displaystyle{(10)\quad \begin{array}{l} M(q)\dot{s}=M(q)\dot{v}-M(q)\dot{\alpha}_1\\ =\tau-C(q,v)v-g(q)-M(q)\dot{\alpha}_1\\ =\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)-C(q,v)s \end{array} }$

$\displaystyle{(11)\quad \boxed{V_2=V_1+\frac{1}{2}s^TM(q)s} }$

$\displaystyle{(12)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+s^TM(q)\dot{s}+\frac{1}{2}s^T\dot{M}(q)s\\ =\tilde{q}^TK_q(-\Lambda\tilde{q}+s)\\ +s^T(\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)-C(q,v)s)\\ +\frac{1}{2}s^T\dot{M}(q)s\\ =s^T(\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)+K_q\tilde{q})\\ +\underbrace{s^T(\frac{1}{2}\dot{M}(q)-C(q,v))s}_{0}-\tilde{q}^TK_q\Lambda\tilde{q}\\ \end{array} }$

$\displaystyle{(13)\quad \boxed{\tau=M(q)(\dot{v}-\dot{s})+C(q,v)(v-s)+g(q)-K_ds-K_q\tilde{q}} }$

$\displaystyle{(12')\quad \begin{array}{l} \dot{V}_2=s^T(\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)+K_q\tilde{q})\\ -\tilde{q}^TK_q\Lambda\tilde{q}\\ =-s^TK_ds-\tilde{q}^TK_q\Lambda\tilde{q} \end{array} }$

SISO BackStepping

投稿日時: 2022年2月1日投稿者: cacsd

予備的考察

BackSteppingの邦訳は、読み方のバックステッピング以外には適切なものがないようですが、非線形系に対する一つの制御方式を表しています。そこで、以下ではBackSteppingをBS制御と呼ぶことにします。

BS制御の基本的なアイデアを、次の例を用いて説明します。

$\displaystyle{ \begin{array}{cll} (1.1) & \dot{x}_1=f(x_1)+x_2 & (x_1(0)\ne0)\\ (1.2) & \dot{x}_2=u & (x_2(0)=0)\\ (1.3) & y=x_1 \end{array} }$

ここで、(1.1)が非線形動作を、(1.2)がアクチュエータ動作を、(1.3)が観測式を表しています。したがって、制御対象は状態変数 $x_1,x_2$ をもつ２次元の非線形系です。

制御目的は状態変数 $x_1$ を速やかに零（平衡状態）に戻すこととします。

状態変数は $x_1,x_2$ の２つなので、BS制御系設計のために２回の変数変換を行います。

$\displaystyle{(*)\quad \boxed{\left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2) \end{array}\right. \Leftrightarrow \left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2) \end{array}\right.} }$

以下では、変数変換後の状態変数 $z_1,z_2$ について２次安定性（したがって漸近安定性）を示し、逆変換により、元の状態変数 $x_1,x_2$ についての漸近安定性を示します。

●Step 1

まず第１番目の変数変換として、次を考えます。

$\displaystyle{(2)\quad \boxed{z_1=x_1} }$

また $x_2$ に対する仮想的な操作変数 $\alpha_1$ を導入して、第２番目の変数変換

$\displaystyle{(3)\quad x_2=\alpha_1+z_2\quad\Rightarrow\quad\boxed{z_2=x_2-\alpha_1} }$

を考えます。このとき(1.1)は次式で表されます。

$\displaystyle{(4)\quad \dot{z}_1=f(z_1)+\alpha_1+z_2 }$

このフィードバック線形化を行うために

$\displaystyle{(5)\quad \boxed{\alpha_1=-f(z_1)-k_1z_1\quad(k_1>0)} }$

と定義すると、 $z_1$ の状態方程式として

$\displaystyle{(4')\quad \dot{z}_1=-k_1z_1+z_2 }$

を得ます。これに対するリャプノフ関数

$\displaystyle{(6)\quad \boxed{V_1=\frac{1}{2}z_1^2} }$

に対して、次式を得ます。

$\displaystyle{(7)\quad \boxed{\dot{V}_1=z_1\dot{z}_1=-k_1z_1^2+z_1z_2} }$

●Step 2

$z_2$ の状態方程式として、(1.2)を用いて

$\displaystyle{(8)\quad \dot{z}_2=\dot{x}_2-\dot{\alpha}_1=u-\dot{\alpha}_1 }$

を得ます。これに対するリャプノフ関数を

$\displaystyle{(9)\quad \boxed{V_2=V_1+\frac{1}{2}z_2^2} }$

と定義すると、次式を得ます。

$\displaystyle{(10)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+z_2\dot{z}_2\\ =-k_1z_1^2+z_1z_2+z_2\dot{z}_2\\ =-k_1z_1^2+z_2(z_1+\dot{z}_2)\\ =-k_1z_1^2+z_2(z_1+u-\dot{\alpha}_1) \end{array} }$

ここで、操作入力を

$\displaystyle{(11)\quad \boxed{u=\dot{\alpha}_1-z_1-k_2z_2\quad(k_2>0)} }$

と選ぶと次式を得ます。

$\displaystyle{(10')\quad \boxed{\dot{V}_2=-k_1z_1^2-k_2z_2^2<0} }$

●以上の変数変換とその逆変換は、次式のように表すことができます。

$\displaystyle{(12)\quad \left\{\begin{array}{l} z_1=x_1\\ z_2=x_2-\alpha_1\\ =x_2+f(x_1)+k_1x_1 \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2) \end{array}\right.} }$

$\displaystyle{(13)\quad \left\{\begin{array}{l} x_1=z_1\\ x_2=z_2-f(z_1)-k_1z_1 \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2) \end{array}\right.} }$

変数変換後の閉ループ系は次式で表されます。

$\displaystyle{(14)\quad \left\{\begin{array}{l} \dot{z}_1=-k_1z_1+z_2\\ \dot{z}_2=(\dot{\alpha}_1-z_1-k_2z_2)-\dot{\alpha}_1=-k_2z_2-z_1 \end{array}}\right. }$

すなわち

$\displaystyle{(15)\quad \underbrace{ \left[\begin{array}{l} \dot{z}_1\\ \dot{z}_2 \end{array}\right]}_{\dot z}=- \underbrace{ \left[\begin{array}{cc} k_1 & 0\\ 0 & k_2 \end{array}\right]}_{K} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z}+ \underbrace{ \left[\begin{array}{cc} 0 & 1\\ -1 & 0 \end{array}\right]}_{S} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z} }$

このとき、２次安定性

$\displaystyle{(16)\quad V_2=\frac{1}{2}z^Tz \quad\Rightarrow\quad \boxed{\dot{V}_2=z^T\dot{z}=z^T(-Kz+Sz) =-z^TKz<0} }$

が示されており

$\displaystyle{(17)\quad \boxed{||z(t)||\le ||z(0)||e^{-\frac{1}{2}\alpha t}\quad(\alpha=\sigma_n(K))} }$

が成り立ち、(13)から制御目的が達成されていることが分かります。

●フィードバック線形化(5)の実施においては、非線形関数 $f(z_1)= f(x_1)$ を正確にモデリングしておく必要がありますが、実際にはこれは一般には困難です。たとえば次の例を考えます。

$\displaystyle{(18)\quad f(x_1)=-a_0x_1-a_1x_1^2-a_2|x_1|x_1\quad(a_0>0, a_1>0, a_2>0) }$

ここで、 $a_0, a_1, a_2$ は未知とします。第１項と第３項は線形の減衰項とみなせますが、第２項は非線形の減衰力で打ち消す必要があります。たとえば、 $z_1=x_1$ に注意して

$\displaystyle{(19)\quad \alpha_1=\underbrace{-k_1z_1}_{linear\ damping}-\underbrace{\kappa_1 z_1^3}_{nonlinear\ damping}\quad(k_1>0, \kappa_1>0) }$

と選ぶとき、次式が成り立ちます。

$\displaystyle{(20)\quad \begin{array}{l} \dot{z}_1=f(z_1)+(\alpha_1+z_2)\\ =-a_0z_1-a_1z_1^2-a_2|z_1|z_1-(k_1+\kappa_1 z_1^2)z_1+z_2\\ =-(\underbrace{a_0+a_2|z_1|}_{good\ damping}+k_1)z_1\underbrace{-a_1z_1^2}_{bad\ damping}-\kappa_1 z_1^3+z_2 \end{array} }$

このとき、(6)の下で、次式を得ます。

$\displaystyle{(21)\quad \dot{V}_1=z_1\dot{z}_1=-(a_0+a_2|z_1|+k_1)z_1^2-a_1z_1^3-\kappa_1 z_1^4+z_1z_2 }$

また、(9)の下で

$\displaystyle{(22)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+z_2\dot{z}_2\\ =-(a_0+a_2|z_1|+k_1)z_1^2-a_1z_1^3-\kappa_1 z_1^4+z_1z_2+z_2(z_1+u-\dot{\alpha}_1) \end{array} }$

ここで、(11)のように $u=\dot{\alpha}_1-z_1-k_2z_2$ と選択すると

$\displaystyle{(23)\quad \begin{array}{l} \dot{V}_2 =-(a_0+a_2|z_1|+k_1)z_1^2-a_1z_1^3-\kappa_1 z_1^4-k_2z_2^2 \end{array} }$

ここで、恒等式

$\displaystyle{(24)\quad \begin{array}{l} (\frac{1}{2\sqrt{\kappa_1}}x+\sqrt{\kappa_1}y)^2=\frac{1}{4\kappa_1}x^2+xy+\kappa_1y^2\\ \Lueftrightarrow -xy-\kappa_1y^2=-(\frac{1}{2\sqrt{\kappa_1}}x+\sqrt{\kappa_1}y)^2+\frac{1}{4\kappa_1}x^2 \end{array} }$

において、 $x=a_1z_1, y=z_1^2$ とおけば、次式を得ます。

$\displaystyle{(25)\quad \begin{array}{l} \dot{V}_2 =-(\frac{a_1}{2\sqrt{\kappa_1}}z_1+\sqrt{\kappa_1}z_1^2)^2+\frac{a_1^2}{4\kappa_1}z_1^2-(a_0+a_2|z_1|+k_1)z_1^2-k_2z_2^2\\ \le -(a_0+k_1-\frac{a_1^2}{4\kappa_1})z_1^2-k_2z_2^2 \end{array} }$

したがって、 $\kappa_1>0, k_1>\frac{a_1^2}{4\kappa_1}-a_0, k_2>0$ と選べば、 $\dot{V}_2<0$ を達成できます。すなわち、制御則(19)と(11)は、未知の $a_0, a_1, a_2$ を使わないので、BS制御はロバストであると言えます。

Mass-Damper-Spring System

次のMDS（マス・ダンパ・バネ）系を考えます。

$\displaystyle{ \begin{array}{ll} (1.1) & \dot{x}=v\\ (1.2) & m\dot{v}+d(v)v+k(x)x=\tau\\ (1.3) & y=x \end{array} }$

ここで、 $x$ は位置を、 $v$ は速度を、 $m$ は質量を、 $d(v)$ は非線形の減衰係数を、 $k(x)$ は非線形のバネ係数を表しています。(1.1)と(1.2)が状態変数 $x,v$ をもつ非線形状態方程式、(1.3)が観測方程式です。

制御目的は、目標軌道 $y_d$ に対する追従誤差

$\displaystyle{(2)\quad \boxed{e=y-y_d} }$

を速やかに零とし目標軌道に乗せることです。基礎式は次のようにまとめられます。

$\displaystyle{(3)\quad \left\{\begin{array}{l} \dot{x}=v\\ m\dot{v}+d(v)v+k(x)x=\tau\\ y=x\\ e=y-y_d \end{array}\right. \Rightarrow \left\{\begin{array}{l} m\dot{v}+d(v)v+k(x)x=\tau\\ \dot{e}=v-\dot{y}_d \end{array}\right. }$

ここで、第１式が非線形動作を、第２式が追従誤差の振舞い(積分動作とは異なる)を表しています。

状態変数は $x_1=e$ 、 $x_2=\dot{e}$ の２つなので、BS制御系設計のために２回の変数変換を行います。

$\displaystyle{(*)\quad \boxed{ \left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2) \end{array}\right. \Leftrightarrow \left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2) \end{array}\right.} }$

●Step 1

まず第１番目の変数変換として、次式を考えます。

$\displaystyle{(4)\quad \boxed{z_1=e=y-y_d}\quad\Rightarrow\quad\dot{z}_1=v-\dot{y}_d }$

$v$ に対する仮想的な操作変数 $\alpha_1$ を導入して、第２番目の変数変換

$\displaystyle{(5)\quad v=\alpha_1+z_2\quad\Rightarrow\quad \boxed{z_2=v-\alpha_1} }$

を考えます。このとき $z_1$ の状態方程式

$\displaystyle{(6)\quad \dot{z}_1=\alpha_1+z_2-\dot{y}_d }$

を得ます。このフィードバック線形化を行うために

$\displaystyle{(7)\quad \boxed{\alpha_1=\dot{y}_d-(k_1+n_1(z_1))z_1}\quad(k_1>0, n_1(z_1)=\kappa_1|z_1|^{\nu_1}) }$

と定義すると、次式を得ます。

$\displaystyle{(6')\quad \dot{z}_1=-(k_1+n_1(z_1))z_1+z_2 }$

これに対するリャプノフ関数

$\displaystyle{(8)\quad \boxed{V_1=\frac{1}{2}z_1^2} }$

に対して、次式を得ます。

$\displaystyle{(9)\quad \boxed{\dot{V}_1=z_1\dot{z}_1=-(k_1+n_1(z_1))z_1^2+z_1z_2} }$

●Step 2

$z_2$ の状態方程式として、(5)から

$\displaystyle{(10)\quad \dot{z}_2=\dot{v}-\dot{\alpha}_1 }$

(3)を用いて次式を得ます。

$\displaystyle{(11)\quad m\dot{z}_2=m\dot{v}-m\dot{\alpha}_1=\tau-d(v)v-k(x)x-m\dot{\alpha}_1 }$

これに対するリャプノフ関数を

$\displaystyle{(12)\quad \boxed{V_2=V_1+\frac{1}{2}mz_2^2} }$

と定義すると、次式を得ます。

$\displaystyle{(13)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+mz_2\dot{z}_2\\ =-(k_1+n(z_1))z_1^2+z_1z_2+(\tau-d(v)v-k(x)x-m\dot{\alpha}_1)z_2 \end{array} }$

ここで、操作入力を

$\displaystyle{(14)\quad \begin{array}{l} \boxed{\tau=m\dot{\alpha}_1+d(v)v+k(x)x-z_1-k_2z_2-n_2(z_2)z_2}\\ (k_2>0, n_2(z_1)=\kappa_2|z_2|^{\nu_2}) \end{array} }$

と選ぶと次式を得ます。

$\displaystyle{(13')\quad \boxed{\dot{V}_2=-(k_1+n_1(z_1))z_1^2-(k_2+n_2(z_2))z_2^2} }$

●以上の変数変換とその逆変換は、次式のように表すことができます。

$\displaystyle{(15)\quad \left\{\begin{array}{l} z_1=e=x_1\\ z_2=v-\alpha_1\\ =v-(\dot{y}_d-(k_1+n_1(z_1))z_1)\\ =\dot{e}+(k_1+n_1(z_1))z_1\\ =x_2+(k_1+n_1(x_1))x_1 \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2) \end{array}\right.} }$

$\displaystyle{(16)\quad \left\{\begin{array}{l} e=z_1\\ \dot{e}=z_2-(k_1+n_1(z_1))z_1 \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2) \end{array}\right.} }$

変数変換後の閉ループ系は次式で表されます。

$\displaystyle{(17)\quad \left\{\begin{array}{l} \dot{z}_1=-(k_1+n_1(z_1))z_1+z_2\\ m\dot{z}_2=m\dot{\alpha}_1+d(v)v+k(x)x-z_1-k_2z_2-n_2(z_2)z_2\\ -d(v)v-k(x)x-m\dot{\alpha}_1\\ =-(k_2+n_2(z_2))z_2-z_1 \end{array}}\right. }$

すなわち

$\displaystyle{(18)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{cc} 1 & 0\\ 0 & m \end{array}\right]}_{M} \underbrace{ \left[\begin{array}{l} \dot{z}_1\\ \dot{z}_2 \end{array}\right]}_{\dot z}=- \underbrace{ \left[\begin{array}{cc} k_1+n_1(z_1) & 0\\ 0 & k_2+n_2(z_2) \end{array}\right]}_{K} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z}\\ + \underbrace{ \left[\begin{array}{cc} 0 & 1\\ -1 & 0 \end{array}\right]}_{S} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z} \end{array} }$

このとき、２次安定性

$\displaystyle{(19)\quad V_2=\frac{1}{2}z^TMz \quad\Rightarrow\quad \boxed{\dot{V}_2=z^TM\dot{z}=z^T(-Kz+Sz) =-z^TKz<0} }$

が示されており

$\displaystyle{(20)\quad \boxed{||z(t)||\le \beta||z(0)||e^{-\frac{1}{2}\alpha t}\quad(\alpha=\sigma_n(M^{-1}K), \beta=\sqrt{\frac{\sigma_1(M)}{\sigma_n(M)})} }$

が成り立ち、制御目的が達成されていることが分かります。

●平衡状態の安定化問題は、次のように目標軌道が一定の特別な場合と考えられます。

$\displaystyle{(21)\quad \left\{\begin{array}{l} y_d=0\\ \dot{y}_d=0 \end{array}\right. }$

簡単のため、 $n_1(z_1)=0$ 、 $n_1(z_1)=0$ とすると

$\displaystyle{(22)\quad \left\{\begin{array}{l} z_1=x\\ \alpha_1=-k_1z_1 \end{array}\right. }$

となり、次式を得ます。

$\displaystyle{(23)\quad \tau=m\dot{\alpha}_1+d(v)v+k(x)x-z_1-k_2z_2 }$

これは、次のような非線形のPD制御とみなすことができます。

$\displaystyle{(24)\quad \tau=-K_P(x)x-K_D(v)v }$

実際、(4)より $z_1=x$ 、 $\dot{z}_1=v$ 、(5)より $z_2=v-\alpha_1=v+k_1z_1$ に注意して

$\displaystyle{(25)\quad \begin{array}{l} \tau=m(-k_1v)+d(v)v+k(x)x-z_1-k_2(v+k_1z_1)\\ =(d(v)-mk_1)v+(k(x)-1)x-k_2(v+k_1x)\\ =\underbrace{(d(v)-mk_1-k_2)}_{K_D(x)}v+\underbrace{(k(x)-1-k_1k_2)}_{K_P(x)}x \end{array} }$

以下では積分動作を導入するための２つのアプローチを示します。

Integrator Augmentation

外乱の影響を受ける次のMDS（マス・ダンパ・バネ）系を考えます。

$\displaystyle{ \begin{array}{ll} (1.1) & \dot{x}=v\\ (1.2) & m\dot{v}+d(v)v+k(x)x=\tau+w\\ (1.3) & y=x \end{array} }$

ここで、 $w$ は外乱を表しています。

制御目的は、外乱に抗して、目標軌道 $y_d$ に対する追従誤差

$\displaystyle{(2)\quad \boxed{e=y-y_d} }$

を速やかに零とし目標軌道に乗せることです。

積分動作を入れて状態方程式を拡大した基礎式は次のようにまとめられます。

$\displaystyle{(3)\quad \left\{\begin{array}{l} \dot{x}=v\\ m\dot{v}+d(v)v+k(x)x=\tau+w\\ y=x\\ e=y-y_d \end{array}\right. \Rightarrow \left\{\begin{array}{l} m\dot{v}=\tau-d(v)v-k(x)x+w\\ \dot{e}=v-\dot{y}_d\\ \dot{e}_I=e \end{array}\right. }$

ここで、第１式が非線形動作を、第２式が追従誤差の振舞いを、第３式が積分動作を表しています。

状態変数は $x_1=e_I$ 、 $x_2={e}$ 、 $x_3=\dot{e}$ の３つなので、BS制御系設計のために３回の変数変換を行います。

$\displaystyle{(**)\quad \boxed{\left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2)\\ z_3=\phi_3(x_1,x_2,x_3) \end{array}\right. \Leftrightarrow \left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2)\\ x_3=\phi_3^{-1}(z_1,z_2,z_3) \end{array}\right.} }$

以下では、変数変換後の状態変数 $z_1,z_2,z_3$ について２次安定性（したがって漸近安定性）を示し、逆変換により、元の状態変数 $x_1,x_2,x3$ についての漸近安定性を示します。

●Step 1

まず第１番目の変数変換として、次式を考えます。

$\displaystyle{(4)\quad \boxed{z_1=e_I}\quad\Rightarrow\quad\dot{z}_1=e }$

$e$ に対する仮想的な操作変数 $\alpha_1$ を導入して、第２番目の変数変換

$\displaystyle{(5)\quad e=\alpha_1+z_2\quad\Rightarrow\quad \boxed{z_2=e-\alpha_1} }$

を考えます。このとき $z_1$ の状態方程式

$\displaystyle{(6)\quad \dot{z}_1=\alpha_1+z_2 }$

を得ます。いま

$\displaystyle{(7)\quad \boxed{\alpha_1=-k_1z_1} }$

と定義すると

$\displaystyle{(6')\quad \dot{z}_1=-k_1z_1+z_2 }$

これに対するリャプノフ関数

$\displaystyle{(8)\quad \boxed{V_1=\frac{1}{2}z_1^2} }$

に対して、次式を得ます。

$\displaystyle{(9)\quad \boxed{\dot{V}_1=z_1\dot{z}_1=-k_1z_1^2+z_1z_2} }$

●Step 2

$z_2$ の状態方程式として、(5)から

$\displaystyle{(10)\quad \dot{z}_2=\dot{e}-\dot{\alpha}_1=v-\dot{y}_d-\dot{\alpha}_1 }$

$v$ に対する仮想的な操作変数 $\alpha_2$ を導入して、第３番目の変数変換

$\displaystyle{(11)\quad v=\alpha_2+z_3\quad\Rightarrow\quad \boxed{z_3=v-\alpha_2} }$

を考えます。このとき $z_2$ の状態方程式

$\displaystyle{(10')\quad \dot{z}_2=\alpha_2+z_3-\dot{y}_d-\dot{\alpha}_1 }$

を得ます。これに対するリャプノフ関数を

$\displaystyle{(12)\quad \boxed{V_2=V_1+\frac{1}{2}mz_2^2} }$

と定義すると、次式を得ます。

$\displaystyle{(13)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+z_2\dot{z}_2\\ =-k_1z_1^2+z_1z_2+(\alpha_2+z_3-\dot{y}_d-\dot{\alpha}_1)z_2 \end{array} }$

ここで

$\displaystyle{(14)\quad \boxed{\alpha_2=\dot{\alpha}_1+\dot{y}_d-z_1-k_2z_2} }$

と定義すると、次式を得ます。

$\displaystyle{(10'')\quad \dot{z}_2=-z_1-k_2z_2+z_3 }$

これから、次式を得ます。

$\displaystyle{(13')\quad \boxed{\dot{V}_2=-k_1z_1^2-k_2z_2^2+z_2z_3} }$

●Step 3

$z_3$ の状態方程式として、(11)から

$\displaystyle{(15)\quad \dot{z}_3=\dot{v}-\dot{\alpha}_2 }$

を得ます。このとき

$\displaystyle{(16)\quad \begin{array}{l} m\dot{z}_3=m\dot{v}-m\dot{\alpha}_2\\ =\tau+w-d(v)v-k(x)x-m\dot{\alpha}_2\\ =\tau-d(v)\alpha_2-d(v)z_3-k(x)x-m\dot{\alpha}_2\quad({\rm assuming}\ w=0) \end{array} }$

これに対するリャプノフ関数

$\displaystyle{(17)\quad \boxed{V_3=V_2+\frac{1}{2}mz_3^2} }$

に対して、次式を得ます。

$\displaystyle{(18)\quad \begin{array}{l} \dot{V}_3=\dot{V}_2+mz_3\dot{z}_3\\ =-k_1z_1^2-k_2z_2^2+(z_2+m\dot{z}_3)z_3\\ =-k_1z_1^2-k_2z_2^2+(z_2+\tau-d(v)\alpha_2-d(v)z_3-k(x)x-m\dot{\alpha}_2)z_3\\ \end{array} }$

ここで、操作入力を

$\displaystyle{(19)\quad \boxed{\tau=m\dot{\alpha}_2+d(v)\alpha_2+k(x)x-z_2-k_3z_3 }$

と選ぶと、次式を得ます。

$\displaystyle{(18')\quad \boxed{\dot{V}_3=-k_1z_1^2-k_2z_2^2-(d(v)+k_3)z_3^2} }$

●以上の変数変換とその逆変換は、次式のように表すことができます。

$\displaystyle{(15)\quad \left\{\begin{array}{l} z_1=e_I=x_1\\ z_2=e-\alpha_1\\ =e+k_1e_I\\ =x_2+k_1x_1\\ z_3=v-\alpha_2\\ =v-(\dot{\alpha}_1+\dot{y}_d-z_1-k_2z_2)\\ =\dot{e}+k_1\dot{z}_1+z_1+k_2z_2\\ =x_3+k_1\dot{x}_1+x_1+k_2(x_2+k_1x_1)\\ \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2)\\ z_3=\phi_3(x_1,x_2,x_3) \end{array}\right.} }$

$\displaystyle{(16)\quad \left\{\begin{array}{l} e_I=z_1\\ e=z_2-k_1z_1\\ \dot{e}=z_3-k_1\dot{z}_1-z_1-k_2z_2 \end{array}}\right. \quad \boxed{\left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2)\\ x_3=\phi_3^{-1}(z_1,z_2,z_3) \end{array}\right.} }$

閉ループ系は次式で表されます。

$\displaystyle{(20)\quad \left\{\begin{array}{l} \dot{z}_1=-k_1z_1+z_2\\ \dot{z}_2=-z_1-k_2z_2+z_3\\ m\dot{z}_2=m\dot{\alpha}_2+d(v)\alpha_2+k(x)x-z_2-k_3z_3\\ -d(v)\alpha_2-d(v)z_3-k(x)x-m\dot{\alpha}_2\\ =-(d(v)+k_3)z_3-z_2 \end{array}}\right. }$

すなわち

$\displaystyle{(21)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & m \end{array}\right]}_{M} \underbrace{ \left[\begin{array}{l} \dot{z}_1\\ \dot{z}_2\\ \dot{z}_3 \end{array}\right]}_{\dot z}=- \underbrace{ \left[\begin{array}{ccc} k_1 & 0 & 0\\ 0 & k_2 & 0\\ 0 & 0 & d(v)+k_3 \end{array}\right]}_{K} \underbrace{ \left[\begin{array}{l} z_1\\ z_2\\ z_3 \end{array}\right]}_{z}\\+ \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ -1 & 0 & 1\\ 0 & -1 & 0 \end{array}\right]}_{S} \underbrace{ \left[\begin{array}{l} z_1\\ z_2\\ z_3 \end{array}\right]}_{z} \end{array} }$

このとき、２次安定性

$\displaystyle{(22)\quad V_3=\frac{1}{2}z^TMz \quad\Rightarrow\quad \boxed{\dot{V}_2=z^TM\dot{z}=z^T(-Kz+Sz) =-z^TKz<0} }$

が示されており

$\displaystyle{(23)\quad \boxed{||z(t)||\le \beta||z(0)||e^{-\frac{1}{2}\alpha t}\quad(\alpha=\sigma_n(M^{-1}K), \beta=\sqrt{\frac{\sigma_1(M)}{\sigma_n(M)})} }$

が成り立ち、制御目的が達成されていることが分かります。

Integral Action by Constant Parameter Adaptation

次の定値外乱の影響を受けるMDS（マス・ダンパ・バネ）系を考えます。

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{x}=v\\ m\dot{v}+d(v)v+k(x)x=\tau+w\quad (\dot{w}=0)\\ y=x \end{array}\right. }$

ここで、 $x$ は位置を、 $v$ は速度を、 $w$ は定値外乱を、 $m$ は質量を、 $d(v)$ は非線形の減衰係数を、 $k(x)$ は非線形のバネ係数を表しています。

制御目的は、定値外乱に抗して、目標軌道 $y_d$ に対する追従誤差

$\displaystyle{(2)\quad \boxed{e=y-y_d} }$

を速やかに零とし目標軌道に乗せることです。基礎式は次のようにまとめられます。

$\displaystyle{(3)\quad \begin{array}{l} \left\{\begin{array}{l} \dot{x}=v\\ m\dot{v}+d(v)v+k(x)x=\tau+w\quad (\dot{w}=0)\\ y=x\\ e=y-y_d=x-x_d \end{array}\right.\\ \quad\Downarrow\\ \left\{\begin{array}{l} m\dot{v}+d(v)v+k(x)x=\tau+w\quad (\dot{w}=0)\\ \dot{e}=v-\dot{y}_d=v-\dot{x}_d \end{array}\right. \end{array} }$

ここで、第１式が定値外乱下の非線形動作を、第２式が追従誤差の振舞いを表しています。

●Step 1

まず第１番目の変数変換として、次式を考えます。

$\displaystyle{(4)\quad \boxed{z_1=e=x-x_d}\quad\Rightarrow\quad\dot{z}_1=v-\dot{x}_d }$

$v$ に対する仮想的な操作変数 $\alpha_1$ を導入して、第２番目の変数変換

$\displaystyle{(5)\quad v=\alpha_1+z_2\quad\Rightarrow\quad \boxed{z_2=v-\alpha_1} }$

を考えます。このとき $z_2$ の状態方程式

$\displaystyle{(6)\quad \dot{z}_1=\alpha_1+z_2-\dot{x}_d }$

を得ます。このフィードバック線形化を行うために

$\displaystyle{(7)\quad \boxed{\alpha_1=\dot{x}_d-k_1z_1} }$

と定義すると、次式を得ます。

$\displaystyle{(6')\quad \dot{z}_1=-k_1z_1+z_2 }$

これに対するリャプノフ関数

$\displaystyle{(8)\quad \boxed{V_1=\frac{1}{2}z_1^2+\frac{1}{2p}(\hat{w}-w)^2} }$

に対して、次式を得ます。

$\displaystyle{(9)\quad \boxed{\dot{V}_1=z_1\dot{z}_1=-k_1z_1^2+z_1z_2+\frac{1}{p}(\hat{w}-w)\dot{\hat{w}}} }$

●Step 2

$z_2$ の状態方程式として、(5)と(7)から

$\displaystyle{(10)\quad \dot{z}_2=\dot{v}-\dot{\alpha}_1=\dot{v}-(\ddot{x}_d-k_1\dot{z}_1)=\dot{v}-\ddot{x}_d+k_1(v-\dot{x}_d) }$

よって、(3)から次式を得ます。

$\displaystyle{(11)\quad \begin{array}{l} m\dot{z}_2=m\dot{v}-m\ddot{x}_d+mk_1(v-\dot{x}_d)\\ =\tau-d(v)v-k(x)x+w-m\ddot{x}_d+mk_1(v-\dot{x}_d) \end{array} }$

これに対するリャプノフ関数を

$\displaystyle{(12)\quad \boxed{V_2=V_1+\frac{1}{2}mz_2^2} }$

と定義すると、次式を得ます。

$\displaystyle{(13)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+mz_2\dot{z}_2\\ =-k_1z_1^2+z_1z_2+\frac{1}{p}(\hat{w}-w)\dot{\hat{w}}\\ +(\tau-d(v)v-k(x)x+w-m\ddot{x}_d+mk_1(v-\dot{x}_d))z_2 \end{array} }$

ここで、操作入力を

$\displaystyle{(14)\quad \boxed{\tau=d(v)(v-z_2)+k(x)x-\hat{w}+m\ddot{x}_d-mk_1(v-\dot{x}_d)-z_1-k_2z_2 }$

と選ぶと次式を得ます。

$\displaystyle{(13')\quad \boxed{\dot{V}_2=-k_1z_1^2-(k_2+d(v))z_2^2}+(\hat{w}-w)(\frac{1}{p}\dot{\hat{w}}-z_2) }$

ここで、外乱の予測式を

$\displaystyle{(15)\quad \boxed{\dot{\hat{w}}=pz_2} }$

と選べばよいことが分かります。

●閉ループ系は次式で表されます。

$\displaystyle{(16)\quad \left\{\begin{array}{l} \dot{z}_1=-k_1z_1+z_2\\ m\dot{z}_2=d(v)(v-z_2)+k(x)x-\hat{w}+m\ddot{x}_d-k_1(v-\dot{x}_d)-z_1-k_2z_2\\ -d(v)v-k(x)x+w-m\ddot{x}_d+k_1(v-\dot{x}_d)\\ =-z_1-(k_2+d(v))z_2-(\hat{w}-w) \end{array}}\right. }$

すなわち

$\displaystyle{(17)\quad \underbrace{ \left[\begin{array}{cc} 1 & 0\\ 0 & m \end{array}\right]}_{M} \underbrace{ \left[\begin{array}{l} \dot{z}_1\\ \dot{z}_2 \end{array}\right]}_{\dot z}=- \underbrace{ \left[\begin{array}{cc} k_1 & 1\\ -1 & -k_2-d(v) \end{array}\right]}_{A} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z}+ \underbrace{ \left[\begin{array}{c} 0 \\ -1 \end{array}\right]}_{b} (\hat{w}-w) }$

$\displaystyle{(18)\quad \dot{\hat{w}}=-p \underbrace{ \left[\begin{array}{cc} 0 & -1 \end{array}\right]}_{b^T} \underbrace{ \left[\begin{array}{l} z_1\\ z_2 \end{array}\right]}_{z} }$

YMCPBのBS制御（暫定版）

投稿日時: 2022年2月1日投稿者: cacsd

Case１：３次元モデルに基づくBS制御

●制御対象YMCPBのピッチに関する運動方程式として次式を考えます。

$\displaystyle{(1)\quad \begin{array}{l} (I_y+J_y)\ddot{\theta}=D_b(H_{CG}-H_D)+T(H_T-H_{De}+H_e)\\ +D_eH_e+N_LL_L+N_eL_e+N_B(L_{CG}\cos\theta-L_B) -c_{\theta z}\dot{z}-c_{\theta\theta}\dot{\theta} \end{array} }$

これは次の形をとると考えられます。

$\displaystyle{(2)\quad J\ddot{\theta}(t)+D(\theta,\dot{\theta})\dot{\theta}+K(\theta)\theta(t)=B\theta_e(t)+w(t) }$

ここで、減衰係数 $D$ 、復元係数 $K$ は、 $\theta$ 、 $\dot{\theta}$ の非線形関数、 $w$ は外乱やモデル化誤差を表しています。また、 $\theta_e$ は船外機の取付角で、その動作は操作変数 $u_e$ を用いて、次式で表されます。

$\displaystyle{(3)\quad \dot{\theta}_e(t)=\Omega_e u_e(t)\quad(u_e(t)=0,\pm 1) }$

(2)と(3)を合わせた制御対象全体の非線形状態方程式を線形化すると次式となります。

$\displaystyle{(4)\quad \begin{array}{l} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ a_{63} & a_{66} & b_{62}\\ 0 & 0 & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{cc} 0 \\ 0 \\ \Omega_e \end{array}\right] }_{B} u_e(t)\\ +\underbrace{ \left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ a_{62} & a_{64} & a_{65} & b_{61}\\ 0 & 0 & 0 & 0 \end{array}\right] \left[\begin{array}{c} z(t)-z^*\\ \dot{x}(t)-V^*\\ \dot{z}(t)\\ T(t)-T^* \end{array}\right] }_{w(t)} \end{array} }$

ここで、 $^*$ は平衡状態を表しています。

●以上から、制御対象は次式でモデル化されているとします。

$\displaystyle{(5)\quad \left\{\begin{array}{l} \dot{\theta}(t)=\omega(t)\\ \dot{\omega}(t)+a_{63}\theta(t)+a_{66}\omega(t)=b_{62}\theta_e(t)+w(t)\\ \dot{\theta}_e(t)=\Omega_e u_e(t) \end{array}\right. }$

制御目的は、まず船外機の取付角を $\theta_e^*$ に移動させること、その上で外乱に抗して船体のピッチ角の振動を抑え、平衡状態を保持することです。そのために、制御則には誤差

$\displaystyle{(6)\quad e(t)=\theta_e(t)-\theta_e^* }$

による積分動作

$\displaystyle{(7)\quad \dot{e}_I(t)=e(t) }$

を入れます。積分動作を入れて拡大した基礎式は次のようにまとめられます。

$\displaystyle{(8)\quad \left\{\begin{array}{l} \dot{\theta}(t)=\omega(t)\\ \dot{\omega}(t)+a_{66}\omega(t)+a_{63}\theta(t)=b_{62}\theta_e(t)+w(t)\\ \dot{\theta}_e(t)=\Omega_e u_e(t)\quad(\dot{e}(t)=\Omega_e u_e(t))\\ \dot{e}_I(t)=e(t) \end{array}\right. }$

ここで、第１式と第２式が非線形動作を、第３式がアクチュエータの動作を、第４式が積分動作を表しています。以下では、簡単のため $w=0$ とします。

状態変数は $x_1=\theta$ 、 $x_2=\omega$ 、 $x_3=\theta_e$ 、 $x_4=e_I$ の４つなので、BS制御系設計のために４回の変数変換を行います。

$\displaystyle{(9)\quad \boxed{ \begin{array}{l} z=\phi(x)\\ \left\{\begin{array}{l} z_1=\phi_1(x_1)\\ z_2=\phi_2(x_1,x_2)\\ z_3=\phi_3(x_1,x_2,x_3)\\ z_4=\phi_4(x_1,x_2,x_3,x_4)\\ \end{array}\right. \end{array} \Leftrightarrow \begin{array}{l} x=\phi^{-1}(z)\\ \left\{\begin{array}{l} x_1=\phi_1^{-1}(z_1)\\ x_2=\phi_2^{-1}(z_1,z_2)\\ x_3=\phi_3^{-1}(z_1,z_2,z_3)\\ x_4=\phi_4^{-1}(z_1,z_2,z_3,z_4)\\ \end{array}\right. \end{array}} }$

以下では、変数変換後の状態変数 $z_1,z_2,z_3,z_4$ について２次安定性（したがって漸近安定性）を示し、逆変換により、元の状態変数 $x_1,x_2,x_3,x_4$ についての漸近安定性を示します。

●Step 1

まず第１番目の変数変換として、次式を考えます。

$\displaystyle{(10)\quad \boxed{z_1=\theta}\quad\Rightarrow\quad\dot{z}_1=\omega }$

また $\omega$ に対する仮想的な操作変数 $\alpha_1$ を導入して、第２番目の変数変換

$\displaystyle{(11)\quad \omega=\alpha_1+z_2\quad\Rightarrow\quad \boxed{z_2=\omega-\alpha_1} }$

を考えます。このとき $z_1$ の状態方程式は

$\displaystyle{(12)\quad \dot{z}_1=\alpha_1+z_2 }$

となります。いま

$\displaystyle{(13)\quad \boxed{\alpha_1=-k_1z_1} }$

と定義すると

$\displaystyle{(12')\quad \dot{z}_1=-k_1z_1+z_2 }$

となります。これに対するリャプノフ関数

$\displaystyle{(14)\quad \boxed{V_1=\frac{1}{2}z_1^2} }$

に対して、次式を得ます。

$\displaystyle{(15)\quad \boxed{\dot{V}_1=z_1\dot{z}_1=-k_1z_1^2+z_1z_2} }$

●Step 2

$\theta_e$ に対して仮想的な操作変数 $\alpha_2$ を導入して、第３番目の変数変換

$\displaystyle{(16)\quad \theta_e=\alpha_2+z_3\quad\Rightarrow\quad \boxed{z_3=\theta_e-\alpha_2} }$

を考えます。このとき $z_2$ の状態方程式として

$\displaystyle{(17)\quad \begin{array}{l} \dot{z}_2=\dot{\omega}-\dot{\alpha}_1\\ =b_{62}\theta_e-a_{63}\theta-a_{66}\omega-\dot{\alpha}_1\\ =b_{62}\alpha_2+b_{62}z_3-a_{63}z_1-a_{66}\alpha_1-a_{66}z_2-\dot{\alpha}_1\\ =b_{62}\alpha_2+b_{62}z_3-a_{63}z_1+a_{66}k_1z_1-a_{66}z_2+k_1\dot{z}_1\\ \end{array} }$

を得ます。このフィードバック線形化を行うために

$\displaystyle{(18)\quad \boxed{\alpha_2=\frac{1}{b_{62}}(a_{63}z_1-a_{66}k_1z_1+a_{66}z_2-k_1\dot{z}_1-z_1-k_2z_2)} }$

と定義すると、次式を得ます。

$\displaystyle{(17')\quad \dot{z}_2=-z_1-k_2z_2+b_{62}z_3 }$

これに対するリャプノフ関数を

$\displaystyle{(19)\quad \boxed{V_2=V_1+\frac{1}{2}z_2^2} }$

と定義すると、次式を得ます。

$\displaystyle{(20)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+z_2\dot{z}_2\\ =-k_1z_1^2+z_1z_2+(-z_1-k_2z_2+b_{62}z_3)z_2\\ =-k_1z_1^2-k_2z_2^2+b_{62}z_2z_3 \end{array} }$

●Step 3

$z_3$ の状態方程式として

$\displaystyle{(22)\quad \begin{array}{l} \dot{z}_3=\dot{\theta}_e-\dot{\alpha}_2 =\Omega_e u_e-\dot{\alpha}_2 \end{array} }$

このフィードバック線形化を行うために

$\displaystyle{(23)\quad \boxed{u_e=\frac{1}{\Omega_e}(\dot{\alpha}_2-b_{62}z_2-k_3z_3) }$

と定義すると、次式を得ます。

$\displaystyle{(22')\quad \begin{array}{l} \dot{z}_3 =\Omega_e\frac{1}{\Omega_e}(\dot{\alpha}_2-b_{62}z_2-k_3z_3)-\dot{\alpha}_2\\ =\dot{\alpha}_2-b_{62}z_2-k_3z_3-\dot{\alpha}_2\\ =-b_{62}z_2-k_3z_3 \end{array} }$

これに対するリャプノフ関数

$\displaystyle{(24)\quad \boxed{V_3=V_2+\frac{1}{2}z_3^2} }$

に対して、次式を得ます。

$\displaystyle{(25)\quad \begin{array}{l} \dot{V}_3=\dot{V}_2+z_3\dot{z}_3\\ =-k_1z_1^2-k_2z_2^2+b_{62}z_2z_3+(-b_{62}z_2-k_3z_3)z_3\\ =-k_1z_1^2-k_2z_2^2-k_3z_3^2 \end{array} }$

●Step 4

$e_I$ に対して仮想的な操作変数 $\alpha_3$ を導入して、第４番目の変数変換

$\displaystyle{(26)\quad e_I=\alpha_3+z_4\quad\Rightarrow\quad \boxed{z_4=e_I-\alpha_3} }$

を考えます。 $z_4$ の状態方程式は

$\displaystyle{(27)\quad \dot{z}_4=\dot{e}_I-\dot{\alpha}_3=e-\dot{\alpha}_3 }$

いま

$\displaystyle{(28)\quad \boxed{\dot{\alpha}_3=e+k_4z_4} }$

と定義すると

$\displaystyle{(27')\quad \dot{z}_4=e-(e+k_4z_4)=-k_4z_4 }$

これに対するリャプノフ関数

$\displaystyle{(29)\quad \boxed{V_4=V_3+\frac{1}{2}z_4^2} }$

に対して、次式を得ます。

$\displaystyle{(30)\quad \begin{array}{l} \dot{V}_4=\dot{V}_3+z_4\dot{z}_4\\ =-k_1z_1^2-k_2z_2^2-k_3z_3^2-k_4z_4^2 \end{array} }$

●以上の変数変換とその逆変換は、次式のように表すことができます。

$\displaystyle{(31)\quad \left\{\begin{array}{l} z_1=\theta=x_1\\ z_2=\omega-\alpha_1=\omega+k_1z_1=x_2+k_1x_1\\ z_3=\theta_e-\alpha_2=x_3-\alpha_2\\ =x_3-\frac{1}{b_{62}}(a_{63}z_1-a_{66}k_1z_1+a_{66}z_2-k_1\dot{z}_1-z_1-k_2z_2)\\ z_4=e_I-\alpha_3=x_4-\int(e+k_4z_4)dt \end{array}}\right. %\quad\boxed{z=\phi(x)} }$

$\displaystyle{(32)\quad \left\{\begin{array}{l} x_1=z_1\\ x_2=z_2-k_1z_1\\ x_3=z_3+\frac{1}{b_{62}}(a_{63}z_1-a_{66}k_1z_1+a_{66}z_2-k_1\dot{z}_1-z_1-k_2z_2)\\ x_4=z_4+\int(e+k_4z_4)dt \end{array}}\right. %\quad\boxed{x=\phi^{-}(z)} }$

YMCPBの出力FB型SMI制御

投稿日時: 2022年2月1日投稿者: cacsd

YMCBのOF-SM制御

投稿日時: 2022年2月1日投稿者: cacsd

これまでのSM制御系の線形制御則は状態フィードバック型のものを設計してきました。そして、制御対象の３次元モデルを用いても十分な結果が得られることを確認しました。そこで、制御対象の５次元モデルに基づいて線形制御則として出力フィードバック型のものを設計してもうまく機能することが予想されます。これはヒーブを完全に無視するのではなく、間接的に考慮していることになることがメリットと考えられます。

Case１：５次元モデルに基づくOF-SM制御

MATLAB

%cYMCPB5_of_sm.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント id=128=ID(33)
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys5(:,:,33));
%=====OF-SM
 C=CM;
%Establish the number of inputs and outputs
 [nn,mm]=size(B);
 [pp,nn]=size(C);
%Change coordinates so the output distribution matrix is [0 I]
 nc=null(C);
 Tc=[nc'; C];
 Ac=Tc*A*inv(Tc);
 Bc=Tc*B;
 Cc=C*inv(Tc);
%Partition the input distribution matrix conformably
 Bc1=Bc(1:nn-pp,:);
 Bc2=Bc(nn-pp+1:nn,:);
%Find a transformation to bring about a special structure 
%in input and output distribution matrices
 [T,temp]=qr(Bc2);
 T=(flipud(T'))';
 Tb=[eye(nn-pp) -Bc1*inv(Bc2'*Bc2)*Bc2';
     zeros(pp,nn-pp) T'];
%In this new cordinate system, we have C=[0 T] and B=[0 B2']'
%A has no special structure yet
 Aa=Tb*Ac*inv(Tb);
 Ba=Tb*Bc;
 Ca=Cc*inv(Tb);
 A1111=Aa(1:nn-pp,1:nn-pp);
 A1121=Aa(nn-pp+1:nn-mm,1:nn-pp);
%The aim is to put (A1111,A1121) in the observability canonical form
 [Ab,Bb,Cb,Tobs,k]=obsvf(A1111,zeros(nn-pp,1),A1121,1000*eps);
%r is the dimension of the unobservable subspace 
%the number of invariant zeros of the system (A,B,C)
 r=nn-pp-sum(k);
 disp('Dimension of the unobservable subspace:'),r
 Ta=[Tobs zeros(nn-pp,pp);
     zeros(pp,nn-pp) eye(pp)];
 Af=Ta*Aa*inv(Ta);
 Bf=Ta*Ba;eig(Af)
 Cf=Ca*inv(Ta); 
%=====
 A11ti=Aa(1:nn-mm,1:nn-mm);
 B1ti=Aa(1:nn-mm,nn-mm+1:nn);
 C1ti=[zeros(pp-mm,nn-pp) eye(pp-mm,pp-mm)];
 [Kopt,pl]=opt(A11ti,B1ti,C1ti,diag([1 1]),1)
 K=Kopt/C1ti 
 A11bar=A11ti-B1ti*K*C1ti;
 lambda=eig(A11bar)
%---- 
 Tbar=[eye(nn-mm) zeros(nn-mm,mm);
       K*C1ti eye(mm)]; 
 Abar=Tbar*Aa*inv(Tbar);  
 A11bar2=Abar(1:nn-mm,1:nn-mm);
 lambda=eig(A11bar2) 
 A12bar=Abar(1:nn-mm,nn-mm+1:nn);
 A21bar=Abar(nn-mm+1:nn,1:nn-mm);
 A22bar=Abar(nn-mm+1:nn,nn-mm+1:nn); 
 Q1=eye(nn-mm); 
 P1=lyap(A11bar2',Q1)
% 
 P2=1
 B2=Ba(nn,mm)
 F2=B2'*P2 
 F=F2*[K eye(mm)]*T'
% 
 Q2=P1*A12bar+A21bar'*P2;
 Q3=P2*A22bar+A22bar'*P2; 
 W=inv(F2)'*(Q3+Q2'*inv(Q1)*Q2)*inv(F2);
 gamma0=0.5*norm(W) 
 gamma=gamma0*1.1
 rho=1
%-----
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth 
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2
%-----
%eof

図1 YMCPBに対する出力FB型SM制御のシミュレーション例

Case２：５次元モデルに基づくOF-SMI制御

●補償器を用いる方法
●ＳＭオブザーバを用いる方法（ｍ＝ｐ＝１の制約）

YMCPBのSMI制御(2)

投稿日時: 2022年2月1日投稿者: cacsd

ここでは、 LQI制御に倣って積分動作をもつスライディングモード制御（SMI2制御）系の設計について述べます。その評価は本来は非線形シミュレータ上で行うべきですが、ここでは６次元線形モデルに関して行います。

Case	設計モデル	状態FB	状態OB	積分動作	注意点
1	５次	５次	未使用	要	「絵に描いた餅」
2	５次	３次	不要	要	ヒーブのゲインを強制的に零
3	３次	３次	不要	要	ヒーブからの連成を抑制できるか
4	５次	５次	要	要	コントローラが微分方程式

Case１：５次元モデルに基づくSMI制御

●LQI制御における偏差系(7)を、改めて次のように書きます。

$\displaystyle{(1)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot x_2(t) \end{array}\right] }_{\dot{x}_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} + \underbrace{ \left[\begin{array}{c} 0\\ I_m \end{array}\right] }_{B_{E3}} {\dot u}(t)\\ (x_1(t)=x(t)-x_\infty, x_2(t)=u(t)-u_\infty) \end{array} }$

スイッチング関数として、次式を考えます。

$\displaystyle{(2)\quad s(t)= \underbrace{ \left[\begin{array}{cc} S_1 & S_2 \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} = \underbrace{S_2 \left[\begin{array}{cc} M & I \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} \ (M=S_2^{-1}S_1) }$

(1)に対して、座標変換

$\displaystyle{(3)\quad \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} I & 0 \\ S_1 & S_2 \\ \end{array}\right] }_{T_s} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)}\\ \Leftrightarrow \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} I & 0 \\ -S_2^{-1}S_1 & S_2^{-1} \\ \end{array}\right] }_{T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} }$

を行って、次式を得ます。

$\displaystyle{(4)\quad \underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot s(t) \end{array}\right] }_{\dot{x}'_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ \bar{A}_{21} & \bar{A}_{22} \\ \end{array}\right] }_{T_sA_{E3}T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} + \underbrace{ \left[\begin{array}{cc} 0\\ S_2 \end{array}\right] }_{T_sB_{E3}} {\dot u}(t) }$

$\displaystyle{(5)\quad \left\{\begin{array}{l} \bar{A}_{11}=A-BM\\ \bar{A}_{12}=BS_2^{-1}\\ \bar{A}_{21}=S_1(A-BM)\\ \bar{A}_{22}=S_1BS_2^{-1} \end{array}\right. }$

以下では、 $\bar{A}_{11}$ が安定行列となるようにスイッチング関数が選ばれていると仮定します。

●偏差系E3に対するSM制御は次式で与えられます。

$\displaystyle{(6)\quad {\dot u}(t)={\dot u}_\ell(t)+{\dot u}_n(t) }$

$\displaystyle{(7)\quad {\dot u}_\ell(t)=-\underbrace{(SB_{E3})^{-1}(SA_{E3}-\Phi S)}_{K_{E3}=\left[\begin{array}{cc} K_1 & K_2 \end{array}\right]}}x_{E3}(t) }$

$\displaystyle{(8)\quad {\dot u}_n(t)=-(SB_{E3})^{-1}\rho\frac{P_2s(t)}{||P_2s(t)||}}=-(SB_{E3})^{-1}\rho\,{\rm sgn}(P_2Sx_{E3}(t)) }$

ここで、 $\Phi$ は安定行列、 $P_2>0$ は $P_2\Phi+\Phi^TP_2=-I$ の解です。

これらを積分して、制御対象に対する積分動作をもつSM制御（SMI制御）を導出します。

$\displaystyle{(9)\quad {u}(t)={u}_\ell(t)+{u}_n(t) }$

まず(7)は次式のように書けます。

$\displaystyle{(10)\quad {\dot u}_\ell(t)=- \boxed{\underbrace{ (SB_{E3})^{-1}(SA_{E3}-\Phi S)S_E^{-1} }_{\left[\begin{array}{cc} F & F_I \end{array}\right]}} \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)} }$

これを積分して

$\displaystyle{(11)\quad \boxed{u_\ell(t)=-Fx(t)+F_I\int_0^t(r-y(\tau))d\tau} }$

次に(8)は次式のように書けます。

$\displaystyle{(12)\quad {\dot u}_n(t) =-(SB_{E3})^{-1}\rho\, {\rm sgn}( \boxed{\underbrace{ P_2SS_E^{-1} }_{\left[\begin{array}{cc} G & G_I \end{array}\right]}} \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)}) }$

これを積分すれば

$\displaystyle{(13)\quad \boxed{u_n(t)=(SB_{E3})^{-1}\rho\,\int_0^t\underbrace{{\rm sgn}(-G{\dot x}(\tau)+G_I(r-y(\tau)))}_{0,\pm 1}d\tau} }$

●制御対象YMCPBに対するSMI制御系を設計するプログラムを次に示します。

MATLAB

%cYMCPB5_smi2.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント id=128=ID(33)
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys5(:,:,33));
%-----
 [n,m]=size(B);
 Mz=0.05;
 Mth=3/180*pi;
 Mthe=10/180*pi; 
 Mue=1;
 Tue=0.05;
 QE=diag([1/Mz^2,1/Mth^2,0,0,1/Mthe^2,1/Mue^2]);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 SE=[A B;C 0];
 S=swflqr(AE,BE,QE)
%-----
 Phi=-5
 P2=0.5*inv(-Phi);
 P2S=P2*S/SE;
 Leq=inv(S*BE)*S*AE;
 LPhi=-inv(S*BE)*Phi*S;
 L=(Leq+LPhi)/SE; 
 rho=1
%----- 
 F=L(1:n)
 FI=L(n+m)
 G=P2S(1:n)
 GI=P2S(n+m)  
 Ln=inv(S*BE)*rho 
%F(1)=0; F(3)=0;
%-----
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([2,3,5,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=4;
%-----
%eof
% F =
%    -1.4925   50.3338    0.2214  -11.8405   55.6410
% FI =
%    31.9622
% G =
%    -0.0113    0.1943   -0.0020    0.1934   -0.8772
% GI =
%    -0.6392
% Ln =
%     -1

図1　SMI2制御系シミュレーション

これを実行して次のシミュレーション結果を得ます。

図2　５次元モデルに基づくSMI2制御系と符号関数を通したSMI制御

Case1’：Case1でヒーブゲインを零とする場合

図3　ヒーブゲインを零とした場合のSMI2制御系と符号関数を通したSMI制御

Case２：３次元モデルに基づくSMI制御

●制御対象YMCPBに対するSMI制御系を設計するプログラムを次に示します。

MATLAB

%cYMCPB3_smi2.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----３次元モデル
%(1)th,(2)dth,(3)the
 k=[3,6]; j=[2];
 for id=ID
   AA(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,3)];
 end 
 omegae=0.0380*3;
 B=[zeros(2,1);omegae];
 E=eye(3,3);
 C=E(3,:);
 sys3=ss(AA(:,:,ID),B,eye(3,3),[]);  
%-----設計ポイント id=128,ID=33
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys3(:,:,33));
%-----
 [n,m]=size(B);
 Mth=3/180*pi;
 Tth=0.5
 Mthe=5/180*pi; 
 Mue=1;
 Tue=0.05;
 CE=eye(n+m,n+m); 
 QE=diag([1/Mth^2,(Tth/Mth)^2,1/Mthe^2,1/Mue^2]);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 SE=[A B;C 0];
 S=swflqr(AE,BE,QE)
%-----
 Phi=-5
 P2=0.5*inv(-Phi);
 P2S=P2*S/SE;
 Leq=inv(S*BE)*S*AE;
 LPhi=-inv(S*BE)*Phi*S;
 L=(Leq+LPhi)/SE;
 rho=1
%----- 
 F=L(1:n) 
 FI=L(n+m)  
 G=P2S(1:n)
 GI=P2S(n+m)  
 Ln=inv(S*BE)*rho 
%-----
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2;
%-----
%eof
% F =
%    48.8314  -14.5654   61.5153
% FI =
%    57.9634
% G =
%     0.3434    0.2175   -0.8772
% GI =
%    -1.1593
% Ln =
%     -1

図4　３次元モデルに基づくSMI2制御系と符号関数を通したSMI2制御

YMCPBのSMI制御

投稿日時: 2022年2月1日投稿者: cacsd

ここでは、次の４種類の積分動作をもつスライディングモード制御（SMI制御）系の設計について述べます。

Case	設計モデル	状態FB	状態OB	積分動作	注意点
1	５次	５次	未使用	要	「絵に描いた餅」
2	５次	３次	不要	要	ヒーブのゲインを強制的に零
3	３次	３次	不要	要	ヒーブからの連成を抑制できるか
4	５次	５次	要	要	コントローラが微分方程式

Note e13に示すように、データセットNo.1について、id=128の場合を設計用モデルとして採用したとき、Case2とCase3のどちらを用いても制御性能に大きな差異が見られません。そこでこのコントローラがどれくらいのモデル変動をカバーできるかという意味のロバスト性をシミュレーションにより調べてみます。ここで、モデル変動としては、前進速度を変える場合と、重心位置を変える場合の２通りを考えます。そのために次の３組のデータセットを考えます。

	艇	艇質量[kg]	長手方向重心位置[m]	備考
No3	SPTM247	2845	1.937	2022/1/19重量重心測定結果より
No4	SPTM247	2945	1.937	No3重量に+100kg
No5	SPTM247	2745	1.937	No3重量に-100kg

設計用モデルとして、データセットNo.3における船外機取付角-2[deg]のときのid=128の場合を採用し、Case 2とCase 3のLQIコントローラを設計します。そしてデータセットNo.3,4,5における船外機取付角-2[deg]のときのモデル（id=126～133）を安定化できるかを調べます。シミュレーション結果を次に示します。

⇒Case2:いくつかの動作点で不安定
⇒Case3:すべて安定
図1　データセットNo.3の場合、SMIコントローラのロバスト性

⇒Case2:いくつかの動作点で不安定
⇒Case3:すべて安定
図2　データセットNo.4の場合、SMIコントローラのロバスト性

⇒Case2:いくつかの動作点で不安定
⇒Case3:すべて安定
図3　データセットNo.5の場合、SMIコントローラのロバスト性

これらのシミュレーションは本来は非線形シミュレータ上で行うべきですが、ここでは６次元線形モデルを用いて簡易的に行っています。

第１の注意点は、６次元線形モデルの第４番目の状態変数 $\dot{x}(t)-V^*$ の係数を強制的に $a_{44}=0$ としていることです。これは $a_{44}>0$ となる場合があって、 $\dot{x}(t)-V^*$ が発散してしまうからです。

第２の注意点は、上のシミュレーションではCase 2のコントローラではすべてを安定化できていませんが、設計用モデルとして、データセットNo.1における船外機取付角-2[deg]のときのid=128の場合を採用すると、すべて安定化されることです。

これらの事情は、線形モデルが暫定版の非線形シミュレータから得られているので正確ではないためとも考えられます。いずれにしてもできるだけ実機のダイナミックスを反映した非線形シミュレータ上での再検討が必要になります。

MATLAB

%cYMCPB5_robust_smi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
%-----設計用データセット
 no=3
 load(dataset(no))
 ID=[7,8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 96,97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント 
 index=35 %[3,11,19,27,35]
 id=ID(index); 
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,index));
 [A,B]=ssdata(sys5(:,:,index));
%----- 
% Commands to make a unit vec tor controller incorporating integral action.
% It assumes the triple (A,B,C) is already in regular form
% For simplicity robust pole placement is used for the hyperplane design
% (with suitable modification any of the methods in chap 4 could be used)
% the nth order vector p1 specifies the reduced order motion poles
% the mth order vector p2 specifies  robust pole placement
% the range space dynamics
%-----
 [nn,mm]=size(B);
 [pp,nn]=size(C);
% Augment the orignal (A,B) with the integrator states
 bigA=[zeros(pp,pp) -C;
       zeros(nn,pp) A];
 bigB=[zeros(pp,mm);
       B];
% Since it is assumed that (A,B) is in regular form the augmented 
% pair (bigA,bigB) is also automatically in refular form
 A11=bigA(1:pp+nn-mm,1:pp+nn-mm);
 A12=bigA(1:pp+nn-mm,nn+pp-mm+1:nn+pp);
 B2=bigB(nn+pp-mm+1:nn+pp,:);
% Hyperplane design using robust pole placement
 j=sqrt(-1);
 p1=[-4.7/20+j*4.7, -4.7/20-j*4.7, -7.8/20+j*7.8, -7.8/20-j*7.8, -2/10];
 M=place(A11,A12,p1);
 A11s=A11-A12*M;
 S2=eye(mm)  % For simplicity
 S=S2*[M eye(mm)];
% Form the range space dynamics system matrix
 p2=-1
 Phi=diag(p2,0);
 P2hat=lyap(Phi,eye(mm));
 P2=inv(P2hat);
% Calculate the time varying component of the switching function
 Br=[eye(pp);
     zeros(nn-mm,pp)];
 Sr=-S2*inv(Br'*inv(A11s)*A12)*Br'*inv(A11s)*Br;
% Computes the gains for the feedback and feed-forward components
 L=-inv(S*bigB)*S*bigA+inv(S*bigB)*Phi*S
 L(2)=0; L(4)=0;
 Lr=-inv(S*bigB)*(Phi*Sr+S(:,1:pp))
 Ldr=inv(S*bigB)*Sr
 rho=1
 Ln=inv(S*bigB)*rho
%-----全モデルに対するシミュレーション
for no=[3,4,5] 
for iSIM=index-2:index+5
 id=ID(iSIM);
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,iSIM));
 Asys(4,4)=0;
 [A,B]=ssdata(sys5(:,:,iSIM));  
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([2,3,5,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=4;
 simout=sim('nYMCPB_smi.slx');
%[no,iSIM,id,180/pi*us]
 figure(no)
 subplot(221),plot(simout.x.Time,100*simout.x.Data(:,2)),hold on
 subplot(223),plot(simout.y.Time,180/pi*simout.y.Data(:,1)),hold on
 subplot(222),plot(simout.y.Time,180/pi*simout.y.Data(:,3)),hold on
 subplot(224),plot(simout.u.Time,simout.u.Data),hold on
end %iSIM
 figure(no)
 subplot(221),title("z[cm]"),grid
 subplot(223),title("th[deg]"),grid
 subplot(222),title("the[deg]"),grid
 subplot(224),title("ue"),grid
end %no 
%-----
%eof

MATLAB

%cYMCPB3_robust_smi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
%-----設計用データセット
 no=3
 load(dataset(no))
 ID=[7,8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 96,97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----３次元モデル
%(1)th,(2)dth,(3)the
 k=[3,6]; j=[2];
 for id=ID
   AA(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,3)];
 end 
 omegae=0.0380*3;
 B=[zeros(2,1);omegae];
 E=eye(3,3);
 C=E(3,:);
 sys3=ss(AA(:,:,ID),B,eye(3,3),[]);  
%-----設計ポイント 
 index=35 %[3,11,19,27,35]
 id=ID(index); 
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,index));
 [A,B]=ssdata(sys3(:,:,index));
%----- 
% Commands to make a unit vec tor controller incorporating integral action.
% It assumes the triple (A,B,C) is already in regular form
% For simplicity robust pole placement is used for the hyperplane design
% (with suitable modification any of the methods in chap 4 could be used)
% the nth order vector p1 specifies the reduced order motion poles
% the mth order vector p2 specifies  robust pole placement
% the range space dynamics
%-----
 [nn,mm]=size(B);
 [pp,nn]=size(C);
% Augment the orignal (A,B) with the integrator states
 bigA=[zeros(pp,pp) -C;
       zeros(nn,pp) A];
 bigB=[zeros(pp,mm);
       B];
% Since it is assumed that (A,B) is in regular form the augmented 
% pair (bigA,bigB) is also automatically in refular form
 A11=bigA(1:pp+nn-mm,1:pp+nn-mm);
 A12=bigA(1:pp+nn-mm,nn+pp-mm+1:nn+pp);
 B2=bigB(nn+pp-mm+1:nn+pp,:);
% Hyperplane design using robust pole placement
 j=sqrt(-1);
 p1=[-4.7/20+j*4.7, -4.7/20-j*4.7, -2/10];
 M=place(A11,A12,p1);
 A11s=A11-A12*M;
 S2=eye(mm)  % For simplicity
 S=S2*[M eye(mm)];
% Form the range space dynamics system matrix
 p2=-1
 Phi=diag(p2,0);
 P2hat=lyap(Phi,eye(mm));
 P2=inv(P2hat);
% Calculate the time varying component of the switching function
 Br=[eye(pp);
     zeros(nn-mm,pp)];
 Sr=-S2*inv(Br'*inv(A11s)*A12)*Br'*inv(A11s)*Br;
% Computes the gains for the feedback and feed-forward components
 L=-inv(S*bigB)*S*bigA+inv(S*bigB)*Phi*S
 Lr=-inv(S*bigB)*(Phi*Sr+S(:,1:pp))
 Ldr=inv(S*bigB)*Sr
 rho=1
 Ln=inv(S*bigB)*rho 
%-----全モデルに対するシミュレーション
for no=[3,4,5] 
for iSIM=index-2:index+5
 id=ID(iSIM);
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,iSIM));
 Asys(4,4)=0;
 [A,B]=ssdata(sys3(:,:,iSIM));    
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2;
 simout=sim('nYMCPB_smi.slx');
%[no,iSIM,id,180/pi*us]
 figure(no)
 subplot(221),plot(simout.x.Time,100*simout.x.Data(:,2)),hold on
 subplot(223),plot(simout.y.Time,180/pi*simout.y.Data(:,1)),hold on
 subplot(222),plot(simout.y.Time,180/pi*simout.y.Data(:,3)),hold on
 subplot(224),plot(simout.u.Time,simout.u.Data),hold on
end %iSIM
 figure(no)
 subplot(221),title("z[cm]"),grid
 subplot(223),title("th[deg]"),grid
 subplot(222),title("the[deg]"),grid
 subplot(224),title("ue"),grid
end %no  
%-----
%eof

Note e13　SMI制御系の設計
Case 1：５次元モデルに基づくSMI制御

●制御対象

$\displaystyle{(1)\quad \begin{array}{l} \dot{x}(t)=Ax(t)+Bu(t)\\ y(t)=Cx(t)\\ (x(t)\in{\rm\bf R}^n, u(t)\in{\rm\bf R}^m, y(t)\in{\rm\bf R}^p, m=p) \end{array} }$

の出力を、コマンド（次式の解）

$\displaystyle{(2)\quad \begin{array}{l} %\dot{r}(t)=\Gamma(r(t)-r_c)\\ \frac{d}{dt}(r(t)-r_c)=\Gamma(r(t)-r_c)\\ (r(t),r_c\in{\rm\bf R}^m) \end{array} }$

に追従させることを考えます（ $\Gamma$ は安定行列）。そのために、積分動作

$\displaystyle{(3)\quad \begin{array}{l} \dot{x}_r(t)=r(t)-y(t)\\ (x_r(t)\in{\rm\bf R}^m) \end{array} }$

を導入し、次の拡大系を構成します。ここで、(1)はすでに標準形であるとしています。

$\displaystyle{(4)\quad \begin{array}{l} \left[\begin{array}{c} \dot x_r(t)\\ \dot x(t) \end{array}\right] = \left[\begin{array}{c|cc} 0 & -C_1 & -C_2\\\hline 0 & A_{11} & A_{12} \\ 0 & A_{21} & A_{22} \end{array}\right] \left[\begin{array}{c} x_r(t)\\ x(t) \end{array}\right] + \left[\begin{array}{c} 0\\\hline 0\\ B_2 \end{array}\right] u(t) + \left[\begin{array}{c} I_m \\\hline 0 \\ 0 \end{array}\right] r(t)\\ (x_r(t)\in{\rm\bf R}^m, x(t)\in{\rm\bf R}^n) \end{array} }$

これを、次のように分割し直しても標準形であることには変わりありません。

$\displaystyle{(5a)\quad \begin{array}{l} \left[\begin{array}{c} \dot{x}_1(t)\\ \dot{x}_2(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc|c} 0 & -C_1 & -C_2\\ 0 & A_{11} & A_{12} \\\hline 0 & A_{21} & A_{22} \end{array}\right] }_{A_E} \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0\\ 0\\\hline B_2 \end{array}\right] }_{B_E} u(t) + \left[\begin{array}{c} I_m \\ 0 \\\hline 0 \end{array}\right] r(t)\\ (x_1(t)\in{\rm\bf R}^n, x_2(t)\in{\rm\bf R}^m) \end{array} }$

ただし

$\displaystyle{(5b)\quad %\begin{array}{l} \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] = \left[\begin{array}{c} x_r(t)\\ x(t) \end{array}\right] %\end{array} }$

●この積分器による拡大系を安定化できれば、積分器の値 $x_r(t)$ は定値となり、被積分項 $r(t)-y(t)$ の値は零となり、 $y(t)$ は $r(t)$ へ漸近します。そこで、SM制御によって拡大系を安定化し、追従制御系を構成することを考えます。この制御系は特別な $r(t)=0$ の場合を含みますので、まずスイッチング関数として、次式を考えます。

$\displaystyle{(6)\quad s(t)= \underbrace{ \left[\begin{array}{cc} S_1 & S_2 \\ \end{array}\right] }_{S} %\underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] %}_{x(t)} = \underbrace{S_2 \left[\begin{array}{cc} M & I_m \\ \end{array}\right] }_{S} %\underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] %}_{x(t)} \ (M=S_2^{-1}S_1) }$

(5)に対して、座標変換

$\displaystyle{(7)\quad \begin{array}{l} \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc} I_n & 0 \\ S_1 & S_2 \\ \end{array}\right] }_{T_s} \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right]\\ \Leftrightarrow \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc} I_n & 0 \\ -S_2^{-1}S_1 & S_2^{-1} \\ \end{array}\right] }_{T_s^{-1}} \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] \end{array} }$

を行って、次式を得ます。

$\displaystyle{(8a)\quad \begin{array}{l} %\underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot s(t) \end{array}\right] %}_{\dot{x}'(t)} = \underbrace{ \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ S_2\bar{A}_{21} & S_2\bar{A}_{22}S_2^{-1} \\ \end{array}\right] }_{T_sA_ET_s^{-1}} %\underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] %}_{x'(t)} + \underbrace{ \left[\begin{array}{cc} 0\\ S_2B_2 \end{array}\right] }_{T_sB_E} u(t)\\ + \left[\begin{array}{cc} B_r \\ S_1B_r \end{array}\right] r(t) \end{array} }$

ただし

$\displaystyle{(8b)\quad \left\{\begin{array}{l} \bar{A}_{11}= \left[\begin{array}{cc} 0 & -C_1 \\ 0 & A_{11} \end{array}\right] -\left[\begin{array}{c} -C_2\\ A_{12} \end{array}\right]M\\ \bar{A}_{12}= \left[\begin{array}{c} -C_2\\ A_{12} \end{array}\right]S_2^{-1}\\ \bar{A}_{21}=S_2(M\bar{A}_{11} + \left[\begin{array}{cc} 0 & A_{21} \end{array}\right] -A_{22}M)\\ \bar{A}_{22}=S_2(M \left[\begin{array}{c} -C_2\\ A_{12} \end{array}\right] +A_{22})S_2^{-1}\\ B_r=\left[\begin{array}{cc} I_m \\ 0 \end{array}\right] \end{array}\right. }$

ここで、 $\bar{A}_{11}$ が安定行列となるように行列 $M$ が選ばれているとします。

●このときSM制御則は次式で与えられます。

$\displaystyle{(9)\quad { \boxed{\begin{array}{l} u(t)=u_\ell(t)+u_n(t)\\ u_\ell(t)=-\underbrace{(SB_E)^{-1}(SA_E-\Phi S)}_{L=L_{eq}+L_\Phi}\left[\begin{array}{c} x_r(t)\\ x(t) \end{array}\right]\\ -\underbrace{(SB_E)^{-1}(\Phi S_r+S_1B_r)}_{L_r} r(t) +\underbrace{(SB_E)^{-1}S_r}_{L_{\dot r}} \dot{r}(t)\\ u_n(t)=-\underbrace{(SB_E)^{-1}\rho(t,x)}_{L_n}\frac{P_2(s(t)-S_rr(t))}{||P_2(s(t)-S_rr(t))||} \end{array}}} }$

MATLAB

%cYMCPB5_smi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント id=128=ID(33)
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys5(:,:,33));
%-----
 AE=[0 -C ;
    zeros(5,1) A];
 BE=[0; B];
 lambda=[-4.7/20 4.7 -7.8/20 7.8 -2/10];
 nocomp=2;
 specpos=rand(6,5);
 specent=rand(6,5);
 S=swfvpl(AE,BE,lambda,nocomp,specpos,specent);   
 FE=(S*BE)\(S*AE);
 eig(AE-BE*FE);
%-----
 [nn,mm]=size(B);
 S1=S(:,1:nn-mm);
 S2=S(:,nn-mm+1:nn);
 Br=eye(nn-mm,mm);
 B2=B(nn-mm+1:nn,:); 
 Lambda=S2*B2;
 Phi=-0.5
 P2=0.5*inv(-Phi);
 Leq=inv(S*BE)*S*AE;
 LPhi=-inv(S*BE)*Phi*S;
 rho=2
 Sr=0
%----- 
 L=Leq+LPhi
 Ln=inv(S*BE)*rho
 Lr=inv(Lambda)*(Phi*Sr+S1*Br)
 Ldr=inv(Lambda)*Sr
%L(2)=0; L(4)=0;
%-----
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([2,3,5,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=4;
%-----
%eof
% L =
%    -0.6767   53.6065  -82.8461    3.7131   18.0214    2.8876
% Ln =
%    39.6861
% Lr =
%   -11.3251
% Ldr =
%      0

図1　SMI制御系シミュレーション

図2　５次元モデルに基づくSMI制御系と符号関数を通したSMI制御

Case 2：Case1でヒーブゲインを零とする場合

図3　ヒーブゲインを零とした場合のSMI制御系と符号関数を通したSMI制御

Case 3：３次元モデルに基づくSMI制御

●制御対象YMCPBに対するSMI制御系を設計するプログラムを次に示します。

MATLAB

%cYMCPB3_smi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----３次元モデル
%(1)th,(2)dth,(3)the
 k=[3,6]; j=[2];
 for id=ID
   AA(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,3)];
 end 
 omegae=0.0380*3;
 B=[zeros(2,1);omegae];
 E=eye(3,3);
 C=E(3,:);
 sys3=ss(AA(:,:,ID),B,eye(3,3),[]);  
%-----設計ポイント id=128,ID=33
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys3(:,:,33));
%----- 
 AE=[0 -C ;
    zeros(3,1) A];
 BE=[0; B];
 lambda=[-4.7/20 4.7 -2/10];
 nocomp=1;
 specpos=rand(4,3);
 specent=rand(4,3);
 S=swfvpl(AE,BE,lambda,nocomp,specpos,specent);    
 FE=(S*BE)\(S*AE);
 eig(AE-BE*FE);
%---
 [nn,mm]=size(B);
 S1=S(:,1:nn-mm);
 S2=S(:,nn-mm+1:nn);
 Br=eye(nn-mm,mm);
 B2=B(nn-mm+1:nn,:); 
 Lambda=S2*B2;
 Phi=-0.5
 P2=0.5*inv(-Phi);
 Leq=inv(S*BE)*S*AE;
 LPhi=-inv(S*BE)*Phi*S;
 rho=2
 Sr=0
%-----
 L=Leq+LPhi
 Ln=inv(S*BE)*rho
 Lr=inv(Lambda)*(Phi*Sr+S1*Br)
 Ldr=inv(Lambda)*Sr
%-----
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2;
%-----
%eof
% L =
%    -0.6402   73.9799   24.2004   11.2935
% Ln =
%    54.0449
% Lr =
%     5.5635
% Ldr =
%      0

図4　３次元モデルに基づくSMI制御系と符号関数を通したSMI制御

YMCPBのLQI制御

投稿日時: 2022年2月1日投稿者: cacsd

ここでは、次の４種類のLQI制御系設計について述べます。

Case	設計モデル	状態FB	状態OB	積分動作	注意点
1	５次	５次	未使用	要	「絵に描いた餅」
2	５次	３次	不要	要	ヒーブのゲインを強制的に零
3	３次	３次	不要	要	ヒーブからの連成を抑制できるか
4	５次	５次	要	要	コントローラが微分方程式

Note e12に示すように、データセットNo.1について、id=128の場合を設計用モデルとして採用したとき、Case2とCase3のどちらを用いても制御性能に大きな差異が見られません。そこでこのコントローラがどれくらいのモデル変動をカバーできるかという意味のロバスト性をシミュレーションにより調べてみます。ここで、モデル変動としては、前進速度を変える場合と、重心位置を変える場合の２通りを考えます。そのために次の３組のデータセットを考えます。

	艇	艇質量[kg]	長手方向重心位置[m]	備考
No3	SPTM247	2845	1.937	2022/1/19重量重心測定結果より
No4	SPTM247	2945	1.937	No3重量に+100kg
No5	SPTM247	2745	1.937	No3重量に-100kg

⇒Case2:いくつかの動作点で不安定
⇒Case3:いくつかの動作点で不安定
図1　データセットNo.3の場合、LQIコントローラのロバスト性

⇒Case2:いくつかの動作点で不安定
⇒Case3:いくつかの動作点で不安定
図2　データセットNo.4の場合、LQIコントローラのロバスト性

⇒Case2:いくつかの動作点で不安定
⇒Case3:いくつかの動作点で不安定
図3　データセットNo.5の場合、LQIコントローラのロバスト性

これらのシミュレーションは本来は非線形シミュレータ上で行うべきですが、ここでは６次元線形モデルを用いて簡易的に行っています。

第２の注意点は、上のシミュレーションでは各データセットのすべてのモデルを安定化できていませんが、設計用モデルとして、データセットNo.1における船外機取付角-2[deg]のときのid=128の場合を採用すると、各データセットのすべてのモデルは安定化されることです。

MATLAB

%cYMCPB5_robust_lqi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
%-----設計用データセット
 no=3
 load(dataset(no))
 ID=[7,8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 96,97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント 
 index=35 %[3,11,19,27,35]
 id=ID(index); 
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,index));
 [A,B]=ssdata(sys5(:,:,index));
%-----
 [n,m]=size(B);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 S=[A B;C 0];
 Mz=0.05;
 Mth=3/180*pi;
 Mthe=10/180*pi; 
 Mue=1;
 Tue=0.05;
 CE=eye(n+m,n+m); 
 QE=diag([1/Mz^2,1/Mth^2,0,0,1/Mthe^2,1/Mue^2]);
 RE=(Tue/Mue)^2;
 [KE,PE]=opt(AE,BE,CE,QE,RE);
 poles=eig(AE-BE*KE);
 FE=KE/S;
%----- 
 F=FE(:,1:n)
 FI=FE(:,n+1:n+m)
 F(1)=0; F(3)=0;
%-----全モデルに対するシミュレーション
for no=[3,4,5] 
for iSIM=index-2:index+5  
 id=ID(iSIM);
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,iSIM));
 Asys(4,4)=0;
 [A,B]=ssdata(sys5(:,:,iSIM));  
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([2,3,5,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=4;
 simout=sim('nYMCPB_lqi.slx');
%[no,iSIM,id,180/pi*us]
 figure(no)
 subplot(221),plot(simout.x.Time,100*simout.x.Data(:,2)),hold on
 subplot(223),plot(simout.y.Time,180/pi*simout.y.Data(:,1)),hold on
 subplot(222),plot(simout.y.Time,180/pi*simout.y.Data(:,3)),hold on
 subplot(224),plot(simout.u.Time,simout.u.Data),hold on
end %iSIM
 figure(no)
 subplot(221),title("z[cm]"),grid
 subplot(223),title("th[deg]"),grid
 subplot(222),title("the[deg]"),grid
 subplot(224),title("ue"),grid
end %no 
%-----
%eof

MATLAB

%cYMCPB3_robust_lqi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
%-----設計用データセット
 no=3
 load(dataset(no))
 ID=[7,8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 96,97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----３次元モデル
%(1)th,(2)dth,(3)the
 k=[3,6]; j=[2];
 for id=ID
   AA(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,3)];
 end 
 omegae=0.0380*3;
 B=[zeros(2,1);omegae];
 E=eye(3,3);
 C=E(3,:);
 sys3=ss(AA(:,:,ID),B,eye(3,3),[]);  
%-----設計ポイント 
 index=35 %[3,11,19,27,35]
 id=ID(index); 
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,index));
 [A,B]=ssdata(sys3(:,:,index));
%-----
 [n,m]=size(B);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 S=[A B;C 0];
 Mth=3/180*pi;
 Tth=0.5;
 Mthe=5/180*pi; 
 Mue=1;
 Tue=0.05;
 CE=eye(n+m,n+m); 
 QE=diag([1/Mth^2,(Tth/Mth)^2,1/Mthe^2,1/Mue^2]);
 RE=(Tue/Mue)^2;
 [KE,PE]=opt(AE,BE,CE,QE,RE);
 poles=eig(AE-BE*KE);
 FE=KE/S;
%-----  
 F=FE(:,1:n)
 FI=FE(:,n+1:n+m) 
%-----全モデルに対するシミュレーション
for no=[3,4,5] 
for iSIM=index-2:index+5 
 id=ID(iSIM);
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,iSIM));
 Asys(4,4)=0;
 [A,B]=ssdata(sys3(:,:,iSIM));    
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2;
 simout=sim('nYMCPB_lqi.slx');
%[no,iSIM,id,180/pi*us]
 figure(no)
 subplot(221),plot(simout.x.Time,100*simout.x.Data(:,2)),hold on
 subplot(223),plot(simout.y.Time,180/pi*simout.y.Data(:,1)),hold on
 subplot(222),plot(simout.y.Time,180/pi*simout.y.Data(:,3)),hold on
 subplot(224),plot(simout.u.Time,simout.u.Data),hold on
end %iSIM
 figure(no)
 subplot(221),title("z[cm]"),grid
 subplot(223),title("th[deg]"),grid
 subplot(222),title("the[deg]"),grid
 subplot(224),title("ue"),grid
end %no 
%-----
%eof

Note e12　LQI制御系の設計
Case 1：５次元モデルに基づくLQI制御

●LQI制御系を設計するために、次の５次元モデルを考えます。

$\displaystyle{(1.1)\quad \begin{array}{l} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{ccccc} 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 & 0\\ a_{52} & a_{53} & a_{55} & a_{56} & b_{52}\\ a_{62} & a_{63} & a_{65} & a_{66} & b_{62}\\ 0 & 0 & 0 & 0 & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)}\\ + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ 0 \\ \omega_e \end{array}\right] }_{B} u(t) \end{array}}$

$\displaystyle{(1.2)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{y(t)} = \underbrace{ \left[\begin{array}{ccccc} 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 1 \end{array}\right] }_{C_M} \underbrace{ \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)} \end{array}}$

ここで操作入力は $u_e(t)$ ではなく $u(t)$ としていますが、制御系の評価の際に

$\displaystyle{(2)\quad u_e(t)={\rm sign}(u(t)) }$

とします。

●制御対象YMCPBに対する制御目的は、あるスラスト $T^*$ と取付角 $\theta_e^*$ の下で、巡航速度 $V^*$ で走行するとき、一定の姿勢 $z^*,\theta^*$ を保つこととします。制御目的を達成する制御則として次式を考えます。

$\displaystyle{(3)\quad u(t)=-Fx(t)+\int_0^t(\theta_e^*-\theta_e(t))dt }$

ここで、右辺第１項は状態フィードバック、第２項は船外機取付角をある収納状態 $\theta_e(0)\ne0$ から所定の平衡入力値 $\theta_e^*$ に設定するための積分動作です。いま

$\displaystyle{(4)\quad \begin{array}{l} \theta_e(t)-\theta_e^* = \underbrace{ \left[\begin{array}{ccccc} 0 & 0 & 0 & 0 & 1 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)} \rightarrow \theta_e^c=0\quad(t\rightarrow\infty) \end{array}}$

となって、船外機取付角を設定値 $\theta_e^*$ に維持できたとすると、次式が成り立つ必要があります。

$\displaystyle{(4)\quad \left\{\begin{array}{l} \underbrace{\dot{x}(\infty)}_{0}=A\underbrace{x(\infty)}_{x_\infty}+B\underbrace{u(\infty)}_{u_\infty}\\ \underbrace{ \theta_e(t)-\theta_e^* }_{\theta_e^c=0}=C\underbrace{x(\infty)}_{x_\infty} \end{array}\right.}$

すなわち

$\displaystyle{(4')\quad \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\\hline \theta_e^c \\ \end{array}\right] = \underbrace{ \left[\begin{array}{ccccc|c} 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0\\ a_{52} & a_{53} & a_{55} & a_{56} & b_{52}& 0\\ a_{62} & a_{63} & a_{65} & a_{66} & b_{62}& 0\\ 0 & 0 & 0 & 0 & 0 & \omega_e\\\hline 0 & 0 & 0 & 0 & 1 & 0 \end{array}\right] }_{S= \left[\begin{array}{cc} A & B\\ C & 0 \end{array}\right] } \left[\begin{array}{c} z_\infty\\ \theta_\infty\\ 0\\ 0\\ \theta_e^c \\\hline 0 \end{array}\right] }$

ここで、システム行列 $S$ は正則ですから、任意の平衡入力値 $\theta_e^*$ に対して、 $\theta_e(\infty)=\theta_e^*$ をもたらす状態変数 $z_\infty$ と $\theta_\infty$ が定まることが分かります。そこで次の偏差系E3を考えます。

$\displaystyle{(5)\quad \begin{array}{l} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} (z(t)-z^*)-z_\infty\\ (\theta(t)-\theta^*)-\theta_\infty\\ \dot{z}(t)\\ \dot{\theta}(t)\\ (\theta_e(t)-\theta_e^*)-\theta_e^c \\\hline u(t) \end{array}\right] }_{\dot{x}_E(t)} = \underbrace{ \left[\begin{array}{ccccc|c} 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0\\ a_{52} & a_{53} & a_{55} & a_{56} & b_{52}& 0\\ a_{62} & a_{63} & a_{65} & a_{66} & b_{62}& 0\\ 0 & 0 & 0 & 0 & 0 & \omega_e\\\hline 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right] }_{A_E}\\ \times\underbrace{ \left[\begin{array}{c} (z(t)-z^*)-z_\infty\\ (\theta(t)-\theta^*)-\theta_\infty\\ \dot{z}(t)\\ \dot{\theta}(t)\\ (\theta_e(t)-\theta_e^*)-\theta_e^c \\\hline u(t) \end{array}\right] }_{x_E(t)} + \underbrace{ \left[\begin{array}{cccc} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\\hline 1 \\ \end{array}\right] }_{B_E} \dot{u}(t) \end{array} }$

これに対する状態フィードバック

$\displaystyle{(6)\quad \dot{u}(t)=- \underbrace{ \left[\begin{array}{ccccc|c} k_{1} & k_{2} & k_{3} & k_{4} & k_{5} & k_{6} \end{array}\right] }_{K_E} \underbrace{ \left[\begin{array}{c} (z(t)-z^*)-z_\infty\\ (\theta(t)-\theta^*)-\theta_\infty\\ \dot{z}(t)\\ \dot{\theta}(t)\\ (\theta_e(t)-\theta_e^*)-\theta_e^c \\\hline u(t) \end{array}\right] }_{x_E(t)} }$

を、二次形式評価関数

$(7.1)\quad \begin{array}{l} \displaystyle{J=\int_0^\infty\left\{q_1^2(z(t)-z^*-z_\infty)^2+q_2^2(\theta(t)-\theta^*-\theta_\infty)^2+q_5^2(\theta_e(t)-\theta_e^*-\theta_e^c)^2\right.}\\ \left.+q_6^2u^2(t)+r^2\dot{u}^2(t)\right\}\,dt}}\\ \end{array} }$

$\displaystyle{(7.2)\quad \begin{array}{l} q_1=\frac{1}{M_z}, q_2=\frac{1}{M_\theta}, q_5=\frac{1}{M_{\theta_e}}, q_6=\frac{1}{M_{u}}, r=\frac{1}{M_{u}/T_{u}} \end{array} }$

$\displaystyle{(7.3)\quad \begin{array}{l} |z(t)-z^*-z_\infty|<{M_z}, |\theta(t)-\theta^*-\theta_\infty|<{M_\theta}, |\theta_e(t)-\theta_e^*-\theta_e^c|<M_{\theta_e}},\\ |u(t)|<M_{u}, |\dot{u}(t)|<\frac{M_{u}}{T_{u}} \end{array} }$

を最小化して求めます。(6)は

$\displaystyle{(8)\quad \left[\begin{array}{c} \frac{d}{dt} \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \\\hline \end{array}\right]\\\hline (\theta_e(t)-\theta_e^*)-\theta_e^c \end{array}\right]=S \left[\begin{array}{c} (z(t)-z^*)-z_\infty\\ (\theta(t)-\theta^*)-\theta_\infty\\ \dot{z}(t)\\ \dot{\theta}(t)\\ (\theta_e(t)-\theta_e^*)-\theta_e^c \\\hline u(t) \end{array}\right] }$

に注意して

$\displaystyle{(9)\quad \dot{u}(t)=- \underbrace{ \left[\begin{array}{ccccc|c} k_{1} & k_{2} & k_{3} & k_{4} & k_{5} & k_{6} \end{array}\right]S^{-1} }_{F_E} \left[\begin{array}{c} \frac{d}{dt} \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \\\hline \end{array}\right]\\\hline (\theta_e(t)-\theta_e^*)-\theta_e^c \end{array}\right] }$

と書けるので、これを積分して次のLQI制御則を得ます。

$\displaystyle{(10)\quad u(t) =- \underbrace{ \left[\begin{array}{ccccc} f_{1} & f_{2} & f_{3} & f_{4} & f_{5} \end{array}\right] }_{F} \underbrace{ \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)} +\underbrace{f_6}_{F_I}\int_0^t(\theta_e^c-(\theta_e(t)-\theta_e^*))dt} }$

●制御対象YMCPBに対するLQI制御系を設計するプログラムを次に示します。

MATLAB

%cYMCPB5_lqi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント id=128=ID(33)
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys5(:,:,33));
%-----
 [n,m]=size(B);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 S=[A B;C 0];
 Mz=0.05;
 Mth=3/180*pi;
 Mthe=10/180*pi; 
 Mue=1;
 Tue=0.05;
 CE=eye(n+m,n+m); 
 QE=diag([1/Mz^2,1/Mth^2,0,0,1/Mthe^2,1/Mue^2]);
 RE=(Tue/Mue)^2;
 [KE,PE]=opt(AE,BE,CE,QE,RE);
 poles=eig(AE-BE*KE);
 FE=KE/S;
%----- 
 F=FE(:,1:n)
 FI=FE(:,n+1:n+m)
%F(1)=0; F(3)=0;
%-----  
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([2,3,5,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=4;
%-----
%eof
% F = 0.0728   20.4471    0.4015  -39.3709  186.9394
% FI = 127.8489

図1　LQI制御系シミュレーション

これを実行して次のシミュレーション結果を得ます。

図1　５次元モデルに基づくLQI制御系と符号関数を通したLQI制御

Case 2：Case1でヒーブゲインを零とする場合

図2　ヒーブゲインを零とした場合のLQI制御系と符号関数を通したLQI制御

Case 3：３次元モデルに基づくLQI制御

●制御対象YMCPBに対するLQI制御系を設計するプログラムを次に示します。

MATLAB

%cYMCPB3_lqi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----３次元モデル
%(1)th,(2)dth,(3)the
 k=[3,6]; j=[2];
 for id=ID
   AA(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,3)];
 end 
 omegae=0.0380*3;
 B=[zeros(2,1);omegae];
 E=eye(3,3);
 C=E(3,:);
 sys3=ss(AA(:,:,ID),B,eye(3,3),[]);  
%-----設計ポイント id=128=ID(33)
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys3(:,:,33));
%-----
 [n,m]=size(B);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 S=[A B;C 0];
 Mth=3/180*pi;
 Tth=0.5
 Mthe=5/180*pi; 
 Mue=1;
 Tue=0.05;
 CE=eye(n+m,n+m); 
 QE=diag([1/Mth^2,(Tth/Mth)^2,1/Mthe^2,1/Mue^2]);
 RE=(Tue/Mue)^2;
 [KE,PE]=opt(AE,BE,CE,QE,RE);
 poles=eig(AE-BE*KE);
 FE=KE/S;
%-----  
 F=FE(:,1:n)
 FI=FE(:,n+1:n+m)
%-----  
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2;
%-----
%eof
% F =
%    -2.8411  -45.8237  192.5801
% FI =
%   231.8536

図3　３次元モデルに基づくLQI制御系と符号関数を通したLQI制御

Case 4：Case 1で状態オブザーバを用いる場合

いま、観測変数が状態変数の最初にくるように、状態方程式と観測方程式における状態変数の順番を変えておきます。

$\displaystyle{(11)\quad \begin{array}{l} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)\\\hline z(t)-z^*\\ \dot{z}(t) \end{array}\right] }_{\dot{x}'(t)} = \underbrace{ \left[\begin{array}{ccc|cc} 0 & 1 & 0 & 0 & 0 \\ a_{63} & 0 & b_{62} & a_{62} & 0 \\ 0 & 0 & 0 & 0 & 0 \\\hline 0 & 0 & 0 & 0 & 1 \\ a_{53} & 0 & b_{52} & a_{52} & 0 \end{array}\right] }_{A'=\left[\begin{array}{cc} A'_{11} & A'_{12}\\ A'_{21} & A'_{22} \end{array}\right] } \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)\\\hline z(t)-z^*\\ \dot{z}(t) \end{array}\right] }_{x'(t)}\\ + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ \omega_e\\\hline 0 \\ 0 \end{array}\right] }_{B'=\left[\begin{array}{cc} B'_1 \\ B'_2 \end{array}\right] } u(t) \end{array}}$

$\displaystyle{(12)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t) \end{array}\right] }_{y(t)} = \underbrace{ \left[\begin{array}{ccc|cc} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \end{array}\right] }_{C'=[I_p\ 0]} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)\\\hline z(t)-z^*\\ \dot{z}(t) \end{array}\right] }_{x'(t)} \end{array}}$

このとき、 $U$ を

$\displaystyle{(13)\quad U= \underbrace{\left[\begin{array}{ccc|cc} -\ell_{11} & -\ell_{12} & -\ell_{13} & 1 & 0 \\ -\ell_{21} & -\ell_{22} & -\ell_{23} & 0 & 1 \end{array}\right]}_{[-L\ I_{n-p}]} }$

のように選んで、オブザーバのパラメータを次のように求めることができます。

$\displaystyle{(14)\quad \left[\begin{array}{cc} \hat{C} & \hat{D} \end{array}\right]= \left[\begin{array}{cc} U\\ C' \end{array}\right]^{-1}= \left[\begin{array}{cc} -L & I_{n-p}\\ I_p& 0 \end{array}\right]^{-1} = \left[\begin{array}{cc} 0 & I_p\\ I_{n-p}& L \end{array}\right] }$

$\displaystyle{(15)\quad \begin{array}{l} \left[\begin{array}{cc} \hat{A} & \hat{B} \end{array}\right]=UA' \left[\begin{array}{cc} U\\ C' \end{array}\right]^{-1}= \left[\begin{array}{cc} -L & I_{n-p} \end{array}\right] \left[\begin{array}{cc} A'_{11} & A'_{12}\\ A'_{21} & A'_{22} \end{array}\right] \left[\begin{array}{cc} 0 & I_p\\ I_{n-p}& L \end{array}\right]\\ =\left[\begin{array}{cc} A'_{22}-LA'_{12} & A'_{21}-LA'_{11}+\hat{A}L \end{array}\right] \end{array} }$

$\displaystyle{(16)\quad \hat{J}=UB'= \left[\begin{array}{cc} -L & I_{n-p} \end{array}\right] \left[\begin{array}{c} B'_1\\ B'_2 \end{array}\right] =B'_2-LB'_1 }$

ここで、設計パラメータはサイズ $n-p\times p$ の行列 $L$ で

$\displaystyle{(17)\quad \hat{A} = \underbrace{\left[\begin{array}{ccc} 0 & 1 \\ a_{52} & 0 \end{array}\right]}_{A'_{22}}- \underbrace{\left[\begin{array}{ccc} \ell_{11} & \ell_{12} & \ell_{13} \\ \ell_{21} & \ell_{22} & \ell_{23} \end{array}\right]}_{L} \underbrace{\left[\begin{array}{ccc} 0 & 0 \\ a_{62} & 0 \\ 0 & 0 \end{array}\right]}_{A'_{12}} }$

を安定行列とするように選びます。そのためには、仮想システムに対する状態フィードバックによる安定化問題

$\displaystyle{ \begin{array}{cl} (18.1) &\left[\begin{array}{c} \dot{w}_1 \\ \dot{w}_2 \end{array}\right] = \underbrace{\left[\begin{array}{ccc} 0 & a_{52} \\ 1 & 0 \end{array}\right]}_{A'_{22}^T} \left[\begin{array}{c} w_1 \\ w_2 \end{array}\right]+ \underbrace{\left[\begin{array}{ccc} 0 & a_{62} & 0 \\ 0 & 0 & 0 \end{array}\right]}_{A'_{12}^T} \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \end{array}\right]\\ (18.2) &\left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \end{array}\right]=- \underbrace{\left[\begin{array}{ccc} \ell_{11} & \ell_{21} \\ \ell_{12} & \ell_{22} \\ \ell_{13} & \ell_{23} \end{array}\right]}_{L^T} \left[\begin{array}{c} w_1 \\ w_2 \end{array}\right] \end{array} }$

すなわち

$\displaystyle{ \begin{array}{cl} (19.1) &\left[\begin{array}{c} \dot{w}_1 \\ \dot{w}_2 \end{array}\right] = \left[\begin{array}{ccc} 0 & a_{52} \\ 1 & 0 \end{array}\right] \left[\begin{array}{c} w_1 \\ w_2 \end{array}\right]+ \left[\begin{array}{c} a_{62} \\ 0 \end{array}\right]v_2\\ (19.2) &v_2=- \left[\begin{array}{ccc} \ell_{12} & \ell_{22} \end{array}\right] \left[\begin{array}{c} w_1 \\ w_2 \end{array}\right] \end{array} }$

を解けばよいと言えます（ $\ell_{11}=\ell_{21}=0$ 、 $\ell_{13}=\ell_{23}=0$ ）。