## 概要
現代制御の勉強メモです．実装するまでの過程をトレースしたかったので，それに必要な知識の確認程度に書いたものです．
内容は状態フィードバック則と同一次元状態オブザーバのまとめと，それを使って適当な制御対象を制御してみてみます．
なお授業で扱った内容を思い出すためのものである上，幾つか扱ってない部分があったり間違ってたり省略してたりするので，ちゃんとした説明は教科書見直してください．学部生なんだ許されてくれ．
## 勉強メモ
### メインコンセプト
次のような状態方程式と呼ばれる形で表された微分方程式がある．ただし\(x\)はベクトルで内部状態を表し，\(u\)は入力，\(y\)は出力である．また\(A,B,C,D\)は定数行列である．
\[
\begin{eqnarray}
\dot{x}&=&Ax+Bu\\
y&=&Cx+Du
\end{eqnarray}
\]
メインコンセプトは，この微分方程式の内部状態\(x\)を，0ベクトルにどうやって近づけるかということである．\(x\)が0ベクトルに収束するためには行列「\(A\)の固有値の実部がすべて負」でなければいけない．「\(A\)の固有値の実部がすべて負」の時，\(x\)は指数関数的に減衰していくことが知られているため，\(x\)は0ベクトルに収束する．
さて，実際にモデルを立てた時に「\(A\)の固有値の実部がすべて負」という条件が満たされていなかったとする．その場合，\(x\)は0に収束するどころか発散してしまう場合がある．しかし，このとき入力\(u\)をうまく与えてやることで擬似的に「\(A\)の固有値の実部をすべて負」にできる．その最も単純な方式として状態フィードバックを用いた極配置という方法を取る．
この方法では，入力\(u\)を次のような形で与える．ただし\(K\)は適当な定数行列である．
\[
u=Kx+v
\]
これを上式に代入すると次のようになる．
\[
\begin{eqnarray}
\dot{x}&=&(A+BK)x+Bv\\
y&=&Cx+Du
\end{eqnarray}
\]
したがって定数行列\(A\)が，擬似的に\(A+BK\)となった．したがって\(K\)をうまいこと決めてやれば，定数行列\(A\)の固有値を変えられる．固有値の実部が全て負になるような\(K\)にすれば，\(x\)は0ベクトルに収束し目的が達成できる．
さて，現代制御論のメインコンセプトは上に述べたとおりである．しかし上では語弊を承知で具体的な計算などはすべて除いた．そのため実際には安定化させられない条件というのがいくつかある．また，入力\(u\)は\(u=Kx+v\)とすればよいといったが，\(x\)がわからなければ\(u\)を決定できない．そのため\(x\)を推定する必要がある．
以下では制御ができるかできないかを表す指標「可制御性」，状態\(x\)が推定できるかできないかを表す指標「可観測性」について述べる．
### 状態フィードバック則(文献[1]P35)
先程定数行列\(K\)をうまく決めてやれば，定数行列\(A\)の固有値を擬似的に変えられることをのべた．実際には\(K\)はその要素を適当な変数で置いてやって固有値方程式
\[
\det(\lambda I-(A+BK))=0
\]
が，実部が負となる適当な固有値 \(\lambda_1,...,\lambda_n\)を自分で決めてやって，次式のようにその解がその固有値になるようにしてやれば良い．
\[
\det(\lambda I-(A+BK))=\prod_{i=1}^n(\lambda - \lambda_i)
\]
ただし\(\lambda_1,...,\lambda_n\)は係数が実数の多項式の解となるような状態(複素平面上においた時に実軸対称)でなければならない．
### 可制御性(文献[1]P35)
さて上記の式を実際に立ててみると連立方程式は\(K\)の要素に関する線形連立方程式になることが保証されているらしい．しかし妥当な解が得られないような場合がある．すなわち，思い通りに制御することができない場合がある．思い通りに制御できないことを不可制御性を持つと良い，逆に思い通りに制御できることを可制御であるという．
可制御であるということの必要十分条件は次の条件が満たされている場合である事が示されている(文献[1]P38)．
\[
rank M_c=rank [B \,\, AB\,\,\dots\,\,A^{n-1}B]=n
\]
この行列\(M_c\)は可制御性行列とよばれるもので，微分方程式の解析解を求めると出てくる
\[
\begin{eqnarray}
x(t)&=&e^{At}x(0)+\int_0^t e^{A(t-\tau)}Bu(\tau)d\tau\\
&=&e^{At}x(0)+\int_0^t (\beta_0 (t-\tau)I+\beta_1(t-\tau)A+\dots+\beta_{n-1}(t-\tau)A^{n-1})Bu(\tau)d\tau \\
&=&e^{At}x(0)+[B \,\, AB\,\,\dots\,\,A^{n-1}B][\int\beta_0udt \dots\int\beta_{n-1}udt ]^T\\
&=&e^{At}x(0)+M_c [\int\beta_0udt \dots\int\beta_{n-1}udt ]^T
\end{eqnarray}
\]
不可制御の場合はどうやっても不安定であったりするので，そういう場合は制御対象を何かしら変える必要がある．
### 同一次元状態オブザーバ(文献[1]P121)
状態フィードバック則では，入力\(u\)を決定するのに\(u=Kx+v\)としているから，内部状態\(x\)がわかっている必要がある．
普通は内部状態\(x\)を知ることはできず，ここでは出力\(y\)と入力\(u\)，および定数行列\(A,B,C\)のみから推定するしか無い．内部状態を推定する方法はいくつかあるらしいが同一次元状態オブザーバというものを紹介する．
まず\(x\)と同じ次元を持ち，次のような微分方程式で表される関数\(z(t)\)について考える．
\[
\dot{z}=Az+Bu+G(y-Cz)
\]
この\(z\)が\(x(t)=z(t)\)となってくれれば内部状態\(x\)がわかったことになる．ここで\(x\)と\(z\)の差\(e(t)=x(t)-z(t)\)について考えると，\(\dot{e}=\dot{x}-\dot{z}\)であるから，
\[
\begin{eqnarray}
\dot{e(t)}&=&A(x-z)-G(Cx-Cz)\\
&=&A(x-z)-GC(x-z)\\
&=&(A-GC)e(t)
\end{eqnarray}
\]
となるから，行列\(A-GC\)の固有値の実部が全て負であれば\(e\)は0に収束する，すなわち\(x=z\)となる．したがって\(A-GC\)の固有値の実部が全て負となるように\(G\)を状態フィードバック則と同じように決めてやればよい．
### 可観測性(文献[1]P41)
ただ同一次元状態オブザーバを作れない場合がある．すなわち，内部状態をどれだけ頑張っても出力\(y\)から知ることができない場合がある．そのようなシステムを不可観測であるといい，逆に内部状態を推定できるシステムを可観測であるという．
システムが可観測であるという必要十分条件は次の可観測性行列\(M_o\)が
\[
rank M_o = rank\begin{bmatrix}C\\CA\\\vdots\\CA^{n-1}\end{bmatrix}=n
\]
であることである．この可観測性行列は入力が0である場合の\(y\)の解析解
\[
\begin{eqnarray}
y&=&C e^{At}x(0)\\
&=&C(\beta_0 (t)I+\beta_1(t)A+\dots+\beta_{n-1}(t)A^{n-1})x(0)\\
&=&[\beta_0\dots \beta_{n-1}]M_o x(0)
\end{eqnarray}
\]
に出てくる行列である(文献[1]P43)．
## 具体例
### 運動方程式の導出
ここでは台車にくっついた倒立振子について考えます．ただし運動方程式の導出は面倒なので参考文献[2]のモデルを用います(というかこの運動方程式の導出がちゃんとできないとだめというか，現代制御ではモデル化ができてしまえば後は勝手に制御則が出てくるので，でもう少し運動方程式の導出を再勉強するべきな気がする)．
参考文献[2]より，台車の位置及び振り子の回転角度の運動方程式は次式のようになる．
\[
\begin{bmatrix}
M+m&ml\cos(\theta)\\
ml\cos(\theta)&J+ml^2
\end{bmatrix}
\begin{bmatrix}
\ddot{x}\\\ddot{\theta}
\end{bmatrix}
+
\begin{bmatrix}
-ml\dot{\theta}^2\sin\theta\\
-mlg\sin\theta\end{bmatrix}
+
\begin{bmatrix}
B\dot{x}\\C\dot{\theta}
\end{bmatrix}
=\begin{bmatrix}
u\\0
\end{bmatrix}
\]
ここで
\[
T=\begin{bmatrix}
M+m&ml\cos(\theta)\\
ml\cos(\theta)&J+ml^2
\end{bmatrix}
\]
とおけば
\[
\begin{eqnarray}
\begin{bmatrix}
\ddot{x}\\\ddot{\theta}
\end{bmatrix}
&=&T^{-1}\left(\begin{bmatrix}
ml\dot{\theta}^2\sin\theta\\
mlg\sin\theta\end{bmatrix}
+\begin{bmatrix}
-B&0\\0&-C
\end{bmatrix}\begin{bmatrix}
\dot{x}\\\dot{\theta}
\end{bmatrix}
+\begin{bmatrix}
u\\0
\end{bmatrix}\right)
\end{eqnarray}
\]
となる．数値解析する分にはこの程度の形で書けていればルンゲクッタでシミュレーションできる．
### 線形化
次に，現代制御を適用するために\(\dot{\theta},\theta\)が微小であるという近似をすると
\[
T=\begin{bmatrix}
M+m&ml\\
ml&J+ml^2
\end{bmatrix}
\]
であり，微分方程式は次のようになる．
\[
\begin{eqnarray}
\begin{bmatrix}
\ddot{x}\\\ddot{\theta}
\end{bmatrix}
&=&T^{-1}\left(\begin{bmatrix}
0&0\\
0&mlg\end{bmatrix}\begin{bmatrix}
{x}\\{\theta}
\end{bmatrix}
+\begin{bmatrix}
-B&0\\0&-C
\end{bmatrix}\begin{bmatrix}
\dot{x}\\\dot{\theta}
\end{bmatrix}
+\begin{bmatrix}
1\\0
\end{bmatrix}u\right)\\
&=&\frac{1}{\det T}\left(
\left[\begin{matrix}0 & - g l^{2} m^{2}\\0 & g l m \left(M + m\right)\end{matrix}\right]\begin{bmatrix}
{x}\\{\theta}
\end{bmatrix}\\+\left[\begin{matrix}- B \left(J + l^{2} m\right) & C l m\\B l m & - C \left(M + m\right)\end{matrix}\right]\begin{bmatrix}
\dot{x}\\\dot{\theta}
\end{bmatrix}
+\left[\begin{matrix}J + l^{2} m\\- l m\end{matrix}\right]u
\right)
\end{eqnarray}
\]
ここで状態量を(変数名かぶってますが)\(x=\begin{bmatrix}x&\dot{x}&\theta&\dot{\theta}\end{bmatrix}^T\)とすると
\[
\frac{d}{dt}x=\frac{1}{\det T}
\begin{bmatrix}
0&\det T&0&0\\
0&-B(J+ml^2)&-m^2l^2g&Cml\\
0&0&0&\det T\\
0&Blm&mgl(M+m)&-C(M+m)
\end{bmatrix}x+
\frac{1}{\det T}
\begin{bmatrix}
0\\J+ml^2\\0\\-ml
\end{bmatrix}u
\]
となるから，
\[
\begin{eqnarray}
A&=&\frac{1}{\det T}
\begin{bmatrix}
0&\det T&0&0\\
0&-B(J+ml^2)&-m^2l^2g&Cml\\
0&0&0&\det T\\
0&Blm&mgl(M+m)&-C(M+m)
\end{bmatrix}\\
B&=&
\frac{1}{\det T}
\begin{bmatrix}
0\\J+ml^2\\0\\-ml
\end{bmatrix}\\
C&=&
\begin{bmatrix}
1&0&0&0\\
0&0&1&0\\
\end{bmatrix}\\
\end{eqnarray}
\]
とおけば線形化した場合の状態方程式が書ける．
### オブザーバ，フィードバックゲインの決定
オブザーバ，フィードバックゲインは手計算で求めるとめんどくさそうなのでライブラリを使います．python-controlやmatlabにはplace関数というものがあって，それを用いるとフィードバックゲインなどを求めることができます．
なお，place関数は\(A+BK\)の\(K\)を決めてくれる関数です．したがって状態オブザーバのゲイン\(G\)を決める方法については，\(A-GC\)の転置をとった\(A^T-C^TG^T\)の\(G^T\)を求めることをすれば良いです．
コードはこんな感じです．
### シミュレーション
シミュレーションはProcessingで行いました．自分用の行列ライブラリ，ODEライブラリを作っておきたかったので読みにくいかもしれないです．
コードは
ここに上げてあります．
上が制御対象(RK4でシミュレーションした非線形微分方程式)，下がオブザーバの出力を可視化したものです．初期値は0にしてあり，キーボードの矢印キーで外乱(入力)を与えてやれば動きます．
制御していない場合は発散し，制御している場合にはちゃんと倒立していることがわかります．オブザーバの出力もだいたい上のと同じ感じになってます．
## 感想
大変だったけどいい経験になりました．あと先輩に聞くと結構授業では端折っているところがあったりするらしいので，もっと勉強しないとなと思いました．
## 参考文献
[1] 吉川恒夫，井村順一，現代制御論，(2014)，コロナ社
[2]
http://www.robot.mach.mie-u.ac.jp/~nkato/class/sc/Invpend_eq3.pdf
[3] Python で任意極配置のフィードバックゲインを求める | org-技術,
http://org-technology.com/posts/pole-placement.html
[4] python-controlを利用する際に直面したエラー - szmlb.net,
http://szmlb.hatenablog.com/entry/2015/09/08/203643
