CN102782723B - 位置和方位估计方法及其设备 - Google Patents

Abstract

提供了一种即使物体在形状上比具有标准形状的三维几何模型更离散，也精确地估计该物体的位置和方位的方法。估计构成表示物体的三维标准形状的三维模型的特征的偏差的统计量，以确定每个特征的可靠性。计算从通过成像设备获得的观测数据提取的特征与三维模型中的特征之间的偏差量。基于与从三维模型提取的每个特征相关的可靠性和偏差量来估计物体的三维位置和方位。

Description

位置和方位估计方法及其设备

技术领域

本发明涉及一种用于估计其三维形状已知的物体的位置和方位的技术。

背景技术

随着近年来机器人技术的发展，直到现在为止由人类执行的复杂任务（诸如制成品的装配）正改为由机器人执行。这样的机器人用末端执行器（诸如手）握住部件，并且执行装配。为了让机器人握住部件，有必要精确地估计将被握住的部件与机器人（手）之间的相对位置和方位（orientation）。位置和方位的这样的估计多方面地不仅用于让机器人握住部件，而且还用于让机器人估计它自己的位置以自主地移动以及使增强真实性中的真实空间与虚拟物体重合。

一种用于估计位置和方位的方法包括使用通过照相机捕捉的二维图像或者从距离传感器获取的距离图像（rangeimage）的方法。其中，通常使用利用模型拟合的估计，在所述模型拟合中，针对从捕捉图像提取的图像特征或者从距离图像获取的点云数据拟合物体的三维几何模型。例如，存在下述方法，该方法用于估计物体的位置和方位，以使得针对在灰阶图像上检测到的边缘拟合物体的线框模型的投影图像。此外，存在另一种方法，该另一种方法用于通过针对从距离图像获取的点云数据拟合三维几何模型（诸如网格模型（meshmodel））来估计物体的位置和方位。

通常，由于机加工精度和成本的问题，大量生产的工业部件的形状和尺寸离散。对于这样的部件的每一个个体生成三维几何模型是不现实的，使得通常通过使用表示部件的标准形状的一个三维几何模型来估计位置和方位。换句话讲，三维几何模型在形状上不总是与实际部件一致。如果通过使用模型拟合来估计部件的位置和方位，则在模型与实际部件之间的差异大的情况下引起不能进行精确估计的问题。

专利文献1讨论了用于在使用模型识别物体的位置时吸收该物体的形状的离散度（dispersion）的方法。在这种方法中，基于表示物体的标准模型和图像（测量数据）来认识到物体的位置，并且对测量数据进行统计处理以顺序地更新标准模型。

非专利文献1讨论了用于在估计脸的位置和方位时吸收个人与脸的三维模型之间的差异的方法。在这种方法中，每一个个体的特征点的偏差被从事先获取的偏差的分布和实际测量数据获取，并且对具有偏差的特征点进行模型拟合。对于每一个个体生成模型，以使得可与形状的离散度无关地估计位置和方位。

在专利文献1中，物体的形状的离散度被吸收，以提高物体的识别率。这种方法更新标准模型以便吸收实际物体的离散度，使得这种方法适合于识别物体并且粗略估计位置和方位，但是不适合于精确地估计位置和方位。

非专利文献1中所讨论的方法是用于明确地计算偏差的方法，并且由于来自测量数据的特征的错误检测，在所获取的偏差本身中可能包含误差，使得这种方法不适合于精确地估计位置和方位。

引文列表

专利文献

[PTL1]日本专利申请公开No.09-81744

非专利文献

[NPL1]X.Chai,Shan,L.Qing和W.Gao,"PoseestimationbasedonGaussianerrormodels,"InternationalConferenceonBiometrics(ICB2006),LNCS3832,pp.136-143,2005.

