最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（下）

1. Introduction of Optimal Control (OC)

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.1 System dynamics

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.2 Optimal control problem, cost function and constraints formulation

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.3 Control policy

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.4 Bellman’s Principle of Optimality (BPO) and Dynamic Programming (DP)

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.5 Hamiltonian-Jacobian-Bellman

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

1.6 Pontryagin’s Maximum Principle (PMP)

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（上）.

2. Linear Quadratic Regulator (LQR)

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（中）.

2.1 LQR with indirect shooting based on PMP

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（中）.

2.2 LQR as Quadratic Programming

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（中）.

2.3 LQR as Dynamic Programming

Please refer to this 最优控制 LQR 与 Differential Dynamic Programming(DDP) 详细公式推导（中）.

3. Differential Dynamic Programming (DDP)

In the last section we discussed the Linear Quadratic Regulator (LQR) problem, which is a special case of the optimal control problem (OCP) where the system dynamics are linear and the cost function is quadratic. How about non-linearities, either in dynamics or cost functions? How can we tackle the value function? In this section, we will discuss a more general framework for solving OCPs, namely Differential Dynamic Programming (DDP) for discrete process. DDP is a trajectory optimization method that is based on the principle of optimality. It is a two-step method that first computes a locally optimal trajectory and then iteratively refines it. The method is iterative and can be used to solve non-linear OCPs with non-linear dynamics and non-linear cost functions.

Recall that we have an OCP defined in the discrete-time domain as follows:
$$\begin{array}{rlrl}& \underset{\mathbf{u}}{\min }& & J\left({\mathbf{x}}_0,{\mathbf{u}}_{0,...,N-1}\right)\\ & \mathrm{subject}\mathrm{to}& & {\mathbf{x}}_{k+1}=\mathbf{f}({\mathbf{x}}_k,{\mathbf{u}}_k)\\ & & & \mathbf{x}\in \mathcal{X}\\ & & & \mathbf{u}\in \mathcal{U}\end{array}\text{\hspace{1em}(51)}$$
where the cost function is defined as:
$$\begin{array}{rl}& J\left({\mathbf{x}}_0,{\mathbf{u}}_{0,...,N-1}\right)=\sum _{k=0}^{N-1}\underset{\text{cost-to-go}}{\underbrace{\ell ({\mathbf{x}}_k,{\mathbf{u}}_k)}}+\underset{\text{terminal cost}}{\underbrace{{\ell }_f({\mathbf{x}}_N)}}\end{array}\text{\hspace{1em}(52)}$$

As in DP, we need to define the value function and find the recursion for it. To begin with, the value function is defined as:

$$\begin{array}{rl}V({\mathbf{x}}_k)& =\underset{{\mathbf{u}}_{k,\dots ,N-1}}{\min }J\left({\mathbf{x}}_k,{\mathbf{u}}_{k,\dots ,N-1}\right)\\ & =\underset{{\mathbf{u}}_{k,\dots ,N-1}}{\min }\left[\sum _{i=k}^{N-1}\ell ({\mathbf{x}}_i,{\mathbf{u}}_i)+{\ell }_f({\mathbf{x}}_N)\right]\\ & =\underset{{\mathbf{u}}_k}{\min }\ell ({\mathbf{x}}_k,{\mathbf{u}}_k)+\underset{{\mathbf{u}}_{k+1,\dots ,N-1}}{\min }\left[\sum _{i=k+1}^{N-1}\ell ({\mathbf{x}}_i,{\mathbf{u}}_i)+{\ell }_f({\mathbf{x}}_N)\right]\\ & =\underset{{\mathbf{u}}_k}{\min }\ell ({\mathbf{x}}_k,{\mathbf{u}}_k)+V({\mathbf{x}}_{k+1})\end{array}\text{\hspace{1em}(53)}$$

Here we don’t stick with the assumption on linear dynamics and quadratic cost function any more, so we can’t use the Riccati equation to solve the value function. Instead, we can use differential-based approximation upon both dynamics and cost function.

3.1 Local Approximation

Start with the dynamics, we can use Taylor expansion to approximate the dynamics around the current state and control input, say $(\bar{\mathbf{x}},\bar{\mathbf{u}})$, then we can approximate the dynamics around $(\bar{\mathbf{x}},\bar{\mathbf{u}})$ as:

