文章概括

引用：

@article{liu2025activepose,
  title={ActivePose: Active 6D Object Pose Estimation and Tracking for Robotic Manipulation},
  author={Liu, Sheng and Li, Zhe and Wang, Weiheng and Sun, Han and Zhang, Heng and Chen, Hongpeng and Qin, Yusen and Ajoudani, Arash and Wang, Yizhao},
  journal={arXiv preprint arXiv:2509.11364},
  year={2025}
}

Liu, S., Li, Z., Wang, W., Sun, H., Zhang, H., Chen, H., Qin, Y., Ajoudani, A. and Wang, Y., 2025. ActivePose: Active 6D Object Pose Estimation and Tracking for Robotic Manipulation. arXiv preprint arXiv:2509.11364.

主页：
原文：
代码、数据和视频：

系列文章：
请在 $《$文章$》$ 专栏中查找

---

宇宙声明！

引用解析部分属于自我理解补充，如有错误可以评论讨论然后改正！

---

ABSTRACT

精确的 6 自由度（6-DoF）物体位姿估计与跟踪对于可靠的机器人操作至关重要。 然而，零样本（zero-shot）方法在由视角引发的歧义情况下往往会失败；而固定相机的设置在物体移动或发生自遮挡（self-occluded）时也会遇到困难。 为了解决这些挑战，我们提出了一条主动式位姿估计流水线，将视觉-语言模型（VLM）与“机器人想象（robotic imagination）”结合起来，以在实时过程中动态检测并消解歧义。 在离线阶段，我们对 CAD 模型渲染出稠密的一组视角，计算每个视角对应的 FoundationPose 熵（entropy），并构建一个几何感知（geometric-aware）的提示词（prompt），其中包含低熵（无歧义）与高熵（有歧义）的示例。 在运行时，系统执行： (1) 在实时图像上查询 VLM，得到一个歧义评分；(2) 若检测到歧义，则通过渲染虚拟视图来“想象”一组离散的候选相机位姿，并基于“VLM 歧义概率”和“FoundationPose 熵”的加权组合对每个候选位姿进行打分，然后将相机移动到下一最佳视角（Next-Best-View, NBV），以获得去歧义后的位姿估计。 此外，由于运动物体可能会离开相机视野，我们引入一个主动位姿跟踪模块：使用模仿学习训练的扩散策略（diffusion-policy），用于生成相机轨迹，从而保持物体可见性并最小化位姿歧义。 仿真与真实世界实验表明，我们的方法显著优于经典基线方法。

I. INTRODUCTION

6D 物体位姿估计旨在恢复物体相对于相机的平移与旋转信息，在抓取 [1], [2], [3] 和装配 [4] 等机器人操作任务中起着关键作用。 以往的研究工作 [5], [6], [7], [8], [9], [10] 通过在特定数据集上训练，取得了较强的性能。 近年来，零样本方法 [11], [12], [13] 展示了仅依赖物体的 CAD 模型、无需额外真实世界标注即可对新物体进行位姿估计的能力。

尽管性能出色，但大多数现有方法假设单一视角的观测信息足以估计唯一的物体位姿。 然而在实际应用中，自遮挡和物体间遮挡常常掩盖关键判别区域，从而导致位姿估计在本质上成为病态问题。 这一问题在工业金属零件中尤为突出，其对称结构和无纹理表面天然减少了可区分区域，显著加剧了位姿歧义。

由于位姿歧义在实际场景中不可避免，对其进行消解变得至关重要。 尽管通过物理方式重新摆放目标物体或移除遮挡物可以消除位姿歧义，但在涉及高精度部件或精密仪器的任务中，这类直接干预往往是不可行的。 相较之下，使相机能够主动调整观察视角、模拟人类的观察方式，提供了一种更通用的解决方案。 尽管已有研究 [14], [15] 尝试预测位姿歧义，但它们不仅依赖昂贵的先验信息，而且未能提供可执行的歧义消解方案。 已有一些方法通过训练神经网络来预测用于主动位姿估计的下一最佳视角（NBV）[16], [17], [18]。 尽管这些方法展现出潜力，但它们需要大量的离线训练。 因此，在实际运行过程中确定最优的相机运动策略以缓解位姿歧义，仍然是一个开放且具有挑战性的问题。

基于上述动机，我们提出了一种几何感知提示框架，将视觉-语言模型（VLM）引入零样本主动 6D 位姿估计。 在物体几何理解阶段，我们使用 CAD 模型渲染多样化的视角，并由 FoundationPose [12] 生成初始位姿估计，从而自动识别产生高位姿歧义的视角。 随后，我们将规范投影视图（轴测图、底视图和后视图）与选定的歧义视图相结合，构建基于图像的提示词，从而将丰富的物体几何信息编码进 VLM。 在在线运行阶段，我们引入一种基于 VLM 的歧义预测机制，用于判断当前相机视角是否支持唯一的位姿估计。 FoundationPose 输出一组候选位姿，我们据此计算位姿熵指标以量化歧义程度。 若位姿熵超过阈值，机器人想象模块将从其他相机位姿合成假想视图； VLM 对这些提示进行评估以选择下一个无歧义视角，并重新计算位姿熵以确认歧义已被消解。 此外，当物体处于运动状态时，它们可能暂时移出相机视野，从而使准确位姿估计难以持续。 为此，我们引入一个基于等变扩散策略的主动位姿跟踪模块，该策略通过模仿学习进行训练。 该模块使机器人能够生成平滑的相机轨迹，在动态操作过程中持续保持关键物体特征的可见性，并降低位姿歧义。

据我们所知，这是首个同时评估位姿歧义并能够自主调整相机视角以主动消解歧义的位姿估计框架。 为促进研究社区的发展，我们将开源本工作的代码。

综上所述，我们的主要贡献如下：

