IEEE Transactions on Robotics

Real-Time Dual-Arm Cooperative Manipulation Under Multiple Constraints: A Two-Stage Sampling MPC Approach

作者：Tianqi Zhu, Jianliang Mao, Jun Yang, Shihua Li · 机构：School of Automation, Key Laboratory of Measurement and Control of CSE, Ministry of Education, Southeast University, Nanjing, China; Faculty of Artificial Intelligence, Shanghai University of Electric Power, Shanghai, China; Robotics and Autonomous Systems Thrust of Systems Hub, The Hong Kong University of Science and Technology (Guangzhou), Guangzhou, China

会议/期刊：IEEE Transactions on Robotics · 发布日期：2026 · 页码：1942-1960

路径规划控制操作安全

精读笔记

Problem Setting

《Real-Time Dual-Arm Cooperative Manipulation Under Multiple Constraints: A Two-Stage Sampling MPC Approach》（IEEE Transactions on Robotics / 2026）关注的是一个很具体但很难的控制问题：双臂共同抓持同一物体时，系统必须在闭链相对位姿约束下实时移动物体，同时处理障碍、自碰撞、关节限位、物体倾斜和终端平滑。

真正困难点在于，双臂闭链不是普通高维机械臂避障。相对位姿等式约束把可行运动压到一个低维流形上，而外部障碍、自碰撞、关节限制又把这个流形切成多个非凸连通区域。采样方法如果在原始 joint velocity 空间里采，绝大部分样本在等式约束上无效；如果用 penalty 逼近等式约束，权重会被不可行样本淹没。另一方面，解析 null-space / CDTS 控制虽然能保持相对位姿，但面对非凸障碍和多模态路径选择时基本是局部反应式控制，容易卡死。

这篇论文抓住的关键矛盾是：实时性要求不能做重型全局规划，闭链协作又要求不能把约束只作为 soft cost；而避障又需要足够大的探索方差，终端稳定却需要低方差和平滑控制。单一控制器很难同时满足这三个要求。

Motivation

已有路线各自缺一块。传统 motion planning 能处理非凸空间，但闭链约束下需要 manifold projection，实时性和动态响应不稳定；CBF/VFI/RMP/CDTS 这类 reactive controller 在局部约束处理上干净，但长视野、多障碍、多模态选择能力弱；标准 MPPI/STORM/cuRobo 类 sampling MPC 有实时潜力，但默认假设采样空间中的加权平均是有意义的，这在双臂多模态避障里经常不成立。

作者的核心观察是两个：第一，双臂协作中最硬的不是障碍，而是相对位姿等式约束；这个约束不应该靠 penalty 学出来，而应该在 rollout propagation 里被结构性满足。第二，MPPI 的指数加权平均在单模态 basin 内合理，但在“左绕/右绕/下绕”等多模式同时存在时会平均出一个不属于任何模式的动作，这正是双臂窄空间避障中常见的失败模式。

因此这篇的动机不是提出一个更复杂的 optimizer，而是给 sampling MPC 加上两个 inductive bias：约束流形 bias 和 mode-preserving aggregation bias。

Core Idea

核心思想可以概括为：先把采样限制在协作等式约束的可行运动子空间内，再在这个子空间里做显式模式发现与局部 refinement。也就是说，它不是让 MPPI 在全 joint space 中“碰运气”找到既避障又闭链可行的控制，而是先用 relative Jacobian null-space projection 把所有候选控制投影到不会改变双臂相对位姿的方向上，然后让采样预算主要花在避障、目标推进和平滑性之间的权衡上。

第二个核心是把 MPPI 的单阶段 weighted averaging 改成 two-stage mode selection。高方差阶段负责覆盖多个可行通道，但不直接输出最终控制；它先用 k-means 在轨迹描述空间中选出包含最多 elite rollout 的模式，只在该模式内做加权平均。低方差阶段再围绕这个模式局部采样，输出平滑控制。这个改变本质上是在采样控制中引入“先离散选择 mode，再连续优化”的结构，而不是继续用一个 unimodal Gaussian update 去拟合多模态 posterior。

和 prior 的本质区别在于：它没有试图把更多 constraints 塞进优化器求解，也没有依赖学习模型预测动作，而是重新组织 sampling MPC 的信息流：硬约束进入 forward dynamics，多模态结构进入 sample aggregation，终端稳定性交给解析控制器。这使得方法在工程上更 scalable，因为每个机制都避免了最昂贵或最脆弱的部分。

Method

1. Null-space constraint-aware rollout：解决 equality constraint 在采样中几乎不可命中的问题。每一步候选 joint velocity 先经相对位姿 Jacobian 的 null-space projector 过滤，使 predicted motion 理论上满足 J_r dotTheta = 0。核心变化是把相对位姿保持从 soft penalty 变成 dynamics-level hard bias。注意这仍是局部微分意义上的严格，离散积分和 clamping 后的全局严格性文中未充分说明。

