概要

可制御性が満たされるシステムでは，閉ループ系の極を任意に配置することができます．
このような制御系をレギュレータまたは状態フィードバックといいます．
状態フィードバックによって閉ループ系の極を任意に配置することでシステムを安定にすることができます．
今回は，そのためのフィードバックゲインを自動で計算してくれるプログラムを作っていこうと思います．

MATLABやScilab，PythonControlを使えば，簡単にできますが，
行列操作の練習の意味合いも込めて，プログラムをつくってみました．

動作環境

Windows10(64bit)
Python 3.7.2

レギュレータ

今回は証明等は置いといて，結果だけを使っていきたいと思います．

システムが可制御であれば，閉ループ系の固有値を任意に配置できます．

特に，システムが1入力であれば，閉ループ系の固有値が任意に与えられた多項式
$p(s) = s^n + \beta_{n-1}s^{n-1} + \cdots + \beta_{1}s + \beta_0$
の根に一致するレギュレータは次式で与えられました．
ただし，$\alpha_0, \alpha_1, \cdots, \alpha_{n-1}$は$A$の特性多項式
$det(sI-A) = s^n + \alpha_{n-1}s^{n-1} + \cdots + \alpha_{1}s + \alpha_0$
の係数であり，$T=(U_{c}V)^{-1}$です．


u = fx\\
f = \begin{bmatrix}
\alpha_0 - \beta_0 & \alpha_1 - \beta_1 &\cdots & \alpha_{n-1} - \beta_{n-1}
\end{bmatrix}T

コード

では，早速実装していきましょう．

place.py


"""フィードバックゲインを求めるプログラム"""
import numpy as np

# レギュレータの極
# ---------------------------------------------------------
new_pole = [-1, -1, -1]
# ---------------------------------------------------------

# 状態空間表現
# ---------------------------------------------------------
A = np.array([[0,1,0],[1,1,-1],[0,0,1]])
b = np.array([[1],[0],[1]])

dim = np.shape(A)[0]
print("A = \n"+ str(A) + "\n")
print("b = \n"+ str(b) + "\n")
# ---------------------------------------------------------

# 固有多項式の係数を求める
# ---------------------------------------------------------
eig = np.linalg.eigvals(A)
sI_A = np.poly1d(eig, True, variable='s')
# print('sI_A = \n' + str(sI_A) + "\n")

alpha = np.round(np.array(sI_A))
alpha = alpha[::-1]
# print('alpha = \n' + str(alpha) + "\n")
# ---------------------------------------------------------

# 可制御行列
# ---------------------------------------------------------
Uc = np.array(b)
for i in range(dim - 1):
    temp = Uc[:,i]
    temp = np.reshape(temp, (dim, 1))
    # print('temp =' + str(temp) + "\n")
    # print('Uc =' + str(Uc) + "\n")
    Uc = np.hstack((Uc, np.dot(A, temp)))

print("Uc = \n"+ str(Uc) + "\n")

rank_Uc = np.linalg.matrix_rank(Uc)
print("rank_Uc = " + str(rank_Uc) + "\n")
# ---------------------------------------------------------

# 行列Vを求める
# ---------------------------------------------------------
V = np.zeros((dim, dim))
for y in range(dim):
    for x in range(dim):
        if x + y < dim - 1:
            V[y,x] = alpha[x+y+1]
        elif x + y == dim - 1:
            V[y,x] = 1

print("V = \n"+ str(V) + "\n")
# ---------------------------------------------------------

# 変換行列Tを求める
# ---------------------------------------------------------
Tc_inv = np.dot(Uc, V)
print("Tc^-1 = (UcV) = \n"+ str(Tc_inv) + "\n")
Tc = np.linalg.inv(Tc_inv)
print("Tc = (UcV)^-1 = \n"+ str(Tc) + "\n")
# ---------------------------------------------------------

# 極配置ゲインを求める
# ---------------------------------------------------------
p_s = np.poly1d(new_pole, True, variable='s')
# print('p_s = \n' + str(p_s) + "\n")

beta = np.round(np.array(p_s))
beta = beta[::-1]
# print('beta = \n' + str(beta) + "\n")

a_b = alpha-beta
a_b = np.delete(a_b, dim, 0)
# print('a_b = \n' + str(a_b) + "\n")