• 我们提出了一种几何感知提示框架，结合 VLM 与逐视角位姿熵，实现鲁棒的零样本歧义检测与主动 6D 位姿估计。

• 我们引入统一的评分机制和可微渲染模块，用于选择 NBV，从而消解歧义并提升估计置信度。

• 我们提出了一种基于模仿学习训练的等变扩散策略的主动位姿跟踪模块，用于在物体运动时生成保持可见性和准确性的平滑相机轨迹。

• 我们通过大量仿真与真实世界实验验证了该框架，相较基线方法取得了显著性能提升。

II. RELATED WORK

A. 6D 位姿估计

近年来，基于单张 RGB-D 图像并利用神经网络的 6D 位姿估计方法 [5], [6], [7], [19], [20], [21] 在性能上已超越经典方法 [22]。 然而，这些方法并未考虑在特定视角下可能存在的位姿歧义问题。 为解决这一问题，主动位姿估计技术通过调整相机视角来提升整体估计置信度，但这类方法通常依赖于针对特定物体的训练、人工设计的启发式规则或高成本的标注数据 [16], [17], [18]。 不同于上述方法，我们的方法通过一种零样本、几何感知的框架主动检测并消解几何歧义，并引入可微渲染模块来模拟替代视角，从而实现主动去歧义。

B. 视觉-语言模型在机器人中的应用

视觉-语言模型（VLM）的快速发展进一步推动了机器人领域的进步，使智能体能够以高度泛化、与具体任务无关的方式理解自然语言指令并对视觉场景进行推理 [23]。 这类基础模型的一个关键优势在于，它们已在大规模数据集上进行预训练，通常可以直接作为高层规划器 [24], [25], [26], [27] 或低层控制器 [28], [29], [30], [31] 部署到机器人系统中，而无需从零开始学习，从而显著降低训练成本并提升泛化能力。 在本工作中，我们将 VLM 的作用从语义推理扩展到几何去歧义，使其能够支持零样本的主动位姿估计。

C. 面向机器人的等变扩散模型

近年来，基于扩散的模型已成为机器人操作领域中的一种强有力范式 [32]。 尤其是等变扩散模型，能够利用机器人所处的三维欧几里得空间中的内在对称性，例如旋转不变性。 现有的等变扩散方法大致可分为两类：一类用于预测单个末端执行器位姿 [33], [34], [35], [36], [37]，另一类用于生成完整的操作轨迹 [38], [39], [40], [41]。在此基础上，我们的工作提出了一种基于等变扩散策略的相机跟踪器，用于在运动过程中最小化位姿歧义并保持物体的可见性。

III. PROBLEM FORMULATION

我们将主动 6D 物体位姿估计与跟踪形式化为一个统一的决策问题。在仅给定一个新物体的 CAD 模型以及相机初始位姿的情况下，目标是在操作过程中持续估计准确且无歧义的 6D 位姿。该过程首先进行零样本歧义检测：利用几何感知提示词与 FoundationPose，我们查询 VLM 来计算当前视角的位姿熵。若该熵超过阈值，表明歧义较高，我们将从预先定义的候选集合中选择下一最佳视角（NBV）。这一选择通过可微视图合成实现：渲染多个虚拟视角，并由 VLM 与位姿熵对其评分，再进行排序以确定信息量最大的视角。与此同时，在物体持续运动与遮挡的情况下，我们通过在示范数据上训练等变扩散模型来学习主动跟踪策略，使机器人能够生成保持物体处于视野内的相机轨迹，从而保证高精度操作。

0）这段话到底在解决什么问题？

你要的输出是什么？

输出是物体的 6D 位姿：

• 3D 位置：物体在空间里在哪（x, y, z）
• 3D 朝向：物体怎么转的（roll, pitch, yaw 或旋转矩阵/四元数）

为什么难？

因为真实操作里会出现：

• 遮挡：手、夹爪、桌沿挡住物体
• 视角歧义：从某个角度看，物体“像是”好几种姿态（比如对称物体、纹理少、只看到一面）
• 物体在动：你在操作，它也在移动、旋转

所以系统目标是：在整个操作过程中持续输出“准确且无歧义”的 6D 位姿。注意两点：

• “持续”：不是只估一次，而是每一帧/每一时刻都要跟上
• “无歧义”：不是“看起来差不多”，而是要让系统确信“就是这个姿态”，不含糊

---

1）“统一的决策问题”是什么意思？

它把整个过程当成一个“智能体做决策”的问题（像强化学习/规划那种形式）：

• 状态（state）：当前相机看到的图像、当前估计的姿态、物体CAD模型、可能还有历史信息
• 动作（action）：下一步相机往哪移动/看哪里（选NBV），以及在跟踪阶段怎么走一条相机轨迹保证一直看得到
• 目标（objective）：让位姿误差小，同时把歧义降低（让“确定性”更高）

用人话说：不是被动地站着看，而是像“摄影师”一样主动找角度，让自己看得更准、更确定。

---

2）系统总体结构：两条“并行”主线

这段话其实有两条互补的线：

A. 视角歧义检测 + 选 NBV（看不清就换角度）

• 先判断现在这个视角“是否容易看错”
• 如果歧义高，就从候选视角里挑一个“信息量最大”的下一个视角（NBV）

B. 主动跟踪策略（物体动/遮挡时也要一直看着它）

• 物体在动、会被挡：相机不能乱走，要学会走一条轨迹，让物体尽量一直在视野里

你可以理解为：

• A 负责“消除歧义”（把“看不准”变成“看准”）
• B 负责“不断线”（别跟丢，别被遮挡搞崩）

---

3）零样本歧义检测：它怎么判断“我现在看得含糊不含糊”？

3.1 “零样本”是什么意思？

