投稿者「cacsd」のアーカイブ

SISO BackStepping

投稿日時: 投稿者:

予備的考察

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.} }

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

●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制御（暫定版）

投稿日時: 投稿者:

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)} }

YMCBのOF-SM制御

投稿日時: 投稿者:

これまでの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)

投稿日時: 投稿者:

ここでは、 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制御

投稿日時: 投稿者:

ここでは、次の４種類の積分動作をもつスライディングモード制御（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制御

投稿日時: 投稿者:

ここでは、次の４種類の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

設計用モデルとして、データセット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の場合、LQIコントローラのロバスト性

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

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

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

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

第２の注意点は、上のシミュレーションでは各データセットのすべてのモデルを安定化できていませんが、設計用モデルとして、データセット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）。

YMCPBの特性解析

投稿日時: 投稿者:

YMCPBの線形モデル

●制御対象YMCPBの運動方程式として次式を考えます。

\displaystyle{ \begin{array}{ll} (1.1) & (M+M_x)\ddot{x}=D_b\cos\theta+(T+D_e)\cos(\theta+\theta_e)\\ &+N_L\sin\theta+N_e\sin(\theta+\theta_e)\\ (1.2)&(M+M_z)\ddot{z}=-D_b\sin\theta-(T+D_e)\sin(\theta+\theta_e)\\ &+N_L\cos\theta+N_e\cos(\theta+\theta_e)+N_B+Mg\\ &-c_{zz}\dot{z}-c_{z\theta}\dot{\theta}\\ (1.3)&(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{(1')\quad \begin{array}{l} \left[\begin{array}{ccc} M+M_x & 0 & 0 \\ 0 & M+M_z & 0\\ 0 & 0 & I_y+J_y \end{array}\right] \left[\begin{array}{c} \ddot{x}\\ \ddot{z}\\ \ddot{\theta} \end{array}\right]= \left[\begin{array}{ccc} 0 & 0 & 0 \\ 0 & c_{zz} & c_{z\theta}\\ 0 & c_{\theta z} & c_{\theta\theta} \end{array}\right] \left[\begin{array}{c} \dot{x}\\ \dot{z}\\ \dot{\theta} \end{array}\right]\\ +\left[\begin{array}{c} D_b\cos\theta+(T+D_e)\cos(\theta+\theta_e)\\ -D_b\sin\theta-(T+D_e)\sin(\theta+\theta_e)\\ D_b(H_{CG}-H_D)+T(H_T-H_{De}+H_e)+D_eH_e \end{array}\right]\\ +\left[\begin{array}{c} N_L\sin\theta+N_e\sin(\theta+\theta_e)\\ N_L\cos\theta+N_e\cos(\theta+\theta_e)\\ N_LL_L+N_eL_e \end{array}\right] + \left[\begin{array}{c} 0 \\ N_B+Mg\\ N_B(L_{CG}\cos\theta-L_B) \end{array}\right] \end{array} }

ここで、x,z,\thetaはそれぞれサージ、ヒーブ、ピッチを、T,\theta_eはそれぞれスラスト、船外機取付角を表しています。その他の物理パラメータの説明はここでは省略します。

●いま状態変数ベクトルと操作変数ベクトルをそれぞれ

\displaystyle{(2)\quad \xi=\left[\begin{array}{c} x\\ z\\ \theta\\\hline \dot{x}\\ \dot{z}\\ \dot{\theta} \end{array}\right],\ \zeta=\left[\begin{array}{c} T\\ \theta_e \end{array}\right] }

ととると、次の非線形状態方程式を得ます。

\displaystyle{(3.1)\quad \underbrace{\frac{d}{dt}\left[\begin{array}{c} x\\ z\\ \theta\\\hline \dot{x}\\ \dot{z}\\ \dot{\theta} \end{array}\right]}_{\dot{\xi}}= \underbrace{ \left[\begin{array}{c} f_1(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)\\ f_2(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)\\ f_3(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)\\\hline f_4(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)\\ f_5(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)\\ f_6(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e) \end{array}\right]}_{f(\xi,\zeta)} }

ただし

\displaystyle{(3.2)\quad \left\{\begin{array}{l} f_1(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)=\dot{x}\\ f_2(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)=\dot{z}\\ f_3(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)=\dot{\theta}\\ f_4(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)=\\ \quad\frac{1}{M+M_x}(D_b\cos\theta+(T+D_e)\cos(\theta+\theta_e)\\ \quad+N_L\sin\theta+N_e\sin(\theta+\theta_e))\\ f_5(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)=\\ \quad\frac{1}{M+M_z}(-Db\sin\theta-(T+D_e)\sin(\theta+\theta_e)\\ \quad+N_L\cos\theta+N_e\cos(\theta+\theta_e)+N_B+Mg\\ \quad-c_{zz}\dot{z}-c_{z\theta}\dot{\theta})\\ f_6(x,z,\theta,\dot{x},\dot{z},\dot{\theta},T,\theta_e)=\\ \quad\frac{1}{I_y+J_y}(D_b(H_{CG}-H_D)+T(H_T-H_{De}+H_e)\\ \quad+D_eH_e+N_LL_L+N_eL_e+N_B(L_{CG}\cos\theta-L_B)\\ \quad-c_{\theta z}\dot{z}-c_{\theta\theta}\dot{\theta}) \end{array}\right. }

●制御対象YMCPBに対する制御目的は、あるスラストT^*と取付角\theta_e^*の下で、巡航速度V^*で走行するとき、一定の姿勢z^*,\theta^*を保つこととします。これは、次の平衡状態\xi^*を、次の平衡入力\zeta^*によって保持することとみなすことができます。

\displaystyle{(4)\quad \xi^*=\left[\begin{array}{c} V^*t\\ z^*\\ \theta^*\\\hline V^*\\ 0\\ 0 \end{array}\right],\ \zeta^*=\left[\begin{array}{c} T^*\\ \theta_e^* \end{array}\right] }

この平衡状態と平衡入力を定めるためには、T^*とV^*を所与として、残りのz^*,\theta^*,\theta_e^*を次の非線形連立方程式を解いて求めます。

\displaystyle{(5)\quad \left\{\begin{array}{l} f_4(V^*t,z,\theta,V^*,0,0,T^*,\theta_e)=0\\ f_5(V^*t,z,\theta,V^*,0,0,T^*,\theta_e)=0\\ f_6(V^*t,z,\theta,V^*,0,0,T^*,\theta_e)=0 \end{array}\right. }

したがって、次式が成り立つことに注意します。

\displaystyle{(6)\quad f(\xi^*,\zeta^*)= \left[\begin{array}{c} V^*\\ 0\\ 0\\\hline 0\\ 0\\ 0 \end{array}\right]=\frac{d}{dt}\xi^* }

●この平衡状態\xi^*と平衡入力\zeta^*の回りで、非線形状態方程式\dot{\xi}=f(\xi,\zeta)を線形近似します。

\displaystyle{(7)\quad \dot{\xi}\simeq f(\xi^*,\zeta^*) +\underbrace{\left.\frac{\partial f(\xi,\zeta)}{\partial \xi}\right|_{\xi=\xi^*,\zeta=\zeta^*}}_{\frac{\partial f(\xi^*,\zeta^*)}{\partial \xi}}(\xi-\xi^*) +\underbrace{\left.\frac{\partial f(\xi,\zeta)}{\partial \zeta}\right|_{\xi=\xi^*,\zeta=\zeta^*}}_{\frac{\partial f(\xi^*,\zeta^*)}{\partial \zeta}}(\zeta-\zeta^*) }

ここで(6)に注意して、線形状態方程式

\displaystyle{(8)\quad \underbrace{\frac{d}{dt}(\xi-\xi^*)}_{\dot{x}}= \underbrace{\frac{\partial f(\xi^*,\zeta^*)}{\partial \xi}}_{A}\underbrace{(\xi-\xi^*)}_{x} +\underbrace{\frac{\partial f(\xi^*,\zeta^*)}{\partial \zeta}}_{B}\underbrace{(\zeta-\zeta^*)}_{u} }

すなわち

\displaystyle{(9)\quad \begin{array}{c} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} x(t)-V^*t\\ z(t)-z^*\\ \theta(t)-\theta^*\\\hline \dot{x}(t)-V^*\\ \dot{z}(t)\\ \dot{\theta}(t) \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{ccc|ccc} 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\\hline 0 & a_{42} & a_{43} & a_{44} & a_{45} & a_{46} \\ 0 & a_{52} & a_{53} & a_{54} & a_{55} & a_{56} \\ 0 & a_{62} & a_{63} & a_{64} & a_{65} & a_{66} \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} x(t)-V^*t\\ z(t)-z^*\\ \theta(t)-\theta^*\\\hline \dot{x}(t)-V^*\\ \dot{z}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)}\\ + \underbrace{ \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \\ 0 & 0 \\\hline b_{41} & b_{42} \\ b_{51} & b_{52} \\ b_{61} & b_{62} \end{array}\right] }_{B} \underbrace{ \left[\begin{array}{c} T(t)-T^*\\ \theta_e(t)-\theta_e^*\\ \end{array}\right] }_{u(t)} \end{array}}

を得ます。

●次の５組のデータセットが与えられています。

艇 艇質量[kg] 長手方向重心位置[m] 備考
No1 SPTM247 2709 1.9756 INM抵抗試験レポートC2620-07CT18-RAP01による
No2 SPTM247 2813 1.925 2021/8/2重量重心測定結果より
No3 SPTM247 2845 1.937 2022/1/19重量重心測定結果より
No4 SPTM247 2945 1.937 No3重量に+100kg
No5 SPTM247 2745 1.937 No3重量に-100kg

各データセットは、133個（k=1,\cdots,133）の平衡状態と平衡入力とA行列を含みます（B行列とC行列は固定）。これらは、たとえば、データセットNo1に対しては図1のようにグラフ化されます。


図1 データセットNo1における平衡状態

ここで、赤い〇の平衡状態は、制御系の設計および評価のために選ばれた、次表に示す39点です。k=128の場合の平衡状態回りの線形モデルについてコントローラを設計します。

-\theta_e^* run k
-4 deg run1 8,9,10,11,12,13,14
0 deg run1 36,37,38,39,40,41,42,43
4 deg run1 66,67,68,69,70,71,72,73
8 deg run1 96,97,98,99,100,101,102,103
2 deg run1 126,127,128,129,130,131,132,133

 

またデータセットNo1に対する行列Aの固有値は図2のようにグラフ化されます。


図2 データセットNo1における行列Aの固有値

各線形モデルに対して複素固有値が２対、実固有値が２個（うち１個は零）あることがわかります。不安定な複素固有値はピッチの振舞いに、安定な複素固有値はヒーブの振舞いに、非零実固有値は速度変動に関係しています。速度変動が発散する場合があることが気になるところです。

制御系設計用線形モデル

●制御系設計用線形モデルを得るために、まずアクチュエータについては、船外機の取付角を操作するためにレバーを引いた期間だけ一定の角速度\omega_eで回転するものとします。これを次式で表します。

\displaystyle{(10)\quad \dot{\theta}_e(t)=\omega_e u_e(t)\quad(u_e(t)=0,\pm 1) }

ここで、u_eは操作入力u(t)が正のとき+1、負のとき-1の値をとるものとします。すなわち

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

したがって、制御対象の状態方程式(9)とアクチュエータ(10)を合わせて、次式を得ます。

\displaystyle{(12)\quad \begin{array}{c} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} x(t)-V^*t\\ z(t)-z^*\\ \theta(t)-\theta^*\\\hline \dot{x}(t)-V^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\\hline \theta_e(t)-\theta_e^* \end{array}\right] }_{\dot{x}_a(t)} = \underbrace{ \left[\begin{array}{ccc|ccc|c} 0 & 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 & 0\\\hline 0 & a_{42} & a_{43} & a_{44} & a_{45} & a_{46} & b_{42}\\ 0 & a_{52} & a_{53} & a_{54} & a_{55} & a_{56} & b_{52}\\ 0 & a_{62} & a_{63} & a_{64} & a_{65} & a_{66} & b_{62}\\\hline 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right] }_{A_a} \underbrace{ \left[\begin{array}{c} x(t)-V^*t\\ z(t)-z^*\\ \theta(t)-\theta^*\\\hline \dot{x}(t)-V^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\\hline \theta_e(t)-\theta_e^* \end{array}\right] }_{x_a(t)}\\ + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ 0 \\\hline 0 \\ 0 \\ 0 \\\hline \omega_e \end{array}\right] }_{B_a}u_e(t) + \left[\begin{array}{c} 0 \\ 0 \\ 0 \\\hline b_{41} \\ b_{51} \\ b_{61} \\\hline 0 \end{array}\right]\underbrace{(T(t)-T^*)}_{0} \end{array}}

