背景
在之前的文章《离散系统的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)\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}
\]
我们有必要注意一下公式（6）中对于状态转移函数\(f\)的泰勒展开，会发现它只保留了一阶项，这一点是iLQR算法和DDP（differential dynamic programming）算法的一个主要区别，DDP算法会计算全二阶展开，应该在计算上会更加复杂。
反向传播
根据控制系统的控制目标，我们需要对系统的状态进行反向传播，即从系统的最终状态\(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\)及其之前的值的递推。
额外说明：观察公式（21）和公式（19），如果将\(d_i\)代入\(\Delta {\hat J}_i\)到中会得到一个它的更简洁的形式：
\[\begin{aligned}
\Delta \hat J_i&=\frac{1}{2}d_i^TQ_{u_i^2}d_i+Q_{u_i}^Td_i\\
&=\frac{1}{2}(-Q_{u_i^2}^{-1}Q_{u_i})^TQ_{u_i^2}(-Q_{u_i^2}^{-1}Q_{u_i})+Q_{u_i}^T(-Q_{u_i^2}^{-1}Q_{u_i})\\
&=\frac{1}{2}Q_{u_i}^TQ_{u_i^2}^{-1}Q_{u_i}-Q_{u_i}^TQ_{u_i^2}^{-1}Q_{u_i}\\
&=-\frac{1}{2}Q_{u_i}^TQ_{u_i^2}^{-1}Q_{u_i}\\
\end{aligned}
\tag{21.1}
\]
不过我们我们不使用这个形式，因为在后续的“前向传播”过程中的LineSearch过程会通过一个scale参数来调整\(d_i\)，因此我们把每一个点的\(\Delta \hat J_i\)都写成一个关于\(d_i\)的形式，并且分别存储它的一次项和二次项的系数，即\(Q_{u_i}\)和\(Q_{u_i^2}\)，这样在调整scale参数的时候，我们只需要对这两个系数进行scale就可以了。
正向传播
现在我们已经计算了每个时间步长的最佳反馈增益，现在我们通过模拟动力学来更新标称轨迹。由于初始状态是固定的，整个正向模拟可以通过以下公式表示：
\[\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算法的讨论。
未经授权，禁止转载
浙公网安备 33010602011771号