接触隐式轨迹优化与 MPC：从 CI-TO 到实时 CI-MPC¶

专题定位：本章把接触力学专题 10–50（互补与 Signorini、摩擦锥凸松弛、时步法、可微接触、混合系统与 Saltation）的全部数学，收束到一个工程终点——机器人在接触环境中如何实时做出"踩、推、抓、爬、扶"的决策。

档位：核心档位 3（博士入学）＋进阶档位 4（博士毕业＋） 建议时长：档位 3 约 18–24 h；档位 4 额外 10–16 h 前置：本目录 §10 互补问题与 Signorini 条件、§20 摩擦锥理论与凸松弛、§30 时步法与数值积分、§40 可微接触仿真、§50 混合动力系统与 Saltation、§70 非光滑分析基础；控制理论 §50 LQR/LQG 与 Riccati；数值优化（SQP/IPM/ADMM）下游：04 移动机器人规控 §30 不确定性规划（chance-complementarity）、05 运动控制 §10 足式（§140/§150 落脚与接触规划）、第八批 Learning-based Control

前置自测¶

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

互补条件：写出标量互补条件 $0\le a\;\perp\;b\ge 0$ 的完整含义（三个分量），并说明它为什么等价于 $\min(a,b)=0$。如果 $a$ 是间隙距离、$b$ 是法向接触力，这个条件在物理上表达了什么？（→ §10）
摩擦锥：库仑摩擦的二阶锥约束 $\|\lambda_t\|\le\mu\lambda_n$ 描述了什么几何对象？为什么在优化里常把它替换成多面体（polyhedral）近似？这种替换引入了什么误差？（→ §20）
时步法：Stewart-Trinkle 时步法把一个时间步内的接触求解写成一个 LCP（线性互补问题）。为什么这种"隐式"积分能保证刚性接触不穿透，而显式积分会爆破？（→ §30）
隐函数定理与可微接触：若把接触力 $\lambda$ 看成状态 $q,\dot q$ 的隐函数（由 NCP 定义），在什么条件下 $\lambda(q,\dot q)$ 对参数是 $C^1$ 可微的？active set 在什么时候会破坏可微性？（→ §40）
Saltation 矩阵：混合系统穿越 guard 面 $g(x)=0$ 时，状态对扰动的敏感度不再是平凡的单位矩阵。写出 saltation 矩阵 $\Xi=\frac{\partial x^+}{\partial x^-}$ 的公式，并解释它为什么不等于 reset map 的 Jacobian $\partial R/\partial x$。（→ §50）
Riccati 反馈：离散 LQR 的 backward pass 如何从终端代价递推出每一步的反馈增益 $K_k$？为什么"值函数是状态的二次型"这一结构使递推闭合？（→ 控制理论 §50）

若你能流畅回答第 1、2、5 题，可直接进入 §6.1；若第 3、4 题卡壳，强烈建议先把 §30、§40 重读一遍——本章的"实时化"全部建立在"接触求解器又快又可微"这个前提上。

本章目标¶

学完本章后，你应当能够：

解释动机：用一句话说清"接触隐式（contact-implicit）"到底隐式了什么，并论证为什么"不预先指定接触序列"是一个值得付出巨大数值代价去换取的能力。
写出 MPCC 形式：在白板上写出 Posa-Cantu-Tedrake 2014 的接触隐式轨迹优化（CI-TO）非线性规划（NLP），逐项解释动力学约束、法向互补约束、摩擦约束的来历。
诊断病态根源：严格证明互补约束在双活点（biactive point）处使 MFCQ（Mangasarian-Fromovitz 约束品性）必然失效，并解释这为什么导致直接喂给 IPOPT/SNOPT 时乘子爆炸、步长趋零。
掌握松弛技术：推导 Scholtes 松弛、Lin-Fukushima 光滑化、$\ell_1$ 罚函数（elastic mode）三条救援路径，理解它们各自收敛到哪一级稳定性（C-/M-/B-stationarity）。
对比两条范式：在"混合整数（mixed-integer）显式枚举接触模式"与"互补松弛隐式涌现接触模式"之间做出有依据的工程选型，量化二者的计算复杂度差异。
理解实时化三支柱：解释 Fast-CI-MPC（Le Cleac'h 2024）的 bilevel 结构、IDTO（Kurtz 2023/2026）的逆动力学变量消元、C3（Aydinoglu-Posa 2022/2024）的 ADMM 共识分解，分别用什么手段把 CI-MPC 压进 10 ms。
架起桥梁：把接触隐式 MPC 与 04 批的 chance-complementarity 不确定性建模、05 批的足式落脚规划连接起来，说明它们共享同一套互补约束语言。

本章知识导航¶

本章围绕一条主线展开："不指定接触序列"这一愿望 → 互补约束这一数学载体 → 病态这一代价 → 松弛/整数/分解这三类对策 → 实时 MPC 这一工程终点。

涵盖的核心知识点（共 9 个）：

编号	知识点	一句话定位	难度
§6.1	接触隐式的动机	为什么"免预设接触序列"值得追求	⭐⭐
§6.2	CI-TO 的 MPCC 形式化	Posa 2014 的核心 NLP 长什么样	⭐⭐
§6.3	互补约束的病态	MFCQ 为什么必然失效	⭐⭐⭐
§6.4	松弛与光滑化技术	Scholtes / Lin-Fukushima / 罚函数	⭐⭐⭐
§6.5	混合整数 vs 互补松弛	显式枚举与隐式涌现的对决	⭐⭐⭐
§6.6	固定序列 TO 作为反面	DIRCON / HZD / 质心 MPC	⭐⭐
§6.7	接触隐式 MPC 的实时化	bilevel / IDTO / C3 三条路	⭐⭐⭐⭐
§6.8	数值困难的系统诊断	病态、奇异、局部极小的工程对策	⭐⭐⭐
§6.9	与不确定性和足式规划的桥接	chance-complementarity 与落脚规划	⭐⭐⭐

知识点之间的关系（推荐阅读路径）：

                       §6.1 动机（免预设接触序列）
                              │
                              ▼
                  §6.2 MPCC 形式化（数学载体）
                       │              │
            ┌──────────┘              └──────────┐
            ▼                                     ▼
   §6.3 病态根源（MFCQ 失效）          §6.6 固定序列 TO（反面对照）
            │                                     │
            ▼                                     │
   §6.4 松弛技术（救援）                          │
            │                                     │
            ▼                                     ▼
   §6.5 混合整数 vs 互补 ◄──────────────── 两条范式对决
            │
            ▼
   §6.7 实时 CI-MPC（工程终点：bilevel / IDTO / C3）
            │
            ▼
   §6.8 数值诊断 ──► §6.9 桥接（不确定性 + 足式落脚）

主干：§6.1 → §6.2 → §6.3 → §6.4 → §6.7。这条线是"理解 CI-MPC 为什么难、怎么变得可行"的最短路径。
分支：§6.5（整数对比）和 §6.6（固定序列对照）是横向拓展，帮助你把 CI 放进更大的"接触决策"图景；§6.8（诊断）和 §6.9（桥接）是收尾与延展。

注意：本导航只展示**结构**，每个知识点的具体推导在对应小节展开。

前置知识桥接¶

本章是接触力学专题的**收束章**，它把前面五章的工具一次性全部用上。在深入之前，用 2-3 行重新激活每个前置工具的核心要点：

回顾 §10（互补与 Signorini）：互补条件 $0\le\phi\;\perp\;\lambda_n\ge 0$ 把"要么没接触（$\phi>0,\lambda_n=0$），要么在接触（$\phi=0,\lambda_n>0$）"这一**非此即彼的逻辑**压缩成一组代数不等式。本章把这组不等式从"仿真时正向求解"升级为"优化时作为约束嵌入"——这正是"接触隐式"四个字的全部含义。
回顾 §20（摩擦锥凸松弛）：库仑摩擦的二阶锥 $\|\lambda_t\|\le\mu\lambda_n$ 是非光滑但凸的。在 NLP 里我们要么保留二阶锥（SOCP），要么用多面体近似换取线性约束。本章 §6.2 的摩擦互补约束直接继承这套建模。
回顾 §30（时步法）：Stewart-Trinkle / Anitescu-Potra 把一个时间步内的接触动力学写成 LCP，保证刚性约束下能量不增长、不穿透。本章 §6.2 的"离散动力学约束 $h_k=0$"就是把这种时步法离散化嵌进优化的等式约束。
回顾 §40（可微接触）：通过隐函数定理（Implicit Function Theorem, IFT），接触力 $\lambda(q,\dot q)$ 在 NCP 正则时对参数 $C^1$ 可微。本章 §6.7 的 IDTO 与 C3、Dojo 的实时性，全部建立在"接触求解器能吐出梯度"这一前提上。
回顾 §50（混合系统与 Saltation）：状态穿越 guard 面时，敏感度要乘以 saltation 矩阵 $\Xi$。本章 §6.7 的 Hybrid iLQR-MPC（Kong-Johnson 2023）把 $\Xi$ 显式插进 Riccati backward pass，从而**不光滑化也能正确传播梯度**。
回顾 §70（非光滑分析）：互补约束、$\max(0,\cdot)$、绝对值 $\|v_t\|$ 都是非光滑对象。Clarke 次梯度、半光滑（semismooth）函数的概念支撑了本章对"为什么标准光滑优化理论在这里失灵"的全部讨论。
回顾控制理论 §50（LQR/Riccati）：离散 LQR 的 backward pass 通过"值函数是二次型"递推出反馈增益 $K_k$。本章 §6.7 的所有 DDP/iLQR-based CI-MPC 都把 LQR 当作内层子程序反复调用——CI-MPC 在某种意义上就是"带接触的、在线滚动的、被反复线性化的 LQR"。

本章在这套工具链中解决的新问题：前五章分别教会我们"如何建模一次接触""如何仿真一段接触轨迹""如何对接触求导"。本章要回答的是一个更难的系统级问题——如何让一个优化器在不预先知道接触何时发生的情况下，自动规划出一整段含接触的最优轨迹，并且快到能在机器人控制周期内反复重解。

如果跳过本章会怎样¶

不学本章，你会在以下具体场景中卡住：

场景一：让四足机器人去推一扇门。你不知道哪只脚、在什么时刻、以多大力接触门把手——这是一个交互序列未知的任务。如果没有接触隐式的概念，你只能手工枚举"先抬右前腿→接触→施力→收回"这样的固定脚本，一旦门的位置变了脚本就失效。CI-TO 让你把"接触何时发生"交给优化器自己决定。
场景二：读不懂 2022 年后的足式 SOTA 论文。Le Cleac'h 2024、Kim 2025、Kurtz 2026 这批论文的摘要里全是 "contact-implicit MPC running at 50–100 Hz on hardware"。不理解 bilevel、warm-start shift、saltation Riccati，你连摘要都看不懂，更别说复现。
场景三：把 MPCC 直接喂给 IPOPT 然后困惑为什么不收敛。这是新手最常踩的坑：写出了正确的互补约束，却不知道 MFCQ 必然失效，于是 SQP 无限迭代、乘子爆炸。本章 §6.3-§6.4 专门拆解这个陷阱并给出标准解法。

预计阅读时间¶

阅读方式	时间	适合谁
精读（含推导与练习）	18–24 h	要做接触轨迹优化研究、需在白板推导 MPCC 的读者
速读（跳过松弛收敛性证明细节）	8–10 h	有凸优化基础、只想理解 CI-MPC 工程范式的读者
速查（只看表格、定理速查卡、故障排查）	40 分钟	已掌握主线、遇到具体问题回来查的读者

里程碑自检：完成本章后，你应能在白板上 (1) 写出 Posa 2014 的完整 MPCC，(2) 证明其 MFCQ 失效，(3) 用 30 分钟向同行讲清楚 Fast-CI-MPC 的 bilevel 结构与 IDTO 的变量消元差异。

§6.0 总论：接触隐式为什么是一个独立的工程难题¶

在进入具体推导之前，先建立全局图景——这是 R5"先动机后理论"在章节尺度上的体现。

一句话定位¶

接触隐式轨迹优化（Contact-Implicit Trajectory Optimization, CI-TO）的核心含义是：不预先指定接触日程表（contact schedule）。 接触的建立时刻（make）、断开时刻（break）、接触面、法向力大小、摩擦力方向——这一切都不是人手工指定的脚本，而是作为优化变量内生地从一个统一的优化问题里"涌现"出来。

这个定义看似平淡，却划出了一条深刻的分水岭。让我们用对比来感受它的分量：

视角	传统（固定序列）方法	接触隐式方法
谁决定接触时序	工程师手工指定（"第 0.3 s 左脚落地"）	优化器自动发现
数学对象	在固定模式下的光滑 NLP / 凸 QP	带互补约束的 MPCC
能否发现新步态	不能（只能在指定模式内优化）	能（蹦跳、攀爬、用墙借力）
数值难度	低（约束品性正常）	高（约束品性退化）

本质洞察：接触隐式不是"一种更高级的求解算法"，而是"一种把组合决策（接触模式的离散选择）和连续决策（轨迹与力的连续优化）熔合进单一优化问题的建模哲学"。它的全部困难和全部威力，都源于这一次"熔合"——把本属于离散世界的"接触/不接触"逻辑，用互补约束塞进连续优化的框架里。

三十年三条支流的首次汇流¶

接触隐式 MPC 的成熟（2022–2025）是机器人控制三条独立发展了三十年的支流首次汇合的结果。理解这段历史，才能理解为什么 2022–2025 是"黄金窗口"：

支流一：混合零动力学（Hybrid Zero Dynamics, HZD）。从 Westervelt-Grizzle-Koditschek（2003 起）到 Ames（IEEE T-AC 2014），这条线把双足步态看成混合系统的周期轨道，用 Poincaré 映射分析稳定性。它的特点是**事先设计好接触序列**（哪只脚什么时候落地是 hybrid 系统的 mode 切换，预先固定）。

支流二：质心动力学 MPC（Centroidal / SRBD-MPC）。从 Orin-Goswami 的质心动量，到 Di Carlo-Wensing-Katz-Bledt-Kim（MIT Cheetah 3, IROS 2018），这条线把全身简化成一个 6 自由度的单刚体（Single Rigid Body Dynamics, SRBD），接触力作为输入，求解一个凸 QP，能跑到 1 kHz。它的特点是**用简化模型换实时性**，但接触序列仍由步态调度器（gait scheduler）外部给定。

支流三：穿越接触的 DDP（DDP-through-Contact）。从 Tassa-Erez-Todorov（IROS 2012）的软接触光滑化，到 Mastalli 等（Crocoddyl, ICRA 2020）的多接触 DDP，这条线把微分动态规划（Differential Dynamic Programming, DDP）/ iLQR 推广到含接触的全身动力学。它的特点是**全身、二阶收敛**，但早期要么光滑化接触（物理不真），要么固定接触序列（holonomic 约束）。

这三条支流各自解决了接触决策的一个侧面，却都回避了那个最硬的核心问题——让接触序列本身成为优化的输出。直到 2022–2025，三个条件同时成熟，汇流才发生：

可微仿真成熟：Dojo（Howell 等 2022）、MuJoCo XLA、Drake Hydroelastic 把接触梯度从"hack"变成一等公民——这呼应了我们在 §40 学的隐函数定理可微接触。
硬件窗口打开：Unitree Go1/Go2、ANYmal D、KAIST HOUND、Agility Digit 等提供 1 kHz 关节力矩接口，让 50–500 Hz 的全身 CI-MPC 能真正闭环。
算法合流：IDTO、C3、Fast-CI-MPC 几乎同时在 2023–2024 把"在硬件上以 ≥100 Hz 跑接触隐式 MPC"从博士论文里的示意图，变成 GitHub 上能 clone 的事实。

一条人物与论文的时间线¶

为了让你在阅读后续小节时心里有一张地图，这里先给出本章反复引用的关键工作的时间线（每个工作的细节在对应小节展开，这里只建立坐标）：

1996  Stewart-Trinkle      LCP 时步法（仿真稳定性的数学源头）
1997  Anitescu-Potra       刚体接触 LCP 可解性（互补建模奠基）
2000  Scheel-Scholtes      MPCC 的 MFCQ 失效定理（病态的理论诊断）
2001  Scholtes             Scholtes 松弛收敛定理（最稳的工程救援）
2012  Tassa-Erez-Todorov   软接触光滑化 DDP（穿越接触 DDP 开端）
2014  Posa-Cantu-Tedrake   CI-TO 开山之作（首次把 LCP 嵌进 NLP）★
2014  Tassa-Mansard-Todorov Control-limited DDP（whole-body MPC 基础）
2016  Posa-Kuindersma-Tedrake DIRCON（固定模式高阶 collocation）
2019  Manchester-Doshi-Wood-Kuindersma 变分积分 CI-TO（高阶精度）
2019  Howell-Jackson-Manchester ALTRO（AL-iLQR）
2022  Howell et al.        Dojo（可微接触仿真器）
2022  Aydinoglu-Posa       C3 原始版（首次 kHz 级实时多接触 MPC）★
2023  Kong-Li-Council-Johnson HiLQR-MPC（saltation 进 Riccati）
2023  Kurtz-Castro-Önol-Lin IDTO（逆动力学范式）
2024  Aydinoglu-Wei-Huang-Posa C3 期刊版（30+ Hz 机械臂操纵）
2024  Le Cleac'h-Howell et al. Fast-CI-MPC（当前 SOTA，Go1 硬件）★★
2024  Wensing-Posa et al.  优化控制综述（四路线并列，必读）
2025  Kim-Kang-Kim-Hong-Park HOUND 四足 CI-MPC（高速）
2026  Kurtz et al.         IDTO IJRR 正式版

带 ★ 的三篇（Posa 2014、C3 2022、Fast-CI-MPC 2024）是本章的三个里程碑，分别回答了"接触隐式是否可能""能否实时""能否在真机上又快又稳"。

阶段小结：到这里我们已经建立了全局图景——接触隐式的定义、它划出的分水岭、三条支流的汇流、关键论文的时间坐标。接下来 §6.1 将深入"动机"这一层，回答一个更尖锐的问题：既然接触隐式数值上这么难，为什么我们非要它不可？

§6.1 接触隐式的动机：组合爆炸与"免预设接触序列" ⭐⭐¶

动机：一个看似简单的问题¶

考虑一个具体到几乎琐碎的问题：让一个平面单腿跳跃机器人（planar hopper）从地面 A 点跳到 B 点。它只有一条腿，腿端要么接触地面、要么离地——只有两种"接触模式"。

现在我们想用最优控制求出这段跳跃的力矩轨迹。最自然的想法是套用我们已经掌握的工具：把它写成一个轨迹优化问题，最小化能量、满足动力学约束。但马上撞上第一堵墙——动力学是什么？

当腿端接触地面（站立相，stance）：地面给一个法向支撑力，动力学是一组方程；
当腿端离地（飞行相，flight）：没有接触力，动力学是另一组方程。

这两组动力学方程**不同**。所以"系统的动力学 $\dot x=f(x,u)$"在跳跃过程中会切换。要写出完整的优化问题，我们似乎必须先知道：腿什么时候离地、什么时候落地？

反面：如果预先指定接触序列会怎样¶

最直接的做法是手工指定接触序列。比如规定："$[0,0.2\,\text{s}]$ 站立蹬地，$[0.2,0.5\,\text{s}]$ 飞行，$[0.5,0.7\,\text{s}]$ 落地缓冲。"一旦序列固定，每一段的动力学就确定了，问题退化成一个光滑的轨迹优化——这正是 §6.6 要讲的固定序列方法，求解又快又稳。

但这样做付出了三重代价：

代价一：序列是猜的，不是优化出来的。 你凭什么知道最优跳跃恰好是"蹬 0.2 s、飞 0.3 s"？也许最优解需要蹬 0.18 s、或者中途有一次微小的二次触地。固定序列把这些可能性全部排除在外——你优化的不是"最优跳跃"，而是"在你猜的这个序列下的最优跳跃"。

代价二：组合爆炸。 单腿两种模式还能枚举。但考虑四足机器人，每条腿独立地接触/离地，任一时刻就有 $2^4=16$ 种接触模式。一段 $N=50$ 步的轨迹，模式序列的组合数是 $16^{50}\approx 10^{60}$。手工枚举或穷举搜索在这种规模下彻底无望。这就是**接触模式的组合爆炸**——它是整个领域的核心痛点。

代价三：无法发现新行为。 这是最深刻的代价。如果机器人面对的是一个**未知交互**的任务——推一扇没见过的门、扶一面墙借力、跨越一个意外的台阶——它根本不知道"正确的接触序列"长什么样。固定序列方法在这里完全失效，因为人类工程师也写不出正确的脚本。

本质洞察：固定序列方法的根本局限不在于"求解慢"，而在于**它把一个本应由优化器回答的问题（接触何时发生）偷偷地交给了人**。接触隐式方法的全部价值，就是把这个问题的回答权从人手里夺回来，交还给优化器。当任务的接触序列"显然"时（如平地小跑），这个夺回是浪费；当任务的接触序列"未知"时（如推门、攀爬），这个夺回是唯一出路。

对比：两种"不这样做的后果"¶

让我们把"不用接触隐式"的两种退路都摆出来，看看它们各自的死路：

退路 A：穷举所有接触模式序列，对每个序列解一个光滑 TO，取最优。 - 后果：组合爆炸。$16^{50}$ 个子问题，每个都要解一个 NLP。即使每个 NLP 只要 1 ms，总时间也是 $10^{57}$ 秒——宇宙年龄的 $10^{40}$ 倍。 - 这条路在理论上"正确"，在工程上"自杀"。

退路 B：固定一个人工猜测的接触模式序列，只解一个光滑 TO。 - 后果：见上述代价一和代价三。结果次优、且对未知任务无能为力。 - 这条路在工程上"高效"，在能力上"残废"。

接触隐式方法是第三条路：不枚举、不固定，而是把"接触/不接触"这个离散逻辑用互补约束写成连续约束，让单个优化问题同时决定连续轨迹和离散接触模式。 它避开了退路 A 的组合爆炸（不显式枚举），也避开了退路 B 的能力残废（不固定序列）。代价是 §6.3 要讲的数值病态——但这是一个可以用 §6.4 的技术管理的代价，而组合爆炸和能力残废是无法管理的死路。

历史：Posa 的关键一跃（2014）¶

接触隐式的思想可以追溯到更早——LCP 时步法（Stewart-Trinkle 1996，Anitescu-Potra 1997）早就用互补约束**仿真**接触。但"仿真"是正向问题：给定当前状态和输入，求下一步状态。把同一套互补约束用于**优化**（反向问题：给定目标，求整段轨迹和输入）是一次概念跃迁。

Michael Posa 在 MIT 的博士工作（Posa-Cantu-Tedrake, IJRR 2014，DOI 10.1177/0278364913506757）完成了这一跃迁。论文标题《A Direct Method for Trajectory Optimization of Rigid Bodies Through Contact》里，"Direct"（直接法）和"Through Contact"（穿越接触）两个词点出了全部新意：用直接转录（direct transcription）把整段轨迹离散成优化变量，把接触的互补条件作为约束**直接嵌入** NLP，让 SQP（序列二次规划）在优化轨迹的同时"自然地"解出接触力——不需要先验的模式排序（"eliminates the requirement for a priori mode ordering"，这是论文摘要的原话）。

Posa 的洞察可以用一句话概括：接触力学的互补结构，既能用于仿真，也能用于优化；用于优化时，互补约束就成了"让优化器自己决定接触模式"的开关。

理论：接触隐式到底"隐式"了什么¶

现在我们可以精确回答标题里的问题——"接触隐式"中"隐式"二字的确切含义。

考虑接触的两个核心物理量：间隙距离 $\phi(q)$（gap function，腿端到地面的有符号距离，接触时为 0，分离时为正）和法向接触力 $\lambda_n$（接触时为正，分离时为 0）。它们满足互补条件：

\[0\le \phi(q)\;\perp\;\lambda_n\ge 0.\]

这个 $\perp$ 符号是整个领域的灵魂，它简记三个条件：

\[\phi(q)\ge 0,\qquad \lambda_n\ge 0,\qquad \phi(q)\cdot\lambda_n=0.\]

$\phi(q)\ge 0$：不能穿透地面（间隙非负）；
$\lambda_n\ge 0$：接触力只能推不能拉（单边约束，地面不会"吸住"脚）；
$\phi(q)\cdot\lambda_n=0$：互补松弛——要么间隙为零（在接触，此时力可正），要么力为零（已分离，此时间隙可正），二者不能同时严格为正。

这第三个条件就是"隐式"的来源。在**显式**方法里，我们先用一个布尔变量 $s_k\in\{0,1\}$ 显式声明"第 $k$ 步是否接触"，然后根据 $s_k$ 选择动力学。在**隐式**方法里，我们不声明 $s_k$；接触模式被**隐式地编码**在 $\phi\cdot\lambda_n=0$ 这个等式里——优化器在求解时，会让某些时刻 $\phi=0$（接触）、某些时刻 $\lambda_n=0$（分离），从而"涌现"出一个接触序列，而这个序列从未被显式写出来。

本质洞察：互补条件 $\phi\cdot\lambda_n=0$ 是一个"逻辑或"的连续化身。它说"$\phi=0$ 或 $\lambda_n=0$"，但用乘积等于零的代数形式表达，从而能塞进连续优化。整个接触隐式方法的精妙与痛苦都在这里：精妙在于把离散逻辑连续化了，痛苦在于这个"乘积为零"的约束在原点处有一个尖角（kink），使得标准的光滑优化理论（依赖约束品性）在这里崩溃——这正是 §6.3 的主题。

多视角理解：互补约束的三个面孔¶

为了真正吃透互补条件（这是本章一切的基石），我们从三个互补的视角理解同一个对象 $0\le\phi\;\perp\;\lambda_n\ge 0$：

视角一（物理视角）：它是单边接触的力学定律。地面只能推不能拉，物体只能在地面之上或之上。这是 Signorini 在 1933 年为弹性体接触提出的边界条件（见 §10），是接触力学的公理。

视角二（几何视角）：在 $(\phi,\lambda_n)$ 平面上，满足互补条件的点构成一个"L 形"集合——正 $\phi$ 轴（分离，$\lambda_n=0$）和正 $\lambda_n$ 轴（接触，$\phi=0$）的并集。这个 L 形集合**非凸**（两条射线的并不是凸集），且在拐角原点 $(0,0)$ 处有一个尖角。非凸性是组合困难的几何根源，尖角是数值病态的几何根源。

视角三（优化视角）：互补条件等价于 $\min(\phi,\lambda_n)=0$，也等价于把 $\phi,\lambda_n$ 约束到非负卦限的边界上的一种"垂直"关系。它是变分不等式（variational inequality）和互补问题（complementarity problem）的标准砌块。

类比（标注边界）：互补约束像电路里的"理想二极管"——二极管要么导通（电压降为零，电流可正，对应 $\phi=0,\lambda_n>0$），要么截止（电流为零，电压降可负，对应 $\lambda_n=0,\phi>0$），二者互补。像的部分：都是"两个量不能同时非零"的开关逻辑。不像的部分：二极管的 I-V 曲线在工程上常用光滑函数近似（如 Shockley 方程），而接触的互补约束在刚体假设下是严格非光滑的尖角；且接触还涉及摩擦这一耦合的切向维度，远比一维二极管复杂。不要把这个类比延伸到"接触可以像二极管一样用一个解析函数完全描述"。

⚠️ 常见陷阱¶

💡 概念误区：认为"接触隐式"是指"接触被隐藏起来、不建模"

新手想法："既然叫隐式，那是不是不用管接触力，求解器内部自己处理就行？" 实际上：恰恰相反。接触隐式方法**显式地把接触力 $\lambda$ 作为优化变量**，并**显式地写出互补约束**。"隐式"指的是**接触模式序列**（哪个时刻接触）是隐式涌现的，而不是接触力本身被隐藏。接触力和互补约束都明明白白写在 NLP 里。 为什么重要：理解这一点才能区分"接触隐式（contact-implicit，模式隐式但力显式）"和"软接触光滑化（soft contact，把接触力写成穿透深度的光滑函数，回避互补约束）"——后者是 §6.7 IDTO/Tassa 用的另一条路，二者不是一回事。

💡 概念误区：把组合爆炸的数字 $16^{50}$ 当成"接触隐式要枚举这么多"

新手想法："接触隐式既然要考虑所有接触模式，那它是不是内部偷偷枚举了 $16^{50}$ 种？" 实际上：接触隐式的全部精妙正在于**它不枚举**。$16^{50}$ 是固定序列方法"穷举退路"的代价。接触隐式用连续的互补约束代替离散枚举，单个连续优化问题的变量数只随时域 $N$ 线性增长（每步加一组 $\lambda$ 变量和互补约束），完全没有 $16^N$ 这个指数因子。它把指数组合问题降维成了一个（病态的）连续优化问题。 为什么重要：这是接触隐式相对混合整数方法（§6.5）的根本优势——混合整数显式引入 $0/1$ 变量，规模随 $N$ 指数增长（最坏情况）；互补松弛把组合性藏进连续约束，规模随 $N$ 线性增长。代价是把"组合难"换成了"病态难"。

🧠 思维陷阱：认为"既然接触隐式更强大，就应该永远用它，固定序列方法过时了"

新手想法："Posa 2014 之后，固定序列方法（Crocoddyl、质心 MPC）是不是都被淘汰了？" 实际上：完全没有。固定序列方法在**任务步态已知**时（平地小跑 trot、行走 walk）是工程首选——它能跑到 1 kHz，数值稳定，约束品性正常。接触隐式只在**交互未知**时才显出价值（推门、攀爬、跨障）。MIT、ETH 的真机系统至今大量使用固定序列质心 MPC。 正确思维：选型应问一个问题——"这个任务的接触序列是否事先可知？"可知 → 固定序列（快、稳）；不可知 → 接触隐式（灵活、但慢且病态）。二者是互补工具，不是新旧替代。这一思维框架将在 §6.5、§6.6 反复强化。

练习¶

练习 6.1.1（概念辨析，草稿纸上完成） 用自己的话写出"接触隐式"中"隐式"二字的精确含义，并举一个具体任务说明：在该任务中，固定序列方法为什么会失效，而接触隐式为什么能成功。要求你的任务不能是本节用过的"推门"或"跳跃"，自己设计一个（提示：考虑"机器人手在桌面上滑动一个箱子，箱子可能翻倒也可能滑动"这类接触模式取决于力的任务）。

练习 6.1.2（组合复杂度估算，草稿纸上完成） 一个双足人形机器人，每只脚用 4 个接触点建模（脚的四个角），每个接触点独立地接触/分离。(a) 任一时刻有多少种接触模式？(b) 一段 $N=40$ 步的轨迹，固定序列方法的"穷举退路"需要解多少个子 NLP？(c) 接触隐式方法的优化变量数随 $N$ 如何增长（给出量级即可，假设每步状态维度 $n$、接触点数 $n_c$）？用这三个数字定量论证为什么接触隐式在人形机器人上是必需的。

练习 6.1.3（开放思考题） 互补条件 $\phi\cdot\lambda_n=0$ 把"逻辑或"连续化了。请思考：是否存在其他方式把"$\phi=0$ 或 $\lambda_n=0$"这个逻辑或连续化？（提示：考虑 $\min(\phi,\lambda_n)=0$、或 Fischer-Burmeister 函数 $\phi+\lambda_n-\sqrt{\phi^2+\lambda_n^2}=0$。）这些不同的连续化在原点处的光滑性如何？哪一种可能数值上更友好？这个问题直接通向 §6.4 的光滑化技术。

§6.2 CI-TO 的形式化：互补约束优化（MPCC） ⭐⭐¶

动机：把"涌现的接触"写成一个可解的优化问题¶

§6.1 建立了直觉——接触隐式用互补约束让接触模式涌现。但直觉不能喂给求解器。本节要把这个直觉**形式化**成一个数学上精确、计算机能接收的优化问题。我们将一步步搭建 Posa-Cantu-Tedrake 2014 的核心 NLP，并指出它属于一类有专门名字、专门理论的优化问题——带互补约束的数学规划（Mathematical Program with Complementarity Constraints, MPCC），也叫**带均衡约束的数学规划（Mathematical Program with Equilibrium Constraints, MPEC）**。

反面：如果用普通 NLP 的写法会缺什么¶

假设我们不知道互补约束，试图用普通 NLP 写接触轨迹优化。我们会写出：

\[\min_{x,u}\ J(x,u)\quad\text{s.t.}\quad \dot x=f(x,u),\ x(0)=x_0.\]

问题立刻暴露：$f(x,u)$ 是什么？接触时和分离时不同。我们没有地方表达"接触力的存在与否取决于间隙"。如果硬要给一个固定的 $f$，就回到了固定序列的老路。普通 NLP 缺的，正是表达"接触力与间隙互补"的那一组约束。 补上这组约束，普通 NLP 就升级成 MPCC。

理论：逐项搭建 CI-TO 的 MPCC¶

我们采用**直接转录（direct transcription）**：把连续时间轨迹离散成 $N$ 个时间步，每步的状态 $x_k$、输入 $u_k$、接触力 $\lambda_k$ 都成为优化变量。下面逐项构建（这是 R8 理论教学"LaTeX 推导模式"——先列假设，再逐步搭建，每项解释来历）。