また状態変数のうち、\theta、\dot{\theta}、\theta_eが計測できるものとします。このとき、状態方程式(12)に対する観測方程式は次式で表されます。

\displaystyle{(13)\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}{ccc|ccc|c} 0 & 0 & 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} x(t)-V^*t\\ z(t)-z^*\\ \theta(t)-\theta^*\\\hline \dot{x}(t)-V^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\\hline \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)} \end{array}}

●制御対象YMCPBに対する制御目的は、あるスラストT^*と取付角\theta_e^*の下で、巡航速度V^*で走行するとき、一定の姿勢z^*,\theta^*を保つこととします。この制御目的に照らして、サージ方向の状態変数は省いてよく、また操作変数も船外機取付角に限定することができます。ヒーブ方向の状態変数を残すか省くかによって、次の２種類の線形モデルが考えられます。

５次元モデル

\displaystyle{(14.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_e(t) + \left[\begin{array}{c} 0 \\ 0 \\ a_{54} \\ a_{64} \\ 0 \end{array}\right] \underbrace{(\dot{x}(t)-V^*)}_{0} + \left[\begin{array}{c} 0 \\ 0 \\ b_{51} \\ b_{61} \\ 0 \end{array}\right]\underbrace{(T(t)-T^*)}_{0} \end{array}}

\displaystyle{(14.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} \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}}

データセットNo1に対する行列Aの固有値は図3のようにグラフ化されます。ここで、安定な固有値はヒーブの振舞いに、不安定な固有値はピッチの振舞いに関係しています。


図3 データセットNo1を用いた５次元モデルの行列Aの固有値

３次元モデル

