CN104006740B - 物体检测方法和物体检测装置 - Google Patents

Abstract

本发明提供一种物体检测方法和物体检测装置。在物体的模型的外部设定多个外部基准点，在模型的内部设定内部基准点。存储将如下二者关联起来而成的表：包含从位于所述模型的表面的点群中依次选择出的起点和终点的对的局部表面的特征量，和起点对应的外部基准点、内部基准点的位置的组。从物体表面的样本点群中依次选择起点、终点的对，计算包含起点、终点的对的局部表面的物体的特征量。从表中取得与和物体的特征量一致的特征量关联的位置的组，并将其变换为实空间中的位置的组，在位置的组中包含的内部基准点的位置位于物体的外部的情况下，将位置的组中包含的外部基准点的位置从估计素材中排除，来估计物体的位置和姿态。

Description

物体检测方法和物体检测装置

技术领域

本发明公开的实施方式涉及物体检测方法和物体检测装置。

背景技术

在专利文献1中，公开了如下物体检测方法：对从距离传感器到位于物体边缘上的点的距离进行霍夫（hough）变换，并投票到霍夫空间，根据霍夫空间中的得票数多的点，来检测物体的位置和姿态。

现有技术文献

专利文献1：日本特许第5088278号公报

发明内容

但是，在使用了霍夫变换的物体检测方法中存在如下问题：由于对霍夫空间的投票数庞大，因而物体的位置和姿态检测所需的处理量增多。

实施方式的一个方式是鉴于上述而完成的，其目的在于，提供一种能够降低物体的位置和姿态的检测所需的处理量，并且提高检测精度的物体检测方法和物体检测装置。

在实施方式的一个方式的物体检测方法中，在物体的模型的外部空间中，设定作为所述物体的位置和姿态的估计素材的多个外部基准点，在所述模型的内部空间中，设定作为所述估计素材的可否判定素材的内部基准点。存储将如下二者关联起来而成的表：包含从位于所述模型的表面的点群中依次选择出的起点和终点的对的局部表面的特征量，和所述起点对应的所述外部基准点和所述内部基准点的位置的组。从位于实空间中存在的物体的表面的样本点群中，依次选择起点和终点的对，计算所述物体在包含该起点和终点的对的局部表面中的特征量。从所述表中取得与和所述物体的特征量一致的特征量关联的所述位置的组，并将其变换为实空间中的位置的组，在该位置的组中包含的所述内部基准点的位置位于所述物体的外部的情况下，将该位置的组中包含的所述外部基准点的位置从所述估计素材中排除，来估计所述物体的位置和姿态。

根据实施方式的一个方式，能够降低物体的位置和姿态的检测所需的处理量，提高检测精度。

附图说明

图1A是示出实施方式的物体检测方法的说明图。

图1B是示出实施方式的物体检测方法的说明图。

图2是示出实施方式的机器人系统的说明图。

图3是示出实施方式的物体检测装置的结构的一例的说明图。

图4A是示出实施方式的表的生成顺序的说明图。

图4B是示出实施方式的表的生成顺序的说明图。

图5A是示出实施方式的表的生成顺序的说明图。

图5B是示出实施方式的表的生成顺序的说明图。

图6是示出实施方式的表的生成顺序的说明图。

图7是示出实施方式的表的一例的说明图。

图8A是示出检测实施方式的物体的位置和姿态的顺序的说明图。

图8B是示出检测实施方式的物体的位置和姿态的顺序的说明图。

图9是示出检测实施方式的物体的位置和姿态的顺序的说明图。

图10是示出检测实施方式的物体的位置和姿态的顺序的说明图。

图11是示出实施方式的表的生成处理的流程图。

图12是示出实施方式的物体的位置和姿态的检测处理的流程图。

图13是示出与实施方式的把持候选相关的信息的生成处理的流程图。

标号说明

1机器人系统；2机器人；3传感器；4物体检测装置；5机器人控制装置；7处理部；71模型数据取得部；72场景数据取得部；73基准点设定部；74边缘检测部；75surflet选择部；76运算部；77位置/姿态估计部；78把持对象信息生成部；8存储部；81表；W物体；M模型。

具体实施方式

以下，参照附图，对本申请公开的物体检测方法和物体检测装置的实施方式进行详细说明。此外，本发明不限于以下所示的实施方式。此外，以下，以作为位置和姿态的检测对象的物体的形状是圆筒形状的情况为例进行说明，但是检测对象的物体的形状不限于圆筒形状。

此外，在以下的各图中，为了容易理解物体检测装置的各个功能，而将物体检测装置划分为各个功能部进行示出，但应该理解为，以下所示的各个功能部的功能整体上都是由物体检测装置来实现的。

此外，只要能够实现物体检测装置的各个功能，则物体检测装置的各个功能既可以由硬件来实现，也可由软件（计算机程序）来实现。当然，也可以由硬件与软件的组合来实现。

作为具体的例子，物体检测装置例如由计算机构成。此时，物体检测装置例如是CPU（Central Processing Unit：中央处理单元）、MPU（Micro Processing Unit：微处理单元）、MCU（Mrico Control Unit：微控制单元）等。此外，物体检测装置的各个功能可通过把预先制作的程序加载到物体检测装置中进行执行来实现。

另外，所述程序可以记录在计算机可读记录介质中，并从该记录介质安装到计算机中，其中，所述计算机可读记录介质是记录有计算机可执行的多个命令的非一时性的记录介质。另外，作为计算机可读记录介质，例如有硬盘驱动器（HD）、软盘（FD）、压缩盘（CD）、磁光盘（MO）、存储卡等。

此外，物体检测装置可以由一个物理实体来实现，也可由多个物理实体来实现。图1A和图1B是示出实施方式的物体检测方法的说明图。如图1A所示，在实施方式的物体检测方法中，取得表示模型M的表面形状的信息，其中，模型M与检测对象的物体为相同形状，且在实空间中的位置和姿态是已知的。

具体而言，取得与位于模型M的表面的点群中的各点相关的信息。在与各点相关的信息中，包含各点在实空间中的3维位置信息（位置矢量）和各点相对于模型M的表面的法线矢量的信息。

接下来，在模型M的外部空间中，设定多个外部基准点O₁、O₂、O₃作为物体的位置和姿态的估计素材，在模型M的内部空间中，设定内部基准点O₄作为物体的位置和姿态的估计素材的可否判定素材。此外，所设定的基准点的个数不限于此。

