IEEE Transactions on Robotics

Fast Contact-Implicit Model Predictive Control

作者：Simon Le Cleac'h, Taylor A. Howell, Shuo Yang, Chi-Yen Lee, John Zhang, Arun Bishop, Mac Schwager, Zachary Manchester · 机构：Department of Mechanical Engineering, Stanford University, Stanford, CA, USA; The Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA; Department of Aeronautics and Astronautics, Stanford University, Stanford, CA, USA

会议/期刊：IEEE Transactions on Robotics · 发布日期：2024 · 页码：1617-1629

运动规划控制优化仿生机器人

精读笔记

Problem Setting

Fast Contact-Implicit Model Predictive Control（IEEE Transactions on Robotics / 2024）关心的是接触丰富机器人在 MPC 中的模式选择问题：控制器不仅要跟踪轨迹，还要能在扰动、地形变化或参考失配时在线改变接触时序和接触力。

关键矛盾是：接触模式自由度越大，优化问题越接近 nonsmooth/nonconvex contact-implicit planning；而 MPC 又要求每几十毫秒稳定返回可执行控制。传统线性/凸 MPC 通常把接触序列、足点选择或简化动力学外包给启发式，因此快但不真正 contact-implicit；完整 contact-implicit trajectory optimization 可以不预设模式，但在线不可靠；MIQP/hybrid 枚举模式表达清楚但随接触数爆炸。

这篇论文真正要打通的是一个中间层：让 MPC 在不枚举模式、不固定 gait 的情况下，仍能用一个局部近似的接触动力学模型在线做模式调整。

Motivation

已有路线的缺口不是“没有接触模型”，而是没有一个能同时满足三件事的模型：可快速 rollout、可给优化器提供有效梯度、仍保留接触切换的自由度。

作者的核心观察是：MPC 中的模型不必是全局精确物理模型。类似 linear MPC 对光滑系统有效，接触系统也可以围绕 reference 做局部近似；但不同于普通线性化，不能把接触模式也冻结，否则就退化成固定 contact schedule tracking。因此应当线性化连续动力学和几何项，却显式保留 complementarity 结构。

另一个动机是梯度问题。硬接触在模式边界处给出的 subgradient 对二阶优化很差；interior-point 中间解虽然物理上是软化接触，但对优化器更有信息。这篇论文隐含的立场是：对控制而言，有用的平滑近似比物理上“更硬”的接触更重要。

Core Idea

核心思想可以概括为：用 reference-local LCP 替代 online nonlinear contact dynamics，并用 interior-point central path 把接触模式切换变成可优化的平滑近似。在线 MPC 不是解原始 contact-implicit NLP，而是在短 horizon 内反复调用这些 time-varying LCP dynamics，做 tracking optimization。

这改变了建模的信息流：reference 提供局部动力学、几何和摩擦锥近似；在线状态偏差和控制输入通过 LCP 决定下一步接触力和接触状态；上层 optimizer 通过隐式微分获得 smooth dynamics Jacobian。换句话说，reference 不是规定接触序列，而是提供一个局部物理坐标系，互补约束仍允许 planner 偏离原 contact mode。

和 prior 的本质区别在于，它不是靠枚举 hybrid modes，也不是靠固定 contact schedule 的 convex MPC，而是把“模式选择”编码在每一步 LCP oracle 中，再通过 direct trajectory optimization 使用这个 oracle。它的 inductive bias 是 rigid contact complementarity，而不是 gait heuristic。

Method

1. Time-varying LCP contact dynamics：论文把 mass matrix、bias、Jacobian、input map、friction basis、signed-distance linearization 都围绕参考轨迹预计算。这样在线 dynamics evaluation 只需解一个线性互补问题。它解决的是完整非线性接触动力学在线过慢的问题，同时避免把接触状态直接固定。

2. Structure-exploiting interior-point solver：LCP 的 Newton system 中大量块在 reference-local formulation 下固定，可以离线预分解；在线只更新和 complementarity slack 相关的对角项。这个部分主要是 engineering/scaling，但很关键，因为没有它 CI-MPC 很难达到实时。实质贡献在于把 contact dynamics oracle 的代价降到能被 MPC 高频调用。

3. Smooth gradients through central-path solutions：论文对非零 κ 的 interior-point 中间解做 implicit differentiation，而不是对硬接触极限解求 subgradient。它解决的是模式切换附近优化器没有方向信息的问题。这里的核心变化是把 contact discontinuity 转换为一种可调 smoothness 的 planner model。

4. Bilevel direct MPC tracking：上层优化状态/控制轨迹，下层 LCP 给出一步 dynamics。使用 Gauss–Newton/KKT sparse solve、warm start、固定迭代数和短 horizon。这里的重点不是求全局最优，而是用 test-time compute 快速修正 reference tracking，并在局部发现新接触。

Key Insight / Why It Works

最重要的 insight 是：contact-rich MPC 不一定要在线求精确硬接触；它需要的是一个能够在接触事件前后提供“可行动梯度”的模型。非零 κ 的 interior-point 解本质上是一种接触软化/regularization，它让优化器在未接触但接近接触、即将滑动、即将脱离等区域仍看到连续变化的力学响应。这很可能是方法有效性的核心，而不仅是数值细节。

第二个 insight 是局部线性化不能简单套用在接触系统上。普通 dynamics linearization 会冻结当前模式，无法生成新接触；CI-MPC选择线性化连续物理项，但保留 complementarity。这个拆分很漂亮：它把昂贵的非线性结构变成 reference-local data，把离散模式自由度留给在线 LCP。可迁移的思想是“线性化模型参数，不线性化模式逻辑”。

