💻 computer science

ContactIPM: A Structure-Exploiting Interior-Point Solver for Contact-Implicit Trajectory Optimization

ContactIPM 是一种新型的利用结构特性的内点求解器，它结合了阶段性最优控制分解与原对偶一致性，用以高效且稳健地求解接触隐式轨迹优化问题，并证明了其相比现有特定于接触的求解器具有显著的速度提升和更高的成功率。

原作者： Yucheng Chen

发布于 2026-08-13

📖 1 分钟阅读☕ 轻松阅读

原作者： Yucheng Chen

原始论文采用 CC BY 4.0 许可（http://creativecommons.org/licenses/by/4.0/）。 ✨ 这是对下方论文的AI生成解释。它不是由作者撰写或认可的。如需技术准确性，请参阅原始论文。阅读完整免责声明

想象一下，一个机器人正试图将一个沉重的箱子推过房间。对人类来说，这看起来很简单：抓取、推动、滑动、停止。但对于机器人的计算机大脑而言，这是一场数学噩梦。机器人必须精确地决定何时手部接触到箱子、推力有多大，以及何时松手，同时还要计算如何移动自身的关节。这被称为“接触隐式轨迹优化”（contact-implicit trajectory optimization）。把它想象成在要求一个 GPS 不仅仅是寻找一条路线，还要决定是开车走马路、步行走人行道，还是飞越篱笆，同时还要计算每一秒钟的完美速度。

难点在于“接触”。在数学中，触摸某物是一个被称为“互补性”（complementarity）的奇特条件。它就像一条规则在说：“你要么正在接触箱子（力 > 0），要么没有接触（间隙 > 0），但你永远不能同时处于这两种状态。”这创造了一个数学上的死胡同，让标准的计算机求解器感到困惑并陷入停滞，就像一辆车试图行驶在一条突然消失的道路上一样。长期以来，科学家们不得不面临两个糟糕的选择：使用一个会在“接触”规则上卡住的超级智能求解器，或者使用一个忽略问题结构但计算极其缓慢的简单求解器。

于是，由 Yucheng Chen 开发的新工具 ContactIPM 登场了，它扮演着这些机器人问题“大师级机械师”的角色。ContactIPM 并没有在速度和智能之间做选择，而是将两者结合了起来。它不把“接触”规则视为一道坚硬、不可逾越的墙，而将其视为一根具有弹性的橡皮筋。这使得计算机能够轻轻地引导机器人的计划穿过那些决定“接触或不接触”的棘手时刻，而不会被卡住。一旦找到计划，它又会弹回真实的物理规则，以确保机器人的动作确实可行。

论文表明，这种方法是一个游戏规则的改变者。在与其他顶尖机器人求解器的对比测试中，ContactIPм 的速度通常快了 2 到 8 倍。在一次涉及机器人推箱子的特定测试中，即使在受到噪声或误差干扰导致机器人产生困惑的情况下，它平均仅用 2.08 毫秒 就解决了问题。最令人印象深刻的是，在 50 次 模拟机器人必须从错误中恢复的困难且接近现实世界的测试中，ContactIPM 在 全部 50 个案例 中都取得了成功，而其他方法有时会放弃或无法找到有效的路径。这证明了你并不需要为了让机器人处理“接触物体”这种混乱、粘滞的现实情况而牺牲速度。

技术摘要：ContactIPM

问题陈述

接触隐式轨迹优化（Contact-implicit trajectory optimization, CITO）旨在同时优化机器人状态、输入、接触力及接触模式，而无需预设接触序列。这种灵活性引入了数学规划中的互补约束问题（Mathematical Programs with Complementarity Constraints, MPCCs），其中单边接触和摩擦通过互补条件（例如 $0 \le a \perp b \ge 0$ ）进行建模。

这些问题存在一个根本性的数值挑战：在可行的 MPCC 点处，激活的非负性和乘积约束的梯度在结构上是相关的，导致标准的约束限定条件（如 LICQ）失效。这种退化现象使得传统的原对偶非线性规划求解器变得不可靠，经常导致 KKT 系统出现奇异或病态。

现有方法通过放弃完全耦合的原对偶 Newton 步来解决这一鲁棒性问题。例如，CRISP 通过凸子问题仅优化原轨迹；IMPACT 则使用增广拉格朗日外层循环配合块坐标下降法。相反，利用结构特性的最优控制求解器（如 acados）利用 Riccati 递归来实现高效性，但在不进行特殊处理的情况下，若强制执行精确的互补性，往往无法生成有效的接触。在 MPCC 特有的鲁棒性与结构化最优控制（OCP）求解器的高效性之间仍存在差距。

方法论：ContactIPM

ContactIPM 提出了一种统一的原对偶内点法，该方法在处理互补约束的同时，保留了 Riccati 递归所需的块带状 OCP 结构。

1. 屏障耦合弹性内部松弛（Barrier-Coupled Elastic Interior Relaxation）

ContactIPM 没有使用惩罚权重或增广拉格朗日，而是为每个标记的互补对 $(a, b)$ 引入了阶段局部弹性内部松弛：

松弛变量： 引入正侧松弛变量（ $s_a, s_b$ ）和乘积松弛变量（ $s_c$ ）。
弹性提升（Elastic Lift）： 将互补条件转化为一个等式系统：
$g_a + s_a = 0, \quad g_b + s_b = 0, \quad g_a g_b - \theta(\mu) + s_c = 0$
其中 $\theta(\mu) = \gamma_{mpcc}\mu$ 将互补收紧程度直接与屏障参数 $\mu$ 挂钩。
正则性： 这种构造确保了提升后的约束 Jacobian 矩阵相对于松弛变量包含一个单位块。因此，松回后的问题在每个内部点都满足线性独立约束限定条件（LICQ），保证了唯一乘子和正则的原对偶系统。
无独立惩罚： 与惩罚法不同，互补性的收紧由定义中心路径的同一屏罚参数 $\mu$ 控制，消除了对独立调节惩罚系数的需求。

