前回 : Pythonによる制御工学入門第5回 - ma-hi-ro Hobby

Pythonによる制御工学入門第6回

今回は、状態フィードバック制御について取り扱う
参考書では、
5.6 「状態フィードバック制御」
7.1 「オブサーバを用いた出力フィードバック制御」
に該当する

Pythonによる制御工学入門第6回

状態フィードバック

極配置

下記のモデルで、極配置による状態フィードバック制御を行う

import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
import control.matlab as pyctrl

A = '0 1; -4 5'
B = '0; 1'
C = '1 0 ; 0 1'
D = '0; 0'
P = pyctrl.ss(A, B, C, D)

このままだと、固有値は下記のように、[1, 4] になる

np.linalg.eigvals(P.A)

array([1., 4.])

固有値が [-1, -1] になるように、フィードバックゲインを求める
下記のように求めると [3, -7] が得られる

Pole = [-1, -1]
F_pole = -pyctrl.acker(P.A, P.B, Pole)
F_pole

matrix([[ 3., -7.]])

このゲインで状態フィードバックを実施した際の固有値を確認すると [-1, -1] となっている

Acl = P.A + P.B * F_pole
np.linalg.eigvals(Acl)

array([-1., -1.])

ゼロ入力応答をプロットすると下図になる

Pfb = pyctrl.ss(Acl, P.B, P.C, P.D)
x_pole, t = pyctrl.initial(Pfb, np.arange(0, 5, 0.01), [-0.3, 0.4]) #ゼロ入力応答

fig = plt.figure(figsize=(6, 5))
ax = fig.add_subplot(111)

ax.plot(t, x_pole[:,0], label = '$x_1$')
ax.plot(t, x_pole[:,1], ls = '-.', label = '$x_2$')

ax.set_xlabel('t'); ax.set_ylabel('x')
ax.grid(); ax.legend()
fig.tight_layout()

数値シミュレーション版も確認

dt = 0.001
t_s  = [0.0]
x1_s = [-0.3]
x2_s = [0.4]
F_s = [0.0]

for i in range(5000):
    x1_tmp = x1_s[-1]
    x2_tmp = x2_s[-1]
    F    = 3.0 * x1_tmp + -7.0 * x2_tmp
    x1_s += [x1_tmp + ( 0.0 * x1_tmp +  1.0 * x2_tmp + 0.0 * F) * dt]
    x2_s += [x2_tmp + (-4.0 * x1_tmp +  5.0 * x2_tmp + 1.0 * F) * dt]
    t_s  += [t_s[-1] + dt]
    F_s  += [F]

df = pd.DataFrame()
df['x1']  = x1_s[::10]
df['x2']  = x2_s[::10]
df['u']   = F_s[::10]
df.index  = t_s[::10]

fig = plt.figure(figsize=(8, 4))
ax1 = fig.add_subplot(121)
ax2 = fig.add_subplot(122)

ax1.plot(df.index, df['x1'], ls='-.', label='sim')
ax2.plot(df.index, df['x2'], ls='-.', label='sim')

# モジュールの結果と比較
ax1.plot(t, x_pole[:,0], label='pyctrl')
ax2.plot(t, x_pole[:,1], label='pyctrl')

ax1.set_xlabel('t'); ax1.set_ylabel('$x_1$')
ax1.grid(); ax1.legend()
ax2.set_xlabel('t'); ax2.set_ylabel('$x_2$')
ax2.grid(); ax2.legend()
fig.tight_layout()

結果は一致している

最適レギュレータ

極配置法で、任意の固有値を設定できる
理論的には、負の大きな値に設定すれば、収束は改善するが、
実際は、動力を伝えるモータなどのアクチュエータの出力の制限がある
最適レギュレータを設定することで、収束性と出力制限のバランスを取りつつ設計できる

フィードバックゲインの設計

下記のように、Q, r のパラメータを与えることで、フィードバックゲインを設計できる

Q = [[100, 0], [0, 1]]
R = 1

F_lqr, PX, E = pyctrl.lqr(P.A, P.B, Q, R)
F_lqr = -F_lqr

print('--- フィードバックゲイン ---')
print(F_lqr)
print(-(1/R) * P.B.T * PX)
print('--- 閉ループ極 ---')
print(E)
print(np.linalg.eigvals(P.A + P.B * F_lqr))

