iLQR学习

• LQR基础
  • $k=N-1$ 时刻的最优控制律 $u^*_{N-1}$
  • 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 函数系数
      • 梯度 $\boldsymbol{q}$ (对 $\boldsymbol{\delta x}_k$ 的线性项)
      • 梯度 $\boldsymbol{r}$ (对 $\boldsymbol{\delta u}_k$ 的线性项)
      • Hessian $\boldsymbol{Q}$ (对 $\boldsymbol{\delta x}_k, \boldsymbol{\delta x}_k$ 的二次项)
      • Hessian $\boldsymbol{R}$ (对 $\boldsymbol{\delta u}_k, \boldsymbol{\delta u}_k$ 的二次项)
      • Hessian $\boldsymbol{M}$ (对 $\boldsymbol{\delta x}_k, \boldsymbol{\delta u}_k$ 的交叉项)
  • 求解最优控制扰动
  • 递推过程 k->k-1
  • 前向更新
    • A. 步长与线性搜索 (Line Search)
    • B. 前向更新的执行步骤
  • 迭代与收敛
    • 一些疑惑与解答
      • 什么时候计算ck呢？
      • 给出一条轨迹没有动力学模型怎么办？
  • LQR与iLQR
  • iLQR, ALiLQR, CILQR 算法特征对比
    • 核心特征对比表
    • 1. iLQR (Iterative Linear Quadratic Regulator)
    • 2. ALiLQR (Augmented Lagrangian iLQR)
    • 3. CILQR (Constrained iLQR)
• ALiLQR在项目中的实际应用
  • 车辆动力学模型
    • 1.定义状态变量和控制变量
    • 2.实现运动方程与积分离散
  • 反向传播，求解最优控制
  • 前向递推Forward Pass
  • 轨迹cost计算
  • ALiLQR的外层循环
  • ALiLQR的ConstrainCost构建
    • 组装：

LQR基础

状态转移：

$$x_{k+1} = A_k x_t + B_ku_t$$ 需要使如下目标函数最优

$$J(x_0,U) = \frac{1}{2} \Sigma_{k=1}^ {N-1} (x_k^TQ_kx_k + u_k^TR_ku_k) + \frac{1}{2}x_N^TQ_fx_N$$

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

在终端时刻 $N$，剩余成本即为终端惩罚：

$$V_N(x_N) = \frac{1}{2} x_N^T Q_f x_N$$ 为便于递推，我们定义：

$$\boldsymbol{S_N = Q_f}$$ 所以：

$$V_N(x_N) = \frac{1}{2} x_N^T S_N x_N$$

$k=N-1$ 时刻的最优控制律 $u^*_{N-1}$

$$V_N(x_N) = \frac{1}{2} x_N^TQ_fx_N=\frac{1}{2}x_N^TS_Nx_N$$

根据贝尔曼最优性原理

$$\begin{aligned}V_{i}(x_{i}) &= \min_{u_{i}} V_{i+1}(x_{i+1})+ \frac{1}{2}u_{i}^TR_{i}u_{i} + \frac{1}{2}x_{i}^TQ_{i}x_{i} \\&=\min_{u_{i}} \frac{1}{2}u_{i}^TR_{i}u_{i} + \frac{1}{2}x_{i}^TQ_{i}x_{i} + \frac{1}{2}(A_{i}x_{i} + B_{i}u_{i})^TS_{i+1}(A_{i}x_{i} + B_{i}u_{i})\\ &=\min_{u_{i}} \frac{1}{2}u_{i}^TR_{i}u_{i} + \frac{1}{2}x_{i}^TQ_{i}x_{i} +\frac{1}{2} x_i^TA^TS_{i+1} Ax_i + \frac{1}{2} u_i^TB_i^TS_{i+1}B_iu_i + x_i^TA_i^TS_{i+1}B_iu_i \\ &=\min_{u_{i}} \frac{1}{2} u_i^T(R_i + B_i^TS_{i+1}B_i)u_i + \frac{1}{2} x_i^T(Q_{i} + A_i^TS_{i+1}A_i)x_i + u_i^T(B_i^TS_{i+1}A_i)x_i \\ \end{aligned}$$

接下来求导

$$\begin{aligned} \frac{\partial V}{\partial u} &= (R_i + B_i^TS_{i+1}B_i)u_i + (B_i^TS_{i+1}A_i)x_i &= 0 \end{aligned}$$ $$u_i = - (R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i)x_i$$ $$u_i = -K_ix_i, \ \ \ \ \ K_i = (R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i)$$

Riccati

代入最优控制

$$\begin{aligned}V_i(x_i) = &\frac{1}{2} u_i^{*T}(R_i + B_i^TS_{i+1}B_i)u_i^* + \frac{1}{2} x_i^T(Q_{i} + A_i^TS_{i+1}A_i)x_i + u_i^{*T}(B_i^TS_{i+1}A_i)x_i \\ = & \frac{1}{2} x_i^TK_i^T(R_i + B_i^TS_{i+1}B_i)K_ix_i + \frac{1}{2} x_i^T(Q_{i} + A_i^TS_{i+1}A_i)x_i - x_i^TK_i^T(B_i^TS_{i+1}A_i)x_i \\ = & \frac{1}{2} x_i^T (K_i^T(R_i + B_i^TS_{i+1}B_i)K_i + Q_{i} + A_i^TS_{i+1}A_i - 2K_i^T(B_i^TS_{i+1}A_i)) x_i \end{aligned}$$

完整形式Riccati方程

$$\begin{aligned}S_i = & Q_i + A_i^TS_{i+1}A_i - 2K^T(B_i^TS_{i+1}A_i) + K_i^TR_iK_i + K_i^T(B_i^TS_{i+1}B_i)K_i \\ = & Q_i + K_i^TR_iK_i + (A_i - B_iK_i)^TS_{i+1}(A_i - B_iK_i) \end{aligned}$$

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

将Ki代入