2. 阶段性原对偶消除与 Riccati 递归

求解器保持原对偶一致性，将轨迹、动力学乘子、不等式乘子及松弛变量作为单个扰动 KKT 系统的组成部分进行更新。

局部消除： 由于互补对及其弹性变量在各个阶段是局部的，求解器逐阶段消除局部松弛变量（ $\Delta s_k$ ）和对偶变量（ $\Delta \lambda_k$ ）。
简化系统： 这种消除仅修改局部阶段的海森矩阵（ $\tilde{H}_k$ ）和梯度（ $\tilde{q}_k$ ）。动力学仍然是相邻阶段之间唯一的耦合项，从而保留了块带状结构。
高效求解： 简化后的系统通过向后 Riccati 递归随后进行前向代入进行求解，实现了相对于时界长度 $N$ 的线性复杂度 $O(N)$ 。

3. 多阶段 MPCC 恢复机制

为了处理与不同接触模式相关的多个平稳分支，ContactIPM 采用了包含以下步骤的固定恢复策略：

延续（Continuation）： 逐渐收紧屏障参数，同时尝试留在当前的接触模式盆地（basin）内。
重启（Restart）： 如果延续失败（例如由于条件恶化），求解器会恢复原始原变量猜测值，并以较大的屏障参数和 Gauss–Newton 曲率重新开始，以探索不同的盆地。
终止（Termination）： 只有当未松弛的物理互补残差（ $r_{phys} = \max |a_k b_k|$ ）低于指定容差时，求解器才会终止，以确保最终轨迹满足物理接触模型，而非仅仅满足松回后的公式。

4. 缩放与全局化

该方法应用了基于阶段导数量级的时界一致性对角坐标缩放。全局化通过过滤线搜索（filter line search）实现，该方法在降低目标函数值与减少约束违反之间取得平衡，并结合分数边界规则（fraction-to-boundary rule）以维持正向松弛。

核心贡献

统一公式： 一种阶段性公式，将标记的互补对转化为非线性动力学、代价函数及不等式框架内的屏障耦合弹性乘积行。
感知 MPCC 的原对偶 Newton 法： 一种在利用局部松弛/对偶消除和 Riccati 递归的同时，保持耦合扰动 KKT 结构的算法。它包含了轨迹缩放预处理和多阶段恢复策略。
可复现的评估： 一个全面的实验套件，将 ContactIPM 与 CRISP、IMPACT 以及带有精确互补性的 OCP 结构化 acados 基准进行对比，包括在各种扰动下的 50 次闭环展开测试。

实验结果

对比 CRISP 的性能

速度： 在四个固定基准案例（Cartpole, Push Box, Transport, Push T）中，在 20 次配对计时重复测试下，ContactIPM 比 CRISP 快 2.17 倍至 8.87 倍。
鲁棒性： ContactIPM 在 Cartpole 和 Transport 上表现与 CRISP 持平，但在 5 个额外的 Push Box 案例中取得了成功（24/25 对比 19/25）。在 Push T 系列测试中，ContactIPP 在所有 50 个案例中均获得成功，而 CRISP 在实施严格物理互补性时仅在 27/50 的案例中成功。

对比 IMPACT 的性能

速度： ContactIPM 在 Push T 上快 2.96 倍，在 Cart Transport 上快 4.91 倍。然而，IMPACT 在 Push Box 上快 4.46 倍。
鲁棒性： 在三个 50 例系列的测试中，ContactIPM 在 150/150 个案例中均成功，而 IMPACT 为 147/150。

与 OCP 结构化基准（acados）的比较

当 acados 配置为精确互补性（无弹性松弛）时，它经常无法生成有效的接触。例如，在 Push Box 基准测试中，acados 通过了所有 25 个案例的物理可行性检查，但 0/25 个案例达到了终端目标，通常终止于零接触轨迹。
在 165 个接触丰富的案例（CRISP Transport, CRISP Push T, IMPACT Push T, 以及 IMPACT Cart Transport）中，acados 仅产生了 1 条被接受的轨迹，而 ContactIPM 成功了 158 条。这表明仅利用 OCP 结构不足以克服 MPCC 退化问题。

闭环验证

在涉及模型失配、噪声和状态重置的 50 次闭环 Push Box 展开测试中，ContactIPM 全部成功（50/50）。
延迟： 中位求解时间为 2.08 ms，99.37% 的求解满足 100 ms 的控制时限。
可行性： 闭环反馈保持了原变量可行性和物理互补性，最大物理互补乘积保持在 $1.01 \times 10^{-6}$ 以下。

意义与主张

本文主张，MPCC 鲁棒性、原对偶一致性与最优控制结构可以在同一个求解器中并存。ContactIPM 证明了将互补性处理与原对偶 Newton 步分离（如 CRISP 和 IMPACT 所见）并非不可避免的代价。通过结合弹性内部松弛、阶段性消除和 Riccati 递归，该求解器既实现了处理接触隐式问题所需的数值鲁棒性，又实现了结构化最优控制的高效计算。结果表明，即使在使用先进的 OCP 线性代数技术时，专门的 MPCC 处理也是必要的。