不需要针对这个新物体再训练一个分类器/歧义判别器。给你一个新物体 CAD 模型，直接就能用。

3.2 FoundationPose 在这里扮演什么角色？

你可以把 FoundationPose 当作一个“基础的姿态估计器”：

• 输入：图像 + CAD（几何先验）
• 输出：一个姿态估计（可能还有候选姿态/分数）

它提供了“几何对齐”的能力：利用 CAD 的形状去解释图像里看到的轮廓/边缘/关键几何。

3.3 VLM 在这里做什么？

VLM（视觉语言模型）在这里不是聊天用的，而是当一个 “智能打分器/评估器” ：它看当前视角下的几何与匹配情况，给出对“当前姿态到底有多确定”的评价。 这里关键是“几何感知提示词（geometry-aware prompt）”：

• 你不是随便问“这是什么”，而是用提示词让 VLM 关注几何一致性、可见面、遮挡程度、对称导致的多解等信息。
• 本质上：把“我现在这张图是否足以唯一确定姿态”这个问题，交给 VLM 做判断/打分。

3.4 “位姿熵”到底是什么？（最容易卡住的点）

熵（entropy）你可以当成“不确定性”的数值化表达：

• 如果系统认为姿态只有一种可能，概率几乎集中在一个解上 → 熵低（很确定）
• 如果系统觉得姿态可能有好几种都说得通（比如正反面分不清、对称物体转180度一样） → 概率分散 → 熵高（很含糊）

所以“计算当前视角的位姿熵”就是：

用 VLM + FoundationPose 的信息，估计“姿态分布有多分散”，也就是“我到底有多不确定”。

然后设一个阈值：

• 熵 > 阈值：说明“现在这个角度太容易看错/看不准” → 触发去选 NBV
• 熵 ≤ 阈值：说明“现在够确定了” → 可以继续跟踪/操作

---

4）NBV（下一最佳视角）怎么选？为什么要“可微视图合成”？

4.1 NBV 是什么？

Next Best View：下一步把相机挪到哪里，能让信息量最大、歧义最小。

4.2 为什么不是随便转一转？

因为你在操作任务里时间很宝贵，相机移动有成本。你希望：

• 少走冤枉路
• 一次移动就尽量把歧义打掉

4.3 “候选集合”是什么意思？

不是无限多角度都试，而是先准备一批合理的相机位姿候选（比如围绕物体的一圈、或在可达空间里采样的若干点）。 然后从里面挑最优的那个。

4.4 “可微视图合成”在干嘛？（核心）

它做的是：先在电脑里“假装”相机去了那些候选视角，看看会看到什么，再决定真实相机该去哪。 具体步骤用人话拆开：

1. 渲染多个虚拟视角

   • 你已知 CAD 和当前估计姿态（或者姿态候选）
   • 对每个候选相机位姿，在仿真/渲染器里生成一张“如果我站在那里会看到的图”（synthetic view）

2. 用 VLM + 位姿熵给每个虚拟视角打分

   • 直觉：如果某个角度能看到之前看不到的关键几何特征（比如一个缺口、一个非对称边），那它会更容易“唯一确定姿态”
   • 打分就是在评估：这个视角能把不确定性降多少（信息量多大）

3. 排序，选分最高的视角作为 NBV

   • 然后真实相机移动过去拍真实图像

4. 为什么强调“可微” 可微的好处是：

   • 你可以把“视角→渲染→熵/评分”这一整条链条当成可优化的函数
   • 未来可以用梯度来更高效地找更优视角（即使这里最终只是在候选集中挑，也强调它的“可优化性”和系统可端到端整合）

你不用把“可微”想得很玄，它的直观意义就是：

这不是一个黑箱试错，而是一个能被优化的、可计算的选视角过程。

---

5）同时进行的“主动跟踪策略”：为什么需要“等变扩散模型”？

NBV 解决的是“歧义高时换角度”，但你还面临另一个更现实的问题： 物体在动 + 会被遮挡，相机如果只按 NBV 跳来跳去，可能会“跟丢”或被手挡住。 所以它引入第二个模块：学一个相机运动策略，让物体尽量一直在视野里。

5.1 “示范数据”是什么？

就是一些人类/系统演示的“在操作过程中相机应该怎么走”的轨迹数据。
比如：当手要遮挡了，相机往侧上方挪一点；当物体转到背面，相机绕一下保持关键面可见。

5.2 扩散模型在这里做什么？（别把它当画图模型）

扩散模型你可以理解为一种“从随机噪声一步步生成合理动作/轨迹”的生成器。

在这里它生成的不是图像，而是：

• 接下来一段相机轨迹（或相机控制序列）

为什么用它？因为跟踪轨迹通常是：

• 多峰的（很多条轨迹都可能合理）
• 需要平滑、连续、符合动力学/几何约束 扩散模型很擅长从示范里学这种“分布”，生成平滑且多样的动作序列。

5.3 “等变（equivariant）”是什么意思？（另一个容易卡住的点）

等变的直观意思：世界整体旋转/平移，策略也应该跟着一致变化。

举个最直接的例子：

• 如果同一个操作场景整体绕 z 轴转了 90°
• 一个“好的相机跟踪策略”不应该重新学一套，而是输出的相机运动也整体跟着转 90°

这叫“结构化的对称性利用”，能带来：

• 泛化更好（换摆放方向也行）
• 数据效率更高（不用每个角度都学一遍）

所以“等变扩散模型”就是：

用一个满足几何对称性规律的扩散网络，从示范中学会生成相机轨迹。

5.4 它怎么保证“物体在视野内”？

目标很明确：生成相机轨迹，让物体一直可见（减少被遮挡或出画面）。 你可以把它理解成学会了“摄影师跟拍技巧”：

• 预测接下来哪里会遮挡
• 提前移动到更安全、更能看到物体关键部分的位置

