文章概括

作者：Elizabeth A. Olson, Jana Pavlasek, Jasmine A. Berry and Odest Chadwicke Jenkins
来源：
原文：
代码、数据和视频：
系列文章目录：
上一篇：
https://blog.csdn.net/xzs1210652636?spm=1000.2115.3001.5343
下一篇：

摘要

粒子滤波是一种常见的六自由度（6D）姿态估计技术，因为它能够有效地表示物体姿态的信念。然而，由于6D姿态的高维性质，粒子滤波很容易出现粒子剥夺现象。当粒子剥夺发生时，在重要性采样过程中会导致基本信念分布的模式坍塌。如果真实状态周围的区域受到模式崩溃的影响，恢复它的信念是具有挑战性的，因为该区域在粒子形成的概率质量中不再被代表。以前的方法通过随机化和重设信念分布中的粒子来缓解这个问题，但确定重振的频率依赖于手工调整抽象的启发式方法。在本文中，我们通过引入反假设似然函数来估计每个时间步骤的必要重振率，该函数与标准似然一起使用。受证据推理中的可信度和不可信度概念的启发，我们的反假设可能性函数的增加为每个粒子分配了一个怀疑等级。整个粒子集的信心和怀疑的竞争累积值被用来估计过滤器内的失败程度，以确定需要重新激活的粒子部分。我们在刚体物体6D姿势跟踪任务上证明了我们方法的有效性。

1. 简介

随着机器人助手被赋予完成复杂家务的任务，如准备膳食或整理房间，它们必须能够与各种物体互动。在非结构化场景中的物体姿态估计仍然是一个挑战，因为感知中的模糊性来自于遮挡和对称性。基于粒子的推理方法已经被广泛应用于六自由度（6D）物体姿势估计和跟踪，因为它们能够用有限的样本集表示高维空间[1], [2], [3]。这些方法对估计的不确定性进行建模，并能够保持多种可能的姿势假设，这为物体遮挡和模糊的对称性等挑战性场景提供了鲁棒性。

尽管有这些有前途的特性，粒子过滤器算法被迫限制其样本集的大小，以确保机器人应用的可操作性。当应用于6D姿势跟踪时，与连续状态空间的总体大小相比，粒子过滤器通常只能提供少量的样本。由于样本集无法完全覆盖空间，状态空间的某些区域将不包含粒子，使其在信念分布中的表示崩溃。这种现象被称为粒子剥夺，可能是由于初始化不良或重要性抽样的随机性造成的。在这些区域重新获得信念是具有挑战性的，可能会导致过滤器收敛到一个不正确的局部最优。

缓解粒子匮乏的一个策略是粒子重振[4]，在这个策略中，重新初始化的样本被定期添加到集合中。然而，确定在一个给定的迭代中被重新激活的粒子部分往往需要通过试验和错误进行繁琐的手工调整。已经提出了一些适应性方法，通过利用似然函数中提供的信息来缓解这一挑战[5], [6], [7]。似然函数通过测量假设与观察到的传感器数据的对应关系，对每个样本给予重要性加权。然而，它只提供了一个相对权重，即哪些样本更好或更差，而没有说明样本集的绝对误差。

我们引入了反假设粒子过滤器（Counter-Hypothetical Particle Filter，CH-PF）来对抗高维状态空间中的粒子剥夺问题。我们的方法建议量化真实状态在样本集中未被代表的信心。为了这个目的，我们对反对某一特定假设的证据进行建模，称为反假设可能性。我们的工作通过证据推理（Dempster-Shafer理论）[8]的视角独立于传统的可能性来衡量这一权重。这个框架认为，证明一个结果的合理性和不合理性可以基于不同的因素，并且由于潜在的重叠和基础证据的模糊性而不是“零和”的。每个粒子都被赋予一个可能性和一个反假设可能性的权重。我们的方法利用这两种可能性来量化整个样本集的累积信心和怀疑。这些值之间的关系被用来推理真实状态在我们的样本集中代表性不足的可能性，并反过来被用来计算粒子重振的适应率。

