はじめに

　以前、iLQRのシミュレーションをMATLABで作る記事を書いたのですが、理論的説明はwikipediaの説明に沿って補足するくらいで、数式的な説明をきちんと出来ていませんでした。なので、今回はiLQRの基（？）となるLQRと合わせて理論をまとめようと思います。

環境

MATLAB R2023a

参考文献

LQR

LQRとは

　LQRと聞くと最適レギュレーターを想像する方が多いと思います。日本語の文献でもLQRと言うとこの最適レギュレーターの事を指している事が多いです。言ってしまえば無限ホライズン最適制御です。ただ、LQRにも有限ホライズンのバージョンがあり、こちらもLQRと言うようです。本記事では後者を扱います。

問題設定

　次のような線形システムの制御を考えます。

x_{k+1}=Ax_{k}+Bu_{k}\left(=f(x_{k},u_{k})\right)

また、評価関数は次の様に設定します。

\begin{align}
J &= \frac{1}{2} x_{N}^{T} S x_{N} + s x_{N} 
+ \sum_{k=0}^{N-1} l_{k}(x_{k},u_{k})\\
l_{k}(x_{k},u_{k}) &= \frac{1}{2}x_{k}^{T} Q x_{k} + q x_{k}
+ \frac{1}{2}u_{k}^{T} R u_{k} + r u_{k}
\end{align}

この評価関数を最小化するような制御を考えます。

動的計画法

　この評価関数のうち、$x_{k},u_{k}$以降の部分のみを取り出したものの最適値、つまり次のような関数$V_{k}(x_{k})$を考えます。

V_{k}(x_{k}) = 
\min \frac{1}{2} x_{N}^{T} S x_{N} + s x_{N} 
+ \sum_{k=k}^{N-1} l_{k}(x_{k},u_{k})

この$V_{k}(x_{k})$は強化学習で言う所の状態価値関数に当たるものです。$k$ステップ目に状態変数が$x_{k}$だった時、評価関数のそれ以降の部分の最小値がどうなるかを表す関数です。まあ言ってしまえばある程度先までの出費の最低値です。モデル予測制御の文脈だとcost-to-go、又は残余コストと呼ばれたりします。加えて、次のような関数$Q_{k}$を定義します。

Q_{k}(x_{k},u_{k}) = l_{k}(x_{k},u_{k}) + V_{k+1}(x_{k+1})

こちらは行動価値関数に当たるものです。つまり、ホライズン内の$k$ステップにおいて状態変数が$x_{k}$であり、そこから行動$u_{k}$を選択した場合に評価関数のそこから先の部分の最小値がどうなるかを表す関数です。この$Q_{k}$が最小となるように入力$u_{k}$を選んでいけば評価関数は最小化されることになります。

　動的計画法ではホライズンの後ろから$V_{k}$と$Q_{k}$を考えて行きます。まず、$V_{N}$からスタートします。当然ながら

V_{N}(x_{N})=\frac{1}{2} x_{N}^{T} S x_{N} + s x_{N}

となります。では、$u_{N-1}$の解$u_{N-1}^*$を求めてみましょう。$Q_{N-1}$を考えます。

Q_{N-1}(x_{N-1},u_{N-1})=l_{N-1}(x_{N-1},u_{N-1})+V_{N}(x_{N})

状態変数が$x_{N-1}$となる時、この$Q_{N-1}$が最小値となるような$u_{N-1}$を求めます。つまり、$\partial Q_{N-1}/\partial u_{N-1}$が$0$となれば良いわけです。