[NPL2]Y.Chen和G.Medioni,"Objectmodelingbyregistrationofmultiplerangeimages,"Proc.1991InternationalConferenceonRoboticsandAutomation(ICRA'91),pp.2724-2729,1991.

[NPL3]M.D.Wheeler,Y.Sato和K.Ikeuchi,"Consensussurfacesformodeling3Dobjectsfrommultiplerangeimages,"Proc.6thInternationalConferenceonComputerVision(ICCV'98),pp.917-924,1998.

[NPL4]R.Hartley和A.Zisserman,"MultipleViewGeometryinComputerVision,SecondEdition,"CambridgeUniversityPress,2000.

[NPL5]Y.Liu,T.S.Huang和O.D.Faugeras,"DeterminationofCameraLocationfrom2-Dto3-DLineandPointCorrespondences,"IEEETransactionsonPatternAnalysisandMachineIntelligence,vol.12,no.1,pp.28-37,1990.

发明内容

本发明涉及一种即使物体在形状上比具有标准形状的三维几何模型更离散，也精确地估计该物体的位置和方位的方法。

根据本发明的一方面，一种用于获得物体的位置和方位的位置和方位估计方法包括：统计量计算步骤，其通过统计量计算装置计算构成所保持的物体的三维几何模型的几何特征的偏差的统计量；可靠性计算步骤，其通过可靠性计算装置基于所述偏差的统计量来计算每个几何特征的可靠性；图像特征提取步骤，其通过图像特征提取装置提取由图像捕捉装置捕捉的捕捉图像的图像特征；关联步骤，其通过关联装置将所述几何特征与所述图像特征关联；偏差量计算步骤，其通过偏差量计算装置计算所述几何特征与所述图像特征之间的偏差量；以及位置和方位估计步骤，其通过位置和方位估计装置基于所述可靠性和所述偏差量来估计所述物体的位置和方位。

根据本发明的另一方面，一种用于获得物体的位置和方位的位置和方位估计设备包括：统计量计算装置，其被配置为计算构成所述物体的三维几何模型的几何特征的偏差的统计量；可靠性计算装置，其被配置为基于所述偏差的统计量来计算每个几何特征的可靠性；图像特征提取装置，其被配置为提取由图像捕捉装置捕捉的捕捉图像的图像特征；关联装置，其被配置为将所述几何特征与所述图像特征关联；偏差量计算装置，其被配置为计算所述几何特征与所述图像特征之间的偏差量；以及位置和方位估计装置，其被配置为基于所述可靠性和所述偏差量来估计所述物体的位置和方位。

根据本发明的另一方面，一种程序使计算机执行上述位置和方位估计方法之一的每个步骤。

根据本发明的另一方面，一种存储介质存储用于使计算机执行上述位置和方位估计方法之一的每个步骤的程序。

从以下参照附图对示例性实施例的描述，本发明的进一步的特征将变得清晰。

附图说明

图1是第一示例性实施例中的配置的例子。

图2是第一示例性实施例中的处理的流程图。

图3是示出用于从公差数据计算形状的偏差的处理的例子的流程图(1)。

图4是标准模型和公差数据的例子。

图5是表示偏差候选量的示意图。

图6是所提取的线段和可靠性数据库的例子。

图7是示出用于计算物体的位置和方位的处理的例子的流程图。

图8是示出关联的例子的示意图(1)。

图9是示出用于从测量数据计算偏差的处理的例子的流程图。

图10是示出物体与测量位置之间的关系的例子的示图。

图11是示出用于从公差数据计算形状的偏差的处理的例子的流程图(2)。

图12是示出关联的例子的示意图(2)。

具体实施方式

现在将参照附图来详细描述本发明的实施例。应该指出，除非另有特别说明，否则这些实施例中所阐述的部件的相对布置、数值表达式和数值不限制本发明的范围。

现在以下将参照附图来详细描述本发明的示例性实施例。

在本发明的第一示例性实施例中，通过使用表示物体的标准形状的三维几何模型（以下称为标准模型）与实际物体之间的偏差的不均匀性来精确地估计物体的位置和方位。被认为偏差小的部分的信息被肯定地（positively）用于精确地估计位置和方位。在本示例性实施例中，假定物体为工业部件，基于该物体的公差数据（tolerancedata）来估计构成该物体的标准模型的每个特征的偏差的统计量，并且通过使用所计算的每个特征的可靠性，基于偏差的统计量来估计该物体的位置和方位。

以下使用图1来描述本示例性实施例中的位置和方位估计设备100的配置。在图1中，箭头指示数据流。位置和方位估计设备100包括测量数据获取单元101、特征提取单元102、存储单元103以及位置和方位计算单元104。位置和方位估计设备100与测量设备105连接。

测量设备105测量真实空间。测量设备105通过例如照相机捕捉二维图像。测量设备105测量例如物体的表面的三维位置，并且捕捉距离图像。捕捉的图像被发送到测量数据获取单元101。

测量数据获取单元101从测量装置105获取测量数据。例如，测量数据被获取作为由照相机捕捉的二维图像。所获取的测量数据被发送到特征提取单元102。

特征提取单元102从测量数据提取特征。特征提取单元102使用例如坎尼（Canny）算子来从图像提取边缘作为特征。所提取的特征被发送到位置和方位计算单元104。

存储单元103存储构成物体的标准模型的每个特征的偏差的统计量。物体的标准模型用例如线段集合来描述。所述线段是指表示物体的表面的表面与在该处颜色或密度明显改变的边界线的交线。可替换地，物体的标准模型可用物体上的特征点集合、表示物体的表面的集合或者这些集合的组合来描述。被保持的物体的标准模型被发送到位置和方位计算单元104。

位置和方位计算单元104使用由特征提取单元102从测量数据提取的特征以及存储在存储单元103中的物体的每个特征的标准模型以及与物体的标准模型的偏差的统计量，来估计物体的位置和方位。

以下参照图2来描述本示例性实施例中的位置和方位估计设备100的总处理流程的概要。

步骤S201中的处理是用于计算统计量的处理。在这个步骤中，计算构成物体的标准模型的几何特征的偏差的统计量。如上所述，在本示例性实施例中，物体的标准模型用线段集合表示。这里，在几何特征作为线段的情况下，偏差的统计量基于分割点处的公差来计算，在所述分割点，每个线段被以规则间隔三维分割。分割点用作几何特征的原因是在下述位置和方位估计处理中执行分割点单位处理。

步骤S202中的处理是用于计算可靠性的处理。在这个步骤中，从在步骤S201中获取的每个分割点处的偏差的统计量计算作为几何特征的分割点的可靠性。

步骤S203中的处理是用于计算位置和方位的处理。在这个步骤中，使用在步骤S202中计算的可靠性来计算物体的位置和方位。

步骤S201和S202不总是需要与位置和方位的估计一起执行，并且可作为在步骤S203中执行的位置和方位估计的预处理被单独地离线地执行。

以下详细描述在步骤S201中用于从分割点计算偏差的统计量的处理。计算偏差的统计量的结果存储在存储单元103中。这里，以下参照图3中的流程图来描述用于从物体的公差数据计算偏差的统计量的方法。

步骤S301中的处理是用于提取模型的特征的处理。在这个步骤中，从标准模型提取线段数据。

步骤S302中的处理是用于提取公差的处理。在这个步骤中，提取公差，所述公差可以是所关注的线段（以下称为关注线段）的偏差中的因素（以下称为偏差因素公差）。偏差因素公差是其中包括关注线段的脊线或表面的公差。

图4是标准模型和公差数据的例子。以下对从标准模型400提取的、作为关注线段的线段401进行描述。

尺寸公差（dimensionaltolerance）402至404以及平面公差405至407是偏差因素公差的候选，尺寸公差402至404以及平面公差405至407是其中包括线段401的脊线或表面的公差。在这些候选之中，对位置和方位估计没有贡献的尺寸公差402和平面公差405之外的尺寸公差403和404以及平面公差406和407是偏差因素公差。

步骤S303中的处理是用于计算公差的处理。在这个步骤中，关注线段被按预定间隔三维分割，并且对于每个分割点，获取由在步骤S302中提取的每个偏差因素公差引起的偏差量的最大值（以下称为偏差候选量）。偏差候选量用在标准模型上设置的模型坐标系503的每个分量的幅值表示。

图5是表示偏差候选量502的示意图，偏差候选量502是尺寸公差404对于线段401上的分割点501提供的，其中关注线段为线段401。在这个例子中，分割点501处的偏差候选量502仅用x分量呈现。对所有的分割点进行这个处理。

步骤S303中的处理是用于确定统计量的处理。在这个步骤中，产生下述矢量，并且将该矢量的幅值“d”取作分割点处的偏差的统计量，所述矢量具有在步骤S303中计算的每个分割点处的一组偏差候选量的每个分量的最大值。这导致获取满足所有公差并且在公差中为最大的偏差量。

以下描述例如三个偏差候选量存在于分割点501处的情况。偏差候选量被取作(分量x,分量y,分量z)=(dx1,dy1,dz1)、(dx2,dy2,dz2)、(dx3,dy3,dz3)。分量x、y和z具有关系：dx1>dx2>dx3、dy3>dy2>dy1和dz2>dz3>dz1。在这种情况下，每个分量的最大值由(dx1,dy3,dz2)给出，其幅值“d”用下式表达：

【数学式1】

$d=\sqrt{d_{x1}^2+d_{y3}^2+d_{z2}^2}$

其中，“d”被取作分割点501处的偏差的统计量。

在步骤S305中，如果在步骤S301中提取的所有线段都已经经过步骤S302至S304中的处理，则所述处理完成。如果在步骤S301中提取的所有线段没有都经过步骤S302至S304中的处理，则所述处理进入步骤S306以改变关注线段，并且返回到步骤S302。

以下描述在步骤S202中执行的计算每个分割点的可靠性的方法。

偏差的统计量越小，可靠性被设置得越高。更具体地讲，在机加工精度高的部分处将可靠性设置得较高，在机加工精度低的部分处将可靠性设置得较低。例如，其中偏差的统计量越小、可靠性越高的函数用下式(1)定义：

【数学式2】

$h\left(d\right)=\left\{\begin{array}{cc}\frac{1}{d+1}& d\≤c_1\\ 0& d>c_1\end{array}\right.---\left(1\right)$

在式(1)中，“d”是前述分割点处的偏差的统计量，c1是常数。提供可靠性的函数不限于以上函数，而是可以是其中偏差量越小、可靠性越高的任何其它函数。所计算的可靠性与对于每个线段从标准模型提取的分割点处的三维位置一起作为数据库（图6）存储在存储单元103中。

以下将参照图7的流程图来描述在步骤S203中执行的用于计算物体的位置和方位的处理。

步骤S701中的处理是用于提取图像特征的处理。在这个步骤中，特征提取单元102从捕捉图像提取作为图像特征的边缘。

步骤S702中的处理是用于输入近似值的处理。在这个步骤中，输入标准模型的位置和方位的近似值。

步骤S703中的处理是用于关联的处理。在这个步骤中，将存储在存储单元103中的分割点与在步骤S701中提取的边缘相关联。图8是示出如何执行关联的示意图。这里，使用输入的位置和方位的近似值来将标准模型中的线段801及其分割点802投影在图像上，在针对每个分割点投影的线段的法线方向上的规则范围内搜索边缘，并且将最相邻的边缘取作与分割点对应的点803。

步骤S704中的处理是用于计算用于校正位置和方位的值的处理。在这个步骤中，使用每个分割点802处的可靠性来计算用于在位置和方位计算单元104中计算位置和方位的系数矩阵和误差矢量。每个分割点处的图像上的投影位置被取作(ui,0,vi,0)，并且位于与分割点802对应的点处的图像上的检测点被取作(ui’,vi’)。包括分割点(ui,0,vi,0)的标准模型上的线段801（以下称为模型线段）的投影图像的图像上的梯度804被取作θ。通过对应点803并且其梯度804为θ的直线805的式用式(2)表达：

【数学式3】

u_isinθ_i-v_icosθ_i＝d_i(2)

其中，θ是常数。这里，d_i（常数）用下式表达：

【数学式4】

d_i＝u′_isinθ_i-v′_icosθ_i.

分割点802的图像上的投影位置与物体的位置和方位之间的关系用下式表示：

【数学式5】

$u_{i,0}=u\left(\stackrel{\‾}{s}\right)$

$v_{i,0}=v\left(\stackrel{\‾}{s}\right)---\left(3\right)$

如果物体的位置和方位中的小改变被取作Δs，则式(3)可用下式(4)线性地近似：

【数学式6】

$u_i(\stackrel{\‾}{s}+\mathrm{\Δs})\≈u_{i,0}+\underset{n=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂u_i}{\∂s_n}\mathrm{\Δ}{s}_n$

$v_i(\stackrel{\‾}{s}+\mathrm{\Δs})\≈v_{i,0}+\underset{n=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂v_i}{\∂s_n}\mathrm{\Δ}{s}_n---\left(4\right)$

如果当物体的位置和方位仅改变Δs时，用式(4)表达的分割点处的投影位置位于用式(2)表达的直线上，则可获得用关于Δs的式(5)表达的式：

【数学式7】

$\sin {\mathrm{\θ}}_i\underset{n=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂u_i}{\∂s_n}\mathrm{\Δ}{s}_n-\cos {\mathrm{\θ}}_i\underset{n=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂v_i}{\∂s_n}\mathrm{\Δ}{s}_n=d_i-r_i---\left(5\right)$

其中，r_i是常数：

【数学式8】

r_i＝u_i，0sinθ_i-v_i，0cosθ_i.

将对于标准模型中的线段801的每个分割点802成立的式(5)作为联立式进行求解，以获得物体的位置和方位的校正值Δs。因为基于位置和方位的刚体变换和到图像上的透视投影变换是非线性变换，所以重复校正值Δs的计算和“s”的校正，以获得物体的最终位置和方位。

在本示例性实施例中，将式(5)的两边都乘以作为权重的分割点的可靠性h_i，以计算位置和方位的校正值Δs，其中在机加工精度高的分割点处贡献增大，在机加工精度低的分割点处贡献减小。换句话讲，对式(6)所表达的联立式进行求解，以计算位置和方位的校正值Δs。

【数学式9】

其中，式(6)用下式(7)表达：

【数学式10】

JΔs＝R(7)

使用通过对式(7)进行求解而获得的校正值Δs来校正近似位置和方位，以计算物体的位置和方位。

在步骤S705中，将式(7)的两边都乘以矩阵J的广义逆矩阵(J_T·J)^-1·J^T，以获得位置和方位的校正值Δs。

【数学式11】

Δs＝(J^TJ)^-1J^TR(8)

步骤S706中的处理是用于校正位置和方位的处理。在这个步骤中，用所计算的位置和方位的校正值Δs来校正位置和方位的近似值。

【数学式12】

Δs←s+Δs

在步骤S707中，确定收敛性。如果收敛（在步骤S707中为是），则所述处理完成。如果不收敛（在步骤S707中为否），则所述处理返回到步骤S701。在收敛性确定中，如果校正值Δs几乎为零，或者如果误差矢量R的平方和在校正之前和之后之间几乎不改变，则确定收敛。

如上所述，在第一示例性实施例中，描述了用于获得物体的位置和方位的方法，在该方法中，从公差数据获得构成标准模型的几何特征的偏差的统计量，并且特征中的偏差的统计量越小，将被提供的权重变得越大。肯定地使用可靠性高的特征使得可精确地估计位置和方位。

在第一示例性实施例中，作为基于从公差计算的最大偏差的特定值，计算相对于标准模型的构成物体的几何特征的偏差的统计量。然而，通常，物体的偏差统计地发生，并且下述特征出现，在该特征中，可靠性基于最大偏差来确定以降低超出期望值的可靠性。另一方面，实际公差通常形成以标准值为中心的正态分布。出于这个原因，公差不是相对于标准的差值，并且可用比如正态分布的概率分布来表示。在本发明的第二示例性实施例中，以下描述一种估计位置和方位的方法，在该方法中，构成标准模型的特征的偏差用概率分布表示。

总处理流程类似于在第一示例性实施例中使用图2描述的总处理流程。以下，描述在步骤S201、S202和S203中执行的处理。

以下使用图3来描述在步骤S201中执行的用于计算与标准模型的偏差的处理。除了步骤S304之外的处理在内容上类似于第一实施例中所述的处理，从而仅描述步骤S304中的处理。

在步骤S304中，从在步骤S303中计算的每个分割点处的一组偏离量候选获得每个分量的偏差的分布。每个分割点处的偏差的分布在分量之间是独立的，并且遵循一维正态分布，在该一维正态分布中，对于每个分量，平均值为零，并且标准差为该组偏差候选量的最大值的三分之一。描述三个偏差候选量存在于分割点501处的情况作为例子。假设偏差候选量(分量x,分量y,分量z)被取作(dx1,dy1,dz1)、(dx2,dy2,dz2)、(dx3,dy3,dz3)，并且具有关系dx1>dx2>dx3、dy3>dy2>dy1和dz2>dz3>dz1。在这种情况下，每个分量的最大值为(dx1,dy3,dz2)，并且偏差的标准差为(σx,σy,σz)=(dx1/3,dy3/3,dz2/3)。从而，每个分量的偏差的概率分布用下式表达：

【数学式13】

$\left(\frac{1}{\sqrt{2\mathrm{\π}}{\mathrm{\σ}}_x}\exp (-\frac{x^2}{2{\mathrm{\σ}}_x^2}\right),$ $\frac{1}{\sqrt{2\mathrm{\π}}{\mathrm{\σ}}_y}\exp (-\frac{y^2}{2{\mathrm{\σ}}_y^2}),$ $\frac{1}{\sqrt{2\mathrm{\π}}{\mathrm{\σ}}_z}\exp (-\frac{z^2}{2{\mathrm{\σ}}_z^2}))$

以下描述分别在步骤S202和S203中执行的用于计算可靠性的处理和用于估计位置和方位的处理。首先描述在统计误差被包括在标准模型和测量数据两者中的情况下所使用的最大似然估计方法。然后，与第一示例性实施例的情况一样，详细地描述几何特征被取作线段的分割点的情况。

以下，描述在统计误差被包括在标准模型和测量数据中的情况下所使用的最大似然估计方法。

计算指示测量数据的拟合有多好的似然函数。指示测量数据的特征位置的矢量被取作pi，与矢量pi对应的指示标准模型的特征位置的矢量被取作qi，并且指示物体的位置和方位的六维矢量被取作s。假设测量数据的特征位置pi、标准模型的特征位置qi以及物体的位置和方位s具有式(9)所表达的关系。函数g包括透视投影变换和模型视图变换，在透视投影变换中，三维空间被变换到二维空间中，在模型视图变换中，模型坐标系被变换到照相机坐标系中。

【数学式14】

g(q_i，s)＝p_i(9)

如果标准模型的偏差被取作Δqi并且测量误差被取作Δpi，则测量数据空间中的测量数据与标准模型的特征之间的误差ei用下式(10)线性近似：

【数学式15】

$e_i=p_i+\mathrm{\Δ}{p}_i-g(q_i+\mathrm{\Δ}{q}_i,s)$

$\≈p_i+\mathrm{\Δ}{p}_i-g(q_i,s)-\frac{\∂g}{\∂q_i}\mathrm{\Δ}{q}_i$

$=\mathrm{\Δ}{p}_i-\frac{\∂g}{{\∂q}_i}\mathrm{\Δ}{q}_i---\left(10\right)$

如果Δqi和Δpi是统计误差，则误差ei包括下述统计误差，在该统计误差中，平均值为零，离散度用下式(11)表示：

【数学式16】

${\mathrm{\Σ}}_i={\mathrm{\Σ}}_{p_i}+\left[\frac{\∂g}{\∂q_i}\right]{\mathrm{\Σ}}_{q_i}{\left[\frac{\∂g}{\∂q_i}\right]}^T---\left(11\right)$

如果Δqi和Δpi遵循正态分布，则误差ei中所包括的统计误差也遵循正态分布，使得误差ei的似然函数用下式(12)表达：

【数学式17】

$f\left(e_i\right)=\frac{1}{{\left(2\mathrm{\π}\right)}^{3/2}\sqrt{{\mathrm{\Σ}}_i}}\exp (-\frac{1}{2}{e}_i^T{\mathrm{\Σ}}_i^{-1}{e}_i)---\left(12\right)$

在最大似然估计中，物体的位置和方位被估计，使得下式(13)所表达的似然的乘积被最大化：

【数学式18】

$L=\underset{i=1}{\overset{M}{\mathrm{\Π}}}f\left(e_i\right)={\left(\frac{1}{{\left(2\mathrm{\π}\right)}^{3/2}\sqrt{\left|{\mathrm{\Σ}}_i\right|}}\right)}^M\exp \left(\underset{i=1}{\overset{M}{\mathrm{\Σ}}}(-\frac{1}{2}{e}_i^T{{\mathrm{\Σ}}_i}^{-1}{e}_i)\right)---\left(13\right)$

实际上，式(13)被最小化式(14)的问题取代，在最小化式(14)的问题中，采取似然的乘积的对数，并且其符号被取反。

【数学式19】

$-\log \left(L\right)=-\log {\left(\frac{1}{{\left(2\mathrm{\π}\right)}^{3/2}\sqrt{\left|{\mathrm{\Σ}}_i\right|}}\right)}^M+\frac{1}{2}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}\left({e_i}^T{{\mathrm{\Σ}}_i}^{-1}{e}_i\right)---\left(14\right)$

在式(14)中，其第一项是与位置和方位无关的常数，以使得式(14)最终变为使下式(15)最小化的问题。

【数学式20】

$E=\frac{1}{2}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}\left({e_i}^T{{\mathrm{\Σ}}_i}^{-1}{e}_i\right)---\left(15\right)$

式(15)表示误差矢量的范数（norm）的平方和，其中，离散度∑i的逆矩阵被作为可靠性加权。

以下描述几何特征被取作线段的分割点的情况。

式(9)的函数g是将分割点投影到图像表面上并且用下式(16)表达的函数：

【数学式21】

g(q_i，s)＝g_u(q_i，s)sinθ_i-g_v(q_i，s)cosθ_i(16)

其中

【数学式22】

$g_u(q_i,s)=f\frac{x_c}{z_c}$

$g_v(q_i,s)=f\frac{y_c}{z_c}---\left(17\right)$

并且

【数学式】

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{c}{r}_{11}{x}_w+r_{12}{y}_w+r_{13}{z}_w+t_x\\ r_{21}{x}_w+r_{22}{y}_w+r_{23}{z}_w+t_y\\ r_{31}{x}_w+r_{32}{y}_w+r_{33}{z}_w+t_z\\ 1\end{array}\right]---\left(18\right)$

