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

3Dアニメーション

投稿日時: 投稿者:

[0] 3Dアニメーションの対象として、次の単振り子を考えます。


図1 単振り子

この運動方程式は

\displaystyle{(1)\quad m\ddot{r}=\underbrace{-mg\sin\theta}_f\quad(r=\ell\theta) }

すなわち

\displaystyle{(2)\quad \ddot{\theta}=-\frac{g}{\ell}\sin\theta }

となります。

[1] Simulink 3D Animationを使って、単振り子の3Dアニメーションを作成します。これはたとえば図2のように作成されます。


図2 単振り子アニメーションのためのSimulink図

ここで、下半分は非線形運動方程式(2)をブロック線図で表したものです。ブロックVR Sinkを開くと、図3の単振り子が運動する仮想空間（VR図）が現れます。


図3 単振り子のVR図

設定のタブをクリックすると図4の画面が現れます。


図4 単振り子のVR Sink設定

ここで、VR図を描くX3Dプログラムとして、vrpend.x3dが指定されていることがわかります。また、VR図は２つの要素（pendulum, ground）と２つの視点（view1, view2）から成ります。pendulumのところのrotationにチェックを入れていることに注意してください。これにより、VR Sinkへの入力信号（回転軸を表す３次元ベクトル、回転角を表すスカラー）の受け入れを行うことができます。回転軸は[0;0;1]となっています。

図3において座標軸の設定は図5のようになっています。左右方向がｘ軸、上下方向がy軸、画面の貫通方向がz軸です。回転軸を[0;0;1]で指定するとz軸まわりの回転を意味します。


図5 Simulink 3D Animationにおける座標軸

それでは以下に示されるvrpend.x3dについて詳しく見ていきます。

X3D 
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE X3D PUBLIC "ISO//Web3D//DTD X3D 3.3//EN" 
"http://www.web3d.org/specifications/x3d-3.3.dtd">
<X3D profile='Immersive' version='3.3' 
xmlns:xsd='http://www.w3.org/2001/XMLSchema-instance' 
xsd:noNamespaceSchemaLocation='http://www.web3d.org/specifications/x3d-3.3.xsd'>
<head>
</head>
<Scene>

<Transform DEF='pendulum'  translation='0 0.6 0' center='0 0.5 0'>
  <Shape>
    <Appearance>
      <Material>
      </Material>
    </Appearance>
    <Box size='0.02 1 0.02'>
    </Box>
  </Shape>
  <Viewpoint 
       DEF = 'view2' 
       description = 'inside view' 
       position = '0 0 0'
       orientation = '0 1 0 1.57'> 
  </Viewpoint>
</Transform>

<Transform DEF='ground' >
  <Shape>
    <Appearance>
      <Material diffuseColor = '1.0 0.0 0.0'>
      </Material>
    </Appearance>
    <Box size='2 0.01 2'>
    </Box>
  </Shape>
</Transform>

<Viewpoint 
       DEF = 'view1' 
       description = 'outside view' 
       position = '0 0 0'
       orientation = '1 0 0 -0.5236'>      
</Viewpoint>

</Scene>
</X3D>

プログラム　vrpend.x3d

DEF=’pendulum’の部分について
これは振り子を描く部分で、<Box size=’0.02 1 0.02’></Box>によって、断面が２ｃｍ平方、長さが１ｍのボックス（直方体）として描いています。このボックスの重心を平行移動、または重心を通る軸まわりに回転させることができます。画面には原点があって、何も指定しなければここにボックスの重心が合わせられます。しかし、回転軸は振り子の端点とし、これを１１０ｃｍのところに固定したいので、translation=’0 0.6 0′で全体を原点から６０ｃｍ浮かせ、center=’0 0.5 0′で重心を５０ｃｍシフトさせています。x-y平面内で回転面することになります。


図6 translationとcenter

DEF=’ground’の部分について
これは床を描く部分で、<Box size=’2 0.01 2’></Box>によって、２ｍ平方、厚さ１ｃｍのボックス（直方体）として描いています。

DEF = ‘view1’の部分について
これは振り子の運動を外から観察するために視点の設定をしています。orientation=’1 0 0 -0.5236′により、回転面をx軸回りに30度傾けて見ることになります。ちなみにDEF=’pendulum’の部分においても視点が設定されています。これはちょうどブランコに乗って景色を見ていることに相当します。orientation=’0 1 0 1.57′により、視点の方向を回転面内に入れています。

[2] VRエディタvreditを用いたvrpend.x3dの作成法について述べます。

まず振り子の生成を行います。

●次図のようにメニューを辿って、ROOT の下に、Transform を生成します。

●次図のようにEdit Nameを選択して、名前を pendulum に設定します。

●次図のようにメニューを辿って、children の下に、Shape を生成します。

●次図のようにメニューを辿って、Shape の下に、Appearance を生成します。

●次図のようにメニューを辿って、Appearance の下に、Material を生成します。

●次図のようにメニューを辿って、geometry の下に、Box を生成します。

●次図のように、Box サイズを指定します。

●次図のようにメニューを辿って、内部視点 view2(inside-view) を生成します。

同様にして、床の生成と外部視点 view1(outside-view) の生成を行います。

特異値分解

投稿日時: 投稿者:

特異値分解(Singular-Value Decomposition)

●Aをサイズm\times nの行列({\rm rank}\,A=k)とします。このとき、サイズm\times mの直交行列Uとサイズn\times nの直交行列Vが存在して

\displaystyle{(1)\quad A=U\Sigma V^T }

が成り立ちます。ここで、サイズm\times nの行列\Sigmaは次を満たします。

\displaystyle{(2.1)\quad \Sigma= \left[\begin{array}{cc} \Sigma_1(k\times k) & 0\,(k\times n-k)\\ 0\,(m-k\times k) & 0\,(m-k\times n-k) \end{array}\right] }

\displaystyle{(2.2)\quad \Sigma_1={\rm diag}\{\sigma_1,\cdots,\sigma_k\}\quad(\sigma_1\ge\cdots\ge\sigma_k>0) }

●A^TA\ge 0だから、仮定{\rm rank}\,A=kより、A^TAはk個の正固有値とn-k個の零固有値をもち、互いに直交する固有ベクトルをもちます。そこで、A^TAの固有値の正の平方根を、大きい順に、\sigma_1\ge\cdots\ge\sigma_k>\sigma_{k+1}=\cdots=\sigma_{n}=0のように表し、対応する固有ベクトルv_1,\cdots,v_nをv_i^Tv_i=1\ (i=j),\ 0\ (i\ne j)を満足するようにとることができます。いま、\Sigma_1を上のように、また

\displaystyle{(3)\quad V= [\begin{array}{cc} V_1 & V_2 \end{array}] = [\begin{array}{ccc|ccc} v_1 & \cdots & v_k & v_{k+1}& \cdots & v_n \end{array}] }

とおくと、Vは直交行列となり、つぎが成り立ちます。

\displaystyle{(4)\quad \begin{array}{l} A^TAV_1=V_1\Sigma_1^2\\ A^TAV_2=0 \end{array} }

第2式の左から、V_2^Tをかけて

\displaystyle{(5)\quad V_2^TA^TAV_2=0\Rightarrow AV_2=0 }

また、U_1=AV_1\Sigma_1^{-1}とおくと、第1式からU_1^TU_1=I_{k}を得ます。そこで、U_2をU= [\begin{array}{cc} U_1 & U_2 \end{array}]が直交行列となるように選ぶと

\displaystyle{(6)\quad U^TAV= \left[\begin{array}{cc} U_1^TAV_1 & U_1^TAV_2\\ U_2^TAV_1 & U_2^TAV_2 \end{array}\right] = \left[\begin{array}{cc} \Sigma_1 & 0\\ U_2^TU_1\Sigma_1 & 0 \end{array}\right] =\Sigma }

が成り立ちます。

●行列A= \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 1 \end{array}\right]の特異値分解は

\displaystyle{(7)\quad A= \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 1 & 0 \end{array}\right] }_{U} \underbrace{ \left[\begin{array}{ccc} \sqrt{2} & 0 & 0 \\ 0 & 1 & 0 \end{array}\right] }_{\Sigma} \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0 \\ \frac{1}{\sqrt{2}} & 0 & -\frac{1}{\sqrt{2}} \\ \frac{1}{\sqrt{2}} & 0 & \frac{1}{\sqrt{2}} \end{array}\right]^T }_{V^T} }

のように与えられることを確かめます。

●A^TAのサイズは3\times 3ですが、AA^Tのサイズは2\times 2であるので、AA^Tを計算すると\left[\begin{array}{cc} 1 & 0 \\ 0 & 2 \end{array}\right]となります（サイズm\times nの行列Aの特異値を手計算で求めるには、A^TAとAA^Tのが同じ非零固有値をもつことから、サイズの小さいほうの固有値計算を行えばよい）。これから、AA^Tの固有値は1,2で、その正の平方根1,\sqrt{2}が特異値で、上の\Sigma_1の対角成分の特異値は大きい順に並べる約束ですから

\displaystyle{(8)\quad \underbrace{ \left[\begin{array}{cc} 1 & 0 \\ 0 & 2 \end{array}\right] }_{AA^T} \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 1 & 0 \end{array}\right] }_{U} = \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 1 & 0 \end{array}\right] }_{U} \underbrace{ \left[\begin{array}{cc} 2 & 0 \\ 0 & 1 \end{array}\right] }_{\Sigma_1^2} }

のように、Uと\Sigma_1が決まります。つぎに、Vについては、視察によって

\displaystyle{(9)\quad \underbrace{ \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 1 \\ 0 & 1 & 1 \end{array}\right] }_{A^TA} \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0 \\ x & 0 & -y \\ y & 0 & x \end{array}\right] }_{V} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0 \\ x & 0 & -y \\ y & 0 & x \end{array}\right] }_{V} \underbrace{ \left[\begin{array}{ccc} 2 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \end{array}\right] }_{{\rm diag}\{\Sigma_1^2,0\}} }

とします。ここで、\left[\begin{array}{ccc}x \\y \end{array}\right]と\left[\begin{array}{ccc}-y \\x \end{array}\right]が直交しており、x^2+y^2=1の制約があります。上式からx=yが出て、x=y=\frac{1}{\sqrt{2}}と定まります。

行列のノルム

● x\in{\rm\bf R}^nからy\in{\rm\bf R}^mへの写像y=Axの「伝達特性」をどう測るかを考えます。これはスカラの場合は正比例の関係y=axですから、比例定数aに相当する量を求める話になります。

サイズm\times nの行列Aの次の特異値分解を考えます。

\displaystyle{(1)\quad \begin{array}{l} A= \underbrace{ \left[\begin{array}{cc} U_1 & U_2 \end{array}\right] }_{U(m\times m)} \underbrace{ \left[\begin{array}{cc} {\rm diag}\{\sigma_1,\cdots,\sigma_k\} & 0_{k\times (n-k)} \\ 0_{(m-k)\times k} & 0_{(m-k)\times (n-k)} \end{array}\right] }_{\Sigma= \left[\begin{array}{cc} \Sigma_1 & 0 \\ 0 & 0 \end{array}\right] \quad(\sigma_1\ge\cdots\ge\sigma_k) } \underbrace{ \left[\begin{array}{cc} V_1^T \\ V_2^T \end{array}\right] }_{V(n\times n)^T}\\ =U_1(m\times k)\Sigma_1(k\times k)V_1(n\times k)^T \end{array} }

ここで、k={\rm rank}Aで、UとVは直交行列です。

\displaystyle{(2)\quad UU^T=U^TU=I_m,\quad VV^T=V^TV=I_n }

したがって、次のような３つの線形写像に分解されます。

●n次元ベクトルx\in{\rm\bf R}^nのノルムとして、次の３通りが知られています。

\displaystyle{(3)\quad \left\{\begin{array}{lll} ||x||_1=|x_1|+\cdots+|x_n|&\\ ||x||_2=\sqrt{x_1^2+\cdots+x_n^2}&\Rightarrow&||x||_2^2=x^Tx\\ ||x||_\infty=\max\{|x_1|,\cdots,|x_n|\}& \end{array}\right. }

以下では、ベクトルのノルムとして、２番目の２ノルムを考えます。

このとき、次が成り立ちます。

\displaystyle{(4)\quad ||x'||_2^2=||Vx||_2^2=x^TV^TVx=x^Tx=||x||_2^2\ \Rightarrow\ \frac{||x'||_2}{||x||_2}=1 }

\displaystyle{(5)\quad \begin{array}{l} ||y'||_2^2=||\Sigma x'||_2^2=\sigma_1^2x_1'\,^2+\cdots+\sigma_k^2x_k'\,^2+0x_{n+1}'\,^2+\cdots+0x_n'\,^2\\ \le \sigma_1^2(x_1'\,^2+\cdots+x_n'\,^2)=\sigma_1^2x'\,^Tx'=\sigma_1^2||x'||_2^2 \ \Rightarrow\ \frac{||y'||_2}{||x'||_2}\le\sigma_1 \end{array} }

\displaystyle{(6)\quad ||y||_2^2=||Uy'||_2^2=y'\,^TU^TUy'=y'\,^Ty'=||y'||_2^2\ \Rightarrow\ \frac{||y||_2}{||y'||_2}=1 }

すなわち

\displaystyle{(7)\quad \frac{||x'||_2}{||x||_2}\times\frac{||y'||_2}{||x'||_2}\times\frac{||y||_2}{||y'||_2}\le\sigma_1 \ \Rightarrow\ \frac{||y||_2}{||x||_2}=\frac{||Ax||_2}{||x||_2}\le\sigma_1 }

したがって、線形写像y=Axの伝達特性は、行列Aの２ノルム

\displaystyle{(8)\quad ||A||_2=\max_{x\ne 0}\frac{||Ax||_2}{||x||_2}=\sigma_1(A) }

すなわち行列Aの最大特異値\sigma_1(A)（行列A^TAまたはAA^Tの最大固有値の正の平方根）によって表されます。

●行列のノルムについて次式が成り立ちます。

\displaystyle{(9)\quad \begin{array}{l} ||A+B||\le ||A||+||B||\\ ||AB||\le ||A|| ||B|| \end{array}　 }

(8)より

\displaystyle{(10)\quad \frac{||Ax||}{||x||} \le ||A||\ \Rightarrow\ ||Ax|| \le ||A||||x|| }

に注意して

\displaystyle{(11)\quad \begin{array}{l} ||(A+B)x||=||Ax||+||Bx||\ \le (||A||+||B||)||x||\\ \Rightarrow\ ||(A+B)|| \le ||A||+||B||\\ ||ABx||\le ||A||||Bx||\ \le ||A||||B||||x||\ \Rightarrow\ ||AB|| \le ||A||||B|| \end{array} }

●一方、ベクトルの２ノルムについて次式が成り立ちます。

\displaystyle{(12)\quad \begin{array}{l} ||a+b||\le ||a||+||b||\\ |a^Tb|\le ||a|| ||b|| \end{array}　 }

これらは(9)の特別な場合と考えられますが、ここでは直接導出してみます。

いま、xに関する２次方程式

(13)\quad \begin{array}{l} \displaystyle{\sum_{i=1}^n(a_ix-b_i)^2=\sum_{i=1}^n(a_i^2x^2-2a_ib_ix+b_i^2)}\\ \displaystyle{=(\sum_{i=1}^na_i^2)x^2-2(\sum_{i=1}^na_ib_i)x+(\sum_{i=1}^nb_i^2)=0} \end{array}

を考えると、この実数解は１個または０個となることから、この判別式は零または負でなければならないので

\displaystyle{(14)\quad D=\underbrace{(\sum_{i=1}^na_ib_i)^2}_{(a^Tb)^2}-\underbrace{(\sum_{i=1}^na_i^2)}_{a^Ta}\underbrace{(\sum_{i=1}^nb_i^2)}_{b^Tb}\le 0 }

より

\displaystyle{(15)\quad (a^Tb)^2 \le a^Ta b^Tb \Leftrightarrow |a^Tb|\le||a||||b|| }

すなわち(12)の第２式が得られます。また、第１式は、次式から得られます。

\displaystyle{(16)\quad \begin{array}{l} (\sqrt{a^Ta}+\sqrt{b^Tb})^2-(a+b)^T(a+b)\\ =a^Ta+2\sqrt{a^Ta}\sqrt{b^Tb}+b^Tb-(a^Ta+2a^Tb+b^Tb)\\ =2(\sqrt{a^Ta}\sqrt{b^Tb}-a^Tb) \ge 2(|a^Tb|-a^Tb) >0 \end{array}　 }

●ちなみに、行列のフロベニウスノルムは

\displaystyle{(17)\quad ||A||_F^2={\rm tr}A^TA=\sum_{i=1}^m\sum_{j=1}^n a_{ij}^2 }

で定義されます。これは

\displaystyle{(18)\quad ||A||_F^2={\rm tr}V\Sigma U^TU\Sigma V^T=\sum_{i=1}^{\min\{n,m\}}\sigma_{i}^2 }

と表せるので、次式が成り立ちます。

\displaystyle{(19)\quad \underbrace{\sigma_1^2}_{||A||^2} \le \underbrace{\sum_{i=1}^{\min\{n,m\}}\sigma_{i}^2}_{||A||_F^2} \le \min\{n,m\}\underbrace{\sigma_1^2}_{||A||^2}\ \Rightarrow\ ||A|| \le ||A||_F \le \sqrt{\min\{n,m\}}||A|| }

行列積のフロベニウスノルムについても

\displaystyle{(20)\quad \begin{array}{l} \displaystyle{||AB||_F^2=\sum_{j=1}^{n}\sum_{i=1}^{m}(\sum_{k=1}^{\ell}a_{ik}b_{kj})^2} \displaystyle{\le \sum_{j=1}^{n}\sum_{i=1}^{m} (\sum_{k=1}^{\ell}a^2_{ik}\sum_{k=1}^{\ell}b_{kj}^2)}\\ \displaystyle{=(\sum_{i=1}^{m}\sum_{k=1}^{\ell}a^2_{ik}) (\sum_{k=1}^{\ell}\sum_{j=1}^{n}b_{kj}^2)} \displaystyle{=||A||_F^2||B||_F^2} \end{array}　 }

が成り立ちます。

連立１次方程式

次の線形方程式（連立１次方程式）を考えます。

\displaystyle{(1)\quad Ax=b\quad(A\in{\bf R}^{n\times m}, x\in\bf R}^m, b\in\bf R}^n) }

ここで、Aはフルランク（{\rm rank}A=\min\{n,m\}）とします。

1^\circ n\le mの場合（未知数の数が方程式の数より大きい場合）、(1)はunder-determined（劣決定）と呼ばれ、Aの特異値分解を代入して次のように書けます。

\displaystyle{(2)\quad \begin{array}{l} \underbrace{U\left[\begin{array}{cc} \Sigma_1 & 0_{n\times m-n} \end{array}\right] \left[\begin{array}{c} V_1^H\\ V_2^H \end{array}\right]}_{A}x =U\Sigma_1V_1^Hx=b\\ \Sigma_1={\rm diag}\{\sigma_1,\cdots,\sigma_n\} \end{array} }

(2)の解候補として

\displaystyle{(3)\quad x=V_1\Sigma_1^{-1} U^Hb+V_2c\quad(c\in{\bf R}^{n\times m-n}) }

を考えます。V_1^HV_2=0_{n\times m-n}より第１項と第２項は直交することから

\displaystyle{(4)\quad ||x||^2=||V_1\Sigma_1^{-1} U^Hb||^2+||V_2c||^2 }

(3)においてc=0として得られるx=V_1\Sigma_1^\dagger U^Hbは、||x||を最小化する最小ノルム解と呼ばれます。

2^\circ n\ge mの場合（未知数の数が方程式の数より小さい場合）、(1)はover-determined（過決定）と呼ばれ、Aの特異値分解を代入して次のように書けます。

\displaystyle{(5)\quad \begin{array}{l} \underbrace{\left[\begin{array}{cc} U_1 & U_2 \end{array}\right] \left[\begin{array}{cc} \Sigma_1 \\ 0_{n-m\times m} \end{array}\right]V^H}_{A}x =U_1\Sigma_1V^Hx=b\\ \Sigma_1={\rm diag}\{\sigma_1,\cdots,\sigma_m\} \end{array} }

(5)の解候補として

\displaystyle{(6)\quad x=V\Sigma_1^{-1} U_1^Hb }

を考えます。b=(b-Ax)+Axにおいて、

\displaystyle{(7)\quad \begin{array}{l} b=(b-Ax)+Ax\\ =(I-U_1\Sigma_1V^HV\Sigma_1^{-1} U_1^H)b+U_1\Sigma_1V^HV\Sigma_1^{-1} U_1^Hb\\ =U_2 U_2^Hb+U_1 U_1^Hb \end{array} }

ここで、U_2^HU_1=0_{m\times n-m}より第１項と第２項は直交することから

\displaystyle{(8)\quad ||b||^2=||b-Ax||^2+||Ax||^2 }

(6)は||b-Ax||^2を最小化する最小２乗解と呼ばれます。

行列指数関数

投稿日時: 投稿者:

定義

●n次正方行列Xの行列指数関数
\displaystyle{ \exp(X)=I_n+X+\frac{1}{2}X^2+\cdots+\frac{1}{k!}X^k+\cdots }

●諸性質
\displaystyle{1^{\circ}\ \exp(0)=I_n}
\displaystyle{2^{\circ}\ (\exp(X))^T=\exp(X^T)}
\displaystyle{3^{\circ}\ X=V\Lambda V^{-1}\ \Rightarrow \ \exp(X)=V\exp(\Lambda)V^{-1}}
\displaystyle{4^{\circ}\ XY=YX\ \Rightarrow \ \exp(X+Y)=\exp(X)\exp(Y)}
\displaystyle{5^{\circ}\ (\exp(X))^{-1}=\exp(-X)}
\displaystyle{6^{\circ}\ \frac{d}{dt}\exp(At)=A\exp(At)=\exp(At)A }
\displaystyle{7^{\circ}\ \det A\ne 0\ \Rightarrow \ \int \exp(At) dt=A^{-1}\exp(At)=\exp(At)A^{-1} }

1^\circと2^\circは自明でしょう。

3^\circは一般項が次式となるためです。

\displaystyle{\frac{1}{k!}X^k=\frac{1}{k!}(V\Lambda V^{-1})^k=\frac{1}{k!}V\Lambda^kV^{-1}}

注意すべきは、4^\circで、指数法則は可換な行列に対してのみ成立することです。これは

\displaystyle{{\rm RHS}=(I_n+X+\frac{1}{2}X^2+\cdots)(I_n+Y+\frac{1}{2}Y^2+\cdots)}
\displaystyle{=I_n+X+Y+\frac{1}{2}X^2+XY+\frac{1}{2}Y^2+\cdots}
\displaystyle{=I_n+X+Y+\frac{1}{2}(X^2+2XY+Y^2)+\cdots}

が

\displaystyle{I_n+X+Y+\frac{1}{2}(X^2+XY+YX+Y^2)+\cdots}
\displaystyle{=I_n+X+Y+\frac{1}{2}(X+Y)^2+\cdots={\rm LHS}}

と等しくなるためにはXとYが可換（XY=YX）でなければならないからです。

5^\circは4^\circでY=-Xとおけば出ます。

6^\circは一般項が

\displaystyle{\frac{d}{dt}\frac{1}{k!}(At)^k=A\frac{1}{(k-1)!}(At)^{k-1}=\frac{1}{(k-1)!}(At)^{k-1}A}

となることから出ます。

7^\circは6^\circの両辺を積分して

\displaystyle{\exp(At)=A\int\exp(At)dt=\int\exp(At)dtA }

となることから出ます。

２次の行列指数関数

●n=2の場合、Aの実Jordan標準形\Lambdaは次の３つに分類されます。

\displaystyle{1^{\circ}\ \lambda(A)=\{\lambda_1,\lambda_2\} \ \Rightarrow\ \Lambda_1=\left[\begin{array}{cc} \lambda_1 & 0 \\ 0 & \lambda_2 \end{array}\right]}
\displaystyle{2^{\circ}\ \lambda(A)=\{\lambda_R\pm j\lambda_I\} \ \Rightarrow\ \Lambda_2=\left[\begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array}\right]}
\displaystyle{3^{\circ}\ \lambda(A)=\{\lambda,\lambda\} \ \Rightarrow\ \Lambda_3=\left[\begin{array}{cc} \lambda & 1 \\ 0 & \lambda \end{array}\right]}