\begin{align}
\frac{\partial Q_{N-1}}{\partial u_{N-1}} &=
\frac{\partial l_{N-1}}{\partial u_{N-1}} + \frac{\partial V_{N}}{\partial u_{N-1}}\\
& = u_{N-1}^{T}R + r + \frac{\partial V_{N}}{\partial x_{N}} \frac{x_{N}}{u_{N-1}}\\
& = u_{N-1}^{T}R + r + \left( x_{N}^{T}S + s \right) \frac{\partial f}{\partial u_{N-1}}\\
& = u_{N-1}^{T}R + r + \left( x_{N}^{T}S + s \right) B\\
& = u_{N-1}^{T}R + r + \left\{ \left(Ax_{N-1}+Bu_{N-1}\right)^{T}S + s \right\} B\\
& = u_{N-1}^{T} \left(R + B^{T}SB \right) + x_{N-1}^{T}A^{T}SB + r + sB\\
\\
\therefore 0 &= \left(R + B^{T}SB \right) u_{N-1}^* + B^{T}SAx_{N-1} + r^{T} + B^{T}s^{T}\\
\therefore u_{N-1}^* &= - \left(R + B^{T}SB \right)^{-1} B^{T}SAx_{N-1}
- \left(R + B^{T}SB \right)^{-1} \left(r^{T} + B^{T}s^{T} \right)\\
& = K_{N-1}x_{N-1} + d_{N-1}\\
K_{N-1} & \equiv - \left(R + B^{T}SB \right)^{-1} B^{T}SA\\
d_{N-1} & \equiv - \left(R + B^{T}SB \right)^{-1} \left(r^{T} + B^{T}s^{T} \right)
\end{align}

式変形を一つ一つ書いたので少し長いですが、こうなります。$u_{N-1}^*$の式が求まりました。ただ、$x_{N-1}$の値がわからないので、まだ具体的な値はわかりません。なので$K_{N-1}$と$d_{N-1}$の値を記録しておきます。

ここで、$u_{N-1}^*$の式を$Q_{N-1}$に代入し、$V_{N-1}$の関数の形を考えます。

\begin{align}
V_{N-1}(x_{N-1}) &= l_{N-1}(x_{N-1},u_{N-1}^*) + V_{N}(f(x_{N-1},u_{N-1}^*))\\
& = \frac{1}{2}x_{N-1}^{T} Q x_{N-1} + q x_{N-1}
+ \frac{1}{2}u_{N-1}^{*T} R u_{N-1}^* + r u_{N-1}^*
+ \frac{1}{2} \left(Ax_{N-1} + Bu_{N-1}^* \right)^{T} S \left(Ax_{N-1} + Bu_{N-1}^* \right) + s\left(Ax_{N-1} + Bu_{N-1}^* \right)\\
& = \frac{1}{2}x_{N-1}^{T} \left\{ Q + K_{N-1}^{T}(R+B^{T}SB)K_{N-1} + A^{T}SA + A^{T}SBK_{N-1} + K_{N-1}^{T}B^{T}SA \right\} x_{N-1}
+ \left\{q + (d_{N-1}^{T}R+r)K_{N-1} + (d_{N-1}^{T}B^{T}S+s)(A+BK_{N-1}) \right\} x_{N-1} + \frac{1}{2} d_{N-1}^{T} \left(R + B^{T}S B \right) d_{N-1}
+ sBd_{N-1}\\
& = \frac{1}{2}x_{N-1}^{T}S_{N-1}x_{N-1} + s_{N-1}x_{N-1} + \text{Const.}\\
S_{N-1} & \equiv Q + K_{N-1}^{T}(R+B^{T}SB)K_{N-1} + A^{T}SA + A^{T}SBK_{N-1} + K_{N-1}^{T}B^{T}SA\\
s_{N-1} & \equiv q + (d_{N-1}^{T}R+r)K_{N-1} + (d_{N-1}^{T}B^{T}S+s)(A+BK_{N-1})
\end{align}

$V_{N-1}$を$V_{N}$と同様に$x_{N-1}$の二次形式で表す事が出来ました。ここから、$u_{N-1}^*$を求めたのと同じプロセスで$u_{N-2}^*$と$V_{N-2}$が次のように求まります。

\begin{align}
u_{N-2}^* & = K_{N-2}x_{N-2} + d_{N-2}\\
K_{N-2} & \equiv - \left(R + B^{T}S_{N-1}B \right)^{-1} B^{T}S_{N-1}A\\
d_{N-2} & \equiv - \left(R + B^{T}S_{N-1}B \right)^{-1} \left(r^{T} + B^{T}s_{N-1}^{T} \right)\\
\\
V_{N-2}(x_{N-2}) &= \frac{1}{2}x_{N-2}^{T}S_{N-2}x_{N-2} + s_{N-2}x_{N-2} + \text{Const.}\\
S_{N-2} &\equiv Q + K_{N-2}^{T}(R+B^{T}S_{N-1}B)K_{N-2} + A^{T}S_{N-1}A + A^{T}S_{N-1}BK_{N-2} + K_{N-2}^{T}B^{T}S_{N-1}A \\
s_{N-2} & \equiv q + (d_{N-2}^{T}R+r)K_{N-2} + (d_{N-2}^{T}B^{T}S_{N-1}+s_{N-1})(A+BK_{N-2})
\end{align}

