一. ilqr原理推导

1.1 ilqr问题描述

本文参考知乎博主: LQR与iLQR：从理论到实践【详细】

基础LQR只能处理线性系统 (指可以使用$x(k+1)=Ax(k)+Bu(k)$来描述系统 ) ，对目标代价函数也做了要求，而在实际问题中非线性系统占了绝大部分，iLQR（Iterative Linear Quadratic Regulator，迭代线性二次型调节器）则能够通过迭代的方式有效地求解非线性最优控制问题，考虑如下问题表述形式：
$$\begin{array}{cc}\underset{u_0,\dots ,u_{N-1}}{\min }& J=l_f(x_N)+\sum _{k=0}^{N-1}l(x_k,u_k)\\ \text{s.t.}& x_{k+1}=f(x_k,u_k),x_0\text{ given}\end{array}$$
其中$J$是目标函数，需要在控制变量$u_0,...,u_{N-1}$下进行最小化。目标函数由两部分组成：终端状态$x_N$的代价 $l_f(x_N)$ 和 从 $k=0$ 到 $N-1$ 的每一个时间步的代价$l(x_k,u_k)$的总和，约束条件是状态转移方程$x_{k+1}=f(x_k,u_k)$，并且初始状态$x_0$是给定的。

1.2 ilqr算法原理

与LQR问题求解过程类似，定义问题的最优状态值函数${\hat{J}}_i(x_i)$和动作值函数${\tilde{J}}_i(x_i,u_i)$：
$$\begin{array}{rl}{\hat{J}}_i(x_i)& =\underset{u_i}{\min }\left\{l(x_i,u_i)+{\hat{J}}_{i+1}(x_{i+1})\right\}\\ & =\underset{u_i}{\min }\left\{l(x_i,u_i)+{\hat{J}}_{i+1}(f(x_i,u_i))\right\}\\ & =\underset{u_i}{\min }{\tilde{J}}_i\left(x_i,u_i\right)\end{array}\text{\hspace{1em}(3)}$$
下面详细解释(3)是怎么来的：

1. 最优状态值函数 ${\hat{J}}_i(x_i)$ 定义为给定当前状态$x_i$，从时间步 $i$ 到 $N$ 的最小总代价，举个例子，现在状态是$x_{N-1}$，那么可以预期的最优状态值 ${\hat{J}}_{N-1}(x_{N-1})$ 为当前 $N-1$ 时刻的最小即时代价$l(x_{N-1},u_{N-1})$ 加上最小终端代价${\hat{J}}_N(x_N)$，就像一个人开车从A市去B市，计算最优燃油消耗量Y，那么他最省油的开法肯定是当前时刻采用最省油的开法，并在到达B市之前都一直采用最省油的开法。
2. 动作值函数 ${\tilde{J}}_i\left(x_i,u_i\right)$ 定义为在当前状态$x_i$下采取某个控制输入$u_i$并遵循最优策略直到终端状态的总代价，动作值函数为最优需要满足采取的控制输入$u_i$为最优。
3. 状态转移：状态$x_{i+1}$ 由当前状态$x_i$和$u_i$通过状态转移函数$f(x_i,u_i)$决定，因此，可以将当前状态$x_i$的最优状态值函数 ${\hat{J}}_i(x_i)$ 表达为：
   $$\begin{array}{rl}{\hat{J}}_i(x_i)& =\underset{u_i}{\min }\left\{l(x_i,u_i)+{\hat{J}}_{i+1}(x_{i+1})\right\}\\ & =\underset{u_i}{\min }\left\{l(x_i,u_i)+{\hat{J}}_{i+1}(f(x_i,u_i))\right\}\\ & =\underset{u_i}{\min }{\tilde{J}}_i(x_i,u_i)\end{array}\text{\hspace{1em}(4)}$$
4. 动作值函数 ${\tilde{J}}_i\left(x_i,u_i\right)$ 可以定义为：
   $$\begin{array}{r}{\tilde{J}}_i\left(x_i,u_i\right)=l(x_i,u_i)+{\hat{J}}_{i+1}(f(x_i,u_i))\end{array}\text{\hspace{1em}(5)}$$
