1. 写在最前面

最近一直在调研增强学习在机器人应用的文章, 看了一些文献, 其中看到15年IROS上的一篇论文[2], 大致浏览了一下, 在PR2上进行了实现, 较之前看到的不少论文在仿真上实现感觉要靠谱一些, 论文提供了视频DEMO, 下载下来看了一下, 直接感觉吊炸天了有没有. 随后就查了查他到底是属于哪个学校的哪个实验室. 最后聚焦到Sergey Levine身上. 进了Sergey Levine的个人主页, 看了他的一些成果, 瞬间吓尿了.

跟着Sergey看了他好几篇论文, 个人感觉, 每一篇论文都好长. 而且公式很多, 涉及的知识点更多, 这都不是重点. 重点是, 在Sergey的论文里面, 论文[2]中反复提到之前的工作 [3 - 5], [3]中提及[4 - 5]的工作, 一直这么往前延续, 当前的工作是建立在以前工作基础之上, 让人蛋疼无比.最后, 干脆心一横, 找出Sergey在13年发出的这篇论文, Guided Policy Search[1]进行阅读.

按照时间顺序, 依次看到几年的几篇论文, 花了很长的时间和精力. 作出了一些总结. 之后, 我会按照时间先后, 依次将我整理的一些资料发布出来, 我发现这方面中文资料几乎没有(或许是我没有找到-_-). 也方便后来者有一些中文的资料可以参考一下.

文档中并不是逐字按照原文翻译, 加入了我的一些理解, 以及省略了部分内容. 如果是想深入研究该算法的朋友, 最好还是以原文为准, 以我的资料为辅. 其中难免会有一些错误的地方, 有大神看到有理解错误的地方, 还请指出, 方便我对错误思想进行纠正, 也防止别人受了误导.

2. Guided Policy Search

直接策略搜索(Direct Policy Search)能够有效的应对高维系统, 但是对于具有数以百计的参数的策略一直是具有挑战性的, 需要数量非常大的样本来进行学习, 并且容易陷入局部最优.本论文提出引导策略搜索算法(Guided Policy Search), 将轨迹优化(trajectory optimization)应用到直接策略学习中, 避免了陷入局部最优. 该论文提出, 可以使用差分动态规划(Differential Dynamic Programming)生成合适引导样本(Guiding samples), 并使用了一个新颖的正则化项, 提出正则化重要样本策略优化(regularized importance sampled policy optimization), 用来合并这些引导样本到策略搜索中. 论文最后, 进行了一系列仿真实验, 验证算法效果.

2.1 Preliminaries

增强学习可以表述如下:

增强学习目的是找到一个最优的策略$\pi$, 在一个随机的环境中控制agent. 将连续的时间划分为离散的时间步$t$(time step $t$), 在每一个时间步, 可以观察(observe)得到agent 的一个状态(state) $x_t$, 依据该状态选择一个行为(action) $\pi(u_t \mid x_t )$, 下一个时间步的状态, 不单依据当前时间步的状态和所选择的行为, 还依赖于状态转移概率 $p(x_{t+1} | x_t, u_t)$, 最优策略$\pi$是能够获得从时间步1到时间步T最大累积回报的策略$\max_{\pi}\sum_{t=1}^Tr(x_t, u_t)$.

从时间步1到时间步$T$的可以得到$(x_t, u_t)$序列, 将其表述成一条轨迹$\zeta$, 使用$r(\zeta)$表示沿着轨迹$\zeta$的总回报值, 使用$\pi(\zeta)$标记在策略$\pi$下轨迹$\zeta$的概率.

策略梯度算法(Policy gradient method), 是通过直接优化用参数$\theta$描述的策略$\pi_\theta$, 使其对应的回报期望值最大, 从而得到最优策略所对应的参数$\theta^*$.在实际操作过程中, 每一次迭代沿着$E[\nabla J(\zeta)]$方向走一个步长, 直至收敛. 每一次梯度的估算使用下述公式.

$$E[\nabla J(\theta)] = E[r(\zeta)\nabla\log{\pi_\theta(\zeta)}] \approx \frac1{m}\sum_{i=1}^m r(\zeta_i) \nabla\log{\pi_\theta(\zeta_i)}$$