同じプロセスを繰り返していく事で$K_{N-1} \sim K_{0}$及び$d_{N-1}\sim d_{0}$が求まります。この流れを一般化して書いておきます。

\begin{align}
u_{k}^* & = K_{k}x_{k} + d_{k}\\
K_{k} & \equiv - \left(R + B^{T}S_{k+1}B \right)^{-1} B^{T}S_{k+1}A\\
d_{k} & \equiv - \left(R + B^{T}S_{k+1}B \right)^{-1} \left(r^{T} + B^{T}s_{k+1}^{T} \right)\\
\\
V_{k}(x_{k}) &= \frac{1}{2}x_{k}^{T}S_{k}x_{k} + s_{k}x_{k} + \text{Const.}\\
S_{k} &\equiv Q + K_{k}^{T}(R+B^{T}S_{k+1}B)K_{k} + A^{T}S_{k+1}A + A^{T}S_{k+1}BK_{k} + K_{k}^{T}B^{T}S_{k+1}A \\
s_{k} & \equiv q + (d_{k}^{T}R+r)K_{k} + (d_{k}^{T}B^{T}S_{k+1}+s_{k+1})(A+BK_{k})
\end{align}

全ての$K$と$d$が求まったら、ホライズン内の最初のステップから$u^*$を求めて行きます。まず$u_{0}^*$を計算します。（$x_{0}$は初期座標であり、ここでは既に判明しているものとして扱います。）

u_{0}^* = K_{0}x_{0} + d_{0}

これだけです。次に$u_{1}^*$を求めます。まず$x_{1}$が必要ですから、$u_{0}^*$を使ってそれを求めます。

\begin{align}
x_{1} &= f(x_{0},u_{0}^*)\\
u_{1}^* &= K_{1}x_{1} + d_{1}
\end{align}

同じプロセスを繰り返して$u_{N-1}^*$まで求めて行きます。モデル予測制御として使う場合は$u_{0}^*$のみを実際の入力として使います。

iLQR

iLQRとは

　LQRを非線形制御に拡張した手法です。正確にはiterative LQRであり、LQRを微小（？）な更新値に対して反復するアルゴリズムになっています。また、評価関数にも二次形式以外の物を使う事が可能になっています。

問題設定

　次のような非線形システムの制御を考えます。

x_{k+1} = f_{k}(x_{k},u_{k})

以下の様な評価関数を設定します。少し文字の置き方を変えましたが、LQRと同じものです。

\begin{align}
J &= \phi(x_{N}) + \sum_{k=0}^{N-1} l_{k}(x_{k},u_{k})\\
\phi(x_{N}) &= \frac{1}{2} x_{N}^{T} S x_{N} + s x_{N}\\
l_{k}(x_{k},u_{k}) &= \frac{1}{2}x_{k}^{T} Q x_{k} + q x_{k}
+ \frac{1}{2}u_{k}^{T} R u_{k} + r u_{k}
\end{align}

動的計画法

　今回は状態方程式が非線形ですから、そのままではLQRと同じような手法は使えません。そこで、線形近似出来るような微小な変分に直して考えると言アイデアを使います。

　まず、予測ホライズン内の入力$u_{0} \sim u_{N-1}$及び状態変数$x_{0} \sim x_{N}$について、何らかの初期値が存在するとします。（この初期値は最適解である必要はありません。前の制御ステップで計算した値を使用する事が多いですが、初期解を全て0にしても計算はきちんと上手く行きます。）その周辺で以下の様に状態方程式の変分を取ります。