在本文中，我们提出了Counter-Hypothetical Particle Filter，这是一种粒子过滤算法，旨在减轻挑战性环境中6D姿态跟踪的粒子剥夺问题。我们介绍了Counter-Hypothetical likelihood，并解释了它是如何与传统的likelihood一起使用，通过利用不可信度信息来抵消粒子剥夺的，如图1所示。我们在YCB视频数据集[9]上对CF-PF的6D单一物体姿势跟踪进行了评估。我们的方法在高度闭塞的情况下取得了更好的准确性，特别是在深度数据不可用的情况下。

图1：为了估计粒子过滤器何时失效，我们通过反假设可能性函数来衡量与每个样本相关的疑问。我们对反对某一估计的证据（灰色）和支持这一估计的证据（黄色）都进行量化。我们对这些数量的估计是相互独立的，因为它们不是由于观察中的模糊性而产生的零点（蓝色）。这些权重的相对大小根据观察和估计的质量而波动，如一个杯子所示，它是（A）明确的不可能（B）可信但由于手柄被遮挡而含糊不清（C）明确的可能（D）高度含糊。

II. 相关工作

A. 机器人的物体姿态估计和跟踪

姿态估计和跟踪在机器人界受到了极大的关注。近年来，各种工作已经证明了数据驱动的方法有能力在单个视图上提供有区别的姿态估计[9]、[10]、[11]、[12]，或在一连串的观察中进行姿态跟踪[13]。这些方法取得了令人印象深刻的结果，但容易出现不准确的情况，特别是在具有挑战性的场景中，如有大量杂波的场景。概率推理方法代替了对姿态估计的信念，以便为机器人操纵应用提供额外的稳健性[14]。我们专注于物体姿势估计和跟踪的概率推理，特别是粒子过滤。

粒子滤波是一种迭代推理算法，它可以用一组从状态空间采样的加权粒子来表示一个任意的非参数信念分布[5]。粒子过滤是6D姿势估计和跟踪的常用技术[2], [15], [16], [17]，因为它能够有效地近似高维状态空间并表示信念中的多个竞争性假设。最近，深度卷积神经网络（CNN）已被应用于粒子过滤的姿势估计。Deng等人[3]从自动编码器嵌入中创建了一个观察模型，该模型被用于RaoBlackwellized粒子过滤器中。最近，这种方法被扩展到类别级跟踪[18]。尽管我们工作的主要贡献是引入了CounterHypothetical似然函数，但我们也展示了如何以端到端方式类似地学习它。

为了缓解与充分采样高维6D姿势空间有关的挑战，以前的工作已经利用了特定领域的知识，如物理约束[19]、机器人手臂关节角度[20]、[1]或上下文信息[21]。使用粒子信念表征的信念传播[22]已被应用于基于部件的物体模型，以便为高维铰接物体定位任务提供因素[23], [24]。同样，我们的工作也解决了基于抽样方法的操纵任务中的粒子剥夺问题，但我们的目标是通过自适应重振来实现，并不依赖于提供的物体-环境交互模型或基于部件的物体模型。

B. 鲁棒性的粒子滤波

许多工作都集中在缓解粒子剥夺的问题上。一种方法是退火法，在这种方法中，重要性权重的分布根据手工调整的时间表进行平滑，以避免在重要性采样过程中出现塌陷模式[25]。Pfaff等人提出了一种自适应方法，利用每个粒子周围的局部密度估计来平滑重要性权重，用于移动机器人定位[26]。相比之下，CH-PF不需要修改采样权重，而是通过重振来处理粒子的剥夺。

在移动机器人蒙特卡洛定位的全局定位阶段，粒子剥夺是一个常见的问题[5]，促使许多工作根据需要重新初始化样本。一种方法是基于传感器读数从反分布中取样[27]，或者当样本集的平均可能性较低时 "重置 "一个粒子子集[6]。增强的蒙特卡洛定位[4], [28]扩展了这一想法，从均匀分布中进行粒子重振，其速度与粒子权重的长期和短期平均数之间的差异成正比，而不是一个固定的阈值。这些方法需要一个传感器模型，从中可以有效地提取样本，这对RGB图像的建模是有挑战性的。Fox等人提出根据样本近似的质量来修改样本集的大小[7]。Zhang等人提出了一种自适应方法，该方法保持一个固定的样本量，并通过 "类似能量区域 "的样本来增加样本量[29]。这种方法需要对状态空间进行离散化。最近在定位方面的工作通过以固定的速率从该提议中取样来利用神经网络的估计，并通过重要性取样将粒子融合到分布中[30]。

