３慣性モデルで工作機械の制御系を設計する（2）状態方程式を立てる

# はじめに
前回の記事で運動方程式と、変数の数値を定義しました。
今回はこれらを使って状態方程式を立てます。

## 運動方程式（前回掲載）
``` math
\begin{align}

M_1\frac{d^2x_1}{dt^2}-D_{12}(\frac{dx_1}{dt}-\frac{dx_2}{dt})-K_{12}(x_1-x_2)&=f(t)\cdots (1)\\

M_2\frac{d^2x_2}{dt^2}-D_{12}(\frac{dx_2}{dt}-\frac{dx_1}{dt})-K_{12}(x_2-x_1)&=D_{23}(\frac{dx_2}{dt}-\frac{dx_3}{dt})+K_{23}(x_2-x_3) \cdots (2)\\

M_3\frac{d2x_3}{dt^2} &= D_{23}(\frac{dx_3}{dt}-\frac{dx_2}{dt})+K_{23}(x_3-x_2) \cdots (3)\\
\end{align}

```

# 状態方程式の導出

状態方程式は運動方程式を1階の微分方程式で表現する物です。
今回の運動方程式では$$ \frac{d^2x}{dt^2} $$ のような形で2階の微分が出てきますが、これを1階に変換していきます。

## 状態変数の定義
まず（1）式から。状態変数を定義します。
$ x_1 $を1階微分したものを$ z_1 $とします。ここからは1階微分を以下のようにドットで示します。ドットが2つだったら2階微分です。

$$ z_1 =\frac{dx_1}{dt}=\dot{x_1}\\ $$
$$ z_2 =\frac{dx_2}{dt}=\dot{x_2}\\ $$
$$ z_3 =\frac{dx_3}{dt}=\dot{x_3}\\ $$

この2つの式は1階微分なのでドット1個です。

これを使って式（1）を置き換えていくと、
$$ M_1\dot{z_1}-D_{12}(z_1-z_2)-K_{12}(x_1-x_2)=f \;\cdots (4)\\ $$

ドット1つなので1階微分ですね。1階微分を別の変数で置き換えただけともいいますが。

同様に式（2）と式（3）も置き換えてみます。

``` math

M_2\dot{z_2}-D_{12}(z_2-z_1)-K_{12}(x_2-x_1)=D_{23}(z_2-z_3)+K_{23}(x_2-x_3) \cdots (5)\\

M_3\dot{z_3} = D_{23}(z_3-z_2)+K_{23}(x_3-x_2) \cdots (6)\\

```

これで全部1階の微分方程式になりました。

## 状態変数をベクトルにする

次は状態変数をベクトルにします。要は縦のベクトルに並べます。順番はなんでもいいのですが、並べてみましょう

``` math
X=
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
\cdots (7)
```

## 状態空間の形に変換する

状態変数が決まったので、状態空間に変えていきます。

よくある状態空間は以下のような式で表現します。

``` math

\left\{
\begin{array}{l}

\dot{X} = AX(t)+BU(t)\\
Y = CX(t)+DU(t)
\end{array}
\right.
\cdots (8)
```
上の式が状態方程式、下の式が出力方程式です。

### 状態方程式
（4）、（5）、（6）式を元にして、状態方程式の形にします。
(7)式で状態変数のベクトルはできているので、これに合わせる形で$ AX(t)+BU(t) $を作っていきます。
最後にできる式の形は$ \dot{X} = AX(t)+BU(t)$ です。

両辺に$\dot{X}$とXがある形なので、まずは$\dot{X}$を左辺に、Xを右辺に集めましょう。

``` math

\begin{array}{lcr}
M_1\dot{z_1}=D_{12}(z_1-z_2)+K_{12}(x_1-x_2)+f\\ 
M_2\dot{z_2}=D_{23}(z_2-z_3)+K_{23}(x_2-x_3)+D_{12}(z_2-z_1)+K_{12}(x_2-x_1)\\
M_3\dot{z_3} = D_{23}(z_3-z_2)+K_{23}(x_3-x_2)\\
\end{array}

```

続きます。状態変数Xにはxも入っていました。