\begin{align}
\delta x_{k+1} &= A_{k}\delta x_{k} + B_{k}\delta u_{k}\\
A_{k} & \equiv \frac{\partial f_{k}}{\partial x_{k}}\\
B_{k} & \equiv \frac{\partial f_{k}}{\partial u_{k}}
\end{align}

この微小な変分の式を線形状態方程式の様に使う事になります。

　まず、LQRの時と同様に$V_{k}$と$Q_{k}$を定義します。

\begin{align}
V_{k}(x_{k}) &= \min \phi(x_{N}) + \sum_{k=k}^{N-1} l_{k}(x_{k},u_{k})\\
Q_{k}(x_{k},u_{k}) &= l_{k}(x_{k},u_{k}) + V_{k+1}(x_{k+1})
\end{align}

ここから、$Q_{k}$を最小化するように$u_{k}$を求めるわけです。が、ここでひと工夫して、$Q_{k}$の変分$\delta Q_{k}$を最小化するように$u_{k}$の微小変化$\delta u_{k}$を求めると言う風に問題の捉え方を変えます。つまり、

\delta Q_{k} = \frac{1}{2}\delta x_{k}^{T} \ Q_{k,xx} \delta x_{k} 
+ Q_{k,x} \delta x_{k}
+ \frac{1}{2} \delta u_{k}^{T} \ Q_{k,uu} \delta u_{k} + Q_{k,u} \delta u_{k}
+ \delta x_{k}^{T} \ Q_{k,xu} \delta u_{k}\\

に対し、$\partial \delta Q_{k}/ \partial \delta u_{k}=0$となるように$\delta u_{k}$を求めれば良いわけです。以下の様になります。

\begin{align}
\frac{\partial \delta Q_{k}}{\partial \delta u_{k}} &= 
\delta u_{k}^{T} \ Q_{k,uu} + Q_{k,u} + \delta x_{k}^{T} \ Q_{k,xu}\\
\therefore 0 &= Q_{k,uu} \delta u_{k} + Q_{k,u}^{T} + Q_{k,ux} \delta x_{k}\\
\therefore \delta u_{k} &= - Q_{k,uu}^{-1} Q_{k,ux} \delta x_{k} - Q_{k,uu}^{-1}Q_{k,u}^{T}\\
&= K_{k} \delta x_{k} + d_{k}\\
\\
K_{k} & \equiv - Q_{k,uu}^{-1} Q_{k,ux}\\
d_{k} & \equiv - Q_{k,uu}^{-1}Q_{k,u}^{T}
\end{align}

尚、偏微分を文字の添え字に$u$や$x$を付ける事で表現しています。各偏微分の計算は以下のようになります。

\begin{align}
Q_{k,uu} &= l_{k,uu}
+ \left( \frac{\partial x_{k+1}}{\partial u_{k}} \right)^{T} V_{k+1,xx} \frac{\partial x_{k+1}}{\partial u_{k}}\\
&= l_{k,uu} + B_{k}^{T} V_{k+1,xx} B_{k} \\
Q_{k,u} &= l_{k,u} + V_{k+1,x} \frac{\partial x_{k+1}}{\partial u_{k}}\\
&= l_{k,u} + V_{k+1,x} B_{k}\\
Q_{k,xx} &= l_{k,xx}
+ \left( \frac{\partial x_{k+1}}{\partial x_{k}} \right)^{T} V_{k+1,xx} \frac{\partial x_{k+1}}{\partial x_{k}}\\
&= l_{k,xx} + A_{k}^{T} V_{k+1,xx} A_{k} \\
Q_{k,x} &= l_{k,x} + V_{k+1,x} \frac{\partial x_{k+1}}{\partial x_{k}}\\
&= l_{k,x} + V_{k+1,x} A_{k}\\
Q_{k,xu} &= l_{k,xu} 
+ \left( \frac{\partial x_{k+1}}{\partial x_{x}} \right)^{T} V_{k+1,xx} \frac{\partial x_{k+1}}{\partial u_{x}}\\
&= l_{k,xu} + A_{k}^{T} V_{k+1,xx} B_{k}\\
Q_{k,ux} &= Q_{k,xu}^{T}
\end{align}

