CN106514712B - 物体的姿势计算系统 - Google Patents

Abstract

提供一种计算一方的物体与另一方的物体的相对姿势或者相对的位置和姿势的物体的姿势计算系统。该系统具备：能够以3个接触点接触的一方的物体和另一方的物体；驱动部，其使一方的物体与另一方的物体接触；力测量部，其测量作用于一方的物体与另一方的物体之间的力；以及物体姿势计算部，其基于一方的物体与另一方的物体以3个接触点接触时由力测量部测量出的力，来计算一方的物体与另一方的物体的相对姿势或者相对的位置和姿势。

Description

物体的姿势计算系统

技术领域

本发明涉及一种物体的姿势计算系统，该物体的姿势计算系统在一方的物体与另一方的物体以3个接触点接触时，基于作用于一方的物体与另一方的物体之间的力，来计算一方的物体与另一方的物体的相对姿势或者相对的位置和姿势。

背景技术

一般来说，在计算一方的物体与另一方的物体的相对姿势时，已知使用照相机、三维传感器等非接触式的传感器来测定、计算相对姿势的方法。

另外，一般来说，在能够对作用于一方的物体与另一方的物体之间的力进行控制的系统的情况下，已知如下方法：使一方的物体的规定的部分与另一方的物体的规定的部分进行面对齐，或者使一方的物体嵌合于另一方的物体，由此根据动作开始时的姿势和动作结束时的姿势来计算一方的物体与另一方的物体的相对姿势。

日本特公平04-043744号公报中公开了以下方法：基于柔顺(compliance)装置的运动模型来控制一方的物体的位置，使得作用于一方的物体与另一方的物体之间的力成为目标值。

在一方的物体和另一方的物体是能够嵌合的物体的情况下，能够通过这种方法，基于作用于一方的物体与另一方的物体之间的力使一方与另一方的物体相嵌合，根据动作开始时的姿势和动作结束时的姿势来计算一方的物体与另一方的物体的相对姿势。

另外，日本特开平09-091026号公报中记载了以下方法：为了对接近点的姿势误差进行修正，预先以规定的力按压与孔部分正交的平面来进行面接触，从而进行面对齐，由此计算姿势误差。

在该方法中，对作用于一方的物体与另一方的物体之间的力进行控制，来使一方的物体与另一方的物体进行面对齐，根据一方的物体的动作开始时的姿势和动作结束时的姿势来计算一方的物体与另一方的物体的相对姿势。并且，还公开了以下方法：基于计算出的所述相对姿势，对嵌合作业之前的接近点进行修正。

在使用照相机、三维传感器等非接触式的传感器来测定、计算一方的物体与另一方的物体的相对姿势的情况下，存在系统变得复杂或成本变高的情况。另外，根据一方的物体和另一方的物体的状态，例如根据一方的物体与另一方的物体的位置关系、与周边物体的干扰状况、一方的物体与另一方的物体的表面形状等状况，还存在难以计算一方的物体与另一方的物体的相对姿势的情况。

在使用能够对作用于一方的物体与另一方的物体之间的力进行控制的系统并根据使一方的物体嵌合于另一方的物体时的动作开始时的姿势和动作结束时的姿势来计算一方的物体与另一方的物体的相对姿势的情况下，嵌合作业耗费时间，系统的循环时间增大。另外，一方的物体和另一方的物体必须是能够相嵌合的物体。另外，必须使一方的物体移动以进行嵌合作业。另外，有时伴随嵌合作业时的一方的物体的移动而产生与周边物体发生干扰等问题。另外，只要一方的物体与另一方的物体的嵌合没有正常地完成，就无法计算一方的物体与另一方的物体的相对姿势。并且，在一方的物体与另一方的物体的相对姿势大的情况下，很多时候难以进行嵌合。

在使用能够对作用于一方的物体与另一方的物体之间的力进行控制的系统、根据使一方的物体的面与另一方的物体的面对齐时的动作开始时的姿势和动作结束时的姿势来计算一方的物体与另一方的物体的相对姿势的情况下，使面对齐的作业耗费时间，系统的循环时间增大。另外，一方的物体和另一方的物体必须是能够使面对齐的物体。另外，必须使一方的物体移动以进行面对齐作业。另外，有时伴随面对齐作业时的一方的物体的移动而产生与周边物体发生干扰等问题。

发明内容

因此，本发明的目的在于提供一种适当地计算一方的物体与另一方的物体的相对姿势或者相对的位置和姿势的系统。

为了达到上述目的，本申请第一发明提供一种物体的姿势计算系统，该物体的姿势计算系统计算一方的物体与另一方的物体的相对姿势、或者计算所述一方的物体与所述另一方的物体的相对的位置和姿势，该物体的姿势计算系统的特征在于，具备：所述一方的物体和所述另一方的物体，所述一方的物体和所述另一方的物体是包含凸部的物体与包含凹部的物体的组合、且是所述凸部与所述凹部能够在3个接触点接触的物体，并且，所述凹部处的所述3个接触点位于所述凹部的孔的端部，并且，当将针对所述凸部的、不与包含所述3个接触点的平面正交的轴作为凸部中心轴时，所述接触点与所述凸部中心轴的各最短距离为固定值，或者，所述接触点与所述凸部中心轴的各最短距离基于从所述接触点对所述凸部中心轴引垂线时的该垂线同所述凸部中心轴的交点与所述凸部中心轴上的所述交点以外的点的距离而确定；一个以上的驱动部，所述驱动部使所述一方的物体与所述另一方的物体相对地移动，并且，保持所述一方的物体和所述另一方的物体中的至少1个物体来使其移动；力测量部，其测量作用于所述一方的物体与所述另一方的物体之间的力；以及物体姿势计算部，其通过所述驱动部使所述一方的物体与所述另一方的物体相对地移动来使所述一方的物体与所述另一方的物体接触，基于所述一方的物体与所述另一方的物体以所述3个接触点接触时由所述力测量部测量出的力以及所述最短距离，来计算所述一方的物体与所述另一方的物体的相对姿势、或者计算所述一方的物体与所述另一方的物体的相对的位置和姿势。

第二发明提供如下一种物体的姿势计算系统：在第一发明中，所述一方的物体包含2个以上的所述凸部或所述凹部，或者，所述另一方的物体包含2个以上的所述凸部或所述凹部，或者，所述一方的物体和所述另一方的物体包含2个以上的所述凸部或所述凹部。

第三发明提供如下一种物体的姿势计算系统：在第一发明或第二发明中，所述一方的物体或者所述另一方的物体所包含的所述凸部和所述凹部中的至少1个物体是能够相对于包含所述至少1个物体的所述一方的物体或者所述另一方的物体拆装的物体。

第四发明提供如下一种物体的姿势计算系统：在第一发明～第三发明中的任一个发明中，在使所述一方的物体与所述另一方的物体以所述3个接触点接触之前，以使所述一方的物体与所述另一方的物体的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使所述一方的物体与另一方的物体移动到相对的大致位置和大致姿势。

第五发明提供如下一种物体的姿势计算系统：在第一发明～第三发明中的任一个发明中，所述力测量部还测量作用于所述驱动部或者所述一方的物体或者所述另一方的物体的力，在使所述一方的物体与所述另一方的物体以所述3个接触点接触之前，基于由所述力测量部测量出的力，以使所述一方的物体与所述另一方的物体的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使所述一方的物体与另一方的物体移动到相对的大致位置和大致姿势。

第六发明提供如下一种物体的姿势计算系统：在第一发明～第三发明中的任一个发明中，所述物体的姿势计算系统还具备物体检测部，该物体检测部能够在规定的范围以内检测所述一方的物体或者所述另一方的物体的位置和姿势中的至少一方，在使所述一方的物体与所述另一方的物体以所述3个接触点接触之前，基于所述物体检测部的检测结果，以使所述一方的物体与所述另一方的物体的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使所述一方的物体与另一方的物体移动到相对的大致位置和大致姿势。

第七发明提供如下一种物体的姿势计算系统：在第一发明～第三发明中的任一个发明中，所述物体的姿势计算系统还具备空间位置指定部，该空间位置指定部由指定空间内的位置或者位置和姿势的装置以及识别所指定的所述位置或者位置和姿势的装置构成，在使所述一方的物体与所述另一方的物体以所述3个接触点接触之前，基于由所述空间位置指定部指定的位置或者位置和姿势，以使所述一方的物体与所述另一方的物体的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使所述一方的物体与另一方的物体移动到相对的大致位置和大致姿势。

第八发明提供如下一种物体的姿势计算系统：在第一发明～第七发明中的任一个发明中，基于所述物体姿势计算部所计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，来进行所述一方的物体或者所述另一方的物体相对于所述驱动部的位置和姿势中的至少一方的计算。

第九发明提供如下一种物体的姿势计算系统：在第一发明～第八发明中的任一个发明中，基于所述物体姿势计算部所计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，来校正使所述驱动部所移动的所述一方的物体或者所述另一方的物体或者所述一方的物体和所述另一方的物体移动到的位置或者姿势或者位置和姿势。

第十发明提供如下一种物体的姿势计算系统：在第一发明～第九发明中的任一个发明中，基于所述物体姿势计算部所计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，来执行所述一方的物体或者所述另一方的物体的好坏判定、或者所述一方的物体或所述另一方的物体的种类判定、或者由所述驱动部保持的所述一方的物体或所述另一方的物体的保持状态的判定中的至少1个判定。

第十一发明提供如下一种物体的姿势计算系统：在第一发明～第十发明中的任一个发明中，在所述一方的物体和所述另一方的物体的所述凸部与所述凹部的组合中，能够使所述凸部与所述凹部相嵌合，所述驱动部使所述一方的物体与所述另一方的物体相对地移动，并且，保持所述一方的物体和所述另一方的物体中的至少1个物体来使其移动，并且，能够以使所述一方的物体与所述另一方的物体相嵌合的方式移动，所述物体的姿势计算系统还具备嵌合执行部，该嵌合执行部基于由所述力测量部测量出的力，通过所述驱动部使所述一方的物体与所述另一方的物体相对地移动并执行力控制，来使所述一方的物体与所述另一方的物体相嵌合，所述嵌合执行部基于由所述物体姿势计算部计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，来使所述一方的物体与所述另一方的物体相嵌合。

第十二发明提供如下一种物体的姿势计算系统：在第十一发明中，所述嵌合执行部基于所述物体姿势计算部所计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，在使所述一方的物体与所述另一方的物体移动以修正所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势之后，使所述一方的物体与所述另一方的物体相嵌合。

第十三发明提供如下一种物体的姿势计算系统：在第十一发明中，所述嵌合执行部基于所述物体姿势计算部所计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，在所述一方的物体与所述另一方的物体的相对姿势大时，以使将所述一方的物体与所述另一方的物体的相对姿势变小的移动速度变大的方式，基于由所述力测量部测量出的力来进行力控制，使所述一方的物体与所述另一方的物体相嵌合。

在本发明中，基于一方的物体与另一方的物体以3个接触点接触时的作用于一方的物体与另一方的物体之间的力，来计算一方的物体与另一方的物体的相对姿势，因此能够通过使一方的物体与另一方的物体接触来简单地计算一方的物体与另一方的物体的相对姿势。

因此，根据本发明，作为系统，只要能够使一方的物体与另一方的物体以3个接触点接触并测量、计算作用于一方的物体与另一方的物体之间的力即可，不需要能够高精度地检测物体的位置和姿势的照相机、三维传感器等非接触式的传感器，从而能够简化系统、抑制成本。另外，一方的物体和另一方的物体也可以是利用照相机、三维传感器等非接触式的传感器例如根据表面状况等而无法检测的物体、难以检测的物体。另外，本发明也能够应用于由于系统的状况所引起的干扰等问题而无法利用照相机、三维传感器等非接触式的传感器检测一方的物体和另一方的物体的状况。

另外，根据本发明，不需要为计算一方的物体与另一方的物体的相对姿势而使一方的物体嵌合到另一方的物体的动作。因此，不需要嵌合作业所耗费的时间，能够缩短系统的循环时间。另外，无论嵌合作业成功或者失败，都能够计算相对的位置和姿势。另外，一方的物体和另一方的物体也可以不是能够嵌合的物体。另外，在一方的物体与另一方的物体的相对姿势大的情况下，能够计算一方的物体与另一方的物体的相对姿势。

另外，根据本发明，不需要为计算一方的物体与另一方的物体的相对姿势而使一方的物体相对于另一方的物体进行面对齐的动作。因此，没有面对齐作业所耗费的时间，能够缩短系统的循环时间。另外，无论面对齐作业成功或者失败，都能够计算相对的位置和姿势。另外，一方的物体和另一方的物体不是能够进行面对齐的物体也可以。

附图说明

图1是表示本发明的第一实施方式所涉及的系统的概要结构的图。

图2是表示本发明的其它第一实施方式所涉及的系统的概要结构的图。

图3a-图3b是表示使一方的物体与另一方的物体以3个接触点接触的形态的图。

图4是表示一方的物体和另一方的物体包含凸部、凹部的形态的图。

图5a-图5h是表示一方的物体和另一方的物体中的至少任一个物体所包含的凸部的形状的一例的图。

图6a-图6b是表示一方的物体和另一方的物体中的至少任一个物体所包含的凸部的截面形状的例子的图。

图7a-图7d是表示一方的物体和另一方的物体中的至少任一个物体所包含的凹部的形状的例子的图。

图8a-图8b是表示一方的物体和另一方的物体中的至少任一个物体所包含的凹部的截面形状的一例的图。

图9a-图9d是表示一方的物体和另一方的物体中的凸部与凹部的组合的一例的图。

图10a-图10b是表示使一方的物体与另一方的物体以3个接触点接触的形态的图。

图11a-图11c是表示使一方的物体与另一方的物体以3个接触点接触的形态的图。

图12a-图12b是表示使一方的物体与另一方的物体以3个接触点接触的形态的图。

图13a-图13b是表示使一方的物体与另一方的物体以2个接触点接触的形态的图。

图14a-图14b是表示姿势计算用的力测量坐标系的设定的一例的图。

图15a-图15b是表示使一方的物体与另一方的物体以3个接触点接触的动作的一例的图。

图16a-图16b是表示姿势计算用的力测量坐标系的设定的一例的图。

图17是表示使另一方的物体包含多个凹部的另一方的物体的形状的图。

图18a-图18d是表示包含有使一方的物体和另一方的物体包含的多个凸部和凹部的形状的图。

具体实施方式

下面，参照附图来说明本发明的实施方式。在下面的附图中，对同样的构件标注了同样的参照标记。为了易于理解，这些附图适当改变了比例尺。

在本发明中，“力”包括力的平移方向分量和力的力矩分量，除非另有说明。另外，“A和/或B”是指包括A和B中的至少一方，例如，“位置和/或姿势”是指包括位置和姿势中的至少一方。

在本实施例中，对空间设定的坐标系是指能够在相对于空间固定的正交坐标系上表示驱动部的顶端部分、保持于驱动部的顶端部分(也包括安装于驱动部的顶端部分的状态)的物体的位置和/或姿势的坐标系。此外，驱动部的顶端部分是指驱动部中能够保持物体来使该物体移动的部分。另外，工具坐标系是指对驱动部的顶端部分或者保持于驱动部的顶端部分的物体设定的坐标系，是能够表示驱动部的顶端部分或者保持于驱动部的顶端部分的物体在对驱动部的台座设定的坐标系上或者对空间设定的基准坐标系上的位置和/或姿势的坐标系。另外，控制点是指表示驱动部的顶端部分、保持于顶端部分的物体的位置的点。另外，力控制坐标系是指为了基于作用于一方的(第一)物体与另一方的(第二)物体之间的力来对保持于驱动部的顶端部分的物体的位置和/或姿势进行控制而被设定成以控制点为原点的坐标系。另外，姿势计算用的力测量坐标系是指为了物体姿势计算部基于作用于一方的物体与另一方的物体之间的力来计算一方的物体与另一方的物体的相对姿势或者相对位置和相对姿势而设定的坐标系。另外，变换为某个坐标系上的力是指将某个力作为对某个坐标系的原点作用的力来在某个坐标系上表示。