``` math

z_1 =\frac{dx_1}{dt}=\dot{x_1}\\ 
z_2 =\frac{dx_2}{dt}=\dot{x_2}\\ 
z_3 =\frac{dx_3}{dt}=\dot{x_3}\\

```
こんな形でした。
これも左辺にドットのついてる変数が来るように並べ替えます。

``` math

\dot{x_1}=z_1\\ 
\dot{x_2}=z_2\\ 
\dot{x_3}=z_3\\

```

材料が揃ったのでXの順番に並べてみます。

``` math

\begin{array}{lcr}
\dot{x_1}=z_1\\ 
\dot{z_1}=\frac{D_{12}}{M_1}(z_1-z_2)+\frac{K_{12}}{M_1}(x_1-x_2)+\frac{f}{M_1}\\ 
\dot{x_2}=z_2\\ 
\dot{z_2}=\frac{D_{23}}{M_2}(z_2-z_3)+\frac{K_{23}}{M_2}(x_2-x_3)+\frac{D_{12}}{M_2}(z_2-z_1)+\frac{K_{12}}{M_2}(x_2-x_1)\\
\dot{x_3}=z_3\\
\dot{z_3} = \frac{D_{23}}{M_3}(z_3-z_2)+\frac{K_{23}}{M_3}(x_3-x_2)\\
\end{array}\cdots(9)

```
これを行列に変換していきます。AとBはまだわからないので？を入れました。

``` math
\dot{
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
}
=
\begin{pmatrix}
? & ? & ? & ? & ? & ?\\
? & ? & ? & ? & ? & ?\\
? & ? & ? & ? & ? & ?\\
? & ? & ? & ? & ? & ?\\
? & ? & ? & ? & ? & ?\\
? & ? & ? & ? & ? & ?\\
\end{pmatrix}
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
+\begin{pmatrix}
?\\
?\\
?\\
?\\
?\\
?\\
\end{pmatrix}
U(t)
```

さて、ここから？に何が入るか並べ替えです。
一番簡単な$\dot{x_n}=z_n$の関係があるので、これを入れていきましょう。


``` math
\dot{
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
}
=
\begin{pmatrix}
0 & 1 & 0 & 0 & 0 & 0\\
? & ? & ? & ? & ? & ?\\
0 & 0 & 0 & 1 & 0 & 0\\
? & ? & ? & ? & ? & ?\\
0 & 0 & 0 & 0 & 0 & 1\\
? & ? & ? & ? & ? & ?\\
\end{pmatrix}
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
+\begin{pmatrix}
0\\
?\\
0\\
?\\
0\\
?\\
\end{pmatrix}
U(t)
```

次です。式（4）を入れていきます。
$ \dot{z_1}-\frac{D_{12}}{M_1}(z_1-z_2)-\frac{K_{12}}{M_1}(x_1-x_2)=\frac{f}{M_1} \;\cdots (4)\\ $
こんな形でしたので、$x_1$,$z_1$,$x_2$,$z_2$の項があります。
xとzの無い項、fはBのベクトルに入れます。

``` math
\dot{
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
}
=
\begin{pmatrix}
0 & 1 & 0 & 0 & 0 & 0\\
\frac{K_{12}}{M_1} & \frac{D_{12}}{M_1} & -\frac{K_{12}}{M_1} & -\frac{D_{12}}{M_1} & 0 & 0\\
0 & 0 & 0 & 1 & 0 & 0\\
? & ? & ? & ? & ? & ?\\
0 & 0 & 0 & 0 & 0 & 1\\
? & ? & ? & ? & ? & ?\\
\end{pmatrix}
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
+\begin{pmatrix}
0\\
\frac{f}{M_1}\\
0\\
?\\
0\\
?\\
\end{pmatrix}
U(t)
```


次は（5）式です。

$ \dot{z_2}=\frac{D_{23}}{M_2}(z_2-z_3)+\frac{K_{23}}{M_2}(x_2-x_3)+\frac{D_{12}}{M_2}(z_2-z_1)+\frac{K_{12}}{M_2}(x_2-x_1)\\ $

これを入れると、状態方程式は以下のようになります。あと1行だ！