$$\begin{aligned} S_i =& Q_i + K_i^TR_iK_i + (A_i - B_iK_i)^TS_{i+1}(A_i - B_iK_i) \\ = & Q_i + ((R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i)) ^T R_i (R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i) + (A_i - B_i(R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i))^TS_{i+1}(A_i - B_i(R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{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{aligned}$$

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去优化轨迹

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

扰动为

$$\begin{aligned} \delta x_k = x_k - \bar x_k\\ \delta u_k = u_k - \bar u_k \end{aligned}$$

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

对状态转移方程进行展开

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

$$\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^Tl_{xu}\delta u_k, \ \ \ \ \ \ \ l(\bar x_k, \bar u_k) 为常数$$

终端成本

$$h(x_N) \approx h(\bar x_N) + h^T_x\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)：纯二次型

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

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

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

$$\frac{\partial l}{\partial \boldsymbol{x}}\bigg|_{\boldsymbol{x}=\mathbf{0}, \boldsymbol{u}=\mathbf{0}} = \mathbf{0}$$

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

由于成本函数在原点没有梯度，所以值函数 $V_k$ 在 $\boldsymbol{x}=\mathbf{0}$ 附近也没有梯度。 值函数 $V_k$ 的泰勒展开式为：

$$V_k(\boldsymbol{x}_k) \approx V_k(\mathbf{0}) + \underbrace{\boldsymbol{v}_k^T}_{\text{梯度}} \boldsymbol{x}_k + \frac{1}{2} \boldsymbol{x}_k^T \boldsymbol{V}_{\boldsymbol{xx}} \boldsymbol{x}_k$$ 由于 $V_k(\mathbf{0})=0$ 且 $\boldsymbol{v}_k = \frac{\partial V_k}{\partial \boldsymbol{x}}|_{\boldsymbol{x}=\mathbf{0}} = \mathbf{0}$，所有线性项都消失了。

LQR 结论： 值函数是纯二次型：

$$V_k(\boldsymbol{x}_k) = \frac{1}{2} \boldsymbol{x}_k^T \boldsymbol{S}_k \boldsymbol{x}_k$$

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

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

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

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

$$\boldsymbol{l}_{\boldsymbol{x}} = \frac{\partial l}{\partial \boldsymbol{x}}\bigg|_{\bar{k}} \neq \mathbf{0}$$

$$\boldsymbol{l}_{\boldsymbol{u}} = \frac{\partial l}{\partial \boldsymbol{u}}\bigg|_{\bar{k}} \neq \mathbf{0}$$

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

这些非零的梯度 $\boldsymbol{l}_{\boldsymbol{x}}$ 和 $\boldsymbol{l}_{\boldsymbol{u}}$ 在 Q 函数 $\mathcal{Q}_k$ 的泰勒展开中引入了线性分量 $\boldsymbol{q}$ 和 $\boldsymbol{r}$：

$$\mathcal{Q}_k \approx \dots + \underbrace{\boldsymbol{q}^T \boldsymbol{\delta x}_k}_{\text{非零}} + \underbrace{\boldsymbol{r}^T \boldsymbol{\delta u}_k}_{\text{非零}} + \frac{1}{2} \boldsymbol{\delta x}_k^T \boldsymbol{Q} \boldsymbol{\delta x}_k + \dots$$ 由于 $\boldsymbol{q}$ 和 $\boldsymbol{r}$ 不为零，通过贝尔曼方程逆向递推得到的值函数 $V_k$ 的梯度 $\boldsymbol{v}_k$ 也不为零：

$$\boldsymbol{v}_k = \boldsymbol{q} - \boldsymbol{M} \boldsymbol{R}^{-1} \boldsymbol{r} \neq \mathbf{0}$$

iLQR 结论： 值函数是仿射二次型（包含线性项）：

$$V_k(\boldsymbol{\delta x}_k) \approx \text{const} + \underbrace{\boldsymbol{v}_k^T \boldsymbol{\delta x}_k}_{\text{非零线性项}} + \frac{1}{2} \boldsymbol{\delta x}_k^T \boldsymbol{V}_{\boldsymbol{xx}} \boldsymbol{\delta x}_k$$

🎯 总结：线性项的作用

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

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

iLQR的最优目标函数

终端： 在k = N 时，最优未来成本是终端成本

$$V_N(x_N) = h (x_N)$$ 将其泰勒展开，去掉常数项，可以得到

$$V_N(\delta x_N) \approx h^T_x\delta x_N + \frac{1}{2} \delta x^T _N h_{xx} \delta x_N$$

一般：

$$V_k(\delta x_k) = C_k + v_k^T\delta x_k + \frac{1}{2}\delta x^T_kV_{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{\boldsymbol{x}}_k, \bar{\boldsymbol{u}}_k)$ 附近展开，得到一个关于扰动 $(\boldsymbol{\delta x}_k, \boldsymbol{\delta u}_k)$ 的二次近似：

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

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

2. 已知信息 (输入)

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

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

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

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

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

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

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

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

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

我们从 $V_{k+1}$ 的已知近似开始：

$$V_{k+1}(\boldsymbol{\delta x}_{k+1}) \approx \text{常数} + \boldsymbol{v}_{k+1}^T \boldsymbol{\delta x}_{k+1} + \frac{1}{2} \boldsymbol{\delta x}_{k+1}^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{\delta x}_{k+1}$$

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

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

将 $\boldsymbol{\delta x}_{k+1}$ 代入 $\boldsymbol{v}_{k+1}^T \boldsymbol{\delta x}_{k+1}$：

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

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

将 $\boldsymbol{\delta x}_{k+1}$ 代入 $\frac{1}{2} \boldsymbol{\delta x}_{k+1}^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{\delta x}_{k+1}$：

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

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

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

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

合成 Q 函数系数

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

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

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

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

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

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

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

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

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

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

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

求解最优控制扰动

求导，就是求导！

$$\mathcal{Q}_k \approx \text{常数} + \boldsymbol{q}^T \boldsymbol{\delta x}_k + \boldsymbol{r}^T \boldsymbol{\delta u}_k + \frac{1}{2} \boldsymbol{\delta x}_k^T \boldsymbol{Q} \boldsymbol{\delta x}_k + \frac{1}{2} \boldsymbol{\delta u}_k^T \boldsymbol{R} \boldsymbol{\delta u}_k + \boldsymbol{\delta x}_k^T \boldsymbol{M} \boldsymbol{\delta u}_k$$ $$\frac{\partial Q_k}{\partial \delta u_k} = r + R\delta u_k + M^T\delta x_k$$ $$\delta u_k^* = - R^{-1}r - R^{-1}M^T\delta x_k = k_k + K_k\delta x_k$$

$$\delta u_k = u_k - \bar u_k$$

前馈：$k_k = - R^{-1}r$，抵消成本的梯度 $\boldsymbol{r}$。这是推动轨迹向最优方向移动的主要力量。

反馈：$K_k = - R^{-1}M^T$，实时修正状态 $\boldsymbol{\delta x}_k$ 的偏差。

递推过程 k->k-1

1. 计算$q_{k}$,$r_{k}$,$Q_{k}$,$R_{k}$,$M_{k}$，得到前馈和反馈系数
2. 计算前馈：$k_k$和反馈：$K_k$
3. 更新hessian$V_{xx,k}$和梯度$v_k$。因为接下来计算k-1时刻时的q之类的系数需要用到这两个量。

$$\boldsymbol{v}_k = \boldsymbol{q}_k + \boldsymbol{K}_k^T \boldsymbol{r}_k + \boldsymbol{K}_k^T \boldsymbol{R}_k \boldsymbol{k}_k + \boldsymbol{M}_k \boldsymbol{k}_k$$ $$\boldsymbol{V}_{\boldsymbol{xx}, k} = \boldsymbol{Q}_k + \boldsymbol{K}_k^T \boldsymbol{R}_k \boldsymbol{K}_k + \boldsymbol{M}_k \boldsymbol{K}_k + \boldsymbol{K}_k^T \boldsymbol{M}_k^T$$ 4. 计算$q_{k-1}$,$r_{k-1}$,$Q_{k-1}$,$R_{k-1}$,$M_{k-1}$ 5. 计算前馈：$k_{k-1}$和反馈：$K_{k-1}$

前向更新

目的： 利用刚刚计算出的 $(\boldsymbol{k}, \boldsymbol{K})$ 策略，生成一条新的、成本更低的轨迹 $(\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}})$。

