LQRの問題はコスト関数の最小化問題だが$u=\theta \cdot x$ とおくと、方策勾配法の要領でコスト関数$J$のヤコビアンを使って最急降下法にて

$$\theta \leftarrow \theta -\alpha {\mathrm{\nabla }}_{\theta }J$$

を繰り返すことにより、最適な状態フィードバック制御のパラメータを求めることができる(できそう)。

問題設定

制御対象の離散システムとして

$$x_{i+1}=f(x_i,u_i)i\in [0,N]$$

を考える。このシステムにおいて$u_i={\pi }_{\theta }(x_i)$の制御則を適用した上でコスト関数

$$J^{{\pi }_{\theta }}(x_k,u_k;k,N)=\sum _{i=k}^{N-1}L(x_i,u_i)+L_f(x_N)$$

を考え、$J^{{\pi }_{\theta }}(x_0,u_0)$を最小化するパラメータ ${\theta }^{\ast }$を求めたい。 ここで、$L$はステージコスト、$L_f$は終端コストである。確認のため、定義から

$$J^{{\pi }_{\theta }}(x_k,u_k;k,N)=L(x_k,u_k)+J^{{\pi }_{\theta }}(x_{k+1},u_{k+1};k+1,N-1)$$

である。以降、簡単のため、単に各関数の添字を省略し$\pi$や$J$と書く。

コスト関数勾配の導出

状態変数勾配の導出

コスト関数の勾配導出に先立ち、${\mathrm{\nabla }}_{\theta }{x}_k$を計算する。${\mathrm{\nabla }}_{\theta }{x}_0=0$と${\mathrm{\nabla }}_{\theta }{u}_0={\mathrm{\nabla }}_{\theta }\pi (x_0)$は自明。 以下、$k\ge 1$について、

$$\begin{array}{rl}{\mathrm{\nabla }}_{\theta }{x}_k& ={\mathrm{\nabla }}_{x}f\cdot {\mathrm{\nabla }}_{\theta }{x}_{k-1}+{\mathrm{\nabla }}_{u}f\cdot {\mathrm{\nabla }}_{\theta }{u}_{k-1}\\ & ={\mathrm{\nabla }}_{x}f\cdot {\mathrm{\nabla }}_{\theta }{x}_{k-1}+{\mathrm{\nabla }}_{u}f\cdot {\mathrm{\nabla }}_x\pi \cdot {\mathrm{\nabla }}_{\theta }{x}_{k-1}+{\mathrm{\nabla }}_{u}f\cdot {\mathrm{\nabla }}_{\theta }\pi \\ & ={\mathrm{\nabla }}_{u}f\cdot {\mathrm{\nabla }}_{\theta }\pi +({\mathrm{\nabla }}_{x}f+{\mathrm{\nabla }}_{u}f\cdot {\mathrm{\nabla }}_x\pi )\cdot {\mathrm{\nabla }}_{\theta }{x}_{k-1}\\ & =\sum _{i=0}^{k-1}(\prod _{j=i}^{k-2}{\mathrm{\nabla }}_{x}f{|}_{x_{j+1},\pi (x_{j+1})}+{\mathrm{\nabla }}_{u}f{|}_{x_{j+1},\pi (x_{j+1})}\cdot {\mathrm{\nabla }}_x\pi {|}_{x_{j+1}})\cdot {\mathrm{\nabla }}_{u}f{|}_{x_i,\pi (x_i)}\cdot {\mathrm{\nabla }}_{\theta }\pi {|}_{x_i}\end{array}$$

コスト関数勾配

先ほどと同様${\mathrm{\nabla }}_{\theta }{u}_k={\mathrm{\nabla }}_{\theta }\pi {|}_{x_k}+{\mathrm{\nabla }}_x\pi {|}_{x_k}\cdot {\mathrm{\nabla }}_{\theta }{x}_k$に注意して、

