论文笔记（四十三）Multi-view object pose distribution tracking for pre-grasp planning on mobile robots

文章概括

作者：Lakshadeep Naik, Thorbjørn Mosekjær Iversen, Aljaz Kramberger, Jakob Wilm and Norbert Kr¨uger
来源：2022 IEEE International Conference on Robotics and Automation (ICRA)
原文：https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9812339
代码、数据和视频：https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9812339
系列文章目录：
上一篇：
https://blog.csdn.net/xzs1210652636/article/details/134431873
下一篇：

摘要

当移动机械手机器人仍在接近物体时，跟踪物体 6D 姿态分布的能力可使机器人预先计划结合底座和手臂运动的抓取。然而，由于机器人摄像头的视角有限，从远处跟踪 6D 物体姿态分布可能具有挑战性。在这项工作中，我们提出了一个框架，将外部固定摄像机的观测数据与移动机器人摄像机的观测数据相融合，并按时间顺序进行跟踪，从而实现远距离 6D 物体姿态分布跟踪。我们将物体姿态后验建模为多模态分布，这样就能更好地应对摄像机与物体之间的较大距离、遮挡物和物体几何形状带来的不确定性。我们使用 YCB 数据集中的物体在模拟多视角数据集上对所提出的框架进行了评估。结果表明，即使机器人摄像头对物体的可见度较低，我们的框架也能实现精确跟踪。

I. 介绍

受仓库中移动物流机器人成功的启发，移动机械手机器人也在解决不同的日常任务方面取得了长足进步（例如，协助食堂工作人员清洁食堂餐桌[1]）。这类机器人的成功在很大程度上取决于两个因素：时间效率，即机器人完成任务的速度和抓取失败的次数。例如，抓取失败会带来更多问题，如杯子里残留的咖啡溢出，因此如果抓取失败的风险很高，最好不要尝试抓取。

现有研究以顺序方式解决移动操纵问题 [2]、[3]、[4]。机器人首先通过基本运动导航到物体附近（导航），然后观察场景、计划并执行抓取（操纵）。将导航和操纵结合起来可以使机器人在接近物体时预先计划抓取，从而提高任务的时间效率。此外，通过估计完整的 6D 物体姿态分布（底层不确定性），机器人可以确定姿态估计的不确定性是否可以通过其抓手对准能力进行补偿[5]，并相应地规划新的动作，以减少姿态不确定性或执行抓取，如图 1 所示。在这项工作中，我们的重点是视觉问题（图 1 左侧），即跟踪物体的完整 6D 姿势分布，这对于机器人在接近过程中的抓取规划至关重要。

图 1. 右图： 抓手对准能力（可离线优化，并随着时间的推移保持不变），左图： 机器人想要抓取的物体姿态估计值的不确定性（可通过获得更好的物体视图来减少不确定性），中图： 通过耦合基座和手臂运动进行抓取规划（基于姿态估计值的不确定性可由抓手进行补偿，机器人规划行动以获得更好的观察结果或执行抓取）。

大多数关于 6D 物体姿态估计的同类研究都侧重于估计每个物体的单一最佳姿态[6]、[7]、[8]。这些方法提供的姿态估计质量取决于多种因素，如遮挡物、物体对称性、自遮挡物、光照条件等。机器人可以从完整的 6D 物体姿态分布估算中获益匪浅，因为在执行抓取动作之前，机器人有可能采取新的动作以获得更好的观察结果。因此，近期的工作重点是估计姿势分布 [9]、[10]、[11]、[12]。此外，在接近物体时，机器人还可以通过跟踪物体姿态来融合时间信息。Deng 等人[13] 提出了一种能及时跟踪完整物体姿态分布的框架。虽然这种方法可用于在接近物体的最后阶段（当机器人接近物体时）跟踪完整的物体姿态分布，但如果能更早地跟踪物体姿态分布，就能使机器人结合其基座和手臂运动，有效地规划抓取动作。

