Push Anything: Single- and Multi-Object Pushing From First Sight with Contact-Implicit MPC

该论文提出了名为 C3+ 的增强型接触隐式模型预测控制算法，结合完整的感知与执行流程，实现了在无需预知物体物理属性的情况下，对单个或多个任意几何形状物体的实时精准推挤操作，并在硬件实验中取得了 98% 的成功率。

要点

这篇论文介绍了一个名为 "Push Anything"（推一切） 的机器人系统。简单来说，它让机器人学会了像人类一样，用“推”的方式在桌子上整理一堆乱七八糟的东西，而且不管这些东西形状多奇怪、数量有多少，它都能搞定。

为了让你更容易理解，我们可以把这项技术想象成一个超级聪明的“整理收纳师”。

1. 核心挑战：为什么“推”东西这么难？

想象一下，你面前有一堆形状各异的物体：有的像鸡蛋（圆圆的），有的像积木（方方的），有的像易拉罐（滑溜溜的）。你想把它们推到指定的位置。

• 难点在于“接触”：当你推一个东西时，它可能会撞到另一个东西，那个东西又可能会撞墙，或者两个东西互相卡住。
• 以前的机器人：要么只能推形状规则的单一物体（像推一个完美的球），要么在碰到复杂情况（比如一堆东西混在一起）时就会“死机”或乱推。因为它们无法预测“推一下会发生什么连锁反应”。

2. 解决方案：三个步骤的“整理魔法”

这个系统就像是一个拥有三只眼睛和一个超级大脑的整理师：

第一步：超级视力（扫描与重建）

• 以前：机器人需要预先知道每个东西的精确图纸（CAD 模型）。如果桌上有个没见过的玩具，机器人就懵了。
• 现在：机器人自带“扫描眼”（摄像头）。它看一眼桌上的物体，就能在几秒钟内画出它的 3D 模型（就像用橡皮泥捏出物体的形状），并记住它在哪里。
  • 比喻：就像你走进一个陌生的房间，不用看说明书，一眼就能看出桌上的杯子、书本和玩偶长什么样，并且知道它们的位置。

第二步：超级追踪（盯着不放）

• 挑战：当机器人开始推东西时，物体可能会互相遮挡（比如推 A 挡住了 B），或者机器人自己的手臂挡住了视线。
• 现在：系统使用了先进的算法，即使物体被挡住了一部分，它也能“脑补”出物体的位置，并持续追踪，不会跟丢。
  • 比喻：就像在拥挤的人群中玩捉迷藏，即使朋友被其他人挡住了一瞬间，你也能立刻猜出他下一秒会出现在哪里，而不是跟丢了。

第三步：超级大脑（C3+ 算法）

这是论文最核心的创新，叫 C3+。

• 以前的算法：就像是一个近视眼的棋手。它只能看到眼前一步（比如“推一下，碰到墙”），如果这一步看起来不好，它就不敢动，结果卡死在原地。
• C3+ 算法：像是一个拥有“上帝视角”的象棋大师。
  • 它能同时计算成千上万种可能性：“如果我往左推一点，A 会撞到 B，B 会滑到 C 下面，然后 C 正好到了目标位置……"
  • 关键突破：以前的这种计算太慢了，算一次要几分钟，机器人等不起。C3+ 发明了一种**“数学捷径”，把原本需要超级计算机算很久的复杂物理碰撞问题，变成了瞬间能算完的简单公式**。
  • 比喻：以前解这道物理题像是要手算一万次乘法，现在 C3+ 就像是用计算器按了一下，瞬间得出答案。这让机器人能实时（Real-time）思考，一边推一边调整策略。

3. 实验成果：它有多强？

研究人员在真实的实验室里测试了这个系统：

• 对象：33 种不同的物体，包括字母积木、瓶子、玩具、甚至是一盒鸡蛋。
• 场景：从推 1 个物体，到同时推 4 个物体互相“打架”的复杂场景。
• 成绩：
  • 成功率：高达 98%！几乎每次都能成功。
  • 速度：
    • 推 1 个物体：平均 0.5 分钟。
    • 推 4 个物体：平均 5.3 分钟。
  • 精度：能把物体推到离目标只有几厘米、几度误差的地方，非常精准。

