ilqr算法原理推导及代码实践

一. 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}{r}{\hat{J}}_i(\end{array}$$