このときの行列指数関数は次式で与えられます。

\displaystyle{1^{\circ}\quad \exp(\Lambda_1 t)= \left[\begin{array}{cc} e^{\lambda_1t} & 0 \\ 0 & e^{\lambda_2 t} \end{array}\right] }
\displaystyle{2^{\circ}\quad \exp(\Lambda_2 t)=e^{\lambda_R t} \left[\begin{array}{cc} \cos(\lambda_It) & \sin(\lambda_It) \\ -\sin(\lambda_It) & \cos(\lambda_It) \end{array}\right] }
\displaystyle{3^{\circ}\quad \exp(\Lambda_3 t)=e^{\lambda t} \left[\begin{array}{cc} 1 & t \\ 0 & 1 \end{array}\right] }

2^\circは、次式において、XとYが可換であることから出ます。

\displaystyle{ \left[\begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array}\right]= \underbrace{\lambda_RI_2}_{X}+ \underbrace{\lambda_I \left[\begin{array}{cc} 0 & 1 \\ -1 & 0 \end{array}\right]}_{Y} }

3^\circは、次式において、XとYが可換であることから出ます。

\displaystyle{ \left[\begin{array}{cc} \lambda & 1 \\ 0 & \lambda \end{array}\right]= \underbrace{\lambda I_2}_{X}+ \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array}\right]}_{Y} }

３次の行列指数関数

●n=3の場合、Aの実Jordan標準形\Lambdaは次の４つに分類されます。

\displaystyle{1^{\circ}\ \lambda(A)=\{\lambda_1,\lambda_2,\lambda_3\} \ \Rightarrow\ \Lambda_1=\left[\begin{array}{ccc} \lambda_1 & 0 & 0\\ 0 & \lambda_2 & 0\\ 0 & 0 & \lambda_3 \end{array}\right]}
\displaystyle{2^{\circ}\ \lambda(A)=\{\lambda_R\pm j\lambda_I,\lambda\} \ \Rightarrow\ \Lambda_2=\left[\begin{array}{ccc} \lambda_R & \lambda_I & 0 \\ -\lambda_I & \lambda_R & 0 \\ 0 & 0 & \lambda \end{array}\right]}
\displaystyle{3^{\circ}\ \lambda(A)=\{\lambda,\lambda,\lambda\} \ \Rightarrow\ \Lambda_3=\left[\begin{array}{ccc} \lambda & 1 & 0\\ 0 & \lambda & 1\\ 0 & 0 & \lambda \end{array}\right]}
\displaystyle{4^{\circ}\ \lambda(A)=\{\lambda,\lambda,\lambda'\} \ \Rightarrow\ \Lambda_4=\left[\begin{array}{ccc} \lambda & 1 & 0\\ 0 & \lambda & 0\\ 0 & 0 & \lambda' \end{array}\right]}

このときの行列指数関数は次式で与えられます。

\displaystyle{1^{\circ}\quad \exp(\Lambda_1 t)= \left[\begin{array}{ccc} e^{\lambda_1t} & 0 & 0\\ 0 & e^{\lambda_2 t} & 0\\ 0 & 0 & e^{\lambda_3 t} \end{array}\right] }
\displaystyle{2^{\circ}\quad \exp(\Lambda_2 t)= \left[\begin{array}{ccc} e^{\lambda_R t}\cos(\lambda_It) & e^{\lambda_R t}\sin(\lambda_It) & 0\\ -e^{\lambda_R t}\sin(\lambda_It) & e^{\lambda_R t}\cos(\lambda_It) & 0\\ 0 & 0 & e^{\lambda t} \end{array}\right] }
\displaystyle{3^{\circ}\quad \exp(\Lambda_3 t)=e^{\lambda t} \left[\begin{array}{ccc} 1 & t & \frac{t^2}{2} \\ 0 & 1 & t \\ 0 & 0 & 1 \\ \end{array}\right] }
\displaystyle{4^{\circ}\quad \exp(\Lambda_4 t)= \left[\begin{array}{ccc} e^{\lambda t} & te^{\lambda t} & 0\\ 0 & e^{\lambda t} & 0\\ 0 & 0 & e^{\lambda t} \end{array}\right] }

一般の行列指数関数

●一般の場合、Aの実Jordan標準形\Lambdaは

\Lambda= {\rm diag}\{J(\lambda_1,m_1),\cdots,J(\lambda_p,m_p), K(\lambda_{R1},\lambda_{I1},\ell_1),\cdots,K(\lambda_{Rq},\lambda_{Iq},\ell_q)\}
（m_1+\cdots+m_p+2(\ell_1+\cdots+\ell_q)=n）

すなわち、次の２種類のジョルダン細胞のブロック対角行列となります（\lambda, \lambda_R, \lambda_I(\ne0)は実数）。

\displaystyle{1^{\circ}\quad J(\lambda,m)= \left[\begin{array}{cccc} \lambda & 1 & & \\ & \lambda & \ddots & \\ & & \ddots & 1 \\ & & & \lambda \end{array}\right]\in{\bf R}^{m\times m} }

\displaystyle{2^{\circ}\quad K(\lambda_{R},\lambda_{I},\ell)= \left[\begin{array}{c|c|cc|c} \begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array} & \begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array} & & \\ \hline & \begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array} & \ddots & & \\ \hline & & & & \\ \hline & & & \ddots & \begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array} \\ \hline & & & & \begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array} \end{array}\right]\in{\bf R}^{2\ell\times 2\ell} }

このときの行列指数関数は次式で与えられます。

\displaystyle{1^{\circ}\quad \exp(J(\lambda,m)t)=e^{\lambda t} \left[\begin{array}{ccccc} 1 & t & \frac{t^2}{2} & \cdots & \frac{t^{m-1}}{(m-1)!} \\ & 1 & t & \ddots & \vdots \\ & & 1 & \ddots & \frac{t^2}{2} \\ & & & \ddots & t \\ & & & & 1 \end{array}\right] }

\displaystyle{2^{\circ}\quad \exp(K(\lambda_{R},\lambda_{I},\ell)t)= \exp(J(\lambda_R,\ell)t) \bigotimes e^{\lambda_R t} \left[\begin{array}{cc} \cos\lambda_It & \sin\lambda_It \\ -\sin\lambda_It & \cos\lambda_It \end{array}\right] }

1^\circは、次式において、XとYが可換であることから出ます。

\displaystyle{ J(\lambda,m)= \underbrace{\lambda I_m}_{X}+ \underbrace{ \left[\begin{array}{cccc} 0 & 1 & & \\ & 0 & \ddots & \\ & & \ddots & 1 \\ & & & 0 \end{array}\right]}_{Y} }

2^\circは、まず次式のようなXとYの和となります。

\displaystyle{ K(\lambda_{R},\lambda_{I},\ell)= \underbrace{\left[\begin{array}{cccc} \lambda_R & 1 & & \\ & \lambda_R & \ddots & \\ & & \ddots & 1 \\ & & & \lambda_R \end{array}\right] \otimes I_2 }_{X}+ \underbrace{I_\ell\otimes \lambda_I \left[\begin{array}{cc} 0 & 1 \\ -1 & 0 \end{array}\right] }_{Y} }

ここでクロネッカ積に関する恒等式

\displaystyle{\underbrace{(A\otimes B)}_X\underbrace{(C\otimes D)}_Y=AC\otimes BD}

を用いると、AとCが可換、BとDが可換であれば、XとYは可換となります（2^\circの場合、ＯＫです）。また、次式が成り立つことが知られています。

\displaystyle{\exp(A\otimes I+I\otimes B)=\exp(A\otimes I)\exp(I\otimes B)=\exp(A)\otimes \exp(B)}

高次系の漸近安定性

●行列指数関数を用いると、微分方程式

\displaystyle{\underbrace{\frac{dx(t)}{dt}}_{\dot{x}(t)}=Ax(t)}

の解は次のように表されます。

\displaystyle{x(t)=\exp(At)x(0)=V\exp(\Lambda t)V^{-1}x(0)}

ここで

\begin{array}{ll} &\exp(At)=V\exp(\Lambda t)V^{-1}={\displaystyle \sum_{i=1}^p\sum_{k=1}^{m_i}t^{k-1}}e^{\lambda_it}\,R_{ik} \\ &+{\displaystyle \sum_{i=1}^q\sum_{k=1}^{\ell_i}t^{k-1}}e^{\lambda_{Ri}t}\cos\lambda_{Ii}t\,C_{ik} +{\displaystyle \sum_{i=1}^q\sum_{k=1}^{\ell_i}t^{k-1}}e^{\lambda_{Ri}t}\sin\lambda_{Ii}t\,S_{ik} \end{array}

と書けることに注意します（R_{ik},C_{ik},S_{ik}は適当なn次実正方行列）。

したがって、任意のx(0)\ne0に対して、t\rightarrow\inftyのときx(t)\rightarrow 0となるための条件は

\displaystyle{\exp(\Lambda t)\rightarrow 0\quad(t\rightarrow\infty)}

となります。これはAのすべての固有値の実部が負を意味します。

●\dot{x}(t)=ax(t)（a=1,0,0.5）の解のグラフを見ると、a=0の場合は、漸近安定ではないが、発散はしないので、不安定とまではいえないのではないかと思うかもしれません。したがって零の固有値を不安定とみなすのか、安定とみなすか迷うところです。しかし、\dot{x}(t)=Ax(t)において、A=\left[\begin{array}{cc} 0& 1\\ 0& 0 \end{array}\right]の場合、解はx(t)=\left[\begin{array}{cc} 1& t\\ 0& 1 \end{array}\right]x(0)となって、x(0)の第２要素が零でない場合は発散します。したがって、一般には零の固有値は不安定とみなします。

●\dot{x}(t)=Ax(t)において、Aのすべての固有値の実部は負または零の場合を考えます。いま実部が零の固有値\lambda_iの代数的重複度をq_i\ge2とし、次が成り立つとします。

\displaystyle{{\rm rank}(A-\lambda_i I_n)=n-q_i}

これが任意のx(0)\ne0に対して、t\rightarrow\inftyのときx(t)=\exp(At)x(0)が発散しないための必要十分条件であることが知られています。

実Jordan標準形

投稿日時: 投稿者:

固有値と固有ベクトル

●正方行列Aの固有値\lambdaと固有ベクトルv: Av=\lambda vを満足する\lambdaとv\ne0
例) \underbrace{ \left[\begin{array}{cc} a_{11} & a_{12} \\ a_{21} & a_{22} \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{cc} v_{i1} \\ v_{i2} \end{array}\right] }_{v_i} = \lambda_i \underbrace{ \left[\begin{array}{cc} v_{i1} \\ v_{i2} \end{array}\right] }_{v_i}\quad(i=1,2)

●Av=\lambda v\Leftrightarrow (A-\lambda I_n)v=0を満足するv\ne0が存在するためには \det(\lambda I_n-A)=0 が必要。

●正方行列Aの固有値の集合: \lambda(A)=\{\lambda_1,\cdots,\lambda_n\}=\{\lambda_i|\det(\lambda_i I_n-A)=0\}

●正方行列Aの固有ベクトルが１次独立ならば A=V\Lambda V^{-1}
例) \left\{\begin{array}{cc} Av_1=\lambda_1v_1\\ Av_2=\lambda_2v_2 \end{array}\right. \Leftrightarrow A \underbrace{ \left[\begin{array}{cc} v_1 & v_2 \end{array}\right] }_{V} = \underbrace{ \left[\begin{array}{cc} v_1 & v_2 \end{array}\right] }_{V} \underbrace{ \left[\begin{array}{cc} \lambda_1 & 0\\ 0 & \lambda_2 \end{array}\right] }_{\Lambda}

●実ジョルダン標準形（高橋：微分方程式入門、東京大学出版会、pp.110-112, 1988）
n次正方実行列Aは、正則なn次正方実行列Vにより、\Lambda=V^{-1}AVがつぎの形式となるように変換できます。

\displaystyle{ \Lambda= {\rm diag}\{J(\lambda_1,m_1),\cdots,J(\lambda_p,m_p), K(\lambda_{R1},\lambda_{I1},\ell_1),\cdots,K(\lambda_{Rq},\lambda_{Iq},\ell_q)\} }

ただし、m_1+\cdots+m_p+2(\ell_1+\cdots+\ell_q)=n、および

\displaystyle{ J(\lambda,m)= \left[\begin{array}{cccc} \lambda & 1 & & \\ & \lambda & \ddots & \\ & & \ddots & 1 \\ & & & \lambda \end{array}\right] }

\displaystyle{ K(\lambda_{R},\lambda_{I},\ell)= \left[\begin{array}{c|c|cc|c} \begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array} & \begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array} & & \\ \hline & \begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array} & \ddots & & \\ \hline & & & & \\ \hline & & & \ddots & \begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array} \\ \hline & & & & \begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array} \end{array}\right] }

ここで、J(\lambda,m)のサイズはm\times m、K(\lambda_{R},\lambda_{I},\ell)のサイズは2\ell\times 2\ellです。また、\lambda,\,\lambda_R,\,\lambda_I(\ne0)は実数、指定されていない要素はすべて零です。

●n=2の場合、\Lambdaは次の３つに分類されます。

\displaystyle{1^{\circ}\ \lambda(A)=\{\lambda_1,\lambda_2\} \ \Rightarrow\ \Lambda=\left[\begin{array}{cc} \lambda_1 & 0 \\ 0 & \lambda_2 \end{array}\right]}
\displaystyle{2^{\circ}\ \lambda(A)=\{\lambda_R\pm j\lambda_I\} \ \Rightarrow\ \Lambda=\left[\begin{array}{cc} \lambda_R & \lambda_I \\ -\lambda_I & \lambda_R \end{array}\right]}
\displaystyle{3^{\circ}\ \lambda(A)=\{\lambda,\lambda\} \ \Rightarrow\ \Lambda=\left[\begin{array}{cc} \lambda & 1 \\ 0 & \lambda \end{array}\right]}

●n=3の場合、\Lambdaは次の４つに分類されます。