--- フィードバックゲイン ---
[[ -6.77032961 -11.28813639]]
[[ -6.77032961 -11.28813639]]
--- 閉ループ極 ---
[-3.14406819+0.94083198j -3.14406819-0.94083198j]
[-3.14406819+0.94083198j -3.14406819-0.94083198j]

Acl = P.A + P.B * F_lqr
Pfb = pyctrl.ss(Acl, P.B, P.C, P.D)
x_lqr, t = pyctrl.initial(Pfb, np.arange(0, 5, 0.01), [-0.3, 0.4]) #ゼロ入力応答

fig = plt.figure(figsize=(6, 5))
ax = fig.add_subplot(111)

ax.plot(t, x_lqr[:,0], label = '$x_1$')
ax.plot(t, x_lqr[:,1], ls = '-.', label = '$x_2$')

ax.set_xlabel('t'); ax.set_ylabel('x')
ax.grid(); ax.legend()
fig.tight_layout()

fig = plt.figure(figsize=(6, 5))
ax = fig.add_subplot(111)

ax.plot(t, np.array(F_lqr)[0][0]  * x_lqr[:,0]  + np.array(F_lqr)[0][1]  * x_lqr[:,1], label = 'LQR')
ax.plot(t, np.array(F_pole)[0][0] * x_pole[:,0] + np.array(F_pole)[0][1] * x_pole[:,1], label = 'Pole')
ax.plot(t_s, F_s, '--', label='sim')

ax.set_xlabel('t'); ax.set_ylabel('u')
ax.grid(); ax.legend()
fig.tight_layout()

最適レギュレータで設計した方が、最初の立ち上がりが緩やかになる

ハミルトン行列

計算で求める場合は、下記のようにハミルトン行列の固有値を求め、極配置法で設計できる

H1 = np.c_[P.A, -P.B*(1/R)*P.B.T]
H2 = np.c_[Q, P.A.T]
H = np.r_[H1, -H2]
eigH = np.linalg.eigvals(H)
# print(eigH)

print('--- ハミルトン行列の安定固有値 ---')
eigH_stable = [i for i in eigH if i < 0]
print(eigH_stable)

F = -pyctrl.acker(P.A, P.B, eigH_stable)
print('--- フィードバックゲイン ---')
print(F)

--- ハミルトン行列の安定固有値 ---
[(-3.1440681937792796+0.9408319760374402j), (-3.1440681937792796-0.9408319760374402j)]
--- フィードバックゲイン ---
[[ -6.77032961 -11.28813639]]

積分サーボ

先の極配置法でのモデルに、一定の外力が加わった状態をシミュレートする
入力がある場合は、lsim を使用する

Acl = P.A + P.B * F_pole
Pfb = pyctrl.ss(Acl, P.B, P.C, P.D)

Td = np.arange(0, 8, 0.01)
Ud = 0.2 * (Td>0)
x, t, _ = pyctrl.lsim(Pfb, Ud, Td, [-0.3, 0.4])

fig = plt.figure(figsize=(6, 5))
ax = fig.add_subplot(111)

ax.plot(t, x[:,0], label = '$x_1$')
ax.plot(t, x[:,1], ls = '-.', label = '$x_2$')

ax.set_xlabel('t'); ax.set_ylabel('x')
ax.grid(); ax.legend()
fig.tight_layout()

外力によって、0.2に整定してしまう

外力を打ち消す積分項を考慮した、拡大状態変数を考え、2行2列から、3行3列に拡大する

A_integ = '0 1 0; -4 5 0; -1, 0, 0'
B_integ = '0; 1; 0'
C_integ = '1 0 0; 0 1 0; 0 0 1'
D_integ = '0; 0; 0'
P_integ = pyctrl.ss(A_integ, B_integ, C_integ, D_integ)
Pole = [-1, -1, -5]
F_integ = -pyctrl.acker(P_integ.A, P_integ.B, Pole)
F_integ

matrix([[ -7., -12.,   5.]])

Acl = P_integ.A + P_integ.B * F_integ
Pfb = pyctrl.ss(Acl, P_integ.B, P_integ.C, P_integ.D)

Td = np.arange(0, 8, 0.01)
Ud = 0.2 * (Td>0)
x, t, _ = pyctrl.lsim(Pfb, Ud, Td, [-0.3, 0.4, 0])