在本实施例的说明中，数式中的“·”表示矩阵与矢量的乘法运算，“×”表示外积，“*”表示乘法运算，“/”表示除法运算，“Cos(x)”表示余弦函数，“Sin(x)”表示正弦函数，“Arccos(x)”表示余弦函数的反三角函数，“Arcsin(x)”表示正弦函数的反三角函数，“Arctan(x)”表示正切函数的反三角函数，“≈”表示设左边与右边近似。另外，P(点b_坐标系a)表示点b在坐标系a上的位置。另外，(矢量A)＾T表示将矢量A转置后的转置矢量。

物体的姿势计算系统是计算一方的物体与另一方的物体的相对姿势的系统。图1是表示本发明的第一实施方式所涉及的物体的姿势计算系统11的结构例的概要图。物体的姿势计算系统11具备：一方的(第一)物体21；另一方的(第二)物体22；驱动部50，其作为保持一方的物体21和另一方的物体22中的至少1个物体来使其移动的驱动部而使一方的物体21移动；力测量部31，其测量作用于一方的物体21与另一方的物体22之间的力；以及物体姿势计算部32，其计算一方的物体21与另一方的物体22的相对姿势。物体的姿势计算系统11还具备驱动部控制装置52。驱动部控制装置52除了具备生成驱动部50的动作指令的动作指令生成部33以外，还具备力测量部31、物体姿势计算部32。在本实施例中，设力测量部31、物体姿势计算部32由驱动部控制装置52所具备，但是也可以包括于与驱动部控制装置52连接的中央控制装置、其它的控制装置、运算处理装置等。

驱动部50由1个以上的控制轴构成，各控制轴由驱动部控制装置52按规定的控制周期来控制，由此能够将驱动部50的顶端部分定位于指定的位置。驱动部控制装置52具有包括CPU、ROM、RAM等的硬件结构，除此以外还具备存储部、显示输出部、输入部、输出各种信号的信号输出部等来执行后述的各种功能，但未进行图示。图1所图示的驱动部50、图2所图示的驱动部50’为由6个轴构成的垂直多关节型机器人，但是只要能够使一方的物体21和另一方的物体22相对地平移移动或者旋转移动或者平移移动及旋转移动即可，同样也能够应用于具有其它形态的任意的公知的机器人、具备移动机构的公知的驱动装置。也可以是，在另一方的物体22移动时，基于另一方的物体22的位置和姿势来使一方的物体21移动。

动作指令生成部33生成动作指令并向驱动部50的各控制轴输出动作指令，以使驱动部50能够顺畅地移动到期望的位置和/或姿势。另外，动作指令生成部33也可以进行滤波处理以避免加加速度突然变大、或者进行振动的减少处理以使驱动部50的动作的振动变少、或者考虑驱动部50的挠曲等来校正指令的位置和/或姿势、或者调整动作指令，以使得能够准确地移动到作为目标的位置。在如图2所示那样驱动部控制装置52使驱动部50、驱动部50’移动的情况下，驱动部控制装置52对驱动部50’也与驱动部50同样地，生成动作指令并向驱动部50’的各控制轴输出动作指令，以使驱动部50’能够顺畅地移动到期望的位置和/或姿势。

图2是表示本发明的其它第一实施方式所涉及的物体的姿势计算系统11a的结构例的概要图。物体的姿势计算系统11a具备一方的物体21、另一方的物体22、作为保持一方的物体21和另一方的物体22中的至少1个物体来使其移动的驱动部的保持一方的物体21来使其移动的驱动部50和保持另一方的物体22来使其移动的驱动部50’、力测量部31以及物体姿势计算部32。在驱动部50’的顶端部分安装有6轴的力传感器51’。

也可以如物体的姿势计算系统11a那样，使物体的姿势计算系统11还具备使另一方的物体22移动的驱动部50’。这样，物体的姿势计算系统也可以具备2个驱动部以能够使一方的物体与另一方的物体相独立地移动。由驱动部控制装置52控制驱动部50’的各控制轴，来使驱动部50’移动。相对于物体的姿势计算系统11中的驱动部控制装置52，物体的姿势计算系统11a中的驱动部控制装置52的动作指令生成部33还生成驱动部50’的动作指令。另外，力测量部31也可以基于作用于驱动部50’的保持另一方的物体22的部分的力来测量作用于一方的物体21与另一方的物体22之间的力。另外，也可以与驱动部控制装置52相分别地具备使驱动部50’移动的驱动部控制装置。

在使用图2所示的驱动部50’的情况下，在使一方的物体21与另一方的物体22接触时，也可以考虑驱动部50的可移动范围、可移动方向、周边装置的状况等，来通过驱动部50’使另一方的物体22移动，使得驱动部50易于使一方的物体21的凸部与另一方的物体22的凹部接触，如驱动部50不受干扰等而易于移动、或者驱动部50只要平移移动或者旋转移动即可。另外，优选的是，在驱动部50’使另一方的物体22移动时，考虑该另一方的物体22的移动量来使驱动部50移动。另外，也可以使用驱动部50’的顶端部分所具备的力传感器51’，基于该力传感器51’的输出，通过力测量部31来测量作用于一方的物体21与另一方的物体22之间的力。

下面，使用图1所示的本发明的一个实施方式所涉及的物体的姿势计算系统11来说明本发明的一个实施方式。

物体的姿势计算系统11通过驱动部50来使一方的物体21与另一方的物体22相对地移动，从而使一方的物体21与另一方的物体22接触，在一方的物体21与另一方的物体22以3个接触点接触时，基于对作用于一方的物体21与另一方的物体22之间的力进行测量的力测量部31的输出，通过物体姿势计算部32来计算一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势。

一方的物体21和另一方的物体22是包含凸部的物体与包含凹部的物体的组合，且能够使所述凸部与所述凹部在3个接触点接触。也就是说，在一方的物体21包含凸部时，另一方的物体22包含1个以上的能够与所述凸部在3个接触点接触的凹部。另外，在一方的物体21包含凹部时，另一方的物体22包含1个以上的能够与所述凹部在3个接触点接触的凸部。另外，所述凹部处的所述3个接触点位于所述凹部的孔的端部。另外，当将针对所述凸部的轴、即不与包含所述3个接触点的平面正交的轴作为凸部中心轴时，所述接触点与所述凸部中心轴的各最短距离为规定的值(固定值)，或者，所述接触点与所述凸部中心轴的各最短距离基于从所述接触点对所述凸部中心轴引垂线时的该垂线同所述凸部中心轴的交点与所述凸部中心轴上的规定的点(所述交点以外的点)的距离而确定。

图3a和图3b中图示了使一方的物体21和另一方的物体22所包含的凸部与凹部在3个接触点接触的形态。图3a中示出了以下形态：将一方的物体21所包含的圆筒形的外侧部分作为凸部，将另一方的物体22所包含的圆筒形的内侧部分作为凹部，使一方的物体21的凸部与另一方的物体22的凹部在3个接触点接触。图3b中示出了以下形态：将一方的物体21所包含的圆筒形的内侧部分作为凹部，将另一方的物体22所包含的圆筒形的外侧部分作为凸部，使一方的物体21的凹部与另一方的物体22的凸部在3个接触点接触。这样，既可以使一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触，也可以使一方的物体21的凹部与另一方的物体22的凸部以3个接触点接触。另外，在为大小程度相同的圆筒形的情况下，彼此之间既能够成为凸部也能够成为凹部。这样，在如圆筒形那样是适当大小的物体且物体之中存在空间的物体的情况下，既能够成为凸部也能够成为凹部。另外，如图3a所示，一方的物体21的凸部相对于另一方的物体22的凹部而言，用于接触的圆筒的筒径既可以大也可以小。另外，如图3b所示，一方的物体21所包含的凹部相对于另一方的物体22所包含的凸部而言，用于接触的圆筒的筒径既可以大也可以小。

图4中示出了以下形态：一方的物体21和另一方的物体22均包含凸部、凹部。一方的物体21和另一方的物体22各自具有至少1组以上的凸部和凹部，能够使一方的物体21的凹部与另一方的物体22的凸部、或者一方的物体21的凸部与另一方的物体22的凹部在3个接触点相互接触。另外，一方的物体21和另一方的物体22也可以包含多个大小、形状不同的凸部、凹部。

图5a-图5h中例示了一方的物体21和另一方的物体22中的至少任一个物体所包含的凸部23的形状的一例。凸部23既可以如图5a所示那样为圆柱形状的物体，也可以如图5b所示那样为截面形状是圆欠缺了一部分的形状的物体，还可以如图5c所示那样为截面形状是相比于图5b所示的截面形状而言圆欠缺了更大部分的形状的物体。或者，凸部23也可以如图5d所示那样为截面形状是圆欠缺了3处的形状的物体，还可以如图5e所示那样为直到规定的位置为止截面形状是圆欠缺了2处的形状、且规定的位置以外的部分是圆柱形状的物体。或者，凸部23也可以如图5f所示那样为如圆筒形状的物体那样在物体的内侧具有不存在物体的空间的物体，还可以如图5g、图5h所示那样为如锥形状那样径与距顶端的距离相应地发生变化的物体。在锥形状的情况下，既可以如图5g所示那样使径以线性方式增加或减少，也可以如图5h所示那样使径以指数函数方式、二次函数方式等非线性方式增加或减少。在图5a、图5f、图5g、图5h所示的物体的情况下，能够将使凸部23成为轴对称的轴作为凸部中心轴。另外，在图5b、图5c、图5d、图5e所示的物体的情况下，能够将以最小限度来包含凸部23的圆柱形状的中心轴作为凸部中心轴。

图6a-图6b中例示了一方的物体21和另一方的物体22中的至少任一个物体所包含的凸部23的、沿着对凸部23设定的凸部中心轴的截面形状的一例。凸部23也可以是如下的物体：在利用沿着对凸部23设定的凸部中心轴的截面形状来进行观察的情况下，截面形状如图6a所示的截面形状那样在顶端部分具有例如半球形状那样的凸形状，除此以外的部分是圆柱形状。在该情况下，凸部的顶端部分为如半球形状那样的凸形状，因此凸部23无法与另一方的物体22的凹部的端部等处存在的平面进行面对齐，因此无法通过与存在于另一方的物体22的面进行面对齐来计算凸部与凹部的相对姿势。另外，凸部23也可以是如下的物体：在利用沿着对凸部23设定的凸部中心轴的截面形状来进行观察的情况下，截面形状如图6b所示的截面形状那样，在顶端部分具有例如半球形状的凹陷那样的凹形状，除此以外的部分是圆柱形状。

这样，一方的物体21和另一方的物体22中的至少任一个物体所包含的凸部也可以是包含如下的凸部形状的部分的形状：顶端部分的截面形状是构成圆形状或包含圆形状的一部分的形状的形状，该截面形状持续到规定位置为止，规定位置以后呈圆柱形状。另外，一方的物体21和另一方的物体22中的至少任一个物体所包含的凸部也可以是中空的或锥形的。另外，一方的物体21和另一方的物体22中的至少任一个物体所包含的凸部也可以是在该凸部的顶端部分具有凸形状、凹形状的物体。这样，一方的物体21和另一方的物体22中的至少任一个物体所包含的凸部只要满足上述条件即可，除凸部与凹部以3个接触点接触的部分以外的部分可以是任何形状。

图7a-图7d中例示了一方的物体21和另一方的物体22中的至少任一个物体所包含的凹部24的孔的端部的形状的一例。凹部24既可以如图7a所示那样为凹部的孔的端部的形状是圆形状的物体，也可以如图7b所示那样为凹部的孔的端部的形状是圆欠缺了一部分的形状的物体。或者，凹部24也可以如图7c所示那样为凹部的孔的端部的形状是相比于图7b所示的凹部的孔的端部的形状而言圆欠缺了更大部分的形状的物体，还可以如图7d所示那样为凹部的孔的端部的形状是圆欠缺了3处的形状的物体。

图8a-图8b中例示了一方的物体21和另一方的物体22中的至少任一个物体所包含的凹部24的、沿着凹部的孔的中心轴方向的截面形状的一例。凹部24既可以如图8a所示那样为与凹部的孔的端部相同的形状持续到规定的位置为止的物体，也可以如图8b所示那样为存在例如半球形状的凹陷的形状的、凹部在与凹部24的孔的中心轴正交的平面上的截面形状沿着凹部24的孔的中心轴方向而改变那样的物体。

这样，一方的物体21和另一方的物体22中的至少任一个物体所包含的凹部也可以为凹部的孔的端部的形状是圆形状、包含圆形状的一部分的形状那样的凹陷。另外，一方的物体21和另一方的物体22中的至少任一个物体所包含的凹部也可以是在与所述凹部的孔的中心轴正交的平面上的截面形状沿着凹部的孔的中心轴方向而改变等，而不是与凹部的孔的端部的形状相同的形状持续的物体。另外，一方的物体21和另一方的物体22中的至少任一个物体所包含的凹部也可以是以下形状等：在包围凹部的孔的周围形状沿着凹部的孔的中心轴方向而逐渐发生变化的、例如半球形状的凸形状的物体上，存在所述凹部的孔。这样，一方的物体21和另一方的物体22中的至少任一个物体所包含的凹部只要满足上述条件即可，除凸部与凹部以3个接触点接触的部分以外的部分可以是任何形状。

图9a-图9d中例示了如前述那样说明的一方的物体21和另一方的物体22中的任一个物体所包含的凸部23与不同于前述物体的一方的物体21和另一方的物体22中的任一个物体所包含的凹部24的组合的一例。图9a中示出了凸部23是圆柱形状、凹部24是圆筒形状的情况。图9b中示出了凸部23、凹部24都是圆筒形状的情况。图9c中示出了凸部23是圆柱形状、凹部24是在如半球那样的凸形状的物体上存在凹部的孔的形状的情况。图9d中示出了凸部23是在圆柱形状的顶端存在如半球那样的凸形状的物体的形状、凹部24是圆筒形状的情况。

另外，也可以是一方的物体21和另一方的物体22中的至少任一个物体所包含的凸部的部分以及一方的物体21和另一方的物体22中的至少任一个物体所包含的凹部的部分中的至少1个部分是能够相对于包含该至少1个部分的物体拆装的物体。也就是说，一方的物体21所包含的凸部的部分、凹部的部分也可以是能够相对于一方的物体21拆装的物体。另外，另一方的物体22所包含的凸部的部分、凹部的部分也可以是能够相对于另一方的物体22拆装的物体。

驱动部构成为存在1个以上的使物体移动的机构部，从而能够使一方的物体21与另一方的物体22相对地移动，驱动部保持一方的物体21和另一方的物体22中的至少1个物体来使该物体移动。在本实施例中，驱动部保持物体是指形成以下状态的动作：使物体以能够通过驱动部移动的方式存在的状态，如驱动部把持物体或者将物体设置或安装于驱动部等状态。如参照图2进行说明的那样，也可以在使一方的物体21移动的情况下使用驱动部50，在使另一方的物体22移动的情况下使用驱动部50’。这样，也可以通过如前所述的1个以上的驱动部来使一方的物体21与另一方的物体22相对地移动。驱动部只要能够通过平移移动或者旋转移动或者平移移动及旋转移动来使一方的物体21与另一方的物体22相对地移动从而使一方的物体21与另一方的物体22在3个接触点接触即可，可以是具备任何形式的移动机构的机构部。在图1所示的驱动部50的保持一方的物体21的顶端部分，安装有用于检测作用于驱动部50的顶端部分的力的力传感器51。

力传感器51是检测作用于一方的物体21与另一方的物体22之间的力的检测装置。另外，通过力传感器51能够检测作用于驱动部50的顶端部分的力。驱动部控制装置52基于按规定时间检测出的力传感器51的输出，通过力测量部31来测量作用于一方的物体21与另一方的物体22之间的力。力传感器51在本实施例中为6轴的力传感器，但是只要是具有作业所需的自由度的力传感器即可。另外，只要能够检测作用于一方的物体21与另一方的物体22之间的力，力传感器51的安装位置可以是驱动部50的顶端部分、驱动部50的各控制轴、驱动部50的设置部分、另一方的物体22的设置部分、图2所示的驱动部50’的顶端部分等任意的位置。此外，在本实施例中，力测量部31基于力传感器51的输出来测量作用于一方的物体21与另一方的物体22之间的力，但是也可以不使用力传感器51，而是检测其它物理信息来测量作用于一方的物体21与另一方的物体22之间的力。