$$\begin{array}{rl}\frac{d\mathbf{x}}{dt}=\mathbf{f}(\mathbf{x},\mathbf{u})=\mathbf{f}(\bar{\mathbf{x}},\bar{\mathbf{u}})& +\underset{\mathrm{linear}\mathrm{term}}{\underbrace{{\nabla }_{\mathbf{x}}\mathbf{f}(\bar{\mathbf{x}},\bar{\mathbf{u}})(\mathbf{x}-\bar{\mathbf{x}})}}+\underset{\mathrm{linear}\mathrm{term}}{\underbrace{{\nabla }_{\mathbf{x}}\mathbf{f}(\bar{\mathbf{x}},\bar{\mathbf{u}})(\mathbf{u}-\bar{\mathbf{u}})}}\\ & +\underset{\text{quadratic term}}{\underbrace{\frac{1}{2}{\nabla }_{\mathbf{x}\mathbf{x}}^{2}f(\bar{\mathbf{x}},\bar{\mathbf{u}})(\mathbf{x}-\bar{\mathbf{x}}{)}^2}}+\underset{\text{quadratic term}}{\underbrace{\frac{1}{2}{\nabla }_{\mathbf{u}\mathbf{u}}^{2}f(\bar{\mathbf{x}},\bar{\mathbf{u}})(\mathbf{u}-\bar{\mathbf{u}}{)}^2}}\\ & +\underset{\text{cross quaduatic lerm}}{\underbrace{{\mathbf{f}}_{\mathbf{x}\mathbf{u}}(\bar{\mathbf{x}},\bar{\mathbf{u}})(\mathbf{x}-\bar{\mathbf{x}})(\mathbf{u}-\bar{\mathbf{u}})}}\\ & +\mathcal{O}\left(\Vert \mathbf{x}-\bar{\mathbf{x}}{\Vert }^3\right)+\mathcal{O}\left(\Vert \mathbf{u}-\bar{\mathbf{u}}{\Vert }^3\right)\end{array}\text{\hspace{1em}(54)}$$

or we can put it into a more compact matrix form with notation $\delta \mathbf{x}=\mathbf{x}-\bar{\mathbf{x}}$ and $\delta \mathbf{u}=\mathbf{u}-\bar{\mathbf{u}}$:

$$\begin{array}{rl}\frac{d\delta \mathbf{x}}{dt}& =\frac{d(\mathbf{x}-\bar{\mathbf{x}})}{dt}=\mathbf{f}(\mathbf{x},\mathbf{u})-\mathbf{f}(\bar{\mathbf{x}},\bar{\mathbf{u}})\\ & \approx {\left[\begin{array}{c}{\nabla }_{\mathbf{x}}\mathbf{f}(\bar{\mathbf{x}},\bar{\mathbf{u}})\\ {\nabla }_{\mathbf{x}}\mathbf{f}(\bar{\mathbf{x}},\bar{\mathbf{u}})\end{array}\right]}^T\left[\begin{array}{c}\delta x\\ \delta u\end{array}\right]+\frac{1}{2}{\left[\begin{array}{c}\delta x\\ \delta u\end{array}\right]}^T\left[\begin{array}{cc}{\nabla }_{\mathbf{x}\mathbf{x}}^{2}f(\bar{\mathbf{x}},\bar{\mathbf{u}})& {\nabla }_{\mathbf{x}\mathbf{u}}^{2}f(\bar{\mathbf{x}},\bar{\mathbf{u}})\\ {\nabla }_{\mathbf{u}\mathbf{x}}^{2}f(\bar{\mathbf{x}},\bar{\mathbf{u}})& {\nabla }_{\mathbf{u}\mathbf{u}}^{2}f(\bar{\mathbf{x}},\bar{\mathbf{u}})\end{array}\right]\left[\begin{array}{c}\delta x\\ \delta u\end{array}\right]\end{array}\text{\hspace{1em}(55)}$$

Then similarly for the value function, we have
$$\begin{array}{rl}V({\mathbf{x}}_k,{\mathbf{u}}_k)& =\underset{{\mathbf{u}}_k}{\min }\ell ({\mathbf{x}}_k,{\mathbf{u}}_k)+V({\mathbf{x}}_{k+1})\\ & =\underset{\delta {\mathbf{u}}_k}{\min }\ell ({\bar{\mathbf{x}}}_k+\delta {\mathbf{x}}_k,{\bar{\mathbf{u}}}_k+\delta {\mathbf{u}}_k)+V({\bar{\mathbf{x}}}_{k+1}+\delta {\mathbf{x}}_{k+1})\end{array}\text{\hspace{1em}(56)}$$
as in DP backward iteration, we need to express the value function at next time step $k+1$ as a function of the current state ${\mathbf{x}}_k$ and control ${\mathbf{u}}_k$ in order to solve for ${\mathbf{u}}_k^{\ast }.$ Hence we can use the Taylor expansior again:

$$\begin{array}{rl}V({\bar{\mathbf{x}}}_{k+1}+\delta {\mathbf{x}}_{k+1})& =V({\bar{\mathbf{x}}}_{k+1})+{\nabla }_{\mathbf{x}}V({\bar{\mathbf{x}}}_{k+1}{)}^T\delta {\mathbf{x}}_{k+1}+\frac{1}{2}\delta {\mathbf{x}}_{k+1}^T{\nabla }_{\mathbf{x}\mathbf{x}}^{2}V({\bar{\mathbf{x}}}_{k+1})\delta {\mathbf{x}}_{k+1}\\ & +\mathcal{O}\left(\Vert \delta {\mathbf{x}}_{k+1}{\Vert }^3\right)\end{array}\text{\hspace{1em}(57)}$$

to go further, we need to express $\delta {\mathbf{x}}_{k+1}$ as a function of $\delta {\mathbf{x}}_k$ and $\delta {\mathbf{u}}_k$, which can be done by the dynamics in Eq.55 as
$$\begin{array}{rl}\delta {\mathbf{x}}_{k+1}& \approx \delta {\mathbf{x}}_k+{\left[\begin{array}{c}{\nabla }_{\mathbf{x}}\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\\ {\nabla }_{\mathbf{x}}\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\end{array}\right]}^T\left[\begin{array}{c}\delta {\mathbf{x}}_k\\ \delta {\mathbf{u}}_k\end{array}\right]\\ & +\frac{1}{2}{\left[\begin{array}{c}\delta {\mathbf{x}}_k\\ \delta {\mathbf{u}}_k\end{array}\right]}^T\left[\begin{array}{cc}{\nabla }_{\mathbf{x}\mathbf{x}}^2\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)& {\nabla }_{\mathbf{x}\mathbf{u}}^2\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\\ {\nabla }_{\mathbf{u}\mathbf{x}}^2\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)& {\nabla }_{\mathbf{u}\mathbf{u}}^2\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\end{array}\right]\left[\begin{array}{c}\delta {\mathbf{x}}_k\\ \delta {\mathbf{u}}_k\end{array}\right]\end{array}\text{\hspace{1em}(58)}$$

Here we trim the $2^{nd}$ order term for simplicity, and use the non-matrix for for plugging it back into approximating the value function at $k+1$ as in Eq.57
$$\begin{array}{rl}\delta {\mathbf{x}}_{k+1}& \approx \delta {\mathbf{x}}_k+{\nabla }_{\mathbf{x}}\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{u}}_k\\ & =\underset{{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)}{\underbrace{(\mathbf{I}+{\nabla }_{\mathbf{x}}\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k))}}\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{u}}_k\end{array}\text{\hspace{1em}(59)}$$
which yields
$$\begin{array}{rl}V({\bar{\mathbf{x}}}_{k+1}+\delta {\mathbf{x}}_{k+1})\approx & V({\bar{\mathbf{x}}}_{k+1})\\ & +{\nabla }_{\mathbf{x}}V({\bar{\mathbf{x}}}_{k+1}{)}^T\left[{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{u}}_k\right]\\ & +\frac{1}{2}{\left[{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{u}}_k\right]}^T{\nabla }_{\mathbf{x}\mathbf{x}}^{2}V({\bar{\mathbf{x}}}_{k+1})\\ & +\left[{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}\mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{u}}_k\right]\end{array}\text{\hspace{1em}(60)}$$

