CN104040590A - 用于估计物体的姿态的方法 - Google Patents

Abstract

通过首先将一组对特征定义为几何图元的组来估计物体的姿态，其中，几何图元包括取向表面点、取向边界点和边界线段。对于物体的模型，基于一组对特征来确定模型对特征。根据由3D传感器获取的数据，基于一组对特征来确定场景对特征，并且然后，将模型对特征与场景对特征匹配以估计物体的姿态。

Description

用于估计物体的姿态的方法

技术领域

本发明一般地涉及估计3D物体的姿态，并且更具体地涉及从由3D传感器获取的数据估计姿态。

背景技术

物体的姿态具有6个自由度(6-DoF)，即3D平移和3D旋转。姿态估计的问题是指找出物体相对于参考坐标系(通常是传感器的坐标系)的姿态。能够利用3D模型使用来自传感器的测量结果(例如，2D图像、3D点云)获取姿态。姿态估计在很多机器人应用中都扮演了重要的角色，这些机器人应用例如为箱式拾取、抓握、位置测定、自主导航和3D重建。

近来，姿态估计主要使用2D图像来进行，这是因为相机是经济有效的并且允许快速图像获取。利用2D图像的主要问题在于将2D图像与模型中的它们对应的3D特征进行匹配。这由于相机的各种照明条件和不同的视点引起了图像空间中的旋转和尺度方面的变化而变得具有挑战性。此外，物体的一些视图能够理论上导致模糊的姿态。已知若干不变的特征描述符来确定输入图像与图像的数据库之间的对应性，其中，2D关键点与3D坐标匹配。

很多工业部件是无纹理的，例如，机械加工后的金属部件或者模塑成型的塑料部件。因此，需要严重地依赖于图像中的可观察的边缘。当使用物体的边界时，物体的一组边缘模板常常已知为先验，并且在查询边缘图(query edge maps)中搜索模板。已知包括边缘取向或层级表示的若干变化。基于强度的边缘检测常常产生了过多的边缘像素，其中，只有一些是来自深度不连续性的有用的边缘。能够使用多闪光灯相机来通过从多个闪光方向投射阴影来直接估计深度边缘。

一般来说，与2D数据相反地，利用3D传感器获得的3D数据具有少的多的变化。主要的挑战在于在存在传感器噪声、遮挡和杂乱时解决对应性问题。对应性问题是指找出数据中的特征与模型中的特征之间的一一匹配。这些特征通常被构造为以物体的大小和形状为特征。已知若干3D特征描述符，其使用表面点和法向的分布以及匹配过程。这些描述符通常对于刚性体变换来说是不变的，但是对于噪声和遮挡敏感。此外，这些特征要求密集的点云，这可能是不可获得的。

利用传感器3D数据与模型之间的各种对应性能够进行姿态估计：3D对应性、2线对应性和6点至3或更多平面。通常，这些对应性在诸如随机样本一致性(RANdomSample Consensus)(RANSAC)的假设测试框架中使用以确定姿态。替选地，能够使用霍夫投票方案或者参数空间中的聚类来从假设的姿态分布的模式获得姿态。这些方法当在没有图像或其它事先的信息的情况下仅能够获得3D传感器数据时受困于下述两个问题。点、线和平面不是能够单独地区分并且是组合的以匹配，并且难以在没有对模型进行任何事先处理的情况下实现快速的计算。

能够通过物体的表面上的两个取向点之间的距离和相对取向来限定对特征。物体由存储在哈希表中用于快速取回的一组取向点对特征来表示。从传感器数据中采样任意两个点并且每个这样的对对于特定姿态进行投票。所要求的姿态对应于具有最大数目的票的姿态。由像素与偏移像素之间的深度差构成的简单的对特征用于利用随机森林集合分类器的人类姿态估计。

发明内容

本发明的实施方式提供了一种基于投票的方法，其用于根据由3D传感器获取的数据估计物体的姿态。虽然在现有技术中使用的取向表面点(具有法向的物体的表面上的点)对于具有显著的曲率变化的物体来说是可区分的，但是这些表面点对于很多工业和真实世界的物体(其是平面的)来说紧凑和可分辨程度不够。