力测量部31基于由力传感器51检测出的力数据来测量作用于一方的物体21与另一方的物体22之间的力。另外，力测量部31也可以不使用安装于驱动部50的顶端部分的力传感器51来测量作用于一方的物体21与另一方的物体22之间的力，在该情况下，也可以是，若用于使构成驱动部50的轴移动的致动器是电动机则基于电流值、或者基于构成驱动部50的控制轴的指令位置与实际的控制轴的位置之间的偏差、或者基于安装于驱动部50的各控制轴的转矩传感器的输出等，来估计并计算作用于一方的物体21与另一方的物体22之间的力，由此测量作用于一方的物体21与另一方的物体22之间的力。

另外，力测量部31也可以测量作用于驱动部50、一方的物体21、另一方的物体22的力。此时，基于安装于驱动部50的力传感器51的输出、或者在使构成驱动部50的轴移动的致动器是电动机的情况下基于电流值、或者基于构成驱动部50的控制轴的指令位置与实际的控制轴的位置之间的偏差、或者基于安装于驱动部50的各控制轴的转矩传感器的输出等，来测量作用于驱动部50(包括构成驱动部50的机构部、驱动部50的顶端部分等)的力、作用于驱动部50所保持且移动的物体的力。另外，关于作为驱动部50所不移动的物体的另一方的物体22，也可以使用设置于另一方的物体22之下的力传感器进行测量，或者在另外的驱动部50’保持另一方的物体22的情况下，与驱动部50的情况同样地进行测量。

物体姿势计算部32通过驱动部50使一方的物体21与另一方的物体22相对地移动来使一方的物体21与另一方的物体22接触，基于在一方的物体21与另一方的物体22以3个接触点接触时由力测量部31测量出的力以及接触点与凸部中心轴的最短距离，来计算一方的物体21与另一方的物体22的相对姿势、或者计算一方的物体21与另一方的物体22的相对的位置和姿势。

接着，使用图10a至图17来说明对一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势进行计算的方法的一个实施例。

图10a中图示了与一方的物体21、另一方的物体22的形状等有关的信息。图10b中图示了将同包含一方的物体21的凸部与另一方的物体22的凹部相接触的3个接触点的面正交且包含一方的物体21的凸部中心轴的平面作为截面来观察一方的物体21的凸部与另一方的物体22的凹部以所述3个接触点接触的状态的情形。

图11a-图11c中图示了使一方的物体21与另一方的物体22以3个接触点接触的状况。图11a中图示了一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的状态。图11b中图示了将同包含一方的物体21的凸部与另一方的物体22的凹部相接触的3个接触点的面正交且包含一方的物体21的凸部中心轴的平面作为截面来观察一方的物体21的凸部与另一方的物体22的凹部以所述3个接触点接触的状态的情形。图11c中针对一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的状态，图示了假设使一方的物体21的凸部与另一方的物体22的凹部相分离的情形以进行参数的说明。

图12a中图示了一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的状态。图12b中图示了将同包含一方的物体21的凸部与另一方的物体22的凹部相接触的3个接触点的面正交且包含一方的物体21的凸部中心轴的平面作为截面来观察一方的物体21的凸部与另一方的物体22的凹部以所述3个接触点接触的状态的情形。

图13a-图13b中图示了一方的物体21与另一方的物体22以2个点接触的状况。图14a-图14b中图示了对一方的物体21设定的姿势计算用的力测量坐标系的设定的情形。图15a-图15b中图示了使一方的物体21与另一方的物体22以3个接触点接触时的动作的状况。图16a-图16b中图示了对一方的物体21设定的姿势计算用的力测量坐标系的设定的情形。图17中记载了关于利用另一方的物体22所包含的多个凹部来计算另一方的物体22相对于一方的物体21的位置和姿势的说明的图。

在本实施例中，为了简化说明，如图10a所示那样设一方的物体21为包含凸部且所述凸部是圆柱形状的物体。另外，设另一方的物体22包含凹部且所述凹部是能够与所述凸部的圆柱形状相嵌合的孔。设一方的物体21的凸部与另一方的物体22的凹部为在相对姿势是规定的值以上时能够如前所述那样在3个接触点接触的物体。

如图10a所示，将一方的物体21的凸部的圆柱形状的半径设为r、将直径设为d。将另一方的物体22的凹部的孔的半径设为R、将直径设为D。设一方的物体21的凸部的圆柱沿圆柱的中心轴方向具有规定的长度，使得凸部与凹部能够如前所述那样以3个接触点接触。将一方的物体21的凸部的圆柱形状的顶端的中心点设为点Oa。作为针对一方的物体21的凸部的轴，将凸部中心轴La设定为圆柱形状的中心轴。此外，一方的物体21的凸部关于凸部中心轴La成轴对称。将另一方的物体22的凹部的孔的端部的中心点表示为点Ob。

图11a中图示了一方的物体21的凸部与另一方的物体22的凹部如前所述那样以3个接触点接触的情形。如图11a所图示的那样，在一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时，作为针对凸部的轴的凸部中心轴La不与包含3个接触点的平面正交。并且，另一方的物体22的凹部处的3个接触点位于凹部的孔的端部。并且，3个接触点与凸部中心轴的各最短距离为规定的值(固定值)，或者，接触点与凸部中心轴的各最短距离基于从3个接触点对凸部中心轴引垂线时的该垂线同凸部中心轴的交点与凸部中心轴上的规定的点(该交点以外的点、在此为点Oa)的距离而确定(其中，在凸部是圆柱形状的情况下，即使所述交点与所述交点以外的点的距离不同，所述各最短距离也为固定值)。

将一方的物体21的凸部与另一方的物体22的凹部如前所述那样以3个接触点接触的情况下的3个点设为点Pe1、点Pe2、点Ps。对另一方的物体22的凹部而言，点Pe1、点Pe2、点Ps这3个接触点存在于另一方的物体22的凹部的孔的端部且一方的物体21侧的端部，并存在于以中心点Ob为中心的圆周上。对一方的物体21的凸部而言，3个接触点中，点Pe1、点Pe2这2个点存在于作为圆柱形状的顶端部分且另一方的物体22侧的端部的圆周上，点Ps这1个点存在于圆柱形状的侧面上。一方的物体21的凸部处的与另一方的物体22接触的3个点与凸部中心轴La的最短距离均为r。

此外，设一方的物体21的凸部的圆柱形状的半径r、直径d、另一方的物体22的凹部的孔的半径R、直径D是已知的，但是关于一方的物体21和另一方的物体22中的任一个物体，也可以在事先进行计算。例如，也可以使一方的物体21相对于墙、台、另外的工件等另外的物体倾斜，基于在将Z轴设定为一方的物体21的凸部中心轴La的轴而得到的工具坐标系中、沿Z轴方向且接近另外的物体的方向按压一方的物体21时的Z轴方向的力以及绕除此以外的轴的力来计算一方的物体21的径。或者，也可以基于与墙、台、另外的工件等另外的物体接触时的位置来计算一方的物体21的径。例如，首先，使一方的物体21的凸部的圆形状的端部与被固定的另外的物体接触。接着，使一方的物体21的凸部绕凸部中心轴La旋转规定的角度后，使一方的物体21的凸部的圆形状的端部与被固定的另外的物体接触。此时，执行以下处理等：使一方的物体21的凸部绕凸部中心轴La旋转180度后接触、或者、以适当的角度进行3次以上的接触动作和位置检测，来求出一方的物体21的凸部处的半径r、直径d。关于另一方的物体22的凹部的孔的半径R、直径D，与一方的物体21的凸部处的半径r、直径d大致相同地求出。另外，关于另一方的物体22的凹部的孔的半径R、直径D，也可以是，对安装于驱动部50的销设定TCP(Tool Center Point：工具中心点)、控制点、工具坐标系，预先利用所述销来计算另一方的物体22的凹部的孔的孔径。

驱动部50保持一方的物体21来使该一方的物体21移动。此时，预先针对一方的物体21的凸部将控制点设定为点Oa，设定以所述控制点为原点的工具坐标系。通过事先设定工具坐标系，来事先确定一方的物体21的凸部相对于驱动部50的位置和姿势。工具坐标系由相互正交的X轴、Y轴、Z轴构成。Z轴被设定成与凸部中心轴一致，+方向为包含从另一方的物体22的凹部离开的方向的分量的方向。关于X轴、Y轴的正负的方向，只要设定对一方的物体21而言适当的方向即可。

如图11b所图示的那样，在一方的物体21的凸部与另一方的物体22的凹部如前所述那样以3个接触点接触时，将包含3个接触点的面的法线与对一方的物体21的凸部设定的凸部中心轴La所形成的角度设为角度α。如后所述，角度α为对另一方的物体22的凹部的孔设定的坐标系CFb的Z轴的+方向与对一方的物体21的凸部设定的姿势计算用的力测量坐标系CFa的Z轴的+方向所形成的角度。

在此，研究一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时的、一方的物体21的凸部的圆柱形状的直径d、另一方的物体22的凹部的孔的直径D以及角度α的关系。

图10b中图示了将同包含一方的物体21的凸部与另一方的物体22的凹部相接触的3个接触点的面正交且包含一方的物体21的凸部中心轴La的平面作为截面来观察一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的状态的情形。

如图10b所图示的那样，在一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时，将一方的物体21的凸部沿凸部中心轴La的方向投影到包含3个接触点的平面、在此为包含另一方的物体22的凹部的孔的端部的平面。关于投影到所述包含3个接触点的平面的部分同与包含3个接触点的面正交且包含凸部中心轴La的平面相交的部分，在将该部分的端设为点Ps和端点Pq时，该部分为将点Ps与端点Pq连结而得到的直线。

将点Ps与端点Pq连结而得到的直线的长度为d/(Cos(α))。在一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的情况下，将点Ps与端点Pq连结而得到的直线的长度为D以上，满足以下的条件。

d/(Cos(α))≥D (1)

在此，D>d。

根据式(1)求出式(2)。

α≥Arccos(d/D) (2)

根据式(2)可知，一方的物体21的凸部的圆柱形状的直径d与另一方的物体22的凹部的孔的直径D的大小越接近，则角度α的值越小、相对姿势小，即使相对姿势小也会在3个接触点接触。另外，根据式(2)可知，例如，在凸部的直径为(30-0.005)mm、凹部的直径为(30+0.005)mm的情况下，当角度α为1.48度左右以上且小于90度的值时，凸部与凹部在3个接触点接触。另外可知，在凸部的直径为(50-0.005)mm、凹部的直径为(50+0.005)mm的情况下，当角度α为1.15度左右以上且小于90度的值时，凸部与凹部在3个接触点接触。

此外，在要使凸部与凹部相嵌合、且凸部与凹部的相对姿势的偏差大的情况下，通常，很多时候难以通过基于作用于一方的物体21的凸部与另一方的物体22的凹部之间的力的力控制来进行嵌合、难以在规定的时间内进行嵌合。在这种情况下，通过本发明，计算一方的物体21的凸部与另一方的物体22的凹部的相对姿势或者相对的位置和姿势，利用计算出的相对姿势或者相对的位置和姿势，例如使一方的物体21的凸部和另一方的物体22的凹部移动到对计算出的所述相对姿势的量进行校正后的位置关系，之后通过力控制进行嵌合等，由此，易于以尽可能少的失败次数或者尽可能短的时间使一方的物体21的凸部与一方的物体22的凹部相嵌合。

当在一方的物体21与另一方的物体22以3个接触点接触时基于由力测量部31测量出的力以及接触点与凸部中心轴的最短距离来计算一方的物体21与另一方的物体22的相对姿势、或者相对位置和相对姿势时，使用姿势计算用的力测量坐标系。基于将作用于一方的物体21与另一方的物体22之间的力变换为姿势计算用的力测量坐标系上的力而得到的力，来计算一方的物体21与另一方的物体22的相对姿势、或者相对位置和相对姿势。如图11a、图11c所图示的那样，将对一方的物体21的凸部设定的姿势计算用的力测量坐标系设为坐标系CFa。此外，在一方的物体21的凸部与另一方的物体22的凹部的相对的位置和姿势处于某个范围以内时、或者一方的物体21与另一方的物体22接触时，只要执行以规定的目标力向包含对一方的物体21的凸部设定的姿势计算用的力测量坐标系CFa的Z轴的-方向的方向按压、并且使与Z轴正交且相互正交的2个方向上的目标力为0的被动式力控制来使一方的物体21的凸部移动，就能够使一方的物体21的凸部与另一方的物体22的凹部以2个或者3个接触点接触。另外，下面，设一方的物体21的凸部和另一方的物体22的凹部处于以下状态：执行以规定的目标力向包含姿势计算用的力测量坐标系CFa的Z轴的-方向的方向按压、并且使与Z轴正交且相互正交的2个方向上的目标力为0的被动式力控制，来使一方的物体21的凸部移动，由此使一方的物体21的凸部与另一方的物体22的凹部能够以2个或者3个接触点接触的状态。

说明姿势计算用的力测量坐标系CFa的坐标系的设定方法。在如下那样设定了姿势计算用的力测量坐标系CFa时，成为姿势计算用的力测量坐标系CFa的坐标系的设定完成的状态。首先，姿势计算用的力测量坐标系CFa由相互正交的X轴、Y轴、Z轴构成。将坐标系的原点设定为凸部中心轴上的一方的物体21的凸部的圆形状的中心点Oa。关于姿势计算用的力测量坐标系CFa的X轴、Y轴、Z轴的设定，设定为在一方的物体21的凸部与另一方的物体22的凹部接触时满足如以下那样的条件。Z轴被设定成与凸部中心轴一致，使+方向为包含从另一方的物体22的凹部离开的方向的分量的方向。接着，Y轴被设定成同将一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时的前述的接触点Pe1与接触点Pe2连结而得到的直线平行。使X轴的+方向为包含从另一方的物体22的凹部离开的方向的分量的方向。

如下那样完成姿势计算用的力测量坐标系CFa的设定。

首先，将由相互正交的X轴、Y轴、Z轴构成的坐标系的原点设定为一方的物体21的凸部的圆形状的中心点Oa。将Z轴设定成与凸部中心轴(在该情况下，为圆柱形状的中心的轴)一致，将+方向设定成为包含从另一方的物体22的凹部离开的方向的分量的方向。之后，对于X轴、Y轴，决定绕Z轴的方向。此外，只要X轴、Y轴中的任一个确定了，则另一个轴的方向也就确定了。

使一方的物体21的凸部相对于另一方的物体22的凹部向以下的一方的物体21的凸部与另一方的物体22的凹部相接触的方向移动：姿势计算用的力测量坐标系CFa的Z轴的-方向、或者对一方的物体21设定的工具坐标系的Z轴的-方向、或者在对空间设定的坐标系上接近另一方的物体22的凹部的方向等。在一方的物体21的凸部与另一方的物体22的凹部接触之后，执行以规定的目标力向姿势计算用的力测量坐标系CFa的Z轴的-方向按压、并且使与Z轴正交且相互正交的2个方向上的目标力为0的被动式力控制，来使一方的物体21的凸部移动，由此使一方的物体21的凸部与另一方的物体22的凹部以2个或者3个接触点接触。此时，在一方的物体21不再移动时，判定为一方的物体21的凸部与另一方的物体22的凹部已以2个或者3个接触点接触。

在此，对像这样使一方的物体21的凸部移动来使一方的物体21的凸部与另一方的物体22的凹部如图13a、图13b所图示的那样以点Pe1和点Pe2这2点接触的情况进行说明，但是一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的情况也同样。在图13b中图示了以下情形：从与另一方的物体22的凹部的孔的端部正交且包含凸部中心轴La的截面来观察如图13a所图示的那样一方的物体21的凸部与另一方的物体22的凹部以2点接触的状况。另外，将接触点Pe1和所作用的力Fe投影到该截面。如图13b所图示的那样，作为一方的物体21的凸部与另一方的物体22的凹部以2点接触的状况，执行以规定的目标力将一方的物体21的凸部相对于另一方的物体22的凹部向姿势计算用的力测量坐标系CFa的Z轴的-方向即按压方向Pd1按压、并且使与Z轴正交且相互正交的2个方向的目标力为0的被动式力控制，来使一方的物体21的凸部移动，由此使一方的物体21的凸部与另一方的物体22的凹部以2个接触点接触。此外，通过该移动，也有时会以3个接触点接触，其说明是同样的。

