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=1}^{N-1}\ell(x_k,u_k)\tag{1}$

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

状态转移方程（State Transition Equation）：
$x_{k+1}=f(x_k,u_k)\tag{2}$

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

$\begin{aligned} \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{aligned} \tag{3}$

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

$\begin{aligned} \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{aligned} \tag{4}$

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

$\begin{aligned} \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{aligned} \tag{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{aligned} 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{aligned} \tag{6}$

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

$\begin{aligned} \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^TP_i\delta x_i \end{aligned} \tag{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{aligned} \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^TQ_{x_i^2}\delta x_i+\frac{1}{2}\delta u_i^TQ_{u_i^2}\delta u_i\\ &+\frac{1}{2}\delta x_i^TQ_{x_iu_i}\delta u_i+\frac{1}{2}\delta u_i^TQ_{u_ix_i}^T\delta x_i \end{aligned} \tag{8}$

于是有：

$\begin{aligned} \delta \tilde{J}_i(x_i,u_i)&=\frac{1}{2} \begin{bmatrix} \delta x_i \\ \delta u_i \end{bmatrix}^T \begin{bmatrix} Q_{x_i^2} & Q_{x_iu_i}\\ Q_{u_ix_i} & Q_{u_i^2} \end{bmatrix} \begin{bmatrix} \delta x_i \\ \delta u_i \end{bmatrix}+ \begin{bmatrix} Q_{x_i} \\ Q_{u_i} \end{bmatrix}^T \begin{bmatrix} \delta x_i \\ \delta u_i \end{bmatrix}\\ \end{aligned} \tag{9}$

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

$\begin{aligned} \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)\\ &=\underbrace{\ell(x_i+\delta x_i,u_i+\delta u_i)}_{10.1}+\underbrace{\hat J_{i+1}(f(x_i+\delta x_i,u_i+\delta u_i))}_{10.2}\\ \end{aligned} \tag{10}$

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

$\begin{aligned} \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}\delta x_i+\ell_{u_i}\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_iu_i}\delta u_i+\frac{1}{2}\delta u_i^T\ell_{u_ix_i}\delta x_i\\ \end{aligned} \tag{10.1}$

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

$\begin{aligned} \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)^TP_{i+1}(A_i\delta x_i+B_i\delta u_i)\\ &=\hat J_{i+1}(f(x_i,u_i))+p_{i+1}^TA_i\delta x_i+p_{i+1}^TB_i\delta u_i\\ &+\frac{1}{2}\delta x_i^TA_i^TP_{i+1}A_i\delta x_i+\frac{1}{2}\delta x_i^TA_i^TP_{i+1}B_i\delta u_i\\ &+\frac{1}{2}\delta u_i^TB_i^TP_{i+1}A_i\delta x_i+\frac{1}{2}\delta u_i^TB_i^TP_{i+1}B_i\delta u_i\\ \end{aligned} \tag{10.2}$

综上我们分别得到 $Q_{x_i}$ ， $Q_{u_i}$ ， $Q_{x_i^2}$ ， $Q_{x_iu_i}$ ， $Q_{u_ix_i}$ ， $Q_{u_i^2}$ ：

$\begin{aligned} Q_{x_i}&=\ell_{x_i}+p_{i+1}^TA_i\\ Q_{u_i}&=\ell_{u_i}+p_{i+1}^TB_i\\ Q_{x_i^2}&=\ell_{x_i^2}+A_i^TP_{i+1}A_i\\ Q_{x_iu_i}&=\ell_{x_iu_i}+A_i^TP_{i+1}B_i\\ Q_{u_ix_i}&=\ell_{u_ix_i}+B_i^TP_{i+1}A_i\\ Q_{u_i^2}&=\ell_{u_i^2}+B_i^TP_{i+1}B_i\\ \end{aligned} \tag{11}$

反向传播

根据控制系统的控制目标，我们需要对系统的状态进行反向传播，即从系统的最终状态 $x_N$ 开始，反向传播到系统的初始状态 $x_0$ ，我们首先对 $J_N$ 进行泰勒展开，根据泰勒展开的公式：

$\begin{aligned} J_N(x_N+\delta x_N)&=J_N(x_N)+\delta J_N(x_N)\\ &\approx J_N(x_N)+\frac{\partial J_N(x_N)}{\partial x_N}\delta x_N+\frac{1}{2}\delta x_N^T\frac{\partial^2 J_N(x_N)}{\partial x_N^2}\delta x_N\\ &=J_N(x_N)+\ell_f(x_N)+p_N^T\delta x_N+\frac{1}{2}\delta x_N^TP_N\delta x_N\\ \end{aligned} \tag{12}$

