Riccati代数方程式(ARE)とは

以下の式。ただし$A,Q,P\in \mathbb R^{n\times n},B\in \mathbb R^{n\times m},R\in \mathbb R^{m\times m}$であり、Q,Rは正定値対称行列とし、$A,B,Q,R$が与えられ$P$を求める。

$$
A P + P A^T + Q - P B R^{-1} B^T P = 0
$$

AREが出てくる場面

現代制御理論のLQRでゲインを求める際に出てくる。LQRとは最適制御理論で、システムが
$$
\frac{dx}{dt} = Ax+Bu
$$
と表すことができるとき、$\min J=\int (x^TQx +u^TRu) dt$を満たす制御則は上のRicati代数方程式を満たす$P$を用いて
$$
u=-R^{-1}B^TPx
$$
と書けるという話。

解く

方針

有本・ポッターの方法（次項で解説）を実装して解くこととする。その際行列の固有値問題を解くことになるが、これはnumpyなどを用いる。

有本・ポッターの方法

手順を説明する。証明は書籍などを参照。

1.行列を置く

ハミルトンマトリクスと呼ばれる行列$H\in \mathbb R^{2n\times 2n}$を

$$
H=\left[ \begin{array}{cc} A^T & -B R^{-1} B^T \newline -Q & -A \end{array} \right]
$$

とおく。

2.ハミルトンマトリクスを固有値分解

Hの固有値には実部が負であるものがn個存在する。これを$\lambda_1,\lambda_2,...,\lambda_n$と置き、対応する固有ベクトルを$\vec w_1,\vec w_2,...,\vec w_n$とする。

3.固有ベクトルを並べて分割した行列を置く

これを用いて$Y,Z\in \mathbb R^{n\times n}$を
$$
\left[ \vec w_1 \vec w_2 ... \vec w_n \right] = \left[ \begin{array}{c} Y \newline Z \end{array} \right]
$$
とおく。

4.Pが求まる

$$
P=ZY^{-1}
$$

実装

import numpy.linalg as LA

def solve_are(A, B, Q, R):
    # 1. ハミルトンマトリクスを置く
    H = np.block([[A.T, -B @ LA.inv(R) @ B.T],
                  [-Q , -A]])
    # 2.固有値分解する
    eigenvalue, w = LA.eig(H)
    # 3.補助行列を置く
    Y_, Z_ = [], []
    n = len(w[0])//2
    # 固有値をsortしておく
    index_array = sorted([i for i in range(2*n)],
        key = lambda x:eigenvalue[x].real)
    # 実部が小さいものからn個を採用する
    for i in index_array[:n]:
        Y_.append(w.T[i][:n])
        Z_.append(w.T[i][n:])
    Y = np.array(Y_).T
    Z = np.array(Z_).T
    # 4.Pが求まる
    if LA.det(Y) != 0:
        return Z @ LA.inv(Y)
    else:
        print("Warning: Y is not regular matrix. Result may be wrong!")
        return Z @ LA.pinv(Y)

テスト

テストコード

A = np.array([[3., 1.],[0., 1.]])
B = np.array([[1.2], [1.]])
Q = np.array([[1., 0.2], [0.2, 1.0]])
R = np.array([[1.]])
P = solve_are(A, B, Q, R)
print("P")
print(P)
print("Riccati代数方程式の左辺")
print(A@P + P@A.T + Q - P@B@LA.inv(R)@B.T@P)

結果

P
[[ 69.20010326 -66.19334596]
 [-66.19334596  67.7487967 ]]
Riccati代数方程式の左辺
[[-1.13686838e-13  5.11590770e-13]
 [ 2.55795385e-13 -5.68434189e-13]]

解けました。

追記

コメント欄で「ハミルトンマトリクスが固有値に0をもつ場合にYが正方行列にならず実行時エラーが出る」という事象をご指摘をいただきました。とりあえず実行時エラーが出ないように変更したのですが、数学的な背景を理解していないためこの実装で正しい解が出るのかはわかっておりません。どなたかコメントをいただければ幸いです。