概要

こんにちは
連続系のシステムのLQR制御はよく取り上げられていて、皆さん知っているとおもうのですが、実際に使用する場合、離散システムの場合を考える必要があることが多いです。本記事では、連続系は知っている前提で説明します。ほとんど知らなくても読めますが、本記事の目的は連続は知っているけど、そこで設計したゲインを離散システム（プログラム）に突っ込んで良いのかな～？という疑問にぶつかった人に向けて書いています
http://mech.u-fukui.ac.jp/~Kawa-Lab/education/robust/pdf/chap5f.pdf
https://qiita.com/taka_horibe/items/5d053cdaaf4c886d27b5
こちらがわかりやすいです。
最後に離散システムと、連続システムで設計した場合の比較を行います。
参考図書は「デジタル制御入門：萩原　朋道」を使っています。とてもわかりやすいのでオヌヌメです。

最適制御

離散システムを次のように定義します

\begin{align*}
\boldsymbol{x}_{n+1} &= \boldsymbol{Ax}_{n} + \boldsymbol{Bu}_{n}\\
\boldsymbol{y}_{n} &= \boldsymbol{Cx}_{n}
\end{align*}

$n$ は時刻ステップ
$\boldsymbol{x}$ は状態変数
$\boldsymbol{u}$ は入力です。

この時つぎの評価価数を最小化するよう、入力uを決めます。

J = \sum_{k=0}^{N} ({\boldsymbol{x}_k}^T \boldsymbol{Q} \boldsymbol{x}_k + {\boldsymbol{u}_k}^T \boldsymbol{S} \boldsymbol{x}_k + {\boldsymbol{u}_k}^T \boldsymbol{R} \boldsymbol{u}_k) + {\boldsymbol{x}_{N+1}}^T \boldsymbol{P}_{N+1} \boldsymbol{x}_{N+1}

この式は、状態変数 $\boldsymbol x$　と　入力 $\boldsymbol u$　の重み付き二乗ノルムの総和を表しています。$P_{N+1}$の項は終端重みで最終的な状態の重要性を追加しています。Sの部分はない場合も多いです。
このJを最小化するようにする制御を、時刻ステップNまでの総和のため、有限時間最適制御と呼ばれます。
時刻ステップを無限大にする場合を無限時間最適制御とよび、終端項はつきません。

uの計算方法

u_Nの求め方

評価関数を次のように分けます

\begin {align}
J&=J_1 + J_2\\
J_1&= \sum_{k=0}^{N-1} (x_k^T Q x_k + u_k^T S x_k + u_k^T R u_k)\\
J_2 &= x_N^T Q x_N + u_N^T S x_N + u_N^T R u_N+ x_{N+1}^T P_{N+1} x_{N+1}

\end {align}

$J_1$の項は$u_N$の影響を受けません。つまり、$J_1$を最小にするために$u_N$をどういじっても影響を与える事ができない部分です。
状態量$x_0,...,x_N$は$u_N$の影響を受けないためです。
よって、$u_N$は$J_2$を最小化させるよう決定させると考えても同じ結果になります。

\boldsymbol{ x}_{N+1} = \boldsymbol{ Ax}_{N} + \boldsymbol{Bu}_{N}\\

を代入し、$u_N$で平方完成すると次のようになります（どうして思いつくのかはわかりません。実際に項を展開して見比べると同じになります）

J_2 = [u_N +(R+B^TP_{N+1}B)^{-1}(S^T+B^TP_{N+1}Ax_N)]^T\\(R+B^TP_{N+1}B)\\
[u_N +(R+B^TP_{N+1}B)^{-1}(S^T+B^TP_{N+1}Ax_N)]\\
+x_{N}^T P_{N} x_{N}

ここで新しく

P_N = A^TP_{N+1}A+Q-(S^T+B^TP_{N+1}A)^T(R+B^TP_{N+1}B)^{-1}(S^T+B^TP_{N+1}A)

を定義しました。実際に計算すると正しい式変換であることがわかります。
ここで$J_2$の項を見ると、$+x_{N}^T P_{N} x_{N}$は$u_N$の影響をうけませんが、第一項は