此外，当今的室内环境通常都装有摄像头（见图 2），用于占用跟踪、监控、智能办公解决方案等。我们利用这种由固定外部摄像头和移动机器人摄像头组成的多视角摄像头设置来实现 6D 物体姿态分布跟踪，从而对移动机器人进行抓取规划。

图 2. 由固定的外部摄像头和移动的机器人摄像头组成的多摄像头装置。

II. 相关工作

在本节中，我们将讨论物体姿态估计、物体姿态分布估计、多视角物体姿态估计等方面的现有工作，然后总结我们的贡献。

A. 6D 物体姿态估计

6D 物体姿态估计问题是指估计被观测物体的三维位置（x、y、z）和三维方向（如滚动、俯仰、偏航）。传统的 6D 物体姿态估计方法主要分为基于模板的方法 [18]、[19]、[20] 和基于特征的方法 [21]、[22]、[23]。

过去十年中，随着深度学习技术的进步[24]，一些研究工作重点关注使用基于数据驱动学习的方法进行姿势估计。最初的工作[25]侧重于从图像直接回归到 6D 姿态。然而，对于对称物体来说，由于从不同的视角看该物体具有相同的视图，因此可能有多个方向是真实的。因此，后来的研究尝试使用考虑到物体对称性的新损失函数 [6] 或将方向作为分类问题来解决 [7]、[11] 来解决这一问题。为了提高对遮挡的鲁棒性，最近的研究集中于预测物体关键点 [26]、[27]、[28]，然后解决姿态估计的 "透视-n-点 "问题。还有一些研究预测了物体每个像素的位置，并将其用于物体姿态估计 [8]。

在抓取等机器人任务中，对物体 6D 姿态估计的精度要求非常高。这通常需要使用迭代方法来实现，如迭代最近点（ICP）[29]。最近的研究为细化步骤开发了深度迭代方法 [30]、[31]、[32]、[33]，从而获得了非常精确的物体 6D 姿态估计值。

B. 物体姿态分布

在机器人学中，姿势分布比单一姿势估计更受欢迎，因为姿势分布也可用于估计机器人任务的成功概率[5]。除了照明条件和摄像机校准误差，6D 姿态估计的不确定性还取决于物体的几何形状和其他物体的遮挡[34]。姿势不确定性传统上使用单模态分布（如高斯分布）建模，但姿势分布通常是多模态的 [34]。因此，在模拟物体姿态估计的不确定性时，多模态分布（如高斯混合分布或直方图）更受青睐。有几项研究也倾向于使用反正交分布（如宾汉混合分布[35]）来模拟方位的不确定性[36]、[37]、[38]、[39]、[40]，而其他研究[12]、[41]、[11]、[10]则尝试使用不同的模型来模拟位置和方位的不确定性。位置不确定性用简单的高斯模型建模，而方位不确定性则用宾汉姆混合模型[42]、[9]或多模态直方图分布[11]、[10]建模。

C. 多视角物体姿态估计

多视角姿态估计是指通过考虑物体的多个视角来估计物体的 6D 姿态 [43]、[44]、[45]、[14]、[15]、[16]。 其中一些多视角物体姿态估计方法侧重于依次纳入更多的物体视角（也称为 6D 物体姿态跟踪）。这些方法大多是基于物体的 SLAM 方法 [46]、[47] 或主动视觉方法 [48]、[49]、[50]，可捕捉物体的多个视图并随时间推移改进姿势估计。最近的一些研究[10]和[51]也侧重于在操纵环境中对 6D 物体姿态进行跟踪。其他研究则侧重于同时使用来自不同摄像机视点的多个物体视图。其中一些作品联合估计了来自多个物体视角的三维姿态 [52], [53], [45]，其他作品 [15], [44], [43], [14], [16] 则首先生成每个视角的姿态估计，然后使用不同的技术将多个姿态估计融合为一个姿态估计。

然而，在多视角全姿态分布估算方面的前期工作并不多。Erkent 等人[54]提出了一种方法，通过假设每个视图的不确定性为高斯，来整合来自不同视图的 6DoF 姿态估计值。如上所述，单模态分布不足以模拟对称或遮挡的物体。此外，他们还假定摄像机是静止的，并且没有纳入任何运动模型来连续跟踪估计的姿势分布，而这对于操纵任务来说是至关重要的。据我们所知，目前还没有任何一项研究利用多模态方位分布，探索多个同步视图和连续时间整合来估算物体姿态和底层姿态分布。

