バネ・マス・ダンパ系の伝達関数・状態空間モデルとPythonによるシミュレーション

# 概要

制御工学の勉強メモ。バネ・マス・ダンパ系の質点の運動方程式から、伝達関数／状態空間モデルを求めて、制御系のPythonライブラリ「[Python Control Systems Library](https://python-control.readthedocs.io/en/0.8.3/index.html)」を使ってシミュレーションをします。
![2020-04-27_23h38_44.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/7c371903-d2a1-1d30-6bcc-cbe20ad20420.png)

- 関連エントリ：[RLC直列回路の伝達関数・状態空間モデルとPythonによるシミュレーション](https://qiita.com/code0327/items/423b0f0380e8c64f3580)

# バネ・マス・ダンパ系における1自由度の質点の運動方程式

$$ m\ddot{y}(t) + c\dot{y}(t) + ky(t) = f(t)$$

$\ddot{y}(t)$ は加速度、$\dot{y}(t)$ は速度、$y(t)$ は変位。$m$ は質量、$c$ は減衰係数、$k$ はバネ定数である（いずれも物理定数なので非負の値）。

# 伝達関数でシステムをモデル化

力 $f(t)$ をシステムに対する**入力**、変位 $y(t)$ をシステムからの**出力**として、ラプラス変換して、システムの**伝達関数** $G(s)=Y(s)/F(s)$ を求めると次のようになる（この際、速度と変位の初期値はゼロ、つまり $\dot{y}(0)=0$ と $y(0)=0$ とする）。

$$ ms^2Y(s) + c sY(s) + kY(s) = F(s)$$

$$ \big(ms^2+c s + k\big) Y(s) = F(s)$$

$$ G(s) = \frac{Y(s)}{F(s)} = \frac{1}{m s^2 + c s + k}$$ 

# Pythonでモデル化・シミュレーション

Python の **Python Control Systems Library** を利用して、シミュレーションをします。伝達関数を設定してシステムをモデル化して、**インパルス応答**をシミュレーションします。

```py:準備（GoogleColab.環境）
!pip install --upgrade sympy
!pip install slycot
!pip install control
```

```py:インパルス応答のシミュレーション（伝達関数）
import numpy as np
import control.matlab as ctrl
import matplotlib.pyplot as plt

m = 1    # 質量 [kg] 非負
c = 1    # 減衰係数 [N/m] 非負
k = 10   # バネ係数 [Ns/m] 非負

sys = ctrl.tf((1),(m,c,k)) # 伝達関数
print(sys)

t_range = (0,10) # 0～10秒の範囲をシミュレーション
y, t = ctrl.impulse(sys, T=np.arange(*t_range, 0.01))

plt.figure(figsize=(7,4),dpi=120,facecolor='white')
plt.hlines(0,*t_range,colors='gray',ls=':')
plt.plot(t,y)
plt.xlim(*t_range)
plt.show()
```

```:実行結果
           1
-----------------------
1e-08 s^2 + 2e-05 s + 1
```

![f1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/6cdac5a0-bca6-67b7-50f9-41b60e9dd3bb.png)


# 状態空間モデルでモデル化

状態空間モデルでは、複数の観測量（出力）を設定できる。また、任意の初期値で計算が可能である。

システムを、次のような状態空間モデル $\mathcal{P}$ にモデル化する。

```math:
\mathcal{P}\,:\,\left\{
\begin{array}{l}
\dot{\boldsymbol{x}} = \mathbf{A}\boldsymbol{x} + \mathbf{B}\boldsymbol{u}& 状態方程式\\
\boldsymbol{y} = \mathbf{C}\boldsymbol{x} + \mathbf{D}\boldsymbol{u}& 出力方程式（観測方程式）
\end{array}
\right.
```

準備として、運動方程式を $\ddot{y}(t)=...$ に変形しておく。

$$ m\ddot{y}(t) + c\dot{y}(t) + ky(t) = f(t)$$

$$ m\ddot{y}(t) = - ky(t) - c\dot{y}(t) + f(t)$$

$$ \ddot{y}(t) = - \frac{k}{m}y(t) - \frac{c}{m} \dot{y}(t) +\frac{1}{m} f(t)$$

$x\_1(t)=y(t)$、$x\_2(t)=\dot{y}(t)$ として、**状態**　$\boldsymbol{x}=\[x\_1\,\,x\_2]^T$ を定める。<font color='red'>**変位** $y(t)$ を状態 $x_1$、**速度** $\dot{y}(t)$ を状態 $x_2$ とした。</font>

これより、$\dot{x}\_1(t)=\dot{y}(t)=x\_2$ となる。

また、$\dot{x}\_2(t)=\ddot{y}(t)= - \frac{k}{m}y(t) - \frac{c}{m} \dot{y}(t) +\frac{1}{m} f(t)$ となる。

力 $f(t)$ を、入力 $u(t)$ とすれば、以上より、次の**状態方程式**を得る。

```math:
\left[
    \begin{array}{c}
      \dot{x}_1 \\
      \dot{x}_2
    \end{array}
  \right]
=\left[
    \begin{array}{cc}
      0 & 1  \\
      -\frac{k}{m} & -\frac{c}{m} 
    \end{array}
  \right]
  \left[
    \begin{array}{c}
      x_1 \\
      x_2
    \end{array}
  \right]
+ \left[
    \begin{array}{c}
      0 \\
      \frac{1}{m}
    \end{array}
  \right] u
```

```math:
\dot{\boldsymbol{x}} 
=\left[
    \begin{array}{cc}
      0 & 1  \\
      -\frac{k}{m} & -\frac{c}{m} 
    \end{array}
  \right]
\boldsymbol{x} + \left[
    \begin{array}{c}
      0 \\
      \frac{1}{m}
    \end{array}
  \right] u
```

$$\dot{\boldsymbol{x}} 
=\mathbf{A}\boldsymbol{x} + \mathbf{B} u$$

また、次の**出力方程式**（観測方程式）を得る（**変位**に相当する状態 $x_1$ と、**速度**に相当する状態 $x_2$ を観測する形）。

```math:
y = \left[
    \begin{array}{cc}
      1 & 0 \\
      0 & 1 \\
    \end{array}
  \right]   \left[
    \begin{array}{c}
      x_1 \\
      x_2
    \end{array}
  \right]
```

$$y
=\mathbf{C}\boldsymbol{x} + \mathbf{D} u$$

ただし、$\mathbf{D}=0$。

# Pythonでモデル化・シミュレーション

状態空間モデルで、**インパルス応答**をシミュレーションします。伝達関数の場合と違って、変位の初期値として $0.1$ を設定し、速度についても出力（観測）します。

```py:グラフで日本語利用するための準備（GoogleColab.環境）
!pip install japanize-matplotlib
```

```py:インパルス応答のシミュレーション（状態空間モデル・初期値付き）
import numpy as np
import control.matlab as ctrl
import matplotlib.pyplot as plt
import japanize_matplotlib

m = 1    # 質量 [kg] 非負
c = 1    # 減衰係数 [N/m] 非負
k = 10   # バネ係数 [Ns/m] 非負

A = [[0,1],[-k/m, -c/m]]
B = [[0],[1/m]]
C = [[1,0],[0,1]]
D = 0

sys = ctrl.ss(A,B,C,D)
#print(sys)

t_range = (0,10)

# x1（=変位）の初期値を 0.1 に設定。x2（=速度）の初期値は 0 に設定
y, t = ctrl.impulse(sys, T=np.arange(*t_range, 0.01), X0=[0.1, 0.0])

fig, ax = plt.subplots(nrows=2, figsize=(7,5),dpi=120,sharex='col')
fig.patch.set_facecolor('white') 
fig.subplots_adjust(hspace=0.1)  
for i,s in enumerate(['変位','速度']) :
  ax[i].hlines(0,*t_range,colors='gray',ls=':')
  ax[i].plot(t,y[:,i])
  ax[i].set_ylabel(s)
  ax[i].set_xlim(*t_range)
plt.show()
```

実行結果は次のようになります。<br>![f2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152805/5c57cce3-43e4-9ffa-ac95-19e45dd4e1f9.png)