線形最適制御（LQR）とRiccatiソルバーについて

はじめに

今回は制御工学でよく目にするLQRについて書いてみます．LQRとはLinear Quadratic Regulator の略で，日本語で言うと「線形二次安定化器」って感じでしょうか．

この記事は，線形二次？え，線形なの？二次なの？どっちなの？って感じの人向けです．

LQRについて徒然なるままに，いろいろ書いています．
LQRの理論からriccati方程式のソルバーまで．最後にc++でriccatiソルバーのコードを載せてあります．
githubはこちらから：https://github.com/TakaHoribe/Riccati_Solver/blob/master/riccati_solver.cpp

何に使えるの？

LQRを使うのはこんな時ですね

• 手軽にいい感じ（最適）の制御がしたい
• 多入力多出力系を扱いたい

まずは，手軽さについてです．LQRは最近流行りのモデル予測制御などと比較して，設計が非常に簡単です．制御対象を数学的にモデル化できれば，理論によって安定性や最適性は保証されますので，適当なチューニングでもある程度の性能を出すことができます．

次に多入力多出力系への適用．これはLQRよりも現代制御理論の特徴（メリット）でしょうか．
世の中で一番有名な制御は，おそらくPID制御でしょう．いまだに産業機械の90%くらいがPIDで動いてるらしいです．PIDのメリットは，非常に直感的で，設計がしやすいことだと思います．PIDはそれぞれ比例（Proportional），積分（Integral），微分（Differential）を表しており，それぞれの特性をちゃんと理解していれば，比較的簡単な設計でしっかりとした制御を組むことができます．PIDの設計法も有名なものがたくさんあり，特許もけっこうあるみたいです．PIDのまとめ論文

なんですが，PIDにも大きな欠点があります．それは，多入力多出力系を取り扱えないという点です．多入力多出力システムというのは，例えば下の絵みたいに，2つの力を使って3つの質点の位置をいい感じに制御したい，とかいうケースです．他には航空機や最近のモータとかもそうですね．

PID（というか古典制御全般）はラプラス変換と微積分を用いて制御性能を解析するのですが，多次元システムになるとなかなか上手く解析できないんです．そこで誕生したのが現代制御理論です．

現代制御の発展

現代制御が発達したのは1960年代で，古典制御から10年後くらいです．この現代制御の目的は，多入力多出力のシステムを制御すること．そのために，当時制御の業界ではあまり使われていなかった線形代数が使われることになりました．例えば航空機の制御をしようと思った場合，制御したい量は飛行機の角度：ロール・ピッチ・ヨー角と，飛行速度とかですね．制御入力はエンジン出力や，飛行角度を制御するための取り舵（エレベータ・エルロン・ラダー）になります．つまり4つの入力で4つの値を制御したいことになります（この説明では，です．実際はもっと複雑だったり，逆に単純化しているかもしれません）．

飛行速度を上げたければ，エンジンの出力を上げればいいやん！と思うかも知れませんが，飛行速度はピッチ角の安定性に大きくかかわっているので，エンジンを変化させる場合は，エレベータやエルロンもちゃんと使って制御しないと，最悪の場合は制御に失敗して墜落してしまいます．4つの入出力が複雑に絡み合って，航空機の運動を形成しているわけですね．

こういう場合は，1入出力しか使えない古典制御では設計が困難になります．なので，4つの入出力関係をしっかりと取り扱える制御の設計法が求められていたわけです．

LQRについて

LQRの詳細

ここから本題のLQRについてまとめていきます．まずはLQRが取り扱っている問題設定について説明しましょう．

LQRを含め，現代制御理論が取り扱う制御対象は

\dot x = f(x,u)

といった数式（状態方程式）で表されることを前提にしています．ここで$x\in R^n$はシステムの状態，$u\in R^{m}$はシステムへの制御入力を示しています．
LQRは線形システムに限った話ですので，今回の対象とするシステムは以下の線形系で書けることが前提となっています．

\dot x = Ax+Bu

ここで$A\in R^{n\times n}$，$B\in R^{n\times m}$は係数行列とよばれ，制御対象の特性を示しています．