为了从边界获得信息，边缘在2D记录中扮演了重要的角色，但是深度不连续性在3D中是重要的。实施方式提供了姿态估计方法，其更好地研究了该边界信息。除了取向表面点之外，我们使用两个其它几何图元，包括取向边界点(位于具有方向的物体的边界上的点)和边界线分段。

仔细选择的图元对更多信息进行压缩编码并且从而为更广泛的工业部件提供了更大的准确性并且允许更快的计算。

附图说明

图1是能够使用本发明的实施方式的机器人组件设备的示意图；

图2A是用于使用根据本发明的实施方式的基于投票的方法确定3D物体的姿态的方法的框图；

图2B是使用基于投票的方法确定3D物体的姿态的方法的细节的框图；

图3A-图3D是根据本发明的实施方式的用于基于投票的姿态估计的对特征的示意图；以及

图4是根据本发明的实施方式的从传感器数据获得的线对特征与从模型获得的线对特征之间的变换的示意图。

具体实施方式

图1示出了估计物体的姿态的系统100。系统包括具有抓握器120的6轴机械臂110。3D传感器130被布置在臂上。抓握器根据它们的姿态101从箱150拾取物体140。应注意的是，箱能够包含不同的物体。

一个示例3D传感器使用由红外激光器生成的结构化光。其它传感器也是可能的。传感器获取3D“点云”160作为640×480个像素的深度图。3D传感器被相对于机械臂校准，从而允许使用该姿态抓握和拾取物体。

3D点云由在处理器170中执行的方法处理。处理器能够包括本领域中已知的存储器和输入/输出接口。该方法确定姿态，该姿态能够反馈到控制器180以指导臂拾取物体140。

图2A示出了方法200的框图，其包括下述步骤：场景的3D感测210；基于投票的姿态估计220、姿态改善230和根据其姿态的物体的抓握240。

系统使用3D传感器扫描物体的箱以检测物体并且根据由该方法确定的姿态拾取该物体。给定箱中的每个不同物体的3D CAD模型，基于投票的方法使用扫描的3D点云160执行物体的检测和姿态估计。该步骤提供的多个粗略姿态。系统选择若干最佳粗略姿态并且使用迭代最近点(ICP)过程来对姿态进行分别地改善。

一般来说，ICP使得两个3D点云之间的差异最小。ICP常常用于从在不同位置获取的多个空间扫描重构完整的3D模型并且相对于3D环境模型定位机器人。该过程能够通过反复修正最小化对应的点之间的距离所需的变换(平移和旋转)来实时地执行。该输入是两个3D点云、变换的初始估计和终止标准。该输出是改善后的变换。主要步骤包括通过最近相邻标准找到对应的点；使用均方误差作为成本函数来估计变换参数；使用所估计的参数来对点进行变换；以及进行迭代。

我们的系统中的ICP过程使用当前的姿态估计对物体的CAD模型进行渲染并且通过对渲染后的模型的表面进行采样来生成用于模型的3D点云。然后，ICP对于模型中的每个3D点确定扫描的点云中的最近3D点并且使用3D点对应性更新姿态估计。

在ICP收敛之后，测量记录误差作为扫描的点云和模型中的对应的3D点之间的平均距离。记录误差在ICP收敛到不正确的姿态时或者在物体的一部分由于其它物体的遮挡而丢失时会变高。因此，如果记录误差较高，则我们的系统不使用该估计的姿态用于抓握。除了记录误差之外，我们的系统还检查处于该姿态的物体是否能够由机器人臂达到以确定是否能够拾取该物体。

坐标系

为了该描述的目的，使用三个坐标系。在传感器(相机)坐标系C的观点中表达传感器。在物体坐标系O的观点中表达物体的模型。场景中的物体使用世界坐标系W。传感器坐标系S能够被校准到世界坐标系W，这是因为一般来说传感器在真实世界中处于已知的位置和取向。

姿态估计方法确定传感器坐标系与物体坐标系之间的变换。然后，能够使用传感器和世界坐标系之间的关系来处理场景中的物体。

基于投票的姿态估计