III. 问题的提出

如图 3 所示，我们提出了一种多视角全 6D 姿态分布跟踪框架，它能同时融合来自多个摄像机的信息，然后随着时间的推移依次进行跟踪。我们的工作灵感来自 Deng 等人提出的单摄像头 6D 姿势跟踪框架 [13]。如图 2 所示，我们将其扩展到由固定和移动摄像机组成的多摄像机设置。粒子根据不同摄像机视角的物体检测结果进行初始化，利用多个视角的观测结果进行验证，然后按时间顺序进行跟踪。姿态分布在机器人帧（移动摄像头）中直接估算，以实现抓取规划。下一节，我们将详细介绍数学框架。

图 3. Deng 等人提出的 Rao-blackwelized 粒子滤波器[13]扩展用于多视角 6D 姿态分布跟踪

IV. 建议的方法

A. 后验分布建模

如公式 1 所示，我们利用 Rao-Blackwellization 假设将 6D 状态变量 ($X_t$) 分成旋转 ($R_t$) 和平移 ($T_t$) 两部分，以便在 6D 空间进行高效采样。

$Z_{1:t}$ 指的是观测值 1 到 t 的序列。旋转分量以平移分量为条件建模。

假定跟踪物体的平移分量在每个摄像机帧中都遵循高斯分布。将来自不同相机帧的估计值组合起来，就会产生高斯后验分布的混合物：

为了有效捕捉物体的对称性并处理遮挡问题，跟踪物体的旋转分量被建模为一个三维直方图分布，由物体离散化的方位空间组成。离散化采用分辨率为 5 度的欧拉角方位表示法，从而产生 37x72x72（仰角、方位角、倾斜角）的分区[13]。

在跟踪初始化过程中，平移分量的粒子根据不同相机帧的物体检测结果进行采样，而方向分布则初始化为均匀直方图分布。

B. 运动传播

在每个时间步长 t 处，使用运动模型将时间 t-1 的粒子传播到时间 t。由于粒子来自不同的帧，因此需要不同的运动模型进行传播。此外，来自外部摄像机帧的粒子还需要转换为机器人帧，也就是我们的参考帧。如果 "cr "是机器人相机帧，"ce "是外部相机帧，那么来自相机帧和机器人帧的粒子的运动传播模型分别如式 4 和式 3 所示，其中 T 指的是变换。

物体运动（${}_{o_{t-1}}^{o_t}T$）被模拟为布朗运动，固定摄像机运动（${}_{c{e}_{t-1}}^{c{e}_t}T$）被假定为恒定运动，机器人摄像机运动（${}_{c{e}_t}^{c{r}_t}T$）被模拟为odometry 运动模型[56]。

C. 观测似然和多视角融合

在这一步中，我们要计算不同帧的观测结果有多少与传播的粒子一致（公式 5）。在所有公式中，$r$ 表示机器人相机帧，而 $e_1,e_2,...e_n$ 表示外部相机帧。

我们使用 Deng 等人[13]提出的方法计算每个粒子的方位分布，其中他们使用去噪自动编码器[57]为离散化的物体方位空间计算了一个编码本。然后按照公式 6 所述将这些方位分布融合在一起。

图 4 显示了一个示例，在每个摄像机帧中，根据 Yale-CMU-Berkeley (YCB) 基准[58]对芥末瓶对象的第一方位分布进行估算。然后将这些分布转换到机器人帧，并按照公式 6 所述进行融合。通过融合来自三个不同来源的证据，可以消除由于物体对称性造成的不确定性，并收敛到单模态方向假设。

图 4. 不同帧中估算的方向分布融合图

然后，根据公式 7 中所述的融合方向分布的边际概率，确定各个平移粒子的权重。

D. 重新采样和姿势估计