进而，对模型M的局部表面处的特征量进行计算，其中，所述局部表面包含从位于模型M的表面的点群中依次选择出的起点P_i与终点P_j的对。此处的特征量例如是起点P_i与终点P_j之间的距离、起点P_i与终点P_j的法线的内积、起点P_i的法线与连接起点P_i和终点P_j的矢量的内积等。

接下来，生成将计算出的特征量与外部基准点O₁、O₂、O₃和内部基准点O₄相对于起点P_i的位置矢量（参照从起点P_i朝向外部基准点O₁、O₂、O₃、和内部基准点O₄的箭头）的组关联的表，并进行存储。此外，到目前为止的步骤要预先在检测存在于实空间的物体的位置和姿态之前离线地执行。

然后，如图1B所示，取得表示存在于实空间的物体W的表面的表面形状的信息。此处，虽然省略了图示，但是在表示物体W的表面的表面形状的信息中，包含位于物体W表面的样本点群在实空间中的3维位置信息（位置矢量）。

接下来，从样本点群中依次选择起点P_（i）和终点P_（j）的对，计算物体W在包含起点P_（i）和终点P_（j）的对的局部表面中的特征量，并从先前存储的表中，取得与和物体W的特征量一致的特征量关联的多个位置矢量的组。

此处，如果所取得的位置矢量的起点P_i在模型M中的位置与从样本群中选择出的起点P_（i）在物体W中的位置一致，那么通过将位置矢量的组变换为实空间的位置矢量，能够取得物体W的正确的基准点位置。因此，根据所述变换后的3个点的位置矢量，能够估计并检测出物体W的正确的位置和姿态。

不过，从表中取得的位置矢量的起点P_i在模型M中的位置有时与从样本群中选择出的起点P_（i）在物体W中的位置不一致。例如，根据物体W的位置或姿态，即使起点P_（i）在物体W中的位置与起点P_i在模型M中的位置不同的情况下，物体W中的局部表面处的特征量与模型M中的局部表面处的特征量有时是一致的。

在该情况下，即使将从表中取得的位置矢量的组变换为实空间的位置矢量，也不能取得物体W的正确的基准点位置，不能检测物体W的正确的位置和姿态。

因此，在实施方式的物体检测方法中，利用霍夫变换，将从表中取得的多个位置矢量的组变换为实空间中的位置矢量的组，将变换后的位置矢量表示的3维位置投票到成为霍夫空间的实空间的投票点。进而，在每次以3个点进行投票的投票点的组中，将得票数多的投票点的组中包含的位置矢量判定为物体W的正确的基准点的位置，并根据该位置来判定物体W的位置和姿态。

但是，在将从表中取得的全部位置矢量的组作为投票对象的情况下，存在检测物体W的位置和姿态的处理量增多这样的问题。此外，如前面所述，在从表中取得的多个位置矢量的组中，还包含不适合作为物体W的位置和姿态的估计素材的位置矢量。

此外，根据物体W的位置或姿态等，存在不适合作为位置和姿态的估计素材的位置矢量的投票点有时比合适的位置矢量的投票点获得更多的票的情形，这有可能降低物体W的位置和姿态的检测精度。

因此，在实施方式的物体检测方法中，在进行投票之前，通过将不适合作为位置和姿态的估计素材的位置矢量的组从投票对象中排除，能够降低物体的位置和姿态的检测所需的处理量，提高检测精度。

具体而言，在将从表中取得的位置矢量的组变换为实空间的位置矢量的情况下，如图1B所示，假设对模型M设定的外部基准点O₁、O₂、O₃在实空间中位于A、B、C这3个点，而内部基准点O₄位于物体W外部的点D。

这样，在本来应该存在于物体W的内部的内部基准点O₄位于物体W外部的点D的情况下，将被包含在与该点D相同的组中的实空间中的A、B、C这3个点从投票对象、即物体W的位置和姿态的估计素材中排除。

因此，根据实施方式的物体检测方法，在进行投票之前，能够将不适合作为位置和姿态的估计素材的位置矢量的组从投票对象中排除，因此，能够降低物体W的位置和姿态的检测所需的处理量，提高检测精度。

此外，在与对模型M设定的内部基准点O₄对应的实空间的点D恰当地存在于物体W的内部的情况下，对与外部基准点O₁、O₂、O₃对应的实空间的A、B、C这3个点的位置进行投票，并根据投票结果，检测物体W的位置和姿态。此外，对于该物体检测方法的详细情况，后面将结合实施方式的物体检测装置的动作进行描述。

接下来，参照图2，对具有实施方式的物体检测装置的机器人系统进行说明。图2是示出实施方式的机器人系统1的说明图。如图2所示，机器人系统1具有机器人2、传感器3、物体检测装置4和机器人控制装置5。

机器人2具有设置于地面等的体部21以及从体部21延伸的右臂22和左臂23。右臂22和左臂23均是具有7轴自由度的机械臂。此外，在右臂22的前端设有把持箱体6的手24，其中，箱体6在内部散乱地堆积有圆筒状的物体W，在左臂23的前端设有从箱体6的内部中拾取物体W的手25。

传感器3是检测散乱地堆积在箱体6内部的物体W的3维形状的设备，传感器3例如是三维扫描器。该传感器3被支承体31支承而配置在箱体6的垂直上方。

传感器3利用激光扫描物体W，根据从物体W反射来的反射光，检测物体W的3维形状。进而，传感器3将表示物体W的3维形状的信息（以下，记作“场景数据”）输出到物体检测装置4。在场景数据中，包含位于上述物体W的表面的样本点群在实空间中的3维位置信息（位置矢量）。

物体检测装置4根据从传感器3输入的场景数据和上述表，检测散乱地堆积在箱体6内部的物体W的位置和姿态，将作为机器人2的把持对象的物体W的位置和姿态输出到机器人控制装置5。此外，后面将参照图3，对物体检测装置4的结构的一例进行描述。

机器人控制装置5根据从物体检测装置4输入的把持对象的物体W的位置和姿态，生成机器人2的控制信号，通过将控制信号输出到机器人2，使机器人2进行物体W的拾取动作。