---

6）把所有步骤串起来：一眼看懂的执行流程

下面是这段话隐含的“系统运行时序”（我用最不绕的方式写）：

1. 输入条件：新物体 CAD + 初始相机位姿（不用为新物体重新训练）
2. 当前视角先估姿态（FoundationPose 提供几何对齐的姿态估计）
3. 做歧义检测：VLM 根据几何提示词，输出当前视角的“位姿熵”（不确定性）
4. 如果熵太高（太不确定）：
   4.1 在一组候选视角上做可微渲染，得到一批“虚拟看到的图”
   4.2 VLM + 熵对每个虚拟视角打分
   4.3 选得分最高的那个 → NBV
   4.4 相机移动到 NBV，重新看、重新估姿态（歧义被压下去）
5. 无论是否触发 NBV，同时都要跟踪： 用示范训练出来的等变扩散策略，生成相机轨迹，让物体尽量一直在视野中，避免遮挡导致跟丢
6. 最终效果：在操作全过程里持续输出“准确 + 不含糊”的 6D 位姿

IV. METHOD

我们的方法由两个紧密集成的模块组成：一个用于消解 6-DoF 位姿歧义的主动位姿估计流水线（算法 1），以及一个在物体运动过程中利用等变扩散策略维持位姿估计精度的主动位姿跟踪模块（算法 2）。

0. 一句话先抓住全局：这篇方法解决什么？

你想在机器人操作物体时，持续得到物体的6D位姿（位置+朝向）。但现实中会遇到：

• 视角歧义：从某个角度看，姿态可能有“好几种都说得通”（尤其对称物体、只看到一面、纹理少）
• 遮挡+运动：你在操作，手/夹爪会挡住，物体也会移动，导致跟踪容易漂、丢

所以作者把系统拆成两个紧密耦合模块：

1. 主动位姿估计（Active Pose Estimation）： 发现“现在这个视角是否歧义”，如果歧义就主动选一个“下一最佳视角NBV”去拍新图，把歧义打掉，再用FoundationPose出最终位姿。（Fig.2 + Algorithm 1）

2. 主动位姿跟踪（Active Pose Tracking）： 位姿确定以后，在物体运动过程中，学习一个相机运动策略，让相机总能“跟得住、看得清”，从而维持位姿精度。（Fig.3 + Algorithm 2，用等变扩散模型做相机轨迹生成）

A. 主动位姿估计

1. 模块A：主动位姿估计（Fig.2）到底怎么跑？

Fig.2分两段：离线（offline） 做准备，在线（online） 实时决策。

我们的主动位姿估计流水线（见图 2）包含两个紧密耦合的阶段：离线的几何感知提示词构建，以及在线的歧义检测与下一最佳视角（NBV）选择。 给定实时图像$I_{\text{live}}$及其相机到基座的变换$T_b^c$，我们按如下步骤进行。

图 2：主动位姿估计流程。 （1）离线阶段：系统基于渲染视角计算位姿熵，并使用有歧义与无歧义的示例构建几何感知提示词。 （2）在线阶段：系统通过 VLM 从实时图像评估歧义程度，并通过渲染与熵引导的评分机制选择下一最佳视角（NBV）。

首先，我们对 CAD 模型渲染一组稠密的视角 { R k } \{R_k\} {Rk​}。 对于每一个渲染视角$R_k$，我们调用 FoundationPose 得到其 6-DoF 旋转分布，并计算其香农熵
$$H_k=\text{Entropy}(\text{FoundationPose}(R_k)).$$

1.1 离线阶段：造一个“几何感知提示词”（Geometric-Aware Prompt）

目标

让VLM学会一件事：

“哪些视角会让这个物体很容易看错/看不唯一，哪些视角能看得很确定。”

你怎么教它？你用CAD渲染出来的图当“教材”。

Step 1：从CAD渲染一堆视角$R_k$

• 这一步就是“把物体从很多角度拍照”（在渲染器里拍）。
• 你叫它“稠密视角”，意思是角度很多、覆盖很全面。

Step 2：每个视角丢给FoundationPose，得到“旋转分布”，算熵

$$H_k=\text{Entropy}(\text{FoundationPose}(R_k)).$$

这句最容易卡，我给你翻译成白话：

• FoundationPose在某个视角下，并不一定“只给一个旋转答案”。 它可能觉得“旋转A也像，旋转B也像……”

• 所以你把它看成输出了一个“旋转的概率分布”（哪个旋转更可能）。

• 熵（Shannon entropy） 就是“这个分布有多分散”的数字：

  • 分布很集中（几乎只有一个旋转可能）→熵低→不歧义
  • 分布很分散（好几个旋转都像）→熵高→有歧义

所以离线阶段你就在做：

“哪些渲染视角会让FoundationPose很确定？哪些会让它很不确定？”

随后，我们按$H_k$对视角进行排序，并选取一组低熵的“无歧义”视角$U$（如轴测视图、底视图、后视图）以及一组高熵的“有歧义”视角$A$。 几何感知提示词通过聚合$U\cup A$中的图像构建而成。

Step 3：按熵挑两组示例：无歧义$U$和有歧义$A$

• 低熵视角→“无歧义”集合$U$
• 高熵视角→“有歧义”集合$A$

然后用$U\cup A$这些图去拼一个prompt（图2(a)里那个“Geometric Analyser”），让VLM知道：

• “这种长相的视角往往歧义大，要小心”
• “这种视角往往能看清楚”

离线阶段的本质： 你用“FoundationPose的熵”给渲染图打标签，然后把这些标签塞进prompt，教VLM什么叫“歧义视角”。

在运行时，我们获取实时图像$I_{\text{live}}$，并计算歧义概率
$$p_{\text{amb}}=P_{\text{VLM}}(\text{“ambiguous”}\mid I_{\text{live}},\text{prompt}).$$