其中, $\nabla\log{\pi_\theta(\zeta_i)}$能够分解到$\sum_t{\nabla\log{\pi_\theta(u_t \mid x_t)}}$, 因为状态转移概率$p(x_{t+1} \mid x_t, u_t)$不依赖与优化变量$\theta$.

上诉过程, 每一次迭代需要使用到评估器对目标期望函数进行梯度评估. 一般方法是, 使用当前策略产生一系列的样本, 然后使用这些样本来估计函数的当前的梯度, 这样会产生巨大的工作量, 让学习过程变得非常耗时, 并且每一次迭代产生的样本使用之后将会被丢弃, 下一次迭代并无法使用. 这类的算法不允许使用off-policy样本, 同时对于每一梯度步的步长选择也需要谨慎才能确保达到收敛.

2.2 Importance Sampled Policy Search

重要采样(Importance Sampled)是一项技术, 使用另一个概率$q(x)$来估算关于概率$p(x)$的期望$E_p[f(x)]$.数学表达如下:

$$E_p[f(x)] = E_q[\frac{p(x)}{q(x)} f(x)] \approx \frac1{Z} \sum_{i=1}^{m} \frac{p(x_i)}{q(x_i)} f(x_i)$$

其中, 如果使用的概率分布$q(x)$是无偏的, 则$Z=m$.

通过Importance Sampled技术的使用, 允许我们使用off-policy样本在下述评估器中.

$$E[J(\theta)] \approx \frac1{Z(\theta)} \sum_{i=1}^{m} {\frac{\pi_\theta(\zeta_i)}{q(\zeta_i)}r(\zeta_i)} *** *** (1)$$

由于过去的回报值并不依赖于以后的回报值, 上述评估器的变量可以进一步分解如下:

$$E[J(\theta)] \approx \sum_{t=1}^{T}{\frac1{Z_t(\theta)} \sum_{i=1}^{m} {\frac{\pi_\theta(\zeta_{i, 1:t})}{q(\zeta_{i, 1:t})}r(x_t^i, u_t^i)}} *** *** (2)$$

其中, $\pi_\theta(\zeta_{i, 1:t})$标记$\zeta_i$的前$t$步的概率, $Z_t(\theta)$是正则化权重.

为了能够利用到更多可用的引导分布, 该论文follow之前的工作, $q(\zeta) = \frac1{n} \sum_j {q_j(\zeta)}$, 其中$q_j(\zeta)$是前一次迭代的策略或使用DDP构建的候选策略.

之前的工作中, 很多人都是直接优化Eq. (1), (2). 事实证明, 其效果往往不好.由此, 该论文引入了一个新颖的正则化项.改进目标函数如下:

$$\Phi(\theta) = \sum_{t=1}^{T} {\left[ \frac1{Z_t(\theta)}\sum_{i=1}^{m}\frac{\pi_\theta(\zeta_{i, 1:t})}{q(\zeta_{i, 1:t})} r(x_t^i, u_t^i) + \omega_r \log{Z_t(\theta)}\right]}$$
该函数的梯度可由下述公式计算(详细推算见[1]中附录):
$$\nabla\Phi(\theta) = \sum_{t=1}^{T} \sum_{i=1}^{m} \nabla\mu(x_t^i)\frac{u_t^i - \mu(x_t^i)}{\sigma^2} \sum_{t^{‘}=t}^{T} \frac1{Z_{t^{‘}}(\theta)} \frac{\pi_\theta(\zeta_{i, 1:t^{‘}})}{q(\zeta_{i, 1:t^{‘}})}\xi_{t^{‘}}^i$$

2.3 Guiding Samples

之前一些工作中, 也采取用了Importance Sample的方式来重复利用off-policy样本, 但当策略参数维度很大时, 如此的局部优化很难找到一个比较满意的解. 为了解决这个问题, 论文论证如何利用DDP来补充off-policy样本集, 引导策略向高回报区域搜索, 以获得一个更好的解.