A. 步长与线性搜索 (Line Search)

我们不能简单地将 $\boldsymbol{\delta u}_k^* = \boldsymbol{k}_k + \boldsymbol{K}_k \boldsymbol{\delta x}_k$ 完全应用。

为什么？ 因为我们的 $(\boldsymbol{k}, \boldsymbol{K})$ 策略是基于一个局部线性近似 $f$ 计算出来的。如果 $f$ 的非线性程度很高，应用完整的 $\boldsymbol{\delta u}_k^*$ （即 $\alpha=1$）可能会导致 $\boldsymbol{x}_{\text{new}}$ 严重偏离预期，新轨迹的总成本 $J$ 反而可能上升。

为了确保收敛，我们引入一个步长 $\alpha$ (Alpha)，$\alpha \in (0, 1]$。

B. 前向更新的执行步骤

1. 初始化：

   • 设置新轨迹的起点：$\boldsymbol{x}_{\text{new}, 0} = \boldsymbol{x}_0$ (初始状态始终不变)。
   • 尝试一个初始步长，例如 $\alpha = 1.0$。

2. 模拟（Rollout）： 从 $k=0$ 到 $N-1$ 循环：

   计算状态偏差：

$$\boldsymbol{\delta x}_k = \boldsymbol{x}_{\text{new}, k} - \bar{\boldsymbol{x}}_k$$ (注意：这里是用新状态 $\boldsymbol{x}_{\text{new}, k}$ 和旧名义状态 $\bar{\boldsymbol{x}}_k$ 计算偏差。)

**计算新的控制 $\boldsymbol\{u\}\_\{\text\{new\}, k\}$：**

$$\boldsymbol{u}_{\text{new}, k} = \bar{\boldsymbol{u}}_k + \alpha \cdot \boldsymbol{k}_k + \boldsymbol{K}_k \boldsymbol{\delta x}_k$$ (这是关键：前馈项 $\boldsymbol{k}_k$ 被 $\alpha$ 缩放，而反馈项 $\boldsymbol{K}_k$ 通常不缩放或与 $\alpha$ 一起缩放，具体取决于实现。)

**积分（使用真实的非线性动力学 $f$）：**

$$\boldsymbol{x}_{\text{new}, k+1} = f(\boldsymbol{x}_{\text{new}, k}, \boldsymbol{u}_{\text{new}, k})$$

3. 评估成本： 计算新轨迹 $(\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}})$ 的真实总成本 $J_{\text{new}}$ (使用原始非线性成本函数 $l$ 和 $h$)。

4. 接受或拒绝 (Line Search)： 如果 $J_{\text{new}} < J_{\text{old}}$ (成本下降)： 接受！ 新轨迹 $(\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}})$ 优于旧轨迹。 如果 $J_{\text{new}} \ge J_{\text{old}}$ (成本上升或不变)： 拒绝！ 步子迈得太大了。 减小 $\alpha$ (例如 $\alpha = \alpha / 2$)。 返回步骤 2，用更小的 $\alpha$ 重新尝试整个前向模拟。

迭代与收敛

前向更新成功找到一条成本更低的轨迹 $(\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}})$ 后：

1. 更新名义轨迹： 将 $(\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}})$ 设为下一次迭代的名义轨迹 $(\bar{\boldsymbol{x}}, \bar{\boldsymbol{u}})$。

2. 重复： 返回逆向递推 (Backward Pass) 阶段，在新的 $(\bar{\boldsymbol{x}}, \bar{\boldsymbol{u}})$ 附近重新计算 $\boldsymbol{A}_k, \boldsymbol{B}_k, l_{\boldsymbol{x}}, l_{\boldsymbol{xx}} \dots$ 并求解新的 $(\boldsymbol{k}, \boldsymbol{K})$ 策略。