\displaystyle{1^{\circ}\ \lambda(A)=\{\lambda_1,\lambda_2,\lambda_3\} \ \Rightarrow\ \Lambda=\left[\begin{array}{ccc} \lambda_1 & 0 & 0\\ 0 & \lambda_2 & 0\\ 0 & 0 & \lambda_3 \end{array}\right]}
\displaystyle{2^{\circ}\ \lambda(A)=\{\lambda_R\pm j\lambda_I,\lambda\} \ \Rightarrow\ \Lambda=\left[\begin{array}{ccc} \lambda_R & \lambda_I & 0 \\ -\lambda_I & \lambda_R & 0 \\ 0 & 0 & \lambda \end{array}\right]}
\displaystyle{3^{\circ}\ \lambda(A)=\{\lambda,\lambda,\lambda\} \ \Rightarrow\ \Lambda=\left[\begin{array}{ccc} \lambda & 1 & 0\\ 0 & \lambda & 1\\ 0 & 0 & \lambda \end{array}\right]}
\displaystyle{4^{\circ}\ \lambda(A)=\{\lambda,\lambda,\lambda'\} \ \Rightarrow\ \Lambda=\left[\begin{array}{ccc} \lambda & 1 & 0\\ 0 & \lambda & 0\\ 0 & 0 & \lambda' \end{array}\right]}

対称行列の場合

●対称行列: Q=Q^T
例) \displaystyle{ Q=\left[\begin{array}{cc} q_1 & q_3 \\ q_3 & q_2 \end{array}\right] }

●対称行列の固有値は実数
例) \displaystyle{ \lambda_1=\frac{1}{2}(q_1+q_2+\sqrt{D}),\ \lambda_2=\frac{1}{2}(q_1+q_2-\sqrt{D}) }
\displaystyle{ D=(q_1-q_2)^2+4q_3^2>0 }

●対称行列の固有ベクトルは直交
例) \displaystyle{ Qv_i=\lambda_iv_i\quad(i=1,2) }
\displaystyle{ v_1=\left[\begin{array}{c} q_1-q_2+\sqrt{D} \\ 2q_3 \end{array}\right],\quad v_2=\left[\begin{array}{c} q_1-q_2-\sqrt{D} \\ 2q_3 \end{array}\right] }
\displaystyle{ v_1^Tv_2=(q_1-q_2)^2-(q_1-q_2)^2-4q_3^2+4q_3^2=0 }

●対称行列Qが正定行列
\displaystyle{ Q>0 \leftrightarrow \forall x\ne0:x^TQx>0 }
\displaystyle{ Q>0 \Leftrightarrow \lambda_1,\lambda_2>0 \Leftrightarrow q_1>0,q_1q_2-q_3^2>0 }

●対称行列Qが準正定行列
\displaystyle{ Q\ge0 \leftrightarrow \forall x\ne0:x^TQx\ge0 }
\displaystyle{ Q\ge0 \Leftrightarrow \lambda_1>0,\lambda_2=0 \Leftrightarrow q_1>0,q_1q_2-q_3^2=0 }

行列の記法

投稿日時: 投稿者:

行列の記法

●実数の集合: {\rm\bf R}
●実ベクトル: x\in{\rm\bf R}^n
例) \underbrace{ \left[\begin{array}{cc} x_1\\ x_2 \end{array}\right] }_{x} \in{\rm\bf R}^2

●実行列: A\in{\rm\bf R}^{m\times n}
●サイズm\times nの行列: A(m\times n)
例1) A=\left[\begin{array}{ccc} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \end{array}\right](fat)
例2) A=\left[\begin{array}{ccc} a_{11} & a_{12} \\ a_{21} & a_{22} \\ a_{31} & a_{32} \end{array}\right](tall)

●サイズm\times nの零行列: 0_{m\times n}
例) 0_{2\times 3}=\left[\begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right]

●n次の正方行列: A(n\times n)
例) A=\left[\begin{array}{cc} a_{11} & a_{12} \\ a_{21} & a_{22} \end{array}\right]

●n次の単位行列: I_n
例) I_2:=\left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right]

●n次の対角行列
例) {\rm diag}\{d_1,d_2\}:=\left[\begin{array}{cc} d_{1} & 0 \\ 0 & d_{2} \end{array}\right]

以下では、次の行列AとBを考えます。

A=\left[\begin{array}{cc} a_{11} & a_{12} \\ a_{21} & a_{22} \end{array}\right]，B=\left[\begin{array}{cc} b_{11} & b_{12} \\ b_{21} & b_{22} \end{array}\right]

●行列の和と差
例) A\pm B=\left[\begin{array}{cc} a_{11}\pm b_{11} & a_{12}\pm b_{12} \\ a_{21}\pm b_{21} & a_{22}\pm b_{22} \end{array}\right]

●行列の積
例) AB=\left[\begin{array}{cc} a_{11}b_{11}+a_{12}b_{21} & a_{11}b_{12}+a_{12}b_{22} \\ a_{21}b_{11}+a_{22}b_{21} & a_{21}b_{12}+a_{22}b_{22} \end{array}\right]
●一般には、AB\ne BA

●行列のスカラ倍
例) \alpha A=\left[\begin{array}{cc} \alpha a_{11} & \alpha a_{12} \\ \alpha a_{21} & \alpha a_{22} \end{array}\right]

●正方行列のn乗
例) A^2=A A

●行列の転置: A^T
例) A^T=\left[\begin{array}{cc} a_{11} & a_{21} \\ a_{12} & a_{22} \end{array}\right]
●(AB)^T=B^TA^T

●行列のクロネッカ積
例) A\bigotimes B = \left[\begin{array}{cc} a_{11}B & a_{12}B \\ a_{21}B & a_{22}B \end{array}\right]

●正方行列のトレース: {\rm tr} A
例) {\rm tr} A=a_{11}+a_{22}

●正方行列の行列式: \det A
例) \det A=a_{11}a_{22}-a_{12}a_{21}
●\det(AB)=\det A\det B

●正則行列: \det A\ne0を満足する正方行列A
●特異行列: \det A=0を満足する正方行列A
●正則行列Aの逆行列: AX=XA=I_nを満足する正則行列X=A^{-1}
例) A^{-1}=\frac{1}{\det A} \left[\begin{array}{cc} a_{22} & -a_{12} \\ -a_{21} & a_{11} \end{array}\right]\ (\det A\ne0)
●(AB)^{-1}=B^{-1}A^{-1}

行列の階数

●not(P\Rightarrow Q)\quad \Leftrightarrow\quad {P\ and\ not(Q)}
●P\Rightarrow Q\quad \Leftrightarrow\quad not(Q)\Rightarrow not(P)

●ベクトルv_1,v_2は線形独立（１次独立）:
\alpha_1v_1+\alpha_2v_2=0 \Rightarrow \alpha_1=\alpha_2=0
●ベクトルv_1,v_2は線形独立でない（１次独立でない）: \alpha_1v_1+\alpha_2v_2=0 \quad (\alpha_1\ne0\ or\ \alpha_2\ne0)

●行列Aの階数: {\rm rank} A
列（行）ベクトルのうち線形独立（１次独立）なベクトルの個数
例1) 横長(fat)行列が行フルランク(row full rank)
\displaystyle{{\rm rank}\left[\begin{array}{ccc} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \end{array}\right] ={\rm rank}\left[\begin{array}{ccc} \left[\begin{array}{ccc} a_{11} & a_{12} & a_{13} \end{array}\right]\\ \left[\begin{array}{ccc} a_{21} & a_{22} & a_{23} \end{array}\right] \end{array}\right]}
例2) 縦長(tall)行列が行フルランク(column full rank)
\displaystyle{{\rm rank}\left[\begin{array}{ccc} a_{11} & a_{12} \\ a_{21} & a_{22} \\ a_{31} & a_{32} \end{array}\right] ={\rm rank}\left[\begin{array}{ccc} \left[\begin{array}{ccc} a_{11} \\ a_{21} \\ a_{31} \end{array}\right] \left[\begin{array}{ccc} a_{12} \\ a_{22} \\ a_{32} \end{array}\right] \end{array}\right]}

●線形方程式（連立１次方程式）: A(n\times n)x(n\times 1)=b(n\times 1)
例) \left\{\begin{array}{cc} a_{11}x_1+a_{12}x_2=b_1 \\ a_{21}x_1+a_{22}x_2=b_2 \end{array}\right. \Leftrightarrow \underbrace{ \left[\begin{array}{cc} a_{11} & a_{12} \\ a_{21} & a_{22} \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{cc} x_1 \\ x_2 \end{array}\right] }_{x} = \underbrace{ \left[\begin{array}{cc} b_1 \\ b_2 \end{array}\right] }_{b}
●Aが正則行列のとき、Ax=bの解はx=A^{-1}bとして一意に定まります。

●命題１：\det A\ne0\Leftrightarrow {\rm rank}A=n\ (Ax=0 \Rightarrow x=0)
この対偶をとって
●命題２：\det A=0\Leftrightarrow {\rm rank}A<n\ (Ax=0, x\ne0)

命題１⇒は、次から明らかです。
\det A\ne0 \Rightarrow (Ax=0\Rightarrow x=A^{-1}0=0)\Rightarrow {\rm rank}A=n
命題１⇒の逆は、命題２⇒が次のように示されることから出ます。
\det A=0 \Rightarrow (Ax=0, x\ne0)\Rightarrow {\rm rank}A<n

●シルベスターの不等式(nはAの列数＝Bの行数)
{\rm rank}A+{\rm rank}B-n\le{\rm rank}AB\le{\rm min}\{{\rm rank}A,{\rm rank}B\}

SICE BP（３慣性系）

投稿日時: 投稿者:

●原、千田、佐伯、野波：ロバスト制御のためのベンチマーク問題(I)
-３慣性系に対する位置制御・速度制御
●佐伯、千田、野波、原：ロバスト制御のためのベンチマーク問題(II)
-位置制御系の設計例

[1]　次図のような３慣性系を考えます。

これは次の運動方程式で表されます。

\displaystyle{(1) \begin{array}{l} \bullet j_1\ddot{\theta}_1(t)+d_1\dot{\theta}_1(t)=k_a(\theta_2(t)-\theta_1(t))+d_a(\dot{\theta}_2(t)-\dot{\theta}_1(t))+\tau(t)+\tau_{d1}\\ \bullet j_2\ddot{\theta}_2(t)+d_2\dot{\theta}_2(t)=-k_a(\theta_2(t)-\theta_1(t))-d_a(\dot{\theta}_2(t)-\dot{\theta}_1(t))\\+k_b(\theta_3(t)-\theta_2(t))+d_b(\dot{\theta}_3(t)-\dot{\theta}_2(t))+\tau_{d2}\\ \bullet j_3\ddot{\theta}_3(t)+d_3\dot{\theta}_3(t)=-k_b(\theta_3(t)-\theta_2(t))-d_b(\dot{\theta}_3(t)-\dot{\theta}_2(t))+\tau_{d3} \end{array} }

ここで、物理定数は次の値が想定されています。

物理定数 最小値 公称値 最大値
j_1 0.0009 0.001 0.0011
j_2 0.0009 0.001 0.0011
j_3 0.001 0.002 0.003
d_1 0.045 0.05 0.055
d_2 0.0009 0.001 0.0011
d_3 0.0014 0.007 0.035
d_a 0.0002 0.001 0.01
d_b 0.0009 0.001 0.0011
k_a 828 920 1012
k_b 72 80 88

●以下では、j_3,d_3,k_bの３つのパラメータ誤差を考慮して、回転体３に対してインパルス外乱が加わるときのシミュレーションを行ってみます。

図1

原論文にはさまざまな問題設定がなされていますが、ここでは、回転体３に対するインパルス外乱の下で、回転体３を速やかに整定させる制御系設計を検討します。ただし、操作入力には次の振幅制限

\displaystyle{(3)\quad \begin{array}{l} |u(t)|\le 3 \end{array} }

が課され、観測出力は回転体１の変位\theta_1(t)とします。

[2] 制御対象のLPVモデルを導出します。

\displaystyle{(4)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{ccc} j_1 & 0 & 0\\ 0 & j_2 & 0\\ 0 & 0 & j_3 \end{array}\right] }_{J} \underbrace{ \left[\begin{array}{ccc} \ddot{\theta}_1(t)\\ \ddot{\theta}_2(t)\\ \ddot{\theta}_3(t) \end{array}\right] }_{\ddot{\xi}(t)} +\underbrace{ \left[\begin{array}{ccc} d_1+d_a & -d_a & 0\\ -d_a & d_2+d_a+d_b & -d_b\\ 0 & -d_b & d_3+d_b \end{array}\right] }_{D} \underbrace{ \left[\begin{array}{ccc} \dot{\theta}_1(t)\\ \dot{\theta}_2(t)\\ \dot{\theta}_3(t) \end{array}\right] }_{\dot{\xi}(t)}\\ +\underbrace{ \left[\begin{array}{ccc} k_a & -k_a & 0\\ -k_a & k_a+k_b & -k_b\\ 0 & -k_b & k_b \end{array}\right] }_{K} \underbrace{ \left[\begin{array}{ccc} {\theta}_1(t)\\ {\theta}_2(t)\\ {\theta}_3(t) \end{array}\right] }_{{\xi}(t)} = \underbrace{ \left[\begin{array}{ccc} 1\\ 0\\ 0 \end{array}\right] }_{E_{3\times1}}\tau(t) +\underbrace{ \left[\begin{array}{ccc} \tau_{d1}\\ \tau_{d2}\\ \tau_{d3} \end{array}\right] }_{\tau_{d}} \end{array} }

\displaystyle{(5)\quad \begin{array}{l} \left[\begin{array}{c} \dot{\xi}(t)\\ \ddot{\xi}(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} 0_{3\times3} & I_3\\ -J^{-1}K & -J^{-1}D \end{array}\right] }_{A(d_3/j_3,k_b/j_3)} \left[\begin{array}{c} {\xi}(t)\\ \dot{\xi}(t) \end{array}\right] + \left[\begin{array}{c} 0\\ J^{-1}E_{3\times1} \end{array}\right]\tau(t) + \left[\begin{array}{c} 0\\ J^{-1} \end{array}\right]\tau_d \end{array} }

\displaystyle{(6)\quad \begin{array}{l} J^{-1}D={ \left[\begin{array}{ccc} d_1/j_1+d_a/j_1 & -d_a/j_1 & 0\\ -d_a/j_2 & d_2/j_2+d_a/j_2+d_b/j_2 & -d_b/j_2\\ 0 & -d_b/j_3 & d_3/j_3+d_b/j_3 \end{array}\right]\\ =(d_1/j_1)D_1+(d_2/j_2)D_2+(d_3/j_3)D_3\\ +(d_a/j_1)D_4+(d_a/j_2)D_5+(d_b/j_2)D_6+(d_b/j_3)D_7\\ J^{-1}K= \left[\begin{array}{ccc} k_a/j_1 & -k_a/j_1 & 0\\ -k_a/j_2 & k_a/j_2+k_b/j_2 & -k_b/j_2\\ 0 & -k_b/j_3 & k_b/j_3 \end{array}\right]\\ =(k_a/j_1)K_1+(k_a/j_2)K_2+(k_b/j_2)K_3+(k_b/j_3)K_4\\ \end{array} }

\displaystyle{(6)\quad {\theta}_1(t)=\left[\begin{array}{cccc} E_{1\times 3} & 0_{1\times 3} \end{array}\right] \left[\begin{array}{c} {\xi}(t)\\ \dot{\xi}(t) \end{array}\right] }

ここで、\alpha_1\le\alpha\le\alpha_2、\beta_1\le\beta\le\beta_2に対する次の内分式に注目します。

\displaystyle{(8)\quad \begin{array}{l} \left[\begin{array}{c} \alpha\\ \beta \end{array}\right]= \underbrace{\frac{\alpha_2-\alpha}{\alpha_2-\alpha_1}\frac{\beta_2-\beta}{\beta_2-\beta_1}}_{p_{11}(\alpha,\beta)}\left[\begin{array}{c} \alpha_1\\ \beta_1 \end{array}\right]+ \underbrace{\frac{\alpha_2-\alpha}{\alpha_2-\alpha_1}\frac{\beta-\beta_1}{\beta_2-\beta_1}}_{p_{12}(\alpha,\beta)}\left[\begin{array}{c} \alpha_1\\ \beta_2 \end{array}\right]\\+ \underbrace{\frac{\alpha-\alpha_1}{\alpha_2-\alpha_1}\frac{\beta_2-\beta}{\beta_2-\beta_1}}_{p_{21}(\alpha,\beta)}\left[\begin{array}{c} \alpha_2\\ \beta_1 \end{array}\right]+ \underbrace{\frac{\alpha-\alpha_1}{\alpha_2-\alpha_1}\frac{\beta-\beta_1}{\beta_2-\beta_1}}_{p_{22}(\alpha,\beta)}\left[\begin{array}{c} \alpha_2\\ \beta_2 \end{array}\right] \\ \end{array} }

\displaystyle{(9)\quad p_{11}(\alpha,\beta)+p_{12}(\alpha,\beta)+p_{21}(\alpha,\beta)+p_{22}(\alpha,\beta)=1 }

\alpha=d_3/j_3,\beta=k_b/j_3のとき上の状態方程式は、端点モデル

\displaystyle{(10)\quad \begin{array}{l} \dot{x}(t)=\underbrace{A(\alpha_1,\beta_1)}_{A_{11}}x(t)+Bu(t)\\ \dot{x}(t)=\underbrace{A(\alpha_1,\beta_2)}_{A_{12}}x(t)+Bu(t)\\ \dot{x}(t)=\underbrace{A(\alpha_2,\beta_1)}_{A_{21}}x(t)+Bu(t)\\ \dot{x}(t)=\underbrace{A(\alpha_2,\beta_2)}_{A_{22}}x(t)+Bu(t) \end{array} }

を、p_{11},p_{12},p_{12},p_{22}によって重み付けして、LPVモデル

\displaystyle{(11)\quad \dot{x}(t)=\underbrace{(p_{11}A_{11}+p_{12}A_{12}+p_{21}A_{21}+p_{22}A_{22})}_{A(\alpha,\beta)}x(t)+Bu(t) }

として表されます。

[3]　状態FB

図2

[3]　出力FB

図3

演習13.3…Flipped Classroom

1^\circ 図1

MATLAB
SCILAB

2^\circ 図2

MATLAB
SCILAB

3^\circ 図3

MATLAB
SCILAB

２慣性系

投稿日時: 投稿者:

２慣性系の制御について、次の論文をフォローします。

松井義弘：PID制御による2慣性系の速度制御

２慣性系…Homework

[0]　次図のような２慣性系を考えます。

これは次の運動方程式で表されます。

\displaystyle{(1)\quad \begin{array}{l} j_1\dot{\omega}_1(t)+k\theta_1(t)=\tau_1(t)+k\theta_2(t)\\ j_2\dot{\omega}_2(t)+k\theta_2(t)=\tau_2(t)+k\theta_1(t) \end{array} }

ここで、\tau_1を操作トルク、\tau_2を外乱トルクとみなし、また

\displaystyle{(2)\quad \tau_s(t)=k(\theta_1(t)-\theta_2(t)) }

は軸トルクと呼ばれ、計測可能とします。これを用いると(1)は次式となります。

\displaystyle{(3)\quad \begin{array}{l} j_1\dot{\omega}_1(t)=-\tau_s(t)+\tau_1(t)\\ j_2\dot{\omega}_2(t)=\tau_s(t)+\tau_2(t) \end{array} }

以下では、一定の角速度での回転時に、大きさ1の定値外乱トルクが発生しても、望ましい角速度\omega^*での回転させる速度制御問題を検討します。回転体１の速度制御ですから基本はPI制御ですが、回転体２からの外乱トルクの影響を除くために、軸トルク（両軸のトルク差）のFBも考慮します（回転体２の角速度はFBしません）。両回転体の慣性モーメントの大小によって制御ゲインは変わることになります。

[1]　軸トルクFBを加えたPI制御

\displaystyle{(4)\quad \dot{\tau}_s(t)=k({\omega}_1(t)-{\omega}_2(t)) }

に注意して、次の状態方程式を得ます。

\displaystyle{(5)\quad \boxed{ \begin{array}{l} \underbrace{ \left[\begin{array}{c} \dot{\tau}_s(t)\\ \dot{\omega}_1(t)\\ \dot{\omega}_2(t) \end{array}\right] }_{\dot{\xi}(t)} = \underbrace{ \left[\begin{array}{cccc} 0 & k & -k\\ -1/j_1 & 0 & 0\\ 1/j_2 & 0 & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t) \end{array}\right] }_{\xi(t)} + \underbrace{ \left[\begin{array}{c} 0 \\ 1/j_1 \\ 0 \end{array}\right] }_{B} \tau_1(t) + \underbrace{ \left[\begin{array}{c} 0\\ 0\\ 1/j_2 \end{array}\right] }_{w} \\ \omega_1(t)= \underbrace{ \left[\begin{array}{cccc} 0 & 1 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t) \end{array}\right] }_{\xi(t)}\\ \end{array}} }

