从LQR到iLQR的简明易懂过程（一） - 指南

文章目录

• LQR基础
• • • $k=N-1$时刻的最优控制律$u_{N-1}^{\ast }$
    • Riccati
    • • 完整形式Riccati方程
      • 标准的 离散时间 Riccati 矩阵方程 (DARE)：
• iLQR
• • 如果我们用iLQR去优化轨迹
  • 目标函数的二次化（局部成本）
  • • 运行成本
    • 终端成本
  • iLQR 与 LQR 的线性项差异
  • • LQR (Linear Quadratic Regulator)：纯二次型
    • • A. 零参考和零梯度 (Zero Reference & Zero Gradient)
      • B. 贝尔曼方程的性质：线性项消失
    • iLQR (Iterative LQR)：仿射二次型 (Affine Quadratic)
    • • A. 轨迹偏离原点，存在非零梯度 (Non-Zero Gradient)
      • B. 线性项的引入：驱动优化过程
  • 总结：线性项的作用
  • iLQR的最优目标函数
  • • 递推过程k + 1 ----> k：
    • • 2. 已知信息 (输入)
      • 瞬时成本 $l$的展开 (直接展开)
      • 未来成本 $V_{k+1}$的展开 (链式法则)
      • • $V_{k+1}$的线性项展开
        • $V_{k+1}$的二次项展开
    • 合成 Q 函数系数
    • • 梯度 $\mathbf{q}$ (对 ${\mathbf{\delta x}}_k$ 的线性项)
      • 梯度 $\mathbf{r}$ (对 ${\mathbf{\delta u}}_k$ 的线性项)
      • Hessian$\mathbf{Q}$ (对 ${\mathbf{\delta x}}_k,{\mathbf{\delta x}}_k$ 的二次项)
      • Hessian$\mathbf{R}$ (对 ${\mathbf{\delta u}}_k,{\mathbf{\delta u}}_k$ 的二次项)
      • Hessian$\mathbf{M}$ (对 ${\mathbf{\delta x}}_k,{\mathbf{\delta u}}_k$ 的交叉项)

LQR基础

状态转移：
$$x_{k+1}=A_k{x}_t+B_k{u}_t$$
要求使如下目标函数最优
$$J(x_0,U)=\frac{1}{2}{\Sigma }_{k=1}^{N-1}(x_k^T{Q}_k{x}_k+u_k^T{R}_k{u}_k)+\frac{1}{2}{x}_N^T{Q}_f{x}_N$$

其中三部分为状态成本，控制成本和终点状态成本。

在终端时刻 $N$，剩余成本即为终端惩罚：
$$V_N(x_N)=\frac{1}{2}{x}_N^T{Q}_f{x}_N$$
为便于递推，我们定义：
$${\mathbf{S}}_{\mathbf{N}}={\mathbf{Q}}_{\mathbf{f}}$$
所以：
$$V_N(x_N)=\frac{1}{2}{x}_N^T{S}_N{x}_N$$

$k=N-1$时刻的最优控制律$u_{N-1}^{\ast }$

$$V_N(x_N)=\frac{1}{2}{x}_N^T{Q}_f{x}_N=\frac{1}{2}{x}_N^T{S}_N{x}_N$$

根据贝尔曼最优性原理
$$\begin{array}{rl}{V}_i(x_i)& =\underset{u_i}{\min }{V}_{i+1}(x_{i+1})+\frac{1}{2}{u}_i^T{R}_i{u}_i+\frac{1}{2}{x}_i^T{Q}_i{x}_i\\ & =\underset{u_i}{\min }\frac{1}{2}{u}_i^T{R}_i{u}_i+\frac{1}{2}{x}_i^T{Q}_i{x}_i+\frac{1}{2}(A_i{x}_i+B_i{u}_i{)}^T{S}_{i+1}(A_i{x}_i+B_i{u}_i)\\ & =\underset{u_i}{\min }\frac{1}{2}{u}_i^T{R}_i{u}_i+\frac{1}{2}{x}_i^T{Q}_i{x}_i+\frac{1}{2}{x}_i^T{A}^T{S}_{i+1}A{x}_i+\frac{1}{2}{u}_i^T{B}_i^T{S}_{i+1}{B}_i{u}_i+x_i^T{A}_i^T{S}_{i+1}{B}_i{u}_i\\ & =\underset{u_i}{\min }\frac{1}{2}{u}_i^T(R_i+B_i^T{S}_{i+1}{B}_i)u_i+\frac{1}{2}{x}_i^T(Q_i+A_i^T{S}_{i+1}{A}_i)x_i+u_i^T(B_i^T{S}_{i+1}{A}_i)x_i\end{array}$$