2. Hybrid inequality handling：解决约束类型混杂导致采样效率低的问题。关节位置/速度/加速度这类易建模约束通过 deterministic clamping 处理，碰撞、倾斜、奇异性等通过 cost penalty 处理。核心变化是避免把采样预算浪费在简单 box constraints 上，让采样主要区分几何可行路径。

3. Two-stage sampling：解决探索和平滑的冲突。第一阶段大方差探索，cost 中加入 stagnation penalty，鼓励不要在目标误差仍大时停在局部最优；第二阶段小方差 refinement，只使用 standard cost，降低输出 jitter。机制上类似 coarse-to-fine / annealed sampling，但这里重点是先选模式再 refine。

4. k-means mode selection：解决 MPPI 多模态平均失败。聚类特征包括 exploration cost 和整体关节变化，选择 elite trajectories 最集中的 cluster，再在该 cluster 内做指数加权。这个模块的价值在于防止左绕和右绕控制相互抵消。它是论文中最接近“实质算法贡献”的部分之一，但也相当 heuristic。

5. Auxiliary CDTS controller + unexplored-degree switching：解决采样控制近稳态随机抖动。传统 null-space controller 不考虑障碍，但近目标、无复杂约束时更稳定；通过比较传统控制 rollout cost 与 sampling MPC cost，积分更新 L，决定由 sampling navigator 还是 analytic finisher 执行。核心变化是把全局探索与局部收敛分给两个控制器，而不是强迫 MPPI 在所有阶段工作。

6. Low-level relative-pose correction：解决高层低频 MPC 与真实机器人之间的相对位姿漂移。用 J_r^dagger K_r e_r 做小误差反馈，再叠加 N_r u_h。它更像必要的 deployment stabilizer，而不是主要算法创新。

Key Insight / Why It Works

这篇最重要的 insight 是：双臂闭链 sampling MPC 的瓶颈不是缺少更复杂的 cost，而是采样分布与可行流形错位。只要候选 rollout 大量违反相对位姿约束，MPPI 的权重更新就基本是在不可行样本中做选择；即便加大 penalty，也只是让有效样本稀疏化。null-space propagation 直接改变了有效采样维度，这是方法能工作的第一原因。

第二个有效原因是 mode averaging 被显式处理。标准 MPPI 的指数加权平均隐含单模态假设，在双臂绕障中这个假设非常差。k-means 先选 cluster 再 average，相当于在 test-time 引入一个离散 latent mode。这个 latent structure 不需要训练数据，但强依赖 descriptor 是否能把行为模式分开。它不是深层理论创新，但非常贴合问题结构。

第三个原因是探索-收敛被时间分工。大方差探索解决局部最优，小方差 refinement 解决 jitter，解析 controller 解决终端稳态。这实际上是一种 test-time compute allocation：复杂阶段消耗 GPU rollout，简单阶段回到低噪声解析控制。它比单一 MPPI 更稳定，不是因为 optimizer 更“聪明”，而是因为它承认不同阶段需要不同控制范式。

我认为最核心贡献排序是：null-space rollout > mode-preserving two-stage aggregation > adaptive switch。双四元数表示和 GPU CUDA kernel 是重要 engineering enabler，但不是概念核心；如果换成 SE(3) matrix / Lie algebra 表示，思想仍成立。GPU speedup 对实时性贡献很大，部分性能提升可能主要来自 scaling / implementation，而非算法本身。

文中未充分说明几个归因问题：两阶段结构相对多轮 CEM/annealed MPPI 的独立贡献有多大；k-means 是否优于更简单的 elite-set mode splitting；unexplored-degree indicator 的阈值是否对任务敏感。实验有 ablation，但还不足以完全拆开 algorithmic insight 与 engineering scaling 的贡献。

Relation To Prior Work

这篇属于 MPPI / sampling-based MPC 与 CDTS/null-space dual-arm control 的交叉谱系，更接近“结构化采样控制”而不是传统运动规划或学习控制。

和 CDTS/null-space reactive control 相比，真正不同点是引入长视野随机 rollout 来处理非凸障碍与多模态路径选择；传统 CDTS 只能给出局部绝对位姿收敛和相对位姿保持，无法自然决定绕障模式。

和标准 MPPI/STORM/cuRobo 相比，不同点不是 GPU 并行，而是 closed-chain equality constraint 的处理方式：它不把相对位姿当作普通 penalty，而是投影到 relative Jacobian null space 后再 rollout。这是对双臂协作任务的关键适配。