一个有效的引导分布, 是能够覆盖高回报区域, 同时避免大$q(\zeta)$密度对应小的重要权重. 可以断言, 一个好的引导分布是高回报区域密度($\rho(\zeta) \propto \exp{r(\zeta)}$)的I-projection. $\rho$的I-projection定义为: $q^* = \arg \min_{q \in P} { D_{KL}(q \parallel \rho) }$, $D_{KL}(q\parallel\rho)$表示KL距离(KL-Divergence), 可用来衡量$q$与$\rho$两个分布的差异, 当差距越大时, KL距离越大, 当$q=\rho$时, KL距离为0. 定义为: $D_{KL}(q\parallel\rho) = \sum {q\log\frac{q}{\rho}} = E_q[-r(\zeta)] - \mathcal{H}(q)$. $\mathcal{H}$代表熵, 其中第一项强制$q$位于回报更高的区域, 第二项鼓励$q$具有更大的不确定性, 更broad distribution.

给定一条轨迹$(\bar{x}_1, \bar{u}_1), ..., (\bar{x}_T, \bar{u}_T)$, 并且, $\hat{x}_t=x_t-\bar{x}_t, \hat{u}_t=u_t-\bar{u}_t$, 动态函数和回报函数分别近似如下(典型的线性动态, 二阶代价形式的动态系统):

$$\begin{align*} \hat{x}_{t+1} &\approx f_{xt}\hat{x}_t + f_{ut}\hat{u}_t \ r(x_t, u_t) &\approx \hat{x}_t^Tr_{xt} + \hat{u}^Tr_{ut} + \frac1{2}\hat{x}_t^Tr_{xxt}\hat{x}_t + \frac1{2}\hat{u}_t^Tr_{uut}\hat{u}_t \ &+ \hat{u}_t^Tr_{uxt}\hat{x}_t + r(\bar{x}_t, \bar{u}_t) \end{align*}$$

依据DDP算法中推到, 可以求得Q函数:

$$\begin{align*} Q_{xt} &= r_{xt} + f_{xt}^TV_{xt+1} \Q_{ut} &= r_{ut} + f_{ut}^TV_{xt+1} \Q_{xxt} &= r_{xxt} + f_{xt}^TV_{xxt+1}f_{xt} \Q_{uut} &= r_{uut} + f_{ut}^TV_{xxt+1}f_{ut} \Q_{uxt} &= r_{uxt} + f_{ut}^TV_{xxt+1}f_{xt} \end{align*}$$

值函数:

$$\begin{align*} V_{xt} &= Q_{xt} - Q_{uxt}^TQ_{uut}^{-1}Q_u \V_{xxt} &= Q_{xxt} - Q_{uxt}^TQ_{uut}^{-1}Q_{ux} \\ k_t &= -Q_{uut}^{-1}Q_{ut} \K_t &= -Q_{uut}^{-1}Q_{uxt} \end{align*}$$

线性策略:

$$\begin{equation} g(x_t) = \bar{u}_t + k_t + K_t(x_t - \bar{x}_t) \end{equation}$$

从线性策略的表述中可以看到, 该控制器由两部分构成, 第一部分是均值部分, 第二部分是回馈部分, $K_t$是回馈增益.

重复计算策略可以得到一条新轨迹.这个算法将会收敛到局部最优.

现, 将回报函数进行改进:

$$\tilde{r}(x_t, u_t) = r(x_t, u_t) - D_{KL}\left( \pi_g(\cdot \mid x_t)\parallel p(\cdot \mid x_t) \right)$$

其中, $p$是一个”passive dynamics”分布, 如果$p$是一个均匀分布, 则:

$$E_{\pi_g}\left[\tilde{r}(\zeta)\right] = E_{\pi_g}\left[r(\zeta)\right] + \mathcal{H}(\pi_g)$$

最优策略[6]:

$$\begin{equation} \pi_g(u_t \mid x_t) = \exp{\left(Q_t(x_t, u_t) - V_t(x_t)\right)} *** ***(3) \end{equation}$$

其中, 值函数:$V_t(x_t)=\log{\int{\exp{\left(Q_t(x_t, u_t)\right)}}\mathrm{d}u_t}$.