第一项：目标函数。 累积每步的运行代价 $\ell$ 加上终端代价 $\ell_f$：

\[J(x,u)=\sum_{k=0}^{N-1}\ell(x_k,u_k)+\ell_f(x_N).\]

运行代价 $\ell$ 典型地包含控制能量 $\tfrac12 u_k^\top R u_k$、状态偏差 $\tfrac12(x_k-x_k^{\mathrm{ref}})^\top Q(x_k-x_k^{\mathrm{ref}})$；终端代价 $\ell_f$ 鼓励到达目标位形。这部分和普通 LQR 代价没有区别——接触隐式的全部特殊性在约束里，不在目标里。

第二项：离散动力学约束。 这是把 §30 的时步法离散化嵌进优化。我们要求相邻状态满足离散动力学：

\[h_k(x_k,x_{k+1},u_k,\lambda_k)=0,\qquad k=0,\dots,N-1.\]

这里 $h_k$ 是把连续动力学 $M(q)\dot v + C(q,v)v + g(q)=B u + J_c(q)^\top\lambda$（带接触力 $\lambda$ 经接触 Jacobian $J_c$ 映射到广义力）用某种积分格式（隐式 Euler、变分积分器、Stewart-Trinkle 时步法）离散后的残差。

回顾桥接 §30：我们在 §30 学过，Stewart-Trinkle 时步法把"速度更新"和"接触冲量"写在同一个时间步里隐式求解，从而保证刚性接触不穿透、能量不增长（§50 定理 T7）。这里的 $h_k=0$ 就是把那个时步更新方程作为优化的等式约束写出来——区别仅在于：仿真时我们正向解这个方程，优化时我们把它当约束，让优化器在满足它的同时优化整段轨迹。

第三项：法向互补约束（接触隐式的核心）。 对每个接触点、每个时间步：

\[0\le \phi(x_k)\;\perp\;\lambda_k^n\ge 0.\]

这就是 §6.1 反复讨论的互补条件，现在在每个时间步 $k$ 上各写一份。$\phi(x_k)$ 是第 $k$ 步的间隙距离（位形 $q_k$ 的函数），$\lambda_k^n$ 是第 $k$ 步的法向接触力。正是这一组约束让接触模式涌现——优化器在不同时间步上自动选择 $\phi=0$（接触）或 $\lambda_n=0$（分离），从而产生一个未被显式指定的接触序列。

第四项：库仑摩擦约束。 切向接触力 $\lambda_k^t$ 受摩擦锥约束，且其方向由切向滑动速度 $v_t$ 决定：

\[\|\lambda_k^t\|\le \mu\,\lambda_k^n,\qquad \lambda_k^t\in -\partial\|v_t\|\cdot(\text{某个非负标量}).\]

第一个不等式是摩擦锥（§20）：切向力不超过法向力的 $\mu$ 倍。第二个条件是**最大耗散原理**的体现——滑动时摩擦力反向于滑动速度（$\partial\|v_t\|$ 是切向速度范数的次微分，在 $v_t\neq 0$ 时就是单位方向 $v_t/\|v_t\|$，在 $v_t=0$ 时是整个单位球，对应静摩擦的方向不定）。

回顾桥接 §20：摩擦锥 $\|\lambda_t\|\le\mu\lambda_n$ 是二阶锥（second-order cone, SOC），凸但非光滑。我们在 §20 学过两条处理路线：(i) 保留 SOC，用 SOCP 求解器或内点法处理；(ii) 用多面体近似（polyhedral approximation）把锥换成若干线性不等式，换取纯线性约束。Posa 2014 及多数 LCP-based 方法采用 (ii)，把摩擦锥近似成一个 $d$ 边棱锥，从而整个接触问题落进线性互补（LCP）框架。代价是引入了 $O(1/d^2)$ 量级的各向异性误差（§20 已分析）。

第五项：边界与路径约束。 初值固定、终端可行域、状态/输入边界：

\[x_0=x_{\mathrm{init}},\qquad g(x_N)\le 0,\qquad x_k\in\mathcal X,\ u_k\in\mathcal U.\]

把五项拼起来，就得到 Posa-Cantu-Tedrake 2014 的接触隐式轨迹优化 NLP：

\[ \boxed{ \begin{aligned} \min_{x,u,\lambda}\quad & J(x,u)=\sum_{k=0}^{N-1}\ell(x_k,u_k)+\ell_f(x_N) \\ \text{s.t.}\quad & h_k(x_k,x_{k+1},u_k,\lambda_k)=0 && \text{(离散动力学)} \\ & 0\le \phi(x_k)\;\perp\;\lambda_k^n\ge 0 && \text{(法向互补)} \\ & \|\lambda_k^t\|\le\mu\lambda_k^n,\quad \lambda_k^t\in\partial\|v_t\| && \text{(库仑摩擦)} \\ & x_0=x_{\mathrm{init}},\quad g(x_N)\le 0 && \text{(边界)} \end{aligned} } \]

阶段小结：到这里我们完成了 CI-TO 的形式化——五项约束拼成一个 NLP，其中第三项（法向互补）是接触隐式的灵魂。接下来要回答两个问题：(1) 这个 NLP 属于哪一类优化问题，有什么现成理论？(2) 它为什么不能直接喂给标准求解器？第一个问题是本节剩余部分，第二个问题是 §6.3。

这个 NLP 的名字：MPCC¶

带有 $0\le a\;\perp\;b\ge 0$ 这种互补约束的优化问题，在优化理论里有专门的名字和专门的理论分支：带互补约束的数学规划（MPCC）。它是更广的**带均衡约束的数学规划（MPEC）**的一个子类（MPEC 的"均衡约束"可以是任意变分不等式，MPCC 特指互补形式）。

把互补约束 $0\le\phi\;\perp\;\lambda\ge 0$ 用标准 NLP 语言展开，它等价于三组约束：

\[\phi\ge 0,\qquad \lambda\ge 0,\qquad \phi^\top\lambda\le 0.\]

（注意：因为 $\phi\ge 0,\lambda\ge 0$，所以 $\phi^\top\lambda\ge 0$ 自动成立，故 $\phi^\top\lambda\le 0$ 与 $\phi^\top\lambda=0$ 等价。工程上常写成 $\le 0$ 的不等式形式以便松弛。）

本质洞察：CI-TO 是 MPCC 的一个**具体物理实例**——接触力学情境下的 MPCC。这意味着我们可以**继承 MPCC 三十年的理论积累**（约束品性、稳定性刻画、松弛方法的收敛性）。Posa 2014 的深远影响，一半在于提出了 CI-TO 这个建模，另一半在于把机器人接触问题接入了 MPCC 这条成熟的理论管线。反过来，这也警示我们：MPCC 的全部理论困难（CQ 失效、稳定性弱、求解病态）会原封不动地落到 CI-TO 头上——这正是 §6.3 的内容。

对比性思维：CI-TO 不是 MPCC，而是 MPCC 的实例¶

这里要纠正一个极常见的术语混淆（这是 R6B"不是 X 而是 Y"）：

不是 "CI-TO = MPCC"（把两者当同义词）；而是 "CI-TO ⊂ MPCC"（CI-TO 是 MPCC 在接触力学情境下的一个具体实例）。

MPCC 是一大类问题，涵盖经济均衡（Stackelberg 博弈）、交通网络均衡、双层优化、超参数调优等——互补约束在这些领域同样出现。CI-TO 只是其中一个分支。在论文写作中混用这两个词是新手常犯的错误（这条陷阱在本章末的误解汇总里会再次出现）。理解这个包含关系的实际好处是：当你在 CI-TO 里遇到数值困难时，你可以去 MPCC 的文献（Luo-Pang-Ralph 1996 的《Mathematical Programs with Equilibrium Constraints》、Scholtes 2001）里找通用解法，而不必从头发明。

多视角理解：MPCC 的两种等价写法¶

同一个互补约束 $0\le\phi\;\perp\;\lambda\ge 0$ 有两种常见的 NLP 编码，理解它们的等价与差异很重要：

写法一（乘积形式）：$\phi\ge 0,\ \lambda\ge 0,\ \phi^\top\lambda\le 0$。 - 优点：直接、变量少。 - 缺点：$\phi^\top\lambda\le 0$ 这个约束的 Jacobian 在双活点退化（这是 §6.3 病态的来源）。

写法二（逐分量形式）：对每个分量 $i$，$\phi_i\ge 0,\ \lambda_i\ge 0,\ \phi_i\lambda_i\le 0$（或 $\phi_i\lambda_i=0$）。 - 优点：把耦合解开到逐分量，便于逐点松弛（§6.4 的 Scholtes 松弛逐分量做）。 - 缺点：约束数量随接触点数和时间步数相乘膨胀。

两种写法在数学上等价（写法一的 $\phi^\top\lambda=\sum_i\phi_i\lambda_i\le 0$ 配合各分量非负，强制每个 $\phi_i\lambda_i=0$），但在松弛和数值实现上，逐分量形式更灵活——这是为什么实际代码（如 dairlib、Drake 的 CI-TO）多用逐分量形式。

⚠️ 常见陷阱¶

💡 概念误区：以为 $\phi^\top\lambda\le 0$ 和 $\phi^\top\lambda=0$ 是两个不同的约束

新手想法："互补条件要求乘积为零，为什么代码里写成 $\le 0$？这不是放松了吗？" 实际上：在 $\phi\ge 0$ 且 $\lambda\ge 0$ 的前提下，$\phi^\top\lambda$ 是非负数之和，必然 $\ge 0$；再要求 $\le 0$，就只能等于 0。所以在这两个非负约束的语境下，$\le 0$ 与 $=0$ 完全等价。写成 $\le 0$ 纯粹是工程习惯——不等式约束在 SQP/IPM 里有现成的处理机制（active set、对数障碍），且后续 Scholtes 松弛要把右端从 0 抬到正数 $t_k$，写成 $\le t_k$ 比 $=t_k$ 更自然（松弛后不再要求严格相等）。 为什么重要：理解这个等价关系，才不会在读不同论文/代码时被 $\le$ 和 $=$ 的写法差异迷惑。

🧠 思维陷阱：把"接触力 $\lambda$ 作为优化变量"误解为"维度爆炸不可接受"

新手想法："每个接触点每个时间步都加一组 $\lambda$ 变量（法向+切向），变量数岂不是爆炸？" 实际上：变量数确实增加，但只是**线性**增加——$O(N\cdot n_c\cdot d)$（$N$ 时间步、$n_c$ 接触点、$d$ 摩擦锥边数）。对一个 $N=50$、$n_c=4$、$d=4$ 的四足问题，多出约 $50\times4\times5=1000$ 个接触变量，对现代 NLP 求解器完全在可接受范围。真正的困难**不是变量多，而是互补约束让问题病态**（§6.3）。把困难归因于"变量多"是误诊。 正确思维：诊断 CI-TO 的困难时，先区分"规模问题"（变量/约束多，可用稀疏性、并行化缓解）和"病态问题"（约束品性退化，必须用松弛/特殊算法）。CI-TO 的核心困难是后者，不是前者。

💡 概念误区：以为离散动力学约束 $h_k=0$ 可以用任意显式积分格式

新手想法："$h_k=0$ 不就是把 $\dot x=f$ 离散化吗？用显式 Euler $x_{k+1}=x_k+\Delta t\,f(x_k,u_k,\lambda_k)$ 不就行了？" 实际上：接触力学**不能用显式格式**。原因在 §30 详述过——刚性接触的法向力在接触瞬间是冲量型的（理论上无穷大、有限冲量），显式格式会让脚穿透地面后才"反应过来"，导致穿透累积、能量爆破。必须用**隐式**时步法（Stewart-Trinkle、Anitescu-Potra），把接触冲量和速度更新放在同一步隐式求解。这就是为什么 $h_k$ 把 $x_{k+1}$ 和 $\lambda_k$ 耦合在一个隐式方程里，而不是显式赋值。 为什么重要：误用显式格式是 CI-TO 仿真/优化不一致的常见根源——优化器解出的"最优轨迹"在真机/高保真仿真里穿透地面，因为离散格式本身就违背接触物理。

练习¶

练习 6.2.1（推导题，草稿纸上完成） 以一维垂直落体接触地面为例（质量 $m$，垂直位置 $q$，速度 $v$，地面在 $q=0$），写出：(a) 间隙函数 $\phi(q)$；(b) 用隐式 Euler 时步法离散的动力学约束 $h_k=0$（提示：$v_{k+1}=v_k+\Delta t(-g + \lambda_k/m)$，$q_{k+1}=q_k+\Delta t\,v_{k+1}$）；(c) 法向互补约束。把这三项组装成这个最小系统的单步 MPCC。

练习 6.2.2（形式化练习，草稿纸上完成） 把上题的摩擦补上——假设落体还有水平自由度 $x$、水平速度 $v_x$。写出：(a) 摩擦锥约束；(b) 切向力与切向速度的耦合（最大耗散）。讨论：当 $v_x=0$（静止）时，切向力 $\lambda_t$ 的取值如何被约束？（提示：$\partial\|v_t\|$ 在 $v_t=0$ 处是 $[-1,1]$，对应 $|\lambda_t|\le\mu\lambda_n$ 但方向不定——这是静摩擦。）

练习 6.2.3（开放思考题） Posa 2014 的 MPCC 把接触力 $\lambda$ 显式地作为优化变量。§6.7 的 IDTO（Kurtz 2023）却**消掉了** $\lambda$，只保留位形 $q$。请预测：消掉 $\lambda$ 之后，互补约束去哪了？（提示：IDTO 用 MuJoCo 的软接触把 $\lambda$ 写成 $q,\dot q,\ddot q$ 的光滑函数，从而互补约束被"光滑化"隐藏起来。）这种做法的好处和代价分别是什么？带着这个问题进入 §6.7，验证你的预测。

§6.3 互补约束的病态：约束品性为什么必然失效 ⭐⭐⭐¶

动机：为什么"写对了 NLP"还是解不出来¶

假设你严格按 §6.2 写出了 CI-TO 的 MPCC，逐项都正确，然后满怀信心地把它喂给业界最强的 NLP 求解器 IPOPT 或 SNOPT。结果大概率是这样的：

迭代次数飙到上限仍不收敛；
对偶变量（拉格朗日乘子）的范数越来越大，最后变成 inf 或 nan；
SQP 的步长 $\alpha$ 一路缩到 $10^{-12}$ 然后卡住；
求解器报告 "restoration failed" 或 "iterates diverging"。

这不是你的 NLP 写错了，也不是求解器有 bug。这是 MPCC 的内禀病态——互补约束破坏了几乎所有标准 NLP 求解器赖以工作的理论前提。本节要严格地解释这个病态的根源：约束品性（Constraint Qualification, CQ）在双活点处必然失效。

反面：标准 NLP 理论依赖什么¶

要理解"失效"，先回顾"正常情况下依赖什么"。标准 NLP

\[\min f(x)\quad\text{s.t.}\quad g_i(x)\le 0,\ h_j(x)=0\]

的一阶最优性条件是 KKT（Karush-Kuhn-Tucker）条件。但 KKT 条件成为最优性的**必要条件**，需要一个前提——约束品性（CQ）。最常用的 CQ 是：

MFCQ（Mangasarian-Fromovitz 约束品性）：在可行点 $x^*$ 处，(i) 所有等式约束的梯度 $\{\nabla h_j(x^*)\}$ 线性无关；(ii) 存在一个方向 $d$，使得 $\nabla h_j(x^*)^\top d=0$（对所有 $j$）且 $\nabla g_i(x^*)^\top d<0$（对所有活动不等式约束 $i$）。

MFCQ 的几何含义：约束的线性化在 $x^*$ 处"忠实地"刻画了可行域的局部形状——存在一个方向能严格地往可行域内部走。如果 MFCQ 成立，KKT 乘子存在且**有界**；如果 MFCQ 失效，乘子可能不存在，或存在但无界——这正是 IPOPT 乘子爆炸的根源。

回顾桥接（凸优化/数值优化）：在凸优化里我们学过 Slater 条件（存在严格内点）保证强对偶；在 NLP 里 MFCQ 是它的局部推广。MFCQ 失效 ⟺ 对偶问题的解集无界或空 ⟺ 原始-对偶迭代发散。这个等价关系是理解所有 MPCC 数值病态的总钥匙。

历史：Scheel 与 Scholtes 的诊断（2000）¶

MPCC 的 CQ 失效不是一个偶然的数值现象，而是一个有定理保证的**结构性事实**。Holger Scheel 和 Stefan Scholtes 在 2000 年的论文（Mathematics of Operations Research 25(1):1–22）中证明了这一点。这篇论文是 MPCC 数值困难的"诊断书"——它告诉我们病在哪、为什么治不了（用标准方法）。

Scholtes 后续（2001, SIAM J. Optim. 11(4):918–936）又提出了"治疗方案"——Scholtes 松弛（§6.4）。诊断和治疗都出自同一人，这在科学史上颇为优雅。

理论：MFCQ 在双活点必然失效（完整证明）¶

现在我们严格证明本章最重要的理论结论。这是 R1 理论教学"完整推导链"的核心示范——每一步都写出来，不允许"显然"。

定理（MPCC 的 MFCQ 失效，T1）：考虑互补约束的标准 NLP 编码 $$\phi_i\ge 0,\qquad \lambda_i\ge 0,\qquad \phi_i\lambda_i= 0.$$ 若在可行点处某个分量满足 $\phi_i=0$ 且 $\lambda_i=0$（称为**双活点 / biactive point**，即该互补对的两侧同时取到边界），则 MFCQ 在该点失效。

证明。 为清晰起见，先看单个互补对 $(\phi,\lambda)$（下标 $i$ 省略），把 $\phi,\lambda$ 看成优化变量空间里的两个坐标（或更一般地，看成决策变量 $z$ 的函数 $\phi(z),\lambda(z)$，但先用最简情形 $\phi=z_1,\lambda=z_2$ 建立直觉，再推广）。

Step 1：列出三条约束及其梯度。 三条约束是：

\[g_1: -\phi\le 0\quad(\text{即 }\phi\ge 0),\qquad g_2: -\lambda\le 0\quad(\text{即 }\lambda\ge 0),\qquad h: \phi\lambda= 0.\]

在 $z=(\phi,\lambda)$ 坐标下，梯度分别为：

\[\nabla g_1=\begin{pmatrix}-1\\0\end{pmatrix},\qquad \nabla g_2=\begin{pmatrix}0\\-1\end{pmatrix},\qquad \nabla h=\nabla(\phi\lambda)=\begin{pmatrix}\lambda\\\phi\end{pmatrix}.\]

最后一个用了乘积求导：$\partial(\phi\lambda)/\partial\phi=\lambda$，$\partial(\phi\lambda)/\partial\lambda=\phi$。

Step 2：代入双活点 $(\phi,\lambda)=(0,0)$。 此时：

\[\nabla g_1=\begin{pmatrix}-1\\0\end{pmatrix},\qquad \nabla g_2=\begin{pmatrix}0\\-1\end{pmatrix},\qquad \nabla h=\begin{pmatrix}0\\0\end{pmatrix}.\]

关键观察：等式约束 $h$ 的梯度 $\nabla h$ 在双活点处退化为零向量！ 这是因为 $\nabla(\phi\lambda)=(\lambda,\phi)$ 在 $\phi=\lambda=0$ 时两个分量都是 0。

Step 3：检验 MFCQ 的条件 (i)——等式约束梯度线性无关。 在双活点，活动约束是 $g_1$（因 $\phi=0$）、$g_2$（因 $\lambda=0$）、$h$（等式约束总活动）。等式约束只有 $h$ 一条，要求 $\{\nabla h\}$ 线性无关，即 $\nabla h\neq 0$。但 Step 2 算出 $\nabla h=0$。单个零向量是线性相关的（$1\cdot\nabla h=0$ 是非平凡线性组合等于零）。条件 (i) 失效。 证毕——MFCQ 已经在第一个条件上崩溃。

Step 4（补充，理解失效的深度）：即使绕过条件 (i)，条件 (ii) 也失效。 MFCQ 的条件 (ii) 要求存在方向 $d=(d_\phi,d_\lambda)$，满足等式约束的切向 $\nabla h^\top d=0$（因 $\nabla h=0$，这对任意 $d$ 平凡成立），且严格满足活动不等式 $\nabla g_1^\top d<0$ 和 $\nabla g_2^\top d<0$：

\[\nabla g_1^\top d=-d_\phi<0\ \Rightarrow\ d_\phi>0,\qquad \nabla g_2^\top d=-d_\lambda<0\ \Rightarrow\ d_\lambda>0.\]

所以需要 $d_\phi>0,d_\lambda>0$。这样的 $d$ 是存在的（例如 $d=(1,1)$）。但问题在于：沿这个方向走，会立刻违反等式约束 $h=\phi\lambda=0$！ 因为从 $(0,0)$ 沿 $(1,1)$ 走到 $(\epsilon,\epsilon)$，$\phi\lambda=\epsilon^2>0\neq 0$。也就是说，线性化告诉我们"可以往 $(1,1)$ 方向走"（因为 $\nabla h^\top d=0$），但真实的非线性约束 $\phi\lambda=0$ 不允许。线性化"骗了"我们——这正是 CQ 失效的本质：约束的线性化不再忠实刻画可行域的局部形状。

Step 5：推广到一般 $\phi(z),\lambda(z)$。 当 $\phi,\lambda$ 是决策变量 $z$ 的非平凡函数时，$\nabla_z(\phi\lambda)=\lambda\nabla_z\phi+\phi\nabla_z\lambda$。在双活点 $\phi=\lambda=0$，仍有 $\nabla_z(\phi\lambda)=0\cdot\nabla_z\phi+0\cdot\nabla_z\lambda=0$。结论不变：等式约束 $\phi\lambda=0$ 的梯度在双活点恒为零向量，条件 (i) 必然失效。证毕。

本质洞察：MPCC 病态的几何根源，是互补可行集那个 "L 形尖角"（§6.1 视角二）。在尖角顶点（双活点），可行集没有良好定义的切锥——它由两条互相垂直的射线拼成，在拐点处方向突变。约束的一阶线性化（梯度）无法捕捉这个尖角，于是 CQ 失效。这不是某个特定 NLP 编码的缺陷，而是互补可行集**几何形状的内禀属性**——无论你怎么写约束，尖角都在那里。

失效的后果：连锁反应¶

MFCQ 失效不是一个孤立的技术细节，它引发一连串后果，每一个都对应你在 IPOPT 输出里看到的一种症状：

MFCQ 失效导致	数学后果	你在求解器里看到的症状
乘子集合无界	KKT 乘子可以任意大	对偶变量 → `inf`/`nan`，乘子爆炸
线性化失真	SQP 的 QP 子问题与真问题脱节	步长 → 0，迭代停滞
严格互补缺失	无内点（违反 IPM 的中心路径假设）	内点法的 barrier 参数无法推进
LICQ 也失效	不能用标准的二阶充分条件	收敛性证明前提全部失效

对比性思维（反事实）：如果互补约束的可行集不是 L 形尖角，而是一个光滑流形（比如一条光滑曲线），会怎样？→ 那么切锥处处良定，MFCQ 成立，标准 NLP 理论畅通无阻。正是"尖角"——两条射线在原点的非光滑拼接——杀死了 CQ。 这个反事实告诉我们：所有的救援方法（§6.4）本质上都在做同一件事——把尖角磨圆，让可行集局部变得光滑或近似光滑，从而恢复某种约束品性。Scholtes 松弛把尖角换成一条双曲线（$\phi\lambda\le t$），Lin-Fukushima 用光滑函数把 L 形磨成圆角，罚函数则干脆把约束移进目标。

多视角理解：为什么严格互补点反而"健康"¶

一个深刻的对照：双活点（$\phi=\lambda=0$）病态，但严格互补点（$\phi=0,\lambda>0$ 或 $\phi>0,\lambda=0$）健康。 理解这个对照，就理解了病态的精确位置。

考虑严格互补点 $\phi=0,\lambda>0$（脚正接触地面，有正法向力）。此时：

$g_1:\phi\ge 0$ 活动（$\phi=0$），$g_2:\lambda\ge 0$ 不活动（$\lambda>0$）；
$\nabla h=\nabla(\phi\lambda)=(\lambda,\phi)=(\lambda,0)$，其中 $\lambda>0$，非零向量！

于是等式约束梯度 $\nabla h\neq 0$，条件 (i) 满足；可以验证条件 (ii) 也满足。MFCQ 在严格互补点成立。 病态**只发生在双活点**——也就是接触的**临界状态**：脚恰好碰到地面但还没受力、或恰好离地的那一瞬间（make/break 的瞬间）。

本质洞察：CI-TO 的全部数值困难，集中在接触的 make/break 瞬间——那一刻 $\phi$ 和 $\lambda$ 都趋于零，系统正站在 L 形尖角的顶点上。在接触稳定建立后（$\phi=0,\lambda>0$）或完全分离后（$\phi>0,\lambda=0$），问题是健康的。这解释了为什么 CI-TO 在"接触序列频繁切换"的任务（蹦跳、攀爬）上特别难，而在"接触稳定"的任务（站立、稳定抓握）上相对容易——前者反复经过尖角，后者远离尖角。这也预告了 §6.7 为什么实时 CI-MPC 要在参考轨迹附近做 Taylor 近似（远离尖角时近似有效）。

MPCC 的稳定性层级：C-/M-/S-/B-stationarity¶

既然 KKT/MFCQ 那套标准理论在 MPCC 上失效，优化理论家们发展了一套**专门为 MPCC 设计的稳定性（stationarity）刻画**。这套刻画是理解 §6.4 各松弛方法"收敛到什么"的语言。你不需要记住每一条的精确定义，但要知道它们的**强弱顺序**和**工程含义**。

对 MPCC，存在一个稳定性概念的层级（从强到弱）：

\[\text{S-stationarity}\ \Rightarrow\ \text{M-stationarity}\ \Rightarrow\ \text{C-stationarity}\ \Rightarrow\ \text{W-stationarity}.\]

稳定性	全称	直觉含义	强度
S	Strong	双活点处乘子符号约束最严，等价于"把 MPCC 当普通 NLP 的 KKT"	最强（最接近真正的局部最优）
M	Mordukhovich	双活点乘子或同号、或一个为零，用极限法锥刻画	较强
C	Clarke	双活点乘子之积非负（$\mu_\phi\mu_\lambda\ge 0$）	中等
W	Weak	最弱，只要求各分量乘子存在	最弱（可能不是局部最优）

工程含义：一个松弛算法收敛到越强的稳定性，解的质量越有保证。S-stationarity 几乎等价于真正的局部最优；C-stationarity 较弱（允许一些"伪稳定点"），但在工程上通常够用。Scholtes 松弛在一般条件下收敛到 C-stationarity，在额外条件（MPEC-LICQ + 二阶条件）下可达 B-stationarity（B-stationarity 是基于切锥的最优性概念，与 S 在适当条件下一致）。这是 §6.4 定理 T2 的核心，也是为什么 Scholtes 是工程默认——它收敛到的稳定性级别在可接受范围，且数值最稳。

类比（标注边界）：MPCC 的稳定性层级像考试的"及格线分级"——S 是满分（真最优），M、C 是不同档次的及格，W 是勉强划线。像的部分：都是对"解有多好"的分级评价。不像的部分：考试分数是连续的，而稳定性是离散的逻辑层级，且它们之间是严格的逻辑蕴含关系（强的自动满足弱的），不是简单的数值高低。不要把这个类比理解成"C-stationarity = 60 分"这种定量对应。

⚠️ 常见陷阱¶

🧠 思维陷阱：忽视 MFCQ 失效，直接把 MPCC 喂给 IPOPT/SNOPT

新手想法："IPOPT 是工业级求解器，什么 NLP 都能解，我直接把互补约束写进去让它解就行。" 现象/后果：SQP 无限迭代、乘子范数爆炸、步长趋零、求解器报 restoration failure；即使偶尔"收敛"，得到的也常是伪解（不满足真正的互补）。 根本原因：MFCQ 在所有双活点（接触 make/break 瞬间）必然失效（本节定理 T1）。IPOPT/SNOPT 的收敛理论建立在 CQ 成立的前提上，前提崩了，算法行为就无保证。 正确做法：永远不要把 raw MPCC 喂给通用 NLP 求解器。必须先用 §6.4 的松弛（Scholtes、Lin-Fukushima）或罚函数（elastic mode）把互补约束正则化，或用专门为 MPCC/接触设计的求解器（CALIPSO、IMPACT、Dojo 的定制内点法）。

💡 概念误区：以为"病态"是因为问题非凸

新手想法："互补可行集非凸，所以求解难——这就是病态的原因吧？" 实际上：非凸和病态是**两个独立的困难**。非凸导致"可能陷入局部最优"（全局最优性问题）；病态（CQ 失效）导致"连局部最优都求不出来"（一阶算法本身发散）。即使你只要局部解，CQ 失效仍让标准算法崩溃。反过来，有些非凸问题 CQ 良好（如一般光滑非凸 NLP），照样能用 IPOPT 求局部解。MPCC 的特殊性在于它同时具有非凸和 CQ 失效两重困难，而 CQ 失效是更致命的那一重。 为什么重要：混淆二者会导致误诊——以为加个全局优化器（如分支定界）就能解决，实际上全局优化器的每个子问题仍受 CQ 失效之害。

🧠 思维陷阱：以为病态发生在所有接触时刻

新手想法："只要有接触，互补约束就病态。" 实际上：病态**只发生在双活点**（$\phi=\lambda=0$，接触的临界/切换瞬间）。接触稳定建立后（$\phi=0,\lambda>0$）是严格互补点，MFCQ 成立，问题健康（见本节"为什么严格互补点反而健康"）。 正确思维：诊断 CI-TO 收敛困难时，先问"是不是接触模式在频繁切换？"——频繁切换 = 频繁经过双活点 = 频繁病态。如果接触稳定却仍不收敛，病因可能在别处（如动力学约束尺度失衡、warm-start 差），见 §6.8。

练习¶

练习 6.3.1（证明题，草稿纸上完成） 仿照本节定理 T1 的证明，证明：互补约束 $0\le\phi\;\perp\;\lambda\ge 0$ 写成 $\phi\ge 0,\lambda\ge 0,\phi\lambda\le 0$（不等式形式而非等式）时，在双活点处 LICQ（线性无关约束品性，要求所有活动约束梯度线性无关）也失效。（提示：在双活点，$g_1=-\phi,g_2=-\lambda,g_3=\phi\lambda$ 三条不等式都活动，写出三个梯度 $(-1,0),(0,-1),(0,0)$，注意第三个是零向量，零向量与任何向量组都线性相关。）

练习 6.3.2（开放思考题，草稿纸上完成） §6.3 证明了 MFCQ 在双活点失效。请反向思考：是否存在一种互补约束的编码方式，使得 CQ 不失效？ 提示：考虑 Fischer-Burmeister 函数 $\psi_{FB}(\phi,\lambda)=\phi+\lambda-\sqrt{\phi^2+\lambda^2}$，它满足 $\psi_{FB}(\phi,\lambda)=0\iff 0\le\phi\perp\lambda\ge0$。计算 $\psi_{FB}$ 在原点的梯度——它存在吗？$\psi_{FB}$ 在原点光滑吗？这个练习揭示了一个深刻事实：互补约束的非光滑性是几何内禀的，换编码只能转移困难，不能消除困难。

练习 6.3.3（跨节综合题） 结合 §6.1 的"L 形可行集"几何视角和本节的"MFCQ 失效"代数证明，用一段话向一个只懂凸优化、没接触过 MPCC 的同学解释：为什么接触轨迹优化"看起来只是多了几个约束，实际上换了一个理论世界"。要求同时说清几何根源（尖角）和代数后果（梯度退化、乘子爆炸），并指出病态的精确位置（双活点 = make/break 瞬间）。

§6.4 松弛与光滑化：把尖角磨圆的三条路 ⭐⭐⭐¶

动机：既然不能硬解，就把问题改造成能解的¶

§6.3 诊断出病根——互补可行集的 L 形尖角让 CQ 失效。本节给出治疗方案。所有治疗的共同思路在 §6.3 末尾的反事实里已经点明：把尖角磨圆。具体有三条路，对应三类技术：

松弛（relaxation）：把互补等式 $\phi\lambda=0$ 放松成不等式 $\phi\lambda\le t_k$，让可行集从两条射线"加厚"成一个带状区域，随 $t_k\downarrow 0$ 逼近原问题。代表：Scholtes 松弛。
光滑化（smoothing）：用一个含参数的光滑函数逼近非光滑的互补关系，随参数趋零恢复尖角。代表：Lin-Fukushima 光滑化、Fischer-Burmeister 光滑化。
罚函数（penalty / elastic mode）：把互补约束移进目标函数，作为惩罚项 $\rho\|\phi^\top\lambda\|$，随罚因子 $\rho\uparrow\infty$ 强制互补。代表：$\ell_1$ 罚 / elastic mode。

这三条路是 R6E"系统性分类"的体现——它们不是随意罗列的技巧，而是按"如何处理那个为零的约束"穷举出的三个维度：放松约束本身（松弛）、替换约束函数（光滑化）、移走约束（罚函数）。下面逐一推导，并给出各自的收敛保证。

