增广拉格朗日时空联合规划ALTRO-iLQR （一）

ALTRO 算法

本文档旨在介绍 ALTRO 算法的背景知识，并帮助读者更好地理解源代码。我们会刻意将算法推导和代码结构设计背后的动机交织在一起。

概览

轨迹优化是一种强大的机器人控制框架，其核心价值在于高度的通用性：只要系统动力学满足马尔可夫性质，就可以应用此方法。所谓马尔可夫动力学，即

$$\dot{x}=f(x,u)$$

其中 $\dot{x}\in {\mathbb{R}}^n$ 是状态 $x\in {\mathbb{R}}^n$ 的时间导数，$u\in {\mathbb{R}}^m$ 是控制输入。

轨迹优化要解决的连续最优控制问题可表示为

$$\begin{array}{rl}\underset{x(t),u(t)}{\mathrm{minimize}}& {\ell }_T\left(x_T\right)+{\int }_0^T\ell (x(t),u(t))dt,\\ \text{subject to}& \dot{x}(t)=f(x(t),u(t)),\\ & g_i(x(t),u(t))\le 0\\ & h(x(t),u(t))=0\end{array}$$

其中：

• ${\ell }_T(x_T)$ 是终端代价；
• $\ell (x,u)$ 是运行代价；
• $g_i(\cdot )\le 0$ 表示广义锥约束；
• $h(\cdot )=0$ 表示等式约束。

为了数值实现，最常见的做法是时间离散化：将区间 $[0,T]$ 等分为 $N$ 段，每段时长 $h=T/N$，得到 $N+1$ 个“结点”（knot points）。令 $x_k\approx x(kh),u_k\approx u(kh)$，则动力学差分方程写成

$$x_{k+1}=f(x_k,u_k,\Delta t).$$

相应的离散最优控制问题即

$$\begin{array}{rl}\underset{x_{0:N},u_{0:N-1}}{\mathrm{minimize}}& {\ell }_N(x_N)+\sum _{k=0}^{N-1}{\ell }_k(x_k,u_k,dt),\\ \text{subject to}& x_{k+1}=f(x_k,u_k),k=0,\dots ,N-1,\\ & g_k(x_k,u_k)\le 0,k=0,\dots ,N,\\ & h_k(x_k,u_k)=0,k=0,\dots ,N.\end{array}$$

求解此类离散问题的方法大致可分两类：

1. 直接方法（Direct Methods）
   将所有 $x_k,u_k$ 作为决策变量，借助通用 NLP 求解器（如 SNOPT、IPOPT）。常用隐式积分，提高数值稳定性，但依赖大型闭源库，计算较慢。

2. 间接方法（Indirect Methods）
   利用问题的马尔可夫结构，显式在前向仿真中满足动力学，代表有 DDP 和 iLQR。每次迭代将非线性目标与约束在线性/二次近似后，求解一系列 LQR 子问题，速度快、内存少，适合嵌入式场景。历史上对非线性约束处理较弱，但近年已有多款高质量开源实现（如 OCS2、Crocoddyl）。

本库实现了增广拉格朗日 iLQR（Augmented Lagrangian iLQR，亦称 AL‑iLQR），即原始 ALTRO 算法的核心（参见 原始论文 和 Julia 实现）。下文先介绍增广拉格朗日方法，再给出 AL‑iLQR 的推导。

---

符号约定

• ${\nabla }_{x}f(\cdot )\in {\mathbb{R}}^n$：函数 $f$ 对状态 $x\in {\mathbb{R}}^n$ 的偏导（列向量）。

• ${\nabla }_{xx}^{2}f(\cdot )\in {\mathbb{R}}^{n\times n}$：若 $f(x)\in \mathbb{R}$，则为 Hessian 矩阵。

• 当 $f:{\mathbb{R}}^n\to {\mathbb{R}}^m$ 时，二阶导是三阶张量。为简化处理，我们只取“Jacobian–transpose–vector”形式：

  $${\nabla }_{xx}^{2}f(\cdot ,b)\in {\mathbb{R}}^{n\times n},b\in {\mathbb{R}}^m.$$

---

增广拉格朗日方法

考虑带锥约束的优化问题

$$\begin{array}{rl}\underset{x}{\mathrm{minimize}}& f(x),\\ \text{subject to}& g_i(x)\le 0,i=0,\dots ,n_K-1.\end{array}$$

增广拉格朗日法将约束通过拉格朗日乘子和二次罚项“移入”到目标中，构造增广拉格朗日：

$${\mathcal{L}}_{\rho }(x,\lambda )=f(x)+\sum _{i=0}^{n_K-1}\left[{\lambda }_i^T{g}_i(x)+\frac{1}{2\rho }(\Vert {\Pi }_{K_i^{\ast }}({\lambda }_i-\rho g_i(x)){\Vert }_2^2-\Vert {\lambda }_i{\Vert }_2^2)\right],$$

• ${\lambda }_i$ 是第 $i$ 个约束的拉格朗日乘子；
• $\rho >0$ 是罚因子；
• ${\Pi }_{K_i^{\ast }}$ 表示投影到对偶锥 $K_i^{\ast }$。

该计算在 augmented_lagrangian::ALCost::Evaluate 和 constraints::ConstraintValues::AugLag 中实现。当前支持的锥有：

• ZeroCone（零锥，对应等式约束）
• NegativeOrthant（负正交锥，对应不等式约束）

若仅有等式约束，上式可化简为

$${\mathcal{L}}_{\rho }=f(x)-{\lambda }^{T}g(x)+\frac{\rho }{2}g(x{)}^{T}g(x).$$