接下来求导
$$\begin{array}{rlr}\frac{\partial V}{\partial u}& =(R_i+B_i^T{S}_{i+1}{B}_i)u_i+(B_i^T{S}_{i+1}{A}_i)x_i& =0\end{array}$$

$$u_i=-(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i)x_i$$
$$u_i=-K_i{x}_i,K_i=(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i)$$

Riccati

代入最优控制

$$\begin{array}{rl}{V}_i(x_i)=& \frac{1}{2}{u}_i^{\ast T}(R_i+B_i^T{S}_{i+1}{B}_i)u_i^{\ast }+\frac{1}{2}{x}_i^T(Q_i+A_i^T{S}_{i+1}{A}_i)x_i+u_i^{\ast T}(B_i^T{S}_{i+1}{A}_i)x_i\\ =& \frac{1}{2}{x}_i^T{K}_i^T(R_i+B_i^T{S}_{i+1}{B}_i)K_i{x}_i+\frac{1}{2}{x}_i^T(Q_i+A_i^T{S}_{i+1}{A}_i)x_i-x_i^T{K}_i^T(B_i^T{S}_{i+1}{A}_i)x_i\\ =& \frac{1}{2}{x}_i^T(K_i^T(R_i+B_i^T{S}_{i+1}{B}_i)K_i+Q_i+A_i^T{S}_{i+1}{A}_i-2K_i^T(B_i^T{S}_{i+1}{A}_i))x_i\end{array}$$

完整形式Riccati方程

$$\begin{array}{rl}{S}_i=& Q_i+A_i^T{S}_{i+1}{A}_i-2K^T(B_i^T{S}_{i+1}{A}_i)+K_i^T{R}_i{K}_i+K_i^T(B_i^T{S}_{i+1}{B}_i)K_i\\ =& Q_i+K_i^T{R}_i{K}_i+(A_i-B_i{K}_i{)}^T{S}_{i+1}(A_i-B_i{K}_i)\end{array}$$

标准的 离散时间 Riccati 矩阵方程 (DARE)：

将Ki代入

$$\begin{array}{rl}{S}_i=& Q_i+K_i^T{R}_i{K}_i+(A_i-B_i{K}_i{)}^T{S}_{i+1}(A_i-B_i{K}_i)\\ =& Q_i+((R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i){)}^T{R}_i(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i)+(A_i-B_i(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i){)}^T{S}_{i+1}(A_i-B_i(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}(B_i^T{S}_{i+1}{A}_i))\\ =& Q_i+A_i^T{S}_{i+1}{A}_i-A_i^T{S}_{i+1}{B}_i(R_i+B_i^T{S}_{i+1}{B}_i{)}^{-1}{B}_i^T{S}_{i+1}{A}_i\end{array}$$

iLQR

状态转移方程变成非线性，即

$$x_{k+1}=f(x_k,u_k)$$

目标函数为

$$J(x_0,U)=h(x_N)+{\Sigma }_{k=0}^{N-1}l(x_k,u_k)$$

如果我们用iLQR去优化轨迹

假设大家有一个粗节轨迹$(\bar{\mathbf{x}},\bar{\mathbf{u}})$，大家希望计算小的扰动$\delta x,\delta u$来改善轨迹。

扰动为

$$\begin{array}{r}\delta x_k=x_k-{\bar{x}}_k\\ \delta u_k=u_k-{\bar{u}}_k\end{array}$$

我们需将非线性泰勒展开，得到线性的局部目标函数及状态转移方程。

对状态转移方程进行展开
$$\begin{array}{rl}& x_{k+1}\approx f({\bar{x}}_k,{\bar{u}}_k)+\frac{\partial f}{\partial x}{|}_{\bar{k}}(x_k-{\bar{x}}_k)+\frac{\partial f}{\partial u}{|}_{\bar{k}}(u_k-{\bar{u}}_k)\\ & A_k=\frac{\partial f}{\partial x}{|}_{\bar{k}}\\ & B_k=\frac{\partial f}{\partial u}{|}_{\bar{k}}\\ & c_k=f({\bar{x}}_k,{\bar{u}}_k)-{\bar{x}}_{k+1}\end{array}$$