反面：不松弛、直接用大罚因子会怎样¶

在进入正题前，先看一个诱人但危险的"伪解法"。有人会想：既然互补难满足，我就给 $\phi^\top\lambda$ 加一个**固定的大罚因子** $\rho=10^6$，让 $\min J + 10^6\,\phi^\top\lambda$，强迫优化器把 $\phi^\top\lambda$ 压到接近零。

后果：固定大罚因子让目标函数极度病态。Hessian 的条件数随 $\rho$ 线性恶化（罚项主导，且其曲率集中在互补方向），优化器在"陡峭的罚谷"里反复震荡，收敛极慢甚至不收敛。这就是为什么罚函数法必须用**渐增**的 $\rho$（continuation），而不是一开始就上大数——这一点和松弛法用渐减的 $t_k$ 是对偶的思想。理解这个反面，才能理解为什么三条路都是"连续化（continuation）"框架：用一个同伦参数（$t_k$ 或 $\rho$）把易解的近似问题逐步逼向难解的原问题。

理论一：Scholtes 松弛（工程默认）¶

Scholtes 松弛（Scholtes 2001, SIAM J. Optim. 11(4):918–936）是工程上最稳、最常用的方法。思想极简单：把互补等式放松成一个带参数的不等式。

原互补约束（逐分量）：$\phi_i\ge 0,\ \lambda_i\ge 0,\ \phi_i\lambda_i= 0$。

Scholtes 松弛后：

\[\phi_i\ge 0,\qquad \lambda_i\ge 0,\qquad \phi_i\lambda_i\le t_k,\]

其中 $t_k>0$ 是松弛参数，按 $t_k\downarrow 0$ 的序列逐步缩小。

几何意义（这是理解松弛的钥匙）：原可行集是 L 形（正 $\phi$ 轴 ∪ 正 $\lambda$ 轴）。松弛后，约束 $\phi\lambda\le t_k$ 在 $(\phi,\lambda)$ 第一象限里画出一条双曲线 $\phi\lambda=t_k$ 以下的区域——可行集从两条射线"加厚"成一个包住原点的带状区域，尖角被磨成了光滑的双曲线边界。在这个加厚的可行集上，MFCQ 恢复成立（双曲线边界处处有良定的法向），标准 NLP 求解器可以正常工作。随 $t_k\downarrow 0$，双曲线越来越贴近两条坐标轴，带状区域收缩回 L 形，松弛解收敛到原 MPCC 的解。

算法（分阶段 Scholtes，Posa 2014 采用的形式）：

给定初始 t_0（如 1e-2），缩减因子 β（如 0.1）
for k = 0, 1, 2, ...:
    固定 t_k，把松弛后的 NLP 喂给 SQP/IPM，跑到收敛，得 x_k*
    用 x_k* 作为下一阶段的 warm-start
    t_{k+1} = β * t_k
until t_k < 容差（如 1e-8）或互补残差足够小

注：这段伪代码是算法骨架，不是可运行实现，故不计入 text:code 比例的"代码"——它服务于讲清算法流程（理论教学允许少量算法骨架，但主体仍是推导与解释）。

收敛定理（T2，Scholtes 2001）：在适当假设下，当 $t_k\to 0$ 时，松弛问题解序列 $\{x_k^*\}$ 的任意聚点 $x^*$ 是原 MPCC 的 C-stationary 点；若进一步满足 MPEC-LICQ 和 ULSC（上层严格互补条件），则 $x^*$ 是 B-stationary 点（在适当条件下等价于 S-stationarity，即真正的局部最优一阶条件）。

为什么 Scholtes 是"最稳"的：Hoheisel-Kanzow-Schwartz（2013, Math. Program. 137:257–288）系统比较了六种松弛方法（Scholtes、Lin-Fukushima、Kadrani、Steffensen-Ulbrich、Kanzow-Schwartz、DeMiguel-Friedlander-Nogales-Scholtes）的稳定性级别和数值表现。结论：Scholtes 松弛在最弱的假设下就有收敛保证，且实现最简单（只改一个约束的右端），数值行为最稳健。这就是它成为工程默认的原因。

本质洞察：Scholtes 松弛保留了互补的**结构**（仍是 $\phi\ge0,\lambda\ge0$ 加一个耦合约束），只放松了**精度**（$=0$ 变 $\le t_k$）。这种"保结构、松精度"的哲学是它稳健的根源——它从不破坏问题的物理意义（$\phi,\lambda$ 始终非负，始终是合法的间隙和力），只是允许接触在 make/break 瞬间有一个 $O(t_k)$ 的"软化"。随 $t_k\to0$，软化消失，物理恢复严格。

理论二：Lin-Fukushima 光滑化¶

Lin-Fukushima 光滑化（Lin-Fukushima 2005, Ann. Oper. Res.）走另一条路——不放松约束，而是用一个**光滑函数**替换非光滑的互补关系。

回顾 §6.3 练习 6.3.2 提到的 Fischer-Burmeister（FB）函数：

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

它有一个漂亮的性质：$\psi_{FB}(a,b)=0\iff 0\le a\;\perp\;b\ge 0$（习题：验证——若 $a=0,b\ge0$，则 $\psi=b-|b|=b-b=0$；若 $b=0,a\ge0$ 同理；反之若 $\psi=0$ 可推出互补）。所以互补约束等价于光滑方程 $\psi_{FB}(\phi,\lambda)=0$——单个等式代替了"$\ge,\ge,\cdot=0$"三条约束。

但 $\psi_{FB}$ 在原点 $(0,0)$ 不可微（$\sqrt{a^2+b^2}$ 在原点有尖角）。Lin-Fukushima 引入**光滑参数** $\mu>0$：

\[\psi_\mu(a,b)=a+b-\sqrt{a^2+b^2+\mu^2}.\]

加上 $\mu^2$ 后，根号内恒正，$\psi_\mu$ 处处光滑（包括原点）。约束变成 $\psi_\mu(\phi,\lambda)=0$，随 $\mu\downarrow 0$ 恢复 FB 函数、恢复严格互补。

几何意义：$\psi_\mu=0$ 的零水平集是一条**光滑曲线**，它逼近 L 形的两条射线但在拐角处被磨成一个半径约 $\mu$ 的圆角。又是"磨圆尖角"，只是这次磨出的是圆弧而非双曲线。

Scholtes vs Lin-Fukushima 的对比：

维度	Scholtes 松弛	Lin-Fukushima 光滑化
改造对象	约束右端（$=0\to\le t_k$）	约束函数（用光滑 $\psi_\mu$ 替换）
约束类型	保留不等式结构	变成单个光滑等式
可行集	L 形加厚成带状	L 形磨成圆角曲线
同伦参数	$t_k\downarrow 0$	$\mu\downarrow 0$
实现难度	最简（改右端）	中（换函数、算新雅可比）
收敛稳定性级别	C-stationarity（一般），B（强条件）	C-stationarity（一般）
工程偏好	默认首选	备选，某些问题更快

对比性思维（不是 X 而是 Y）：Scholtes 和 Lin-Fukushima 不是 "两个互相竞争、必须二选一"的方法，而是 "同一个连续化框架下的两种磨圆方式"。它们的共同骨架是：引入同伦参数 → 把尖角磨圆 → 参数趋零恢复原问题。理解了这个共同骨架，你遇到任何 MPCC 松弛方法（Kadrani、Steffensen-Ulbrich……）都能一眼看穿它在"磨圆"这件事上做了什么变体。

理论三：罚函数 / Elastic Mode¶

第三条路最直接——把互补约束移进目标函数（Anitescu 2005 等）：

\[\min_{x,u,\lambda}\ J(x,u)+\rho\sum_k\|\phi(x_k)^\top\lambda_k\|_1,\qquad \phi\ge0,\ \lambda\ge0,\]

其中 $\rho>0$ 是罚因子，按 $\rho\uparrow\infty$ 渐增。互补约束不再硬性要求，而是作为惩罚——优化器在最小化原代价 $J$ 的同时被推着减小 $\phi^\top\lambda$。当 $\rho$ 足够大，最优解会让 $\phi^\top\lambda\to 0$，恢复互补。

$\ell_1$ 罚的关键性质——精确罚（exact penalty）：与 $\ell_2$ 罚不同，$\ell_1$ 罚是**精确的**——存在一个**有限**的阈值 $\rho^*$，当 $\rho>\rho^*$ 时，罚问题的解就是原问题的解（不需要 $\rho\to\infty$）。这是 $\ell_1$ 范数的非光滑性带来的好处（次微分在零点是一个区间，能"吸收"乘子）。$\ell_2$ 罚则需要 $\rho\to\infty$ 才精确，且如 §6.4 反面所述会让 Hessian 病态。

Elastic mode 是 SNOPT 等求解器内置的精确罚实现：引入松弛变量 $s\ge 0$，把难约束 $c(x)=0$ 改成 $c(x)=s-t$（或类似），并在目标里罚 $\|s\|_1$。它本质上是 $\ell_1$ 精确罚的一种工程封装。

三条路的统一视角：

路	怎么处理 $\phi\lambda=0$	同伦参数	收敛到	工程地位
Scholtes 松弛	放松成 $\phi\lambda\le t_k$	$t_k\downarrow0$	C-stat（一般），B-stat（强条件）	默认首选
Lin-Fukushima 光滑	换成光滑 $\psi_\mu=0$	$\mu\downarrow0$	C-stat	备选
$\ell_1$ 罚 / elastic	移进目标罚 $\rho\\|\phi\lambda\\|_1$	$\rho\uparrow$（精确罚有限即可）	C-stat	SNOPT 内置

本质洞察：三条路殊途同归——都是把"病态的硬约束"换成"良态的近似"，再用一个同伦参数把近似逐步逼回原问题。它们收敛到的稳定性级别都是 C-stationarity（一般条件下），这不是巧合：C-stationarity 是 MPCC 在"从外部逼近"（松弛/光滑/罚都从可行集外部或目标侧逼近）这一框架下能达到的自然极限。要拿到更强的 B/S-stationarity，需要问题本身满足 MPEC-LICQ 等额外结构条件。

理论-工程桥接：松弛参数的调度是一门手艺¶

推导完三条路的数学，立刻连接工程实践（R6D）。在真实代码里，松弛/光滑/罚都不是"设一个参数解一次"，而是一个**连续化（continuation）循环**，参数调度直接决定成败：

初始参数：$t_0$ 太大 → 松弛问题离原问题太远，解无意义；$t_0$ 太小 → 一开始就病态，warm-start 失效。经验值 $t_0\sim 10^{-2}$。
缩减因子：$\beta$ 太激进（如 0.01）→ 相邻阶段跳跃大，warm-start 失效，每阶段重新挣扎；$\beta$ 太保守（如 0.5）→ 阶段数多，总时间长。经验值 $\beta\sim 0.1$。
每阶段收敛容差：早期阶段（$t_k$ 大）不必解到高精度，松一点快速推进；后期阶段（$t_k$ 小）才收紧——这叫"inexact continuation"。
终止：当互补残差 $\max_k\phi(x_k)^\top\lambda_k<\epsilon_{\text{comp}}$（如 $10^{-6}$）即可停，不必把 $t_k$ 压到机器精度。

正因为 Scholtes 松弛把困难集中在"参数调度"这一可控旋钮上，**工程上才**把 CI-TO 的求解封装成"外层 continuation 循环 + 内层标准 SQP"的两层结构——这不是随意的软件设计，而是 §6.3 病态分析的直接产物：病态只在 $t_k\to0$ 时浮现，所以让 $t_k$ 缓慢下降、每步 warm-start，就能"贴着病态的边缘"逐步逼近真解。这个两层结构的思想，将在 §6.7 的 Fast-CI-MPC bilevel 结构里以另一种面貌重现。

多视角理解：松弛、罚、光滑与你已知的方法¶

为了把这三条路挂到你已有的知识树上（R2 跨章关联）：

Scholtes 松弛 ≈ 内点法的障碍参数：内点法用 $-\mu\log(\cdot)$ 障碍把不等式"软化"，随 $\mu\downarrow0$ 逼近边界。Scholtes 的 $t_k$ 扮演同样的同伦参数角色。像的部分：都用一个趋零的参数做 continuation。不像的部分：内点法软化的是可行域内部，Scholtes 软化的是互补约束本身。
$\ell_1$ 罚 ≈ Lasso 的稀疏正则：机器学习里 $\ell_1$ 正则诱导稀疏。这里 $\ell_1$ 罚 $\phi^\top\lambda$ 诱导"互补"（让乘积稀疏地为零）。像的部分：都利用 $\ell_1$ 在零点的次微分区间。不像的部分：Lasso 罚的是参数本身，这里罚的是两个非负量的乘积。
光滑化 ≈ ReLU 的 softplus 近似：深度学习把不可微的 ReLU 用 softplus $\log(1+e^x)$ 光滑近似。FB 函数的 $\mu$-光滑化是同一思想在互补函数上的应用。像的部分：都用含参光滑函数逼近非光滑函数，参数趋零恢复尖角。不像的部分：softplus 近似的是单变量尖角，$\psi_\mu$ 近似的是二变量互补关系。

⚠️ 常见陷阱¶

🧠 思维陷阱：把软接触光滑化的刚度 $k$ 当作物理刚度来调

新手想法："软接触用 $F=k\cdot\max(0,-\phi)$ 近似刚性接触，$k$ 就是地面的弹性刚度，按真实材料设就行。" 现象/后果：$k$ 设太大 → 梯度爆炸、数值失败、积分步长被迫极小；$k$ 设太小 → 脚严重穿透地面、物理失真，优化解在真机上不可行。 根本原因：$k$（以及 Scholtes 的 $t_k$、Lin-Fukushima 的 $\mu$）是**数值同伦参数，不是物理量**。它的作用是控制"尖角磨得多圆"，与地面的真实材料刚度无关。 正确做法：把 $k$（或 $t_k$、$\mu$）当作 continuation 参数，从一个数值友好的值开始，随迭代逐步收紧（$k\uparrow$ 或 $t_k\downarrow$），最终逼近刚性极限。绝不固定在某个"物理"值。这条陷阱在 §6.7 IDTO 的软接触里会再次出现。

💡 概念误区：以为松弛参数越小越好，直接设 $t=10^{-12}$

新手想法："既然 $t_k\to0$ 才恢复真问题，我一步到位设 $t=10^{-12}$ 不就最精确？" 实际上：一步到位设极小 $t$ = 一开始就站在病态悬崖边（§6.3：$t\to0$ 时 MFCQ 失效重新逼近）。没有好的 warm-start，求解器在极病态的问题上从零开始，几乎必然失败。continuation 的全部意义就是"不要一步到位"——用大 $t$ 解一个良态问题，拿它的解 warm-start 稍小 $t$ 的问题，逐步逼近。 为什么重要：这是 continuation 类方法的通用智慧——同伦参数的下降路径本身就是算法的核心，不是可有可无的"调参"。

💡 概念误区：以为三条路收敛到的解一定是全局最优

新手想法："松弛收敛到 C-stationary 点，那不就是最优解？" 实际上：C-stationary（甚至 S-stationary）只是**局部一阶最优**，不保证全局最优。MPCC 是非凸的（§6.3），松弛只解决了 CQ 失效（让局部解可求），没解决非凸（多个局部解）。不同的初值/warm-start 可能收敛到不同的局部解（对应不同的接触序列）。 正确思维：松弛 + 好的初值 → 一个高质量局部解。要更接近全局，需多初值重启、或结合全局搜索（§6.5 混合整数、或 Pang-Tedrake 2023 的 sampling-based 全局接触搜索）。这条认识直接通向 §6.5。

练习¶

练习 6.4.1（推导题，草稿纸上完成） 对 Scholtes 松弛后的 NLP $$\min J(x,u)\quad\text{s.t.}\quad h_k=0,\ \phi_k\ge0,\ \lambda_k\ge0,\ \phi_k\lambda_k\le t,$$ 写出其拉格朗日函数 $L$（引入乘子 $\mu_k$ 对 $h_k$、$\nu_k^\phi$ 对 $-\phi_k$、$\nu_k^\lambda$ 对 $-\lambda_k$、$\eta_k$ 对 $\phi_k\lambda_k-t$），并对 $x_k,u_k,\lambda_k$ 求一阶 KKT 条件。注意 $\phi_k=\phi(x_k)$ 对 $x_k$ 的偏导有链式法则。验证：在 $t>0$ 时，这组 KKT 条件的乘子是有界的（与 §6.3 的 $t=0$ 乘子爆炸对比）。

练习 6.4.2（收敛速率比较，草稿纸上完成） 比较 Tassa 2012 软接触刚度 $k$ 与 Scholtes 松弛 $t$ 的收敛速率。提示：软接触 $F=k\max(0,-\phi)$ 是 penalty method，逼近误差 $O(1/k)$；Scholtes 松弛在 MPEC-LICQ + 二阶充分条件下 $\|x(t)-x^*\|=O(t)$。论证：两者同阶，但 Scholtes 保持互补结构（$\phi,\lambda$ 始终是合法的间隙和力），而软接触 $k$ 破坏物理（接触力变成穿透深度的虚构函数）。这个区别为什么在"优化解要拿到真机执行"时至关重要？

练习 6.4.3（开放思考题） 三条松弛路各有适用场景。请为以下三个任务各推荐一条路并说明理由：(a) 用通用求解器 IPOPT 解一个一次性的 CI-TO（离线规划）；(b) 用 SNOPT 的内置功能解 CI-TO；(c) 实时 CI-MPC，需要在 reference 附近反复 warm-start 重解。（提示：(a) Scholtes 最简最稳；(b) SNOPT 有 elastic mode 内置；(c) 实时场景下 continuation 的多阶段循环太慢，可能需要固定一个小 $t$ 配合极好的 warm-start，或转向 §6.7 的专门算法。）

§6.5 混合整数 vs 互补松弛：处理离散接触的两种哲学 ⭐⭐⭐¶

动机：接触模式本质上是离散决策，怎么编码它¶

回到最根本的问题。接触模式——"第 $k$ 步、第 $i$ 个接触点，是接触还是分离"——本质上是一个离散（二元）决策。优化领域处理"连续优化里嵌入离散决策"有两大武器库：

混合整数规划（Mixed-Integer Programming, MIP）：显式引入二元变量 $s\in\{0,1\}$ 表示离散选择，用整数规划求解器（分支定界）处理。
互补/松弛：把离散逻辑用互补约束连续化（前四节的路线），用连续优化处理。

这两条路是两种**哲学**——一种**显式枚举**（让 $0/1$ 变量明确声明接触状态，由分支定界搜索组合），一种**隐式涌现**（让互补约束连续编码，由连续优化让模式浮现）。本节系统对比这两种哲学，让你在面对一个新接触问题时知道该选哪条路。

反面：如果只懂一种哲学会怎样¶

只懂互补松弛，你会在"接触模式数少、但需要全局最优保证"的问题上吃亏——例如规划一个机器人跳上特定台阶的少数几个落脚点，混合整数能给出**全局最优 + 最优性证书（optimality gap），而互补松弛只能给局部解、无证书。反过来，只懂混合整数，你会在"接触点多、时域长"的问题上撞上组合爆炸——分支定界的最坏复杂度随二元变量数指数增长。**两种哲学各有死穴，懂两种才能扬长避短。

历史：两条路的并行发展¶

互补松弛路线：Posa 2014（CI-TO）→ Manchester 2019（变分积分 CI-TO）→ C3/IDTO/Fast-CI-MPC（2022–2024 实时化）。这条线我们已经走了四节。
混合整数路线：Deits-Tedrake（2014, 平面足式 footstep planning via MICP）→ Aceituno-Cabezas 等（Mastalli 组，Simultaneous Contact, Gait and Motion Planning via Mixed-Integer Convex Optimization, RA-L 2018）→ Ding 等的混合整数质心规划。这条线用 MICP（Mixed-Integer Convex Programming）同时规划接触位置、步态切换和运动。

两条路在 2014 年前后几乎同时起步（Posa 的 CI-TO 和 Deits-Tedrake 的 MICP footstep 都在 2014 左右），此后并行发展，各自服务不同的问题谱系。

理论：混合整数如何编码接触¶

混合整数方法用二元变量 $s_k^i\in\{0,1\}$ 显式声明"第 $k$ 步第 $i$ 个接触点是否接触"。配合 **big-M 技术**把"接触时力可正、分离时力为零、分离时间隙可正"编码成线性约束：

\[ \begin{aligned} &\lambda_k^i\le M\,s_k^i, && \text{（}s_k^i=0\Rightarrow\lambda_k^i=0\text{，分离时无力）}\\ &\phi(x_k^i)\le M\,(1-s_k^i), && \text{（}s_k^i=1\Rightarrow\phi=0\text{，接触时间隙为零）}\\ &\lambda_k^i\ge 0,\ \phi(x_k^i)\ge 0,\ s_k^i\in\{0,1\}. \end{aligned} \]

这里 $M$ 是一个足够大的常数（big-M）。验证逻辑： - 若 $s_k^i=0$（声明分离）：第一式 $\lambda\le 0$ 配合 $\lambda\ge 0$ 强制 $\lambda=0$（无力）；第二式 $\phi\le M$ 平凡（间隙可正）。✓ 分离语义。 - 若 $s_k^i=1$（声明接触）：第一式 $\lambda\le M$ 平凡（力可正）；第二式 $\phi\le 0$ 配合 $\phi\ge0$ 强制 $\phi=0$（接触）。✓ 接触语义。

关键对比：混合整数用 $s$ 把互补的"非此即彼"显式地**写成两个分支（$s=0$ 或 $s=1$），让整数求解器去搜索哪个分支最优；互补松弛把同一个"非此即彼"**隐式地**写成 $\phi\lambda=0$，让连续优化去逼近。**同一个物理逻辑，两种数学编码。

本质洞察：混合整数和互补是同一枚硬币的两面。互补约束 $0\le\phi\perp\lambda\ge0$ 在数学上**等价于**一个析取（disjunction）："$\phi=0$ 或 $\lambda=0$"，而析取正是混合整数 big-M 编码的本质。所以**互补松弛是混合整数的连续松弛（continuous relaxation）的一种形式**——它放弃了整数变量的精确组合搜索，换取连续优化的可扩展性。理解这个等价，你就明白为什么有些论文（如 Marcucci-Tedrake 的 GCS, Graphs of Convex Sets）试图在两者之间找中间地带。

理论：big-M 与 MFCQ——两种病态的对照¶

一个深刻的对照：互补松弛的病态是 CQ 失效（§6.3），混合整数的病态是组合爆炸 + big-M 弱松弛。它们是两种不同性质的困难：

困难类型	互补松弛	混合整数
病态根源	CQ 失效（双活点尖角）	组合爆炸（$2^{Nn_c}$ 个分支）
病态表现	乘子爆炸、SQP 停滞	分支定界树指数膨胀
big-M 影响	无 big-M	$M$ 太大 → 线性松弛弱 → 分支多；$M$ 太小 → 切掉可行解
全局最优	不保证（局部解）	保证（含 optimality gap 证书）
规模扩展	变量随 $N$ 线性，但每步病态	变量随 $N$ 线性，但求解时间随二元变量指数（最坏）
实时性	可（C3/IDTO/Fast-CI 等特化算法）	难（除非接触模式数极少或用学习加速）

对比性思维（反事实）：如果接触点数 $n_c$ 和时域 $N$ 都很小（如 $n_c=2,N=10$，共 $2^{20}\approx 10^6$ 个分支），混合整数会怎样？→ 现代 MIP 求解器（Gurobi/Mosek）配合分支定界的剪枝，能在秒级求出**全局最优**，且给出最优性证书。这时混合整数**完胜**互补松弛（后者只有局部解、无证书）。反之，若 $n_c=4,N=50$（共 $2^{200}$ 个分支），混合整数彻底无望，互补松弛是唯一现实选择。问题规模决定哲学选择——这是本节最重要的工程判断。

理论：MICP 在足式规划中的成功¶

混合整数凸规划（MICP）在**足式落脚规划**（footstep planning）上特别成功，因为这个问题恰好落在"接触模式数适中、需要全局最优、可凸化"的甜区：

接触模式适中：足式机器人腿数有限（双足 2 腿、四足 4 腿），每步落脚到哪个"地形块"（terrain segment）是少数离散选择。
可凸化：把地形分解成若干凸多边形（convex regions），"脚落在哪个区域"用二元变量编码，区域内的位置是连续凸约束——这就是 Deits-Tedrake 2014 的 IRIS + MICP 框架。
需要全局保证：跨越障碍、跳上台阶这类任务，落脚点选错（局部解）可能导致摔倒，全局最优很值钱。

Aceituno-Cabezas 等（RA-L 2018）进一步把接触位置、步态序列、质心运动**同时**用 MICP 规划，得到鲁棒的多足运动。这条线直接通向 05 批的足式落脚规划（§6.9 桥接）——落脚规划的经典方法（05 批 §140）和优化驱动方法（05 批 §150）大量使用 MICP。

理论-工程桥接：MICP 在落脚规划成功、互补松弛在全身接触轨迹成功，不是因为一个比另一个先进，而是因为两个问题的结构不同。落脚规划是"在离散地形块上选位置"（少数离散选择 + 凸子问题）→ MICP 甜区；全身接触轨迹是"连续时间上接触力的精细优化"（连续变量主导 + 接触模式可能频繁切换）→ 互补松弛甜区。选对哲学的关键是**识别问题的离散结构有多重**。

多视角理解：第三条路——学习与采样¶

混合整数和互补松弛之外，2022 年后兴起了第三类思路，它们试图绕过两者的死穴：

采样式（sampling-based）：MuJoCo MPC（mjpc）用 Predictive Sampling——并行采样大量动作序列，用 rollout 评估，选最优。完全不解互补约束、不用整数变量，靠暴力采样让接触模式涌现。优点：无导数、不怕病态、易并行；缺点：维度高时采样效率低，无最优性保证。
学习加速：Sleiman 2021 用神经网络给 OCS2 做 warm-start；Chi 2023 的 Diffusion Policy 用扩散模型生成动作序列，在接触丰富任务上替代/辅助 CI-MPC。这些把"接触模式的选择"部分地交给学到的先验。
全局接触搜索：Pang-Suh-Tedrake 2023 用 sampling 在全局尺度搜索接触模式，再用局部 CI-TO 精化——混合了"全局采样 + 局部连续优化"。

系统性分类（R6E）：处理"连续优化中的离散接触决策"，本质上有四个维度的方法：(i) 显式整数枚举（MIP/MICP，全局最优但组合爆炸）；(ii) 互补连续松弛（MPCC，可扩展但病态且局部）；(iii) 无导数采样（Predictive Sampling，鲁棒但低效）；(iv) 学习先验（neural warm-start / diffusion，快但需数据且泛化存疑）。这四个维度可组合（如 iii+ii 的 Pang 2023、iv+ii 的 Sleiman 2021）。没有银弹——选择取决于问题规模、是否需全局最优、是否有数据、是否需实时。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为混合整数"更精确"所以总是更好

新手想法："混合整数显式枚举、保证全局最优，肯定比互补松弛的局部解强。" 现象/后果：在接触点多、时域长的全身轨迹问题上用 MICP，求解器跑几个小时也出不来结果，因为分支定界树指数膨胀。 根本原因：混合整数的"全局最优"是用**指数最坏复杂度**换来的。当二元变量数 $Nn_c$ 上百时，这个代价不可承受。 正确做法：用问题规模决定哲学。少数离散选择（落脚规划、跳上台阶）→ 混合整数（享受全局最优）；大量连续接触变量（全身操纵、多接触轨迹）→ 互补松弛（享受可扩展性）。不要被"全局最优"四个字诱惑而忽视其复杂度代价。

💡 概念误区：把 big-M 的 $M$ 当作物理上界来设

新手想法："$M$ 是接触力的上界，那我按机器人能产生的最大力来设。" 实际上：$M$ 的设置是一门数值艺术。$M$ 太大 → 线性规划松弛（LP relaxation）太松，分支定界要探索很多无效分支，求解慢；$M$ 太小 → 可能切掉真实的可行解（如果真实接触力超过 $M$）。$M$ 应设为"刚好覆盖物理可达范围"的紧上界，而非保守的大数。 为什么重要：松弛紧度直接决定 MIP 求解速度。一个好的 big-M（紧致、问题相关）可能让求解快几个数量级。这是 MICP 工程实现的核心调优点。

💡 概念误区：以为互补松弛和混合整数毫不相关

新手想法："一个是连续优化，一个是整数规划，两套完全不同的东西。" 实际上：互补约束 $0\le\phi\perp\lambda\ge0$ 与 big-M 编码的二元析取在**数学上等价**（都表达"$\phi=0$ 或 $\lambda=0$"）。互补松弛可以看作混合整数问题的一种连续松弛。这个联系是理解 GCS（Graphs of Convex Sets, Marcucci-Tedrake）等"中间地带"方法的基础。 正确思维：把两种哲学看成同一个析取逻辑的两种数学化身，而非孤立的两套技术。这样你能理解为什么有研究者努力在两者间架桥（GCS 用凸集图把组合结构暴露给凸优化，兼具部分全局性和可扩展性）。

练习¶

练习 6.5.1（编码练习，草稿纸上完成） 对一维落体接触地面（§6.2 练习 6.2.1 的系统），写出其单步接触的**混合整数 big-M 编码**（引入 $s\in\{0,1\}$）。然后与该系统的**互补约束编码**并列。验证：当 $s=0$ 和 $s=1$ 时，big-M 约束分别退化成什么；它们的并集是否恰好等于互补可行集的 L 形。

练习 6.5.2（复杂度估算，草稿纸上完成） 一个四足机器人（4 个接触点），规划 $N=30$ 步轨迹。(a) 混合整数方法有多少个二元变量？分支定界最坏要探索多少个叶节点？(b) 互补松弛方法的优化变量数量级是多少（连续变量，无整数）？(c) 用这两个数字论证：为什么这个规模的全身轨迹问题应该用互补松弛而非混合整数。再问：如果只规划落脚点（不规划全身轨迹），接触模式数大幅减少，结论会变吗？

练习 6.5.3（开放思考题） §6.5 给出了处理离散接触的四个维度（整数、互补、采样、学习）。请设计一个**混合策略**解决以下任务："四足机器人在崎岖地形上行走，大部分时间是已知的 trot 步态，但偶尔需要跨越意外障碍。" 提示：考虑"已知步态段用固定序列质心 MPC（快），检测到障碍时切换到互补松弛 CI-MPC（灵活），并用学习的先验给 warm-start（加速）"。论证你的混合策略如何利用每种方法的长处、规避其短处。这个练习直接预演 §6.7 的实时化思路。

§6.6 反面对照：固定序列轨迹优化 ⭐⭐¶

动机：理解接触隐式，必须理解它的反面¶

R5 的"反面"原则告诉我们：要真正理解一个方法，必须理解"不用它会怎样"。§6.1 已经从动机角度论证了固定序列的代价。本节从**方法论角度**系统梳理固定序列轨迹优化（fixed-schedule / mode-prescribed TO）的三条主流路线——它们不是"过时的旧方法"，而是与接触隐式互补、且在各自领域至今主导的工程主力。理解它们，才能在 §6.7 看清接触隐式 MPC 站在什么样的工程基线之上。

反面：如果没有固定序列方法会怎样¶

如果世界上只有接触隐式方法，那么连"让四足机器人平地小跑"这种接触序列显然的任务，你都得付出 MPCC 病态的代价去求解——杀鸡用牛刀，且这把牛刀还容易卷刃（不收敛）。固定序列方法的存在，让"已知步态"的任务能用又快又稳的凸优化解决，把宝贵的接触隐式火力留给真正需要它的未知交互任务。

理论：三条固定序列路线¶

路线一：DIRCON（Direct Collocation with Contact Constraints）。

Posa-Kuindersma-Tedrake（WAFR 2016）。在**固定的接触模式序列**下做直接配点（direct collocation），把接触力和接触约束作为**流形约束（manifold constraint）处理。关键贡献是三阶积分精度——配点法用多项式插值状态轨迹，在配点处强制满足动力学和接触约束。因为模式固定，接触约束是等式（holonomic，$\phi\equiv0$ 在接触段），**没有互补约束，MFCQ 正常成立，求解又快又稳。

DIRCON 与 Posa 自己的 CI-TO（2014）形成鲜明对照：同一个人，2014 年做"模式自动发现"的 CI-TO（病态），2016 年做"模式预先固定"的 DIRCON（良态）。这不是倒退，而是**针对不同任务谱系的两个工具**。

路线二：HZD / FROST（Hybrid Zero Dynamics）。

Hereid-Ames（IROS 2017，FROST 工具）。先用混合零动力学理论**设计**好接触序列和切换面，再用 NLP 生成满足 HZD 的步态。HZD 的特点是用 Poincaré 映射保证周期步态的**稳定性**——这是接触隐式方法所不提供的（CI-TO 给轨迹，不直接给稳定性证书）。

路线三：SRBD / 质心 MPC（Centroidal MPC）。