图14a中图示了以下形态：在一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时，在由姿势计算用的力测量坐标系CFa的X轴、Y轴构成的平面上观察一方的物体21的凸部的顶端部分。在由姿势计算用的力测量坐标系CFa的X轴、Y轴构成的平面上存在接触点Pe1、接触点Pe2。角度β为在由姿势计算用的力测量坐标系上的X轴和Y轴构成的平面上Y轴的-方向同将点Oa与一方的物体21的凸部处的接触点Pe1连结而得到的线段所形成的角度。

在此，设处于姿势计算用的力测量坐标系CFa的设定完成的状态，相对于姿势计算用的力测量坐标系CFa的Z轴，姿势计算用的力测量坐标系CFa的X轴、Y轴为图14a所图示那样的某个姿势。

此外，将姿势计算用的力测量坐标系CFa的X轴方向的力设为Fx，将Y轴方向的力设为Fy，将Z轴方向的力设为Fz，将绕X轴的力设为Mx，将绕Y轴的力设为My，将绕Z轴的力设为Mz。如前所述，角度β为在由姿势计算用的力测量坐标系上的X轴和Y轴构成的平面上Y轴的-方向同将点Oa与一方的物体21的凸部处的接触点Pe1连结而得到的线段所形成的角度。μ为静摩擦系数。沿姿势计算用的力测量坐标系上的Z轴方向进行作用的力Fe和沿姿势计算用的力测量坐标系上的X轴方向进行作用的力μ*Fe作用于接触点Pe1。沿姿势计算用的力测量坐标系上的Z轴方向进行作用的力Fe和沿姿势计算用的力测量坐标系上的X轴方向进行作用的力μ*Fe作用于接触点Pe2。接触点Pe1与接触点Pe2在一方的物体21的凸部和另一方的物体22的凹部上，关于与包含另一方的物体22的凹部的孔的端部的面正交且包含点Oa、凸部中心轴La的面成面对称。也就是说，关于接触点Pe1和接触点Pe2在姿势计算用的力测量坐标系CFa上的位置，X轴上、Z轴上的值相同，Y轴上的值为正负不同的值。另外，关于接触点Pe1和接触点Pe2在对另一方的物体22的凹部设定的坐标系CFb上的位置，X轴上、Z轴上的值相同，Y轴上的值为正负不同的值。

此时，当设静摩擦系数μ能够近似为0时，Fx、Fy、Fz、Mx、My、Mz分别表示为以下的式(3)-式(8)。

Fx＝0 (3)

Fy＝0 (4)

Fz＝2*Fe (5)

Mx＝0 (6)

My＝2*Fe*r*Sin(β) (7)

Mz＝0 (8)

接着，研究另外的某个姿势计算用的力测量坐标系CFa。在一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时，将由以下的Z轴、X轴、Y轴构成的坐标系作为所述另外的某个坐标系：Z轴相同，当将上述的坐标系作为基准的坐标系时，将上述的基准的坐标系的X轴设为X’轴、将Y轴设为Y’轴，使X’轴、Y’轴绕Z轴旋转θ而得到X轴、Y轴。也就是说，如果使另外的某个坐标系绕Z轴旋转-θ，则成为与图14a所示的基准的坐标系相同的坐标系。研究将作用于一方的物体21的凸部与另一方的物体22的凹部之间的力变换到所述另外的某个坐标系上而得到的力。

图14b中图示了在一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时在由姿势计算用的力测量坐标系CFa的X轴、Y轴构成的平面上进行观察的形态。在由姿势计算用的力测量坐标系CFa的X轴、Y轴构成的平面上存在接触点Pe1、接触点Pe2。角度β为在由姿势计算用的力测量坐标系上的X轴和Y轴构成的平面上Y轴的-方向同将点Oa与一方的物体21的凸部处的接触点Pe1连结而得到的线段所形成的角度。另外，图14b中图示了所述基准的坐标系的X’轴和Y’轴以及另外的某个坐标系的X轴、Y轴。

此时，关于将作用于一方的物体21的凸部与另一方的物体22的凹部之间的力变换到所述另外的某个坐标系上而得到的力，当设静摩擦系数μ能够近似为0时，以下的式(9)-式(14)成立。

Fx＝0 (9)

Fy＝0 (10)

Fz＝2*Fe (11)

Mx＝Fe*r*(Sin(-θ+π/2-β)

+Sin(-θ-π/2+β)) (12)

My＝Fe*r*(Cos(-θ+π/2-β)

+Cos(-θ-π/2+β)) (13)

Mz＝0 (14)

根据式(12)、(13)，通过以下的式(15)来计算θ。

θ＝Arctan(-Mx/My) (15)

另外，根据另一方的物体22的凹部存在于为了使一方的物体21的凸部与另一方的物体22的凹部接触而靠近的方向、Z轴的-方向这样的条件，以使X轴的+方向为包含从另一方的物体22的凹部的离开方向的分量的方向的方式求出角度θ。基于计算出的θ，根据所述另外的某个基准坐标系来求出所述基准的坐标系。通过这样，能够设定满足式(3)至式(8)的姿势计算用的力测量坐标系CFa。

由此，能够设定如下的坐标系：将Y轴设定成同将点Pe1与点Pe2连结而得到的直线平行，并使X轴的+方向为包含从另一方的物体22的凹部离开的方向的分量的方向。该设定方法在一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的情况下也是同样的。通过以上，能够完成姿势计算用的力测量坐标系CFa的设定。

另外，也可以如下那样完成姿势计算用的力测量坐标系CFa的设定。

设定姿势计算用的力测量坐标系CFa的原点和Z轴、使一方的物体21的凸部与另一方的物体22的凹部以2个或者3个接触点接触的方法与前述是同样的。由此，如图13a-图13b所示，一方的物体21的凸部与另一方的物体22的凹部如前所述那样以2个或者3个接触点接触。以后，对于X轴、Y轴决定绕Z轴的方向。此外，只要X轴、Y轴中的任一个确定了，另一个轴的方向也就确定了。

图16a中图示了以下情形：从与另一方的物体22的凹部的孔的端部正交且包含凸部中心轴La的截面观察一方的物体21的凸部与另一方的物体22的凹部以2点接触的情形。在此说明2点接触的情况，但是在3点的情况下也是同样的。在如图16a所图示的那样一方的物体21的凸部与另一方的物体22的凹部接触之后，执行以下的被动式力控制：以规定的目标力将一方的物体21的凸部相对于另一方的物体22的凹部向Z轴的-方向即按压方向Pd1按压，使与Z轴正交且相互正交的2个方向的目标力为0。

此时，在从当前的位置以规定的距离为上限地向与按压方向Pd1正交的下面叙述的方向移动时，在施力的情况下，进行力控制使得以规定的目标力进行按压，并在经过规定时间后停止。此时，事先记录直到停止为止的移动量。之后，以恢复到移动之前的位置的方式移动。执行这种力控制以及检测此时的移动量的动作。作为此时的移动方向即按压方向的方向是由包含点Oa且与Z轴正交的平面内的、以点Oa为基准的正方向的矢量以及与该矢量关于Z轴成轴对称的、以点Oa为基准的反方向的矢量确定的方向。一边改变绕Z轴方向一边执行以下动作：一边进行1次以上的向该某个方向及其反方向的往复动作，一边检测所述移动量。例如，针对多个方向、即+Mv1方向和-Mv1方向、+Mv2方向和-Mv2方向、+Mv3方向和-Mv3方向、+Mv4方向和-Mv4方向重复以下类型的动作：重复1次以上的检测正方向的+Mv1方向和反方向的-Mv1方向的所述移动量的往复动作，接着，重复1次以上的检测+Mv2方向和-Mv2方向的所述移动量的往复动作。

这样，在想要使一方的物体21的凸部移动时，执行以下的Y轴的搜索动作：将一方的物体21的凸部的位置不移动或者移动量为规定的值以下或最小时的、正方向和反方向判断为Y轴上的方向。通过这种方向的搜索方法等，搜索并检测与将一方的物体21的凸部的顶端部分的2个接触点连结而得到的线段平行的方向。此外，也可以使用与如前所述的方法不同的如下的搜索方法：能够搜索与将一方的物体21的凸部的顶端部分的2个接触点连结而得到的线段平行的方向的搜索方法。根据另一方的物体22的凹部存在于为了使一方的物体21的凸部与另一方的物体22的凹部接触而靠近的方向、Z轴的-方向这一条件，以使X轴的+方向为包含从另一方的物体22的凹部离开的方向的分量的方向的方式进行设定，由此Y轴的正负的方向也确定了。或者也可以是，暂时设定X轴、Y轴之后，在以向X轴的-方向移动时若检测出超过规定的值的力则停止的方式移动时，在绕Y轴的力矩变大的情况下判定为设定是正确的，在绕Y轴的力矩变小的情况下判定为X轴、Y轴的正负相反，并改变X轴、Y轴的正负的方向。

另外，也可以如下那样使姿势计算用的力测量坐标系CFa的设定成为完成的状态。

在一方的物体21的凸部与另一方的物体22的凹部接触时，预先对一方的物体21的凸部设定姿势计算用的力测量坐标系CFa，使得成为对一方的物体21的姿势计算用的力测量坐标系CFa的设定完成的状态，并使一方的物体21的凸部向另一方的物体22的凹部移动。在该情况下，使一方的物体21的凸部与另一方的物体22的凹部接触，以使同包含一方的物体21的凸部与另一方的物体22的凹部相接触的3个接触点的平面正交且包含凸部中心轴La的平面成为已知的平面。在如前所述地对一方的物体21的姿势计算用的力测量坐标系CFa的设定预先完成的情况下，执行以规定的目标力将一方的物体21的凸部向姿势计算用的力测量坐标系CFa的Z轴的-方向按压并且以规定的目标力向姿势计算用的力测量坐标系CFa的X轴的-方向按压、使Y轴方向的目标力为0的被动式力控制，在以规定的目标力沿Z轴、X轴方向按压而一方的物体21不再移动时，判定为一方的物体21的凸部与另一方的物体22的凹部已以3个接触点接触。此外，规定的目标力也可以在各方向上大小不同。另外，也可以是，执行将一方的物体21的凸部以规定的目标力向姿势计算用的力测量坐标系CFa的Z轴的-方向按压、并且使与Z轴正交且相互正交的2个方向的目标力为0的被动式力控制，在以规定的目标力沿Z轴方向按压一方的物体21的凸部而该一方的物体21的凸部不再移动之后，执行将一方的物体21的凸部以规定的目标力向姿势计算用的力测量坐标系CFa的Z轴的-方向按压并且以规定的目标力向姿势计算用的力测量坐标系CFa的X轴的-方向按压、使Y轴方向的目标力为0的被动式力控制，来以规定的目标力沿Z轴、X轴方向按压一方的物体21的凸部，从而使该一方的物体21的凸部移动。

为了如前所述那样设定姿势计算用的力测量坐标系CFa，也可以安装另外的机构部等物体、或者限制与一方的物体21的凸部的姿势有关的移动动作等，以约束一方的物体21的凸部与另一方的物体22的凹部的姿势。通过这样，一方的物体21的凸部与另一方的物体22的凹部在包含姿势计算用的力测量坐标系CFa的X轴和Z轴的平面及其平行的平面上事先被相对地约束。即使要针对一方的物体21的凸部和另一方的物体22的凹部求出与如前所述的姿势有关的条件，只要由具有使这种条件成立的机构的装置、机构部使一方的物体21相对于另一方的物体移动、或由驱动部50在这种条件下使一方的物体21相对于另一方的物体移动等，就不需要估计针对一方的物体21的凸部的绕姿势计算用的力测量坐标系CFa的Z轴的姿势，而能够通过所需最低限度的方法来更准确地计算一方的物体21的凸部与另一方的物体22的凹部的相对的位置和姿势。这样，在一方的物体21与另一方的物体22位于同一平面内而相对的角度改变的情况下，能够基于作用于一方的物体21与另一方的物体22之间的力来计算一方的物体21与另一方的物体22的相对姿势，因此在为了进行另外的作业、状态监视等而具备力传感器的情况下，无需利用其它传感器等就能够计算一方的物体21与另一方的物体22的相对姿势。

通过以上那样，通过完成姿势计算用的力测量坐标系CFa的设定，来设定对一方的物体21设定的姿势计算用的力测量坐标系CFa相对于对空间设定的坐标系、对驱动部50设定的坐标系、工具坐标系的齐次变换矩阵、位置和姿势。

接着，研究在姿势计算用的力测量坐标系CFa的设定完成的状态下、或者为了完成姿势计算用的力测量坐标系CFa的设定而以规定的目标力将一方的物体21的凸部向姿势计算用的力测量坐标系CFa的Z轴的-方向按压来使一方的物体21的凸部与另一方的物体22的凹部以3点接触或者以3点接触之前的2点接触时的移动。此外，在一方的物体21的凸部与另一方的物体22的凹部的相对姿势是能够以3个接触点接触的相对姿势时，能够使一方的物体21的凸部与另一方的物体22的凹部以2个接触点接触。如针对一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的条件的前述的说明那样，如果作为一方的物体21的凸部与另一方的物体22的凹部的相对姿势的角度α大于规定的值，则能够使一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触。

在已知一方的物体21的凸部与另一方的物体22的凹部的相对姿势大、能够以3个接触点接触的情况下，使一方的物体21的凸部向姿势计算用的力测量坐标系CFa的Z轴的-方向、或者在对空间设定的坐标系上接近另一方的物体22的凹部的方向等一方的物体21的凸部与另一方的物体22的凹部接触的方向移动。在一方的物体21的凸部与另一方的物体22的凹部接触之后，执行以规定的目标力将一方的物体21的凸部向姿势计算用的力测量坐标系CFa的Z轴的-方向按压、并且使与Z轴正交且相互正交的2个方向的目标力为0的被动式力控制，来使一方的物体21的凸部移动，由此能够使一方的物体21的凸部与另一方的物体22的凹部以2个或者3个接触点接触。此时，在一方的物体21不再移动时，判定为一方的物体21的凸部与另一方的物体22的凹部已以2个或者3个接触点接触。

在不知道一方的物体21的凸部与另一方的物体22的凹部的相对姿势是否为能够以3个接触点接触的姿势的情况下，以使一方的物体21的凸部与另一方的物体22的凹部的相对姿势改变规定的值的方式进行移动。在该情况下，预先具有相对姿势改变的方向、相对姿势是什么程度的范围以内等信息，基于这些信息来将姿势改变规定的值，由此形成一方的物体21的凸部与另一方的物体22的凹部能够以3个接触点接触的相对姿势(事先记录此时的姿势的移动量，之后在计算出一方的物体21的凸部与另一方的物体22的凹部的相对的位置和姿势后，以该移动量对计算出的该相对的位置和姿势进行校正，来计算进行所述姿势的移动之前的一方的物体21的凸部与另一方的物体22的凹部的相对姿势)。之后，使一方的物体21的凸部向姿势计算用的力测量坐标系CFa的Z轴的-方向、或者在对空间设定的坐标系上接近另一方的物体22的凹部的方向等一方的物体21的凸部与另一方的物体22的凹部接触的方向移动。在一方的物体21的凸部与另一方的物体22的凹部接触之后，执行以规定的目标力将一方的物体21的凸部向姿势计算用的力测量坐标系CFa的Z轴的-方向按压、并且使与Z轴正交且相互正交的2个方向的目标力为0的被动式力控制，来使一方的物体21的凸部移动，由此能够使一方的物体21的凸部与另一方的物体22的凹部以2个或者3个接触点接触。此时，在一方的物体21不再移动时，判定为一方的物体21的凸部与另一方的物体22的凹部已以2个或者3个接触点接触。

