微分动态规划(Differential Dynamic Programming, DDP)

变分法思路

由贝尔曼最优性原理得到：
$$\begin{array}{rl}V(X,t)& =\underset{u\in \Omega }{\min }\left\{\varphi \left[X\left(t_f\right),t_f\right]+{\int }_{t_0}^{t_f}L(x(t),u(t),t)dt\right\}\\ & =\underset{u\in \Omega }{\min }\left\{{\int }_{t_0}^{t_0+dt}L(x(\tau ),u(\tau ),\tau )d\tau +V(X+\Delta X,t+dt)\right\}\end{array}$$
离散化得：
$$\begin{array}{l}{x}_{k+1}=f\left(x_k,u_k\right)\\ \\ V_k={\min }_u\left[l\left(x_k，u_k\right)+V_{k+1}\left(x_{k+1}\right)\right]\end{array}$$
对$k$时刻的值函数$V(x,k)$做变分，令$Q(\delta x,\delta u)=V_k(x+\delta x)-V_k(x)$，对$Q$在标称轨迹$(x_k,u_k)$进行二阶泰勒展开：
$$\begin{array}{rl}Q(\delta x,\delta u)& =V(x+\delta x)-V(x)\\ & =l\left(x_k+\delta x_k,u_k+\delta u_k\right)+V_{k+1}\left(x_{k+1}+\delta x_{k+1}\right)-\left(l\left(x_k,u_k\right)+V_{k+1}\left(x_{k+1}\right)\right)\\ & \approx \delta x_k^T{l}_{x_k}+\delta u_k^T{l}_{u_k}+\frac{1}{2}\left(\delta x_k^T{l}_{x{x}_k}\delta x_k+2\delta x_k^T{l}_{x{u}_k}\delta u_k+\delta u_k^T{l}_{u{u}_k}\delta u_k\right)+\\ & \delta x_{k+1}^T{V}_{x_{k+1}}+\frac{1}{2}\delta x_{k+1}^T{V}_{x{x}_{k+1}}\delta x_{k+1}\end{array}$$
又有$x_{k+1}=f(x_k,u_k)$，二阶泰勒展开得到：
$$\delta x_{k+1}=\delta f\left(x_k,u_k\right)=f_{x_k}\delta x_k+f_{u_k}\delta u_k+\frac{1}{2}\left(\delta x_k^T{f}_{x{x}_k}\delta x+2\delta x_k^T{f}_{x{u}_k}\delta u_k+\delta u_k^T{f}_{u{u}_k}\delta u\right)$$
将$\delta x_{k+1}$带入$Q(\delta x,\delta u)$中，得到：
$$Q(\delta x,\delta u)\approx \frac{1}{2}{\left[\begin{array}{c}1\\ \delta x_k\\ \delta u_k\end{array}\right]}^T\left[\begin{array}{ccc}0& Q_{{\mathrm{x}}_k}^T& Q_{{\mathrm{u}}_k}^T\\ Q_{{\mathrm{x}}_k}& Q_{{\mathrm{x}\mathrm{x}}_k}& Q_{{\mathrm{x}\mathrm{u}}_k}^T\\ Q_{{\mathrm{u}}_k}& Q_{{\mathrm{u}\mathrm{x}}_k}& Q_{{\mathrm{u}\mathrm{u}}_k}\end{array}\right]\left[\begin{array}{c}1\\ \delta x_k\\ \delta u_k\end{array}\right]$$
其中：
$$\begin{array}{rl}& Q_x=l_{x_k}+f_{x_k}^T{V}_{x_{k+1}}\\ & Q_u=l_{u_k}+f_{u_k}^T{V}_{x_{k+1}}\\ & Q_{xx}=l_{x{x}_k}+f_{x_k}^T{V}_{x{x}_{k+1}}{f}_{x_k}+V_{x_{k+1}}{f}_{x_k{x}_k}\\ & Q_{uu}=l_{u{u}_k}+f_{u_k}^T{V}_{x{x}_{k+1}}{f}_{u_k}+V_{x_{k+1}}{f}_{u_k{u}_k}\\ & Q_{ux}=l_{u{x}_k}+f_{u_k}^T{V}_{x{x}_{k+1}}{f}_{x_k}+V_{x_{k+1}}{f}_{u_k{x}_k}\end{array}$$
将$Q(\delta x,\delta u)$视为$\delta u$的二次函数，令$\frac{\partial Q(\delta x,\delta u)}{\delta u}=0$，有：
$$\begin{array}{rl}\frac{\partial Q(\delta x,\delta u)}{\delta u}& =\frac{1}{2}(2Q_{{\mathbf{u}\mathbf{u}}_k}\delta u+Q_{{\mathbf{u}\mathbf{x}}_k}\delta x+\delta x^T{Q}_{{\mathbf{x}\mathbf{u}}_k}+Q_{{\mathbf{u}}_k}+Q_{{\mathbf{u}}_k})\\ & =Q_{{\mathbf{u}\mathbf{u}}_k}\delta u+Q_{{\mathbf{u}\mathbf{x}}_k}\delta x+Q_{{\mathbf{u}}_k}\\ & =0\end{array}$$
计算得：$\delta u^{\ast }=-Q_{{\mathbf{u}\mathbf{u}}_k}^{-1}(Q_{{\mathbf{u}}_k}+Q_{{\mathbf{u}\mathbf{x}}_k}\delta x)$
原理如下图：