ここで、\tau_1から\omega_1までの伝達関数

\displaystyle{(6)\quad G(s)=\frac{s^2+\omega_0^2}{j_1s(s^2+\omega_p^2)} }

から、次の共振角周波数\omega_pと反共振角周波数\omega_0を定義しておきます。

\displaystyle{(7)\quad \omega_p^2=\frac{(j_1+j_2)k}{j_1j_2}, \omega_0^2=\frac{k}{j_2} }

さて、制御目的が達成されているとすると

\displaystyle{(8)\quad \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ \omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} 0 & k & -k & 0\\ -1/j_1 & 0 & 0 & 1/j_1\\ 1/j_2 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 \end{array}\right] }_{S= \left[\begin{array}{cc} A & B\\ C & 0 \end{array}\right] } \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ \tau_1(\infty) \end{array}\right] }

より

\displaystyle{(9)\quad \underbrace{ \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ \tau_1(\infty) \end{array}\right] }_{\left[\begin{array}{c} \xi_\infty \\ \tau_{1,\infty} \end{array}\right]} = \underbrace{ \left[\begin{array}{cccc} 0 & 0 & j_2 & 0\\ 0 & 0 & 0 & 1\\ -1/k & 0 & 0 & 1\\ 0 & j_1 & j_2 &0\end{array}\right] }_{S^{-1}} \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ \omega^* \end{array}\right] = \left[\begin{array}{c} -1\\ \omega^*\\ \omega^*\\ -1 \end{array}\right] }

を得ます。したがって

\displaystyle{(10)\quad \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_{1,\infty} \end{array}\right] }

に注意して、偏差系

\displaystyle{(11)\quad \frac{d}{dt} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_{1,\infty} \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_{1,\infty} \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_{E3}} \dot{\tau}_1(t) }

に対する安定化状態フィードバック

\displaystyle{(12)\quad \begin{array}{l} \dot{\tau}_1(t) = K_{E3} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_{1,\infty} \end{array}\right] = \underbrace{ K_{E3}S^{-1} }_{ \left[\begin{array}{cccc} F_1 & F_2 & F_3 & F_4 \end{array}\right]} \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] \end{array} }

すなわち

\displaystyle{(13)\quad \boxed{\tau_1(t)=-F_1\tau_s(t)-\underbrace{F_2}_{K_P}\omega_1(t)-\underbrace{F_3}_{0}\omega_2(t) -\underbrace{F_4}_{K_I}\int_0^t(\omega_1(\tau)-\omega^*)d\tau} }

を求めます。

ここで本来は２次形式評価関数を設定したいところですが、ゲインに制約をつけるために、上記論文では閉ループ系の望ましい特性多項式を

\displaystyle{(14)\quad {\rm det}(sI_4-A_{E3}+B_{E3}K_{E3})=s^4+\underbrace{\beta_1\omega}_{\alpha_1} s^3+\underbrace{\beta_2\omega^2}_{\alpha_2} s^2+\underbrace{\beta_3\omega^3}_{\alpha_3} s+\underbrace{\beta_4\omega^4 }_{\alpha_4}}

と設定し、極配置問題を解いています。これは、たとえば

\displaystyle{(15)\quad \lambda(A_{E3}-B_{E3}K_{E3})=\{(-\zeta\pm\sqrt{1-\zeta^2})\omega,(-\zeta\pm\sqrt{1-\zeta^2})\omega\} }

とすると

\displaystyle{(16)\quad \begin{array}{l} \alpha_1=4\zeta\omega\\ \alpha_2=(4\zeta^2+2)\omega^2\\ \alpha_3=4\zeta\omega^3\\ \alpha_4=\omega^4\\ \end{array} }

の形式になること、また減衰係数\zetaと固有角周波数\omegaを指定できるなどの理由によると思われます。

上の望ましい特性多項式を達成する状態フィードバックゲインは、次式によって計算できます。

\displaystyle{(17)\quad \begin{array}{l} K_{E3}= \left[\begin{array}{cccc} 0 & 0 & 0 & 1 \end{array}\right]\\ \times \left[\begin{array}{cccc} B_{E3} & A_{E3}B_{E3} & A_{E3}^2B_{E3} & A_{E3}^3B_{E3} \end{array}\right]^{-1}\\ \times (A_{E3}^4+\alpha_1 A_{E3}^3+\alpha_2 A_{E3}^2+\alpha_3 A_{E3}+\alpha_4 I_4) \end{array} }

数式処理プログラムを用いて、

\displaystyle{(18)\quad k=j_1(w_p^2-w_0^2), j_2=j_1(\frac{w_p^2}{w_0^2}-1) }

を代入して、次のように計算できます（K_2,K_3,K_0,K_4は論文中の記号）。

\displaystyle{(19)\quad \begin{array}{l} F_1=-(\omega_0^2 \omega_p^2-\beta_2 \omega^2 \omega_0^2+\omega^4)/(\omega_0^2 \omega_p^2-\omega_0^4)=K_2\\ F_2=\beta_1 j_1 \omega=K_3\\ F_3=-(\beta_1 j_1 \omega \omega_0^2-\beta_3 j_1 \omega^3)/\omega_0^2=K_1\\ F_4=(j_1 \omega^4)/\omega_0^2=K_4 \end{array} }

実際

\displaystyle{(20)\quad \begin{array}{l} F_3=0 \\ \beta_1 \omega_0^2-\beta_3 \omega^2=0\\ \omega^2=\frac{\beta_1}{\beta_3}\omega_0^2 \end{array} }

\displaystyle{(21)\quad \begin{array}{l} F_2=j_1 \beta_1 \sqrt{\frac{\beta_1}{\beta_3}} \omega_0\\ \end{array} }

\displaystyle{(22)\quad \begin{array}{l} F_4=j_1 \frac{\beta_1^2}{\beta_3^2}\omega_0^2 \end{array} }

\displaystyle{(23)\quad \begin{array}{l} F_1=-\frac{1}{\omega_p^2-\omega_0^2}(\omega_p^2-\beta_2 \omega^2 +\frac{\omega^4}{\omega_0^2})\\ =-\frac{1}{\omega_p^2-\omega_0^2}(\omega_p^2-\beta_2 \frac{\beta_1}{\beta_3}\omega_0^2 +\frac{\beta_1^2}{\beta_3^2}\omega_0^2)\\ =-\frac{1}{\omega_p^2-\omega_0^2}(\omega_p^2-\omega_0^2+\omega_0^2-\beta_2 \frac{\beta_1}{\beta_3}\omega_0^2 +\frac{\beta_1^2}{\beta_3^2}\omega_0^2)\\ =-\frac{\omega_0^2}{\omega_p^2-\omega_0^2}(1-\beta_2 \frac{\beta_1}{\beta_3} +\frac{\beta_1^2}{\beta_3^2})-1\\ =\frac{j_1}{j_2}(\beta_2 \frac{\beta_1}{\beta_3} -\frac{\beta_1^2}{\beta_3^2}-1)-1\\ \end{array} }

まず、負荷の角速度をFBしなくて済むためには、(20)が必要となり、\omega自身には選択の余地はありません。興味深いのは、(23)から、軸トルクのFBゲインの符号が正にも負にもなることです。

MAXIMA 
/*BP.wxm*/
A:matrix([0,k,-k],[-1/j1,0,0],[1/j2,0,0]);
B:matrix([0],[1/j1],[0]);
C:matrix([0,1,0]);
D:matrix([0]);
S:addrow(addcol(A,B),addcol(C,D));
xinf:invert(S).matrix([0],[0],[1/j2],[-r]);
AE:addrow(addcol(A,B),[0,0,0,0]);
BE:matrix([0],[0],[0],[1]);
V:BE;
V:addcol(BE,AE.V);
V:addcol(BE,AE.V);
V:addcol(BE,AE.V);
p:s^4+b1*w*s^3+b2*w^2*s^2+b3*w^3*s+w^4;
q:expand( (s^2+2*z*w*s+w^2)^2 );
a4:coeff(p,s,0);
a3:coeff(p,s,1);
a2:coeff(p,s,2);
a1:coeff(p,s,3);
a0:coeff(p,s,4);
U:matrix([0,0,0,1]);
W0:a4*ident(4);
W1:a3*AE;
W2:a2*AE^^2;
W3:a1*AE^^3;
W4:a0*AE^^4;
W:W4+W3+W2+W1+W0;
KE:U.invert(V).W;
KS:ratsimp(KE.invert(S));
FE:KS,k=j1*(wp^2-w0^2),j2=j1*(wp^2/w0^2-1);
K1:ratsimp(FE[1][3]);
K2:ratsimp(FE[1][1]);
K3:ratsimp(FE[1][2]);
K4:ratsimp(FE[1][4]);
/*---*/
/*K1=-(b1*j1*w*w0^2-b3*j1*w^3)/w0^2*/
/*K2=-(w0^2*wp^2-b2*w^2*w0^2+w^4)/(w0^2*wp^2-w0^4)*/
/*K3=b1*j1*w*/
/*K4=(j1*w^4)/w0^2*/
MATLAB 
%cBP.m
%-----
 clear all, close all
 j1=1; w0=1; r=0.4;
 j2=r*j1; k=w0^2*j2; wp=sqrt(k*(j1+j2)/(j1*j2));
 A=[0 k -k;-1/j1 0 0;1/j2 0 0];
 B=[0;1/j1;0]; wd=[0;0;1/j2];
 C=[0 1 0];
%-----
 w=1; z=1/sqrt(2);
 p=conv([1 2*z*w w^2],[1 2*z*w w^2]);
 a1=p(2); a2=p(3); a3=p(4); a4=p(5);
 %a1=4*z*w; a2=(4*z^2+2)*w^2; a3=4*z*w^3; a4=w^4;
 AE=[A B;zeros(1,4)]; BE=[0;0;0;1];
 K1=[0 0 0 1];
 K2=[BE AE*BE AE^2*BE AE^3*BE];
 K3=AE^4+a1*AE^3+a2*AE^2+a3*AE+a4*eye(4,4);
 KE=K1*(K2\K3);
 AECL=AE-BE*KE;
 S=[A B;C 0];
 FE=KE/S;
 sys=ss(AECL,[wd;0],[[C 0];-FE],[]); 
 t=0:0.2:20;
 figure
 step(sys,t), grid
%-----
%  b1=4*z; b2=4*z^2+2; b3=4*z; b4=1;
%  F1=-(w0^2*wp^2-b2*w^2*w0^2+w^4)/(w0^2*wp^2-w0^4)
%  F2=b1*j1*w
%  F3=-(b1*j1*w*w0^2-b3*j1*w^3)/w0^2
%  F4=(j1*w^4)/w0^2
%-----静止・インパルス外乱（大きさ0.2）
 r=0
 w=0
 tau0=0
 om10=0
 om20=1/j2*0.2
 sim("nBP")
%-----静止・定値外乱（大きさ0.2）
 r=0
 w=0.2
 tau0=0
 om10=0
 om20=0
 sim("nBP")
%-----定速回転・インパルス外乱（大きさ0.2）
 r=1
 w=0
 tau0=0
 om10=0
 om20=1/j2*0.2
 sim("nBP")
%-----定速回転・定値外乱（大きさ0.2）
 r=1
 w=0.2
 tau0=0
 om10=0
 om20=0
 sim("nBP")
%-----
%eof

●数値微分による軸トルクを加えたPI制御

軸トルクは次式のように速度を１階微分して得ることができます。

\displaystyle{(24)\quad \boxed{\tau_s(t)=k(\theta_1(t)-\theta_2(t))=\tau_1(t)-j_1\frac{d}{dt}\omega_1(t)} }

これを用いると

(25)\quad \begin{array}{l} \displaystyle{\tau_1(t)=-F_1(\tau_1(t)-j_1\frac{d}{dt}\omega_1(t))-F_2\omega_1(t) -F_4\int_0^t(\omega_1(\tau)-\omega^*)d\tau}\\ \displaystyle{(1+F_1)\tau_1(t)=F_1j_1\frac{d}{dt}\omega_1(t)-F_2\omega_1(t)-F_4\int_0^t(\omega_1(\tau)-\omega^*)d\tau} \end{array} }

\displaystyle{(26)\quad %\begin{array}{l} \boxed{\tau_1(t)=-\underbrace{\frac{F_2}{1+F_1}}_{F'_2}\omega_1(t)+\underbrace{\frac{F_1}{1+F_1}}_{F'_1}j_1\frac{d}{dt}\omega_1(t)-\underbrace{\frac{F_4}{1+F_1}}_{F'_4}\int_0^t(\omega_1(\tau)-\omega^*)d\tau} %\end{array} }

これはPID制御となっており、軸トルクのFBは不要となっています。

●外乱オブザーバによる軸トルクを加えたPI制御

外乱オブザーバの仕組みは、数値微分を用いた軸トルクを１次フィルタを通したものに等しいことが知られています。これは

\displaystyle{(27)\quad \boxed{\begin{array}{l} \dot{\theta}_R(t)=-\frac{1}{T_R}\theta_R(t)+\frac{1}{T_R}\underbrace{\tau_s(t)}_{\tau_1(t)-j_1\frac{d}{dt}\omega_1(t)} \end{array}} }

を用いて

\displaystyle{(28)\quad \boxed{\tau_1(t)=-F_1 \theta_R(t)-F_2\omega_1(t) -F_4\underbrace{\int_0^t(\omega_1(\tau)-\omega^*)d\tau}_{x_I(t)}} }

のように実施されます。以下では、これもPID制御であることを示します。

\displaystyle{(29)\quad \begin{array}{l} \dot{\theta}_R(t)=-\frac{1}{T_R}\theta_R(t)+\frac{1}{T_R}(-F_1 \theta_R(t)-F_2\omega_1(t) -F_4x_I(t)-j_1\frac{d}{dt}\omega_1(t))\\ =-\frac{1}{T_R}(1+F_1 )\theta_R(t)-\frac{F_4}{T_R}x_I(t) -\frac{1}{T_R}(F_2\omega_1(t)+j_1\frac{d}{dt}\omega_1(t))\\ =-\underbrace{\frac{1+F_1 }{T_R}}_{\frac{1}{T_D}}\theta_R(t)-\underbrace{\frac{F_4}{1+F_1 }}_{F'_4}\frac{1}{T_D}x_I(t) -\underbrace{\frac{F_2}{1+F_1 }}_{F'_2}\frac{1}{T_D}\omega_1(t) -\underbrace{\frac{j_1}{1+F_1 }}_{j'_1}\frac{1}{T_D}\frac{d}{dt}\omega_1(t) \end{array} }

のように書けるので、コントローラの状態空間表現は次式となります（簡単のため\omega^*=0としています）。

\displaystyle{(30)\quad \begin{array}{l} \left[\begin{array}{c} \dot{x}_I(t)\\ \dot{\theta}_R(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} 0 & 0\\ -\frac{F'_4}{T_D} & -\frac{1}{T_D} \end{array}\right] }_{A_F} \left[\begin{array}{c} x_I(t)\\ \theta_R(t) \end{array}\right] + \underbrace{ \left[\begin{array}{cc} 1\\ -\frac{1}{T_D}(F'_2+j'_1\frac{d}{dt}) \end{array}\right] }_{B_F}\omega_1(t)\\ \tau_1(t)= \underbrace{ \left[\begin{array}{cccc} -F_4 & -F_1 \end{array}\right] }_{C_F} \left[\begin{array}{c} x_I(t)\\ \theta_R(t) \end{array}\right] +\underbrace{(-F_2)}_{D_F}\omega_1(t) \end{array} }

これからコントローラの伝達関数が次式のように計算できます。

\displaystyle{(31)\quad \begin{array}{l} K(s)=\underbrace{ \left[\begin{array}{cccc} -F_4 & -F_1 \end{array}\right] }_{C_F} \underbrace{ \left[\begin{array}{cccc} s & 0\\ \frac{F'_4}{T_D} & s+\frac{1}{T_D} \end{array}\right]^{-1} }_{(sI-A_F)^{-1}} \underbrace{ \left[\begin{array}{cc} 1\\ -\frac{1}{T_D}(F'_2+j'_1s) \end{array}\right] }_{B_F} + \underbrace{(-F_2)}_{D_F}\\ = \left[\begin{array}{cccc} -F_4 & -F_1 \end{array}\right] \underbrace{ \left[\begin{array}{cccc} s & 0\\ \frac{F'_4}{T_D} & s+\frac{1}{T_D} \end{array}\right]^{-1} }_{ \frac{1}{s(s+\frac{1}{T_D})} \left[\begin{array}{cccc} s+\frac{1}{T_D} & 0\\ -\frac{F'_4}{T_D} & s \end{array}\right] } \left[\begin{array}{cc} 1\\ -\frac{1}{T_D}(F'_2+j'_1s) \end{array}\right] -F_2\\ =-F_4\frac{1}{s}+F_1 \frac{F'_4}{T_D}\frac{1}{s(s+\frac{1}{T_D})} +F_1 \frac{1}{s+\frac{1}{T_D}}\frac{1}{T_D}(F'_2+j'_1s)-F_2\\ =-(1+F_1 )(F'_2+F'_4\frac{1}{s})+F_1 \frac{1}{T_Ds+1}(F'_4\frac{1}{s}+F'_2+j'_1s)\\ =(-1-F_1 +F_1 \frac{1}{T_Ds+1}})(F'_2+F'_4\frac{1}{s})+F_1'\frac{j_1s}{T_Ds+1}}\\ =(-1-F_1 \frac{T_Ds}{T_Ds+1}})(F'_2+F'_4\frac{1}{s})+F_1'\frac{j_1s}{T_Ds+1}}\\ =(-1-F'_1\frac{T_Rs}{T_Ds+1}})(F'_2+F'_4\frac{1}{s})+F_1'\frac{j_1s}{T_Ds+1}}\\ =-F'_2-F'_4\frac{1}{s}-\frac{T_Rs}{T_Ds+1}}F'_1F'_2-\frac{T_Ds+1-T_Ds}{T_Ds+1}}T_RF'_1F'_4+F_1'\frac{j_1s}{T_Ds+1}}\\ =-F'_2-T_RF'_1F'_2-F'_4\frac{1}{s}-\frac{T_Rs}{T_Ds+1}}F'_1F'_2+\frac{T_Ds}{T_Ds+1}}T_RF'_1F'_4+F_1'\frac{j_1s}{T_Ds+1}}\\ =-\underbrace{(F'_2+T_RF'_1F'_2)}_{K_P}-\underbrace{F'_4}_{K_I}\frac{1}{s}-\frac{s}{T_Ds+1}}\underbrace{(T_RF'_1F'_2-T_DT_RF'_1F'_4-F_1'j_1)}}_{K_D}\\ %=F_P+F_I\frac{1}{s}+\frac{F_Ds}{Ts+1} \end{array} }