5. 最小化动作值函数：最优状态值函数也可以通过最小化所有可能的动作值函数得到：
   $$\begin{array}{r}{\hat{J}}_i(x_i)=\underset{u_i}{\min }{\tilde{J}}_i(x_i,u_i)\end{array}\text{\hspace{1em}(6)}$$
   坐稳了，下面开始推导，终端最优状态值函数 ${\hat{J}}_N(x_N)=l_f(x_N)$ ，很好理解，因为到终端状态就没有输入了，所以只有状态代价，没有控制代价。
   使用泰勒展开将系统在当前状态$x_i$附近线性化，进而转换为LQR问题进行求解：
   状态转移函数展开，这里需要了解下非线性系统是如何线性化的，链接: 雅可比矩阵几何意义的直观解释及应用
   通过雅可比矩阵得到$A_i$和$B_i$
   $$\begin{array}{rl}f\left(x_i,u_i\right)+\delta f\left(x_i,u_i\right)& =f\left(x_i+\delta x_i,u_i+\delta u_i\right)\\ & \approx f\left(x_i,u_i\right)+\frac{\partial f\left(x_i,u_i\right)}{\partial x_i}\delta x_i+\frac{\partial f\left(x_i,u_i\right)}{\partial u_i}\delta u_i\\ & \triangleq f\left(x_i,u_i\right)+A_i\delta x_i+B_i\delta u_i\end{array}\text{\hspace{1em}(7)}$$
   状态值函数展开，这里也很好理解，注意这个$p_i^T$和$P_i$，后面backward pass的时候通过更新它们俩来实现状态递推
   $$\begin{array}{rl}{\hat{J}}_i(x_i)+\delta {\hat{J}}_i(x_i)& ={\hat{J}}_i(x_i+\delta x_i)\\ & \approx {\hat{J}}_i(x_i)+\frac{\partial {\hat{J}}_i(x_i)}{\partial x_i}\delta x_i+\frac{1}{2}\delta x_i^T\frac{{\partial }^2{\hat{J}}_i(x_i)}{\partial x_i^2}\delta x_i\\ & \triangleq {\hat{J}}_i(x_i)+p_i^T\delta x_i+\frac{1}{2}\delta x_i^T{P}_i\delta x_i\end{array}\text{\hspace{1em}(8)}$$
   动作值函数展开，常规的泰勒一阶展开和二阶展开，没有什么难理解的
   $$\begin{array}{rl}{\tilde{J}}_i\left(x_i,u_i\right)+\delta {\tilde{J}}_i\left(x_i,u_i\right)& ={\tilde{J}}_i\left(x_i+\delta x_i,u_i+\delta u_i\right)\\ & \approx {\tilde{J}}_i\left(x_i,u_i\right)+\frac{\partial {\tilde{J}}_i\left(x_i,u_i\right)}{\partial x_i}\delta x_i+\frac{\partial {\tilde{J}}_i\left(x_i,u_i\right)}{\partial u_i}\delta u_i\\ & +\frac{1}{2}\delta x_i^T\frac{{\partial }^2{\tilde{J}}_i\left(x_i,u_i\right)}{\partial x_i^2}\delta x_i+\frac{1}{2}\delta u_i^T\frac{{\partial }^2{\tilde{J}}_i\left(x_i,u_i\right)}{\partial u_i^2}\delta u_i\\ & +\frac{1}{2}\delta x_i^T\frac{{\partial }^2{\tilde{J}}_i\left(x_i,u_i\right)}{\partial x_i\partial u_i}\delta u_i+\frac{1}{2}\delta u_i^T\frac{{\partial }^2{\tilde{J}}_i\left(x_i,u_i\right)}{\partial u_i\partial x_i}\delta x_i\\ & \triangleq {\tilde{J}}_i\left(x_i,u_i\right)+Q_{x_i}^T\delta x_i+Q_{u_i}^T\delta u_i+\frac{1}{2}\delta x_i^T{Q}_{x_i^2}\delta x_i\\ & +\frac{1}{2}\delta u_i^T{Q}_{u_i^2}\delta u_i+\frac{1}{2}\delta x_i^T{Q}_{x_i{u}_i}\delta u_i+\frac{1}{2}\delta u_i^T{Q}_{u_i{x}_i}^T\delta x_i\end{array}(9)$$
   根据公式(5)，对 $\tilde{J}\left(x,u\right)$做泰勒展开有：
   $$\begin{array}{rl}{\tilde{J}}_i(x_i,u_i)+\delta {\tilde{J}}_i(x_i,u_i)& ={\tilde{J}}_i(x_i+\delta x_i,u_i+\delta u_i)\\ & =l(x_i+\delta x_i,u_i+\delta u_i)+{\hat{J}}_{i+1}(f(x_i+\delta x_i,u_i+\delta u_i))\end{array}(10)$$
   其中，即时代价展开为：
   $$\begin{array}{rl}l(x_i+\delta x_i,u_i+\delta u_i)& =l(x_i,u_i)+\delta l(x_i,u_i)\\ & \approx l(x_i,u_i)+\frac{\partial l(x_i,u_i)}{\partial x_i}\delta x_i+\frac{\partial l(x_i,u_i)}{\partial u_i}\delta u_i\\ & +\frac{1}{2}\delta x_i^T\frac{{\partial }^{2}l(x_i,u_i)}{\partial x_i^2}\delta x_i+\frac{1}{2}\delta u_i^T\frac{{\partial }^{2}l(x_i,u_i)}{\partial u_i^2}\delta u_i\\ & +\frac{1}{2}\delta x_i^T\frac{{\partial }^{2}l(x_i,u_i)}{\partial x_i\partial u_i}\delta u_i+\frac{1}{2}\delta u_i^T\frac{{\partial }^{2}l(x_i,u_i)}{\partial u_i\partial x_i}\delta x_i\\ & \triangleq l(x_i,u_i)+l_{x_i}\delta x_i+l_{u_i}\delta u_i+\frac{1}{2}\delta x_i^T{l}_{x_i^2}\delta x_i\\ & +\frac{1}{2}\delta u_i^T{l}_{u_i^2}\delta u_i+\frac{1}{2}\delta x_i^T{l}_{x_i{u}_i}\delta u_i+\frac{1}{2}\delta u_i^T{l}_{u_i{x}_i}\delta x_i\end{array}(11)$$
   下一时刻的状态值函数展开为(这里参考式(8))：
   $$\begin{array}{rl}{\hat{J}}_{i+1}\left(f\left(x_i+\delta x_i,u_i+\delta u_i\right)\right)& ={\hat{J}}_{i+1}\left(f\left(x_i,u_i\right)+\delta f\left(x_i,u_i\right)\right)\\ & \approx {\hat{J}}_{i+1}\left(f\left(x_i,u_i\right)\right)+\frac{\partial {\hat{J}}_{i+1}\left(x_{i+1}\right)}{\partial x_{i+1}}\delta f\left(x_i,u_i\right)\\ & +\frac{1}{2}\delta f{\left(x_i,u_i\right)}^T\frac{{\partial }^2{\hat{J}}_{i+1}\left(x_{i+1}\right)}{\partial x_{i+1}^2}\delta f\left(x_i,u_i\right)\\ & ={\hat{J}}_{i+1}\left(f\left(x_i,u_i\right)\right)+p_{i+1}^T\left(A_i\delta x_i+B_i\delta u_i\right)\\ & +\frac{1}{2}{\left(A_i\delta x_i+B_i\delta u_i\right)}^T{P}_{i+1}\left(A_i\delta x_i+B_i\delta u_i\right)\\ & ={\hat{J}}_{i+1}\left(f\left(x_i,u_i\right)\right)+p_{i+1}^T{A}_i\delta x_i+p_{i+1}^T{B}_i\delta u_i\\ & +\frac{1}{2}\delta x_i^T{A}_i^T{P}_{i+1}{A}_i\delta x_i+\frac{1}{2}\delta x_i^T{A}_i^T{P}_{i+1}{B}_i\delta u_i\\ & +\frac{1}{2}\delta u_i^T{B}_i^T{P}_{i+1}{A}_i\delta x_i+\frac{1}{2}\delta u_i^T{B}_i^T{P}_{i+1}{B}_i\delta u_i\end{array}(12)$$
   其中，$p_i=\frac{\partial {\hat{J}}_i(x_i)}{\partial x_i}$，$P_i=\frac{{\partial }^2{\hat{J}}_i(x_i)}{\partial x_i^2}$。 联合(8)(9)(10)(11)(12)可得${\tilde{J}}_i(x_i,u_i)$在$(x_i,u_i)$附近泰勒展开的多项式系数矩阵：
   $$\begin{array}{rl}{Q}_{x_i}& =l_{x_i}+p_{i+1}^T{A}_i\\ Q_{u_i}& =l_{u_i}+p_{i+1}^T{B}_i\\ Q_{x_i^2}& =l_{x_i^2}+A_i^T{P}_{i+1}{A}_i\\ Q_{x_i{u}_i}& =l_{x_i{u}_i}+A_i^T{P}_{i+1}{B}_i\\ Q_{u_i{x}_i}& =l_{u_i{x}_i}+B_i^T{P}_{i+1}{A}_i\\ Q_{u_i^2}& =l_{u_i^2}+B_i^T{P}_{i+1}{B}_i\end{array}\text{\hspace{1em}(13)}$$
   一般来说有$Q_{x_i{u}_i}=Q_{x_i{u}_i}^T$，根据式(9)，如果当前状态$x_i$，输入是$u_i$，希望$x_i+\delta x_i$的代价值函数${\tilde{J}}_i\left(x_i+\delta x_i,u_i+\delta u_i\right)$最小，则需要使$\delta {\tilde{J}}_i(x_i,u_i)$最小，有：
   $$\begin{array}{rl}\delta {\hat{J}}_i(x_i)& =\underset{\delta u_i}{\min }\delta {\tilde{J}}_i(x_i,u_i)\\ & =\underset{\delta u_i}{\min }\{Q_{x_i}^T\delta x_i+Q_{u_i}^T\delta u_i+\frac{1}{2}\delta x_i^T{Q}_{x_i^2}\delta x_i+\frac{1}{2}\delta u_i^T{Q}_{u_i^2}\delta u_i\\ & +\frac{1}{2}\delta x_i^T{Q}_{x_i{u}_i}\delta u_i+\frac{1}{2}\delta u_i^T{Q}_{u_i{x}_i}^T\delta x_i\}\\ & =\underset{\delta u_i}{\min }\delta {\tilde{J}}_i(x_i,u_i)\end{array}(14)$$
   $\delta {\tilde{J}}_i(x_i,u_i)$对$\delta u_i$求偏导，使其取得最小值得到$\delta {\hat{J}}_i(x_i)$，并让其等于0有：
   $$\begin{array}{rl}\frac{\partial \delta {\tilde{J}}_i(x_i)}{\partial \delta u_i}& =Q_{u_i}+\frac{1}{2}{Q}_{u_i{x}_i}^T\delta x_i+Q_{u_i^2}\delta u_i+\frac{1}{2}{Q}_{x_i{u}_i}\delta x_i\\ & =Q_{u_i}+Q_{u_i{x}_i}\delta x_i+Q_{u_i^2}\delta u_i=0\end{array}(15)$$
   解出来在$i$时刻的最优控制输入的变化量：
   $$\begin{array}{rl}\delta {\hat{u}}_i^{\ast }& =-Q_{u_i^2}^{-1}\left(Q_{u_i}+Q_{u_i{x}_i}\delta x_i\right)\\ & \triangleq K_i\delta x_i+d_i\end{array}(16)$$
   iLQR的核心思想就是在给定初解$u_i$的基础上，求最优的补偿输入$\delta {\hat{u}}_i^{\ast }$ 使得在$u_i+\delta {\hat{u}}_i^{\ast }$的作用下获得更优的解，并循环这个过程。
   $K_i$为反馈增益， $d_i$为前馈增益。将式(16)代入到式(9)整理得：
   $$\begin{array}{rl}\delta {\tilde{J}}_i(x_i,u_i^{\ast })& =Q_{x_i}^T\delta x_i+Q_{u_i}^T(K_i\delta x_i+d_i)+\frac{1}{2}\delta x_i^T{Q}_{x_i^2}\delta x_i\\ & +\frac{1}{2}(K_i\delta x_i+d_i{)}^T{Q}_{u_i^2}(K_i\delta x_i+d_i)\\ & +\frac{1}{2}\delta x_i^T{Q}_{x_i{u}_i}(K_i\delta x_i+d_i)+\frac{1}{2}(K_i\delta x_i+d_i{)}^T{Q}_{u_i{x}_i}\delta x_i\\ & =\frac{1}{2}\delta x_i^T[Q_{x_i^2}+K_i^T{Q}_{u_i^2}{K}_i+Q_{x_i{u}_i}{K}_i+K_i^T{Q}_{u_i{x}_i}]\delta x_i\\ & +[Q_{x_i}+K_i^T{Q}_{u_i^2}{d}_i+Q_{x_i{u}_i}{d}_i+K_i^T{Q}_{u_i}{d}_i{]}^T\delta x_i\\ & +\frac{1}{2}{d}_i^T{Q}_{u_i^2}{d}_i+Q_{u_i}^T{d}_i\end{array}(17)$$
   这里需要注意的是$\delta {\tilde{J}}_i(x_i,u_i^{\ast })=\delta {\hat{J}}_i(x_i)$，结合式(8)式(17)可以得到$p_i$和$P_i$的递推关系式：
   $$\begin{array}{rl}{p}_i& =Q_{x_i}+K_i^T{Q}_{u_i^2}{d}_i+Q_{x_i{u}_i}{d}_i+K_i^T{Q}_u\\ P_i& =Q_{x_i^2}+K_i^T{Q}_{u_i^2}{K}_i+Q_{x_i{u}_i}{K}_i+K_i^T{Q}_{u_i{x}_i}\\ \Delta {\hat{J}}_i& =\frac{1}{2}{d}_i^T{Q}_{u_i^2}{d}_i+Q_{u_i}^T{d}_i\end{array}(18)$$
   其中，$p_N^T=\frac{\partial l_f(x_N)}{\partial x_N}$，$P_N=\frac{{\partial }^2{l}_f(x_N)}{\partial x_N^2}$；$\Delta {\hat{J}}_i$是${\hat{J}}_i(x_i)$二阶泰勒展开的高阶无穷小项，与$\delta x$无关，不影响$\delta u$的计算。
   从上面的推导可以看出，如果确定$p_N^T$和$P_N$，就可以通过式(18)来递推得到$p_{N-1}^T$和$P_{N-1}$，之所以有backward pass，就是为了从$N-1$时刻，一直倒推到当前时刻，求出每个时刻的最优控制输入的变化量。