令$k=-Q_{{\mathbf{u}\mathbf{u}}_k}^{-1}{Q}_{{\mathbf{u}}_k},K=-Q_{{\mathbf{u}\mathbf{u}}_k}^{-1}{Q}_{{\mathbf{u}\mathbf{x}}_k}$，则$\delta u^{\ast }=\underset{\delta u}{\mathrm{argmin}}Q(\delta x,\delta u)=k+K\delta x$，将其带入$Q(\delta x,\delta u)$，可以整理成如下形式：
$$Q(\delta x,\delta u)\approx \Delta V+V_{{\mathbf{x}}_k}^T\delta x+\frac{1}{2!}\delta x^T{V}_{{\mathbf{x}\mathbf{x}}_k}\delta x$$
对比可以得到：
$$\begin{array}{rl}& \Delta V=-\frac{1}{2}{Q}_{{\mathbf{u}}_k}^T{Q}_{{\mathbf{u}\mathbf{u}}_k}^{-1}{Q}_{{\mathbf{u}}_k}\\ & V_{{\mathbf{x}}_k}=Q_{{\mathbf{x}}_k}-Q_{{\mathbf{u}\mathbf{x}}_k}^T{Q}_{{\mathbf{u}\mathbf{u}}_k}^{-1}{Q}_{{\mathbf{u}}_k}\\ & V_{{\mathbf{x}\mathbf{x}}_k}=Q_{{\mathbf{x}\mathbf{x}}_k}-Q_{{\mathbf{x}\mathbf{u}}_k}^T{Q}_{{\mathbf{u}\mathbf{u}}_k}^{-1}{Q}_{{\mathbf{u}\mathbf{x}}_k}\end{array}$$
其中$Q_{{\mathbf{u}\mathbf{u}}_k}$是半正定对称阵。

DDP伪代码：

1. 由给定的控制序列${\bar{u}}_k$，正向得带计算标称轨迹。
   $$\begin{array}{rl}& {\bar{x}}_{k+1}=f({\bar{x}}_k,{\bar{u}}_k)\\ & l_{x_k},l_{u_k},l_{x{x}_k},l_{u{x}_k},l_{u{u}_k}\\ & f_{x_k},f_{u_k},f_{x_k{x}_k},f_{u_k{u}_k},f_{u_k{x}_k}\end{array}$$
2. 反向迭代：从$T$到$1$迭代
   • 计算$V_{x_{k+1}}$，$V_{x{x}_{k+1}}$
   • 计算$Q$函数
   • 计算$\delta u_k^{\ast }$，得到$k_k$，$K_k$
3. 正向迭代更新控制序列
   $$\begin{array}{rl}& x_1=\bar{x}(1)\\ & u_k={\bar{u}}_k+k_k+K_k(x_k-{\bar{x}}_k)\\ & x_{k+1}=f(x_k,u_k)\end{array}$$
4. 是否收敛，否就跳转1，是就结束