``` math
\dot{
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
}
=
\begin{pmatrix}
0 & 1 & 0 & 0 & 0 & 0\\
\frac{K_{12}}{M_1} & \frac{D_{12}}{M_1} & -\frac{K_{12}}{M_1} & -\frac{D_{12}}{M_1} & 0 & 0\\
0 & 0 & 0 & 1 & 0 & 0\\
-\frac{K_{12}}{M_2} & -\frac{D_{12}}{M_2} & \frac{K_{12}+K_{23}}{M_2} & \frac{D_{12}+D_{23}}{M_2} & -\frac{K_{23}}{M_2} & -\frac{D_{23}}{M_2}\\
0 & 0 & 0 & 0 & 0 & 1\\
? & ? & ? & ? & ? & ?\\
\end{pmatrix}
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
+\begin{pmatrix}
0\\
\frac{f}{M_1}\\
0\\
0\\
0\\
?\\
\end{pmatrix}
U(t)
```


最後、$\dot{z_3}$の列です。
$ \dot{z_3} = D_{23}(z_3-z_2)+K_{23}(x_3-x_2)\\ $
これも順番直してみてから代入します。
$ \dot{z_3} = -K_{23}x_2-D_{23}z_2+K_{23}x_3+D_{23}z_3\\ $
こういう順番変えたりとかするときに書き間違いが発生します。自動化したい。

``` math
\dot{
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
}
=
\begin{pmatrix}
0 & 1 & 0 & 0 & 0 & 0\\
\frac{K_{12}}{M_1} & \frac{D_{12}}{M_1} & -\frac{K_{12}}{M_1} & -\frac{D_{12}}{M_1} & 0 & 0\\
0 & 0 & 0 & 1 & 0 & 0\\
-\frac{K_{12}}{M_2} & -\frac{D_{12}}{M_2} & \frac{K_{12}+K_{23}}{M_2} & \frac{D_{12}+D_{23}}{M_2} & -\frac{K_{23}}{M_2} & -\frac{D_{23}}{M_2}\\
0 & 0 & 0 & 0 & 0 & 1\\
0 & 0 & -\frac{K_{23}}{M_3} & -\frac{D_{23}}{M_3} & \frac{K_{23}}{M_3} & \frac{D_{23}}{M_3}\\
\end{pmatrix}
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
+\begin{pmatrix}
0\\
\frac{f}{M_1}\\
0\\
0\\
0\\
0\\
\end{pmatrix}
U(t)\cdots(10)
```

できました〜
# まとめ

状態変数のベクトルを定義して、それに対応する、A,Bの定数を算出しました。

``` math
X=\left[\begin{array}
\\
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{array}\right]
```

``` math
A=\left[\begin{array}
& 0 & 1 & 0 & 0 & 0 & 0\\
\frac{K_{12}}{M_1} & \frac{D_{12}}{M_1} & -\frac{K_{12}}{M_1} & -\frac{D_{12}}{M_1} & 0 & 0\\
0 & 0 & 0 & 1 & 0 & 0\\
-\frac{K_{12}}{M_2} & -\frac{D_{12}}{M_2} & \frac{K_{12}+K_{23}}{M_2} & \frac{D_{12}+D_{23}}{M_2} & -\frac{K_{23}}{M_2} & -\frac{D_{23}}{M_2}\\
0 & 0 & 0 & 0 & 0 & 1\\
0 & 0 & -\frac{K_{23}}{M_3} & -\frac{D_{23}}{M_3} & \frac{K_{23}}{M_3} & \frac{D_{23}}{M_3}\\
\end{array}\right]
```

``` math
B=\left[\begin{array}
&0\\
\frac{f}{M_1}\\
0\\
0\\
0\\
0\\
\end{array}\right]
```

#最後に
途中で計算式にミスが見つかったら大変だよなーとか考えていて、定数の符号はチェックしていたのですが・・・
やっぱりミスがありました。ちなみに質量のM1〜M3を入れ忘れて計算していたのでやっちまった感強いです。

次回は出力方程式の話を書こうと思いましたが、
こうです

``` math
C=
\left[
\begin{array}
& 0 & 0 & 0 & 0 & 1 & 0\\ 
\end{array}
\right]\\

D＝\left[
0
\right]

```

$x_3$を出すだけでした。