在线性动态, 二次回报的环境下, Eq. (3) 是一个线性高斯, 均值是$g(x_t)$, 方差是$-Q_{uut}^{-1}$. 则我们可以从形式为 $\pi_g(u_t|x_t) = g(u_t; g(x_t), -Q_{uut}^{-1})$的随机策略中采样. 值得注意的是, 该策略的假设前提是线性动态, 且已知.

2.4 Adaptive Guiding Distribution

在前面的讨论中, 引导分布的构建只是获取高回报区域, 而没有思考当前策略. 我们可以简单的改进回报函数为$\bar{r}(x_t, u_t)=r(x_t, u_t)+\log{\pi_\theta(u_t \mid x_t)}$. 获取的结果分布是近似$\rho_\theta(\zeta) \propto \exp{(\bar{r}(\zeta))}$的I-projection.

2.5 Incorporating Guiding Distribution

该论文构建多个DDP结果, 用以补充Importance Sample策略搜索算法的中的样本. 同时, 这些结果也可以通过人为演示或离线的样本规划获得. DDP策略$\pi_g$是一个时变的控制器, 但仅对单一轨迹有效, 而最终的策略, 是需要具有更强健的鲁棒性. GPS算法可以看做是将一系列的轨迹集合(DDP生成或人为演示或离线规划生成)转换为控制器, 这个控制器应该能够被任意参数化, 如使用神经网络进行表达, 并反应出传感器或计算约束.

2.6 Guided Policy Search

Guided Policy Search
01: 生成DDP结果: $\pi_{g_1}, ..., \pi_{g_n}$
02: 从$q(\zeta) = \frac1{n}\sum_i {\pi_{g_i}(\zeta)}$中采样: $\zeta_i, ..., \zeta_m$
03: 初始化最优策略: $\theta^* \leftarrow \arg\max_\theta {\log \pi_{\theta^*}(\zeta_i)}$
04: 构建初始化样本集合$\mathcal{S} = \lbrace \pi_{g_1}, ..., \pi_{g_n}, \pi_{\theta^*} \rbrace$
05: for $k = 1 : K$
06: 　　选择当前样本集: $\mathcal{S}_k \subset S$
07: 　　$\theta_k \leftarrow \arg\max_\theta {\Phi_{S_k}(\theta)}$
08: 　　从$\pi_{\theta_k}$中获取样本增加到$\mathcal{S}_k$和$\mathcal{S}$中
09:　　生成自适应引导样本(可选)
10:　　在$\mathcal{S}_k$上评估$\pi_{\theta_k}$和$\pi_{\theta^*}$
11:　　if $\pi_{\theta_k} < \pi_{\theta^*}$
12:　　　　$\theta^* \leftarrow \theta_k$
13:　　　　$\omega_r\downarrow$
14:　　else
15:　　　　$\omega_r\uparrow$
16:　　　　从$\pi_{\theta^*}$中重采样(可选)
17:　　end if
18: end for
19: return $\pi_{\theta^*}$

整体算法伪代码整理如上表所示.

第1行, 通过DDP算法构建初始结果, 也可以通过人为演示初始化结果;

第2行, 初始化引导样本;

第3行, 使用这些引导样本初始化初始最优策略参数$\theta^*$.;

第4行, 样本集$\mathcal{S}$由引导样本和初始最优样本初始化;

第6行, 如果样本集$\mathcal{S}$过大, 则选择其子集$\mathcal{S}_k$, 实际操作中, 论文简单选择当前最优策略$\pi_{\theta^*}$下高权重的样本与构建的引导样本.

第7行, 在样本集上使用LBFGS算法优化目标函数$\Phi(\theta)$, 定义于Eq. (3), 获得优化参数$\theta_k$, 论文重复优化两次, 一次从当前最优参数出发, 另一次是从当前最大对数似然估计的参数开始.

第8行, 增加$\pi_{\theta_k}$到样本集;

第9行, 如果采用自适应引导分布, 则在此步完成自适应工作;

第10行, 使用Eq. (2) 来评估当前优化策略$\pi_{\theta_k}$和当前最优策略$\pi_{\theta^*}$的优劣;