Di Carlo-Wensing-Katz-Bledt-Kim（MIT Cheetah 3, IROS 2018）；Winkler 等（TOWR, RA-L 2018）。把全身简化成单刚体（SRBD）或质心动力学（6 自由度 + 接触力），接触序列由**步态调度器（gait scheduler）外部给定，求解一个**凸 QP，跑到 1 kHz。这是当今足式机器人最广泛部署的方法——MIT Mini Cheetah、Unitree、ANYmal 的平地运动大量使用。

理论：CI-TO vs 固定序列的全维度对比¶

把接触隐式和固定序列做一次系统的、多维度的对比（这是 R6E 系统性分类，也是本章的核心判断表）：

维度	CI-TO（接触隐式）	固定序列 TO
是否需先验接触序列	否（自动发现）	是（人工/调度器给定）
数学对象	MPCC（互补约束）	光滑 NLP / 凸 QP
约束品性（CQ）	失效，需松弛（§6.3-§6.4）	正常，MFCQ 成立
NLP 规模	大（$\lambda$ 变量 + 互补约束）	小（接触段 holonomic 等式）
能否发现新行为	能（蹦跳、攀爬、借力）	不能（限于指定模式内）
稳定性证书	无（给轨迹不给证书）	HZD 路线有（Poincaré）
实时性	需 C3/IDTO/Fast-CI 等特化	质心 MPC 可达 1 kHz
工程成熟度	2022 后才上真机	部署多年，主流
典型任务	推门、攀爬、未知交互	平地小跑/行走（已知步态）

工程决策规则（一句话）：已知任务步态（trot/walk/stand）→ 固定序列；未知交互（推门、扶墙、跨越意外障碍）→ CI-TO。

本质洞察：CI-TO 和固定序列的关系，不是"先进 vs 落后"，而是"通用 vs 专用"。固定序列像专用集成电路（ASIC）——为特定步态高度优化，又快又省，但不灵活；CI-TO 像通用 CPU——什么接触都能处理，但慢且费电（病态）。成熟的机器人系统两者都用：日常步态跑 ASIC（质心 MPC），遇到未知交互时切到 CPU（CI-MPC）。§6.5 练习 6.5.3 设计的混合策略正是这个思想的具体化。

对比性思维：HZD 与 CI-TO 的方向之争¶

一个微妙但重要的关系（这是常见误解，本章末误解汇总会再提）：

不是 "HZD 步态是 CI-TO 的一个特例"（方向反了）；而是 "CI-TO 可以**收敛到** HZD-like 的周期步态，但 HZD 是事先用 Poincaré 分析导出的固定序列方法"。

方向很重要：HZD 是**自顶向下**——先用理论设计好切换面和零动力学，再生成步态；CI-TO 是**自底向上**——从头优化，让接触序列涌现，可能涌现出一个周期步态（碰巧 HZD-like），但这是优化的结果而非设计的前提。Esteban-Kurtz-Ghansah-Ames（2024）把 HLIP（HZD 的简化）与 CI-MPC 结合，在不平地形上跑双足——这正是"用 HZD 的稳定性结构 + CI 的灵活性"的融合尝试。

⚠️ 常见陷阱¶

💡 概念误区：以为 Crocoddyl 是接触隐式 MPC

新手想法："Crocoddyl 是多接触 DDP，处理接触，那它就是 CI-MPC 吧？" 实际上：Crocoddyl 的接触是 holonomic 约束（固定序列），不是 complementarity。它要求用户**预先指定**哪只脚在哪个时间段接触（contact schedule 是输入）。所以 Crocoddyl 不是 CI-MPC，它是高效的固定序列全身 DDP。要用 Crocoddyl 做接触隐式，必须外挂 IDTO 或 C3 这样的上层来决定接触序列。 为什么重要：这是框架选型的关键区分（§6.7 框架对比表会再列）。误以为 Crocoddyl 能自动发现接触，会导致选错工具、白费力气。

🧠 思维陷阱：把固定序列方法当成"低级"而轻视

新手想法："固定序列要人工指定步态，太原始了，研究就该做接触隐式。" 实际上：固定序列质心 MPC 是当今**绝大多数**真机足式系统的主力（MIT、ETH、Unitree 的平地运动）。它的 1 kHz 实时性、数值稳定性、成熟度是接触隐式至今难以企及的。研究接触隐式不等于轻视固定序列——恰恰相反，理解固定序列的工程优势，才知道接触隐式要付出什么代价去换灵活性。 正确思维：评价一个方法看它解决什么问题，而非看它"新不新"。固定序列解决"已知步态高效执行"，接触隐式解决"未知交互灵活规划"，二者都不可或缺。

练习¶

练习 6.6.1（对比分析，草稿纸上完成） 为以下五个任务各选择 CI-TO 或固定序列方法，并用一句话说明理由：(a) 四足机器人平地直线小跑；(b) 机器人手把一个未知形状的物体翻面；(c) 双足机器人按固定节奏上楼梯；(d) 四足机器人被人从侧面猛推后恢复平衡（可能需要意外的接触）；(e) 人形机器人扶着栏杆下坡。检验你的选择是否符合"已知步态 → 固定序列，未知交互 → CI-TO"的规则。

练习 6.6.2（开放思考题，草稿纸上完成） DIRCON（固定序列）和 CI-TO（接触隐式）出自同一作者 Posa。请思考：为什么一个人要同时发展两个看似矛盾的方法？这反映了机器人轨迹优化领域怎样的工程现实？（提示：没有银弹，工具箱里要有针对不同任务的多把工具。）

练习 6.6.3（跨节综合题） 综合 §6.5（混合整数 vs 互补）和 §6.6（固定序列 vs 接触隐式），画一张"接触决策方法选择决策树"：根节点问"接触序列是否已知？"，已知分支再问"是否需简化模型实时？"，未知分支再问"接触模式数是否少（可枚举）？"……一直细分到叶节点（具体方法：质心 MPC / DIRCON / MICP / C3 / IDTO / Fast-CI-MPC）。这棵树是你日后做接触问题选型的速查图。

§6.7 接触隐式 MPC 的实时化：bilevel、IDTO、C3 三条路 ⭐⭐⭐⭐¶

动机：从"离线规划一次"到"在线每 10 ms 重解一次"¶

前六节讲的 CI-TO 是**轨迹优化（trajectory optimization）——给定一个任务，离线求出一整段最优轨迹。但机器人控制需要的是**模型预测控制（Model Predictive Control, MPC）——在线地、每个控制周期（10–20 ms）重新求解一个 receding-horizon 优化，用最新状态反馈滚动更新。

把 CI-TO 升级成 CI-MPC（Contact-Implicit MPC），难度陡增一个量级。回顾 §6.3：CI-TO 已经因 MPCC 病态而难解，离线允许跑几秒甚至几分钟。但 MPC 要求**在 10 ms 内**解完一个含接触的优化——这在 2021 年前被普遍认为不可能。2022–2024 年，三条独立的技术路线几乎同时攻克了这个"不可能"：

Bilevel + 结构化分解（Le Cleac'h-Howell et al. 2024, Fast-CI-MPC）：把问题分成外层 Riccati、内层 LCP，对内层做 Taylor 近似和对偶解耦。
逆动力学变量消元（Kurtz-Castro-Önol-Lin 2023/2026, IDTO）：消掉速度、力矩、接触力变量，只优化位形序列。
ADMM 共识分解（Aydinoglu-Posa 2022/2024, C3）：把多接触问题分解成每个时间步独立并行的子问题。

本节逐一拆解这三条路的核心机制，并补充 HiLQR-MPC（saltation 进 Riccati）这条第四路。这是本章技术含量最高的一节——理解了这三条路，你就能读懂 2022 年后所有 CI-MPC 的 SOTA 论文。

反面：直接把 §6.4 的 continuation 拿来在线跑会怎样¶

最朴素的想法：CI-MPC 不就是每个周期跑一次 §6.4 的 Scholtes continuation 吗？

后果：continuation 是多阶段循环（每阶段一个完整 SQP），离线跑几秒，在线根本来不及。一个控制周期 10 ms，而 continuation 的外层循环动辄十几个阶段、每阶段几十次 SQP 迭代——总时间是秒级，慢了两三个数量级。实时 CI-MPC 的全部技巧，就在于如何避开这个多阶段 continuation，用结构 + warm-start 把单周期求解压进 10 ms。 三条路是三种不同的"压缩"策略。

共同前提：Warm-start 与实时迭代¶

在拆解三条路之前，先讲一个它们**共享**的关键武器——warm-start（热启动），以及它在 MPC 里的特殊形式。

MPC 每个周期解的优化问题，与上一周期**几乎相同**——只是时域向前滑动一格、初始状态更新为最新测量。所以上一周期的解是这一周期极好的初值。具体做法是 warm-start on shifted trajectory（平移热启动）：把上一次解的轨迹整体**向前平移一格**（丢掉第一步，末尾补一步），作为这一周期的初值。

回顾桥接（控制理论 §50 / 实时迭代）：Diehl 等（2005）的**实时迭代（Real-Time Iteration, RTI）思想——每个 MPC 周期**只做一次 SQP 迭代，不等收敛。因为相邻周期问题相似，单次迭代的解已足够好，且下一周期会继续改进。RTI 是所有实时 NLP-MPC 的基石。在 CI-MPC 里，warm-start + RTI 让"duality gap 已接近 0"，从而避开 §6.4 的多阶段 continuation。

本质洞察：实时 CI-MPC 之所以可能，根本上依赖一个事实——MPC 是在求解一串高度相似的优化问题，而非一个孤立的难问题。warm-start 把"每周期从零解一个病态 MPCC"变成"每周期对上一个解做一次小修正"。§6.3 的病态在"从零解"时致命，在"小修正"时被驯服（因为 warm-start 让迭代始终停留在远离双活点的良态区域，或贴着接触模式缓慢演化）。这是理解全部三条路的总钥匙。

路线一：Fast-CI-MPC 的 bilevel 结构（当前 SOTA）¶

Le Cleac'h-Howell-Yang-Lee-Zhang-Bishop-Schwager-Manchester（2024, IEEE T-RO 40:1617–1638, arXiv:2107.05616, DOI 10.1109/TRO.2024.3351554） 是当前 CI-MPC 的标杆论文，标题就叫《Fast Contact-Implicit Model-Predictive Control》。它的核心是 **bilevel（双层）结构**配合三把斧。

bilevel 结构的思想：把 CI-MPC 分成两层——

上层（upper level）：用**线性化动力学**做标准 LQR/Riccati 反馈。这一层是光滑的，用我们在控制理论 §50 学的 Riccati 递推高效求解。
下层（lower level）：用**时变线性互补问题（time-varying LCP）处理接触。关键创新：**LCP 不在线求解，而是预先在**参考轨迹周围做 Taylor 近似**，下层用定制的原始-对偶内点法（custom primal-dual interior-point）在 warm-start 下极快收敛。

为什么 Taylor 近似在这里合法：回顾 §6.3 的核心洞察——病态只发生在双活点（接触 make/break 瞬间）。MPC 在参考轨迹**附近**滚动，大部分时间远离双活点（接触稳定或稳定分离），此时接触关系近似光滑，Taylor 近似有效。Fast-CI-MPC 正是利用"在 reference 附近接触模式近似稳定"这一局部性，把病态的全局 MPCC 替换成一串局部良态的 LCP。

三把斧：

Bilevel 结构：upper level 线性化 + Riccati，lower level 时变 LCP + Taylor 近似。把"病态接触"隔离到下层，上层享受光滑 Riccati 的速度。
Structured dual decomposition（结构化对偶分解）：把接触的对偶变量从 Riccati 递推中**解耦**，避免 dense Hessian。这是关键——朴素地把接触约束塞进 Riccati 会让 Hessian 稠密、$O(N n^3)$ 爆炸；解耦后接触只在局部稀疏地修正 Riccati，计算量仅增 $O(n_c)$（$n_c$ = 接触数）。
Warm-start on shifted trajectory：每步把上次解平移一格，duality gap 已接近 0，单次迭代即收敛。

硬件结果：Unitree Go1 四足，被推、爬墙、把两只脚放上台阶——全部**在线发现接触模式**，跑在 CPU 上约 50–100 Hz。这是"接触隐式 + 实时 + 真机"首次同时实现的里程碑之一。配套开源代码 ContactImplicitMPC.jl（Julia）。

理论-工程桥接：Fast-CI-MPC 的 bilevel 结构，是 §6.4 末尾"两层结构（外层 continuation + 内层 SQP）"思想的**实时变体**。区别在于：§6.4 的两层是串行的 continuation（外层调参、内层重解，慢）；Fast-CI-MPC 的两层是嵌套的 bilevel（上层 Riccati、下层 LCP，每周期一次，快）。同一个"分层处理病态"的哲学，在离线和在线两种约束下长出两种不同的算法形态。

路线二：IDTO 的逆动力学变量消元¶

Kurtz-Castro-Önol-Lin（2023, arXiv:2309.01813；2026 IJRR 正式版, DOI 10.1177/02783649251344635） 提出 逆动力学轨迹优化（Inverse Dynamics Trajectory Optimization, IDTO），核心是一次**范式转变**——决策变量只保留广义坐标序列 $q_{0:N}$。

对比经典 forward CI-TO： - 经典：决策变量 $(q,v,\tau,\lambda)$（位形、速度、力矩、接触力），约束含前向动力学 $f_{\mathrm{fwd}}=0$ 加互补约束。 - IDTO：只有 $q$。速度 $v$、加速度通过有限差分从 $q$ 算出；接触力 $\lambda$ 通过 MuJoCo 的软接触（compliance-based smoothing） 得到 $\lambda(q,\dot q,\ddot q)$；力矩通过**逆动力学**算出：

\[\tau = M(q)\ddot q + C(q,\dot q)\dot q + g(q) - J_c(q)^\top\lambda.\]

目标函数是 $\|\tau_{\mathrm{unact}}\|^2 + \text{tracking}$——惩罚**欠驱动坐标上的残余力矩**（欠驱动坐标如浮动基座没有电机，残余力矩必须为零才物理可行），加上轨迹跟踪。

为什么逆动力学更适合实时 CI-MPC（这是 IDTO 的全部精髓）：

变量数少 3–4 倍：消掉了 $v,\tau,\lambda$，只剩 $q$。变量少 → Hessian 小 → 求解快。
Hessian 稀疏、带状（banded）：每个 $q_k$ 只通过有限差分耦合 $q_{k-1},q_{k+1}$，所以 Hessian 是三对角块带状结构。带状 Hessian 的线性求解是 $O(N)$ 而非 $O(N^3)$——这是实时性的数学保证。
不需显式解 LCP：互补约束被 MuJoCo 的软接触 smoothing **隐藏**了——$\lambda$ 是 $q,\dot q,\ddot q$ 的光滑函数，没有显式互补约束、没有 §6.3 的 CQ 失效。
Gauss-Newton + trust region 就足够：因为目标是最小二乘型（$\|\tau_{\mathrm{unact}}\|^2$），Gauss-Newton 近似 Hessian 很好，配合信赖域全局化即可，不需要复杂的内点法。

代价：在接触 make/break 的瞬间，逆动力学的 Jacobian 接近奇异——因为那一刻 $\dot q$ 不连续、$\ddot q$ 理论上是 Dirac。IDTO 通过 $\tau$ 正则化项和 soft-contact smoothing 绕过这个奇异（smoothing 把 Dirac 抹平成有限值），代价是接触瞬间的局部收敛变慢。

硬件结果：双臂 20-DoF 操纵任务 100+ Hz（TRI Drake 实现，开源 github.com/ToyotaResearchInstitute/idto）。

对比性思维（不是 X 而是 Y）：IDTO 不是 "另一种解 MPCC 的算法"，而是 "通过改变决策变量的选择，让 MPCC 不再以 MPCC 的面目出现"。它消掉 $\lambda$、用软接触光滑化，把互补约束彻底藏进了 MuJoCo 的接触模型里——表面上 IDTO 解的是一个光滑的最小二乘问题（无互补约束！），接触的复杂性被封装在 $\lambda(q,\dot q,\ddot q)$ 这个光滑函数内部。这是和 Fast-CI-MPC（显式保留 LCP、做 Taylor 近似）截然不同的哲学：一个**藏**，一个**近似**。

回顾桥接 §40 + §6.4：IDTO 的软接触 smoothing 正是 §40 可微接触和 §6.4 软接触光滑化的工程落地。还记得 §6.4 那条陷阱吗——"软接触刚度是数值参数不是物理量"？IDTO 的 MuJoCo soft contact 的 compliance 参数就是这样一个 continuation 旋钮，调度它从软到硬逼近刚性极限。IDTO 把 §6.4 的光滑化路线和带状 Hessian 的稀疏性结合，达到了 100+ Hz。

路线三：C3 的 ADMM 共识分解¶

Aydinoglu-Posa（2022, ICRA, DOI 10.1109/ICRA46639.2022.9811957）及期刊版 Aydinoglu-Wei-Huang-Posa（2024, T-RO, arXiv:2304.11259） 提出 共识互补控制（Consensus Complementarity Control, C3），用 ADMM（交替方向乘子法） 把多接触问题分解到每个时间步并行。

第一步：线性互补系统（LCS）局部线性化。把多接触动力学在参考轨迹周围做 Taylor 近似，得到**线性互补系统（Linear Complementarity System, LCS）**：

\[ \begin{cases} x_{k+1}=A x_k + B u_k + D\lambda_k + d,\\ 0\le \lambda_k\;\perp\;E x_k + F u_k + H\lambda_k + c \ge 0. \end{cases} \]

这里 $\lambda$ 同时代表法向力、摩擦力、切向速度绝对值的对偶变量。LCS 是 multi-contact 动力学的局部一阶近似（又是利用 §6.3 的"远离双活点近似有效"）。

第二步：写成 consensus 形式。引入两个副本——光滑副本 $z_k$ 和互补副本 $\delta_k$，约束它们一致 $z_k=\delta_k$：

\[\min_{z,\delta}\ \sum_k \ell_k(z_k) + \mathbb{I}_{\mathrm{LCS}}(\delta_k)\quad\text{s.t.}\ z_k=\delta_k.\]

$\ell_k$ 是光滑的运行代价（含动力学），$\mathbb{I}_{\mathrm{LCS}}$ 是 LCS 互补约束的示性函数（满足互补取 0，否则取 $+\infty$）。这个"复制 + 一致性约束"的技巧是 ADMM 的标准套路——把耦合的难问题拆成两个各自好解的子问题。

第三步：ADMM 交替迭代。augmented Lagrangian $L_\rho = \sum_k\ell(z_k) + \mathbb{I}_{\mathrm{LCS}}(\delta_k) + w_k^\top(z_k-\delta_k) + \frac{\rho}{2}\|z_k-\delta_k\|^2$，交替更新：

$z$-update：固定 $\delta,w$，解 $\min_z \sum_k\ell(z_k)+\frac\rho2\|z_k-\delta_k+w_k/\rho\|^2$。这是 $N$ 个**并行的 QP**（光滑部分，无互补，好解）。
$\delta$-update：固定 $z,w$，解 $\min_\delta \sum_k\mathbb{I}_{\mathrm{LCS}}(\delta_k)+\frac\rho2\|z_k-\delta_k+w_k/\rho\|^2$。因为 $\mathbb{I}_{\mathrm{LCS}}$ 在每个 $k$ 独立，这是 $N$ 个**并行的小 LCP/MIQP 投影**（互补部分，但每个只是一个时间步的小问题）。
$w$-update（对偶上升）：$w_k \leftarrow w_k + \rho(z_k-\delta_k)$。

关键性质——contact reasoning 被分解到每个时间步独立并行：这是 C3 的全部威力所在。朴素的 multi-contact MIQP 是一个巨大的耦合组合问题；C3 通过 ADMM 把它拆成 $N$ 个**互不耦合、可并行**的单步子问题。实时性随时间步数**近乎线性**扩展（而非 MIQP 的指数）。

性能：Aydinoglu 2022 在 cart-pole with contact 上达 100 Hz；Aydinoglu 2024 在机械臂 multi-contact 操纵任务上达 30+ Hz（比 MIQP baseline 快 2.5×）。

局限：ADMM 在非凸问题上**不保证全局收敛**（LCS 的 $\mathbb{I}_{\mathrm{LCS}}$ 非凸）。实践中 C3 收敛到"near-optimal"解，工程上够用。Wang-Yin-Zeng（2019）证明了 ADMM 在非凸可分问题（目标 prox-regular、约束 Lipschitz）下残差趋零（定理 T8），这是 C3 收敛分析的理论依据。

本质洞察：C3 的精髓是**"分而治之"应用于互补约束的时间结构**。互补约束 $\mathbb{I}_{\mathrm{LCS}}(\delta_k)$ 在时间上是**可分的**（每步独立），只有光滑代价 $\ell$ 和动力学把不同时间步耦合起来。ADMM 把这两类约束分到两个子问题：耦合的光滑部分（$z$-update，并行 QP）和可分的互补部分（$\delta$-update，并行单步 LCP）。这是 §6.5"互补 = 析取"洞察的算法兑现——单步 LCP 投影本质上是在解一个小的析取（每个接触点接触或分离），但因为是单步，规模极小、可并行。

对比性思维：Fast-CI-MPC 用 bilevel（纵向分层：上层 Riccati / 下层 LCP），C3 用 ADMM consensus（横向分时：$N$ 个并行单步子问题）。一个按"光滑 vs 接触"纵切，一个按"时间步"横切。两种分解都把"大而病态的耦合 MPCC"拆成"小而良态的子问题"，但切的方向正交——这体现了同一个困难可以有正交的分解策略。

路线四：Hybrid iLQR-MPC（saltation 进 Riccati）¶

Kong-Li-Council-Johnson（2023, IEEE T-RO 39(6):4712–4727, arXiv:2207.04591, DOI 10.1109/TRO.2023.3308773） 提出 混合 iLQR（Hybrid iLQR, HiLQR），把 iLQR 推广到含状态跳变的分段光滑混合系统。这条路与前三条的哲学不同——它**不光滑化、不松弛**，而是**显式建模混合跳变**。

核心机制：回顾 §50 的 saltation 矩阵。当系统在前向 rollout 中穿越 guard 面 $g(x)=0$（接触模式切换）时，HiLQR 在 backward pass 处乘以 saltation 矩阵 $\Xi$ 来更新梯度信息：

\[\Xi = \frac{\partial R}{\partial x} + \frac{(f^+ - R f^-)\nabla g^\top}{\nabla g^\top f^-},\]

其中 $R$ 是 reset map，$f^-,f^+$ 是切换前后的向量场（这是 §50 定理 T6）。三个关键设计：

forward pass 允许改变混合模式：rollout 时如果状态越过 guard，就切换到新模式继续——接触序列可以在迭代中改变。
backward pass 用 saltation 矩阵更新梯度：在 guard 穿越处，值函数的二阶信息 $V_{xx}$ 要乘以 $\Xi$ 修正——这让 Riccati 递推正确地传播跨模式的敏感度。
reference extension 处理模式不匹配：当实际模式与参考模式不一致时，延拓参考轨迹以对齐。

为什么计算量只增 $O(n_c)$：saltation 矩阵 $\Xi$ 只在 guard 穿越的少数时间步插入 $V_{xx}$ 的递推，每次是一个稀疏的秩修正（rank-update），不破坏 Riccati 的 $O(Nn^3)$ 主体。这与 Fast-CI-MPC 的 structured dual decomposition 异曲同工——都把接触的影响限制成对 Riccati 的稀疏局部修正。

对比软接触光滑化：Tassa 2012 的软接触把接触切换抹平成光滑（物理不真、$k$ 敏感）；HiLQR 不抹平，而是显式承认混合跳变、用 saltation 矩阵精确传播梯度。这在大扰动下（接触序列需要改变时）特别有用——HiLQR 能修改参考行为的接触序列、协调地规划全身运动。验证平台：Unitree A1。

回顾桥接 §50：HiLQR 是 §50 混合系统理论在 MPC 上最直接的应用。还记得 §50 的核心结论吗——"穿越 guard 时敏感度要乘 saltation 矩阵 $\Xi$，因为 reset map 的 Jacobian 不足以刻画切换面带来的额外敏感度"？HiLQR 把这个结论嵌进 iLQR 的 backward pass，从而在不光滑化的前提下做含接触的 MPC。这是"显式混合系统"路线与前三条"连续化/分解"路线的根本分野。

四条路的统一对比¶

把四条实时化路线做一次系统对比（R6E 系统性分类，本节核心总表）：

维度	Fast-CI-MPC	IDTO	C3	HiLQR-MPC
核心思想	bilevel + Taylor LCP	逆动力学消元	ADMM 共识分时	saltation 进 Riccati
怎么处理互补	下层 LCP Taylor 近似	软接触光滑化（藏起来）	单步 LCP 投影	显式混合跳变（不松弛）
决策变量	$(x,u,\lambda)$	仅 $q$	$(z,\delta)$ 双副本	$(x,u)$ + 模式
分解方向	纵向（光滑/接触分层）	变量消元	横向（时间步并行）	沿轨迹（guard 处修正）
求解器	定制原始-对偶 IPM	Gauss-Newton + 信赖域	并行 QP + LCP 投影	iLQR backward/forward
实时性	50–100 Hz（Go1，CPU）	100+ Hz（双臂，Drake）	30–100 Hz（机械臂/cart-pole）	真机（A1）
全局/局部	局部	局部	near-optimal（非凸 ADMM）	局部
开源	ContactImplicitMPC.jl	github.com/.../idto	dairlib/c3	—
适用	足式、在线发现模式	操纵、双臂	multi-contact 操纵	大扰动、需改接触序列

本质洞察：四条路看似迥异，却共享一个深层结构——它们都在"参考轨迹附近"工作，利用接触模式的局部稳定性，把全局病态的 MPCC 替换成局部良态的子问题。Fast-CI-MPC 用 Taylor 近似 LCP，IDTO 用软接触光滑，C3 用单步 LCS 线性化，HiLQR 用 saltation 处理局部跳变。"局部性 + warm-start"是实时 CI-MPC 的统一原理——它们都放弃了"一次求出全局最优含接触轨迹"的奢望，转而"在当前轨迹邻域快速做一次接触感知的修正，靠 MPC 的滚动反馈累积成全局行为"。这正是 §6.3 病态分析的工程回报：既然病态只在双活点、只在远离参考时严重，那就让 MPC 始终贴着参考滚动，永远不离开良态邻域。

多视角理解：CI-MPC 是"带接触的、在线的、被反复线性化的 LQR"¶

为了把 CI-MPC 挂到你最熟悉的控制理论知识树上（R2 跨章关联、R6A 多视角）：

回顾控制理论 §50 的 LQR——它对线性系统、二次代价给出闭式的 Riccati 反馈 $u=-Kx$。CI-MPC 可以理解成 LQR 的三重推广：

"带接触"：在 LQR 的光滑动力学上叠加接触约束（互补/LCS/saltation）。
"在线"：不是离线算一次 $K$，而是每个周期 receding-horizon 重解（MPC 的本质）。
"被反复线性化"：因为动力学非线性 + 接触非光滑，每个周期在当前轨迹附近线性化（iLQR/DDP 的本质），把 LQR 当内层子程序反复调用。

类比（标注边界）：CI-MPC 像"会随机应变的 LQR"。像的部分：核心都是 Riccati 反馈、值函数二次近似、$u=-Kx$ 的反馈律结构。不像的部分：LQR 的 $K$ 一劳永逸、全局有效；CI-MPC 的"$K$"每周期重算、只在当前邻域有效，且要处理接触带来的非光滑跳变（saltation）或互补约束。不要把这个类比延伸到"CI-MPC 有像 LQR 一样的闭式解或全局稳定性证书"——它没有，它的稳定性依赖 MPC 的递归可行性（§6.8 会提到定理 T9）和 warm-start 的成功。

⚠️ 常见陷阱¶

🧠 思维陷阱：以为实时 CI-MPC 是靠"更快的求解器"实现的

新手想法："2022 年能实时跑 CI-MPC，是不是因为 IPOPT 变快了 / CPU 变强了？" 实际上：不是。实时化靠的是**算法结构创新**——bilevel 分层、逆动力学消元、ADMM 分时、saltation 局部修正——配合 warm-start + 局部性。同一个 IPOPT、同一个 CPU，朴素地解全局 MPCC 仍然要几秒；用了这些结构才压到 10 ms。 正确思维：实时性的来源是"如何利用问题结构（局部性、可分性、稀疏性）把大病态问题拆成小良态问题"，而非"用更猛的通用求解器硬刚"。这是优化工程的核心智慧——结构 > 蛮力。

💡 概念误区：以为 warm-start 平移量可以随便对齐

新手想法："warm-start 就是把上次解平移一格，平移多少无所谓。" 现象/后果：MPC 外环的实际耗时与名义 $\Delta t$ 不一致时（比如这周期算了 15 ms 而名义 $\Delta t=10$ ms），若仍按一格平移，predicted 轨迹与 measured 状态会**漂移**，warm-start 质量下降，可能导致发散。 根本原因：平移量必须对应**实际经过的时间**，而非名义步长。实际 $\Delta t$ 波动时，shift 操作要按实际 $\Delta t$ 重采样。 正确做法：测量每周期实际耗时，按实际时间平移/插值 warm-start 轨迹，保证 predicted 与 measured 时间对齐。

💡 概念误区：把 C3 的 LCS 局部线性化当作全局有效

新手想法："C3 把动力学线性化成 LCS，那这个 LCS 在整个状态空间都能用吧？" 现象/后果：当机器人状态离开参考轨迹邻域时，LCS 的接触模式分类**错误**，C3 会把机器人推向错误方向（基于错误的接触预测）。 根本原因：LCS 只是参考轨迹**局部**的一阶 Taylor 近似（同 Fast-CI-MPC 的 LCP、IDTO 的线性化）。离开邻域，近似失效、接触模式判断错。 正确做法：LCS/Taylor 近似只在小邻域有效，必须靠 MPC 的高频滚动反馈把状态拉回参考附近；若任务需要大范围探索接触（如从未见过的抓取），需与 sampling-based 全局接触搜索（Pang-Suh-Tedrake 2023）结合，由全局搜索提供参考，C3/Fast-CI 做局部跟踪。

🧠 思维陷阱：把四条路当成互相竞争、必须选一个"最好"的

新手想法："Fast-CI-MPC 是 SOTA，那我只学它，其他三条不用管。" 实际上：四条路适用不同问题（见统一对比表）——足式在线发现模式用 Fast-CI-MPC，双臂操纵用 IDTO，multi-contact 操纵用 C3，大扰动需改接触序列用 HiLQR。它们是工具箱里的不同工具，"最好"取决于任务。 正确思维：理解每条路的核心机制（藏/近似/分时/显式跳变）和适用边界，遇到任务时按结构匹配，而非盲目追 SOTA。这与 §6.5、§6.6 的选型智慧一脉相承。

练习¶

练习 6.7.1（机制推导，草稿纸上完成） 解释为什么 Fast-CI-MPC 在 backward pass 用 saltation 矩阵时计算量仅增加 $O(n_c)$（$n_c$ = 接触数）。提示：saltation $\Xi$ 只在 guard 穿越时乘入 $V_{xx}$；Fast-CI-MPC 把 LCP 的 active set 在 reference 附近预处理，每个时间步仅需一次稀疏矩阵-向量乘。把这个论证与 §50 的 saltation 矩阵公式和控制理论 §50 的 Riccati 递推复杂度结合起来写。

练习 6.7.2（ADMM 分解推导，草稿纸上完成） 把 $N$-horizon 的 LCS-MPC 写成 C3 的 consensus 形式，完整给出 $z$-update、$\delta$-update、$w$-update 三步。提示：引入光滑副本 $z_k$ 和互补副本 $\delta_k$，约束 $z_k=\delta_k$；写出 augmented Lagrangian $L_\rho$；论证 $z$-update 是 $N$ 个并行 QP（因为 $\ell$ 是光滑的二次型加动力学线性约束），$\delta$-update 是 $N$ 个并行 LCP 投影（因为 $\mathbb{I}_{\mathrm{LCS}}$ 在每个 $k$ 独立可分）。

练习 6.7.3（范式对比，草稿纸上完成） IDTO 消掉了 $\lambda$，C3 显式保留 $\lambda$ 并用 ADMM 分解，Fast-CI-MPC 显式保留 $\lambda$ 但用 bilevel + Taylor。请用一张表对比这三种处理 $\lambda$（接触力）的策略，列出各自：(a) 互补约束去哪了；(b) 主要计算瓶颈；(c) 接触 make/break 瞬间如何处理；(d) 最适合的任务类型。这道题检验你是否真正理解了三条路的本质差异（提示：呼应 §6.2 练习 6.2.3 你对 IDTO 的预测）。

练习 6.7.4（跨章综合题，难） 综合控制理论 §50（LQR/Riccati）、§50 混合系统（saltation）、本章 §6.3（病态）、§6.7（实时化），向一个只懂 LQR、没接触过机器人接触问题的控制专业研究生，用一页纸讲清楚："为什么 CI-MPC 可以理解成带接触的在线 LQR，它比普通 LQR 多了哪三重困难，工程上用什么结构（bilevel/消元/ADMM/saltation）化解这些困难。" 要求把 LQR 的 Riccati 递推作为出发点，自然过渡到 CI-MPC。

§6.8 数值困难的系统诊断：病态、奇异、局部极小的工程对策 ⭐⭐⭐¶