---

对偶变量更新

在每次内层无约束优化（最小化 ${\mathcal{L}}_{\rho }$）后，对偶变量按

$${\lambda }_i\leftarrow {\Pi }_{K_i^{\ast }}({\lambda }_i-\rho g_i(x))$$

更新。该逻辑在 constraints::ConstraintValues::UpdateDuals 及
augmented_lagrangian::AugmentedLagrangianiLQR::UpdateDuals 中实现。

---

罚因子更新

理论上，当 $\rho$ 以几何级数增长时可实现超线性收敛。实践中通常按常数因子 $\varphi \in [2,10]$ 更新：

$$\rho \leftarrow \varphi \rho .$$

---

增广拉格朗日 iLQR

有了增广拉格朗日背景，下面推导 AL‑iLQR。DDP 和 iLQR 的区别仅在于是否包含动力学二阶项（即 Gauss–Newton 近似）。

反向过程（Backward Pass）

我们将离散问题改写为增广拉格朗日形式的无约束优化：

$$\begin{array}{rl}\underset{x_{0:N},u_{0:N-1}}{\mathrm{minimize}}& {\mathcal{L}}_N(x_N)+\sum _{k=0}^{N-1}{\mathcal{L}}_k(x_k,u_k),\\ \text{subject to}& x_{k+1}=f(x_k,u_k).\end{array}$$

定义在时刻 $k$ 的状态函数

$$V_k(x)=\underset{u_{k:N-1}}{\min }\{{\mathcal{L}}_N+\sum _{j=k}^{N-1}{\mathcal{L}}_j(x_j,u_j)\},x_{j+1}=f(x_j,u_j).$$

根据贝尔曼最优性原理，$k$时刻状态价值函数$V_k(x)$等于状态$x$下取得最优的$u$使得动作价值函数$Q_k(x,u)$最小的值。
动作价值函数等于目前状态$x$和输入$u$下获取到函数${\mathcal{L}}_k$的值，加上下一个状态下状态价值函数$V_{k+1}$的值。

$$V_k(x)=\underset{u}{\min }{Q}_k(x,u),Q_k(x,u)={\mathcal{L}}_k(x,u)+V_{k+1}(f_k(x,u)).$$

在当前位置做二次近似：

$$V_k(x)\approx V_k({\bar{x}}_k)+p_k^T\delta x_k+\frac{1}{2}\delta x_k^T{P}_k\delta x_k,\delta x_k=x-{\bar{x}}_k.$$

终端条件：

$$\begin{array}{rl}{P}_N& ={\nabla }_{xx}^2{\mathcal{L}}_N({\bar{x}}_N),\\ p_N& ={\nabla }_x{\mathcal{L}}_N({\bar{x}}_N),\end{array}$$

在 ilqr::KnotPointFunctions::CalcTerminalCostToGo 中实现。

然后对 $Q_k$ 做二阶展开：

$$\begin{array}{rl}{Q}_k(x,u)\approx & Q_k({\bar{x}}_k,{\bar{u}}_k)\\ & +{\left[\begin{array}{c}\delta x_k\\ \delta u_k\end{array}\right]}^T\left[\begin{array}{cc}{Q}_{xx}& Q_{xu}\\ Q_{ux}& Q_{uu}\end{array}\right]\left[\begin{array}{c}\delta x_k\\ \delta u_k\end{array}\right]+{\left[\begin{array}{c}{Q}_x\\ Q_u\end{array}\right]}^T\left[\begin{array}{c}\delta x_k\\ \delta u_k\end{array}\right],\end{array}$$

其中

$$\begin{array}{rl}{Q}_{xx}& ={\nabla }_{xx}^2{\mathcal{L}}_k+{\nabla }_x{f}^T{P}_{k+1}{\nabla }_{x}f+{\nabla }_{xx}^{2}f(\cdot ,p_{k+1}),\\ Q_{xu}& ={\nabla }_{xu}^2{\mathcal{L}}_k+{\nabla }_x{f}^T{P}_{k+1}{\nabla }_{u}f+{\nabla }_{xu}^{2}f(\cdot ,p_{k+1}),\\ Q_{uu}& ={\nabla }_{uu}^2{\mathcal{L}}_k+{\nabla }_u{f}^T{P}_{k+1}{\nabla }_{u}f+{\nabla }_{uu}^{2}f(\cdot ,p_{k+1}),\\ Q_x& ={\nabla }_x{\mathcal{L}}_k+{\nabla }_x{f}^T{p}_{k+1},\\ Q_u& ={\nabla }_u{\mathcal{L}}_k+{\nabla }_u{f}^T{p}_{k+1}.\end{array}$$

在 ilqr::KnotPointFunctions::CalcActionValueExpansion 中实现。若去掉张量项，即为 iLQR 的 Gauss–Newton 近似。

令 $\partial Q/\partial \delta u=0$ 可得局部最优控制偏差：

$$\delta u_k^{\ast }=-(Q_{uu}+\mu I{)}^{-1}(Q_{ux}\delta x_k+Q_u)=K_k\delta x_k+d_k,$$

其中 $\mu >0$ 是自适应正则化参数。正则化与增减策略在以下函数中实现：

• ilqr::KnotPointFunctions::RegularizeActionValue
• ilqr::KnotPointFunctions::CalcGains
• ilqr::iLQR::IncreaseRegularization
• ilqr::iLQR::DecreaseRegularization

当 Cholesky 分解失败或前向步无法有效降成本时，会自动调整 $\mu$。