3. 收敛： 当成本 $J$ 的改善量非常小（低于某个阈值 $\epsilon$）时，算法收敛，当前的 $(\bar{\boldsymbol{x}}, \bar{\boldsymbol{u}})$ 即为局部最优轨迹。

一些疑惑与解答

什么时候计算ck呢？

$\boldsymbol{c}_k$ 的计算发生在每次迭代的开始（前向更新阶段）：

给出一条轨迹没有动力学模型怎么办？

必须有，没有你就蒙一个，比如"Longitudinal Kinematic Point Model" (纵向运动学点模型)

LQR与iLQR

iLQR, ALiLQR, CILQR 算法特征对比

这三者都是用于轨迹优化的高效算法，它们的核心都基于 iLQR（迭代线性二次调节器）。它们最大的区别在于如何处理优化问题中的“约束”。

核心特征对比表

1. iLQR (Iterative Linear Quadratic Regulator)

• 全称：迭代线性二次调节器
• 核心思想：
  1. 在一个名义轨迹（Nominal Trajectory）周围，将非线性的系统动力学 线性化。
  2. 将非二次的代价函数 二次化。
  3. 这样问题就变成了一个局部的 LQR 问题，可以通过动态规划（DP）高效求解，得到一个更优的控制策略。
  4. 不断重复这个“线性化-二次化-求解”的过程，直到收敛。
• 约束处理：
  • 标准 iLQR 不直接支持硬约束（如关节限位、避障）。
  • 在实践中，人们有时会用一些技巧（Hacks）来模拟约束，比如：
    • 将约束作为软约束（Soft Constraint）添加到代价函数中（例如：cost += w * (q - q_limit)^2）。
    • 在控制输入上进行简单的“裁剪”（Clipping）。
  • 这些方法并不严谨，且效果通常不好。
• 比喻：短跑运动员。他只管在没有障碍的直跑道上用最快的速度冲刺（最小化代价）。

2. ALiLQR (Augmented Lagrangian iLQR)

• 全称：增广拉格朗日 iLQR
• 核心思想：
  • 采用增广拉格朗日方法，将一个有约束问题转化为一系列无约束问题来求解。
  • 它在原代价函数 J 的基础上，增加了“拉格朗日乘子项 λ”和“二次惩罚项 μ”。
  • 新的代价变为：J_aug = J + λ * c(x) + μ * c(x)^2（简化示意）。
• 算法结构（双循环）：
  1. 内循环：固定当前的 λ 和 μ，调用 iLQR 来最小化这个 J_aug（这步是无约束的）。
  2. 外循环：在 iLQR 收敛后，检查当前轨迹对约束 c(x) 的违反程度。
     • 如果违反严重，就增大惩罚系数 μ（加大罚款力度）。
     • 根据违反情况，更新拉格朗日乘子 λ（调整罚款基准）。
  3. 重复1和2，直到约束被满足且代价最优。
• 比喻：罚款制障碍赛。运动员可以“撞到”栏架（暂时违反约束），但每次都会被“罚款”（惩罚项）。他会通过调整策略（更新 λ 和 μ），最终找到一条既跑得快、又不再撞栏（满足约束）的路线。

3. CILQR (Constrained iLQR)

• 全称：约束 iLQR (通常特指基于内点法的实现)
• 核心思想：
  • 采用**内点法（或称障碍函数法）**来处理不等式约束（例如 g(x) <= 0）。
  • 它在代价函数 J 中加入一个障碍项 (Barrier Term)，最常见的是对数障碍函数：J_barrier = J - μ * log(-g(x))。
• 工作机制：
  • 当轨迹 x 远离约束边界时（g(x) 是一个较大的负数），log(-g(x)) 变化很小，对优化影响不大。
  • 当轨迹 x 接近约束边界时（g(x) 趋近于0），log(-g(x)) 会迅速趋向负无穷，导致 -μ * log(-g(x)) 变成一个无穷大的代价。
  • 这个“无穷大”的代价墙会阻止 iLQR 优化器在求解时越过约束边界。
• 关键要求：
  • 这种方法要求初始轨迹必须严格可行（即 g(x) < 0）。如果初始轨迹就在边界外，log 函数的输入为正数，算法会直接崩溃。

ALiLQR在项目中的实际应用

车辆动力学模型

1.定义状态变量和控制变量

$$x = \begin{bmatrix} x_0 \\ x_1 \\ x_2 \\ x_3 \\ x_4 \\ x_5 \end{bmatrix} = \begin{bmatrix} \text{pos\\_x} & (\text{X坐标}) \\ \text{pos\\_y} & (\text{Y坐标}) \\ \theta & (\text{航向角 Yaw}) \\ v & (\text{纵向速度}) \\ a & (\text{纵向加速度}) \\ \omega & (\text{角速度 YawRate}) \end{bmatrix}$$

$$u = \begin{bmatrix} u_0 \\ u_1 \end{bmatrix} = \begin{bmatrix} \text{Jerk} & (\text{加加速度/变加速率}) \\ \dot{\omega} & (\text{角加速度}) \end{bmatrix}$$

2.实现运动方程与积分离散

使用龙格库塔积分

1
2

void LKPointModel::forward_calculation(const State& state, const Control& ctrl,
                                       State& state_output, double step) const 

我们需要得到 $$A_t = \frac{\partial f_{RK4}(x_t, u_t)}{\partial x} \quad (6 \times 6 \text{ 矩阵})$$ $$B_t = \frac{\partial f_{RK4}(x_t, u_t)}{\partial u} \quad (6 \times 2 \text{ 矩阵})$$

一个使用python自动推导并得到C++代码的方式

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88

import sympy as sp

