💻 computer science

High-Performance Dual-Arm Task and Motion Planning for Tabletop Rearrangement

本文提出了名为 SDAR 的同步双臂任务与运动规划框架，通过紧密耦合依赖驱动的任务规划器与基于 GPU 的同步双臂运动规划器，有效解决了桌面上强纠缠物体的非单调、长程重排难题，并在仿真与 UR-5e 硬件实验中实现了 100% 的成功率及远超现有技术的求解质量。

原作者： Duo Zhang, Junshan Huang, Jingjin Yu

发布于 2026-03-03

📖 1 分钟阅读☕ 轻松阅读

CC BY 4.0

原作者： Duo Zhang, Junshan Huang, Jingjin Yu

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

这篇论文介绍了一种名为 SDAR 的智能系统，它能让两台机器人手臂像人类的双手一样，默契配合，把桌子上杂乱无章的物体重新摆放整齐。

为了让你更容易理解，我们可以把这项技术想象成**“两位超级大厨在狭小的厨房里同时备菜”**。

1. 核心挑战：拥挤的厨房与复杂的依赖

想象一下，你有一个小厨房（桌面），里面堆满了各种食材（物体）。你的目标是把这些食材从“现在的乱堆状态”搬到“完美的摆盘状态”。

单臂 vs. 双臂：以前，机器人通常只有一只手（单臂），像是一个人在厨房干活，只能一次拿一个东西。现在，SDAR 让机器人拥有两只手（双臂），就像两个大厨同时工作。
难点：
- 空间狭小：两只手在同一个桌子上工作，很容易互相打架（碰撞）。
- 依赖关系：有些食材被压在下面，你必须先移开上面的才能拿到下面的。这就好比你想拿最底下的番茄，得先把上面的洋葱和土豆移走。如果顺序错了，整个计划就卡住了。
- 死循环：有时候，A 压在 B 上，B 又压在 A 上，或者它们互相挡路，这就形成了一个“死循环”，必须有人先暂时把东西放到旁边的空位（缓冲区）去，才能解开死结。

2. SDAR 的解决方案：两位大厨的“完美配合”

SDAR 系统由两个核心部分组成，它们就像是大厨的**“大脑”和“双手”**。

第一部分：任务规划器 (SDAR-T) —— 聪明的“总指挥”

这个“总指挥”负责制定策略。它不像普通机器人那样死板地按顺序干活，而是会先画一张**“依赖关系图”**。

剥洋葱法：它会把任务像剥洋葱一样分层。
- 第一层（独立物体）：那些谁也不挡、谁也不被挡的物体，直接拿走。
- 第二层（链条）：像多米诺骨牌一样，移走 A 才能拿 B，移走 B 才能拿 C。总指挥会安排两只手分别去拿链条两头的东西，或者按顺序接力。
- 第三层（死循环）：遇到互相挡路的死循环，总指挥会果断决定：“好吧，先把其中一个暂时放到旁边的空盘子里（缓冲区），等路通了再拿回来。”
双臂协同：最厉害的是，它不是先想好左手干什么，再想右手干什么，而是同时考虑两只手。它能计算出：“左手拿这个，右手拿那个，这样效率最高，总步数最少。”

第二部分：运动规划器 (SDAR-M) —— 灵活的“双手”

有了总指挥的指令，具体的“双手”（运动规划器）就要负责怎么动才不会撞到对方或桌子边缘。

GPU 加速的“超级大脑”：它利用强大的显卡（GPU）进行超高速计算。就像在几秒钟内模拟了成千上万种拿东西的姿势（比如从上面抓、从侧面抓、抓的角度是 30 度还是 45 度）。
层层筛选：它会从成千上万个方案中，瞬间挑出那个最顺滑、最快、最不容易撞车的动作。
自动纠错（ fallback）：如果两只手真的卡住了（比如手臂互相缠绕），系统不会死机，而是会启动“降级模式”：
1. 先尝试解开缠绕。
2. 如果还不行，就变成“左手做完再右手做”的单人模式，虽然慢一点，但保证能完成任务。

3. 惊人的效果：100% 的成功率

作者做了大量的实验，把桌子摆得乱七八糟，甚至故意制造“死循环”的难题。

对比结果：以前的机器人（单臂或简单的双臂系统）在遇到复杂情况时，经常卡住或失败（成功率只有 85%）。而 SDAR 在所有测试中都100% 成功完成了任务。
速度快：它不仅成功率高，而且速度极快。完成同样的任务，它比以前的方法快了3 倍。
真实世界验证：作者真的把这套系统装在了两台真实的机械臂（UR5e）上。在视频演示中，两台机械臂配合默契，把木块从拼成字母"I"变成了"C"，中间没有发生任何碰撞，动作流畅得像真人一样。

总结

简单来说，SDAR 就是给机器人装上了一个“双核处理器”：

一个超级聪明的策略家，能一眼看穿复杂的物体堆叠关系，规划出最高效的双手配合路线。
一个反应极快的执行者，能在毫秒级时间内算出无数种动作，选出最安全、最流畅的那一种，并且懂得在遇到死胡同时灵活变通。

这项技术让机器人从“笨拙的独臂侠”进化成了“灵巧的双手工匠”，未来它们可以真正走进我们的家庭或工厂，帮我们整理杂乱的桌面、装配精密的零件，甚至做更复杂的家务。

论文技术总结：SDAR 框架

1. 研究问题 (Problem)