我们的用于姿态估计的特征是基于取向点或线的对，我们可以将其称为对特征。由一对取向表面点构成的对特征由S2S表示，这是因为我们从一个取向点(基准)到另一个(所指的)点非对称性地构造特征向量。类似地，我们具有基于一对取向边界点(B2B)、一对线段(L2L)和包括取向表面点和取向边界点(S2B和B2S)的异构对的四个其它对特征，参见图3A-图3C。

点对特征描述符F_S2S、F_B2B、F_S2B和F_B2S由一对取向点(m，n)的相对位置f₁和取向f₂、f₃和f₄限定，其中，点指示具有表面法向的表面点或者具有方向的边界点。在图3D中，线对特征描述符F_L2L由两个无穷线之间的最小距离f₁、两个线方向之间的锐角f₂和两个线段之间的最大距离f₃定义。

为了估计物体的姿态，需要建立从场景和模型获得的对特征之间的对应性。我们使用来自对特征的各种几何约束作为其描述符。然后，通过匹配它们的描述符来建立场景与模型对特征之间的对应性。

对特征

S2S-表面对特征

使用物体表面上的两个点及其法向来限定表面对特征S2S。给定来自场景的取向点和来自模型的对应初元，能够通过对齐点位置和它们的法向来将3D姿态恢复到平面旋转。为了解决旋转模糊性并且恢复完整的6-DoF姿态，至少需要场景与模型图元的两个点之间的对应性。

{(m_r,n_r),(m_i,n_i)}表示对特征，其中，m_r和m_i是物体表面上的参考和提及点，并且n_r和n_i分别是对应的法向。与该对特征关联的描述符为：

F_S2S＝(f₁，f₂，f₃，f₄)^T (1)

＝(||d||₂,∠(n_r，d),∠(n_i,d),∠(n_r，n_i))^T (2)

其中，d是从参考点到提及点的向量，并且∠(v₁,v₂)∈[0,π)表示两个向量之间的角度。

因此，描述符的第一分量表示f₁＝||m_i-m_r||₂＝||d||₂，其是两个表面点之间的欧拉距离。第二和第三分量f₂和f₃分别是向量d与表面法向量n_r和n_i之间的角度。最后的分量f₄是两个法向量之间的角度。在图3A中示出了S2S特征。如果物体横跨较宽范围的发现，则该特征提供了对于物体的良好描述。

B2B-边界对特征

S2S特征没有提供对于不横跨宽范围的表面发现的形状的良好描述。不幸的是，在机器人组装期间使用的很多工业部件都是平面的并且具有非常小的组的法向方向。另外，由于3D数据中的噪声，难以在表面上的高曲率区域中准确地估计法向，这使得问题进一步复杂化。

我们基于物体边界上的两个点定义了新颖的点对特征B2B。与表面点相反地，边界点不具有很好定义的法向。对于边界点来说，我们对线段进行拟合并且使用线段的方向用于取向。为了提取物体边界上的线段，我们首先使用Canny边缘检测器来确定深度图中的边缘。边缘被存储在边缘图中。来自边缘图的点被随机地采样并且使用RANSAC过程在以这些点为重心的局域区域上对3D线进行拟合。通过反复地定位和移除具有最大内围层(inliers)的线段，我们恢复了所有线段。通过向内围层应用最小二乘法来进一步改善这些线段。在线拟合之后，我们对3D线段上的边界点进行均匀的采样。

在图3B中，点示出了两条3D线段上的边界点。然后，利用下式定义边界对特征描述符

$\begin{array}{c}{F}_{B2B}={(f_1,f_2,f_3,f_4)}^T---\left(3\right)\\ ={({\left|\right|d\left|\right|}_2,\∠({\stackrel{\‾}{n}}_r,d),\∠({\stackrel{\‾}{n}}_i,d),\∠({\stackrel{\‾}{n}}_r,{\stackrel{\‾}{n}}_i))}^T.---\left(4\right)\end{array}$

除了和是3D线的方向之外，该特征等价于F_S2S。注意的是，这些方向不是唯一地确定的。因此，当我们使用B2B特征时，我们考虑两个可能的方向和