即：

$\begin{aligned} p_N^T&=\frac{\partial J_N(x_N)}{\partial x_N}\\ P_N&=\frac{\partial^2 J_N(x_N)}{\partial x_N^2}\\ \end{aligned} \tag{13}$

如果终末代价函数 $\hat J(x_N)$ 是如下形式：

$\begin{aligned} \hat J(x_N)&=\ell_f(x_N)\\ &=\frac{1}{2}(x_N-x_f)^TQ_f(x_N-x_f)\\ \end{aligned} \tag{14}$

则有：

$\begin{aligned} p_N^T&=\frac{\partial J_N(x_N)}{\partial x_N}\\ &=Q_f(x_N-x_f)\\ P_N&=\frac{\partial^2 J_N(x_N)}{\partial x_N^2}\\ &=Q_f\\ \end{aligned} \tag{15}$

现在我们得到了轨迹尾部的 $p$ 和 $P$ ，下面我们来计算他们的递推关系，根据公式（5）和公式（8），有：

$\begin{aligned} \delta \hat J_i(x_i) &= \min_{\delta u_i}\delta \tilde J_i(x_i,u_i)\\ &= \min_{\delta u_i}[Q_{x_i}^T\delta x_i+Q_{u_i}^T\delta u_i+\frac{1}{2}\delta x_i^TQ_{x_i^2}\delta x_i+\frac{1}{2}\delta u_i^TQ_{u_i^2}\delta u_i\\ &+\frac{1}{2}\delta x_i^TQ_{x_iu_i}\delta u_i+\frac{1}{2}\delta u_i^TQ_{u_ix_i}^T\delta x_i] \end{aligned} \tag{16}$

为了使得 $\delta \tilde J_i(x_i)$ 最小，我们需要对 $\delta \tilde J_i(x_i)$ 求导，令导数为0，有：

$\begin{aligned} \frac{\partial \delta \tilde J_i(x_i)}{\partial \delta u_i}&=Q_{u_i}+\frac{1}{2}Q_{u_ix_i}^T\delta x_i+Q_{u_i^2}\delta u_i+ \frac{1}{2} Q_{x_iu_i}\delta x_i\\ &=Q_{u_i}+Q_{u_ix_i}^T\delta x_i+Q_{u_i^2}\delta u_i = 0\\ \end{aligned} \tag{17}$

解上述方程，有：

$\begin{aligned} \delta \hat u_i &= -Q_{u_i^2}^{-1}(Q_{u_i}+Q_{u_ix_i}^T\delta x_i)\\ &\triangleq K_i\delta x_i + d_i\\ \end{aligned} \tag{18}$

即：

$\begin{aligned} K_i &= -Q_{u_i^2}^{-1}Q_{u_ix_i}^T\\ d_i &= -Q_{u_i^2}^{-1}Q_{u_i}\\ \end{aligned} \tag{19}$

将上述公式（18）代入公式（9），并展开，有：

$\begin{aligned} \delta \tilde J_i(x_i,u_i)&=\frac{1}{2} \begin{bmatrix} \delta x_i \\ K_i\delta x_i + d_i \end{bmatrix}^T \begin{bmatrix} Q_{x_i^2} & Q_{x_iu_i}\\ Q_{u_ix_i} & Q_{u_i^2} \end{bmatrix} \begin{bmatrix} \delta x_i \\ K_i\delta x_i + d_i \end{bmatrix}+ \begin{bmatrix} Q_{x_i} \\ Q_{u_i} \end{bmatrix}^T \begin{bmatrix} \delta x_i \\ K_i\delta x_i + d_i \end{bmatrix}\\ &=\frac{1}{2} \delta x_i^TQ_{x_i^2}\delta x_i+\frac{1}{2}(K_i\delta x_i+d_i)^TQ_{u_i^2}(K_i\delta x_i+d_i)\\ &+\frac{1}{2}\delta x_i^TQ_{x_iu_i}(K_i\delta x_i+d_i)+\frac{1}{2}(K_i\delta x_i+d_i)^TQ_{u_ix_i}\delta x_i\\ &+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}+K_i^TQ_{u_i^2}K_i+Q_{x_iu_i}K_i+K_i^TQ_{u_ix_i}]\delta x_i\\ &+[Q_{x_i}+K_i^TQ_{u_i^2}d_i+Q_{x_iu_i}d_i+K_i^TQ_u]^T\delta x_i\\ &+\frac{1}{2}d_i^TQ_{u_i^2}d_i+Q_{u_i}^Td_i\\ \end{aligned} \tag{20}$

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

