2023-10-12-iLQR算法公式推导（一）

iLQR算法公式推导

背景

在之前的文章《离散系统的LQR推导》中，我们讨论了LQR在线性系统中的应用，我们讨论了该算法在线性时变系统中的一般情况，也讨论了在线性时不变系统中的特殊情况；以及**有限时域（Finite Horizon）和无限时域（Infinite Horizon）**这两种情况。不过在实际的控制系统中，大部分系统的模型都是非线性的，传统的LQR只能在系统的当前状态下对系统进行线性化近似，这种近似并不能保证基于线性假设得到的最优控制率实际上真的是最优的。

由于传统的LQR存在上述局限性，有人对它进行了改进，提出了iLQR算法，它的全称是迭代线性二次调节器（iterative Linear Quadratic Regulator），下面我们对该算法进行详细介绍。

算法概览

iLQR算法的关键思想是，在每次迭代中，所有非线性约束和目标都使用一阶或二阶泰勒级数展开来近似，因此，现在对标称轨迹的偏差进行操作的近似函数可以使用离散LQR来求解。最优反馈控制策略在“反向传播”阶段计算，因为和LQR算法一样，动态规划（Dynamic Programming）的步骤是从轨迹的尾部开始。然后在“正向传播”期间将“反向传播”期间得到的最优控制策略产生的控制输入偏差应用于标称轨迹的控制输入量，并使用更新之后的输入量来正向模拟更新轨迹（rollout）。这个过程在每次迭代中重复，直到收敛为止。

非线性系统优化问题

考虑下述非线性系统的优化问题：

代价（Cost）：

$$J(x_0,U)={\ell }_f(x_N)+\sum _{k=0}^{N-1}\ell (x_k,u_k)\text{\hspace{1em}(1)}$$

其中 $U=[u_0^T,\cdots ,u_{N-1}^T{]}^T$ ,

状态转移方程（State Transition Equation）：

$$x_{k+1}=f(x_k,u_k)\text{\hspace{1em}(2)}$$

根据《离散系统的LQR推导》的中的公式（8）中的相关推导，我们可以得到cost to go：

$$\begin{array}{rl}{\hat{J}}_i& =\underset{u_i}{\min }\{\ell (x_i,u_i)+{\hat{J}}_{i+1}\}\\ {\hat{J}}_N& =J_N={\ell }_f(x_N)\end{array}\text{\hspace{1em}(3)}$$

根据上述公式，我们可以发现 ${\hat{J}}_N$ 是关于 $x_N$ 的函数，即 ${\hat{J}}_N(x_N)={\ell }_N(x_N)$ ，下面再看 $J_{N-1}$ ：

$$\begin{array}{rl}{\hat{J}}_{N-1}& =\underset{u_{N-1}}{\min }\{\ell (x_{N-1},u_{N-1})+{\hat{J}}_N(x_N)\}\\ & =\underset{u_{N-1}}{\min }\{\ell (x_{N-1},u_{N-1})+{\hat{J}}_N(f(x_{N-1},u_{N-1}))\}\\ & =\underset{u_{N-1}}{\min }{\tilde{J}}_{N-1}(x_{N-1},u_{N-1})\\ & ={\hat{J}}_{N-1}(x_{N-1})\end{array}\text{\hspace{1em}(4)}$$

根据上式，可知 $\hat{J}(x_{N-1})$ 是关于 $x_{N-1}$ 的函数，因为在对 $u_{N-1}$ 做优化之后，会得到一个最优控制率 ${\underset{^}{u}}_{N-1}(x_{N-1})$ 是关于， $x_{N-1}$ 的函数。如果我们将最优控制率 ${\underset{^}{u}}_{N-1}(x_{N-1})$ 代入到公式（4），得到的最优代价 ${\hat{J}}_{N-1}(x_{N-1})$ 也是 $x_{N-1}$ 的函数，因此根据公式（3）的递归，可知：

$$\begin{array}{rl}{\hat{J}}_i& =\underset{u_i}{\min }\{\ell (x_i,u_i)+{\hat{J}}_{i+1}\}\\ & =\underset{u_i}{\min }\tilde{J}(x_i,u_i)\\ & ={\hat{J}}_i(x_i)\end{array}\text{\hspace{1em}(5)}$$

