CN103512548B - 距离测量装置及距离测量方法 - Google Patents

Abstract

本发明提供距离测量装置及距离测量方法。指定由感测用图案光投影的目标对象的图像的摄像单元感测的图像中的、用图案光投影的区域。基于所指定的区域，从所感测的图像中的、除用所述图案光投影的区域以外的区域中，搜索所述目标对象的几何特征。基于所搜索到的所述目标对象的几何特征以及所述目标对象的模型的几何特征，来推导所述目标对象的位置及姿态。

Description

距离测量装置及距离测量方法

技术领域

本发明涉及距离测量装置及距离测量方法，尤其涉及用于测量给定了三维形状的对象的位置及姿态的技术。

背景技术

近年来，随着机器人技术的发展，机器人逐渐替代人类来处理复杂任务。复杂任务的典型示例包括组装工业产品。为了控制机器人自主地执行组装任务，机器人必须通过诸如手等的末端执行器来抓取部件。为了通过机器人手来抓取部件，必须测量要抓取的部件与机器人之间的相对位置及姿态，并且必须基于测量结果来制定移动方案，以便对致动器进行实际控制。

在机器人以外的领域中，需要测量相对于环境和对象的位置及姿态。例如，在MR（混合现实，MixedReality）中，为了在真实图像上合成由计算机生成的虚拟对象，必须测量位置及姿态（orientation）。使用机器人上安装的照相机或者距离（range）传感器来执行机器人的位置/姿态测量，并且作为代表性的方法，使用利用二维图像或距离图像的方法。在MR领域中，已研究了如下的技术，即利用由安装在HMD（头戴式显示器，HeadMountedDisplay）上的照相机感测的图像，来执行HMD的位置/姿态测量。

专利文献1（日本特愿2010-137680号公报）公开了精确且快速地测量高速移动的对象的位置及姿态的方法。在该方法中，通过利用普通照相机来感测用图案光投影的对象的图像、而获得的一个观测图像，被分离为用于边缘提取的区域和用于距离图像生成的区域，并且，利用这些区域来执行边缘提取和距离图像生成。这样，通过互补地利用基于二维边缘的位置／姿态测量和基于距离图像的位置／姿态测量，实现了位置／姿态测量的高精度化和高速化。

然而，利用专利文献1中公开的方法，由于基于通过对三维模型进行投影而获得的边缘模型来去除一部分图案光投影区域，因此能够准确地检测图像的边缘，但是，由图案光生成的距离图像的质量倾向于粗糙。当目标对象被放置为跨越观测图像的深度方向上的宽范围时，该现象尤其显著，从而经常导致位置／姿态测量的精度下降。

发明内容

本发明是鉴于上述问题而作出的，并且提供用于以更高的精度计算对象（尤其是被放置为在图像深度方向上有延伸的对象）的位置及姿态的技术。

根据本发明的一个方面，提供了一种距离测量装置，该距离测量装置包括：图像感测单元，其被配置为感测用图案光投影的目标对象的图像；指定单元，其被配置为在由所述图像感测单元感测的图像中，指定用所述图案光投影的区域；搜索单元，其被配置为基于所指定的区域，从所感测的图像中的、除用所述图案光投影的区域以外的区域中，搜索所述目标对象的几何特征；以及推导单元，被配置为基于由所述搜索单元搜索到的所述目标对象的几何特征以及所述目标对象的模型的几何特征，来推导所述目标对象的位置及姿态。

根据本发明的另一方面，提供了一种距离测量方法，该距离测量方法包括以下步骤：在由被配置为感测用图案光投影的目标对象的图像的图像感测单元感测的图像中，指定用图案光投影的区域；基于所指定的区域，从所感测的图像中的、除用所述图案光投影的区域以外的区域中，搜索所述目标对象的几何特征；以及基于在所述搜索步骤中搜索到的所述目标对象的几何特征以及所述目标对象的模型的几何特征，来推导所述目标对象的位置及姿态。

通过以下参照附图对示例性实施例的描述，本发明的其他特征将变得清楚。

附图说明

图1是示出系统的结构示例的图；

图2是示出图像处理装置104的功能结构示例的框图；

图3A至图3F是用于说明三维几何模型的图；

图4是要由图像处理装置104执行的处理的流程图；

图5是示出步骤S403中的处理的详情的流程图；

图6是示出感测图像的示例的图；

图7是示出二值图像的示例的图；

图8是示出图案光掩模的示例的图；

图9是示出步骤S404中的处理的详情的流程图；

图10A和图10B是用于说明关联处理的图；

图11是用于说明线段的投影图像与检测的边缘之间的关系的图；以及

图12是示出机器人系统的结构示例的图。

具体实施方式

下面，将参照附图描述本发明的实施例。请注意，下文中要描述的实施例是当具体实施本发明时的示例，并且是在权利要求的范围中记载的结构的具体实施例。

［第一实施例］

首先，将参照图1描述根据本实施例的系统的结构。对象101是作为要测量位置及姿态（位置／姿态）的对象的目标对象。在本实施例中，为了简单起见，假定在某一位置仅放置作为位置／姿态测量目标的一个对象101，如图1中所示。然而，下面要描述的位置／姿态测量处理并不在很大程度上依赖于对象的形状、数量和放置形式。例如，本发明适用于如下的情况，在该情况下，要在具有不同形状的多个对象被混合和堆积的状态下，测量某一对象的位置及姿态。

投影机102利用规定的图案光来照射对象。在本实施例中，使用狭缝光作为要用以照射对象101的图案光。稍后将描述投影机102的详情。