外乱オブザーバによる軸トルクを加えたPI制御は、１次フィルタ付微分動作をもつPID制御となっていることが分かります。

[2]　１次フィルタを通すPI制御

\displaystyle{(32)\quad \begin{array}{l} \dot{\tau}_1(t)=-\frac{1}{T_F}\tau_1(t)+\frac{1}{T_F}u(t) \end{array} }

\displaystyle{(33)\quad \boxed{ \begin{array}{l} \underbrace{ \left[\begin{array}{c} \dot{\tau}_s(t)\\ \dot{\omega}_1(t)\\ \dot{\omega}_2(t)\\ \dot{\tau}_1(t) \end{array}\right] }_{\dot{\xi}(t)} = \underbrace{ \left[\begin{array}{cccc} 0 & k & -k & 0\\ -1/j_1 & 0 & 0& 1/j_1\\ 1/j_2 & 0 & 0& 0\\ 0 & 0 & 0 & -1/T \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t)\\ \tau_1(t) \end{array}\right] }_{\xi(t)}\\ + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ 1/T \end{array}\right] }_{B} u(t) + \underbrace{ \left[\begin{array}{c} 0\\ 0\\ 1/j_2\\ 0 \end{array}\right] }_{w}\\ \dot{\theta}_1(t)= \underbrace{ \left[\begin{array}{cccc} 0 & 1 & 0& 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t)\\ \tau_1(t) \end{array}\right] }_{\xi(t)}\\ \end{array}} }

\displaystyle{(34)\quad \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ 0\\ \omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{ccccc} 0 & k & -k & 0& 0\\ -1/j_1 & 0 & 0 & 1/j_1& 0\\ 1/j_2 & 0 & 0 & 0& 0\\ 0 & 0 & 0 & -1/T& 1/T\\ 0 & 1 & 0 & 0& 0 \end{array}\right] }_{S} \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ \tau_1(\infty)\\ u(\infty) \end{array}\right] }

\displaystyle{(35)\quad \underbrace{ \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ \tau_1(\infty)\\ u(\infty) \end{array}\right] }_{\left[\begin{array}{c} \xi_\infty \\ u_\infty \end{array}\right]} = \underbrace{ \left[\begin{array}{ccccc} 0 & 0 & j_2 & 0& 0\\ 0 & 0 & 0 & 0& 1\\ -1 & 0 & 0 & 0& 1\\ 0 & j_1 & j_2 &0& 0\\ 0 & j_1 & j_2 &T& 0 \end{array}\right] }_{S^{-1}} \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ 0\\ \omega^* \end{array}\right] = \left[\begin{array}{c} -1\\ \omega^*\\ \omega^*\\ -1\\ -1 \end{array}\right] }

\displaystyle{(36)\quad \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ u(t)-u_\infty \end{array}\right] }

\displaystyle{(37)\quad \frac{d}{dt} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ u(t)-u_\infty \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ u(t)-u_\infty \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_{E3}} \dot{u}(t) }

\displaystyle{(38)\quad \dot{u}(t) = K_{E3} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ u(t)-u_\infty \end{array}\right] =\underbrace{ K_{E3}S^{-1} }_{ \left[\begin{array}{ccccc} K_2 & K_3 & K_1 & K_0& K_4 \end{array}\right]} \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] }

(39)\quad \boxed{\begin{array}{l} \displaystyle{u(t)=-\underbrace{K_2}_{0}\tau_s(t)-\underbrace{K_3}_{K_P}\omega_1(t)-\underbrace{K_1}_{0}\omega_2(t)-\underbrace{K_0}_{0}\tau_1(t)}\\ \displaystyle{-\underbrace{K_4}_{K_I=\frac{K_P}{T_I}}\int_0^t(\omega_1(\tau)-\omega^*)d\tau} \end{array}} }

\displaystyle{(40)\quad {\rm det}(sI_5-A+BF)=s^5+\beta_1\omega s^4+\beta_2\omega^2 s^3+\beta_3\omega^3 s^2+\beta_4\omega^4 s+\beta_5\omega^5 }

\displaystyle{(41)\quad \lambda(A_{E3}-B_{E3}K_{E3})=\{(-\zeta\pm\sqrt{1-\zeta^2})\omega,(-\zeta\pm\sqrt{1-\zeta^2})\omega, \omega\} }

\displaystyle{(42)\quad \begin{array}{l} \alpha_1=(4\zeta+1)\omega\\ \alpha_2=(4\zeta^2+4\zeta+1)\omega^2\\ \alpha_3=(4\zeta^2+4\zeta+1)\omega^3\\ \alpha_4=(4\zeta+1)\omega^4\\ \alpha_5=\omega^5 \end{array} }

\displaystyle{(43)\quad \begin{array}{l} K_{E3}= \left[\begin{array}{ccccc} 0 & 0 & 0 & 0 &1 \end{array}\right]\\ \times \left[\begin{array}{ccccc} B_{E3} & A_{E3}B_{E3} & A_{E3}^2B_{E3} & A_{E3}^3B_{E3}& A_{E3}^4B_{E3} \end{array}\right]^{-1}\\ \times (A_{E3}^5+\alpha_1A_{E3}^4+\alpha_2A_{E3}^3+\alpha_3A_{E3}^2+\alpha_4A_{E3}+\alpha_5I_n) \end{array} }

\displaystyle{(44)\quad \begin{array}{l} K_2=-(T \beta_1 \omega \omega_0^2 \omega_p^2-T \beta_3 \omega^3 \omega_0^2+T \omega^5)/(\omega_0^2 \omega_p^2-\omega_0^4)\\ K_3=T \beta_2 j_1 \omega^2-T j_1 \omega_p^2\\ K_1=(T j_1 \omega_0^2 \omega_p^2-T \beta_2 j_1 \omega^2 \omega_0^2+T \beta_4 j_1 \omega^4)/\omega_0^2\\ K_0=T \beta_1 \omega-1\\ K_4=(T j_1 \omega^5)/\omega_0^2 \end{array} }

\displaystyle{(45)\quad K_0=0\Rightarrow T=\frac{1}{\beta_1 \omega} }

\displaystyle{(46)\quad \begin{array}{l} K_1=0\\ \omega_0^2 \omega_p^2-\beta_2 \omega^2 \omega_0^2+\beta_4 \omega^4=0\\ \end{array} }

\displaystyle{(47)\quad \begin{array}{l} K_2=0\\ \beta_1 \omega_0^2 \omega_p^2-\beta_3 \omega^2 \omega_0^2+ \omega^4=0\\ \end{array} }

\displaystyle{(48)\quad \begin{array}{l} (\beta_1\beta_4-1)\omega_0^2\omega_p^2-(\beta_3\beta_4-\beta_2)\omega^2 \omega_0^2=0\\ \omega^2=\frac{\beta_1\beta_4-1}{\beta_3\beta_4-\beta_2}\omega_p^2 \end{array} }

\displaystyle{(49)\quad \begin{array}{l} (\beta_3-\beta_1\beta_2)\omega^2\omega_0^2+(\beta_1 \beta_4-1)\omega^4=0\\ \omega^2=\frac{\beta_1\beta_2-\beta_3}{\beta_1 \beta_4-1}\omega_0^2 \end{array} }

\displaystyle{(50)\quad \begin{array}{l} \omega^4=\frac{\beta_1\beta_4-1}{\beta_3\beta_4-\beta_2}\frac{\beta_1\beta_2-\beta_3}{\beta_1 \beta_4-1}\omega_0^2\omega_p^2 =\frac{\beta_1\beta_2-\beta_3}{\beta_3\beta_4-\beta_2}\omega_0^2\omega_p^2\\ \end{array} }

\displaystyle{(51)\quad \begin{array}{l} \frac{\omega_p^2}{\omega_0^2}=\frac{\beta_3\beta_4-\beta_2}{\beta_1\beta_4-1}\frac{\beta_1\beta_2-\beta_3}{\beta_1 \beta_4-1} =\frac{(\beta_3\beta_4-\beta_2)(\beta_1\beta_2-\beta_3)}{(\beta_1\beta_4-1)^2} \end{array} }

\displaystyle{(52)\quad \begin{array}{l} K_3=T j_1(\beta_2 \omega^2- \omega_p^2)=\frac{1}{\beta_1 \omega}j_1(\beta_2 \frac{\beta_1\beta_4-1}{\beta_3\beta_4-\beta_2}\omega_p^2- \omega_p^2)\\ =\frac{j_1}{\beta_1 \omega}(\beta_2 \frac{\beta_1\beta_4-1}{\beta_3\beta_4-\beta_2}-1) \omega_p^2=\frac{j_1}{\beta_1 \omega}\frac{\beta_1\beta_2-\beta_3}{\beta_3\beta_4-\beta_2} \beta_4\omega_p^2\\ =\frac{j_1}{\beta_1 \omega} \frac{\omega^4}{\omega_0^2\omega_p^2}\beta_4\omega_p^2=\frac{j_1 \beta_4}{\beta_1} \frac{\omega^3}{\omega_0^2} \end{array} }

\displaystyle{(53)\quad \begin{array}{l} K_4=\frac{1}{\beta_1 \omega}\frac{j_1\omega}{\omega_0^2}\omega^4 =\frac{j_1}{\beta_1}\frac{\omega^4}{\omega_0^2} \end{array} }

\displaystyle{(54)\quad T_I=\frac{K_3}{K_4}=\frac{j_1 \beta_4}{\beta_1} \frac{\omega^3}{\omega_0^2}\frac{\beta_1}{j_1}\frac{\omega_0^2}{\omega^4} =\frac{\beta_4}{\omega} }

\displaystyle{(55)\quad \begin{array}{l} K_I=K_4T_I=\frac{j_1}{\beta_1}\frac{\omega^4}{\omega_0^2}\frac{\beta_4}{\omega} =\frac{j_1 \omega\beta_4}{\beta_1}\frac{\omega^2}{\omega_0^2}\\ =\frac{j_1 \omega\beta_4}{\beta_1 \omega_0^2}\frac{\beta_1\beta_2-\beta_3}{\beta_1\beta_4-1}\omega_0^2 =\frac{j_1 \omega\beta_4}{\beta_1}\frac{\beta_1\beta_2-\beta_3}{\beta_1\beta_4-1} \end{array} }

MAXIMA 
/*BP.wxm*/
A:matrix([0,k,-k,0],[-1/j1,0,0,1/j1],[1/j2,0,0,0],[0,0,0,-1/T]);
B:matrix([0],[0],[0],[1/T]);
C:matrix([0,1,0,0]);
D:matrix([0]);
S:addrow(addcol(A,B),addcol(C,D));
Sinv:invert(S);
xinf:Sinv.matrix([0],[0],[-1/j2],[0],[r]);
AE:addrow(addcol(A,B),[0,0,0,0,0]);
BE:matrix([0],[0],[0],[0],[1]);
V:BE;
V:addcol(BE,AE.V);
V:addcol(BE,AE.V);
V:addcol(BE,AE.V);
V:addcol(BE,AE.V);
p:s^5+b1*w*s^4+b2*w^2*s^3+b3*w^3*s^2+b4*w^4*s+w^5;
q:expand( (s^2+2*z*w*s+w^2)^2*(s+w) );
a5:coeff(p,s,0);
a4:coeff(p,s,1);
a3:coeff(p,s,2);
a2:coeff(p,s,3);
a1:coeff(p,s,4);
a0:coeff(p,s,5);
U:matrix([0,0,0,0,1]);
W0:a5*ident(5);
W1:a4*AE;
W2:a3*AE^^2;
W3:a2*AE^^3;
W4:a1*AE^^4;
W5:a0*AE^^5;
W:W5+W4+W3+W2+W1+W0;
KE:U.invert(V).W;
KS:ratsimp(KE.Sinv);
FE:KS,k=j1*(wp^2-w0^2),j2=j1*(wp^2/w0^2-1);
K2:ratsimp(FE[1][1]);
K3:ratsimp(FE[1][2]);
K1:ratsimp(FE[1][3]);
K0:ratsimp(FE[1][4]);
K4:ratsimp(FE[1][5]);
/*---*/
/*K2=-(T*b1*w*w0^2*wp^2-T*b3*w^3*w0^2+T*w^5)/(w0^2*wp^2-w0^4)*/
/*K3=T*b2*j1*w^2-T*j1*wp^2*/
/*K1=(T*j1*w0^2*wp^2-T*b2*j1*w^2*w0^2+T*b4*j1*w^4)/w0^2*/
/*K0=T*b1*w-1*/
/*K4=(T*j1*w^5)/w0^2*/
MATLAB 
%cBP_PI.m

[3]　１次フィルタ付微分動作をもつPID制御

\displaystyle{(56)\quad \begin{array}{l} \dot{x}_{D}(t)=-\frac{1}{T_D}x_{D}(t)+\frac{1}{T_D}\omega_1(t) \end{array} }

\displaystyle{(57)\quad \boxed{ \begin{array}{l} \underbrace{ \left[\begin{array}{c} \dot{\tau}_s(t)\\ \dot{\omega}_1(t)\\ \dot{\omega}_2(t)\\ \dot{x}_{D}(t)\\ \end{array}\right] }_{\dot{\xi}(t)} = \underbrace{ \left[\begin{array}{cccc} 0 & k & -k & 0\\ -1/j_1 & 0 & 0& 0\\ 1/j_2 & 0 & 0& 0\\ 0 & 1/T_D & 0 & -1/T_D \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t)\\ x_{D}(t)\\ \end{array}\right] }_{\xi(t)}\\ + \underbrace{ \left[\begin{array}{c} 0 \\ 1/j_1 \\ 0 \\ 0 \end{array}\right] }_{B} \tau_1(t) + \underbrace{ \left[\begin{array}{c} 0\\ 0\\ 1/j_2\\ 0 \end{array}\right] }_{w}\\ \omega_1(t)= \underbrace{ \left[\begin{array}{cccc} 0 & 1 & 0& 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t)\\ x_{D}(t)\\ \end{array}\right] }_{\xi(t)}\\ \end{array}} }

\displaystyle{(58)\quad \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ 0\\ \omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{ccccc} 0 & k & -k & 0& 0\\ -1/j_1 & 0 & 0& 0& 1/j_1 \\ 1/j_2 & 0 & 0& 0& 0\\ 0 & 1/T_D & 0 & -1/T_D& 0\\ 0 & 1 & 0 & 0& 0 \end{array}\right] }_{S} \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ x_{D}(\infty)\\ \tau_1(\infty) \end{array}\right] }

\displaystyle{(59)\quad \underbrace{ \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ x_{D}(\infty)\\ \tau_1(\infty) \end{array}\right] }_{\left[\begin{array}{c} \xi_\infty \\ \tau_\infty \end{array}\right]} = \underbrace{ \left[\begin{array}{ccccc} 0 & 0 & j_2 & 0& 0\\ 0 & 0 & 0 & 0& 1\\ -1/k & 0 & 0 & 0& 1\\ 0 & 0 & 0 &-1/T_D& 0\\ 0 & j_1 & j_2 &0& 0 \end{array}\right] }_{S^{-1}} \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ 0\\ \omega^* \end{array}\right] = \left[\begin{array}{c} -1\\ \omega^*\\ \omega^*\\ \omega^*\\ -1 \end{array}\right] }

\displaystyle{(60)\quad \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_\infty \end{array}\right] }

\displaystyle{(61)\quad \frac{d}{dt} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_\infty \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_\infty \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_{E3}} \dot{u}(t) }

\displaystyle{(62)\quad \begin{array}{l} \dot{\tau}_1(t)(t) = K_{E3} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_\infty \end{array}\right]\\ =\underbrace{ K_{E3}S^{-1} }_{ \left[\begin{array}{ccccc} F_1(K_2) & F_2(K_3) & F_3(K_1) & F_4(K_0)& F_5(K_4) \end{array}\right]} \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] \end{array} }

(63)\quad \boxed{\begin{array}{l} \displaystyle{\tau_1(t)=-\underbrace{F_1}_{0}\tau_s(t)-\underbrace{F_2}_{K_P+K_D/T}\omega_1(t)-\underbrace{F_3}_{0}\omega_2(t)-\underbrace{F_4}_{-K_D/T}x_D(t)}\\ \displaystyle{-\underbrace{F_5}_{K_I}\int_0^t(\omega_1(\tau)-\omega^*)d\tau} \end{array}} }

\displaystyle{(64)\quad {\rm det}(sI_5-A+BF)=s^5+\beta_1\omega s^4+\beta_2\omega^2 s^3+\beta_3\omega^3 s^2+\beta_4\omega^4 s+\omega^5 }

\displaystyle{(65)\quad \begin{array}{l} F_1=-((T^2 \omega_0^4+\omega_0^2) \omega_p^2+(-T^2 \beta_2 \omega^2+T \beta_1 \omega-1) \omega_0^4\\ +(T^2 \beta_4 \omega^4-T \beta_3 \omega^3) \omega_0^2+T \omega^5)/((T^2 \omega_0^4+\omega_0^2) \omega_p^2-T^2 \omega_0^6-\omega_0^4)\\ F_2=(T \beta_1 j_1 \omega-j_1)/T\\ F_3=-((T^2 \beta_1 j_1 \omega-T j_1) \omega_0^4+(T \beta_2 j_1 \omega^2-T^2 \beta_3 j_1 \omega^3) \omega_0^2\\ +T^2 j_1 \omega^5-T \beta_4 j_1 \omega^4)/(T^2 \omega_0^4+\omega_0^2)\\ F_4=-(T^5 j_1 \omega^5-T^4 \beta_4 j_1 \omega^4+T^3 \beta_3 j_1 \omega^3-T^2 \beta_2 j_1 \omega^2\\+T \beta_1 j_1 \omega-j_1)/(T^3 \omega_0^2+T)\\ F_5=(T j_1 \omega^5)/\omega_0^2\\ \end{array} }