as shown above, the Taylor expansion equation can be pretty long and will get even longer afterwards, hence we need to adapt some notations to make it more compact.
$$\begin{gathered} V({\bar{\mathbf{x}}}_{k+1})=V^{\prime } \\ \mathbf{f}({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)=\mathbf{f}\text{\hspace{1em}(61)} \end{gathered}$$

then
$$\begin{array}{rl}V({\bar{\mathbf{x}}}_{k+1}+\delta {\mathbf{x}}_{k+1})& \approx V^{\prime }+{\nabla }_{\mathbf{x}}{V}^{\prime T}\left[{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}f\delta {\mathbf{u}}_k\right]\\ & +\frac{1}{2}{\left[{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}f\delta {\mathbf{u}}_k\right]}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }\left[{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}f\delta {\mathbf{u}}_k\right]\\ & =V^{\prime }+{\nabla }_{\mathbf{x}}{V}^{\prime T}{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{x}}{V}^{\prime T}{\nabla }_{\mathbf{u}}f\delta {\mathbf{u}}_k\\ & +\frac{1}{2}\delta {\mathbf{x}}_k^T{\nabla }_{\mathbf{x}}{\tilde{\mathbf{f}}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}\delta {\mathbf{x}}_k+\frac{1}{2}\delta {\mathbf{x}}_k^T{\nabla }_{\mathbf{x}}{\tilde{\mathbf{f}}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{u}}f\delta {\mathbf{u}}_k\\ & +\frac{1}{2}\delta {\mathbf{u}}_k^T{\nabla }_{\mathbf{u}}{\mathbf{f}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}\delta {\mathbf{x}}_k+\frac{1}{2}\delta {\mathbf{u}}_k^T{\nabla }_{\mathbf{u}}{\mathbf{f}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{u}}\mathbf{f}\delta {\mathbf{u}}_k\end{array}\text{\hspace{1em}(62)}$$