def generate_jacobian_code():
    # 1. 定义符号变量
    # 状态变量 x (6维)
    x, y, theta, v, a, omega = sp.symbols('x y theta v a omega')
    state = sp.Matrix([x, y, theta, v, a, omega])
    
    # 控制变量 u (2维)
    jerk, d_omega = sp.symbols('jerk d_omega')
    control = sp.Matrix([jerk, d_omega])
    
    # 时间步长
    h = sp.symbols('h')

    # 2. 定义连续时间动力学方程 (ODE) f(x, u)
    # 这对应代码中的 continue_gradiant
    dx = v * sp.cos(theta)
    dy = v * sp.sin(theta)
    dtheta = omega
    dv = a
    da = jerk
    domega = d_omega
    
    f_continuous = sp.Matrix([dx, dy, dtheta, dv, da, domega])

    # 3. 定义 RK4 积分步骤 (符号化)
    # k1
    k1 = f_continuous
    
    # k2 (注意：这里需要把 f_continuous 里的 state 替换为 state + 0.5*h*k1)
    state_k2 = state + 0.5 * h * k1
    # 替换规则：将原始变量替换为中间变量
    subs_k2 = list(zip(state, state_k2)) 
    k2 = f_continuous.subs(subs_k2)
    
    # k3
    state_k3 = state + 0.5 * h * k2
    subs_k3 = list(zip(state, state_k3))
    k3 = f_continuous.subs(subs_k3)
    
    # k4
    state_k4 = state + h * k3
    subs_k4 = list(zip(state, state_k4))
    k4 = f_continuous.subs(subs_k4)
    
    # 4. 得到离散时间模型 x_{k+1}
    state_next = state + (h / 6.0) * (k1 + 2*k2 + 2*k3 + k4)

    # 5. 求导！计算 Jacobian A 和 B
    # A = d(state_next) / d(state)
    A = state_next.jacobian(state)
    
    # B = d(state_next) / d(control)
    B = state_next.jacobian(control)

    # 6. 代码优化：公共子表达式消除 (CSE)
    # 这一步是"极致速度"的关键。它会把重复计算的 sin, cos 提取出来
    # 比如截图里的 double theta_K1 = ...; double cos_theta_K1 = ...;
    sub_exprs, (A_simple, B_simple) = sp.cse([A, B])

    # 7. 打印 C++ 代码
    print("// --- 自动生成的 C++ 代码开始 ---")
    
    # 打印中间变量
    for var, expr in sub_exprs:
        # 转换 sympy 表达式为 C++ 格式 (简单替换)
        c_expr = str(expr).replace("sin", "std::sin").replace("cos", "std::cos")
        print(f"double {var} = {c_expr};")
    
    print("\n// 填充 A 矩阵")
    rows, cols = A_simple.shape
    for r in range(rows):
        for c in range(cols):
            val = A_simple[r, c]
            if val != 0: # 忽略0以节省赋值时间
                print(f"A_output({r}, {c}) = {val};")

    print("\n// 填充 B 矩阵")
    rows, cols = B_simple.shape
    for r in range(rows):
        for c in range(cols):
            val = B_simple[r, c]
            if val != 0:
                print(f"B_output({r}, {c}) = {val};")

if __name__ == "__main__":
    generate_jacobian_code()

反向传播，求解最优控制

我们优化的目标函数 $J$ 是同时取决于 状态 $x$ 和 控制 $u$ 的。

$$J(x, u) = \text{StateCost}(x) + \text{ControlCost}(u)$$ 为了在矩阵运算中方便，iLQR 这里暂时将状态和控制拼成一个增广向量 (Augmented Vector)：

$$\mathbf{z} = \begin{bmatrix} x \\ u \end{bmatrix} \quad (\text{维度 } 6 + 2 = 8)$$ 后面在计算梯度等内容时会拆开的。 对每一个时间步/参考点i，ActionCostFunction输出：

1. hessians_[i] (矩阵 H)：一个 $8 \times 8$ 的矩阵（6维状态 + 2维控制）。代表二次项系数。

