はじめに
前回の記事で運動方程式と、変数の数値を定義しました。
今回はこれらを使って状態方程式を立てます。
運動方程式（前回掲載）
\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）も置き換えてみます。
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階の微分方程式になりました。
状態変数をベクトルにする
次は状態変数をベクトルにします。要は縦のベクトルに並べます。順番はなんでもいいのですが、並べてみましょう
X=
\begin{pmatrix}
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{pmatrix}
\cdots (7)
状態空間の形に変換する
状態変数が決まったので、状態空間に変えていきます。
よくある状態空間は以下のような式で表現します。
\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を右辺に集めましょう。
\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も入っていました。
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}\\
こんな形でした。
これも左辺にドットのついてる変数が来るように並べ替えます。
\dot{x_1}=z_1\\
\dot{x_2}=z_2\\
\dot{x_3}=z_3\\
材料が揃ったのでXの順番に並べてみます。
\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はまだわからないので？を入れました。
\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$の関係があるので、これを入れていきましょう。
\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のベクトルに入れます。
\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行だ！
\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\ $
こういう順番変えたりとかするときに書き間違いが発生します。自動化したい。
\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の定数を算出しました。
X=\left[\begin{array}
\\
x_1\\
z_1\\
x_2\\
z_2\\
x_3\\
z_3\\
\end{array}\right]
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]
B=\left[\begin{array}
&0\\
\frac{f}{M_1}\\
0\\
0\\
0\\
0\\
\end{array}\right]
最後に
途中で計算式にミスが見つかったら大変だよなーとか考えていて、定数の符号はチェックしていたのですが・・・
やっぱりミスがありました。ちなみに質量のM1〜M3を入れ忘れて計算していたのでやっちまった感強いです。
次回は出力方程式の話を書こうと思いましたが、
こうです
C=
\left[
\begin{array}
& 0 & 0 & 0 & 0 & 1 & 0\\
\end{array}
\right]\\
D＝\left[
0
\right]
$x_3$を出すだけでした。