PAPER DEEP DIVE
通过搜索分配与零空间体现多手操作策略
提出通过搜索分配与零空间体现多手操作策略的方法,提升多手/多指机器人操作的协调与泛化。
Ω-CBSA:通过搜索分配与零空间实现多手操作策略
作者:Yorai Shaoul, Jiaoyang Li, Maxim Likhachev | 机构:卡内基梅隆大学 | arXiv:2607.22020v1 | 项目主页:omcbsa.github.io
一句话总结
本文提出了 Ω-CBSA(On-Manifold Conflict-Based Search with Assignments),一个基于搜索的框架,通过联合搜索离散的"轨迹-机械臂分配"与连续的"雅可比零空间冗余运动",将学习型多手操作策略输出的末端轨迹完整、安全地落地到多臂机器人上,并在理论上证明了分辨率完备性。
研究背景与动机
现代操作策略(如 π₀、ACT 等)越来越倾向于直接输出末端执行器(end-effector)的运动轨迹,而非关节角命令。这种抽象使得演示数据易于采集,且能跨机器人平台迁移。然而在多臂机器人场景中,存在一个关键的执行鸿沟(execution gap):策略不会指定哪条轨迹由哪只手臂执行,也不会考虑多臂之间的碰撞 avoidance 与冗余关节的协调。
当前实践中,工程师通常将单臂逆运动学(IK)流程以临时方式扩展到多臂,没有任何可行性或安全性保证。标准单臂 IK 跟踪方法在多臂场景中即使存在可行实现也会失败,因为它会过早地承诺某一条构型空间轨迹,无法探索冗余流形上后续可避免碰撞的构型。
图1:左——冗余流形 $\mathcal{M}$ 上存在多个构型 $q$,有些安全、有些碰撞,但末端位姿相同 $FK(q)=x$;右——三臂协作翻转/推动物体。
本文将该问题形式化为 OM-AMRAMP(On-Manifold Anonymous Multi-Robot-Arm Motion Planning)。其核心挑战在于同时处理三类决策:(1)将 $N$ 条末端轨迹分配给 $N$ 个机械臂的双射 $\sigma$;(2)每个机械臂在冗余流形上的构型空间运动;(3)避免臂间碰撞与障碍物碰撞。
问题形式化(OM-AMRAMP)
考虑 $N$ 个操作臂 $\{R^i\}_{i=1}^N$,每个有 $d$ 个关节,构型空间 $\mathcal{Q}_R^i \subset \mathbb{R}^d$。正向运动学映射 $FK^i: \mathcal{Q}_R^i \rightarrow SE(3)$ 将构型映射到末端位姿。输入为 $N$ 条末端轨迹 $X^j = \{x_1^j, \dots, x_H^j\}$,其中 $x_t^j \in SE(3)$。
目标是找到双射 $\sigma: \{1,\dots,N\} \to \{1,\dots,N\}$ 和构型空间轨迹 $\tau^i = \{\tau_1^i, \dots, \tau_H^i\}$,满足:
$$FK^i(\tau_t^i) = x_t^{\sigma(i)}, \quad R^i(\tau_t^i) \cap R^k(\tau_t^k) = \emptyset, \quad R^i(\tau_t^i) \cap \mathcal{O} = \emptyset$$即每个臂精确跟踪分配的轨迹,且不与其他臂或障碍物碰撞。这里 $R^i(q^i) \subset \mathbb{R}^3$ 表示臂在构型 $q^i$ 下占据的工作空间体积,$\mathcal{O} \subset \mathbb{R}^3$ 为静态障碍区域。
方法详解
1. 雅可比零空间与冗余探索
对于超过6个关节的冗余机械臂,$FK(q) = x_t$ 有无穷多解,形成低维流形:
$$\mathcal{M}_t = \{q \in \mathcal{Q}_R \mid FK(q) = x_t\}$$数值 IK 只返回由种子点决定的单一解,无法探索 $\mathcal{M}_t$ 上可能避免碰撞的其他构型。本文利用雅可比零空间进行系统性的流形上探索。在构型 $q$ 处,雅可比 $J(q)$ 的零空间 $\mathrm{Null}(J(q))$ 包含所有不改变末端位姿的关节空间方向:
$$\Delta q = N(q)\,\alpha$$其中 $N(q)$ 是零空间基矩阵,$\alpha$ 为小系数向量。这种零空间运动提供了约束流形上的系统"近邻",允许局部探索冗余构型。
2. Ω-A*(单轨迹版本)
Ω-A*$_\text{single}$ 是单机器人、单轨迹的位姿约束流形上的 A* 搜索。状态为 $s = (q, t)$,其中 $FK(q) = x_t$。Open 列表用 $x_1$ 的多个 IK 解初始化。每个状态存储代价 $g(s)$、启发式 $h(s) = H - t$、碰撞计数 $c(s)$ 和优先级:
$$f(s) = g(s) + w_h\,h(s) + w_c\,c(s)$$从状态 $(q, t)$ 生成后继时,先计算标称投影 $q' = IK(x_{t+1}, q)$,然后通过零空间运动探索冗余实现:对每个基方向 $n \in N(q')$ 和步长 $\pm\epsilon$,提出 $q'' = q' + \epsilon n$,再校正 $q'' \leftarrow IK(x_{t+1}, q'')$ 回到流形。参数设为 $\epsilon = 0.2, w_h = 10, w_c = 0.1$。
3. Ω-A*(多目标版本)
为处理"机器人可选择多条轨迹"的场景,设计多目标变体 Ω-A*。Open 列表用所有候选轨迹 $\{X^1, \dots, X^M\}$ 的首个位姿的 IK 解初始化。状态扩展为 $(q, t, j)$,编码跟踪 $X^j$ 的定时构型。分配决策被隐式处理——规划器在搜索过程中同时决定跟踪哪条轨迹和如何跟踪。
4. 优先级规划(OM-PP-A*)
简单的多臂协调方法:机器人按优先级排序,依次规划。高优先级机器人先规划,后续机器人将已规划轨迹视为运动障碍。产生两个变体 OM-PP-A* 和 OM-PP-A*$_\text{single}$,高效但不完备。
5. Ω-CBSA:核心算法
Ω-CBSA 将多目标 Ω-A* 集成到 Conflict-Based Search(CBS)框架中。初始为每个机器人规划流形上轨迹,可能包含两类冲突:
- 分配冲突:两个机器人选择了同一条轨迹 $X^j$
- 几何冲突:两个机器人在时刻 $t$ 发生碰撞
Ω-CBSA 在约束树(CT)中组织搜索。每个 CT 节点包含:(1)每机器人约束集;(2)在约束下用 Ω-A* 计算的轨迹;(3)检测到的冲突。每次迭代选择冲突最少的 CT 节点,若无冲突则返回解。否则通过生成两个子节点解决一个冲突,每个子节点添加不同约束后重新规划一个机器人。
对于分配冲突,一个子节点禁止 $R^i$ 选择 $X^j$,另一个禁止 $R^k$。对于几何冲突,选择碰撞点 $p$,一个子节点禁止 $R^i$ 在时刻 $t$ 占据 $p$,另一个禁止 $R^k$。
每个 CT 节点的代价定义为所有机器人轨迹代价之和加上冲突惩罚:
$$ \text{cost}(n) = \sum_{i=1}^{N} g(\tau^i) + \lambda \cdot |\text{conflicts}(n)| $$其中 $g(\tau^i)$ 是机器人 $R^i$ 轨迹的代价,$|\text{conflicts}(n)|$ 是节点 $n$ 中的冲突数,$\lambda$ 为冲突惩罚权重。Ω-CBSA 每次选择代价最小的 CT 节点展开。
flowchart TD
A["初始化: 为每个机器人用Ω-A*规划轨迹"] --> B["选择冲突最少的CT节点"]
B --> C{"是否有冲突?"}
C -- "无冲突" --> D["返回解决方案"]
C -- "有冲突" --> E["选择一个冲突"]
E --> F{"冲突类型?"}
F -- "分配冲突" --> G["生成两个子节点:
禁止Ri选Xj / 禁止Rk选Xj"]
F -- "几何冲突" --> H["生成两个子节点:
禁止Ri占p@t / 禁止Rk占p@t"]
G --> I["用Ω-A*重新规划被约束机器人"]
H --> I
I --> B
理论分析:分辨率完备性
Ω-CBSA 采用有限分辨率的离散时间抽象,基于运动原语生成底层运动。论文证明了分辨率完备性(resolution-completeness)。
引理1(Ω-A* 的分辨率完备性):若在运动原语离散分辨率下,机器人 $R^i$ 跟随固定路径 $X^j$ 的可行位姿约束轨迹 $\tau^i$ 存在,则 Ω-A* 必能找到它。证明基于 Ω-A* 搜索有限图,其节点为三元组 $(q, t, j)$,系统性地探索所有可达节点。
引理2(约束互斥性):Ω-CBSA 的分配约束与几何点约束互为互斥。对于分配冲突,若 $R^1$ 和 $R^2$ 同时选择 $X^j$,生成约束 $c^1 = \{R^1 \text{ 不可选 } X^j\}$ 和 $c^2 = \{R^2 \text{ 不可选 } X^j\}$。任何同时违反两者的联合解必然将 $X^j$ 分配给两个机器人,与双射 $\sigma$ 的要求矛盾。
定理1(Ω-CBSA 的分辨率完备性):由引理2,所有约束互斥;由引理1,底层规划器 Ω-A* 分辨率完备。因此 CBS 框架下 Ω-CBSA 对 OM-AMRAMP 分辨率完备。
实验结果
实验模拟了 450 个基准问题,包含最多6个末端轨迹和机器人,布置在多样布局与障碍物中。测试套件包含类似学习型操作策略输出的运动以及需要紧密协调执行的人形机器人场景。还在物理3臂平台上演示了布料旋转、盒子翻转和平面推动。
| 方法 | 类型 | 完备性 | 特点 |
|---|---|---|---|
| IK-Tracking | IK跟踪 | 不完备 | 枚举所有分配,快速但近视,成功率低 |
| PP-Descartes | 位姿约束路标图 | 不完备 | 枚举分配与优先级,路标图构建代价高 |
| Composite A* | 隐式图搜索 | 完备 | 状态空间随机器人数指数增长 |
| Ω-CBSA | CBS+流形搜索 | 分辨率完备 | 联合分配与零空间搜索,成功率最高 |
| OM-PP-A* | 优先级+流形 | 不完备 | 速度-性能权衡优良 |
在所有450个基准问题中,本文方法在成功率上一致优于所有基线。Ω-CBSA 整体成功率最高,紧随其后的是优先级变体 OM-PP-A* 和 OM-PP-A*$_\text{single}$。按机器人数量分解时,随臂数增加,本文方法的优势更加明显。
| 指标 | Ω-CBSA | IK-Tracking | 说明 |
|---|---|---|---|
| 2机器人平均规划时间 | ~150 ms | ~160 ms | IK更快但成功率低 |
| 全部基准平均运行时间 | ~640 ms (σ=990) | — | min 16ms, max 4.9s |
| 优先级变体 | <1s 大部分 | — | 速度-性能权衡优良 |
Descartes 因路标图构建代价高性能明显受损。Composite A* 因分配引入的分支因子快速增长而搜索困难。IK-Tracking 虽然成功时很快(约160ms),但成功率低限制了实用价值。
实现细节:使用 Pinocchio 库计算运动学,Kinova Gen3 机械臂,Intel Core i9-12900H CPU(5.2 GHz)。Ω-CBSA 的根 CT 节点用重复 IK 跟踪初始化(上限500ms),产生较少碰撞的初始轨迹但不影响理论保证。
物理实验
在物理3臂平台上验证了布料旋转、盒子翻转和平面推动任务,展示了 Ω-CBSA 在真实多臂系统上的可行性。
局限性
- 分辨率完备性依赖于运动原语的离散化分辨率,分辨率越高搜索空间越大但计算代价越高。
- 零空间探索的步长 $\epsilon$ 和权重 $w_h, w_c$ 需要针对不同机器人平台调参。
- 当前仅验证了最多6个机器人场景,更大规模系统的可扩展性有待进一步研究。
- 到达初始构型 $\tau_1^i$ 依赖标准多臂运动规划器,未在本框架内统一处理。
总结与展望
本文提出了 Ω-CBSA,一个用于在多臂机器人上实现多手操作策略的理论完备规划器。通过将流形单机器人规划与 CBS 协调方案相结合,系统性地推理冗余、碰撞和分配。最多6臂的实验表明,Ω-CBSA 及其优先级变体在保持实时性能的同时,成功率高于 IK、采样和隐式图搜索基线。
金句:"我们希望这项工作能降低部署多手操作策略的实际门槛。"——通过统一处理分配与零空间运动,Ω-CBSA 为学习型操作策略从仿真到真实多臂系统的落地提供了理论上完备、实践上高效的桥梁。
深度解读由 RobotWorld paper-detail-generator 基于全文精读生成 | arXiv:2607.22020v1