在一方的物体21的凸部与另一方的物体22的凹部的相对姿势的范围小而无法以3个接触点接触的姿势的情况下，以使姿势改变规定的角度的方式进行移动，使得一方的物体21的凸部与另一方的物体22的凹部的相对姿势成为能够使一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的相对姿势(事先记录此时的姿势的移动量，之后在计算出一方的物体21的凸部与另一方的物体22的凹部的相对的位置和姿势后，以该移动量对计算出的相对的位置和姿势进行校正，来计算进行所述姿势的移动之前的一方的物体21的凸部与另一方的物体22的凹部的相对姿势)。之后，使一方的物体21的凸部向姿势计算用的力测量坐标系CFa的Z轴的-方向、或者在对空间设定的坐标系上接近另一方的物体22的凹部的方向等一方的物体21的凸部与另一方的物体22的凹部接触的方向移动。在一方的物体21的凸部与另一方的物体22的凹部接触之后，执行以规定的目标力向姿势计算用的力测量坐标系CFa的Z轴的-方向按压、并且使与Z轴正交且相互正交的2个方向的目标力为0的被动式力控制，来使一方的物体21的凸部移动，由此能够使一方的物体21的凸部与另一方的物体22的凹部以2个或者3个接触点接触。此时，在一方的物体21不再移动时，判定为一方的物体21的凸部与另一方的物体22的凹部已以2个或者3个接触点接触。

另外，在上述内容中，也可以基于使一方的物体21的凸部与另一方的物体22的凹部接触时的力矩的大小来判断一方的物体21与另一方的物体22是否为能够以3个接触点接触的姿势。

这样，也可以是，使一方的物体21和另一方的物体22中的至少1个将姿势改变规定的值以上，使得成为能够形成使一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的状态的相对姿势，并事先记录此时的姿势的移动量，在计算一方的物体21与另一方的物体22的相对姿势之后，以所记录的所述移动量对计算出的该相对姿势进行校正，来计算一方的物体21与另一方的物体22的最初的相对姿势。

在对一方的物体21完成凸部的姿势计算用的力测量坐标系CFa的设定之后，使一方的物体21移动，来使一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触。

图15a、图15b中图示了以下情形：将与包含另一方的物体22的凹部的孔的端部的平面正交且包含凸部中心轴La的平面作为截面，来观察使一方的物体21的凸部与另一方的物体22的凹部接触的状况。另外，姿势计算用的力测量坐标系CFa的Y轴与纸面正交。

在姿势计算用的力测量坐标系CFa的设定完成之后，执行以规定的目标力向作为Z轴的-方向的按压方向Pd1和作为X轴的-方向的按压方向Pd2按压、使Y轴方向的目标力为0的被动式力控制，来使一方的物体21的凸部移动，在以规定的目标力向按压方向Pd1、按压方向Pd2按压而一方的物体21不再移动时，判断为一方的物体21的凸部与另一方的物体22的凹部已以3个接触点接触。在图15a中，如前所述那样使一方的物体21的凸部与另一方的物体22的凹部以2个接触点接触且完成姿势计算用的力测量坐标系CFa的设定时的一方的物体21的凸部的位置被表示为一方的物体21a。另外，图15a中图示了之后如前所述那样移动从而一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时的情形。

另外，也可以是，在对一方的物体21的凸部完成姿势计算用的力测量坐标系CFa的设定之后，通过另外的移动方法使一方的物体21的凸部和另一方的物体22的凹部移动从而以3个接触点接触。执行以规定的目标力将一方的物体21的凸部向姿势计算用的力测量坐标系CFa的Z轴的-方向即按压方向Pd1按压、并且使与Z轴正交且相互正交的2个方向的目标力为0的被动式力控制，来使一方的物体21的凸部移动，由此使一方的物体21的凸部与另一方的物体22的凹部以2个或者3个接触点接触。此时，在以规定的目标力向按压方向Pd1按压而一方的物体21不再移动时，判断为一方的物体21的凸部与另一方的物体22的凹部已以2个或者3个接触点接触。如前所述，在姿势计算用的力测量坐标系CFa的设定完成之后，执行以规定的目标力将一方的物体21的凸部向Z轴的-方向即按压方向Pd1按压、并且使与Z轴正交且相互正交的2个方向的目标力为0的被动式力控制，来在绕Y轴的-方向即RY方向上以Y轴为中心进行旋转，在绕Y轴的力变为规定的目标力、或者检测出接触时，判定为已以3个接触点接触，使旋转移动停止。由此，能够使一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触。图15b中图示了以下情形：姿势计算用的力测量坐标系CFa的设定完成时的一方的物体21的凸部的位置被表示为一方的物体21a，向按压方向Pd1、绕Y轴的方向RY移动后，一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触。事先记录此时绕Y轴旋转的角度，在计算出凸部中心轴La与对另一方的物体22的凹部设定的坐标系CFb的Z轴的相对角度之后，以所记录的所述角度量对计算出的相对角度进行校正，由此能够关于绕Y轴旋转之前的一方的物体21的凸部与另一方的物体22的凹部计算凸部中心轴La与对另一方的物体22的凹部设定的坐标系CFb的Z轴的相对角度。

此外，在如图5b、图5c、图5d、图5e所示那样凸部的顶端部分是圆欠缺的形状等的情况下，优选的是，在使凸部与凹部接触之前，使凸部和凹部移动到某个规定的范围的位置和姿势，使得能够使凸部与凹部如前所述那样以2个接触点、3个接触点接触。

如前所述，形成完成了姿势计算用的力测量坐标系CFa的设定的状态，基于该设定来使一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触。由此，能够使一方的物体21的凸部与另一方的物体22的凹部的孔部的端部以3个接触点接触，使所设定的姿势计算用的力测量坐标系CFa与3个接触点之间的位置和姿势的关系成为如图11a-图11c所示的状况。

研究已使一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的状态。

在一方的物体21的凸部与另一方的物体22的凹部以接触点Pe1、接触点Pe2、接触点Ps接触时，3个接触点与也作为凸部中心轴La的姿势计算用的力测量坐标系CFa的Z轴的各最短距离为固定值r(在本实施例中，设接触点与姿势计算用的力测量坐标系CFa的Z轴的各最短距离为固定值，但是接触点与姿势计算用的力测量坐标系CFa的Z轴的各最短距离也可以基于姿势计算用的力测量坐标系CFa的Z轴方向的长度而确定。在该情况下，一方的物体21与另一方的物体22的相对姿势、或者相对的位置和姿势的计算方法也与本实施例中的说明相同)。另外，也作为凸部中心轴La的姿势计算用的力测量坐标系CFa的Z轴不与包含3个接触点的平面正交。另外，3个接触点位于另一方的物体22的凹部的孔的端部。另外，如以下那样设定姿势计算用的力测量坐标系CFa的X轴、Y轴、Z轴与3个接触点之间的位置和姿势的关系。姿势计算用的力测量坐标系CFa的Y轴同将接触点Pe1与接触点Pe2连结而得到的直线平行，并且，与从接触点Pe1向接触点Pe2的方向平行的方向为+方向。另外，Z轴的-方向、X轴的-方向包含向另一方的物体22的凹部靠近的方向的分量。另外，包含姿势计算用的力测量坐标系CFa的Z轴和X轴的平面与包含3个接触点的平面正交。

以后，使用如前所述那样确定的姿势计算用的力测量坐标系CFa、3个接触点以及将作用于一方的物体21的凸部与另一方的物体22的凹部之间的力变换为姿势计算用的力测量坐标系CFa上的力而得到的力，来计算姿势计算用的力测量坐标系CFa与对另一方的物体22的凹部的孔的端部设定的坐标系之间的位置和姿势的关系。

为了计算另一方的物体22的凹部相对于对一方的物体21的凸部设定的姿势计算用的力测量坐标系CFa的位置和姿势，如图11b、图11c所图示的那样，对另一方的物体22的凹部的孔设定坐标系CFb。坐标系CFb由相互正交的X轴、Y轴、Z轴构成。设坐标系CFb的原点为另一方的物体22的凹部的孔的端部的中心点Ob。点Ob为一方的物体21的凸部与另一方的物体22的凹部接触的接触点Pe1、接触点Pe2、接触点Ps这3个接触点的中心点。将与包含3个接触点的平面正交的轴设为坐标系CFb的Z轴。设Z轴的+方向为包含从另一方的物体22的凹部离开的孔的方向的分量的方向。将坐标系CFb的Y轴设定成同一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时的姿势计算用的力测量坐标系CFa的Y轴平行、且+方向为相同方向。根据坐标系CFb的Z轴和坐标系CFb的Y轴来唯一地确定坐标系CFb的X轴。

参照图11a、图11b、图11c，将在一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的情况下作用于一方的物体21的凸部与另一方的物体22的凹部之间的力表示为姿势计算用的力测量坐标系CFa上的力。也就是说，设某个力对姿势计算用的力测量坐标系CFa的原点进行作用，来在姿势计算用的力测量坐标系CFa上表示某个力。

在此，如前所述，设角度α为在如图11b所图示的那样一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时包含3个接触点的面的法线与对一方的物体21的凸部设定的凸部中心轴La所形成的角度、即对另一方的物体22的凹部的孔设定的坐标系CFb的Z轴的+方向与姿势计算用的力测量坐标系CFa的Z轴的+方向所形成的角度。设μ为静摩擦系数。另外，如图11a所示，沿姿势计算用的力测量坐标系上的Z轴的正方向进行作用的力Fe和沿姿势计算用的力测量坐标系上的X轴的正方向进行作用的力μ*Fe作用于接触点Pe1。沿姿势计算用的力测量坐标系上的Z轴的正方向进行作用的力Fe和沿姿势计算用的力测量坐标系上的X轴的正方向进行作用的力μ*Fe作用于接触点Pe2。沿姿势计算用的力测量坐标系上的X轴的正方向进行作用的力Fs和沿姿势计算用的力测量坐标系上的Z轴的正方向进行作用的力μ*Fe作用于接触点Ps。

另外，h是在将从接触点Ps向凸部中心轴La引垂线时的交点设为点Pt时从中心点Oa到点Pt为止的距离，是点Pt在姿势计算用的力测量坐标系CFa的Z轴上的位置。设角度β为在由姿势计算用的力测量坐标系CFa的X轴和Y轴构成的平面上Y轴的-方向同将点Oa与接触点Pe1连结而得到的线段所形成的角度。如图11c所图示的那样，设角度β’为在由对另一方的物体22的凹部设定的坐标系CFb的X轴和Y轴构成的平面上Y轴的-方向同将点Ob与另一方的物体22的凹部处的接触点Pe1连结而得到的线段所形成的角度。此外，在图11c中，为了进行角度β’的说明，设想使一方的物体21的凸部与另一方的物体22的凹部相分离地进行了图示，而一方的物体21的凸部与另一方的物体22的凹部在接触点Pe1、接触点Pe2、接触点Ps接触。

如图11a所图示的那样，接触点Pe1与接触点Pe2在一方的物体21的凸部处和另一方的物体22的凹部处，关于同包含一方的物体21的凸部与另一方的物体22的凹部相接触的3个接触点的面正交且包含凸部中心轴La的面成面对称。也就是说，关于接触点Pe1和接触点Pe2在姿势计算用的力测量坐标系CFa上的位置，X轴上、Z轴上的值相同，Y轴上的值为大小相同而正负不同的值。另外，关于接触点Pe1和接触点Pe2在对另一方的物体22的凹部设定的坐标系CFb上的位置，X轴上、Z轴上的值相同，Y轴上的值为大小相同而正负不同的值。

将作用于一方的物体21的凸部与另一方的物体22的凹部之间的力变换为姿势计算用的力测量坐标系CFa上的力、也就是说变换为作用于姿势计算用的力测量坐标系CFa的原点的力，将姿势计算用的力测量坐标系CFa上的X轴方向的力设为Fx，将Y轴方向的力设为Fy，将Z轴方向的力设为Fz，将绕X轴的力设为Mx，将绕Y轴的力设为My，将绕Z轴的力设为Mz。Fx、Fy、Fz、Mx、My、Mz分别通过以下的式(16)-(21)来表示。

Fx＝Fs+2*μ*Fe (16)

Fz＝μ*Fs+2*Fe (17)

My＝Fs*h+μ*Fs*r+2*Fe*r*Sin(β) (18)

Fy＝0 (19)

Mx＝0 (20)

Mz＝0 (21)

在此，在μ已知的情况下，以下的式(22)-(24)成立。

Fe＝(Fz-μ*Fx)/(2*(1-μ*μ)) (22)

Fs＝(Fx-μ*Fz)/(1-μ*μ) (23)

(r*Fz-My)*μ*μ+

{h*Fz+[Sin(β)-1]*r*Fx}*μ+

[My-h*Fx-Sin(β)*r*Fz]＝0 (24)

在此，在μ≈0(μ小到能够近似为0)的情况下，My能够近似为以下的式(25)。

My≈h*Fx+Sin(β)*r*Fz (25)

研究姿势计算用的力测量坐标系CFa、一方的物体21的凸部与另一方的物体22的凹部相接触的接触点Pe1、接触点Pe2、接触点Ps这3个接触点、对另一方的物体22的凹部设定的坐标系CFb之间的位置和姿势的关系。

接触点Pe1、接触点Pe2、接触点Ps在姿势计算用的力测量坐标系CFa上的位置分别如以下的式(26)、(28)、(30)那样表示，接触点Pe1、接触点Pe2、接触点Ps在对另一方的物体22的凹部设定的坐标系CFb上的位置分别如以下的式(27)、(29)、(31)那样表示。

P(点Ps_坐标系CFa)＝[-r、O、h]^T (26)

P(点Ps_坐标系CFb)＝[-R、O、O]^T (27)

P(点Pe1_坐标系CFa)

＝[-r*Sin(β)、-r*Cos(β)、O]^T (28)

P(点Pe1_坐标系CFb)

＝[R*Sin(β’)、-R*Cos(β’)、O]^T (29)

P(点Pe2_坐标系CFa)

＝[-r*Sin(β)、r*Cos(β)、O]^T (30)

P(点Pe2_坐标系CFb)

＝[R*Sin(β’)、R*Cos(β’)、O]^T (31)

另外，关于β、β’，以下的式(32)所示的条件成立。

R*Cos(β’)＝r*Cos(β) (32)

如以下的式(33)那样表示从对另一方的物体22的凹部设定的坐标系CFb向姿势计算用的力测量坐标系CFa的齐次变换矩阵Tb。在此，设ΔPx为点Oa在对另一方的物体22的凹部设定的坐标系CFb的X轴方向上的位置。另外，设ΔPz为点Oa在对另一方的物体22的凹部设定的坐标系CFb的Z轴方向上的位置。

使用齐次变换矩阵Tb以及接触点Ps、接触点Pe1、接触点Pe2在坐标系CFa上的位置，分别以下面的式(34)-(36)来表示接触点Ps、接触点Pe1、接触点Pe2在坐标系CFb上的位置。

[P(点Ps_坐标系CFb)^T、1]^T

＝Tb·[P(点Ps_坐标系CFa)^T、1]^T (34)

[P(点Pe1_坐标系CFb)^T、1]^T

＝Tb·[P(点Pe1_坐标系CFa)^T、1]^T (35)

[P(点Pe2_坐标系CFb)^T、1]^T

＝Tb·[P(点Pe2_坐标系CFa)^T、1]^T (36)

根据式(26)至式(36)，在一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时，在对另一方的物体22的凹部设定的坐标系CFb上，导出以下的式(37)至(41)所表示的条件。

R*Sin(β’)+r*Sin(β)*Cos(α)＝ΔPx (37)

R*Cos(β’)＝r*Cos(β) (38)

r*Sin(β)*Sin(α)＝ΔPz (39)

r*Cos(α)+h*Sin(α)＝R+ΔPx (40)

-r*Sin(α)+h*Cos(α)＝-ΔPz (41)

在此，设R的大小能够近似为与r的大小相同的程度。此时，β能够近似为β’。

使用式(37)至(41)，分别利用以下的式(42)-(45)来计算h、β、ΔPx、ΔPz。

h＝2*r*(1-Cos(α))/Sin(α) (42)

β＝Arcsin((1-Cos(α))/(1+Cos(α))) (43)

