本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

学习融合¶

难度: ⭐⭐⭐~⭐⭐⭐⭐ | 前置: 足式/60_QP_NLP建模 NLP/QP, 足式/80_接触力学与约束优化接触力学与互补约束, 足式/100_DDP家族与Crocoddyl DDP, 足式/140_落脚点规划经典方法落脚点经典方法 | 学时: 20-25 小时

前置自测¶

📋 前置自测（答不出 >= 2 题 → 先回对应章节复习）

互补约束 $0 \le \phi \perp \lambda \ge 0$ 的三个组成条件分别是什么？其几何含义是什么？（→ 足式/80_接触力学与约束优化）
**NLP 的 KKT 条件**中，互补松弛条件 $\mu_i g_i(x) = 0$ 的物理含义是什么？为什么它会导致数值困难？（→ 足式/60_QP_NLP建模）
DDP 的 backward pass 中，$Q_{xx}, Q_{xu}, Q_{uu}$ 矩阵分别代表什么？为什么 $Q_{uu}$ 必须正定？（→ 足式/100_DDP家族与Crocoddyl）
Raibert 落脚启发式 $\boldsymbol{p}_f = \boldsymbol{p}_{\text{hip}} + \frac{T_s}{2}\dot{\boldsymbol{p}} + k(\dot{\boldsymbol{p}} - \dot{\boldsymbol{p}}_{\text{ref}})$ 中每一项的物理意义是什么？它在什么场景下会失效？（→ 足式/140_落脚点规划经典方法）
LICQ（线性独立约束品性） 失效时，NLP 求解器会出现什么问题？（→ 足式/60_QP_NLP建模）

本章目标¶

学完本章，你应能：

理解"接触作为决策变量"这一范式转变——从足式/110_OCS2完整栈与双线程MPC-58 的预定义步态到让优化器自主发现接触
完整写出 Contact-Implicit TO 的 NLP 公式——包括互补约束嵌入、变量结构、约束雅可比
掌握互补约束的三大数值处理方法——松弛法、Fischer-Burmeister 函数、增广拉格朗日法——并理解各自的收敛性与适用场景
精读 Posa 2014 IJRR 论文——理解 time-stepping + LCP 嵌入的技术细节
理解变分接触隐式优化——Manchester 2019 如何利用变分积分器提高物理忠实度
从双层优化视角统一理解接触规划——上层选接触序列、下层优化轨迹
了解学习驱动的前沿方法——RL、扩散模型、基础模型在接触规划中的应用
在 Towr 上实践 CITO 工程实现——Winkler 的 phase-duration 参数化

本章知识导航¶

本章的逻辑主线是一条从"问题为什么难"到"怎么把它解出来"再到"怎么把它跑快"的递进链。下图给出全章的知识结构与依赖关系：

                  ┌─────────────────────────────────────────┐
                  │ 59.1 范式转变：接触从"输入"变为"决策变量"   │  ← 动机层
                  └────────────────────┬────────────────────┘
                                       │
          ┌────────────────────────────┼────────────────────────────┐
          ▼                            ▼                            ▼
  ┌───────────────┐          ┌──────────────────┐         ┌──────────────────┐
  │ 59.2 CITO 公式 │  ──依赖──▶│ 59.3 互补约束数值 │ ──应用──▶│ 59.4 Posa 2014   │  ← 理论层
  │ 时间步进/雅可比 │          │ 松弛/FB/ALM/MPCC │         │ 奠基论文精读      │
  └───────┬───────┘          └──────────────────┘         └──────────────────┘
          │                                                        │
          ▼                                                        ▼
  ┌───────────────┐          ┌──────────────────┐         ┌──────────────────┐
  │ 59.5 变分 CITO │          │ 59.6 质心轨迹优化 │         │ 59.7 双层优化     │  ← 降维/分解层
  │ 辛结构/二阶精度 │          │ 降维加速          │ ──分解──▶│ MIQP+NLP / GCS    │
  └───────────────┘          └──────────────────┘         └──────────────────┘
                                       │                            │
                                       └────────────┬───────────────┘
                                                    ▼
                            ┌──────────────────────────────────────┐
                            │ 59.8 学习驱动  +  59.9 Towr 工程实现   │  ← 加速/落地层
                            │ RL/扩散/warm-start    Ifopt+Ipopt      │
                            └──────────────────────────────────────┘

四个认知层次：

层次	小节	解决的核心问题	关键认知
动机层	59.1	为什么启发式不够用？	接触是决策变量而非输入
理论层	59.2–59.5	怎么把接触编码进优化？	互补约束 + 它带来的数值噩梦及解法
分解层	59.6–59.7	全身 CITO 太慢怎么办？	降维（质心）+ 分层（双层/GCS）
加速/落地层	59.8–59.9	怎么逼近实时、怎么落地？	学习 warm-start + Towr 工程实现

两条推荐阅读路径：

理论研究路径（想做 CITO 方向博士）：59.1 → 59.2 → 59.3（精读三种方法）→ 59.4（Posa 精读）→ 59.5（变分）→ 59.7（双层 + GCS）。重点啃透互补约束的 MPCC 理论（59.3.5）。
工程落地路径（想在机器人上跑通）：59.1 → 59.2.1（只看 NLP 结构）→ 59.6（质心降维）→ 59.9（Towr 实操）→ 59.8.2（学习 warm-start）。59.3/59.5 可速读。

前置知识桥接¶

本章是整个足式优化控制栈的"集大成"章节，几乎复用了前面所有理论章的工具。下表把每个前置概念的核心要点重述一遍，并说明本章如何复用——这样你不必翻回前章也能跟上：

前置章节	你需要记住的核心要点	本章如何复用
足式/60_QP_NLP建模	NLP 的 KKT 条件由"驻点 + 原始可行 + 对偶可行 + 互补松弛"四部分组成；LICQ（约束梯度线性独立）是 KKT 成立的前提	59.3 的全部内容建立在"互补约束破坏 LICQ"这一观察上；理解 KKT 才能理解为什么 Ipopt 会崩
足式/80_接触力学与约束优化	接触建模为互补条件 $0 \le \phi \perp \lambda \ge 0$；摩擦用库仑锥 $\\|\boldsymbol{\lambda}^t\\| \le \mu\lambda^n$；质心动量方程把全身动力学投影到 6 维	59.2 把这些互补条件"展平"为 NLP 约束；59.6 直接用质心动量方程做降维优化
足式/100_DDP家族与Crocoddyl	DDP 的 backward pass 计算 $Q_{xx},Q_{ux},Q_{uu}$；$Q_{uu}$ 须正定才能求控制增量；shooting 法的决策变量只有控制序列	59.4 对比 shooting（DDP）与 direct collocation（Posa）；ACAL-iLQR（小结）把 ALM 嵌入 DDP backward pass
足式/110_OCS2完整栈与双线程MPC	MPC 的接触序列 ModeSchedule 是输入；双线程架构 1 Hz 重规划 + 高频 MPC	59.1 的范式转变正是把 ModeSchedule 从输入变为决策变量；全章的"离线 CITO + 在线 MPC"分层正是 OCS2 双线程的推广
足式/140_落脚点规划经典方法	Raibert 启发式 $\boldsymbol{p}_f = \boldsymbol{p}_{\text{hip}} + \frac{T_s}{2}\dot{\boldsymbol{p}} + k(\dot{\boldsymbol{p}}-\dot{\boldsymbol{p}}_{\text{ref}})$ 是逐腿独立、无全局视野的工程折中	59.1 用 stepping stones 场景展示 Raibert 的天花板，引出优化驱动

如果跳过本章会怎样¶

场景	不学本章的后果
让四足跨越 stepping stones	只会用 Raibert + 感知修正，落脚点频繁落到石头边缘或空地，机器人踉跄摔倒——因为你不知道"接触位置本身应该被优化"
读到一篇 CITO 论文（如 Posa 2014 / Winkler TOWR）	看到 $\phi \cdot \lambda = 0$ 一脸茫然，不理解为什么作者要花半篇论文讲"松弛"——因为你不知道互补约束会破坏 LICQ
调一个不收敛的轨迹优化器	看到 Ipopt 报 "Restoration Failed" 只会随机调参，不知道根因可能是互补约束的数值病态，也不知道换 FB 函数或 ALM
设计一个跨越障碍的运动规划系统	试图用单个 NLP 同时优化"踩哪 + 怎么走"，陷入组合爆炸，永远收敛不到全局最优——因为你不知道双层分解 / GCS 这条路

预计阅读时间¶

模式	时间	覆盖范围
精读（动手推公式 + 跑代码）	20–25 小时	全章 + 累积项目 M1–M4 + 跨章综合题
速读（理解思路，不抠每步推导）	6–8 小时	59.1–59.4、59.6–59.7 正文 + 所有"本质洞察"和"陷阱"
速查（用时回查）	1 小时	方法选择决策树 + 三种互补方法对比表 + 故障排查手册 + API 速查表

59.1 从经典到优化：为什么需要优化驱动 ⭐¶

动机：启发式方法的天花板¶

本节解决的问题：足式/110_OCS2完整栈与双线程MPC-58 构建了一套完整的足式运动控制栈——步态管理器给定接触时序、Raibert 启发式计算落脚点、MPC 优化连续轨迹、WBC 输出关节扭矩。这套架构在平坦或轻度不平坦地形上工作良好。但当任务复杂度超过一定阈值时，这套"人类预设规则 + 优化器填细节"的范式就会触及根本性的天花板。

让我们用一个具体场景感受这个天花板：

场景：四足机器人穿越 stepping stones

地面布局（俯视图）:

  起点 ●                                    ● 终点
       [石1]  [石2]       [石4]  [石5]
              [石3]  [石6]       [石7]
                          [石8]

Raibert 启发式的困境：

困境	原因	后果
不知道踩哪块石头	Raibert 公式只管"步幅"，不管"地形"	可能落脚到空地上
不知道哪条路径最优	启发式没有全局视野	可能选了一条走不通的路
不知道何时切换步态	步态是预定义的 trot/walk	某些间距需要 gallop 才能跨过
不知道如何协调四条腿	Raibert 对每条腿独立计算	四条腿的落脚点可能导致动力学不可行

反面推演：如果强行用 Raibert + MPC

Step 1: Raibert 计算落脚点 → 落在空地上（没有石头）
Step 2: 感知修正 → 找最近的石头，但离得远 → 需要拉伸步幅
Step 3: MPC 发现步幅太大 → 接触力不够（摩擦锥约束违反）
Step 4: 机器人踉跄 → 重新规划 → 但下一步更远
Step 5: 级联失效 → 机器人摔倒

根因：每一步独立决策，没有全局最优性

优化驱动的核心思想¶

让优化器同时决定"何时接触"、"在哪接触"、"接触力多大"，把离散的接触选择和连续的运动轨迹统一在一个优化问题中。

这个思想可以用一句话总结：

\[\boxed{\text{Contact as Decision Variable, not as Input}}\]

在足式/110_OCS2完整栈与双线程MPC-58 中，接触序列（ModeSchedule）是 MPC 的**输入**。而在本章中，接触序列是优化问题的**决策变量**——这是根本性的范式转变。

什么时候需要优化驱动？¶

并非所有场景都需要这个重型工具。下表给出决策指南：

场景	推荐方法	理由
平坦地面 trot 行走	Raibert + MPC	简单高效，实时性好
轻度不平坦地形	感知修正 Raibert + MPC	启发式修正足够
stepping stones	MIQP / CITO	需要全局规划接触位置
未知地形跳跃	CITO	需要发现新颖的接触策略
非周期动作（翻越障碍）	CITO + 学习	无预定义步态可用
多接触操作（抓取+行走）	CITO	手/脚/物体接触耦合

历史脉络：从动画到机器人¶

接触优化的思想并非起源于机器人学，而是**计算机图形学**。2012 年，Mordatch、Todorov 和 Popovic 在 SIGGRAPH 发表了开创性的 Contact-Invariant Optimization（CIO），展示了仿真角色仅通过优化就能"发现"爬行、站立、翻身等复杂行为——完全不需要人类设计步态。

历史演进:
2005 ─ Stewart & Trinkle ─ LCP time-stepping 成为标准接触仿真方法
       │
2012 ─ Mordatch et al. ─ Contact-Invariant Optimization (SIGGRAPH)
       │                  → 证明优化可以发现复杂接触行为
       │
2014 ─ Posa, Cantu, Tedrake ─ Contact-Implicit TO (IJRR)
       │                       → 将 LCP 嵌入 NLP，奠定 CITO 数学基础
       │
2018 ─ Winkler et al. ─ TOWR (RA-L)
       │                 → phase-duration 参数化，工程可用
       │
2020 ─ Manchester, Kuindersma ─ 变分 CITO (IJRR)
       │                         → 变分积分器提高物理忠实度
       │
2023 ─ Pang et al. ─ Quasi-Dynamic Smoothing (T-RO)
       │              → 局部光滑化，10x 加速
       │
2024 ─ ACAL-iLQR ─ Contact-Implicit iLQR + 增广拉格朗日
       │            → 无需固定接触序列的 iLQR
       │
2025 ─ Diffusion + CITO ─ 扩散模型生成接触计划初值
                           → 学习与优化融合前沿

本章的四条技术路线¶

                    ┌─────────────────────────────┐
                    │  接触规划问题                  │
                    │  "何时何地接触？"              │
                    └──────────┬──────────────────┘
           ┌──────────┬───────┴───────┬──────────┐
           ▼          ▼               ▼          ▼
    ┌──────────┐ ┌──────────┐ ┌──────────┐ ┌──────────┐
    │ CITO     │ │ 双层优化  │ │ 质心轨迹  │ │ 学习驱动  │
    │ 互补约束  │ │ MIQP+NLP │ │ 协同优化  │ │ RL/扩散   │
    │ 嵌入 NLP │ │ 分层解耦  │ │ 连续松弛  │ │ 端到端    │
    └──────────┘ └──────────┘ └──────────┘ └──────────┘
    §59.2-59.5    §59.7        §59.6        §59.8

⚠️ 概念误区：CITO 可以实时运行

新手想法："CITO 能自动发现接触，那就直接替代 MPC 吧！"

实际上：当前最快的 CITO 在四足机器人上仍需 1-10 秒求解（Posa 2014 报告 10-100 秒；Pang 2023 在操作任务上约 1 分钟；ACAL-iLQR 2024 对简单系统约 1 秒）。MPC 要求 5-50 毫秒。两者差 2-3 个数量级。

正确理解：CITO 是**离线规划**工具，生成的轨迹交给在线 MPC 跟踪。或者 CITO 在 1 Hz 重规划层运行，MPC 在 50 Hz 跟踪层运行。

唯一例外：Aydinoglu 2023 在低自由度机械臂上实现了约 50 ms 的 Contact-Implicit MPC，但四足机器人的维度（37 维状态）远超目前能力。

练习¶

练习 59.1a（⭐）：列举三个 Raibert 启发式无法处理但 CITO 可以处理的场景，并解释为什么启发式会失败。

练习 59.1b（⭐⭐）：如果 CITO 求解需要 5 秒，MPC 跟踪频率 50 Hz，地形每 2 秒变化一次，设计一个分层架构来融合两者。画出架构图并标注每层的频率和输入/输出。

59.2 Contact-Implicit TO 数学基础 ⭐⭐⭐¶

动机：为什么要把接触"隐含"在优化中？¶

本节解决的问题：如何把"何时接触"和"接触力多大"统一编码为一个 NLP，让求解器同时决定接触模式和运动轨迹？

**传统方法（显式接触）**的做法：人类指定接触序列（如 trot 步态的摆动/支撑时序），然后在每个模式段内分别优化。这等价于枚举所有可能的接触模式序列，对每种序列解一个 NLP，再选最好的——组合爆炸。

反面推演：枚举为什么不可行？

一条四足机器人的腿有两种状态: 支撑(1) 或 摆动(0)
四条腿 → 每个时间步有 2^4 = 16 种模式
10 个时间步 → 16^10 ≈ 10^12 种模式序列
每种序列解一个 NLP (假设 1 秒) → 需要 31,000 年

即使只考虑"合理"的序列，仍然是 NP-hard

CITO 的核心洞察：不枚举模式，而是**把接触力和距离作为连续决策变量**，让互补约束自动决定"有没有接触"：

如果优化器发现 $\lambda_k > 0$（有接触力），则互补约束迫使 $\phi_k = 0$（脚在地面上）
如果优化器发现 $\phi_k > 0$（脚离地），则互补约束迫使 $\lambda_k = 0$（无接触力）

接触模式从**输入**变成了**输出**——由优化器自主发现。

59.2.1 完整 NLP 公式 ⭐⭐⭐¶

CITO 将接触动力学轨迹优化问题写成如下 NLP：

\[\boxed{\begin{aligned} \min_{\mathbf{x}_{0:N},\, \mathbf{u}_{0:N-1},\, \boldsymbol{\lambda}_{0:N-1}} \quad & \sum_{k=0}^{N-1} l_k(\mathbf{x}_k, \mathbf{u}_k) + l_N(\mathbf{x}_N) \\[6pt] \text{s.t.} \quad & \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k, \boldsymbol{\lambda}_k) & \text{(离散动力学)} \\ & \phi_i(\mathbf{x}_k) \ge 0, \quad \forall i \in \mathcal{C} & \text{(非穿透)} \\ & \lambda_i^n{}_k \ge 0, \quad \forall i \in \mathcal{C} & \text{(法向力非负)} \\ & \phi_i(\mathbf{x}_k) \cdot \lambda_i^n{}_k = 0, \quad \forall i \in \mathcal{C} & \text{(法向互补)} \\ & \|\boldsymbol{\lambda}_i^t{}_k\| \le \mu_i \lambda_i^n{}_k & \text{(摩擦锥)} \\ & \gamma_i \ge 0, \quad \boldsymbol{\lambda}_i^t{}_k + \gamma_i \frac{\mathbf{v}_i^t{}_k}{\|\mathbf{v}_i^t{}_k\|} = 0 & \text{(最大耗散)} \\ & \mathbf{x}_0 = \mathbf{x}_{\text{init}}, \quad h(\mathbf{x}_N) = 0 & \text{(边界条件)} \end{aligned}}\]

其中： - $\mathbf{x}_k = [\mathbf{q}_k^T, \dot{\mathbf{q}}_k^T]^T$ 是广义位置和速度 - $\mathbf{u}_k$ 是关节扭矩 - $\boldsymbol{\lambda}_k = [\lambda_1^n, \boldsymbol{\lambda}_1^t, \dots, \lambda_{n_c}^n, \boldsymbol{\lambda}_{n_c}^t]$ 是所有潜在接触点的法向力和切向力（连续时间公式中 $\boldsymbol{\lambda}$ 为力；离散时间步进公式中 $\boldsymbol{\lambda}$ 常指冲量 $\boldsymbol{\Lambda} = h\mathbf{f}$，详见 59.2.2） - $\mathcal{C}$ 是**所有潜在**接触点的集合（不仅仅是当前接触的点） - $\phi_i(\mathbf{x}_k)$ 是第 $i$ 个接触点到地面的有符号距离 - $\mu_i$ 是摩擦系数 - $\gamma_i$ 是最大耗散原理中的辅助变量 - $\mathbf{v}_i^t$ 是接触点的切向滑移速度