动机：从"知道为什么难"到"知道难在哪、怎么修"¶

到 §6.7 为止，我们已经把 CI-TO/CI-MPC 的理论骨架搭完了：动机（§6.1）、形式化（§6.2）、病态根源（§6.3）、松弛救援（§6.4）、整数对照（§6.5）、固定序列反面（§6.6）、实时化四条路（§6.7）。但当你真正坐在屏幕前，把一个接触轨迹优化问题喂给求解器，看着它吐出 restoration failed、max iterations reached、或者一条明显穿模的轨迹时，前面的理论并不会自动告诉你该改哪一行。

本节是全章唯一一节**面向"调试现场"的内容。它把 §6.3–§6.7 散落的诊断线索收拢成一套**系统的故障定位流程——不是再讲一遍"为什么难"，而是回答工程师的三个实操问题：

我现在遇到的失败，到底是哪一类困难？（病态？奇异？局部极小？尺度问题？）——因为不同困难的修法完全不同，误诊会越改越糟。
怎么用可观测的信号把它们区分开？——求解器吐出的乘子范数、步长、KKT 残差、互补残差，每一个都是诊断的"化验单"。
确诊之后，对应的修法是什么？——而且要知道每个修法治的是哪一类病、会有什么副作用。

本质洞察（先给结论）：CI-TO 的数值失败可以归结为**四类独立的病因**——(A) 约束品性失效（CQ-病态，§6.3 已证）、(B) 雅可比奇异（make/break 瞬间的退化）、(C) 非凸局部极小（接触模式被困在次优分配）、(D) 尺度与条件数失衡（数值而非结构问题）。它们的**症状会互相伪装**（都表现为"不收敛"），但**根因和修法正交**。本节的核心任务，就是教你把这四类病因从一团乱麻的"不收敛"里**解耦**出来。这种"先分类病因再对症下药"的诊断思维，本身就是 R6E 系统性分类在工程调试上的应用。

反面：如果不做系统诊断，会陷入什么困境¶

设想一个没有系统诊断框架的工程师，遇到 CI-TO 不收敛时的典型"乱试"轨迹：

看到不收敛 → "可能是松弛参数不对" → 调 $t_0$、调 $\beta$ → 偶尔好一点，但换个初值又崩 → 以为是 continuation 调度问题。
再不收敛 → "可能是 IPOPT 不行" → 换 SNOPT → 换 Knitro → 都不行 → 以为是求解器问题。
再不收敛 → "可能是初值差" → 随机重启几十次 → 偶尔撞上一个解 → 以为是初始化问题。

这条乱试轨迹的根本错误：它把四类正交的病因当成"同一个调参问题"，在参数空间里盲目搜索。如果真实病因是 (C) 局部极小（接触序列被困），那么无论怎么调松弛参数、换求解器、重随机，都治不好——因为问题不在数值层、不在求解器、不在初值精度，而在**优化景观的非凸结构**，需要的是 §6.5 的整数枚举或 §6.7 的 sampling 暖启动来跳出盆地。误诊导致的不是"慢一点"，而是"方向完全错，永远修不好"。

这就是为什么本节值得单独存在：诊断框架的价值不在于让你少调几次参数，而在于让你不要在错误的维度上浪费时间。

理论：四类病因的精确刻画与鉴别诊断¶

我们逐一精确刻画四类病因，并给出**可观测的鉴别信号**。

病因 A：约束品性失效（CQ-病态）。 这是 §6.3 定理 T1 已经严格证明的——互补约束在双活点使 MFCQ 必然失效。

数学特征：等式约束 $\phi\lambda=0$ 的梯度在双活点退化为零向量，乘子集合无界，KKT 系统在双活点附近无界条件数。
可观测信号：对偶变量（乘子）范数随迭代**单调爆炸**（$\to 10^{6}$ 以上甚至 inf），SQP 步长**单调趋零**，求解器报 restoration failure（IPOPT）或 condition number too large（部分 QP 子求解器）。
触发条件：接触序列在轨迹中**频繁切换**——每次 make/break 都经过一个双活点。蹦跳、攀爬、快速 gait 切换最易触发。
修法：§6.4 的松弛/光滑/罚（把尖角磨圆，恢复 CQ）。这是唯一治本的修法，换求解器、改尺度都无效。

病因 B：雅可比奇异（退化但 CQ 不一定失效）。 这与 A 不同——A 是约束几何的尖角，B 是动力学雅可比本身在 make/break 瞬间的退化。

回顾 §6.7 IDTO 的代价分析：在接触 make/break 瞬间，逆动力学的雅可比 $\partial\tau/\partial q$ 接近奇异，因为那一刻 $\dot q$ 不连续、$\ddot q$ 理论上是 Dirac。更一般地，接触雅可比 $J_c(q)$ 在脚刚触地的瞬间，其对应法向行从"无效"（脚在空中，该行不参与动力学）突变为"有效"（脚着地，该行进入约束），这个突变让 $J_c$ 的有效秩跳变。

数学特征：动力学约束雅可比 $\partial h_k/\partial(q,v,\lambda)$ 的最小奇异值在 make/break 瞬间趋零，导致 Newton 系统的系数矩阵接近奇异（注意：这不是 CQ 失效，而是约束函数本身的雅可比退化）。
可观测信号：迭代在**特定时间步**（恰好是接触切换的那一步）卡住，该步的原始残差（primal residual）下降停滞，而其他时间步正常。条件数估计在该步局部飙升。
修法：(1) 雅可比正则化——给 Newton 系统加 $\delta I$（Levenberg-Marquardt 阻尼）；(2) 软接触光滑化（§6.4）把 Dirac 抹成有限值，IDTO 正是靠这个绕过奇异；(3) saltation 矩阵（§6.7 HiLQR）——不抹平而是精确传播跨切换的敏感度。

一个具体的秩跳变示范（理解病因 B 的退化机理）：以单脚着地为例，看接触雅可比的有效秩如何在 touchdown 瞬间跳变。设脚端位置由位形 $q$ 通过正运动学 $p(q)$ 给出，接触雅可比 $J_c=\partial p/\partial q$。考虑法向方向的接触约束行 $J_n=\partial\phi/\partial q$（间隙距离对位形的梯度，即脚端法向速度方向的映射行）。

脚在空中（$\phi>0$）：互补约束强制 $\lambda_n=0$，法向接触力不参与动力学。此时动力学约束 $h_k$ 不含 $\lambda_n$ 的有效耦合——$\partial h_k/\partial\lambda_n$ 这一列对结果无影响（因 $\lambda_n\equiv0$），相当于该列从有效雅可比里"消失"。有效约束雅可比少一行/列，秩为 $r$。
脚刚着地（$\phi=0,\lambda_n>0$）：法向力突然进入动力学，$\partial h_k/\partial\lambda_n=-J_n^\top$ 这一列**突然变为有效**，约束雅可比的有效秩从 $r$ 跳到 $r+1$。

问题就出在跳变的那一瞬间（双活点 $\phi=0,\lambda_n=0$）：法向力"半进半出"——$\lambda_n$ 名义上可以非零（脚已触地）但实际还是零（还没受力），$\partial h_k/\partial\lambda_n$ 这一列处于"将激活未激活"的临界。此刻把 $\partial h_k/\partial\lambda_n$ 算进雅可比，它对应的奇异值 $\sigma_{\min}\to0$（因为该列的"有效性"趋于零），Newton 系统的系数矩阵接近奇异，Newton 步 $\Delta z=-J^{-1}r$ 因 $J$ 近奇异而被放大到病态。这就是为什么求解器**恰好在 touchdown 那一步**卡住——那一步的雅可比正经历秩跳变。

修法的机理对应：(1) 加 $\delta I$ 阻尼，把 $\sigma_{\min}$ 从 $\approx0$ 抬到 $\ge\sqrt\delta$，Newton 系统重新可逆；(2) 软接触把 $\lambda_n=k\max(0,-\phi)$ 变成 $\phi$ 的光滑函数，$\partial\lambda_n/\partial\phi$ 在临界处是有限的 $k$（而非 Dirac），秩跳变被抹成平滑过渡，$\sigma_{\min}$ 不再趋零。理解了"秩跳变"这个机理，就明白为什么这两个修法都有效——它们都在用不同方式**阻止 $\sigma_{\min}$ 趋零**。

对比性思维：病因 A 与病因 B 的精确区分。两者都发生在 make/break 瞬间、都让求解器卡住，极易混淆。关键鉴别：A 是**约束可行集的几何尖角**（互补约束 $\phi\lambda=0$ 的 L 形），表现为**乘子爆炸**（对偶侧病态）；B 是**约束函数雅可比的秩退化**（动力学 $h_k$ 的雅可比），表现为**原始残差停滞**（原始侧病态）。一句话：A 病在对偶、B 病在原始。修法也因此不同——A 必须磨圆互补尖角（改约束几何），B 可以靠雅可比正则化（改数值条件）。把 B 误当 A 去加更激进的互补松弛，治不好原始侧的秩退化；把 A 误当 B 去加 Newton 阻尼，压不住对偶侧的乘子爆炸。

病因 C：非凸局部极小（接触模式被困）。 这是和 A、B 完全不同维度的困难——前两者是"求不出局部解"，C 是"求出了局部解但它是次优的"。

互补可行集非凸（L 形并集，§6.1），且每一种接触模式分配对应优化景观里的一个"盆地"。优化器从某个初值出发，只能收敛到当前盆地的局部最优——如果初值落在"错误接触序列"的盆地里，求解器会顺利收敛（无报错！）到一个**次优甚至物理上荒谬**的解（如"机器人用不该接触的部位发力"）。

数学特征：目标景观存在多个局部极小，每个对应一种接触模式分配；松弛/光滑只能恢复 CQ（治 A），不能改变景观的多盆地结构（治不了 C）。
可观测信号：求解器**顺利收敛、无报错**，但目标值明显高于预期，或轨迹在物理上不合理（穿模残差小但接触序列怪异）。换初值会收敛到不同目标值的解——这是局部极小的判据（凸问题换初值收敛到同一解）。
修法：(1) §6.5 的混合整数枚举（全局最优但贵）；(2) §6.7 的 sampling-based 全局接触搜索（Pang-Suh-Tedrake 2023）提供好的初值/参考；(3) 多初值重启（multi-start）取最好；(4) 课程式 continuation——从软接触（景观平滑、盆地少）逐步变硬（盆地变多），让解"骑着"平滑景观滑到好盆地，再硬化。

本质洞察：松弛治 A 不治 C，这是最容易踩的认知陷阱。§6.4 的 Scholtes 松弛把尖角磨圆，恢复了 CQ（治 A），让求解器**能**收敛。但它**完全不改变**景观的多盆地结构——磨圆后的景观仍然是非凸的、仍然有多个局部极小（治不了 C）。这就是为什么"加了松弛还是得到怪轨迹"——松弛让你能收敛，但不保证收敛到**对的**接触序列。要治 C，必须从**初值/全局搜索**层面下手，而非松弛层面。把 A 和 C 混为一谈（"不收敛就加松弛"），会在 C 类问题上徒劳无功。

为什么课程式 continuation（从软到硬）能跳出坏盆地——一个景观演化的视角。上面修法 (4) 提到"从软接触逐步硬化"，这里讲清它治 C 的机理，因为它和"松弛治 A"看似都用 continuation，实则治的是不同的病。

关键在于：软接触的优化景观比硬接触的"盆地更少、更平滑"。回顾 §6.4——软接触把刚性互补 $\phi\lambda=0$ 换成有限刚度 $\lambda=k\max(0,-\phi)$ 的光滑关系。当刚度 $k$ 很小（很软）时，接触"模糊"——脚靠近地面就开始有渐增的力，没有"接触/不接触"的硬切换，于是**不同接触模式之间的盆地壁垒被抹平**，景观近似单峰（盆地少）。随 $k\uparrow$（变硬），接触切换变锐利，盆地壁垒重新长出来、盆地数目增多，最终在 $k\to\infty$（刚性）时恢复原始的多盆地景观。

这给出一条"骑着平滑景观滑进好盆地"的策略：

从很软的接触（$k$ 小）开始：景观近似单峰，无论初值在哪，优化器都滑向那个唯一（或少数几个）的全局盆地——这一步跳过了坏盆地，因为坏盆地此时还不存在。
逐步增大 $k$（变硬）：每硬化一档，用上一档的解 warm-start。因为景观是**连续演化**的，上一档的解通常落在新景观对应盆地的吸引域内——解"骑着"缓慢分裂的景观，被带进**对的**盆地。
直到 $k\to$ 刚性：恢复物理真实的刚性接触，此时解已经在好盆地里，收敛到对的接触序列。

对比性思维（两种 continuation 治两种病）：§6.4 的 Scholtes continuation（$t_k\downarrow0$）和这里的软硬 continuation（$k\uparrow$）形似而神不同。Scholtes 的 $t_k\downarrow0$ 治**病因 A**——它磨圆尖角恢复 CQ，目的是让求解器在**固定盆地内**能收敛；它不改变盆地结构（同一接触序列）。软硬 continuation 的 $k\uparrow$ 治**病因 C**——它通过"软景观盆地少 → 硬景观盆地多"的演化，把解**跨盆地**地引导到好的接触序列。一个在盆地**内**救收敛（A），一个在盆地**间**找正确归属（C）。误把软硬 continuation 当成"又一种松弛"（以为也是治 A），就会困惑"为什么 Scholtes 松弛了还要再做软硬调度"——答案是它们治的是正交的两类病。

病因 D：尺度与条件数失衡（纯数值，非结构）。 这是最"无辜"也最常被忽略的一类——它和接触没有本质关系，纯粹是变量/约束量纲不匹配导致的数值病态。

CI-TO 的决策变量横跨多个量纲：位形 $q$（米/弧度，$O(1)$）、速度 $v$（米/秒，$O(1$–$10)$）、力矩 $\tau$（牛米，$O(10$–$100)$）、接触力 $\lambda$（牛，$O(100$–$1000)$）。如果不做缩放，Hessian/雅可比的条件数会因量纲差异天然恶化几个数量级——即使没有任何接触病态（A/B/C），纯数值条件数也会让求解器步履蹒跚。

数学特征：KKT 系统条件数 $\kappa$ 因变量/约束量纲跨度而 $\gg 1$，但**与接触模式无关**（关掉接触约束、纯光滑问题也病态）。
可观测信号：求解器**全程**（不只 make/break 瞬间）收敛缓慢，条件数估计**始终**偏高；做变量缩放后立刻显著改善。关键鉴别：把问题改成无接触版本（去掉互补约束）若仍慢，则病因在 D（尺度），不在 A/B/C（接触）。
修法：变量缩放（把 $q,v,\tau,\lambda$ 各自归一化到 $O(1)$）、约束缩放（每条约束除以其典型尺度）、单位制选择（用 SI vs 缩放单位）。这是优化建模 ABC，但在 CI-TO 里因变量量纲跨度大而格外重要。

一个定量的尺度失衡示范（理解病因 D 的数值机理）：为什么量纲跨度会恶化条件数？让我们用一个最简的两变量例子算清楚。设决策向量 $z=(q,\lambda)$，其中 $q$ 是位形（典型尺度 $1$ m）、$\lambda$ 是接触力（典型尺度 $10^3$ N）。假设目标的 Hessian 在物理量纲下是良态的单位阵 $H=\mathrm{diag}(1,1)$，但因为我们用 SI 单位、变量数值差 $10^3$ 倍，实际数值化后的 Hessian 会被变量的自然尺度"拉伸"。

具体地，若把目标写成对**无量纲化变量** $\tilde z=(q/1,\lambda/10^3)$ 的良态二次型，再换回原始变量 $z$，则原始变量下的 Hessian 变成

\[H_z = S^\top \tilde H\, S,\qquad S=\mathrm{diag}\!\Big(1,\ \tfrac{1}{10^3}\Big),\]

其中 $S$ 是从原始变量到无量纲变量的缩放矩阵。代入 $\tilde H=I$ 得 $H_z=\mathrm{diag}(1,10^{-6})$。它的条件数

\[\kappa(H_z)=\frac{\lambda_{\max}}{\lambda_{\min}}=\frac{1}{10^{-6}}=10^{6}.\]

也就是说，仅仅因为接触力的数值比位形大 $10^3$ 倍，条件数就天然恶化到 $10^6$——即使问题在物理上完全良态、没有任何接触病态（A/B/C）。Newton/内点法的收敛速度对条件数敏感（共轭梯度的迭代数 $\sim\sqrt{\kappa}$），$\kappa=10^6$ 意味着求解器要多花约 $\sqrt{10^6}=10^3$ 倍的内层迭代。

修法的定量效果：做变量缩放 $z\mapsto S z$（即用 $\tilde z$ 作决策变量），Hessian 变回 $\tilde H=I$，$\kappa=1$，病态消失。这就是为什么"变量缩放"不是可有可无的调参，而是**把 $10^6$ 的条件数压回 $1$** 的关键操作。CI-TO 因为同时含位形（$O(1)$）、速度（$O(10)$）、力矩（$O(10^2)$）、接触力（$O(10^3)$），不缩放时条件数可轻易到 $10^6$–$10^{12}$，这正是场景四"全程缓慢"的数值根源。

本质洞察：病因 D 的可怕之处在于它**伪装成接触病态**——同样表现为"求解器很慢"，让人误以为是互补约束的锅（病因 A）。但它的根源纯粹是线性代数的：条件数 $\kappa=(\text{最大尺度}/\text{最小尺度})^2$ 随变量量纲跨度平方增长。"去掉接触约束若仍慢则是 D" 这个鉴别动作之所以有效，正是因为 D 与接触无关——纯光滑版本的 Hessian 仍是 $\mathrm{diag}(1,10^{-6})$，仍然病态。这个定量例子也解释了为什么 §6.7 那些实时实现（Fast-CI-MPC、IDTO）都把变量缩放当作"出厂默认"——在 10 ms 预算里，没人能承受 $10^3$ 倍的多余迭代。

四类病因的鉴别诊断总表¶

把四类病因的鉴别信号汇成一张"化验单对照表"（R6E 系统分类，本节核心总表，调试现场直接查）：

病因	本质	病在哪侧	可观测信号	关键鉴别动作	对症修法	治标/治本
A CQ-病态	互补尖角（几何）	对偶	乘子爆炸 + 步长趋零 + restoration failure	看乘子范数是否随迭代单调爆炸	§6.4 松弛/光滑/罚	治本（唯一）
B 雅可比奇异	动力学雅可比秩退化	原始	特定（切换）时间步 primal 残差停滞	看是否只在 make/break 步卡住	雅可比正则化 / 软接触 / saltation	治本
C 局部极小	非凸多盆地（接触序列错）	景观	顺利收敛但目标偏高/轨迹荒谬；换初值变解	换初值看是否收敛到不同解	整数枚举 / sampling 暖启动 / multi-start / 课程式硬化	治本
D 尺度失衡	量纲跨度大（纯数值）	数值条件	全程慢、条件数始终高，与接触无关	去掉接触约束若仍慢则是 D	变量/约束缩放	治本

对比性思维（四类病因的正交性）：这四类病因**互相正交**——可以单独出现，也可以叠加。一个真实的 CI-TO 失败，可能同时有 A（频繁切换）+ D（没缩放）；修好 D（缩放）后求解器快了，但 A（乘子爆炸）依旧，还得上松弛；松弛后能收敛了，却发现是 C（怪接触序列），还得换初值。诊断的精髓是逐层剥离：先排除最容易的 D（缩放一下试试），再看是 A（乘子爆炸→松弛）还是 B（特定步停滞→正则化），最后查 C（换初值验证全局性）。这个剥离顺序不是随意的——D 最廉价（改几行缩放）、最不涉及结构，应当最先排除；C 最昂贵（涉及全局搜索）、最难，应当最后处理。

理论：实时 CI-MPC 特有的困难——递归可行性与求解超时¶

前面四类病因是 CI-TO（离线）和 CI-MPC（在线）共有的。但实时 CI-MPC 还有**两类离线优化没有的、由"在线滚动 + 硬实时"带来的特有困难**，这正是 §6.7 末尾给类比标注边界时埋下的伏笔（"它的稳定性依赖 MPC 的递归可行性"）。

特有困难一：递归可行性（recursive feasibility）。 离线 CI-TO 只需解出一条可行轨迹即可；但 CI-MPC 每个周期解一个**有限时域**子问题，并只执行第一步。一个致命的问题是：这一周期可行，不保证下一周期还可行——执行了第一步、时域向前滚动一格后，新的初始状态可能让子问题变得不可行（infeasible）。一旦某周期 infeasible，MPC 就"没有控制可输出"，系统失控。

我们把这个性质形式化为定理（这是 §6.7 末尾承诺的 T9）：

定理（CI-MPC 递归可行性的充分条件，T9）：设 CI-MPC 在每个周期求解时域为 $N$ 的子问题，可行集为 $\mathcal{F}_k$（依赖当前状态 $x_k$）。若满足：

存在控制不变终端集（control invariant terminal set） $\mathcal{X}_f$：对任意 $x\in\mathcal{X}_f$，存在控制 $u$（含一个可行接触模式）使下一状态 $f(x,u,\lambda)\in\mathcal{X}_f$ 且满足互补约束；
终端约束 $x_{k+N}\in\mathcal{X}_f$ 被纳入每周期子问题；
当前周期可行（初始 $x_0$ 使子问题有解）；

则 CI-MPC 递归可行——每个后续周期的子问题都有解。

证明思路（构造性）。 假设第 $k$ 周期可行，得最优解 $(x^*_{k:k+N}, u^*_{k:k+N-1}, \lambda^*)$，其中 $x^*_{k+N}\in\mathcal{X}_f$（终端约束）。执行第一步 $u^*_k$，系统到达 $x_{k+1}=x^*_{k+1}$。现在为第 $k+1$ 周期构造一个**可行**（不必最优）解：取**移位后的尾段** $(x^*_{k+1:k+N}, u^*_{k+1:k+N-1})$ 作为前 $N-1$ 步——这段由上一周期的最优性自动满足所有动力学和互补约束。问题只剩**最后一步**：从 $x^*_{k+N}\in\mathcal{X}_f$ 再走一步到 $x_{k+N+1}$。由条件 1（终端集控制不变），存在控制 $u$ 和可行接触模式 $\lambda$ 使 $x_{k+N+1}\in\mathcal{X}_f$ 且满足互补约束。把这一步补在尾部，就构造出第 $k+1$ 周期的一个**可行解**。故子问题有解，递归可行性成立。证毕。

本质洞察：递归可行性的关键，是**终端集承担了"无限时域可行性"的责任**——有限时域看不到无穷远处，但只要把状态约束到一个"自我维持"的不变集里，就等于给后面无穷长的时间买了保险。这个思想完全继承自标准（光滑）MPC 的递归可行性理论（Mayne 等），CI-MPC 的特殊性仅在于：终端集必须包含一个"可行接触模式"——也就是说，终端集里的每个状态都必须有一个满足互补约束的可行接触配置。对足式机器人，最自然的终端集就是"双脚着地的静态可行站立姿态集"——它显然控制不变（站着不动就一直在集合里）、且有可行接触（双脚法向力支撑体重）。这就是为什么很多足式 CI-MPC 把"末端回到可站立姿态"作为终端约束。

为什么"含可行接触模式"不可省——一个反例。条件 1 里"含一个可行接触模式"这几个字，是 CI-MPC 区别于光滑 MPC 的关键，也最容易被忽略。我们用一个反例说明：一个控制不变、但不含可行接触模式的终端集，会破坏递归可行性。

设想一个双足机器人，终端集错误地选成"双脚都悬空、质心以某速度平抛的飞行相位状态集" $\mathcal{X}_f^{\text{bad}}$。这个集合在**纯运动学**意义上似乎"控制不变"——飞行中机器人确实可以靠摆腿维持在某个飞行姿态流形上。但它**不含可行接触模式**：飞行相位没有任何脚着地，法向力 $\lambda_n=0$，无法产生支撑体重的接触力。

现在看递归可行性的构造性证明在哪一步断裂：当上一周期把末端状态停在 $x^*_{k+N}\in\mathcal{X}_f^{\text{bad}}$（一个飞行状态），下一周期要"从这个飞行状态再走一步并保持在 $\mathcal{X}_f^{\text{bad}}$ 里"。但飞行状态在重力下必然下落——要维持飞行（不落地），需要一个向上的力；唯一能提供向上力的是**接触力**（脚蹬地），而飞行相位**没有接触**（$\phi>0$，互补约束强制 $\lambda_n=0$）。于是**不存在**满足互补约束的控制使下一状态仍在 $\mathcal{X}_f^{\text{bad}}$——构造证明的"补最后一步"失败，递归可行性断裂。机器人会在这个终端集下"规划出一个永远悬空的末态"，物理上立刻坠落。

对比性思维（好终端集 vs 坏终端集）：好终端集"可站立姿态集"和坏终端集"飞行姿态集"的差别，不在控制不变性（两者在各自意义下都像是不变的），而在是否含可行接触模式。可站立姿态集里每个状态都有"双脚着地、法向力支撑体重"这个可行接触配置（互补约束 $\phi=0,\lambda_n>0$ 可满足），所以能"靠接触力自我维持"；飞行姿态集里每个状态的互补约束强制 $\lambda_n=0$，无法对抗重力，无法自我维持。这就是为什么定理 T9 的条件 1 必须显式写"含可行接触模式"——光有控制不变性（光滑 MPC 的要求）对 CI-MPC 不够，因为 CI-MPC 的"控制"要通过互补约束允许的接触力才能起作用。这个反例精确地暴露了 CI-MPC 递归可行性比光滑 MPC 多出来的那一层要求。

特有困难二：求解超时与次优执行。 离线优化解到收敛为止；CI-MPC 在硬实时约束下，控制周期到了就必须输出，哪怕没解到最优。这带来两难：解得不够会输出次优甚至不可行控制，解得太久会错过控制周期、用上一周期的旧解（更危险）。

实时 CI-MPC 通过两个机制应对：(1) 实时迭代（RTI）——只做固定次数（通常 1 次）SQP 迭代就输出，靠 warm-start 的连续性保证单次迭代的解"足够好"（§6.7 共同前提）；(2) 次优但稳定的保证——理论上，只要 warm-start 提供的初始可行解的代价**不增**（每周期至少不变坏），即使不解到最优，闭环仍可被证明稳定（这与定理 T9 配合：T9 保证可行性，代价不增保证稳定性，二者合起来给出"次优 MPC 仍稳定"）。

"代价不增"为什么成立——把它和 T9 的构造连起来：这个看似额外的假设，其实和定理 T9 的证明用的是**同一个移位构造**。T9 证明里，第 $k+1$ 周期的可行解是"上一周期解的移位尾段 + 终端集补一步"。这个移位可行解的代价，等于上一周期最优代价**减去已执行第一步的运行代价、加上新补那一步的代价**。在终端代价是合适的"控制 Lyapunov 函数"（终端代价 $\ge$ 终端集内一步的运行代价）这一标准假设下，可以证明这个移位解的代价 $\le$ 上一周期最优代价。而本周期的真最优代价 $\le$ 这个移位可行解的代价（最优不差于任意可行）。两步合起来：本周期最优代价 $\le$ 上一周期最优代价——这就是"代价不增"。把最优代价当 Lyapunov 函数，它单调不增且有下界，于是闭环稳定。关键在于：即使 RTI 只解一次迭代没拿到真最优，只要拿到的解不差于那个移位可行解（warm-start 本身就是它），代价不增依然成立——这就是为什么次优的 RTI 仍能保稳定。

理论-工程桥接（R6D）：递归可行性（T9）+ 代价不增 = 实时 CI-MPC 稳定性的两大支柱。正因为 有 T9 这个理论保证，**工程上才**敢让 CI-MPC 在硬实时下"解不完就输出"——因为终端集保证了"即使这一步只拿到 warm-start 的可行解，下一步仍有解可拿"，系统不会因为某一周期没解到最优就失控。反过来，如果一个 CI-MPC 实现**没有**终端集约束（很多快速实现为了省时间省掉了它），那它就**没有**递归可行性保证——它能跑往往是因为任务温和、状态始终在良态区，一旦遇到大扰动把状态推出良态区，就可能突然 infeasible 而失控。这是 §6.8 给所有"裸跑无终端约束的快速 CI-MPC"的安全警告。

为什么单次 Newton 迭代就够——RTI 有效性的收缩视角。§6.7 的 RTI 每周期只做一次 SQP 迭代就输出，这听起来很冒险——一次迭代怎么可能解出好控制？这里讲清它为什么成立，把 §6.7 的工程承诺落到理论根据上。

关键在于**两次相邻 MPC 子问题的解相距很近**。设第 $k$ 周期的精确最优解为 $z_k^*$，第 $k+1$ 周期的为 $z_{k+1}^*$。因为相邻周期只是时域平移一格、初始状态只变了一个 $\Delta t$ 的小量，在适当正则性下有 $\|z_{k+1}^*-z_k^*\|\le L\Delta t$（解对参数 Lipschitz 连续，$L$ 是 Lipschitz 常数）。也就是说，上一周期的解平移后，离这一周期的真最优解只有 $O(\Delta t)$ 之遥——这正是一个绝佳的 warm-start。

现在用 Newton 法的**局部二次收敛**：若 warm-start $z^{(0)}$ 落在真解 $z_{k+1}^*$ 的二次收敛域内（误差 $\|z^{(0)}-z_{k+1}^*\|\le\epsilon_0$），则一次 Newton 迭代后误差降到 $\|z^{(1)}-z_{k+1}^*\|\le c\|z^{(0)}-z_{k+1}^*\|^2\le c\epsilon_0^2$。当 $\Delta t$ 足够小使 $\epsilon_0=O(\Delta t)$ 时，一次迭代后误差就是 $O(\Delta t^2)$——比 warm-start 本身还小一个量级。于是单次迭代的解已经"足够接近"真最优，作为控制输出绰绰有余。

本质洞察：RTI 的有效性建立在一条隐含的"收缩链"上——MPC 高频滚动（$\Delta t$ 小）⟹ 相邻解相近（$O(\Delta t)$）⟹ warm-start 落在二次收敛域 ⟹ 单次 Newton 把误差压到 $O(\Delta t^2)$。这条链解释了为什么"频率越高，单次迭代越够用"——高频不只是"反应快"，它还**主动制造了好的 warm-start**（相邻解更近），从而让每周期的优化负担更轻。这是 §6.7"局部性 + warm-start"原理的定量内核，也回答了"解不完就输出为什么不会越输出越差"——只要 $\Delta t$ 足够小、状态在良态区（满足前述正则性），单次迭代的解误差被 $O(\Delta t^2)$ 牢牢控制。注意这条链的前提（二次收敛域、解的 Lipschitz 连续）在接触切换瞬间（病因 A/B 处）可能失效——这就是为什么大扰动或频繁切换时 RTI 会"翻车"，需要回退到多次迭代或全局搜索。

多视角理解：把四类病因映射到你已知的优化困难¶

为了把这套诊断框架挂到你已有的数值优化知识树上（R2 跨章关联、R6A 多视角），我们把四类病因与一般非线性优化里的经典困难做对照：

病因 A（CQ-病态）≈ 一般 NLP 的约束退化：标准 NLP 教材里"LICQ/MFCQ 失效导致 KKT 乘子不唯一/无界"，在 MPCC 里是**必然且结构性**发生的（不是偶发的退化，而是每个双活点都发生）。像的部分：都是约束品性失效导致乘子病态。不像的部分：一般 NLP 的 CQ 失效是"运气不好碰上"，MPCC 的 CQ 失效是"数学保证一定碰上"。
病因 B（雅可比奇异）≈ Newton 法的奇异 Hessian：无约束优化里 Hessian 奇异导致 Newton 步无定义，靠 Levenberg-Marquardt 阻尼救。CI-TO 的雅可比奇异同理用阻尼救。像的部分：都靠加 $\delta I$ 正则化恢复可逆。不像的部分：B 的奇异只在 make/break 瞬间局部发生，可针对性地只在那几步加阻尼。
病因 C（局部极小）≈ 一般非凸优化的多模态：这就是非凸优化的经典困难，与接触无关的版本你早就熟悉（神经网络训练、非凸 NLP）。像的部分：都是多盆地、初值决定收敛到哪。不像的部分：CI-TO 的盆地有清晰的**组合语义**（每个盆地 = 一种接触序列），这让它可以用整数枚举（§6.5）这种"按组合结构枚举盆地"的专门武器，而一般非凸优化的盆地无此结构。
病因 D（尺度失衡）≈ 任何优化的预条件（preconditioning）问题：这是最通用的数值优化常识，CI-TO 只是因变量量纲跨度特别大而格外突出。像的部分：都靠缩放/预条件改善条件数。不像的部分：无本质不同，纯粹是 CI-TO 的变量构成（位形/力矩/力混在一起）放大了它。

类比（标注边界）：四类病因诊断像医生的**鉴别诊断（differential diagnosis）——同一个主诉"不收敛"（如同病人主诉"发烧"），背后可能是四种不同的病。**像的部分：都要先靠"化验单"（乘子范数/残差/条件数 vs 血常规/体温曲线）把病因区分开，再对症下药，绝不能见"发烧"就一律退烧药。不像的部分：优化的"化验单"是确定性的数值信号（乘子是否爆炸是客观可读的），不像医学诊断有概率性和个体差异。不要把这个类比延伸到"CI-TO 调试需要像医学那样的经验直觉"——它的诊断是可以靠明确信号机械执行的流程，而非玄学。