また、求まった$\delta u_{k}$の式を$\delta Q_{k}$の式に代入すると、$\delta V_{k}$の式が求まります。

\begin{align}
\delta V_{k} &= \frac{1}{2}\delta x_{k}^{T} \ Q_{k,xx} \delta x_{k} 
+ Q_{k,x} \delta x_{k}
+ \frac{1}{2} (K_{k} \delta x_{k} + d_{k})^{T} \ Q_{k,uu} (K_{k} \delta x_{k} + d_{k}) + Q_{k,u} (K_{k} \delta x_{k} + d_{k})
+ \delta x_{k}^{T} \ Q_{k,xu} (K_{k} \delta x_{k} + d_{k})\\
&= \frac{1}{2} \delta x_{k}^{T} \left( Q_{k,xx} +K_{k}^{T} Q_{k,uu} K_{k} +K_{k}^{T}Q_{k,ux} + Q_{k,xu}K_{k} \right) \delta x_{k}
+ \left( Q_{k,x} + d_{k} Q_{k,uu}K_{k} + Q_{k,u}K_{k} + d_{k}Q_{k,ux} \right) \delta x_{k} + \frac{1}{2} d_{k} Q_{k,uu} d_{k} + Q_{k,u}d_{k}\\
&= \frac{1}{2} \delta x_{k}^{T} V_{k,xx} \delta x_{k} + V_{k,x} \delta x_{k} + \text{Const.}\\
\therefore V_{k,xx} &= Q_{k,xx} +K_{k}^{T} Q_{k,uu} K_{k} +K_{k}^{T}Q_{k,ux} + Q_{k,xu}K_{k}\\
V_{k,x} &= Q_{k,x} + d_{k} Q_{k,uu}K_{k} + Q_{k,u}K_{k} + d_{k}Q_{k,ux}
\end{align}

このプロセスを繰り返して$V_{k,xx},V_{k,x}$を更新しながら$K,d$を求め、記録して行きます。当然、$V_{N,xx}=S,V_{N,x}=s$となります。後はLQRと全く同じです。求めた$K_{k},d_{k}$を使い、$\delta x_{k}$を更新しながら$\delta u_{k}$を計算して行きます。ただし、$\delta x_{0}=0$です。

\begin{align}
\delta u_{0} &= K_{0} \delta x_{0} + d_{0}\\
\delta x_{1} &= A_{0} \delta x_{0} + B_{0} \delta u_{0}\\
\delta u_{1} &= K_{1} \delta x_{1} + d_{1}\\
\vdots
\end{align}

これを繰り返すだけです。そうすれば新しい解$u_{0}\sim u_{N-1},x_{0} \sim x_{N}$が求まります。今度はこれを初期解としてまた$\delta u,\delta x$を求めて行きます。これを評価関数の値が変化しなくなる（≒最適解に到達する）まで繰り返します。

ここで、LQRとの関連を明確にするために少し説明を追加します。$\delta Q_{k}$を敢えて下の様に分解してみます。

\begin{align}
\delta Q_{k} &= \delta l_{k} + \delta V_{k+1}\\
\delta l_{k} &= \frac{1}{2}\delta x_{k}^{T} \ l_{k,xx} \delta x_{k} 
+ l_{k,x} \delta x_{k}
+ \frac{1}{2} \delta u_{k}^{T} \ l_{k,uu} \delta u_{k} + l_{k,u} \delta u_{k}
+ \delta x_{k}^{T} \ l_{k,xu} \delta u_{k}\\
\delta V_{k+1} &= \frac{1}{2} \delta x_{k+1}^{T} \ V_{k+1,xx} \delta x_{k+1}
+ V_{k+1,x} \delta x_{k+1} + \text{Const.}
\end{align}

この$\delta Q_{k}$を最小にするような$\delta u_{k}$を求めるわけですが、$l_{k,xu}=0$とすれば、これが下の様なLQRにおける式と同じ形をしています。

\begin{align}
Q_{k} &= l_{k}(x_{k},u_{k}) + V_{k+1}(x_{k+1})\\
l_{k}(x_{k},u_{k}) &= \frac{1}{2}x_{k}^{T} Q x_{k} + q x_{k} + \frac{1}{2}u_{k}^{T} R u_{k} + r u_{k}\\
V_{k+1}(x_{k+1}) &= \frac{1}{2} x_{k+1}^{T} S_{k+1} x_{k+1} + s_{k+1} x_{k+1} + \text{Const.}
\end{align}