即第 $i$ 个最优代价（cost to go） ${\hat{J}}_i(x_i)$ 是 $x_i$ 的函数，还未对 $u_i$ 进行优化的代价 $\tilde{J}(x_i,u_i)$ 是 $x_i$ 和 $u_i$ 的函数。

使用泰勒展开将系统局部线性化

为了线性化非线性系统以及非线性代价函数，我们需要对 $f(x_k,u_k)$ 、 ${\hat{J}}_i(x_i)$ 和 $\tilde{J}(x_i,u_i)$ 进行泰勒展开，我们首先对 $f(x_k,u_k)$ 进行泰勒展开，根据泰勒展开的公式：

$$\begin{array}{rl}f(x_i,u_i)+\delta f(x_i,u_i)& =f(x_i+\delta x_i,u_i+\delta u_i)\\ & \approx f(x_i,u_i)+\frac{\partial f(x_i,u_i)}{\partial x_i}\delta x_i+\frac{\partial f(x_i,u_i)}{\partial u_i}\delta u_i\\ & \triangleq f(x_i,u_i)+A_i\delta x_i+B_i\delta u_i\end{array}\text{\hspace{1em}(6)}$$

下面对 ${\hat{J}}_i(x_i)$ 进行泰勒展开，根据泰勒展开的公式：

$$\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}(7)}$$

上式定义 $p_i^T\triangleq \frac{\partial {\hat{J}}_i(x_i)}{\partial x_i}$ ， $P_i\triangleq \frac{{\partial }^2{\hat{J}}_i(x_i)}{\partial x_i^2}$ ，下面对 ${\tilde{J}}_i(x_i,u_i)$ 进行泰勒展开，根据泰勒展开的公式：

$$\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)\\ & \approx {\tilde{J}}_i(x_i,u_i)+\frac{\partial {\tilde{J}}_i(x_i,u_i)}{\partial x_i}\delta x_i+\frac{\partial {\tilde{J}}_i(x_i,u_i)}{\partial u_i}\delta u_i\\ & +\frac{1}{2}\delta x_i^T\frac{{\partial }^2{\tilde{J}}_i(x_i,u_i)}{\partial x_i^2}\delta x_i+\frac{1}{2}\delta u_i^T\frac{{\partial }^2{\tilde{J}}_i(x_i,u_i)}{\partial u_i^2}\delta u_i\\ & +\frac{1}{2}\delta x_i^T\frac{{\partial }^2{\tilde{J}}_i(x_i,u_i)}{\partial x_i\partial u_i}\delta u_i+\frac{1}{2}\delta u_i^T\frac{{\partial }^2{\tilde{J}}_i(x_i,u_i)}{\partial u_i\partial x_i}\delta x_i\\ & \triangleq {\tilde{J}}_i(x_i,u_i)+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}\text{\hspace{1em}(8)}$$

于是有：

$$\begin{array}{rl}\delta {\tilde{J}}_i(x_i,u_i)& =\frac{1}{2}{\left[\begin{array}{c}\delta x_i\\ \delta u_i\end{array}\right]}^T\left[\begin{array}{cc}{Q}_{x_i^2}& Q_{x_i{u}_i}\\ Q_{u_i{x}_i}& Q_{u_i^2}\end{array}\right]\left[\begin{array}{c}\delta x_i\\ \delta u_i\end{array}\right]+{\left[\begin{array}{c}{Q}_{x_i}\\ Q_{u_i}\end{array}\right]}^T\left[\begin{array}{c}\delta x_i\\ \delta u_i\end{array}\right]\end{array}\text{\hspace{1em}(9)}$$

根据公式（5）的定义，有：

$$\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)\\ & =\underset{10.1}{\ell (x_i+\delta x_i,u_i+\delta u_i)}+\underset{10.2}{{\hat{J}}_{i+1}(f(x_i+\delta x_i,u_i+\delta u_i))}\end{array}\text{\hspace{1em}(10)}$$

首先对上述的公式（10.1）进行泰勒展开，根据泰勒展开的公式：