这些方法中的每一个都使用粒子的似然权重来估计样本集的质量。而CH-PF则使用一个单独的信息源，即反假设似然，与似然函数一起，对粒子集的整体质量进行估计。我们从 “证据推理”[8]中得到启发，在寻找支持性证据的似然函数之外，单独测量反驳假设估计的证据。

III. 背景：粒子滤波

我们考虑在一段时间内跟踪一个已知物体的问题。给定一串RGB图像或RGB-D数据，$z_{1:t}$，我们试图定位一个物体在时间$t$的姿势，$x_t\in \mathcal{X}$。我们还用$u_{1:t}$来模拟任何由用户输入或抖动引起的系统运动。这里$\mathcal{X}$代表6D姿势的空间，由3D平移和3D旋转组成。

贝叶斯过滤器试图通过在每个时间步骤$t$迭代更新分布来模拟状态的后验分布$p(x_t|x_{1:t-1},z_{1:t},u_{1:t})$，后验被称为$x_t$的信念，$bel(x_t)$。在每个时间步骤，预测的信念，$\widehat{bel}(x_t)$，是通过将行动模型应用于先验信念分布而得到的。采用马尔科夫假设：

然后，我们可以根据当前的观察结果$z_t$更新这个分布，以估计后验分布：

A. 粒子滤波

粒子过滤器是一种贝叶斯滤波算法，其中信念分布$bel(x_t)$是一个由粒子集${\mathbb{X}}_t$近似的非参数分布：

每个粒子，$x_t^i$都有一个相应的权重，${\pi }_t^i$。方程（1）中的预测信念是通过对前一个样本集${\mathbb{X}}_{t-1}$中的每个粒子应用行动$u_t$形成的。

粒子集${\mathbb{X}}_t$是通过重要性抽样产生的，其中目标分布是$bel(x_t)$，提议分布是$\widehat{bel}(x_t)$。从提议中抽取样本，并进行替换，其中一个粒子被抽取的概率与它的权重${\pi }_t^i$成正比。通常情况下，权重是用代表观察模型的似然函数$\mathcal{L}(x_t^i)$计算的：

B. 粒子剥夺和粒子重振

如果提议分布不包括接近状态真值的样本，那么在这个区域取样值的概率就小得可以忽略不计。这种现象被称为粒子剥夺，如图2（左）所示。它的发生可能是由于初始化不良、状态中未建模的运动或重要性采样中一系列不幸的抽样，导致粒子集收敛到局部最优。

图2：对粒子过滤器的传统重采样步骤的修改建议，在凝结算法的工作中得到了直观的说明[31] 。 在传统的粒子滤波器中（左），每个迭代从一组加权粒子开始（上）。采样被替换抽取，然后应用行动模型和扩散来创建预测分布（蓝色）。然后，每个样本通过似然函数（黄色）来接受一个加权。这个后验分布然后成为下一次迭代的先验。在我们提议的修改中（右图），每个迭代开始时都有一组加权粒子（顶部），以及反假设的加权（黑色）。在重新取样阶段，8个粒子中只有5个是通过对先验分布（蓝色）进行取样而产生的，而反假设加权导致3个样本从候选分布中随机取样（粉色）。所有这些样本都通过反假设可能性来分配一个反假设权重（灰色）。这些原始权重被相加，以创建一个新的、单一的反假设权重，代表整个集合的怀疑。所有的样本也都通过传统的似然函数（黄色）。后验分布和Counter-Hypothetical权重就成为下一次迭代的输入。