接下来，参照图3，对物体检测装置4的结构的一例进行说明。图3是示出实施方式的物体检测装置4的结构的一例的说明图。如图3所示，物体检测装置4具有处理部7和存储部8。存储部8存储上述表81。此外，后面将参照图7，对表81的一例进行描述。

处理部7具有模型数据取得部71、场景数据取得部72、基准点设定部73、边缘检测部74、surflet选择部75、运算部76、位置/姿态估计部77以及把持对象信息生成部78。

在物体检测装置4中，通过模型数据取得部71、基准点设定部73、边缘检测部74、surflet选择部75和运算部76，生成上述表81，并将其存储于存储部8。

此处，配合参照图4A～图7，对基于物体检测装置4实现的表81的生成顺序和表81的一例进行说明。图4A～图6是示出实施方式的表81的生成顺序的说明图，图7是示出实施方式的表81的一例的说明图。

模型数据取得部71从规定的信息处理装置（省略图示）中读入尺寸和形状与作为机器人2的把持对象的物体W相等的图4A所示的模型M的CAD数据。进而，模型数据取得部71从CAD数据中提取并取得表示模型M的表面形状的模型数据。

具体而言，如图4B所示，模型数据取得部71取得位于模型M表面的点群中的各点在实空间中的3维位置信息（位置矢量）和各点相对于模型M的表面的法线矢量的信息等，作为模型数据。进而，模型数据取得部71将取得的模型数据输出到基准点设定部73。

如图4B所示，基准点设定部73针对模型数据，在模型M的外部空间中设定外部基准点O₁、O₂、O₃这3个点，并且，在模型M的内部空间中设定内部基准点O₄。此外，在不区分外部基准点O₁、O₂、O₃和内部基准点O₄的情况下，将它们统称为基准点O。该基准点设定部73将设定了基准点O的模型数据输出到边缘检测部74。

如图5A所示，边缘检测部74根据从基准点设定部73输入的模型数据，检测模型M的边缘Ed。此外，边缘检测部74对形成模型M的边缘Ed的点群附加表示是边缘Ed上的点的信息（以下，记作“标签E”）。此外，边缘检测部74对形成模型M的边缘Ed以外的部分的点群附加表示是表面上的点的信息（以下，记作“标签F”）。进而，边缘检测部74将附加有标签E和附加有标签F的模型数据输出到surflet选择部75。

从边缘检测部74输入的模型数据中，surflet选择部75依次选择位于模型M的表面的2个点的对作为起点P_i和终点P_j的对，并取得与选择出的点相关的信息（以下，记作“surflet”）。

例如，如图5B所示，在surflet选择部75选择了位于圆筒形状的模型M的圆周面的起点P_i的情况下，选择起点P_i在实空间中的位置矢量p_i、标签F、法线矢量n_i的组作为起点P_i的surfletΔ（p_i）。

此外，在surflet选择部75参照位于选择模型M的边缘Ed（图5A）的终点P_j的情况下，取得终点P_j在实空间中的位置矢量p_j、标签E、终点P_j的梯度矢量g_j作为终点P_j的surfletΔ（p_j）。

此处的梯度矢量g_j是模型M的周面与端面在终点P_j所成的角度。此外，在选择出表面上的点作为终点的情况下，surflet选择部75取得终点的位置矢量、标签F、终点的法线矢量的组，作为终点的surflet。

surflet选择部75对选择出的起点P_i和终点P_j进行变更，并取得多个surfletΔ（p_i）与surfletΔ（p_j）的对（以下，记作“surflet对”），输出到运算部76。

该surflet对是在运算部76对模型M的局部表面处的特征量进行运算的处理等中使用的信息，其中，所述局部表面包含起点P_i和终点P_j的对。因此，surflet选择部75可以构成为选择位于模型M的表面的全部的点作为起点P_i和终点P_j来取得surflet对，也可以构成为选择附加有标签E的点或者附加有标签F的点作为终点P_j。

如果构成为选择位于模型M的表面的全部的点作为起点P_i和终点P_j的话，由于能够在表81蓄积更多的模型M中的局部表面处的特征量，因此能够提高物体W的检测精度。

此外，在模型占据的平面部分的区域较大的情况下，限定于选择附加有标签E的点作为终点P_j的结构是有效的。具体而言，在平面部分占据的区域大的模型中，无论选择平面部分中的哪一个点作为起点P_i和终点P_j，计算出的局部表面处的特征量都没有大的区别，而在选择出边缘Ed部分作为终点P_j的情况下，可以得到更能够表现出模型的特征的特征量。而且，由于能够将附加有标签F的表面上的点从终点P_j的选择项中排除，因此能够减小计算特征量的处理量。

因此，根据限定于选择附加有标签E的点作为终点P_j的结构，对于平面部分占据的区域大的模型，能够降低处理量，提高物体的位置和姿态的检测精度。

另一方面，在模型占据的曲面部分的区域较大的情况下，限定于选择附加有标签F的点作为终点P_j的结构是有效的。具体而言，在曲面部分占据的区域大的模型中，在模型的面状的各个局部平面中有模型的特征。

因此，根据限定于选择附加有标签F的点作为终点P_j的结构，针对模型的面状中的各个局部平面取得特征量，由此，能够提高曲面部分占据的区域大的物体的位置和姿态的检测精度。

运算部76使用从surflet选择部75输入的surflet对，计算包含起点P_i和终点P_j的对的模型M的局部表面处的特征量。例如，运算部76计算起点P_i与终点P_j之间的距离、起点P_i与终点P_j的法线矢量n_i的内积、起点P_i的法线矢量n_i与连结起点P_i和终点P_j的矢量f_i，j的内积作为特征量。

此外，运算部76对以起点P_i为原点的局部坐标系进行计算，计算出局部坐标系中的基准点O的位置矢量（参照图5B所示的单点划线箭头）。在基准点O的位置矢量的计算中，使用了起点P_i的位置矢量p_i、终点P_j的位置矢量p_j以及位置矢量p_i与位置矢量p_j的差分矢量f_i，j。

具体而言，各基准点O在实空间中的位置矢量可由下式（1）表示。

[公式1]

$d_i^k=o_k-p_i(k=\mathrm{1,2,3,4})...\left(1\right)$

使用由<p_i，n_i，f_i，j>确定的局部坐标系来表示该实空间中的各基准点O的位置矢量。局部坐标系确定为如下这样。