这个公式要解决什么问题？ 它把传统轨迹优化中"给定接触序列，优化连续变量"的范式，推广为"同时优化接触决策和连续轨迹"。关键创新是把互补约束 $\phi \cdot \lambda^n = 0$ 作为 NLP 约束——这个约束**隐式地**编码了接触模式的切换。

它是怎么被推导出来的？ 起点是 Stewart-Trinkle 的 LCP 接触仿真公式（足式/80_接触力学与约束优化）。在正向仿真中，给定状态 $\mathbf{x}_k$ 和控制 $\mathbf{u}_k$，接触力 $\boldsymbol{\lambda}_k$ 通过求解 LCP 得到。Posa 的洞察是：不在内层求解 LCP，而是把 LCP 的条件"展平"为 NLP 的约束——这样接触力变成了与状态、控制地位平等的决策变量。

如果没有互补约束会怎样？ 没有 $\phi \cdot \lambda^n = 0$ 的话，优化器可以自由地在 $\phi > 0$（脚在空中）时施加接触力 $\lambda^n > 0$——等于"隔空推力"，物理上不可能。互补约束正是排除了这种非物理解。

变量规模分析：

变量类别	维度	ANYmal 示例
状态 $\mathbf{x}_k$	$2n_q$	$2 \times 19 = 38$（含浮动基座）
控制 $\mathbf{u}_k$	$n_u$	$12$（12 个关节）
接触力 $\boldsymbol{\lambda}_k$	$3 n_c$	$3 \times 4 = 12$（4 个足端）
每步决策维度	$2n_q + n_u + 3n_c$	$38 + 12 + 12 = 62$
N=50 步总维度	$N \times (2n_q + n_u + 3n_c) + 2n_q$	$\approx 3138$

对比足式/100_DDP家族与Crocoddyl 的 DDP（不含接触力变量）：$N \times (2n_q + n_u) + 2n_q = 2538$。CITO 多了 $N \times 3n_c = 600$ 个接触力变量，代价是约 24% 的维度增长——但带来了接触自主决策的能力。

59.2.2 离散动力学：时间步进法（Time-Stepping） ⭐⭐⭐¶

CITO 的离散动力学不是简单的欧拉或 RK4 积分。因为接触力可能在一个时间步内突然出现或消失（碰撞），需要专门的**时间步进（time-stepping）**格式。

为什么需要专门的格式？ 碰撞在数学上是一个**不连续事件**——速度在碰撞瞬间发生跳变。标准 ODE 积分器（欧拉、RK4）假设右端函数连续，无法正确处理速度跳变。时间步进法的核心思想是：在每个时间步内，允许速度不连续，用冲量（而非瞬时力）来描述接触效应。

Stewart-Trinkle 时间步进公式（1996）：

\[\boxed{\begin{aligned} \mathbf{M}(\mathbf{q}_k)(\dot{\mathbf{q}}_{k+1} - \dot{\mathbf{q}}_k) &= h[\boldsymbol{\tau}_k - \mathbf{C}(\mathbf{q}_k, \dot{\mathbf{q}}_k)\dot{\mathbf{q}}_k - \mathbf{g}(\mathbf{q}_k)] + \mathbf{J}_c(\mathbf{q}_k)^T \boldsymbol{\lambda}_k \\ \mathbf{q}_{k+1} &= \mathbf{q}_k + h \dot{\mathbf{q}}_{k+1} \end{aligned}}\]

其中 $h$ 是时间步长，$\mathbf{M}$ 是质量矩阵，$\mathbf{J}_c$ 是接触雅可比。

为什么用半隐式（semi-implicit）格式？

注意上式先更新速度，再用新速度更新位置——这是半隐式欧拉。对比三种选择：

积分格式	速度更新	位置更新	稳定性	能量守恒
显式欧拉	$\dot{q}_{k+1} = \dot{q}_k + h a_k$	$q_{k+1} = q_k + h \dot{q}_k$	差（发散）	能量增长
半隐式	$\dot{q}_{k+1} = \dot{q}_k + h a_k$	$q_{k+1} = q_k + h \dot{q}_{k+1}$	好	近似守恒
隐式欧拉	需解方程	需解方程	很好	能量耗散

半隐式欧拉是 CITO 的事实标准，因为它在**稳定性**和**计算成本**之间取得了最佳平衡。显式欧拉在接触问题中会因为能量增长而发散（想象一个弹跳球越弹越高）；隐式欧拉虽然稳定但需要在每步内部解一个非线性方程，计算代价太高。

关键细节：接触力是冲量

在时间步进公式中，$\boldsymbol{\lambda}_k$ 实际上是**冲量**（impulse），不是瞬时力。这是因为碰撞在 $h \to 0$ 时力趋于无穷、持续时间趋于零，但冲量有限。对于持续接触，$\boldsymbol{\lambda}_k \approx h \cdot \mathbf{f}_{\text{contact}}$。这个区别在编程时很重要——如果你需要从 CITO 的解中提取接触力（而非冲量），需要除以时间步长 $h$。

59.2.3 约束雅可比的结构 ⭐⭐⭐¶

NLP 求解器（如 Ipopt）需要约束对决策变量的雅可比矩阵。CITO 的约束雅可比有特殊的**块稀疏**结构：

约束雅可比矩阵 J 的结构（N=4 步，3 个约束类型）:

         x0   u0   λ0   x1   u1   λ1   x2   u2   λ2   x3
dyn_0  [ Fx0  Fu0  Fλ0  -I                              ]
dyn_1  [                 Fx1  Fu1  Fλ1  -I               ]
dyn_2  [                              Fx2  Fu2  Fλ2  -I  ]
comp_0 [ ∂(φλ)                                           ]
comp_1 [                 ∂(φλ)                            ]
comp_2 [                              ∂(φλ)               ]
fric_0 [ ∂(μλn-||λt||)                                   ]
fric_1 [                 ∂(μλn-||λt||)                    ]
fric_2 [                              ∂(μλn-||λt||)       ]

→ 带状稀疏！每行只有 2-3 个非零块
→ Ipopt 可以利用稀疏性加速

稀疏性为什么重要？

CITO 的总决策维度约 3000，如果约束雅可比是稠密的，存储需要 $3000^2 \approx 10^7$ 个元素。但由于带状稀疏，实际非零元素只有 $O(N \cdot n^2) \approx 10^5$——减少两个数量级。Ipopt 使用 MA57 或 MUMPS 稀疏分解器，能高效处理这种结构。

⚠️ 编程陷阱：忘记提供稀疏模式给 Ipopt

错误做法：在 Ipopt 的 eval_jac_g 回调中返回稠密矩阵。

症状：求解时间从几秒变成几分钟，内存占用暴增。

根因：Ipopt 默认假设雅可比是稀疏的。如果不提供稀疏模式（iRow, jCol），Ipopt 会退化为稠密运算。

正确做法：在 eval_jac_g 的第一次调用（values == nullptr）时返回非零元素的行列索引；后续调用只填充非零值。Ifopt 框架（Towr 使用的 NLP 接口）会自动处理稀疏模式。

💡 概念误区：CITO 的 NLP 维度"太大"所以不实用

新手想法："3000 维的 NLP？这不可能解得动吧！"

实际上：现代 NLP 求解器（Ipopt 2025 版）配合稀疏线性代数库（MA57, MUMPS）可以在几秒内解 10000+ 维的 NLP——前提是利用了稀疏性。CITO 慢的原因不是维度大，而是互补约束导致的**非凸性**和**LICQ 失效**。去掉互补约束的标准轨迹优化在同样维度下只需 0.1 秒。

练习¶

练习 59.2a（⭐⭐）：对于一个 2D 单腿弹跳机器人（$n_q = 3$：水平位置、竖直位置、腿长；$n_u = 1$：腿力；$n_c = 1$：足底接触），写出完整的 CITO NLP 公式，包括所有约束。计算 $N = 20$ 时的总决策变量数。

练习 59.2b（⭐⭐⭐）：证明 CITO 的互补约束 $\phi_i \cdot \lambda_i^n = 0$ 违反了 LICQ。提示：考虑 $\phi_i = 0, \lambda_i^n = 0$ 的情况（同时为零），计算此点处约束梯度的线性相关性。

练习 59.2c（⭐⭐）：为什么时间步进法用半隐式欧拉而不用 RK4？列举至少两个技术原因。

59.2.4 摩擦锥的 LCP 线性化与最大耗散原理 ⭐⭐⭐¶

本节解决的问题：59.2.1 的 NLP 公式（行内式）写了一条最大耗散约束 $\boldsymbol{\lambda}_i^t + \gamma_i \frac{\mathbf{v}_i^t}{\|\mathbf{v}_i^t\|} = 0$，但没说它从哪来、为什么长这样。这一条约束是整个接触摩擦建模的灵魂——它决定了"脚滑动时摩擦力指向哪里"。本节把它彻底讲清，并解释 Stewart-Trinkle 为什么要把光滑的摩擦锥"切成多面体"。

动机：库仑摩擦不只是"力的大小有上限"¶

回顾足式/80_接触力学与约束优化：库仑摩擦给出的约束是 $\|\boldsymbol{\lambda}^t\| \le \mu\lambda^n$——切向力的模长不超过法向力的 $\mu$ 倍。很多初学者以为这就是摩擦的全部，于是直接把它当作一条不等式塞进 NLP。

反面推演：只用 $\|\boldsymbol{\lambda}^t\| \le \mu\lambda^n$ 会漏掉什么？

考虑一只脚正在地面上**向右滑动**（切向速度 $\mathbf{v}^t$ 指向右）。物理上，动摩擦力必须满足两个条件：

大小：滑动时摩擦力取到最大值，即 $\|\boldsymbol{\lambda}^t\| = \mu\lambda^n$（**等号**成立，落在锥面上，不是锥内部）；
方向：摩擦力必须**反向**于滑动速度，即指向左。

只写 $\|\boldsymbol{\lambda}^t\| \le \mu\lambda^n$ 完全没有约束方向——优化器可以让一只向右滑的脚受到一个**向右**的摩擦力（顺着滑动方向推），这会凭空给系统注入能量，是物理上荒谬的解。这就是为什么必须额外引入一条约束来锁定摩擦力的方向。

最大耗散原理（Maximum Dissipation Principle）¶

锁定方向的原理叫**最大耗散原理**：在所有满足摩擦锥约束 $\|\boldsymbol{\lambda}^t\| \le \mu\lambda^n$ 的切向力中，真实的摩擦力是使**摩擦功率耗散最大**的那一个。形式化为一个内层优化：

\[\boldsymbol{\lambda}^t = \arg\max_{\|\boldsymbol{\lambda}^t\| \le \mu\lambda^n} \; \left( -\boldsymbol{\lambda}^t \cdot \mathbf{v}^t \right)\]

其中 $-\boldsymbol{\lambda}^t \cdot \mathbf{v}^t$ 是摩擦力做的负功率（耗散的功率）。

推导这个 argmax 的解：目标 $-\boldsymbol{\lambda}^t \cdot \mathbf{v}^t$ 关于 $\boldsymbol{\lambda}^t$ 是线性的，可行域是一个半径为 $\mu\lambda^n$ 的圆盘。线性函数在凸集上的最大值必在边界取得，且最优 $\boldsymbol{\lambda}^t$ 与梯度方向 $-\mathbf{v}^t$ 同向、模长顶到边界：

\[\boldsymbol{\lambda}^t = \mu\lambda^n \cdot \frac{-\mathbf{v}^t}{\|\mathbf{v}^t\|} = -\mu\lambda^n \frac{\mathbf{v}^t}{\|\mathbf{v}^t\|} \quad (\text{当 } \|\mathbf{v}^t\| > 0)\]

这正是"摩擦力反向于滑动、大小取满"的数学表达。

怎么把这个内层 argmax 变成 NLP 约束？ 内层是一个凸优化，写出它的 KKT 条件：引入乘子 $\gamma \ge 0$ 对应约束 $\|\boldsymbol{\lambda}^t\| \le \mu\lambda^n$，则驻点条件给出

\[\boldsymbol{\lambda}^t + \gamma \frac{\mathbf{v}^t}{\|\mathbf{v}^t\|} = 0, \qquad \gamma \ge 0, \qquad \gamma\,(\mu\lambda^n - \|\boldsymbol{\lambda}^t\|) = 0\]

这就是 59.2.1 出现的那条约束的来历——它是最大耗散内层优化的一阶最优性条件。乘子 $\gamma$ 有清晰的物理意义：它正是**滑动速率的代理**——只有当脚真的在滑（$\|\mathbf{v}^t\|>0$）时 $\gamma>0$，此时互补条件 $\gamma(\mu\lambda^n-\|\boldsymbol{\lambda}^t\|)=0$ 迫使 $\|\boldsymbol{\lambda}^t\|=\mu\lambda^n$（力顶到锥面）；脚不滑（黏滞）时 $\gamma=0$，力可以落在锥内部。

本质洞察：摩擦不是一条"力的大小上限"约束，而是一个**嵌套的优化问题**——大自然在每个接触点都在悄悄解一个"最大耗散"的小优化。CITO 把这个内层优化的 KKT 条件展平到外层 NLP 里，于是接触点的"滑/不滑"决策和机器人的整体运动被统一求解。这与 59.2.1 把 LCP"展平"成 NLP 是同一种思想的两次运用：把内层求解器替换为内层约束。

Stewart-Trinkle 的多面体摩擦锥（Faceted Friction Cone）¶

上面的最大耗散约束含有 $\frac{\mathbf{v}^t}{\|\mathbf{v}^t\|}$ —— 一个在 $\mathbf{v}^t=0$ 处奇异、且本质非线性的项。Stewart-Trinkle time-stepping 为了把整个接触问题写成一个**线性互补问题（LCP），做了一个关键工程近似：**用多面体（棱锥）逼近光滑的二阶锥摩擦锥。

怎么做？ 在切向平面内取 $d$ 个均匀分布的方向向量 $\mathbf{d}_1,\dots,\mathbf{d}_d$（例如 3D 接触取 $d=4$ 或 $d=8$，对应"东南西北"等方向），把切向力分解为这些方向的非负组合：

\[\boldsymbol{\lambda}^t = \sum_{j=1}^{d} \beta_j \mathbf{d}_j, \qquad \beta_j \ge 0\]

摩擦锥约束 $\|\boldsymbol{\lambda}^t\| \le \mu\lambda^n$ 被近似为线性约束：

\[\sum_{j=1}^{d} \beta_j \le \mu\lambda^n\]

最大耗散原理则离散化为：每个方向分量 $\beta_j$ 与"该方向上的滑动趋势"互补。引入辅助变量 $\zeta$（所有方向共享，代表滑动速率的下确界代理），得到 LCP 形式：

\[\begin{aligned} & 0 \le \beta_j \perp \big(\zeta + \mathbf{d}_j^T \mathbf{v}^t\big) \ge 0, \quad j=1,\dots,d \\ & 0 \le \zeta \perp \big(\mu\lambda^n - \textstyle\sum_j \beta_j\big) \ge 0 \end{aligned}\]

这组互补条件在说什么？ 第一行：方向 $\mathbf{d}_j$ 上只有当它最"逆着"滑动速度时（$\mathbf{d}_j^T\mathbf{v}^t$ 最负）才激活力分量 $\beta_j$；第二行：只要有任何滑动（$\zeta>0$），切向力就顶满摩擦锥（$\sum\beta_j=\mu\lambda^n$）。这恰好是离散化的"反向 + 取满"。

为什么要做这个近似？代价是什么？

方面	光滑二阶锥（true cone）	多面体近似（faceted）
数学形式	二阶锥约束（SOCP / 非线性）	线性约束 + LCP
求解器	需要 SOCP 或 NLP	可用纯 LP/LCP 求解器（Lemke 算法）
摩擦方向	连续任意方向	只能取 $d$ 个离散方向的组合
误差	精确	摩擦力被高估/低估，最大相对误差约 $1-\cos(\pi/d)$
$d\to\infty$	——	收敛到真锥

近似误差随 $d$ 增大而减小：$d=4$ 时摩擦各向异性可达约 30%（沿对角方向摩擦偏大），$d=8$ 降到约 8%。工程权衡：$d$ 越大越精确，但 LCP 维度随 $d$ 线性增长。多数仿真器（含早期 MuJoCo、Bullet）默认 $d=4$ 或在金字塔锥（pyramidal cone）与椭圆锥（elliptic cone）之间提供选项。

💡 概念误区：CITO 里一定要用多面体锥

新手想法："Posa 用了 Stewart-Trinkle，所以 CITO 必须把摩擦锥切成多面体。"

实际上：多面体化是**LCP time-stepping 仿真**的需要——因为 LCP 求解器只吃线性约束。但在 CITO 里，我们用的是**NLP 求解器（Ipopt），它本来就能处理非线性约束。因此现代 CITO 实现（如 Drake、CALIPSO）常常**直接保留光滑二阶锥 $\|\boldsymbol{\lambda}^t\|\le\mu\lambda^n$，避免多面体化带来的各向异性误差。Posa 2014 沿用多面体锥更多是历史原因（直接继承 Stewart-Trinkle 公式）。

正确理解：是否多面体化取决于**下游求解器吃什么**——LCP 求解器要线性，NLP/SOCP 求解器可以非线性。

⚠️ 编程陷阱：$\frac{\mathbf{v}^t}{\|\mathbf{v}^t\|}$ 在零速度处除零

错误做法：直接在最大耗散约束里写 lambda_t + gamma * v_t / v_t.norm()。

症状：脚处于黏滞（stick）状态、切向速度恰好为零时，v_t.norm() 为 0，约束求值返回 NaN，整个 NLP 雅可比污染为 NaN，Ipopt 立即报 "Invalid number in NLP function evaluation"。

根因：黏滞与滑动的过渡点 $\mathbf{v}^t=0$ 是约束的奇异点——这正是多面体化（用 LCP 形式避开归一化）或软化（下式）要解决的。

正确做法：要么改用多面体 LCP 形式（无归一化项），要么对范数做软化 $\|\mathbf{v}^t\|\approx\sqrt{\|\mathbf{v}^t\|^2+\varepsilon^2}$，要么直接用光滑锥 $\|\boldsymbol{\lambda}^t\|\le\mu\lambda^n$ 配合 Ipopt（推荐）。

练习¶

练习 59.2.4a（⭐⭐）：对一个 2D 接触点（切向只有一维），最大耗散原理退化为什么形式？写出 $\boldsymbol{\lambda}^t,\gamma,\mathbf{v}^t$ 之间的关系，并解释为什么 2D 情形不需要多面体化。

练习 59.2.4b（⭐⭐⭐）：取 $d=4$ 的多面体摩擦锥，方向向量为 $\pm\mathbf{e}_x,\pm\mathbf{e}_y$。证明这个金字塔锥的内切圆半径为 $\mu\lambda^n/\sqrt{2}$、外接情况下沿对角方向可表示的最大切向力为 $\sqrt{2}\,\mu\lambda^n$。由此估计 $d=4$ 时摩擦力的各向异性误差。