f = np.dot(a_b, Tc)
print("f = \n"+ str(f) + "\n")
# ---------------------------------------------------------

解説

では，解説をしていきます．

レギュレータの極の指定


new_pole = [-1, -1, -1]

まずはじめに，レギュレータの極を指定します．
これは，つまりA+bfの固有値ですね．

状態空間の登録


A = np.array([[0,1,0],[1,1,-1],[0,0,1]])
b = np.array([[1],[0],[1]])

dim = np.shape(A)[0]

次に，状態空間を登録し，システムの次元数を取得しておきます．
いまは，以下のようなシステムにします．また，ベクトルcは使わないので登録していません．


A = \begin{bmatrix}
0 & 1 & 0\\
1 & 1 & -1\\
0 & 0 & 1
\end{bmatrix}\\

b = \begin{bmatrix}
1\\
0\\
1\\
\end{bmatrix}

システムの次元数の取得は，np.shape()で行いました．
np.shape()は行列の形を$n$行$m$列を$(n,m)$というタプルで返すため，
その0番目のみを取得します．今回は正方行列なので1番目でも構いません．

固有多項式の係数を求める


eig = np.linalg.eigvals(A)
sI_A = np.poly1d(eig, True, variable='s')

alpha = np.round(sI_A)
alpha = alpha[::-1]

np.linalg.eigvals()は引数の固有値をnumpy.ndarrayで返します．

np.poly1d()は，第1引数に係数または根を渡し，
第2引数にTrueとすると根にFalseとすると係数になります.
variableは$s$としておきました．デフォルトでは$x$のようです．

sI_Aをprintするとわかりますが，係数はきれいな整数値にならないことがあります．
なので，np.round()で整数値に変換します．
この時点では，$\alpha$は降べきの順の係数になっているので，
alpha[::-1]で逆順にします．

可制御行列$U_c$を求める


Uc = np.array(b)
for i in range(dim - 1):
    temp = Uc[:,i]
    temp = np.reshape(temp, (dim, 1))
    Uc = np.hstack((Uc, np.dot(A, temp)))

rank_Uc = np.linalg.matrix_rank(Uc)

可制御行列をつくっていきます．まずは，bを登録します．
その後，$Ab$, $A^2b$ $\cdots$をnp.hstack()で列を追加していきます．
内積はA*bではなく，np.dot()なので注意してください．

最後に，np.linalg.matrix_rank()で可制御行列のランクを求めておきます．
これを元に，if文にしてレギュレータを設計可能か否かを判別してもよかったのですが，
今回はとりあえず，rankを表示するにとどめました．

行列Vを求める


V = np.zeros((dim, dim))
for y in range(dim):
    for x in range(dim):
        if x + y < dim - 1:
            V[y,x] = alpha[x+y+1]
        elif x + y == dim - 1:
            V[y,x] = 1

変換行列を求めるための行列Vを求めます．
まずは，$n次元 \times n次元$のゼロ行列を作成します．
その後，$x$方向(行方向)と$y$方向(列方向)に対してfor文を回し，
行列の斜め方向で値は等しいので，$x+y$の値に応じて
alpha[]を参照させます．
右下三角行列の要素は$0$なので$x+y$が次元dim-1より大きくなったところは
特に値を書き換える必要はありません．

変換行列Tを求める


Tc_inv = np.dot(Uc, V)
Tc = np.linalg.inv(Tc_inv)

np.dot()で変換行列を求めます．
また，その逆行列も必要なので，np.linalg.inv()で求めておきます．

フィードバックゲインを求める


p_s = np.poly1d(new_pole, True, variable='s')

beta = np.round(np.array(p_s))
beta = beta[::-1]

a_b = alpha-beta
a_b = np.delete(a_b, dim, 0)

f = np.dot(a_b, Tc)

最後に，オブザーバゲインを求めにかかります．
前半部分はalphaを求めたところと何ら変わりはありません．

alpha-betaの最後の要素は必要ないので，np.delete()で削除します．
この時点ではa_bは横ベクトルなのでreshape()で縦ベクトルに整形します．
あとは，内積を取ってオブザーバゲインが求まります．

まとめ

いかがだったでしょうか？とてもいい行列操作の練習になったのではないかと思います．
次回はオブザーバの設計についてまとめていきたいと思います．
コードに関して改善点や気になった点などあればコメントからお願いします！