[公式2]

$e_1^{(i,j)}=\frac{n_i}{\left|\right|n_i\left|\right|}...\left(2\right)$

$e_2^{(i,j)}=\frac{n_i\&times;f_{i,j}}{\left|\right|n_i\&times;f_{i,j}\left|\right|}...\left(3\right)$

$e_3^{i,j}=\frac{e_2^{(i,j)}\&times;n_i}{\left|\right|e_2^{(i,j)}\&times;n_i\left|\right|}...\left(4\right)$

使用根据上述式（2）、式（3）、式（4）而计算出的正交基，根据下式（5）来计算各基准点O在局部坐标系中的位置矢量。

[公式3]

$d_{i,j}^k=a_{i,j}^k{e}_1^{(i,j)}+b_{i,j}^k{e}_2^{(i,j)}+c_{i,j}^k{e}_3^{(i,j)}...\left(5\right)$

此处， $<a_{i,j}^k,b_{i,j}^k,c_{i,j}^k;k=\mathrm{1,2,3,4}>$ 的各项为标量，

如果确定了则能够根据式（5）计算出位置矢量

这样，如图6所示，计算出以起点P_i为原点的局部坐标系中的外部基准点O₁、O₂、O₃、和内部基准点O₄的各位置矢量（参照图6所示的单点划线箭头）。

接下来，如图7所示，运算部76生成表81，其中，表81是将包含先前计算出的起点P_i和终点P_j的对的局部表面处的特征量（H₁，H₂等）与基准点O在以起点P_i原点的局部坐标系中的位置矢量（此处为外部基准点O₁、O₂、O₃、和内部基准点O₄的4个位置矢量）的组关联的表。

此处，在模型M中，存在特征量一致的多个局部表面。因此，在表81中，针对1个特征量（H₁，H₂等），分别与局部坐标系中的多个基准点O的组关联起来。即，表81成为以各特征量（H₁，H₂等）为键的哈希表的数据结构。运算部76将这样形成的表81存储在存储部8中。

此外，在物体检测装置4中，通过场景数据取得部72、边缘检测部74、surflet选择部75、运算部76、位置/姿态估计部77、把持对象信息生成部78，来估计和检测物体W的位置和姿态。

此处，配合参照图8A～图10，对物体检测装置4检测物体W的位置和姿态的顺序的一例进行说明。图8A～图10是示出检测实施方式的物体W的位置和姿态的顺序的说明图。

场景数据取得部72从传感器3取得表示图8A所示的散乱地堆积的物体W的3维形状的场景数据。具体而言，如图8B所示，场景数据取得部72取得位于物体W表面的样本点群中的各点在实空间中的3维位置信息（位置矢量）和各点相对于物体W的表面的法线矢量n_（i）的信息等，作为场景数据。进而，场景数据取得部72将取得的场景数据输出到边缘检测部74。

边缘检测部74根据从场景数据取得部72输入的场景数据，检测物体W的边缘。此外，边缘检测部74对形成物体W的边缘的样本点群附加上表示是边缘上的点的标签E，并计算出附加有标签E的各样本点的梯度矢量g_（j）。

此外，边缘检测部74针对形成物体W的边缘以外的部分的样本点群，附加上表示是表面上的点的标签F。进而，边缘检测部74将附加有标签E和标签F的模型数据输出到surflet选择部75。此外，边缘检测部74还将边缘上的样本点的梯度矢量g_（j）输出到surflet选择部75。

surflet选择部75在从边缘检测部74输入的场景数据中，依次选择位于物体W的表面的2个点的对作为起点P_（i）和终点P_（j）的对，并取得与选择出的样本点相关的surflet。

例如，surflet选择部75在选择出位于圆筒形状的物体W的周面的起点P_（i）的情况下，取得起点P_（i）在实空间中的位置矢量p（i）、标签F、法线矢量n_（i）的组，作为起点P_（i）的surfletΔ（p_i）。

此外，surflet选择部75在选择出位于物体W的边缘的终点P_（j）的情况下，取得终点P_（j）在实空间中的位置矢量p（j）、标签E、终点P_（j）的梯度矢量g_（j），作为终点P_（j）的surflet（（p_j）。

此处的梯度矢量g_（j）是物体W的周面与端面在终点P_（j）所成的角度。此外，在选择出表面上的点作为终点的情况下，surflet选择部75取得终点的位置矢量、标签F、终点的法线矢量的组，作为终点的surflet。

surflet选择部75对选择出的起点P_（i）和终点P_（j）进行变更，取得多个作为surfletΔ（p_i）与surfletΔ（p_j）的对的surflet对，并输出到运算部76。

该surflet对是运算部76在对物体W的局部表面处的特征量进行运算的处理等中使用的信息，其中，所述局部表面包含起点P_（i）和终点P_（j）的对。因此，与取得模型M的surflet对的情况同样地，通过surflet选择部75，选择全部的点作为起点P_（i）和终点P_（j）来取得surflet对，由此，能够提高物体W的检测精度。

此外，通过surflet选择部75，限定于选择附加有标签E的终点P_j，由此，对于平面部分占据的区域大的物体，能够降低处理量，提高物体的位置和姿态的检测精度。另一方面，通过surflet选择部75，限定于选择附加有标签F的终点P_j，由此，对于曲面部分占据的区域大的物体，能够提高位置和姿态的检测精度。

运算部76使用从surflet选择部75输入的surflet对，计算包含起点P_（i）和终点P_（j）的对的物体W的局部表面处的特征量。例如，运算部76计算起点P_（i）与终点P_（j）之间的距离、起点P_（i）与终点P_（j）的法线矢量n_（i）的内积、起点P_（i）的法线矢量n_（i）与连结起点P_（i）和终点P_（j）的矢量f_（i，j）的内积作为特征量。进而，运算部76将在特征量的计算中使用的surflet对和计算出的物体W的局部表面处的特征量输出到位置/姿态估计部77。

位置/姿态估计部77从表81取得多个与和从运算部76输入的物体W的局部表面处的特征量一致的特征量关联的基准点O的位置矢量的组。此外，位置/姿态估计部77计算出以起点P_（i）为原点的局部坐标系。进而，位置/姿态估计部77根据下式（6），将从表81取得的位置矢量的组变换为以起点P_（i）为原点的局部坐标系中的位置矢量的组。