练习 59.2.4c（⭐⭐⭐）：从最大耗散的内层优化 $\max_{\|\boldsymbol{\lambda}^t\|\le\mu\lambda^n}(-\boldsymbol{\lambda}^t\cdot\mathbf{v}^t)$ 出发，用 Lagrange 乘子法完整推导出 59.2.1 中的约束 $\boldsymbol{\lambda}^t+\gamma\frac{\mathbf{v}^t}{\|\mathbf{v}^t\|}=0$ 与互补条件 $\gamma(\mu\lambda^n-\|\boldsymbol{\lambda}^t\|)=0$。

59.3 互补约束的数值处理 ⭐⭐⭐¶

动机：互补约束为什么是数值噩梦？¶

本节解决的问题：CITO 的核心困难不在于动力学约束（那是标准的 NLP），而在于互补约束 $\phi \cdot \lambda = 0$。这个看似简单的约束为什么让所有标准 NLP 求解器头疼？如何处理它？

为什么互补约束这么难？

考虑一维互补约束 $a \ge 0, b \ge 0, a \cdot b = 0$。这个约束定义了一个"L 形"可行域——只有 $a$ 轴和 $b$ 轴上的点是可行的。

b ↑
  |●●●●
  |
  |
  --●●●●--→ a
    可行域 = 两条射线的并集（非凸！）

三个致命问题：

问题	数学描述	后果
非凸性	$\{(a,b): a \ge 0, b \ge 0, ab = 0\}$ 不是凸集	局部最优不等于全局最优
LICQ 失效	在 $a = b = 0$ 处，$\nabla(ab) = [b, a]^T = [0, 0]^T$	KKT 条件不适用，Lagrange 乘子不唯一
非光滑性	可行域在原点处"弯折"	梯度不连续，Newton 法收敛性丧失

为什么 LICQ 失效是致命的？ LICQ（Linear Independence Constraint Qualification）是 KKT 条件成立的前提。在互补约束的"退化点"$\phi = \lambda = 0$ 处（即接触恰好开始/结束的瞬间），约束 $\phi \cdot \lambda = 0$ 的梯度为零向量——与其他约束的梯度线性相关。此时 Lagrange 乘子不唯一，Ipopt 的内点法步长计算会失败。

Ipopt 遇到互补约束会怎样？

Ipopt 是内点法求解器，它把不等式约束转化为等式约束加障碍项：$\min f(x) - \mu \sum \ln(s_i)$, s.t. $g(x) + s = 0$。但互补约束 $ab = 0$ 作为**等式约束**会导致约束雅可比在可行域上秩亏——Ipopt 的线性系统会变得病态，迭代震荡不收敛。

59.3.1 方法一：松弛法（Relaxation） ⭐⭐⭐¶

核心思想：把精确互补 $\phi \cdot \lambda = 0$ 松弛为 $\phi \cdot \lambda \le \epsilon$，其中 $\epsilon > 0$ 是小量。

\[\phi_i \cdot \lambda_i^n = 0 \quad \xrightarrow{\text{松弛}} \quad \phi_i \cdot \lambda_i^n \le \epsilon\]

这个方法要解决什么问题？ 把 MPCC（Mathematical Program with Complementarity Constraints）——一类结构上病态的优化问题——转化为标准的 NLP——可以用 Ipopt 等成熟求解器处理。

为什么这能帮助？

松弛后的可行域从"L 形"变成了一个包含 L 形的"厚 L 形"——在原点附近有一个 $\epsilon$ 大小的"缓冲区"
在缓冲区内，LICQ 恢复——约束雅可比满秩
Ipopt 可以正常工作

如果不做松弛会怎样？ 直接把 $\phi \cdot \lambda = 0$ 作为等式约束传给 Ipopt，求解器会在接触切换点附近反复震荡——线性系统条件数爆炸（$10^{10}$ 以上），Newton 步方向错误，最终报 "EXIT: Restoration Failed" 或 "EXIT: Converged to a point of local infeasibility"。

续延策略（Continuation / Homotopy）：

逐步减小 $\epsilon$，让解从松弛问题"滑"向精确问题：

Algorithm: Relaxation with Continuation
────────────────────────────────────
Input: 初始猜测 z0, 松弛序列 ε₁ > ε₂ > ... > ε_final
Output: CITO 解 z*

for k = 1, 2, ... do
    z* ← Ipopt.solve(NLP(εk), warm_start = z_{k-1})
    if εk <= ε_final:
        return z*
    end if
end for

续延策略的选择：

策略	$\epsilon$ 更新	优点	缺点
固定比率	$\epsilon_{k+1} = 0.1 \cdot \epsilon_k$	简单	可能跳过解
自适应	根据 KKT 残差调整	稳健	实现复杂
Ipopt 内置	利用 Ipopt 的 barrier parameter	代码量最少	收敛可能慢

Posa 2014 用的就是这个方法——初始 $\epsilon = 10^{-1}$，最终 $\epsilon = 10^{-4}$，通常需要 3-5 轮续延。

59.3.2 方法二：Fischer-Burmeister 函数 ⭐⭐⭐¶

来龙去脉：Fischer-Burmeister (FB) 函数是 1992 年 Andreas Fischer 提出的一种**非线性互补问题（NCP）函数**。它能把互补约束 $a \ge 0, b \ge 0, ab = 0$ 等价地转化为一个**等式约束**——消除了 LICQ 失效的问题。

它是怎么被发明的？ 互补问题 (NCP) 的研究可以追溯到 1960 年代的线性规划和博弈论。数学家们一直在寻找一种函数 $\psi(a, b)$，满足 $\psi(a, b) = 0 \iff a \ge 0, b \ge 0, ab = 0$。这样的函数称为 NCP 函数。Fischer 1992 年发现了一个优雅的构造——利用欧几里得范数。

定义：

\[\boxed{\psi_{\text{FB}}(a, b) = \sqrt{a^2 + b^2} - a - b}\]

性质（为什么它能替代互补约束？）：

$\psi_{\text{FB}}(a, b) = 0$ 当且仅当 $a \ge 0, b \ge 0, ab = 0$。

推导（完整证明）：

\[\begin{aligned} \psi_{\text{FB}} = 0 &\iff \sqrt{a^2 + b^2} = a + b \\ &\iff a^2 + b^2 = (a + b)^2 = a^2 + 2ab + b^2 \quad \text{(两边平方，注意 $a + b \ge 0$ 时等价)} \\ &\iff 2ab = 0 \\ &\iff ab = 0 \end{aligned}\]

同时，$\sqrt{a^2 + b^2} = a + b$ 要求 $a + b \ge 0$（因为左边非负），加上 $ab = 0$，必然 $a \ge 0, b \ge 0$。

反向验证：若 $a = 3, b = 0$，则 $\psi_{\text{FB}} = \sqrt{9} - 3 - 0 = 0$ ✓。若 $a = 0, b = 5$，则 $\psi_{\text{FB}} = \sqrt{25} - 0 - 5 = 0$ ✓。若 $a = 1, b = 1$，则 $\psi_{\text{FB}} = \sqrt{2} - 2 \approx -0.586 \ne 0$ ✓（不满足互补）。$\square$

直觉理解：$\sqrt{a^2 + b^2}$ 是 $(a, b)$ 到原点的距离，$a + b$ 是沿 45 度线到原点的距离。只有当 $(a, b)$ 落在坐标轴上时，两者相等——这恰好是互补条件的几何含义。

FB 函数相比松弛法的优势：

方面	松弛法	FB 函数
约束类型	不等式 $\phi\lambda \le \epsilon$	等式 $\psi_{\text{FB}} = 0$
LICQ	仍然可能有问题	总是满足（梯度在 $a=b=0$ 处非零）
续延	需要多轮	不需要
梯度	在 $\phi = \lambda = 0$ 处有问题	在 $a = b = 0$ 处梯度存在但不光滑

光滑化 FB 函数：原始 FB 函数在 $a = b = 0$ 处不可微（$\sqrt{a^2 + b^2}$ 的梯度不存在）。光滑化版本：

\[\psi_{\text{FB}}^{\sigma}(a, b) = \sqrt{a^2 + b^2 + \sigma^2} - a - b\]

其中 $\sigma > 0$ 是光滑化参数。$\psi_{\text{FB}}^{\sigma}$ 处处可微，且 $\sigma \to 0$ 时收敛到原始 FB 函数。

梯度（供 NLP 求解器使用）：

\[\frac{\partial \psi_{\text{FB}}^{\sigma}}{\partial a} = \frac{a}{\sqrt{a^2 + b^2 + \sigma^2}} - 1, \quad \frac{\partial \psi_{\text{FB}}^{\sigma}}{\partial b} = \frac{b}{\sqrt{a^2 + b^2 + \sigma^2}} - 1\]

注意这两个偏导数在 $a = b = 0$ 时为 $-1$（而非 0），因此 LICQ 恢复。

59.3.3 方法三：惩罚法与增广拉格朗日法 ⭐⭐⭐¶

惩罚法：把互补约束作为惩罚项加入目标函数：

\[\min_{\mathbf{x}, \mathbf{u}, \boldsymbol{\lambda}} \quad J(\mathbf{x}, \mathbf{u}) + \rho \sum_{k,i} (\phi_i(\mathbf{x}_k) \cdot \lambda_i^n{}_k)^2\]

当 $\rho \to \infty$ 时，惩罚项迫使互补约束趋于满足。

惩罚法的来龙去脉：惩罚法是约束优化的最经典方法之一，可追溯到 1960 年代 Courant 的外点惩罚法。思想很简单：不满足约束就"罚钱"，罚得越多，解就越接近可行域。

惩罚法的问题：

$\rho$ 太小 → 互补约束违反严重
$\rho$ 太大 → NLP 数值病态（Hessian 条件数 $\propto \rho$，达到 $10^{8}$ 以上）
需要逐步增大 $\rho$（外层循环），每轮 $\rho$ 翻倍直到满足精度

增广拉格朗日法（ALM）——惩罚法的改良版：

\[\mathcal{L}_{\text{aug}} = J + \sum_{k,i} \left[ \mu_{k,i} \cdot (\phi_i \lambda_i^n) + \frac{\rho}{2} (\phi_i \lambda_i^n)^2 \right]\]

其中 $\mu_{k,i}$ 是拉格朗日乘子的估计值。相比纯惩罚法，多了一个**线性项** $\mu \cdot c$——这是精妙之处。

为什么线性项使 $\rho$ 不需要趋于无穷？

纯惩罚法的最优性条件：$\nabla J + \rho \cdot c \cdot \nabla c = 0$。要使 $c \to 0$，需要 $\rho \to \infty$。

ALM 的最优性条件：$\nabla J + (\mu + \rho c) \cdot \nabla c = 0$。如果 $\mu$ 趋近真实 Lagrange 乘子 $\mu^*$，则即使 $c = 0$，最优性条件仍然满足（$\nabla J + \mu^* \nabla c = 0$ 正是 KKT 条件）。因此 $\rho$ 只需要"足够大"使外层迭代收敛，不需要趋于无穷。

ALM 的更新规则：

Algorithm: Augmented Lagrangian for Complementarity
────────────────────────────────────
Input: 初始猜测 z0, mu0 = 0, rho0 = 1
Output: CITO 解 z*

for k = 0, 1, 2, ... do
    z_{k+1} ← solve min L_aug(z; mu_k, rho_k)  // 内层 NLP

    // 更新乘子
    for each complementarity constraint i:
        mu_i ← mu_i + rho * (phi_i * lambda_i^n)

    // 自适应更新 rho
    if ||violation|| > 0.25 * ||prev_violation||:
        rho ← 10 * rho  // 收敛太慢，加大惩罚

    if ||violation|| < tolerance:
        return z_{k+1}
end for

2024 年前沿：CALIPSO 求解器

CALIPSO（Conic Augmented Lagrangian Interior-Point SOlver）是 Taylor Howell 等人在 2022 年（ISRR）提出的求解器，专门为带互补约束和锥约束的轨迹优化设计。它结合了增广拉格朗日法（处理互补约束）和内点法（处理锥约束），在 CITO 问题上比 Ipopt 快 5-10 倍。CALIPSO 已集成在 Julia 的 ContactImplicitMPC.jl 包中。

59.3.4 三种方法的对比 ⭐⭐¶

方法	数学原理	优点	缺点	代表实现
松弛法	$\phi\lambda \le \epsilon$	简单，与 Ipopt 无缝集成	需要续延，$\epsilon$ 调参	Posa 2014
FB 函数	$\psi_{\text{FB}} = 0$	不需续延，LICQ 恢复	需要光滑化	学术研究
增广拉格朗日	$\min J + \mu c + \frac{\rho}{2}c^2$	$\rho$ 不需趋于 $\infty$，条件好	外层循环，实现复杂	CALIPSO

实用建议： - 入门/原型：用松弛法 + Ipopt，最简单 - 研究级别：用 ALM + 定制求解器（CALIPSO） - 如果你的 NLP 已经不收敛：先检查初值，再换 FB 函数试试

💡 概念误区：互补约束等价于 if-else 分支

新手想法："$\phi \cdot \lambda = 0$ 不就是 if (contact) lambda > 0 else phi > 0 吗？直接 if-else 不就好了？"

实际上：if-else 在优化中是不可微的。NLP 求解器需要目标和约束对所有决策变量的梯度。if-else 分支在切换点梯度不存在——Newton 法无法通过切换点。互补约束把 if-else 编码为一个**连续**（虽然不光滑）的等式，使得通过上述三种方法可以光滑化后求解。

类比：ReLU 函数 $\max(0, x)$ 也是 if-else（$x > 0$ 时输出 $x$，否则输出 0），但在深度学习中我们直接对它求梯度（虽然 $x = 0$ 处次梯度不唯一），效果很好。互补约束的处理思路类似。

🧠 思维陷阱：认为松弛就是"近似"，不是精确解

新手想法："松弛后 $\phi\lambda \le \epsilon$ 不是零，解不准确吧？"

实际上：续延策略让 $\epsilon \to 0$，最终得到的解满足 $\phi\lambda = O(10^{-4})$——在工程上等价于精确互补。而且，即使有微小违反，物理上只意味着"脚离地面有 $10^{-4}$ m 的间隙时仍有微小接触力"——完全可接受。

真正的问题不是精度，而是收敛性：松弛法可能收敛到**错误的局部最优**——比如所有接触力都是零（机器人"飞"起来）。这才是需要担心的。

练习¶

练习 59.3a（⭐⭐）：手推 Fischer-Burmeister 函数在以下三个点的值和梯度：$(a, b) = (0, 5)$、$(3, 0)$、$(0.01, 0.01)$。验证前两个点确实满足互补条件。

练习 59.3b（⭐⭐⭐）：用 Python/CasADi 实现一个 1D 互补约束问题：$\min (a - 2)^2 + (b - 3)^2$, s.t. $a \ge 0, b \ge 0, ab = 0$。分别用松弛法和 FB 函数求解，比较解和迭代次数。

练习 59.3c（⭐⭐⭐）：解释为什么增广拉格朗日法的 $\rho$ 不需要趋于无穷就能精确满足约束。提示：写出 ALM 的最优性条件，说明乘子 $\mu$ 在迭代中会收敛到真实 Lagrange 乘子。

59.3.5 MPCC 的平稳性理论：为什么"LICQ 失效"不是末日 ⭐⭐⭐⭐¶

本节解决的问题：前面我们反复说"互补约束破坏 LICQ，所以 KKT 条件失效"。这听起来像是判了死刑——既然 KKT 都不成立，我们求出来的"最优解"还有意义吗？本节给出否定这个悲观结论的严谨理论：带互补约束的优化（MPCC，又称 MPEC）有**自己的一套最优性理论**，把标准 KKT 替换为 C-/M-/S-平稳性（stationarity）。这是理解"为什么三种数值方法收敛到不同质量的解"的理论根基。

动机：标准 NLP 理论为什么在这里集体失效¶

把 CITO 的互补约束写成抽象形式，得到的优化问题叫 MPCC（Mathematical Program with Complementarity Constraints）或 MPEC（Mathematical Program with Equilibrium Constraints）：

\[\min_{x} f(x) \quad \text{s.t.} \quad g(x)\le 0,\; h(x)=0,\; 0 \le G(x) \perp H(x) \ge 0\]

其中 $G(x)\perp H(x)$ 是逐分量互补 $G_i(x)H_i(x)=0$（在 CITO 里 $G=\phi$、$H=\lambda^n$）。

为什么 MPCC 在任何可行点都违反 LICQ（甚至更弱的 MFCQ）？ 把互补 $G_iH_i=0$ 当作等式约束 $c_i(x)=G_i(x)H_i(x)=0$。它的梯度是

\[\nabla c_i = H_i \nabla G_i + G_i \nabla H_i\]

在任意可行点上，互补意味着 $G_i,H_i$ 中至少一个为零。

若 $G_i=0,H_i>0$：$\nabla c_i = H_i\nabla G_i$，方向由 $\nabla G_i$ 决定；
若 $G_i>0,H_i=0$：$\nabla c_i = G_i\nabla H_i$；
若 $G_i=H_i=0$（退化点 / biactive）：$\nabla c_i = 0$ —— 梯度恒为零向量。

退化点上一条约束的梯度是零向量，它必然与其它约束梯度线性相关——LICQ 在此处一定失效。可以进一步证明，即便不在退化点，把 $G_i\ge 0,H_i\ge 0,G_iH_i=0$ 三条放在一起，它们的积极约束梯度也无法线性独立。结论很强：MPCC 在每一个可行点都不满足 LICQ、MFCQ。这意味着标准 NLP 的 KKT 必要条件理论**根本不能直接套用**。

本质洞察：互补约束的"病"不是数值精度问题，而是**约束品性（constraint qualification）层面的结构性缺陷**。这就是为什么单纯"调小容差""换更好的线性求解器"治标不治本——病根在可行集的几何（两条射线的并，在拐点处无法被光滑约束良好刻画），不在浮点运算。三种数值方法（松弛 / FB / ALM）本质上都是在**重构约束的几何或品性**：松弛把拐点"充气"成有内点的区域从而恢复 MFCQ；FB 把两条射线编码成一条处处梯度非零的曲线；ALM 把约束移进目标函数从而完全绕开约束品性的要求。

MPCC 的专属约束品性：MPEC-LICQ¶

为了挽救理论，学界定义了一个**放宽后**的约束品性——把互补约束"拆开看"。定义 MPCC 的 Tightened NLP（TNLP）：在一个可行点 $x^*$ 处，按 $G_i,H_i$ 谁为零，把互补约束替换为对应的等式：

$\mathcal{I}_G=\{i: G_i(x^*)=0<H_i(x^*)\}$ → 保留 $G_i(x)=0$；
$\mathcal{I}_H=\{i: H_i(x^*)=0<G_i(x^*)\}$ → 保留 $H_i(x)=0$；
$\mathcal{I}_{GH}=\{i: G_i(x^*)=H_i(x^*)=0\}$（退化集）→ **同时**保留 $G_i(x)=0$ 与 $H_i(x)=0$。

