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

学习融合¶

难度: ⭐⭐⭐~⭐⭐⭐⭐ | 前置: Ch50 NLP/QP, Ch52 接触力学与互补约束, Ch54 DDP, Ch58 落脚点经典方法 | 学时: 20-25 小时

前置自测¶

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

互补约束 $0 \le \phi \perp \lambda \ge 0$ 的三个组成条件分别是什么？其几何含义是什么？（→ Ch52）
**NLP 的 KKT 条件**中，互补松弛条件 $\mu_i g_i(x) = 0$ 的物理含义是什么？为什么它会导致数值困难？（→ Ch50）
DDP 的 backward pass 中，$Q_{xx}, Q_{xu}, Q_{uu}$ 矩阵分别代表什么？为什么 $Q_{uu}$ 必须正定？（→ Ch54）
Raibert 落脚启发式 $\boldsymbol{p}_f = \boldsymbol{p}_{\text{hip}} + \frac{T_s}{2}\dot{\boldsymbol{p}} + k(\dot{\boldsymbol{p}} - \dot{\boldsymbol{p}}_{\text{ref}})$ 中每一项的物理意义是什么？它在什么场景下会失效？（→ Ch58）
LICQ（线性独立约束品性） 失效时，NLP 求解器会出现什么问题？（→ Ch50）

本章目标¶

学完本章，你应能：

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

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

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

本节解决的问题：Ch55-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}}\]

在 Ch55-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 接触仿真公式（Ch52）。在正向仿真中，给定状态 $\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$

对比 Ch54 的 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.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.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 群积分器（Ch49 的知识），把变分积分器推广到 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 的运动。代价是丢失了关节层面的信息——优化出的质心轨迹可能因为运动学约束（腿长有限、关节限位）而不可执行。

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

\[\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（Ch51）多了角动量维度，但比全身动力学少了关节维度。

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$，下层就是 Ch54-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，Ch54 的 DDP 或 Ch55 的 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.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

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

方法选择决策树¶

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

关键 takeaway¶

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

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

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

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

项目进度:
Ch49: 加载 URDF, 前向运动学       [完成]
Ch50: NLP 框架 (Ifopt + Ipopt)    [完成]
Ch51: 简化模型 (LIPM, SRBD)       [完成]
Ch52: 接触力学约束                 [完成]
Ch53: WBC 基础                    [完成]
Ch54: DDP 实现                    [完成]
Ch55: OCS2 MPC                    [完成]
Ch58: Raibert 落脚                [完成]
Ch59: ← 本章新增
  ├── [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 用于足式接触序列，凸松弛的理论保证	⭐⭐⭐	中（新兴）

变量类别	维度	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

方面	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 / 互补松弛 / 双层分解 / 学习融合¶