[公式4]

${\hat{d}}_{i,j}^k=a_{i,j}^k{\hat{e}}_1^{(i,j)}+b_{i,j}^k{\hat{e}}_2^{(i,j)}+c_{i,j}^k{\hat{e}}_3^{(i,j)}...\left(6\right)$

此处，根据n_（i）、f_（i，j），

由式（2）～（4）求出。

接下来，位置/姿态估计部77根据下式（7），将以起点P_（i）为原点的局部坐标系中的变换后的位置矢量的组变换为实空间中的位置矢量的组。

[公式5]

${\hat{q}}_{i,j}^k=p_{\left(i\right)}+{\hat{d}}_{i,j}^k(k=\mathrm{1,2,3,4})...\left(7\right)$

由此，如图8B中单点划线所示，计算出外部基准点O₅、O₆、O₇和内部基准点O8在实空间中的位置矢量。此处，如图9所示，位置/姿态估计部77根据黑圈所示的物体W表面的样本点与白圈所示的内部基准点O8的位置关系，来判定外部基准点O₅、O₆、O₇的可否。此外，图9所示的X轴和Y轴是位于与水平面平行的平面上的正交矢量，Z轴是与XY平面正交、且以垂直下方为正方向的轴。

进而，如图9所示，在内部基准点O8位于物体W的内部的情况下，位置/姿态估计部77判定包含在与内部基准点O8的位置矢量相同组中的外部基准点O₅、O₆、O₇的位置矢量是适合作为物体W的位置和姿态的估计素材的基准点。

另一方面，在如内部基准点O9那样位于物体W的外部的情况下，位置/姿态估计部77判定被包含在与内部基准点O9的位置矢量相同的组中的外部基准点是不适合作为物体W的位置和姿态的估计素材的基准点，并将其从估计素材中排除。

具体而言，在位置/姿态估计部77中，设内部基准点O8在实空间中的坐标值为（x_k，y_k，z_k），根据下述式（8），验证内部基准点O8是否位于物体W的内部，来判定外部基准点O5、O6、O7的可否。

[公式6]

$\underset{k=1}{\overset{s}{\&cap;}}(z_k-\mathrm{scanZ}(x_k,y_k\&GreaterEqual;-\mathrm{th}))=\mathrm{true}...\left(8\right)$

其中，上式（8）中的scanZ（x，y）是实空间中的x坐标值和y坐标值与内部基准点O8的x坐标值和y坐标值一致的样本点的z坐标值。

这样，位置/姿态估计部77将不适合作为物体W的位置和姿态的估计素材的基准点O从物体W的位置和姿态的估计素材中排除。由此，位置/姿态估计部77能够降低后面进行外部基准点O₅、O₆、O₇的投票处理量，提高物体W的位置和姿态的估计精度。

此外，在本实施方式中，将内部基准点O₄设定在模型M的内部空间中，但是，只要是在模型M的内部空间以外即可，例如可以将内部基准点O₄设定在模型M的表面。不过，在该情况下，如果将与内部基准点O₄对应的内部基准点O₁₀是否位于物体W的表面作为外部基准点O₅、O₆、O₇的可否判定基准，则可否判定基准过高，有可能将合适的基准点O错误判定为不合适。

因此，在位置/姿态估计部77中，对上述式（8）设置规定的阈值th，使外部基准点O₅、O₆、O₇的可否判定基准具有一定的范围，由此抑制发生将合适的基准点错误判定为不合适的情况。

接下来，位置/姿态估计部77进行如下处理：分别投票到与判定为适合作为物体W的位置和姿态的判定素材的外部基准点O₅、O₆、O₇在实空间中的位置一致的实空间内的投票点。

例如，位置/姿态估计部77将图10的上图所示的、外部基准点O₅、O₆、O₇在以起点P_（i）为原点的局部坐标系中的位置分别投票到图10的下图所示的实空间中的对应的3个点A₁、B₁、C₁。位置/姿态估计部77对判定为适合作为估计素材的全部外部基准点，依次进行对实空间的投票。

由此，例如，图10所示的3个点A₁、B₁、C₁的投票点的组的得票数逐渐增加。此外，在实空间中，对于3个点A₁、B₁、C₁以外的投票点，也按照3个点的外部基准点为一组，分别对实空间内的其它3个投票点的组依次进行投票。位置/姿态估计部77针对各3个投票点的组中的每一组，监视各投票点的得票数。

此处，例如，在对9维空间内的投票点进行投票的情况下，根据得票数最多的投票点，能够检测出最正确的外部基准点，其中，在所述9维空间中，由3个点的外部基准点的组中包含的各外部基准点的各x、y、x坐标值、即9个坐标值确定1个点。但是，在该情况下，投票空间变得庞大，投票所需的计算量也变得庞大。

因此，位置/姿态估计部77将3个点的外部基准点的组中包含的各外部基准点的位置分别投票到3维实空间中的独立的3投票点的组。由此，能够大幅降低投票所需的计算量。不过，在位置/姿态估计部77中，产生如下所谓干扰现象：在本来应该投票到同一个3投票点的组的3个票中，某一个票被投票到其他投票点的组。

因此，在3投票点的组中包含的各投票点中，投票数最小的投票点的得票数达到规定的阈值Vth的情况下，位置/姿态估计部77列出该3投票点的组。进而，在位置/姿态估计部77中，对列出的每一3个投票点的组汇总出总得票数，按照得票数多到少的组的顺序，根据该组中包含的投票点在实空间中的位置，来估计物体W的位置和姿态。

例如，在图10所示的3个点A₁、B₁、C₁的组中点A₁的得票数最少且点A₁的得票数达到阈值Vth的情况下，位置/姿态估计部77列出3个点A₁、B₁、C₁的组。由此，例如，即使由于干扰而使B₁、C₁这2个点的得票数增加，只要点A₁的得票数没有达到阈值Vth，则能够抑制过早地将尚未确定是否合适的3个点A₁、B₁、C₁的组判定为和适的得票点的组。

接下来，位置/姿态估计部77将表示估计出的多个物体W的位置和姿态的信息输出到把持对象信息生成部78。把持对象信息生成部78根据从位置/姿态估计部77输入的信息，判定作为机器人2的把持候选的物体W的有无，在存在把持候选的情况下，向机器人控制装置5输出表示把持候选的位置和姿态的信息，在不存在把持候选的情况下，向机器人控制装置5输出表示该情况的信息。

