#Pythonで学ぶ制御工学< 状態フィードバック制御 >

はじめに

基本的な制御工学をPythonで実装し，復習も兼ねて制御工学への理解をより深めることが目的である．
その第19弾として「状態フィードバック制御」を扱う．

状態フィードバック制御

ここでは，状態フィードバック制御というくくりで，極配置法・最適レギュレータ・ハミルトン行列・積分型サーボ・可制御性・可観測性まで触れる．まず，以下に状態フィードバック制御についての概要を示す．

極配置法

概要

実装

以下では，極配置法による状態フィードバック制御について，ソースコードとそのときの出力を示す．

ソースコード

acker.py

"""
2021/03/21
@Yuya Shimizu

状態フィードバック制御（極配置法）
"""
import numpy as np
import matplotlib.pyplot as plt
from control import ss
from control.matlab import acker, initial
from for_plot import plot_set

## 状態空間モデル
A = [[ 0, 1],
        [-4, 5]]
B = [[0],
        [1]]
C = [[ 1, 0],
        [ 0, 1]]
D = [[0],
        [0]]

P = ss(A, B, C, D)

## 極配置法
Pole = [-1, -1] #望みの極を指定
F = -acker(P.A, P.B, Pole)  #アッカーマンの極配置アルゴリズムが実装された関数を使用

print(f"<結果>\nF={F}\n<結論>\nどうやらフィードバックゲインを{F}にすることで，極を{Pole}に配置できるようだ")

## 状態フィードバック制御
Acl = P.A + P.B*F   #入力をFとする
Pfb = ss(Acl, P.B, P.C, P.D)    #状態フィードバック制御

Td = np.arange(0, 5, 0.01)  #シミュレーション時間
x0 = [-0.3, 0.4]    #初期状態
x, t = initial(Pfb, Td, x0)

# 描画
fig, ax = plt.subplots()
ax.plot(t, x[:, 0], label = '$x_1$')
ax.plot(t, x[:, 1], ls = '-.', label = '$x_2$')
ax.set_title('response of state - pole placement method')
plot_set(ax, 't', 'x', 'best')
plt.show()

出力

<結果>
F=[[ 3. -7.]]
<結論>
どうやらフィードバックゲインを[[ 3. -7.]]にすることで，極を[-1, -1]に配置できるようだ

状態が安定している．(0に収束)

最適レギュレータ

概要

実装

以下では，最適レギュレータによる状態フィードバック制御について，ソースコードとそのときの出力を示す．また，最適レギュレータのフィードバックゲイン$F_{opt}$を用いると，$A+BF_{opt}$の固有値は安定になるが，その固有値はハミルトン行列に等しいことが知られているため，それについての実装も示しておく．

ソースコード:最適レギュレータ

regulator.py

"""
2021/03/21
@Yuya Shimizu

状態フィードバック制御（最適レギュレータ）
"""
import numpy as np
import matplotlib.pyplot as plt
from control import ss
from control.matlab import lqr, initial, care
from for_plot import plot_set

## 状態空間モデル
A = [[ 0, 1],
        [-4, 5]]
B = [[0],
        [1]]
C = [[ 1, 0],
        [ 0, 1]]
D = [[0],
        [0]]

P = ss(A, B, C, D)

## 最適レギュレータ
Q = [[100, 0],
        [   0, 1]]
R = 1

F, X, E = lqr(P.A, P.B, Q, R)  #F: フィードバックゲイン  X: リカッチ方程式の解  E: 閉ループ系の極
F = -F  #関数の仕様で符号が反転するため修正（lqrではu=-Fxに対するFが得られる．）

print('---フィードバックゲイン---')
print(F)
print(-(1/R)*P.B.T*X)
print('---閉ループ極---')
print(E)
print(np.linalg.eigvals(P.A+P.B*F))
"""
# care関数を使っても同様のことができる
X, E, F = care(P.A, P.B, Q, R)  #lqrとは戻り値の順が違う
F = -F
print('---フィードバックゲイン---')
print(F)
print('---閉ループ極---')
print(E)
"""

## 状態フィードバック制御
Acl = P.A + P.B*F   #入力をFとする
Pfb = ss(Acl, P.B, P.C, P.D)    #状態フィードバック制御

Td = np.arange(0, 5, 0.01)  #シミュレーション時間
x0 = [-0.3, 0.4]    #初期状態
x, t = initial(Pfb, Td, x0)

