PID 制御

2020年4月22日

はじめに

バネ-ダンパー-質点系について、PID 制御を行う。

バネ-ダンパー-質点系のシミュレーション

2 自由度のバネ-ダンパー-質点系を線形システム状態方程式として解く。SymPy を用いてラグランジュの手法で運動方程式を導出し、線形化する。

from sympy import symbols, diff, Matrix, lambdify
from sympy.physics.mechanics import dynamicsymbols
from sympy.physics.mechanics import ReferenceFrame, Point, Particle
from sympy.physics.mechanics import LagrangesMethod, Lagrangian

t = symbols("t")
z1, z2, f1, f2 = dynamicsymbols("z1 z2 f1 f2")
m1, m2, c1, c2, k1, k2 = symbols("m1 m2 c1 c2 k1 k2")
q = Matrix([z1, z2])

N = ReferenceFrame("N")

p1 = Point("p1")
v1 = z1.diff(t)
p1.set_vel(N, v1*N.x)
pa1 = Particle("pa1", p1, m1)
pa1.potential_energy = k1*z1**2/2 + k2*(z2 - z1)**2/2

p2 = Point("p2")
v2 = z2.diff(t)
p2.set_vel(N, v2*N.x)
pa2 = Particle("pa2", p2, m2)

F = c1*v1**2/2 + c2*(v2 - v1)**2/2
fc1 = -F.diff(v1)
fc2 = -F.diff(v2)

fl = [(p1, (f1 + fc1)*N.x), (p2, (f2 + fc2)*N.x)]

L = Lagrangian(N, pa1, pa2)
LM = LagrangesMethod(L, q, forcelist = fl, frame = N)

LM.form_lagranges_equations()
As, Bs, u = LM.linearize(q_ind=q, qd_ind=q.diff(t), A_and_B=True)

具体的な数値を設定する。

m1n = 1.
m2n = 1.
c1n = 0.5
c2n = 0.5
k1n = 0.2
k2n = 0.2

As_func = lambdify((m1, m2, c1, c2, k1, k2), As, modules="numpy")
A0 = As_func(m1n, m2n, c1n, c2n, k1n, k2n)

Bs_func = lambdify((m1, m2), Bs, modules="numpy")
B0 = Bs_func(m1n, m2n)

C0 = [0., 1., 0., 0.]
x0 = [0.5, 1., 0., 0.]

dt = 0.1
end_time = 10.
t = np.linspace(0., end_time, int(end_time/dt) + 1)

線形システム状態方程式をシミュレートする。ここでは、PIC 制御のため SciPy などを用いずに自力で計算する。線形システム状態方程式を次式とする。

この連続時間表現を、次の離散時間表現で表す。

ここで \(A = e^{A_0 T}\)、\(B = (e^{A_0 T} - I) A_0^{-1} B_0\) で、\(e^{A_0 T}\) は行列指数関数、\(T = t_{k+1} - t_k\) はサンプリング周期、要するに時間刻み幅である。

以下では入出力は省略している。

from scipy.linalg import expm
from matplotlib import pyplot as plt
import seaborn as sns
sns.set()

n = len(x0)
A = expm(A0*dt)

steps = int(end_time/dt) + 1
x = np.zeros((steps, n))

x[0, :] = x0

x1 = np.array(x0)
for step in range(1, steps):
    x1[:] = A.dot(x1)
    x[step, :] = x1

plt.plot(t, x[:, 0], label="$x_1$")
plt.plot(t, x[:, 1], label="$x_2$")
plt.plot(t, x[:, 2], label="$x_3$")
plt.plot(t, x[:, 3], label="$x_4$")
plt.xlim(0., end_time)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend()
plt.show()

PID 制御

PID 制御は、比例 (P)、積分 (I)、微分 (D) の 3 つの動作で制御を行うものである。目標値を \(r(t)\) として、出力 \(y(t)\) との偏差 \(e(t) = r(t) - y(t)\) に対して、入力 \(u(t)\) を次のように決める。

\(k_P\)、\(k_I\)、\(k_D\) をそれぞれ比例ゲイン、積分ゲイン、微分ゲインという。これらは制御パラメータで、問題に合わせて調整する必要がある。パラメータの決め方の一つに限界感度法というものがあり、次のように決める。

1. \(k_P\) だけ効かせて、値を徐々に大きくしていって、出力が振動して発散傾向になる直前の値を特定する。これを限界感度 \(k_U\) とする。また、そのときの振動周期を限界周期 \(T_U\) とする。
2. 積分時間 \(T_I = 0.5 T_U\)、微分時間 \(T_D = 0.125 T_U\) とする。
3. \(k_P = 0.6 k_U\)、\(k_I = k_P/T_I\)、\(k_D = k_P T_D\) とする。
4. 上の値をベースにして、あとは個別に調整する。

以下の例では、\(x_2 = z_2\) を出力として、それに対して目標値 \(r = 0\) として PID 制御を適用している。

from numpy.linalg import inv

ku = 10. # 限界感度
tu = 2. # 限界周期
ti = 0.5*tu
td = 0.125*tu

#kp = 0.6*ku
#ki = kp/ti
#kd = kp*td
kp = 4.
ki = 1.
kd = 1.5
print("kp =", kp)
print("ki =", ki)
print("kd =", kd)

n = len(x0)
n2 = int(n/2)
A = expm(A0*dt)
B = (A - np.eye(n)).dot(inv(A0)).dot(B0)
C = np.array(C0)

r = 0.
u = np.zeros(n2)
cu = np.zeros(n2)
cu[-1] = 1.

e = 0.
e0 = 0.
int_e = 0.
diff_e = 0.

steps = int(end_time/dt) + 1
x = np.zeros((steps, n))

x[0, :] = x0

x1 = np.array(x0)
for step in range(1, steps):
    y = C.dot(x1)

    e = r - y
    int_e += 0.5*(e + e0)*dt
    diff_e = (e - e0)/dt
    u[:] = (kp*e + ki*int_e + kd*diff_e)*cu
    e0 = e

    x1[:] = A.dot(x1) + B.dot(u)
    x[step, :] = x1

plt.plot(t, x[:, 0], label="$x_1$")
plt.plot(t, x[:, 1], label="$x_2$")
plt.plot(t, x[:, 2], label="$x_3$")
plt.plot(t, x[:, 3], label="$x_4$")
plt.xlim(0., end_time)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend()
plt.show()

参考

• 大住晃: 構造物のシステム制御, 森北出版 (2013).
• 川田昌克 編著: 倒立振子で学ぶ制御工学, 森北出版 (2017).