ΔPx＝r*(1-Cos(α)) (44)

ΔPz＝

r*Sin(α)*(1-Cos(α))/(1+Cos(α)) (45)

将式(42)和式(43)代入到式(25)，由此利用以下的式(46)来计算角度α。

另外，利用以下的式(47)来计算姿势计算用的力测量坐标系CFa的原点Oa在对另一方的物体22的凹部设定的坐标系CFb中的X轴、Y轴、Z轴上的位置。

[r*(1-Cos(α))、O、

r*Sin(α)*(1-Cos(α))/(1+Cos(α))]^T

(47)

如以上那样，计算姿势计算用的力测量坐标系CFa相对于对另一方的物体22的凹部设定的坐标系CFb的角度α、位置。另外，还计算3个接触点在对另一方的物体22的凹部设定的坐标系CFb上的位置。由此，计算一方的物体21的凸部相对于另一方的物体22的凹部的相对的位置和姿势。

在此，根据式(42)可知，为了使一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触，一方的物体21的凸部的圆柱形状只要在凸部中心轴La的方向上具有2*r*(1-Cos(α))/Sin(α)以上的长度即可。另外，通过式(42)、式(43)求出h、β，因此能够求出接触点Pe1、接触点Pe2、接触点Ps在坐标系CFa上的位置，从而求出接触点在一方的物体21上的位置。

另外，在前述的说明中，将一方的物体21的凸部的形状设为圆柱形状，但是接着说明以下形状的情况：如图5g、图5h所图示那样的锥形状等、一方的物体21的径根据在凸部中心轴La的轴方向上的相距一方的物体21的凸部的顶端的距离而变化的形状那样，接触点与凸部中心轴的各最短距离基于从接触点对凸部中心轴引垂线时的该垂线凸部中心轴的交点与凸部中心轴上的规定的点(所述交点以外的点)的距离而确定。

图12a中图示了一方的物体21的凸部是截头圆锥形状、另一方的物体22的凹部是孔且一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的状态。图12b中图示了以下情形：将与包含相接触的3个接触点的面正交且包含一方的物体21的凸部中心轴的平面作为截面，来观察一方的物体21的凸部是截头圆锥形状、另一方的物体22的凹部是孔且一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触的状态。研究以下情况：使一方的物体21的凸部从如图11a-图11c所示的前述的圆柱形状变为如图12a-图12b所示那样的、凸部的顶端的圆的形状与圆柱形状的情况相同地根据h的大小而径沿着凸部中心轴La的轴方向改变的截头圆锥等形状，一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触。另外，除凸部的物体的径沿着凸部中心轴La的轴方向改变以外的状况、说明与前述相同。此外，将所述凸部中心轴上的规定的点设为姿势计算用的力测量坐标系的原点。在该情况下，仅接触点Ps在姿势计算用的力测量坐标系CFa中的X轴的-方向上的位置改变。在这种状况、即一方的物体21的凸部的径r’根据h而确定的情况下，如果将式(37)至式(41)中的式(40)、式(41)中的r变更为根据h计算出的径r’、例如常数A*h+常数B、常数A*h*h等，则与前述同样地导出作为如前所述的物体的一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触时的对另一方的物体22的凹部设定的坐标系CFb上的几何学条件。在前述的条件下，在将接触点Ps与凸部中心轴的最短距离表示为r’的情况下，式(40)、式(41)分别变为以下的式(48)、(49)。

r’*Cos(α)+h*Sin(α)＝R+ΔPx (48)

-r’*Sin(α)+h*Cos(α)＝-ΔPz (49)

在前述的说明中，具体地研究了将一方的物体21的凸部的圆柱形状设为截头圆锥的形状的情况。如图12a、图12b所示，一方的物体21的凸部的截头圆锥形状与另一方的物体22的凹部以接触点Pe1、接触点Pe2、接触点Ps这3个接触点接触。图12a、图12b所示的参数等与图11a、图11b、前述的说明相同。凸部的物体的径沿着凸部中心轴La的轴方向而改变，这一点与图11a-图11c的状况不同。如图12a-图12b所示，设从接触点Pe1、接触点Pe2到凸部中心轴La为止的各最短距离为r。另外，将从接触点Ps对凸部中心轴La引垂线时的该垂线与凸部中心轴的交点设为Pt。将交点Pt与作为凸部中心轴上的规定的点的点Oa的距离设为h。接触点Ps与凸部中心轴La的最短距离是接触点Ps与交点Pt的距离，根据一方的物体21的凸部的截头圆锥的形状而确定为r+a*h。在此，设a为常数。这样，在一方的物体21的凸部是截头圆锥形状的情况下，一方的物体21的凸部为圆柱形状的情况下的前述的式(40)、式(41)分别变为以下的式(50)、(51)。

(r+a*h)*Cos(α)+h*Sin(α)＝R+ΔPx (50)

-(r+a*h)*Sin(α)+h*Cos(α)＝-ΔPz (51)

通过使用变更后的式(50)、式(51)以及原来的式(37)、式(38)、式(39)、式(41)、式(25)，能够与前述同样地计算角度α。

如以上那样，不仅在接触点与凸部中心轴的各最短距离为固定值的情况下，在接触点与凸部中心轴的各最短距离基于从接触点对凸部中心轴引垂线时的该垂线同凸部中心轴的交点与凸部中心轴上的规定的点(所述交点以外的点)的距离而确定的情况下，也能够计算姿势计算用的力测量坐标系CFa相对于对另一方的物体22的凹部设定的坐标系CFb的角度α、位置，从而能够计算一方的物体21的凸部相对于另一方的物体22的凹部的相对的位置和姿势。此外，在研究将一方的物体21的凸部的截头圆锥形状的侧面向包含另一方的物体22的凹部的孔的端部的平面等存在于另一方的物体22的平面按压的情况下，只要求出锥部分的角度θc并且使用角度α和角度θc来使一方的物体21的凸部的截头圆锥旋转并进行按压即可。

根据由式(33)、式(44)、式(45)、式(46)表示的齐次变换矩阵Tb的反矩阵来计算从姿势计算用的力测量坐标系CFa向对另一方的物体22的凹部设定的坐标系CFb的齐次变换矩阵。也就是说，求出姿势计算用的力测量坐标系CFa与对另一方的物体22的凹部的孔的端部设定的坐标系之间的位置和姿势的关系。由此，能够计算另一方的物体22的凹部的孔的端部的中心位置和端部所存在的面相对于一方的物体21的凸部的姿势，从而计算另一方的物体22的凹部相对于一方的物体21的凸部的相对的位置和姿势。此外，在只要求出另一方的物体22的凹部的孔的端部的中心位置和端部所存在的面的姿势就足够的情况下，不需要对另一方的物体22的凹部设定的坐标系CFb与表示另一方的物体22的位置和姿势的坐标系之间的位置和姿势的关系。

另外，能够如前所述那样针对对空间设定的坐标系、对驱动部50设定的坐标系、工具坐标系等设定或者计算向姿势计算用的力测量坐标系CFa的齐次变换矩阵、位置和姿势。

另外，能够根据针对对空间设定的坐标系、对驱动部50设定的坐标系、工具坐标系等的向姿势计算用的力测量坐标系CFa的齐次变换矩阵以及从姿势计算用的力测量坐标系CFa向对另一方的物体22的凹部设定的坐标系CFb的齐次变换矩阵，来计算针对对空间设定的坐标系、对驱动部50设定的坐标系、工具坐标系等的向对另一方的物体22的凹部设定的坐标系CFb的齐次变换矩阵、位置和姿势。

另外，在对另一方的物体22的凹部设定的坐标系CFb与表示另一方的物体22的位置和姿势的坐标系之间的关系已被设定、或已知、或已被计算出的情况下，能够基于从对另一方的物体22的凹部设定的坐标系CFb向表示另一方的物体22的位置和姿势的坐标系的齐次变换矩阵以及从对空间设定的坐标系、对驱动部50设定的坐标系、工具坐标系等向对另一方的物体22的凹部设定的坐标系CFb的齐次变换矩阵，来计算另一方的物体22相对于对空间设定的坐标系、对驱动部50设定的坐标系、工具坐标系等的位置和姿势。

在对另一方的物体22的凹部设定的坐标系CFb与表示另一方的物体22的位置和姿势的坐标系之间的关系是仅有绕对另一方的物体22的凹部设定的坐标系CFb的Z轴的姿势不同的情况下，也可以计算存在于另一方的物体22的另外的凹部处存在的面的姿势、或者使一方的物体21与另一方的物体22的另外的部分接触并进行检测来计算绕坐标系CFb的Z轴的角度，由此计算出绕所述坐标系CFb的Z轴的姿势。这样，也可以利用另一方的物体22的形状的特征，使原本无法计算的姿势也可以求出。

另外，也可以将表示另一方的物体22的位置和姿势的坐标系预先设定成同将点Pe1与点Pe2连结而得到的直线平行的轴为Y轴，考虑使一方的物体21的凸部与另一方的物体22的凹部接触的状况来设定X轴的+方向，其中，点Pe1是一方的物体21的凸部与另一方的物体22的凹部的接触点。

另外，也可以利用存在于另一方的物体22且相互的位置和/或姿势的关系已知的多个凹部，来计算姿势计算用的力测量坐标系CFa与表示另一方的物体22的位置和姿势的坐标系之间的位置和姿势的关系。首先，计算姿势计算用的力测量坐标系CFa与对存在于另一方的物体22的某个凹部设定的坐标系CFb之间的位置和姿势的关系。接着，利用存在于另一方的物体22的另外的凹部，来计算姿势计算用的力测量坐标系CFa与对另一方的物体22的另外的凹部设定的坐标系CFb’之间的位置和姿势的关系。基于该结果以及存在于另一方的物体22的某个凹部与存在于另一方的物体22的另外的凹部之间的已知的位置和/或姿势的关系，来计算姿势计算用的力测量坐标系CFa与表示另一方的物体22的位置和姿势的坐标系之间的位置和姿势的关系。此时，也可以针对多个凹部计算与姿势计算用的力测量坐标系CFa之间的位置和姿势的关系，使得能够以更高的维度计算另一方的物体22的位置和姿势，或更准确地计算一方的物体21与另一方的物体22的相对的位置和姿势。这样，也可以利用一方的物体21和另一方的物体22的形状、存在于一方的物体21或另一方的物体22的多个凸部或凹部，使得能够以更高的维度计算另一方的物体22的位置和姿势，或更准确地计算一方的物体21与另一方的物体22的相对的位置和姿势。

下面，参照图17来叙述以下方法：利用存在于另一方的物体22的另外的凹部，来计算表示另一方的物体22的位置和姿势的坐标系与对另一方的物体22的凹部设定的坐标系CFb之间仅有绕坐标系CFb的Z轴的姿势不同的情况下的、所述绕Z轴的姿势的偏差。

如图17所图示的那样，另一方的物体22包含凹部22a和凹部22b。在此，设另一方的物体22的凹部22a和另一方的物体22的凹部22b为径相同的圆筒形状。另外，设一方的物体21的凸部也是圆筒形状，且是径与另一方的物体22的凹部22a及另一方的物体22的凹部22b的径相同的圆筒形状。另外，设构成为另一方的物体22的凹部22a的圆筒形状的中心轴与另一方的物体22的凹部22b的圆筒形状的中心轴正交。设另一方的物体22的位置和姿势由坐标系CF0来表示。坐标系CF0由在图17中表示为X0的X轴、表示为Y0的Y轴、表示为Z1的Z轴这3个相互正交的轴构成，设坐标系原点为点Ob。将在通过使一方的物体21的凸部与另一方的物体22的凹部以3个接触点接触来计算一方的物体21的凸部与另一方的物体22的凹部22a的相对的位置和姿势时对另一方的物体22设定的坐标系设为坐标系CFb。坐标系CFb由相互正交的、在图17中表示为X1的X轴、表示为Y1的Y轴、表示为Z1的Z轴构成，设坐标系的原点为点Ob。坐标系CF0与坐标系CFb之间，坐标系的原点一致，且Z轴一致。设坐标系CF0与坐标系CFb之间仅在绕坐标系CFb的Z轴的方向上姿势不同。

接着，将在通过使一方的物体21的凸部与另一方的物体22的凹部22b以3个接触点接触来计算一方的物体21的凸部与另一方的物体22的凹部22b的相对的位置和姿势时对另一方的物体22设定的坐标系设为坐标系CFb2。坐标系CFb2由相互正交的、在图17中表示为X2的X轴、表示为Y2的Y轴、表示为Z2的Z轴构成，坐标系的原点为点Ob2。由此，能够基于坐标系CFa的位置和姿势来计算坐标系CFb与坐标系CFb2的相对位置、相对姿势。

研究使坐标系CFb绕坐标系CFb的Z轴旋转来计算坐标系CF0。在使坐标系CFb绕坐标系CFb的Z轴旋转时，使包含坐标系CFb2的X轴和Y轴的平面与坐标系CFb的Y轴平行、且使点Ob2存在于旋转后的X轴的正方向，或者使X轴与坐标系CFb2的Z轴平行且方向相同。由此，能够使坐标系CFb绕坐标系CFb的Z轴旋转，来计算坐标系CF0。这样，通过使用存在于另一方的物体22的多个凹部的相对的位置和姿势的关系，能够计算另一方的物体22相对于一方的物体21的三维的位置和姿势，从而能够计算更多自由度的姿势。另外，能够基于存在于另一方的物体22的多个凹部的位置和姿势来更准确地计算另一方的物体22的位置和姿势。

如以上那样，通过使用存在于一方的物体21或另一方的物体22的多个凸部或凹部、该物体中的多个凸部或凹部的相对的位置和姿势的关系，能够计算多自由度的姿势、或者能够计算更准确的位置和姿势。另外，也可以基于另一方的物体22的凹部22a与另一方的物体22的凹部22b的相对位置、姿势，来进行另一方的物体22的好坏判定、也就是说判定工件、工具等物体的好坏。例如，也可以对作为判定对象的工件、工具判定状态是否变为与期望的状态不同的状态、是否为有缺陷的不良品、种类是否为与期望的种类不同的种类、工件是否产生了损伤等。

如前所述那样，对使一方的物体21、另一方的物体22存在多个凸部或凹部的形状的另一实施例进行说明。

图18a-图18d是表示使一方的物体21、另一方的物体22包含多个凸部、凹部的情况下的另一例的图。如图18a所示，作为一方的物体21、另一方的物体22所包含的凸部或者凹部，也可以构成为设置3个圆筒形状的部分且所述3个圆筒形状的中心轴相互正交。另外，如图18b所示，作为一方的物体21、另一方的物体22所包含的凸部或者凹部，也可以构成为设置2个圆筒形状的部分且所述2个圆筒形状的中心轴相互正交。另外，如图18c所示，作为一方的物体21、另一方的物体22所包含的凹部，也可以构成为设置3个孔且3个孔的中心轴相互正交。另外，如图18d所示，作为一方的物体21、另一方的物体22所包含的凹部，也可以构成为设置2个孔且2个孔的中心轴相互正交。也可以如图18a、图18b、图18c、图18d所图示的那样，在一方的物体21、另一方的物体22中包含多个凸部或者凹部。另外，如图18a、图18b、图18c、图18d所图示那样的包含多个凸部或者凹部的物体也可以是能够相对于一方的物体21、另一方的物体22进行拆装的物体。

通过以上那样，能够通过驱动部50使一方的物体21与另一方的物体22相对地移动来使一方的物体21与另一方的物体22接触，物体姿势计算部32基于一方的物体21与另一方的物体22以3个接触点接触时由力测量部31测量出的力以及接触点与凸部中心轴的最短距离，来计算一方的物体21与另一方的物体22的相对位置和/或姿势。另外，也可以利用计算出的一方的物体21与另一方的物体22的相对位置和/或姿势，来计算一方的物体21相对于对空间设定的坐标系、对驱动部50设定的坐标系、工具坐标系等坐标系的位置和/或姿势。另外，也可以计算另一方的物体22相对于对空间设定的坐标系、对驱动部50设定的坐标系、工具坐标系等坐标系的位置和/或姿势。另外，还可以计算对空间设定的坐标系、对驱动部50设定的坐标系、工具坐标系等坐标系相对于另一方的物体22的位置和/或姿势。