1.2 在线阶段：先判歧义，再决定要不要选NBV

在线输入：实时图像$I_{\text{live}}$和相机外参$T_b^c$。

Step 1：VLM做“歧义二分类”，输出$p_{\text{amb}}$

你定义： $$p_{\text{amb}}=P_{\text{VLM}}(\text{“ambiguous”}\mid I_{\text{live}},\text{prompt}).$$

把实时图+离线prompt喂给VLM，让它回答“这张图的视角会不会导致姿态估计不唯一？” 它输出一个概率$p_{\text{amb}}$。

若$p_{\text{amb}}<\tau$（阈值$\tau =0.5$），则立即调用
$$T_s^c=\text{FoundationPose}(I_{\text{live}})$$
以获得最终的 6-DoF 位姿。

Step 2：如果不歧义（$p_{\text{amb}}<\tau$），直接FoundationPose出最终位姿

$$T_s^c=\text{FoundationPose}(I_{\text{live}}).$$

你这里的阈值$\tau =0.5$，意思是：VLM觉得歧义概率不到一半，就别折腾，直接估姿态。

Step 3：如果歧义（$p_{\text{amb}}\ge \tau$），进入“NBV选择”

这一步就是图2(c)里“Robot Imagination”。

否则，对于每一个候选虚拟相机位姿$C_j$，我们渲染其对应的假想视图
$${\hat{I}}_j=\text{Render}(C_j),$$

Step 3.1 你先准备一组候选相机位姿$C_j$

这是你允许相机下一步可能去的位置/角度（通常来自可达空间采样或预定义环绕点）。

Step 3.2 对每个候选位姿$C_j$，渲染“假想视图”${\hat{I}}_j$

$${\hat{I}}_j=\text{Render}(C_j).$$

这一步的直觉非常重要：

你不是让真实机器人一个个去试（那太慢），你先在渲染器里“幻想”站到每个候选视角会看到什么。

计算二级歧义概率
$$p_{\text{amb},j}=P_{\text{VLM}}(\text{“ambiguous”}\mid {\hat{I}}_j,\text{prompt}),$$
并计算其位姿熵
$$H_j=\text{Entropy}(\text{FoundationPose}({\hat{I}}_j)).$$

Step 3.3 对每个假想视图，同时算两件事：VLM歧义概率+FoundationPose熵

$$p_{\text{amb},j}=P_{\text{VLM}}(\text{“ambiguous”}\mid {\hat{I}}_j,\text{prompt}),$$ $$H_j=\text{Entropy}(\text{FoundationPose}({\hat{I}}_j)).$$

这俩各自代表什么？

• $p_{\text{amb},j}$：VLM从“语义/几何理解”角度觉得这视角歧义不歧义
• $H_j$：FoundationPose从“几何匹配不确定性”角度觉得分布散不散

随后，我们为每个候选视角计算联合评分
$$S_j=\lambda H_j+(1-\lambda )p_{\text{amb},j},$$
该评分在几何不确定性（熵）与语义歧义之间进行权衡。 接着，我们选择
$$j^{\ast }=\arg \underset{j}{\min }{S}_j,$$

Step 3.4 把两种不确定性混成一个分数$S_j$，选最小的

$$S_j=\lambda H_j+(1-\lambda )p_{\text{amb},j},$$ $$j^{\ast }=\arg \underset{j}{\min }{S}_j.$$

人话： 这个分数就是“这个视角有多可能让你看得更确定”。

• 熵越小越好
• 歧义概率越小越好 所以你最小化它。

将相机移动到$C_{j^{\ast }}$，采集新的实时图像$I_{\text{live}}^{\ast }$，并调用
$$T_s^c=\text{FoundationPose}(I_{\text{live}}^{\ast })$$

Step 3.5 真相机移动到$C_{j^{\ast }}$，拍新图$I_{\text{live}}^{\ast }$再做FoundationPose

$$T_s^c=\text{FoundationPose}(I_{\text{live}}^{\ast }).$$

到这里，模块1结束：你得到“已消歧”的6D位姿。

以获得最终的、已消歧的位姿（见算法 1）。

B. 主动位姿跟踪

2. 模块2：主动位姿跟踪（图3 / Algorithm 2）

模块1解决“某一时刻看不清就换角度”。
模块2解决“连续操作过程中一直别跟丢、别被遮挡”。

你这里的关键选择是：不用手工写一个复杂的优化目标（比如显式最小熵/最大可见性），而是直接模仿示范学一个相机运动策略。

在获得物体的 6D 位姿之后，我们将主动跟踪建模为一个条件扩散问题，以维持持续的可见性和精确的位姿估计（见图 3）。 不同于手工设计重规划目标函数，我们训练一个等变扩散网络来模仿示范中的相机运动。 在每个时间步$t$，我们构建观测
$$O_t=\left\{{}^o{T}_b^{t-H+1:t},{}^e{T}_b^{t-H+1:t}\right\},$$
其中包含过去$H$帧的：(i) 物体在机器人基座坐标系下的位姿${}^o{T}_b$，以及 (ii) 末端执行器的位姿${}^e{T}_b$。

2.1 你把“相机怎么动”当成一个条件扩散生成问题

2.1.1 观测$O_t$是什么？

$$O_t=\left\{{}^o{T}_b^{t-H+1:t},{}^e{T}_b^{t-H+1:t}\right\},$$

人话：你给策略看的输入是过去$H$帧的历史：

• ${}^o{T}_b$：物体相对基座的位姿序列（物体怎么在动）
• ${}^e{T}_b$：末端执行器相对基座的位姿序列（手/夹爪怎么在动）

这一步很关键：