理论-工程桥接：如何读求解器日志做鉴别诊断¶

上面的鉴别诊断框架要落地，关键是把"四类病因"翻译成"求解器日志里的可读信号"。这里把抽象的诊断信号对应到 IPOPT/SNOPT 日志的具体字段（R6D 理论-工程桥接，让诊断真正可执行）。

以 IPOPT 的迭代日志为例，每行通常输出若干诊断量。诊断时重点盯三个量随迭代的演化：

日志字段（IPOPT）	含义	哪类病因的信号
`inf_pr`（primal infeasibility）	原始约束违反量	在特定步停滞不降 → 病因 B（雅可比奇异）；全程缓慢降 → 可能 D
`inf_du`（dual infeasibility）	对偶（KKT 平稳性）残差	配合乘子爆炸 → 病因 A
`lg(mu)`（barrier 参数对数）	内点法 barrier 参数	卡住不降（barrier 推不进）→ 病因 A（无严格内点）
`\|\|d\|\|`（步长范数）	本次迭代的步长	单调趋零 → 病因 A（线性化失真）
`lg(rg)`（正则化量对数）	Hessian/雅可比正则化强度	频繁非空（求解器被迫加正则）→ 病因 B 或 D
目标值 `objective`	当前目标函数值	收敛后偏高 + 换初值变 → 病因 C（局部极小）

一套可机械执行的判读流程（对应 §6.8 "D→A/B→C 逐层剥离"）：

先看是否全程慢（查 D）：若 inf_pr/inf_du 从第一步起就缓慢下降、lg(rg) 频繁非空，且与接触模式无关 → 先做变量缩放，再看是否改善。这一步最廉价，永远先做。
再看乘子与 barrier（查 A）：若对偶变量（乘子）范数单调爆炸、lg(mu) 卡住推不进、||d|| 趋零、最终 restoration failed → 病因 A，上 §6.4 松弛。
看是否特定步停滞（查 B）：若 inf_pr 只在某一步（接触切换步）停滞、其他步正常、该步 lg(rg) 持续非空 → 病因 B，加雅可比正则化/软接触。
最后查收敛后的解质量（查 C）：若求解器**正常收敛**（无报错）、但目标值偏高或接触序列怪异，换初值收敛到不同目标值 → 病因 C，用全局搜索/多初值/课程式硬化。

本质洞察：求解器日志不是"看个收敛与否"的黑箱输出，而是一张**逐迭代的化验单**——inf_pr/inf_du/lg(mu)/||d||/lg(rg) 五个量的演化模式，几乎一一对应四类病因的特征签名。学会读这张化验单，你就把"玄学调参"变成了"按签名对症"。SNOPT 的日志字段名不同（如 Feasible/Optimal/nInf），但对应的诊断逻辑完全一致——原始侧停滞看 B、对偶侧爆炸看 A、全程慢看 D、收敛后质量看 C。这套判读流程是 §6.8 全部理论（四类病因 + 逐层剥离）在调试终端上的最终落点。

⚠️ 常见陷阱¶

🧠 思维陷阱：把所有"不收敛"都归因于松弛参数没调好

新手想法："CI-TO 不收敛，肯定是 $t_0$ 或 $\beta$ 没调对，多试几组就行。" 现象/后果：在松弛参数空间里盲目搜索，偶尔撞上能收敛的配置，但换个任务/初值又崩；真实病因若是 C（局部极小）或 D（尺度），无论怎么调松弛参数都治不好，浪费大量时间。 根本原因：松弛参数只治病因 A（CQ-病态）。把四类正交病因（A/B/C/D）当成"同一个调参问题"，是误诊的根源。 正确做法：遇到不收敛，先按鉴别诊断总表逐层剥离——先排 D（去掉接触约束看是否仍慢）、再看是 A（乘子爆炸）还是 B（特定步停滞）、最后查 C（换初值验证）。确诊病因后再对症下药，而非一上来就调松弛参数。

💡 概念误区：以为加了松弛恢复了 CQ，就一定能得到对的接触序列

新手想法："Scholtes 松弛恢复了 MFCQ，求解器能收敛了，那收敛到的解就是对的。" 现象/后果：求解器顺利收敛、无任何报错，但得到一条接触序列荒谬的轨迹（如机器人用膝盖而非脚发力），还误以为这是"算法找到的创新步态"。 根本原因：松弛只治病因 A（恢复 CQ，让求解器**能**收敛），完全不改变景观的多盆地结构（病因 C）。求解器收敛到的是当前初值所在盆地的局部最优，可能对应错误的接触序列。 正确做法：得到解后必须做**物理合理性检查**（接触序列是否符合任务直觉、各接触力是否在合理范围）；若怀疑 C 类局部极小，换多个初值看是否收敛到不同解，或用 sampling/整数枚举提供更好的初值/参考。

💡 概念误区：以为裸跑的快速 CI-MPC（无终端约束）和有终端约束的一样安全

新手想法："为了实时，我把终端约束省了，反正平时跑得挺好。" 现象/后果：平时（状态在良态区）一切正常，但遇到大扰动把状态推出良态区时，某周期突然 infeasible，MPC 无控制可输出，系统失控/摔倒。 根本原因：终端集（控制不变 + 含可行接触模式）是递归可行性（定理 T9）的充分条件。省掉终端约束就丧失了递归可行性保证——平时能跑只是因为状态恰好始终落在"碰巧可行"的区域，没有理论护栏。 正确做法：安全攸关的 CI-MPC 应保留终端约束（如足式的"末端回到可站立姿态"），或至少用一个保守的终端代价近似终端集的作用；若为极致实时必须省略，要在系统层面加独立的安全兜底（如跌倒保护、紧急停机），不能依赖 MPC 自身的可行性。

🧠 思维陷阱：用换求解器来"解决"病态问题

新手想法："IPOPT 不收敛，换 SNOPT/Knitro 应该就好了。" 现象/后果：换遍所有通用 NLP 求解器都不收敛，浪费时间在求解器选型上，得出"这问题无解"的错误结论。 根本原因：病因 A（CQ-病态）是问题**结构**的内禀属性（§6.3 定理 T1），所有依赖 CQ 成立的通用求解器都会失败——换求解器不改变问题结构。 正确做法：病因 A 必须从问题侧改造（§6.4 松弛/光滑/罚），或用专为 MPCC/接触设计的求解器（CALIPSO、Dojo 的定制内点法）。换通用求解器只在病因 D（尺度，某些求解器内置缩放更好）时偶尔有效，对 A/B/C 无用。

在线诊断 vs 离线诊断：CI-MPC 没有"坐下来调试"的时间¶

前面的鉴别诊断流程默认你是在**离线 CI-TO** 现场——可以反复重解、换初值、做对照实验。但 CI-MPC 在线运行时根本没有这个奢侈：控制周期只有 10 ms，不可能在周期内换初值跑对照、也不可能人工读日志。在线诊断必须是**自动的、预防性的、运行时的**，这是一个常被忽略但工程上至关重要的区别。

在线 CI-MPC 的诊断哲学因此从"事后鉴别"转为"运行时监控 + 自动降级"：

运行时监控（轻量信号）：每周期记录几个廉价指标——求解是否在预算内收敛、互补残差是否超阈、解与 warm-start 的偏离量。这些不需要额外计算（求解器顺带就吐出），却能实时反映健康度。
自动降级（fallback）而非诊断：在线检测到异常（如某周期不收敛、互补残差爆表）时，没时间诊断病因，只能立即**降级**——回退到上一周期的可行解、或切换到一个保守的安全控制器（如固定步态 + 站立平衡），而非现场修问题。
预防性设计（把诊断前移到离线）：在线的鲁棒性主要靠**离线就把病因消除**——离线充分调好松弛、缩放、终端集，让在线运行时永远处于良态区，根本不触发病因。这就是为什么 §6.7 那些实时实现把变量缩放、warm-start、终端集都做成"出厂默认"——它们是把离线诊断的成果固化下来，换取在线无需诊断。

对比性思维（离线诊断 vs 在线诊断）：离线像"医院做全面检查"——有时间反复化验、对照、确诊；在线像"飞行中的应急处置"——没时间诊断，只能按预案降级保命。相同处：都依赖前面四类病因的知识。不同处：离线**用**这些知识去**诊断并修复**，在线**用**这些知识去**预防（离线消除）和设计降级预案**。一句话：在线 CI-MPC 的可靠性不是靠在线诊断挣来的，是靠离线把病因消干净 + 设计好降级预案挣来的。 这也解释了为什么 §6.8 定理 T9 的终端集如此重要——它正是一种"预防性设计"，在离线就保证了在线永远有可行解可退，是最关键的降级护栏。

练习¶

练习 6.8.1（鉴别诊断，开放思考题） 你把一个四足机器人的 trot 步态 CI-TO 喂给 IPOPT，观察到：求解器在第 23 次迭代后乘子范数从 $10^2$ 飙到 $10^7$，步长降到 $10^{-12}$，最终报 restoration failed。请：(a) 判断这最可能是 A/B/C/D 哪类病因，给出你的判据；(b) 设计一个"对照实验"进一步确认（提示：如何用"无接触版本"或"换初值"区分病因）；(c) 给出对症修法及其预期副作用。

练习 6.8.2（递归可行性证明，草稿纸上完成） 仿照本节定理 T9 的构造性证明，补全以下细节：(a) 严格写出"移位尾段 $(x^*_{k+1:k+N}, u^*_{k+1:k+N-1})$ 自动满足动力学和互补约束"这一断言的理由（提示：用第 $k$ 周期解的最优性/可行性）；(b) 论证为什么终端集必须"包含可行接触模式"而非仅仅"控制不变"——给出一个反例：一个控制不变但不含可行接触的集合会如何破坏递归可行性。

练习 6.8.3（终端集设计，开放思考题） 为一个双足机器人的 CI-MPC 设计终端集 $\mathcal{X}_f$。要求：(a) 论证你选的集合（如"双脚平行着地、质心在支撑多边形内、速度为零的姿态集"）满足定理 T9 的两个条件（控制不变 + 含可行接触模式）；(b) 讨论这个终端集对 MPC 行为的影响——它会让 MPC 倾向于什么样的轨迹？(c) 如果任务是"单脚跳"（终态本就不该双脚着地），这个终端集合理吗？该如何修改？

练习 6.8.4（跨节综合题） 综合 §6.3（病态）、§6.4（松弛）、§6.7（实时化）、§6.8（诊断），向一个刚把 CI-TO 跑出 restoration failed 的同学，写一份"分四步排查"的诊断清单（checklist），每步给出：检查什么信号、如何区分病因、对应修法。要求清单可以被机械执行（不依赖玄学经验），并解释为什么排查顺序应当是 D→A/B→C（提示：从最廉价、最不涉及结构的病因开始剥离）。

§6.9 与不确定性和足式规划的桥接：互补约束的两个下游 ⭐⭐⭐¶

动机：为什么本章是一座"桥"而非"终点"¶

本章从 §6.0 一路推到 §6.8，给出的是一套**确定性、单系统**的接触决策理论——假设动力学已知、接触参数精确、机器人是唯一的决策主体。但真实机器人世界里，这两个假设都会被打破：

不确定性：接触参数（摩擦系数 $\mu$、地面刚度、间隙距离 $\phi$）往往**测不准**——脚下是冰还是砂？石头会不会松动？这种不确定性如果不显式建模，CI-MPC 规划出的"刚好够"的接触力会在 $\mu$ 比预期小时打滑。
专门的足式结构：本章讲的是**通用**接触决策（手、脚、任意接触点一视同仁）。但足式机器人有特殊结构——离散的"落脚点选择"、支撑多边形、步态周期——值得有专门的、利用这些结构的方法。

本节的任务，是把本章建立的**互补约束语言**，分别延伸到这两个下游：(1) 04 批不确定性规划——把确定性互补约束升级为 chance-complementarity（机会互补约束）；(2) 05 批足式落脚规划——把通用 CI-TO 特化为利用足式结构的 优化驱动落脚规划。理解这两座桥，你就知道本章的数学不是孤岛，而是整个"接触决策"知识网络的中枢。

本质洞察（先给结论）：本章的核心数学对象——互补约束 $0\le\phi\perp\lambda\ge0$——是一种**通用的"接触决策语言"。它向上游连着 §10–§50 的接触力学基础，向下游分叉成两条：一条注入**概率（chance-complementarity，处理"接触会不会发生"的不确定性），一条注入**结构**（足式落脚规划，利用"哪只脚踩哪里"的离散结构）。同一套互补约束，加概率得到鲁棒接触规划，加结构得到足式落脚规划——这就是为什么本章被定位为"收束章"又是"桥接章"：它收束了上游的接触力学，又分发给下游的不确定性与足式两个方向。

反面：如果不架这两座桥会怎样¶

设想一个只学了本章确定性 CI-MPC、不知道这两座桥的工程师：

不知道 chance-complementarity：他在仿真里（$\mu$ 精确已知）调出了完美的攀爬轨迹，部署到真机（脚下有薄冰，实际 $\mu$ 只有仿真值的一半）就打滑摔倒。他会困惑"仿真明明完美，为什么真机不行"——因为他的 CI-MPC 规划的接触力**贴着摩擦锥边界**（数学上最优），没有为 $\mu$ 的不确定性留任何裕度。他不知道，正确的做法是把摩擦互补约束升级成"以高概率满足摩擦锥"的机会约束。
不知道足式落脚规划的专门结构：他用通用 CI-TO 去解四足越障，发现求解器极慢（通用 CI-TO 不利用"落脚点是离散选择"这一结构），还频繁陷入局部极小（§6.8 病因 C）。他不知道，足式有专门的方法（MICP 枚举可踩区域、双层分解上层选落脚点）能利用这些结构大幅加速。

这两个反面的共同教训：本章的确定性通用理论是**必要但不充分**的——它给了你接触决策的数学骨架，但真实部署还需要 (1) 用概率处理参数不确定性、(2) 用结构特化到具体形态。不架这两座桥，你的 CI-MPC 知识就停留在"仿真能跑、真机摔跤、越障极慢"的半成品状态。

桥一：从互补约束到 chance-complementarity（机会互补约束）¶

核心思想：把"硬"互补约束升级为"以高概率满足"的机会约束。

回顾本章的确定性摩擦互补约束（§6.2）：在接触点，要么滑动（切向速度非零、摩擦力在锥边界）、要么粘滞（切向速度零、摩擦力在锥内），这被编码成一组互补条件。其中摩擦锥约束 $\|\lambda_t\|\le\mu\lambda_n$ 用到了摩擦系数 $\mu$。

问题在于 $\mu$ 是不确定的。 设 $\mu$ 服从某个分布（如均值 $\bar\mu$、标准差 $\sigma_\mu$ 的高斯，或区间 $[\mu_{\min},\mu_{\max}]$ 的均匀）。确定性约束 $\|\lambda_t\|\le\bar\mu\lambda_n$ 用均值 $\bar\mu$ 是危险的——真实 $\mu$ 有一半概率小于 $\bar\mu$，那时约束被违反、脚打滑。

chance-complementarity 的做法：把硬约束换成**机会约束**——要求约束以至少 $1-\delta$ 的概率满足：

\[\Pr\big[\|\lambda_t\|\le\mu\lambda_n\big]\ge 1-\delta,\]

其中 $\delta$ 是允许的违反概率（如 $\delta=0.05$ 表示"允许 5% 概率打滑"）。当 $\mu\sim\mathcal{N}(\bar\mu,\sigma_\mu^2)$ 时，这个机会约束可以**解析地重写**为一个确定性约束（这是 04 批 U3 机会约束的标准技术）：

\[\|\lambda_t\|\le\big(\bar\mu-\Phi^{-1}(1-\delta)\,\sigma_\mu\big)\lambda_n,\]

其中 $\Phi^{-1}$ 是标准正态的分位数函数。几何意义：机会约束等价于用一个**收缩后的有效摩擦系数** $\mu_{\text{eff}}=\bar\mu-\Phi^{-1}(1-\delta)\sigma_\mu$ 来规划——不确定性越大（$\sigma_\mu$ 越大）或要求越严（$\delta$ 越小，$\Phi^{-1}(1-\delta)$ 越大），有效摩擦锥收缩得越多，规划的接触力离锥边界越远、裕度越大。

代入具体数字感受一下：设标称摩擦 $\bar\mu=0.6$（普通干燥地面）、不确定性 $\sigma_\mu=0.15$（地面材质不确定）、要求 $\delta=0.05$（允许 5% 打滑概率）。查表 $\Phi^{-1}(0.95)\approx1.645$，于是

\[\mu_{\text{eff}}=0.6-1.645\times0.15\approx0.6-0.247=0.353.\]

也就是说，虽然名义摩擦是 0.6，但为了 95% 不打滑，规划时要**当作摩擦只有 0.35** 来用——有效摩擦锥几乎收缩了 40%！把不同 $\delta$ 与 $\sigma_\mu$ 的有效摩擦系数列成一张速查表（$\bar\mu=0.6$）：

$\delta$（允许打滑概率）	$\Phi^{-1}(1-\delta)$	$\sigma_\mu=0.03$（标定过）	$\sigma_\mu=0.15$（不确定）
0.10	1.282	0.562	0.408
0.05	1.645	0.551	0.353
0.01	2.326	0.530	0.251

这张表一眼看出两件事：(1) 沿行看（$\delta$ 变小、要求变严），有效摩擦缩水加剧；(2) 沿列看（$\sigma_\mu$ 变大、地面更不确定），缩水急剧放大——$\sigma_\mu=0.15$ 列的缩水远比 $\sigma_\mu=0.03$ 列严重。这个数字直观说明：接触不确定性的代价是"可用摩擦的大幅缩水"——这正是为什么真机上机器人在不确定地面要"小心翼翼、少蹬多稳"，因为它（隐式或显式地）在用一个远小于名义值的有效摩擦系数行事。反过来，标定过地面（$\sigma_\mu=0.03$）几乎不缩水——这量化了"减小感知不确定性"的工程价值，也解释了为什么"先看清地面再走"（感知驱动落脚，05 批 §160）能换来更激进的运动。

回顾桥接 04 批 U3：还记得 04 批不确定性规划总论那条核心思想吗——"裕度应当由不确定性模型**推导**出来，而非人工硬塞"？这里 chance-complementarity 正是这一思想在**接触约束**上的兑现：摩擦锥的裕度不是工程师拍脑袋设的"安全系数 0.8"，而是由 $\mu$ 的协方差 $\sigma_\mu$ 经 $\Phi^{-1}(1-\delta)$ 缩放**自动推导**出来的。当你调大对 $\mu$ 不确定性的估计（$\sigma_\mu\uparrow$），有效摩擦锥自动收缩、接触力裕度自动变大——这正是 04 批 U3 机会约束"裕度随不确定性自动伸缩"性质在接触问题上的精确再现。

为什么叫 chance-complementarity 而不只是 chance-constraint？ 因为不确定的不只是摩擦锥这一个不等式——整个互补结构（"接触会不会发生"本身）都可能不确定。例如间隙距离 $\phi$ 不确定（地面高度测不准）时，"脚到底着没着地"是一个概率事件，互补条件 $0\le\phi\perp\lambda_n\ge0$ 的"哪一侧活动"也变成概率性的。把互补约束**整体**置于概率框架下，就得到 chance-complementarity——这是 04 批不确定性规划与本章互补约束的精确交汇点。

间隙不确定情形的完整推导（把上一段的"概率性活动"落到公式）：设地面高度测不准，导致间隙距离含一个不确定偏移 $\phi=\bar\phi+\eta$，其中 $\bar\phi$ 是名义间隙、$\eta\sim\mathcal{N}(0,\sigma_\phi^2)$ 是感知噪声。我们希望保证"脚确实压到地面"（即真实 $\phi\le0$，脚穿过名义地面留出压紧裕度）以高概率成立——否则名义上以为着地、实际还悬空，接触力规划落空。

要求"以至少 $1-\delta$ 概率确实接触"，即

\[\Pr[\bar\phi+\eta\le 0]\ge 1-\delta.\]

标准化 $z=\eta/\sigma_\phi\sim\mathcal{N}(0,1)$，则 $\Pr[\eta\le-\bar\phi]=\Phi(-\bar\phi/\sigma_\phi)\ge1-\delta$。两边取 $\Phi^{-1}$（$\Phi$ 单调增）：

\[-\frac{\bar\phi}{\sigma_\phi}\ge\Phi^{-1}(1-\delta)\quad\Longrightarrow\quad \bar\phi\le -\Phi^{-1}(1-\delta)\,\sigma_\phi.\]

结论：要以 $1-\delta$ 概率保证真实接触，名义间隙必须被压到 $-\Phi^{-1}(1-\delta)\sigma_\phi$ 以下——也就是规划时要让脚**多压进地面** $\Phi^{-1}(1-\delta)\sigma_\phi$ 这么多，作为对地面高度不确定性的裕度。几何意义与摩擦情形对称：摩擦不确定让有效摩擦锥**收缩**（接触力留裕度），间隙不确定让名义接触面**下移**（穿透量留裕度）。二者都是"用确定性等价的裕度吸收概率约束"，且裕度都由 $\Phi^{-1}(1-\delta)\times$ 标准差给出——这是 04 批 U3 机会约束确定性等价的统一形式。

对比性思维（两类接触不确定性的对称）：摩擦系数 $\mu$ 不确定与间隙距离 $\phi$ 不确定，是 chance-complementarity 的两个面孔。相同处：都把一个含随机量的约束（$\|\lambda_t\|\le\mu\lambda_n$ 或 $\phi\le0$）转成确定性约束加裕度，裕度形式都是 $\Phi^{-1}(1-\delta)\times\sigma$。不同处：$\mu$ 的不确定影响的是**切向**（会不会打滑），$\phi$ 的不确定影响的是**法向**（着没着地）；前者收缩摩擦锥、后者下移接触面。理解这个对称，你就能把任何"含不确定接触参数"的约束机械地转成确定性等价——这正是把本章互补约束接入 04 批机会约束框架的通用配方。

本质洞察：chance-complementarity 把本章 §6.5 讲的"接触模式是离散决策"提升到了"接触模式是**概率性**离散决策"。确定性 CI-TO 让优化器**确定地**决定每个接触点接触或分离；chance-complementarity 让优化器在**接触是否发生本身有概率**的前提下，规划出对这种概率鲁棒的轨迹。这与 04 批 U1 分支规划（为离散的多模态各准备一条轨迹）一脉相承——只不过这里的"离散多模态"是接触模式，而非他车意图。

机会约束（随机）vs 鲁棒（最坏情况）——两种处理不确定的哲学。上面用的是**机会约束**（chance / stochastic）哲学——假设知道 $\mu$ 的分布，要求"以 $1-\delta$ 概率满足"。还有一条平行的路是**鲁棒**（robust / worst-case）哲学——不假设分布，只假设 $\mu$ 落在区间 $[\mu_{\min},\mu_{\max}]$，要求"对区间内**任意** $\mu$ 都满足"。

两者在摩擦约束上的具体形式：

机会约束（已知分布）：$\|\lambda_t\|\le\big(\bar\mu-\Phi^{-1}(1-\delta)\sigma_\mu\big)\lambda_n$ —— 用分位数缩放，留 $\Phi^{-1}(1-\delta)\sigma_\mu$ 的裕度。
鲁棒（仅知区间）：$\|\lambda_t\|\le\mu_{\min}\lambda_n$ —— 直接用区间下界 $\mu_{\min}$，保证对任意 $\mu\ge\mu_{\min}$ 都满足。

对比与权衡：鲁棒用 $\mu_{\min}$ 最保守（永不打滑，但裕度大、动作畏手畏脚）；机会约束用 $\bar\mu-\Phi^{-1}(1-\delta)\sigma_\mu$ 居中（允许 $\delta$ 概率打滑，换取更灵活的动作）。当 $\delta\to0$ 时机会约束趋向"几乎不打滑"，但只要分布无界（如高斯），它永远比鲁棒的硬下界略激进（高斯尾部允许极小概率的极端值）；若分布有界且 $\delta=0$，二者重合。

回顾桥接 04 批 U3：这正是 04 批不确定性规划总论里"随机优化 vs 鲁棒优化"那条主线在接触约束上的具体化。还记得 04 批那条洞察吗——"随机优化精确利用分布但对分布假设敏感（假设高斯实际重尾会失效），鲁棒优化不需分布假设但保守"？接触问题里完全一样：若你对 $\mu$ 的分布有把握（如标定过的实验室地面），用机会约束更灵活；若 $\mu$ 完全未知、只知一个保守下界（如野外未知地形），用鲁棒的 $\mu_{\min}$ 更安全。选哪条不是技术问题而是风险偏好问题——这与 04 批"五种安全谱"的选型智慧同源。理解这一点，你就知道 chance-complementarity 不是唯一选项，而是"接触不确定性处理谱系"里的一个点，另一端是鲁棒接触规划。

桥二：从通用 CI-TO 到足式落脚规划¶

核心思想：本章的通用 CI-TO，在足式机器人上被特化为利用足式特殊结构的"优化驱动落脚规划"。

足式机器人的接触决策有三个本章通用框架未利用的**特殊结构**：

离散的落脚点选择：脚只能踩在**可行区域**（地面上的石头、台阶），不能踩空地。这是一个天然的离散选择——"踩哪块石头"。
支撑多边形与静/动稳定：着地脚构成支撑多边形，质心投影/ZMP 必须落在其中（或满足动态稳定判据）。这是足式特有的稳定性约束。
步态周期结构：行走/小跑有近似周期的接触序列（左右脚交替），这个先验结构可以被利用来 warm-start 或约束搜索空间。

05 批 §150（优化驱动落脚与接触规划）正是利用这些结构的专门章节，它把本章的通用方法在足式上落地为几条路线：

互补松弛路线：直接用本章 §6.2 的 CI-TO + §6.4 的松弛，让落脚点和接触序列从优化中涌现。这是本章方法的直接应用。
混合整数路线（MICP）：用本章 §6.5 的混合整数编码"踩哪块可行区域"——每块可行凸区域配一个二元变量，big-M 约束选择落脚区域。这正是 §6.5 提到的"MICP 在足式规划中的成功"（Deits-Tedrake 的可行区域分解）。
双层分解路线：上层用离散搜索/MICP 选接触序列和落脚区域，下层用本章 §6.7 的 CI-MPC（或固定序列 TO）优化连续轨迹。这与 §6.7 Fast-CI-MPC 的 bilevel 结构同源。

MICP 落脚编码的完整写法（把"踩哪块石头"写成 big-M 约束）：上面"MICP 路线"那一条值得展开成具体公式，因为它精确展示了 §6.5 的 big-M 机制如何在足式落脚上落地。设地形被分解成 $R$ 块可踩凸区域 $\{\mathcal{R}_1,\dots,\mathcal{R}_R\}$（每块是一个凸多边形，由线性不等式 $A_r p\le b_r$ 描述，$p$ 是脚端位置）。规划第 $j$ 个落脚点 $p_j$ 时，要决定它落在哪块区域。

引入二元变量 $z_{j,r}\in\{0,1\}$：$z_{j,r}=1$ 表示"第 $j$ 个落脚点踩在第 $r$ 块区域"。落脚编码写成：

\[ \begin{aligned} &\sum_{r=1}^{R} z_{j,r}=1 &&\text{(每个落脚点恰好选一块区域)},\\ &A_r\, p_j \le b_r + M(1-z_{j,r})\mathbf{1},\quad r=1,\dots,R &&\text{(big-M：被选中的区域才生效)}. \end{aligned} \]

big-M 约束怎么起作用：当 $z_{j,r}=1$（选中区域 $r$），约束变成 $A_r p_j\le b_r$——强制 $p_j\in\mathcal{R}_r$（脚必须踩在这块区域内）。当 $z_{j,r}=0$（未选），约束变成 $A_r p_j\le b_r+M\mathbf{1}$——只要 $M$ 足够大，这条约束被**松弛到恒成立**（不约束 $p_j$）。于是 $p_j$ 只受被选中那一块的约束。这正是 §6.5 讲的 big-M 编码"用大常数 $M$ 把未选分支的约束失效化"的机制。

与本章 §6.5 病态分析的呼应：还记得 §6.5 那条陷阱"big-M 的 $M$ 不是物理上界"吗？这里同样要小心选 $M$——$M$ 太大，松弛后的 LP 松弛（branch-and-bound 的下界）太松、剪枝弱、求解慢，且数值上 $b_r+M$ 与 $A_r p_j$ 量级悬殊引发病态（§6.8 病因 D）；$M$ 太小，可能把合法落脚点错误裁掉。经验做法是取 $M$ 略大于地形包围盒的尺度，而非天文数字。

为什么这比通用 CI-TO 优雅：通用 CI-TO 要表达"不能踩空地"，得给空地区域也写互补约束（脚到空地的间隙永远 $>0$），但"哪些是空地"本身是连续位置的复杂函数，CI-TO 在连续空间里盲目搜索（§6.9 反面）。MICP 直接把"可踩区域"作为**离散选项**枚举，用二元变量干净地编码"选哪块"——结构（地形的离散可踩性）被显式利用，求解器不必在连续空间里重新发现它。这就是结构桥的价值：把隐藏在连续约束里的离散结构显式提出来，交给擅长离散的 MICP。

第二个足式结构的形式化——支撑多边形稳定约束。上面 MICP 处理了结构 #1（离散落脚），这里把结构 #2（支撑多边形）也写成约束，因为它是足式区别于通用接触的另一道关键护栏。设当前着地脚的接触点为 $\{p_1,\dots,p_m\}$（$m$ 只脚着地），它们的水平投影构成支撑多边形 $\mathcal{P}=\mathrm{conv}\{p_1^{xy},\dots,p_m^{xy}\}$（凸包）。

静稳定约束：质心水平投影 $c^{xy}$ 必须落在支撑多边形内：

\[c^{xy}\in\mathcal{P}=\mathrm{conv}\{p_i^{xy}\}\quad\Longleftrightarrow\quad c^{xy}=\sum_{i=1}^m \alpha_i p_i^{xy},\ \ \alpha_i\ge0,\ \sum_i\alpha_i=1.\]

也就是 $c^{xy}$ 是着地脚位置的凸组合。若违反（质心投影出多边形），机器人静态下会翻倒。动稳定约束（更一般）：把质心换成 ZMP（零力矩点） 或 CP（capture point），要求 ZMP $\in\mathcal{P}$——这允许动态运动（如奔跑时质心可短暂出多边形，只要 ZMP 在内）。

与互补约束的耦合：支撑多边形 $\mathcal{P}$ 由**哪些脚着地**决定，而"哪些脚着地"正是互补约束 $0\le\phi_i\perp\lambda_{n,i}\ge0$ 决定的（$\lambda_{n,i}>0$ 的脚才贡献到 $\mathcal{P}$）。于是稳定约束与互补约束**动态耦合**——接触模式一变，支撑多边形跟着变，稳定约束的可行域也跟着变。这就是为什么足式落脚规划不能把"选落脚"和"保稳定"分开做——它们通过互补约束（谁在接触）紧紧绑在一起。

本质洞察：支撑多边形约束是足式接触决策**特有**的、通用 CI-TO **没有**的结构——通用框架对"接触点构成的几何形状是否包住质心"一无所知。把它显式加进规划（如 05 批 §150 所做），等于告诉优化器"不只要选可踩的区域，还要选**能站稳**的区域组合"。结合 MICP 的离散落脚，足式落脚规划实际在解一个"既要踩在可行区域、又要让着地脚的凸包包住质心、还要满足互补与动力学"的联合问题——这是本章通用互补约束被注入足式结构（离散落脚 + 支撑多边形）后的完整形态。

回顾桥接 05 批 §140/§150：05 批 §140（落脚点规划经典方法）讲的是 Raibert 启发式——$p_f = p_{\text{hip}} + \frac{T_s}{2}\dot p + k(\dot p-\dot p_{\text{ref}})$，一个**人工设计的解析公式**直接算落脚点。它快、稳、但只管"步幅"不管"地形"（§150 那条陷阱："Raibert 公式可能让脚落到空地上"）。§150 的优化驱动落脚规划用本章的 CI-TO/MICP 取代 Raibert 启发式——让落脚点从"对地形和动力学的优化"中涌现，而非从一个不看地形的公式中算出。这正是本章 §6.1"免预设"哲学在足式落脚上的体现：Raibert 是"预设的落脚规则"，CI-TO 落脚是"涌现的落脚决策"。

足式落脚规划与本章的对应关系（R6E 系统分类，本节核心对照表）：

本章通用概念（§6.x）	足式落脚的特化（05 批 §150）	特化利用了什么足式结构
互补约束 $0\le\phi\perp\lambda\ge0$（§6.2）	脚-地面接触互补	接触点是离散的脚端
接触模式涌现（§6.1）	步态/接触序列涌现	左右脚交替的周期先验
混合整数编码（§6.5）	MICP 选可踩凸区域	地形分解成离散可行区域
bilevel 实时化（§6.7）	上层选落脚区域 / 下层优化轨迹	落脚点选择与轨迹优化的天然分层
病态诊断（§6.8）	落脚 CI-TO 的局部极小（怪步态）	接触序列盆地有步态语义
chance-complementarity（桥一）	不确定地形上的鲁棒落脚	地面高度/摩擦的感知不确定性