然后对加权粒子进行重新采样，以增加权重较高的粒子数量。根据后验分布建模，对来自不同帧的粒子的平均值进行加权平均，从而估算出 t 时刻姿势估计值的平移分量。每个帧的权重是由特定帧中重新取样的粒子的权重之和与所有粒子的权重之和归一化计算得出的。在估计方向分量时，我们采用了 [13] 中使用的方法，即从时间 t-1 开始对旋转邻域内的旋转进行加权平均。

V. 实验评估

A. 数据集

我们在 YCB 机器人操纵基准[58]中的物体子集上对所提出的算法进行了评估。由于现有的 YCB 数据集并不具备所述多视角设置的基本事实，因此我们使用 BlenderProc [17] 照片逼真渲染器创建了一个模拟数据集。我们生成的数据集由多个序列组成，其中机器人正朝着物体移动，模拟了底座和手臂的运动[59]。数据集由 16 个这样的序列和 8 个不同的 YCB 物体组成。每个序列由随机放置的 YCB 基准中的一个选定对象子集组成。外部摄像头放置在距离物体 3 米左右的位置。机器人摄像机从距离物体 3 米处开始向物体移动，直至距离物体 0.5 米处，在此过程中记录 100 个帧。此外，由于渲染器没有为深度图像使用任何噪声模型，我们假定高斯噪声的均值为零，每个像素深度测量值的标准偏差为 5%。本文附带的代码和数据集在这里提供（https://lakshadeep.github.io/research/
2021-09-15-multi-view-object-pose-tracking/）.

B. 评估方法

现有的 6D 姿态估计工作大多使用 ADD/ADD-S 指标进行定量评估。然而，这些指标并不能捕捉到 6D 姿势估计中单个平移和方向组件的误差。直观地说，当摄像机靠近物体时，应该会产生良好的平移估计值，而多摄像机设置应该会产生良好的方位估计值。因此，为了了解当使用多视角设置从远处跟踪物体时，平移和方位估计会受到怎样的影响，我们同时报告了平移和方位误差（所有三个轴的总和）。我们的实验仅限于具有唯一 6D 姿态估计值的物体。其中许多物体都有单/多对称轴。

此外，我们还评估了估计姿态的潜在不确定性。我们报告了平移粒子的标准偏差，以了解姿势分布中平移分量的扩散情况。我们还报告了地面真实方位的对数似然值，类似于 [9]，以评估跟踪方位分布中分配给地面真实方位的概率。

我们使用上述指标将我们提出的多视角姿势分布跟踪框架与单视角基线 [13] 进行了比较。

VI. 结果

由于仅用上述指标难以证明跟踪性能，我们首先介绍两个定性例子，然后再介绍定量结果。

A. 例 1：简单情景

如图 5（第一行）所示，在本示例中，机器人通过靠近物体来跟踪 YCB 基准中的 "饼干盒 "物体。图 5 第 2 行和第 3 行的曲线图描述了跟踪过程中上述指标随时间的变化情况。从图 5 a) 和 b) 中我们可以看到，在多视图情况下，即使机器人摄像头距离物体非常远，平移和方向误差也会立即收敛。而仅使用单摄像头时（图 5 e) 和 f)，只有当机器人足够接近物体时才会出现收敛。值得注意的是，单视角情况下的最终收敛效果要好得多，因为当机器人靠近物体时，机器人摄像头的观测能力更强，而多视角情况下也会使用更多来自外部摄像头的不确定观测数据。此外，图 5 c), d) 和 g), h) 显示了方向和平移不确定性的变化。平移粒子的标准偏差与平移误差成正比，因此可用于确定现有平移估计的置信度。 图 5 d) 和 h) 显示，在使用外部和机器人摄像机融合时，地面真实方位的可能性要高得多，因此方位分布更加确定。

图 5. 第一行 不同时间帧的机器人视图、外部摄像头视图、跟踪物体（饼干盒）；第二行和第三行：分别为多视图和单视图的平移误差、旋转误差、平移标准偏差和地面真实方位的对数似然值

B. 例 2：复杂情况