\displaystyle{(66)\quad \begin{array}{l} F_1=0\Rightarrow\\ (T^2 \omega_0^4+\omega_0^2) \omega_p^2+(-T^2 \beta_2 \omega^2+T \beta_1 \omega-1) \omega_0^4\\ +(T^2 \beta_4 \omega^4-T \beta_3 \omega^3) \omega_0^2+T \omega^5=0\\ F_1'=-T \omega^5-T^2 \beta_4 \omega_0^2 \omega^4+T \beta_3 \omega_0^2 \omega^3+T^2 \beta_2 \omega_0^4 \omega^2-T \beta_1 \omega_0^4 \omega\\ +(-T^2 \omega_0^4-\omega_0^2) \omega_p^2+\omega_0^4=0 \end{array} }

\displaystyle{(67)\quad \begin{array}{l} \frac{1}{j_1T}F_3=0\Rightarrow\\ (T^2 \beta_1 j_1 \omega-T j_1) \omega_0^4+(T \beta_2 j_1 \omega^2-T^2 \beta_3 j_1 \omega^3) \omega_0^2\\ +T^2 j_1 \omega^5-T \beta_4 j_1 \omega^4=0\\ F_3'=-T \omega^5+\beta_4 \omega^4+T \beta_3 \omega_0^2 \omega^3-\beta_2 \omega_0^2 \omega^2-T \beta_1 \omega_0^4 \omega+\omega_0^4=0 \end{array} }

\displaystyle{(68)\quad \begin{array}{l} \frac{F_1'-F_3'}{T^2\omega_0^2+1} =-\beta_4 \omega^4+\beta_2 \omega_0^2 \omega^2-\omega_0^2 \omega_p^2=0 \end{array} }

\displaystyle{(69)\quad \begin{array}{l} F_1'=-T \omega^5-T^2 \omega_0^2(\beta_2 \omega_0^2 \omega^2-\omega_0^2 \omega_p^2)+T \beta_3 \omega_0^2 \omega^3+T^2 \beta_2 \omega_0^4 \omega^2-T \beta_1 \omega_0^4 \omega\\ +(-T^2 \omega_0^4-\omega_0^2) \omega_p^2+\omega_0^4=0\\ -T \omega^5+T \beta_3 \omega_0^2 \omega^3 -T \beta_1 \omega_0^4 \omega +\omega_0^4-\omega_0^2 \omega_p^2=0\\ \end{array} }

\displaystyle{(70)\quad \begin{array}{l} F_5'=-T \omega^5+(\beta_2 \omega_0^2 \omega^2-\omega_0^2 \omega_p^2)+T \beta_3 \omega_0^2 \omega^3-\beta_2 \omega_0^2 \omega^2-T \beta_1 \omega_0^4 \omega+\omega_0^4=0\\ -T \omega^5+T \beta_3 \omega_0^2 \omega^3-T \beta_1 \omega_0^4 \omega+\omega_0^4-\omega_0^2 \omega_p^2=0 \end{array} }

\displaystyle{(71)\quad \begin{array}{l} T=\frac{\omega_0^4-\omega_0^2 \omega_p^2}{\omega^5- \beta_3 \omega_0^2 \omega^3+\beta_1 \omega_0^4 \omega} =\frac{\beta_4(\omega_0^4-\omega_0^2 \omega_p^2)} {\omega(\beta_2\omega_0^2\omega^2-\omega_p^2\omega_0^2- \beta_3\beta_4 \omega_0^2 \omega^2+\beta_1\beta_4 \omega_0^4)}\\ =\frac{\beta_4(\omega_0^2-\omega_p^2)} {\omega((\beta_2-\beta_3\beta_4)\omega^2+\beta_1\beta_4 \omega_0^2-\omega_p^2)} \end{array} }

\displaystyle{(72)\quad \omega_p^2=\frac{(j_1+j_2)k}{j_1j_2}, \omega_0^2=\frac{k}{j_2} \Rightarrow k=j_1(w_p^2-w_0^2), j_2=j_1(w_p^2/w_0^2-1) }

\displaystyle{(73)\quad F_5=T j_1 \frac{j_2}{k}\omega^5=T j_1 \frac{\omega^5}{\omega_0^2}=K_I }

\displaystyle{(74)\quad \begin{array}{l} F_2=j_1\frac{T \beta_1 \omega-1}{T}=K_P+\frac{K_D}{T} \Rightarrow j_1(T \beta_1 \omega-1)=K_PT+K_D \end{array} }

-T \omega^5+T \beta_3 \omega_0^2 \omega^3-T \beta_1 \omega_0^4 \omega+\omega_0^4-\omega_0^2 \omega_p^2=0
\beta_3 \omega^3=\frac{\omega^5}{\omega_0^2}+\beta_1 \omega_0^2 \omega+\frac{1}{T}(\omega_p^2-\omega_0^2)

-\beta_4 \omega^4+\beta_2 \omega_0^2 \omega^2-\omega_0^2 \omega_p^2=0
\beta_2 \omega^2=\beta_4 \frac{\omega^4}{\omega_0^2}+ \omega_p^2

\displaystyle{(75)\quad \begin{array}{l} F_4=-\frac{j_1 j_2}{T^3 k+T j_2}(T^5 \omega^5-T^4 \beta_4 \omega^4+T^3 \beta_3 \omega^3-T^2 \beta_2 \omega^2 +T \beta_1 \omega - 1)=-\frac{K_D}{T}\\ K_D=\frac{j_1}{T^2\omega_0^2+1}(T^5 \omega^5-T^4 \beta_4 \omega^4+T^3 \beta_3 \omega^3-T^2 \beta_2 \omega^2 +T \beta_1 \omega - 1)\\ (T^2\omega_0^2+1)K_D=j_1(T^5 \omega^5-T^4 \beta_4 \omega^4+T^3 (\frac{\omega^5}{\omega_0^2}+\beta_1 \omega_0^2 \omega+\frac{1}{T}(\omega_p^2-\omega_0^2))\\ -T^2 (\beta_4 \frac{\omega^4}{\omega_0^2}+ \omega_p^2) +T \beta_1 \omega - 1)\\ =j_1(T^3 \omega^5 \frac{(T^2\omega_0^2+1)}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{(T^2\omega_0^2+1)}{\omega_0^2} + (\beta_1 T^3\omega_0^2 \omega-T^2\omega_0^2) +T\beta_1\omega-1)\\ =j_1(T^3 \omega^5 \frac{(T^2\omega_0^2+1)}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{(T^2\omega_0^2+1)}{\omega_0^2} +(T\beta_1\omega-1)(T^2\omega_0^2+1))\\ K_D=j_1(T^3 \omega^5 \frac{1}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{1}{\omega_0^2} +(T\beta_1\omega-1))\\ =j_1(T^3 \omega^5 \frac{1}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{1}{\omega_0^2}) +K_PT+K_D\\ 0=j_1(T^3 \omega^5 \frac{1}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{1}{\omega_0^2} +(T\beta_1\omega-1))\\ =j_1(T^3 \omega^5 \frac{1}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{1}{\omega_0^2}) +K_PT\\ K_P=-j_1(T^2 \omega^5 \frac{1}{\omega_0^2} -T \beta_4 \omega^4 \frac{1}{\omega_0^2}) =-K_IT+j_1T \beta_4 \omega^4 \frac{1}{\omega_0^2} \end{array} }

\displaystyle{(76)\quad \begin{array}{l} K_I=\frac{j_1T\omega^5}{\omega_0^2}\\ K_P=\frac{\beta_4j_1T\omega^4}{\omega_0^2}-K_IT =\frac{\beta_4j_1T\omega^4}{\omega_0^2}-\frac{j_1T^2\omega^5}{\omega_0^2} =-\frac{j_1}{\omega_0^2}(T^2\omega^5-T\beta_4\omega^4)\\ K_D=\beta_1j_1T\omega-j_1-K_PT =j_1(T\beta_1\omega-1)+\frac{j_1}{\omega_0^2}(T^3\omega^5-T^2\beta_4\omega^4) \end{array} }

MAXIMA 
/*BP.wxm*/
A:matrix([0,k,-k,0,0],[-1/j1,0,0,0,0],[1/j2,0,0,0,0],[0,1,0,0,0],[0,1/T,0,0,-1/T]);
B:matrix([0],[1/j1],[0],[0],[0]);
V:B;
V:addcol(B,A.V);
V:addcol(B,A.V);
V:addcol(B,A.V);
V:addcol(B,A.V);
p:s^5+b1*w*s^4+b2*w^2*s^3+b3*w^3*s^2+b4*w^4*s+w^5;
q:expand( (s^2+2*z*w*s+w^2)^2*(s+w) );
a5:coeff(p,s,0);
a4:coeff(p,s,1);
a3:coeff(p,s,2);
a2:coeff(p,s,3);
a1:coeff(p,s,4);
a0:coeff(p,s,5);
U:matrix([0,0,0,0,1]);
W0:a5*ident(5);
W1:a4*A;
W2:a3*A^^2;
W3:a2*A^^3;
W4:a1*A^^4;
W5:a0*A^^5;
W:W5+W4+W3+W2+W1+W0;
K:ratsimp(U.invert(V).W);
F:K,k=j1*(wp^2-w0^2),j2=j1*(wp^2/w0^2-1);
F1:ratsimp(F[1][1]);
F2:ratsimp(F[1][2]);
F3:ratsimp(F[1][3]);
F4:ratsimp(F[1][4]);
F5:ratsimp(F[1][5]);
/*---*/
/*F1=-((T^2*w0^4+w0^2)*wp^2+(-T^2*b2*w^2+T*b1*w-1)*w0^4+(T^2*b4*w^4-T*b3*w^3)*w0^2+T*w^5)/
  ((T^2*w0^4+w0^2)*wp^2-T^2*w0^6-w0^4)*/
/*F2=(T*b1*j1*w-j1)/T*/
/*F3=-((T^2*b1*j1*w-T*j1)*w0^4+(T*b2*j1*w^2-T^2*b3*j1*w^3)*w0^2+T^2*j1*w^5-T*b4*j1*w^4)/
 (T^2*w0^4+w0^2)*/
/*F4=(T*j1*w^5)/w0^2*/
/*F5=-(T^5*j1*w^5-T^4*b4*j1*w^4+T^3*b3*j1*w^3-T^2*b2*j1*w^2+T*b1*j1*w-j1)/(T^3*w0^2+T)*/
MATLAB 
%cBP_pid.m
%-----
 clear all, close all
 j1=1; w0=1; r=0.4;
 j2=r*j1; k=w0^2*j2; wp=sqrt(k*(j1+j2)/(j1*j2));
 A=[0 k -k;-1/j1 0 0;1/j2 0 0];
 B=[0;1/j1;0]; wd=[0;0;1/j2];
 C=[0 1 0];
% sys=ss(A,B,C,[]); 
% figure
% margin(sys), grid
%-----
 z=1/sqrt(2);
%-----
 b1=4*z+1; b2=4*z^2+4*z+2; b3=4*z^2+4*z+2; b4=4*z+1;
 check1=b2^2/(4*b4)-wp^2/w0^2
%ax^2+bx+c=0 -> x=(-b+-sqrt(b^2-4ac))/(2a)
 w=(b2*w0^2+sqrt(b2^2*w0^4-4*b4*w0^2*wp^2))/(2*b4)
 check2=w^2-(b1*b4*w0^2-wp^2)/(b3*b4-b2)
 TD=b4*(w0^2-wp^2)/(w*((b2-b3*b4)*w^2+b1*b4*w0^2-wp^2))
 KP=-j1/w0^2*(TD^2*w^5-TD*b4*w^4)
 KD=j1*(TD*b1*w-1)+j2/w0^2*(TD^3*w^5-TD^2*b4*w^4)
 KI=j1*TD*w^5/w0^2
%-----静止・インパルス外乱（大きさ0.2）
r=0
w=0
tau0=0
om10=0
om20=1/j2*0.2
sim("nBP_pid")
%-----静止・定値外乱（大きさ0.2）
r=0
w=0.2
tau0=0
om10=0
om20=0
sim("nBP_pid")
%-----定速回転・インパルス外乱（大きさ0.2）
r=1
w=0
tau0=0
om10=0
om20=1/j2*0.2
sim("nBP_pid")
%-----定速回転・定値外乱（大きさ0.2）
r=1
w=0.2
tau0=0
om10=0
om20=0
sim("nBP_pid")
%-----
%eof

LQGI制御

投稿日時: 投稿者:

LQGI制御…Homework

[1]　可制御かつ可観測なn次系を考えます。

\displaystyle{(1)\quad \dot{x}(t)=Ax(t)+Bu(t)+w\quad(x(t),w\in{\bf R}^n,u(t)\in{\bf R}^m) }

\displaystyle{(2)\quad y(t)=C_Mx(t)\quad(y(t)\in{\bf R}^p) }

ここで、状態方程式には、操作入力u(t)のほかに、定値外乱wが加わっていること、出力方程式における行列CをC_Mと書いたことに注意します。いま、出力変数の一部やそれらの組合せからなる新しいm個の被制御変数を

\displaystyle{(3)\quad z(t)=C_Sy(t)=\underbrace{C_SC_M}_{C}x(t)\quad(z(t)\in{\bf R}^m) }

のように選び((A,C)は可観測対)、定値外乱があるにも拘わらず、制御目的

\displaystyle{(4)\quad z(t)\ \rightarrow\ r \quad (t\rightarrow\infty) }

を達成したいとします。ここで、定数ベクトルrはm個の設定値からなる。もし制御目的(4)が物理的に可能とすると、ある状態x_\inftyと入力u_\inftyが確定し

\displaystyle{(5)\quad \left[\begin{array}{c} -w \\ r \end{array}\right] = \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] \left[\begin{array}{c} x_\infty \\ u_\infty \end{array}\right] }

の関係を満足しているはずです。したがって、どのようなwとrに対しても、x_\inftyとu_\inftyが定まるように、被制御変数(3)を

\displaystyle{(6)\quad {\rm rank}\, \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S}=n+m }

が成り立つように選ぶものとします。

●さて、制御目的(4)を達成するために、つぎの積分動作を加えた状態フィードバックを考えました。

\displaystyle{(7)\quad u(t)=-Fx(t)-F_Ix_I(t)}

ただし

\displaystyle{(8)\quad \dot{x}_I(t)=z(t)-r \qquad (x_I(0)=0) }

実際には、式(7)の代わりに、状態オブザーバ

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

の出力を用いて

\displaystyle{(10)\quad u(t)=-F\hat{x}(t)-F_Ix_I(t) }

を実施することになります。ここでの積分動作を加えたオブザーバベース コントローラは、(10)を(9)に代入した

\displaystyle{(11)\quad \dot{\hat{x}}(t)=(A-HC_M-BF)\hat{x}(t)-BF_Ix_I(t)+Hy(t) }

と、(8)を合わせて、つぎのように表されます。

\displaystyle{(12)\quad \boxed{\begin{array}{rl} \left[\begin{array}{c} \dot{\hat{x}}(t) \\ \dot{x}_I(t) \end{array}\right] =& \underbrace{ \left[\begin{array}{cc} A-HC_M-BF & -BF_I \\ 0 & 0 \end{array}\right] }_{A_K} \left[\begin{array}{c} \hat{x}(t) \\ x_I(t) \end{array}\right]\\[10mm] &+ \underbrace{ \left[\begin{array}{cc} H & 0\\ C_S & -I_m \end{array}\right] }_{B_K} \left[\begin{array}{c} y(t) \\ r \end{array}\right] \end{array}} }

\displaystyle{(13)\quad \boxed{u(t)= \underbrace{- \left[\begin{array}{cc} F & F_I \end{array}\right] }_{C_K} \left[\begin{array}{c} \hat{x}(t) \\ x_I(t) \end{array}\right]} }

これによる閉ループ系は、(10)を(1)に代入した

\displaystyle{(14)\quad \dot{x}(t)=Ax(t)-BF\hat{x}(t)-BF_Ix_I(t)+w(t) }

と、(8)、(3)を合わせて

\displaystyle{(15)\quad \boxed{\left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_I(t) \\ \dot{\hat{x}}(t) \end{array}\right] = \left[\begin{array}{ccc} A & -BF_I & -BF \\ C & 0 & 0 \\ HC_M & -BF_I & A-HC_M-BF \end{array}\right] \left[\begin{array}{c} x(t) \\ x_I(t) \\ \hat{x}(t) \end{array}\right] + \left[\begin{array}{c} w \\ -r \\ 0 \end{array}\right]} }

のように表されます。そのブロック線図を図1に示します。

図1 積分動作を加えたオブザーバベースコントローラによる閉ループ系

●いま、閉ループ系(15)に、座標変換

\displaystyle{(16)\quad \left[\begin{array}{c} x(t) \\ x_I(t) \\ e(t) \end{array}\right] = \left[\begin{array}{ccc} I_n & 0 & 0 \\ 0 & I_m & 0 \\ -I_n & 0 & I_n \end{array}\right] \left[\begin{array}{c} x(t) \\ x_I(t) \\ \hat{x}(t) \end{array}\right] }

を行えば

\displaystyle{(17)\quad \boxed{\left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_I(t) \\\hline \dot{e}(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc|c} A-BF & -BF_I & -BF \\ C & 0 & 0 \\\hline 0 & 0 & A-HC_M \end{array}\right] }_{ A_{EF}'= \left[\begin{array}{c|c} A_{EF} & - \left[\begin{array}{cc} B \\ 0 \end{array}\right] F \\[5mm] \hline 0 & \widehat{A} \end{array}\right] } \left[\begin{array}{c} x(t) \\ x_I(t) \\\hline e(t) \end{array}\right] + \left[\begin{array}{c} w \\ -r \\\hline -w \end{array}\right]} }

となり、閉ループ系の固有値は、積分動作を加えた状態フィードバックだけの閉ループ系の固有値と状態オブザーバの固有値からなります。

ここで、A_{EF}は安定行列であるとします。このとき、(17)より

\displaystyle{(18)\quad \left[\begin{array}{c} x(t) \\ x_I(t) \\\hline e(t) \end{array}\right] \ \rightarrow\ \underbrace{ \left[\begin{array}{c|c} A_{EF}^{-1} & -A_{EF}^{-1} \left[\begin{array}{cc} B \\ 0 \end{array}\right] F\widehat{A}^{-1} \\[5mm]\hline 0 & \widehat{A}^{-1} \end{array}\right] }_{A_{EF}'\,^{-1}} \left[\begin{array}{c} -w \\ r \\\hline w \end{array}\right] \quad (t\rightarrow\infty) }

ここで

\displaystyle{(19)\quad S^{-1} \left[\begin{array}{cc} B \\ 0 \end{array}\right] = \left[\begin{array}{cc} 0 \\ I_m \end{array}\right] }

\displaystyle{(20)\quad A_{EF}^{-1} = \left[\begin{array}{cc} I_n & 0 \\ -F_I^{-1}F & -F_I^{-1} \end{array}\right] S^{-1} }

を用いて

\displaystyle{(21)\quad x(t)\ \rightarrow\ x_\infty \quad (t\rightarrow\infty) }

\displaystyle{(22)\quad -F_Ix_{I}(t)\ \rightarrow\ F(x_\infty+e_\infty)+u_\infty \quad (t\rightarrow\infty) }

\displaystyle{(23)\quad e(t)\ \rightarrow\ e_\infty=\widehat{A}^{-1}w \quad (t\rightarrow\infty) }

を得ます。したがって、定値外乱が存在するときは状態オブザーバに関して定常偏差が残るにもかかわらず、制御目的(4)が成り立つことがわかります。

[2]　以下に、偏差系E3をLQG制御により安定化して、積分動作を加えたオブザーバベースコントローラを構成する手順を示します。

アルゴリズム　＜LQGI制御＞

ステップ1　被制御変数の決定

\left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]が正則となるように(C=C_SC_M)、セレクタ行列C_S(m\times p)を決めます。

ステップ2　偏差系の安定化

偏差系

\displaystyle{(24)\quad \frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} + \underbrace{ \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }_{B_{E}} {\dot u}(t) }

を、状態フィードバック

\displaystyle{(25)\quad {\dot u}(t)=- \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }

によるLQ制御で安定化します。その際、評価関数としては

\displaystyle{(26)\quad \int_0^\infty \left( \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right]^T Q_E \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] +{\dot u}^T(t)R_E{\dot u}(t)\right)\,dt }