MPEC-LICQ 的定义：如果上述 TNLP 在 $x^*$ 处满足标准 LICQ，就说原 MPCC 满足 MPEC-LICQ。换句话说，把互补约束"按当前激活情况固定下来"之后，剩下的约束系统梯度线性独立——这是一个**可以被满足**的条件（不像原始 LICQ 必然失效）。CITO 实践中只要退化集 $\mathcal{I}_{GH}$ 不太"病态"，MPEC-LICQ 通常成立。

三级平稳性：W- / C- / M- / S-stationarity¶

在 MPEC-LICQ 下，存在乘子使最优解满足一组替代 KKT 的条件。关键的差别全部落在**退化集 $\mathcal{I}_{GH}$ 上两个乘子 $\nu_i^G,\nu_i^H$ 的符号要求**上。由弱到强排成一个谱：

平稳性	退化集上的乘子条件	强度	含义
W-stationary（弱）	无符号要求	最弱	只是拉格朗日驻点，可能不是局部极小
C-stationary（Clarke）	$\nu_i^G \cdot \nu_i^H \ge 0$	弱	两乘子乘积非负（同号或有零）
M-stationary（Mordukhovich）	$\nu_i^G\nu_i^H=0$ 或（$\nu_i^G>0$ 且 $\nu_i^H>0$）	中	排除了一象限
S-stationary（强）	$\nu_i^G \ge 0$ 且 $\nu_i^H \ge 0$	最强	等价于把 MPCC 当普通 NLP 的 KKT 点

S-stationarity 是我们真正想要的——它等价于原问题的 B-稳定（Bouligand）局部极小的一阶条件。但不同数值方法收敛到的平稳性强度不同，这直接决定了解的质量：

数值方法（59.3）	典型收敛到	后果
松弛法（$\epsilon\to 0$，Scholtes 正则化）	C-stationary（一般情形）；额外二阶条件下可达 M-/S-	可能停在 C-平稳但非局部极小的"伪解"
Fischer-Burmeister（光滑化）	C-stationary（光滑参数 $\to 0$）	与松弛类似，依赖收敛路径
增广拉格朗日 / 精确罚	适当条件下 M-stationary	比纯松弛强，排除更多伪解

这解释了一个一直困扰你的现象：59.3 末尾的思维陷阱说"松弛法可能收敛到错误局部最优（所有力为零）"。从平稳性理论看，那个"飞起来"的平凡解往往是一个 **C-平稳但非 S-平稳**的点——它满足放宽后的一阶条件，但不是真正的局部极小。这就是为什么需要好的初值、续延策略、二阶充分条件检查来把解"推"向更强的 S-平稳性。

🧠 思维陷阱：以为求解器"收敛了"就等于找到了局部最优

新手想法："Ipopt 报 'Optimal Solution Found'，那一定是局部最优了。"

实际上：对松弛后的 MPCC，求解器报告的"最优"是**松弛问题**的 KKT 点。当 $\epsilon\to 0$，这串解的极限只保证是**C-平稳**——这是比局部最优弱的条件。C-平稳点可能是鞍点甚至局部极大方向上的驻点。

根因：MPCC 的最优性理论与标准 NLP 不同，"收敛"和"局部最优"之间隔着 W-/C-/M-/S- 四级阶梯。

正确做法：(1) 多初值重启取最优；(2) 检查解处的二阶充分条件（MPEC-SOSC）；(3) 优先选 ALM 类方法（更易达 M-平稳）；(4) 用物理先验（如重力补偿项）排除平凡 C-平稳解。

工程启示：理论怎样指导调参¶

这套理论不是纯数学游戏，它给出可操作的调试指南：

你观察到	平稳性解读	行动
解收敛但物理上荒谬（飞起来 / 穿地）	停在了 C-平稳的伪解	换 ALM、加物理先验、多初值
续延 $\epsilon\to 0$ 时解剧烈跳变	退化集 $\mathcal{I}_{GH}$ 大，MPEC-LICQ 临界	减小续延步长、warm-start
不同松弛参数得到本质不同的解	C-平稳点不唯一	报告解时附上 $\epsilon$ 与多次重启统计

练习¶

练习 59.3.5a（⭐⭐⭐）：对一维 MPCC $\min (a-1)^2+(b-1)^2$ s.t. $0\le a\perp b\ge 0$，几何上求出全局最优解。验证最优点处 $a,b$ 哪个在退化集 $\mathcal{I}_{GH}$ 中，并写出该点的 TNLP，检查 MPEC-LICQ 是否成立。

练习 59.3.5b（⭐⭐⭐⭐）：构造一个二维 MPCC 的可行点，使其为 C-平稳但不是 S-平稳（提示：让退化集上两个乘子异号）。说明一个只检查"梯度范数 < tol"的求解器为什么会在此点错误停机。

59.4 Posa 2014 IJRR 精读 ⭐⭐⭐¶

动机：CITO 奠基论文为什么重要？¶

本节解决的问题：Posa, Cantu, Tedrake 2014 年发表在 IJRR 的 "A Direct Method for Trajectory Optimization of Rigid Bodies Through Contact" 是 CITO 领域的奠基性工作。理解这篇论文对于理解后续所有 CITO 变体都是必要的。

论文要解决什么问题？

在 Posa 2014 之前，轨迹优化处理接触的标准方法是**先指定接触模式序列，再优化连续轨迹**。例如：

传统方法（已知模式序列）:
Phase 1: 双脚支撑 → 优化 NLP_1
Phase 2: 右脚摆动 → 优化 NLP_2
Phase 3: 双脚支撑 → 优化 NLP_3
...
约束: 相邻 phase 的边界状态匹配

问题: 模式序列本身是人类给定的，无法发现新颖接触

Posa 的核心贡献是**消除对模式序列的先验假设**——让一个统一的 NLP 自动决定模式。

它是怎么被发现的？ Posa 的导师 Russ Tedrake 一直在 MIT 研究 underactuated robots（欠驱动机器人），这类系统的轨迹优化天然需要处理接触（如被动行走器的脚与地面碰撞）。Tedrake 实验室注意到 Stewart-Trinkle 的 LCP 时间步进公式——用于接触仿真——可以被"反过来用"：不是给定状态求接触力，而是把接触力作为待优化变量。这个"反转"的洞察催生了 Posa 2014。

59.4.1 Posa 方法的技术细节 ⭐⭐⭐¶

动力学离散化：半隐式时间步进

Posa 采用 Stewart-Trinkle time-stepping 作为动力学约束。离散化后的接触动力学：

\[\mathbf{M}(\mathbf{q}_k)(\mathbf{v}_{k+1} - \mathbf{v}_k) = h[\mathbf{B}\boldsymbol{\tau}_k - \mathbf{c}(\mathbf{q}_k, \mathbf{v}_k) + \mathbf{J}_n^T \lambda_n + \mathbf{J}_t^T \boldsymbol{\lambda}_t]\]

\[\mathbf{q}_{k+1} = \mathbf{q}_k + h \mathbf{v}_{k+1}\]

其中 $\mathbf{B}$ 是控制输入选择矩阵（对于欠驱动系统，$\mathbf{B}$ 不是方阵），$\mathbf{c}$ 包含科氏力和重力。

符号约定：此处 $\lambda_n, \boldsymbol{\lambda}_t$ 为接触**力**（单位 N），整个右侧乘以步长 $h$ 得到冲量。与 59.2.2 节的冲量约定不同——59.2.2 节直接将 $\boldsymbol{\lambda}$ 定义为冲量。两种约定在文献中都常见，关键是**检查 $h$ 是否已被吸收**。

互补约束的松弛处理

Posa 把原始 MPCC（Mathematical Program with Complementarity Constraints）松弛为 NLP：

\[\phi_i(\mathbf{q}_k) \cdot \lambda_{i,k}^n \le \epsilon\]

同时保留： - $\phi_i(\mathbf{q}_k) \ge 0$（非穿透） - $\lambda_{i,k}^n \ge 0$（法向力非负）

直接配置法（Direct Collocation）

Posa 使用直接配置法（不同于 DDP 的 shooting 方法）。这个方法选择的**原因**至关重要——理解它有助于理解 CITO 的架构设计。

方面	Shooting（DDP）	Direct Collocation（Posa）
决策变量	只有 $\mathbf{u}_{0:N-1}$	$\mathbf{x}_{0:N}, \mathbf{u}_{0:N-1}, \boldsymbol{\lambda}_{0:N-1}$
动力学	前向积分得到 $\mathbf{x}_{1:N}$	作为等式约束
优点	变量少	初值不需要满足动力学，更鲁棒
缺点	初值敏感，混沌系统不稳定	变量多，NLP 维度大
对 CITO 的关键影响	接触力无法作为决策变量	接触力自然成为决策变量

为什么 Direct Collocation 对 CITO 是自然选择？ 在 shooting 方法中，给定 $\mathbf{u}_k$，前向积分会在内部求解 LCP 得到 $\boldsymbol{\lambda}_k$——接触力是积分器的"副产品"，不是优化的自由度。这意味着优化器无法直接控制接触力，只能通过调整 $\mathbf{u}_k$ 间接影响接触。而 direct collocation 把 $\boldsymbol{\lambda}_k$ 从积分器中"提升"为一等决策变量，使优化器可以**直接**优化接触力的大小和时机。

59.4.2 Posa 的关键实验结果 ⭐⭐¶

Posa 论文展示了三个关键实验：

实验 1：2D 弹跳球

设置	结果
目标	球从地面弹跳到指定高度
接触模式	优化器自动发现"着地 → 弹起 → 飞行"模式
互补违反	$\phi \cdot \lambda < 10^{-4}$
求解时间	约 1 秒

实验 2：2D 双足行走

设置	结果
目标	从静止开始行走 5 步
自由度	5-link planar biped，10 维状态
接触模式	自动发现左右交替步态
求解时间	约 30 秒

实验 3：3D 四足运动

设置	结果
目标	四足前进 2 米
自由度	完整四足模型
接触模式	自动发现 trot 步态
求解时间	数分钟

关键发现： 1. 优化器能**自动发现**合理的接触模式，不需要人类指定 2. 发现的模式与直觉一致（双足交替、四足 trot） 3. 求解时间随系统维度**急剧增长**——3D 四足需要数分钟，远离实时 4. 初值敏感——差的初始猜测容易收敛到"所有力为零"的平凡解

59.4.3 论文的局限与后续影响 ⭐⭐¶

Posa 2014 的局限：

局限	原因	后续解决方案
求解慢	NLP 维度大 + 续延迭代	Manchester 2019（结构保持），Pang 2023（光滑化）
初值敏感	非凸 + 多局部最优	Mordatch 2012（接触指示器放松），学习初值
摩擦模型简化	只有库仑摩擦，无滚动	后续扩展到软接触模型
不处理碰撞反弹	纯非弹性碰撞	需要恢复系数建模
不考虑机器人运动学	纯刚体动力学	Towr 加入运动学约束

Posa 2014 的深远影响：

这篇论文建立了 CITO 的**标准数学框架**——后续几乎所有 CITO 工作都是在 Posa 的公式基础上做改进： - Manchester 2019：换积分器（变分积分器） - Mordatch 2012（虽然时间在前）：换互补约束处理方式 - Pang 2023：换接触模型（准静态光滑化） - ACAL-iLQR 2024：换求解算法（iLQR + ALM）

🧠 思维陷阱：因为 Posa 2014 求解慢就否定 CITO

新手想法："CITO 需要几分钟才能解，没有实用价值。"

实际上： 1. 离线规划**不需要实时——花 1 分钟规划一个动作库，MPC 在线选择并跟踪 2. **研究价值：CITO 能验证"理论最优"的接触策略，作为学习方法的训练数据或 baseline 3. 速度在持续改善：从 2014 年的几分钟到 2024 年的约 1 秒（特定系统），10 年加速了约 100 倍 4. 硬件在进步：GPU 并行 + 稀疏求解器加速未来可期

练习¶

练习 59.4a（⭐⭐）：解释 Posa 为什么选择 Direct Collocation 而不是 Shooting 方法。如果用 DDP 做 CITO，接触力变量应该放在哪里？

练习 59.4b（⭐⭐⭐）：阅读 Posa 2014 论文的 Section III-B（互补约束松弛），回答：为什么 $\epsilon$ 不能一步到位设为 $10^{-8}$，而必须从 $10^{-1}$ 逐步减小？从 NLP 的可行域几何角度解释。

59.5 变分接触隐式优化 ⭐⭐⭐⭐¶

动机：为什么 Posa 的时间步进法不够好？¶

本节解决的问题：Posa 2014 使用 Stewart-Trinkle 时间步进格式，这是一种一阶精度的离散化。Manchester 和 Kuindersma 2019（IJRR）提出的变分 CITO 用**变分积分器**替换时间步进，获得更高精度和更好的结构保持性。为什么这很重要？

时间步进法的精度问题

半隐式欧拉（Posa 使用的）只有一阶精度：$\mathbf{x}_{k+1} = \mathbf{x}_k + h \mathbf{f}(\mathbf{x}_k) + O(h^2)$。这意味着：

时间步长 $h = 0.01$ s 时，每步误差 $O(10^{-4})$
$N = 100$ 步后累积误差 $O(10^{-2})$——可能导致轨迹漂移

能不能用高阶积分器（如 RK4）？ 不能直接用。原因是接触力在碰撞瞬间是**冲量**（Dirac delta 函数），不满足 RK4 要求的"被积函数连续"前提。RK4 在每个时间步内部有 4 个评估点（$k_1, k_2, k_3, k_4$），如果碰撞发生在两个评估点之间，RK4 无法正确处理速度的不连续跳变。

变分积分器提供了第三条路：不需要对力显式积分，而是从**离散化的 Hamilton 原理**出发，自动得到高精度、能量守恒的离散方程。

59.5.1 变分原理回顾 ⭐⭐⭐¶

Hamilton 原理（最小作用量原理）：物理系统的真实运动使作用量取极值：

\[\delta S = \delta \int_{t_0}^{t_f} L(\mathbf{q}, \dot{\mathbf{q}}) \, dt = 0\]

其中 $L = T - V$ 是拉格朗日量（动能减势能）。

这个原理为什么对计算有用？ 传统方法是先从 Hamilton 原理推导出 Euler-Lagrange 方程（连续 ODE），然后用数值方法离散化 ODE。但离散化过程中可能破坏物理系统的结构（能量守恒、辛结构）。变分积分器的思想是**颠倒顺序**：先离散化作用量积分，再对离散化后的作用量取变分——得到的离散方程自动保持物理结构。

变分积分器的思想：

传统方法:
  连续 Hamilton 原理 → 连续 Euler-Lagrange 方程 → 离散化(可能破坏结构)

变分积分器:
  连续 Hamilton 原理 → 离散 Hamilton 原理 → 离散 Euler-Lagrange 方程
                                              ↑ 自动保持结构!

离散 Lagrangian：

\[L_d(\mathbf{q}_k, \mathbf{q}_{k+1}) \approx \int_{t_k}^{t_{k+1}} L(\mathbf{q}(t), \dot{\mathbf{q}}(t)) \, dt\]

最简单的近似（梯形规则）：

\[L_d(\mathbf{q}_k, \mathbf{q}_{k+1}) = \frac{h}{2}\left[L\left(\mathbf{q}_k, \frac{\mathbf{q}_{k+1} - \mathbf{q}_k}{h}\right) + L\left(\mathbf{q}_{k+1}, \frac{\mathbf{q}_{k+1} - \mathbf{q}_k}{h}\right)\right]\]

离散 Euler-Lagrange 方程：对离散作用量 $S_d = \sum_k L_d(\mathbf{q}_k, \mathbf{q}_{k+1})$ 取变分 $\delta S_d / \delta \mathbf{q}_k = 0$，得到：

\[D_2 L_d(\mathbf{q}_{k-1}, \mathbf{q}_k) + D_1 L_d(\mathbf{q}_k, \mathbf{q}_{k+1}) + \mathbf{f}_k^+ + \mathbf{f}_{k+1}^- = 0\]

其中 $D_1, D_2$ 是对第一、第二个参数的偏导数，$\mathbf{f}_k^{\pm}$ 是离散外力（包括控制力和接触力）。

59.5.2 Manchester 2019 的关键创新 ⭐⭐⭐⭐¶

Manchester 和 Kuindersma 的论文全称是 "Contact-Implicit Trajectory Optimization Using Variational Integrators"（IJRR 2019，内容基于 ISRR 2017 报告）。

创新 1：二阶精度的接触离散化

通过使用梯形规则的离散 Lagrangian，Manchester 获得了二阶精度的时间步进——比 Posa 的一阶半隐式欧拉高一个数量级。关键是**接触力的离散化方式**：

\[\mathbf{f}_k^+ = \frac{h}{2}[\mathbf{J}_c(\mathbf{q}_k)^T \boldsymbol{\lambda}_k^+ + \mathbf{B}\boldsymbol{\tau}_k]$$ $$\mathbf{f}_{k+1}^- = \frac{h}{2}[\mathbf{J}_c(\mathbf{q}_{k+1})^T \boldsymbol{\lambda}_{k+1}^- + \mathbf{B}\boldsymbol{\tau}_{k+1}]\]

接触力在每个时间步的开头和结尾分别有一个分量（$\lambda^+$ 和 $\lambda^-$），这提供了更精确的冲量分配。为什么这比 Posa 的方法好？因为一阶方法在每个时间步只有一个接触力变量（相当于把力集中在时间步的中点），而梯形规则在两端各有一个，等价于对力做线性插值——精度自然提升。

创新 2：辛结构保持

变分积分器自动保持**辛结构**（symplecticity）——这意味着离散系统的相空间体积守恒。

什么是辛结构？为什么重要？

在 Hamiltonian 力学中，系统的演化保持辛形式 $\omega = \sum_i dp_i \wedge dq_i$（相空间中的"面积元素"）。这意味着： - 能量误差有界（不会单调增长或衰减） - 轨迹不会发散到非物理区域 - 长时间仿真的定性行为正确

非辛积分器（如显式欧拉）在长时间仿真中能量会系统性漂移——对 CITO 来说，这意味着优化器可能利用"数值泄漏"的能量产生非物理轨迹（如弹跳球越弹越高）。辛积分器从根本上避免了这个问题。

创新 3：与 Stewart-Trinkle 的理论联系

Manchester 证明了他的二阶变分积分器在特定参数选择下**退化为** Stewart-Trinkle 的一阶时间步进。这意味着 Posa 2014 的方法是 Manchester 方法的一个特例——建立了优雅的理论统一。

59.5.3 变分 CITO 的实验验证 ⭐⭐⭐¶

Manchester 论文的关键实验：

实验：四足微型机器人（Harvard HAMR）

方面	一阶时间步进（Posa）	二阶变分积分器（Manchester）
精度	$O(h)$	$O(h^2)$
同等精度所需步数	$N = 200$	$N = 50$
求解时间	约 120 s	约 30 s
能量误差	累积增长	有界振荡