你不是只看一帧，而是看一段历史，让模型能判断“趋势”：物体是往哪转、遮挡可能从哪里来。

2.1.2 你要生成的$A_t$是什么？

从图3和你后面的定义看，$A_t$代表“未来一段相机位姿序列”（连续SE(3) poses），最后你写：
{ A t 0 } ≡ { e T ^ b t + 1 : t + K } \{A^0_t\}\equiv\{{}^{e}\hat{T}_{b}^{t+1:t+K}\} {At0​}≡{eT^bt+1:t+K​}

也就是说：去噪完得到未来$K$步的相机（或末端/相机相关）的SE(3)轨迹预测。

我们初始化一个标准高斯噪声$A_t^K$，随后通过以下方式执行$K$步学习到的反向扩散过程：

$$A_t^{k-1}={\alpha }_k(A_t^k-{\gamma }_k{ϵ}_{\theta }(O_t,A_t^k,k))+{\sigma }_k\mathcal{N}(0,I),\text{\hspace{1em}(1)}$$

$$\mathcal{L}=\mathbb{E}\left[\right||{ϵ}_k-{ϵ}_{\theta }(O_t,{\hat{\alpha }}_k{A}_t^0+{\hat{\beta }}_k{ϵ}_k,k\left)\right|{|}^2],\text{\hspace{1em}(2)}$$

其中${\alpha }_k,{\gamma }_k,{\sigma }_k$（及其一步等价形式${\hat{\alpha }}_k,{\hat{\beta }}_k$）定义了 DDIM 噪声调度方案 [42]，旋转部分采用连续 6D 表示 [43]。 我们的${ϵ}_{\theta }$是一个轻量级的等变卷积网络，并且为了加快推理速度，我们直接预测样本而非噪声。 在运行时，我们采用滚动时域（receding-horizon）策略：在去噪得到
{ A t 0 } ≡ { e T ^ b t + 1 : t + K } \{A^0_t\}\equiv\{{}^{e}\hat{T}_{b}^{t+1:t+K}\} {At0​}≡{eT^bt+1:t+K​}
之后，执行最后$k$个相机位姿，然后向前滑动时间窗口并重复该过程。 该策略生成平滑且自适应的相机运动，能够自然地保持物体处于视野内并维持位姿精度——无需显式引入熵最小化项，因为歧义的减少是从学习到的策略中隐式涌现的。

2.2 扩散推理：从纯噪声一步步“洗干净”成一条合理轨迹

2.2.1 初始化噪声

$$A_t^K\sim \mathcal{N}(0,I).$$ 人话：一开始就是一坨随机乱轨迹。

2.2.2 做$K$步反向扩散（DDIM调度）

$$A_t^{k-1}={\alpha }_k(A_t^k-{\gamma }_k{ϵ}_{\theta }(O_t,A_t^k,k))+{\sigma }_k\mathcal{N}(0,I),\text{\hspace{1em}(1)}$$

这句的直觉你一定要抓住：

• $A_t^k$：当前“还带噪”的轨迹
• ${ϵ}_{\theta }(\cdot )$：网络输出一个“该怎么改”的方向（可以理解成“去噪建议”）
• ${\alpha }_k,{\gamma }_k,{\sigma }_k$：每一步改多少、保留多少随机性（DDIM的调度）

所以每一步就是：

根据观测$O_t$，把轨迹往“更像示范、更合理、更能看见物体”的方向推一点点，重复$K$次，最后得到$A_t^0$。

2.2.3 训练损失：让网络学会“预测去噪方向”

$$\mathcal{L}=\mathbb{E}\left[\right||{ϵ}_k-{ϵ}_{\theta }(O_t,{\hat{\alpha }}_k{A}_t^0+{\hat{\beta }}_k{ϵ}_k,k\left)\right|{|}^2],\text{\hspace{1em}(2)}$$

人话：训练时你有示范轨迹$A_t^0$，你人为给它加噪，得到带噪样本，然后要求网络把“加了多少噪声/该怎么还原”预测对。 这就是标准扩散训练套路：从“带噪”学会“还原”。
2.3 网络结构：等变卷积网络 + U-Net去噪（图3）

图3里很清楚：

• 输入：Observation + Proprioception（视觉观测+本体状态）
• 先过一个“Equi Encoder”（等变编码器）
• 再进“U-Net Denoising Model”（去噪U-Net）

2.3.1 “等变（equivariant）”到底解决什么？

一句话：

场景整体旋转/平移，模型输出的轨迹也跟着以同样方式旋转/平移。

这会让模型：

• 泛化更稳（物体摆放换个方向也能跟）
• 更省数据（不用每种朝向都学一遍）

2.3.2 “旋转采用连续6D表示”是为啥？

旋转如果用欧拉角会有奇异点（万向节锁），用四元数有符号/归一化等麻烦。 连续6D表示的核心目的就是：

让网络回归旋转时更平滑、更好学、不容易跳变。

（你文里引用[43]就是干这个的。）

2.3.3 “预测样本而非噪声”是什么意思？

扩散里常见两种参数化：

• 预测噪声$ϵ$
• 直接预测去噪后的样本$x_0$

你写“为了加快推理速度，我们直接预测样本而非噪声”，人话就是：

让网络更直接地产生“更干净的轨迹”，减少一些中间换算/提高速度。

2.4 运行策略：滚动时域（Receding Horizon）

你写：

• 去噪得到未来$K$步轨迹 { A t 0 } ≡ { e T ^ b t + 1 : t + K } \{A^0_t\}\equiv\{{}^{e}\hat{T}_{b}^{t+1:t+K}\} {At0​}≡{eT^bt+1:t+K​}
• 然后“执行最后$k$个相机位姿”，再窗口前移重复

别纠结“为什么是最后$k$个”这个措辞（论文这么定义我们就按它的定义说）。你要抓住它的控制思想：