例として，質点の運動を考えましょう．上の図で示したような3つの質点は式を書くのが大変なので，質点が1個の状態を考えます（下図）．

この質点の運動を状態方程式で書いてみます．とりあえず，運動方程式は

m\ddot y=F

になります．状態変数を$x=[x_1,x_2]=[y, \dot y]^T$と定義すると（この定義は状態方程式が書ければどんな形でもいいのですが，だいたいこの定義を使います），状態方程式は

\frac{d}{dt}
\begin{bmatrix}
x_1\\x_2
\end{bmatrix}
=
\begin{bmatrix}
0 & 1 \\ 0 & 0
\end{bmatrix}
\begin{bmatrix}
x_1\\x_2
\end{bmatrix}
+
\begin{bmatrix}
0\\1/m
\end{bmatrix}
F

となります．運動方程式と状態方程式は，ばらして書けば同じ式を意味しているのですが，現代制御理論はすべてこの状態方程式で表現されるシステムについて論じられているので，毎回この状態方程式を導出する必要があります（状態方程式を見ると$\dot x=f(x,u)$のように微分が1回しか出てこないので，数学的に取り扱いが楽になるのです．今回の質点系だと，もとの運動方程式は$m\ddot y=F$のように微分が2回出てきています．状態方程式では次元を2次元に増やすことによって，微分を1回しか出さないようにしています）．
状態方程式の一般系$\dot x=Ax+Bu$と比較すれば，

A=
\begin{bmatrix}
0 & 1 \\ 0 & 0
\end{bmatrix}
,\ B=
\begin{bmatrix}
0\\1/m
\end{bmatrix}

であることが分かります.この$A, B$を対象システムのパラメータとして，LQRを設計します．

LQRは「安定化」問題を解く手法なので，LQRができることは，初期の状態変数$x$が非0のシステムに制御を加え，$x=0$の状態に持っていくことです．時間が進むにつれ，$x=0$に収束していくような制御を組むことを安定化と呼びます．

-- 余談：サーボ問題について --

もちろん，安定化以外にも目標値追従問題としてLQRを用いることもできます．そういった場合は目標値を$x_e$として，新しく状態変数$\tilde x:=x-x_e$を定義します．「$\dot x_e=0$」かつ「$Ax_e+Bu_e=0$となるような入力$u_e$が作れる」と仮定すると（というか，ほとんどの場合，そういう仮定が成り立つように目標値$x_e$を取ります．成り立たない場合は制御不可となります），システムの状態方程式は$\tilde x$および$\tilde u := u-u_e$を用いて

\dot {\tilde x} + \dot x_e=A(\tilde x+x_e) + B(\tilde u + u_e)

と書けます．定義より$Ax_e+Bu_e=0$かつ$\dot x_e=0$なので，上の式は

\dot {\tilde x} =A\tilde x + B\tilde u

となります．この式に対して安定化，つまり$\tilde x = 0$となるような制御を設計すれば，つまりそれは$x-x_e=0$なので$x=x_e$となる制御を組んだことになるのです．このとき，例えば上の式に対して状態フィードバック制御を組んだとすると，その式は$\tilde u=-K\tilde x$のようになります．この式を実際の制御入力$u$で書くと，$\tilde u = u-u_e$なので，結局

u=u_e-Kx

の形の入力をもとのシステムに加えてやれば，システムへの目標値追従制御が設計できるわけです．

-- 余談終わり --

さて，それではLQRの制御に戻ります．現代制御では状態フィードバック制御がよく使われます．これは全ての状態変数にゲインを掛けてフィードバックを行う制御で，質点の例題ならば

u=-k_1 x_1 -k_2 x_2 = -Kx

のように書きます．LQRは，ここまで説明した「線形システム」に対して「状態フィードバック」を施した際の，制御ゲイン（$K$）を決めてくれます．その指針は，次の評価関数$J$

J=\int _0^\infty x^TQx+u^TRu\ dt