如图 6 第一行所示，在本示例中，机器人一边朝物体靠近，一边转动摄像头观察物体，并跟踪 YCB 基准中的 "芥末瓶 "物体。起初，由于遮挡的原因，机器人摄像机帧中看不到该物体，直到 t=20 时左右才可以看到。然而，由于该物体在其他帧中可见，多视角框架已经可以以较高的误差估算出其姿态。当物体在 t = 20 左右开始出现在机器人框架中时，平移误差会增加（图 6 a），因为此时我们的框架开始融合来自机器人摄像机的信息，而由于物体仍有部分遮挡，因此会产生嘈杂的深度信息。这不会影响旋转估算，因为旋转估算是通过对上一时间步附近的旋转进行加权平均计算得出的。不过，当物体在机器人帧中开始完全可见时（t=40 左右），旋转开始出现偏差，因为在开始收敛之前，旋转方向会与之前的方向接近。 此外，从图 6 c)和 d)中我们可以看到，平移和方向的不确定性都很高，这可以解释为最初的物体是被遮挡的，即使当它变得可见时，机器人也无法看到物体的任何明显侧面。不过，如图 6 d) 所示，随着机器人收集的证据越来越多，它对地面真实方位的信心也越来越大。在单一视图的情况下（图 6 第三行），我们看不到任何清晰的模式，因为最初由于遮挡，机器人无法跟踪物体，即使在物体可见时也无法恢复，因为物体与其估计的分布相差甚远。

图 6. 第一行 不同时间帧的机器人视图、外部摄像头视图、跟踪物体（芥末瓶） ；第二行和第三行：分别为多视图和单视图的平移误差、旋转误差、平移标准偏差和地面真实方位的对数似然值

C. 定量结果

在表 I 中，我们列出了针对不同 YCB 对象所生成序列的不同指标的平均值，这进一步验证了定性示例的结果。在多视角实验中，我们使用了 2 个外部相机帧。在每个外部相机帧中，围绕每个物体的地面真实平移初始化了 30 个粒子，并从机器人相机帧中初始化了 50 个粒子，因此总共生成了 110 个粒子。在单视角实验中，所有 110 个粒子都是围绕机器人相机帧的地面实况平移进行初始化的。

我们可以看到，与单视角方法相比，多视角方法在估计物体姿态方面的表现要好得多。与平移估算相比，融合多台摄像机的信息可以获得更好的方向估算结果。不过，可以注意到，高平移误差的背后也有高平移标准偏差的支持，这可以用来确定不确定性。此外，与单视图相比，多视图融合会产生更高的地面真实方位似然，这可以从多视图框架中的高对数似然值推断出来。此外，正如预期的那样，地面真实方位的对数似然值越高，方位估计就越准确。

我们还可以看到，上述指标的大小因物体而异。大而明显的物体，如 “饼干盒”，除非有遮挡物，否则往往从很远的地方也能看到，因此多视角和单视角的平移和方位误差没有明显差别；而小而自遮挡的物体，如 “杯子”，或部分对称的物体，如 "芥末瓶 "或 “香蕉”，则有明显差别。

VII. 结论和今后的工作

在这项工作中，我们提出了一个用于移动机器人多视角 6D 物体姿态分布跟踪的框架。我们的研究结果表明，即使在物体远离机器人或物体被遮挡的情况下，使用外部摄像头也能改进姿势估计。因此，本研究成果被认为是移动机器人早期抓取预规划的辅助工具。此外，由于对整个姿势分布进行了跟踪，它还提供了成功抓取所需的潜在不确定性的详细信息。我们相信，通过有效地处理相互冲突的证据和自适应地从不同的观测源对粒子进行采样，该算法还能得到进一步改进。我们还计划将该框架扩展为部分可观测马尔可夫决策框架（PO-MDP），以确定能减少不确定性并提供更好姿势估计的行动。

致谢

这项工作得到了丹麦创新基金（Innovation Fund Denmark）在 FacilityCobot 项目和大众汽车基金会（VolkswagenStiftung）在 ReThiCare 项目中的资助。