第12, 13行, 若当前优化策略更好, 则更新最优策略$\theta^* \leftarrow \theta_k$, 减小$\omega_r$;

第15, 16行, 若当前优化策略更差, 则增大$\omega_r$. 可选的从$\pi_{\theta^*}$中重采样, 解决因样本不好而导致优化失败的问题..

高的正则化权重, 使得下一次优化时更接近于该样本, 以使评估更加精确, 在实际操作中, 一直保持$\omega_r \in [10^{-6}, 10^{-1}]$, 调整因子为10.

GPS算法提供了公开的实现版本, 支持C++和Python两种语言, 可运行于ROS中仿真器Gazebo, 并且提供多种方式仿真结果.

2.7 实验 + 讨论

这俩部分我就没有细看了, 也就没有形成文档. 因为作者是在仿真平台上做的, 并且这篇论文已经是几年前的成果了. 后面还有好几篇等着我, 所以这个阶段的讨论和实验就不那么重要了.

3. 个人总结

GPS算法[1], 由Levine 在13年提出, 使用DDP产生处于高回报区域的引导样本, 辅助策略搜索. 类似然比率评估器的重要采样技术用于合并这些引导样本到策略搜索中. GPS算法使用model-free的一些方法, 并结合model-based的DDP生成引导样本对学习过程进行引导. 可以将其视为将引导轨迹集合转换为一个控制器的过程.

待学习的策略, 是以参数来表达的某种一般性,灵活的模型, 比如神经网络. 直接策略搜索, 目标函数形式化为Eq. (1), 通过on-policy的方式采样来估计梯度, 使用梯度下降的方法, 对参数进行优化, 得到最优参数的策略. GPS改进目标函数为 Eq. (3), 通过Importance Sample技术, 再加入一个新颖的正则化项构成该目标函数. 允许算法使用off-policy样本, 大大缩减了策略搜索时间, 并且更加充分的利用之前策略采样得到的样本. 但由于这样学习到的策略很容易陷入局部最优, 不能学习到满意的策略. 论文中, 又结合轨迹优化中最优控制技术, 通过DDP算法构建覆盖高回报区域的引导样本. 构建引导样本的过程中, 如果考虑之前策略对引导样本的影响, 提出自适应的引导样本生成(在某些条件下具有不错的效果). 将生成的引导样本加入训练样本集中, 用于引导目标函数向高回报区域搜索最佳的策略.

该论文中, 系统状态$x_t$由机器人Configuration和环境构成, 如关节角度, 运动速度, 距目标物体的距离等. 显然, 系统状态是全状态, 需要依赖一些更为低级别的模块, 如距目标物体的距离, 需要已有的视觉模块进行评估. 系统输入或机器人行为$u_t$定义为关节力矩. 机器人自身的运动, 需要依赖低级别的PD控制等. 另一方面, 该算法中所使用的DDP方法产生引导样本, 假设前提是线性动态, 并且已知. 不能运用到实际系统中, 因为实际系统常常都是非线性动态模型, 并且一般都是未知的. 最终学习到的控制器, 仅能应对部分简单机器人任务, 对于非静态学习问题, 复杂组合任务等, 都极具挑战性. 所以算法均是在仿真平台上的简单模型上进行验证.

Appendix

下述部分, 是我在阅读这篇论文时, 查阅的一些其他资料, 几乎都来自与Wiki.

A.1 Linear-Quadratic Control

LQ (Linear Quadratic) 控制问题是经典的最优控制问题. 其动态函数是一阶线性函数, 代价函数是二阶函数.数学描述如下:
代价函数:

$$J = \frac{1}{2} x^T(t_f)S_{f}x(t_f) + \frac{1}{2}\int_{t_0}^{t_f}[x^T(t)Q(t)x(t) + u^T(t)R(t)u(t)]\mathrm{d}t$$
s.t.
$$\dot x(t) = A(t)x(t) + B(t)u(t) x(t_0) = x_0$$
LQR[7](Linear Quadratic Regular)能够提供LQ问题的一个控制方案.LQR是一个回馈控制器, 描述如下:

Finite-horizon, continuous-time LQR