关键结论：二阶精度使得可以用**更少的时间步**达到同等精度，NLP 维度更低，求解更快。在 HAMR 微型机器人上，优化出的轨迹直接在实物上运行，验证了方法的实用性。

⚠️ 编程陷阱：变分积分器中的位形更新

错误做法：对于 SO(3) 上的旋转，用 $\mathbf{q}_{k+1} = \mathbf{q}_k + h\dot{\mathbf{q}}_k$ 更新四元数。

症状：四元数范数不是 1，旋转矩阵不正交，仿真发散。

根因：SO(3) 不是向量空间，加法不封闭。$\mathbf{q} + \delta$ 可能不再是单位四元数。变分积分器中必须用**指数映射更新**：$\mathbf{q}_{k+1} = \mathbf{q}_k \otimes \exp(\frac{h}{2}\boldsymbol{\omega}_k)$，其中 $\otimes$ 是四元数乘法。

正确做法：使用 Lie 群积分器（足式/50_空间向量与浮动基座动力学的知识），把变分积分器推广到 SE(3)。Manchester 的实现中用了 Cayley 映射作为替代（计算更快，精度略低）。

💡 概念误区：变分积分器"总是"比普通积分器好

新手想法："变分积分器既高精度又保结构，为什么不总是用它？"

实际上：变分积分器有两个代价：(1) 隐式格式需要在每步解非线性方程（半隐式欧拉不需要）；(2) 实现复杂度更高（需要推导离散 Lagrangian 的梯度和 Hessian）。对于短时域问题（MPC 的 0.5 秒窗口），一阶精度通常足够——用变分积分器是"杀鸡用牛刀"。变分积分器的优势在**长时域**规划（如 10 秒的步态优化）中才充分体现。

练习¶

练习 59.5a（⭐⭐⭐）：对于一维自由落体 $L = \frac{1}{2}m\dot{q}^2 - mgq$，用梯形规则写出离散 Lagrangian $L_d(q_k, q_{k+1})$，然后推导离散 Euler-Lagrange 方程。验证它与半隐式欧拉一致。

练习 59.5b（⭐⭐⭐⭐）：解释为什么变分积分器是辛的。提示：从离散 Legendre 变换出发，定义离散动量 $p_k^+ = D_2 L_d(q_k, q_{k+1})$, $p_{k+1}^- = -D_1 L_d(q_k, q_{k+1})$，说明离散流映射 $(q_k, p_k^+) \mapsto (q_{k+1}, p_{k+1}^-)$ 保持辛形式 $dp \wedge dq$。

59.6 质心轨迹优化与接触规划 ⭐⭐⭐¶

动机：全身 CITO 太慢，有没有折中？¶

本节解决的问题：Posa/Manchester 的全身 CITO（状态维度约 40）求解太慢，不适合在线使用。质心轨迹优化（Centroidal Trajectory Optimization）用 6 维质心动力学代替全身动力学，大幅降维，同时保留接触规划能力。

从全身到质心：降维的代价与收益

方面	全身 CITO	质心轨迹优化
状态维度	约 40（浮动基座 + 12 关节）	约 12（CoM 位置、速度、角动量）
接触力维度	$3 n_c$（每接触点 3 维）	$3 n_c$（相同）
NLP 总维度（N=50）	约 3000	约 900
求解时间	分钟级	秒级
物理忠实度	高（全身约束）	中（忽略运动学限制）
可行性保证	全身可行	可能运动学不可行

这个降维是怎么来的？ 全身动力学方程 $\mathbf{M}\ddot{\mathbf{q}} + \mathbf{c} = \boldsymbol{\tau} + \mathbf{J}_c^T \boldsymbol{\lambda}$ 有 $n_q$ 个方程（约 18 维）。质心动力学只保留其中 6 维——线动量和角动量的变化率。这等价于把关节角度"积掉"（边缘化），只关注 CoM 的运动。代价是丢失了关节层面的信息——优化出的质心轨迹可能因为运动学约束（腿长有限、关节限位）而不可执行。

质心动力学回顾：足式/80_接触力学与约束优化从 Newton-Euler 方程出发，将全身多体动力学投影到质心空间，得到了质心动量方程——外力（接触力 + 重力）的合力和合力矩等于质心动量的变化率。这个方程的关键价值在于它只有 6 维（3 维线动量 + 3 维角动量），却完整捕获了整个多体系统的宏观运动。在足式/80_接触力学与约束优化中我们用它分析了接触力如何维持平衡；这里我们用同样的方程，但目标变为**同时优化接触力和接触位置**：

\[\dot{\mathbf{h}}_G = \begin{bmatrix} \dot{\mathbf{k}} \\ \dot{\mathbf{l}} \end{bmatrix} = \begin{bmatrix} \sum_i (\mathbf{p}_i - \mathbf{c}) \times \mathbf{f}_i \\ \sum_i \mathbf{f}_i + m\mathbf{g} \end{bmatrix}\]

其中 $\mathbf{c}$ 是 CoM 位置，$\mathbf{p}_i$ 是第 $i$ 个接触点位置，$\mathbf{f}_i$ 是接触力，$\mathbf{k}$ 是角动量，$\mathbf{l}$ 是线动量。

59.6.1 质心+接触协同优化的 NLP 公式 ⭐⭐⭐¶

决策变量：

\[\mathbf{z} = \{\mathbf{c}_{0:N}, \dot{\mathbf{c}}_{0:N}, \mathbf{k}_{0:N}, \mathbf{p}_{i,0:N}, \mathbf{f}_{i,0:N-1}\}\]

其中 $\mathbf{p}_i$ 是足端位置（也是决策变量），$\mathbf{f}_i$ 是接触力。

约束：

\[\begin{aligned} & m\ddot{\mathbf{c}}_k = \sum_i \mathbf{f}_{i,k} + m\mathbf{g} & \text{(线动量守恒)} \\ & \dot{\mathbf{k}}_k = \sum_i (\mathbf{p}_{i,k} - \mathbf{c}_k) \times \mathbf{f}_{i,k} & \text{(角动量方程)} \\ & \phi(\mathbf{p}_{i,k}) \ge 0, \quad f_{i,k}^n \ge 0, \quad \phi \cdot f^n \le \epsilon & \text{(接触互补)} \\ & \|\mathbf{f}_i^t\| \le \mu f_i^n & \text{(摩擦锥)} \\ & \|\mathbf{p}_{i,k} - \mathbf{c}_k\| \le l_{\max} & \text{(腿长约束——粗略运动学)} \\ & \|\mathbf{p}_{i,k+1} - \mathbf{p}_{i,k}\| \le v_{\max} h & \text{(足端速度限制)} \end{aligned}\]

非线性项：角动量方程中的 $(\mathbf{p}_i - \mathbf{c}) \times \mathbf{f}_i$ 是**双线性**的（两个决策变量的乘积），使问题非凸。这比 LIPM（足式/70_腿足简化模型理论）多了角动量维度，但比全身动力学少了关节维度。

59.6.2 接触时序的连续松弛 ⭐⭐⭐¶

在质心优化中，可以用 Mordatch 2012 的**接触指示器**来处理接触时序：

引入连续变量 $c_{i,k} \in [0, 1]$： - $c_{i,k} = 1$：第 $i$ 条腿在时间步 $k$ 支撑 - $c_{i,k} = 0$：第 $i$ 条腿在时间步 $k$ 摆动 - $c_{i,k} \in (0, 1)$：过渡状态

约束修改为：

\[\begin{aligned} & c_{i,k} \cdot \phi(\mathbf{p}_{i,k}) = 0 & \text{(支撑时脚在地面)} \\ & (1 - c_{i,k}) \cdot \|\mathbf{f}_{i,k}\| = 0 & \text{(摆动时无力)} \\ & c_{i,k} \cdot \|\dot{\mathbf{p}}_{i,k}\| = 0 & \text{(支撑时脚不滑)} \\ \end{aligned}\]

这个方法是怎么被想到的？ Mordatch 2012 的洞察来自计算机图形学——在动画中，人物的运动是连续生成的，角色的手脚是否接触环境是一个"创作决策"。Mordatch 把这个创作决策编码为连续变量 $c \in [0, 1]$，让优化器"渐变"地决定接触——从"轻微触碰"($c \approx 0$) 过渡到"完全支撑"($c = 1$)。

优势：不需要预定义步态——优化器自动决定每条腿何时支撑、何时摆动。Mordatch 2012 用这个方法在 SIGGRAPH 中展示了仿真人物自动发现爬行、站立、倒立等复杂动作。

劣势：$c_{i,k}$ 可能停留在 0.5 附近（"半接触"），物理上无意义。需要后处理舍入，或加惩罚项鼓励 $c_{i,k}$ 趋向 0 或 1：

\[J_{\text{binary}} = w \sum_{k,i} c_{i,k}(1 - c_{i,k})\]

这个惩罚项在 $c = 0$ 或 $c = 1$ 时为零，在 $c = 0.5$ 时最大。

59.6.3 代表工作：分阶段方法 ⭐⭐¶

许多实用工作将质心轨迹优化和接触序列规划分成两阶段：

阶段 1: MIQP
─────────────
输入: 环境地图 (stepping stones)
决策: 每步踩哪个 stone (离散), 步序
输出: 接触序列 + 粗略 CoM 轨迹

阶段 2: NLP
─────────────
输入: 阶段 1 的接触序列
决策: CoM 轨迹细化, 接触力
输出: 可执行的质心轨迹

→ MIQP 解决组合问题 (秒级)
→ NLP 解决连续优化 (秒级)
→ 总共约 10 秒，比全身 CITO 快 10-100 倍

这种分阶段方法是工程中最常用的——ANYmal parkour 的规划层就采用类似架构。

⚠️ 编程陷阱：角动量方程中的叉积方向

错误做法：写成 $\dot{\mathbf{k}} = \sum_i \mathbf{f}_i \times (\mathbf{p}_i - \mathbf{c})$（力叉乘力臂）。

症状：角动量符号相反，机器人旋转方向错误。

根因：力矩 = 力臂 $\times$ 力，即 $\boldsymbol{\tau} = \mathbf{r} \times \mathbf{f}$，不是 $\mathbf{f} \times \mathbf{r}$。叉积不满足交换律：$\mathbf{a} \times \mathbf{b} = -\mathbf{b} \times \mathbf{a}$。

正确做法：$\dot{\mathbf{k}} = \sum_i (\mathbf{p}_i - \mathbf{c}) \times \mathbf{f}_i$。

练习¶

练习 59.6a（⭐⭐）：写出四足机器人质心轨迹优化的完整 NLP 公式（不用互补约束，假设步态时序已知）。计算 $N = 30$ 时的总决策变量数，与全身 CITO 比较。

练习 59.6b（⭐⭐⭐）：角动量方程中的双线性项 $(\mathbf{p}_i - \mathbf{c}) \times \mathbf{f}_i$ 使问题非凸。提出一种线性化策略（如 SQP），说明如何在每次 SQP 迭代中处理这个项。提示：把 $\mathbf{p}_i, \mathbf{c}, \mathbf{f}_i$ 分别在当前迭代点展开。

59.7 双层优化视角 ⭐⭐⭐⭐¶

动机：CITO 为什么收敛困难？¶

本节解决的问题：单一 NLP 同时优化离散决策（接触序列）和连续轨迹，这是一个混合整数非线性规划（MINLP）——NP-hard。双层优化把问题分解为上层（接触序列）和下层（轨迹优化），降低单层问题的难度。

CITO 作为单层 NLP 的困境：

单层 CITO:
  min J(x, u, lambda)
  s.t. 动力学 + 互补约束 + 摩擦锥

问题 1: 互补约束使可行域非凸 → 很多局部最优
问题 2: 接触模式改变 = 拓扑变化 → 梯度信息无法跨模式传递
问题 3: 初值必须"猜对"接触模式 → 几乎不可能

为什么拓扑变化导致梯度失效？ 考虑一个简单例子：从"两脚支撑"变为"一脚支撑"。这个变化是**离散**的——不存在"1.5 脚支撑"。NLP 的梯度是连续变化的，它无法"跳过"模式切换。结果是：如果初值在"两脚支撑"模式中，梯度下降只会在这个模式内优化，永远无法发现"单脚跳"可能是更优策略。

59.7.1 双层优化的数学公式 ⭐⭐⭐⭐¶

\[\boxed{\begin{aligned} \text{上层:} \quad & \min_{\sigma \in \Sigma} \quad J^*(\sigma) \\ \text{下层:} \quad & J^*(\sigma) = \min_{\mathbf{x}, \mathbf{u}} J(\mathbf{x}, \mathbf{u}) \quad \text{s.t. dynamics}(\sigma), \text{ constraints}(\sigma) \end{aligned}}\]

其中 $\sigma$ 是接触序列（哪条腿在什么时候接触），$\Sigma$ 是所有可能的接触序列集合，$J^*(\sigma)$ 是给定接触序列下的最优代价。

物理直觉：上层像"教练"决定战术（步态和落脚策略），下层像"运动员"在给定战术下发挥最优表现。整个双层结构好比餐厅运营——经理（上层）决定菜单上有哪些菜品（接触序列），厨师（下层）在给定菜单下把每道菜做到最好（轨迹优化）。经理不需要知道每道菜的烹饪细节，厨师也不需要操心菜品组合是否合理——分工使两边的问题都变简单了。

为什么双层比单层好？

方面	单层 CITO	双层优化
上层问题	不存在	MIQP（组合但凸）
下层问题	NLP + 互补约束（非凸、LICQ 失效）	NLP（无互补约束，标准）
初值敏感性	极高	上层不敏感，下层温和
可保证性	局部最优	上层全局最优（MIQP），下层局部最优

59.7.2 上层：MIQP 接触序列优化 ⭐⭐⭐¶

MIQP 公式

对于 stepping stones 场景，上层可以用 MIQP：

\[\begin{aligned} \min_{\mathbf{p}_f, \mathbf{z}} \quad & \sum_i \|\mathbf{p}_{f,i} - \mathbf{p}_{f,i}^{\text{ref}}\|^2 + w_{\text{step}} \|\mathbf{p}_{f,i} - \mathbf{p}_{f,i-1}\|^2 \\ \text{s.t.} \quad & \sum_j z_{i,j} = 1, \quad \forall i & \text{(每步选一块石头)} \\ & \mathbf{A}_j \mathbf{p}_{f,i} \le \mathbf{b}_j + M(1 - z_{i,j}), \quad \forall i, j & \text{(Big-M)} \\ & z_{i,j} \in \{0, 1\} & \text{(整数约束)} \\ & \|\mathbf{p}_{f,i} - \mathbf{p}_{f,i-1}\| \le d_{\max} & \text{(步长约束)} \end{aligned}\]

Big-M 方法详解：

Big-M 方法是混合整数规划中编码"选择性约束"的标准技巧。核心思想：用一个大常数 $M$ 使得当对应的二值变量为 0 时，约束被"打开"（自动满足）。

推导：第 $i$ 步的落脚点 $\mathbf{p}_{f,i}$ 必须在所选石头 $j$ 的内部。石头 $j$ 定义为凸多边形 $\{\mathbf{x}: \mathbf{A}_j \mathbf{x} \le \mathbf{b}_j\}$。

当 $z_{i,j} = 1$（踩石头 $j$）：$\mathbf{A}_j \mathbf{p}_{f,i} \le \mathbf{b}_j + M \cdot 0 = \mathbf{b}_j$ → 约束生效
当 $z_{i,j} = 0$（不踩石头 $j$）：$\mathbf{A}_j \mathbf{p}_{f,i} \le \mathbf{b}_j + M$ → 约束自动满足（$M$ 足够大）

$M$ 的选择是一门"调参艺术"： - $M$ 太大 → LP 松弛变弱，branch-and-bound 需要更多节点，求解变慢 - $M$ 太小 → 可行域被错误截断，可能排除最优解 - 经验法则：$M = 10 \times \max(\|\mathbf{b}_j\|)$ - 更好的方法：用 indicator constraints（Gurobi 支持），完全避免 Big-M

MIQP 求解器：

求解器	类型	许可证	典型性能
Gurobi	商业	免费学术	最快，支持 indicator constraints
CPLEX	商业（IBM）	免费学术	接近 Gurobi
SCIP	开源	ZIB	比商业慢 2-5x
HiGHS	开源	MIT	持续改进中，LP 部分已很强

59.7.3 下层：给定接触序列的轨迹优化 ⭐⭐⭐¶

给定上层输出的接触序列 $\sigma$，下层就是足式/100_DDP家族与Crocoddyl-55 的标准轨迹优化——DDP/SQP/FDDP，不需要互补约束。

上层输出（接触序列 sigma）:
  腿 RF: 支撑 [0, 0.3s], 摆动 [0.3, 0.5s], 支撑 [0.5, 0.8s]
  腿 LF: 摆动 [0, 0.2s], 支撑 [0.2, 0.6s], 摆动 [0.6, 0.8s]
  ...
  落脚点: RF→石2, LF→石3, ...

下层 NLP:
  min Sigma l(x_k, u_k)
  s.t. 动力学
       摩擦锥（仅在支撑相）
       足端位置 = 指定落脚点（仅在支撑相）
       运动学约束
       → 标准 TO，足式/100_DDP家族与Crocoddyl 的 DDP 或 足式/110_OCS2完整栈与双线程MPC 的 SQP 可解

59.7.4 上下层的耦合与迭代 ⭐⭐⭐⭐¶

上下层并非完全独立——上层选的接触序列可能在下层不可行（运动学不可达、动力学不足）。需要**迭代**：

Algorithm: Bilevel Contact Planning
────────────────────────────────────
1. 上层: 求解 MIQP → 接触序列 sigma_1
2. 下层: 求解 NLP(sigma_1) → 最优轨迹 or 不可行
3. if 不可行:
       添加不可行性反馈 (cuts) 到上层
       回到 1
4. if 可行但代价不满意:
       修改上层目标 (warm start with J*)
       回到 1
5. 收敛 → 输出最终轨迹

Benders 分解的思想：每次下层失败，告诉上层"这种接触序列不行"（添加 Benders cut），上层在缩小的可行域中重新选择。理论上有限步收敛（因为 $\Sigma$ 是有限集）。

什么是 Benders cut？ 假设下层在接触序列 $\sigma_1$ 下不可行，我们添加一个线性约束到上层：$\sum_{(i,j) \in \sigma_1} z_{i,j} \le |\sigma_1| - 1$。这个约束排除了 $\sigma_1$（因为 $\sigma_1$ 需要所有对应的 $z_{i,j} = 1$，但约束要求至少有一个为 0）。更精细的 cut 可以只排除导致不可行的"子集"——这需要分析下层不可行的原因（哪个约束被违反）。

59.7.5 2024 前沿：双层优化的实时化 ⭐⭐⭐¶

2024 年的研究（Aydinoglu et al.）提出了将双层优化实时化的方法：