需要图像感测设备103来感测真实空间中的静止图像或电影，并且使用图像感测设备103来感测由投影机102用狭缝光照射的对象101的图像。然后，图像感测设备103将自身感测的图像（感测图像或者观测图像）输出至图处理装置104。稍后将描述图像感测设备103的详情。

图像处理装置104执行投影机102和图像感测设备103的操作控制，并且使用从图像感测设备103获得的感测图像来计算对象101的位置及姿态。

下面，将参照图2中示出的框图描述图像处理装置104的功能结构示例。

位置／姿态计算单元201使用基于图像感测设备103感测的图像的、边缘对应检测单元205和距离图像生成单元206的处理结果，来计算对象101的位置及姿态。然后，位置／姿态计算单元201将计算出的对象101的位置及姿态输出至合适的输出目的地。

通过把能够用三维CAD软件来处理的CAD模型本身或者三维CAD模型，转换成在计算机图形学领域中使用的多个多边形元素，来获得三维模型信息202。在该实施例中，使用模拟对象101的形状并且由多边形元素构成的三维几何模型。下面，将参照图3A至3F来描述由多边形元素构成的三维几何模型。

由多边形元素构成的三维几何模型包括诸如点、线和面的元素，如图3A至3F所示。图3A至3C示出了相同的三维几何模型。

由多边形元素构成的三维几何模型的模型信息与图3A中例示的三维几何模型的各顶点相关联地，来管理各顶点的索引以及各顶点的三维坐标值，如图3D所示。

此外，该模型信息与图3B所例示的三维几何模型的各个边相关联地，来管理各个边的索引以及各个边的两端的顶点的索引，如图3E所示。

此外，该模型信息与图3C中所例示的三维几何模型的各个面（多边形）相关联地，来管理各个多边形的索引、各个多边形的各个边的索引、以及各多边形的法向量，如图3F所示。

这样的三维模型信息202被存储在图像处理装置104中的合适存储器或者图像处理装置104能够访问的外部存储器中。

初始近似位置／姿态信息203被给定作为计算对象101的位置及姿态所需的迭代计算的初始值。作为该初始近似位置／姿态信息203，合适的值被预先设置，并且被存储在图像处理装置104中的合适的存储器或者图像处理装置104能够访问的外部存储器中。该初始近似位置／姿态信息203可以由用户使用操作单元（未示出）来设置。

除非另外指定，否则，假定位置及姿态均是以图像感测设备103的位置及姿态为基准的、在坐标系（照相机坐标系）上的位置及姿态。此外，假定在下面的描述中的“对象的坐标系”表示针对各个对象而在本地设置的坐标系。

边缘对应检测单元205检测投影到由图像感测设备103感测的图像上的三维几何模型与感测图像中包括的对象101的图像之间的对应边缘。

距离图像生成单元206根据由图像感测设备103感测的图像生成距离图像，该距离图像用各个像素代表从图像感测设备103到对象101上的各个位置的间距（distance）。在本实施例中，与距离图像的生成相关联地，通过使用多狭缝光作为图案光的三角测量来计算间距。然而，图案光不限于此，并且也可以使用其他图案，只要不需要多次图像感测操作即可。例如，可以使用随机点图案或者局部平面图案。作为另一选择，可以使用聚光灯。对于没有投射用于距离测量的照明的区域，当能够通过环境光获得足够的明亮度时，不投射特别的光。否则，改变照明图案，以投射例如均匀白光。此外，在图案投影方法中，可以反转投影区域以将图案表现为阴影。

投影机102是液晶投影机，并且是投射狭缝光作为规定的图案光的投影设备。例如，通过以下文献中公开的方法来预先校准投影机的固有参数（焦距、主点位置以及透镜畸变参数）。

Z.Zhang,"Aflexiblenewtechniqueforcameracalibration,"IEEETransactionsonPatternAnalysisandMachineIntelligence,vol.22,no.11,pp.1330-1334,2000。

请注意，可以使用除投影机102以外的设备，只要该设备能够投射狭缝光即可。例如，可以使用利用DMD（数字微镜器件，DigitalMirrorDevice）或LCOS的投影机。

例如，通过前述Zhang的方法来预先校准图像感测设备103的固有参数（焦距、主点位置以及透镜畸变参数）。此外，例如，通过前述Zhang的方法或者以下文献中公开的方法，来预先校准图像感测设备103与投影机102之间的相对位置及姿态，由此使得能够使用图案光进行三角测量。

SeijiIguchiandKosukeSato,"Three-dimensionalImageMeasurement"(SHOKODO,1990)。

下面，将参照图4描述由图像处理装置104执行以计算对象101的位置及姿态的处理，其中，图4示出了该处理的流程图。

在步骤S401中，位置／姿态计算单元201从图像处理装置104的内部或外部存储器中，读出三维模型信息202。请注意，位置／姿态计算单元201可以获取由用户通过操作操作单元（未示出）而输入的三维模型信息202。作为另一选择，可以将多个三维几何模型的多个三维模型信息202存储在存储器中，可以识别由图像感测设备103感测的图像中包括的对象，并且可以读出与所识别的对象相对应的三维模型信息202。

接下来，在步骤S402中，位置／姿态计算单元201从图像处理装置104的内部或外部存储器中，读出初始近似位置／姿态信息203。请注意，位置／姿态计算单元201可以获取由用户通过操作操作单元（未示出）而输入的初始近似位置／姿态信息203。作为另一选择，可以通过光学传感器来决定和获取初始近似位置／姿态信息203，其中，该光学传感器使用固定于场景的照相机，通过感测安置在对象上的标记来测量位置及姿态。此外，可以使用任意的传感器，只要这些传感器能够测量6个自由度的位置及姿态即可。此外，当预先知道对象被放置的近似位置及姿态时，可以将这些值用作初始近似位置／姿态信息203。