$$\delta x_{k+1}=A_k\delta x_k+B_k\delta u_k+c_k$$

目标函数的二次化（局部成本）

$$\delta J=\delta h(x_N)+{\Sigma }_{k=0}^{N-1}\delta l(x_k,u_k)$$

运行成本

对l进行泰勒展开（在粗解的point上）

$$l(x_k,u_k)\approx l({\bar{x}}_k,{\bar{u}}_k)+l_x^T\delta x_k+l_u^T\delta u_k+\frac{1}{2}\delta x_k^T{l}_{xx}\delta x_k+\frac{1}{2}\delta u_k^T{l}_{uu}\delta u_k+\delta x_k^T{l}_{xu}\delta u_k,l({\bar{x}}_k,{\bar{u}}_k)为常数$$

终端成本

$$h(x_N)\approx h({\bar{x}}_N)+h_x^T\delta x_N+\frac{1}{2}\delta x_N^T{h}_{xx}\delta x_N,h({\bar{x}}_N)为常数$$

此时可以发现，与LQR相比，iLQR除了二次型外，还有线性项

iLQR 与 LQR 的线性项差异

LQR (Linear Quadratic Regulator)：纯二次型

核心目标：将状态驱动到原点$\mathbf{x}=0$。

A. 零参考和零梯度 (Zero Reference & Zero Gradient)

LQR 的局部目标函数是纯二次型，且假设最优轨迹是$\mathbf{x}=0,\mathbf{u}=0$。
在原点 $\mathbf{x}=0$处，成本函数对于状态$\mathbf{x}$ 的梯度（一阶导数）总是零：
$$\frac{\partial l}{\partial \mathbf{x}}{|}_{\mathbf{x}=0,\mathbf{u}=0}=0$$

B. 贝尔曼方程的性质：线性项消失

由于成本函数在原点没有梯度，所以值函数$V_k$ 在 $\mathbf{x}=0$附近也没有梯度。
值函数 $V_k$的泰勒展开式为：
$$V_k({\mathbf{x}}_k)\approx V_k(0)+\underset{\text{梯度}}{\underbrace{{\mathbf{v}}_k^T}}{\mathbf{x}}_k+\frac{1}{2}{\mathbf{x}}_k^T{\mathbf{V}}_{\mathbf{xx}}{\mathbf{x}}_k$$
由于 $V_k(0)=0$ 且 ${\mathbf{v}}_k=\frac{\partial V_k}{\partial \mathbf{x}}{|}_{\mathbf{x}=0}=0$，所有线性项都消失了。

LQR 结论： 值函数是纯二次型：
$$V_k({\mathbf{x}}_k)=\frac{1}{2}{\mathbf{x}}_k^T{\mathbf{S}}_k{\mathbf{x}}_k$$

---

iLQR (Iterative LQR)：仿射二次型 (Affine Quadratic)

核心目标： 在一条任意名义轨迹$(\bar{\mathbf{x}},\bar{\mathbf{u}})$附近进行局部优化。

A. 轨迹偏离原点，存在非零梯度 (Non-Zero Gradient)

名义轨迹 $(\bar{\mathbf{x}},\bar{\mathbf{u}})$通常不经过原点，且在优化过程中通常不是最优的。
我们在名义点${\bar{\mathbf{x}}}_k$处计算局部成本$\mathbf{\delta l}$。由于 ${\bar{\mathbf{x}}}_k$最优轨迹，局部成本函数就是不$l(\mathbf{x},\mathbf{u})$ 在 ${\bar{\mathbf{x}}}_k$ 处对 $\mathbf{x}$ 和 $\mathbf{u}$ 的导数不为零：
$${\mathbf{l}}_{\mathbf{x}}=\frac{\partial l}{\partial \mathbf{x}}{|}_{\bar{k}}\ne 0$$
$${\mathbf{l}}_{\mathbf{u}}=\frac{\partial l}{\partial \mathbf{u}}{|}_{\bar{k}}\ne 0$$

B. 线性项的引入：驱动优化过程