# 描画
fig, ax = plt.subplots()
ax.plot(t, x[:, 0], label = '$x_1$')
ax.plot(t, x[:, 1], ls = '-.', label = '$x_2$')
ax.set_title('response of state - regulator')
plot_set(ax, 't', 'x', 'best')
plt.show()

出力

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

ソースコード:ハミルトン行列

固有値をハミルトン行列により求めるため，最終的には極配置法のようになっている．

Hamilton.py

"""
2021/03/21
@Yuya Shimizu

状態フィードバック制御（ハミルトン行列の固有値を求める）
"""
import numpy as np
import matplotlib.pyplot as plt
from control import ss
from control.matlab import acker, initial, care
from for_plot import plot_set

## 状態空間モデル
A = [[ 0, 1],
        [-4, 5]]
B = [[0],
        [1]]
C = [[ 1, 0],
        [ 0, 1]]
D = [[0],
        [0]]

P = ss(A, B, C, D)

## ハミルトン行列
Q = [[100, 0],
        [   0, 1]]
R = 1

H1 = np.c_[P.A, -P.B*(1/R)*P.B.T]   #配列の連結np.c_はnp.hstackと同じ
H2 = np.c_[Q, P.A.T]
H = np.r_[H1, -H2]                          #配列の連結np.c_はnp.vstackと同じ
eigH = np.linalg.eigvals(H)
print(eigH)

print('---ハミルトン行列の安定固有値---')
eigH_stable = [i for i in eigH if i < 0]    #実部が負の固有値を抽出
print(eigH_stable)

F = -acker(P.A, P.B, eigH_stable)  #関数の仕様で符号が反転するため修正（lqrではu=-Fxに対するFが得られる．）

print('---フィードバックゲイン---')
print(F)

## 状態フィードバック制御
Acl = P.A + P.B*F   #入力をFとする
Pfb = ss(Acl, P.B, P.C, P.D)    #状態フィードバック制御

Td = np.arange(0, 5, 0.01)  #シミュレーション時間
x0 = [-0.3, 0.4]    #初期状態
x, t = initial(Pfb, Td, x0)

# 描画
fig, ax = plt.subplots()
ax.plot(t, x[:, 0], label = '$x_1$')
ax.plot(t, x[:, 1], ls = '-.', label = '$x_2$')
ax.set_title('response of state - pole placement method by Hamilton')
plot_set(ax, 't', 'x', 'best')
plt.show()

出力

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

積分型サーボ系

概要

実装

以下では，外乱の入ったときの状態フィードバック制御と積分型サーボ系制御について，ソースコードとそのときの出力を示す．

ソースコード：状態フィードバック制御（外乱あり）

with_disturbance.py

"""
2021/03/21
@Yuya Shimizu

状態フィードバック制御（外乱あり）
"""
import numpy as np
import matplotlib.pyplot as plt
from control import ss
from control.matlab import acker, lsim
from for_plot import plot_set

## 状態空間モデル
A = [[ 0, 1],
        [-4, 5]]
B = [[0],
        [1]]
C = [[ 1, 0],
        [ 0, 1]]
D = [[0],
        [0]]

P = ss(A, B, C, D)

## 極配置法
Pole = [-1, -1]
F = -acker(P.A, P.B, Pole)

## 状態フィードバック制御
Acl = P.A + P.B*F   #入力をFとする
Pfb = ss(Acl, P.B, P.C, P.D)    #状態フィードバックで安定化したシステム

Td = np.arange(0, 8, 0.01)  #シミュレーション時間
Ud = 0.2 * (Td>0)   #ステップ状の外乱
x0 = [-0.3, 0.4]    #初期状態
x, t, _ = lsim(Pfb, Ud, Td, x0)

# 描画
fig, ax = plt.subplots()
ax.plot(t, x[:, 0], label = '$x_1$')
ax.plot(t, x[:, 1], ls = '-.', label = '$x_2$')
ax.set_title('response of state - with disturbance')
plot_set(ax, 't', 'x', 'best')
plt.show()

出力

ソースコード：積分型サーボ系制御（外乱あり）

integral_servo.py

"""
2021/03/21
@Yuya Shimizu

積分型サーボ系（外乱あり）
"""
import numpy as np
import matplotlib.pyplot as plt
from control import ss
from control.matlab import acker, lsim
from for_plot import plot_set

## 状態空間モデル
A = [[ 0, 1],
        [-4, 5]]
B = [[0],
        [1]]