接下来，在步骤S403中，边缘对应检测单元205和距离图像生成单元206基于由图像感测设备103感测的图像，分别生成图案光掩模和距离图像。稍后，将参照图5中示出的流程图描述步骤S403中的处理的详情。

接下来，在步骤S404中，位置／姿态计算单元201、边缘对应检测单元205或者控制单元（未示出）基于在步骤S401至S403中获得的各个信息，来决定对象101的三维几何模型的位置及姿态。由此，计算出位置及姿态，作为对象101的位置及姿态。

下面，将参照图5描述在上述步骤S403中的处理的详情，其中，图5示出了该处理的流程图。

在步骤S501中，控制单元（未示出）控制投影机102利用作为规定的图案光的狭缝光来照射对象101。

在步骤S502中，控制单元（未示出）控制图像感测设备103感测利用规定的图案光照射的对象101的图像。图6示出了图像感测设备103的感测图像的示例。为了简单起见，在下面的描述中，将假定由图像感测设备103感测的图像是图6中所例示的图像。

在步骤S503中，控制单元（未示出）控制投影机102结束作为规定的图案光的狭缝光的照射。

在步骤S504中，边缘对应检测单元205检测感测图像中的具有图案光的区域，作为图案光投影区域，并且生成二值图像，该二值图像具有与感测图像相同的垂直大小和水平大小，并使用不同的像素值来表现图案光投影区域和非图案光投影区域。以如下的方式生成二值图像。

也即，参照感测图像中的各个像素位置的亮度值，并且当所参照的像素位置（x，y）处的亮度值不小于阈值时，判断该像素位置属于图案光投影区域，并且将二值图像中的像素位置（x，y）处的像素值设置为“1”。另一方面，如果感测图像中的像素位置（x，y）处的亮度值小于阈值，则判断该像素位置不属于图案光投影区域，并且将二值图像中的像素位置（x，y）处的像素值设置为“0”。请注意，“1”和“0”的意义可以反转，并且本发明不限于这些值。也就是说，二值图像中的各个像素位置处的像素是否位于属于图案光投影区域的像素位置处，仅仅需要通过像素值来判别。

除了如上所述比较亮度值与阈值以外，还可以通过各种其他方法，来判断感测图像中的各个像素位置处的像素是否属于图案光投影区域。例如，可以通过如下两种方法来实施此类判断，其中，一种方法是基于狭缝光的颜色的成分信息以及感测图像中的各个像素的颜色的成分信息来进行此种判别，另一方法是基于颜色信息改变的方式来进行此种判别。

图7示出了由图6所示的感测图像生成的二值图像的示例。在图7中，图6中示出的感测图像中的具有图案光投影区域的区域用黑线表示，并且这些黑线由像素值为“1”的像素构成（黑线以外的区域由像素值为“0”的像素构成）。

接下来，在步骤S505中，距离图像生成单元206使用在步骤S504中生成的二值图像生成距离图像。可以根据以下文献中记载的方法，来计算作为距离图像的各个像素的像素值的间距。

R.HartleyandA.Zisserman"MultipleviewgeometryincomputervisionSecondEdition"(CambridgeUniversityPress,2003)。

在该实施例中，基于由投影机102投影的图案光中的像素位置、感测图像上的对应位置、投影机102和图像感测设备103的固有参数以及投影机102与图像感测设备103之间的相对位置及姿态，来计算间距。然后，将与感测图像的各个像素位置相对应的视线向量乘以这些像素位置的间距，由此计算作为穿过感测图像的各个像素位置的视线向量的指向目的地的位置（照相机坐标系中的三维位置），作为点组数据。

在步骤S506中，边缘对应检测单元205生成通过改变二值图像（即通过放大二值图像中的像素值为“1”的区域）而获得的图像作为图案光掩模（具有与二值图像相同的垂直大小和水平大小）。

首先，生成图案光掩模作为二值图像的副本。然后，参照图案光掩模中的像素位置，并且当所参照的像素位置（x，y）处的像素的像素值为“1”时，将关注像素的邻近像素（例如，与关注像素相邻的像素）的像素值更新为“1”。

图8示出了由图7所示的二值图像生成的图案光掩模的示例。在图8中，图7中示出的二值图像中的图案光投影区域（由像素值为“1”的像素构成）被放大为更粗的黑线，并且放大的黑线由像素值为“1”的像素构成（除放大的黑线以外的区域由像素值为“0”的像素构成）。

在稍后的位置/姿态计算处理中，该图案光掩模被用于计算感测图像上的三维几何模型与对象101之间的边缘的偏差（误差）。图案光投影区域被放大的原因，是为了降低三维几何模型的边缘与通过图案光投影形成的边缘之间的对应错误的风险。稍后将详细描述该关联。

接下来，将参照图9描述步骤S404中的处理的详情，其中，图9示出了该处理的流程图。在该实施例中，在决定对象101的位置及姿态所需的迭代计算中，使用Gauss-Newton方法（高斯-牛顿方法）。因此，通过迭代计算迭代地校正对象101的位置及姿态（在下面的描述中用“s”表示）的近似值，从而计算位置及姿态。