\displaystyle{(15.1)\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}{c} 0 \\ 0 \\ \omega_e \end{array}\right] }_{B} u_e(t)\\ + \underbrace{\left[\begin{array}{cc} 0 & 0 \\ a_{62} & a_{65} \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} z(t)-z^*\\ \dot{z}(t) \end{array}\right]}_{w(t)} + \left[\begin{array}{cc} 0 & 0 \\ a_{64} & b_{61} \\ 0 & 0 \end{array}\right] \underbrace{\left[\begin{array}{c} \dot{x}(t)-V^* \\ T(t)-T^* \end{array}\right]}_{0} \end{array}}

\displaystyle{(15.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} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)} \end{array}}

データセットNo1に対する行列Aの固有値は図4のようにグラフ化されます。これからピッチに対応する固有値は不安定であることが分かります。


図4 データセットNo1を用いた３次元モデルの行列Aの固有値

●制御目的を達成する制御系を設計する立場から、２つのモデルを比較してみます。まず制御則として次式を考えます。

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

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

５次元モデル(14.1)の場合は、ヒーブに関する状態変数を計測できないので、状態オブザーバを用いてこれらを推定して、状態フィードバックを実施することになります。

一方３次元モデル(15.1)の場合は、状態フィードバックは実施できますが、ヒーブの振舞いの影響が外乱w(t)として現れますので、これを抑制する必要があります。そのヒーブの振舞いは次式で表されます。

\displaystyle{(17)\quad \begin{array}{l} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} z(t)-z^*\\ \dot{z}(t) \end{array}\right] }_{\dot{x}'(t)} = \underbrace{ \left[\begin{array}{ccccc} 0 & 1 \\ a_{52} & a_{55} \end{array}\right] }_{A'} \underbrace{ \left[\begin{array}{c} z(t)-z^*\\ \dot{z}(t)\\ \end{array}\right] }_{x'(t)}\\ + \underbrace{ \left[\begin{array}{ccccc} 0 & 1 & 0\\ a_{53} & a_{56} & b_{52} \end{array}\right] }_{B'} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{y(t)}\\ + \left[\begin{array}{c} 0 \\ a_{54} \end{array}\right] \underbrace{(\dot{x}(t)-V^*)}_{0} + \left[\begin{array}{c} 0 \\ b_{51} \end{array}\right]\underbrace{(T(t)-T^*)}_{0} \end{array}}

これからヒーブの振舞いには操作入力u_eは直接影響を及ぼさず、ピッチを通して間接的に影響すること分かります。ただ、これは漸近安定（A'は安定行列）なので、ピッチの安定化を速やかにできれば（y(t)が零に収束すれば）、(17)は考慮しなくともよいことになります。しかしながら、３次元モデルに対する状態フィードバックを５次元モデルに適用して、閉ループ系が漸近安定かどうかを調べておくことが考えられます。

制御系設計に３次元モデルを用いる場合は、状態オブザーバが不要となる利点がありますが、ヒーブの振舞いの影響が抑制されていることを確かめるために、５次元モデルに対する閉ループシミュレ－ションを行うことが重要になります。

●以上の結果を、k=128の場合の平衡状態回りの線形モデルについて数値で確認しておきます。

まず元の線形モデル(12)の行列Aとその固有値は次のようになります。

\displaystyle{(18.1)\quad A= \left[\begin{array}{cccccc} 0 & 0 & 0 & 1.0000 & 0 & 0\\ 0 & 0 & 0 & 0 & 1.0000 & 0\\ 0 & 0 & 0 & 0 & 0 & 1.0000\\ 0 & 8.3458 & -6.9684 & -0.2549 & 0 & 0\\ 0 & -58.4515 & 67.9416 & 0.4686 & -0.7383 & -0.0369\\ 0 & 0.4559 & -30.3422 & -0.1922 & -0.0117 & 0.1175 \end{array}\right] }