u_N=-(R+B^TP_{N+1}B)^{-1}(S^T+B^TP_{N+1}Ax_N)

とすれば、0 にできます。
よって、最適入力が定まります。そして、そのときの$J_2$は$x_{N}^T P_{N} x_{N}$になります。
ここで大切なことは、
$J_2$(時刻N　＋それ以降の評価関数) の最小値は、入力を最適にすると、$x_{N}$の関数になる
という事です。システムが状態方程式に従うとき、入力と初期値を決めると、その時刻以降のシステムの時刻歴はすべて決定します。
今、時刻Nとしたとき、初期値$x_{N}$を定めると、自動的に最適入力$u_N=-(R+B^TP_{N+1}B)^{-1}(S^T+B^TP_{N+1}Ax_N)$を決定し、$J_2$の最小値が$x_{N}^T P_{N} x_{N}$に決定されます。$ P_{N}$は、$P_{N+1}$で決定され、$x_N$は関与しません。
つまり、$J_2$の最小値は$x_{N}$の関数になるのです。

u_N-1　の求め方

時間をさかのぼって、入力を決定します。次に$u_{N-1}$の最適入力を考えます。
評価関数を、また、時刻N-1より前と今と後に分けます。

\begin {align}
J&=J_a + J_b+ J_c\\
J_a&= \sum_{k=0}^{N-2} (x_k^T Q x_k + u_k^T S x_k + u_k^T R u_k)\\
J_b&=x_{N+1}^T Q x_{N+1} + u_{N+1}^T S x_{N+1} + u_{N+1}^T R u_{N+1}\\
J_c &= x_N^T Q x_N + u_N^T S x_N + u_N^T R u_N+ x_{N+1}^T P_{N+1} x_{N+1}

\end {align}

$J_a$は$u_{N-1}$の影響を受けません。$J_b,J_c$は影響を受けます。
ここで、$J_c$は、$x_{N}$によって、$x_{N}^T P_{N} x_{N}$に決定されるため、

J_c = x_{N}^T P_{N} x_{N}

におきかえます。この説明をするために、先ほど、くどく説明しました。

J_b+J_c = x_{N+1}^T Q x_{N+1} + u_{N+1}^T S x_{N+1} + u_{N+1}^T R u_{N+1} + x_{N}^T P_{N} x_{N}

この式は先ほど見た覚えはありませんか？
この式は、$u_N$を決定するための式

\begin {align}
J&=J_1 + J_2\\
J_1&= \sum_{k=0}^{N-1} (x_k^T Q x_k + u_k^T S x_k + u_k^T R u_k)\\
J_2 &= x_N^T Q x_N + u_N^T S x_N + u_N^T R u_N+ x_{N+1}^T P_{N+1} x_{N+1}

\end {align}

の$J_2$と一致します（$N$を$N-1$に、$N+1$を$N$に置き換えれば）！
つまり、先ほどの最適な$u_N$の決定の方法を$N$を$N-1$に、$N+1$を$N$に置き換えれば、同様に評価関数を最小化する入力が決定できます。
よって、

\begin{align}
u_{N-1}&=-(R+B^TP_{N}B)^{-1}(S^T+B^TP_{N}Ax_{N-1})

\end{align}

となります。このとき、評価関数$J$は

\begin{align}
J&=J_a + x_{N-1}^T P_{N-1} x_{N-1}\\
P_{N-1} &= A^TP_{N}A+Q-(S^T+B^TP_{N}A)^T(R+B^TP_{N}B)^{-1}(S^T+B^TP_{N}A)

\end{align}

になります。

u_k　任意時刻の入力の求め方

以上から、任意時刻kでの$x_k$が定まると、自動的に最適な入力$u_k,...,u_N$が定まり、その入力は

\begin{align}
u_{k}&=-(R+B^TP_{k+1}B)^{-1}(S^T+B^TP_{k+1}Ax_{k})\\
P_{k} &= A^TP_{k+1}A+Q-(S^T+B^TP_{k+1}A)^T(R+B^TP_{k+1}B)^{-1}(S^T+B^TP_{k+1}A)
\end{align}