在本实施例中的位置及姿态的计算中，以下述方式优化估计的位置及姿态。也即，通过使两类间距的总和最小化，来优化估计的位置及姿态，其中，一类间距是感测图像上的对象101的边缘、与基于估计的位置及姿态而投影到该图像上的三维几何模型的线段之间的间距，另一类间距是由点组数据表示的各个位置、与估计的位置及姿态的三维几何模型之间的间距。更具体地说，二维图像上的点和线之间的带符号的间距和三维空间中的点和面之间的带符号的间距通过线性Taylor展开（泰勒展开），而表现为对象101的位置及姿态的极小改变的线性函数。然后，通过建立并求解与位置及姿态的极小改变相关联的线性联立方程，使得带符号的间距变为0，来计算对象101的位置及姿态的极小改变，以重复地校正位置及姿态。

在步骤S901中，位置/姿态计算单元201将上述初始近似位置/姿态信息203设置为以下迭代计算的初始值（初始位置及姿态s）。

在步骤S902中，边缘对应检测单元205将具有由当前值s表示的位置及姿态的三维几何模型（在实际中仅边被投影）投影到感测图像上。此外，位置/姿态计算单元201与构成三维几何模型的各个面相关联地，将各面的各个顶点的坐标值转换为照相机坐标系中的坐标值。

然后，边缘对应检测单元205在感测图像中的对象101与投影到感测图像上的三维几何模型之间，来进行边缘的关联。下面，将参照图10A和图10B描述该关联处理。在图10A中，与图案光掩模中的像素值为“1”的区域相对应的感测图像中的区域被表示为图案光掩模1004。

通过投影具有由当前值s表示的位置及姿态的三维几何模型的线段而获得的线段，被示出为线段1001。线段的投影图像在图像上也是线段。接下来，在投影的线段1001上设置控制点1002，使得控制点1002在图像上具有相等的间隔，并且针对各个控制点1002，在投影的线段1001的法线方向上设置搜索线1003。然后，针对各个控制点1002，在如下的线上检测一维边缘，其中，所述的线位于针对该控制点设置的搜索线1003上、该控制点的规定的间距以内，并且不包括图案光掩模1004。然后，保持最靠近检测的边缘的控制点1002的点，作为对应点1005。在实际中，尽管图案光掩模1004包括由图案光形成的边缘，但是由于以这种方式搜索对应点，因此能够避免由图案光形成的边缘被错误地关联。

图10B示出了原点是控制点、横坐标标绘搜索线、并且纵坐标标绘亮度梯度的绝对值的图。在前一图像中，边缘被检测为像素值的亮度梯度的绝对值的极值。在这种情况下，亮度梯度的绝对值的极值大于预定值并且最接近控制点的点，被检测作为对应点1006（图10A中的对应点1005）。

请注意，在本实施例中，当检测并非由图案光导致的边缘时，在除图案光掩模1004以外的区域中检测边缘。然而，此时的处理顺序也可以是以下的顺序，但是获得相同的效果。也即，在检测到边缘之后，判别检测到的边缘是否位于掩模区域内。如果检测到的边缘位于掩模区域内，则采用该边缘；如果检测到的边缘位于该区域之外，则检测下一边缘。

在步骤S903中，位置/姿态计算单元201计算计算位置及姿态所需的（求解线性联立方程所需的）系数矩阵以及误差向量。为了使作为三维几何模型的边的线段与边缘相关联，并且为了计算对象101的位置及姿态，必须计算计算位置及姿态所需的系数矩阵以及误差向量。

请注意，系数矩阵的各元素是针对位置及姿态的近似值的极小改变的线性偏微分系数。针对边缘，计算图像坐标的偏微分系数，并且针对点组，计算三维坐标的偏微分系数。各个误差向量与检测到的边缘相关联的，是投影的线段与该边缘之间在图像上的间距，而与点组数据相关联的，是模型的面与点之间在三维空间中的间距。

图11是用于说明线段的投影图像和检测到的边缘的图。在图11中，图像的水平方向和垂直方向分别被表示为u轴和v轴。某一控制点（在图像上等间隔地分割各个投影线段的点）在图像上的坐标用（u0，v0）来表示，控制点所属的线段在图像上的倾角被表示为相对于u轴的倾角θ。通过投射线段的两端的三维坐标，倾角θ被计算为使得图像上的两端的坐标耦合的线的倾角。线段在图像上的法向量用（sinθ，-cosθ）来表示。此外，控制点的对应点在图像上的坐标用（u'，v'）来表示。穿过对应点的坐标（u'，v'）并且具有倾角θ的直线（图11中的虚线）上的点（u，v）可以通过下式来表示：

usinθ-vcosθ＝d(1)

d＝u′sinθ-v′cosθ

控制点在图像上的位置依据对象101的位置及姿态而改变。对象101的位置及姿态的自由度是6个自由度。也即，s是六维向量，其包括代表对象101的位置的三个元素和代表姿态的三个元素。代表姿态的三个元素用例如Euler角（欧拉角）或如下的三维向量来表示，在所述三维向量中，方向代表穿过原点的旋转轴，并且法线代表旋转角。可以通过在坐标（u0，v0）附近进行线性Taylor展开，来近似求得依据位置及姿态而改变的点在图像上的坐标（u，v），比如利用下式：

$\begin{array}{c}u\≈u_0+\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂u}{\∂s_i}\mathrm{\Δ}{s}_i\\ v\≈v_0+\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂v}{\∂s_i}\mathrm{\Δ}{s}_i\end{array}---\left(2\right)$

其中，Δs_i（i=1、2、…、6）是s的各个成分的极小改变。