物体边界的信息量非常大。与S2S、B2B相比，由于存在比表面点更少的边界点，因此提供了更精确的建模。此外，从局部线段开始的取向也比表面法向对于噪声更具抵抗力。

S2B和B2S-表面-边界对特征

边界对特征与物体的深度边缘关联。仅依赖于边界点的这样的特征可能对于具有高表面曲率的物体来说不是最佳选择。例如，球形物体的表面上的任意点能够基于姿态而潜在地成为深度边缘，而多边形对向上的深度边缘更稳定，并且始终出现在平面交叉上。

为了一起高效地对平面和弯曲物体进行建模，我们提供了使用取向表面点和取向边界点定义的异构对特征。通过考虑表面和边界上的取向点，与B2B特征相比，该特征减少了姿态采样。

如图2C中所示，通过选择取向表面点作为参考点并且选择取向边界点作为提及点，利用下式定义S2B特征描述符

$\begin{array}{c}{F}_{S2B}={(f_1,f_2,f_3,f_4)}^T---\left(5\right)\\ ={({\left|\right|d\left|\right|}_2,\∠(n_r,d),\∠({\stackrel{\‾}{n}}_i,d),\∠(n_r,{\stackrel{\‾}{n}}_i))}^T.---\left(6\right)\end{array}$

类似地，通过选择取向边界点作为参考点并且选择取向表面点作为提及点来定义B2S特征。

L2L-线对特征：

如图3D中所示，我们还提供了用于3D线段的对特征L2L。构造用于两条线段的特征比构造用于曲线点对的特征包含得略多。在我们的方法中，我们使用RANSAC过程对线进行拟合。线段通常由于噪声、随机线拟合和遮挡而分为若干较小的线段。因此，不能够可靠地确定线段的端点(表示为{l_r ¹,l_r ²,l_i ¹,l_i ²})。

因此，我们考虑由c_r和c_i表示的两个点以确定我们的特征描述符。如图2D中所示，c_r和c_i是每条线上彼此最近的点。具有方向v1和v2的两条线短之间的锐角为：

其中， ${\∠}_a(v_1,v_2)\∈[0;\frac{\mathrm{\π}}{2}]..$

我们的L2L特征描述符为：

$\begin{array}{c}{F}_{L2L}={(f_1,f_2,f_3)}^T---\left(8\right)\\ ={({\left|\right|d\left|\right|}_2,{\∠}_a(l_r^2-l_r^1,l_i^2-l_i^1),f_3)}^T.---\left(9\right)\end{array}$

其中

$f_3=\max ({\left|\right|l_i^1-l_r^1\left|\right|}_2,{\left|\right|l_i^1-l_r^2\left|\right|}_2,{\left|\right|l_i^2-l_r^1\left|\right|}_2,{\left|\right|l_i^2-l_r^2\left|\right|}_2)..$

第一分量是最近点c_r和c_i之间的距离(即，两条无限长线之间的距离)，并且第二分量是两条线段之间的锐角。最后的分量表示两条线段之间的最大距离。该最大距离能够是一条线段中的端点与另一条线段中的端点之间的所有可能的距离中的最大的一个。该最大距离用于减少具有类似距离和角度的两条线段之间的错误匹配。例如，任意对共面正交线具有相同的距离和角度。然而，如上所述，该最大距离由于线段的中断而是不可靠的。因此，我们在姿态估计中对于该分量使用较大量化步骤。我们没有对于平行线段的对构造L2L特征，这是因为这样的对导致姿态的模糊。

线对特征提供了非常高效的匹配，这是因为线段的数目少于边界点或表面点的数目。该特征对于多面物体和具有长边界线段的物体来说是非常有效的。

在实际实施中，通过扫描并且二次采样深度图像来获得表面点，经由我们的从3D扫描估计3D线段的RANSAC过程确定3D线，并且通过沿着线段二次采样来获得边界点。

物体表现

我们首先获得几何图元M：用于S2S的取向表面点、用于B2B的取向边界点、用于S2B和B2S的取向表面和边界点以及用于L2L的3D线段。这些几何图元能够根据在传感器与物体之间进行了已知的校准的3D扫描数据或者从已知的CAD模型渲染得到的合成深度数据来确定。

