LQRが必要になったので実装した

November 9, 2022

現代制御理論の勉強をするとよく目にする Linear Quadratic Regulator (LQR)が必要になったので実装しました．調べながら実装したところ，答えを知っていればあっけないものだということがわかったのでメモを残します．

コードは github に置いておきます． https://github.com/Actat/riccati_solver

解決する課題

最適制御による状態フィードバックをしたいです．制御対象はシステムの状態を$\boldsymbol{x}(t)$，制御入力を$\boldsymbol{u}(t)$として次のような状態方程式で表されます．

$$ \dot{\boldsymbol{x}}(t) = \boldsymbol{A}\boldsymbol{x}(t) + \boldsymbol{B}\boldsymbol{u}(t) $$

制御入力はシステムの状態$\boldsymbol{x}(t)$に応じて

$$ \boldsymbol{u}(t) = -\boldsymbol{K}\boldsymbol{x}(t) $$

で定めることにします．この$\boldsymbol{K}$をシステムが良い感じにふるまうように定めるのが LQR の役割です．

ここで Riccati 方程式

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

を満たす$\boldsymbol{P}$を用いて

$$ \boldsymbol{K} = \boldsymbol{R}^{-1}\boldsymbol{B}^T\boldsymbol{P} $$

としておくと

$$ J = \int(\boldsymbol{x}^T(t)\boldsymbol{Q}\boldsymbol{x}(t) + \boldsymbol{u}^T(t)\boldsymbol{R}\boldsymbol{u}(t))dt $$

が最小になることが知られています．この$\boldsymbol{Q}$と$\boldsymbol{R}$は対角行列で，誤差の大きさと制御入力の大きさを重視する度合いを決めるパラメータです． $J$を小さくするのは，小さな入力ですばやく目標の状態へもっていくことに対応しており，$J$が最小になる$\boldsymbol{K}$を採用すると都合が良いです．

そこで，$J$が最小になる$\boldsymbol{K}$を返す関数LQR(A, B, Q, R)を用意して事前に$\boldsymbol{K}$を計算しておきます．例えば matlab にはこの関数があるので普段は自分で実装する必要はないのですが，必要になってしまったので実装しました．

Riccati 方程式の解法（有本-ポッターの方法）

$\boldsymbol{A}$, $\boldsymbol{B}$, $\boldsymbol{Q}$, $\boldsymbol{R}$が与えられるので，Riccati 方程式を満たす$\boldsymbol{P}$を求めて，$\boldsymbol{K}$を計算して返します．問題は Riccati 方程式を満たす$\boldsymbol{P}$をどうやって求めるかですが，教科書[1]に有本-ポッターの方法が載っていたのでこれを用いました．その他にインターネットで見つけた講義資料[2]も参考にしました．以下に解を得る手段を書いておきますが，証明は理解していません．

状態変数の数を n とします．ハミルトン行列$\boldsymbol{H}$（2n 行 2n 列の正方行列）を

$$ \boldsymbol{H} = \begin{bmatrix} \boldsymbol{A} & -\boldsymbol{B}\boldsymbol{R}^{-1}\boldsymbol{B}^T \ -\boldsymbol{Q} & -\boldsymbol{A}^T \end{bmatrix} $$

で定義します．ハミルトン行列の固有値を調べると，実部が負のものが$n$個見つかります． $n$個の固有値に対応する固有ベクトルを並べた$2n$行$n$列の行列を考えます．この行列を上下半分に分割してできる 2 つの$n$行$n$列の行列をそれぞれ$\boldsymbol{S}_1,\boldsymbol{S}_2$とします． $\boldsymbol{P} = \boldsymbol{S}_2\boldsymbol{S}_1^T$で得られる$\boldsymbol{P}$は Riccati 方程式を満たし，正定な行列です．

実装

言語は C++で Eigen を用いました． https://github.com/Actat/riccati_solverに置いてあります．

#include <eigen3/Eigen/Eigenvalues>
#include <eigen3/Eigen/Geometry>

Eigen::MatrixXd riccati_solver(Eigen::MatrixXd A,
                               Eigen::MatrixXd B,
                               Eigen::MatrixXd Q,
                               Eigen::MatrixXd R) {
  int const n = (int)A.cols();
  auto H      = Eigen::MatrixXd(2 * n, 2 * n);
  H << A, -B * R.inverse() * B.transpose(), -Q, -A.transpose();

  Eigen::EigenSolver<Eigen::MatrixXd> s(H);

  int j   = 0;
  auto S1 = Eigen::MatrixXcd(n, n);
  auto S2 = Eigen::MatrixXcd(n, n);
  for (int i = 0; i < 2 * n; i++) {
    if (s.eigenvalues()[i].real() < 0) {
      S1.col(j) = s.eigenvectors().block(0, i, n, 1);
      S2.col(j) = s.eigenvectors().block(n, i, n, 1);
      j++;
    }
  }

  auto P = S2 * S1.inverse();

  return P.real();
}

Eigen::MatrixXd lqr(Eigen::MatrixXd A,
                    Eigen::MatrixXd B,
                    Eigen::MatrixXd Q,
                    Eigen::MatrixXd R) {
  auto P = riccati_solver(A, B, Q, R);
  return R.inverse() * B.transpose() * P;
}

LQR の実行例がインターネットにあったので，同じ入力を入れて同じ解が得られることを確認しました．

感想

最適レギュレータの問題は Riccati 方程式になることは現代制御の授業で習いましたが，この方程式の解を得るのは困難なので matlab を使いましょうということになっていました．調べてみた結果，方法を知っていれば解を得る操作の実装はあっけないものだなぁと思いました．もっとも，簡単にできるのは Eigen が固有値・固有ベクトルを求めてくれるおかげなのですが．

参考

[1] 豊橋技術科学大学・高等専門学校　制御工学教育連携プロジェクト, 専門基礎ライブラリー制御工学技術者のための，理論・設計から実装まで. 実教出版株式会社, 2012. 実教出版のページ.
[2] 山下裕, 「ディジタル制御」(後半). https://stlab.ssi.ist.hokudai.ac.jp/~yuhyama/lecture/digital/digi-part2.pdf, 2022 年 11 月 9 日閲覧.

【2023 年 5 月 2 日追記】本来 Riccati 方程式を満たす$\boldsymbol{P}$の成分は実数になりますが，ハミルトン行列の固有値や固有ベクトルには複素数が出てきます．この部分の取り扱いに失敗しており，条件によっては nan を返す不具合が見つかりました．修正済みです．