Iterative Linear Quadratic Gaussian（iLQG）

论文简述

原论文：http://maeresearch.ucsd.edu/groups/skelton/publications/weiwei_ilqg_CDC43.pdf
本文工作把一个非线性最优控制问题，在每次迭代中都在局部归化为控制理论里面研究很成熟的Linear Quadratic Gaussian（LQG）问题，然后迭代地去求解更好的控制序列，直到收敛。

与强化学习的区别在于强化学习中不直接知道系统的dynamics，而最优控制的问题可以知道系统的dynamics。控制理论是强化理论的重要基础，相比于更为玄学的强化学习，控制理论的理论分析更细致，能够更好的帮助我们理解强化学习。

公式理解

任意找一个控制序列 {u‾(k)}\{\overline{u}(k)\}{u(k)}，然后按照这个控制做一次推导，得到轨迹 {x‾(k)}\{\overline{x}(k)\}{x(k)} 。在这个轨迹附近，将系统的动力学特性线性化、将损失函数二次化，并考虑如何在 {u‾(k)}\{\overline{u}(k)\}{u(k)}和 {x‾(k)}\{\overline{x}(k)\}{x(k)}附近找到扰动δu=u−u‾\delta u=u-\overline{u}δu=u−u 和δx=x−x‾\delta x=x-\overline{x}δx=x−x使得新的控制比之前的更好。迭代地做更新直到收敛即得到最优控制序列。

算法的每一次迭代都在轨迹 {x‾(k),u‾(k)}\{\overline{x}(k), \overline{u}(k)\}{x(k),u(k)}附近先找到控制序列 u=u‾+δuu=\overline{u}+\delta uu=u+δu ，然后根据动力学规律找到新的轨迹 x=x‾+δxx=\overline{x}+\delta xx=x+δx，反复迭代直到收敛

具体步骤：
首先可以将系统的动力学特性线性化并且将损失函数二次化，有
δxk+1=Akδxk+Bkδuk+Ck(δuk)ξkCk(δuk)≜[c1,k+C1,kδuk⋯cp,k+Cp,kδuk]cost⁡k=qk+δxk⊤qk+12δxk⊤Qkδxk+δuk⊤rk+12δuk⊤Rkδuk\begin{aligned} \delta \mathbf{x}_{k+1}=& A_{k} \delta \mathbf{x}_{k}+B_{k} \delta \mathbf{u}_{k}+\mathcal{C}_{k}\left(\delta \mathbf{u}_{k}\right) \boldsymbol{\xi}_{k} \\ \mathcal{C}_{k}\left(\delta \mathbf{u}_{k}\right) \triangleq &\left[\mathbf{c}_{1, k}+C_{1, k} \delta \mathbf{u}_{k} \cdots \mathbf{c}_{p, k}+C_{p, k} \delta \mathbf{u}_{k}\right] \\ \operatorname{cost}_{k}=& q_{k}+\delta \mathbf{x}_{k}^{\top} \mathbf{q}_{k}+\frac{1}{2} \delta \mathbf{x}_{k}^{\top} Q_{k} \delta \mathbf{x}_{k} \\ &+\delta \mathbf{u}_{k}^{\top} \mathbf{r}_{k}+\frac{1}{2} \delta \mathbf{u}_{k}^{\top} R_{k} \delta \mathbf{u}_{k} \end{aligned}δxk+1=Ck(δuk)≜costk=Akδxk+Bkδuk+Ck(δuk)ξk[c1,k+C1,kδuk⋯cp,k+Cp,kδuk]qk+δxk⊤qk+21δxk⊤Qkδxk+δuk⊤rk+21δuk⊤Rkδuk

其中 AkA_{k}Ak 和 BkB_{k}Bk 可以由已知的动力学规律在轨迹 {x‾(k)}\{\overline{x}(k)\}{x(k)} 附近求导得到，各种Q和R都可以对于损失函数在现有轨迹附近求导得到， [公式] 是由上述列向量拼成的，反映的是控制引起的噪声。即，认为各种A、B、C、Q、R已知

假设有state value function写成如下形式

vk(δx)=sk+δx⊤sk+12δx⊤Skδxv_{k}(\delta \mathbf{x})=s_{k}+\delta \mathbf{x}^{\top} \mathbf{s}_{k}+\frac{1}{2} \delta \mathbf{x}^{\top} S_{k} \delta \mathbf{x}vk(δx)=sk+δx⊤sk+21δx⊤Skδx

应用Bellman方程

vk(δx)=v_{k}(\delta \mathbf{x})=vk(δx)= immediate cost +E[vk+1(next state )]+\mathrm{E}\left[v_{k+1}(\text { next state })\right]+E[vk+1( next state )]

可以求得到