对于连续时间系统, 定义$t \in [t_0, t_1]$, 则:

线性动态函数:

$$\dot x = Ax + Bu$$

二阶代价函数:

$$J = \frac1{2} x^T(t_1)F(t_1)x(t_1) + \int_{t_0}^{t_1} (x^TQx + u^TRu + 2x^TNu) \mathrm{d}t$$

由回馈控制法则可知, 欲使代价函数最小, 则:

$$u = -Kx$$

其中, $K = R^{-1}(B^TP(t) + N^T)$, $P$由 $A^TP(t) + P(t)A - (P(t)B + N)R^{-1}(B^TP(t) + N^T) + Q = -\dot P(t)$, $P(t_1) = F(t_1)$计算得到.

Finite-horizon, discrete-time LQR

对于离散时间系统,

线性动态函数:

$$x_{k+1} = Ax_k + Bu_k$$

二阶代价函数:

$$J = \sum_{k=0}^{N} { \left( x_k^TQx_k + u_k^TRu_k + 2x_k^TNu_k \right)}$$

由回馈控制法则可知, 欲使代价函数最小, 则:

$$u_k = -K_kx_k$$

其中, $K_k = (R + B^TP_kB)^{-1}(B^TP_kA + N^T)$, $P_k$可反向迭代计算, $P_{k-1} = A^TP_kA - (A^TP_kB + N)(R + B^TP_kB)^{-1}(B^TP_kA + N^T) + Q$计算得到, 终止条件$P_N = Q$.

A.2 Linear-Quadratic-Gaussian Control

在控制理论中, 线性-二次高斯控制问题(Linear-quadratic-gaussian[8])是最优控制中LQ问题的一个基本问题.LQG关注含有高斯白噪声的不确定性系统, 状态信息不完整, 并且满足二阶代价函数.

LQG 控制器是对Kalman Filter和Linear quadratic regular(LQR)的简单合并. LQG控制可以应用到线性时变系统和线性时不变系统.LQG问题有如下数学描述:

Continuous time

动态函数:

$$\begin{align*} \dot{x}(t) &= A(t)x(t) + B(t)u(t) + v(t) \ y(t) &= C(t)x(t) + w(t) \end{align*}$$

其中, $x(t)$是系统状态控制量, $u(t)$是系统输入控制量. $y(t)$是系统可测量的输出信号量. $v(t)$是系统噪声, 服从高斯白噪声. $w(t)$是系统测量噪声, 同样服从高斯白噪声.

代价函数:

$$J = E \left[ x^T(T)Fx(T) + \int_{0}^{T} \left( x^T(t)Q(t)x(t) + u^T(t)R(t)u(t) \right) \mathrm{d}t \right]$$
其中, $F \geq 0, Q(t) \geq 0, R(t) > 0$.

使用LQG控制器解决LQG问题, 给定等式如下:

$$\dot{\hat{x(t)}} = A(t)\hat{x}(t) + B(t)u(t) + K(t) \left( y(t) - C(t)\hat{x}(t) \right), \hat{x}(0) = E(x(0))$$
$$u(t) = -L(t)\hat{x}(t)$$

其中, 在每一个时刻$t$, 滤波器使用过去的评估值和当前输入对系统状态$x(t)$产生一个新的状态评估$\hat{x}(t)$.

$K(t)$是对应于Kalman Filter的Kalman增益(Kalman gain).计算公式如下:

$$K(t) = P(t)C^T(t)W^{-1}(t).$$
$$\dot{P}(t) = A(t)P(t) + P(t)A^T(t) - P(t)C^T(t)W^{-1}(t)C(t)P(t) + V(t)$$
$$P(0) = E(x(0)x^T(0))$$

其中, $W(t)$, $V(t)$分别是$w(t)$, $v(t)$的密度矩阵.

$L(t)$是回馈增益矩阵, 计算公式如下:

$$L(t) = R^{-1}(t)B^T(t)S(t)$$
$$-\dot{S}(t) = A^T(t)S(t) + S(t)A(t) - S(t)B(t)R^{-1}(t)B^T(t)S(t) + Q(t)$$
$$S(T) = F$$