能够进行如下假设，即通过校正值s获得的控制点在图像上的位置位于方程（1）给定的直线上。将由式（2）近似求出的u和v代入方程（1），得到：

$\sin \mathrm{\θ}\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂u}{\∂s_i}\mathrm{\Δ}{s}_i-\cos \mathrm{\θ}\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂v}{\∂s_i}\mathrm{\Δ}{s}_i=d-r---\left(3\right)$

r＝u₀sinθ-v₀cosθ

可以使用对象101的位置及姿态s，来将照相机坐标系中的三维坐标转换为对象101的坐标系中的三维坐标（x，y，z）。假定基于近似位置及姿态将某一点转换为对象101的坐标系中的点（x0，y0，z0）。坐标（x，y，z）依据对象101的位置及姿态而改变，并且可以通过在点（x0，y0，z0）附近进行线性Taylor展开来近似求出，比如利用下式：

$\begin{array}{c}x\≈x_0+\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂x}{\∂s_i}\mathrm{\Δ}{s}_i\\ y\≈y_0+\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂y}{\∂s_i}\mathrm{\Δ}{s}_i\\ z\≈z_0+\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂z}{\∂s_i}\mathrm{\Δ}{s}_i\end{array}---\left(4\right)$

假定通过ax+by+cz=e（a2+b2+c2=1，a、b、c和e是常数），来给定在步骤S902中与由点组数据中的某一数据表示的位置相关联的三维模型的面的、在对象101的坐标系中的方程。此外，假定使用校正值s转换的坐标（x，y，z）满足二维平面的方程ax+by+cz=e（a2+b2+c2=1）。当将上述式（4）代入平面的方程时，得到：

$a\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂x}{\∂s_i}\mathrm{\Δ}{s}_i+b\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂y}{\∂s_i}\mathrm{\Δ}{s}_i+c\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\frac{\∂z}{\∂s_i}\mathrm{\Δ}{s}_i=e-q---\left(5\right)$

q＝ax₀+by₀+cz₀

在本实施例中，如上所述，通过三维几何模型的面与对应的测量点之间的间距，来表示三维空间中的误差值（方程（5））。然而，误差值的定义方法不限于此，并且可以使用任意其他方法，只要这些方法能够定义三维几何模型与点组数据之间的间距（偏差量）即可。例如，当点组数据包括线信息和面信息时，如果使用边缘间间距或者面间间距来定义误差值，则能够类似地计算位置及姿态。

方程（3）可以对所有相关联的边缘均成立。此外，由于方程（5）对所有相关联的点组数据均成立，因此与Δs_i相关联的线性联立方程成立，如下式所给定的：

请注意，方程（6）表示如下：

JΔs＝E(7)

根据例如在以下文献中公开的方法，来计算计算线性联立方程（6）的系数矩阵J所需的微偏分系数：

V.LepetitandP.Fua,"Keypointrecognitionusingrandomizedtrees,"IEEETransactionsonPatternAnalysisandMachineIntelligence,vol.28,no.9,2006。

在步骤S904中，位置/姿态计算单元201计算计算线性联立方程的系数矩阵J所需的微偏分系数。基于方程（7），位置/姿态计算单元201使用矩阵J的广义逆矩阵(J^T·J)^-1·J^T，来计算位置/姿态校正值Δs。然而，由于边缘和点组数据包括由于检测错误而产生的许多异常值，因此使用下面要描述的稳健估计方法。一般来说，在作为异常值的边缘（点组数据）处，误差d-r（e-q）变大。因此，这种边缘对方程（6）和（7）的联立方程具有大的贡献度，并且Δs的精度因而降低。

因此，对具有大的误差d-r（e-q）的数据赋予小的权重，对具有小的误差d-r（e-q）的数据赋予大的权重。使用Tukey函数来赋予权重，如下式所示：