$$\begin{array}{rl}\ell (x_i+\delta x_i,u_i+\delta u_i)& =\ell (x_i,u_i)+\delta \ell (x_i,u_i)\\ & \approx \ell (x_i,u_i)+\frac{\partial \ell (x_i,u_i)}{\partial x_i}\delta x_i+\frac{\partial \ell (x_i,u_i)}{\partial u_i}\delta u_i\\ & +\frac{1}{2}\delta x_i^T\frac{{\partial }^2\ell (x_i,u_i)}{\partial x_i^2}\delta x_i+\frac{1}{2}\delta u_i^T\frac{{\partial }^2\ell (x_i,u_i)}{\partial u_i^2}\delta u_i\\ & +\frac{1}{2}\delta x_i^T\frac{{\partial }^2\ell (x_i,u_i)}{\partial x_i\partial u_i}\delta u_i+\frac{1}{2}\delta u_i^T\frac{{\partial }^2\ell (x_i,u_i)}{\partial u_i\partial x_i}\delta x_i\\ & \triangleq \ell (x_i,u_i)+{\ell }_{x_i}^T\delta x_i+{\ell }_{u_i}^T\delta u_i+\frac{1}{2}\delta x_i^T{\ell }_{x_i^2}\delta x_i+\frac{1}{2}\delta u_i^T{\ell }_{u_i^2}\delta u_i\\ & +\frac{1}{2}\delta x_i^T{\ell }_{x_i{u}_i}\delta u_i+\frac{1}{2}\delta u_i^T{\ell }_{u_i{x}_i}\delta x_i\end{array}\text{\hspace{1em}(10.1)}$$

然后对公式（9.2）进行泰勒展开，根据泰勒展开的公式：

$$\begin{array}{rl}{\hat{J}}_{i+1}(f(x_i+\delta x_i,u_i+\delta u_i))& ={\hat{J}}_{i+1}(f(x_i,u_i)+\delta f(x_i,u_i))\\ & \approx {\hat{J}}_{i+1}(f(x_i,u_i))+\frac{\partial {\hat{J}}_{i+1}(x_{i+1})}{\partial x_{i+1}}\delta f(x_i,u_i)\\ & +\frac{1}{2}\delta f(x_i,u_i{)}^T\frac{{\partial }^2{\hat{J}}_{i+1}(x_{i+1})}{\partial x_{i+1}^2}\delta f(x_i,u_i)\\ & ={\hat{J}}_{i+1}(f(x_i,u_i))+p_{i+1}^T(A_i\delta x_i+B_i\delta u_i)\\ & +\frac{1}{2}(A_i\delta x_i+B_i\delta u_i{)}^T{P}_{i+1}(A_i\delta x_i+B_i\delta u_i)\\ & ={\hat{J}}_{i+1}(f(x_i,u_i))+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}\text{\hspace{1em}(10.2)}$$

综上我们分别得到 $Q_{x_i}$ ， $Q_{u_i}$ ， $Q_{x_i^2}$ ， $Q_{x_i{u}_i}$ ， $Q_{u_i{x}_i}$ ， $Q_{u_i^2}$ ：

$$\begin{array}{rl}{Q}_{x_i}& ={\ell }_{x_i}+A_i^T{p}_{i+1}\\ Q_{u_i}& ={\ell }_{u_i}+B_i^T{p}_{i+1}\\ Q_{x_i^2}& ={\ell }_{x_i^2}+A_i^T{P}_{i+1}{A}_i\\ Q_{x_i{u}_i}& ={\ell }_{x_i{u}_i}+A_i^T{P}_{i+1}{B}_i\\ Q_{u_i{x}_i}& ={\ell }_{u_i{x}_i}+B_i^T{P}_{i+1}{A}_i\\ Q_{u_i^2}& ={\ell }_{u_i^2}+B_i^T{P}_{i+1}{B}_i\end{array}\text{\hspace{1em}(11)}$$

我们有必要注意一下公式（6）中对于状态转移函数$f$的泰勒展开，会发现它只保留了一阶项，这一点是iLQR算法和DDP（differential dynamic programming）算法的一个主要区别，DDP算法会计算全二阶展开，应该在计算上会更加复杂。

（未完待续）