上层用轻量级 MIP 求解器（warm-started），利用上一时刻的接触序列作为初值
下层用 iLQR（单次迭代，利用上一时刻的解）
总计约 50 ms/cycle，接近实时

这标志着双层优化从纯离线方法向在线方法的转变。关键技术是**极致的 warm-starting**——在上下两层都利用时间连续性。

💡 概念误区：双层优化就是"两次优化"

新手想法："先解上层，再解下层，一共两次就行。"

实际上：双层优化需要**迭代**——上层的选择可能在下层不可行，需要反馈修正。而且上层的目标函数 $J^*(\sigma)$ 是下层最优值函数——这个函数通常没有解析表达式，只能通过求解下层来评估。每评估一次 $J^*$ 就需要解一个完整的 NLP。

计算代价：如果上层有 $K$ 种接触序列候选，每种需要解一个下层 NLP（约 1 秒），那么最坏情况需要 $K$ 秒。这就是为什么上层的搜索空间缩减（剪枝、warm-start、可行性预筛选）非常重要。

练习¶

练习 59.7a（⭐⭐）：对于一个有 5 块 stepping stones、3 步 horizon 的 MIQP，计算二值决策变量的数量和可能的组合数。如果 MIQP 求解每个组合需要 1 ms，穷举需要多久？

练习 59.7b（⭐⭐⭐）：解释 Big-M 方法中 $M$ 的值如何影响 MIQP 的 LP 松弛质量。为什么 $M$ 太大会导致 branch-and-bound 效率下降？提示：LP 松弛的可行域如何随 $M$ 变化？

练习 59.7c（⭐⭐⭐⭐）：设计一个简单的 Benders cut：假设下层在接触序列 $\sigma_1 = \{(\text{RF}, \text{石}2), (\text{LF}, \text{石}4)\}$ 下不可行，构造一个线性不等式添加到上层 MIQP，排除 $\sigma_1$。

59.7.6 凸优化图（GCS）：接触规划的第三种范式 ⭐⭐⭐⭐¶

本节解决的问题：59.7 的双层优化把"离散接触序列 + 连续轨迹"拆成两层迭代求解，但上下层耦合（Benders cut、warm-start）实现复杂，且 MIQP 与下层 NLP 各自仍可能陷入次优。2022 年以来 Marcucci、Tedrake 等人提出的 Graph of Convex Sets（GCS，凸集图） 提供了第三种范式：把"选哪个接触模式"和"在该模式下怎么动"统一编码为一张图上的最短路径问题，再用一个**紧凸松弛**一次性求解——既不枚举模式，也不做双层迭代。这是 2024–2025 接触规划最受关注的方向之一，也填补了本章开放方向里"GCS + 腿足"留下的空白。

动机：MIQP 与 CITO 各自的"原罪"¶

回顾本章两条主线各自的根本困难：

范式	根本困难
CITO（59.2–59.5）	互补约束使单个连续问题非凸，对初值极度敏感，只能保证局部最优
双层 MIQP（59.7）	上层是组合问题（指数多模式），Big-M 松弛松，上下层耦合需迭代

有没有一种表述，既不在连续空间里被非凸性折磨，又不在离散空间里组合爆炸？ GCS 的回答是：把问题搬到一个**图**上——离散结构由图的拓扑承载，连续优化由每个顶点上的凸集承载，二者在"最短路径"这一个问题里自然耦合。

GCS 的核心构造¶

一张凸集图 $\mathcal{G}=(\mathcal{V},\mathcal{E})$ 由三件东西定义：

顶点 $v\in\mathcal{V}$：每个顶点配一个**凸集** $\mathcal{X}_v\subseteq\mathbb{R}^n$，以及一个定义在其上的连续变量 $x_v\in\mathcal{X}_v$。在接触规划里，一个顶点 = 一个固定接触模式（如"左脚支撑右脚摆"），凸集 $\mathcal{X}_v$ 描述该模式下的**准静态动力学可行域**（物体位姿、接触点、接触力的可行组合）。
边 $e=(u,v)\in\mathcal{E}$：每条边配一个**凸的边代价** $\ell_e(x_u,x_v)$ 和凸的边约束。边表示"接触模式 $u$ 可以切换到模式 $v$"，连续性约束（切换瞬间状态匹配）写成边约束。
源点 $s$、汇点 $t$：起始模式与目标模式。

关键洞察：从 $s$ 到 $t$ 的一条**路径**就对应一个**接触模式序列** $\sigma$；路径上每个顶点的连续变量 $x_v$ 就是该模式段内的轨迹。于是

\[\underbrace{\min_{\sigma}\;\min_{x}\;J(\sigma,x)}_{\text{双层：先选序列再优化}} \quad\Longleftrightarrow\quad \underbrace{\text{GCS 上的最短路径：同时选路径与路径上的连续变量}}_{\text{单层凸松弛}}\]

左边是 59.7 的双层结构，右边是 GCS——把上层的离散选择和下层的连续优化压进同一个数学对象。

为什么 GCS 能高效求解：紧凸松弛¶

朴素地看，"在图上找最短路径**且**优化路径上的连续变量"仍是一个混合整数凸规划（MICP）——给每条边一个二值变量 $y_e\in\{0,1\}$ 表示是否走这条边。GCS 的突破在于它的 MICP 表述有一个**异常紧的凸松弛**：

把二值 $y_e\in\{0,1\}$ 松弛为 $y_e\in[0,1]$；
用**透视函数（perspective function）**重写每条边的代价与约束——透视运算 $\tilde\ell_e(x,y)=y\,\ell_e(x/y)$ 保持凸性，且在 $y\in\{0,1\}$ 处恢复原问题；
加上流守恒约束（进每个顶点的流 = 出的流）。

结果：松弛后的凸问题（通常是 SOCP 或 SDP）的最优值与原 MICP 极其接近——实验中常常在全局最优的几个百分点以内，且松弛解可以被**快速舍入**（rounding）为一条可行路径。这与 59.7 的 Big-M MIQP 形成鲜明对比：Big-M 的 LP 松弛往往很松（间隙大），GCS 的透视松弛则很紧。

本质洞察：GCS 的威力来自一个表述层面的选择——用透视函数而非 Big-M 来编码"开关"约束。Big-M 用一个大常数把约束"撑开"，松弛后可行域被严重放大；透视函数则让连续变量的尺度随二值变量"成比例收缩"，松弛后的可行域紧贴整数可行域的凸包。这再次印证本章反复出现的主题：接触规划的难易，极大程度上取决于你如何把离散决策编码进连续优化——同一个组合结构，Big-M、互补约束、透视函数给出难度迥异的松弛。

应用到接触规划：Graesdal–Marcucci–Tedrake 2024¶

Graesdal 等人在 RSS 2024 的 "Towards Tight Convex Relaxations for Contact-Rich Manipulation"（arXiv:2402.10312）把 GCS 用于**接触丰富的操作**（以平面推物 planar pushing 为例），其构造对足式接触规划有直接借鉴意义：

GCS 要素	在接触规划中的实例化
顶点（接触模式）	物体与机械手/地面的一组接触激活组合（哪些面接触、黏滞还是滑动）
顶点上的凸集	该模式下准静态动力学的可行域——位姿、接触位置、接触力同时满足力平衡
非凸 → 凸	同时优化位姿、接触位置、接触力时出现的双线性项（力 $\times$ 力臂，正是 59.6.1 角动量方程里的非凸源）用 SDP 松弛为凸
边（模式切换）	接触的建立 / 断开 / 黏滑转换
求解	最短路径凸松弛 + 舍入，无需初值，常达全局最优的小百分比内

与双层方法（59.7）的对比：

维度	双层 MIQP+NLP（59.7）	GCS
离散与连续	分两层，需迭代耦合	单层，图上统一
凸松弛紧度	Big-M，间隙大	透视/SDP，间隙小
初值依赖	下层 NLP 依赖初值	无需初值
全局性	上层全局、下层局部	松弛 + 舍入，接近全局
成熟度	工程成熟（ANYmal parkour）	新兴（2022–2025），腿足应用刚起步
主要代价	迭代次数	SDP 规模随模式数增长，大问题仍吃力

局限与开放问题¶

GCS 不是银弹。当前主要瓶颈：

模式数爆炸进入图规模：双层方法的组合爆炸在 GCS 里转化为**图的顶点数**，足式 4 腿 $\times$ 多接触面时图很大，SDP 求解成本高；
SDP 可扩展性：半定松弛对大问题（高维、长时域）求解慢，目前更适合**操作**（低自由度物体）而非**全身足式**；
动态接触：现有 GCS 接触工作多基于**准静态**假设（忽略惯性），把它推广到含完整动力学的动态足式运动是活跃的开放问题。

💡 概念误区：GCS 让 CITO/MIQP 都过时了

新手想法："GCS 又凸又全局又不要初值，那 CITO 和双层 MIQP 不就被淘汰了？"

实际上：GCS 目前主要在**准静态、低自由度操作**任务上验证了优势。对**高维动态全身足式**运动，SDP 松弛的规模与求解时间还不实用——这正是 CITO（处理完整动力学）和双层 MIQP（工程成熟、实时化版本已上机器人）仍是主力的原因。

正确理解：GCS 是接触规划工具箱里的**第三件工具**，与 CITO、双层优化**互补**。它在"需要全局最优、无好初值、问题维度中等"的场景（很多操作任务、部分 stepping-stone 落脚规划）最有价值。

练习¶

练习 59.7.6a（⭐⭐⭐）：对 stepping stones 落脚规划（5 块石头、3 步），把它建模为一个 GCS：说明顶点（接触模式）、凸集（每块石头的凸多边形 + 落脚点变量）、边（相邻两步的连接 + 步长约束）分别是什么。与 59.7.2 的 Big-M MIQP 表述对比，指出二者编码"选哪块石头"的方式差异。

练习 59.7.6b（⭐⭐⭐⭐）：透视函数 $\tilde\ell(x,y)=y\,\ell(x/y)$（$y>0$）对凸 $\ell$ 保持凸性。以 $\ell(x)=x^2$ 为例写出 $\tilde\ell(x,y)$，验证它在 $\{(x,y):y>0\}$ 上凸，并说明 $y\in\{0,1\}$ 时它如何恢复原代价（含 $y=0$ 的极限行为）。解释为什么这比 Big-M 给出更紧的松弛。

59.8 学习驱动的接触规划 ⭐⭐⭐¶

动机：优化太慢，学习能帮忙吗？¶

本节解决的问题：CITO 和双层优化在求解速度上都有局限。能否用机器学习方法（RL、模仿学习、扩散模型）来加速甚至替代基于优化的接触规划？

59.8.1 RL 用于接触时序决策 ⭐⭐⭐¶

核心思想：训练一个神经网络策略 $\pi_\theta(\text{接触决策} \mid \text{状态, 地形})$，直接输出接触时序和落脚点。

为什么 RL 适合接触规划？ 接触规划的本质是一个**序贯决策问题**——每一步的落脚选择影响后续所有步。这恰好是 RL（马尔可夫决策过程）的建模对象。而且接触切换是离散的——RL 天然处理离散动作空间（如 DQN）或混合动作空间（如 SAC-Hybrid）。

形式化为 MDP：

MDP 元素	定义
状态 $s$	机器人状态（CoM 位置/速度/朝向 + 关节角度） + 地形高度图（局部 $1 \times 1$ m 网格）
动作 $a$	下一步的落脚点偏移 $\Delta\mathbf{p}_f \in \mathbb{R}^2$ + 接触时长 $\Delta t \in [0.1, 1.0]$
奖励 $r$	前进距离 $- 0.01 \times$ 能耗 $- 100 \times$ 摔倒惩罚
环境	MuJoCo / Isaac Gym 物理仿真器

代表工作：

ContactNet（Melon & Hutter, 2022, RA-L）：用 GNN（Graph Neural Network）预测可行的接触序列，比 MIQP 快 100 倍。GNN 的输入是 stepping stones 的图结构，输出是每步的石头选择概率。
DiffuseLoco（2024）：扩散策略生成多样的运动轨迹，包含隐式的接触规划——不直接输出"踩哪里"，而是生成完整的运动轨迹，接触信息隐含在轨迹中。
Parkour with RL（Zhuang et al., 2023, RSS）：端到端 RL 学习 parkour，自动发现跳跃、攀爬等接触策略。在 Unitree Go1 上实物部署。

RL vs CITO 的互补性：

维度	CITO	RL
需要模型	是（精确动力学）	否（model-free）或粗略模型
可解释性	高（KKT 条件）	低（神经网络黑盒）
单实例效率	一次求解（秒~分钟）	需数百万仿真训练（小时~天）
推理速度	慢（NLP 在线求解）	快（神经网络前向约 1 ms）
泛化	弱（per-instance）	强（训练后对新地形泛化）
最优性	局部最优保证	无保证
处理高维	难（>50 维吃力）	易（百维可训练）

59.8.2 CITO + 学习的融合策略 ⭐⭐⭐¶

策略 1：CITO 生成训练数据 → 模仿学习

离线阶段:
  for 1000 种地形:
      sigma*, x*, u* ← CITO.solve(地形)  // 每个约 10 秒
  → 得到 10000 条 (地形, 最优轨迹) 对

在线阶段:
  pi_theta(a | s) ← 训练策略网络模仿 CITO 的最优决策
  → 在线推理约 1 ms，接近 CITO 质量

代表工作：MPC-Net（Carius et al., ICRA 2020）用这个思路学习 MPC 的最优策略。

策略 2：学习 warm-start → CITO 精化

在线:
  z0 ← pi_theta(state, terrain)  // 学习的初值猜测, 约 1 ms
  z* ← CITO.solve(z0)            // 从好初值出发, 1-3 次迭代, 约 100 ms
  → 总计约 100 ms，比冷启动 CITO 快 10-100 倍

为什么 warm-start 能加速这么多？ NLP 求解器（Ipopt、iLQR）的收敛速度取决于初值距离最优解的远近。Newton 法在最优解附近有二次收敛速度——也就是说，如果初值误差为 $\delta$，经过 $k$ 次迭代后误差为 $O(\delta^{2^k})$。从 $\delta = 0.1$（好初值）出发，3 次迭代误差降到 $10^{-8}$；从 $\delta = 10$（坏初值）出发，需要 20+ 次迭代。

策略 3：分层——RL 高层 + 优化低层

RL 策略 (10 Hz):
  输出: 步态选择, 粗略落脚点, 接触时长
  → 替代 MIQP 的上层

MPC (50 Hz):
  输入: RL 给定的接触序列
  优化: 连续轨迹 (DDP/SQP)
  → 替代 NLP 的下层

WBC (500 Hz):
  输出: 关节力矩

这是目前工业界和学术界最热门的架构——ETH 的 ANYmal parkour、MIT 的 Cheetah 3、Unitree 的 Go2 都在探索这条路线。

59.8.3 扩散模型在接触规划中的应用（2024-2025 前沿） ⭐⭐⭐⭐¶

**扩散模型（Diffusion Models）**在机器人学中的应用正在迅速扩展。对于接触规划，扩散模型有两个关键优势：

多模态输出：stepping stones 问题可能有多条可行路径，扩散模型可以生成**多样化**的接触计划——而 RL 策略通常只输出一个（mode-seeking）
条件生成：可以条件在地形、目标、约束上生成——类似于"画一条满足物理约束的轨迹"

DIAL-MPC（Diffusion-Inspired Annealing for Legged MPC, 2024）：

将扩散过程的"去噪"思想融入 MPC 的求解过程： - 初始：在策略空间中随机采样（高噪声）→ 覆盖多个接触模式 - 逐步去噪：每步根据代价函数梯度修正采样 → 集中到好的模式 - 最终：收敛到高质量的轨迹

这种方法在全局搜索（找到正确的接触模式）和局部精化（在该模式下优化轨迹）之间取得了良好平衡——解决了传统 MPC 容易陷入局部最优的问题。

开放问题：

问题	难度	当前状态
约束满足	高	扩散模型不保证物理约束，需后处理或投影
实时性	中	当前约 100 ms/sample，需降至约 10 ms
安全性	高	无理论保证，需要 safety filter
与 MPC 的集成	中	DIAL-MPC 是初步尝试

🧠 思维陷阱：认为 RL 可以完全替代 CITO

新手想法："RL 训练好后推理只需 1 ms，比 CITO 快几千倍，为什么还需要 CITO？"

实际上： 1. RL 训练数据从哪来？ 如果没有 CITO 提供高质量 demonstration，纯 RL 需要更多探索（reward shaping 困难） 2. RL 无法保证最优性——它可能学到一个"还行"的策略，但不是最优的 3. RL 难以处理硬约束——关节限位、摩擦锥等物理约束在 RL 中只能用惩罚项近似，无法严格保证 4. RL 的泛化有限——训练分布外的地形可能完全失败（如训练时没见过冰面） 5. 可解释性——CITO 的解有 KKT 条件支撑，可以分析为什么选择某个接触；RL 的策略是黑盒

最佳实践：CITO 和 RL 是互补工具。CITO 用于离线分析、验证最优性、生成训练数据；RL 用于在线快速推理。两者结合优于任何单一方法。

练习¶

练习 59.8a（⭐⭐）：设计一个简单的 MDP 来训练 RL 策略选择落脚点。定义状态空间、动作空间、奖励函数。讨论奖励工程（reward shaping）的难点——为什么"到达目标"的稀疏奖励不够？

练习 59.8b（⭐⭐⭐）：解释为什么 CITO warm-start + 少量 NLP 迭代可以比冷启动快 10-100 倍。用 Newton 法的二次收敛性给出定量估计：如果冷启动初值误差为 $\delta_0 = 10$，warm-start 初值误差为 $\delta_0 = 0.1$，收敛到 $\delta < 10^{-6}$ 各需多少次迭代？

59.9 工程实现：Ifopt + Towr ⭐⭐¶

动机：理论到代码的桥梁¶

本节解决的问题：前面几节讲了 CITO 的数学理论。Winkler 的 Towr 是目前最易上手的 CITO 工程实现——它用 Ifopt + Ipopt 求解足式机器人的轨迹优化，支持 phase-duration 作为决策变量。

59.9.1 Towr 的架构 ⭐⭐¶

Towr 的代码结构:
─────────────────────────────
towr/
├── include/towr/
│   ├── models/          ← 机器人模型 (质量, 惯量, 腿长)
│   │   ├── endeffector_mappings.h
│   │   └── robot_model.h
│   ├── variables/       ← NLP 决策变量
│   │   ├── spline_holder.h        ← 样条参数
│   │   ├── phase_durations.h      ← 相位时长（核心创新!）
│   │   └── nodes_variables.h      ← 样条节点
│   ├── constraints/     ← NLP 约束
│   │   ├── dynamic_constraint.h   ← 动力学 (LIP / centroidal)
│   │   ├── range_of_motion_constraint.h ← 运动学
│   │   ├── terrain_constraint.h   ← 地形
│   │   └── total_duration_constraint.h ← 总时长
│   ├── costs/           ← NLP 代价
│   └── nlp_formulation.h  ← 顶层: 组装变量+约束+代价
└── src/
    └── ...

Towr 的核心设计决策：