在本发明的第二实施方式中，优选的是，相对于本发明的第一实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：一方的物体21包含2个以上的所述凸部或所述凹部，或者，另一方的物体22包含2个以上的所述凸部或所述凹部，或者，一方的物体21和另一方的物体22包含2个以上的所述凸部或所述凹部。

关于本发明的第一实施方式中说明的凸部和凹部，例如也可以是，另一方的物体22包含2个以上的所述凹部，一方的物体21包含1个以上的能够以3个接触点与各所述凹部接触的所述凸部。另外，也可以是，另一方的物体22包含1个以上的所述凹部、且包含1个以上的所述凸部，一方的物体21包含1个以上的能够以3个接触点与另一方的物体22的所述凹部接触的所述凸部、且包含1个以上的能够以3个接触点与另一方的物体22的所述凸部接触的所述凹部。

通过事先包含多个所述凸部或者所述凹部，能够根据作为驱动部50的机构部所能够移动的区域、由于与物体之间的干扰而能够移动的区域、物体上的能够接触的部分等系统的状况，来使能够以3个接触点进行接触的物体与位于能够移动的场所的所述凸部或者所述凹部接触。另外，通过针对1个物体中的多个所述凸部或者所述凹部的部分计算相对位置和/或姿势，并且，通过针对1个物体中的多个相互的位置和/或姿势的关系已知的所述凸部或者所述凹部的部分计算相对位置和/或姿势，能够针对一方的物体21与另一方的物体22的相对位置和/或姿势进行更准确的相对位置和/或姿势的计算、还能够计算任意的三维的位置和姿势。

例如，如前所述，也可以在另一方的物体22上安装相互的位置和姿势已知的2个凹部，根据2个凹部的位置和姿势的信息来计算另一方的物体22的位置和姿势。另外，例如也可以在另一方的物体22上安装多个所述凸部或者所述凹部，针对所述多个部分仅计算所述凸部或者所述凹部的位置，根据该多个位置的信息来计算另一方的物体22的位置和姿势。

在本发明的第三实施方式中，优选的是，相对于本发明的第一实施方式或第二实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：一方的物体21或者另一方的物体22所包含的所述凸部和所述凹部中的至少1个物体是能够相对于包含所述至少1个物体的一方的物体21或者另一方的物体22拆装的物体。

关于本发明的第一实施方式中说明的凸部和凹部，通过所述凸部和所述凹部中的至少任1个能够相对于安装有该物体的一方的物体21或者另一方的物体22拆装，也就是说，通过形成能够拆装所述凸部、所述凹部的物体，仅在想要计算一方的物体21与另一方的物体22的相对位置和/或姿势时，将所述凸部和所述凹部中的至少任1个安装于一方的物体21或者另一方的物体22即可。

由此，即使是不具有如所述凸部或者所述凹部那样的部分的物体，也能够根据需要来使其包含所述凸部或者所述凹部。此外，只要在计算出一方的物体21与另一方的物体22的相对位置和/或姿势之后卸下即可。

另外，也可以是，将所述凸部或者所述凹部安装到一方的物体21或者另一方的物体22的某个部分，在计算出一方的物体21与另一方的物体22的相对位置和/或姿势之后，卸下所安装的所述凸部或者所述凹部。接着，将所述凸部或者所述凹部安装到一方的物体21或者另一方的物体22的另外的某个部分，在计算出一方的物体21与另一方的物体22的相对位置和/或姿势之后，卸下所安装的所述凸部或者所述凹部。通过执行多次所述操作，来更准确地计算一方的物体21与另一方的物体22的相对位置和/或姿势、或使得能够计算任意的三维的位置和姿势。

在本发明的第四实施方式中，优选的是，相对于本发明的第一实施方式至第三实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：在使一方的物体21与另一方的物体22以所述3个接触点接触之前，以使一方的物体21与另一方的物体22的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使一方的物体21与另一方的物体22移动到相对的大致位置和大致姿势。

如果使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势，则能够如本发明的第一实施方式中说明的那样计算一方的物体21与另一方的物体22的相对位置和/或姿势。

此时，也可以在使用力传感器、转矩传感器、触觉传感器、接触传感器等对力、接触的感觉、接触进行检测的接触式传感器来自动检测一方的物体21或者另一方的物体22的大致的位置和姿势之后，使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势。

另外，也可以是，在使用利用照相机、激光、LED、超声波等的非接触式传感器来自动检测一方的物体21或者另一方的物体22的大致的位置和姿势之后，使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势。在该情况下，能够在利用无法以很高的精度检测物体的位置和/或姿势、或者只能够识别物体的大致的位置和/或姿势的传感器来移动到大致的范围的位置和姿势之后更准确地计算一方的物体21与另一方的物体22的相对的位置和姿势。

另外，也可以由作业者、示教者利用所述接触式的传感器、所述非接触式的传感器来以引导驱动部50的方式使其移动，从而使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势。

另外，也可以使用由加速度传感器、陀螺仪传感器等构成的示教操作装置，利用所述示教操作装置在三维空间中的倾斜、加速度等来操作驱动部的平移移动、旋转移动，从而使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势。

另外，也可以使用利用照相机、激光、LED、超声波等的用于指定空间内的位置或者位置和姿势的装置，来使驱动部50所移动的一方的物体21或者另一方的物体22移动到空间内的指定的位置或者位置和姿势，从而使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势。

另外，也可以是以下情况：在与个人计算机、驱动部控制装置52通过有线或者无线方式连接的示教装置等中利用系统的CAD模型等来设定一方的物体21与另一方的物体22的相对的位置和姿势，基于该设定，通过驱动部50使实际的一方的物体21与另一方的物体22相对地移动，此时一方的物体21与另一方的物体22变为规定的位置和姿势的范围内的大致的相对的位置和姿势。

如以上那样，在通过各种方法使一方的物体21与另一方的物体22大致地移动到规定的位置和姿势的范围内的大致的相对的位置和姿势的情况下，也能够如本发明的第一实施方式中说明的那样计算一方的物体21与另一方的物体22的相对位置和/或姿势。由此，能够基于计算出的相对位置和/或姿势来使驱动部50所移动的一方的物体21或者另一方的物体22准确地移动到期望的位置和/或姿势、或适当地执行期望的作业。

在本发明的第五实施方式中，优选的是，相对于本发明的第一实施方式至第三实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：所述力测量部还测量作用于所述驱动部或者一方的物体21或者另一方的物体22的力，在使一方的物体21与另一方的物体22以所述3个接触点接触之前，基于由所述力测量部测量出的力，以使一方的物体21与另一方的物体22的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使一方的物体21与另一方的物体22移动到相对的大致位置和大致姿势。

也可以是，作业者通过力测量部31测量作用于驱动部50或者作用于驱动部50所移动的一方的物体21或另一方的物体22的力，基于进行作用的力来以引导驱动部50的方式使其移动，从而使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势。由此，作业者能够直观且直接地使驱动部50对驱动部50所要移动的物体进行移动。

另外，也可以是，作业者通过力测量部31测量作用于驱动部50所不移动的一方的物体21或者另一方的物体22的力，基于进行作用的力来以引导驱动部50的方式使其移动，从而使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势。此外，此时，为使作业者能够检测作用于驱动部50所不移动的一方的物体21或者另一方的物体22的力，事先设置另外的力传感器，或者，在驱动部50所不移动的一方的物体21或者另一方的物体22位于另外的驱动部50’的情况下使得能够测量并计算作用于驱动部50’的力。由此，作业者在使一方的物体21与另一方的物体22相对地移动时，能够容易地向从驱动部50所不移动的一方的物体21或者另一方的物体22来看的想要相对地移动的期望的方向移动。

另外，也可以是，在力测量部31测量作用于驱动部50或者作用于驱动部50所移动的物体的力的情况下，使驱动部50移动来与不是驱动部50所移动的物体那一方物体接触，由此计算不是驱动部50所移动的物体的那一方物体的大致的位置和姿势。基于计算出的该位置和姿势，来使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势。

这样，在使一方的物体21与另一方的物体22大致移动到规定的位置和姿势的范围内的大致的相对的位置和姿势的情况下，也能够如本发明的第一实施方式中说明的那样计算一方的物体21与另一方的物体22的相对位置和/或姿势。由此，能够基于计算出的相对位置和/或姿势，使驱动部50所移动的一方的物体21或者另一方的物体22准确地移动到期望的位置和/或姿势，或者适当地执行期望的作业。

在本发明的第六实施方式中，优选的是，相对于本发明的第一实施方式至第三实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：所述物体的姿势计算系统还具备物体检测部34，该物体检测部34能够在规定的范围以内检测一方的物体21或者另一方的物体22的位置和/或姿势，在使一方的物体21与另一方的物体22以所述3个接触点接触之前，基于物体检测部34的检测结果，以使一方的物体21与另一方的物体22的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使一方的物体21与另一方的物体22移动到相对的大致位置和大致姿势。

所述物体的姿势计算系统还具备物体检测部34，该物体检测部34使用利用照相机、激光、LED、超声波等的非接触式传感器、或者力传感器、转矩传感器、触觉传感器、接触传感器等对力、接触的感觉、接触进行检测的接触式传感器，该物体检测部34能够在规定的范围以内检测一方的物体21或者另一方的物体22、其凸部或者凹部的位置和/或姿势，或者，能够在规定的范围以内检测不是驱动部50所移动的物体的那一方物体、其凸部或者凹部的位置和/或姿势。此外，也可以是，例如在将驱动部50所移动的物体设为一方的物体21时，由于一方的物体21和另一方的物体22的物体形状复杂、产生干扰等，在使一方的物体21相对于另一方的物体22的相对位置和/或姿势移动到规定的范围以内的情况下，当需要以某种程度识别一方的物体21的物体的形状时，在规定的范围内检测作为驱动部50所移动的物体的一方的物体21的位置和/或姿势。

在通过物体检测部34检测出一方的物体21或者另一方的物体22的位置和/或姿势之后，基于所述检测结果来使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势。

这样，在使一方的物体21与另一方的物体22大致移动到规定的位置和姿势的范围内的大致的相对的位置和姿势的情况下，也能够如本发明的第一实施方式中说明的那样计算一方的物体21与另一方的物体22的相对位置和/或姿势。由此，能够基于计算出的相对位置和/或姿势来使驱动部50所移动的一方的物体21或者另一方的物体22准确地移动到期望的位置和/或姿势，或适当地执行期望的作业。

物体检测部34只要能够在规定的范围以内检测物体的位置和/或姿势即可，因此只要由能够获取所需最低限度的信息的传感器等构成即可。因此，不需要能够高精度地检测物体的位置和/或姿势的传感器等，另外，不使用复杂的传感器，因此能够减少传感器的故障等。在利用这种无法以很高的精度检测物体的位置和/或姿势、或者只能够识别物体的大致的位置和/或姿势的传感器来移动到大致的范围的位置和姿势之后，能够更准确地计算一方的物体21与另一方的物体22的相对的位置和姿势。因此，能够成为廉价且系统运用上的稳定性高的系统。

在本发明的第七实施方式中，优选的是，相对于本发明的第一实施方式至第三实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：所述物体的姿势计算系统还具备空间位置指定部35，该空间位置指定部35由指定空间内的位置或者位置和姿势的装置以及识别所指定的所述位置或者位置和姿势的装置构成，在使一方的物体21与另一方的物体22以所述3个接触点接触之前，基于由空间位置指定部35指定的位置或者位置和姿势，以使一方的物体21与另一方的物体22的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使一方的物体21与另一方的物体22移动到相对的大致位置和大致姿势。

优选的是，物体的姿势计算系统还具备空间位置指定部35，该空间位置指定部35由利用照相机、激光、LED、超声波等的、指定空间内的位置或者位置和姿势的装置以及识别所指定的所述位置或者位置和姿势的装置构成。例如也可以是，为了能够指定位置或者位置和姿势，配置标记并利用照相机来识别该标记，或者利用激光对想要指定位置或者位置和姿势的凸部或者凹部照射点、十字，或者照射LED、超声波，并且使用受光器、接收机，从而构成空间位置指定部35。

也可以是，基于由空间位置指定部35对所指定的位置或者位置和姿势进行识别而得到的所述识别结果，来使一方的物体21与另一方的物体22移动到规定的位置和姿势的范围内的大致的相对的位置和姿势。

这样，在使一方的物体21和另一方的物体22大致移动到规定的位置和姿势的范围内的大致的相对的位置和姿势的情况下，也能够如本发明的第一实施方式中说明的那样计算一方的物体21与另一方的物体22的相对位置和/或姿势。由此，能够基于计算出的相对位置和/或姿势来使驱动部50所移动的一方的物体21或者另一方的物体22准确地移动到期望的位置和/或姿势，或者适当地执行期望的作业。

在本发明的第八实施方式中，优选的是，相对于本发明的第一实施方式至第七实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：基于所述物体姿势计算部所计算出的一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势，来进行一方的物体21或者另一方的物体22相对于所述驱动部的位置和/或姿势的计算。

优选的是，物体的姿势计算系统基于所述物体姿势计算部32所计算出的一方的物体21与另一方的物体22的相对位置和/或姿势，来计算一方的物体21或者另一方的物体22相对于驱动部50的位置和/或姿势。此外，也可以是，驱动部50使包含凸部的一方的物体21移动，计算针对一方的物体21的姿势计算用的力测量坐标系CFa，在与工具坐标系姿势不同的情况下，对工具坐标系进行校正。另外，在已知驱动部50所保持且移动的一方的物体21的凸部的位置、但是不知道一方的物体21的姿势的情况下，计算针对一方的物体21的姿势计算用的力测量坐标系CFa，由此求出一方的物体21的姿势。另外，也可以是，在已知由驱动部50保持并使之移动的一方的物体21的与另一方的物体22的凹部接触的凸部的位置、但是不知道存在于一方的物体21的另外的多个凸部的准确的位置和/或姿势且不知道确定一方的物体21的位置和姿势的坐标系的位置和姿势的情况下，也就是说，在就与另一方的物体22的凹部接触的一方的物体21的凸部处设定的某个点而言一方的物体21的姿势改变的情况下，根据通过使存在于一方的物体21的另外的多个凸部与另一方的物体22的凹部接触而求出的存在于一方的物体21的另外的多个凸部的位置和/或姿势，来求出确定一方的物体21的位置和姿势的坐标系的位置和姿势，从而求出一方的物体21相对于驱动部50的位置和姿势。

通过计算一方的物体21或者另一方的物体22相对于驱动部50的位置和/或姿势，可知一方的物体21或者另一方的物体22相对于驱动部50的位置和/或姿势，因此能够更加适当地、准确地执行期望的作业。

在本发明的第九实施方式中，优选的是，相对于本发明的第一实施方式至第八实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：基于所述物体姿势计算部所计算出的一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势，来校正使所述驱动部所移动的一方的物体21和/或另一方的物体22移动到的位置和/或姿势。

基于所述物体姿势计算部32所计算出的一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势，来校正使驱动部50所移动的一方的物体21和/或另一方的物体22移动到的位置和/或姿势，由此，在使用一方的物体21和另一方的物体22的作业、例如组装作业、加工作业等中，能够执行更加适当且准确的作业。

另外，在物体的姿势计算系统中的驱动部如图2所示那样存在使一方的物体21移动的驱动部50以及使另一方的物体22移动的驱动部50’这2个的情况下、且驱动部50与驱动部50’的位置和姿势的关系已知、一方的物体21相对于驱动部50的位置和姿势已知、另一方的物体22相对于驱动部50’的位置和姿势未知的情况下，能够如本发明的第一实施方式中说明的那样计算一方的物体21与另一方的物体22的相对的位置和姿势。由此，基于计算出的相对的位置和姿势，另一方的物体22相对于驱动部50’的位置和姿势成为已知的，因此驱动部50’能够使另一方的物体22准确地移动到期望的位置和/或姿势、或能够适当地执行期望的作业。

在本发明的第十实施方式中，优选的是，相对于本发明的第一实施方式至第九实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：基于所述物体姿势计算部所计算出的一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势，来执行一方的物体21或者另一方的物体22的好坏判定、一方的物体21或另一方的物体22的种类判定、所述驱动部50所保持的一方的物体21或另一方的物体22的保持状态的判定中的至少1个判定。