每次规划一段（长度$K$），但只执行一小段（长度$k$），马上用新的观测再规划下一段。

这就是滚动时域的核心好处：

• 物体在动、遮挡在变：你不能一次计划到底
• 你必须“边走边重算”，才能自适应

3. 把两模块合成一个“完整闭环”你就彻底懂了

你这篇方法的整体逻辑是：

1. 先用模块1把“当前姿态搞准且不歧义”

• 不歧义：直接FoundationPose
• 歧义：渲染候选视角→用VLM+熵评分→选NBV→再FoundationPose

2. 然后用模块2在操作过程中“别跟丢、别被遮挡”

• 用过去$H$帧的物体/末端历史作为条件
• 扩散模型生成未来相机轨迹
• 滚动时域执行一小段、重规划一小段
• 结果就是：相机运动会“自然地”把物体保持在视野里，从而位姿估计也一直稳

你最后那句“无需显式引入熵最小化项，因为歧义减少从策略中隐式涌现”，用人话就是：

我不在损失里硬写“你必须让熵变小”，但因为示范里的好相机运动本来就会趋向于看清楚关键面，所以学出来的策略自己就会做出“降低歧义”的行为。

---

如果你接下来想把它“真正写成算法2的伪代码+张量维度”，我可以继续把：

• $O_t$具体如何拼成网络输入（序列怎么编码）
• $A_t$在SE(3)里怎么数值化（平移+6D旋转）
• ${ϵ}_{\theta }$用什么等变层（SE(3)-Transformer？e3nn卷积？）
• U-Net在“时间维K”和“特征维”上怎么卷 按实现级别拆到每一步。

V. EXPERIMENTS

在本节中，我们介绍硬件与软件配置、实现细节以及评测流程，并在具有挑战性的操作任务上报告主动位姿估计与主动位姿跟踪的实验结果。

A. 实验设置

实现细节：针对主动位姿估计，我们同时在仿真环境和真实世界中进行实验。 仿真实验基于 PyBullet [44] 实现，该平台为候选视角渲染和下一最佳视角（NBV）选择提供了受控环境。对于主动位姿跟踪和集成场景实验，我们仅在真实机器人平台上进行评估。所有真实世界实验均在两台 Franka Emika Panda 七自由度机械臂上进行：其中一台作为感知机械臂，在腕部安装 Intel RealSense D435 RGB-D 相机（640×480，30Hz）用于观测；另一台作为操作机械臂，配备平行夹爪（见图 1）。

如图 4 所示，我们从 MP6D 数据集 [45] 中选取三个物体用于评估，并另外选择一个物体用于装配任务。所选的四个物体均具有较高的位姿歧义性，适合用于测试歧义检测与主动视角选择能力。

在主动位姿估计流水线中，我们调用 ChatGPT4o API 作为视觉-语言模型（VLM）。 每一帧实时 RGB 图像都会被裁剪并缩放至 224×224，并与几何感知提示词配对输入。 在每次歧义检测中，我们渲染$M=12$个候选视角，设置融合权重$\lambda =0.6$，并使用 FoundationPose v1.0 进行 6-DoF 位姿估计与熵计算。

对于主动位姿跟踪模块，我们为每个任务采集 10 条专家示范，每条示范包含约 200 帧、以 10 Hz 频率记录的物体与末端执行器位姿。 我们使用 AdamW 优化器（batch size 为 40，学习率为$3\times 10^{-4}$）训练等变扩散策略，共训练 2000 个 epoch。 在推理阶段，该策略预测未来$K=20$个末端执行器动作；系统在重新规划前执行其中最后$k=5$个动作。 所有训练与推理均在单张 NVIDIA RTX 4090 GPU 上完成。

位姿估计场景。 我们在两种条件下评估主动位姿估计方法：

1）随机放置：每个物体在工作空间中以随机的位置和朝向放置。
2）高熵放置：每个物体被刻意放置在预先计算得到的高熵（即高歧义）视角位置。

位姿估计评测基线方法。

• 固定视角（Fixed-View）：在初始相机位姿上仅运行一次 FoundationPose，不进行 NBV 选择。

• 随机 NBV（Random-NBV）：在检测到歧义后，从 M 个候选视角中随机选择一个进行第二次 FoundationPose 估计。

位姿估计评测指标。 我们报告成功率（SR），其定义为：最终估计的 6D 位姿中，平移误差小于 5 mm 且旋转误差小于 5° 的试验所占比例。 每种方法在每个场景下测试 10 次。

位姿跟踪场景。 我们在四种具有挑战性的条件下测试所有跟踪方法：

1）长距离直线运动：物体沿直线路径从点 A 移动到点 B，其中点 B 位于随机位置。

2）圆周旋转运动：物体绕桌面上的固定点旋转。

3）短时遮挡：在跟踪过程中物体被短暂遮挡（最长 1 秒），用于测试系统在遮挡期间维持位姿估计或在物体重新出现后恢复估计的能力。

4）随机空间运动：物体在三维空间中进行不可预测的平移和旋转运动，包括急转弯和速度变化，以考验跟踪器稳定跟随复杂轨迹的能力。

位姿跟踪基线方法。 针对主动位姿跟踪模块，我们将提出的扩散策略跟踪器与以下方法进行比较：

• 位姿伺服（Pose-Servo）：一种经典的基于位姿的视觉伺服方法。腕部相机提供当前的 6-DoF 物体位姿，比例控制器将位姿误差转换为末端执行器的速度指令。

• 世界相机（World-Camera）：使用固定且已标定的顶置相机，通过 FoundationPose 持续跟踪物体的 6-DoF 位姿。

位姿跟踪评测指标。 我们报告每个试验的成功率（SR），定义为：方法在无任何“位姿丢失（pose-loss）”事件的情况下完成任务的比例。 每种方法在每个场景下进行 20 次独立试验。

