オブザーバ

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

オブザーバ

直接観測できない状態量を、観測量から推定する仕組みをオブザーバという。

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

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

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

これを安定化する。極配置の関数を用いるには \(A - B K\) の形にする必要があるが、欲しいのは \(K\) であって、\(C\) と前後が逆である。行列の固有値は転置しても変わらないので、\((A_0 - K_0 C)^T = A_0^T - C^T K_0^T\) として適用する。ここでは SciPy の signal.place_poles() 関数を用いる。ここでは 質点 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)
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):
    x1[:] = A.dot(x1)
    x[step, :] = x1

    y = C.dot(x1)

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

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

低次元オブザーバ

\(x_2\) を観測ているのだから、\(x_2\) 自体は推定しなくてよいように思われる。わからないものだけに限って推定するものを低次元オブザーバという (そうでない上の方法を同一次元オブザーバという)。推定を以下のもので生成する。

めんどくさいので詳細は割愛。以下の方法は B. Gopinath の方法だそうである。

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):
    x1[:] = A.dot(x1)
    x[step, :] = x1

    y = C.dot(x1)

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

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

たしかに \(x_2\) の推定値は最初から真値に沿っている。

参考

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