如本发明的第一实施方式中说明的那样，通过使一方的物体21与另一方的物体22以3个接触点接触，能够计算一方的物体21与另一方的物体22的相对位置和/或姿势。另外，也能够计算所述3个接触点相对于一方的物体21和另一方的物体22的位置。另外，能够基于一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势来计算驱动部50所不移动的物体相对于驱动部50的姿势或者位置和姿势。也可以基于这些计算得到的结果，针对一方的物体21或者另一方的物体22执行好坏判定、种类判定等各种判定，或者进行驱动部50所移动且保持的一方的物体21和/或另一方的物体22的保持状态的判定。

在此，物体的好坏判定是指进行物体是否为意想的物体的、工件、工具等物体的好坏的判定。例如，对作为判定对象的工件、工具等判定状态是否变为与意想的状态不同的状态、是否为有缺陷的不良品、种类是否为与意想的种类不同的种类、工件是否产生了损伤等。

另外，物体的种类判定是对于物体的种类的判定。另外，物体的保持状态的判定是驱动部50的顶端部分与所保持的物体是否处于规定的姿势的关系等的判定。

也可以进行以下的好坏判定：在驱动部50使一方的物体21移动时，在另一方的物体22相对于一方的物体21的位置和/或姿势处于规定的位置和/或姿势的范围内的情况下，判定为另一方的物体22是意想的物体、判定为好，在不处于规定的位置和/或姿势的范围内的情况下，判定为另一方的物体22不是意想的物体、判定为不好。另外，也可以进行以下的保持状态的判定：在驱动部50使一方的物体21移动时，在另一方的物体22相对于一方的物体21的位置和/或姿势处于规定的位置和/或姿势的范围内的情况下，判定为一方的物体21是意想的保持状态，在不处于规定的位置和/或姿势的范围内的情况下，判定为一方的物体21不是意想的保持状态。

另外，进行以下的种类判定：在驱动部50使一方的物体21移动时，在另一方的物体22相对于一方的物体21的位置和/或姿势处于规定的位置和/或姿势的范围内的情况下，判定为另一方的物体22是某个物体，在不处于规定的位置和/或姿势的范围内的情况下，判定为是另外的物体。此时，也可以进行以下的种类判定：根据规定的位置和/或姿势的范围来细致地判定物体的种类。

另外，在设一方的物体21存在凸部、驱动部50使一方的物体21移动时，使其与另一方的物体22的凹部接触，由此对一方的物体21的凸部设定姿势计算用的力测量坐标系。根据此时完成了设定的姿势计算用的力测量坐标系的姿势与对一方的物体21的凸部设定的工具坐标系的姿势的不同，来进行以下的保持状态等的判定：相对于驱动部50，一方的物体21是期望的姿势、还是接近期望的姿势的状态、还是非意想的姿势。另外，也可以根据完成了设定的姿势计算用的力测量坐标系的姿势与对一方的物体21的凸部设定的工具坐标系的姿势的相对姿势的大小，来判定是否以不期望的姿势被获取并被保持、判定所保持的一方的物体21的种类，或者判定所保持的物体是否为意想的工具、工件等物体。

另外，也可以基于所述3个接触点相对于一方的物体21或者另一方的物体22的位置来执行以下判定等：一方的物体21、另一方的物体22是否为意想的物体的好坏判定；判定一方的物体21、另一方的物体22的物体的种类的种类判定；所保持的物体是否为期望的姿势或接近期望的姿势的状态，另外，所保持的物体的种类、所保持的物体是否为意想的物体等保持状态的判定。

此外，既可以执行前述的好坏判定、种类判定、保持状态的判定中的至少1个判定，也可以同时执行多个判定。此时，通过将多个判定进行组合，能够提高各种判定的准确度。

在一方的物体21或者另一方的物体22存在多个凸部或者凹部时，也可以基于计算出的多个凸部或者凹部的相对位置和/或姿势，来执行包含多个凸部或者凹部的所述物体的好坏判定、种类判定。另外，也可以基于已知的多个凸部或者凹部的位置和/或姿势的关系以及计算出的多个凸部或者凹部的相对位置和/或姿势，来执行包含多个凸部或者凹部的所述物体的好坏判定、种类判定。

在本发明的第十一实施方式中，优选的是，相对于本发明的第一实施方式至第十实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：在一方的物体21和另一方的物体22的所述凸部与所述凹部的组合中，能够使所述凸部与所述凹部相嵌合，所述驱动部使一方的物体21与另一方的物体22相对地移动，并且，保持一方的物体21和另一方的物体22中的至少1个物体来使其移动，并且，能够以使一方的物体21与另一方的物体22相嵌合的方式移动，所述物体的姿势计算系统还具备嵌合执行部36，该嵌合执行部36基于由所述力测量部测量出的力，通过所述驱动部使一方的物体21与另一方的物体22相对地移动并执行力控制，来使一方的物体21与另一方的物体22相嵌合，嵌合执行部36基于由所述物体姿势计算部计算出的一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势，来使一方的物体21与另一方的物体22相嵌合。此外，如前所述，嵌合执行部36也可以由驱动部控制装置52所具备，还可以与物体姿势计算部32同样地包括于与驱动部控制装置52连接的中央控制装置、另外的控制装置、运算处理装置等。

设一方的物体21的凸部与另一方的物体22的凹部、或者一方的物体21的凹部与另一方的物体22的凸部为以下物体：如本发明的第一实施方式中说明的那样满足规定的条件，并且如本发明的第一实施方式中说明的那样能够嵌合。另外，也可以是，能够嵌合的所述凸部或者所述凹部能够相对于包含该凸部或凹部的一方的物体21或者另一方的物体22拆装。另外，如本发明的第一实施方式那样，驱动部50能够如由6个轴构成的垂直多关节型机器人那样以使一方的物体21与另一方的物体22相嵌合的方式移动。另外，优选的是，所述物体的姿势计算系统11还具备嵌合执行部36，该嵌合执行部36基于由力测量部31测量出的力，通过驱动部50使一方的物体21与另一方的物体22相对地移动，并执行力控制来使一方的物体21与另一方的物体22相嵌合。

嵌合执行部36利用由物体姿势计算部32计算出的一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势，由此，即使在相对姿势大的情况下，也能够使一方的物体21与另一方的物体22以短时间可靠地嵌合。在一方的物体21与另一方的物体22的相对姿势大的情况下，很多时候难以通过力控制使一方的物体21与另一方的物体22相嵌合。根据本发明，预先以使相对姿势变小的方式移动之后通过力控制进行嵌合，或者一边基于计算出的相对姿势或者相对的位置和姿势来进行力控制一边进行嵌合，由此使一方的物体21与另一方的物体22能够可靠地相嵌合且能够以短时间相嵌合。

在本发明的第十二实施方式中，优选的是，相对于本发明的第十一实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：嵌合执行部36基于所述物体姿势计算部所计算出的一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势，在使一方的物体21与另一方的物体22移动以修正一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势之后，使一方的物体21与另一方的物体22相嵌合。

在一方的物体21与另一方的物体22的相对姿势大的情况下，在预先以使相对姿势变小的方式移动之后执行嵌合作业，由此，使一方的物体21与另一方的物体22能够可靠地相嵌合且能够以短时间相嵌合。

在本发明的第十三实施方式中，优选的是，相对于本发明的第十一实施方式的物体的姿势计算系统11，设为以下的物体的姿势计算系统：嵌合执行部36基于所述物体姿势计算部所计算出的一方的物体21与另一方的物体22的相对姿势或者相对的位置和姿势，在一方的物体21与另一方的物体22的相对姿势大时，以使将一方的物体21与另一方的物体22的相对姿势变小的移动速度变大的方式，基于由所述力测量部测量出的力来进行力控制，使一方的物体21与另一方的物体22相嵌合。

在通过力控制使一方的物体21与另一方的物体22相嵌合时，在一方的物体21与另一方的物体22的相对姿势大时，也就是说，在利用预先计算出的相对姿势使一方的物体21与另一方的物体22向减小相对姿势的方向相对地移动基于相对姿势的大小的规定的角度时，为了使旋转移动速度变大，减小力控制的力控制增益来减小与力相应的移动指令量，增加与移动指令量相加的旋转速度指令等来调整速度。由此，能够大致地快速移动直到设想为相对姿势的大小变小为止。

在使一方的物体21与另一方的物体22向减小相对姿势的方向相对地移动基于相对姿势的大小的规定的角度之后，增大力控制的力控制增益来增大与力相应的移动指令量，减小与该移动指令量相加的旋转速度指令等来调整速度。由此，在设想为相对姿势的大小已变小的状况下，能够与力相应地细致移动而进行精密的动作。

另外，优选的是，在逐渐改变一方的物体21与另一方的物体22的相对姿势时，基于预先计算出的一方的物体21与另一方的物体22的相对位置，在接近图11a-图11c所示的一方的物体21的凸部中心轴La与另一方的物体22的凹部的孔的中心轴重合的状态之前，即、在凸部中心轴La上与点Ob的最短距离大于规定的值时，大致快速地移动，在凸部中心轴La上与点Ob的最短距离已变为规定的值以下时，根据力细微地移动来进行精密的动作。这样，关于平移方向，也是如下那样来根据状况来改变力控制的动作：在对另一方的物体22设定的坐标系CFb的X轴、Y轴方向上，大致地快速移动直到设想为相对位置的大小变小为止，在设想为相对位置的大小已变小的状况下，根据力细微地移动来进行精密的动作。

通过如前所述那样根据状况来改变力控制的动作，能够使一方的物体21与另一方的物体22可靠地相嵌合，并且能够以短时间相嵌合。

Claims (13)

1.一种物体的姿势计算系统，计算一方的物体与另一方的物体的相对姿势、或者计算所述一方的物体与所述另一方的物体的相对的位置和姿势，该物体的姿势计算系统的特征在于，具备：

所述一方的物体和所述另一方的物体，所述一方的物体和所述另一方的物体是包含凸部的物体与包含凹部的物体的组合、且是所述凸部与所述凹部能够在3个接触点接触的物体，并且，所述凹部处的所述3个接触点位于所述凹部的孔的端部，并且，当将针对所述凸部的、不与包含所述3个接触点的平面正交的轴作为凸部中心轴时，所述接触点与所述凸部中心轴的各最短距离为固定值，或者，所述接触点与所述凸部中心轴的各最短距离基于从所述接触点对所述凸部中心轴引垂线时的该垂线同所述凸部中心轴的交点与所述凸部中心轴上的所述交点以外的点的距离而确定；

一个以上的驱动部，所述驱动部使所述一方的物体与所述另一方的物体相对地移动，并且，保持所述一方的物体和所述另一方的物体中的至少1个物体来使其移动；

力测量部，其测量作用于所述一方的物体与所述另一方的物体之间的力；以及

物体姿势计算部，其通过所述驱动部使所述一方的物体与所述另一方的物体相对地移动来使所述一方的物体与所述另一方的物体接触，基于所述一方的物体与所述另一方的物体以所述3个接触点接触时由所述力测量部测量出的力以及所述最短距离，来计算所述一方的物体与所述另一方的物体的相对姿势、或者计算所述一方的物体与所述另一方的物体的相对的位置和姿势。

2.根据权利要求1所述的物体的姿势计算系统，其特征在于，

所述一方的物体包含2个以上的所述凸部或所述凹部，或者，

所述另一方的物体包含2个以上的所述凸部或所述凹部。

3.根据权利要求1所述的物体的姿势计算系统，其特征在于，

所述一方的物体或者所述另一方的物体所包含的所述凸部和所述凹部中的至少1个物体是能够相对于包含所述至少1个物体的所述一方的物体或者所述另一方的物体拆装的物体。

4.根据权利要求1～3中的任一项所述的物体的姿势计算系统，其特征在于，

在使所述一方的物体与所述另一方的物体以所述3个接触点接触之前，以使所述一方的物体与所述另一方的物体的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使所述一方的物体与另一方的物体移动到相对的大致位置和大致姿势。

5.根据权利要求1～3中的任一项所述的物体的姿势计算系统，其特征在于，

所述力测量部还测量作用于所述驱动部或者所述一方的物体或者所述另一方的物体的力，

在使所述一方的物体与所述另一方的物体以所述3个接触点接触之前，基于由所述力测量部测量出的力，以使所述一方的物体与所述另一方的物体的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使所述一方的物体与另一方的物体移动到相对的大致位置和大致姿势。

6.根据权利要求1～3中的任一项所述的物体的姿势计算系统，其特征在于，

所述物体的姿势计算系统还具备物体检测部，该物体检测部能够在规定的范围以内检测所述一方的物体或者所述另一方的物体的位置和姿势中的至少一方，

在使所述一方的物体与所述另一方的物体以所述3个接触点接触之前，基于所述物体检测部的检测结果，以使所述一方的物体与所述另一方的物体的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使所述一方的物体与另一方的物体移动到相对的大致位置和大致姿势。

7.根据权利要求1～3中的任一项所述的物体的姿势计算系统，其特征在于，

所述物体的姿势计算系统还具备空间位置指定部，该空间位置指定部由指定空间内的位置或者位置和姿势的装置以及识别所指定的所述位置或者位置和姿势的装置构成，

在使所述一方的物体与所述另一方的物体以所述3个接触点接触之前，基于由所述空间位置指定部指定的位置或者位置和姿势，以使所述一方的物体与所述另一方的物体的相对的位置和姿势变为规定的位置和姿势的范围内的方式，预先使所述一方的物体与另一方的物体移动到相对的大致位置和大致姿势。

8.根据权利要求1～3中的任一项所述的物体的姿势计算系统，其特征在于，

基于所述物体姿势计算部所计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，来进行所述一方的物体或者所述另一方的物体相对于所述驱动部的位置和姿势中的至少一方的计算。

9.根据权利要求1～3中的任一项所述的物体的姿势计算系统，其特征在于，

基于所述物体姿势计算部所计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，来校正使所述驱动部所移动的所述一方的物体或者所述另一方的物体或者所述一方的物体和所述另一方的物体移动到的位置或者姿势或者位置和姿势。

10.根据权利要求1～3中的任一项所述的物体的姿势计算系统，其特征在于，

基于所述物体姿势计算部所计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，来执行所述一方的物体或者所述另一方的物体的好坏判定、或者所述一方的物体或所述另一方的物体的种类判定、或者由所述驱动部保持的所述一方的物体或所述另一方的物体的保持状态的判定中的至少1个判定。

11.根据权利要求1～3中的任一项所述的物体的姿势计算系统，其特征在于，

在所述一方的物体和所述另一方的物体的所述凸部与所述凹部的组合中，能够使所述凸部与所述凹部相嵌合，

所述驱动部使所述一方的物体与所述另一方的物体相对地移动，并且，保持所述一方的物体和所述另一方的物体中的至少1个物体来使其移动，并且，能够以使所述一方的物体与所述另一方的物体相嵌合的方式移动，

所述物体的姿势计算系统还具备嵌合执行部，该嵌合执行部基于由所述力测量部测量出的力，通过所述驱动部使所述一方的物体与所述另一方的物体相对地移动并执行力控制，来使所述一方的物体与所述另一方的物体相嵌合，

所述嵌合执行部基于由所述物体姿势计算部计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，来使所述一方的物体与所述另一方的物体相嵌合。

12.根据权利要求11所述的物体的姿势计算系统，其特征在于，

所述嵌合执行部基于所述物体姿势计算部所计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，在使所述一方的物体与所述另一方的物体移动以修正所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势之后，使所述一方的物体与所述另一方的物体相嵌合。

13.根据权利要求11所述的物体的姿势计算系统，其特征在于，

所述嵌合执行部基于所述物体姿势计算部所计算出的所述一方的物体与所述另一方的物体的相对姿势或者相对的位置和姿势，在所述一方的物体与所述另一方的物体的相对姿势大时，以使将所述一方的物体与所述另一方的物体的相对姿势变小的移动速度变大的方式，基于由所述力测量部测量出的力来进行力控制，使所述一方的物体与所述另一方的物体相嵌合。