Numpyによる現代制御理論の初等問題の計算

記事の概要

Pythonのnumpyを用いた現代制御理論の計算方法を幾つか紹介します。
用途としては、現代制御理論を勉強する際に、大変な行列計算を省略したり、計算結果が正しいかを検算するのを想定しています。
リカッチ方程式の解を求めるのには、無償ライブラリpython-controlを使用しました。

固有値と固有ベクトルの計算

以下の計算は全て、あらかじめnumpyをインポートしているものとします。

import numpy as np

# 行列
A = np.array([[2, -1 , 1], [-1, 2, 1], [1, 1, 1]])

# 固有値と固有ベクトルの計算
np.linalg.eig(A)
eigen_val, eigen_vec = np.linalg.eig(A)

eigen_vec[np.abs(eigen_vec) <= 1.0e-8] = 0.0 # 計算結果を見やすくする為、小さい値を0にする
print("固有ベクトル：\n", eigen_vec)
eigen_val[np.abs(eigen_val) <= 1.0e-8] = 0.0 # 計算結果を見やすくする為、小さい値を0にする
print("固有値：\n", eigen_val)

対角行列の計算

上の式で求めた固有値と固有ベクトルを使用します。

# 固有ベクトルの逆行列の計算
eigen_vec_inv = np.linalg.inv(eigen_vec)

# 対角行列の計算
X = np.dot(eigen_vec_inv,A)
diag_vec = np.dot(X,eigen_vec)
diag_vec[np.abs(diag_vec) <= 1.0e-8] = 0.0 # 計算結果を見やすくする為、小さい値を0にする
print("対角行列：\n", diag_vec)

固有値から対角行列を出力する関数もあります

diag_vec=np.diag(eigen_val)

print("対角行列：\n", diag_vec)

行列のランク

行列のランクを計算で求めるには、行列を変形したり、線形独立なベクトルを数えたり、小行列式を調べたりと、いずれも計算に苦労しますが、数値計算を用いれば1行で済みます。

matrix = np.array([[1, 2, -1, 4], [2, 4, 3, 5], [-1, -2, 6, -7]])
rank_m = np.linalg.matrix_rank(matrix)
print(rank_m)

可制御性の判定

以下ではn次元1入力1出力システムを考えます。

\dot{\boldsymbol{x}} = A\boldsymbol{x}(t) + \boldsymbol{b}u(t) \\
y = \boldsymbol{c}\boldsymbol{x}(t) 

1入力システムの場合は可制御性行列の行列式が0にならなければ可制御と判定できます。

def Controllability_check(A, b):
    A_dim = A.shape[0]
    
    U = b
    for i in range(A_dim-1):
        X = np.dot(A, b)
        for j in range(i):
            X = np.dot(A, X)
        U = np.hstack((U, X))
        
    print("U = \n", U)
    det_U = np.linalg.det(U)
    if np.abs(det_U) <= 1.0e-8:
        det_U = 0.0    
    print("|U| = ", det_U)
    
    if(det_U == 0.0):
        print("This system is Uncontrollable.")
    else:
        print("This system is Controllable.")

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

もしくは、可制御性行列のランクと次元が一致すれば可制御と判定できます。

def Controllability_Rank_check(A, b):
    A_dim = A.shape[0]
    
    U = b
    for i in range(A_dim-1):
        X = np.dot(A, b)
        for j in range(i):
            X = np.dot(A, X)
        U = np.hstack((U, X))
        
    print("U = \n", U)
    rank_U = np.linalg.matrix_rank(U)
    print("Rank is ", rank_U)
    print("Dimension is ", A_dim)
    
    if(rank_U == A_dim):
        print("This system is Controllable.")
    else:
        print("This system is Uncontrollable.")

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

可観測性の判定

1出力システムの場合は可観測性行列の行列式が0にならなければ可観測と判定できます。

def Observability_check(A, c):
    A_dim = A.shape[0]
    
    U = c
    for i in range(A_dim-1):
        X = np.dot(c,A)
        for j in range(i):
            X = np.dot(X, A)
        U = np.vstack((U, X))
        
    print("U = \n", U)
    det_U = np.linalg.det(U)
    if np.abs(det_U) <= 1.0e-8:
        det_U = 0.0    
    print("|U| = ", det_U)
    
    if(det_U == 0.0):
        print("This system is Unobservable.")
    else:
        print("This system is Observable.")

A = np.array([[-1, 0], [0, 2]])
c = np.array([-2, 1])
Observability_check(A, c)

多出力システムの場合は可観測性行列のランクと次元が一致すれば可観測と判定できます。

def Observability_Rank_check(A, c):
    A_dim = A.shape[0]
    #if(A_dim < 2):
    #    return False   
    
    U = c
    for i in range(A_dim-1):
        X = np.dot(c,A)
        for j in range(i):
            X = np.dot(X, A)
        U = np.vstack((U, X))
        
    print("U = \n", U)
    rank_U = np.linalg.matrix_rank(U)
    print("Rank is ", rank_U)
    print("Dimension is ", A_dim)
    
    if(rank_U == A_dim):
        print("This system is Observable.")
    else:
        print("This system is Unobservable.")

A = np.array([[1, 2, 0], [3, -1, -3], [-1, 2, 2]])
c = np.array([[-2, 1, 3] , [1, -1, -1]])
Observability_Rank_check(A, c)

リカッチ方程式を解く

リカッチ方程式の説明は省略します。
Q、Rは評価関数Jの定数です。

J = \frac{1}{2} \int_0^{\infty} \biggl\{ x^T(t)Qx(t)+u^T(t)Ru(t) \biggl\} dt

$x^T(t)Qx(t)$は安定点からのズレを示しており、この値が小さいほど早く安定点に収束します。
$u^T(t)Ru(t)$は安定化に要するエネルギーを示しており、この値が小さいほど供給エネルギーは小さくなります。Rの値は現実に供給可能な電力（例えばマイコンシステムなら3.3V）やパワーを参照して決定します。

以下は「Python で連続時間代数 Riccati 方程式を解く」を参照しました。

連続時間のリカッチ方程式を解く場合にはcontrol.care（careはthe continuous-time algebraic Riccati equationの略）を用います。

import numpy as np
import control

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

P, L, G = control.care(A, b, Q, R)
print("リカッチ方程式の解 =\n", P)
print("A - b Gの固有値 =\n", L)
print("状態フィードバックゲイン =\n", G)

離散時間のリカッチ方程式を解く場合にはcontrol.dare（dareはthe discrete-time algebraic Riccati equationの略）を用います。
例えば、マイコンで最適レギュレータを用いる場合は、離散時間のリカッチ方程式を使用します。

P, L, G = control.dare(A, b, Q, R)

参考

• 森泰親「演習で学ぶ現代制御理論」
  • 手計算での解法が多く紹介されています。
• トランジスタ技術2019年7月号
  • 現代制御理論の解説記事があります。連続時間と離散時間の最適レギュレータの証明が分かりやすかったです。付属DVDにはpython-controlのインストール方法と、最適レギュレータの計算を行う倒立振子シミュレーションプログラムがあります。シミュレーションでは離散時間のリカッチ方程式を用いています。