を_最小にすること（ここで$Q, R$は評価関数の重み行列と呼ばれ，我々LQRを設計する側が決める設計パラメータです．）．これはざっくりいうと，「できるだけ早く状態を安定化させたい（$x\rightarrow 0$にしたい）」でも「入力を小さく抑えたい」という欲求のバランスをとっている形となっており，そのバランスを$Q\in R^{n\times n}$，$R\in R^{m\times m}$という行列の大きさで表しています．なお，この評価関数が$x^TQx$，$u^TRu$などの$x$，$u$の二乗の項からなっていることが，LQR，線形「二次」安定化と呼ばれる所以です．

仮に$Q=0$，$R=1$だとすると，評価関数は$J=\int_0^\infty u^2\ dt$となります．これは下の図のように，制御入力-時刻のグラフにおける色で塗りつぶした面積を示しています．※すいません，絵を描いたはいいですが，間違ってました．．面積は$\int_0^\infty |u|dt$を表しているので，若干違います．．．あくまでイメージとしてご参考ください．

この面積を小さくする制御を最小にするような制御ゲイン$k_1$，$k_2$をLQRは求めてくれるわけです．

実際にLQRを使ってフィードバックゲインを求めるには，Riccati（リッカチ）方程式を解く必要があります．これは下に示すような行列方程式で書かれます．

PA+A^TP-PBR^{-1}B^TP+Q=0

ここまでの説明で，$A, B, Q, R$は出てきました．これらの行列を用いて，このRiccati方程式を満たす行列$P\in R^{n\times n}$が計算出来たら，この$P$を用いて評価関数$J$を最小化するフィードバックゲイン$K$は$K=R^{-1}B^TP$と計算することができます．
この導出について数式をだらだらと書くのはやめますが，イメージとしては最小にすべき評価関数が入力の二次関数になっている（$u^TRu$の項）ので，その最小値を求めるためには$u$で微分して$=0$になる点が最小値だよね！って流れです．

最終的に制御入力は

u=-R^{-1}B^TPx

となります．

実際に質点系の式に対して，Riccati方程式を解き，フィードバックゲインを計算してみます．
先ほど求めた質点系の状態方程式から

A=
\begin{bmatrix}
0 & 1 \\ 0 & 0
\end{bmatrix}
,\ B=
\begin{bmatrix}
0\\1/m
\end{bmatrix}

であることが分かります．今回は簡単のために

Q=
\begin{bmatrix}
q & 0 \\ 0 & q
\end{bmatrix}
,\ R=1

，質点の質量は$m=1$として解きます．riccati方程式の解$P$を

P=
\begin{bmatrix}
P_{11} & P_{12} \\ P_{21} & P_{22}
\end{bmatrix}

と置いてRiccati方程式の各成分を計算していくと

\begin{bmatrix}
P_{11} & P_{12} \\ P_{21} & P_{22}
\end{bmatrix}
\begin{bmatrix}
0 & 1 \\ 0 & 0
\end{bmatrix}
+
\begin{bmatrix}
0 & 0 \\ 1 & 0
\end{bmatrix}
\begin{bmatrix}
P_{11} & P_{12} \\ P_{21} & P_{22}
\end{bmatrix}
-
\begin{bmatrix}
P_{11} & P_{12} \\ P_{21} & P_{22}
\end{bmatrix}
\begin{bmatrix}
0\\1
\end{bmatrix}
\begin{bmatrix}
0 & 1
\end{bmatrix}
\begin{bmatrix}
P_{11} & P_{12} \\ P_{21} & P_{22}
\end{bmatrix}
+
\begin{bmatrix}
q & 0 \\ 0 & q
\end{bmatrix}
=
\begin{bmatrix}
0 & 0 \\ 0 & 0
\end{bmatrix}

これを各成分$P_{11},\ P_{12},\ P_{21},\ P_{22}$ごとに計算すると...　$P=$
いや，計算したくないですね．．．めちゃくちゃめんどくさいです．2次元なのでまだ計算してみようかと思いますが，これが10次元とかになると最悪です．展開すらしたくありません．
こういう計算は普通，数値計算で行います．説明は下に．