缓解粒子剥夺的一种常见方法是粒子重振，即从预测信念$\widehat{bel}(x_t)$和候选分布中共同抽取粒子。候选分布的选择可能包括对状态空间感兴趣的区域的均匀分布，或者围绕初始估计的宽高斯分布。这种修改允许重要性抽样从样本集之外抽取，重新引入代表性不足的区域的样本。一个超参数$\alpha$，其中$0\le \alpha \le 1$，控制从中抽取的样本比例。最终的粒子集被定义为从每个粒子集中抽取的粒子的联合：

其中为，为。注意在实践中，$\alpha N$被限制为整数。

IV. 反假设粒子滤波

选择重振率$\alpha$，在实践中具有挑战性。从候选分布中取样太频繁会丢掉信念分布中的关键信息，而从信念分布中取样太频繁则会导致粒子被剥夺。自适应粒子重振通过确定在每个时间步骤中从每个分布中在线抽取的频率来缓解这一挑战。反假设粒子过滤器（CH-PF）自适应地选择重振率$\alpha$，使其根据真实信念分布中被${\mathbb{X}}_t$估计为代表不足的部分而波动。

实现自适应粒子重振的一种方法是传感器复位定位（SRL）[6]。这种方法定义了一个概率阈值，$\beta$，它代表了 "好的 "非正常化似然值的阈值。重振率定义为：

CH-PF以这个方程为基础，使用反假设的可能性来计算重振率，而不是使用概率阈值。

A. 反假设重取样

为了激励我们所提出的确定重振率的方法，我们首先重写方程（6）：

其中。有了这个符号，分子和分母的右侧是对样本集的可能性的综合测量。分母的左手边，，衡量整个样本集的不良表现。这样一来，计算SRL中的粒子再活化率可以被看作是同时测量样本集的积极表现和不良表现。然而，对不良表现的衡量，$f(x_t^i)$，取决于对积极表现的衡量，因为它由$\mathcal{L}(x_t^i)$定义。这种依赖性是由于传统的贝叶斯概率，其中一个状态为真的观察概率和一个状态为假的观察概率总是“零和”的。

我们的方法通过从证据推理（Evidential Reasoning），也被称为Dempster-Shafer理论[8]中得到的灵感，放松了这一假设。这个范式允许对一个事件打折扣的证据，即广义的不信，独立于与支持一个事件相关的证据，即广义的信，进行量化。如图3所示，证据推理对这些概念以及模糊性或无知的存在进行建模[32]。广义信仰是对所有无可否认地支持某一事件的证据的衡量，并且从上到下都以可信度为界限，因为可信度包括信仰中的模糊性。同样，广义的不相信量化了用于反驳一个事件的证据，而不可信是一个上限，它也考虑了模糊性。

图3：证据推理中各数量的关系[8]。这一范式启发了我们的反假设可能性函数，该函数在图1中得到了可视化。

我们认为，这个框架适合于我们基于图像评估物体姿势的应用。遮挡和几何对称的存在表明，在一个给定的姿势如何被证据支持或不支持方面存在模糊性，促使我们独立测量这些数量。我们认为可能性函数类似于证据推理的广义信仰概念，因此引入了一个反假设可能性，以按照广义不信的原则发挥作用。

我们设计了Counter-Hypothetical likelihood来衡量观察到的图像如何提供反对假设的拟议状态的证据。我们的方法是用独立于$\mathcal{L}(x_t^i)$的Counter-Hypothetical可能性取代方程(6)中的$f(x_t^i)$，该可能性是独立估计的。我们引入一个函数来推理与我们给定的假设相反的状态的可信度，即反假设可能性，$\mathcal{C}(x_t)$。通过比较整个提议分布中的（未归一化的）似然量$\mathcal{L}(x_t^i)$和$\mathcal{C}(x_t^i)$，我们可以推理出我们样本集中的累积信心和怀疑。为此，我们通过修改方程（7），重新定义$\alpha$，即重振率，如下：

然后，我们根据我们对这组粒子的怀疑，从中抽出$\alpha N$粒子，并根据我们对这组粒子的信心，从$\widehat{bel}$中抽出剩余的$（1-\alpha ）N$粒子。因此，Counter-Hypothetical likelihood量化了我们的广义不信的概念，它控制了要进行的粒子重振的数量。