在式(17)和(18)中，f表示照相机的焦距，xc、yc和zc指示分割点在照相机坐标系中的三维位置，xw、yw和zw表示分割点在世界坐标系（以物体为中心的坐标系）中的位置，矩阵R和t分别是指从世界坐标系到照相机坐标系的变换矩阵的旋转分量和平行平移分量。

误差ei用分割点的投影位置与通过对应点的直线之间的距离（标量）来表示。如果误差不包括在通过对应点的直线的梯度θi中，则误差ei用下式(19)来表达：

【数学式24】

e_i＝g(q_i，s)-(u′_isinθ_i-v′_icosθ_i)

＝g_u(q_i，s)sinθ_i-g_v(q_i，s)cosθ_i-(u′_isinθ_i-v′_icosθ_i)(19)

误差ei是标量，使得误差ei的协方差矩阵∑i（式(11)）也是标量。在步骤S202中，可靠性被计算为∑i的逆数。在步骤S203中，执行在第一示例性实施例中使用图7描述的流程，以获得物体的位置和方位。在步骤S704中计算的系数扩展矩阵和误差矢量通过用通过式(18)计算的∑i的逆数取代式(6)中的权重h来获得。

以下详细描述在几何特征为线段的情况下的协方差矩阵∑i的计算方法。