为什么 bilevel 是足式落脚的"天然"结构（对照表第四行的展开）：上面把 bilevel 列为足式落脚的一条路线，这里讲清它为什么"天然"——因为足式落脚问题本身就有一个**天生的两层结构**，bilevel 只是顺应了它。

观察足式落脚决策的两类变量：(1) 离散的、低频的——"踩哪块石头、什么接触序列"，这类决策每个步态周期才变一次，且本质是离散选择；(2) 连续的、高频的——"接触力、关节角、质心轨迹"，这类变量在每个控制周期都要精细优化，且本质是连续的。把这两类变量塞进同一个优化问题，正是 §6.8 病因 C（局部极小）和混合整数爆炸的根源——离散和连续混在一起，景观既非凸又组合爆炸。

bilevel 的精髓是**按这个天然的"离散/连续 + 低频/高频"接缝把问题切开**：

上层（离散、低频）：只管"选落脚区域 + 接触序列"，用 MICP 或离散搜索，每步态周期解一次。规模小（只有离散变量），可承受组合搜索。
下层（连续、高频）：在上层选定的接触序列下，用本章 §6.7 的 CI-MPC（或固定序列 TO）精细优化连续轨迹，每控制周期解一次。规模大但**良态**（接触序列已定，互补约束的离散部分被上层固定，下层只剩光滑优化 + 局部接触力，§6.3 病态大幅缓解）。

本质洞察（bilevel 的统一性）：足式落脚的 bilevel 分层（上层离散落脚 / 下层连续轨迹）与 §6.7 Fast-CI-MPC 的 bilevel 分层（上层 Riccati / 下层 LCP）是同一个思想的两次出现——都按"难处理的离散/接触部分"与"良态的连续/光滑部分"的接缝纵切，让各层各司其职。这不是巧合：bilevel 之所以反复出现在接触问题里，是因为接触问题普遍具有"离散决策（接触模式）+ 连续优化（轨迹）"的二元结构，而 bilevel 正是顺应这个二元结构的自然分解。理解了这一点，你就明白为什么从 §6.4 的"外层 continuation + 内层 SQP"、到 §6.7 的 Fast-CI-MPC bilevel、到这里的足式落脚 bilevel，"两层结构"像主题旋律一样贯穿全章——它们都是同一个"按接缝纵切病态问题"哲学的变奏。

三个下游方向的统一图景¶

把本章承上启下的位置画成一张全景图（R2 跨章关联、R6E 系统分类）：

        上游：接触力学基础（本目录 §10–§50）
        互补/Signorini · 摩擦锥 · 时步法 · 可微接触 · 混合系统
                              │
                              ▼
        ┌─────────────────────────────────────────┐
        │   本章 §60：接触隐式 TO/MPC（确定性通用）   │
        │   互补约束 = 通用"接触决策语言"            │
        └─────────────────────────────────────────┘
              │              │              │
       加概率 │       加结构 │      加学习  │（§6.5/§6.7 提到，第八批展开）
              ▼              ▼              ▼
    04 批 U3            05 批 §150       第八批
    chance-            优化驱动          Learning-based
    complementarity    落脚规划          Control
    （鲁棒接触）        （足式特化）       （学习接触/残差）

加概率 → 04 批不确定性规划：互补约束 + 机会约束 = chance-complementarity，处理接触参数/事件的不确定性。
加结构 → 05 批足式落脚规划：CI-TO + 足式结构（离散落脚、支撑多边形、步态周期）= 优化驱动落脚规划。
加学习 → 第八批 Learning-based Control：§6.5 末尾提到的"第三条路（学习与采样）"和 §6.7 的 sampling-based 全局接触搜索，是本章与学习方法的接口——用学习提供接触模式先验/初值、或用 RL 端到端学接触策略。

对比性思维（三条下游的不同注入物）：三条下游分别向本章的确定性通用框架**注入了不同的东西**——04 批注入**概率**（处理"测不准"）、05 批注入**结构**（处理"足式特有"）、第八批注入**数据/学习**（处理"建模难"）。它们不是互相替代，而是从三个正交维度增强同一套互补约束语言。一个真实的高性能足式系统，往往三者并用：用足式结构（05）特化问题、用 chance-complementarity（04）处理地形不确定、用学习（第八批）提供接触先验暖启动。这与 04 批那条"五种范式可叠加"的智慧同构——本章的下游也是可叠加的工具层，而非互斥的选项。

桥三（简述）：从优化到学习的三种接口¶

桥一（概率）、桥二（结构）会在 04/05 批充分展开，第三条桥（学习）属于第八批，这里只点明它与本章互补约束的**三种接口方式**，便于你知道本章方法如何与学习方法衔接（不展开，留给第八批）：

学习提供先验/初值（最浅接口，治病因 C）：用神经网络从历史经验预测"好的接触序列"或"好的落脚区域"，作为 §6.8 病因 C 的初值/参考。优化器仍是本章的 CI-TO/CI-MPC，学习只负责"猜个好起点"。§6.7 的 sampling-based 全局接触搜索（Pang 2023）和这类学习先验是同一接口的两种实现（采样 vs 学习）。
可微优化层（中等接口）：把本章的互补约束求解器（§40 可微接触）作为神经网络里的**一层**（differentiable optimization layer），梯度可以穿过互补约束反传。这让"学习 + 优化"端到端训练——网络学到的特征喂给可微 CI 层、CI 层的输出再算损失反传。互补约束在这里仍**显式存在**，只是被包进了可微层。
端到端 RL（最深接口，互补约束隐退）：用强化学习直接学一个"状态→关节力矩"的策略，接触决策**隐式地**蕴含在学到的策略里，不再有显式互补约束。这是接口最深也最"叛逆"的一条——它放弃了本章的互补约束显式建模，赌"数据 + 网络能隐式学会接触逻辑"。

把三种接口的关键维度列成表，便于对照（R6E 系统分类）：

接口	互补约束显隐	学习扮演的角色	优点	缺点	治本章哪个痛点
1 先验/初值	完全显式	外围给好起点	稳健、可解释、易加	仍受优化求解开销限制	病因 C（局部极小）
2 可微优化层	显式但包进层	端到端训练特征	兼得结构与学习	实现复杂、需可微求解器	建模不准（学残差）
3 端到端 RL	完全隐退	替代优化器	推理快、无在线求解	黑箱、需大量数据、难保证	实时性 + 建模难（一锅端）

对比性思维（互补约束的"显隐程度"递减）：三种接口按互补约束的**显式程度**排成一条谱——接口 1 互补约束完全显式（学习只在外围给初值）、接口 2 互补约束显式但被包进可微层、接口 3 互补约束完全隐退（被网络权重吸收）。这条谱回答了练习 6.9.3 的核心问题"端到端 RL 是否还显式用互补约束"——不，接口 3 不再显式用，它把接触逻辑压进了网络。这也揭示了"学习型接触控制"的真正分野：是把学习当**优化的辅助**（接口 1/2，仍信任互补约束的物理结构）还是当**优化的替代**（接口 3，赌数据能学会一切）。前者稳健可解释、后者灵活但黑箱——这与本章反复出现的"结构 vs 蛮力/数据"张力一脉相承（§6.7"结构 > 蛮力"那条洞察）。第八批会详细展开这条谱上的各种方法。

桥接的工程兑现：一个"不确定地形四足越障"的端到端走查¶

前面把两座桥（概率、结构）分开讲清了，但真实系统是把它们**叠在一起**用的。这里走查一个具体场景——四足机器人越过一片有若干离散可踩石块、其余是空地、且地面摩擦/高度感知不确定的地形——看本章的互补约束如何同时承载概率与结构两条桥（R6D 理论-工程桥接，把全章串成一个可落地的系统）。这个走查也是练习 6.9.4 的"参考答案骨架"，但这里给出完整推理而非留作习题。

模块一：接触决策的形式化（用什么写）。 选 bilevel + MICP——上层用混合整数（§6.5）选"四只脚各踩哪块可踩凸区域"（利用结构桥的离散落脚），下层用 CI-MPC（§6.7）在选定区域内优化连续轨迹。为什么不用纯通用 CI-TO：可踩区域是离散的（§6.9 桥二），纯 CI-TO 不利用这个结构会极慢、易陷局部极小（§6.8 病因 C，怪步态）。为什么不用纯 MICP：连续轨迹（接触力、关节角）是连续的，整数化会爆炸——所以分层，让整数管离散选择、连续优化管轨迹。

模块二：概率桥的注入（怎么处理不确定）。 两类不确定各用一座桥的一个面孔：

摩擦不确定（石块表面是干是湿）：用 §6.9 桥一的摩擦机会约束，把每个着地脚的摩擦锥换成有效摩擦锥 $\|\lambda_t\|\le\mu_{\text{eff}}\lambda_n$，$\mu_{\text{eff}}=\bar\mu-\Phi^{-1}(1-\delta)\sigma_\mu$。湿滑石块 $\sigma_\mu$ 大 → 锥收缩多 → 规划自动避免在该石块上施加大切向力（倾向于垂直踩、少蹬）。
高度不确定（石块高度测不准）：用 §6.9 桥一间隙情形的推导，把名义间隙压到 $\bar\phi\le-\Phi^{-1}(1-\delta)\sigma_\phi$ —— 规划让脚多压进石块 $\Phi^{-1}(1-\delta)\sigma_\phi$，保证以高概率确实着地（不会名义着地、实际悬空）。

模块三：结构桥的加速（怎么变快）。 用 §6.9 桥二的足式结构：(1) MICP 只在**预先分解的可踩凸区域**上选落脚（地形分解成离散区域，整数变量数 = 区域数 × 脚数，可控）；(2) 支撑多边形约束保证静/动稳定；(3) 用 Raibert 启发式或上一步态周期的解 warm-start 上层整数选择，缩小搜索。

模块四：实时化路线（怎么压进控制周期）。 选 §6.7 的 Fast-CI-MPC——足式、需在线发现/微调接触模式，正是它的主场（统一对比表）。下层 CI-MPC 用 bilevel 结构（上层 Riccati、下层 LCP Taylor 近似），贴着上一周期的解 warm-start，RTI 每周期一次迭代，压到 50–100 Hz。

模块五：安全保证（怎么不失控）。 用 §6.8 定理 T9——终端集 $\mathcal{X}_f$ 取"四脚着地、质心在支撑多边形内、速度为零的可站立姿态集"（控制不变 + 含可行接触模式），纳入每周期终端约束，保递归可行性。再加系统层跌倒保护兜底（不依赖 MPC 自身可行性，应对桥外的极端扰动）。

把五个模块的依赖画成数据流（R6E 系统分类，展示互补约束如何贯穿全栈）：

   感知（地形高度/摩擦 + 不确定性 σ_φ, σ_μ）
              │
              ▼
   模块三：MICP 选可踩区域 ──(离散落脚, 结构桥)──┐
              │                                  │
              ▼                                  │
   模块二：chance-complementarity                │
   （μ_eff 收缩锥 + 间隙下移, 概率桥）            │
              │                                  │
              ▼                                  ▼
   模块四：Fast-CI-MPC 下层（互补约束 = 共享语言）
   bilevel + warm-start + RTI, 50–100 Hz
              │
              ▼
   模块五：终端集 X_f 保递归可行（T9）+ 跌倒兜底
              │
              ▼
        关节力矩输出 → 机器人

本质洞察（全章收束）：注意这五个模块**全部围绕同一个核心对象——互补约束**展开：MICP 编码的是"踩哪个区域"的互补/析取（结构桥），chance-complementarity 包裹的是摩擦锥与间隙互补（概率桥），Fast-CI-MPC 的下层 LCP 求解的是互补约束的 Taylor 近似，终端集要求的是"含可行接触模式"（仍是互补可满足性）。互补约束是把感知、规划、控制、安全四层粘合起来的那张通用接口——这正是 §6.9 开篇"互补约束 = 通用接触决策语言"主张的最终兑现。一个工程师只要真正掌握了本章的互补约束语言，就掌握了把这四层串成一个能在不确定地形上自主越障的系统的钥匙。这就是本章作为"接触力学专题收束章 + 足式/不确定性入口章"的全部意义。

多视角理解：互补约束作为"接触决策"的通用接口¶

为了把本章在整个知识网络中的位置看透（R6A 多视角、R6C 本质洞察），换三个视角再审视互补约束这个核心对象：

代数视角：互补约束是一组不等式 $\phi\ge0,\lambda\ge0,\phi\lambda=0$。这个视角下，本章讲的是如何把这组不等式嵌进优化（§6.2）、为什么它病态（§6.3）、怎么救（§6.4）。
逻辑视角：互补约束编码"非此即彼"的析取（§6.5："互补 = 析取"）——要么接触要么分离。这个视角下，下游的延伸是给析取**加概率**（chance：析取的哪一支发生是概率性的）或**加结构**（足式：析取的选项是离散落脚区域）。
决策视角：互补约束是优化器做"接触决策"的载体。这个视角下，本章是"确定性单主体接触决策"，04 批是"概率接触决策"，05 批是"足式结构化接触决策"，第八批是"学习型接触决策"。互补约束是贯穿这四者的通用决策接口。

类比（标注边界）：互补约束在接触决策里的地位，像**关系型数据库的 SQL** 在数据处理里的地位——它是一种**通用的声明式接口**，你声明"接触要满足互补"（如同声明"查满足条件的行"），具体怎么求解（松弛/整数/分解，如同查询优化器选执行计划）交给求解器。像的部分：都是"声明约束/意图，求解细节交给底层"的声明式范式，且同一声明可有多种求解策略。不像的部分：SQL 的求解是良态的（关系代数有完备理论），互补约束的求解是病态的（§6.3 CQ 失效）——这正是本章存在的理由。不要把这个类比延伸到"互补约束像 SQL 一样有成熟的查询优化器"——恰恰相反，它的"查询优化"（如何高效求解）至今仍是活跃研究（§6.7 的四条路就是不同的"执行计划"）。

同一个互补约束，在四个批次里的同一性验证¶

"互补约束是通用接触决策语言"这个主张容易听起来像口号。这里做一次具体验证——把**同一个法向互补条件** $0\le\phi\perp\lambda_n\ge0$ 在它出现的每个批次里写出来，看它如何**逐字保持不变**，只是被赋予了不同的"任务语义"和"附加成分"（R2 跨章关联、R6C 本质洞察，全章统一性的最终落点）：

批次/章节	同一个互补约束的化身	任务语义	在它之上"加"了什么
本目录 §10（Signorini）	$0\le\phi\perp\lambda_n\ge0$	仿真：正向求解接触力	无（裸的互补，仿真时正解）
本章 §6.2（CI-TO）	$0\le\phi\perp\lambda_n\ge0$	优化：作约束嵌入 NLP	嵌入优化（§6.4 松弛化求解）
04 批 U3（chance）	$\Pr[\,\text{互补可行}\,]\ge1-\delta$	鲁棒：高概率满足	加概率（间隙/摩擦不确定→裕度）
05 批 §150（落脚）	$0\le\phi_{\text{脚}}\perp\lambda_{n,\text{脚}}\ge0$ + 区域选择	足式：选落脚 + 接触	加结构（MICP 选可踩区域）
第八批（学习）	互补作可微层 / 软化先验	学习：策略/残差	加数据（学习接触模式先验）

读这张表的方法：盯住第二列——核心的 $0\le\phi\perp\lambda_n\ge0$ 在前三行里几乎逐字相同，到第四行只是把通用的脚标具体成"脚"、到第五行被软化成可微层。这不是巧合或牵强类比，而是因为"接触发生与否"这件事的逻辑本质（间隙与力不能同时为正，§6.5 的析取）在所有这些任务里都一样——变的只是"拿这个逻辑去做什么"（仿真/优化/鲁棒/落脚/学习）和"在它周围加什么"（嵌入/概率/结构/数据）。

本质洞察（全章最高层收束）：一个数学对象能成为"通用语言"，标志是它在不同问题里**保持形式不变、只换语义与配料**——就像 $F=ma$ 在天体、车辆、分子动力学里形式不变、只换力的来源。互补约束 $0\le\phi\perp\lambda_n\ge0$ 正是接触决策领域的这样一个对象：从仿真（§10）到优化（本章）到鲁棒（04）到足式（05）到学习（第八批），它的形式贯穿不变。真正掌握本章，不是记住 CI-TO 的某个具体公式，而是认出这个贯穿全网的互补约束，并知道在每个下游该往它身上"加"什么（概率/结构/数据）。 这就是为什么本章被反复定位为"收束章 + 桥接章"——它教的不是一个孤立技术，而是一门贯穿接触决策全领域的通用语言。

⚠️ 常见陷阱¶

💡 概念误区：以为确定性 CI-MPC 在真机摩擦不确定时也安全

新手想法："我的 CI-MPC 在仿真里规划的接触力满足摩擦锥 $\|\lambda_t\|\le\mu\lambda_n$，那真机上也安全。" 现象/后果：仿真完美（$\mu$ 精确），真机打滑摔倒（实际 $\mu$ 比仿真小）。因为确定性优化规划的接触力**贴着摩擦锥边界**（最优解通常在约束边界上），没有任何裕度，$\mu$ 一小就违反。 根本原因：确定性约束用 $\mu$ 的标称值（如均值 $\bar\mu$），没有为 $\mu$ 的不确定性留裕度。真实 $\mu<\bar\mu$ 时约束失效。 正确做法：用 chance-complementarity（桥一）——把摩擦锥约束换成机会约束 $\Pr[\|\lambda_t\|\le\mu\lambda_n]\ge1-\delta$，等价于用收缩的有效摩擦系数 $\mu_{\text{eff}}=\bar\mu-\Phi^{-1}(1-\delta)\sigma_\mu$ 规划，让接触力自动留出与不确定性匹配的裕度。

🧠 思维陷阱：用通用 CI-TO 硬解足式问题而不利用足式结构

新手想法："足式落脚也是接触问题，直接用本章的通用 CI-TO 解就行，没必要学专门的足式方法。" 现象/后果：求解极慢（通用 CI-TO 不利用"落脚点是离散选择"的结构）、频繁陷局部极小（§6.8 病因 C，怪步态）、越障任务几乎解不出来。 根本原因：足式有特殊结构（离散落脚区域、支撑多边形、步态周期），通用 CI-TO 对这些一无所知，等于让优化器在巨大的无结构空间里盲目搜索。 正确做法：用 05 批 §150 的专门方法——MICP 显式枚举可踩凸区域（利用地形离散结构）、双层分解（上层选落脚点、下层优化轨迹）、用 Raibert 启发式或步态先验提供 warm-start。结构利用得当，可比通用 CI-TO 快几个数量级。

💡 概念误区：把 chance-complementarity 的 $\delta$ 理解成"整条轨迹的打滑概率"

新手想法："我设 $\delta=0.05$，那整条轨迹打滑的概率就是 5%。" 现象/后果：实际整条轨迹的打滑概率远高于 5%——因为一条轨迹有 $N$ 步、每步多个接触点，每个都有 $\delta$ 的违反概率，联合概率经 union bound 累积放大。 根本原因：$\delta$ 是**单步/单约束**的违反概率上界，不是整条轨迹的。这与 04 批 U3 那条"把 $\delta$ 当轨迹整体失败率"的陷阱同源。 正确做法：分清"逐项机会约束"与"联合机会约束"，用 04 批 U3 的风险分配（risk allocation / IRA）把总风险预算 $\delta_{\text{total}}$ 合理分到各步各接触点上，并用蒙特卡洛实测整条轨迹的经验打滑率验证。

练习¶

练习 6.9.1（chance-complementarity 推导，草稿纸上完成） 设摩擦系数 $\mu\sim\mathcal{N}(\bar\mu,\sigma_\mu^2)$，接触力的法向分量 $\lambda_n$ 是确定的决策变量。从机会约束 $\Pr[\|\lambda_t\|\le\mu\lambda_n]\ge1-\delta$ 出发，完整推导出确定性等价形式 $\|\lambda_t\|\le(\bar\mu-\Phi^{-1}(1-\delta)\sigma_\mu)\lambda_n$。提示：把约束改写成 $\mu\ge\|\lambda_t\|/\lambda_n$，对 $\mu$ 的高斯分布用标准化 $z=(\mu-\bar\mu)/\sigma_\mu$，再用 $\Phi^{-1}$ 反解。讨论：当 $\delta\to0$（要求绝对不打滑）时有效摩擦系数会怎样？这在物理上意味着什么？

练习 6.9.2（足式特化对比，开放思考题） 对比"通用 CI-TO"和"05 批 MICP 落脚规划"在解一个四足越障（地面有若干可踩石块、其余是空地）问题上的差异。要求列出：(a) 决策变量分别是什么；(b) 通用 CI-TO 如何（笨拙地）表达"不能踩空地"，MICP 如何（优雅地）表达；(c) 为什么 MICP 在这个问题上更快、更不易陷局部极小；(d) 什么情况下通用 CI-TO 反而更合适（提示：可踩区域无法预先离散化时）。

练习 6.9.3（统一语言论证，开放思考题） 本节主张"互补约束是贯穿确定性 CI-TO、chance-complementarity、足式落脚、学习型接触的通用接触决策语言"。请用一段话论证或反驳这个主张：举出至少两个下游方向，说明它们如何复用同一套互补约束，以及各自在其上"加"了什么（概率/结构/数据）。如果你认为某个下游方向**没有**真正复用互补约束（而是另起炉灶），请指出并说明理由（提示：端到端 RL 是否还显式用互补约束？）。

练习 6.9.4（跨章综合题，难） 综合本章全部内容（§6.1–§6.9）、04 批 U3（机会约束）、05 批 §150（足式落脚），设计一个"在不确定地形上的四足越障"完整方案。要求说清：(a) 用什么形式化接触决策（CI-TO / MICP / bilevel，给出理由）；(b) 如何用 chance-complementarity 处理地面高度和摩擦的感知不确定性；(c) 如何利用足式结构（可踩区域、支撑多边形）加速；(d) 实时化用哪条 §6.7 的路线、为什么；(e) 如何保证递归可行性（§6.8 定理 T9，终端集怎么选）。不要求实现，要求说清每个模块管什么、为什么这样选，以及各模块如何共享互补约束这一语言。

本章常见误解汇总¶

把全章散落各节的高频误解收拢成一张表，便于复习前快速自检（这是 §6.2、§6.6 多次预告的"本章末误解汇总"）。每条给出"误解 → 真相 → 出处"：

#	常见误解	真相	出处
1	"接触隐式"=接触被隐藏、不建模	恰恰相反——接触被显式建模为互补约束，"隐式"指的是接触序列不预先指定，由优化涌现	§6.1
2	组合爆炸数字 $16^{50}$ 是"接触隐式要枚举这么多"	那是固定序列方法要枚举的；接触隐式正是为了避免枚举才用连续松弛	§6.1
3	$\phi^\top\lambda\le0$ 和 $\phi^\top\lambda=0$ 是两个不同约束	在 $\phi\ge0,\lambda\ge0$ 下二者等价（非负数之积 $\le0$ 必 $=0$）	§6.2
4	CI-TO 就是 MPCC	CI-TO 是 MPCC 的一个实例；MPCC 是更大的一类（含博弈、双层、超参优化）	§6.2
5	接触力 $\lambda$ 作决策变量会导致维度爆炸不可接受	维度只线性增加（每接触点几个变量），换来的是免预设序列，划算	§6.2
6	病态是因为问题非凸	非凸（局部最优）与病态（CQ 失效）是两个独立困难；CQ 失效更致命	§6.3
7	病态发生在所有接触时刻	只发生在双活点（make/break 瞬间）；接触稳定后是严格互补点，健康	§6.3
8	换编码（如 FB 函数）能消除互补的非光滑性	非光滑是几何内禀的，换编码只转移困难（FB 在原点仍不可微），不消除	§6.3
9	松弛参数越小越好，直接设 $t=10^{-12}$	太小一开始就病态、warm-start 失效；要从 $\sim10^{-2}$ 逐步缩	§6.4
10	软接触光滑化的刚度 $k$ 是物理刚度	它是数值 continuation 旋钮，不是物理量，按求解需要调度	§6.4
11	三条松弛路收敛到的解是全局最优	只收敛到局部稳定点（一般 C-stationarity）；全局性需另想办法	§6.4
12	混合整数"更精确"所以总是更好	整数精确但指数复杂度；互补松弛多项式但只得局部解——按任务权衡	§6.5
13	big-M 的 $M$ 是物理上界	$M$ 太大数值病态、太小裁掉可行解；是数值参数，需仔细标定	§6.5
14	互补松弛和混合整数毫不相关	二者是同一离散决策的两种编码，存在转化关系（§6.5 末尾洞察）	§6.5
15	Crocoddyl 是接触隐式 MPC	Crocoddyl 默认是固定序列 DDP（给定接触相位），非接触隐式	§6.6
16	固定序列方法"低级"、过时了	固定序列在已知步态任务上又快又稳，是工程主力；二者各擅其场	§6.6
17	实时 CI-MPC 靠"更快的求解器"实现	靠算法结构创新（bilevel/消元/ADMM/saltation）+ warm-start，非蛮力	§6.7
18	warm-start 平移量可随便对齐	必须按实际经过时间平移，否则 predicted 与 measured 漂移	§6.7
19	C3 的 LCS 局部线性化全局有效	只是参考轨迹局部一阶近似，离开邻域接触模式判断错	§6.7
20	四条实时化路互相竞争、必须选最好的	适用不同任务（足式/双臂/multi-contact/大扰动），是工具箱	§6.7
21	所有"不收敛"都是松弛参数没调好	松弛只治病因 A；C（局部极小）/D（尺度）调松弛无用，要鉴别诊断	§6.8
22	加了松弛恢复 CQ 就一定得到对的接触序列	松弛治 A（能收敛）不治 C（多盆地）；可能收敛到怪序列	§6.8
23	裸跑无终端约束的快速 CI-MPC 和有终端约束一样安全	无终端集就无递归可行性保证（T9），大扰动下可能突然 infeasible	§6.8
24	确定性 CI-MPC 在真机摩擦不确定时也安全	确定性解贴摩擦锥边界无裕度，$\mu$ 一小就打滑；需 chance-complementarity	§6.9
25	chance-complementarity 的 $\delta$ 是整条轨迹打滑概率	$\delta$ 是单步/单约束违反上界；整条轨迹经 union bound 累积放大	§6.9

本章小结¶

本章把接触力学专题的全部数学（§10–§50）收束到一个工程终点——机器人如何实时做出含接触的决策，又把它分发给两个下游（04 批不确定性、05 批足式）。我们用一条主线串起九节：

主线回顾：愿望（免预设接触序列，§6.1）→ 数学载体（互补约束 MPCC，§6.2）→ 代价（CQ 病态，§6.3）→ 救援（松弛/光滑/罚，§6.4）→ 对照（混合整数 §6.5、固定序列 §6.6）→ 工程终点（实时化四条路，§6.7）→ 调试现场（数值诊断，§6.8）→ 桥接下游（不确定性 + 足式，§6.9）。

九节的一句话精华：

§6.1 动机：接触隐式"隐"的是**接触序列**（不预设何时接触），不是隐藏接触本身。它用增加连续变量（接触力 $\lambda$）的代价，换取"免预设序列、让接触模式涌现"的能力——避免了固定序列方法的组合爆炸。
§6.2 形式化：CI-TO 是把动力学约束 $h_k=0$、法向互补 $0\le\phi\perp\lambda_n\ge0$、摩擦互补写进一个 NLP，这个 NLP 属于 MPCC 大类。
§6.3 病态：互补约束在双活点（$\phi=\lambda=0$，make/break 瞬间）使 MFCQ 必然失效（定理 T1，因 $\nabla(\phi\lambda)=0$），导致乘子爆炸、步长趋零。病态**只在双活点**，严格互补点健康。
§6.4 松弛：Scholtes（$\phi\lambda\le t_k$，工程默认）、Lin-Fukushima（光滑 FB 函数）、$\ell_1$ 罚（移进目标）三条路殊途同归——把尖角磨圆、用同伦参数逼回原问题，一般收敛到 C-stationarity（定理 T2）。
§6.5 整数 vs 松弛：混合整数显式枚举接触模式（精确但指数），互补松弛隐式涌现（多项式但局部）——两种哲学按任务权衡，且存在转化关系。
§6.6 固定序列：DIRCON / HZD / 质心 MPC 是接触隐式的反面——预设接触序列，在已知步态任务上又快又稳，是工程主力而非过时方法。
§6.7 实时化：Fast-CI-MPC（bilevel + Taylor LCP）、IDTO（逆动力学消元）、C3（ADMM 分时）、HiLQR（saltation 进 Riccati）四条路，共享"局部性 + warm-start"原理——贴着参考轨迹滚动，把全局病态 MPCC 替换成局部良态子问题。
§6.8 诊断：四类正交病因——A（CQ 病态，对偶侧，松弛治）、B（雅可比奇异，原始侧，正则化治）、C（局部极小，景观，全局搜索治）、D（尺度失衡，数值，缩放治）。逐层剥离（D→A/B→C）。实时特有困难：递归可行性（定理 T9，靠终端集）+ 求解超时（靠 RTI + 代价不增）。
§6.9 桥接：互补约束是通用"接触决策语言"——加概率得 chance-complementarity（04 批，鲁棒接触）、加结构得足式落脚规划（05 批，MICP/bilevel）、加学习得第八批。

全章最重要的四个洞察：

病态的精确位置：CI-TO 的全部数值困难集中在 make/break 瞬间的双活点（L 形尖角的顶点）。这一个洞察解释了为什么频繁切换的任务难、为什么所有救援方法都在"磨圆尖角"、为什么实时化要贴着参考轨迹滚动（远离尖角）。
局部性 + warm-start 是实时化的统一原理：四条实时化路看似迥异，都放弃"一次求出全局最优含接触轨迹"，转而"在当前轨迹邻域快速做接触感知修正，靠 MPC 高频反馈累积成全局行为"。这是 §6.3 病态分析的工程回报。
互补约束是贯穿全网的接触决策接口：从上游接触力学，到本章确定性 CI-TO，到下游 chance-complementarity（加概率）、足式落脚（加结构）、学习型接触（加数据）——同一套互补约束语言贯穿始终。
bilevel/两层结构是贯穿全章的分解主题：从 §6.4 的"外层 continuation + 内层 SQP"、到 §6.7 Fast-CI-MPC 的"上层 Riccati / 下层 LCP"、到 §6.9 足式落脚的"上层离散落脚 / 下层连续轨迹"——同一个"按'病态离散/接触部分'与'良态连续/光滑部分'的接缝纵切"哲学反复出现。它的根源是接触问题普遍的二元结构（离散接触模式 + 连续轨迹优化），bilevel 正是顺应这个二元结构的自然分解。

知识点总表¶

知识点	难度	核心结论	一句话记忆
§6.1 动机	⭐⭐	隐式的是接触序列，非接触本身	"隐序列不隐接触"
§6.2 MPCC 形式化	⭐⭐	动力学 + 法向/摩擦互补写进 NLP	"CI-TO 是 MPCC 的实例"
§6.3 病态	⭐⭐⭐	双活点 MFCQ 必失效（T1）	"病在尖角顶点"
§6.4 松弛	⭐⭐⭐	磨圆尖角 + 同伦逼回（T2，C-stat）	"Scholtes 是默认"
§6.5 整数 vs 松弛	⭐⭐⭐	精确指数 vs 局部多项式	"按任务权衡"
§6.6 固定序列	⭐⭐	预设序列，快稳，工程主力	"反面但不低级"
§6.7 实时化	⭐⭐⭐⭐	四条路 = 局部性 + warm-start	"贴参考滚动"
§6.8 诊断	⭐⭐⭐	四类病因正交，逐层剥离；T9 递归可行	"D→A/B→C，终端集保命"
§6.9 桥接	⭐⭐⭐	互补 = 通用接触决策语言	"加概率/结构/数据"

速查表¶

遇到具体问题时的快速决策卡：

你的问题	去看	一句话答案
该用接触隐式还是固定序列？	§6.1 / §6.6	步态已知用固定序列（快稳）；需发现新接触用接触隐式
CI-TO 的 NLP 怎么写？	§6.2	动力学 $h_k=0$ + 法向互补 + 摩擦互补，目标加 tracking
IPOPT 报 restoration failed？	§6.8	鉴别 A/B/C/D；最可能是 A（CQ 病态），上松弛
用哪个松弛方法？	§6.4	默认 Scholtes（$\phi\lambda\le t_k$）；备选 Lin-Fukushima
松弛参数怎么调？	§6.4	$t_0\sim10^{-2}$，$\beta\sim0.1$，残差 $<10^{-6}$ 停
该用整数还是松弛？	§6.5	模式少/要全局最优用整数；模式多/要快用松弛
实时 CI-MPC 选哪条路？	§6.7	足式 Fast-CI；双臂 IDTO；multi-contact C3；大扰动 HiLQR
怎么保证 MPC 不会突然失控？	§6.8	加终端集（控制不变 + 含可行接触），保递归可行性 T9
真机摩擦不确定怎么办？	§6.9	chance-complementarity，用收缩有效摩擦系数规划
足式落脚规划怎么做？	§6.9	05 批 §150：MICP 选可踩区域 / bilevel 分解