C = [1, 0]
D = [0]

P = ss(A, B, C, D)

# 拡大系
Ae1 = np.c_[P.A, np.zeros((2, 1))]
Ae = np.r_[Ae1, -np.c_[P.C, 0]]
Be = np.c_[P.B.T, 0].T
Ce = np.c_[P.C, 0]

## 拡大系に対する極配置法
Pole = [-1, -1, -5]
Fe = -acker(Ae, Be, Pole)  #Fe: 拡大系に対するフィードバックゲイン

## 閉ループ系
Acl = Ae + Be*Fe   #入力をFとする
Pfb = ss(Acl, Be, np.eye(3), np.zeros((3, 1)))

Td = np.arange(0, 8, 0.01)  #シミュレーション時間
Ud = 0.2 * (Td>0)   #ステップ状の外乱
x0 = [-0.3, 0.4, 0]    #初期状態
x, t, _ = lsim(Pfb, Ud, Td, x0)

# 描画
fig, ax = plt.subplots()
ax.plot(t, x[:, 0], label = '$x_1$')
ax.plot(t, x[:, 1], ls = '-.', label = '$x_2$')
ax.set_title('response of state - integral servo')
plot_set(ax, 't', 'x', 'best')
plt.show()

出力

外乱が入ったときに状態フィードバック制御では収束しなかった状態が積分型サーボ系制御により収束できていることがグラフから分かる．

可制御性

概要

実装

以下では，可制御性について，ソースコードとそのときの出力を示す．

ソースコード：可制御性

controllability.py

"""
2021/03/21
@Yuya Shimizu

可制御性
"""
import numpy as np
from control import ss
from control.matlab import ctrb

def controllability(A, B, C, D):
    #状態空間モデル（Matlab表記でも記述可能）
    P = ss(A, B, C, D)

    #可制御性check
    Uc = ctrb(P.A, P.B)                     #可制御性行列
    n = len(Uc)
    det = np.linalg.det(Uc)                #行列式
    rank = np.linalg.matrix_rank(Uc)  #ランク
    print(f"Uc = \n{Uc}")
    print(f"n = {n}")
    print(f"det(Uc) = {det}")
    print(f"rank(Uc) = {rank}")

    if n == rank:
        print('可制御である')
        return True
    else:
        print('可制御でない')
        return False


if __name__ == '__main__':
    A = '0 1; -4 5'
    B = '0; 1'
    C = '1 0'
    D = '0'

    controllability(A, B, C, D)

出力

Uc = 
[[0. 1.]
 [1. 5.]]
n = 2
det(Uc) = -1.0
rank(Uc) = 2
可制御である

可観測性

概要

実装

以下では，可制御性について，ソースコードとそのときの出力を示す．

ソースコード

observability.py

"""
2021/03/21
@Yuya Shimizu

可観測性
"""
import numpy as np
from control import ss
from control.matlab import obsv

def observability(A, B, C, D):
    #状態空間モデル（Matlab表記でも記述可能）
    P = ss(A, B, C, D)

    #可制御性check
    Uo = obsv(P.A, P.C)                     #可観測性行列
    n = len(Uo)
    det = np.linalg.det(Uo)                #行列式
    rank = np.linalg.matrix_rank(Uo)  #ランク
    print(f"Uo = \n{Uo}")
    print(f"n = {n}")
    print(f"det(Uo) = {det}")
    print(f"rank(Uo) = {rank}")

    if n == rank:
        print('可観測である')
        return True
    else:
        print('可観測でない')
        return False


if __name__ == '__main__':
    A = '0 1; -4 5'
    B = '0; 1'
    C = '1 0'
    D = '0'

    observability(A, B, C, D)

出力

Uo = 
[[1. 0.]
 [0. 1.]]
n = 2
det(Uo) = 1.0
rank(Uo) = 2
可観測である

感想

だいぶ話が難しそうになってきたが，今回は閉ループ系の伝達関数モデルではなく状態空間モデルにおける状態をフィードバックすることを背景にフィードバックゲインについて極配置法と最適レギュレータについて学んだ．最適レギュレータは極配置法での問題点を解決する方法の1つであることを改めて知った．また，極配置・オブザーバ設計において必要な条件として可制御性と可観測性について学んだ．設計にあたって重要な部分である．完璧にマスターでできているわけではないが，大まかには理解できているのではないかと思う．

参考文献

Pyhtonによる制御工学入門　　南祐樹　著　　オーム社