式(11)中的协方差矩阵∑i的右手侧的第一项∑pi是从图像检测的线段的检测误差的协方差矩阵。假设线段的检测误差仅在搜索线的方向上具有不定性，并且遵循一维高斯分布，在该一维高斯分布中，平均值为0，标准差为σ。难以估计实际图像中的σ。出于这个原因，线段的检测误差被认为主要由图像的量化误差造成，并且σ例如被设置为一个像素。换句话讲，∑pi变为1。式(11)中的协方差矩阵∑i的右手侧的第二项为下述项，在该项中，在步骤S201中获得的标准模型的协方差矩阵∑qi的两边都被乘以雅可比行列式如果标准模型的偏差的协方差矩阵∑qi被认为在分量之间不具有相关性并且在步骤S201中获得的偏差的标准差被取作(σx,σy,σz)，则可获得下式(20)。

【数学式25】

${\mathrm{\Σ}}_{q_i}=\left[\begin{array}{ccc}{\mathrm{\σ}}_x^2& 0& 0\\ 0& {\mathrm{\σ}}_y^2& 0\\ 0& 0& {\mathrm{\σ}}_z^2\end{array}\right]---\left(20\right)$

雅可比行列式可使用式(16)、(17)和(18)被写为式(21)：

【数学式26】