$\begin{array}{c}w(d-r)=\left\{\begin{array}{cc}{(1-{((d-r)/c_1)}^2)}^2& |d-r|\≤c_1\\ 0& |d-r|>c_1\end{array}\right.\\ w(e-q)=\left\{\begin{array}{cc}{(1-{((e-q)/c_2)}^2)}^2& |e-q|\≤c_2\\ 0& |e-q|>c_2\end{array}\right.\end{array}---\left(8\right)$

其中，c₁和c₂是常数。请注意，用于赋予权重的函数并非必须是Tukey函数，并且可以使用诸如Huber函数（休伯函数）的任何其他函数，只要这些函数能够对具有大误差的信息赋予小的权重、并对具有小误差的信息赋予大的权重即可。设w_i为与各个测量信息（边或点组数据）相对应的权重。请注意，权重矩阵W由下式来定义:

权重矩阵W是方形矩阵，其除了对角线元素以外均为“0”，对角线上为权重wi。使用该权重矩阵W，对方程（7）进行变形，类似如下：

WJΔs＝WE(10)

通过求解方程（10）来计算校正值Δs，类似如下：

Δs＝(J^TWJ)^-1J^TWE(11)

在步骤S905中，位置/姿态计算单元201通过使用在步骤S904中计算的位置/姿态校正值Δs进行如下计算，来校正（更新）位置/姿态近似值s：

s←s+Δs

在步骤S906中，位置/姿态计算单元201进行s的收敛确定。如果s是收敛的，则处理结束；否则，处理返回到步骤S902以继续进行迭代计算。在收敛确定中，当校正值Δs几乎为零时，或者当误差向量的平方和在校正前后几乎相同时，确定收敛。以这种方式，通过迭代地进行计算直到收敛，能够计算出位置及姿态。

如上所述，根据本实施例，能够使得位置/姿态测量相对于对象的表面形状和几何形状具有更高的稳定性，尤其是针对被布置为在图像深度方向上有延伸的对象具有高的精度。

请注意，本实施例使用Gauss-Newton方法作为模型拟合的示例而说明了计算方法。模型拟合方法不限于此。例如，可以使用能够实现更具稳健性的计算的Levenberg-Marquardt方法（利文贝格-马夸特方法）来进行计算，或者使用作为更简单的方法的最陡下降法来进行计算。此外，也可以使用诸如共轭梯度法和ICCG方法的其他非线性优化计算方法。

此外，在本实施例中，使用在步骤S403中获得的距离图像和图案光掩模，在步骤S404中通过迭代计算的模型拟合来计算对象101的位置及姿态。然而，也可以使用如下的方法，即在步骤S404中利用图案匹配替代模型拟合，来直接计算位置及姿态。

例如，将从CAD模型的中心到顶点具有相等间距的GeodesicSphere（测地球）设置作为对象的CAD数据。然后，预先准备从GeodesicSphere的各个顶点向CAD模型的中心观察时的边缘图像，并且在这些视点相关边缘模型与已经经历图案光掩模处理的感测图像之间，执行图案匹配。以这种方式，能够计算对象的近似位置及姿态，同时抑制图案光噪声的影响。此外，在步骤S403中获得的距离图像和图案光掩模的用途不限于在模型拟合和图案匹配中使用。例如，距离图像和图案光掩模广泛适用于利用图案光掩模来消除图案光噪声的影响的普通对照处理。

注意，在本实施例中，图像处理装置104直接从图像感测设备103获取感测图像。然而，本发明不限于此。例如，图像处理装置104可以从存储由图像感测设备103感测的图像的存储器中获取感测图像。在这种情况下，图像感测设备103并不需要总是连接至图像处理装置104。

也即，根据第一实施例和以下实施例的图像处理装置104仅仅是下述结构的示例，并且该结构的各种变形例均是适用的，只要这些变形例具有与下述结构等同的功能即可。

也就是说，获取包括用图案光投影的目标对象的图像。然后，在图像上叠加模拟目标对象的形状的模型信息，从图像中的、除通过放大具有图案光的区域而获得的放大区域以外的区域中，指定与叠加的模型信息的各点相对应的目标对象的点。然后，基于指定的点，计算叠加在图像上的模型信息与目标对象之间的位置偏差。然后，计算模型信息的位置及姿态，作为目标对象的位置及姿态，从而使位置偏差最小化。

在上述搜索中，将感测图像中具有图案光的区域定义为图案光区域，并且，将感测图像中没有任何图案光的区域定义为非图案光区域。生成如下的掩模图像，该掩模图像在与图案光区域相对应的掩模图像中的区域中、和在与非图案光区域相对应的掩模图像中的区域中，具有不同的像素值；并且，将感测图像中的如下区域定义为放大区域，所述区域对应于通过放大与掩模光区域相对应的掩模图像中的区域而获得的区域。然后，将除放大区域以外的区域中的搜索结果定义为“目标对象的对应点”。作为另一选择，把通过从搜索结果中排除放大区域中的位置而获得的结果，定义为“目标对象的对应点”。

在上述计算中，根据掩模图像，生成以从图像感测设备到目标对象上的各个位置的间距作为像素值的距离图像。然后，进行迭代计算，以在由距离图像表示的目标对象上的各个位置与和模型信息上的这些位置相对应的各个位置之间，使位置偏差最小化，并且使所述间距最小化。

[第二实施例]

第一实施例说明了从感测图像中检测图案光投影区域、并且在步骤S506中基于检测到的区域生成图案光掩模的方法。该方法即使在图案光的形状未知的情况下也是广泛适用的。作为另一方法，当图案光的形状被给定时，根据该图案光形状、对象的近似位置及姿态、以及对象的几何形状，来计算图案光在感测图像中的投影区域，由此来生成图案光掩模。

基于在投影机投射图案光的状态下的光源信息，以及以近似位置及姿态布置的对象的模型信息，来计算感测图像的照相机狭缝光被投影到的特定像素。更具体地说，利用射线追踪法的原理来计算这种像素，所述射线追踪法追踪并计算从狭缝光源照射的各照相机光线经由对象表面的上的突起、而到达图像传感器上的哪些位置。因此，能够获得在步骤S504中生成的、表示图案光投影区域的与位图（图7）等同的信息。随后的处理与在第一实施例中描述的方法的处理相同，由此测量对象的位置及姿态。

[第三实施例]

第一实施例和第二实施例中描述的方法是检测边缘同时避开图案光的方法。上述误差计算方法（在下文中称为“间距优先误差计算方法”）优先考虑图案光检测，并且对深度方向上的误差具有高的检测精度。与之相比，专利文献1中公开的方法（在下文中称为“边缘优先误差计算方法”）优先考虑边缘检测，并且特征在于对感测图像平面方向上的误差具有高的检测精度。

基于这些特性，配设了用于基于误差计算时的对象形状、来判别采用间距优先误差计算方法还是边缘优先误差计算方法的机制，由此使得能够进行更高精度的测量。下面将描述该判别标准的具体示例。

第一种方法基于对象形状。首先，在对象的三维几何模型的体积内均匀地设置样本点。对这些样本点的集合应用主成分分析，以计算第一、第二和第三主成分的贡献率（各个主成分与三个主成分的和的比率）。此外，这些贡献率的关系和特征分类如下。

1)第一主成分贡献率>>第二主成分贡献率

→对象的体积呈线性分布

2)第一主成分贡献率≈第二主成分贡献率，并且第二主成分贡献率>>第三主成分贡献率