接下来，参照图11～图13，对物体检测装置4的处理部7执行的处理进行说明。图11是示出实施方式的表81的生成处理的流程图。图12是示出实施方式的物体W的位置和姿态的检测处理的流程图。图13是示出与实施方式的把持候选相关的信息的生成处理的流程图。

物体检测装置4的处理部7在检测物体W的位置和姿态以前，通过预先执行图11所示的处理来生成表81。具体而言，如图11所示，处理部7首先取得模型数据（步骤S101），并从模型数据提取模型M的边缘Ed（步骤S102）。

接下来，处理部7将与从模型M的表面选择的出起点P_i在模型M表面中的位置对应的变量i设定为1（步骤S103）。然后，处理部7将与从模型M的表面选择出的终点P_j在模型M表面中的位置对应的变量j设定为1（步骤S104）。

接下来，处理部7判定变量i的值与变量j的值是否不相等（步骤S105）。即，处理部7判定从模型M的表面选择出的起点P_i与终点P_j是否是同一点。此外，此处，变量i的值和变量j的值均为1。因此，处理部7判定为起点P_i和终点P_j是同一点（步骤S105：否），将处理转到步骤S109。

在步骤S109中，处理部7将处理转到变量j的循环的起始处、即转到步骤S104。在步骤S104中，处理部7将变量j的值加1，将处理转到步骤S105。以后，处理部7每当处理运行到步骤S109时，在步骤S104中将变量j的值加1，在变量j的值达到M_j后，使处理运行到步骤S109时，将处理转到步骤S110。即，处理部7从模型M的表面选择出M_j个终点P_j。

此外，在步骤S105中判定为变量i的值与变量j的值不相等的情况下（步骤S105：是），处理部7将处理转到步骤S106。在步骤S106中，处理部7根据起点P_i的位置矢量p_i、起点P_i的法线矢量n_i和终点P_j的位置矢量p_j，计算以起点P_i为原点的局部坐标系。

接下来，处理部7对计算出的局部坐标系中的各基准点O的位置矢量进行计算（步骤S107）。此处，处理部7还计算surflet对<Δ（p_j），Δ（p_i）>的特征量，其中，surflet对是包含起点P_i和终点P_j的模型M的局部表面处的特征量。

接下来，处理部7将计算出的基准点O的位置矢量存储在以surflet对<Δ（p_j），Δ（_pi）>的特征量为键的表81中（步骤S108），将处理转到步骤S109。进而，处理部7针对1个起点P_i和M_j个终点P_j，反复步骤S104～步骤S109的处理，然后将处理转到步骤S110。

在步骤S110中，处理部7将处理转到变量i的循环的起始处，即，转到步骤S103。在步骤S103中，处理部7将变量i的值加1，将处理转到步骤S104。以后，处理部7每当处理运行到步骤S110时，在步骤S103中将变量i的值加1，在变量i的值达到M_i后，当处理运行到步骤S110时，将处理转到步骤S111。

即，处理部7从模型M的表面选择M_i个起点P_i，针对M_i个各起点P_i和M_j个终点P_j，反复步骤S103～步骤S110的处理，然后将处理转到步骤S111。最后，处理部7将生成的表81保存在存储部8中，结束处理。

此外，可以通过物体检测装置4以外的装置来生成表81。在该情况下，物体检测装置4从其它装置取得生成的表81，并保存在存储部8中。

此外，处理部7通过执行图12所示的处理，来进行检测物体W的位置和姿态的表面匹配。具体而言，如图12所示，处理部7首先从传感器3取得散乱地堆积的物体W的场景数据（步骤S201），根据场景数据提取物体W的边缘（步骤S202）。

接下来，处理部7根据场景数据计算各样本点的法线矢量。此处，处理部7针对形成边缘的样本点，计算梯度矢量（步骤S203）。接下来，处理部7将与从物体W的表面选择出的起点P_（i）在物体W表面中的位置对应的变量i设定为1（步骤S204），并根据场景数据取得起点P_（i）的surfletΔ（p_i）（步骤S205）。

然后，处理部7将与从物体W的表面选择出的终点P_（j）在物体W表面中的位置对应的变量j设定为1（步骤S206），并从场景数据中取得终点P_（j）的surfletΔ（p_j）（步骤S207）。

接下来，处理部7判定所取得的surflet对<Δ（p_j），Δ（p_i）>是否满足制约条件（步骤S208）。此处，在例如起点P_（i）和终点P_（j）均为表面上的点、且起点P_（i）的法线矢量n_（i）与终点P_（j）的法线矢量n（j）所成的角度大于规定的阈值T_f的情况下，处理部7判定为满足制约条件。另一方面，在起点P_（i）的法线矢量n_（i）与终点P_（j）的法线矢量n（j）所成的角度为规定的阈值T_f以下的情况下，处理部7判定为不满足制约条件。

此外，例如，在起点P_（i）为表面上的点、终点P_（j）为边缘上的点、且起点P_（i）的法线矢量n_（i）与终点P_（j)的梯度矢量g_（j）所成的角度大于规定的阈值T_e的情况下，处理部7判定为满足制约条件。另一方面，在起点P_（i）的法线矢量n_（i）与终点P_（j）的梯度矢量g_（j）所成的角度为规定的阈值T_e以下的情况下，处理部7判定为不满足制约条件。

通过设置该制约条件，能够防止处理部7针对难以表现出明显特征的起点P_（i）和P_（j）的对，无用地取得surflet对<Δ_（p_j），Δ（p_i）>，因此能够减小处理量。

进而，处理部7在判定为取得的surflet对<Δ（p_j），Δ（p_i）>满足制约条件的情况下（步骤208：是），将处理转到步骤S209。另一方面，在判定为不满足制约条件的情况下（步骤S208：否），处理部7将处理转到步骤S218。

在步骤S209中，处理部7从表81取得特征量与在步骤S205和步骤S207取得的surflet对<Δ（p_j），Δ（p_i）>一致的全部surflet对（Nv个）的基准点O的位置矢量的组。

