2024.3.4 Euler法によるばね-マス-ダンパ系のシミュレーション

その１は入力無し

その２はLQR

その３はP制御

その４はPID制御

いずれもNumPyを利用

$ \left[ \begin{array}{c} \dot{y} (t) \\ \ddot{y}(t) \end{array} \right] = \left[ \begin{array}{cc} 0 &1\\ -k/m & - c/m \end{array} \right] \left[ \begin{array}{c} y(t) \\ \dot{y}(t) \end{array} \right] + \left[ \begin{array}{c} 0 \\ 1/m \end{array} \right] \left[ \begin{array}{c} 0 \\ u(t) \end{array} \right]

その１（入力なし）

$ \dot{\mathbf{x}}(t) = A\mathbf{x}(t)

code:euler1.py

import numpy as np

import matplotlib.pyplot as plt

def xd(x):

return A@x # Aはグローバル変数

def solve_euler(x0, t_span, h):

hist_t, hist_x = [], []

x = np.array(x0, dtype=float).reshape(2, 1)

hist_t.append(t)

hist_x.append(x.flatten())

x = x + h*xd(x)

t = t + h

return np.stack(hist_t), np.stack(hist_x)

h = 0.01 # 時間の刻み幅

m, c, k = 1.0, 1.0, 1.0 # 質量、減衰係数、ばね定数

##

hist_t, hist_x = solve_euler(x0, t_span, h)

plt.grid()

plt.legend()

plt.show()

パラメータはビルトイン型で与え、必要に応じてndarray, float型などに変換する。

その２（LQR）

scipy.linalg.solve_continuous_areを用いてリカッチ代数方程式を解きフィードバックゲインを求める。

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

$ \mathbf{u} = F\mathbf{x}

$ F = -R^{-1} B^\intercal P

code:euler2.py

import numpy as np

from numpy.linalg import inv as inv

from scipy.linalg import solve_continuous_are as care

import matplotlib.pyplot as plt

def xd(x):

return (A + B@F)@x # A, B, Fはグルーバル変数

def solve_euler(x0, t_span, h):

hist_t, hist_x = [], []

x = np.array(x0, dtype=float).reshape(dim_m,1) # dim_mはグローバル変数

hist_t.append(t)

hist_x.append(x.flatten())

x = x + h*xd(x)

t = t + h

return np.stack(hist_t), np.stack(hist_x)

###

h = 0.01 # 時間の刻み幅

m, c, k = 1.0, 1.0, 1.0 # 質量、減衰係数、ばね定数

###

dim_m, dim_n = B.shape # m状態-n入力

P = care(A, B, Q ,R)

F = -inv(R)@B.T@P

hist_t, hist_x = solve_euler(x0, t_span, h)

plt.grid()

plt.legend()

plt.show()

状態数mはdim_mとすることで、台車の質量mとの変数名の重複を避ける。

その３(P制御)

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

$ \mathbf{y}=C\mathbf{x}

$ \mathbf{e} = \mathbf{r} - \mathbf{y}

$ \mathbf{u} = K_p \mathbf{e}

code:euler3.py

import numpy as np

import matplotlib.pyplot as plt

# A, B, Kp, r はグローバル変数

def xd(x):

y = C@x

e = r - y

u = Kp@e

xd = A@x + B@u

return xd

def solve_euler(x0, t_span, h):

hist_t, hist_x = [], []

x = np.array(x0, dtype=float).reshape(dim_m, 1)

hist_t.append(t)

hist_x.append(x.flatten())

x = x + h*xd(x)

t = t + h

return np.stack(hist_t), np.stack(hist_x)

##

h = 0.01 # 時間の刻み幅

m, c, k = 1.0, 1.0, 1.0 # 質量、減衰係数、ばね定数

r = 1 # 目標

Kp = 1 # Pゲイン、適当に設定

##

dim_m, dim_n = B.shape # n-状態, m-入力

Kp = np.array(Kp).reshape(1, 1)

hist_t, hist_x = solve_euler(x0, t_span, h)

plt.grid()

plt.legend()

plt.show()

その４（PID)

PID制御を行う。

$ \dot{\mathbf{x}}(t) = A \mathbf{x}(t) + B \mathbf{u}(t)

$ \mathbf{y}(t) = C \mathbf{x}(t)

$ \mathbf{e}(t) = \mathbf{r}(t) - \mathbf{y}(t)

$ \mathbf{u}(t) = K_p \mathbf{e}(t) + K_i \int_{0}^{t} \mathbf{e}(\tau) d \tau + K_d \dot{\mathbf{e}}(t)

関数部をクラス化し、前ステップにおける偏差(e_pre)と積分値(e_int)をインスタンス変数で保持している。C言語であればstatic変数で解決できる。

code:euler4.py

import numpy as np

import matplotlib.pyplot as plt

class Func_xd:

def __init__(self, params, h):

A, B, C, Kp, Ki, Kd, r = params

self.h = h # 時間の刻み幅

self.r = r # 目標値

self.A, self.B, self.C = A, B, C

self.Kp, self.Ki, self.Kd = Kp, Ki, Kd

self.e_int = 0 # 偏差の積分値の初期値

self.e_pre = 0 # 偏差の初期値

def __call__(self, _, x):

return self.xd(x)

def xd(self, x):

y = self.C@x

e_now = self.r - y

self.e_int = self.e_int + e_now*self.h

e_dif = (e_now - self.e_pre)/self.h

u = self.Kp@e_now + self.Ki*self.e_int + self.Kd*e_dif

xd = self.A@x + self.B@u

self.e_pre = e_now

return xd

def solve_euler(x0, t_span, h, params):

hist_x, hist_t = [], []

xd = Func_xd(params, h)

x = np.array(x0, dtype=float).reshape(dim_m, 1)

hist_t.append(t)

hist_x.append(x.flatten())

x = x + h*xd(t, x)

t = t + h

return np.stack(hist_t), np.stack(hist_x)

##

h = 0.01 # 時間の刻み幅

m, c, k = 1.0, 1.0, 1.0 # 質量、減衰係数、ばね定数

r = 1.0 # 目標

Kp = 1.0 # Pゲイン、適当に設定

Ki = 1.0 # Iゲイン、適当に設定

Kd = 1.0 # Dゲイン、適当に設定

##

dim_m, dim_n = B.shape # n-状態, m-入力

Kp = np.array(Kp).reshape(1, 1)

Ki = np.array(Ki).reshape(1, 1)

Kd = np.array(Kd).reshape(1, 1)

params = (A, B, C, Kp, Ki, Kd, r)

hist_t, hist_x = solve_euler(x0, t_span, h, params)

plt.grid()

plt.legend()

plt.show()