何やったか

制御工学でよく用いられる最適レギュレータLQRをC++で実装してみた
下記コードあり

背景

普段Matlabで実装されている関数lqrを用いていたのでROSで動かすとき動かすとき困った．
Pythonの場合python controlというパッケージに入っているが，C++では見当たらなかった．

前提条件

Eigenライブラリーが入っている

参考文献

解説

最適レギュレータとは

問題設定

制御対象 $ y=Ax+Bu $ がある時，そのシステムを安定化させるようなフィードバックゲイン $ K $ を求める．
フィードバックをかけた時の閉ループ系 $ \dot{x} = (A-BK)x $ が安定化することを目指す

最適制御問題

制御対象 $ y=Ax+Bu $ がある時，

評価関数 V = \int_{0}^{\infty} (x^{T}Qx + u^{T}Ru) dt

を最小化にするような制御入力$u$を求める

制御入力$u$ は

 u = -R^{-1}B^{T}Px

で求められるが，ここで$R$はリカッチ方程式ので得られる正定解である．

リカッチ方程式　A^{T}P+PA-PBR^{-1}BP + Q = 0

を求めて得られる解$P$を代入することで制御入力$u$が求められる．

リカッチ方程式の求め方

数値積分法
- ループ，遅い．微分リカッチ方程式の数値積分
有本-ポッター法
- 固有値分解．ハミルトン行列の応用
Schur分解法
- 固有値分解の改良版
ニュートン法
- ループ計算，遅い，でも実装楽．

有本ポッター法を用いる

コード

Eigen::MatrixXd calcGainK()
{
    /**
    * Calculate LQR Gain K
    * Solves Riccati Equation using Arimoto Potter Method
    */
    Eigen::MatrixXd  P = care(A, B, Q, R);
    return R.inverse() * B.transpose() * P;
}

Eigen::MatrixXd care(const Eigen::MatrixXd A,const Eigen::MatrixXd B,const Eigen::MatrixXd Q,const Eigen::MatrixXd R)
{
    int n = (int)A.rows();
    // Hamilton Matrix
    Eigen::MatrixXd Ham(2*n, 2*n);
    Ham << A, -B*R.inverse()*B.transpose(), -Q, -A.transpose();

    // EigenVec, Value
    Eigen::EigenSolver<Eigen::MatrixXd> Eigs(Ham);
    if (Eigs.info() != Eigen::Success) abort();

    // eigenvector storage
    Eigen::MatrixXcd eigvec(2*n, n);
    int j = 0;

    // store those with negative real number
    for(int i = 0; i < 2*n; ++i){
        if(Eigs.eigenvalues()[i].real() < 0){
            eigvec.col(j) = Eigs.eigenvectors().block(0, i, 2*n, 1);
            ++j;
        }
    }

    Eigen::MatrixXcd U(n, n);
    Eigen::MatrixXcd V(n, n);

    U = eigvec.block(0,0,n,n);
    V = eigvec.block(n,0,n,n);

    return (V * U.inverse()).real();
}

最適レギュレータ LQR (Linear–quadratic regulator) をC++で書いてみた