接下来，处理部7计算在步骤S205和步骤S207中取得的surflet对<Δ（p_j），Δ（p_i）>的局部坐标系（步骤S210）。即，处理部7根据surflet对<Δ（p_j），Δ（p_i）>，计算以起点P_（i）为原点的局部坐标系。

然后，处理部7将表示要在步骤S209中取得的位置矢量的组的取得顺序的变量r设定为1（步骤S211）。接下来，处理部7根据在步骤S210中计算出的局部坐标系和第r个基准点O的位置矢量的组，计算各基准点O在实空间中的位置矢量的组（步骤S212）。

此处，处理部7判定内部基准点O8是否位于物体W的内部（步骤S213）。进而，处理部7在判定为内部基准点O8位于物体W的内部的情况下（步骤S213：是），将处理转到步骤S214。另一方面，在判定为内部基准点O8位于物体W的外部的情况下（步骤S213：否），将处理转到步骤S217。

在步骤S214中，处理部7进行对实空间内的投票点的投票，其中，所述实空间内的投票点与如下的外部基准点O₅、O₆、O₇的位置矢量一致：被包含在与在步骤S213中判定为位于物体W的内部的内部基准点O8相同的组中。进而，处理部7在步骤S214中进行投票的3个点的投票点的组中，进行投票数最小的投票点的得票数是否达到阈值Vth的判定（步骤S215）。

此处，处理部7在判定为达到阈值Vth的情况下（步骤S215：是），将处理转到步骤S216，列出在步骤S214中投票的3个点的投票点的组（步骤S216），将处理转到步骤S217。另一方面，在判定为没有达到阈值Vth的情况下（步骤215：否），将处理转到步骤S217。

在步骤S217中，处理部7将处理转到变量r的循环的起始处，即，转到步骤S211。在步骤S211中，处理部7将变量r的值加1。以后，处理部7在每当处理运行到步骤S217时，在步骤S211中将变量r的值加1，在变量r的值达到Nv后，当处理运行到步骤S217时，将处理转到步骤S218。即，处理部7针对在步骤S209中取得的基准点O的位置矢量的全部组，进行步骤S212～步骤S216的处理。

在步骤S218中，处理部7将处理转到变量j的循环的起始处，即、转到步骤S206。在步骤S206中，处理部7将变量j的值加1。以后，处理部7在每当处理运行到步骤S218时，在步骤S206中将变量j的值加1，在变量j的值达到Nh后，当处理运行到步骤S218时，将处理转到步骤S219。

在步骤S219中，处理部7将处理转到变量i的循环的起始处，即、转到步骤S204。在步骤S204中，处理部7将变量i的值加1。以后，处理部7每当处理运行到步骤S219时，在步骤S204中将变量i的值加1，在变量i的值达到Nt后，当处理运行到步骤S219时，将处理转到步骤S220。

在步骤S220中，处理部7对列出的每一投票点的组汇总出总得票数，按照总得票数高到低的顺序对投票点的组进行排序。接下来，处理部7根据总得票数高的前G个投票点的组来计算物体W的位置和姿态，（步骤S221），结束处理。

此外，处理部7通过执行图13所示的处理，生成与作为机器人2的把持候选的物体W相关的信息，并输出到机器人控制装置5。具体而言，如图13所示，处理部7首先测量场景数据（步骤S301）。

接下来，通过图12所示的处理（表面匹配），检测计算出的G个把持候选（步骤S302）。进而，处理部7将表示对把持候选进行后述的详细匹配的顺序的变量t设定为1（步骤S303），将处理转到步骤S304。在步骤S304中，处理部7判定变量t的值是否小于G（步骤S304）。

此处，处理部7在判定为变量t的值小于G的情况下（步骤S304：是），将处理转到步骤S305。另一方面，在判定为变量t的值已达到G的情况下（步骤S304：否），对机器人控制装置5设定无把持候选标志（步骤S310），结束处理。

在步骤S305中，处理部7对第t个把持候选进行详细匹配。例如，处理部7进行基于ICP（Iterative Closest Point：迭代最近点）算法的详细模式匹配作为详细匹配。进而，处理部7判定详细匹配是否成功（步骤S306）。

在步骤S306中，处理部7在判定为详细匹配成功的情况下（步骤S306：是），将处理转到步骤S307。另一方面，在判定为详细匹配失败的情况下（步骤S306：否），将处理转到步骤S311。

在步骤S307中，处理部7进行如下手干扰检查：在机器人2对把持候选进行拾取动作时，手25是否干扰。进而，处理部7判定是否干扰（步骤S308），在判定为不干扰的情况下（步骤S308：是），将处理转到步骤S309。

另一方面，在判定为存在干扰的情况下（步骤S308：否），将处理转到步骤S311。在步骤S311中，处理部7将变量t的值加1，将处理转到步骤S304。此外，在步骤S309中，处理部7将在步骤S308中判定为没有干扰的把持候选的位置和姿态（拾取位置和姿态）设定到机器人控制装置5中，结束处理。

如上所述，在实施方式的物体检测方法中，在物体的模型的外部空间中，设定作为物体的位置和姿态的估计素材的多个外部基准点，在模型的内部空间中，设定作为估计素材的可否判定素材的内部基准点。进而，存储将如下二者关联起来而成的表：包含从位于模型的表面的点群中依次选择出的起点和终点的对的局部表面处的特征量，和起点对应的外部基准点和内部基准点的位置的组。

此外，在实施方式的物体检测方法中，从位于实空间中存在的物体的表面的样本点群中，依次选择起点和终点的对，计算包含起点和终点的对的局部表面中的物体的特征量。

进而，从表中取得与和物体的特征量一致的特征量关联的位置的组，并将其转换到实空间中的位置的组，在位置的组中包含的内部基准点的位置位于物体的外部的情况下，将位置的组中包含的外部基准点的位置从估计素材中排除，来估计物体的位置和姿态。根据该物体检测方法，能够降低物体的位置和姿态的检测所需的处理量，提高检测精度。

此外，在实施方式的物体检测方法中，在变换后的位置的组中包含的内部基准点的位置位于物体的内部的情况下，分别投票到实空间内的与位置的组中包含的多个外部基准点的位置一致的投票点。进而，在根据1个位置的组中包含的多个外部基准点的位置而进行投票的投票点的组中，在投票数最小的投票点的得票数达到规定的阈值的情况下，列出投票点的组。

