IEEE Transactions on Robotics

Tracking and Control of Multiple Objects During Nonprehensile Manipulation in Clutter

作者：Zisong Xu, Rafael Papallas, Jaina Modisett, Markus Billeter, Mehmet R. Dogar · 机构：School of Computer Science, University of Leeds, Leeds, U.K.; Department of Computer Science, American University of Beirut - Mediterraneo, Paphos, Cyprus

会议/期刊：IEEE Transactions on Robotics · 发布日期：2025 · 页码：3929-3947

控制操作视觉状态估计

精读笔记

Problem Setting

《Tracking and Control of Multiple Objects During Nonprehensile Manipulation in Clutter》（IEEE Transactions on Robotics / 2025）处理的是一个比常规 6D pose tracking 更苛刻的问题：机器人正在推、碰、间接移动多个物体，物体之间也在相互接触，而视觉经常被机器人手臂、其他物体或目标本身遮挡。

关键矛盾是：非抓取操作需要连续闭环反馈，但最常用的反馈源——相机——在最需要反馈的时候最不可靠。传统单帧 pose estimator 可以在物体清晰可见时工作，但它没有机制在完全不可见时维持状态；纯视觉 tracker 也通常依赖外观连续性或局部几何对齐，一旦遮挡导致漂移或 identity switch，后续很难恢复。

这篇论文真正要解决的不是“把 pose 估得更准”这个静态问题，而是“在视觉间歇性失效时，如何让物体状态 belief 不塌掉，并能被控制器继续使用”。困难点在于 contact dynamics 不光非线性，而且多物体耦合；但这也正是可利用的信息源：物体不能穿透、机器人执行过的动作已知、其他可见物体会约束被遮挡物体的可能位置。

Motivation

已有路线的问题在于它们默认 perception 是被动的：给一帧 RGB/RGB-D，输出 pose；或者给视频，依赖视觉连续性更新 pose。这在 grasp-before-contact 的 pick-and-place 中足够，因为物体在被接触前基本静止。但在 pushing/toppling/rearrangement 中，接触本身就是任务过程，物体状态在机器人动作下持续变化。

作者的核心观察很直接但重要：机器人并不是旁观者，它知道自己过去做了什么。执行控制序列本身就是一个强观测源，尤其在物体被遮挡时，它比当前图像更有信息量。进一步，多物体场景中可见物体也能通过物理一致性约束不可见物体的位置；这类信息视觉-only 方法几乎不用。

因此缺的不是另一个更强 detector，而是把 action、physics、visibility、RGB-D 组织到同一个时序 belief 中的机制。本文的动机本质上是把 manipulation perception 从 snapshot recognition 改成 action-conditioned probabilistic state estimation。

Core Idea

核心思想是：不要把每一帧视觉估计当作 pose 的主来源，而是把物理预测作为 belief propagation，把视觉当作 correction。每个粒子表示整组物体的联合 pose；给定上一时刻粒子和机器人关节状态，用 physics engine rollout 得到下一时刻可能状态；再根据 RGB detector 输出、物体在该粒子中的可见性、以及渲染 depth 与真实 depth 的一致性给粒子加权。

这个建模方式引入了一个很强的 inductive bias：物体运动必须由机器人控制和接触物理解释，且多物体状态必须整体物理可行。相比视觉 tracker，它不是在图像空间追踪 appearance，而是在物理状态空间中维护一组可行假设。相比单物体 physics-based tracking，联合粒子允许物体间接触和遮挡关系成为状态估计的一部分。

本质区别在于信息流被重新组织了：prior work 多是 image → pose → control；本文是 control + physics → pose belief，image/depth 只负责筛选 belief。这个改变使它在遮挡下自然更稳，因为遮挡不再等价于没有状态更新。

Method

方法中值得保留的机制有四个。

第一，联合粒子状态。每个粒子包含所有 tracked objects 的 pose，而不是为每个物体单独维护滤波器。这解决的是多物体接触和遮挡的耦合问题：一个被遮挡物体的位置可以通过其他物体和机器人几何间接约束。核心变化是 belief 从 object-wise independent tracking 变成 scene-level physical hypothesis。

第二，physics-based motion update。给定机器人关节状态和上一粒子状态，用物理引擎推进物体运动，并通过采样摩擦、质量、恢复系数以及额外噪声来近似运动不确定性。它解决的是视觉不可见期间无法传播状态的问题。必要性在于非抓取操作的运动不是 constant velocity/random walk 能描述的，而是由接触和控制共同决定。