$\delta x,\delta u$を$x,u$に、$l_{k,xx}$や$l_{k,uu}$を$Q$や$R$に変えれば同じ式になります。$\delta u_{k}$の式も$\delta Q_{k}$の様に分解してみましょう。

\begin{align}
\delta u_{k} &= K_{k}\delta x_{k} + d_{k}\\
&= - Q_{k,uu}^{-1} Q_{k,ux} \delta x_{k} - Q_{k,uu}^{-1}Q_{k,u}^{T}\\
&= - \left( l_{k,uu} + B_{k}^{T}V_{k+1,xx}B_{k} \right)^{-1} B_{k}^{T} V_{k+1,xx} A_{k} \delta x_{k}
- \left( l_{k,uu} + B_{k}^{T}V_{k+1,xx}B_{k} \right)^{-1} \left( l_{k,u}^{T} + B_{k}^{T} V_{k+1,x}^{T} \right)
\end{align}

LQRの$u_{k}^*$の式を見てみましょう。

u_{k}^* = - \left(R + B^{T}S_{k+1}B \right)^{-1} B^{T}S_{k+1}Ax_{k} - \left( R + B^{T}S_{k+1}B \right)^{-1} \left(r^{T} + B^{T}s_{k+1}^{T} \right)

同じ形をしていますね。$V_{k+1,x},V_{k+1,x}$を$S_{k+1},s_{k+1}$に、$A_{k},B_{k}$を$A,B$に、その他も適宜置き換えれば同じ式になります。同じ形の$\delta Q_{k},Q_{k}$から同じプロセス（最小になるように$\delta u_{k},u_{k}$を求める）を経て求まった式ですから当然です。

これが結局何を意味しているかと言うと、iLQRにおいて何らかの解$u,x$から$\delta u,\delta x$を求めるのは、線形システム

\delta x_{k+1} = A_{k} \delta x_{k} + B_{k} \delta u_{k}

に対し、評価関数（の変分）

\delta J = \frac{1}{2} \delta x_{N}^{T} V_{N,xx} \delta x_{N} + V_{N,x} \delta x_{N}
+ \sum_{k=0}^{N-1} \frac{1}{2} \delta x_{k}^{T} l_{k,xx} \delta x_{k} + l_{k,x} \delta x_{k}
+ \frac{1}{2} \delta u_{k}^{T} l_{k,uu} \delta u_{k} + l_{k,u} \delta u_{k}

を最小化するようなLQR問題に相当するということです。つまるところ、システムを線形近似可能な微小変化の世界でLQR問題を繰り返し解いて行くのがiLQRです。

最後に

　LQRとiLQRについてまとめました。非線形最適制御入門にもLQRは離散時間LQ問題と言う名前で載っているのですが、そちらは主にオイラー・ラグランジュ方程式の切り口から説明しています（動的計画法も載ってはいます）。間違い等ありましたらコメントで教えて頂けると助かります。

追伸

　非線形最適制御入門は自分も好きな本なのですが、オイラー・ラグランジュ法に偏っていて動的計画法が全体としてチョイ役になってしまっているのが個人的にはモヤモヤする点です。特に、オイラー・ラグランジュ方程式での説明において、動手的計画法を先にやっていれば置く必要のない仮定を突然出す事になっていたり、離散時間LQRを載せているのにiLQRを載せていないのもまた個人的にはモヤモヤします。また、数式に時々$\partial V/\partial x$が出てきますが、その数値計算法が扱われていないせいで数式でしかわからない（実際に試せない）状態なのもかゆいです（そもそも数式理論の説明に重きを置いていて、実装はあまり考えていない書籍なのだとは思いますが）。ここら辺の具体例を実際に作る記事を作れたら面白そうです。

LQR/iLQRを理解する

はじめに

環境

参考文献

LQR

LQRとは

問題設定

動的計画法

iLQR

iLQRとは

問題設定

動的計画法

最後に

追伸