$$\mathbf{H} = \begin{bmatrix} \color{blue}{w_{x}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & \color{blue}{w_{y}} & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & \color{gray}{0} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \color{gray}{0} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & \color{blue}{w_{a}} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & \color{gray}{0} & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & \color{red}{w_{j}} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & \color{red}{w_{\dot{\omega}}} \end{bmatrix}$$

1. gradiants_[i] (向量 g)：一个 $8 \times 1$ 的向量。代表一次项系数。

$$\mathbf{g} = \begin{bmatrix} -2 \cdot w_{x} \cdot \text{ref\\_x}[i] \\ -2 \cdot w_{y} \cdot \text{ref\\_y}[i] \\ \color{gray}{0} \\ \color{gray}{0} \\ -2 \cdot w_{a} \cdot \text{ref\\_a}[i] \\ \color{gray}{0} \\ \color{green}{0} \\ \color{green}{0} \end{bmatrix}$$

如何获取这两个量？： 我们要追踪一个参考点 $x_{ref}$。代价函数定义为误差的平方：

$$J(x) = w \cdot (x - x_{ref})^2 = \underbrace{w}_{H} \cdot x^2 + \underbrace{(-2w \cdot x_{ref})}_{g} \cdot x + \text{const}$$

$$l_x = \frac{\partial J}{\partial x} = 2 \cdot w \cdot (x - x_{ref}) = 2wx - 2wx_{ref}$$

$$l_{xx} = \frac{\partial^2 J}{\partial x^2} = 2w$$

那么代入可以有 $$l_x = g + 2Hx$$

$$l_{xx} = 2H$$ 我们先预计算出g和H，待进行反向传播时候，代入x，得到真正的梯度。 这里可以把矩阵的状态变量部分和控制变量部分拆开，即可得到

$l_x, l_u$ (梯度)

$l_{xx}, l_{uu}, l_{xu}$ (Hessian)

除此之外，我们也可以得到在某个点处的状态转移方程（来自车辆动力学） $A_k = \frac{\partial f}{\partial x}$

$B_k = \frac{\partial f}{\partial u}$

然后就是由未来时刻传递来的未来梯度、hessian $V_x’$ (未来的梯度，代码 Vdx)

$V_{xx}'$ (未来的 Hessian，代码 Vddx)

接下来只需要依据公式组装对应的q,r,Q,R,M即可

1
2
3
4
5
6
7
8
9
10

// Px 初始值已经是 lx
Px.noalias() += cur_A.transpose() * Vdx;
// Pu 初始值已经是 lu
Pu.noalias() += cur_B.transpose() * Vdx;
// Pxx 初始值已经是 lxx
Pxx.noalias() += cur_A.transpose() * Vddx * cur_A;
// Puu 初始值已经是 luu
Puu.noalias() += cur_B.transpose() * Vddx * cur_B;
// Pxu 初始值通常是 0 (除非 ActionCost 有交叉项)
Pxu.noalias() += cur_A.transpose() * Vddx * cur_B;

然后求解最优控制量

$$\delta u = \underbrace{-\mathbf{R}^{-1} \mathbf{r}}_{\text{前馈 } k} + \underbrace{-\mathbf{R}^{-1} \mathbf{M}^T}_{\text{反馈 } K} \delta x$$

我们要计算两个量： 前馈向量 $k$ (Feedforward) $\rightarrow$ 代码中的 grad_temp

反馈矩阵 $K$ (Feedback) $\rightarrow$ 代码中的 gain_temp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

// 1. 使用 Cholesky 分解检查正定性并准备求逆
// Puu_regular 就是 R 矩阵 (2x2)
Eigen::LLT<Eigen::MatrixXd> lltOfA(Puu_regular); 

if (lltOfA.info() == Eigen::NumericalIssue) {
    return false; // 如果不是正定的（比如是个马鞍面），就报错，让外层增加正则化 mu
}

// 2. 求逆
Puu_inv = Puu_regular.inverse();

// Puu_inv 是 (2x2), Pu 是 (2x1)
// 结果 grad_temp 是 (2x1)
grad_temp.noalias() = -Puu_inv * Pu;

// Pxu_regular 实际上存储的是 Q_xu (6x2)
// Puu_inv 是 (2x2)
gain_temp.noalias() = -Pxu_regular * Puu_inv;

经过反向传播，从末端到起点遍历参考路径上的每一个点之后，我们得到了每一个点的前馈和反馈系数。

前向递推Forward Pass

（初始化假设jerk和dyawrate都为0） 假设我们正在进行第 step = i 的计算： 输入： 上一步（$i-1$）算出来的当前最新状态 state ($x_{new, i}$)

上一轮迭代留下的旧状态 ($x_{old, i}$) 和 旧控制 ($u_{old, i}$)。

这一步的策略 $K_i$ 和 $k_i$。

计算最优控制： $\delta x = x_{new, i} - x_{old, i}$

$$u_{new, i} = u_{old, i} + \alpha \cdot k_i + K_i \cdot \delta x$$

更新状态： $x_{new, i+1} = f(x_{new, i}, u_{new, i})$

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

for (int step = 0; step < horizon_; step++) {
    
    // -------------------------------------------------------------
    // 1. 保存当前状态 (Save Current State)
    // -------------------------------------------------------------
    // 把上一轮迭代推演出的 state 存入新轨迹列表
    (*new_states_ptr_)[step] = state;


    // -------------------------------------------------------------
    // 2. 计算状态偏差 (Calculate State Error)
    // -------------------------------------------------------------
    // delta_x = x_new - x_old
    // optimal_states_ptr_ 存储的是上一轮的旧轨迹 (x_old)
    State state_error = state - (*optimal_states_ptr_)[step];


    // -------------------------------------------------------------
    // 3. 计算最优控制 u_new (Calculate Optimal Control)
    // -------------------------------------------------------------
    // 公式: u_new = u_old + K * delta_x + alpha * k
    // 
    // (*optimal_inputs_ptr_)[step]  -> u_old (旧控制)
    // gain_mats_[step].transpose()  -> K (反馈矩阵，注意转置)
    // grad_vecs_[step]              -> k (前馈向量)
    
    Control input = (*optimal_inputs_ptr_)[step] + 
                    gain_mats_[step].transpose() * state_error + 
                    alpha * grad_vecs_[step];

    // 保存新计算出的控制量
    (*new_inputs_ptr_)[step] = (input);

    // -------------------------------------------------------------
    // 4. 更新状态 / 动力学推演 (Dynamics Update)
    // -------------------------------------------------------------
    // 公式: x_new_next = f(x_new_curr, u_new_curr)
    // 
    // 输入: 当前状态 state, 当前控制 input
    // 输出: 下一时刻状态 (直接更新到 state 变量中)
    model_ptr_->forward_calculation(state, input, state);
}

轨迹cost计算

遍历整条新轨迹，把 ActionCost, ObstacleCost 等所有代价加起来。

1

new_cost = get_traj_value(new_states_ptr_, new_inputs_ptr_);

验收：线搜索 (Line Search Check) 这是 iLQR 稳定性的关键。我们比较 new_cost 和 old_cost。

情况 A： 新轨迹更好了 ($J_{new} < J_{old}$) 判定： 迭代成功！ 动作：把 new_states 扶正，覆盖掉 optimal_states（旧轨迹）。 把 new_inputs 扶正，覆盖掉 optimal_inputs。 更新当前的 optimal_cost = new_cost。

情况 B： 新轨迹更烂了 ($J_{new} \ge J_{old}$) 原因： 这通常是因为模型是非线性的，而 Backward Pass 是基于线性近似算的。步子迈太大（$\alpha=1.0$），导致“甚至不如不改”。 判定： 拒绝接收！ 动作：减小步长： $\alpha = \alpha \times 0.5$ (或者除以某个比例，代码里是 solver_config_.alpha_line_search_regular)。重来！ 带着这个更小的 $\alpha$，重新执行刚才那个 for 循环（前向递推）。

iLQR迭代过程

graph TD
    A[开始迭代] --> B[Backward Pass: 算 K, k]
    B --> C{设置 alpha = 1.0}
    C --> D[Forward Pass: 刚才写的代码, 算出 x_new, u_new]
    D --> E[计算新代价 J_new]
    E --> F{J_new < J_old ?}
    
    F -- Yes (更好) --> G[接受: 更新旧轨迹, 迭代结束]
    F -- No (更烂) --> H[拒绝: alpha 减半]
    H --> I{alpha < 阈值?}
    I -- No --> D
    I -- Yes (实在没救了) --> J[终止: 增加正则化 mu, 重做 Backward]

Levenberg-Marquardt 正则化： 正定意味着代价函数的形状是一个碗（有最低点）。如果 $Q_{uu}$ 不是正定的（比如它是平的，或者是一个马鞍面），求逆就会出错，或者算出来的方向是错误的（反而让代价变大了）。 当 $Q_{uu}$ 不正定时，Levenberg-Marquardt 算法说：“别急，我们在对角线上加个正数 $\mu$。”

$$Q_{uu}^{new} = Q_{uu} + \mu \cdot \mathbf{I}$$ 加了 $\mu$ 之后：矩阵会变得“更正定”，形状会强行变成一个“碗”，保证一定能求逆。代价：算出来的控制方向会偏离纯粹的牛顿方向，更像梯度下降的方向（步长变小，更保守）。

ALiLQR的外层循环

AL-iLQR 的目标函数（Lagrangian）实际上由 三部分 组成：

$$L(x) = \underbrace{J(x)}_{\text{原始代价}} + \underbrace{\lambda \cdot C(x)}_{\text{拉格朗日项}} + \underbrace{\frac{1}{2} \mu \cdot C(x)^2}_{\text{二次惩罚项}}$$

前文所说的iLQR过程其实没加这些约束，现在考虑加上

$$L(x, u) = \underbrace{J_{action}(x, u)}_{\text{ActionCostFunction}} + \underbrace{\sum \left( \lambda C(x) + \frac{1}{2}\mu C(x)^2 \right)}_{\text{ConstrainCost (Linear Obstacle)}}$$

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

// 1. 先拿 ActionCost (基础地形)
Px.noalias() = state_cost_func_->gradient_lx(index_step);
Pxx.noalias() = state_cost_func_->hessian_lxx(index_step);

// 2. 再加上 Linear Constraints (AL 修正)
for (auto& iter : linear_constrain_cost_list) {
    Px.noalias() += iter->gradient_lx(index_step); // 累加
    Pxx.noalias() += iter->hessian_lxx(index_step); // 累加
}

// 3. 再加上 Obstacle Constraints (AL 修正)
for (auto& iter : obstacle_constrain_cost_list) {
    Px.noalias() += iter->gradient_lx(index_step); // 累加
    Pxx.noalias() += iter->hessian_lxx(index_step); // 累加
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92

for (int idx = 0; idx < solver_config_.max_iteration; idx++) {
        // 如果正则化参数过大，说明问题很难解，提前退出
        if (mu_ > solver_config_.regular_mu_max) break;
        // --- 3. 准备数据 ---
        // 根据当前的 x 和 u，计算所有 CostFunction 的 Hessian 和 Gradient
        // 这一步就是之前讨论的：计算 l_x, l_u, l_xx, l_uu
        optimal_cost = update_traj_of_costfunc_list(optimal_states_ptr_, optimal_inputs_ptr_);
        last_cost = optimal_cost;
        // --- 4. Backward Pass (计算策略 K, k) ---
        if (!backward_pass()) {
            // 如果矩阵分解失败 (非正定)，说明当前步子迈太大了或者地形太复杂
            // 增大正则化参数 mu_，让矩阵变得“更对角化”，然后重试
            mu_ = mu_ * solver_config_.mu_regular;
            std::cout << "backward pass failed, increase mu to " << mu_ << std::endl;
            continue; // 跳过本次循环，带着更大的 mu 重试
        }
        // --- 5. Forward Pass (生成新轨迹) ---
        bool forward_succ_flag = false;
        // 尝试生成新轨迹，forward_pass 内部包含了 Line Search
        if (!forward_pass(optimal_cost)) {
            // 如果 Line Search 失败 (新轨迹代价没降低)
            // 同样增大正则化参数 mu_，让下一次 Backward 算出的策略更保守
            mu_ = mu_ * solver_config_.mu_regular;
             std::cout << "forward pass failed, increase mu" << std::endl;
            // 注意：代码逻辑可能是 continue，或者在这里就接受失败
        } else {
            // 成功！代价降低了
            forward_succ_flag = true;
            // 减小 mu_，因为优化很顺利，可以迈大步子加速收敛
            mu_ = std::max(solver_config_.regular_mu_min, mu_ / solver_config_.mu_regular);
            // 获取新轨迹的真实 Cost
            optimal_cost = get_traj_value(optimal_states_ptr_, optimal_inputs_ptr_);
        }

        // 如果 Forward 失败了，直接进入下一轮循环（带着更大的 mu 重试）
        if (!forward_succ_flag) continue;

        // --- 6. 约束检查 (AL Update Logic) ---
        bool convergence = true; 
        
        // A. 检查线性约束 (如边界)
        bool linear_convergence = true;
        for (auto& iter : linear_constrain_cost_list) {
            // update_constrain_value 返回 true 表示满足约束
            // 这里的逻辑是：只要有一个不满足，linear_convergence 就为 false
            linear_convergence = (iter->update_constrain_value()) && linear_convergence;
        }
        if (!linear_convergence) {
            // std::cout << "Linear constraint violated" << std::endl;
        }

        // B. 检查障碍物约束 (如碰撞)
        bool obstacle_convergence = true;
        for (auto& iter : obstacle_constrain_cost_list) {
            obstacle_convergence = (iter->update_constrain_value()) && obstacle_convergence;
             if (!obstacle_convergence) break; // 有一个撞了就行了
        }

        // 总的约束收敛标志
        convergence = convergence && linear_convergence && obstacle_convergence;

        // --- 7. 更新 AL 惩罚 (The "Boss" Steps In) ---
        // 只有在约束未满足时，才需要增加惩罚
        
        // 更新线性约束的 Lambda 和 Mu (Penalty)
        for (auto& iter : linear_constrain_cost_list) {
             iter->update_lamda_penalty(solver_config_.penatly_growth_factor);
        }
        
        // 更新障碍物约束的 Lambda 和 Mu (Penalty)
        for (auto& iter : obstacle_constrain_cost_list) {
             iter->update_lamda_penalty(solver_config_.penatly_growth_factor);
        }

        // --- 8. 终止条件检查 ---
        // 必须同时满足两个条件：
        // 1. Cost 变化很小 (std::abs(optimal_cost - last_cost) < threshold) -> 说明 iLQR 收敛了
        // 2. convergence 为 true -> 说明约束都满足了
        
        if ((std::abs(optimal_cost - last_cost) < solver_config_.convergence_threshold) && 
            convergence) {
            
            // 打印成功信息
            // std::cout << "Converged at iter: " << idx << std::endl;
            return SolverStatus::SUCCESS;
        } else {
            // 打印未收敛信息
            // std::cout << "Not converged, iter: " << idx << " Cost: " << optimal_cost << std::endl;
        }

    } // End Loop

在每次AL Iteration中，我们先调用内层的iLQR更新代价，然后接茬线性约束和障碍物约束。 如果没有最优，更新惩罚（update lambda penalty）： Lambda ($\lambda$) 和 Penalty ($\mu$) 不需要像 $x$ 或 $u$ 那样求导、解方程。 它们的更新规则非常简单粗暴，就是**“后验调整”**。 更新拉格朗日乘子 ($\lambda$):$\lambda_{new} = \lambda_{old} + \mu \cdot C(x)$直觉： 刚才撞得越狠（$C(x)$ 越大），$\lambda$ 就变得越大。$\lambda$ 就像一个“记仇本”，它记住了你刚才在这里犯过错，下次经过这里时，基础代价就会很高。

更新惩罚因子 ($\mu$ / Penalty):$\mu_{new} = \mu_{old} \times \text{growth\\_factor}$直觉： 代码里的 penatly_growth_factor 通常是 5 或 10。这意味着下一轮，障碍物这堵墙的“硬度”会变成刚才的 10 倍。iLQR 会发现撞墙的代价变得无法承受，被迫选择绕路。

ALiLQR的ConstrainCost构建

1. 车辆模型：覆盖圆分解 (Circle Decomposition) 在车身中轴线上放一串圆形。只要这几个圆都不撞，车就不撞。 状态：$x = [pos\\_x, pos\\_y, \theta, \dots]$ 第 $j$ 个圆心的位置：

$$\begin{cases}P_{cx} = pos_x + bias_j \cdot \cos(\theta) \\ P_{cy} = pos_y + bias_j \cdot \sin(\theta)\end{cases}$$

2. 障碍物模型：简单的圆/点假设障碍物位于 $(x_{obs}, y_{obs})$。
3. 约束函数 $C(x)$我们定义：如果距离的平方小于某个阈值 $R_{safe}^2$，就是违规。

$$C(x) = R_{safe}^2 - \left( (P_{cx} - x_{obs})^2 + (P_{cy} - y_{obs})^2 \right)$$ 如果 $C(x) > 0$ $\rightarrow$ 撞了（距离太近）。如果 $C(x) \le 0$ $\rightarrow$ 安全。 利用链式法则求导：

1. 对 $pos\\_x$ 求导：

$$\frac{\partial C}{\partial pos\\_x} = -2\Delta x \cdot \frac{\partial P_{cx}}{\partial pos\\_x} = -2\Delta x \cdot 1 = -2\Delta x$$

2. 对 $pos\\_y$ 求导：

$$\frac{\partial C}{\partial pos\\_y} = -2\Delta y \cdot 1 = -2\Delta y$$

3. 对 $\theta$ 求导 (关键！)：

$$\frac{\partial C}{\partial \theta} = -2\Delta x \cdot \frac{\partial P_{cx}}{\partial \theta} - 2\Delta y \cdot \frac{\partial P_{cy}}{\partial \theta}$$ $\frac{\partial P_{cx}}{\partial \theta} = -bias_j \cdot \sin(\theta)$ $\frac{\partial P_{cy}}{\partial \theta} = bias_j \cdot \cos(\theta)$

组装：

梯度 $l_x$ (Gradient)

$$l_x = \frac{\partial L}{\partial x} = (\lambda + \mu \cdot C(x)) \cdot C’(x)$$

Hessian $l_{xx}$ (Hessian)

$$l_{xx} = \mu \cdot C’(x)^T \cdot C’(x) + (\lambda + \mu C) \cdot C’'(x)$$

为什么 ConstraintCost 制造了非对角项？ ConstraintCost 使用了 Gauss-Newton 近似： $H \approx \mu \cdot \mathbf{J} \mathbf{J}^T$。

这里的 $\mathbf{J}$ 是约束函数 $C(x)$ 的梯度向量（$3 \times 1$）：

$$\mathbf{J} = \begin{bmatrix} \frac{\partial C}{\partial x} \\ \frac{\partial C}{\partial y} \\ \frac{\partial C}{\partial \theta} \end{bmatrix}$$

当我们做外积（Outer Product） $\mathbf{J} \mathbf{J}^T$ 时：

$$\mathbf{J} \mathbf{J}^T = \begin{bmatrix} (\frac{\partial C}{\partial x})^2 & \color{red}{\frac{\partial C}{\partial x}\frac{\partial C}{\partial y}} & \color{red}{\frac{\partial C}{\partial x}\frac{\partial C}{\partial \theta}} \\ \color{red}{\frac{\partial C}{\partial y}\frac{\partial C}{\partial x}} & (\frac{\partial C}{\partial y})^2 & \color{red}{\frac{\partial C}{\partial y}\frac{\partial C}{\partial \theta}} \\ \color{red}{\frac{\partial C}{\partial \theta}\frac{\partial C}{\partial x}} & \color{red}{\frac{\partial C}{\partial \theta}\frac{\partial C}{\partial y}} & (\frac{\partial C}{\partial \theta})^2 \end{bmatrix}$$