然后，对列出的每一投票点的组汇总出总得票数，按照总得票数多到少的组的顺序，根据组中包含的投票点在实空间中的位置，估计物体的位置和姿态。由此，即使产生投票到本来不应该投票的投票点的所谓干扰，也能够抑制与物体的位置和姿态相关的估计精度的下降。

此外，在实施方式的物体检测方法中，检测物体和模型的边缘，将位于模型的表面的点群和位于物体的表面的样本点群分类为边缘上的点和表面上的点。

进而，根据物体的形状，从边缘上的点和表面上的点中选择出作为起点而选择的点和作为终点而选择的点。由此，在实施方式的物体检测方法中，由于能够根据与物体的形状对应的、更具有特征的局部表面处的特征量，来估计物体的位置和姿态，因此能够提高通用性。

此外，在实施方式的物体检测方法中，选择表面上的点作为从样本点中选择出的起点，选择边缘上的点作为从样本点中选择出的终点，由此，能够提高物体所占据的平面部分的面积较大的物体的检测精度。

此外，在实施方式的物体检测方法中，选择起点处的法线矢量与终点的法线矢量或者梯度矢量所成的角度大于规定的阈值的2个点，作为从样本点中选择出的起点和终点。由此，能够将物体的相对没有特征的部分的局部表面从位置和姿态的估计素材中排除，因此能够减小位置和姿态的检测所需的处理量。

对本领域技术人员而言，能够容易地导出进一步的效果或变形例。因此，本发明的更广泛的方式不限定于如上所表达和记述的特定的详细和代表性的实施方式限定。因此，在不脱离由所附权利要求的范围及其均等物所定义的总体发明概念精神或者范围的情况下，可以进行各种变更。

Claims (8)

1.一种物体检测方法，其特征在于，

该物体检测方法包含如下步骤：

在物体的模型的外部空间中，设定作为所述物体的位置和姿态的估计素材的多个外部基准点，在所述模型的内部空间中，设定作为判定素材的内部基准点，所述判定素材判定所述外部基准点是否适合作为所述估计素材；

存储将如下二者关联起来而成的表：包含从位于所述模型的表面的点群中依次选择出的起点和终点的对的局部表面的特征量，和所述起点对应的所述外部基准点和所述内部基准点的位置的组；

从位于实空间中存在的物体的表面的样本点群中，依次选择起点和终点的对，计算所述物体在包含该起点和终点的对的局部表面处的特征量；以及

从所述表中取得与和所述物体的特征量一致的特征量关联的所述位置的组，并将其变换为实空间中的位置的组，在所述实空间中的位置的组中包含的所述内部基准点的位置位于所述物体的外部的情况下，将所述实空间中的位置的组中包含的所述外部基准点的位置从所述估计素材中排除，来估计所述物体的位置和姿态。

2.根据权利要求1所述的物体检测方法，其特征在于，所述物体检测方法还包含如下步骤：

检测所述物体和所述模型的边缘；

将位于所述模型的表面的点群和位于所述物体的表面的所述样本点群分类为所述边缘上的点和表面上的点；以及

根据所述物体的形状，从所述边缘上的点和所述表面上的点中，选择出作为所述起点而选择的点和作为所述终点而选择的点。

3.根据权利要求2所述的物体检测方法，其特征在于，所述物体检测方法还包含如下步骤：

选择所述表面上的点作为从所述样本点中选择出的所述起点；以及

选择所述边缘上的点作为从所述样本点中选择出的所述终点。

4.根据权利要求1所述的物体检测方法，其特征在于，所述物体检测方法还包含如下步骤：

在所述变换后的位置的组中包含的所述内部基准点的位置位于物体的内部的情况下，分别投票到与所述变换后的位置的组中包含的多个所述外部基准点的位置一致的实空间内的投票点；

在根据1个所述位置的组中包含的多个所述外部基准点的位置而进行了投票的投票点的组中，在投票数最小的投票点的得票数达到规定的阈值的情况下，列出该投票点的组；以及

对列出的每一所述投票点的组，汇总出总得票数，按照所述总得票数多到少的组的顺序，根据所述投票点的组中包含的所述投票点在实空间中的位置，来估计所述物体的位置和姿态。

5.根据权利要求4所述的物体检测方法，其特征在于，所述物体检测方法还包含如下步骤：

检测所述物体和所述模型的边缘；

将位于所述模型的表面的点群和位于所述物体的表面的所述样本点群分类为所述边缘上的点和表面上的点；以及

根据所述物体的形状，从所述边缘上的点和所述表面上的点中，选择出作为所述起点而选择的点和作为所述终点而选择的点。

6.根据权利要求5所述的物体检测方法，其特征在于，所述物体检测方法还包含如下步骤：

选择所述表面上的点作为从所述样本点中选择出的所述起点；以及

选择所述边缘上的点作为从所述样本点中选择出的所述终点。

7.根据权利要求1～6中的任意一项所述的物体检测方法，其特征在于，所述物体检测方法还包含如下步骤：

选择所述起点处的法线矢量与所述终点处的法线矢量或者梯度矢量所成的角度大于规定的阈值的2个点，作为从所述样本点中选择出的所述起点和所述终点。

8.一种物体检测装置，其特征在于，所述物体检测装置具有：

设定部，其在物体的模型的外部空间中，设定作为所述物体的位置和姿态的估计素材的多个外部基准点，在所述模型的内部空间中，设定作为判定素材的内部基准点，所述判定素材判定所述外部基准点是否适合作为所述估计素材；

存储部，其存储将如下二者关联起来而成的表：包含从位于所述模型的表面的点群中依次选择出的起点和终点的对的局部表面的特征量，和所述起点对应的所述外部基准点和所述内部基准点的位置的组；

运算部，其从位于实空间中存在的物体的表面的样本点群中，依次选择起点和终点的对，计算所述物体在包含该起点和终点的对的局部表面处的特征量；以及

估计部，其从所述表中取得与和所述物体的特征量一致的特征量关联的所述位置的组，并将其变换为实空间中的位置的组，在所述实空间中的位置的组中包含的所述内部基准点的位置位于所述物体的外部的情况下，将所述实空间中的位置的组中包含的所述外部基准点的位置从所述估计素材中排除，来估计所述物体的位置和姿态。