第三，visibility-aware RGB observation。RGB 部分并不直接渲染 photorealistic image，而是调用现成 pose estimator 得到检测结果，再根据粒子内物体是否应当可见来解释“检测到/没检测到”。这解决了一个常见失败模式：detector miss 不应总被解释为该 pose 错误，也可能只是该粒子预测物体被遮挡。这个机制使 detector failure 从 hard failure 变成 likelihood signal。

第四，joint depth rendering。对每个粒子渲染整场景 depth，与真实 depth 比较，而不是先做 per-object segmentation 或 ICP。这解决 clutter 下 depth segmentation 不可靠的问题，也让多物体几何一致性直接参与加权。它更像一种 cheap scene-level consistency check，而不是精细的 pose refinement。

MPC 部分相对不是核心创新。控制器使用 PBPF 的代表粒子更新 simulator，再做随机轨迹优化。它主要展示 tracking belief 对闭环 pushing 有用；但规划本身没有充分利用完整粒子分布，严格说不是 belief-space MPC。

Key Insight / Why It Works

这篇论文有效的主要原因是 better inductive bias，而不是 scaling、data 或更强网络。遮挡下视觉模型失败是结构性问题；再训练更强 detector 只能推迟失败，不能消除观测缺失。PBPF 的优势来自把 manipulation dynamics 作为状态转移先验，使不可见状态仍有可解释的传播路径。

最核心贡献是 action-conditioned physics prior + joint multi-object belief。单独看粒子滤波、物理引擎、RGB-D render weighting 都不是新东西；真正有价值的是把它们放在非抓取 manipulation 的信息结构里：机器人动作提供 causality，物理引擎提供可行状态 manifold，视觉只做约束更新。这个组合恰好匹配 clutter manipulation 的主要 failure mode。

多物体联合粒子是另一个关键点。它不是简单把单物体 tracker 复制 n 次，因为遮挡和接触本身是 scene-level phenomenon。一个物体不可见时，其他可见物体的位置、不可穿透约束、机器人末端轨迹共同缩小其 posterior。这是本文相对视觉-only 或 independent filtering 的实质增益来源。

RGB 和 depth 的作用更像辅助校正。RGB detector 负责 identity/appearance，尤其区分形状相似但纹理不同的物体；depth rendering 负责几何一致性。二者提升明显，但不是根本原因。没有 physics prior，它们仍然会在重遮挡下断裂。

这里的 compute 也是一种 test-time compute：每个粒子都跑物理仿真和渲染，用计算换取遮挡下的状态维持。这不是 learning-based scaling，而是 simulation-based inference scaling。其上限也由此决定：只要物理仿真足够准且实时，方法就稳；一旦 contact regime 超出物理引擎可靠范围，更多粒子也只是更昂贵地传播错误。

需要指出的是，文中 likelihood 设计并不完全严格。RGB 部分用 Bayes 近似把 p(z|x) 替成 p(x|z)，并假设 particle prior 相近；depth error 到 probability 的构造也更像 heuristic weighting。方法成立主要靠工程上合理的 ranking，而不是严密概率模型。

Relation To Prior Work

这篇工作位于三条谱系的交叉处：vision-based 6D pose tracking、physics-based state estimation、model predictive manipulation control。它最接近的不是普通 pose estimator，而是 physics-informed particle filtering for manipulation。

和 DOPE/Diff-DOPE/FoundationPose 这类视觉方法的本质差异是观测模型位置不同。视觉方法把图像作为 pose 的主证据；本文把图像作为筛选物理假设的证据。因此当图像无信息时，视觉方法自然退化，而本文仍可依赖 transition prior。

和 marker-based MPC pushing 的差异在于无需外部 marker 作为部署感知通道，虽然实验 ground truth 用了 OptiTrack。它试图用 RGB-D + robot proprioception 替代人工标记反馈。

和已有 physics-based pose estimation 的差异在于它不是静态稳定性 reasoning，也不是单次 impact 后轨迹拟合，而是在机器人持续施加控制时进行在线、多物体、闭环 tracking。这里真正新增的信息是时间戳对齐的 robot joint/control stream 被系统性用于 pose belief propagation。

不过，很多组件本身是已有思想重组：particle filter、domain-randomized physics parameters、render-and-compare depth、visibility reasoning、MPC with stochastic rollout 都不新。实质创新在于将这些机制组合成一个能处理多物体非抓取遮挡的在线系统，并证明这种信息组织方式比视觉-only 更适合该问题。