By now we have approximated the latter part of the RHS of Eq. 56, and we still need the second order expansion of the running cost function to fully approximate the value function, which is pretty straightforward:
$$\begin{array}{rl}\ell ({\mathbf{x}}_k,{\mathbf{u}}_k)& \approx \ell ({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)+{\nabla }_{\mathbf{x}}\ell ({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k{)}^T\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}\ell ({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k{)}^T\delta {\mathbf{u}}_k\\ & +\frac{1}{2}\delta {\mathbf{x}}_k^T{\nabla }_{\mathbf{x}\mathbf{x}}^2\ell ({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k{)}^T\delta {\mathbf{x}}_k+\frac{1}{2}\delta {\mathbf{u}}_k^T{\nabla }_{\mathbf{u}\mathbf{u}}^2\ell ({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{u}}_k\\ & +\frac{1}{2}\delta {\mathbf{x}}_k^T{\nabla }_{\mathbf{x}\mathbf{u}}^2\ell ({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{u}}_k+\frac{1}{2}\delta {\mathbf{u}}_k^T{\nabla }_{\mathbf{u}\mathbf{x}}^2\ell ({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)\delta {\mathbf{x}}_k\end{array}\text{\hspace{1em}(63)}$$

Now we can plug Eq 62 and 63 into Eq. 56 and get the final approximation of the value function:
$$\begin{array}{rl}V({\mathbf{x}}_k,{\mathbf{u}}_k)& =\underset{{\mathbf{u}}_k}{\min }[\ell +V^{\prime }+({\nabla }_{\mathbf{x}}{\tilde{\mathbf{f}}}^T{\nabla }_{\mathbf{x}}{V}^{\prime }+{\nabla }_{\mathbf{x}}\ell {)}^T\delta {\mathbf{x}}_k+({\nabla }_{\mathbf{u}}{\mathbf{f}}^T{\nabla }_{\mathbf{x}}{V}^{\prime }+{\nabla }_{\mathbf{u}}\ell {)}^T\delta {\mathbf{u}}_k\\ & +\frac{1}{2}\delta {\mathbf{x}}_k^T({\nabla }_{\mathbf{x}}{\tilde{\mathbf{f}}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}+{\nabla }_{\mathbf{x}\mathbf{x}}^2\ell )\delta {\mathbf{x}}_k\\ & +\frac{1}{2}\delta {\mathbf{x}}_k^T({\nabla }_{\mathbf{x}}{\tilde{\mathbf{f}}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{u}}\mathbf{f}+{\nabla }_{\mathbf{x}\mathbf{u}}^2\ell )\delta {\mathbf{u}}_k\\ & +\frac{1}{2}\delta {\mathbf{u}}_k^T({\nabla }_{\mathbf{u}}{\mathbf{f}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}+{\nabla }_{\mathbf{u}\mathbf{x}}^2\ell )\delta {\mathbf{x}}_k\\ & +\frac{1}{2}\delta {\mathbf{u}}_k^T({\nabla }_{\mathbf{u}}{\mathbf{f}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{u}}\mathbf{f}+{\nabla }_{\mathbf{u}\mathbf{u}}^2\ell )\delta {\mathbf{u}}_k]\end{array}\text{\hspace{1em}(64)}$$

theoretically, we can now set ${\nabla }_{{\mathbf{u}}_k}V(\mathbf{x},\mathbf{u})=0$ to get the optimal control input $u_k^{\ast }$ at step $k$. However, please note these terms in brackets are all derivatives of the dynamics, cost function and value function, hence we can bring in yet another handy notation to make the formulas more compact:
$$S(\mathbf{x},\mathbf{u})=\ell (\mathbf{x},\mathbf{u})+V^{\prime }(\mathbf{f}(\mathbf{x},\mathbf{u}))\text{\hspace{1em}(65)}$$

which is commonly referred as the state-action value function. Now we can rewrite the ”optimal” value function at stage $k$ as:
$$\begin{array}{rl}V({\mathbf{x}}_k)& =\underset{{\mathbf{u}}_k}{\min }S({\mathbf{x}}_k,{\mathbf{u}}_k)\\ & =\underset{{\mathbf{u}}_k}{\min }S({\bar{\mathbf{x}}}_k+\delta \mathbf{x},{\bar{\mathbf{u}}}_k+\delta \mathbf{u})\\ & =\underset{{\mathbf{u}}_k}{\min }[S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)+{\nabla }_{\mathbf{x}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k{)}^T\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k{)}^T\delta {\mathbf{u}}_k\\ & +\frac{1}{2}\delta {\mathbf{x}}_k^T{\nabla }_{\mathbf{x}\mathbf{x}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)\delta {\mathbf{x}}_k+\frac{1}{2}\delta {\mathbf{x}}_k^T{\nabla }_{\mathbf{x}\mathbf{u}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)\delta {\mathbf{u}}_k\\ & +\frac{1}{2}\delta {\mathbf{u}}_k^T{\nabla }_{\mathbf{u}\mathbf{x}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)\delta {\mathbf{x}}_k+\frac{1}{2}\delta {\mathbf{u}}_k^T{\nabla }_{\mathbf{u}\mathbf{u}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)\delta {\mathbf{u}}_k]\end{array}\text{\hspace{1em}(66)}$$

where the derivatives of the state-action value function are defined as in Eq. 64:
$$\begin{array}{rl}{\nabla }_{\mathbf{x}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)& ={\nabla }_{\mathbf{x}}{\tilde{f}}^T{\nabla }_{\mathbf{x}}{V}^{\prime }+{\nabla }_{\mathbf{x}}\ell \\ {\nabla }_{\mathbf{u}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)& ={\nabla }_{\mathbf{u}}{f}^T{\nabla }_{\mathbf{x}}{V}^{\prime }+{\nabla }_{\mathbf{u}}\ell \\ {\nabla }_{\mathbf{xx}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)& ={\nabla }_{\mathbf{x}}{\tilde{\mathbf{f}}}^T{\nabla }_{\mathbf{xx}}^2{V}^{\prime }{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}+{\nabla }_{\mathbf{xx}}^2\ell \\ {\nabla }_{\mathbf{xu}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)& ={\nabla }_{\mathbf{x}}{\tilde{\mathbf{f}}}^T{\nabla }_{\mathbf{xx}}^2{V}^{\prime }{\nabla }_{\mathbf{u}}f+{\nabla }_{\mathbf{xu}}^2\ell \\ {\nabla }_{\mathbf{ux}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)& ={\nabla }_{\mathbf{u}}{f}^T{\nabla }_{\mathbf{xx}}^2{V}^{\prime }{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}+{\nabla }_{\mathbf{ux}}^2\ell \\ {\nabla }_{\mathbf{uu}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)& ={\nabla }_{\mathbf{u}}{f}^T{\nabla }_{\mathbf{xx}}^2{V}^{\prime }{\nabla }_{\mathbf{u}}\mathbf{f}+{\nabla }_{\mathbf{uu}}^2\ell \end{array}\text{\hspace{1em}(67)}$$

still, we can re-introduce the second order Taylor expansion of the dynamics which we have omitted earlier:
$$\begin{array}{rl}{\nabla }_{\mathbf{x}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)& ={\nabla }_{\mathbf{x}}{\tilde{\mathbf{f}}}^T{\nabla }_{\mathbf{x}}{V}^{\prime }+{\nabla }_{\mathbf{x}}\ell \\ {\nabla }_{\mathbf{u}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)& ={\nabla }_{\mathbf{u}}{f}^T{\nabla }_{\mathbf{x}}{V}^{\prime }+{\nabla }_{\mathbf{u}}\ell \\ {\nabla }_{\mathbf{x}\mathbf{x}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)& ={\nabla }_{\mathbf{x}}{\tilde{\mathbf{f}}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}+{\nabla }_{\mathbf{x}}{V}^{\prime T}{\nabla }_{\mathbf{x}\mathbf{x}}^2\tilde{\mathbf{f}}+{\nabla }_{\mathbf{x}\mathbf{x}}^2\ell \\ {\nabla }_{\mathbf{x}\mathbf{u}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)& ={\nabla }_{\mathbf{x}}{\tilde{\mathbf{f}}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{u}}\mathbf{f}+{\nabla }_{\mathbf{x}}{V}^{\prime T}{\nabla }_{\mathbf{x}\mathbf{u}}^2\mathbf{f}+{\nabla }_{\mathbf{x}\mathbf{u}}^2\ell \\ {\nabla }_{\mathbf{u}\mathbf{x}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)& ={\nabla }_{\mathbf{u}}{\mathbf{f}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{x}}\tilde{\mathbf{f}}+{\nabla }_{\mathbf{x}}{V}^{\prime T}{\nabla }_{\mathbf{u}\mathbf{x}}^2\mathbf{f}+{\nabla }_{\mathbf{u}\mathbf{x}}^2\ell \\ {\nabla }_{\mathbf{u}\mathbf{u}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)& ={\nabla }_{\mathbf{u}}{\mathbf{f}}^T{\nabla }_{\mathbf{x}\mathbf{x}}^2{V}^{\prime }{\nabla }_{\mathbf{u}}\mathbf{f}+{\nabla }_{\mathbf{x}}{V}^{\prime T}{\nabla }_{\mathbf{u}\mathbf{u}}^2\mathbf{f}+{\nabla }_{\mathbf{u}\mathbf{u}}^2\ell \end{array}\text{\hspace{1em}(68)}$$

these two sets of derivatives make iterative LQR and DDP respectively. The difference between the two methods is that iterative LQR uses the first order Taylor expansion of the dynamics, while DDP uses the second order Taylor expansion of the dynamics.

Now we can solve the optimal control input ${\mathbf{u}}_k^{\ast }$ by letting the gradient of the value function in Eq. 66 over $\mathbf{u}$ be zero:

$$\begin{array}{rl}& {\nabla }_{\mathbf{u}}S({\mathbf{x}}_k,{\mathbf{u}}_k)={\nabla }_{\mathbf{u}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k{)}^T+\delta {\mathbf{x}}_k^T{\nabla }_{\mathbf{x}\mathbf{u}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)+\delta {\mathbf{u}}_k^T{\nabla }_{\mathbf{u}\mathbf{u}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)=0\\ & ⟹{\mathbf{u}}_k^{\ast }=\underset{\text{feedforward term k}}{\underbrace{-{\nabla }_{\mathbf{u}\mathbf{u}}^2{S}^{-1}{\nabla }_{\mathbf{u}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)}}\underset{\text{feedback term K}}{\underbrace{-{\nabla }_{\mathbf{u}\mathbf{u}}^2{S}^{-1}{\nabla }_{\mathbf{u}\mathbf{x}}^{2}S\delta {\mathbf{x}}_k}}\end{array}\text{\hspace{1em}(69)}$$

3.2 Backward Pass

Given the one step optimal policy ${\mathbf{u}}_k^{\ast }$, we can start from the terminal step $k=N$ and working backwards as we did in Dynamic Programming with Bellman’s equation.

Unlike normal DP, with the action-value function introduced, we still need the recursion for $S$ to propagate, and also we are relying heavily on the derivatives of the value function at the next step (in other words, we should calculate and store that for the next iteration backwards at current step), in order to tackle that, we can plug the optimal control input ${\mathbf{u}}_k^{\ast }$ back into Eq. 66.

$$\begin{array}{rl}V({\mathbf{x}}_k)=& S({\mathbf{x}}_k,{\mathbf{u}}_k)\\ =& S({\bar{\mathbf{x}}}_k+\delta \mathbf{x},{\bar{\mathbf{u}}}_k+\delta \mathbf{u})\\ =& S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)+{\nabla }_{\mathbf{x}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k{)}^T\delta {\mathbf{x}}_k+{\nabla }_{\mathbf{u}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k{)}^T\delta {\mathbf{u}}_k^{\ast }\\ & +\frac{1}{2}\delta {\mathbf{x}}_k^T{\nabla }_{\mathbf{x}\mathbf{x}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)\delta {\mathbf{x}}_k+\frac{1}{2}\delta {\mathbf{x}}_k^T{\nabla }_{\mathbf{x}\mathbf{u}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)\delta {\mathbf{u}}_k^{\ast }\\ & +\frac{1}{2}\delta {\mathbf{u}}_k^{\ast T}{\nabla }_{\mathbf{u}\mathbf{x}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)\delta {\mathbf{x}}_k+\frac{1}{2}\delta {\mathbf{u}}_k^{\ast T}{\nabla }_{\mathbf{u}\mathbf{u}}^{2}S({\mathbf{x}}_k,{\mathbf{u}}_k)\delta {\mathbf{u}}_k^{\ast }\end{array}\text{\hspace{1em}(70)}$$

where
$${\mathbf{u}}_k^{\ast }=-{\nabla }_{\mathbf{uu}}^2{S}^{-1}{\nabla }_{\mathbf{u}}S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)-{\nabla }_{\mathbf{uu}}^2{S}^{-1}{\nabla }_{\mathbf{ux}}^{2}S\delta {\mathbf{x}}_k\text{\hspace{1em}(71)}$$
reorgnizing the above equation leads to
$$\begin{array}{rl}V({\mathbf{x}}_k)& =S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)-\frac{1}{2}{\nabla }_{\mathbf{u}}{S}^T{\nabla }_{\mathbf{u}\mathbf{u}}^2{S}^{-1}{\nabla }_{\mathbf{u}}S+({\nabla }_{\mathbf{x}}{S}^T-{\nabla }_{\mathbf{u}}{S}^T{\nabla }_{\mathbf{u}\mathbf{u}}^2{S}^{-1}{\nabla }_{\mathbf{u},\mathbf{x}}^{2}S)\delta \mathbf{x}\\ & +\frac{1}{2}\delta {\mathbf{x}}^T({\nabla }_{\mathbf{x}\mathbf{x}}^2{S}^T-{\nabla }_{\mathbf{x}\mathbf{u}}^{2}S{\nabla }_{\mathbf{u}\mathbf{u}}^2{S}^{-1}{\nabla }_{\mathbf{u}\mathbf{x}}^{2}S)\delta \mathbf{x}\end{array}\text{\hspace{1em}(72)}$$

which has a clear structure of ordered items from 0 to 2, i.e., we can approximately use
$$\begin{array}{rl}\Delta V& =S({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)-\frac{1}{2}{\nabla }_u{S}^T{\nabla }_{uu}^2{S}^{-1}{\nabla }_{u}S\\ {\nabla }_{\mathbf{x}}V& ={\nabla }_x{S}^T-{\nabla }_u{S}^T{\nabla }_{uu}^2{S}^{-1}{\nabla }_{u,x}^{2}S\\ {\nabla }_{\mathbf{xx}}V& ={\nabla }_{xx}^2{S}^T-{\nabla }_{xu}^{2}S{\nabla }_{uu}^2{S}^{-1}{\nabla }_{ux}^{2}S\end{array}\text{\hspace{1em}(73)}$$
for step $k-1$.

3.3 Line Search

Please be noted that the second order expansion of dynamics, cost function and value function can only hold locally, it can be possible that the $\mathbf{u}\ast$ overshoots and leads to inefficiency or even unstable optimisation process. Hence we can introduce a line-search step for evluation the ferformance of applying scaled feedforward term by $\alpha =0\sim 1$ during the iteration.

3.4 Forward Pass

Once we have finished the backward pass from the last timestep to the first one, we can roll-out the system response forward with dynamics.

Algorithm 1 DDP
Require: Initial State ${\mathbf{x}}_0$, dynamics model ${\mathbf{x}}_{i+1}=f({\mathbf{x}}_i,{\mathbf{u}}_i)$, Initial reference control sequence ${\mathbf{U}}_0$
Ensure: Optimal Control Sequence ${\mathbf{U}}^{\ast }$, Optimal State Trajectory ${\mathbf{X}}^{\ast }$
1: function $DDP(f,l,l_f)$
2:     Initial State ${\mathbf{x}}_0$, Initial reference control sequence ${\mathbf{U}}_0$
3:     while not converged do
4:         ${\nabla }_{\mathbf{x}}V({\mathbf{x}}_N)={\nabla }_{\mathbf{x}}{\ell }_f$
5:         ${\nabla }_{\mathbf{xx}}V({\mathbf{x}}_N)={\nabla }_{\mathbf{x}\mathbf{x}}{\ell }_f$
6:         for $i=N-1\to 0$ do
7:             $\mathbf{k},\mathbf{K}\leftarrow BackwardPass$
8:         end for
9:         for $i=0\to N-1$ do
10:             Forward Pass with line search
11:         end for
12:         return Optimal Trajectory
13:     end while
14: end function
15: function Forward Pass
16:     Initialization, ${\bar{\mathbf{x}}}_0\leftarrow {\mathbf{x}}_0,\mathbf{U}={\mathbf{U}}_0,\alpha =1$
17:     for $i=0\to N-1$ do
18:         ${\overline{\mathbf{u}}}_i\leftarrow {\mathbf{u}}_i+\alpha k[i]+K[i]({\overline{\mathbf{x}}}_i-{\mathbf{x}}_i)$
19:         ${\overline{\mathbf{x}}}_{i+1}=f({\overline{\mathbf{x}}}_i,{\overline{\mathbf{u}}}_i)$
20:         $\mathbf{X}[i]\leftarrow {\overline{\mathbf{x}}}_i$
21:         $\mathbf{U}[i]\leftarrow {\overline{\mathbf{u}}}_i$
22:     end for
23:     $\alpha =\rho \alpha$
24:     $\mathbf{X}[N]\leftarrow {\overline{\mathbf{x}}}_N$
25:      T ← { X , U } \bm{T}\leftarrow\{\bm{X},\bm{U}\} T←{X,U}
26:     $J=J(T)$
27:     return $j,\mathbf{T}$
28: end function
29: function Backward Pass
30:     Initialization, $V_{\mathbf{x}}\leftarrow l_{f,\mathbf{x}}({\mathbf{x}}_n),V_{\mathbf{x},\mathbf{x}}\leftarrow l_{f,\mathbf{x}}({\mathbf{x}}_n),k\leftarrow [],K\leftarrow []$
31:     for $i=N-1\to 0$ do
32:         $S_x\leftarrow l_x{|}_{x_i}+(f_x^T{V}_x){|}_{x_i}$
33:         $S_u\leftarrow l_u{|}_{{\mathbf{x}}_i}+(f_u^T{V}_x){|}_{{\mathbf{u}}_i,{\mathbf{x}}_i}$
34:         $S_{\mathbf{x}\mathbf{x}}\leftarrow l_{\mathbf{x}\mathbf{x}}{|}_{{\mathbf{x}}_i}+(f_x^T{V}_{\mathbf{xx}}{f}_x){|}_{{\mathbf{x}}_i}$
35:         $S_{\mathbf{u}\mathbf{u}}\leftarrow l_{\mathbf{u}\mathbf{u}}{|}_{{\mathbf{u}}_i}+(f_u^T{V}_{\mathbf{xx}}{f}_u){|}_{{\mathbf{u}}_i,{\mathbf{x}}_i,{\mathbf{u}}_i}$
36:         $S_{\mathbf{u}\mathbf{x}}\leftarrow l_{\mathbf{u}\mathbf{x}}{|}_{{\mathbf{u}}_i,{\mathbf{x}}_i}+(f_u^T{V}_{\mathbf{xx}}{f}_x){|}_{{\mathbf{u}}_i,{\mathbf{x}}_i,{\mathbf{x}}_i}$
37:         ${\hat{S}}_{\mathbf{u}}\leftarrow l_u{|}_{{\mathbf{x}}_i}+(f_u^T(V_x+\mu {\mathbf{I}}_n)){|}_{{\mathbf{u}}_i,{\mathbf{x}}_i}$
38:         ${\tilde{S}}_{\mathbf{u}\mathbf{u}}\leftarrow l_{\mathbf{u}\mathbf{u}}{|}_{{\mathbf{u}}_i}+(f_u^T(V_{\mathbf{xx}}+\mu {\mathbf{I}}_n))f_u){|}_{{\mathbf{u}}_i,{\mathbf{x}}_i,{\mathbf{u}}_i}$
39:         ${\tilde{S}}_{\mathbf{u}\mathbf{x}}\leftarrow l_{\mathbf{u}\mathbf{x}}{|}_{{\mathbf{u}}_i,{\mathbf{x}}_i}+(f_u^T(V_{\mathbf{xx}}+\mu {\mathbf{I}}_n)f_x){|}_{{\mathbf{u}}_i,{\mathbf{x}}_i,{\mathbf{x}}_i}$
40:         $k[i]\leftarrow -{\tilde{S}}_{\mathbf{uu}}^{-1}{\hat{S}}_{\mathbf{u}}$
41:         $K[i]\leftarrow -{\tilde{S}}_{\mathbf{uu}}^{-1}{\tilde{S}}_{\mathbf{u}\mathbf{x}}$
42:         $V_x\leftarrow S_x-K^T{S}_{\mathbf{uu}}k$
43:         $V_{xx}\leftarrow S_{xx}-K^T{S}_{uu}K$
44:     end for
45:     return $\mathbf{k}$, $\mathbf{K}$
46: end function