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

（接上文）

反向传播

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

$$\begin{array}{rl}{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\\ & ={\ell }_f(x_N)+p_N^T\delta x_N+\frac{1}{2}\delta x_N^T{P}_N\delta x_N\end{array}\text{\hspace{1em}(12)}$$

即：

$$\begin{array}{rl}{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{array}\text{\hspace{1em}(13)}$$

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

$$\begin{array}{rl}\hat{J}(x_N)& ={\ell }_f(x_N)\\ & =\frac{1}{2}(x_N-x_f{)}^T{Q}_f(x_N-x_f)\end{array}\text{\hspace{1em}(14)}$$

则有：

$$\begin{array}{rl}{p}_N^T& =\frac{\partial J_N(x_N)}{\partial x_N}\\ & =(x_N-x_f{)}^T{Q}_f\\ P_N& =\frac{{\partial }^2{J}_N(x_N)}{\partial x_N^2}\\ & =Q_f\end{array}\text{\hspace{1em}(15)}$$

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

$$\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]\end{array}\text{\hspace{1em}(16)}$$

为了使得 $\delta {\tilde{J}}_i(x_i)$ 最小，我们需要对 $\delta {\tilde{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}^T\delta x_i+Q_{u_i^2}\delta u_i=0\end{array}\text{\hspace{1em}(17)}$$

解上述方程，有：

$$\begin{array}{rl}\delta {\hat{u}}_i& =-Q_{u_i^2}^{-1}(Q_{u_i}+Q_{u_i{x}_i}^T\delta x_i)\\ & \triangleq K_i\delta x_i+d_i\end{array}\text{\hspace{1em}(18)}$$

即：

$$\begin{array}{rl}{K}_i& =-Q_{u_i^2}^{-1}{Q}_{u_i{x}_i}\\ d_i& =-Q_{u_i^2}^{-1}{Q}_{u_i}\end{array}\text{\hspace{1em}(19)}$$

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

$$\begin{array}{rl}\delta {\tilde{J}}_i(x_i,u_i)& =\frac{1}{2}{\left[\begin{array}{c}\delta x_i\\ K_i\delta x_i+d_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\\ K_i\delta x_i+d_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\\ K_i\delta x_i+d_i\end{array}\right]\\ & =\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\\ & +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^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{]}^T\delta x_i\\ & +\frac{1}{2}{d}_i^T{Q}_{u_i^2}{d}_i+Q_{u_i}^T{d}_i\end{array}\text{\hspace{1em}(20)}$$

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

$$\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}\text{\hspace{1em}(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{array}{rl}\Delta {\hat{J}}_i& =\frac{1}{2}{d}_i^T{Q}_{u_i^2}{d}_i+Q_{u_i}^T{d}_i\\ & =\frac{1}{2}(-Q_{u_i^2}^{-1}{Q}_{u_i}{)}^T{Q}_{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}^T{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}^T{Q}_{u_i^2}^{-1}{Q}_{u_i}\end{array}\text{\hspace{1em}(21.1)}$$

不过我们我们不使用这个形式，因为在后续的“前向传播”过程中的LineSearch过程会通过一个scale参数来调整$d_i$，因此我们把每一个点的 $\Delta {\hat{J}}_i$ 都写成一个关于 $d_i$ 的形式，并且分别存储它的一次项和二次项的系数，即 $Q_{u_i}$ 和 $Q_{u_i^2}$ ，这样在调整scale参数的时候，我们只需要对这两个系数进行scale就可以了。

正向传播

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

$$\begin{array}{rl}\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{array}\text{\hspace{1em}(22)}$$

其中 ${\overline{x}}_i$ 和 ${\overline{u}}_i$ 是更新之后名义轨迹， $0\le \alpha \le 1$ 是一个对于前馈项的缩放因子。

与所有非线性优化一样，需要沿着下降方向进行线路搜索，以确保足够的代价的降低，我们使用参数α采用简单的回溯线搜索。在应用公式（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 )}\text{\hspace{1em}(23)}$$

其中：

$$\begin{array}{rl}\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^T{Q}_{u_i^2}{d}_i+\alpha Q_{u_i}^T{d}_i]\end{array}\text{\hspace{1em}(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{array}{rl}\delta x_0& ={\overline{x}}_0-x_0\\ \delta u_0& =K_i\delta x_0+\alpha d_i\end{array}$$

因为 $\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{array}{rl}{\overline{Q}}_{u_i^2}& =Q_{u_i^2}+\rho I\\ & ={\ell }_{u_i^2}+B_i^T{P}_{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_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\\ 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_i}\\ 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}\end{array}\text{\hspace{1em}(25)}$$

方法2：

根据公式（11）：

$$\begin{array}{rl}{\overline{Q}}_{u_i^2}& ={\ell }_{u_i^2}+B_i^T(P_{i+1}+\rho I)B_i\\ {\overline{Q}}_{u_i{x}_i}& ={\ell }_{u_i{x}_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}{\overline{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\\ 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_i}\\ 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}\end{array}\text{\hspace{1em}(26)}$$

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

约束（Constraints）

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

终止条件

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

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

---

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