→对象的体积呈二维分布

3)第一主成分贡献率≈第二主成分贡献率≈第三主成分贡献率

→对象的体积在三维空间中均匀分布

请注意，a>>b表示a远大于b（a与b的比率大于阈值）。

当主成分贡献率的关系是上述1）或者2）、并且对象被放置在具有平坦表面的测量台上时，对象被放置为沿着表面延伸，并且观测图像深度方向的分布倾向于很小。另一方面，当主成分贡献率的关系是3）时，对象的观测图像深度方向倾向于很大。

基于以上描述，当主成分贡献率满足关系3）、并且测量目标对象的体积分布在观测图像深度方向上有宽的分布时，采用间距优先误差计算方法；否则，计算方法切换为采用边缘优先误差计算方法。请注意，误差计算方法的切换标准仅仅是示例，并且作为替代，还可以使用着重于对象在图像深度方向上的延伸的标准。也就是说，在下面的两个处理中，执行根据模型信息的形状决定的一个处理。

（处理1）将模拟目标对象的形状的模型信息叠加到图像上，并且从图像中的、除通过放大具有图案光的区域而获得的放大区域以外的区域中，搜索与叠加的模型信息的各点相对应的目标对象的点。

（处理2）将模型信息的边缘模型叠加到图像上，基于叠加在图像上的边缘模型来去除图像中具有图案光的区域，然后从图像中除具有图案光的区域以外的区域中，搜索与叠加在图像上的模型信息的各点相对应的目标对象的点。

作为第二种方法，可以使用基于近似位置及姿态的对象的位置及姿态在深度方向上的模糊度的计算方法。根据以下文献，假定观测图像的像素的模糊度对于所有像素均为“1”，则相对于某一近似位置及姿态的方向的模糊度的协方差矩阵用JTJ来表示。

W.HoffandT.Vincent"IEEETrans.VisualizationandComputerGraphics,Vol.6,No.4,2000."。

设u为通过对该协方差矩阵应用特征值分解而计算出的主方向单位向量，则近似位置及姿态的对象的位置及姿态在深度方向上的模糊度，能够表示为u和深度方向单位向量的内积。该值可以取从0至1距离内的值，并且在该值更接近1时，代表位置及姿态在深度方向上是模糊的。在第一实施例中已描述了该值的计算方法。当计算的模糊度不小于预定基准值时，采用间距优先误差计算方法；否则，计算方法切换为采用边缘优先误差计算方法。

使用在上文中例示的切换标准，采用考虑各个特性的误差计算方法，由此使得能够以更高的精度进行位置/姿态测量。

[第四实施例]

第一至第三实施例的图像处理装置能够用作在使用工业机器人手臂抓取、移动和释放对象的系统（机器人系统）中、测量对象的位置及姿态的装置。下面将参照图12描述该应用示例。

通过整合图像感测设备103和投影机102（二者可以独立地布置）而获得传感器1202。传感器1202将图案光投影到作为位置/姿态测量目标的对象1205上，并感测用图案光投影的对象1205的图像。该传感器1202附装在机器人手臂1203上，并且能够感测机器人手臂1203要抓取的对象1205的图像。

机器人手臂1203具有包括旋转轴和/或平移轴的可移动轴，并且由机器人控制器1204控制。

计算机1201用作图像处理装置104，并且控制机器人控制器1204。机器人控制器1204将从计算机1201接收到的用于机器人手臂1203的控制指令（包括对象1205的位置及姿态）转换为具体的控制命令，由此驱动机器人手臂1203。

除了上述序列以外，本实施例中的图像处理装置104还包括下面的处理序列。首先，计算机1201保持照相机坐标系与以机器人手臂1203为基准的机器人坐标系之间的相对关系。计算器1201测量对象1205的位置及姿态，并将测量结果从照相机坐标系转换到机器人坐标系中。接下来，计算机1201基于转换后的对象在机器人坐标系中的位置及姿态，来控制机器人控制器1204将机器人手臂1203移动至机器人手臂1203能够抓取对象1205的位置及姿态。

因此，本实施例的机器人系统即使在对象的位置不稳定的情况下，也能够通过测量对象的位置及姿态来控制机器人手臂抓取对象。

请注意，图像处理装置104适用于任意的系统，只要这些系统需要测量作为位置/姿态测量目标的对象的位置及姿态即可。换句话说，由图像处理装置104计算的位置及姿态的使用目的不限于特定的使用目的。

[第五实施例]

图1中所示的图像处理装置104中包括的各个单元（位置/姿态计算单元201、边缘对应检测单元205以及距离图像生成单元206）可以通过硬件来实现，但是也可以通过软件（计算机程序）来实现。在后一种情况下，在计算机能够访问的存储器中，三维模型信息202和初始近似位置/姿态信息203被存储为数据，并且位置/姿态计算单元201、边缘对应检测单元205和距离图像生成单元206被存储为计算机程序。然后，当该计算机的CPU读出这些计算机程序和数据并执行处理时，该计算机能够用作图像处理装置104。

<各实施例的效果>

根据第一实施例，能够使得位置/姿态测量相对于对象的表面形状和几何形状具有更高的稳定性，尤其是针对被布置为在图像深度方向上有延伸的对象具有高的精度。