1.3 ilqr算法迭代过程

1. Backward pass
   初始化$p_N^T=\frac{\partial l_f(x_N)}{\partial x_N}$，$P_N=\frac{{\partial }^2{l}_f(x_N)}{\partial x_N^2}$；
   $for$ $i=N-1,...,0$
   根据式(13)式(18)更新$p_i\leftarrow p_{i+1}$，$P_i\leftarrow P_{i+1}$
   根据式(16)计算$K_i=Q_{u_i^2}^{-1}{Q}_{u_i{x}_i}$，$d_i=Q_{u_i^2}^{-1}{Q}_{u_i}$
2. Forward pass
   初始条件$x_0$，对于任意迭代次数$j$有$x_0^j=x_0^{j-1}$，这里也好理解，每次迭代都是基于当前时刻的状态，所以不会变
   $for$ $i=0,...,N-1$
   $\delta x_i=x_i^j-x_i^{j-1}$，$\delta x_i$的含义就是当前轮迭代解和上一轮迭代解在$i$时刻的差值(是不是有牛顿法那味了)
   $\delta {\hat{u}}_i^{\ast }=K_i\delta x_i+d_i$，更新控制输入$u_i^j=u_i^{j-1}+\delta {\hat{u}}_i^{\ast }$
   更新状态$x_{i+1}^j=f(x_i^j,u_i^j)$
3. 满足$\left|\frac{J^{(j+1)}-J^{(j)}}{J^{(j)}}\right|<ϵ$或者迭代达到最大轮次终止迭代。

二. ilqr实践代码

参考这位博主： 最优化理论与自动驾驶（十一）：基于iLQR的自动驾驶轨迹跟踪算法（c++和python版本）