Riccati方程式の解き方

じゃあ，Riccati方程式はどう解くのかというと，大きく分けて，2通りの方法があります．

1つ目はイタレーションベースの方法．
$P_0$を適当な初期値（だいたい$P_0=Q$とします）として，微分方程式

\dot P = PA+A^TP-PBR^{-1}B^TP+Q

を時間積分していき，$P$の変化がなくなったら，それがRiccati方程式の解$P$になっています．ただ，この方法は初期値をどうとるの，とか，離散間隔をどうするの，とか，結構めんどくさいのであまりお勧めはしません．

2つ目は有本-ポッターの方法（固有値分解法）と呼ばれる制御界では珍しく日本人の名前が使われている方法です．こいつは固有値分解を駆使して単純な計算からRiccati方程式の解を求めることができます．（計算は簡単ですが，照明は若干ややこしいので省きます．簡単に言うと，ハミルトン行列というのを定義し，その安定な固有ベクトルを組み合わせてRiccati方程式の解を計算することができます．詳しくはこちら(wiki)　https://en.wikipedia.org/wiki/Algebraic_Riccati_equation ）

ソースコード

C++でEigenを使ったRiccatiソルバーを書きました．適当にA,B,Q,Rを決めてイタレーションベースの解法と有本ポッターの方法でRiccati方程式を解いて，計算時間を比較してます．https://github.com/TakaHoribe/Riccati_Solver/blob/master/riccati_solver.cpp

solve_riccati.cpp

/*
// solvers for Algebraic Riccati equation
// - Iteration (continuous)
// - Iteration (discrete)
// - Arimoto-Potter
//
// author: Horibe Takamasa
*/

#include <Eigen/Dense>
#include <iostream>
#include <time.h>
#include <vector>

#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl

bool solveRiccatiIterationC(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
                            const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,
                            Eigen::MatrixXd &P, const double dt = 0.001,
                            const double &tolerance = 1.E-5,
                            const uint iter_max = 100000) {
  P = Q; // initialize

  Eigen::MatrixXd P_next;

  Eigen::MatrixXd AT = A.transpose();
  Eigen::MatrixXd BT = B.transpose();
  Eigen::MatrixXd Rinv = R.inverse();

  double diff;
  for (uint i = 0; i < iter_max; ++i) {
    P_next = P + (P * A + AT * P - P * B * Rinv * BT * P + Q) * dt;
    diff = fabs((P_next - P).maxCoeff());
    P = P_next;
    if (diff < tolerance) {
      std::cout << "iteration mumber = " << i << std::endl;
      return true;
    }
  }
  return false; // over iteration limit
}

bool solveRiccatiArimotoPotter(const Eigen::MatrixXd &A,
                               const Eigen::MatrixXd &B,
                               const Eigen::MatrixXd &Q,
                               const Eigen::MatrixXd &R, Eigen::MatrixXd &P) {

  const uint dim_x = A.rows();
  const uint dim_u = B.cols();

  // set Hamilton matrix
  Eigen::MatrixXd Ham = Eigen::MatrixXd::Zero(2 * dim_x, 2 * dim_x);
  Ham << A, -B * R.inverse() * B.transpose(), -Q, -A.transpose();

  // calc eigenvalues and eigenvectors
  Eigen::EigenSolver<Eigen::MatrixXd> Eigs(Ham);

  // check eigen values
  // std::cout << "eigen values：\n" << Eigs.eigenvalues() << std::endl;
  // std::cout << "eigen vectors：\n" << Eigs.eigenvectors() << std::endl;

  // extract stable eigenvectors into 'eigvec'
  Eigen::MatrixXcd eigvec = Eigen::MatrixXcd::Zero(2 * dim_x, dim_x);
  int j = 0;
  for (int i = 0; i < 2 * dim_x; ++i) {
    if (Eigs.eigenvalues()[i].real() < 0.) {
      eigvec.col(j) = Eigs.eigenvectors().block(0, i, 2 * dim_x, 1);
      ++j;
    }
  }

  // calc P with stable eigen vector matrix
  Eigen::MatrixXcd Vs_1, Vs_2;
  Vs_1 = eigvec.block(0, 0, dim_x, dim_x);
  Vs_2 = eigvec.block(dim_x, 0, dim_x, dim_x);
  P = (Vs_2 * Vs_1.inverse()).real();

  return true;
}