4. 总结：这意味着什么？

这项技术让机器人不再只是“听话的机械臂”，而是变成了能处理混乱环境的智能助手。

• 以前：机器人只能在整洁、规则的环境里工作（比如工厂流水线）。
• 现在：机器人可以进入你的家，帮你把散落在沙发上的抱枕、书本、遥控器一个个推回原位，哪怕它们形状怪异、互相堆叠。

一句话总结：
这篇论文教会了机器人一套**“边看、边想、边推”**的本领，通过一种超快的数学算法，让它能像人类一样灵活地处理桌面上杂乱无章的物体，把“推东西”这件看似简单的事，变成了高精度的艺术。

技术摘要

1. 研究背景与问题定义 (Problem)

核心挑战：
非抓取式（Non-prehensile）机器人操作，特别是平面推物体（Planar Pushing），是机器人学中的核心难题。其难点在于：

• 未知物理属性： 物体几何形状、质量、惯量等往往未知。
• 接触复杂性： 涉及丰富的接触交互（物体 - 环境、物体 - 物体、物体 - 机械臂末端），导致动力学高度非线性且不连续（如粘滞、滑动、分离）。
• 多物体场景的扩展性： 现有的接触隐式模型预测控制（CI-MPC）方法通常局限于单一物体或已知精确几何模型的简单场景。随着接触数量增加，多物体推挤任务的组合复杂度呈指数级增长，导致传统方法无法处理（Intractable）。

现有方法的局限：

• 基于模型的方法： 显式建模物理接触，但难以处理混合动力学的组合爆炸问题。
• 数据驱动方法（RL）： 虽然泛化性较好，但通常数据需求大，且多局限于单物体任务，难以解决通用的多物体重排问题。
• 传统 CI-MPC： 依赖局部动力学近似，容易陷入局部最优，难以规划长时程（Long-horizon）的复杂接触策略。

---

2. 方法论 (Methodology)

作者提出了 "Push Anything" 系统，这是一个集成了感知、重建和控制的完整流水线，旨在实现实时、高精度的多物体平面推挤。

A. 感知与重建流水线 (Perception Pipeline)

1. 物体网格重建： 使用 RealSense D455 相机拍摄视频，通过 XMem 进行视频分割，利用 BundleSDF 从 RGB-D 数据中重建物体网格（Mesh），并生成 URDF 模型。
2. 鲁棒的多物体跟踪： 并行运行多个 FoundationPose 实例进行姿态估计。为了解决遮挡和漂移问题，集成了 XMem 进行周期性的掩码重注册，并设计了逻辑来纠正对称物体的姿态歧义（如 z 轴方向）。

B. 控制器架构：基于采样的 CI-MPC

系统采用两阶段策略：

1. 离线阶段： 构建物体库（网格、URDF）。
2. 在线阶段：
   • 末端执行器采样策略： 在物体表面随机采样点，沿法向投影到固定高度，生成候选的末端执行器位置。这避免了局部 MPC 陷入局部最优。
   • 局部 CI-MPC 求解： 对每个采样点求解接触隐式 MPC，选择成本最低的路径。

C. 核心创新：Consensus Complementarity Control Plus (C3+)

这是本文最核心的算法贡献，是对前作 C3 的增强版，旨在解决多物体接触下的实时求解难题。