符号表¶

本章新引入或重点使用的数学符号（理论教学必备组件）：

符号	含义	首次出现
$\phi$ / $\phi_i$	间隙距离（gap），脚到接触面的有向距离；$\ge0$ 表示无穿透	§6.2
$\lambda$ / $\lambda_n$	法向接触力（$\ge0$，只能推不能拉）	§6.2
$\lambda_t$	切向（摩擦）力	§6.2
$\mu$	库仑摩擦系数	§6.2
$0\le a\perp b\ge0$	互补条件：$a\ge0,b\ge0,ab=0$	§6.2
$h_k=0$	第 $k$ 步离散动力学约束（时步法嵌入）	§6.2
$J_c(q)$	接触雅可比（把广义速度映到接触点速度）	§6.7
$t_k$	Scholtes 松弛参数（$\phi\lambda\le t_k$，$\downarrow0$）	§6.4
$\mu$（光滑化）/ $\psi_\mu$	Lin-Fukushima 光滑参数 / 光滑化 FB 函数	§6.4
$\psi_{FB}$	Fischer-Burmeister 函数 $a+b-\sqrt{a^2+b^2}$	§6.4
$\rho$	罚因子（$\ell_1$ 罚 / elastic mode）	§6.4
$M$	混合整数 big-M 常数	§6.5
$\Xi$	saltation 矩阵（穿越 guard 的敏感度传播）	§6.7
$V_{xx}$	值函数对状态的二阶导（Riccati 递推核心）	§6.7
LCS	线性互补系统（C3 的局部线性化模型）	§6.7
$z_k,\delta_k$	C3 的光滑副本 / 互补副本（ADMM consensus）	§6.7
$w_k$	C3 的 ADMM 对偶变量	§6.7
$\mathcal{X}_f$	控制不变终端集（递归可行性 T9）	§6.8
$\delta$（机会约束）	允许的约束违反概率（chance-complementarity）	§6.9
$\Phi^{-1}$	标准正态分位数函数（机会约束确定性等价）	§6.9
$\mu_{\text{eff}}$	收缩后的有效摩擦系数 $\bar\mu-\Phi^{-1}(1-\delta)\sigma_\mu$	§6.9

定理速查表¶

本章核心定理/公式（理论教学必备组件）。其中 T1、T2、T8、T9 是本章自身建立或重点使用的定理，T6、T7 是从前置章节（§50、§30）引用：

定理/公式	一句话说明	对应节
T1 MPCC 的 MFCQ 失效	互补约束在双活点 $\nabla(\phi\lambda)=0$，MFCQ 必然失效	§6.3
T2 Scholtes 收敛	$t_k\to0$ 时松弛解聚点是 C-stationary（强条件下 B-stationary）	§6.4
T8 ADMM 非凸残差趋零	非凸可分问题下 ADMM 残差趋零（C3 收敛依据，Wang-Yin-Zeng 2019）	§6.7
T9 CI-MPC 递归可行性	控制不变终端集（含可行接触模式）+ 终端约束 ⟹ 递归可行	§6.8
T6 saltation 矩阵（引自 §50）	穿越 guard 时敏感度乘 $\Xi=\partial R/\partial x+\frac{(f^+-Rf^-)\nabla g^\top}{\nabla g^\top f^-}$	§6.7 引用
T7 时步法能量不增（引自 §30/§50）	Stewart-Trinkle LCP 隐式积分保证刚性接触不穿透、能量不增	§6.2 引用
MPCC 稳定性层级	S ⟹ M ⟹ C ⟹ W（从强到弱）	§6.3
chance-complementarity 等价	$\Pr[\\|\lambda_t\\|\le\mu\lambda_n]\ge1-\delta\iff\\|\lambda_t\\|\le\mu_{\text{eff}}\lambda_n$	§6.9

累积项目：接触力学专题的收束模块¶

接触力学专题的累积项目"最简接触系统全栈"（从 §10 起逐章累积）在本章迎来收束模块。前几章我们已经累积了：§10 的互补条件判定、§20 的摩擦锥投影、§30 的时步法仿真器、§40 的可微接触梯度、§50 的混合系统 saltation。本章添加的模块是**接触隐式轨迹优化与 MPC 控制器**，它把前面所有模块串成一个能"自己想出怎么接触"的决策系统。

本章模块的设计要求（保存在专题项目目录 contact_mechanics_project/06_ci_mpc/，与前章模块同目录）：

CI-TO 离线规划器：在一个最简系统（推荐 planar pushing 或 cart-pole with wall contact）上实现 §6.2 的 CI-TO NLP。决策变量含位形、速度、接触力、间隙；约束含时步法动力学（复用 §30 模块）、法向互补、摩擦互补。用 §6.4 的 Scholtes 松弛 + continuation 求解（外层 $t_k$ 下降循环 + 内层 IPOPT/SNOPT）。
病态诊断面板：记录每个 continuation 阶段的乘子范数、步长、互补残差、条件数，画出它们随 $t_k$ 下降的曲线——直观看到 §6.3 的病态如何随 $t_k\to0$ 浮现、又如何被 continuation 控制住。这是 §6.8 鉴别诊断的可视化实践。
CI-MPC 在线控制器：把离线 CI-TO 改造成 receding-horizon 的 CI-MPC——加 warm-start（按实际时间平移上次解，§6.7）、加终端约束（§6.8 的 $\mathcal{X}_f$，保递归可行性），用 RTI（每周期固定迭代次数）。在仿真里闭环跑，验证它能在线发现接触序列（如 cart-pole 主动用墙借力）。
chance-complementarity 升级（选做）：把摩擦系数 $\mu$ 设为不确定（加噪声），用 §6.9 的有效摩擦系数 $\mu_{\text{eff}}$ 重规划，对比确定性版本在 $\mu$ 被低估时的打滑率——验证机会约束带来的鲁棒性。

与前章模块的衔接：

前章模块	本章如何复用
§10 互补判定	CI-TO 的法向/摩擦互补约束直接用这套判定逻辑
§20 摩擦锥投影	摩擦互补约束的几何（锥/多面体近似）继承自此
§30 时步法仿真器	CI-TO 的动力学约束 $h_k=0$ 就是时步法更新方程
§40 可微接触	CI-MPC 的实时性靠接触求解器吐梯度（IDTO/C3 路线）
§50 saltation	HiLQR 路线把 $\Xi$ 插进 backward pass（选做高级模块）

项目设计的本质：这个累积项目的精髓，是让你**亲手验证全章三个可动手复现的核心洞察**——(1) 在诊断面板上**看见**病态随 $t_k\to0$ 浮现（§6.3）；(2) 在闭环里**体会** warm-start + 局部性如何让在线 CI-MPC 可行（§6.7）；(3) 在 $\mu$ 不确定实验里**量化** chance-complementarity 的鲁棒回报（§6.9）。孤立地读公式，你只是"知道"这些洞察；在最简系统上把它们跑出来，你才"理解"它们。这与全章反复强调的"理论—工程桥接"（R6D）一脉相承——理论的价值要在能跑的系统上兑现。

延伸阅读¶

按"先读哪篇"的优先级组织（不是简单罗列），每篇标注它对应本章哪一节、解决什么问题：

奠基性必读（对应 §6.1–§6.3）：

Posa, Cantu, Tedrake (2014), A direct method for trajectory optimization of rigid bodies through contact, IJRR 33(1):69–81. 接触隐式轨迹优化的奠基论文，§6.2 的 MPCC 形式直接来自此。先读这篇建立全章的形式化基础。
Scholtes (2001), Convergence properties of a regularization scheme for MPCCs, SIAM J. Optim. 11(4):918–936. §6.4 Scholtes 松弛与定理 T2 的原始出处。理解"为什么松弛能救病态"必读。
Luo, Pang, Ralph (1996), Mathematical Programs with Equilibrium Constraints, Cambridge Univ. Press. MPCC 理论的权威专著，§6.3 病态分析的理论根基。深入理解 CQ 失效时查阅。

松弛与稳定性（对应 §6.4）：

Hoheisel, Kanzow, Schwartz (2013), Theoretical and numerical comparison of relaxation methods for MPCCs, Math. Program. 137:257–288. 系统比较六种松弛方法，§6.4 "为什么 Scholtes 最稳"的依据。选型松弛方法时读。
Lin, Fukushima (2005), A modified relaxation scheme for MPCCs, Ann. Oper. Res. §6.4 Lin-Fukushima 光滑化出处。

混合整数与固定序列（对应 §6.5–§6.6）：

Deits, Tedrake (2014), Footstep planning on uneven terrain with mixed-integer convex optimization, Humanoids. §6.5/§6.9 提到的 MICP 落脚规划，混合整数路线在足式上的代表作。
Posa, Kuindersma, Tedrake (2016), Optimization and stabilization of trajectories for constrained dynamical systems（DIRCON）, ICRA. §6.6 固定序列路线 DIRCON 出处。
Westervelt, Grizzle, et al. (2007), Feedback Control of Dynamic Bipedal Robot Locomotion（HZD 专著）. §6.6 HZD 路线的权威参考。

实时化四条路（对应 §6.7，本章重点）：

Le Cleac'h, Howell, et al. (2024), Fast contact-implicit model predictive control, IEEE T-RO. §6.7 路线一 Fast-CI-MPC，当前 SOTA，bilevel 结构出处。开源 ContactImplicitMPC.jl。
Kurtz, Li, et al. (2023/2026), Inverse dynamics trajectory optimization for contact-implicit MPC (IDTO). §6.7 路线二，逆动力学消元。开源 github.com/ToyotaResearchInstitute/idto。
Aydinoglu, Wei, Huang, Posa (2024), Consensus complementarity control for multi-contact MPC, IEEE T-RO (arXiv:2304.11259). §6.7 路线三 C3，ADMM 分时。开源 dairlib/c3。
Kong, Li, Council, Johnson (2023), Hybrid iLQR model predictive control via saltation matrices, IEEE T-RO 39(6):4712–4727. §6.7 路线四 HiLQR，saltation 进 Riccati。

全局接触搜索与学习（对应 §6.5/§6.7/§6.9 桥接）：

Pang, Suh, Zhang, Tedrake (2023), Global planning for contact-rich manipulation via local smoothing of quasi-dynamic contact models, IEEE T-RO. §6.7/§6.8 提到的 sampling-based 全局接触搜索，治病因 C 的工具。
Mordatch, Todorov, Popović (2012), Discovery of complex behaviors through contact-invariant optimization, ACM TOG. 接触不变优化（CIO），接触隐式的另一经典路线。

阅读路径建议：若你是初次进入这个领域，按"Posa 2014 → Scholtes 2001 → Le Cleac'h 2024"三篇主线读，分别建立形式化、松弛、实时化三大支柱，即可掌握全章主干。其余按你的任务方向（足式查 Deits/HZD、操纵查 C3/IDTO、不确定查 04 批）选读。

与后续章节的关系¶

本章在整个教程知识网络中承上启下，明确它与上下游章节的依赖关系，便于规划学习路径：

上游（本章直接依赖，学本章前应掌握）：

本目录 §10–§50（接触力学专题前五章）：互补/Signorini、摩擦锥、时步法、可微接触、混合系统/saltation——本章是它们的收束，几乎每节都复用。
控制理论 §50（LQR/LQG/Riccati）：本章 §6.7 所有 DDP/iLQR-based CI-MPC 把 LQR 当内层子程序，必须先懂 Riccati 递推。
数值优化（SQP/IPM/ADMM）：本章 §6.4 的 SQP/IPM、§6.7 的 ADMM 假定读者已有优化基础。
本目录 §70（非光滑分析）：互补、$\max(0,\cdot)$、$\|v_t\|$ 的非光滑性分析支撑本章 §6.3 的病态讨论。

下游（依赖本章，学完本章后可进入）：

04 移动机器人规控 §30 不确定性规划（尤其 U3 机会约束）：本章 §6.9 桥一 chance-complementarity 是接触约束与机会约束的交汇点。学完本章再学 04 批，能看清"接触决策如何注入概率"。
05 运动控制 §10 足式（尤其 §140 落脚经典方法、§150 优化驱动落脚、§160 感知驱动落脚）：本章 §6.9 桥二把通用 CI-TO 特化为足式落脚规划。05 批 §150 是本章方法在足式上的直接落地与深化。
第八批 Learning-based Control：本章 §6.5 末尾的"学习与采样第三条路"、§6.7 的 sampling-based 全局搜索、§6.9 桥三是本章与学习方法的接口。

平行（与本章互补、可对照学习）：

本目录 §40（可微接触仿真）：本章侧重"用接触求解器做优化决策"，§40 侧重"接触求解器本身如何可微"。二者互补——§40 提供工具（可微接触），本章是工具的下游应用（CI-MPC 的实时性靠它）。

学习路径建议：本章是接触力学专题的"出口"，也是足式控制与不确定性规划的"入口"。推荐路径：(1) 想做足式控制 → 学完本章直接进 05 批 §140→§150→§160；(2) 想做含接触的不确定性规划 → 学完本章进 04 批 §30 不确定性规划；(3) 想做学习型接触控制 → 学完本章进第八批，回头看 §6.5/§6.7 的采样/学习接口。无论哪条路，本章建立的"互补约束 = 接触决策语言"都是后续章节反复复用的通用基础。

故障排查手册¶

本章面向"接触隐式轨迹优化/MPC 调试现场"的结构化排查表（理论教学必备，每个场景给出症状→可能原因→排查步骤→相关章节）。这套手册是 §6.8 鉴别诊断框架的速查化——遇到问题先按手册定位，再回对应节深读。

场景一：IPOPT/SNOPT 报 restoration failure，乘子范数爆炸¶

项目	内容
症状	求解器迭代若干步后乘子（对偶变量）范数从 $O(10^2)$ 单调飙到 $O(10^6)$ 以上甚至 `inf`，SQP 步长降到 $10^{-12}$，最终报 `restoration failed`（IPOPT）或 `infeasible problem`（SNOPT）
可能原因	病因 A（CQ-病态）——把 raw MPCC（未松弛的互补约束）直接喂给了通用 NLP 求解器，MFCQ 在双活点必然失效（定理 T1），乘子集合无界
排查步骤	1. 确认互补约束是否做了松弛——检查约束里是否还是 $\phi\lambda=0$（硬等式）；2. 观察乘子爆炸是否发生在接触 make/break 的时间步（双活点）；3. 若是，确诊病因 A
修法	加 §6.4 的 Scholtes 松弛（$\phi\lambda\le t_k$）+ continuation 循环（$t_0\sim10^{-2}$，$\beta\sim0.1$）；或换专门的 MPCC/接触求解器（CALIPSO、Dojo 定制 IPM）。绝不要靠换通用求解器（SNOPT/Knitro）解决——病因 A 是结构性的
相关章节	§6.3（定理 T1，病态证明）、§6.4（松弛救援）、§6.8（病因 A 鉴别）

场景二：求解器顺利收敛，但轨迹的接触序列物理上荒谬¶

项目	内容
症状	求解器无任何报错、顺利收敛，但得到的轨迹接触序列怪异（如机器人用膝盖/肘部而非脚/手发力、接触时序违反任务直觉），或目标值明显高于预期
可能原因	病因 C（非凸局部极小）——优化器收敛到了"错误接触序列"对应的盆地的局部最优。松弛恢复了 CQ（能收敛），但没改变景观的多盆地结构
排查步骤	1. 用多个不同初值重新求解，观察是否收敛到不同目标值的解（是 → 确诊局部极小，凸问题换初值收敛到同一解）；2. 检查得到的接触序列是否符合任务物理直觉；3. 确认松弛参数已正常工作（排除 A）
修法	(1) 用 §6.7 的 sampling-based 全局接触搜索（Pang 2023）提供好初值/参考；(2) §6.5 的混合整数枚举（小规模时，得全局最优）；(3) multi-start 取最好；(4) 课程式 continuation——从软接触（盆地少）逐步硬化
相关章节	§6.5（整数枚举）、§6.7（sampling 暖启动）、§6.8（病因 C，"松弛治 A 不治 C"洞察）

场景三：迭代在某个特定时间步卡住，其他步正常¶

项目	内容
症状	收敛过程中，某一个特定时间步（恰好是接触切换那一步）的原始残差（primal residual）下降停滞，其他时间步的残差正常下降；该步局部条件数飙升
可能原因	病因 B（雅可比奇异）——动力学约束雅可比在 make/break 瞬间秩退化（接触雅可比 $J_c$ 的法向行从无效突变为有效，逆动力学 Jacobian 接近奇异）。注意：这是原始侧病态，与场景一的对偶侧病态（A）不同
排查步骤	1. 定位停滞的时间步，确认它是否是接触 make/break 的那一步；2. 检查是原始残差停滞（病因 B）还是乘子爆炸（病因 A）——这是区分 B 和 A 的关键；3. 检查该步动力学雅可比的最小奇异值是否趋零
修法	(1) 雅可比正则化——给 Newton 系统加 $\delta I$（Levenberg-Marquardt 阻尼），可只在该步加；(2) 软接触光滑化（§6.4）把 Dirac 抹成有限值（IDTO 的做法）；(3) saltation 矩阵（§6.7 HiLQR）精确传播跨切换敏感度
相关章节	§6.4（软接触光滑化）、§6.7（IDTO 代价分析、HiLQR）、§6.8（病因 B，"A 病对偶 B 病原始"鉴别）

场景四：求解器全程缓慢，条件数始终偏高，与接触模式无关¶

项目	内容
症状	求解器从头到尾都慢（不只 make/break 瞬间），条件数估计始终偏高（$\gg10^6$），但没有乘子爆炸、也没有特定步停滞；把问题改成无接触版本（去掉互补约束）后仍然慢
可能原因	病因 D（尺度与条件数失衡）——决策变量量纲跨度大（位形 $O(1)$、力矩 $O(100)$、接触力 $O(1000)$），未做缩放导致 KKT 系统天然病态。这与接触无关，是纯数值问题
排查步骤	1. 做"无接触对照"——去掉互补约束解纯光滑版本，若仍慢则确诊病因 D（病因 A/B/C 都与接触相关，去掉接触应改善）；2. 检查各变量/约束的典型数值范围，看量纲跨度；3. 检查是否做了变量缩放
修法	变量缩放（把 $q,v,\tau,\lambda$ 各归一化到 $O(1)$）、约束缩放（每条约束除以典型尺度）、选合适单位制。这是优化建模 ABC，但 CI-TO 因变量量纲跨度大而格外重要
相关章节	§6.8（病因 D，"去掉接触约束若仍慢则是 D"鉴别）

场景五：仿真完美，部署真机后打滑/摔倒¶

项目	内容
症状	CI-MPC 在仿真里（接触参数精确已知）规划出完美轨迹，部署到真机后频繁打滑、摔倒，尤其在地面湿滑/材质变化处
可能原因	确定性 CI-MPC 规划的接触力贴着摩擦锥边界（数学最优解在约束边界上），没有为真实摩擦系数 $\mu$ 的不确定性留任何裕度。真实 $\mu<$ 标称值时摩擦约束被违反
排查步骤	1. 检查规划的接触力 $\\|\lambda_t\\|/\lambda_n$ 是否接近标称 $\mu$（贴边界）；2. 测量真机实际 $\mu$ 与仿真假设的差距；3. 确认是否用了 chance-complementarity
修法	用 §6.9 桥一的 chance-complementarity——把摩擦锥约束换成机会约束 $\Pr[\\|\lambda_t\\|\le\mu\lambda_n]\ge1-\delta$，等价于用收缩有效摩擦系数 $\mu_{\text{eff}}=\bar\mu-\Phi^{-1}(1-\delta)\sigma_\mu$ 规划，让接触力自动留出与不确定性匹配的裕度
相关章节	§6.9（chance-complementarity）、04 批 U3（机会约束、风险分配）

场景六：CI-MPC 平时正常，大扰动下突然 infeasible 失控¶

项目	内容
症状	CI-MPC 在正常工况下闭环运行良好，但遇到大扰动（被推、踩空、外力冲击）把状态推离参考轨迹后，某个控制周期突然报 infeasible，MPC 无控制可输出，系统失控/摔倒
可能原因	(1) 缺失终端约束——为追求实时省掉了终端集 $\mathcal{X}_f$，丧失递归可行性保证（定理 T9）；(2) warm-start 失效——状态离参考太远，上次解平移后不再是好初值；(3) LCS/Taylor 近似在远离参考处接触模式判断错（§6.7）
排查步骤	1. 检查子问题是否含终端约束 $x_{k+N}\in\mathcal{X}_f$；2. 检查 infeasible 发生时状态离参考轨迹的距离（是否已离开良态邻域）；3. 检查 warm-start 是否按实际时间正确平移
修法	(1) 加终端集约束（控制不变 + 含可行接触模式，如足式的"可站立姿态集"），恢复递归可行性 T9；(2) 在系统层加独立安全兜底（跌倒保护、紧急停机），不依赖 MPC 自身可行性；(3) 大扰动时与全局搜索/恢复策略配合，把状态先拉回良态区
相关章节	§6.8（定理 T9 递归可行性、终端集设计、"裸跑无终端约束"警告）、§6.7（warm-start、局部性）

场景七：continuation 循环在某个 $t_k$ 阶段卡住不前进¶

项目	内容
症状	Scholtes 松弛的 continuation 外循环在某个中间 $t_k$ 阶段无法收敛，或相邻阶段间求解器要"重新挣扎"很久，总时间远超预期
可能原因	(1) 缩减因子 $\beta$ 太激进（如 0.01）——相邻阶段跳跃太大，warm-start 失效；(2) 初始 $t_0$ 太小——一开始就接近病态；(3) 每阶段收敛容差设置不当（早期阶段解得过精，浪费时间）
排查步骤	1. 检查 $\beta$ 值——是否过小（< 0.05 偏激进）；2. 检查 $t_0$——是否过小（< $10^{-3}$ 偏小）；3. 检查每阶段是否都解到了过高精度（早期不必）
修法	调 $\beta\sim0.1$（适中）、$t_0\sim10^{-2}$；用 inexact continuation——早期阶段（$t_k$ 大）松一点快速推进，后期才收紧容差；保证每阶段用上一阶段解 warm-start。参数调度是一门手艺（§6.4）
相关章节	§6.4（松弛参数调度、"理论-工程桥接"）、§6.8（continuation 相关诊断）

故障排查手册使用说明：遇到 CI-TO/CI-MPC 问题时，先按"症状"列匹配场景，定位最可能的病因，再按"排查步骤"做鉴别（尤其注意场景一 vs 场景三的 A/B 区分、场景四的"无接触对照"判定 D、场景二的"换初值"判定 C），最后按"修法"对症处理并回"相关章节"深读原理。记住 §6.8 的核心原则——四类病因正交，按 D→A/B→C 顺序逐层剥离，不要在错误的维度上浪费时间。

本章收束语：从 §6.0 的"三十年三条支流首次汇流"到这里的故障排查手册，我们走完了"接触隐式"从动机、形式化、病态、救援、对照、实时化、诊断到桥接的完整链路。如果说全章只留下一句话，那就是——互补约束是接触决策的通用语言，掌握它（连同它的病态根源、磨圆之道、局部化实时化、四类病因诊断、以及向概率/结构/数据三个下游的延伸），你就握住了让机器人在接触环境中自主"踩、推、抓、爬、扶"的钥匙。接下来，带着这把钥匙去 04 批为它注入概率、去 05 批为它注入足式结构、去第八批为它注入数据——同一套互补约束，在每个下游等你继续书写。

维度	Scholtes 松弛	Lin-Fukushima 光滑化
改造对象	约束右端（\(=0\to\le t_k\)）	约束函数（用光滑 \(\psi_\mu\) 替换）
约束类型	保留不等式结构	变成单个光滑等式
可行集	L 形加厚成带状	L 形磨成圆角曲线
同伦参数	\(t_k\downarrow 0\)	\(\mu\downarrow 0\)
实现难度	最简（改右端）	中（换函数、算新雅可比）
收敛稳定性级别	C-stationarity（一般），B（强条件）	C-stationarity（一般）
工程偏好	默认首选	备选，某些问题更快

路	怎么处理 \(\phi\lambda=0\)	同伦参数	收敛到	工程地位
Scholtes 松弛	放松成 \(\phi\lambda\le t_k\)	\(t_k\downarrow0\)	C-stat（一般），B-stat（强条件）	默认首选
Lin-Fukushima 光滑	换成光滑 \(\psi_\mu=0\)	\(\mu\downarrow0\)	C-stat	备选
\(\ell_1\) 罚 / elastic	移进目标罚 \(\rho\\|\phi\lambda\\|_1\)	\(\rho\uparrow\)（精确罚有限即可）	C-stat	SNOPT 内置

困难类型	互补松弛	混合整数
病态根源	CQ 失效（双活点尖角）	组合爆炸（\(2^{Nn_c}\) 个分支）
病态表现	乘子爆炸、SQP 停滞	分支定界树指数膨胀
big-M 影响	无 big-M	\(M\) 太大 → 线性松弛弱 → 分支多；\(M\) 太小 → 切掉可行解
全局最优	不保证（局部解）	保证（含 optimality gap 证书）
规模扩展	变量随 \(N\) 线性，但每步病态	变量随 \(N\) 线性，但求解时间随二元变量指数（最坏）
实时性	可（C3/IDTO/Fast-CI 等特化算法）	难（除非接触模式数极少或用学习加速）

本章通用概念（§6.x）	足式落脚的特化（05 批 §150）	特化利用了什么足式结构
互补约束 \(0\le\phi\perp\lambda\ge0\)（§6.2）	脚-地面接触互补	接触点是离散的脚端
接触模式涌现（§6.1）	步态/接触序列涌现	左右脚交替的周期先验
混合整数编码（§6.5）	MICP 选可踩凸区域	地形分解成离散可行区域
bilevel 实时化（§6.7）	上层选落脚区域 / 下层优化轨迹	落脚点选择与轨迹优化的天然分层
病态诊断（§6.8）	落脚 CI-TO 的局部极小（怪步态）	接触序列盆地有步态语义
chance-complementarity（桥一）	不确定地形上的鲁棒落脚	地面高度/摩擦的感知不确定性

批次/章节	同一个互补约束的化身	任务语义	在它之上"加"了什么
本目录 §10（Signorini）	\(0\le\phi\perp\lambda_n\ge0\)	仿真：正向求解接触力	无（裸的互补，仿真时正解）
本章 §6.2（CI-TO）	\(0\le\phi\perp\lambda_n\ge0\)	优化：作约束嵌入 NLP	嵌入优化（§6.4 松弛化求解）
04 批 U3（chance）	\(\Pr[\,\text{互补可行}\,]\ge1-\delta\)	鲁棒：高概率满足	加概率（间隙/摩擦不确定→裕度）
05 批 §150（落脚）	\(0\le\phi_{\text{脚}}\perp\lambda_{n,\text{脚}}\ge0\) + 区域选择	足式：选落脚 + 接触	加结构（MICP 选可踩区域）
第八批（学习）	互补作可微层 / 软化先验	学习：策略/残差	加数据（学习接触模式先验）

符号	含义	首次出现
\(\phi\) / \(\phi_i\)	间隙距离（gap），脚到接触面的有向距离；\(\ge0\) 表示无穿透	§6.2
\(\lambda\) / \(\lambda_n\)	法向接触力（\(\ge0\)，只能推不能拉）	§6.2
\(\lambda_t\)	切向（摩擦）力	§6.2
\(\mu\)	库仑摩擦系数	§6.2
\(0\le a\perp b\ge0\)	互补条件：\(a\ge0,b\ge0,ab=0\)	§6.2
\(h_k=0\)	第 \(k\) 步离散动力学约束（时步法嵌入）	§6.2
\(J_c(q)\)	接触雅可比（把广义速度映到接触点速度）	§6.7
\(t_k\)	Scholtes 松弛参数（\(\phi\lambda\le t_k\)，\(\downarrow0\)）	§6.4
\(\mu\)（光滑化）/ \(\psi_\mu\)	Lin-Fukushima 光滑参数 / 光滑化 FB 函数	§6.4
\(\psi_{FB}\)	Fischer-Burmeister 函数 \(a+b-\sqrt{a^2+b^2}\)	§6.4
\(\rho\)	罚因子（\(\ell_1\) 罚 / elastic mode）	§6.4
\(M\)	混合整数 big-M 常数	§6.5
\(\Xi\)	saltation 矩阵（穿越 guard 的敏感度传播）	§6.7
\(V_{xx}\)	值函数对状态的二阶导（Riccati 递推核心）	§6.7
LCS	线性互补系统（C3 的局部线性化模型）	§6.7
\(z_k,\delta_k\)	C3 的光滑副本 / 互补副本（ADMM consensus）	§6.7
\(w_k\)	C3 的 ADMM 对偶变量	§6.7
\(\mathcal{X}_f\)	控制不变终端集（递归可行性 T9）	§6.8
\(\delta\)（机会约束）	允许的约束违反概率（chance-complementarity）	§6.9
\(\Phi^{-1}\)	标准正态分位数函数（机会约束确定性等价）	§6.9
\(\mu_{\text{eff}}\)	收缩后的有效摩擦系数 \(\bar\mu-\Phi^{-1}(1-\delta)\sigma_\mu\)	§6.9

定理/公式	一句话说明	对应节
T1 MPCC 的 MFCQ 失效	互补约束在双活点 \(\nabla(\phi\lambda)=0\)，MFCQ 必然失效	§6.3
T2 Scholtes 收敛	\(t_k\to0\) 时松弛解聚点是 C-stationary（强条件下 B-stationary）	§6.4
T8 ADMM 非凸残差趋零	非凸可分问题下 ADMM 残差趋零（C3 收敛依据，Wang-Yin-Zeng 2019）	§6.7
T9 CI-MPC 递归可行性	控制不变终端集（含可行接触模式）+ 终端约束 ⟹ 递归可行	§6.8
T6 saltation 矩阵（引自 §50）	穿越 guard 时敏感度乘 \(\Xi=\partial R/\partial x+\frac{(f^+-Rf^-)\nabla g^\top}{\nabla g^\top f^-}\)	§6.7 引用
T7 时步法能量不增（引自 §30/§50）	Stewart-Trinkle LCP 隐式积分保证刚性接触不穿透、能量不增	§6.2 引用
MPCC 稳定性层级	S ⟹ M ⟹ C ⟹ W（从强到弱）	§6.3
chance-complementarity 等价	\(\Pr[\\|\lambda_t\\|\le\mu\lambda_n]\ge1-\delta\iff\\|\lambda_t\\|\le\mu_{\text{eff}}\lambda_n\)	§6.9

项目	内容
症状	求解器迭代若干步后乘子（对偶变量）范数从 \(O(10^2)\) 单调飙到 \(O(10^6)\) 以上甚至 `inf`，SQP 步长降到 \(10^{-12}\)，最终报 `restoration failed`（IPOPT）或 `infeasible problem`（SNOPT）
可能原因	病因 A（CQ-病态）——把 raw MPCC（未松弛的互补约束）直接喂给了通用 NLP 求解器，MFCQ 在双活点必然失效（定理 T1），乘子集合无界
排查步骤	1. 确认互补约束是否做了松弛——检查约束里是否还是 \(\phi\lambda=0\)（硬等式）；2. 观察乘子爆炸是否发生在接触 make/break 的时间步（双活点）；3. 若是，确诊病因 A
修法	加 §6.4 的 Scholtes 松弛（\(\phi\lambda\le t_k\)）+ continuation 循环（\(t_0\sim10^{-2}\)，\(\beta\sim0.1\)）；或换专门的 MPCC/接触求解器（CALIPSO、Dojo 定制 IPM）。绝不要靠换通用求解器（SNOPT/Knitro）解决——病因 A 是结构性的
相关章节	§6.3（定理 T1，病态证明）、§6.4（松弛救援）、§6.8（病因 A 鉴别）

项目	内容
症状	Scholtes 松弛的 continuation 外循环在某个中间 \(t_k\) 阶段无法收敛，或相邻阶段间求解器要"重新挣扎"很久，总时间远超预期
可能原因	(1) 缩减因子 \(\beta\) 太激进（如 0.01）——相邻阶段跳跃太大，warm-start 失效；(2) 初始 \(t_0\) 太小——一开始就接近病态；(3) 每阶段收敛容差设置不当（早期阶段解得过精，浪费时间）
排查步骤	1. 检查 \(\beta\) 值——是否过小（< 0.05 偏激进）；2. 检查 \(t_0\)——是否过小（< \(10^{-3}\) 偏小）；3. 检查每阶段是否都解到了过高精度（早期不必）
修法	调 \(\beta\sim0.1\)（适中）、\(t_0\sim10^{-2}\)；用 inexact continuation——早期阶段（\(t_k\) 大）松一点快速推进，后期才收紧容差；保证每阶段用上一阶段解 warm-start。参数调度是一门手艺（§6.4）
相关章节	§6.4（松弛参数调度、"理论-工程桥接"）、§6.8（continuation 相关诊断）