$$\begin{array}{rl}{\mathrm{\nabla }}_{\theta }J{|}_{x_0,\pi (x_0)}& ={\mathrm{\nabla }}_{x}L{|}_{x_0,u_0}\cdot {\mathrm{\nabla }}_{\theta }{x}_0+{\mathrm{\nabla }}_{u}L{|}_{x_0,u_0}\cdot {\mathrm{\nabla }}_{\theta }{u}_0+{\mathrm{\nabla }}_{\theta }J{|}_{x_1,u_1}\\ & ={\displaystyle \sum _{k=0}^{N-1}({\mathrm{\nabla }}_{x}L{|}_{x_k,u_k}\cdot {\mathrm{\nabla }}_{\theta }{x}_k+{\mathrm{\nabla }}_{u}L{|}_{x_k,u_k}\cdot {\mathrm{\nabla }}_{\theta }{u}_k)+{\mathrm{\nabla }}_{\theta }{L}_f{|}_{x_N}}\\ & \begin{array}{rl}=& {\displaystyle \sum _{k=0}^{N-1}({\mathrm{\nabla }}_{x}L{|}_{x_k,u_k}+{\mathrm{\nabla }}_{u}L{|}_{x_k,u_k}\cdot {\mathrm{\nabla }}_x\pi {|}_{x_k})\cdot {\mathrm{\nabla }}_{\theta }{x}_k}\\ & +{\displaystyle \sum _{k=0}^{N-1}{\mathrm{\nabla }}_{u}L{|}_{x_k,u_k}\cdot {\mathrm{\nabla }}_{\theta }\pi {|}_{x_k}}\\ & +{\mathrm{\nabla }}_x{L}_f{|}_{x_N}\cdot {\mathrm{\nabla }}_{\theta }{x}_N\end{array}\end{array}$$

線形時不変(LTI)システムでの検算

拘束条件なしの線形フィードバック問題を例にとって気休め程度に検算する。ただし、$x\in {\mathbb{R}}^n,u\in {\mathbb{R}}^m$とする。

$$\begin{array}{rl}L(x,u)& =\frac{1}{2}{x}^{⊺}Qx+\frac{1}{2}{u}^{⊺}Ru\\ L_f(x)& =\frac{1}{2}{x}^{⊺}Px\\ x_{k+1}& =A{x}_k+B{u}_k\\ u_k& =-K{x}_k\end{array}$$

このとき、

$$\begin{array}{rl}{x}_{k+1}& =(A-BK)x_k\\ & =(A-BK{)}^k{x}_0\\ u_k& =-K(A-BK{)}^{k-1}{x}_0\end{array}$$

を用いて

$$\begin{array}{rl}{\mathrm{\nabla }}_K{x}_i& =(i-1)(A-BK{)}^{i-2}B\sum _{j=0,d=0}^{n,m}\frac{\partial K}{\partial k_{j,d}}{x}_0\\ & =\sum _{l=0}^{k-1}(A-BK{)}^{k-l-2}B\sum _{j=0,d=0}^{n,m}\frac{\partial K}{\partial k_{j,d}}{x}_l\\ {\mathrm{\nabla }}_{K}J& ={\mathrm{\nabla }}_K(\sum _{i=0}^{N-1}\frac{1}{2}{x}_i^{⊺}(Q+K^{⊺}RK)x_i+\frac{1}{2}{x}_N^{⊺}P{x}_N)\\ & =\sum _{i=0}^{N-1}{\mathrm{\nabla }}_K{x}_i^{⊺}(Q+K^{⊺}RK)x_i+\sum _{i=0}^{N-1}\sum _{j=0,d=0}^{n,m}{x}_i^{⊺}\frac{\partial K}{\partial k_{j,d}}RK{x}_i+{\mathrm{\nabla }}_K{x}_N^{⊺}P{x}_N\end{array}$$

であるので確かに一致している。

さらに、$N\to \mathrm{\infty }$のとき、Global Convergence of Policy Gradient Methods for the Linear Quadratic Regulatorでも示されている通り、

$$P_K=Q+K^{⊺}RK+(A-BK{)}^{⊺}{P}_K(A-BK)$$

と置くと、

$$\begin{array}{rl}{\mathrm{\nabla }}_{K}J& =((R+B^{⊺}{P}_{K}B)K-B^{⊺}{P}_{K}A)\sum _{i=0}^{\mathrm{\infty }}{x}_i^{⊺}{x}_i\end{array}$$

と求まる。上記論文ではモデルフリーの自然勾配法との収束速度比較などを行っているので参照すると面白い。

参考

• The iterative Linear Quadratic Regulator algorithm | studywolf
• 現代制御理論
• Differential Dynamic Programming(DDP)/iterative LQR(iLQR)/Sequential LQR(SLQ)