を用いる。ただし、(A_E,Q_E^{\frac{1}{2}})は可観測対とします。

さらに、FとF_Iを、次式から計算します。

\displaystyle{(27)\quad \left[\begin{array}{cc} F & F_I \end{array}\right] = \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]^{-1} }

ステップ3　オブザーバゲインHの決定

行列B'を、(A,B')が可制御対となるように選び、重み行列W>0とV>0を指定し

\displaystyle{(28)\quad \Gamma A^T+A\Gamma-\Gamma C_M^TV^{-1}C_M\Gamma+B'WB'^T=0 }

を解いて、\Gamma>0を求め、オブザーバゲインHをつぎのように定めます。

\displaystyle{(29)\quad H=V^{-1}C_M\Gamma }

ステップ4　LQGIコントローラの構成

C_S、F、F_I、Hから、つぎを構成します。

\displaystyle{(30)\quad \dot{x}_K(t)=A_Kx_K(t)+B_K \left[\begin{array}{c} y(t) \\ r \end{array}\right] }

\displaystyle{(31)\quad u(t)=C_Kx_K(t) }

ただし

\displaystyle{(32)\quad A_K= \left[\begin{array}{cc} A-HC_M-BF & -BF_I \\ 0\,(m\times n) & 0\,(m\times m) \end{array}\right] }

\displaystyle{(33)\quad B_K= \left[\begin{array}{cc} H & 0\,(n\times m)\\ C_S & -I_m \end{array}\right] }

\displaystyle{(34)\quad C_K=- \left[\begin{array}{cc} F & F_I \end{array}\right] }

この手順で設計された積分動作を加えた状態フィードバックによる制御方式をLQGI制御(LQG control with integral action)と呼びます。

LQI制御の応用

投稿日時: 投稿者:

LQI制御の応用…Homework

[1]　倒立振子は制御なしには平衡状態を保てないので、制御技術の有効性を示すのに、よく研究されています。次図は様々な倒立振子を示しています。

図1 様々な倒立振子

これらの倒立振子について、非線形シミュレータを開発し、平衡状態まわりの挙動を表す線形状態方程式を得ていました。これらに基づいて、以下では、LQ制御系を設計します。

[2] CIP

平衡状態\theta^*=0周りの挙動を表す線形状態方程式は次式で与えられます。

\displaystyle{(2.1)\quad \frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & -\frac{3gm}{4M+m} & 0 & 0\\ 0 & \frac{3(M+m)g}{(4M+m)\ell} & 0 & 0\\ \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{c} 0\\ 0\\ \frac{4}{4M+m}\\ \frac{3}{(4M+m)\ell} \end{array}\right] }_{B} \underbrace{F(t)}_{u(t)} }

これに対して、倒立位置をr_cに変更可能とする、積分動作をもつ状態フィードバック

\displaystyle{(2.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccc} f_1 & f_2 & f_3 & f_4 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} +F_I\int_0^t (r(t)-r_c)dt }

を、LQI制御のアルゴリズムに沿って設計します。

ステップ1　被制御変数の決定

制御目的は倒立位置をr_cに変更し、安定化することですから

\displaystyle{(2.3)\quad \underbrace{r(t)}_{z(t)}= \underbrace{ \left[\begin{array}{cccc} 1 & 0 & 0 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} }

のように選びます。このとき、明らかに次式が成り立ちます。

\displaystyle{(2.4)\quad \underbrace{\left[\begin{array}{c} 0\\ 0\\ 0\\ 0\\\hline r_c \end{array}\right] }_{\left[\begin{array}{c} -w \\ r_c \end{array}\right]} = \underbrace{\left[\begin{array}{cccc|c} 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 & 0\\ 0 & -\frac{3gm}{4M+m} & 0 & 0 & \frac{4}{4M+m}\\ 0 & \frac{3(M+m)g}{(4M+m)\ell} & 0 & 0 & \frac{3}{(4M+m)\ell}\\\hline 1 & 0 & 0 & 0 & 0 \end{array}\right] }_{S=\left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]} \underbrace{\left[\begin{array}{c} r_c\\ 0\\ 0\\ 0\\\hline 0 \end{array}\right] }_{\left[\begin{array}{c} x_\infty \\ u_\infty \end{array}\right]} }

ここで、Sは正則であることから、倒立位置を変えたときの状態x_\inftyと入力u_\inftyが一意に定まります。

ステップ2　偏差系の安定化

偏差系

\displaystyle{(2.5)\quad \frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_{E}} {\dot u}(t) }

を、状態フィードバック

\displaystyle{(2.6)\quad {\dot u}(t)=- \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }

によるLQ制御で安定化します。その際、評価関数としてはたとえば

(2.7)\quad \begin{array}{l} \displaystyle{J=\int_0^\infty (\underbrace{\frac{1}{M_r^2}}_{q_1^2}(r(t)-r_c)^2+\underbrace{\frac{1}{M_\theta^2}}_{q_2^2}\theta^2(t) }\\ \displaystyle{+\underbrace{\frac{1}{M_F^2}}_{q_5^2}u^2(t)+\rho^2\underbrace{\frac{T_F^2}{M_F^2}}_{R}\dot{u}^2(t))\,dt}\\ \end{array} }

\displaystyle{(2.8)\quad |r(t)|<M_r,\ |\theta(t)|<M_\theta,\ |F(t)|<M_F,\ |\dot{F}(t)|<\frac{M_F}{T_F} }

を選び、これを最小にするような(2.6)を設計します。

ステップ3　積分動作を加えた状態フィードバックの構成

次式から積分動作を加えた状態フィードバック(2.2)を構成します。

\displaystyle{(2.9)\quad \left[\begin{array}{cc} F & F_I \end{array}\right] = \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]^{-1} }

MATLAB 
%cCIP_lqi.m
%-----
 clear all, close all
 global mc m ell g th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 th0=0/180*pi %input('th(0) = <0,180> ')/180*pi;
%-----
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=0; 
 A(3,2)=-3*m*g/(m+4*mc); 
 A(4,1)=0; 
 A(4,2)=3*(m+mc)*g/((m+4*mc)*ell);  
 B=zeros(4,1);
 B(3)=4/(m+4*mc);
 B(4)=-3/((m+4*mc)*ell); 
%-----
 Tcart=1; Mr=0.5;
 Tpend=1/4*2*pi*sqrt(4/3*ell/g); Mth=3/180*pi;
 Tamp=0.1; Mamp=1; 
 CM=eye(2,4); CS=[1 0]; C=CS*CM; 
 n=size(A,1); m=size(C,1); r=size(B,2);
%-----
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 CE=eye(n+m);
 QE=diag([1/Mr 1/Mth Tcart/Mr*0 Tpend/Mth*0 1/Mamp].^2);
 RE=(Tamp/Mamp)^2;
 [KE,pcl]=opt(AE,BE,CE,QE,RE);
 FE=KE/[A B;C zeros(m)];
 pcl, F=FE(:,1:n), FI=FE(:,n+1:n+m)
%-----
 CE=diag([100,180/pi])*eye(2,5);
 sysE=ss(AE-BE*KE,[],[CE;-KE],[]);
 t=0:0.05:5;
 rc=0.5
 xE0=[-rc;0;0;0;0];
 initial(sysE,xE0,t), grid
%-----
 xs=[0;0;0;0];
 us=0;
 sim("nCIP_lqi")
%-----

[3] CIP2

平衡状態\theta^*=0と平衡入力F^*=(M+m)g\sin\alpha周りの挙動を表す線形状態方程式は次式で与えられます。

(3.1)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] = \underbrace{\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & -\frac{6\cos\alpha mg}{8M+(5-3\cos2\alpha)m} & 0 & 0\\ 0 & \frac{6(M+m)g}{(8M+(5-3\cos2\alpha)m)\ell} & 0 & 0\\ \end{array}\right] }_{A(\alpha)} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)}}\\ \displaystyle{+ \underbrace{\left[\begin{array}{c} 0\\ 0\\ \frac{8}{8M+(5-3\cos2\alpha)m}\\ \frac{6\cos\alpha}{(8M+(5-3\cos2\alpha)m)\ell} \end{array}\right] }_{B(\alpha)} \underbrace{(F(t)-F^*)}_{u(t)}} \end{array}

これに対して、倒立位置をr_cに変更可能とする、積分動作をもつ状態フィードバック

\displaystyle{(3.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccc} f_1 & f_2 & f_3 & f_4 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} +F_I\int_0^t (r(t)-r_c)dt }

を、LQI制御のアルゴリズムに沿って設計します。

●レールの傾斜角\alphaは未知とするのが実際的です。そこで

\displaystyle{(3.3)\quad \begin{array}{l} \dot{x}(t)=A(\alpha)x(t)+B(\alpha)(F(t)-F^*)\\ =A(0)x(t)+B(0)(F(t)-F^*)\\ +\underbrace{(A(\alpha)-A(0))}_{\Delta A}x(t)+\underbrace{(B(\alpha)-B(0))}_{\Delta B}(F(t)-F^*)\\ =A(0)x(t)+B(0)F(t)+\underbrace{\Delta Ax(t)+\Delta BF(t)-B(\alpha)F^*}_{w} \end{array} }

として、レールの傾斜がないときの(2.1)に外乱wが加わっていると考えます。もちろんwは定値ではないのですが、どれくらいの効果があるかシミュレーションで確かめてみます。

MATLAB 
%cCIP2_lqi.m
%-----
 clear all, close all
 global mc m ell g alpha th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 alpha=3/180*pi;
 th0=3/180*pi %input('ths   = <0,180> ')/180*pi;
%-----
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=0; 
 A(3,2)=-6*cos(alpha)*m*g/(8*mc+(5-3*cos(2*alpha))*m); 
 A(4,1)=0; 
 A(4,2)=6*(m+mc)*g/((8*mc+(5-3*cos(2*alpha))*m)*ell);  
 B=zeros(4,1);
 B(3)=8/(8*mc+(5-3*cos(2*alpha))*m);
 B(4)=-6*cos(alpha)/((8*mc+(5-3*cos(2*alpha))*m)*ell); 
%----- alpha=0の設計モデル
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=0; 
 A(3,2)=-3*m*g/(m+4*mc); 
 A(4,1)=0; 
 A(4,2)=3*(m+mc)*g/((m+4*mc)*ell);  
 B=zeros(4,1);
 B(3)=4/(m+4*mc);
 B(4)=-3/((m+4*mc)*ell); 
%-----
 Tcart=1; Mr=0.5;
 Tpend=1/4*2*pi*sqrt(4/3*ell/g); Mth=3/180*pi;
 Tamp=0.1; Mamp=1; 
 CM=eye(2,4); CS=[1 0]; C=CS*CM; 
 n=size(A,1); m=size(C,1); r=size(B,2);
%-----
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 CE=eye(n+m);
 QE=diag([1/Mr 1/Mth Tcart/Mr*0 Tpend/Mth*0 1/Mamp].^2);
 RE=(Tamp/Mamp)^2;
 [KE,pcl]=opt(AE,BE,CE,QE,RE);
 FE=KE/[A B;C zeros(m)];
 pcl, F=FE(:,1:n), FI=FE(:,n+1:n+m)
%-----
 CE=diag([100,180/pi])*eye(2,5);
 sysE=ss(AE-BE*KE,[],[CE;-KE],[]);
 t=0:0.05:5;
 rc=0.5
 xE0=[-rc;0;0;0;0];
 initial(sysE,xE0,t), grid
%-----
 xs=[0;0;0;0];
 us=(mc+m)*g*sin(alpha)*0;
 sim("nCIP2_lqi")
%-----
%eof

[4] AIP

平衡状態\theta_1^*=\alpha,\ \theta_2^*=0と平衡入力\tau^*=-(m_1+2m_2)\ell_1g\sin\alpha周りの挙動を表す線形状態方程式は次式で与えられます。

(4.1)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} \theta_1(t)\\ \theta_2(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] =}\\ \displaystyle{\underbrace{(\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \frac{3(m_1+2m_2)g}{(4m_1+3m_2)\ell_1} & -\frac{9m_2g}{2(4m_1+3m_2)\ell_1} & 0 & 0\\ -\frac{9(m_1+2m_2)g}{2(4m_1+3m_2)\ell_1} & \frac{9m_2g}{(4m_1+3m_2)\ell_2} & 0 & 0\\ \end{array}\right]+\Delta A) }_{A(\alpha)=A+\Delta A} \underbrace{\left[\begin{array}{c} \theta_1(t)\\ \theta_2(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)}\\ \displaystyle{+ \underbrace{(\left[\begin{array}{c} 0\\ 0\\ \frac{6}{2(4m_1+3m_2)\ell_1^2}\\ -\frac{9}{2(4m_1+3m_2)\ell_1\ell_2} \end{array}\right]+\Delta B) }_{B(\alpha)=B+\Delta B} \underbrace{(\tau(t)-\tau^*)}_{u(t)}}\\ =Ax(t)+B\tau(t)+\underbrace{\Delta Ax(t)+\Delta B\tau(t)-B(\alpha)\tau^*}_{w} \end{array}

これに対して、アームの角度を\alphaに変更可能とする、積分動作をもつ状態フィードバック

\displaystyle{(4.2)\quad \tau(t)= \underbrace{- \left[\begin{array}{cccc} f_1 & f_2 & f_3 & f_4 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} \theta_1(t)\\ \theta_2(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)} +F_I\int_0^t (\theta_1(t)-\alpha)dt }

を、LQI制御のアルゴリズムに沿って設計します。

MATLAB 
%cAIP_lq.m
%-----
 clear all, close all
 global m1 m2 ell1 ell2  g th10 th20
 m1=0.1; m2=0.2; ell1=0.1; ell2=0.2; g=9.8;
 th10=0/180*pi %input('th1(0) = <0,180> ')/180*pi;
 th20=0/180*pi %input('th2(0) = <0,180> ')/180*pi;  
%----- th10=0 における線形モデル
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=3*(2*m2+m1)*g/((3*m2+4*m1)*ell1); 
 A(3,2)=-9*m2*g/(2*(3*m2+4*m1)*ell1); 
 A(4,1)=-9*(2*m2+m1)*g/(2*(3*m2+4*m1)*ell2); 
 A(4,2)=3*(3*m2+m1)*g/((3*m2+4*m1)*ell2);  
 B=zeros(4,1);
 B(3)=3/((3*m2+4*m1)*ell1^2);
 B(4)=-9/(2*(3*m2+4*m1)*ell1*ell2);
%-----
 Tpend1=1/4*2*pi*sqrt(4/3*ell1/g); Mth1=3/180*pi;
 Tpend2=1/4*2*pi*sqrt(4/3*ell2/g); Mth2=3/180*pi;
 Tamp=0.1; Mamp=1; 
 CM=eye(2,4); CS=[1 0]; C=CS*CM; 
 n=size(A,1); m=size(C,1); r=size(B,2);
%-----
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 CE=eye(n+m);
 QE=diag([1/Mth1 1/Mth2 Tpend1/Mth1*0 Tpend2/Mth2*0 1/Mamp].^2);
 RE=(Tamp/Mamp)^2;
 [KE,pcl]=opt(AE,BE,CE,QE,RE);
 FE=KE/[A B;C zeros(m)];
 pcl, F=FE(:,1:n), FI=FE(:,n+1:n+m)
%-----
 CE=diag([180/pi,180/pi])*eye(2,5);
 sysE=ss(AE-BE*KE,[],[CE;-KE],[]);
 t=0:0.01:1;
 th1c=10/180*pi
 xE0=[-th10;-th20;0;0;0];
 initial(sysE,xE0,t), grid
%-----
 xs=[0;0;0;0];
 us=-(m1+2*m2)*ell1*g*sin(th1c)
 sim("nAIP_lqi")
%-----
%eof

[5] PIP

平衡状態\theta_1^*=0,\ \theta_2^*=0周りの挙動を表す線形状態方程式は次式で与えられます。

(5.1)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] =}\\ \displaystyle{\underbrace{\left[\begin{array}{cccccc} 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ 0 & -\frac{3m_1g}{4M+m_1+m_2} & -\frac{3m_2g}{4M+m_1+m_2} & 0 & 0 & 0\\ 0 & \frac{3(4m+4m_1+m_2)g}{4(4M+m_1+m_2)\ell_1} & \frac{9m_2g}{4(4M+m_1+m_2)\ell_1} & 0 & 0& 0\\ 0 & \frac{9gm_1}{4(4M+m_1+m_2)\ell_2} & \frac{3(4m+m_1+4m_2)g}{4(4M+m_1+m_2)\ell_2} & 0 & 0& 0\\ \end{array}\right]}_{A} \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right]}\\ \displaystyle{+ \underbrace{ \left[\begin{array}{c} 0\\ 0\\ 0\\ \frac{4}{4M+m_1+m_2}\\ -\frac{3}{(4M+m_1+m_2)\ell_1}\\ -\frac{3}{(4M+m_1+m_2)\ell_2} \end{array}\right] }_{B} \underbrace{F(t)}_{u(t)} \end{array}

これに対して、倒立位置をr_cに変更可能とする、積分動作をもつ状態フィードバック

\displaystyle{(5.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccccc} f_1 & f_2 & f_3 & f_4 & f_5 & f_6 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)} +F_I\int_0^t (r(t)-r_c)dt }

を、LQI制御のアルゴリズムに沿って設計します。

MATLAB 
%cPIP_lqi.m
%-----
 clear all, close all
 global mc m1 m2 ell1 ell2 g th10 th20
 mc=1; m1=0.1; m2=0.2; ell1=0.15; ell2=0.2; g=9.8; 
 th10=0/180*pi %input('th1(0) = <0,180> ')/180*pi;
 th20=-1/180*pi %input('th2(0) = <0,180> ')/180*pi;  
%-----
 A=[zeros(3,3) eye(3);zeros(3,6)];
 A(4,1)=0; 
 A(4,2)=-3*m1*g/(m1+m2+4*mc); 
 A(4,3)=-3*m2*g/(m1+m2+4*mc);  
 A(5,1)=0; 
 A(5,2)=(12*m1+3*m2+12*mc)*g/((4*m1+4*m2+16*mc)*ell1);  
 A(5,3)=9*m2*g/((4*m1+4*m2+16*mc)*ell1);   
 A(6,1)=0; 
 A(6,2)=9*m1*g/((4*m1+4*m2+16*mc)*ell2);  
 A(6,3)=(3*m1+12*m2+12*mc)*g/((4*m1+4*m2+16*mc)*ell2);   
 B=zeros(6,1);
 B(4)=4/(m1+m2+4*mc);
 B(5)=-3/((m1+m2+4*mc)*ell1);
 B(6)=-3/((m1+m2+4*mc)*ell2); 
%-----
 Tcart=1; Mr=0.5;
 Tpend1=1/4*2*pi*sqrt(4/3*ell1/g); Mth1=3/180*pi;
 Tpend2=1/4*2*pi*sqrt(4/3*ell2/g); Mth2=3/180*pi;
 Tamp=0.1; Mamp=1; 
 CM=eye(3,6); CS=[1 0 0]; C=CS*CM; 
 n=size(A,1); m=size(C,1); r=size(B,2);