这些非零的梯度${\mathbf{l}}_{\mathbf{x}}$ 和 ${\mathbf{l}}_{\mathbf{u}}$在 Q 函数${\mathcal{Q}}_k$的泰勒展开中引入了线性分量$\mathbf{q}$ 和 $\mathbf{r}$：
$${\mathcal{Q}}_k\approx \cdots +\underset{\text{非零}}{\underbrace{{\mathbf{q}}^T{\mathbf{\delta x}}_k}}+\underset{\text{非零}}{\underbrace{{\mathbf{r}}^T{\mathbf{\delta u}}_k}}+\frac{1}{2}{\mathbf{\delta x}}_k^T\mathbf{Q}{\mathbf{\delta x}}_k+\dots$$
由于 $\mathbf{q}$ 和 $\mathbf{r}$不为零，利用贝尔曼方程逆向递推得到的值函数$V_k$ 的梯度 ${\mathbf{v}}_k$也不为零：
$${\mathbf{v}}_k=\mathbf{q}-\mathbf{M}{\mathbf{R}}^{-1}\mathbf{r}\ne 0$$

iLQR 结论： 值函数是仿射二次型（包含线性项）：
$$V_k({\mathbf{\delta x}}_k)\approx \text{const}+\underset{\text{非零线性项}}{\underbrace{{\mathbf{v}}_k^T{\mathbf{\delta x}}_k}}+\frac{1}{2}{\mathbf{\delta x}}_k^T{\mathbf{V}}_{\mathbf{xx}}{\mathbf{\delta x}}_k$$

---

总结：线性项的作用

iLQR 中的非零线性项${\mathbf{v}}_k^T{\mathbf{\delta x}}_k$意味着最优值函数在当前名义点处有一个非零的斜率。

该斜率就是优化过程的驱动力：它告诉我们沿着哪个方向（即最优反馈控制${\mathbf{\delta u}}_k^{\ast }$）移动 $\mathbf{\delta x}$ 可以获得最大的成本下降，从而不断地将名义轨迹推向局部最优。

iLQR的最优目标函数

终端：
在k = N 时，最优未来成本是终端成本
$$V_N(x_N)=h(x_N)$$
将其泰勒展开，去掉常数项，可以得到

$$V_N(\delta x_N)\approx h_x^T\delta x_N+\frac{1}{2}\delta x_N^T{h}_{xx}\delta x_N$$

一般：
$$V_k(\delta x_k)=C_k+v_k^T\delta x_k+\frac{1}{2}\delta x_k^T{V}_{xx,k}\delta x_k$$

其中，在终端的时候
$$v_N=h_x,V_{xx,N}=h_{xx}$$

递推过程k + 1 ----> k：

$$Q_k(\delta x_k,\delta u_k)=l(x_k,u_k)+V_{k+1}(x_{k+1})$$