B. 6D姿势估计的反假设可能性

在我们的应用中，我们设计了反假设似然函数，以便在姿势跟踪处于失败模式时发出信号。传统的似然函数必须经过精确的制作或训练，以确保最准确的样本拥有最高的权重，与此不同的是，反假设似然函数可以更粗略地或直观地构建。

对于一个简单的例子，考虑如何将捕获的深度数据与一个候选姿势的样本的渲染深度图像进行比较。传统的似然函数可能会根据物体的像素数量来分配权重，这些像素的渲染和捕捉的深度在一个给定的阈值内。这种启发式方法由于存在遮挡物，可能会有噪音，而且难以调整。另一方面，Counter-Hypothetical likelihood可以只测量渲染深度小于捕获深度的像素数。测量的深度明显比渲染的深度近，可以用潜在的遮挡来解释，而反之则表明给定的姿势是错误的。

在这项工作中，我们将深度学习用于反假设似然函数，并使用PoseRBPF[9]的编码器架构。我们还使用了他们的合成训练数据设置。然而，我们不是以自动编码器的方式训练表示，而是利用合成数据中存在的真实姿势信息。对于我们的合成渲染场景和渲染的候选姿态对来说，一半的训练数据是正样本，其中真实的姿态在候选渲染中只被轻微扰动。负面样本是由候选姿势渲染随机产生的。这些作物是编码器网络副本的输入，其输出嵌入一起通过三个全连接层。该网络是用二进制交叉熵损失来训练的。通过这种方式，分类器学会了估计一个假设的姿势何时与给定的观察值不一致。在测试时，这些分数被用作反假设的权重。

V. 实验

为了评估所提出的反假设可能性，我们在YCB视频数据集上测量关键性能指标，这是一个基准的真实世界数据集[9]。我们实现了一个标准的粒子过滤器来估计视频序列中给定物体的6D姿态。我们的结果在RGB和RGB-D数据上都进行了测试。所有的结果都是相同的粒子过滤器的变种，使用相同的似然函数，由PoseRBPF[3]提供，并使用相同数量的粒子（50）。然而，每条基线都有不同的策略来对抗粒子的匮乏，如重振或Rao-Blackwellization。每当需要一个用于初始化或重振的候选分布时，就会使用位于PoseCNN[9]估计的二维边界框内的方向的均匀分布。深度值从均匀分布中取样，但当深度数据存在时，它是从物体位置的测量深度中取样。

A. 基线

我们与专门为6D姿势估计设计的粒子剥夺方法以及采用移动机器人定位中常见的其他技术进行比较。

Annealing[25] 不使用任何粒子重振，而是有一个退火的似然函数，循环地平滑似然加权。

SRL[6] 通过比较平均未归一化似然加权与预先确定的用户阈值，从候选分布中执行自适应粒子重振。这就是PoseRBPF中嵌入的最小余弦相似度。

Aug. MCL [4],[28] 也从候选分布中执行自适应粒子重振，但阈值在每个时间步骤中由用户定义的衰减率决定。

MCL+E2E[30] 有固定数量的样本来自预测分布，其余的样本来自神经网络的估计。我们从以PoseCNN为当前帧提供的全部6D姿势估计值为中心的高斯分布中采样。

PoseRBPF[3] 按照出版物中的描述运行，但禁用了系统中使用的任何地面真实信息。原始实现确保初始化接近于地面真实方向，否则就完全重置。在我们的实验中，只有当估计的似然加权下降到阈值以下时，它才被重置。我们还包括其建议的变体–PoseRBPF++，其中一半的样本在每个时间步骤中从候选分布中重新激活。

CH-PF、Annealing、SRL、Aug. MCL和MCL + E2E在所有六个维度上进行过滤，以更好地测试它们承受粒子剥夺的能力。通过他们的Rao-Blackwellized实现，PoseRBPF和PoseRBPF++只在连续状态空间（翻译）的三个维度上进行过滤，因为方向空间是离散的。

VI. 结果