位姿估计与跟踪的集成场景。 为验证主动位姿估计与主动位姿跟踪结合后的综合效果，我们在一个标准的“销-孔（peg-in-hole）”装配任务上进行评估： 机器人从桌面抓取物体，并将其插入一个固定的插孔中，该插孔的位置在每次试验前随机设置。 装配任务本身并非本文的研究重点，因此我们直接使用文献 [46] 中的装配策略。

集成基线方法。

• 固定视角（Fixed-View）：使用单个固定的世界相机视角运行一次 FoundationPose，并基于该估计结果执行装配任务。

• 固定视角 + 位姿伺服（Fixed-View + Pose-Servo）：在初始抓取视角上运行一次 FoundationPose，随后采用经典位姿伺服完成装配。

• 随机 NBV + 位姿伺服（Random-NBV + Pose-Servo）：在抓取阶段检测到歧义后，随机选择一个 NBV 进行第二次 FoundationPose 估计，然后使用位姿伺服进行装配。

评测指标。 我们报告成功率（SR），即成功完成销-孔装配任务的试验比例。 每种方法评估 10 次。

B. Experimental Results

主动位姿估计结果。 在主动位姿估计实验中，我们评估了两种放置条件——随机放置（Random Placement）和高熵放置（High-Entropy Placement，即刻意制造歧义的初始状态）。 在仿真环境中（表 I），固定视角（Fixed-View）基线在随机放置条件下仅达到 60.0% 的成功率，而在高熵放置条件下则下降至 20.0%。 随机 NBV（Random-NBV）选择策略将成功率分别提升至 62.5% 和 52.5%。 相比之下，我们的主动位姿估计流水线在随机放置条件下取得了 97.5% 的成功率，在高熵放置条件下取得了 95.0% 的成功率，展现出在两种场景下均极为稳健的性能。

在真实世界实验中（表 II），固定视角方法在随机放置与高熵放置条件下的成功率分别为 57.5% 和 20.0%，而随机 NBV 在两种条件下均达到 50.0%。 我们的方法在真实环境中依然保持了较高的成功率，分别达到 92.5% 和 95.0%。 尽管成功率相较仿真略有下降——这可能源于传感器噪声和标定误差——但整体性能依然保持稳定且较高。 图 5 展示了一个真实世界中成功完成位姿估计的示例。

主动位姿跟踪结果。 表 III 给出了在四种跟踪场景下、每个物体进行 20 次试验后的成功率（SR）。 在长距离直线运动场景中，位姿伺服（Pose-Servo）仅达到 61.3% 的成功率，其原因在于指令运动可能超出机器人的可达工作空间；而世界相机（World-Camera）由于物体离开固定视野范围（FOV），成功率仅为 48.8%。 我们的扩散策略跟踪器能够预测中间路径点并实现平滑自适应，从而取得了 87.5% 的成功率。 在圆周旋转场景下，位姿伺服由于存在不可达姿态而始终失败（0.0%），世界相机在物体旋转超出固定视野时成功率为 62.5%；相比之下，我们的方法通过持续调整相机位姿，保持了 91.3% 的成功率。 在存在短时遮挡的情况下，位姿伺服和世界相机的成功率分别仅为 1.3% 和 17.5%，而我们的跟踪器即便面对快速遮挡，仍能可靠地重新捕获目标，取得 52.5% 的成功率。 在随机空间运动场景下，基线方法的成功率分别为 50.0%（位姿伺服）和 58.8%（世界相机），而我们的方法达到 72.5%，显示出对不可预测轨迹更强的适应能力。

销-孔装配任务结果。 表 IV 给出了销-孔装配任务中，每种方法在 10 次试验下的成功率（SR）。 固定视角方法的成功率为 40%，这是由于随机放置的插孔可能位于固定相机的视野之外。 在相同固定视角下加入经典位姿伺服系统后，成功率提升至 50%，但伺服控制常因无法到达期望的末端执行器位姿而失败。 引入一次随机 NBV 能够通过降低初始歧义将成功率提升至 70%，但固定相机的伺服系统仍然面临不可达位姿的问题。

我们的 ActivePose 框架通过在抓取阶段选择去歧义视角，并利用基于扩散的策略持续跟踪物体直至插入插孔，实现了 90% 的成功率。 该方法在整个装配过程中始终保持视觉接触——这是固定世界相机基线无法实现的——同时也避免了经典位姿伺服方法对末端执行器位姿所施加的严格约束。

VI. CONCLUSION AND FUTURE WORK

本文提出了 ActivePose，一个用于主动 6D 物体位姿估计与跟踪的统一框架。 我们的方法利用结合几何感知提示词的视觉-语言模型（VLM）进行零样本歧义检测，并将可微的“机器人想象”模块与 FoundationPose 的熵度量相结合，以选择下一最佳视角（NBV）实现位姿去歧义。 此外，等变扩散策略跟踪器在操作过程中生成相机轨迹，从而确保整个过程中的位姿估计精确可靠。 ActivePose 在仿真与真实世界实验中均持续获得高于经典基线的成功率，证明其在整个操作过程中具备鲁棒的位姿去歧义能力与准确的位姿估计表现。

然而，我们观察到，尽管等变扩散策略能够生成平滑的相机轨迹，但当前方法在输入特征上高度依赖位姿信息，使其对旋转变化比对平移变化更为敏感。 未来工作中，我们计划探索更合适的输入表示方式，例如引入基于图像的特征。

VII. ACKNOWLEDGMENTS

我们使用了 ChatGPT（OpenAI，GPT-5）来辅助文献检索，并进行少量语法/写作风格的润色修改。 所有内容均由作者审阅并修订，最终稿件完全为作者独立完成的工作。