オブザーバとレギュレータの組み合わせ

2020年4月30日

はじめに

バネ-ダンパー-質点系について、オブザーバとレギュレータを組み合わせた安定化を行う。

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

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)

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

import numpy as np

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)

線形システム状態方程式をシミュレートする。

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()

レギュレータ

線形システム状態方程式を次式で表す。

$$\dot{x}(t) = A_0 x(t) + B_0 u(t)$$ $$y(t) = C x(t)$$

これに以下のようなフィードバック形の入力を与える。

$$u(t) = F x(t)$$

状態方程式は次式のような形になる。

$$\dot{x}(t) = (A_0 + B_0 F) x(t)$$

これが安定化するように (係数行列の固有値の実数部分が負になるように) フィードバックゲイン \(F\) を決める。

poles = [-2.+0.5j, -2.-0.5j, -2., -1.]

bunch = signal.place_poles(A0, -B0, poles)
F = bunch.gain_matrix
print(F)

A0f = A0 + B0.dot(F)

from scipy import linalg
print(linalg.eigvals(A0f))

これを用いてシミュレーションすると次のようになる。

これはすべての状態量がわかっている場合である。もし質点 2 の位置のみがわかっている場合のシミュレーション結果は以下になる。

もちろん状態量がすべてわかっているほうが安定化させやすいが、現実的には一部の状態量しか観測できない。そこで、未知の状態量をオブザーバで推定する。

同一次元オブザーバ

状態変数の推定値を \(\hat{x}\) とする。これが次式に従うとする。

$$\dot{\hat{x}}(t) = A_0 \hat{x}(t) + B_0 u(t) + K_0 \{y(t) - C \hat{x}(t)\}$$

推定値と真値の差を \(\xi(t) = \hat{x}(t) - x(t)\) とすると、これは次式に従う。

$$\dot{\xi}(t) = (A_0 - K_0 C) \xi(t)$$

これを安定化する。ここでは 質点 2 の位置 (\(x_2\)) のみ観測できるとして、\(C = (0\ 1\ 0\ 0)^T\) とする。

C = np.array(C0).reshape(1, len(C0))
poles = [-3.+0.5j, -3.-0.5j, -1., -0.5]

bunch = signal.place_poles(A0.T, C.T, poles)
K0 = bunch.gain_matrix.T

Ah0 = A0 - K0.dot(C)

from scipy import linalg
print(linalg.eigvals(Ah0))

シミュレーション結果は以下のようになる。

これを用いた推定値からフィードバック形の入力値を計算するレギュレータのシミュレーションは以下のようになる。

from numpy.linalg import inv

n = len(x0)
A = expm(A0*dt)
B = (A - np.eye(n)).dot(inv(A0)).dot(B0)
Ah = expm(Ah0*dt)
K = (Ah - np.eye(n)).dot(inv(Ah0)).dot(K0)

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

x[0, :] = x0
xh[0, :] = xh0

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

    xh1[:] = Ah.dot(xh1) + B.dot(u) + K.dot(y)
    xh[step, :] = xh1

    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.plot(t, xh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, xh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, xh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, xh[:, 3], ".", label="$\hat{x}_4$")
plt.xlim(0., end_time)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

質点 2 の位置のみを用いたフィードバックよりは早く安定化できている。

低次元オブザーバ

観測値である \(x_2\) 自体は推定しない低次元オブザーバを考える。推定を以下のもので生成する。

$$\hat{x}(t) = D z(t) + H y(t)$$ $$\dot{z}(t) = \hat{A} z(t) + \hat{B} u(t) + K y(t)$$
C = np.array(C0).reshape(1, len(C0))

n = len(x0)
m = C.shape[0]

S = np.zeros((n, n))
S[:m, :] = C

j = n - 1
for i in range(n):
    if not np.any(C[:, i]):
        S[j, i] = 1.
        j -= 1

Si = inv(S)
SASi = S.dot(A0.dot(Si))
A11 = SASi[:m, :m]
A12 = SASi[:m, m:]
A21 = SASi[m:, :m]
A22 = SASi[m:, m:]

SB = S.dot(B0)
B1 = SB[:m, :]
B2 = SB[m:, :]

poles = [-1.+0.5j, -1.-0.5j, -0.5]

bunch = signal.place_poles(A22.T, A12.T, poles)
L = bunch.gain_matrix.T

Ah0 = A22 - L.dot(A12)

K0 = Ah0.dot(L) + A21 - L.dot(A11)
Bh0 = -L.dot(B1) + B2

D0 = np.zeros((n, n - m))
D0[m:, :] = np.eye(n - m)
D = Si.dot(D0)

H0 = np.zeros((n, m))
H0[:m, :] = np.eye(m)
H0[m:, :] = L
H = Si.dot(H0)

シミュレーション

n = len(x0)
A = expm(A0*dt)
Ah = expm(Ah0*dt)
K = (Ah - np.eye(n - m)).dot(inv(Ah0)).dot(K0)

steps = int(end_time/dt) + 1
x = np.zeros((steps, n))
xh = np.zeros((steps, n))
xh0 = np.zeros(n)
z0 = np.zeros(n - m)

x[0, :] = x0
xh[0, :] = xh0

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

    z1[:] = Ah.dot(z1) + K.dot(y)
    xh1[:] = D.dot(z1) + H.dot(y)
    xh[step, :] = xh1

    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.plot(t, xh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, xh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, xh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, xh[:, 3], ".", label="$\hat{x}_4$")
plt.xlim(0., end_time)
plt.ylim(-0.5, 1.5)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

これを用いたレギュレータのシミュレーションは以下のようになる。

A = expm(A0*dt)
B = (A - np.eye(n)).dot(inv(A0)).dot(B0)
Ah = expm(Ah0*dt)
Bh = (Ah - np.eye(n - m)).dot(inv(Ah0)).dot(Bh0)
K = (Ah - np.eye(n - m)).dot(inv(Ah0)).dot(K0)

steps = int(end_time/dt) + 1
x = np.zeros((steps, n))
xh = np.zeros((steps, n))
xh0 = np.zeros(n)
z0 = np.zeros(n - m)

x[0, :] = x0
xh[0, :] = xh0

x1 = np.array(x0)
z1 = np.array(z0)
xh1 = np.array(xh0)
for step in range(1, steps):
    y = C.dot(x1)
    u = F.dot(xh1)

    z1[:] = Ah.dot(z1) + Bh.dot(u) + K.dot(y)
    xh1[:] = D.dot(z1) + H.dot(y)
    xh[step, :] = xh1

    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.plot(t, xh[:, 0], ".", label="$\hat{x}_1$")
plt.plot(t, xh[:, 1], ".", label="$\hat{x}_2$")
plt.plot(t, xh[:, 2], ".", label="$\hat{x}_3$")
plt.plot(t, xh[:, 3], ".", label="$\hat{x}_4$")
plt.xlim(0., end_time)
plt.xlabel("$t$")
plt.ylabel("$x$")
plt.legend(ncol=2)
plt.show()

こちらも質点 2 の位置のみを用いたフィードバックよりは早く安定化できている。

参考

  • 大住晃: 構造物のシステム制御, 森北出版 (2013).