本文针对**双机械臂桌面物体重排（Tabletop Rearrangement）任务中的任务与运动规划（TAMP）**挑战。

核心难点：
- 强纠缠依赖：物体的起始和目标配置存在复杂的依赖关系（非单调性），导致物体移动顺序受到严格限制。
- 组合爆炸：从单臂扩展到双臂系统，任务规划空间呈指数级增长（双臂可协作完成单臂无法完成的任务），且运动规划的自由度（DoF）从 6-7 增加到 12+，导致高维空间下的运动规划极其困难。
- 协同与避障：双臂在近距离工作，需同时处理双臂间的碰撞、物体间的遮挡以及双臂与环境的交互。
- 实时性要求：需要在保证高成功率的同时，实现近实时的规划速度。

2. 方法论 (Methodology)

作者提出了 同步双机械臂重排规划器（SDAR, Synchronous Dual-Arm Rearrangement Planner）。该框架采用分层架构，将任务规划器（SDAR-T）与运动规划器（SDAR-M）紧密耦合。

A. 依赖驱动的双臂任务规划 (SDAR-T)

依赖图（Dependency Graph, DG）构建：基于起始和目标配置构建有向图，节点代表物体，边代表依赖关系（即物体 $i$ 必须等物体 $j$ 移开后才能移动）。
分层分解策略：
- 独立对象：直接移除无依赖的物体。
- 链式结构：按顺序处理无环的依赖链。
- 环状结构（Cycles）：针对循环依赖（如 A 挡 B，B 挡 A），采用“破环”策略。对于长环，选择一个物体暂时移至缓冲区（Buffer），打破循环后再处理。
双臂协同分配：不同于先规划单臂最优解再分配，SDAR-T 直接利用双臂能力，通过节点移除排序和节点 - 臂分配，生成多个高质量的下一步子任务（Sub-tasks），旨在减少总操作步数。

B. 基于采样的同步运动生成 (SDAR-M)

硬件加速：利用 NVIDIA GPU 的 SIMD 技术（基于 cuRobo 库）进行大规模并行运动规划。
分层运动策略：
1. 子任务选择与实例化：对 SDAR-T 生成的子任务列表，采样多种抓取姿态（Approach Angles）和缓冲区位置，通过逆运动学（IK）可行性筛选出最优子任务。
2. 同步运动规划：尝试同时规划双臂的平滑轨迹。
3. 故障恢复机制（Robust Failure Recovery）：
  - 第一级回退：若同步规划失败（如发生臂间碰撞），调用 ARMUNTANGLE 模块，基于规则解耦双臂运动。
  - 第二级回退：若仍失败，退化为单臂顺序执行模式，牺牲部分并行性以确保任务完成。

3. 关键贡献 (Key Contributions)

依赖驱动的双臂任务规划：提出了一种直接针对双臂系统设计的任务分解算法，相比单臂最优规划器，显著减少了总操作步数，并有效处理了循环依赖。
基于采样的运动优化：结合 GPU 加速的 IK 计算和分层规划策略，在大量可能的抓取姿态和子任务中筛选出高质量运动方案，大幅提高了规划成功率。
鲁棒的故障恢复机制：设计了多级回退策略（同步 $\to$ 解耦 $\to$ 顺序执行），确保在复杂场景下仍能生成可行解，解决了传统方法在双臂碰撞时容易失败的问题。
端到端的高性能表现：实现了从仿真到真实硬件（UR5e）的无缝迁移，证明了框架的实用性和可靠性。

4. 实验结果 (Results)

成功率：在复杂的非单调、长视野桌面重排任务中，SDAR 实现了 100% 的成功率。相比之下，基线方法（基于单臂任务规划器）的成功率仅为 85%。
执行效率：
- 任务步数：SDAR-T 生成的任务计划步数通常少于或等于基线方法。
- 执行时间：相比基线方法，SDAR 将任务执行时间缩短了约 3 倍。
- 计算时间：单个子任务（双臂）的规划时间约为 5 秒，接近实时。
消融实验：
- 仅使用 SDAR-T 配合基线运动规划器，成功率显著下降，证明了运动规划的重要性。
- 仅使用基线任务规划器配合 SDAR-M（或 cuRobo），成功率极低（如 11%），证明了依赖驱动任务规划的关键作用。
- 实验表明，任务规划与运动规划的紧密集成是提升性能的核心。
真实机器人实验：在两台 UR5e 机械臂上成功完成了将 10 个木块重排为"ICRA2026"字样的任务，验证了框架在真实物理环境中的有效性。

5. 意义与展望 (Significance)

理论意义：解决了双机械臂 TAMP 中任务空间与运动空间耦合的复杂性难题，提出了一种有效的依赖图分解与同步规划相结合的新范式。
应用价值：为服务机器人、物流分拣等需要双臂协同操作的场景提供了高可靠、高效率的解决方案。
未来方向：
- 探索异步双臂协调（Asynchronous Coordination），以进一步扩展解空间（尽管这会带来更大的组合爆炸挑战）。
- 引入更强的感知反馈和在线重规划能力。
- 利用学习引导的采样进一步提升规划效率，实现真正的实时部署。

总结：SDAR 通过“依赖驱动的任务分解”与“基于 GPU 加速的同步运动规划”的深度融合，配合多级故障恢复机制，成功攻克了双机械臂在复杂纠缠场景下的重排难题，在成功率、执行效率和计算速度上均达到了当前最先进水平（State-of-the-Art）。