我们的目标是将${\mathcal{Q}}_k$在名义轨迹点$({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)$附近展开，得到一个关于扰动$({\mathbf{\delta x}}_k,{\mathbf{\delta u}}_k)$ 的二次近似：

$${\mathcal{Q}}_k\approx \text{常数}+{\mathbf{q}}^T{\mathbf{\delta x}}_k+{\mathbf{r}}^T{\mathbf{\delta u}}_k+\frac{1}{2}{\mathbf{\delta x}}_k^T\mathbf{Q}{\mathbf{\delta x}}_k+\frac{1}{2}{\mathbf{\delta u}}_k^T\mathbf{R}{\mathbf{\delta u}}_k+{\mathbf{\delta x}}_k^T\mathbf{M}{\mathbf{\delta u}}_k$$

分别展开 $l$ 和 $V_{k+1}$，然后将它们的系数合成。

2. 已知信息 (输入)

在 $k$时刻的逆向递推中，我们已知以下信息：

1. 瞬时成本 $l$ 的导数：
   ${\mathbf{l}}_{\mathbf{x}},{\mathbf{l}}_{\mathbf{u}}$ (梯度)
   ${\mathbf{l}}_{\mathbf{xx}},{\mathbf{l}}_{\mathbf{uu}},{\mathbf{l}}_{\mathbf{xu}}$(Hessian)

2. 未来成本 $V_{k+1}$ 的近似：
   ${\mathbf{v}}_{k+1}$ ($V_{k+1}$ 对 ${\mathbf{\delta x}}_{k+1}$ 的梯度)
   ${\mathbf{V}}_{\mathbf{xx},k+1}$ ($V_{k+1}$ 对 ${\mathbf{\delta x}}_{k+1}$的 Hessian)

3. 线性化动力学：
   ${\mathbf{\delta x}}_{k+1}={\mathbf{A}}_k{\mathbf{\delta x}}_k+{\mathbf{B}}_k{\mathbf{\delta u}}_k+{\mathbf{c}}_k$

瞬时成本 $l$的展开 (直接展开)

我们首先对 $l({\mathbf{x}}_k,{\mathbf{u}}_k)$ 在 $({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)$附近进行二阶泰勒展开：

$$l({\mathbf{x}}_k,{\mathbf{u}}_k)\approx l({\bar{\mathbf{x}}}_k,{\bar{\mathbf{u}}}_k)+{\mathbf{l}}_{\mathbf{x}}^T{\mathbf{\delta x}}_k+{\mathbf{l}}_{\mathbf{u}}^T{\mathbf{\delta u}}_k+\frac{1}{2}{\mathbf{\delta x}}_k^T{\mathbf{l}}_{\mathbf{xx}}{\mathbf{\delta x}}_k+\frac{1}{2}{\mathbf{\delta u}}_k^T{\mathbf{l}}_{\mathbf{uu}}{\mathbf{\delta u}}_k+{\mathbf{\delta x}}_k^T{\mathbf{l}}_{\mathbf{xu}}{\mathbf{\delta u}}_k$$

未来成本 $V_{k+1}$的展开 (链式法则)

我们必须将 $V_{k+1}$（它是 ${\mathbf{\delta x}}_{k+1}$的函数）转换为$({\mathbf{\delta x}}_k,{\mathbf{\delta u}}_k)$ 的函数。

我们从 $V_{k+1}$的已知近似开始：
$$V_{k+1}({\mathbf{\delta x}}_{k+1})\approx \text{常数}+{\mathbf{v}}_{k+1}^T{\mathbf{\delta x}}_{k+1}+\frac{1}{2}{\mathbf{\delta x}}_{k+1}^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{\delta x}}_{k+1}$$

现在，我们将线性化动力学${\mathbf{\delta x}}_{k+1}={\mathbf{A}}_k{\mathbf{\delta x}}_k+{\mathbf{B}}_k{\mathbf{\delta u}}_k+{\mathbf{c}}_k$ 代入上式。

$V_{k+1}$的线性项展开

将 ${\mathbf{\delta x}}_{k+1}$ 代入 ${\mathbf{v}}_{k+1}^T{\mathbf{\delta x}}_{k+1}$：
$${\mathbf{v}}_{k+1}^T{\mathbf{\delta x}}_{k+1}={\mathbf{v}}_{k+1}^T({\mathbf{A}}_k{\mathbf{\delta x}}_k+{\mathbf{B}}_k{\mathbf{\delta u}}_k+{\mathbf{c}}_k)$$
$${\mathbf{v}}_{k+1}^T{\mathbf{\delta x}}_{k+1}=\underset{\text{对}{\mathbf{\delta x}}_k\text{线性}}{\underbrace{({\mathbf{A}}_k^T{\mathbf{v}}_{k+1}{)}^T{\mathbf{\delta x}}_k}}+\underset{\text{对}{\mathbf{\delta u}}_k\text{线性}}{\underbrace{({\mathbf{B}}_k^T{\mathbf{v}}_{k+1}{)}^T{\mathbf{\delta u}}_k}}+\underset{\text{常数}}{\underbrace{{\mathbf{v}}_{k+1}^T{\mathbf{c}}_k}}$$

$V_{k+1}$的二次项展开

将 ${\mathbf{\delta x}}_{k+1}$ 代入 $\frac{1}{2}{\mathbf{\delta x}}_{k+1}^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{\delta x}}_{k+1}$：
$$\frac{1}{2}({\mathbf{A}}_k{\mathbf{\delta x}}_k+{\mathbf{B}}_k{\mathbf{\delta u}}_k+{\mathbf{c}}_k{)}^T{\mathbf{V}}_{\mathbf{xx},k+1}({\mathbf{A}}_k{\mathbf{\delta x}}_k+{\mathbf{B}}_k{\mathbf{\delta u}}_k+{\mathbf{c}}_k)$$

展开这个二次型（我们只保留到二阶，忽略${\mathbf{c}}_k$的二次项，因为它只是常数）：