式(11)中的协方差矩阵∑i的右手侧的第二项可使用式(20)和(21)来获得。

在第二示例性实施例中，描述了用于在从标准模型提取的特征的偏差用概率分布表示的情况下基于从概率分布计算的可靠性来获得位置和方位的方法。

在第一示例性实施例和第二示例性实施例中，使用在步骤S201中执行的用于获得特征与标准模型的偏差的计算的方法，但是可使用其它方法。例如，实际上可对大量相同类型的物体进行测量，以获得形状的离散度。在第三示例性实施例中，通过距离成像设备执行多个物体的三维形状测量，以基于这些物体的三维测量的结果的离散度来获得与标准模型的偏差。从多个视点对每个样本进行三维测量，以测量整个物体的形状，并且整合结果以产生用线段描述的一个标准模型。从如此产生的多个标准模型计算与标准模型的偏差。

用于测量物体以获得标准模型的物体测量步骤可在步骤S201之前执行。物体的标准模型被认为是事先制备的，但是可基于从三维测量结果获得的多个标准模型来产生它。

使用图9中的流程图来描述用于通过实际测量物体来获得与标准模型的偏差的方法。

在步骤S901中，通过距离成像设备在测量位置处获取物体的测量数据。这里，物体的测量数据是指点云数据，并且每个点用距离成像设备的坐标系中的三维位置来表示。