决策	选择	理由
NLP 框架	Ifopt	轻量、与 Ipopt/Snopt 集成、纯 Eigen
轨迹参数化	三次 Hermite 样条	光滑、导数解析可用、参数少
动力学模型	单刚体 / 质心	降维，加速求解
接触处理	Phase-based（非 CITO）	避免互补约束的数值困难

重要澄清：Towr 不是严格的 CITO

Towr 不直接使用互补约束。它用 **phase-based 参数化**来处理接触时序——这是一个工程上的折中：

CITO 方式:
  互补约束 phi * lambda <= epsilon → 优化器自由选择接触/不接触
  → 完全自由，但数值困难

Towr 方式:
  预定义 phase 结构 (支撑/摆动交替)
  但 phase 的持续时间是决策变量
  → 优化器可以调整"每步走多久"，但不能改变"先左后右"的顺序
  → 数值稳定，但自由度受限

这是一个**折中**：放弃了 CITO 的完全自由度（不能发现全新的接触模式），换取数值稳定性和求解速度。对于大多数实用场景（已知步态类型，只需优化时机和落脚点），这个折中是值得的。

59.9.2 Phase-Duration 参数化 ⭐⭐¶

Winkler 2018 (RA-L) 的核心创新是让 phase duration 成为决策变量。

传统方法（固定时序）：

// 固定: 每个 phase 0.3 秒
// trot 步态: [支撑, 摆动, 支撑, 摆动, ...]
std::vector<double> phase_durations = {0.3, 0.3, 0.3, 0.3, 0.3};
// → 总时间 1.5 秒, 不可优化

Towr 方法（可优化时序）：

// towr/include/towr/variables/phase_durations.h (简化)
class PhaseDurations : public ifopt::VariableSet {
public:
    // 每个 phase 的时长都是决策变量
    VecBound GetBounds() const override {
        VecBound bounds;
        for (int i = 0; i < n_phases_; i++) {
            // 每个 phase 时长在 [0.1, 1.0] 秒之间
            bounds.push_back(ifopt::Bounds(0.1, 1.0));
        }
        return bounds;
    }

    Eigen::VectorXd GetValues() const override {
        return phase_durations_;  // 当前的 phase 时长
    }

    void SetVariables(const Eigen::VectorXd& x) override {
        phase_durations_ = x;  // Ipopt 更新 phase 时长
    }
};

为什么这样设计？ 关节角度和足端位置用三次 Hermite 样条参数化——每段样条的时间跨度由 phase duration 决定。当 phase duration 改变时，样条会"拉伸"或"压缩"，从而改变运动的时间节奏。这意味着优化器可以通过调整 phase duration 来隐式地改变步频和步幅的平衡。

效果：优化器可以自动发现非对称步态。例如，上坡时前腿支撑时间更长（需要更大的推力），下坡时后腿支撑时间更长（需要制动）。

约束：总时间固定

// towr/include/towr/constraints/total_duration_constraint.h
class TotalDurationConstraint : public ifopt::ConstraintSet {
    Eigen::VectorXd GetValues() const override {
        double total = phase_durations_->GetValues().sum();
        return Eigen::VectorXd::Constant(1, total - T_total_);
        // total - T = 0 → 总时间等于指定值
    }
};

59.9.3 用 Towr 实现四足轨迹优化 ⭐⭐¶

完整使用流程：

// 简化的 Towr 使用流程 (基于 towr 官方示例)
#include <towr/nlp_formulation.h>
#include <ifopt/ipopt_solver.h>

int main() {
    // 1. 定义地形
    auto terrain = std::make_shared<towr::FlatGround>(0.0);

    // 2. 定义机器人模型
    towr::RobotModel model = towr::RobotModel(towr::RobotModel::Anymal);

    // 3. 设置初始和目标状态
    towr::NlpFormulation formulation;
    formulation.terrain_ = terrain;
    formulation.model_ = model;
    formulation.initial_base_.lin.at(towr::kPos) << 0.0, 0.0, 0.5;
    formulation.final_base_.lin.at(towr::kPos) << 2.0, 0.0, 0.5;

    // 4. 设置步态 (trot, 4 个 phases)
    auto gait_gen = towr::GaitGenerator::MakeGaitGenerator(4);
    gait_gen->SetCombo(towr::GaitGenerator::C0);  // trot
    formulation.params_.ee_phase_durations_ =
        gait_gen->GetPhaseDurations();
    formulation.params_.ee_in_contact_at_start_ =
        gait_gen->GetContactSchedule();

    // 5. 组装 NLP
    ifopt::Problem nlp;
    for (auto c : formulation.GetVariableSets(formulation.params_))
        nlp.AddVariableSet(c);
    for (auto c : formulation.GetConstraints(formulation.params_))
        nlp.AddConstraintSet(c);
    for (auto c : formulation.GetCosts())
        nlp.AddCostSet(c);

    // 6. 求解
    ifopt::IpoptSolver solver;
    solver.SetOption("linear_solver", "ma57");
    solver.SetOption("max_cpu_time", 20.0);
    solver.Solve(nlp);

    // 7. 提取结果
    // formulation.GetTrajectory() 返回 base motion + EE motion + forces
    return 0;
}

为什么这段代码能工作？ 逐步解释设计选择：

步骤	做了什么	为什么这样做
地形	`FlatGround`	定义高度函数 $z = h(x, y)$，约束足端在地面上
模型	`RobotModel::Anymal`	提供质量、惯量、腿长等参数
变量	样条节点 + phase durations	用少量节点参数化连续轨迹
约束	动力学 + 运动学 + 地形 + 总时间	确保物理可行
求解器	Ipopt + MA57	稀疏非线性规划求解

⚠️ 编程陷阱：Towr 的初始猜测

错误做法：让 Ipopt 从全零初始值开始求解。

症状：Ipopt 报 "restoration failed" 或收敛到静止解（机器人不动）。

根因：Towr 的 NLP 高度非凸。从全零出发，优化器找不到"行走"这个局部最优——它会收敛到"站着不动"这个平凡解（代价为零的局部最优）。

正确做法：Towr 内部会根据初始/终端状态和步态生成一个合理的初始猜测（线性插值基座轨迹 + 默认足端位置）。如果自定义场景，确保初始猜测至少"大致正确"——比如基座轨迹是从起点到终点的直线。

💡 概念误区：Towr 是实时控制器

新手想法："Towr 能在 100 ms 内求解，可以做 MPC！"

实际上：Towr 求解的 100 ms 是**冷启动**时间。每次环境变化都需要重新求解。真正的 MPC 需要 warm-start（利用上一步的解），但 Towr 的样条参数化在时间步移动时需要重新参数化——warm-start 不够自然。

正确用法：Towr 用于**离线**或**低频**规划（约 1 Hz），生成的轨迹交给 OCS2/Crocoddyl 等在线 MPC 跟踪。

练习¶

练习 59.9a（⭐⭐）：安装 Towr（GitHub: ethz-adrl/towr）并运行默认的四足 trot 示例。对比固定 phase durations 和可优化 phase durations 的结果——观察优化器自动调整的步频。

练习 59.9b（⭐⭐⭐）：修改 Towr 示例，将终端高度从 0.5 m 改为 0.8 m（抬高 30 cm），观察优化器是否自动发现需要的动作。如果结果不理想，分析原因并尝试调整约束或初始猜测。

🔧 故障排查手册¶

接触优化问题的非凸性和互补约束使调试尤为困难。以下是 CITO / MIQP 工程实践中最常见的故障场景。

症状	可能原因	排查步骤	相关章节
MIQP 求解超时（>10 s）或 Branch-and-Bound 不收敛	整数变量过多（候选区域 > 20）、Big-M 值过松导致 LP 松弛间隙过大	1. 减少候选接触区域（只保留运动学可达的） 2. 收紧 Big-M 至约束真实上界的 1.5 倍 3. 用上一次解做 warm-start 4. 设置求解器时间上限并接受次优整数解	59.7
CITO 收敛到"所有接触力为零"的平凡解（机器人"飞"起来）	初始猜测远离物理可行解，优化器找到了满足互补约束但无物理意义的局部最优	1. 用静态平衡姿态作为初始猜测 2. 在代价函数中加入重力补偿先验项 $\\|\sum f_i + mg\\|^2$ 3. 对接触力加下界惩罚 4. 尝试多个随机初值取最优	59.2, 59.4
Ipopt 报 "Restoration Failed" 或 "Converged to local infeasibility"	互补约束在接触切换点导致 LICQ 失效，约束雅可比秩亏	1. 检查松弛参数 $\epsilon$ 是否过小（建议从 $10^{-1}$ 开始续延） 2. 切换到 Fischer-Burmeister 函数 3. 打印 KKT 残差定位是哪类约束出问题 4. 检查是否提供了稀疏模式	59.3
凸松弛间隙过大（松弛解与整数解目标差 >50%）	接触区域形状高度非凸、Big-M 过松、或松弛后的连续问题与原整数问题结构差异过大	1. 用 IRIS 或 CSPACE 对区域做更精细的凸分解 2. 增加 cutting plane（Benders cut）收紧松弛 3. 考虑用 GCS 替代传统 MIQP	59.7
Towr 优化出的轨迹在仿真中不可执行（关节超限或滑脚）	质心轨迹优化忽略了全身运动学约束、或摩擦系数设置与仿真不一致	1. 检查 Towr 的运动学约束是否匹配实际机器人 URDF 2. 对比 Towr 的摩擦系数与仿真器设置 3. 在 Towr 解上跑逆运动学验证关节角度范围	59.9
提取出的接触力随时间剧烈震荡（chattering）	互补约束的数值刚性：松弛参数 $\epsilon$ 或光滑化参数 $\sigma$ 过小，求解器在接触切换点反复横跳	1. 适度增大 FB 光滑化参数 $\sigma$（如从 $10^{-6}$ 提到 $10^{-4}$） 2. 在代价中加入接触力变化率惩罚 $\sum_k \\|\boldsymbol{\lambda}_{k+1}-\boldsymbol{\lambda}_k\\|^2$ 3. 检查时间步长 $h$ 是否过大导致冲量分配跳变	59.3, 59.5
变分积分器长时域仿真出现系统性能量漂移	时间步长 $h$ 过大，或位形更新未走流形（SO(3) 上用了加法而非指数映射）	1. 减小 $h$ 或改用更高阶辛格式 2. 在 SE(3) 上用指数映射更新位形（见 59.5.3 陷阱） 3. 监控系统总能量 $E_k = T_k + V_k$，辛积分器应表现为有界振荡而非单调漂移	59.5

59.10 本章小结¶

知识点总结¶

小节	核心知识点	难度	关键公式/概念
59.1	优化驱动的动机	⭐	启发式天花板，接触作为决策变量
59.2	CITO 数学基础	⭐⭐⭐	完整 NLP 公式，时间步进，约束雅可比稀疏性
59.3	互补约束处理	⭐⭐⭐	松弛法，Fischer-Burmeister，增广拉格朗日
59.4	Posa 2014 精读	⭐⭐⭐	Direct collocation + MPCC 松弛
59.5	变分 CITO	⭐⭐⭐⭐	变分积分器，辛结构保持，二阶精度
59.6	质心轨迹优化	⭐⭐⭐	质心动力学 + 接触协同，接触指示器
59.7	双层优化	⭐⭐⭐⭐	MIQP + NLP 分解，Big-M，Benders cut
59.8	学习驱动	⭐⭐⭐	RL/扩散模型，CITO + 学习融合
59.9	Towr 工程实现	⭐⭐	Phase-duration 参数化，Ifopt + Ipopt

方法选择决策树¶

                需要接触规划?
               /             \
            是                 否
           /                     \
      地形已知?              → 足式/110_OCS2完整栈与双线程MPC 标准 MPC
     /        \
   是          否
   /            \
 离线可以?    → RL 端到端策略
 /      \
是       否
/         \
CITO/Towr  双层优化(实时化版本)

本质洞察: Contact-Implicit TO 的数学本质是**用连续优化框架求解离散组合问题**。互补约束 $\phi \cdot \lambda = 0$ 把"接触/不接触"的二值决策编码为连续变量之间的乘积等于零。这种编码虽然在数学上优雅(避免了枚举 $2^{n_c \times N}$ 种模式组合),但代价是可行集变成了两个正象限的并集——一个根本性非凸的集合。所有 CITO 方法的核心技巧,无论是松弛、FB 函数还是 ALM,本质上都是在"近似凸化"这个非凸可行集。松弛法把 L 形可行集"充气"为一个凸区域;FB 函数把 L 形可行集"光滑化"为一条曲线;ALM 把约束违反"罚入"目标函数,让优化器在不可行域中也能定义梯度。理解这一点,就理解了为什么 CITO 对初值敏感、为什么会收敛到局部最优、为什么不同松弛方法在不同问题上表现迥异。

本质洞察: 双层优化(59.7)揭示了接触规划的一个深层结构:离散决策(哪里接触)和连续优化(力和轨迹)之间存在天然的层次分离。上层的离散决策定义了下层连续问题的约束结构;下层的连续优化结果反过来评估上层决策的质量。这种层次结构不是人为强加的,而是问题本身的数学结构——接触模式一旦确定,剩下的就是一个标准的(相对容易的)轨迹优化问题。双层分解的智慧在于:与其在一个巨大的非凸问题中同时搜索离散和连续变量,不如把它们分开处理——用组合搜索(MIQP/MCTS)处理离散部分,用连续优化(NLP/DDP)处理连续部分。

Contact-Implicit MPC 最新进展(2023-2025) ⭐⭐⭐⭐¶

传统观点认为 CITO 只能离线使用,但 2023 年以来,多个团队在推动 Contact-Implicit MPC 的实时化:

Dojo (Howell et al. 2022, RSS):Dojo 是一个可微接触仿真器,核心创新是用**光滑化接触动力学(smoothed contact dynamics, randomized smoothing)**替代传统的 LCP 来处理接触。光滑化方法在接触点处提供平滑的梯度(而非传统 LCP 的不连续梯度),使得基于梯度的 MPC 求解器可以直接优化穿过接触切换的轨迹。在 2D 推箱子任务上,Dojo + iLQR 实现了约 50 ms 的在线重规划。

ContactNets (Pang et al. 2023, T-RO):提出了"准静态光滑化"(Quasi-Dynamic Smoothing)方法。核心思想:在接触点附近用可微的"软接触"模型替代硬互补约束,使得整个动力学方程在接触切换处连续可微。这使得 Newton 型求解器可以二次收敛,比传统 CITO 快 10-50 倍。

ACAL-iLQR (Chen et al. 2025, Advanced Intelligent Systems):将增广拉格朗日法(ALM)嵌入 iLQR 的 backward pass 中,实现了"无需固定接触序列的 iLQR"。在简单系统(弹跳球、单腿跳)上实现了约 1 秒求解,是目前最接近实时的全身 CITO 方法之一。

方法	求解时间(四足)	接触模型	可微性	成熟度
Posa 2014 (IPOPT)	10-100 s	LCP 嵌入 NLP	不可微(松弛近似)	高(经典)
Dojo 2022	0.05-1 s (低维)	光滑化接触(randomized smoothing)	可微	中
ContactNets 2023	1-10 s	准静态光滑化	可微	中
ACAL-iLQR 2025	0.5-5 s	ALM + iLQR	近似可微	低(新)

Gap 分析: 从上表可以看出,最快的 CITO 在四足上仍需约 0.5 秒(低维简化模型)到数十秒(全身模型)。与 MPC 要求的 5-50 ms 仍有 1-2 个数量级的差距。缩小这个差距的三条路径是:(1) GPU 并行化(见下),(2) 学习型 warm-start(59.8 节),(3) 更高效的求解器定制(如 CALIPSO)。

GPU-Accelerated CITO ⭐⭐⭐¶

GPU 加速是 CITO 实时化的最有前景的方向之一。核心思路:CITO 中最耗时的部分是约束雅可比的计算和 KKT 系统的求解,两者都有大量的并行结构。

MuJoCo MJX (2024-2025):MuJoCo 的 JAX 后端(MJX)允许在 GPU 上批量并行运行可微物理仿真。结合 JAX 的自动微分,可以在 GPU 上同时计算数千条轨迹的梯度——这是 CITO 的 shooting method 所需要的。在 NVIDIA A100 上,MJX 可以以 >10 万步/秒的速度仿真四足机器人,比 CPU 快 100-1000 倍。

cuHPIPM / GPU QP 求解器:HPIPM 目前是纯 CPU 实现。将 block-sparse QP 求解移植到 GPU 上是一个活跃的研究方向。cuHPIPM (非官方社区项目) 尝试将 HPIPM 的 Riccati recursion 并行化,但对 CITO 中的变结构 QP(约束集合随接触模式变化)支持有限。

工程前景: 乐观估计,GPU 加速的 Contact-Implicit MPC 有望在 2027-2028 年达到 50-100 ms 级别的求解速度(在高端 GPU 如 Orin/Jetson Thor 上)。这将使得"实时 CITO"从理论走向实践——MPC 在每个控制周期都能重新发现最优的接触策略,而非依赖预定义步态。

关键 takeaway¶

接触规划是混合优化问题——离散选择（哪里接触）+ 连续优化（力和轨迹），本质上 NP-hard
互补约束是核心数学挑战——三种处理方法（松弛、FB、ALM）各有优劣，没有银弹
全身 CITO 目前仍是离线工具——与实时 MPC 之间存在 1-2 个数量级的速度差距,但差距正在快速缩小
分层/双层架构是工程最佳实践——MIQP/CITO 离线规划 + MPC 在线跟踪 + WBC 实时执行
学习是加速 CITO 的关键方向——warm-start、模仿学习、扩散模型正在缩小优化与学习之间的差距
GPU 并行化是硬件层面的加速路径——MuJoCo MJX + JAX 使得可微 CITO 的 GPU 实现成为可能

累积项目：本章新增模块¶

累积项目：从零构建四足轨迹优化器

本章在累积项目中新增以下模块：

项目进度:
足式/50_空间向量与浮动基座动力学: 加载 URDF, 前向运动学       [完成]
足式/60_QP_NLP建模: NLP 框架 (Ifopt + Ipopt)    [完成]
足式/70_腿足简化模型理论: 简化模型 (LIPM, SRBD)       [完成]
足式/80_接触力学与约束优化: 接触力学约束                 [完成]
足式/90_WBC分层优化与TSID: WBC 基础                    [完成]
足式/100_DDP家族与Crocoddyl: DDP 实现                    [完成]
足式/110_OCS2完整栈与双线程MPC: OCS2 MPC                    [完成]
足式/140_落脚点规划经典方法: Raibert 落脚                [完成]
足式/150_优化驱动落脚与接触规划: ← 本章新增
  ├── [M1] 互补约束求解器: 用 CasADi 实现松弛法 + FB 函数
  ├── [M2] 2D CITO: 单腿弹跳的 Contact-Implicit 优化
  ├── [M3] Towr 集成: 安装 Towr, 运行四足 trot 优化
  └── [M4] Phase-duration 实验: 对比固定/可变 phase 的效果