我们提出了估计姿势和地面真实姿势之间的绝对和对称点状匹配误差（通常分别称为ADD和ADD-S）[9]。与其他YCB作品一样，这些误差是通过查看每种方法的曲线下面积（AUC）得分来分析的，误差最大为10厘米。完整的定量结果显示在图4中。

图4：所有方法在YCB视频数据集上进行物体6D姿势跟踪的曲线下面积得分。图4：所有方法在YCB视频数据集上进行物体6D姿势跟踪的曲线下面积得分，其中包括ADD得分和对称版本（ADD-S）。我们提出的方法，即CH-PF，在似然函数的信息是最嘈杂的时候（RGB数据），优于其他专注于缓解闭塞情况下的粒子剥夺的方法。

我们的分析表明，对于使用相同权重但采用不同方法保持粒子多样性的粒子过滤器的变体，在整个数据集上的性能偏差很小。Rao-Blackwellized实现，PoseRBPF和PoseRBPF++，具有最好的性能。然而，当具体观察给定物体被遮挡的序列时，可以看到精度上的差异。在这些情况下，PoseRBPF的性能有所下降。我们假设这是由于PoseRBPF中的方向过滤机制，它使系统对初始化的质量非常敏感；一个被翻转的初始方向是很难纠正的，即使有重新激活。由于这个原因，在部分可观察到的场景中发生的不良初始化降低了它相对于其他方法的准确性排名。8月，当过滤器失去对姿势的跟踪时，MCL可以很好地工作，但在初始化不良的情况下，它没有记录一个理想的似然阈值，因此在闭塞期间几乎没有进行重新激活。SRL和MCL + E2E在有无遮挡物存在的情况下都表现不俗。我们的方法在整个数据集中的表现与其他方法相似，但在有遮挡物的RGB数据中具有最高的AUC准确性。

图5和图6显示了反假设粒子过滤器方法的部分定性结果。在这些例子中，粒子过滤器在序列开始时收敛到了一个不正确的估计。虽然误差很高，但反假设粒子过滤器能够继续进行全局定位，有很高比例的粒子在进行粗略的搜索。一旦误差下降，并且找到了一个合理的区域，它就会降低重振率，将其资源集中在更紧密的探索空间上。

图5：反假设粒子过滤器的部分定性结果。饼干盒被场景中的其他物体明显遮挡住了。在早期迭代中（左边），饼干盒的信念没有收敛，估计值有很高的误差。从信念中计算出来的重振率很高。在后来的迭代中（中间），信念收敛到了地面真实状态，重振率下降了。一旦信念收敛（右），重振率就很低。此图最好以彩色显示。

图6：反假设粒子过滤器的部分定性结果。糖盒的信念在早期帧中收敛到一个局部最大值（左）。CH-PF应用较高的重振率来缓解这种情况。估计的误差会短暂增加（中间），但信念最终会收敛到正确的估计（右边）。

我们工作的主要限制是在测试时使用额外的计算时间，通过一个额外的似然函数来评估每个样本。对于简单的启发式方法，这不会增加很多时间。在我们的另一个神经网络的情况下，它使推理时间翻倍。我们的性能与大多数数据集使用单一似然函数的性能相似，但对于严重遮挡的场景，性能的改进是有希望的。

VII. 结论

这项工作的目的是通过根据当前信念分布的估计不完全性来调整粒子重振率，从而提高粒子过滤器跟踪刚性物体的6D姿势的准确性。我们建议通过一个新的反假设似然函数独立估计每个样本的潜在误差。这种修改使我们能够推理出我们的粒子集的累积怀疑，并根据需要使用这种估计来应用粒子重振。本文证明了这一修改的有效性，因为与克服粒子剥夺的标准方法相比，它的整体性能相匹配。此外，当只有RGB数据时，我们的粒子过滤器提议的修改提高了在严重遮挡的场景中的性能。

未来的工作将探索更多的案例，对于这些案例，我们提出的将证据推理纳入贝叶斯过滤器可以更明显地提高性能，例如高维跟踪案例或更多的动态物体和场景。它还将研究将这种推理纳入实时推理的更多计算上的有效方法。