如果所有预定测量位置处的测量没有完成（在步骤S902中为否），则所述处理进入步骤S903。

在步骤S903中，移动测量位置。图10是示出测量位置的示意图。测量装置1002在物体1001作为原点的球面上以合适的增量移动，以移动测量位置。在移动测量位置之后，所述处理返回到步骤S901，以继续所述处理。如果所有预定测量位置处的测量完成（在步骤S902中为是），则所述处理进入步骤S904。从而，从多个方向测量物体多次，以改进测量精度。

在步骤S904中，将在多个视点处获取的点云数据整合为一个点云数据（参照非专利文献2）。整合的点云数据被转换为网格模型（参照非专利文献3）。三维线段被从所产生的网格模型提取。网格的形成超过某一值的角度的交线被提取为三维线段。

如果所有预定物体的测量没有完成（在步骤S905中为否），则所述处理进入步骤S906。

在步骤S906中，改变将被测量的物体。以上物体是指相同类型的物体，并且具有获得关于每一个个体的偏差的信息的目的，并且改变该物体。在改变物体之后，所述处理返回到步骤S901，以继续所述处理。如果所有预定物体的测量完成（在步骤S905中为是），则所述处理进入步骤S907。

在步骤S907中，计算三维线段与从对每个物体进行测量的结果获得的标准模型的偏差。相同的三维线段被认为是从单个的网格模型提取的。与第一示例性实施例和第二示例性实施例的情况一样，对于当三维线段被按规律间隔分割时所获得的每个分割点计算与标准模型的偏差。

如上所述，在第三示例性实施例中，描述了用于获得实际物体的形状相对于标准模型的离散度的方法。这使得即使关于离散度的信息事先没有作为公差数据被获得，也可通过实际测量估计离散度并使用结果来精确地估计位置和方位。

在第一示例性实施例、第二示例性实施例和第三示例性实施例中，描述了用于将物体的形状描述为线段集合的方法，但是除了以上之外，物体的形状还可用另一种特征来描述。物体的形状可被描述为例如点集合。与在第一示例性实施例和第二示例性实施例中所述的方法的情况一样，对于表示物体的每个点计算相对于标准模型的离散度，以在估计物体的位置和方位时使用该离散度作为权重。这里，假设如第一示例性实施例的情况那样计算标量权重hi。如果物体的近似位置和方位被提供，则以下观测式对于测量数据成立：

【数学式27】

$\underset{n=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂u}{\∂s_n}\mathrm{\Δ}{s}_n=u_i^{\′}-u_i$

$\underset{n=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂v}{\∂s_n}\mathrm{\Δ}{s}_n=v_i^{\′}-v_i---\left(22\right)$

在式(22)中，u’i和v’i是检测位置，ui和vi是通过近似位置和方位计算的图像上的点的位置。

其中对于所有点成立的式(22)的两边都被乘以权重hi的式被取作关于位置和方位的校正值Δsi的联立式，以获得式(23)：

【数学式28】

重复如下处理，其中对式(23)进行求解以计算位置和方位的校正值Δsi，以校正近似位置和方位，从而计算物体的最终位置和方位。

在第一示例性实施例、第二示例性实施例和第三示例性实施例的描述中，测量数据被取作通过照相机捕捉的二维图像，但是测量数据可被取作从距离成像设备获取的三维点云数据。以下描述其中测量数据被取作三维点云数据的处理。

参照图10和图11来描述在步骤S201中执行的用于使用公差数据来计算偏差的方法。这里，物体的标准模型被认为是被描述为表面集合，并且对每个表面设置偏差的统计量。

在步骤S1101中，提取关注表面的偏差因素公差。偏差因素公差是指包括关注表面的脊线和表面的公差。如果从标准模型400提取的表面1201（图12）被取作关注表面，则下述余数，换句话讲，尺寸公差404和平整度公差406变为偏差因素公差，在所述余数中，从尺寸公差402至404以及平整度公差405至407减去尺寸公差402和403以及平整度公差405和407，尺寸公差402和403以及平整度公差405和407提供对位置和方位估计没有贡献的表面的法线的正交方向上的误差，尺寸公差402至404以及平整度公差405至407是包括表面1201的脊线和表面的公差。

在步骤S1102中，获得在步骤S1102中提取的每个偏差因素公差对于关注表面提供的最大偏差（偏差候选量）。偏差量被取作表面的重心相对于标准模型的偏差。

在步骤S1103中，提取当前关注的表面的重心相对于标准模型的偏差候选之中的最大偏差候选量作为该表面的偏差的统计量。

如果确定对所有表面的处理完成（在步骤S1104中为是），则所述过程结束。如果确定对所有表面的处理没有完成（在步骤S1104中为否），则在步骤S1105中改变关注表面，并且所述处理返回到步骤S1102。

以下描述在步骤S202中执行的用于计算可靠性的方法。对于上述关于每个表面的偏差的统计量基于式(1)来获得可靠性。