で決定します。行列$P_k$は$P_{N+1}$からさかのぼって計算されます。

この問題では、$x_0,P_{N+1}$が与えられるので、これで計算されます。
制御系はフィードバック制御なので、時刻kの状態量$x_k$から入力$u_k$を決定すると思われますが、このやり方だとシステムモデル通りに全てが進行すると仮定しているため、$x_0$から全ての入力$u_0,...,u_N$が決定しています。少し違和感があるかも知れません。

以上で離散システムの有限時間LQR最適制御の説明を終わります。

ここで次に疑問になるのは

無限時間は？
定常ゲインにはならないの？
連続システムとの違いは？

だと思います。つぎにこれを説明します。

離散システムの無限時間LQR最適制御

次に、無限時間LQR最適制御を考えます。終端時刻$N$を$\inf$に飛ばせば、無限時間になります。
そして、Pを求める計算の繰り返しで、Pの値は特定の値に収束します。無限時間LQR最適制御では、収束したPを用いてフィードバックゲインFを計算し、定常フィードバックゲインFにして制御を行います。

収束する場合、有限時間の場合の式から

\begin{align}
P &= A^TPA+Q-(S^T+B^TPA)^T(R+B^TPB)^{-1}(S^T+B^TPA)\\
u&=-(R+B^TPB)^{-1}(S^T+B^TP Ax)\\
\end{align}

のように、$P$を求めて、$x$にフィードバックゲインFを掛けて制御を行えば良いことになります。

P = A^TPA+Q-(S^T+B^TPA)^T(R+B^TPB)^{-1}(S^T+B^TPA)

この式は離散時間系のリカッチ方程式と呼ばれます。解法は知りませんが、scilabではriccati、matlabではdareで解けます。matlabを持ってないのでscilabを使って解きます。

//離散時間リカッチ方程式を解く, x(n+1) = AD*x(n) + BD *u(n)  , J = xQx+uRu　の場合
B_ric = BD*inv(R)*BD'
[X1,X2] = riccati(AD,B_ric,Q,'d')
X = X1*inv(X2)
F = -1*inv(R+BD'*X*BD)*(BD'*X*AD)

で解くことができます。XはPの事です。scilabはmatlabと違い、直接定数を突っ込まず、計算処理をする事が多いのでちょっと悩みますが、これで合っているはずです。

disp(X)
hoge = AD'*X*AD+Q-(AD'*X*BD)*inv(R+BD'*X*BD)*(BD'*X*AD)
disp(hoge)
disp("上の二つが一致するならば、離散リカッチ方程式が成立している")

リカッチ方程式の右辺を計算し、Pが一致する事が確認できます。

連続システムのリカッチ方程式による、フィードバックゲインとの違い

最後に、題の通り連続システムとして計算した場合で比較を行いました。

clf();
clear();
for i = 1:10 do
    close
end

//状態方程式設定
A = [5 ];
      
B=[1];

C=[1];

D=[0];

x0=[1];
    
PN1=[0];   //P_N+1

S = [0];

R = [1];
     
Q = [1];
     
T=0.5       //シミュレーション時間[s]
DT =0.01    //制御周期[s]
N = T/DT

sl = syslin('c',A,B,C,D,x0);
sl2= dscr(sl,DT);              //離散化

AD=sl2(2);
BD=sl2(3);
CD=sl2(4);


//離散時間リカッチ方程式を解く
B_ric = BD*inv(R)*BD'
[X1,X2] = riccati(AD,B_ric,Q,'d')
disp(X1)
disp(X2)
X = X1*inv(X2)
disp(X)
hoge = AD'*X*AD+Q-(AD'*X*BD)*inv(R+BD'*X*BD)*(BD'*X*AD)
disp(hoge)
disp("上の二つが一致するならば、離散リカッチ方程式が成立している")