fig = plt.figure(figsize=(6, 5))
ax = fig.add_subplot(111)

ax.plot(t, x[:,0], label = '$x_1$')
ax.plot(t, x[:,1], ls = '-.',label = '$x_2$')
# ax.plot(t, Ud, c='k')

ax.set_xlabel('t'); ax.set_ylabel('x')
ax.grid(); ax.legend()
fig.tight_layout()

定常偏差を無くすことができている

オブザーバ

ここまでは、状態変数が観測可能(Cが単位行列)であるとしたが、
実際は、一部の状態変数しか観測できない
その場合は、オブザーバを設計して、状態変数を推定する
この推定においても、制御と同じようにフィードバックをすることができる

A = '0 1; -4 5'
B = '0; 1'
C = '1 0'
D = '0'
P = pyctrl.ss(A, B, C, D)

A = '0 1; -4 5'
B = '0; 1'
C = '1 0; 0 1'
D = '0; 0'
Ps = pyctrl.ss(A, B, C, D)

オブザーバゲインの設計

オブザーバの極は、フィードバック制御の極より、
収束を早くする（負に大きくする）

# オブザーバ極
observer_poles = [-15 + 5j, -15 - 5j]

# オブザーバゲインの設計（状態フィードバックの双対）
L = -pyctrl.acker(P.A.T, P.C.T, observer_poles).T
print(L)

[[ -35.]
 [-421.]]

# レギュレータ極
regulator_poles = [-5 + 5j, -5 - 5j]

# 極配置
F = -pyctrl.acker(P.A, P.B, regulator_poles)
print(F)

[[-46. -15.]]

Gsf = pyctrl.ss(P.A + P.B * F, P.B, np.eye(2), [[0],[0]])
Obs = pyctrl.ss(P.A + L * P.C, np.c_[P.B, -L], np.eye(2), [[0,0],[0,0]] )

fig = plt.figure(figsize=(8, 4))
ax1 = fig.add_subplot(1, 2, 1)
ax2 = fig.add_subplot(1, 2, 2)

T = np.arange(0, 3, 0.01)
x, t = pyctrl.initial(Gsf, T, X0=[-1, 0.5])
ax1.plot(t, x[:, 0], ls='-.', label='${x}_1$')
ax2.plot(t, x[:, 1], ls='-.', label='${x}_2$')

# 入力 u = Fx
u = [[F[0, 0] * x[i, 0] + F[0, 1] * x[i, 1]] for i in range(len(x))]
# 出力 y = Cx
y = x[:, 0]
# オブザーバで推定した状態の振る舞い
xhat, t, x0 = pyctrl.lsim(Obs, np.c_[u, y], T, [0, 0])
ax1.plot(t, xhat[:, 0], label='$\hat{x}_1$')
ax2.plot(t, xhat[:, 1], label='$\hat{x}_2$')

ax1.set_xlim([0, 3]); ax1.set_ylim([-2, 2])
ax1.set_xlabel('t'); ax1.set_ylabel('$x_1, \hat{x}_1$')
ax1.grid(); ax1.legend()
ax2.set_xlim([0, 3])
ax2.set_xlabel('t'); ax2.set_ylabel('$x_2, \hat{x}_2$')
ax2.grid(); ax2.legend()
fig.tight_layout()

最初は誤差があるが、最終的には推定値と実現値が一致している

オブザーバによる出力フィードバック

オブザーバの推定値を使って、状態フィードバックを実行する

# 出力フィードバック（オブザーバ＋状態フィードバック）
K = pyctrl.ss(P.A + P.B * F + L * P.C, -L, F, 0)
 
# フィードバック系
Gfb = pyctrl.feedback(P, K, sign=1)

fig = plt.figure(figsize=(6, 5))
ax = fig.add_subplot(1, 1, 1)

Td = np.arange(0, 3, 0.01)
y, t = pyctrl.initial(Gfb, Td, [-1, 0.5, 0, 0])
ax.plot(t, y, label='Obs FB')

ax.set_xlim([0, 3]); ax.set_ylim([-1.5, 3.5])
ax.set_xlabel('t'); ax.set_ylabel('y')
ax.grid(); ax.legend()
fig.tight_layout()