最后，以下描述在步骤S203中执行的用于计算位置和方位的方法。计算方法基本上类似于第一示例性实施例中所述的计算方法，从而以下仅补充描述不同部分。

在步骤S701中，从由距离成像设备捕捉的距离图像提取三维点云数据。

以下描述在步骤S703中执行的关联。如图12所示，搜索与从测量点云的每个点起的法线方向对应的表面。假设标准模型被基于近似位置和方位转换到距离成像设备的坐标系中，与搜索线相交的表面被取作对应表面。

以下描述在步骤S704中执行的用于计算系数矩阵和误差矢量的方法。从物体坐标系中的标准模型提取的表面的式子被取作ax+by+cz=k。如果物体的位置和方位校正值被取作Δs并且点云数据的每个点通过校正被放置在对应的标准模型的表面上，则获得下式(24)。

【数学式29】

$a\underset{n=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂x}{\∂s_n}\mathrm{\Δ}{s}_n+b\underset{n=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂y}{\∂s_n}\mathrm{\Δ}{s}_n+c\underset{n=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂z}{\∂s_n}\mathrm{\Δ}{s}_n=k-l---\left(24\right)$

其中，l=ax0+by0+cz0（常数）。

式(24)的两边都被乘以包括控制点的表面的可靠性hi，并且对所有分割点进行一系列处理，从而提供以下联立式(25)：

【数学式30】

在步骤S705中，对式(25)进行求解，以计算位置和方位的校正值Δsi，并且使用校正值Δsi来校正近似位置和方位。

在修改例子中，描述其中物体的标准模型被描述为表面集合的方法，但是标准模型可被描述为其它特征。例如，标准模型可被描述为点云数据。

在上述示例性实施例和修改例子中，假设物体的近似位置和方位被提供并且被重复性地重复以计算最终的位置和方位。然而，用于计算位置和方位的方法不限于以上方法，如果近似位置和方位未知，则可直接计算位置和方位。例如，如果测量数据是二维图像上的特征点，则通常直接通过直接线性变换（DLT）来计算位置和方位（参照非专利文献4）。DLT方法是用于通过对线性联立式进行求解来计算表示物体的位置的三个变量和作为表示方位的旋转矩阵的每个元素的九个变量的方法，并且具有12个未知变量。对于每个点有两个式成立，使得需要至少六条测量数据。如第一示例性实施例中所述的，在对联立式进行求解时，将每个式乘以基于与标准模型的偏差的权重，以使得可优先使用关于机加工精度高的部分的信息。

用于直接计算位置和方位的方法不限于以上方法，而是可基于三维点的对应关系来计算上述12个变量。可基于线段以及点的对应关系来计算特征（参照非专利文献5）。

在上述示例性实施例和修改例子中，与标准模型的偏差量被取作在该处线段被按等距的间隔分割的点之间的距离或者表面的重心之间的距离，但是偏差量可被取作其它值，而不是以上距离。如果线段用作特征，则线段之间的距离可被取作偏差量。线段之间的距离被取作下述距离，该距离为线段的端点之间的距离以及一个线段的端点和从该端点延伸的法线与另一个线段的交点之间的距离中的最短距离。如果表面被作为特征处理，则平整度可被取作偏差量。平整度被取作表面畸变的最大值（峰）与最小值（底部）之间的差值。用于计算偏差量的流程类似于图3或图9中所述的流程，并且可通过用线段以及线段之间的距离或表面和平整度取代所述特征和偏差量来实现。

应指出，本发明可应用于包括单个器件的装置或者由多个器件构成的***。

此外，本发明可通过下述方式来实现，即，将实现前述实施例的功能的软件程序直接或间接供给***或设备，该***或设备的计算机读取所供给的程序代码，然后执行该程序代码。在这种情况下，只要所述***或装置具有所述程序的功能，实现模式就无需依赖于程序。

因此，因为本发明的功能用计算机实现，所以安装在该计算机中的程序代码也实现本发明。换句话讲，本发明的权利要求还覆盖出于实现本发明的功能的目的的计算机程序。

在这种情况下，只要所述***或设备具有所述程序的功能，所述程序就可被以任何形式（诸如目标代码、用解释器执行的程序、或者供给操作***的脚本数据）被执行。

可用于供给所述程序的存储介质的例子是软盘、硬盘、光盘、磁光盘、CD-ROM、CD-R、CD-RW、磁带、非易失性式存储卡、ROM和DVD（DVD-ROM和DVD-R）。

关于供给所述程序的方法，客户端计算机可使用客户端计算机的浏览器与互联网上的网站连接，并且本发明的计算机程序或所述程序的可自动安装的压缩文件可被下载到诸如硬盘的记录介质。此外，本发明的程序可通过下述方式被供给，即，将构成所述程序的程序代码分割为多个文件，并且从不同网站下载这些文件。换句话讲，将通过计算机实现本发明的功能的程序文件下载到多个用户的WWW（万维网）服务器也被本发明的权利要求覆盖。

还可以对本发明的程序进行加密，并且将该程序存储在诸如CD-ROM的存储介质上，将存储介质分发给用户，使得满足某些要求的用户可通过互联网从网站下载解密密钥信息，并且使得这些用户可通过使用该密钥信息来对加密的程序进行解密，由此将所述程序安装在用户计算机中。

除了根据实施例的前述功能通过计算机执行读取的程序来实现的情况之外，在计算机上运行的操作***等也可执行实际处理的全部或部分，以使得前述实施例的功能可通过这个处理来实现。

此外，在从存储介质读取的程序被写入到***到计算机中的功能扩展板或者与计算机连接的功能扩展单元中所提供的存储器之后，安装在功能扩展板上的CPU等或者功能扩展单元执行实际处理的全部或部分，以使得前述实施例的功能可通过这个处理来实现。

虽然已参照示例性实施例描述了本发明，但是要理解本发明不限于所公开的示例性实施例。权利要求的范围应被给以最宽泛的解释，以涵盖所有修改、等同结构和功能。

本申请要求2010年2月25日提交的日本专利申请No.2010-040595的优先权，该日本专利申请的全部内容特此通过引用并入本文。