$\begin{aligned} p_i&=Q_{x_i}+K_i^TQ_{u_i^2}d_i+Q_{x_iu_i}d_i+K_i^TQ_u\\ P_i&=Q_{x_i^2}+K_i^TQ_{u_i^2}K_i+Q_{x_iu_i}K_i+K_i^TQ_{u_ix_i}\\ \Delta \hat J_i&=\frac{1}{2}d_i^TQ_{u_i^2}d_i+Q_{u_i}^Td_i\\ \end{aligned} \tag{21}$

上边的 $p_i$ 是 $\delta \hat J_i$ 的关于 $\delta x_i$ 的一阶项系数， $P_i$ 是 $\delta \hat J_i$ 的关于 $\delta x_i$ 的二阶项系数， $\Delta \hat J_i$ 是 $\delta \hat J_i$ 的常数项，论文中将 $\Delta \hat J_i$ 称为“代价的期望变化（expected change in cost）”，后边我们会将每一个时间步长的“代价的期望变化”相加，得到总的“代价的期望变化”，它将用来判断“前向传播”中每一次迭代是否满足线性搜索（Line Search）的条件，我对这个地方的处理有些困惑。 $\Delta \hat J_i$ 作为一个与 $\delta x_i$ 无关的常数，它不会影响后续对于 $p_i$ 和 $P_i$ 及其之前的值的递推。

正向传播

现在我们已经计算了每个时间步长的最佳反馈增益，现在我们通过模拟动力学来更新标称轨迹。由于初始状态是固定的，整个正向模拟可以通过以下公式表示：

$\begin{aligned} \delta x_i &= \overline x_k - x_k\\ \delta u_i &= K_i\delta x_i + \alpha d_i\\ \overline{u}_i &= u_i + \delta u_i\\ \overline{x}_{i+1} &= f(\overline{x}_i,\overline{u}_i)\\ \end{aligned} \tag{22}$

其中 $\overline x_i$ 和 $\overline u_i$ 是更新之后名义轨迹， $0 \le\alpha\le1$ 是一个对于前馈项的缩放因子。

与所有非线性优化一样，需要沿着下降方向进行线路搜索，以确保足够的代价的降低，我们使用参数α采用简单的回溯线搜索。在应用公式（22）以获取候选状态和控制轨迹之后，我们计算了代价的实际减少与预期减少的比率：

$z=\frac{\Delta J}{\Delta \hat J(\alpha)}=\frac{J(\overline x_0,\overline U)-J(x_0,U)}{\sum_{i=0}^{N-1}\Delta \hat J_i(\alpha)} \tag{23}$

其中：

$\begin{aligned} \Delta \hat J(\alpha) &= \sum_{i=0}^{N-1}\Delta \hat J_i(\alpha)\\ &=\sum_{i=0}^{N-1}[\alpha^2\frac{1}{2}d_i^TQ_{u_i^2}d_i+\alpha Q_{u_i}^Td_i]\\ \end{aligned} \tag{24}$

其中 $\Delta \hat J_i(\alpha)$ 是公式（21）中的 $\Delta \hat J_i$ 的被 $\alpha$ 参数化之后的结果。 $\Delta \hat J(\alpha)$ 可以在“前向传播”时，通过分别存储公式（24）中的两项然后将它们和 $\alpha$ 和 $\alpha^2$ 相乘来计算高效的计算。

如果 $z$ 在 $[\beta_1,\beta_2]$ 之间，通常是 $[1e-4,10]$ ，则我们接受这个候选轨迹，否则我们将 $\alpha$ 缩小到 $\gamma\alpha$ ，其中 $0<\gamma<1$ ，通常是 $\gamma=0.5$ 。我们重复这个过程，直到 $z$ 在 $[\beta_1,\beta_2]$ 之间。

我们可以考察 $\alpha=0$ 这个极限情况，根据公式（22），有：
$\begin{aligned} \delta x_0 &= \overline x_0 - x_0\\ \delta u_0 &= K_i\delta x_0 + \alpha d_i\\ \end{aligned}$

因为 $\alpha=0$ 且 $\overline x_0 = x_0$ ，所以 $\delta u_0=0$ ，根据公式（22）， $\overline u_0$ 相比于 $u_0$ 没有任何变化，相应的 $\overline x_1$ 相对于 $x_1$ 也没有变化，因此在这个情况下整条轨迹都不会有任何变动。相反，如果 $\alpha > 0$ ，那么整个轨迹会根据 $\alpha$ 的值而有所变化， $\alpha$ 的大小决定了变化的程度。如果按照这个逻辑，我们可以这样理解公式（23）中的 $z$ ：其中 $\Delta \hat J(\alpha)$ 是将代价函数线性化之后的期望的代价变化， $\Delta J$ 则表示轨迹更新之后代价的实际变化，如果我们选取的 $\alpha$ 没有使得更新的轨迹偏离线性化的位置过多，那么 $\Delta J$ 和 $\Delta \hat J(\alpha)$ 的比值不会太大，即 $z$ 的值不会太大，反之，如果我们选取的 $\alpha$ 使得更新的轨迹偏离线性化的位置过多，那么 $\Delta J$ 和 $\Delta \hat J(\alpha)$ 的比值会很大，即 $z$ 的值会很大，这样我们就可以通过 $z$ 的值来判断我们选取的 $\alpha$ 是否合适。