根据第二实施例，即使无法通过图像处理来检测感测图像中的图案光，也能够实现校正关联处理。

根据第三实施例，通过基于对象形状或对象布局状态来选择误差计算方法，能够提高测量精度。

根据第四实施例，能够抓取放置在不稳定场所的对象。

<定义>

以上描述中的图案光仅需要指定感测图像中的投影区域边界。为此，各种投影设备均能够应用于用于投射图案光的设备。也就是说，系统中包括的设备可以主动地投射图案光，或者，可以从系统无法控制的外部设备被动地投射图案光。此外，图案光的投影形状不受特别限制，只要能够指定图案光的投影区域边界即可。图案光可以具有狭缝形状、随机点图案或者局部平面图案。作为另一选择，可以投影聚光灯。

感测图像中的图案光的各个投影区域的指定方法不受特别限制。也就是说，如第一实施例中所述，可以使用阈值或者其他颜色信息作为阈值，基于亮度值来指定投影区域。此外，如第二实施例中所述，如果图案光的投影形状被给定，则可以基于该形状、对象的近似位置及姿态以及对象的几何形状，来计算和指定投影区域。

此外，以上描述中的位置/姿态计算方法不受特别限制，只要使用具有图案光的图像来执行方法即可。例如，可以使用如下的模型拟合，该模型拟合基于预先保持的对象三维模型，来决定感测图像中包括的对象的三维位置/姿态参数。作为另一选择，可以使用如下的模板匹配，该模板匹配基于预先保持的模板图像，来决定感测图像中包括的预定模板的位置。

其他实施例

本发明的各方面还可以通过读出并执行记录在存储设备上的用于执行上述实施例的功能的程序的系统或装置的计算机（或诸如CPU或MPU的设备）来实现，以及通过由系统或装置的计算机通过例如读出并执行记录在存储设备上的用于执行上述实施例的功能的程序来执行各步骤的方法来实现。鉴于此，例如经由网络或者从用作存储设备的各种类型的记录介质（例如计算机可读介质）向计算机提供程序。在这种情况下，系统或装置、以及存储程序的记录介质包括在本发明的范围内。

虽然参照示例性实施例对本发明进行了描述，但是应当理解，本发明并不限于所公开的示例性实施例。应当对所附权利要求的范围给予最宽的解释，以使其涵盖所有这些变型例以及等同的结构和功能。

Claims (8)

1.一种位置和姿态测量装置，该位置和姿态测量装置包括：

图像感测单元，其被配置为感测用图案光投影的目标对象的图像；

距离图像生成单元，其被配置为基于所感测的目标对象的图像，生成以从所述图像感测单元到所述目标对象的各个位置的间距作为像素值的距离图像；

指定单元，其被配置为在由所述图像感测单元感测的图像中，指定用所述图案光投影的区域；

搜索单元，其被配置为基于所指定的区域，从所感测的图像中的、除用所述图案光投影的区域以外的区域中，搜索所述目标对象的几何特征；以及

推导单元，被配置为基于由所述搜索单元搜索到的所述目标对象的几何特征与所感测的图像中的所述目标对象的模型的相对应的几何特征之间的间距，以及由所述距离图像表示的所述目标对象的几何特征与所述目标对象的模型上的相对应的几何特征之间的间距，来推导所述目标对象的位置及姿态。

2.根据权利要求1所述的位置和姿态测量装置，其中，所述搜索单元放大用所述图案光投影的区域，并从除所放大的区域以外的区域中搜索所述目标对象的几何特征。

3.根据权利要求1所述的位置和姿态测量装置，其中，所述搜索单元提取所感测的图像中的几何特征，并从所提取的几何特征中排除用所述图案光投影的区域的几何特征。

4.根据权利要求1所述的位置和姿态测量装置，其中，所述指定单元基于所感测的图像中的亮度值，来指定用所述图案光投影的区域。

5.根据权利要求1所述的位置和姿态测量装置，其中，所述指定单元基于被给定作为所述目标对象的位置及姿态的近似位置及姿态、所述目标对象的几何形状以及所述图案光的形状，来指定用所述图案光投影的区域。

6.根据权利要求1所述的位置和姿态测量装置，其中，所述搜索单元包括：

生成单元，其被配置为定义所感测的图像中的具有所述图案光的区域作为图案光区域，并生成与所述图案光区域相对应的掩模图像，并且

所述搜索单元基于所述掩模图像，来从所感测的图像中的、除用所述图案光投影的区域以外的区域中，搜索所述目标对象的几何特征。

7.根据权利要求1所述的位置和姿态测量装置，该位置和姿态测量装置还包括：

输出单元，其被配置为把由所述推导单元推导出的位置及姿态，输出至对操纵所述目标对象的机器人手臂进行控制的控制器。

8.一种位置和姿态测量方法，该位置和姿态测量方法包括以下步骤：

在由被配置为感测用图案光投影的目标对象的图像的图像感测单元感测的图像中，指定用图案光投影的区域；

基于所感测的目标对象的图像，生成以从所述图像感测单元到所述目标对象的各个位置的间距作为像素值的距离图像；

基于所指定的区域，从所感测的图像中的、除用所述图案光投影的区域以外的区域中，搜索所述目标对象的几何特征；以及

基于在所述搜索步骤中搜索到的所述目标对象的几何特征与所感测的图像中的所述目标对象的模型的相对应的几何特征之间的间距，以及由所述距离图像表示的所述目标对象的几何特征与所述目标对象的模型上的相对应的几何特征之间的间距，来推导所述目标对象的位置及姿态。