${\mathbf{\delta x}}_k$ 的二次项:$\frac{1}{2}({\mathbf{A}}_k{\mathbf{\delta x}}_k{)}^T{\mathbf{V}}_{\mathbf{xx},k+1}({\mathbf{A}}_k{\mathbf{\delta x}}_k)=\frac{1}{2}{\mathbf{\delta x}}_k^T({\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{A}}_k){\mathbf{\delta x}}_k$
${\mathbf{\delta u}}_k$ 的二次项:$\frac{1}{2}({\mathbf{B}}_k{\mathbf{\delta u}}_k{)}^T{\mathbf{V}}_{\mathbf{xx},k+1}({\mathbf{B}}_k{\mathbf{\delta u}}_k)=\frac{1}{2}{\mathbf{\delta u}}_k^T({\mathbf{B}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{B}}_k){\mathbf{\delta u}}_k$
交叉项 (${\mathbf{\delta x}}_k,{\mathbf{\delta u}}_k$):${\mathbf{\delta x}}_k^T({\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{B}}_k){\mathbf{\delta u}}_k$
线性项 (来自${\mathbf{c}}_k$):${\mathbf{\delta x}}_k^T({\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{c}}_k)+{\mathbf{\delta u}}_k^T({\mathbf{B}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{c}}_k)$

(注：iLQR 简化忽略了$f$ 的二阶导数 ${\mathbf{f}}_{\mathbf{xx}},{\mathbf{f}}_{\mathbf{uu}}$，它们本应出现在$V_{k+1}$的展开中。)

合成 Q 函数系数

现在我们将第 1 部分 ($l$的展开) 和第 2 部分 ($V_{k+1}$的展开) 的同类项系数相加，得到${\mathcal{Q}}_k$的最终系数。

梯度 $\mathbf{q}$ (对 ${\mathbf{\delta x}}_k$ 的线性项)

$$\mathbf{q}=\frac{\partial {\mathcal{Q}}_k}{\partial {\mathbf{\delta x}}_k}=\underset{\text{来自}l}{\underbrace{{\mathbf{l}}_{\mathbf{x}}}}+\underset{\text{来自}{V}_{k+1}\text{线性项}}{\underbrace{{\mathbf{A}}_k^T{\mathbf{v}}_{k+1}}}+\underset{\text{来自}{V}_{k+1}\text{二次项与}{\mathbf{c}}_k\text{交叉}}{\underbrace{{\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{c}}_k}}$$

梯度 $\mathbf{r}$ (对 ${\mathbf{\delta u}}_k$ 的线性项)

$$\mathbf{r}=\frac{\partial {\mathcal{Q}}_k}{\partial {\mathbf{\delta u}}_k}=\underset{\text{来自}l}{\underbrace{{\mathbf{l}}_{\mathbf{u}}}}+\underset{\text{来自}{V}_{k+1}\text{线性项}}{\underbrace{{\mathbf{B}}_k^T{\mathbf{v}}_{k+1}}}+\underset{\text{来自}{V}_{k+1}\text{二次项与}{\mathbf{c}}_k\text{交叉}}{\underbrace{{\mathbf{B}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{c}}_k}}$$

Hessian$\mathbf{Q}$ (对 ${\mathbf{\delta x}}_k,{\mathbf{\delta x}}_k$ 的二次项)

$$\mathbf{Q}=\frac{{\partial }^2{\mathcal{Q}}_k}{\partial {\mathbf{\delta x}}_k^2}=\underset{\text{来自}l}{\underbrace{{\mathbf{l}}_{\mathbf{xx}}}}+\underset{\text{来自}{V}_{k+1}\text{二次项}}{\underbrace{{\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{A}}_k}}$$

Hessian$\mathbf{R}$ (对 ${\mathbf{\delta u}}_k,{\mathbf{\delta u}}_k$ 的二次项)

$$\mathbf{R}=\frac{{\partial }^2{\mathcal{Q}}_k}{\partial {\mathbf{\delta u}}_k^2}=\underset{\text{来自}l}{\underbrace{{\mathbf{l}}_{\mathbf{uu}}}}+\underset{\text{来自}{V}_{k+1}\text{二次项}}{\underbrace{{\mathbf{B}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{B}}_k}}$$

Hessian$\mathbf{M}$ (对 ${\mathbf{\delta x}}_k,{\mathbf{\delta u}}_k$ 的交叉项)

$$\mathbf{M}=\frac{{\partial }^2{\mathcal{Q}}_k}{\partial {\mathbf{\delta x}}_k\partial {\mathbf{\delta u}}_k}=\underset{\text{来自}l}{\underbrace{{\mathbf{l}}_{\mathbf{xu}}}}+\underset{\text{来自}{V}_{k+1}\text{二次项}}{\underbrace{{\mathbf{A}}_k^T{\mathbf{V}}_{\mathbf{xx},k+1}{\mathbf{B}}_k}}$$