\displaystyle{(18.2)\quad \left\{\begin{array}{lr} \lambda_1=& 0.0000 + 0.0000i\\ \lambda_2=& -0.2052 + 0.0000i\\ \lambda_3=& -0.4030 + 7.7086i\\ \lambda_4=& -0.4030 - 7.7086i\\ \lambda_5=& 0.0678 + 5.4080i\\ \lambda_6=& 0.0678 - 5.4080i \end{array}\right.}

５次元モデル(14.1)における行列Aとその固有値は次のようになります。

\displaystyle{(19.1)\quad A= \left[\begin{array}{ccccc} 0 & 0 & 1.0000 & 0 & 0\\ 0 & 0 & 0 & 1.0000 & 0\\ -58.4515 & 67.9416 & -0.7383 & -0.0369 & -0.0797\\ 0.4559 & -30.3422 & -0.0117 & 0.1175 & -2.7873\\ 0 & 0 & 0 & 0 & 0 \end{array}\right] }

\displaystyle{(19.2)\quad \left\{\begin{array}{lrc} \lambda_1^{(5)}=& -0.3412 + 7.7012i&\approx\lambda_3\\ \lambda_2^{(5)}=& -0.3412 - 7.7012i&\approx\lambda_4\\ \lambda_3^{(5)}=& 0.0308 + 5.4151i&\approx\lambda_5\\ \lambda_4^{(5)}=& 0.0308 - 5.4151i&\approx\lambda_6\\ \lambda_5^{(5)}=& 0.0000 + 0.0000i& \end{array}\right.}

これからサージに関わる状態変数を除いた場合、ヒーブとピッチに関わる固有値\lambda_3,\lambda_4,\lambda_5,\lambda_6がほぼ引き継がれることが分かります。

一方３次元モデル(15.1)における行列Aとその固有値は次のようになります。

\displaystyle{(20.1)\quad A= \left[\begin{array}{ccc} 0 & 1.0000 & 0\\ -30.3422 & 0.1175 & -2.7873\\ 0 & 0 & 0 \end{array}\right] }

\displaystyle{(20.2)\quad \left\{\begin{array}{lrc} \lambda_1^{(3)}=& 0.0587 + 5.5081i&\approx\lambda_5\\ \lambda_2^{(3)}=& 0.0587 - 5.5081i&\approx\lambda_6\\ \lambda_3^{(3)}=& 0.0000 + 0.0000i&\\ \end{array}\right.}

これからさらにヒーブに関わる状態変数を除いた場合、ピッチに関わる固有値\lambda_5,\lambda_6がほぼ引き継がれることが分かります。

●ヒーブとピッチに関わる固有値\lambda_3,\lambda_4,\lambda_5,\lambda_6の可制御性と可観測性を、５次元モデルを用いて調べてみます。

\displaystyle{(21)\quad \left\{\begin{array}{l} \underline{\sigma}(\left[\begin{array}{cc} B & A-\lambda_1^{(5)} I_5 \end{array}\right])=0.0048^{*}\\ \underline{\sigma}(\left[\begin{array}{cc} B & A-\lambda_2^{(5)} I_5 \end{array}\right])=0.0048^{*}\\ \underline{\sigma}(\left[\begin{array}{cc} B & A-\lambda_3^{(5)} I_5 \end{array}\right])=0.0106\\ \underline{\sigma}(\left[\begin{array}{cc} B & A-\lambda_4^{(5)} I_5 \end{array}\right])=0.0106\\ \underline{\sigma}(\left[\begin{array}{cc} B & A-\lambda_5^{(5)} I_5 \end{array}\right])=0.1140\\ \end{array}\right. }

\displaystyle{(22)\quad \left\{\begin{array}{l} \underline{\sigma}(\left[\begin{array}{cc} C^T & A^T-\lambda_1^{(5)} I_5 \end{array}\right])=0.0146^{*}\\ \underline{\sigma}(\left[\begin{array}{cc} C^T & A^T-\lambda_2^{(5)} I_5 \end{array}\right])=0.0146^{*}\\ \underline{\sigma}(\left[\begin{array}{cc} C^T & A^T-\lambda_3^{(5)} I_5 \end{array}\right])=0.3129\\ \underline{\sigma}(\left[\begin{array}{cc} C^T & A^T-\lambda_4^{(5)} I_5 \end{array}\right])=0.3129\\ \underline{\sigma}(\left[\begin{array}{cc} C^T & A^T-\lambda_5^{(5)} I_5 \end{array}\right])=0.9940\\ \end{array}\right. }

(21)から、すべての固有値について行列\left[\begin{array}{cc} B & A-\lambda I_5 \end{array}\right]の最小特異値は正ですから、形式的には可制御性は成り立つと判定できます。ただ、＊印の値が示すように、ヒーブはピッチに比べて相対的に可制御性の程度が弱く、状態フィードバックによる固有値の変更が難しいと言えます。したがって、制御対象は可制御でなく、むしろ可安定と判定することが考えられます。

一方、(22)から、形式的には可観測性は成り立つと判定できます。ただ、＊印の値が示すように、ヒーブはピッチに比べて相対的に可観測性の程度が弱く、状態オブザーバによる推定が難しいと言えます。

●操作入力（取付角）により、まずピッチが制御され、それがヒーブに影響を与えるという連成の仕組みを調べるために、(15.1)と(17)を数値で確かめてみます。

まず(15.1)は

\displaystyle{(15.1')\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}\\ -30.3422 & 0.1175 & -2.7873\\ 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}{c} 0 \\ 0 \\ \omega_e \end{array}\right] }_{B} u_e(t) + \underbrace{\left[\begin{array}{cc} 0 & 0 \\ %a_{62} & a_{65} \\ 0.4559 & -0.0117 \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} z(t)-z^*\\ \dot{z}(t) \end{array}\right]}_{w(t)} \end{array}}

となり、ヒーブのピッチへの影響を表すw(t)に関わる係数a_{62}=0.4559,a_{65}=-0.017の値がそれほど大きくなく、３次元モデルにおいても固有値が継承されることに注意します。

一方(17)は

\displaystyle{(17')\quad \begin{array}{l} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} z(t)-z^*\\ \dot{z}(t) \end{array}\right] }_{\dot{x}'(t)} = \underbrace{ \left[\begin{array}{ccccc} 0 & 1 \\ %a_{52} & a_{55} -58.4515 & -0.7383 \end{array}\right] }_{A'} \underbrace{ \left[\begin{array}{c} z(t)-z^*\\ \dot{z}(t)\\ \end{array}\right] }_{x'(t)}\\ + \underbrace{ \left[\begin{array}{ccccc} 0 & 1 & 0\\ %a_{53} & a_{56} & b_{52} 67.9416 & -0.0369 & -0.0797 \end{array}\right] }_{B'} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{y(t)} \end{array}}

となります。これは漸近安定なので、ピッチの安定化が速やかに行えれば問題ないとと言えます。

●最後に、取付角からの伝達特性を調べておきます。


図5 取付角からヒーブまで（左図）とピッチまで（右図）の伝達特性

左図は２つの共振特性をもちますが、右図は（２つ目がほとんど消えていて）１つだけの共振特性をもちます。これから、ヒーブのピッチへの連成影響はほとんどないことがわかります。

●以上の数値例の結果を得るためのプログラムを以下に示します。

MATLAB 
%ymcpb_ana
%-----
 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];
 id=1:length(ID);
%-----
 figure(1)
 subplot(321),plot(run_eq_Series,"o-"), hold on
 subplot(321),plot(ID,run_eq_Series(ID),"or"),title("No"+num2str(no)+": "+"run")
 subplot(323),plot(No_eq_Series,"o-"), hold on
 subplot(323),plot(ID,No_eq_Series(ID),"or"),title("No"+num2str(no)+": "+"no")
 subplot(325),plot(dx_eq_Series*3.6,"o-"), hold on
 subplot(325),plot(ID,dx_eq_Series(ID)*3.6,"or"),title("No"+num2str(no)+": "+"dxs")
 subplot(322),plot(z_eq_Series*100,"o-"), hold on
 subplot(322),plot(ID,z_eq_Series(ID)*100,"or"),title("No"+num2str(no)+": "+"zs")
 subplot(324),plot(th_eq_Series/pi*180,"o-"), hold on
 subplot(324),plot(ID,th_eq_Series(ID)/pi*180,"or"),title("No"+num2str(no)+": "+"ths")
 subplot(326),plot(the_eq_Series/pi*180,"o-"), hold on
 subplot(326),plot(ID,the_eq_Series(ID)/pi*180,"or"),title("No"+num2str(no)+": "+"thes")
%-----
 figure(2)
 for i=id, a41(i)=A0(4,1,ID(i)); end
 subplot(331),plot(id,a41,"o-",33,a41(33),"*r"),title("No"+num2str(no)+": "+"a41")
 for i=id, a42(i)=A0(4,2,ID(i)); end
 subplot(332),plot(id,a42,"o-",33,a42(33),"*r"),title("No"+num2str(no)+": "+"a42")
 for i=id, a43(i)=A0(4,3,ID(i)); end
 subplot(333),plot(id,a43,"o-",33,a43(33),"*r"),title("No"+num2str(no)+": "+"a43")
 for i=id, a51(i)=A0(5,1,ID(i)); end
 subplot(334),plot(id,a51,"o-",33,a51(33),"*r"),title("No"+num2str(no)+": "+"a51")
 for i=id, a52(i)=A0(5,2,ID(i)); end
 subplot(335),plot(id,a52,"o-",33,a52(33),"*r"),title("No"+num2str(no)+": "+"a52")
 for i=id, a53(i)=A0(5,3,ID(i)); end
 subplot(336),plot(id,a53,"o-",33,a53(33),"*r"),title("No"+num2str(no)+": "+"a53")
 for i=id, a61(i)=A0(6,1,ID(i)); end
 subplot(337),plot(id,a61,"o-",33,a61(33),"*r"),title("No"+num2str(no)+": "+"a61")
 for i=id, a62(i)=A0(6,2,ID(i)); end
 subplot(338),plot(id,a62,"o-",33,a62(33),"*r"),title("No"+num2str(no)+": "+"a62")
 for i=id, a63(i)=A0(6,3,ID(i)); end
 subplot(339),plot(id,a63,"o-",33,a63(33),"*r"),title("No"+num2str(no)+": "+"a63")
%-----
 figure(3)
 for i=id, a44(i)=A0(4,4,ID(i)); end
 subplot(331),plot(id,a44,"o-",33,a44(33),"*r"),title("No"+num2str(no)+": "+"a44")
 for i=id, a45(i)=A0(4,5,ID(i)); end
 subplot(332),plot(id,a45,"o-",33,a45(33),"*r"),title("No"+num2str(no)+": "+"a45")
 for i=id, a46(i)=A0(4,6,ID(i)); end
 subplot(333),plot(id,a46,"o-",33,a46(33),"*r"),title("No"+num2str(no)+": "+"a46")
 for i=id, a54(i)=A0(5,4,ID(i)); end
 subplot(334),plot(id,a54,"o-",33,a54(33),"*r"),title("No"+num2str(no)+": "+"a54")
 for i=id, a55(i)=A0(5,5,ID(i)); end
 subplot(335),plot(id,a55,"o-",33,a55(33),"*r"),title("No"+num2str(no)+": "+"a55")
 for i=id, a56(i)=A0(5,6,ID(i)); end, axis([0 40 -1 1])
 subplot(336),plot(id,a56,"o-",33,a56(33),"*r"),title("No"+num2str(no)+": "+"a56")
 for i=id, a64(i)=A0(6,4,ID(i)); end, axis([0 40 -1 1])
 subplot(337),plot(id,a64,"o-",33,a64(33),"*r"),title("No"+num2str(no)+": "+"a64")
 for i=id, a65(i)=A0(6,5,ID(i)); end, axis([0 40 -1 1])
 subplot(338),plot(id,a65,"o-",33,a65(33),"*r"),title("No"+num2str(no)+": "+"a65")
 for i=id, a66(i)=A0(6,6,ID(i)); end
 subplot(339),plot(id,a66,"o-",33,a66(33),"*r"),title("No"+num2str(no)+": "+"a66")
%-----
 figure(4)
 for i=id, b41(i)=B0(4,1,ID(i)); end
 subplot(321),plot(id,b41,"o-",33,b41(33),"*r"),title("No"+num2str(no)+": "+"b41")
 for i=id, b42(i)=B0(4,2,ID(i)); end
 subplot(322),plot(id,b42,"o-",33,b42(33),"*r"),title("No"+num2str(no)+": "+"b42")
 for i=id, b51(i)=B0(5,1,ID(i)); end
 subplot(323),plot(id,b51,"o-",33,b51(33),"*r"),title("No"+num2str(no)+": "+"b51")
 for i=id, b52(i)=B0(5,2,ID(i)); end
 subplot(324),plot(id,b52,"o-",33,b52(33),"*r"),title("No"+num2str(no)+": "+"b52")
 for i=id, b61(i)=B0(6,1,ID(i)); end
 subplot(325),plot(id,b61,"o-",33,b61(33),"*r"),title("No"+num2str(no)+": "+"b61")
 for i=id, b62(i)=B0(6,2,ID(i)); end
 subplot(326),plot(id,b62,"o-",33,b62(33),"*r"),title("No"+num2str(no)+": "+"b62")
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
 figure(5),pzmap(sys),axis([-10,10,-10,10])
 title("No"+num2str(no)+": "+"k=ID"),hold on
 figure(5),pzmap(sys(:,:,33),"*r")
%-----５次元モデル
%(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 
 B=[zeros(4,1);0.0380*3];
 C=eye(5,5);
 C=C(i,:);
 sys5=ss(A(:,:,ID),B,C,[]);  
 figure(6),pzmap(sys5),axis([-10,10,-10,10])
 title("No"+num2str(no)+": "+"k=ID"),hold on
 figure(6),pzmap(sys5(:,:,33),"*r")
%-----３次元モデル
%(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 
 B=[zeros(2,1);0.0380*3];
 C=eye(3,3);
 sys3=ss(AA(:,:,ID),B,C,[]);  
 figure(7),pzmap(sys3),axis([-10,10,-10,10])
 title("No"+num2str(no)+": "+"k=ID"),hold on
 figure(7),pzmap(sys3(:,:,33),"*r")
%-----設計ポイント id=128,ID=33
 [Asys,Bsys]=ssdata(sys(:,:,33))
 [Asys5,Bsys5,Csys5]=ssdata(sys5(:,:,33))
 [Asys3,Bsys3,Csys3]=ssdata(sys3(:,:,33))
 pl=pole(sys(:,:,33))
 pl5=pole(sys5(:,:,33))
 pl3=pole(sys3(:,:,33))
%-----可制御性 
 w=zeros(5,1);
 for i=1:5
   S=[Bsys5 Asys5-pl5(i)*eye(5,5)];
   w(i)=min(svd(S));
 end
 C0=[pl5 w]
%-----可観測性 
 w=zeros(5,1);
 for i=1:5
   S=[Csys5; Asys5-pl5(i)*eye(5,5)];
   w(i)=min(svd(S));
 end
 OB=[pl5 w]
%-----伝達特性
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 figure(11),hold on
 sigma(sys(:,2,33),logspace(0,2))
 title("No"+num2str(no)+": "+"sys(:,2,33)")
%-----
%(2)z,(5)dz
 figure(12),hold on
 sigma(sys([2,5],2,33),logspace(0,2))
 title("No"+num2str(no)+": "+"sys([2,5],2,33)")
%-----
%(3)th,(6)dth
 figure(13),hold on
 sigma(sys([3,6],2,33),logspace(0,2))
 title("No"+num2str(no)+": "+"sys([3,6],2,33)")
%-----
%eof

参考：漸近安定性

【漸近安定性の定義とその等価な条件】
定義DA:　\forall x(0)\ne 0: x(t)=\exp(At)x(0)\rightarrow 0\quad(t\rightarrow\infty)
条件A1:　{\rm Re}(\lambda_i(A))<0\quad(i=1,\cdots,n)

漸近安定性とは、定義DAより、平衡状態が乱されたとき復帰できるかどうかにかかわる概念で、条件A1より、A行列の固有値がすべて複素左半面にあることを調べて判定されます。

参考：可制御性・可安定性

【可安定性の定義とその等価な条件】
定義DS:　状態フィードバックにより安定化可能
条件S1:　{\rm rank}\, \left[\begin{array}{cc} B & A-\lambda I_n \end{array}\right] =n　(\lambdaはAのすべての不安定固有値)

【可制御性の定義とその等価な条件】
定義DC:　任意初期状態を，任意有限時間内に，任意状態に移動可能
条件C2:　{\rm rank}\, \left[\begin{array}{cccc} B & AB & \cdots & A^{n-1}B \end{array}\right]=n
条件C3:　Fを選んで，A-BFの固有値を任意に設定可能
条件C4:　\boxed{{\rm rank}\, \left[\begin{array}{cc} B & A-\lambda I_n \end{array}\right] =n}　(\lambdaはAのすべての固有値)

可安定性は、定義DSより、状態フィードバックにより安定化可能であることを意味し、条件S1が判定条件として知られています。一方、可制御性については、条件C4が判定条件として知られています。両者の相違は、すでに左半平面にある安定な固有値について関与（再配置）するかどうかにあります。可安定性の判定では不安定な固有値のみで条件S1を判定しますが、可制御性の判定ではすべて固有値について条件C4を判定します。

それでは、\lambda_1,\cdots,\lambda_nに対して、条件S1と条件C4に出てくる行列

\displaystyle{(3)\quad M(\lambda_i)=\left[\begin{array}{cc} B & A-\lambda_i I_n \end{array}\right] }

の階数をどのようにして計算するかですが、これはMの非零特異値の数で決定します。したがって、条件S1と条件C4は、最小特異値が正かどうかで判定します。

参考：可観測性・可検出性

【可検出性の定義とその等価な条件】
定義DD:　状態オブザーバを構成可能
条件D1:　{\rm rank}\, \left[\begin{array}{c} C \\ A-\lambda I_n \end{array}\right]=n　(\lambdaはAのすべての不安定固有値)

【可観測性の定義とその等価な条件】
定義DO:　任意有限時間[0,t]上の入出力データu(\tau),y(\tau)\tau\in[0,t]から，初期状態x(0)を一意に決定できる
条件O2:　{\rm rank}\, \left[\begin{array}{c} C \\ CA \\ \vdots\\ CA^{n-1} \end{array}\right] =n
条件O3:　Hを選んで，A-HCの固有値を任意に設定可能
条件O4:　\boxed{{\rm rank} \left[\begin{array}{c} C \\ A-\lambda I_n \end{array}\right]=n}　(\lambdaはAのすべての固有値)

可検出性は、定義DDより、状態オブザーバを構成可能であることを意味し、条件D1が判定条件として知られています。状態オブザーバのA行列はA-HCと表され、オブザーバゲインHは、仮想システム\dot{v}=A^Tv+C^Twに対する状態ＦＢw=-H^Tvによる閉ループ系\dot{v}=(A-HC)^Tvを安定化して決定します。したがって、可検出性は\dot{v}=A^Tv+C^Twの可安定性を意味します。一方、可観測性については、条件O4が判定条件として知られています。可検出性との相違は、すでに左半平面にある安定なAの固有値についてA-HCで再配置するかどうかにあります。可検出性の判定では不安定な固有値のみで条件D1を判定しますが、可観測性の判定ではすべて固有値について条件O4を判定します。

それでは、\lambda_1,\cdots,\lambda_nに対して、条件D1と条件O4に出てくる行列

\displaystyle{(4)\quad N(\lambda_i)=\left[\begin{array}{c} C \\ A-\lambda I_n \end{array}\right] }

の階数をどのようにして計算するかですが、これはNの非零特異値の数で決定します。したがって、条件D1と条件O4は、最小特異値が正かどうかで判定します。

参考：状態オブザーバ

●通常の状態オブザーバは

\displaystyle{(7)\quad \dot{\hat{x}}(t)=(A-HC)\hat{x}(t)+Hy(t)+Bu(t) }

で表され

\displaystyle{(8)\quad \hat{x}(t)\rightarrow x(t)\quad(t\rightarrow\infty) }

のように漸近的に元の状態を推定するものです。行列Hはオブザーバゲインと呼ばれ、A-HCは安定行列とするように選ばれます。

●最小次元状態オブザーバは

\displaystyle{(9)\quad \left\{\begin{array}{lll} \dot{\hat{z}}(t)=\hat{A}\hat{z}(t)+\hat{B}y(t)+\hat{J}u(t),\ \hat{z}(0)=0\\ \hat{x}(t)=\hat{C}\hat{z}(t)+\hat{D}y(t)\\ (\hat{z}(t)\in{\rm\bf R}^{n-p}, y(t)\in{\rm\bf R}^p, u(t)\in{\rm\bf R}^m) \end{array}\right. }

によって表される漸近安定なシステムで、この出力がx(t)に漸近するように構成するものです。そのためには、あるサイズn-p \times nの行列Uに対して次式を満足させることが必要十分となります。

\displaystyle{ \begin{array}{cl} (10.1) & \hat{A}U+\hat{B}C=UA\\ (10.2) & \hat{C}U+\hat{D}C=I_n\\ (10.3) & \hat{J}=UB \end{array} }

参考：最小実現

一般に，不可制御かつ不可観測な状態空間表現は適当な座標変換により，つぎの正準構造をもつように変換できることが知られています。

\displaystyle{(1)\quad \left[\begin{array}{c|c} TAT^{-1} & TB \\\hline CT^{-1} & D \end{array}\right] = \left[\begin{array}{cccc|c} A_1 & 0 & X_{13} & 0 & B_1 \\ X_{21} & A_2 & X_{23} & X_{24} & B_2 \\ 0 & 0 & A_3 & 0 & 0 \\ 0 & 0 & X_{43} & A_4 & 0 \\\hline C_1 & 0 & C_3 & 0 & 0 \end{array}\right] }

ここで，正方行列A_1，A_2，A_3，A_4の次数は一意に定まり，(A_1,B_1)は可制御対，(A_1,C_1)は可観測対です。この正準構造のブロック線図を図1に示します。


図1 正準構造のブロック線図

さて，つぎが成り立ちます。

\displaystyle{(2)\quad \hat{G}(s)=C(sI_n-A)^{-1}B=C_1(sI_n-A_1)^{-1}B_1 }

すなわち，可制御かつ可観測な部分系が入力から出力までの伝達特性を表しています。

再計画

投稿日時: 投稿者:

これを得るプログラムを、本資料では次のように分割して示しています。

　プログラムIII　************************ 再計画 ************************
・プログラムIII-1　リソース（場所）の定義
・プログラムIII-2　リソース（作業員）の定義
・プログラムIII-3　再計画アクティビティ（仕掛作業、未着手作業）の定義
　　　　　　　　　未着手作業の先行制約の設定
・プログラムIII-4　モードの設定
　　　　　　　　　仕掛作業のモード継承
　　　　　　　　　未着手作業の資源制約（配員）
　　　　　　　　　未着手作業の資源制約（作業場所）
・プログラムIII-5　RCPSP求解

プログラム：Part III

#プログラムIII-1.0
from optseq import *
import math
#=======================================================================
print("")
print(' ************************ リスケ on rdate **********************')
import dill
dill.load_session('usukiR040510b.pkl')  
#=======================================================================
usuki3=Model()
CS=["CS","CC","SS","SC"]

プログラムIII-1

#プログラムIII-1
#=======================================================================
#リソースの定義（場所） 
#=======================================================================
#-----メッシュの使用可否
resPL={}
for id in R:
  resPL[id]=0
  if not id in [4,5,6,13]:
    for i in range(0,R[id][1]):
      for j in range(0,R[id][2]):        
        resPL[id,i,j] =usuki3.addResource(R[id][0]+"[{0:02d}_{1:02d}]".format(i,j), capacity={(0,"inf"):1}) 
  if id==4:
    for i in range(0,R[id][1]):
      for j in range(0,R[id][2]):  
        if i in  range(0,11) and j in range(0,12):
          resPL[id,i,j] =usuki3.addResource(R[id][0]+"[{0:02d}_{1:02d}]".format(i,j), capacity={(0,"inf"):0})
        else:     
          resPL[id,i,j] =usuki3.addResource(R[id][0]+"[{0:02d}_{1:02d}]".format(i,j), capacity={(0,"inf"):1})
  if id==5:
    for i in range(0,R[id][1]):
      for j in range(0,R[id][2]):  
        if i in  range(0,7) and j in range(0,10):
          resPL[id,i,j] =usuki3.addResource(R[id][0]+"[{0:02d}_{1:02d}]".format(i,j), capacity={(0,"inf"):0})
        else:     
          resPL[id,i,j] =usuki3.addResource(R[id][0]+"[{0:02d}_{1:02d}]".format(i,j), capacity={(0,"inf"):1})
  if id==6:       
    for i in range(0,R[id][1]):
      for j in range(0,R[id][2]):  
        if i in [0,22,23,24,25,26,27,28,29,30,31,32,33,34,35] and j in range(0,13):
          resPL[id,i,j] =usuki3.addResource(R[id][0]+"[{0:02d}_{1:02d}]".format(i,j), capacity={(0,"inf"):0})
        elif i in range(1,17) and j in range(0,1):
          resPL[id,i,j] =usuki3.addResource(R[id][0]+"[{0:02d}_{1:02d}]".format(i,j), capacity={(0,"inf"):0})            
        else:     
          resPL[id,i,j] =usuki3.addResource(R[id][0]+"[{0:02d}_{1:02d}]".format(i,j), capacity={(0,"inf"):1})
  if id==13:       
    for i in range(0,R[id][1]):
      for j in range(0,R[id][2]):  
        if i in [31,32,33,34,35,36,37,63,64,65,66,67,68,69,70,71] and j in range(0,24):
          resPL[id,i,j] =usuki3.addResource(R[id][0]+"[{0:02d}_{1:02d}]".format(i,j), capacity={(0,"inf"):0})         
        else:     
          resPL[id,i,j] =usuki3.addResource(R[id][0]+"[{0:02d}_{1:02d}]".format(i,j), capacity={(0,"inf"):1})

プログラムIII-2

#プログラムIII-2
#=======================================================================
#リソースの定義（作業員）
#=======================================================================
eps1=1.25
resWH={}    #取付28人
r=math.floor(28*4*eps1);
resWH=usuki3.addResource("R[WH]", capacity={(0,"inf"):r})
#-----
resWW={}    #溶接43人
r=math.floor(43*4*eps1);
resWW=usuki3.addResource("R[WW]", capacity={(0,"inf"):r})
#-----
eps2=1.25
resWF1={}   #鉄艤5人
r=math.floor(5*4*eps2);
resWF1=usuki3.addResource("R[WF1]", capacity={(0,"inf"):r})
#-----
resWF2={}   #甲配7人
r=math.floor(7*4*eps2);
resWF2=usuki3.addResource("R[WF2]", capacity={(0,"inf"):r})
#-----
resWF3={}   #機配5人⇒10人
r=math.floor(10*4*eps2);
resWF3=usuki3.addResource("R[WF3]", capacity={(0,"inf"):r})
#-----
resWP={}    #塗装内4人⇒8人
r=math.floor(8*4*eps2);
resWP=usuki3.addResource("R[WP]", capacity={(0,"inf"):r})
#-----
resWP2={}   #塗装外8人
r=math.floor(8*4*eps2);
resWP2=usuki3.addResource("R[WP2]", capacity={(0,"inf"):r})

プログラムIII-3

#プログラムIII-3.1
#=======================================================================
#再計画 アクティビティ idata3
#=======================================================================
rdate=179
w1=[]; w2=[]; w3=[];
idata12=idata1.union(idata2)
for i in idata12:
  if data[i][3]+data[i][2] < rdate: w1.append(i)
  if data[i][3] <= rdate  and  rdate <= data[i][3]+data[i][2]: w2.append(i)
  if rdate < data[i][3]: w3.append(i)
idata3a=w2;   #仕掛かり中のアクティビティ
idata3b=w3;   #仕掛かっていないアクティビティ 
idata3=w2+w3; #リスケすべきアクティビティ 
print(idata3) 

#プログラムIII-3.2
#=======================================================================
#再計画 作業の定義
#=======================================================================
act3={}
for i in idata3:    
  j=i  
  while data[j][1]!=0:
    j=data[j][4][0]
  act3[i]=usuki3.addActivity(data[i][0], duedate=data[j][3]+data[j][2], \
                                                         backward=True)

#プログラムIII-3.3
#=======================================================================
#再計画 先行制約
#======================================================================= 
for i in idata3b:
  if data[i][1] in ACT_gisou+ACT_kumitate:
    for iw in[0,1,2,3,4]:  
      if data[i][4][iw] in idata3:
        usuki3.addTemporal(act3[i],act3[data[i][4][iw]],\
        tempType=CS[data[i][5][iw][0]], delay= int(data[i][5][iw][1]*2))  
        if data[i][1] in ["A02010","A02610","A03010","A03210","A04310",\
                                     "A09210"] and data[i][5][iw][0]==0:
          usuki3.addTemporal(act3[data[i][4][iw]],act3[i],\
                        tempType=CS[3], delay=-int(data[i][5][iw][1]*2))        
#-----
for i in  idata3b:
  if data[i][1] in PL_gisou+PL_kumitate:
    for iw in [0]:
      if data[i][4][iw] in idata3b:
        ii=data[i][4][iw]      
        usuki3.addTemporal(act3[i],act3[ii],tempType="CS")
        usuki3.addTemporal(act3[ii],act3[i],tempType="SC")       
        for jj in idata3b:   
          for iw in [0]:
            if data[i][4][iw] in idata3b:            
              if data[jj][4][iw]==i:              
                usuki3.addTemporal(act3[jj],act3[i],tempType="CS")
                usuki3.addTemporal(act3[i],act3[jj],tempType="SC")               
#-----開始日の制約
for i in idata3a:   
  usuki3.addTemporal("source",act3[i],tempType="SS",delay= int(data[i][3]))
  usuki3.addTemporal(act3[i],"source",tempType="SS",delay=-int(data[i][3]))     
#-----     
for i in idata3b:
  usuki3.addTemporal("source",act3[i],tempType="SS",delay= rdate)

プログラムIII-4

#プログラムIII-4.1
#=======================================================================
#仕掛作業 資源制約（配員、場所） 
#=======================================================================
mode3={}
for i in idata3a:  
  if data[i][1] in ACT_gisou:
    mode3[i]=mode1[i] 
    mode3[i].name='mode3['+i+']'
    act3[i].addModes(mode3[i]) 
#-----
  if data[i][1] in [11,1,2,3,4,5,6,12,13,14,15,36,37,38]: #ACT_kumitate.remove(16):  
    mode3[i]=mode2[i] 
    mode3[i].name='mode3['+i+']'    
    act3[i].addModes(mode3[i])
#-----
  if data[i][1] in PL_gisou: 
    mode3[i]=mode1[i] 
    s=mode1[i].name
    mode3[i].name=s[:4]+"3"+s[6:]       
    act3[i].addModes(mode3[i])    
#-----  
  if data[i][1] in PL_kumitate: 
    mode3[i]=mode2[i] 
    s=mode2[i].name
    mode3[i].name=s[:4]+"3"+s[6:]      
    act3[i].addModes(mode3[i])         

#プログラムIII-4.2  
#=======================================================================
#再計画 資源制約（配員）  
#=======================================================================    
for i in idata3b:
  if data[i][1] in ACT_gisou+ACT_kumitate:
#-----
    #1F #2W #3C #4CC #20ブラスト #31鉄艤 #32甲配　#33機配　#34PA #39_甲配 #40_機配   
    if data[i][1] in [1,2,3,4,20,31,32,33,34,39,40]:
      n=math.ceil(data[i][9][0][0]/5)  #１人で何日かかるか
      if data[i][1] in [1,3]:       #取付
        mode3[i]=Mode("mode3["+i+"]",duration=n) 
        mode3[i].addBreak(0,0)
        mode3[i].addResource(resWH,{(0,n):5})
        mode3[i].addParallel(1,n,28)   
      if data[i][1] in [2,4]:       #溶接
        mode3[i]=Mode("mode3["+i+"]",duration=n)    
        mode3[i].addBreak(0,0)
        mode3[i].addResource(resWW,{(0,n):5})
        mode3[i].addParallel(1,n,43)             
      if data[i][1] in [31]:        #鉄艤装
        mode3[i]=Mode("mode3["+i+"]",duration=n)     
        mode3[i].addBreak(0,0)
        mode3[i].addResource(resWF1,{(0,n):5})
        mode3[i].addParallel(1,n,5)          
      if data[i][1] in [32,39]:     #甲板配管
        mode3[i]=Mode("mode3["+i+"]",duration=n)      
        mode3[i].addBreak(0,0)    
        mode3[i].addResource(resWF2,{(0,n):5})
        mode3[i].addParallel(1,n,7)    
      if data[i][1] in [33,40]:     #機関配管
        mode3[i]=Mode("mode3["+i+"]",duration=n)
        mode3[i].addBreak(0,0)    
        mode3[i].addResource(resWF3,{(0,n):5})
        mode3[i].addParallel(1,n,10)     
      if data[i][1]==20:            #塗装内
        mode3[i]=Mode("mode3["+i+"]",duration=n)      
        mode3[i].addBreak(0,0)    
        mode3[i].addResource(resWP, {(0,n):5})   
        mode3[i].addParallel(1,n,8)    
      if data[i][1]==34:            #塗装外
        mode3[i]=Mode("mode3["+i+"]",duration=n)   
        mode3[i].addBreak(0,0)    
        mode3[i].addResource(resWP2,{(0,n):5}) 
        mode3[i].addParallel(1,n,8)    
      act3[i].addModes(mode3[i])
#-----
    #10:入荷 #7:社0 #8:検0 #9:W0 #16:運 #17:運0 #18:運2 #19:合体 #21:運ブ #25:運ブ2	 #35:AT #0:搭載 
    elif data[i][1] in [10,7,8,9,16,17,18,19,21,25,35,0]: 
      mode3[i]=Mode("mode3["+i+"]",duration=data[i][2])   
      act3[i].addModes(mode3[i])
#-----
    #11:開始 #5:社 #6:検 #12:正 #13:反 #14:積み #15:一体 #36:磨き #37:O/P #38:_鉄艤 #16:運 
    elif data[i][1] in [11,5,6,12,13,14,15,36,37,38,16]:      
      mode3[i]=Mode("mode3["+i+"]",duration=data[i][2])  
      act3[i].addModes(mode3[i])       

#プログラムIII-4.3     
#=======================================================================
#再計画 資源制約（作業場所）
#=======================================================================    
for i in idata3b:      
  if data[i][1] in PL_gisou+PL_kumitate:       
#-----
   if data[i][1] in [22,23,24,70,71,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87]: #PL_gisou.remove(72): #72:D5_AH	 
      if data[i][1] in [22,23,24]: data[i][6]=AB
      for no in range(len(data[i][6])):
        no1=data[i][6][no][0];
        L=R[no1][1]; B=R[no1][2];
        L1=data[i][6][no][1]; L2=data[i][6][no][2];
        B1=data[i][6][no][3]; B2=data[i][6][no][4];       
        skipL=math.floor(L/8); skipL=1
        skipB=math.floor(B/2); 
        l=math.floor(data[i][8][0]); b=math.floor(data[i][8][1]);  
        if data[i][1] in [22,23,24]:  l=19; b=16;
        for j in range(L1,L2-l+1,skipL): 
          for k in range(B1,B2-b+1,skipB):
            mode3[i]=Mode("mode3["+i+"]_R[{0:03d}][{1:03d}_{2:03d}][{3:03d}_{4:03d}]".format(no1,j,k,l,b))
            for s in range(0,l):
              for t in range(0,b):
                mode3[i].addBreak(0,0)
                mode3[i].addResource(resPL[no1,j+s,k+t],1,"break")
            act3[i].addModes(mode3[i])            
#-----
    elif data[i][1] in [51,53,54,55,56,57,58,59,60,61,62,63,64]: #PL_kumitate.remove(65): #65:K4_1AS1
      for no in range(len(data[i][6])):
        no1=data[i][6][no][0];
        L=R[no1][1]; B=R[no1][2];
        L1=data[i][6][no][1]; L2=data[i][6][no][2];
        B1=data[i][6][no][3]; B2=data[i][6][no][4];       
        skipL=math.floor(L/8); skipL=1
        skipB=math.floor(B/2); 
        l=math.floor(data[i][8][0]); b=math.floor(data[i][8][1]);  
        if data[i][1] in [22,23,24]:  l=19; b=16;
        for j in range(L1,L2-l+1,skipL): 
          for k in range(B1,B2-b+1,skipB):
            mode3[i]=Mode("mode3["+i+"]_R[{0:03d}][{1:03d}_{2:03d}][{3:03d}_{4:03d}]".format(no1,j,k,l,b))
            for s in range(0,l):
              for t in range(0,b):
                mode3[i].addBreak(0,0)
                mode3[i].addResource(resPL[no1,j+s,k+t],1,"break")
            act3[i].addModes(mode1[i])     
#-----
    elif i in {"A01431","A11701","A11711","A11721"}:  #場所と期間が固定されている場合
      if data[i][6]!=0:
        for no in range(len(data[i][6])):
          no1=data[i][6][no][0];
          L=R[no1][1]; B=R[no1][2];
          L1=data[i][6][no][1]; L2=data[i][6][no][2];
          B1=data[i][6][no][3]; B2=data[i][6][no][4];      
          skipL=math.floor(L/4); skipL=2; 
          skipB=math.floor(B/2); # skipB=1;        
          l=math.floor(data[i][8][0]); b=math.floor(data[i][8][1]);
          for j in range(L1,L2-l+1,skipL):
            for k in range(B1,B2-b+1,skipB):         
              mode3[i]=Mode("mode3["+i+"]_R[{0:03d}][{1:03d}_{2:03d}][{3:03d}_{4:03d}]".format(no1,j,k,l,b),duration=int(data[i][2]))
              for s in range(0,l):
                for t in range(0,b):
                  mode3[i].addResource(resPL[no1,j+s,k+t],1)        
              act3[i].addModes(mode1[i,no1,j,k])

プログラムIII-5

#プログラムIII-5
#=======================================================================
#再計画　問題の規模
#======================================================================= 
N=len(usuki3.act)
print("アクティビティ総数（含待機数）:",N)  
M=[]
for a in usuki3.act: M.append(len(a.modes)) 
print("モード数:",M)  
print("平均モード数:",round(sum(M)/N))
P=math.ceil(math.log10(round(sum(M)/N)**N))
print("問題の規模: 10**",P)                       
#=======================================================================
#組立工程計画　問題求解
#=======================================================================
#usuki3.Params.Makespan=True
usuki3.Params.Initial=False
usuki3.Params.TimeLimit=600
usuki3.Params.Neighborhood=20
usuki3.Params.OutputFlag=False
usuki3.optimize()

プログラムIII-6

#プログラムIII-6
#=======================================================================
#データセットの更新
#=======================================================================
print("")  
for a in usuki3.act:
  for i in idata3b: 
    if a.name==data[i][0]:
      if len(a.modes)==1: nam=a.selected.name
      else: nam=a.selected
      #print("_"+a.name,len(a.modes),nam,a.start,a.completion)
      data[i][3]=a.start
      data[i][2]=a.completion-a.start
      if len(nam)>15:
        r=int(nam[16:19]) 
        x=int(nam[21:24]) 
        y=int(nam[25:28]) 
        l=int(nam[30:33]) 
        b=int(nam[34:37])       
        place=[[ r, x, x+l, y, y+b]]
        data[i][6]=place
        print(data[i][0:8])  
#========================================================================
filename="usu_S1777_resche_R040510c.csv"
print("writing "+filename) 
usuki3.writeExcel(filename)    
import dill
dill.dump_session('usukiR040510c.pkl')  
#------
#eof