利用这些图元M，我们使用一组模型对特征201S2S、B2B、S2B、B2S和L2L来表示每个特定物体140(如图2A中所示)。为了高效的特征匹配，我们将模型对特征存储在哈希表H205中。在一开始，我们对特征对进行离散化，其中距离和角度被分别以Δ_d和Δ_a的步长进行量化。重要的是，适当地定义量化水平。使用大的步长减少了描述符的区分能力，而使用小的步长使得方法对于噪声敏感。接下来，使用离散的对特征描述符作为用于哈希函数的关键字，并且相应地将对特征插入到箱中。利用适当的量化步骤，类似的对特征被分组在一起并且能够以恒定时间进行匹配和投票。

构造物体表现能够是能够离线执行的预处理步骤。图2A示出了在箱中能够随机地混合的用于以后的拾取的三个不同物体。

在在线处理中，我们根据扫描的3D点云160确定场景对特征202，并且将这些特征与所述一组模型对特征匹配。在图2B中示出了该方法的细节。

用于S2S、B2B、S2B和B2S的投票方案

如图2B中所示，在从模型确定了特征202的对并且构建了哈希表H205之后，我们通过计算场景对特征与一组对应的模型对特征之间的刚性体变换来确定姿态。为了高效地进行找到最佳姿态的处理，我们使用投票方案。单纯的方法要求6-DoF姿态空间中的投票，这是低效率的。

相反地，我们使用投票方案，其使用中间坐标变换来将投票空间减少到2D空间。首先，在哈希表H中搜索场景点对(s_r,s_i)，并且然后找到对应的模型点对(m_r,m_i)。然后，对的参考点被变换到中间坐标系，从而它们的位置对应于坐标系的原点，并且它们的法向与x轴对齐。为了完全对齐对，通过以角度α围绕x轴旋转物体模型来对齐提及点m_i和s_i。

上述从(m_r,m_i)到(s_r,s_i)的变换由参考模型点的对和平面旋转角度(m_r,α)来参数化，并且由下式给出

$s_i=T_{s\→g}^{-1}{R}_x\left(\mathrm{\α}\right)T_{m\→g}{m}_i---\left(10\right)$

其中，R_x(α)是以角度α围绕x轴的旋转，T_s→g和T_m→g分别是从场景坐标系和模型坐标系到中间坐标系的变换。

在投票阶段，场景中的参考点s_r与场景中的其它点s_i配对，并且然后，基于它们的描述符从哈希表H获得与场景对特征(s_r,s_i)类似的模型对特征(m_r,m_i)。对于每个对，在中间坐标系中确定旋转角度α，并且然后，对于对(m_rα)进行投票。在投票之后，选择大于预定阈值的元素，从其如等式(10)中那样计算候选姿态(模型坐标与场景坐标之间的刚性变换)206。通过选择场景中不同的参考点来重复该处理。该投票方案可应用于S2S、B2B、S2B和B2S对特征。

用于L2L的投票方案

由于通过一对线段来定义L2L特征，因此我们使用了不同的投票方案用于L2L方案。与用于点对特征的投票方案相比，用于L2L特征的投票方案更加困难。然而，基本理念是相同的：我们在中间坐标系中对齐两个对特征，如图4中所示。

我们首先将线对变换到中间坐标系。通过T_s→g，来自场景的参考线l^s _r与x轴对齐，并且参考中点o^m被移动到原点。来自模型的参考线l^m _r类似地通过T_m→g进行变换。之后，参考场景线c^s _r和参考模型线c^m _r的最近点通过沿着x轴平移参考线而在原点对齐。这些平移之间的差异被表示为τ。然后，提及线l^s _i和l^m _i通过围绕x轴以角度α旋转来对齐。角度α能够由d^s和d^m容易地确定。

因此，用于L2L投票方案的经由中间坐标系从(l^m _r,l^m _i)到(l^s _r,l^s _i)的变换由(l^m _r,α,τ)参数化并且由下式给出

$l_i^s=T_{s\→g}^{-1}{T}_x\left(\mathrm{\τ}\right)R_x\left(\mathrm{\α}\right)T_{m\→g}{l}_i^m---\left(11\right)$