Claims (24)

1.一种位置和方位估计方法，包括：

对于构成物体的三维几何模型的几何特征中的每一个获得该几何特征的偏差的统计量；

基于所述偏差的统计量来设定每个几何特征的可靠性，其中所述统计量越小，所述可靠性被设定为越高；

提取捕捉图像的图像特征；

将所述几何特征与所述图像特征关联；

计算所述几何特征与所述图像特征之间的偏差量；和

基于所述可靠性和所述偏差量来估计所述物体的位置和方位，

其中，所述位置和方位估计方法还包括输入所述物体的位置和方位的近似值，

其中，所述位置和方位估计步骤包括：

基于所述可靠性、所述偏差量和所述近似值来计算所述物体的位置和方位的校正值；和

基于所述校正值来校正所述物体的位置和方位的近似值。

2.根据权利要求1所述的位置和方位估计方法，其中，基于所述三维几何模型的公差数据来获得所述统计量。

3.根据权利要求2所述的位置和方位估计方法，其中，获得统计量包括：

提取构成所述三维几何模型的线段；

提取引起所述线段的偏差的公差；

计算所述线段的各预定间隔的公差；和

产生最大公差中的公差的矢量，并且将该矢量取作所述统计量。

4.根据权利要求2所述的位置和方位估计方法，其中，所述统计量用概率分布表示。

5.根据权利要求4所述的位置和方位估计方法，其中，获得统计量包括：

提取构成所述三维几何模型的线段；

提取引起所述线段的偏差的公差；

计算所述线段的各预定间隔的公差；和

基于所述公差产生正态分布的矢量并且将该矢量取作所述统计量。

6.根据权利要求2所述的位置和方位估计方法，其中，获得统计量包括：

提取构成所述三维几何模型的表面；

计算所述表面的重心的公差；和

产生最大公差中的公差的矢量并且将该矢量取作所述统计量。

7.根据权利要求1所述的位置和方位估计方法，还包括测量构成所述物体的形状的几何特征，

其中，计算统计量包括基于测量结果与所述三维几何模型之间的偏差来测量所述统计量。

8.根据权利要求7所述的位置和方位估计方法，其中，测量几何特征包括对每多个物体执行多次测量。

9.根据权利要求7所述的位置和方位估计方法，其中，测量几何特征包括对每多个测量位置执行多次测量。

10.根据权利要求1所述的位置和方位估计方法，其中，所述几何特征是点、线段或表面。

11.根据权利要求1所述的位置和方位估计方法，其中，使用似然函数来计算所述可靠性。

12.一种位置和方位估计设备，包括：

统计量获得单元，其被配置为对于构成物体的三维几何模型的几何特征中的每一个获得该几何特征的偏差的统计量；

可靠性设定单元，其被配置为基于所述偏差的统计量来计算每个几何特征的可靠性，其中所述统计量越小，所述可靠性被设定为越高；

图像特征提取单元，其被配置为提取由图像捕捉单元捕捉的捕捉图像的图像特征；

关联单元，其被配置为将所述几何特征与所述图像特征关联；

偏差量计算单元，其被配置为计算所述几何特征与所述图像特征之间的偏差量；和

位置和方位估计单元，其被配置为基于所述可靠性和所述偏差量来估计所述物体的位置和方位，

其中，所述位置和方位估计设备还包括被配置为输入所述物体的位置和方位的近似值的单元，

其中，所述位置和方位估计单元包括：

被配置为基于所述可靠性、所述偏差量和所述近似值来计算所述物体的位置和方位的校正值的单元；和

被配置为基于所述校正值来校正所述物体的位置和方位的近似值的单元。

13.根据权利要求12所述的位置和方位估计设备，其中，基于所述三维几何模型的公差数据来计算所述统计量。

14.根据权利要求13所述的位置和方位估计设备，其中，统计量获得单元包括：

被配置为提取构成所述三维几何模型的线段的单元；

被配置为提取引起所述线段的偏差的公差的单元；

被配置为计算所述线段的各预定间隔的公差的单元；和

被配置为产生最大公差中的公差的矢量，并且将该矢量取作所述统计量的单元。

15.根据权利要求13所述的位置和方位估计设备，其中，所述统计量用概率分布表示。

16.根据权利要求15所述的位置和方位估计设备，其中，统计量获得单元包括：

被配置为提取构成所述三维几何模型的线段的单元；

被配置为提取引起所述线段的偏差的公差的单元；

被配置为计算所述线段的各预定间隔的公差的单元；和

被配置为基于所述公差产生正态分布的矢量并且将该矢量取作所述统计量的单元。

17.根据权利要求13所述的位置和方位估计设备，其中，统计量获得单元包括：

被配置为提取构成所述三维几何模型的表面的单元；

被配置为计算所述表面的重心的公差的单元；和

被配置为产生最大公差中的公差的矢量并且将该矢量取作所述统计量的单元。

18.根据权利要求12所述的位置和方位估计设备，还包括被配置为测量构成所述物体的形状的几何特征的单元，

其中，统计量获得单元包括被配置为基于测量结果与所述三维几何模型之间的偏差来测量所述统计量的单元。

19.根据权利要求18所述的位置和方位估计设备，其中，被配置为测量几何特征的单元包括被配置为对每多个物体执行多次测量的单元。

20.根据权利要求18所述的位置和方位估计设备，其中，被配置为测量几何特征的单元包括被配置为对每多个测量位置执行多次测量的单元。

21.根据权利要求12所述的位置和方位估计设备，其中，所述几何特征是点、线段或表面。

22.根据权利要求12所述的位置和方位估计设备，其中，使用似然函数来计算所述可靠性。

23.根据权利要求12所述的位置和方位估计设备，还包括被配置为捕捉所述物体的图像的所述图像捕捉单元。

24.根据权利要求23所述的位置和方位估计设备，其中，所述图像捕捉单元捕捉真实空间中的距离图像作为所述捕捉图像。