M1: 互补约束求解器（Python + CasADi）

目标：实现一个通用的互补约束求解器，支持松弛法和 FB 函数，作为后续 CITO 的基础组件。验证标准：在 1D 互补问题上，两种方法的解误差 < $10^{-6}$。

M2: 2D CITO（Python + CasADi + Ipopt）

目标：对一个 2D 单腿弹跳机器人实现完整的 CITO——从地面跳到目标高度，自动发现起跳时机。验证标准：优化器输出的接触时序应该在物理上合理（先蓄力 → 起跳 → 飞行 → 着陆）。

M3: Towr 集成（C++ + ROS）

目标：安装 Towr，运行四足 trot 示例，可视化轨迹。验证标准：能成功运行并看到合理的行走轨迹。

M4: Phase-duration 实验（C++）

目标：对比 Towr 中固定 phase duration 和可优化 phase duration 的结果差异。验证标准：记录两种设置的求解时间、最终代价、步频变化。

延伸阅读¶

必读经典（⭐⭐）¶

论文/资源	年份	核心贡献	难度
Posa, Cantu, Tedrake. "A direct method for trajectory optimization of rigid bodies through contact" — IJRR	2014	CITO 奠基论文，MPCC 松弛，direct collocation	⭐⭐⭐
Mordatch, Todorov, Popovic. "Discovery of complex behaviors through contact-invariant optimization" — SIGGRAPH	2012	接触指示器，复杂行为自动发现	⭐⭐⭐
Winkler et al. "Gait and trajectory optimization for legged systems through phase-based end-effector parameterization" — RA-L	2018	Towr, phase-duration 参数化	⭐⭐
Manchester, Kuindersma. "Contact-implicit trajectory optimization using variational integrators" — IJRR	2019	变分积分器 CITO，二阶精度，辛结构	⭐⭐⭐⭐

近期前沿（⭐⭐⭐）¶

论文/资源	年份	核心贡献	难度
Pang et al. "Global planning for contact-rich manipulation via local smoothing of quasi-dynamic contact models" — T-RO	2023	准静态光滑化 CITO，采样 + 优化融合	⭐⭐⭐⭐
Howell et al. "CALIPSO: A differentiable solver for trajectory optimization with conic and complementarity constraints" — ISRR	2022	ALM + 内点法定制求解器	⭐⭐⭐⭐
"Bilevel optimization for real-time control with application to locomotion gait generation"	2024	实时双层优化，warm-started MIQP + iLQR	⭐⭐⭐⭐
"A novel contact-implicit trajectory optimization framework for quadruped locomotion" — Adv. Intell. Syst.	2025	ACAL-iLQR，无需固定接触序列	⭐⭐⭐⭐

教材与综述¶

资源	内容	难度
Wensing et al. "Optimization-based control for dynamic legged robots" — 综述	2023 综述，全面覆盖足式优化控制	⭐⭐⭐
Tedrake. "Underactuated Robotics" — 在线教材 Ch12-14	Contact dynamics + CITO 教学	⭐⭐
Towr GitHub: ethz-adrl/towr	最易上手的 CITO 代码	⭐⭐
Drake documentation: Contact planning tutorials	工业级 CITO/GCS 实现	⭐⭐⭐

开放研究方向（博士选题参考）¶

方向	核心问题	难度	活跃度
实时 CITO	如何把 CITO 从秒级降到毫秒级？GPU 并行？定制求解器？	⭐⭐⭐⭐	极高
CITO + 感知	如何将不确定的地形感知融入 CITO？鲁棒优化？随机规划？	⭐⭐⭐⭐	高
扩散模型 + 接触	扩散模型如何保证物理约束满足？投影？引导？	⭐⭐⭐⭐	极高（2025 热点）
多接触 loco-manipulation	手脚协同的接触规划，接触模式组合爆炸更严重	⭐⭐⭐⭐	高
GCS + 腿足	Graph-of-Convex-Sets 用于足式接触序列，凸松弛的理论保证	⭐⭐⭐	中（新兴）

[跨章综合题] 练习 59.X：从互补约束到 MPC 部署的完整管线¶

本题需要综合足式/80_接触力学与约束优化(接触力学)、足式/100_DDP家族与Crocoddyl(DDP)和本章(CITO)的知识。

场景: 你要为一个单腿弹跳机器人设计 CITO,使其从地面跳到 0.5m 高的平台上。

(足式/80_接触力学与约束优化) 写出地面接触的互补约束。弹跳机器人在起跳瞬间,法向力 $\lambda_n$ 从正值跳到零——这个不连续性对优化器意味着什么?为什么 LICQ 在这个点失效?
(本章 59.3) 分别用松弛法($\epsilon = 0.01$)和 Fischer-Burmeister 函数处理这个互补约束,写出修改后的 NLP 约束。用 Python/CasADi 实现两种方法,对比收敛迭代次数和最终约束违反量。
(足式/100_DDP家族与Crocoddyl) 如果用 ACAL-iLQR(DDP 嵌入 ALM)替代 IPOPT 直接求解,DDP 的 backward pass 中如何处理互补约束?提示:增广拉格朗日项 $\frac{\rho}{2}\|\phi \cdot \lambda\|^2 + \mu(\phi \cdot \lambda)$ 加入 running cost 后,$Q_{uu}$ 矩阵如何变化?
(综合) CITO 规划出的跳跃轨迹要交给在线 MPC 跟踪(足式/110_OCS2完整栈与双线程MPC)。设计一个分层架构:CITO 离线生成参考轨迹(包括接触时序和接触力) → OCS2 MPC 在线跟踪。MPC 需要知道哪些来自 CITO 的信息?如何将 CITO 的接触序列转化为 OCS2 的 ModeSchedule?

本章常见误解汇总¶

下表汇总全章出现的高频误解，把散落在各节"概念误区/思维陷阱"里的要点集中成一张速查表，便于复习时一次性自检：

误解	正确理解	出处
CITO 能替代 MPC、可以实时跑	全身 CITO 是离线/低频规划工具（秒~分钟级），与 MPC（5–50 ms）差 1–3 个数量级；生成的轨迹交给在线 MPC 跟踪	59.1
互补约束 $\phi\lambda=0$ 就是 `if(contact)...else...` 分支	if-else 在切换点不可微，Newton 法无法穿过；互补约束把它编码为连续但不光滑的等式，再光滑化后可微求解	59.3
松弛只是"近似"，所以解不准	续延让 $\epsilon\to 0$，最终违反 $O(10^{-4})$ 工程上等价精确；真正的风险不是精度而是收敛到错误局部最优	59.3
求解器报 "Optimal" 就等于找到局部最优	对松弛后的 MPCC，"收敛"只保证 C-平稳，比局部最优弱；C-平稳点可能是伪解	59.3.5
摩擦约束就是 $\\|\boldsymbol{\lambda}^t\\|\le\mu\lambda^n$ 一条不等式	还需最大耗散原理锁定摩擦力方向（反向于滑动），否则优化器会"顺着滑动方向推"	59.2.4
CITO 必须把摩擦锥切成多面体	多面体化是 LCP 仿真的需要；用 NLP 求解器的 CITO 可直接保留光滑二阶锥，避免各向异性误差	59.2.4
变分积分器总比普通积分器好	它需隐式求解 + 实现复杂；短时域（MPC 窗口）一阶足够，优势只在长时域规划体现	59.5
双层优化就是"先上层再下层两次搞定"	上层选的序列可能在下层不可行，需 Benders cut 迭代；上层目标 $J^*(\sigma)$ 每评估一次就要解一个下层 NLP	59.7
Towr 是严格的 CITO，能做实时 MPC	Towr 用 phase-based 参数化而非互补约束（不发现全新接触模式）；其 100 ms 是冷启动，warm-start 不自然，应离线/低频用	59.9
GCS 让 CITO 和 MIQP 都过时了	GCS 目前主要在准静态低自由度操作上验证优势；高维动态全身足式仍靠 CITO / 双层 MIQP	59.7.6
RL 可以完全替代 CITO	RL 无最优性保证、难处理硬约束、泛化有限；CITO 提供训练数据与 baseline，二者互补	59.8

本章与后续章节的关系¶

后续章节	关系	本章铺垫的知识点
足式/160_感知驱动落脚规划	直接延续	本章假设地形已知；160 章把地形感知（高度图、可踏区分割）接入落脚规划，回答"GCS/MIQP 的凸接触区域从哪来"。本章开放方向"CITO + 感知"是其引子
足式/170_实时CPP工程	工程落地	本章的 NLP/Ifopt 求解、稀疏雅可比、warm-start 都需要高效 C++ 实现；170 章讲实时约束下的代码组织
足式/210_RL与MPC混合范式	范式融合	本章 59.8（学习驱动）是其核心动机之一——CITO 生成 demonstration、学习 warm-start、RL 高层 + 优化低层的分层架构在 210 章系统展开
足式/230_Perceptive_MPC	综合应用	本章"离线 CITO + 在线 MPC"分层 + 160 章感知，共同构成感知-规划-控制全栈，230 章把它们整合
足式/240_legged_control精读 / 250_Mini-Legged综合实战	源码与实战	本章的接触序列、摩擦锥、质心动力学是这些实战章里 MPC/WBC 模块的理论前提

API 速查表¶

本章涉及的核心库与 API（按出现顺序）。括号内为所属库：

API / 类	所属	作用	关键参数 / 说明
`ifopt::VariableSet`	Ifopt	NLP 决策变量基类	重写 `GetValues/SetVariables/GetBounds`；Towr 的 `PhaseDurations`、`NodesVariables` 继承它
`ifopt::ConstraintSet`	Ifopt	NLP 约束基类	重写 `GetValues/FillJacobianBlock`；必须提供稀疏模式否则退化为稠密
`ifopt::CostTerm`	Ifopt	NLP 代价基类	重写 `GetCost/FillJacobianBlock`
`ifopt::Problem`	Ifopt	组装变量 + 约束 + 代价	`AddVariableSet / AddConstraintSet / AddCostSet`
`ifopt::IpoptSolver`	Ifopt→Ipopt	调用 Ipopt 求解	`SetOption("linear_solver","ma57")`、`SetOption("max_cpu_time", T)`
`eval_jac_g(iRow,jCol,values)`	Ipopt C 接口	约束雅可比回调	`values==nullptr` 时返回稀疏模式（行列索引），否则填非零值（见 59.2.3 陷阱）
`towr::NlpFormulation`	Towr	顶层 NLP 组装器	`terrain_ / model_ / initial_base_ / final_base_ / params_`
`towr::RobotModel`	Towr	机器人模型（质量/惯量/腿长）	`RobotModel::Anymal` 等预置型号
`towr::GaitGenerator`	Towr	生成 phase 时序与接触表	`MakeGaitGenerator(n_ee)`、`SetCombo(C0)`（C0=trot）
`towr::FlatGround` / `HeightMap`	Towr	地形高度函数 $z=h(x,y)$	约束足端落在地面上
`casadi::SX / MX`	CasADi	符号变量（构造 NLP）	用于累积项目 M1/M2 实现互补约束求解器
`casadi::nlpsol("solver","ipopt",...)`	CasADi	构造 Ipopt 求解器对象	Python 端最简 CITO 原型路径
`GRBaddgenconstrIndicator`	Gurobi	indicator 约束	替代 Big-M，松弛更紧（见 59.7.2）

环境提示：Ipopt 的高性能线性求解器 MA57/MA27 来自 HSL，需单独申请学术许可；开源替代是 MUMPS（linear_solver=mumps），性能略低但免授权。Towr 仓库：ethz-adrl/towr；GCS 参考实现见 Drake 的 GraphOfConvexSets。

研究实践建议¶

按你的目标分层给出建议：

如果你是初次接触 CITO（入门）： - 不要一上来啃 Posa 2014 原文。先用 Python + CasADi 实现累积项目 M1（1D 互补求解器）和 M2（2D 单腿弹跳 CITO），亲手感受"松弛 $\epsilon$ 一调小就不收敛"。 - 跑通 Towr 的四足 trot 示例（M3），对照 59.9 理解 phase-based 与 CITO 的区别。这是性价比最高的"看得见结果"的起点。

如果你要做 CITO 方向的研究（进阶）： - 精读 59.3.5（MPCC 平稳性）+ Scholtes 正则化原文，理解你的求解器到底收敛到哪一级平稳性——这决定了论文里"我们的解是最优的"这句话能不能写。 - 复现 Posa 2014 的 2D 弹跳球 + 双足，再读 Manchester 2019 理解变分积分器的增量。基准要在同一系统上对比一阶 vs 二阶精度（59.5.3 的表）。 - 关注 2022 年以来的实时化路线：Dojo（光滑化）、Pang 2023（准静态光滑化）、GCS（凸松弛）、ACAL-iLQR。选一个 gap（59 章小结的 Gap 分析表）作为切入。

如果你要把接触规划落地到真机（工程）： - 优先选**双层架构**而非单层全身 CITO：上层 MIQP/GCS 离线或 1 Hz 规划接触序列与落脚点，下层用 OCS2/Crocoddyl 的 MPC（足式/100、110）在线跟踪，WBC（足式/90）500 Hz 执行。 - 把 59 章故障排查手册贴在工位上：90% 的"不收敛"来自初值差、$\epsilon$ 过小、忘记稀疏模式、摩擦系数与仿真不一致这四类。 - 上真机前务必在 Towr/CITO 解上跑一遍逆运动学校验关节范围（59.9 最后一条陷阱），质心优化的解很可能运动学不可行。

共同的元建议：本章所有方法的成败都系于一个问题——你把离散接触决策编码成了什么（互补约束 / Big-M / 透视函数 / phase 结构）。遇到新问题时，先问"我的编码让松弛变紧还是变松"，再选求解器。这条主线串起了全章。

变量类别	维度	ANYmal 示例
状态 \(\mathbf{x}_k\)	\(2n_q\)	\(2 \times 19 = 38\)（含浮动基座）
控制 \(\mathbf{u}_k\)	\(n_u\)	\(12\)（12 个关节）
接触力 \(\boldsymbol{\lambda}_k\)	\(3 n_c\)	\(3 \times 4 = 12\)（4 个足端）
每步决策维度	\(2n_q + n_u + 3n_c\)	\(38 + 12 + 12 = 62\)
N=50 步总维度	\(N \times (2n_q + n_u + 3n_c) + 2n_q\)	\(\approx 3138\)

MDP 元素	定义
状态 \(s\)	机器人状态（CoM 位置/速度/朝向 + 关节角度） + 地形高度图（局部 \(1 \times 1\) m 网格）
动作 \(a\)	下一步的落脚点偏移 \(\Delta\mathbf{p}_f \in \mathbb{R}^2\) + 接触时长 \(\Delta t \in [0.1, 1.0]\)
奖励 \(r\)	前进距离 \(- 0.01 \times\) 能耗 \(- 100 \times\) 摔倒惩罚
环境	MuJoCo / Isaac Gym 物理仿真器

积分格式	速度更新	位置更新	稳定性	能量守恒
显式欧拉	\(\dot{q}_{k+1} = \dot{q}_k + h a_k\)	\(q_{k+1} = q_k + h \dot{q}_k\)	差（发散）	能量增长
半隐式	\(\dot{q}_{k+1} = \dot{q}_k + h a_k\)	\(q_{k+1} = q_k + h \dot{q}_{k+1}\)	好	近似守恒
隐式欧拉	需解方程	需解方程	很好	能量耗散

问题	数学描述	后果
非凸性	\(\{(a,b): a \ge 0, b \ge 0, ab = 0\}\) 不是凸集	局部最优不等于全局最优
LICQ 失效	在 \(a = b = 0\) 处，\(\nabla(ab) = [b, a]^T = [0, 0]^T\)	KKT 条件不适用，Lagrange 乘子不唯一
非光滑性	可行域在原点处"弯折"	梯度不连续，Newton 法收敛性丧失

策略	\(\epsilon\) 更新	优点	缺点
固定比率	\(\epsilon_{k+1} = 0.1 \cdot \epsilon_k\)	简单	可能跳过解
自适应	根据 KKT 残差调整	稳健	实现复杂
Ipopt 内置	利用 Ipopt 的 barrier parameter	代码量最少	收敛可能慢

方面	松弛法	FB 函数
约束类型	不等式 \(\phi\lambda \le \epsilon\)	等式 \(\psi_{\text{FB}} = 0\)
LICQ	仍然可能有问题	总是满足（梯度在 \(a=b=0\) 处非零）
续延	需要多轮	不需要
梯度	在 \(\phi = \lambda = 0\) 处有问题	在 \(a = b = 0\) 处梯度存在但不光滑

方法	数学原理	优点	缺点	代表实现
松弛法	\(\phi\lambda \le \epsilon\)	简单，与 Ipopt 无缝集成	需要续延，\(\epsilon\) 调参	Posa 2014
FB 函数	\(\psi_{\text{FB}} = 0\)	不需续延，LICQ 恢复	需要光滑化	学术研究
增广拉格朗日	\(\min J + \mu c + \frac{\rho}{2}c^2\)	\(\rho\) 不需趋于 \(\infty\)，条件好	外层循环，实现复杂	CALIPSO

数值方法（59.3）	典型收敛到	后果
松弛法（\(\epsilon\to 0\)，Scholtes 正则化）	C-stationary（一般情形）；额外二阶条件下可达 M-/S-	可能停在 C-平稳但非局部极小的"伪解"
Fischer-Burmeister（光滑化）	C-stationary（光滑参数 \(\to 0\)）	与松弛类似，依赖收敛路径
增广拉格朗日 / 精确罚	适当条件下 M-stationary	比纯松弛强，排除更多伪解

方面	Shooting（DDP）	Direct Collocation（Posa）
决策变量	只有 \(\mathbf{u}_{0:N-1}\)	\(\mathbf{x}_{0:N}, \mathbf{u}_{0:N-1}, \boldsymbol{\lambda}_{0:N-1}\)
动力学	前向积分得到 \(\mathbf{x}_{1:N}\)	作为等式约束
优点	变量少	初值不需要满足动力学，更鲁棒
缺点	初值敏感，混沌系统不稳定	变量多，NLP 维度大
对 CITO 的关键影响	接触力无法作为决策变量	接触力自然成为决策变量

方面	一阶时间步进（Posa）	二阶变分积分器（Manchester）
精度	\(O(h)\)	\(O(h^2)\)
同等精度所需步数	\(N = 200\)	\(N = 50\)
求解时间	约 120 s	约 30 s
能量误差	累积增长	有界振荡

步骤	做了什么	为什么这样做
地形	`FlatGround`	定义高度函数 \(z = h(x, y)\)，约束足端在地面上
模型	`RobotModel::Anymal`	提供质量、惯量、腿长等参数
变量	样条节点 + phase durations	用少量节点参数化连续轨迹
约束	动力学 + 运动学 + 地形 + 总时间	确保物理可行
求解器	Ipopt + MA57	稀疏非线性规划求解

第 59 章 优化驱动的落脚与接触规划——Contact-Implicit TO / 互补松弛 / 双层分解 / 学习融合¶