观察上诉两个Riccati微分方程(Riccati differential equation), 第一个微分方程是前向的(running forward in time), 第二个微分方程式后向的(running backward in time). 这种相似性被称为对偶性(duality).

第一个Riccati微分方程解决线性-二次评估问题(linear-quadratic estimation problem), 第二个Riccati微分方程解决线性-二次规整问题(linear-quadratic regular problem). 两个对偶, 合并共同解决LQG问题.因此LQG可以独立的分解为LQE和LQR问题.

Discrete time
离散时间LQG问题与连续时间LQG问题类似, 数学描述如下:

动态函数:

$$\begin{align*} x_{k+1} &= A_ix_i + B_iu_i + v_i \ y_i &= C_ix_i + w_i \end{align*}$$

代价函数:

$$J = E \left( x_N^TFx_N + \sum_{i=0}^{N-1}x_i^TQ_ix_i + u_i^TR_iu_i \right)$$
$$F \geq 0, Q_i \geq 0, R_i > 0$$

离散时间LQG控制器:

$$\hat{x}_{i+1} = A_i\hat{x}_i + B_iu_i + K_{i+1}(y_{i+1} - C_{i+1}\hat{x}_i ), \hat{x}_0=E(x_0)$$
$$u_i = -L_i\hat{x}_i$$

其中, kalman增益项$K_i$的计算公式如下:

$$\begin{align*} K_i &= A_iP_iC_i^T(C_iP_iC_i^T + W_i^{-1}) \P_{i+1} &= A_i(P_i - P_iC_i^T(C_iP_iC_i^T + W_i)^{-1}C_iP_i)A_i^T + V_i \P_0 &= E(x_0 - \hat{x}_0)(x_0 - \hat{x}_0)^T \end{align*}$$

其中, 反馈增益矩阵计算公式如下:

$$L_i = (B_i^TS_{i+1}B_i + R_i)^{-1}B_i^TS_{i+1}A_i$$
$$S_i = A_i^T(S_{i+1} - S_{i+1}B_i(B_i^TS_{i+1}B_i + R_i)^{-1}B_i^TS_{i+1})A_i + Q_i, S_N = F$$

A.3 Differential Dynamic Programming

差分动态规划(Differential Dynamic Programming[9]), 是轨迹优化(trajectory optimization)类中最优控制(optimal control)算法.

Finite-horizon discrete-time problems

动态函数:

$$x_{i+1} = f(x_i, u_i)$$

描述时刻$i$的状态$x_i$在给定控制量$u_i$向时刻$t+1$演化的过程.

代价函数:

$$J_0(x, U) = \sum_{i=0}^{N-1} { \ell(x_i, u_i) } + \ell_f(x_N)$$

轨迹最优, 就是给定初始状态$x_0$, 找到一个控制序列$U^*$使得$J_0$最小, 而非针对于所有可能的初始条件. $U^*$即为该最优控制问题的解.数学表达如下, $U^* = \arg{\min_{U}{J_0(x, U)}}$.

Dynamic programming

设$U_i= \lbrace u_i, u_{i+1}, ..., u_{N-1} \rbrace$是部分控制量序列以及cost-to-go $J_i$:

$$J_i(x, U_i) = \sum_{j=i}^{N-1} { \ell(x_j, u_j) } + \ell_f(x_N)$$

则, 在时刻$i$的最优cost-to-go或值函数定义如下:

$$V(x, i) = \min_{U_i} { J_i(x, U_i) }$$

令$V(x, N) = \ell_f(x_N)$, 则可动态规划如下:

$$V(x, i) = \min_{u} { \left[ \ell(x, u) + V \left( f(x, u), i + 1 \right) \right] }$$

显然上诉规划式是一个Bellman等式.

Differential dynamic programming

首先, 我们定义$Q$函数:

$$Q(\delta x, \delta u) = \ell(x + \delta x, u + \delta u) + V\left( f(x + \delta x, u + \delta u), i+1 \right) - \ell(x, u) - V\left( f(x, u), i+1 \right)$$

扩展到二阶如下:

$$\approx \frac1{2} \left[\begin{matrix} 1 \\ \delta x \\ \delta u \end{matrix}\right]^T \left[\begin{matrix} 0 & Q_x^T &Q_u^T \\ Q_x & Q_{xx} & Q_{xu} \\ Q_u & Q_{ux} &Q_{uu} \end{matrix}\right] \left[\begin{matrix} 1 \\ \delta x \\ \delta u \end{matrix}\right]$$

上述公式中下标代表偏导.各个偏导可以使用下述公式计算:

$$\begin{align*} Q_x &= \ell_x + f_x^TV_x^{‘} \ Q_u &= \ell_u + f_u^TV_x^{‘} \ Q_{xx} &= \ell_{xx} + f_x^TV_{xx}^{‘}f_x + V_x^{‘}f_{xx} \ Q_{uu} &= \ell_{uu} + f_u^TV_{xx}^{‘}f_u + V_x^{‘}f_{uu} \ Q_{ux} &= \ell_{ux} + f_u^TV_{xx}^{‘}f_x + V_x^{‘}f_{ux} \end{align*}$$

对上式的二阶展开求最小可得: $\delta u^* = \arg \min_{\delta u} { Q(\delta u, \delta x) } = -Q_{uu}^{-1}(Q_u + Q_{ux}\delta x )$,则可得开环项(open-loop term) $k=-Q_{uu}^{-1}Q_u$, 回馈增益项(feedback gain term)$K=-Q_{uu}^{-1}Q_{ux}$. 带回二次展开式, 可得:

$$\begin{align*} \Delta V(i) &= \frac1{2} Q_uQ_{uu}^{-1}Q_u \ V_x(i) &= Q_x - Q_uQ_{uu}^{-1}Q_u \ V_xx(i) &= Q_{xx} - Q_uQ_{uu}^{-1}Q_u \end{align*}$$

使用上述公式, 递归的从$i=N-1$到$i=1$计算$V(i)$的局部二次模型, 以及控制修正量$\lbrace k(i), K{i} \rbrace$, 执行一次反向pass(the backward pass), 初始值$V(x, N) = \ell_f(x_N)$. 反向pass完成后, 再从头运行一次正向pass(the forward pass)即可计算得到一条新轨迹.

$$\begin{align*} \hat{x}(1) &= x(1) \ \hat{x}(i) &= u(i) + k(i) + K(i)(\hat{x}(i)-x(i)) \ \hat{x}(i + 1) &= f(\hat{x}(i), \hat{u}(i)) \end{align*}$$

如此, 前向pass, 反向pass直至收敛.

参考文献

[1] Sergey Levine, Vladlen Koltun, Guided Policy Search, ICML 2013

[2] Weiqiao Han, Sergey Levine, Pieter Abbeel, Learning Compound Multi-Step Controllers under Unknown Dynamics, IROS 2015

[3] Sergey Levine, Chelsea Finn, Trevor Darrell, Pieter Abbeel, End-to-End Training of Deep Visuomotor Policies, JMLR 17, 2016

[4] Sergey Levine, Pieter Abbeel. Learning Neural Network Policies with Guided Policy Search under Unknown Dynamics, NIPS, 2014

[5] Sergey Levine, Nolan Wagener, Pieter Abbeel, Learning Contact-Rich Manipulation Skills with Guided Policy Search, ICRA 2015

[6] Ziebart, Modeling purposeful adaptive behavior with the principle of maximum causal entropy, Ph.D thesis, Carnegie Mellon University, 2010

[7] Wikipedia contributors. “Linear-quadratic regulator.” Wikipedia, The Free Encyclopedia. Wikipedia, The Free Encyclopedia, 15 May. 2016. Web. 23 May. 2016

[8] Wikipedia contributors. “Linear-quadratic-Gaussian control.” Wikipedia, The Free Encyclopedia. Wikipedia, The Free Encyclopedia, 18 May. 2016. Web. 23 May. 2016

[9] Wikipedia contributors. “Differential dynamic programming.” Wikipedia, The Free Encyclopedia. Wikipedia, The Free Encyclopedia, 28 Jan. 2016. Web. 23 May. 2016