F = -1*inv(R+BD'*X*BD)*(BD'*X*AD)
disp("離散リカッチのフィードバックゲインF")
disp(F)

//連続時間リカッチ方程式を解く
Bn=B*inv(R)*B';
X=riccati(A,Bn,Q,'c');
F1=-1*inv(R)*B'*X;
disp("連続リカッチのフィードバックゲインF")
disp(F1)

disp("正直そんなにゲインの値に差はない,制御周期を大きくすると差が大きくなる")

いろんな定数で比較をしましたが、上の例だと
離散リカッチのフィードバックゲインF

-9.8479371

連続リカッチのフィードバックゲインF

-10.09902

とそんなに違いのない結果になります。制御周期を大きくしてやると差が大きく現れてきます。

DT=0.01から（上の例） -> DT=0.1にすると
離散リカッチのフィードバックゲインF

-8.0777562

連続リカッチのフィードバックゲインF

-10.09902

DT=1.0　にすると
離散リカッチのフィードバックゲインF

-5.0336984

連続リカッチのフィードバックゲインF

-10.09902

微妙に変わっていく感じです。

DT=0.001にすると
離散リカッチのフィードバックゲインF

-10.073337

連続リカッチのフィードバックゲインF

-10.09902

ほとんど差がない結果になります。

Pの収束性は、（A,B）が可制御（可安定で十分だがほとんど同じ）なら保証されます。

実行コード

有限時間最適制御

LimitedTimeLQR_Control.sce

clf();
clear();
for i = 1:10 do
    close
end

//状態方程式設定
A = [1.1 ];
      
B=[1];

C=[1];

D=[0];

x0=[1];
    
PN1=[10000000];   //P_N+1

S = [1];

R = [1];
     
Q = [1];
     
T=0.5       //シミュレーション時間[s]
DT =0.01    //制御周期[s]
N = T/DT
//------------------有限時間LQR　フィードバックゲインF, 評価行列Pの計算
saveP = PN1
disp(N+1)
disp(PN1)
for i = N+1:-1:1
    PN = A'* PN1 *A +Q -(S'+B'*PN1*A)'*inv(R+B'*PN1*B) * (S'+B'*PN1*A)
    FN = -1*inv(R+B'*PN1*B) * (S'+B'*PN1*A)
    
    disp("-----------------")
    disp(i-1)
    disp(PN)
    disp(FN)
    saveP=cat(2,saveP,PN1);  //どんどん横に繋げていく
    if i==N+1
        saveF = FN;
     else
         saveF = cat(2,saveF,FN);
     end
    PN1 = PN
end

//グラフ
disp("F のグラフはN+1から表示しているので、時刻とは逆順に並んでいることに注意") 
plot(saveF);
hl=legend(['F']);
scf()
plot(saveP);
hl=legend(['P']);
scf()

//任意時刻のフィードバックゲインF
k=10
F = saveF((N-k)*1+1:(N-k)*1+2)

//----------------------------シミュレーション-------
//保存用変数設定
xx=x0;
k=0
F = saveF((N-k)*1+1)
uu=F*xx;
yy=C*x0;
xxx=x0;
uuu=uu;
//設定
T=0.5       //シミュレーション時間[s]
DT =0.01    //制御周期[s]
t=0:DT:T;

for    i = 1:T/DT  do
    yy=C*xx;
    k=i
    F = saveF((N-k)*1+1)
    uu=F*xx
  
    xx=A*xx+B*uu;
    
    xxx=cat(2,xxx,xx);  //どんどん列方向に　xxx　に　xx　を繋げていく
    uuu=cat(2,uuu,uu);
,end

//表示
plot(t,xxx);
hl=legend(['1','2']);
scf()

plot(t,uuu);
hl=legend(['u']);
scf();

概要

最適制御

uの計算方法

u_Nの求め方

u_N-1 の求め方

u_k 任意時刻の入力の求め方

離散システムの無限時間LQR最適制御

連続システムのリカッチ方程式による、フィードバックゲインとの違い

実行コード

有限時間最適制御

u_N-1　の求め方

u_k　任意時刻の入力の求め方