vk(δx)=qk+sk+1+12∑ici⊤Sk+1ci+δx⊤(qk+Ak⊤sk+1)+12δx⊤(Qk+Ak⊤Sk+1Ak)δx+π⊤(g+Gδx)+12π⊤Hπ\begin{aligned} v_{k}(\delta \mathbf{x})=& q_{k}+s_{k+1}+\frac{1}{2} \sum_{i} \mathbf{c}_{i}^{\top} S_{k+1} \mathbf{c}_{i} \\ &+\delta \mathbf{x}^{\top}\left(\mathbf{q}_{k}+A_{k}^{\top} \mathbf{s}_{k+1}\right) \\ &+\frac{1}{2} \delta \mathbf{x}^{\top}\left(Q_{k}+A_{k}^{\top} S_{k+1} A_{k}\right) \delta \mathbf{x} \\ &+\pi^{\top}(\mathbf{g}+G \delta \mathbf{x})+\frac{1}{2} \pi^{\top} H \pi \end{aligned}vk(δx)=qk+sk+1+21i∑ci⊤Sk+1ci+δx⊤(qk+Ak⊤sk+1)+21δx⊤(Qk+Ak⊤Sk+1Ak)δx+π⊤(g+Gδx)+21π⊤Hπ

假设具有形如δu=πk(δx)=Ik+Lkδx\delta u=\pi_{k}(\delta x)=I_{k}+L_{k} \delta xδu=πk(δx)=Ik+Lkδx的闭环控制形式，可以得到最优控制（具体讨论见附注）

1k=−H−1g,Lk=−H−1G\mathbf{1}_{k}=-H^{-1} \mathbf{g}, \quad L_{k}=-H^{-1} G1k=−H−1g,Lk=−H−1G

并且可以求得到state value function
vk(δx)=qk+sk+1+12∑ici⊤Sk+1ci+δx⊤(qk+Ak⊤sk+1)+12δx⊤(Qk+Ak⊤Sk+1Ak)δx+1k⊤g+12Ik⊤Hlk+δx⊤(G⊤1k+Lk⊤g+Lk⊤Hlk)+12δx⊤(Lk⊤HLk+Lk⊤G+G⊤Lk)\begin{aligned} v_{k}(\delta \mathbf{x})=& q_{k}+s_{k+1}+\frac{1}{2} \sum_{i} \mathbf{c}_{i}^{\top} S_{k+1} \mathbf{c}_{i} \\ &+\delta \mathbf{x}^{\top}\left(\mathbf{q}_{k}+A_{k}^{\top} \mathbf{s}_{k+1}\right) \\ &+\frac{1}{2} \delta \mathbf{x}^{\top}\left(Q_{k}+A_{k}^{\top} S_{k+1} A_{k}\right) \delta \mathbf{x} \\ &+\mathbf{1}_{k}^{\top} \mathbf{g}+\frac{1}{2} \mathbf{I}_{k}^{\top} H \mathbf{l}_{k}+\delta \mathbf{x}^{\top}\left(G^{\top} \mathbf{1}_{k}+L_{k}^{\top} \mathbf{g}+L_{k}^{\top} H \mathbf{l}_{k}\right) \\ &+\frac{1}{2} \delta \mathbf{x}^{\top}\left(L_{k}^{\top} H L_{k}+L_{k}^{\top} G+G^{\top} L_{k}\right) \end{aligned}vk(δx)=qk+sk+1+21i∑ci⊤Sk+1ci+δx⊤(qk+Ak⊤sk+1)+21δx⊤(Qk+Ak⊤Sk+1Ak)δx+1k⊤g+21Ik⊤Hlk+δx⊤(G⊤1k+Lk⊤g+Lk⊤Hlk)+21δx⊤(Lk⊤HLk+Lk⊤G+G⊤Lk)

进行比对可以得到

其中

算法流程

找到一条初始的轨迹　 {x‾(k),u‾(k)}\{\overline{x}(k), \overline{u}(k)\}{x(k),u(k)} ，然后反复进行如下迭代：

做动态规划找到更好的控制序列：按照 k=K,K−1,⋯,1k=K, K-1, \cdots, 1k=K,K−1,⋯,1的顺序迭代，计算新的控制规律参数δuk=Ik+Lkδxk\delta u_{k}=I_{k}+L_{k} \delta x_{k}δuk=Ik+Lkδxk和 vk(δx)=sk+skTδx+12δxTSkδxv_{k}(\delta x)=s_{k}+s_{k}^{T} \delta x+\frac{1}{2} \delta x^{T} S_{k} \delta xvk(δx)=sk+skTδx+21δxTSkδx ；
更新新的轨迹：按照 k=1,2,⋯,Kk=1, 2, \cdots, Kk=1,2,⋯,K的顺序迭代，根据上步计算到的控制规律和 δxk+1=Akδxk+Bkδuk\delta x_{k+1}=A_{k} \delta x_{k}+B_{k} \delta u_{k}δxk+1=Akδxk+Bkδuk 计算新的轨迹；