Dataset / Evaluation

评估包含真实机器人、多物体 pushing、RGB-D、关节状态、OptiTrack ground truth，并释放包含 robot state 与 object pose 的数据集。这一点很有价值，因为现有 pose benchmark 通常缺少 action/control 信息，无法评估 action-conditioned tracking。

tracking 实验覆盖一到三物体为主，附加四/五物体、相同物体、高冲击、垂直运动等分析。总体上 evaluation 确实验证了核心 claim：在遮挡频繁的非抓取操作中，利用物理预测显著优于视觉-only tracker。按 visibility 分桶的结果尤其支持这一点，因为它直接对应论文的核心失效模式。

控制实验是真机闭环，但规模较小，任务类型集中在 tabletop pushing，目标区域简单，轨迹优化器也相对局部。它证明 PBPF feedback 对 MPC 有帮助，但没有证明复杂 rearrangement、长期规划或 belief-aware control 的能力。MPC-PBPF 实际使用 mean-nearest particle，而不是完整 posterior，因此“概率性 tracking 帮助 robust planning”的说法有一定保留；当前更准确地说是“更稳的状态估计帮助闭环控制”。

baseline 选择基本合理，包含强视觉 tracker FoundationPose。但需要注意，PBPF 使用 DOPE 作为 RGB observation，因此不是完全独立于视觉基线；它是在视觉 detector 上叠加 physics/test-time inference。增益归因主要应归于 physics prior 和 visibility/depth weighting，而不是 RGB detector 本身。

Limitation

最大前提是 known-object world：所有物体、机器人、环境都需要 3D model，且初始时刻物体可见以初始化粒子。未知物体、突然进入场景的外部干扰、人为移动物体都会破坏假设。这不是小限制，而是方法能否部署到开放环境的分界线。

scalability 上限很明确：每个粒子一个 physics simulation，且每个粒子要做渲染和可见性计算。多物体数增加时，状态维度、接触组合、仿真成本同时上升。论文测试到五个物体已经出现性能下降；因此它更适合少量关键对象的 clutter manipulation，而不是 dense bin-level full-scene tracking。

泛化来自物理模型和 CAD，而不是 learned policy/representation 的泛化。优点是无需为每组物体重新训练，缺点是把问题转移到了模型准确性、参数校准和仿真保真度上。摩擦、质量、接触模型不准时可以靠噪声缓冲，但高冲击实验显示物理错误会直接成为错误先验，甚至比视觉-only 更差。

概率建模有明显 heuristic 成分。RGB/depth independence、object-wise factorization、visibility threshold、detector miss likelihood、depth error normalization 都是工程选择。它们在当前数据上有效，但跨相机、跨材质、跨光照、跨 depth noise profile 时是否稳定，文中未充分说明。

控制层没有真正利用 belief。选择 closest-to-mean particle 会丢掉 multi-modality；如果遮挡导致多个物理可行假设并存，mean particle 可能并不是风险最小决策。所谓 robust manipulation planning 目前更多来自 tracker 不容易丢，而不是 planner 在 belief space 中显式规避不确定性。

实时性也有限。约 4 Hz 对慢速 pushing 可行，但对动态 manipulation、高速碰撞、需要快速反应的接触任务不足。未来如果没有更快的 differentiable/parallel physics 或 learned surrogate，这条路线很难扩展到高动态操作。

Takeaway

  • 1. 对非抓取操作，perception 不应被建模成纯视觉问题；机器人动作本身是强观测源，尤其在遮挡时价值最大。
  • 2. 多物体 tracking 的关键不是把单物体 tracker 做 n 次，而是维护 joint physically feasible scene hypotheses；遮挡和接触都是联合状态性质。
  • 3. 这篇推动的不是新 detector，而是一种信息组织范式：physics prior 负责 temporal continuity，RGB-D 负责 correction，visibility reasoning 负责解释 observation absence。
  • 4. 未来真正值得做的是 belief-aware control、在线物理参数/接触模型适配、未知物体的几何-物理快速建模，以及用 learned surrogate 降低 physics rollout 成本；单纯换更强视觉 backbone 不会解决核心问题。

一句话总结

这篇论文是非抓取 clutter manipulation 中从视觉 pose tracking 走向 action-conditioned physics-based belief tracking 的代表性系统工作，真正贡献在于用联合物理粒子假设维持遮挡下的多物体状态，而不是提出一个更强的视觉估计器。