第三，实时性主要来自结构重用和短视窗重规划，而不是某个全新优化理论。预分解、fixed iteration、warm start、coarse tolerance、短 horizon 都是工程上必要的 scaling trick。论文的理论新意和工程收益是耦合的：如果没有 reference-local structure，预计算不可行；如果没有预计算，contact-implicit MPC 不实时。

我会把贡献排序为：第一是 central-path smooth contact gradients for MPC；第二是 reference-local LCP 保留模式切换；第三是结构化 solver 工程。contact-height heuristic、固定迭代数等属于辅助。硬件性能的一部分可能来自高频 replanning、简化 point-foot model 和低层控制器，而不完全是 contact-implicit planner 本身，文中未充分做归因分离。

Relation To Prior Work

这篇论文处在三条谱系的交叉处：linear MPC/LQR tracking、LCP-based rigid contact simulation、contact-implicit trajectory optimization。

相对经典 legged MPC，它最大的差异是接触序列不是外部给定或由 Raibert heuristic 产生，而是由 LCP dynamics 在优化过程中隐式决定。因此它比固定 gait convex MPC 更一般，但也缺少成熟 locomotion pipeline 中那些强启发式，例如在线 foothold selection。

相对 contact-implicit trajectory optimization，它不是把所有时间步的互补约束直接塞进一个大型非凸 NLP，而是把每一步接触动力学封装成可微 oracle，并在短 horizon tracking 中使用。这牺牲了全局规划能力，换来在线可靠性。

相对 MIQP/hybrid methods，它不显式枚举模式，因此避免组合爆炸；但代价是优化问题仍是非凸近似，且模式选择通过软化互补和局部梯度间接完成，没有全局最优保证。

相对 rollout-based DDP / relaxed complementarity 方法，它采用 direct trajectory optimization，并强调可预计算的 time-varying LCP 结构。真正新增的信息不是“LCP 可微”本身，这已有工作很多，而是把可微 LCP 做成一个实时 MPC 可调用的局部 contact dynamics layer。

Dataset / Evaluation

评估覆盖面比较强：从极简 pushbot 到 hopper、平面四足、平面双足，再到 Unitree Go1 真机。任务也不只是周期 gait，包括 push recovery、stair/flip、wall stand、step placement 等非周期行为。这支持其“formulation 跨系统”的 claim。

最能验证核心 claim 的实验是：reference 中没有某些接触，但受扰后 policy 在线生成新 contact sequence。PushBot 和四足受扰 contact sequence 图比单纯 tracking error 更有说服力，因为它们直接展示了 contact-implicit 的价值。

不过 evaluation 仍有边界。真机使用的是简化 point-foot 模型，忽略完整腿部动力学，并依赖已有低层控制器、状态估计和外部 motion capture。它验证了实时 CI-MPC policy 可以接入硬件，而不是验证 full-order contact-implicit MPC 可以直接替代现有 whole-body locomotion stack。

对 baseline 的比较也应谨慎。MIQP 在 pushbot 上慢并不意外；Raibert/Pratt baseline 是系统特定启发式，CI-MPC 的优势更多体现在 general formulation 和非周期任务，而不是全面 locomotion robustness。作者自己也指出经典 convex MPC 对 general locomotion 更稳，这削弱了“实际替代性”的强 claim，但不削弱方法机制价值。

Limitation

核心前提一：必须有高质量 reference trajectory。CI-MPC 是 tracking controller，不是从任务目标出发的全局 planner。离线 contact-implicit TO 的不可靠性被保留下来，只是从 online 移到了 offline。方法没有解决 reference generation 的根问题。

核心前提二：局部近似必须覆盖真实执行偏差。M、J、friction cone、signed distance 都围绕 reference 近似；如果偏差过大、接触几何复杂、长 horizon 需要远离 reference，LCP oracle 的物理意义会下降。论文展示短 horizon 0.1–0.25s 有效，但这也说明能力更像 local reactive correction，而不是长程 contact reasoning。

核心前提三：软化接触梯度的 κ 选择是关键但缺少理论刻画。κ 太小回到不可微信息贫乏，太大则物理失真。文中经验上选择约 1e-4，但增益来源不清：是更真实的接触模型，还是更好的优化 regularizer？文中未充分说明。

scalability 上限主要在 contact 数和模型保真度。虽然 solver 复杂度经过压缩，但多接触三维全身系统、复杂接触面、二阶摩擦锥、手部操作等场景仍可能使 LCP 维度和线性化误差变成瓶颈。

泛化也应理解为“同一数学框架可迁移”，不是 learned policy 意义上的泛化。每个系统仍需要模型、reference、cost tuning 和 low-level execution interface。硬件上甚至承认缺少 Raibert foothold strategy 会使 robustness 不如经典 convex MPC，说明目前 CI-MPC 更像一个可表达接触模式的 tracking layer，而不是完整 locomotion solution。

Takeaway

  • 1. 最值得迁移的思想是：在接触系统中，可以线性化连续动力学和几何数据，但保留互补结构作为模式选择机制。
  • 这比直接线性化整个 dynamics 更有接触泛化能力。
  • 2. 对 nonsmooth control，优化器需要的往往不是硬物理极限，而是可调平滑度的物理近似。
  • interior-point central path 可以被视作一种 principled contact smoothing，用于提供穿越 contact events 的梯度信息。

一句话总结

这篇论文把 contact-implicit planning 从离线非凸优化推进到可实时运行的局部 MPC 层，其实质贡献是用 reference-local LCP 和 interior-point soft gradients 在速度、可微性与接触模式自由度之间做了一个有效折中。