%-----
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 CE=eye(n+m);
 QE=diag([1/Mr 1/Mth1 1/Mth2 Tcart/Mr*0 Tpend1/Mth1*0 Tpend2/Mth2*0 1/Mamp].^2);
 RE=(Tamp/Mamp)^2;
 [KE,pcl]=opt(AE,BE,CE,QE,RE);
 FE=KE/[A B;C zeros(m)];
 pcl, F=FE(:,1:n), FI=FE(:,n+1:n+m)
%-----
 CE=diag([100,180/pi,180/pi])*eye(3,7);
 sysE=ss(AE-BE*KE,[],[CE;-KE],[]);
 t=0:0.05:5;
 rc=0.5
 xE0=[-rc;0;0;0;0;0;0];
 initial(sysE,xE0,t), grid
%-----
 xs=[0;0;0;0;0;0];
 us=0;
 sim("nPIP_lqi")   
%-----
%eof

[6] DIP

平衡状態\theta_1^*=0,\ \theta_2^*=0周りの挙動を表す線形状態方程式は次式で与えられます。

(6.1)\quad \begin{array}{l} \displaystyle{ \frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] =}\\ \displaystyle{ \left[\begin{array}{cccccc} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & -\frac{3m_1g}{Mm_1+m_1^2+(3M+m1)m_2} & -\frac{3m_2g}{Mm_1+m_1^2+(3M+m1)m_2} \\ 0 & \frac{3(4Mm_1+4m_1^2+3m_2^2+3(18M+13m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m1)m_2)\ell_1} & \frac{9(2M+m_1)m_2g}{(4Mm_1+m_1^2+(3M+m1)m_2)\ell_1} \\ 0 & \frac{9(2Mm_1+m_1^2+3(2M+m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} & \frac{3(4Mm_1+m_1^2+12(3M+m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} \\ \end{array}\right.}\\ \displaystyle{\left.\begin{array}{cccccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ 0 & 0 & 0\\ 0 & 0 & 0\\ 0 & 0 & 0\\ \end{array}\right] \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right]}\\ \displaystyle{+ \underbrace{\left[\begin{array}{c} 0\\ 0\\ 0\\ \frac{4m_1+3m_2}{4Mm_1+m_1^2+(3M+m1)m_2}\\ -\frac{3(2m_1+m2)}{2Mm_1+m_1^2+(3M+m1)m_2}\\ \frac{3m_1}{2(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} \end{array}\right] }_{B} \underbrace{F(t)}_{u(t)}} \end{array}

これに対して、倒立位置をr_cに変更可能とする、積分動作をもつ状態フィードバック

\displaystyle{(6.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccccc} f_1 & f_2 & f_3 & f_4 & f_5 & f_6 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)} +F_I\int_0^t (r(t)-r_c)dt }

を、LQI制御のアルゴリズムに沿って設計します。

MATLAB 
%cDIP_lqi.m
%-----
 clear all, close all
 global mc m1 m2 ell1 ell2 g th10 th20
 mc=1; m=0.1; m1=m; m2=m; ell=0.15; ell1=ell; ell2=ell; g=9.8;
 th10=1/180*pi %input('th1(0) = <0,180> ')/180*pi;
 th20=0 %input('th2(0) = <0,180> ')/180*pi;   
 %-----
 A=[zeros(3,3) eye(3);zeros(3,6)];
 A(4,1)=0; 
 A(4,2)=-18*m*g/(2*m+7*mc); 
 A(4,3)=3*m*g/(4*m+14*mc);  
 A(5,1)=0; 
 A(5,2)=(15*m+12*mc)*g/((2*m+7*mc)*ell);  
 A(5,3)=-(9*m+18*mc)*g/((8*m+28*mc)*ell);   
 A(6,1)=0; 
 A(6,2)=-(9*m+18*mc)*g/((2*m+7*mc)*ell);  
 A(6,3)=(15*m+48*mc)*g/((8*m+28*mc)*ell);   
 B=zeros(6,1);
 B(4)=7/(2*m+7*mc);
 B(5)=-9/((4*m+14*mc)*ell);
 B(6)=3/((4*m+14*mc)*ell);
%------
 Tcart=1; Mr=0.5;
 Tpend1=1/4*2*pi*sqrt(4/3*ell1/g); Mth1=3/180*pi;
 Tpend2=1/4*2*pi*sqrt(4/3*ell2/g); Mth2=3/180*pi;
 Tamp=0.1; Mamp=1; 
 CM=eye(3,6); CS=[1 0 0]; C=CS*CM; 
 n=size(A,1); m=size(C,1); r=size(B,2);
%-----
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 CE=eye(n+m);
 QE=diag([1/Mr 1/Mth1 1/Mth2 Tcart/Mr*0 Tpend1/Mth1*0 Tpend2/Mth2*0 1/Mamp].^2);
 RE=(Tamp/Mamp)^2;
 [KE,pcl]=opt(AE,BE,CE,QE,RE);
 FE=KE/[A B;C zeros(m)];
 pcl, F=FE(:,1:n), FI=FE(:,n+1:n+m)
%-----
 CE=diag([100,180/pi,180/pi])*eye(3,7);
 sysE=ss(AE-BE*KE,[],[CE;-KE],[]);
 t=0:0.05:5;
 rc=0.5
 xE0=[-rc;0;0;0;0;0;0];
 initial(sysE,xE0,t), grid
%-----
 xs=[0;0;0;0;0;0];
 us=0;
 sim("nDIP_lqi")   
%-----
%eof

LQI制御

投稿日時: 投稿者:

LQI制御…Homework

[1]　次の可制御かつ可観測なn次系を考えます。

\displaystyle{(1)\quad \dot{x}(t)=Ax(t)+Bu(t)+w\quad(x(t),w\in{\bf R}^n,u(t)\in{\bf R}^m) }

\displaystyle{(2)\quad y(t)=C_Mx(t)\quad(y(t)\in{\bf R}^p) }

ここで、状態方程式には、操作入力u(t)のほかに、定値外乱wが加わっていること、出力方程式における行列CをC_Mと書いたことに注意します。いま、出力変数の一部やそれらの組合せからなる新しいm個の被制御変数(controlled variables)を

\displaystyle{(3)\quad z(t)=C_Sy(t)=\underbrace{C_SC_M}_{C}x(t)\quad(z(t)\in{\bf R}^m) }

のように選び((A,C)は可観測対)、定値外乱があるにも拘わらず、制御目的

\displaystyle{(4)\quad z(t)\ \rightarrow\ r \quad (t\rightarrow\infty) }

を達成したいとします。ここで、定数ベクトルrはm個の設定値からなる。もし制御目的(4)が物理的に可能とすると、ある状態x_\inftyと入力u_\inftyが確定し

\displaystyle{(5)\quad \boxed{\left[\begin{array}{c} -w \\ r \end{array}\right] = \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] \left[\begin{array}{c} x_\infty \\ u_\infty \end{array}\right]} }

の関係を満足しているはずです。したがって、どのようなwとrに対しても、x_\inftyとu_\inftyが定まるように、被制御変数(3)を

\displaystyle{(6)\quad \boxed{{\rm rank}\, \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S}=n+m} }

が成り立つように選ぶものとします。

図1 積分動作を加えた状態フィードバックによる閉ループ系

[2]　さて、制御目的(4)を達成するために、図1に示すような、つぎの積分動作を加えた状態フィードバックを考えます。

\displaystyle{(7)\quad \boxed{u(t)=-Fx(t) -F_I\underbrace{\int_0^t(z(\tau)-r)\,d\tau}_{x_I(t)}} }

ここで、第2項は積分動作を表しています。このようにx_I(t)を定義すると

\displaystyle{(8)\quad \boxed{\dot{x}_I(t)=z(t)-r \qquad (x_I(0)=0)} }

を得ます。(1)と(8)を合わせて

\displaystyle{(9)\quad %\underbrace{ \left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_I(t) \end{array}\right] %}_{\dot{x}_E(t)} = %\underbrace{ \left[\begin{array}{cc} A & 0 \\ C & 0 \end{array}\right] %}_{A_E} %\underbrace{ \left[\begin{array}{c} x(t) \\ x_I(t) \end{array}\right] %}_{x_E(t)} + %\underbrace{ \left[\begin{array}{c} B \\ 0 \end{array}\right] %}_{B_E} u(t) + %\underbrace{ \left[\begin{array}{c} w \\ -r \end{array}\right] %}_{w_E} }

を得ます。(9)に、(7)すなわち

\displaystyle{(10)\quad u(t)=- %\underbrace{ \left[\begin{array}{cc} F & F_I \end{array}\right] %}_{F_E} %\underbrace{ \left[\begin{array}{c} x(t) \\ x_I(t) \end{array}\right] %}_{x_E(t)} }

を代入すると、閉ループ系は、つぎのように表されます。

\displaystyle{(11)\quad \boxed{ %\underbrace{ \left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_I(t) \end{array}\right] %}_{\dot{x}_E(t)} = \underbrace{ \left[\begin{array}{cc} A-BF & -BF_I \\ C & 0 \end{array}\right] }_{A_{EF}} %\underbrace{ \left[\begin{array}{c} x(t) \\ x_I(t) \end{array}\right] %}_{x_E(t)} + %\underbrace{ \left[\begin{array}{c} w \\ -r \end{array}\right] %}_{w_E} }}

いま、A_{EF}は安定行列であるとします。このとき、A_{EF}は正則であり、つぎのように書けます。

\displaystyle{(12)\quad A_{EF} = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} - \left[\begin{array}{cc} B \\ 0 \end{array}\right] \left[\begin{array}{cc} F & F_I+I_m \end{array}\right] }

よって、A_{EF}の逆行列は、公式

\displaystyle{(13)\quad(P-XQ^{-1}Y)^{-1}=P^{-1}+P^{-1}X(Q-YP^{-1}X)^{-1}YP^{-1}}

を用いて

\displaystyle{(14)\quad \begin{array}{ll} A_{EF}^{-1} =& S^{-1}+ S^{-1} \left[\begin{array}{cc} B \\ 0 \end{array}\right] \left(I_m- \left[\begin{array}{cc} F & F_I+I_m \end{array}\right] S^{-1} \left[\begin{array}{cc} B \\ 0 \end{array}\right] \right)^{-1} \\[5mm] &\times \left[\begin{array}{cc} F & F_I+I_m \end{array}\right] S^{-1} \end{array} }

ここで、

\displaystyle{(15)\quad S^{-1} \left[\begin{array}{cc} B \\ 0 \end{array}\right] = \left[\begin{array}{cc} 0 \\ I_m \end{array}\right] }

に注意して、整理すると

\displaystyle{(16)\quad \boxed{A_{EF}^{-1} = \left[\begin{array}{cc} I_n & 0 \\ -F_I^{-1}F & -F_I^{-1} \end{array}\right] S^{-1}} }

のように計算されます。ところで、(11)から

\displaystyle{(17)\quad \left[\begin{array}{c} x(t) \\ x_I(t) \end{array}\right] \ \rightarrow\ A_{EF}^{-1} \left[\begin{array}{c} -w \\ r \end{array}\right] \quad (t\rightarrow\infty) }

を得ます。この第1ブロック行は、(5)より

\displaystyle{(18)\quad x(t)\ \rightarrow\ \left[\begin{array}{cc} I_n & 0 \end{array}\right] S^{-1} \left[\begin{array}{c} -w \\ r \end{array}\right] =x_\infty \quad (t\rightarrow\infty) }

となって、(5)の第2ブロック行から制御目的(4)が成り立ちます。また、(17)の第2ブロック行から

\displaystyle{(19)\quad -F_Ix_{I}(t)\ \rightarrow\ \left[\begin{array}{cc} F & I_m \end{array}\right] S^{-1} \left[\begin{array}{c} -w \\ r \end{array}\right] = Fx_\infty+u_\infty \quad (t\rightarrow\infty) }

を得ます。ここで、設定値rは既知だから、rに関係した項\left[\begin{array}{cc} F & I_m \end{array}\right] S^{-1} \left[\begin{array}{c} 0 \\ r \end{array}\right]をフィードフォワードして、速応性を改善できます。すなわち、制御目的(4)を達成する制御方式は

\displaystyle{(20)\quad u(t)=-Fx(t) +F_I\underbrace{\int_0^t(r-z(\tau))\,d\tau}_{-x_I(t)} +F_rr }

のように表され、ここで、F_rはつぎのように決定できます。

\displaystyle{(21)\quad F_r= \left[\begin{array}{cc} F & I_m \end{array}\right] S^{-1} \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }

[3]　これまで、閉ループ系(11)において、A_{EF}は安定行列であるとしていました。ここでは、これを満足させるための具体的手段として、先に学んだLQ制御を使うことを考えます。LQ制御の議論における閉ループ系は自励系(入力をもたない系)を前提にしていましたが、本章における閉ループ系は入力をもつことに注意が必要です。この前提を満足させるために、定常状態との差をとって得られる偏差系(error system)が用いられます。

制御目的(4)が達成されたとき成り立つ(5)より

\displaystyle{(22)\quad \left[\begin{array}{c} 0 \\ 0 \end{array}\right] = \left[\begin{array}{cc} A & 0 \\ C & 0 \end{array}\right] \left[\begin{array}{c} x_\infty \\ x_{I\infty} \end{array}\right] + \left[\begin{array}{c} B \\ 0 \end{array}\right] u_\infty + \left[\begin{array}{c} w \\ -r \end{array}\right] }

を得ます(x_{I\infty}は定数ベクトル)。まず、(9)から(22)を引いて、つぎの偏差系を得ます。

偏差系E1：
\displaystyle{(23)\quad \boxed{\frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] %}_{x_E(t)-x_{E\infty}} = \underbrace{ \left[\begin{array}{cc} A & 0 \\ C & 0 \end{array}\right] }_{A_{E1}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] %}_{x_E(t)-x_{E\infty}} + \underbrace{ \left[\begin{array}{c} B \\ 0 \end{array}\right] }_{B_{E1}} (u(t)-u_\infty)} }

この両辺を微分すれば、状態変数の中の定数ベクトルを除くことができて

偏差系E2：
\displaystyle{(24)\quad \boxed{\frac{d}{dt} %\underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ z(t)-r \end{array}\right] %}_{\dot{x}_E(t)} = \underbrace{ \left[\begin{array}{cc} A & 0 \\ C & 0 \end{array}\right] }_{A_{E2}} %\underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ z(t)-r \end{array}\right] %}_{\dot{x}_E(t)} + \underbrace{ \left[\begin{array}{c} B \\ 0 \end{array}\right] }_{B_{E2}} {\dot u}(t)} }

を得ます。さらに、(1)と(3)をまとめた

\displaystyle{(25)\quad \left[\begin{array}{c} {\dot x}(t)-w \\ z(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \left[\begin{array}{c} x(t) \\ u(t) \end{array}\right] }

から(5)を引いて、つぎの関係式が成り立ちます。

\displaystyle{(26)\quad \boxed{\left[\begin{array}{c} {\dot x}(t) \\ z(t)-r \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right]} }

これに基づいて、偏差系E2に座標変換を行えば

偏差系E3：
\displaystyle{(27)\quad \boxed{\frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} + \underbrace{ \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }_{B_{E3}} {\dot u}(t)} }

を得ます。ここで、つぎの関係式を用いました。

\displaystyle{(28)\quad \underbrace{ \left[\begin{array}{cc} A & 0 \\ C & 0 \end{array}\right] }_{A_{E2}} \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} }

\displaystyle{(29)\quad \underbrace{ \left[\begin{array}{c} B \\ 0 \end{array}\right] }_{B_{E2}} = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }_{B_{E3}} }

●これら３つの偏差系E1,E2,E3はすべて可制御ですが、どれを用いるのがよいのでしょう？

偏差系E1に対する安定化状態フィードバックを

\displaystyle{(30)\quad u(t)-u_\infty=- \underbrace{\left[\begin{array}{cc} F & F_I \end{array}\right] }_{F_{E1}} \left[\begin{array}{c} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] }

とします。これによりA_{EF}=A_{E1}-B_{E1}F_{E1}を安定行列とすることはできますが、(30)は実装上の難があります。

偏差系E2に対する安定化状態フィードバックを

\displaystyle{(31)\quad {\dot u}(t)=- \underbrace{\left[\begin{array}{cc} F & F_I \end{array}\right] }_{F_{E2}} \left[\begin{array}{c} {\dot x}(t) \\ z(t)-r \end{array}\right] }

とします。これによりA_{EF}=A_{E2}-B_{E2}F_{E2}を安定行列とすることはできますが、LQ設計時の重み係数の決定に難があります。

偏差系E3に対する安定化状態フィードバックを

\displaystyle{(32)\quad {\dot u}(t)=- \underbrace{\left[\begin{array}{cc} K & K_I \end{array}\right] }_{K_{E3}} \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }

とします。これによりA_{E3}-B_{E3}K_{E3}を安定行列とできます。一方、(32)は(26)を用いると

\displaystyle{(33)\quad {\dot u}(t)=- \underbrace{\left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]^{-1} }_{K_{E3}S^{-1}} \left[\begin{array}{c} {\dot x}(t) \\ z(t)-r \end{array}\right] }

と書けます。したがって

\displaystyle{(34)\quad A_{EF}=A_{E2}-B_{E2}F_{E2}=SA_{E3}S^{-1}-SB_{E3}K_{E3}S^{-1}=S(A_{E3}-B_{E3}K_{E3})S^{-1} }

も安定行列となります。そして、

\displaystyle{(35)\quad \boxed{\left[\begin{array}{cc} F & F_I \end{array}\right] = \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]^{-1}} }

とおいて、(33)の両辺を積分すれば、制御測(7)が得られます。このことは偏差系E3の優位性を示唆しているといえます。

[4]　以下に、偏差系E3をLQ制御により安定化して、積分動作を加えた状態フィードバックを構成する手順を示します。

アルゴリズム　＜LQI制御＞

ステップ1　被制御変数の決定

\left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]が正則となるように(C=C_SC_M)、セレクタ行列C_S(m\times p)を決めます（一般に、多入力多出力系の場合、どの操作変数でどの被制御変数を制御するのかについて、物理的に実現可能な1対1対応を考えることが重要です。その際、被制御変数はフィードバックされるので観測量の中から選ばれなけばなりません）。

ステップ2　偏差系の安定化

偏差系

\displaystyle{(36)\quad \frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} + \underbrace{ \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }_{B_{E}} {\dot u}(t) }

を、状態フィードバック

\displaystyle{(37)\quad {\dot u}(t)=- \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }

によるLQ制御で安定化します。その際、評価関数としては

\displaystyle{(38)\quad \int_0^\infty \left( \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right]^T Q_E \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] +{\dot u}^T(t)R_E{\dot u}(t)\right)\,dt }

を用います。ただし、(A_E,Q_E^{\frac{1}{2}})は可観測対とします。

ステップ3　積分動作を加えた状態フィードバックの構成

つぎの積分動作を加えた状態フィードバックを構成します。

\displaystyle{(39)\quad \boxed{u(t)=-Fx(t)-F_I\int_0^t(z(\tau)-r)\,d\tau} }

ただし

\displaystyle{(40)\quad \boxed{\left[\begin{array}{cc} F & F_I \end{array}\right] = \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]^{-1}} }

この手順で設計された積分動作を加えた状態フィードバックによる制御方式をLQI制御(LQ control with integral action)と呼びます。