• 问题建模： 将接触动力学建模为线性互补系统（LCS），并结合二次成本函数，形成带互补约束的二次规划（QPCC），进而转化为混合整数二次规划（MIQP）。
• C3+ 的改进机制：
  • 引入松弛变量： 在互补约束中引入松弛变量 $\eta_k$，将非凸的互补约束解耦。
  • ADMM 优化： 利用交替方向乘子法（ADMM）将问题转化为共识形式。
  • 投影步骤的解析解： 关键突破在于，C3+ 将原本耦合的、计算昂贵的 MIQP 投影步骤，转化为独立的、一维的解析计算。
    • 传统 C3 的投影步骤需要求解复杂的优化问题。
    • C3+ 的投影步骤（Eq. 12）可以直接通过闭式公式计算，将计算复杂度从指数级降低为常数级。
  • 早期终止策略： 为了保持实时性，算法在少量固定迭代次数后终止，接受次优但足够好的解。

---

3. 主要贡献 (Key Contributions)

1. Push Anything 系统： 首个能够处理从单物体到多物体（最多 4 个物体，19 对接触对）的实时平面推挤全流程系统，集成了真实世界的扫描、重建和跟踪。
2. C3+ 算法： 提出了一种增强的 CI-MPC 算法。通过引入松弛变量和解析投影步骤，显著加速了求解过程，使得在长时程规划中处理大量物体间及物体 - 环境接触成为可能。
3. 硬件验证： 在 Franka Emika Panda 机械臂上进行了大规模实验，验证了系统在复杂几何形状和多变初始状态下的鲁棒性。

---

4. 实验结果 (Results)

实验在 Franka Panda 机械臂上进行，使用了 33 种不同的物体（包括凸/非凸形状、3D 打印字母、家居用品等）。

• 成功率：
  • 单物体任务： 在 701 次试验中，成功率为 99.9%（仅 1 次失败，因物体被推出工作区）。
  • 多物体任务： 在 227 次试验中（2-4 个物体），整体成功率为 98%（210/227）。
• 时间效率 (Time-to-Goal)：
  • 1 个物体：平均约 0.5 分钟 (31 秒)。
  • 2 个物体：平均约 1.6 分钟。
  • 3 个物体：平均约 3.2 分钟。
  • 4 个物体：平均约 5.3 分钟。
  • 注：时间随物体数量增加而非线性增长，因为涉及物体间的重排和路径规划复杂度。
• 求解速度对比 (C3 vs C3+)：
  • C3+ 的投影步骤（Projection Step）比 C3 快 4-5 个数量级。
  • 例如在 4 物体场景中，C3 的投影步骤平均耗时 44ms，而 C3+ 仅需 0.007ms。这使得 C3+ 能够维持实时的控制频率（约 14Hz）。
• 精度： 在紧密容差（位置误差 $\le$ 2cm，角度误差 $\le$ 0.1rad）下均能达到高精度目标。

---

5. 意义与局限性 (Significance & Limitations)

意义：

• 突破多物体操作瓶颈： 证明了接触隐式 MPC 可以扩展到复杂的多物体交互场景，无需预先规划接触模式序列。
• 实时性与鲁棒性： C3+ 算法通过数学上的优化，解决了长期困扰该领域的计算速度问题，使得在真实硬件上处理高维接触问题成为现实。
• 通用性： 系统能够处理“未见过的”物体（First Sight），仅需扫描即可重建模型并执行任务，无需人工标注物理参数。

局限性与未来工作：

• 感知依赖： 性能受限于 FoundationPose 的跟踪精度。在多物体严重遮挡时，跟踪漂移会影响控制效果。未来计划引入多视角跟踪。
• 物理参数假设： 假设所有物体具有相同的质量和惯量。对于物理属性差异巨大的物体，需要在线学习或自适应。
• 高层规划缺失： 当前系统缺乏高层任务规划（如“先推 A 再推 B"的序列决策），随着任务复杂度增加，效率可能下降。未来将结合高层推理。
• 维度限制： 目前仅限于平面（2D）推挤，未来目标是扩展到 3D 非抓取操作。

总结：
这篇论文通过引入 C3+ 算法和完整的感知 - 控制流水线，成功解决了机器人非抓取操作中“未知物体”和“多物体接触”的两大难题，实现了在真实硬件上对复杂场景的高精度、实时推挤操作，是接触丰富型机器人控制领域的重要进展。