正则化（Regularization）

由于数值计算精度的问题， $Q_{u_i^2}$ 可能会不满足正定条件，因此我们需要对 $Q_{u_i^2}$ 进行正则化，有以下两种正则化的方法：

方法1：

根据公式（11）：

$\begin{aligned} \overline{Q}_{u_i^2} &= Q_{u_i^2}+\rho I\\ &= \ell_{u_i^2}+B_i^TP_{i+1}B_i+\rho I\\ \\ d_i &= -\overline{Q}_{u_i^2}^{-1}Q_{u_i}\\ K_i &= -\overline{Q}_{u_i^2}^{-1}Q_{u_ix_i}^T\\ \\ \Delta \hat J_i &= \frac{1}{2}d_i^TQ_{u_i^2}d_i+Q_{u_i}^Td_i\\ p_i &= Q_{x_i}+K_i^TQ_{u_i^2}d_i+Q_{x_iu_i}d_i+K_i^TQ_{u_i}\\ P_i &= Q_{x_i^2}+K_i^TQ_{u_i^2}K_i+Q_{x_iu_i}K_i+K_i^TQ_{u_ix_i}\\ \end{aligned} \tag{25}$

方法2：

根据公式（11）：

$\begin{aligned} \overline{Q}_{u_i^2} &= \ell_{u_i^2}+B_i^T(P_{i+1}+\rho I)B_i\\ \overline{Q}_{u_ix_i} &= \ell_{u_ix_i}+B_i^T(P_{i+1}+\rho I)A_i\\ \\ d_i &= -\overline{Q}_{u_i^2}^{-1}Q_{u_i}\\ K_i &= -\overline{Q}_{u_i^2}^{-1}Q_{u_ix_i}^T\\ \\ \Delta \hat J_i &= \frac{1}{2}d_i^TQ_{u_i^2}d_i+Q_{u_i}^Td_i\\ p_i &= Q_{x_i}+K_i^TQ_{u_i^2}d_i+Q_{x_iu_i}d_i+K_i^TQ_{u_i}\\ P_i &= Q_{x_i^2}+K_i^TQ_{u_i^2}K_i+Q_{x_iu_i}K_i+K_i^TQ_{u_ix_i}\\ \end{aligned} \tag{26}$

注意 $Q_{u_i^2}$ 和 $\overline{Q}_{u_i^2}$ （以及 $Q_{u_ix_i}$ 和 $\overline{Q}_{u_ix_i}$ ）在使用上的细微区别，前者被用来计算期望的代价变化（ $\Delta \hat J_i$ ）和向后传播的递归，正则化之后的后者被用来计算反馈增益（ $K_i$ ）和前馈项（ $d_i$ ）。

约束（Constraints）

要处理约束，需要使用增广拉格朗日法（Augmented Lagrange Method） 对原有的代价 $J(x_0,U)$ 进行增广，在这里暂时不做讨论，后边会有专门的文章来讨论增广拉格朗日法在iLQR中的应用。即AL-iLQR算法。

终止条件

iLQR一般具有三种终止条件：

代价函数的变化量小于某个阈值，即 $|J_{pre}-J|<\epsilon$ 。
前馈增益项的变化量小于某个阈值，即 $|d_{pre}-d|<\epsilon$ 。
迭代次数达到某个阈值。

到此为止，我们已经完成了对于iLQR算法的讨论。

最后编辑于：2023.10.12 00:13:39

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 215,794评论 6赞 498
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 92,050评论 3赞 391
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 161,587评论 0赞 351
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 57,861评论 1赞 290
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 66,901评论 6赞 388
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 50,898评论 1赞 295
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 39,832评论 3赞 416
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 38,617评论 0赞 271
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 45,077评论 1赞 308
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 37,349评论 2赞 331
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 39,483评论 1赞 345
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 35,199评论 5赞 341
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 40,824评论 3赞 325
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 31,442评论 0赞 21
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 32,632评论 1赞 268
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 47,474评论 2赞 368
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 44,393评论 2赞 352