其中，T_x(τ)是沿着x轴的平移。

姿态聚类

在上述投票方案中，对于场景中的每个参考图元(点或线)获得候选姿态206。由于通过多个特征对对物体进行建模，因此预计在不同参考图元(点m_r或线l^m _r)上具有支持一致的姿态假设的多个候选姿态。因此，我们聚集来自不同场景图元的类似姿态。

虽然存在用于在3D刚性体变换空间SE(3)中进行聚类的若干方案(例如，李群上的均值漂移)，但是这些方案通常在计算上对于对时间敏感的应用来说是禁止的。

我们使用聚集聚类207方案，其是非常高效的。我们首先按照投票的数目的降序对候选姿态进行排序。从具有最大数目的投票的候选姿态开始，我们创建新的聚类。如果下一候选姿态接近现有聚类之一，则该候选姿态被添加到该聚类，并且聚类中心被更新为聚类内的候选姿态的平均。如果下一候选姿态没有接近任何聚类，则生成新的聚类。对于平移和旋转以固定阈值进行接近测试。在3D欧几里德空间中执行平移的距离计算和平均，而使用旋转矩阵的四元数表示来执行用于旋转的距离计算和平均。

在聚类之后，按照投票总数的降序来对聚类进行排序，这确定估计的姿态的可信度。具有最大数目的投票的聚类对应于最佳姿态208。

本发明的效果。

提供了一组使用取向表面点、取向边界点和边界线段的对特征来对各种物体进行建模。我们在投票框架中使用对特征用于准确且高效的姿态估计。

来自边界的特征是更紧凑且信息量大，从而导致了更好的准确性和更快的计算。

根据本发明的实施方式实施的箱式拾取系统具有高于98％的拾取成功率，以及用于平移和旋转的小于0.3mm和0.3°的姿态估计误差。

Claims (15)

1.一种估计物体的姿态的方法，该方法包括下述步骤：

将一组对特征确定为几何图元的对，其中，几何图元包括取向表面点、取向边界点和边界线段；

对于所述物体的模型，基于所述一组对特征确定模型对特征；

根据由3D传感器获取的数据，基于所述一组对特征确定场景对特征；以及

将所述模型对特征与所述场景对特征匹配以估计所述物体的所述姿态，

其中，在处理器中执行上述的步骤。

2.根据权利要求1所述的方法，其中，所述物体是与其它物体随机混合的多个不同物体中的一个物体，并且对于每个物体存在模型对特征。

3.根据权利要求1所述的方法，其中，所述3D传感器安装在机器人臂上，并且所述机器人臂包括根据姿态拾取所述物体的抓握器。

4.根据权利要求1所述的方法，其中，所述3D传感器使用由红外激光器生成的结构化光。

5.根据权利要求1所述的方法，其中，使用两个取向表面点、两个取向边界点、一个取向表面点和一个取向边界点以及两个边界线段来定义每个对特征。

6.根据权利要求1所述的方法，其中，每个取向表面点包括位于所述物体的表面上的3D位置和3D法向量。

7.根据权利要求1所述的方法，其中，每个取向边界点包括位于所述物体的边界上的3D位置和3D方向向量。

8.根据权利要求7所述的方法，其中，所述3D方向向量被定义为在所述3D位置处与所述边界相切的方向。

9.根据权利要求1所述的方法，其中，每个边界线段是拟合到所述物体的边界上的3D点的线段。

10.根据权利要求1所述的方法，其中，根据每个对特征确定描述符。

11.根据权利要求10所述的方法，其中，通过比较所述模型对特征的描述符与所述场景对特征的描述符来执行所述匹配。

12.根据权利要求11所述的方法，所述方法进一步包括：

对所述对特征的描述符进行离散化；

将所述模型对特征的离散化的描述符存储在哈希表中；以及

使用所述哈希表执行所述匹配。

13.根据权利要求1所述的方法，其中，经由中间坐标变换，基于投票来估计所述物体的候选姿态。

14.根据权利要求13所述的方法，所述方法进一步包括：

对所述候选姿态进行聚类。

15.根据权利要求1所述的方法，所述方法进一步包括：

使用迭代最近点过程来改善所述姿态。