int main() {

  const uint dim_x = 4;
  const uint dim_u = 1;
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(dim_x, dim_x);
  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(dim_x, dim_u);
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x, dim_x);
  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_u, dim_u);
  Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x, dim_x);

  A(0, 1) = 1.0;
  A(1, 1) = -15.0;
  A(1, 2) = 10.0;
  A(2, 3) = 1.0;
  A(3, 3) = -15.0;
  B(1, 0) = 10.0;
  B(3, 0) = 1.0;

  Q(0, 0) = 1.0;
  Q(2, 2) = 1.0;
  Q(3, 3) = 2.0;

  R(0, 0) = 1.0;

  PRINT_MAT(A);
  PRINT_MAT(B);
  PRINT_MAT(Q);
  PRINT_MAT(R);

  /* == iteration based Riccati solution (continuous) == */
  std::cout << "-- Iteration based method (continuous) --" << std::endl;
  clock_t start = clock();
  solveRiccatiIterationC(A, B, Q, R, P);
  clock_t end = clock();
  std::cout << "computation time = " << (double)(end - start) / CLOCKS_PER_SEC
            << "sec." << std::endl;
  PRINT_MAT(P);


  /* == eigen decomposition method (Arimoto-Potter algorithm) == */
  std::cout << "-- Eigen decomposition mathod --" << std::endl;
  start = clock();
  solveRiccatiArimotoPotter(A, B, Q, R, P);
  end = clock();
  std::cout << "computation time = " << (double)(end - start) / CLOCKS_PER_SEC
            << "sec." << std::endl;
  PRINT_MAT(P);

  return 0;
}

結果はこんな感じ．計算速度は有本ポッターが100倍程度早くて圧勝でした．

A:
  0   1   0   0
  0 -15  10   0
  0   0   0   1
  0   0   0 -15

B:
 0
10
 0
 1

Q:
1 0 0 0
0 0 0 0
0 0 1 0
0 0 0 0

R:
1

-- Iteration based method --
iteration mumber = 39528
computation time = 0.51486sec.
P:
     1.57013     0.100326    0.0141629  -0.00331808
    0.100326   0.00654645  0.000944411 -0.000218176
   0.0141629  0.000944411       14.992     0.994991
 -0.00331808 -0.000218176     0.994991    0.0661947

-- Eigen decomposition mathod --
computation time = 0.002128sec.
P:
     1.57107     0.100388   0.00582396  -0.00387677
    0.100388   0.00655059  0.000388428 -0.000255426
  0.00582396  0.000388428      15.0667     0.999992
 -0.00387677 -0.000255426     0.999992    0.0665297

有本ポッターの方法は固有値分解がベースとなっているので，Riccati方程式をイタレーションして解くよりも遥かに高速です（たぶん）．固有値分解はRiccati方程式よりもはるかに需要があるので，高速解法（名前忘れましたが）が確立しているのが大きな要因なんだと個人的には思っています．あとはEigenの固有値分解ソルバーがすごい最適化されているのも要因の一つだと思います．

※matlabだと，lqrコマンドで一発で計算できるので楽なんですけどね．．．ちなみにmatlabのlqrコマンドは内部で有本ポッター法を実装しています。

まとめ

・LQRってどんな制御？
　→　「線形システム」に対して「二次形式の評価関数」を最小にするような「状態フィードバック制御」

・メリットは？
　→　手軽に最適な制御が組める．他入力他出力系に適用できる．

・どうやってLQRを設計するの？
　→　システムのモデルを組んで，A, B, Q, R行列を決定し，Riccati方程式を解く．

・Riccati方程式ってどう解くの？
　→　イタレーションか，有本ポッター法．後者の方が高速（だと思っている）．