和 CEM、annealed MPPI、DIAL-MPC 等多阶段采样优化相比，这篇的新意有限但取舍明确：不用多轮复杂 annealing，而是一次高方差探索 + mode clustering + 一次低方差 refinement，以满足实时双臂控制。这里的 k-means 本质上是一个便宜的 latent mode selector。

和 learning-based/diffusion policy 路线相比，它没有学习复杂 prior，也不依赖数据覆盖；泛化主要来自模型结构和在线采样，而不是 offline dataset。但这也意味着它不能利用经验先验，复杂场景下仍要靠大量 rollout 和手工 cost。

Dataset / Evaluation

评估覆盖较完整的控制场景：基础点到点、窄空间、多动态障碍、静态+动态混合、十字形障碍、自碰撞和突发障碍；同时用了 UR3/UR3e 与双 Franka 两类平台，并包含真机实验。这对验证“实时双臂闭链避障控制”是有意义的，不是纯仿真 toy problem。

仿真比较对象包括多种 MPPI 采样变体、CEM、无 clustering、无 adaptive switch、无 null-space mapping 的高 penalty 版本。这个设计基本能支持作者的主要 claim：单阶段 MPPI 在多模态任务中不稳，null-space 显著提升闭链可行性，adaptive switch 减少终端抖动。

但 evaluation 仍有明显边界。真机实验主要验证 feasibility，而不是系统性 benchmark；障碍感知依赖 AprilTag/RealSense，动态障碍处理是速度膨胀近似，不是复杂 perception-planning 闭环；任务规模和 clutter complexity 仍偏可控。实验没有充分展示在密集未知点云、快速动态障碍、强接触扰动或长期连续任务下的鲁棒性。

另外，所有方法共享一些增强组件的方式略复杂，增益归因不完全干净。比如 baseline 是否都使用同等优化过的 CUDA/DQ/collision pipeline、同等调参预算、同等 controller switching，会影响结论。总体上，实验支持“这套系统在选定双臂场景中有效”，但还不能证明它是一般双臂实时规划的最终解。

Limitation

1. 等式约束严格性依赖局部线性化。null-space projection 保证的是瞬时 J_r dotTheta = 0，离散积分、Jacobian 更新、clamping 以及低层执行误差都会带来 drift。论文通过低层 relative correction 修补，但这意味着所谓 hard equality 实际上仍需要反馈闭环维持。

2. 冗余度是硬前提。如果 equality constraints 接近或超过系统自由度，null space 变小甚至消失，采样器没有可探索方向。作者也承认约束数量不能超过 DoF；这限制了方法在多接触、多工具、多物体协作中的扩展。

3. k-means mode selection 是 heuristic。它假设行为模式能被 cost + joint displacement descriptor 分开，且 K 能大致匹配可行模式数。复杂 3D 操作中，模式可能不是欧氏聚类结构；此时 cluster 可能混合不同行为或切碎同一行为。文中未充分说明失败模式。

4. 动态障碍建模偏弱。速度膨胀把动态障碍转成保守静态近似，适合慢速或简单轨迹，不等价于真正的时序预测。面对主动智能体、非线性运动或遮挡恢复，这个模块可能不足。

5. 实时性可能主要来自 scaling / engineering。GPU batched dual quaternion、cuRobo collision、1000 rollouts、30 Hz high-level，这些是系统能力的重要来源。算法本身如果没有这些并行资源，性能上限会明显下降。

6. 没有动力学与力控制。论文主要在 joint velocity kinematics 层工作，对闭链内力、抓持稳定、柔性物体、接触冲击没有深入建模。对于真正双臂协作搬运，relative pose 保持不等于 internal force 安全。

7. 安全性不是形式化保证。碰撞、倾斜等通过 penalty 和 sampling 处理，没有 CBF/MPC feasibility certificate。实时 deployment 中如果采样未覆盖安全动作，系统仍可能失败。

Takeaway

  • 1. 对闭链双臂任务，最值得迁移的原则是：不要把相对位姿等式约束当作 cost；应该把它嵌入 sampling dynamics 或 action parameterization。
  • 采样空间对齐可行流形，比调大 penalty 更关键。
  • 2. MPPI 在多模态场景中的主要问题是 aggregation，而不是采样本身。
  • 先做 mode selection 再做加权平均，是一个简单但有效的结构化改造；这个 insight 可迁移到移动机器人绕障、多指操作、接触模式选择等问题。

一句话总结

这篇论文把 MPPI 从普通高维采样控制改造成面向双臂闭链协作的结构化实时控制器，核心贡献是用 null-space 对齐可行流形、用两阶段 mode-preserving sampling 处理多模态避障，属于 sampling MPC 向约束感知、系统级工程化演化的一步。