CN109227601B - 控制装置、机器人、机器人系统及校正方法 - Google Patents

Abstract

本发明提供一种控制装置、机器人、机器人系统及校正方法，能够容易地进行机器人用摄像机的校正。臂控制部以如下方式使臂动作：使校正用图案分别以彼此线性独立的三个旋转轴的各旋转轴为中心旋转，并在多个旋转位置停止。摄像机控制部使摄像机拍摄在多个旋转位置的校正用图案的图案图像。摄像机校正执行部使用在多个旋转位置拍摄到的图案图像，推定可计算目标坐标系和摄像机坐标系之间的坐标转换的摄像机的参数。

Description

控制装置、机器人、机器人系统及校正方法

技术领域

本发明涉及机器人用摄像机的校正(校准)。

背景技术

为了使机器人进行高度的处理，有时在机器人上设置摄像机使其具有眼睛的功能。作为摄像机的设置方法，有将摄像机与机器人臂独立地设置的方法和设置于手末端以使摄像机与机器人臂的运动联动的方法(手眼)。

在专利文献1中公开了进行与机器人臂独立设置的摄像机相关的校正(校准)的系统。该系统的目的在于成为使校正夹具的特征部不依存照明条件而稳定地并且精度良好地检测、以及低成本并且容易操作的系统。

现有技术文献

专利文献

专利文献1：日本特开2010-139329号公报。

在专利文献1公开的技术中，需要预先高精度地掌握校正夹具的特征部和校正对象的相对位置关系。例如，在求出摄像机的外部参数时，需要以使校正夹具的特征部和安装有摄像机的支承件的相对位置及相对姿势成为指定值的方式配置特征部。然而，预先高精度地设定校正夹具的特征部和校正对象的相对位置关系未必容易。因此，期待用与专利文献1不同的方法，能够容易地进行摄像机的校正的技术。

发明内容

本发明为了解决上述的课题的至少一部分而完成，能够以以下的方式(方面)实现。

(1)根据本发明的第一方式，可提供一种控制装置，其控制机器人和摄像机，所述机器人具备构成为能够设置所述摄像机的校正用图案的臂，所述摄像机与所述臂独立地设置。该控制装置具备：臂控制部，控制所述臂；摄像机控制部，控制所述摄像机；以及摄像机校正执行部，确定能够计算目标坐标系和所述摄像机的摄像机坐标系之间的坐标转换的所述摄像机的参数，所述目标坐标系具有相对于所述机器人的机器人坐标系已知的相对位置姿势。所述臂控制部使所述臂动作，以使所述校正用图案分别以彼此线性独立的三个旋转轴的各旋转轴为中心进行旋转，并在多个旋转位置停止。所述摄像机控制部使所述摄像机拍摄所述多个旋转位置中的所述校正用图案的图案图像。所述摄像机校正执行部使用在所述多个旋转位置拍摄到的所述图案图像确定所述参数。

根据该控制装置，使用绕各旋转轴旋转中的多个旋转位置的图案图像，可推定在摄像机坐标系观察到的三个旋转轴的方向。此外，由于三个旋转轴彼此线性独立，因此从它们的旋转轴的方向可确定目标坐标系和摄像机坐标系之间的坐标转换矩阵。结果，由于可获得可计算目标坐标系和摄像机坐标系之间的坐标转换的摄像机的参数，因此可进行使用了摄像机的对象物的位置检测。

(2)在上述控制装置中，也可以是所述三个旋转轴设定在所述目标坐标系的原点周围。

根据该控制装置，由于三个旋转轴和目标坐标系的对应关系简单，因此由在摄像机坐标系观察到的旋转轴的方向能够容易地确定目标坐标系和摄像机坐标系之间的坐标转换矩阵。

(3)在上述控制装置中，也可以是所述摄像机校正执行部根据在所述多个旋转位置拍摄到的所述图案图像来推定以各旋转轴的方向为矢量方向、以所述旋转的角度作为矢量长度的三个旋转矢量；所述摄像机校正执行部分别将所述三个旋转矢量正规化，求出三个正规化旋转矢量；所述摄像机校正执行部通过将所述三个正规化旋转矢量作为行分量或列分量排列来确定构成所述目标坐标系和所述摄像机坐标系之间的坐标转换矩阵的旋转矩阵。

根据该控制装置，可由在以各旋转轴为中心的旋转中的多个旋转位置拍摄到的图案图像，容易地求出构成目标坐标系和摄像机坐标系之间的坐标转换矩阵的旋转矩阵。

(4)在上述控制装置中，所述目标坐标系和所述摄像机坐标系之间的坐标转换矩阵可由所述摄像机坐标系和所述校正用图案的图案坐标系之间的第一转换矩阵与所述图案坐标系和所述目标坐标系之间的第二转换矩阵的乘积来表示。此时，也可以是所述摄像机校正执行部：(a)根据在所述多个旋转位置中的一个特定旋转位置拍摄到的所述图案图像来推定所述第一转换矩阵；(b)根据在所述多个旋转位置拍摄到的所述图案图像来推定构成所述第二转换矩阵的平移矢量的三个分量中与各旋转轴正交的两个坐标轴方向上的两个平移矢量分量的平方和，并根据对所述三个旋转轴分别推定的所述平移矢量分量的平方和来算出构成所述第二转换矩阵的所述平移矢量；以及(c)根据在所述特定旋转位置来推定的所述第一转换矩阵和所述第二转换矩阵的平移矢量算出构成所述坐标转换矩阵的平移矢量。

根据该控制装置，可根据在以各旋转轴为中心的旋转中的多个旋转位置拍摄到的图案图像，容易地求出构成目标坐标系和摄像机坐标系之间的坐标转换矩阵的平移矢量。

(5)在上述控制装置中，也可以是所述目标坐标系是具有与所述臂独立地相对于所述机器人的机器人坐标系固定的相对位置姿势的坐标系。

根据该控制装置，由于求出与臂独立设置的目标坐标系和摄像机坐标系之间的坐标转换矩阵，因此可在远离臂的位置提高使用了摄像机的对象物的位置检测精度。

(6)在上述控制装置中，也可以是所述目标坐标系是所述臂的手末端坐标系。

根据该控制装置，可在臂的手末端位置提高使用了摄像机的对象物的位置检测精度。

(7)根据本发明的第二方式，可提供一种控制具备能够设置摄像机的校正用图案而构成的臂的机器人和与所述臂独立地设置的摄像机的控制装置。该控制装置具备处理器。所述处理器使所述臂动作，以使所述校正用图案分别以彼此线性独立的三个旋转轴的各旋转轴为中心旋转，并在多个旋转位置停止；所述处理器使所述摄像机拍摄所述多个旋转位置中的所述校正用图案的图案图像；所述处理器使用在所述多个旋转位置拍摄到的所述图案图像，确定能够计算目标坐标系和所述摄像机的摄像机坐标系之间的坐标转换的所述摄像机的参数，所述目标坐标系具有相对于所述机器人的机器人坐标系已知的相对位置姿势。

根据该控制装置，使用绕各旋转轴旋转中的多个旋转位置的图案图像，可推定在摄像机坐标系观察到的三个旋转轴的方向。此外，由于三个旋转轴彼此线性独立，因此根据它们的旋转轴的方向可确定目标坐标系和摄像机坐标系之间的坐标转换矩阵。结果，由于可获得可计算目标坐标系和摄像机坐标系之间的坐标转换的摄像机的参数，因此可进行使用了摄像机的对象物的位置检测。

(8)本发明的第三方式是与上述控制装置连接的机器人。

根据该机器人，可进行目标坐标系和摄像机坐标系之间的坐标转换，并且可进行使用了摄像机的对象物的位置检测。

(9)本发明的第四方式是具备机器人和与所述机器人连接的上述控制装置的机器人系统。

根据该机器人系统，可进行目标坐标系和摄像机坐标系之间的坐标转换，并且可进行使用了摄像机的对象物的位置检测。

(10)根据本发明的第五方式，可提供一种校正方法，是在具备能够设置摄像机的校正用图案而构成的臂的机器人和与所述臂独立地设置的摄像机的机器人系统中进行所述摄像机的校正的方法。所述校正方法包括：使所述臂动作，以使所述校正用图案分别以彼此线性独立的三个旋转轴的各旋转轴为中心旋转，并在多个旋转位置停止；使所述摄像机拍摄所述多个旋转位置中的所述校正用图案的图案图像；以及使用在所述多个旋转位置拍摄到的所述图案图像，确定能够计算目标坐标系和所述摄像机的摄像机坐标系之间的坐标转换的所述摄像机的参数，所述目标坐标系具有相对于所述机器人的机器人坐标系已知的相对位置姿势。

根据该方法，使用绕各旋转轴旋转中的多个旋转位置的图案图像，可推定在摄像机坐标系观察到的三个旋转轴的方向。此外，由于三个旋转轴彼此线性独立，因此由它们的旋转轴的方向可确定目标坐标系和摄像机坐标系之间的坐标转换矩阵。结果，由于可获得可计算目标坐标系和摄像机坐标系之间的坐标转换的摄像机的参数，因此可进行使用了摄像机的对象物的位置检测。

本发明还可以以上述以外的各种方式来实现。例如，能够以用于实现控制装置的功能的计算机程序、非暂时性存储了该计算机程序的存储介质(non-transitory storagemedium：非易失性存储介质)等方式实现。

附图说明

图1是机器人系统的示意图。

图2是示出机器人和控制装置的功能的框图。

图3是示出机器人的坐标系的说明图。

图4是示出实施方式的处理顺序的流程图。

图5是示出在多个旋转位置的图案图像的例子的说明图。

图6是示出在图4的步骤S160获得的旋转矩阵的例子的图。

图7是在摄像机坐标系的YZ平面上投影平移矢量的图。

图8是示出在图4的步骤S170获得的平移矢量的例子的图。

图9是示出第二实施方式中的机器人的坐标系的说明图。

附图标记说明

100机器人；110底座；120主体部；130肩部；140颈部；150头部；160、160L、160R臂；170、170L、170R摄像机；180L、180R手；190L、190R力觉传感器；200控制装置；210处理器；211臂控制部；212摄像机控制部；213摄像机校正执行部；214转换矩阵推定部；220主存储器；230非易失性存储器；231程序命令；232摄像机内部参数；233摄像机外部参数；240显示控制部；250显示部；260I/O接口；400校正用图案。

具体实施方式

A.机器人系统的构成

图1是一个实施方式涉及的机器人系统的示意图。该机器人系统具备机器人100和控制装置200。该机器人100是可在通过摄像机识别作业对象、自由地加减力、自主地判断的同时进行作业的自主型机器人。此外，机器人100也可以作为按照预先生成的示教数据执行作业的示教作业机器人而动作。

机器人100具备底座110、主体部120、肩部130、颈部140、头部150、两个臂160L、160R。在臂160L、106R上可装卸地安装有手180L、180R。这些手180L、180R是把持工件、工具的末端执行器。在头部150上设置有摄像机170L、170R。这些摄像机170L、170R是与臂160L、160R独立地设置，并且其位置姿势不改变的固定摄像机。在臂160L、160R上可设置摄像机170L、170R的校正用图案400。

在臂160L、160R的手腕部设置有力觉传感器190L、190R。力觉传感器190L、190R是检测相对于手180L、180R对工件带来的力的反向力、力矩的传感器。作为力觉传感器190L、190R，例如可使用可同时检测平移三轴方向的力分量和绕三个旋转轴旋转的力矩分量的六分量的六轴力矩传感器。需要注意的是，力觉传感器190L、190R可以省略。

臂160L、160R、摄像机170L、170R、手180L、180R和力觉传感器190L、190R的标号的末尾附带的“L”“R”字符分别意味着“左”“右”。当不需要区分它们时，使用省略了“L”“R”字符的标号来说明。

控制装置200具有处理器210、主存储器220、非易失性存储器230、显示控制部240、显示部250以及I/O接口260。这些各部分通过总线连接。处理器210例如是微处理器或处理器电路。控制装置200经由I/O接口260与机器人100连接。需要注意的是，可以将控制装置200容纳于机器人100的内部。

作为控制装置200的构成，可采用图1所示的构成以外的各种构成。例如，可从图1的控制装置200去掉处理器210和主存储器220，在与该控制装置200可通信地连接的其它装置上设置处理器210和主存储器220。此时，组合了该其它装置和控制装置200的装置整体作为机器人100的控制装置发挥功能。在其它实施方式中，控制装置200还可以具有两个以上处理器210。进而在其它实施方式中，控制装置200可通过彼此可通信地连接的多个装置来实现。在这些各种实施方式中，控制装置200以具备一个以上处理器210的装置或装置组构成。

图2是示出机器人100和控制装置200的功能的框图。控制装置200的处理器210通过执行预先存储在非易失性存储器230中的各种程序命令231分别实现臂控制部211、摄像机控制部212、摄像机校正执行部213的功能。摄像机校正执行部213具有转换矩阵推定部214。其中，可通过硬件电路实现这些各部分211至214的功能的一部分或全部。关于这些各部分211至214的功能将在后文进行叙述。在非易失性存储器230中存储有程序命令231之外的摄像机内部参数232和摄像机外部参数233。关于这些参数232、233将在后文进行叙述。

B.机器人的坐标系和坐标转换

图3是示出机器人100的臂160的构成和各种坐标系的说明图。在两个臂160L、160R的各自上设置有7个关节J1至J7。关节J1、J3、J5、J7是扭转关节，关节J2、J4、J6是弯曲关节。需要注意的是，在图1的主体部120和肩部130之间设置有扭转关节，但在图3中省略了图示。在各个关节上设置有用于使他们动作的致动器和用于检测旋转角度的位置检测器。

在臂160的手末端设定有工具中心点TCP(Tool Center Point：工具中心点)。典型而言，机器人100的控制是用于控制工具中心点TCP的位置姿势而执行。需要注意的是，位置姿势(position and attitude：位置和姿势)意味着由三维坐标系中的三个坐标值和绕各坐标轴的旋转所限定的状态。在图3的例子中，在右臂160R的手末端固定有摄像机170的校正(校准)所使用的校正用图案400。当在臂160R上安装校正用图案400时，也可以卸下手180R。

摄像机170的校正是确定摄像机170的内部参数和外部参数的处理。内部参数是摄像机170及其透镜系统的固有参数，例如包括投影转换参数、畸变参数等。外部参数是计算摄像机170和机器人100的臂160之间的相对位置姿势时使用的参数，包括表示机器人坐标系Σ_O和摄像机坐标系Σ_C之间的平移、旋转的参数。其中，外部参数也可构成为表示机器人坐标系Σ_O以外的目标坐标系和摄像机坐标系Σ_C之间的平移、旋转的参数。目标坐标系只要是从机器人坐标系Σ_O求得的坐标系即可。例如，可选择将具有相对于机器人坐标系Σ_O固定的已知的相对位置姿势的坐标系、根据臂160的关节的动作量确定与机器人坐标系Σ_O的相对位置姿势的坐标系作为目标坐标系。外部参数相当于“可计算目标坐标系和摄像机坐标系之间的坐标转换的摄像机的参数”。

在图3中，作为与机器人100相关的坐标系，说明了以下的坐标系。

(1)机器人坐标系Σ_O：以机器人100的基准点RO为坐标原点的坐标系；

(2)臂坐标系Σ_A：以臂160的基准点AO为坐标原点的坐标系；

(3)手末端坐标系Σ_T：以TCP(工具中心点)为坐标原点的坐标系；

(4)图案坐标系Σ_P：以校正用图案400上的预定位置为坐标原点的坐标系；

(5)摄像机坐标系Σ_C：设定于摄像机170的坐标系。

臂坐标系Σ_A和手末端坐标系Σ_T分别单独地设定在右臂160R和左臂160L上。由于在图3的例子中校正用图案400固定在右臂160R的手末端，因此在以下的说明中，使用右臂160R的臂坐标系Σ_A和手末端坐标系Σ_T。臂坐标系Σ_A和机器人坐标系Σ_O的相对位置姿势为已知。摄像机坐标系Σ_C也分别单独地设定在右眼摄像机170R和左眼摄像机170L上。在以下的说明中，作为摄像机坐标系Σ_C主要使用左眼摄像机170L的坐标系，但也可使用右眼摄像机170R的坐标系。需要注意的是，在图3中，为了方便图示，将各个坐标系的原点画在偏离实际位置的位置上。

通常，或从坐标系Σ_A向其它坐标系Σ_B转换，或者在这些坐标系上的位置姿势的转换由以下所示的齐次转换矩阵^AH_B(Homogeneous transformation matrix)表示。

[数1]

在此，R是旋转矩阵(Rotation matrix)、T是平移扭矩(Translation vector)、Rx、Ry、Rz是旋转矩阵R的列分量。在下文中，也将齐次转换矩阵^AH_B称为“坐标转换矩阵^AH_B”、“转换矩阵^AH_B”，或简称为“转换^AH_B”。转换的标号“^AH_B”的左侧的上标字符“^A”表示转换前的坐标系，右侧的下标字符“_B”表示转换后的坐标系。需要注意的是，转换^AH_B也可认为是表示在坐标系Σ_A观察到的坐标系Σ_B的基本矢量的分量和原点位置。

转换^AH_B的逆矩阵^AH_B ^-1(＝^BH_A)由下式表示。

[数2]

旋转矩阵R具有以下的重要的性质。

<旋转矩阵R的性质1>

旋转矩阵R是正规正交矩阵，其逆矩阵R^-1与转置矩阵R^T相等。

<旋转矩阵R的性质2>

旋转矩阵R的三个列分量R_x、R_y、R_z与在原来的坐标系Σ_A观察到的旋转后的坐标系Σ_B的三个基本矢量的分量相等。

或者当对坐标系Σ_A依次实施转换^AH_B、^BH_C时，合成的转换^AH_C成为依次在右侧乘以各转换^AH_B、^BH_C的结果。

[数3]

^AH_C＝^AH_B·^BH_C (3)

此外，关于旋转矩阵R，与(3)式同样的关系成立。

[数4]

^AR_C＝^AR_B·^BR_C (4)

C.坐标转换的AX＝XB问题

在图3中，在多个坐标系Σ_O，Σ_T，Σ_P，Σ_C之间，以下的转换成立。

(1)转换^OH_T(可计算)：从机器人坐标系Σ_O向手末端坐标系Σ_T的转换；

(2)转换^TH_P(未知)：从手末端坐标系Σ_T向图案坐标系Σ_P的转换；

(3)转换^PH_C(可推定)：从图案坐标系Σ_P向摄像机坐标系Σ_C的转换；

(4)转换^CH_O(未知)：从摄像机坐标系Σ_C向机器人坐标系Σ_O的转换。

将机器人坐标系Σ_O和摄像机坐标系Σ_C建立关联的参数是转换^CH_O。通常，求该转换^CH_O相当于摄像机170的校正(校准)。

在第一实施方式的摄像机170的校正中，选择以TCP作为校正对象点，以其手末端坐标系Σ_T作为校正对象的目标坐标系。而且，推定手末端坐标系Σ_T和摄像机坐标系Σ_C之间的转换^TH_C(＝^TH_P·^PH_C)或^CH_T(＝^CH_P·^PH_T)。由于手末端坐标系Σ_T和机器人坐标系Σ_O之间的转换^TH_O(或^OH_T)可计算，因此如果能够获得手末端坐标系Σ_T和摄像机坐标系Σ_C之间的转换^TH_C(或^CH_T)，也能够计算机器人坐标系Σ_O和摄像机坐标系Σ_C之间的转换^CH_O(或^OH_C)。需要注意的是，作为目标坐标系，还可选择手末端坐标系Σ_T以外的坐标系，可选择具有相对于机器人坐标系Σ_O已知的相对位置姿势的任意的坐标系。关于选择将手末端坐标系Σ_T以外的坐标系作为目标坐标系的情况将在第二实施方式中说明。

上述的四个转换^OH_T、^TH_P、^PH_C、^CH_O中，转换^OH_T是将机器人坐标系Σ_O和作为校正对象点的TCP的手末端坐标系Σ_T相结合的转换。通常，将求出TCP相对于机器人坐标系Σ_O的位置姿势的处理称为正向运动学处理，如果确定臂160的几何学形状和各关节的动作量(旋转角度)则可计算。即，该转换^OH_T是可计算的转换。需要注意的是，从机器人坐标系Σ_O向臂坐标系Σ_A的转换^OH_A是固定的，且为已知。

转换^TH_P是从手末端坐标系Σ_T向校正用图案400的图案坐标系Σ_P的转换。在专利文献1中，要求该转换^TH_P是已知的被固定的转换，但在本实施方式中假定为未知。

转换^PH_C是从图案坐标系Σ_P向摄像机坐标系Σ_C的转换，可通过由摄像机170拍摄校正用图案400，并相对于该图像进行图像处理来推定。推定该转换^PH_C的处理可使用用于进行摄像机的校正的标准化软件(例如Open CV、MATLAB的摄像机校准函数)来执行。

如果依次探索上述的转换^OH_T、^TH_P、^PH_C、^CH_O则返回至最初的机器人坐标系Σ_O，因此，使用恒等转换I，以下的式成立。

[数5]

⁰H_T·^TH_P·^PH_C·^CH₀＝I (5)

如果在(5)式的两边从左依次乘以各转换的逆矩阵^OH_T ^-1、^TH_P ^-1、^PH_C ^-1则可获得下式。

[数6]

^CH₀＝^PH_C ^-1·^TH_P ^-1·⁰H_T ^-1 (6)

在(6)式中，转换^PH_C可使用摄像机校准函数推定，转换^OH_T可计算。因此，如果转换^TH_P为已知，则右边可计算，并且可知道左边的转换^CH_O。这是假设了在现有技术中已知转换^TH_P的理由。

另一方面，如果转换^TH_P为未知，则不能够计算(6)式的右边，需要其它的处理。例如，若考虑图3中的臂160R的两个姿势i、j，则在各个姿势中上述(5)式成立，成为下式。

[数7]

⁰H_T(i)·^TH_P·^PH_C(i)·^CH₀＝I (7a)

⁰H_T(j)·^TH_P·^PH_C(j)·^CH₀＝I (7b)

如果分别在(7a)、(7b)式的两边从右乘以转换^CH_O的逆矩阵^CH_O ^-1，则可得到下式。

[数8]

⁰H_T(i)·^TH_P·^PH_C(i)＝^CH₀ ^-1 (8a)

⁰H_T(j)·^TH_P·^PH_C(j)＝^CH₀ ^-1 (8b)

虽然(8a)、(8b)式的右边为未知，但由于是相同转换，因此下式成立。

[数9]

⁰H_T(i)·^TH_P·^PH_C(i)＝⁰H_T(j)·^TH_P·^PH_C(j) (9)

若在(9)式的两边从左乘以^OH_T(j)-¹，从右乘以^PH_C(i)^-1则成为下式。

[数10]

(⁰H_T(j)^-1·⁰H_T(i))·^TH_P＝^TH_P·(^PH_C(j)·^PH_C(i)^-1) (10)

在此，将(10)式的左边和右边的括号中的转换的积分别写成A、B，未知的转换^TH_P写成X，得到下式。

[数11]

AX＝XB (11)

这是以AX＝XB问题被熟知的处理，并且为了解答未知的矩阵X需要非线性的优化处理。然而，对于该非线性的优化处理，有所谓的不保证收敛在最优解的问题。

如以下的详细说明，在第一实施方式中，通过利用可任意地控制设置了校正用图案400的臂160，使校正用图案400发生预定的位置姿势的变化，从而推定作为目标坐标系的手末端坐标系Σ_T和摄像机坐标系Σ_C之间的转换^TH_C(＝^TH_P·^PH_C)或^CH_T(＝^CH_P·^PH_T)。结果能够确定摄像机170的外部参数。

D.实施方式的处理流程

图4是示出实施方式中的摄像机170的校正处理的流程的流程图。机器人100具有的两个摄像机170R、170L分别单独地校正，但在下文中没有特别区分而称为“摄像机170”。以下说明的校正处理是图2所示的臂控制部211、摄像机控制部212和摄像机校正执行部213合作来执行。即，使校正用图案400变更成多个位置姿势的动作通过臂控制部211控制臂160来实现。此外，由摄像机170的拍摄通过摄像机控制部212来控制。摄像机170的内部参数、外部参数通过摄像机校正执行部213来确定。此外，在摄像机170的外部参数的确定中，各种矩阵、矢量的推定通过转换矩阵推定部214进行。

步骤S110和步骤S120是确定摄像机170的内部参数的处理。首先，在步骤S110中，使用摄像机170以多种位置姿势拍摄校正用图案400。这些多种位置姿势由于用于确定摄像机170的内部参数，因此可采用任意的位置姿势。在步骤S120中，摄像机校正执行部213使用在步骤S110中获得的多种图案图像，推定摄像机170的内部参数。如上所述，摄像机170的内部参数是摄像机170及其透镜系统的固有参数，例如包括投影转换参数、畸变参数等。内部参数的推定可使用进行摄像机的校正的标准化软件(例如Open CV、MATLAB的摄像机校准函数)来执行。

步骤S130至步骤S180是确定摄像机170的外部参数的处理。在步骤S130中，使校正用图案400绕手末端坐标系Σ_T的三个旋转轴旋转，并在绕各旋转轴的旋转中的多个旋转位置拍摄校正用图案400的图像。以下，将用摄像机170拍摄校正用图案400所获得的图像称为“图案图像”。

图5是示出在步骤S130获得的多个图案图像的例子的说明图。这些图案图像是在以在空间上固定作为手末端坐标系Σ_T的原点的TCP的状态下，分别独立地绕手末端坐标系Σ_T的XYZ轴的各轴进行±θx，±θy，±θz的旋转并使臂160停止，在这些多个旋转位置上拍摄的图像。即，多个旋转位置包括：基本旋转位置、从基本旋转位置绕X轴旋转的两个旋转位置、从基本旋转位置绕Y轴旋转的两个旋转位置、从基本旋转位置绕Z轴旋转的两个旋转位置。将从基本旋转位置的旋转角度θx、θy、θz分别设定成5度，但可采用非0度的任意旋转角度。其中，当旋转角度θ过小时，难以识别由旋转引起的图案图像的差异，当旋转角度θ过大时，难以从图案图像识别校正用图案400的排列。当考虑这些点时，旋转角度θx、θy、θz优选设定在例如3度以上30度以下的范围内。校正用图案400是将黑点配置成9×7的网格状的图案。其中，也可以使用棋盘图案那样的其它校正用图案。图案坐标系Σ_P的坐标原点位于校正用图案400上的预先确定的位置。

在步骤S140中，对在步骤S130拍摄的各个图案图像，推定图案坐标系∑_P和摄像机坐标系∑c之间的转换^PH_C或^CH_P。该推定可利用在步骤S120获得的内部参数，使用推定摄像机的外部参数的标准化软件(例如OpenCV的函数“Find Extrinsic Camera Params2”)执行。

在步骤S150中，使用在步骤S140获得的转换^PH_C或^CH_P推定摄像机坐标系∑_C和手末端坐标系∑_T之间的旋转矩阵^CR_T或^TR_C。以下，首先以绕X轴的旋转为例进行说明。

首先，将从基本旋转位置的图案图像获得的转换^PH_C的旋转矩阵^PR_C简写成R(θ_o)。此外，分别将从绕X轴±θ_x旋转的状态的图案图像获得的转换^PH_C的旋转矩阵^PR_C写成R(θ_o+θ_x)及R(θ_o-θ_x)。此时，下式成立。

[数12]

R(θ₀+θx)＝R(θ₀)·R(θx) (12a)

R(θx)＝R(θ₀)^-1·R(θ₀+θx) (12b)

在此，旋转矩阵R(θ_x)是使坐标系从基本旋转位置仅旋转+θ_x的旋转矩阵。如(12b)式所示，旋转矩阵R(θ_x)能够以基本旋转位置的旋转矩阵R(θ_o)的逆矩阵R(θ_o)^-1和从基本旋转位置仅旋转+θ_x的位置的旋转矩阵R(θ_o+θ_x)的乘积来计算。

然而，通常绕坐标系的三个轴的任意的旋转由旋转矩阵、三个欧拉角来表示的情况较多，但也可取而代之用一个旋转轴和绕该旋转轴的旋转角度来表示。当利用后者的表示时，旋转矩阵R(θ_x)可转换成由下式分配的旋转矢量Rod(θ_x)。

[数13]

在此，n_x、ny、n_z是表示旋转轴的方向的三个轴的分量。即，“旋转矢量Rod”是以旋转轴的方向为矢量方向，以旋转的角度为矢量长度的矢量。从旋转矩阵R(θ_x)向旋转矢量Rod(θ_x)的转换例如可使用OpenCV的函数“Rodrigues2”执行。

如上所述，旋转矩阵R(θ_x)是表示从基本旋转位置使坐标系绕手末端坐标系Σ_T的X轴仅旋转+θ_x的矩阵。因此，与旋转矩阵R(θ_x)等价的旋转矢量Rod(θ_x)的矢量方向表示旋转轴的方向，即在摄像机坐标系Σ_C观察到的手末端坐标系Σ_T的X轴的方向。

在此，考虑从摄像机坐标系Σ_C向手末端坐标系Σ_T的旋转矩阵^CR_T。如关于上述的(1a)式至(1d)式所示的通常的齐次转换矩阵以<旋转矩阵R的性质2>说明的那样，任意的旋转矩阵R的三个列分量Rx、Ry、Rz表示从原来的坐标系观察到的旋转后的坐标系的三个基底矢量。因此，将上述的旋转矢量Rod(θ_x)的长度正规化成1后的正规化旋转矢量Rod*(θ_x)成为从摄像机坐标系Σ_C向手末端坐标系Σ_T的旋转矩阵^CR_T的X分量(最左侧的列分量)。

[数14]

通过对Y轴、Z轴也进行同样的处理，可获得从摄像机坐标系Σ_C向手末端坐标系Σ_T的旋转矩阵^CR_T的三个列分量Rod*(θx)，Rod*(θy)，Rod*(θz)。

[数15]

^CR_T＝(Rod^*(θx) Rod^*(θy) Rod^*(θy)) (15)

需要注意的是，旋转矩阵^CR_T的逆转换^TR_C与旋转矩阵^CR_T的转置矩阵相等。因此，只要以行分量排列来代替将正规化旋转矢量Rod*(θx)，Rod*(θy)，Rod*(θy)作为列分量排列，则可直接获得从手末端坐标系Σ_T向摄像机坐标系Σ_C旋转矩阵^TR_C。

像这样，在步骤S150中，根据在以作为目标坐标系的手末端坐标系Σ_T的各旋转轴为中心的旋转中的多个旋转位置拍摄的图案图像来推定以各旋转轴的方向为矢量方向、以旋转的角度为矢量长度的三个旋转矢量Rod(θx)、Rod(θy)、Rod(θy)。而且，通过将这些旋转矢量正规化后的正规化旋转矢量Rod*(θx)、Rod*(θy)、Rod*(θy)的分量作为行分量或列分量来排列，可确定构成手末端坐标系Σ_T和摄像机坐标系Σ_C之间的坐标转换矩阵^CH_T或^TH_C的旋转矩阵^CR_T或^TR_C。

需要注意的是，对于步骤S150中的处理，可能会包括检测误差。此时，在图5所示的例子中，只要利用在基本旋转位置和从基本旋转位置绕X轴旋转±θx的两个旋转位置的三个旋转位置拍摄的三个图案图像，除了能够推定旋转矩阵R(θx)之外，还能够推定其它的旋转矩阵R(-θx)、R(2θx)。因此，分别利用这些其它的旋转矩阵R(-θx)、R(2θx)，按照上述的流程求出旋转矩阵^TR_P，还可以求出所获得的多个旋转矩阵^TR_P的平均值。需要注意的是，求出多个旋转矩阵的平均值的处理例如可通过将各旋转矩阵转换成四元数，求出多个四元数的平均值后对旋转矩阵逆转换来执行。

此外，在上述的处理中获得的旋转矩阵^TR_P有可能不具有正规正交性。此时，优选使用某种正交化手段(例如Gram-Schmidt正交化法)对旋转矩阵^TR_P的各列进行正交化。需要注意的是，成为进行正交化时的基点的轴优选选择与图像平面正交的轴(在图5的例中为Z轴)。其理由认为是由于如图5所示，图像上的位移在绕与图像平面正交的轴旋转时最大，因此相对地误差变为最小。

需要注意的是，X、Y、Z轴的旋转角度θx、θy、θz为已知。因此，当在上述的处理中检测出的旋转角度和已知的旋转角度的差超过考虑了检测误差的允许范围时，可将其处理结果判定为异常。

在步骤S160中计算出手末端坐标系Σ_T和图案坐标系Σ_P之间的旋转矩阵^TR_P或^PR_T。在上述的步骤S140中，关于各图案图像，推定了图案坐标系Σ_P和摄像机坐标系Σ_C之间的转换^PH_C或^CH_P，构成它们的转换^PH_C或^CH_P的旋转矩阵^PR_C或^CR_P也是已知。例如，使用在特定的旋转位置(例如基本旋转位置)推定的旋转矩阵^CR_P和在步骤S150中获得的旋转矩阵^TR_C，可由下式算出手末端坐标系Σ_T和图案坐标系Σ_P之间的旋转矩阵^TR_P。

[数16]

^TR_P＝^TR_C·^CR_P (16)

图6示出在步骤S160获得的旋转矩阵^TR_P的值。需要注意的是，由于在本实施方式中，手末端坐标系Σ_T和图案坐标系Σ_P之间的转换^TH_P为未知，因此没有应当为旋转矩阵^TR_P的正确答案的值。因此，在图6中，示出分别独立地使用图3所示的机器人100的右眼摄像机170R和左眼摄像机170L所推定的结果。由于两个旋转矩阵^TR_P高度一致，因此可理解高精度地推定了旋转矩阵^TR_P。需要注意的是，步骤160也可以省略。

在步骤S170中，推定手末端坐标系Σ_T和图案坐标系Σ_P之间的平移矢量^TT_P或^PT_T。在此，首先考虑使校正用图案400绕手末端坐标系Σ_T的X轴旋转的情况。

图7是将基本旋转位置的平移矢量^TT_P(θ₀)和使校正用图案400绕手末端坐标系Σ_T的X轴旋转的旋转位置的平移矢量^TT_P(θ₀+θx)、^TT_P(θ₀-θx)投影到YZ平面上的图。在此，当以平移矢量^TT_P的长度为r_x，以平移矢量^TT_P的XYZ分量为(Tx、T_y、Tz)，以两个平移矢量^TT_P(θ₀+θx)，^TT_P(θ₀-θx)的差分为ΔT_x，则以下的式成立。

[数17]

|ΔT_x|＝2r_xSinθ_x (17b)

由于与(17a)至(17c)式同样的式对于绕Y轴的旋转及绕Z轴的旋转也成立，因此如下所述。

[数18]

当变形(18a)至(18c)时，可得到下述式。

[数19]

如上述图5所述，校正用图案400固定作为手末端坐标系Σ_T的坐标原点的TCP并旋转。此外，由于图案坐标系Σ_P的原点位置设定在校正用图案400上的已知的点上，因此可通过分析图案图像来检测图案坐标系Σ_P的原点位置。因此，由从基本旋转位置+θx旋转后的第一图案图像获得的图案坐标系Σ_P的原点位置和由-θx旋转后的第二图案图像获得的图案坐标系Σ_P的原点位置的差分与图7所示的平移矢量^TT_P(θ₀+θx)、^TT_P(θ₀－θx)的差分ΔT_x相等。绕Y轴的旋转及绕Z轴的旋转也同样。因此，根据上述的(18a)至(18c)式及(19a)至(19c)式，可推定从手末端坐标系Σ_T向图案坐标系Σ_P的平移矢量^TT_P。

需要注意的是，在上述的步骤S160中可获得手末端坐标系Σ_T和图案坐标系Σ_P之间的旋转矩阵^TR_P或^PR_T。因此，只要能够通过上述的步骤S170的处理推定从手末端坐标系Σ_T向图案坐标系Σ_P的平移矢量^TT_P，则可由上述(2)式计算出从图案坐标系Σ_P向手末端坐标系Σ_T的平移矢量^PT_T。

像这样，在步骤S170中，根据在以作为目标坐标系的手末端坐标系Σ_T的各旋转轴为中心的旋转的多个旋转位置拍摄的图案图像，推定构成图案坐标系Σ_P和手末端坐标系Σ_T之间的转换矩阵^PH_T或^TH_P的平移矢量^PT_T或^TT_P的三个分量Tx、Ty、Tz中，与各旋转轴正交的两个坐标轴方向的两个平移矢量分量的平方和r_x ²、r_y ²、r_z ²。此外，根据在三个旋转轴中分别推定的平移矢量分量的平方和r_x ²、r_y ²、r_z ²，可计算出构成转换矩阵^PH_T或^TH_P的平移矢量^PT_T或^TT_P。

图8示出在步骤S170中获得的平移矢量^TT_P的值。需要注意的是，在此也与图6同样地示出分别独立地使用右眼摄像机170R和左眼摄像机170L所推定的结果。由于两个平移矢量^TT_P高度一致，因此可理解为高精度地推定了平移矢量^TT_P。

在步骤S180中，由在步骤S140中的特定的旋转位置(例如基本旋转位置)推定的转换矩阵^CH_P或^PH_C和在步骤S170中获得的平移矢量^PT_T或^TT_P，计算出摄像机坐标系Σ_C和手末端坐标系Σ_T之间的平移矢量^CT_T或^TT_C。例如，可由下式计算出从摄像机坐标系Σ_C向手末端坐标系Σ_T的平移矢量^CT_T。

[数20]

在此，^CH_P是由步骤S140中的特定旋转位置(例如基本旋转位置)的图案图像所推定的齐次转换矩阵，^PT_T是在步骤S170中获得的平移矢量。从手末端坐标系Σ_T向摄像机坐标系Σ_C的平移矢量^TT_C也可由同样的式算出。

通过图4的处理，关于表示作为目标坐标系的手末端坐标系Σ_T和摄像机坐标系Σ_C之间的坐标转换的齐次转换矩阵^CH_T或^TH_C，可推定其旋转矩阵^CR_T或^TR_C和平移矢量^CT_T或^TT_C。这样获得的齐次转换矩阵^CH_T或^TH_C作为摄像机170的外部参数233存储在非易失性存储器230中。只要利用摄像机170的外部参数233和内部参数232就能够执行使用了摄像机170的各种检测处理、控制。需要注意的是，作为摄像机170的外部参数233，可采用能够计算出目标坐标系Σ_T和摄像机坐标系Σ_C之间的坐标转换的各种参数。例如，表示机器人坐标系Σo和摄像机坐标系Σ_C之间的坐标转换的齐次转换矩阵^OH_C或^CH_O也可作为外部参数233存储。

如上所述，在本实施方式中，以绕作为目标坐标系的手末端坐标系Σ_T的原点设定三个旋转轴X、Y、Z，以各旋转轴为中心分别使所述校正用图案400旋转并在多个旋转位置停止的方式使臂160动作。而且，在以各旋转轴为中心的旋转的多个旋转位置上用摄像机170拍摄校正用图案400的图案图像，使用这些图案图像推定手末端坐标系Σ_T和摄像机坐标系Σ_C之间的坐标转换矩阵^TH_C或^CH_T。在该处理流程中，使用绕各旋转轴旋转的多个旋转位置的图案图像，可推定在摄像机坐标系Σ_C观察到的三个旋转轴的方向。此外，由于三个旋转轴X、Y、Z彼此线性独立，因此由它们的旋转轴的方向可确定手末端坐标系Σ_T和摄像机坐标系Σ_C之间的坐标转换矩阵^TH_C或^CH_T。结果，由于可获得可计算手末端坐标系Σ_T和摄像机坐标系Σ_C之间的坐标转换的外部参数，因此可进行使用了摄像机170的对象物的位置检测。

需要注意的是，在上述的实施方式中，作为绕手末端坐标系Σ_T的原点的旋转轴，选择了X轴、Y轴和Z轴，但三个旋转轴只要是线性独立即可，可选择任意的三个旋转轴。在使用X轴、Y轴和Z轴以外的三个旋转轴时，从推定的结果的各轴分量，转换成手末端坐标系Σ_T的X轴、Y轴和Z轴的分量即可。其中，只要选择手末端坐标系Σ_T的三个基底矢量的方向(X、Y、Z轴)作为旋转轴则有上述的处理变得容易的优点。此外，三个旋转轴不需要设定在作为目标坐标系的手末端坐标系Σ_T的原点周围，可设定在其它位置。其中，只要将三个旋转轴设定在目标坐标系的原点周围，则由于三个旋转轴和目标坐标系的对应关系变得单纯，因此有可由在摄像机坐标系观察到的旋转轴的方向容易地确定目标坐标系和摄像机坐标系之间的坐标转换矩阵的优点。

此外，在上述的实施方式中，在绕各旋转轴的旋转中从基本旋转位置向正向一侧和负向一侧两个方向旋转，但也可以只向任意一方一侧旋转。其中，只要从基本旋转位置向正向一侧和负向一侧两个方向旋转，则上述的处理更加容易。此外，正向一侧和负向一侧的旋转角等优选是相等的值。

图9是示出第二实施方式的机器人的坐标系的说明图。与第一实施方式的图3的区别在于校正的目标坐标系Σ_t与手末端坐标系Σ_T设定在不同位置这一点，其它的构成与第一实施方式相同。该目标坐标系Σt例如具有与相对于机器人坐标系Σ₀固定的相对位置姿势。在第二实施方式的摄像机170的校正处理中，只将第一实施方式的图4的处理中的“手末端坐标系Σ_T”换成“目标坐标系Σ_t”，“TCP”换成“目标坐标系Σ_t的坐标原点TO”即可，处理流程与第一实施方式相同。

像这样，如果在与手末端坐标系Σ_T不同的位置上设定校正的目标坐标系Σ_t，则可在该目标坐标系Σ_t的附近提高通过摄像机170的物体检测精度。例如，在操作空间小的部位，有时物理上较大的手180无法进入。另一方面，图9所示的目标坐标系Σ_t也可设定在狭窄的间隙、其它物体的内部。因此，只要在与手末端坐标系Σ_T不同的位置上设定校正的目标坐标系Σ_t，可在任意的部位提高通过摄像机170的物体检测精度。

需要注意的是，摄像机170的校正处理是确定可计算具有相对于机器人坐标系Σ₀已知的相对位置姿势的目标坐标系Σ_t和摄像机坐标系Σ_C之间的坐标转换的外部参数的处理。目标坐标系Σ_t和摄像机坐标系Σ_C之间的坐标转换矩阵^CH_t(或^tH_C)由摄像机坐标系Σ_C和图案坐标系Σ_P之间的第一转换矩阵与图案坐标系Σ_P和目标坐标系Σ_t之间的第二转换矩阵^PH_t(或^tH_P)的乘积表示。此时，图4的步骤S140的处理相当于由在以绕目标坐标系Σ_t的原点的三个旋转轴为中心旋转的多个旋转位置中的一个特定旋转位置(在第一实施方式中是基本旋转位置)拍摄的图案图像推定第一转换矩阵^CH_P(或^PH_C)的处理。步骤S150的处理相当于由在多个旋转位置拍摄的图案图像推定以各旋转轴的方向为矢量方向，以旋转的角度为矢量长度的三个旋转矢量，将这三个旋转矢量分别正规化，通过以三个正规化旋转矢量为行分量或列分量来排列从而确定构成目标坐标系Σ_t和摄像机坐标系Σ_C之间的坐标转换矩阵^CH_t(或^tH_C)的旋转矩阵^CR_t(或^tR_C)。此外，步骤S170的处理相当于由在多个旋转位置中的一个特定旋转位置上拍摄的图案图像推定构成第二转换矩阵^PH_t(或^tH_P)的平移矢量的三个分量中与各旋转轴正交的两个坐标轴方向的两个平移矢量分量的平方和，并从三个旋转轴分别推定的平移矢量分量的平方和计算出构成第二转换矩阵^PH_t(或^tH_P)的平移矢量^PT_t(或^tT_P)的处理。而且，步骤S180的处理相当于计算出在特定旋转位置推定的第一转换矩阵^CH_P(或^PH_C)和从第二转换矩阵^PH_t(或^tH_P)的平移矢量^PT_t(或^tT_P)计算出坐标转换矩阵^CH_t(或^tH_C)的平移矢量^CT_t(或^tT_C)的处理。通过执行这样的处理，可由在以各旋转轴为中心的旋转中的多个旋转位置拍摄的图案图像，容易地求出构成目标坐标系Σ_t和摄像机坐标系Σ_C之间的坐标转换矩阵^CH_t(或^tH_C)的旋转矩阵和平移矢量。

在上述的实施方式中，说明了与机器人100的头部150的摄像机170相关的校正，但本发明也可应用在设置于头部150以外的部位的机器人内置型摄像机、与机器人100分开设置的摄像机的校正中。此外，本发明不限于双腕机器人，也可应用于单腕机器人。

本发明不限于上述的实施方式或实施例、变形例，在不脱离其主旨的范围内能够以各种构成实现。例如，为了解决上述课题的一部分或者全部，或者，为了实现上述效果的一部分或者全部，可对记载于发明内容栏中的各方式中的技术特征所对应的实施方式、实施例、变形例中的技术特征适当地进行替换、组合。此外，如果该技术特征在本说明书中没有作为必要的特征被说明，则可以对其进行适当删除。

Claims (8)

1.一种控制装置，其特征在于，控制机器人和摄像机，所述机器人具备构成为能够设置所述摄像机的校正用图案的臂，所述摄像机与所述臂独立地设置，所述控制装置具备：

臂控制部，控制所述臂；

摄像机控制部，控制所述摄像机；以及

摄像机校正执行部，确定能够计算目标坐标系和所述摄像机的摄像机坐标系之间的坐标转换的所述摄像机的参数，所述目标坐标系具有相对于所述机器人的机器人坐标系已知的相对位置姿势，

所述臂控制部使所述臂动作，以使所述校正用图案分别以彼此线性独立的三个旋转轴的各旋转轴为中心进行旋转，并在多个旋转位置停止，

所述摄像机控制部使所述摄像机拍摄所述多个旋转位置中的所述校正用图案的图案图像，

所述摄像机校正执行部使用在所述多个旋转位置拍摄到的所述校正用图案的图案图像确定所述参数。

2.根据权利要求1所述的控制装置，其特征在于，

所述三个旋转轴设定在所述目标坐标系的原点周围。

3.根据权利要求1或2所述的控制装置，其特征在于，

所述摄像机校正执行部根据在所述多个旋转位置拍摄到的所述校正用图案的图案图像来推定以各旋转轴的方向为矢量方向、以所述旋转的角度为矢量长度的三个旋转矢量，

所述摄像机校正执行部分别将所述三个旋转矢量正规化，求出三个正规化旋转矢量，

所述摄像机校正执行部通过将所述三个正规化旋转矢量作为行分量或列分量排列来确定构成所述目标坐标系和所述摄像机坐标系之间的坐标转换矩阵的旋转矩阵。

4.根据权利要求3所述的控制装置，其特征在于，

所述目标坐标系和所述摄像机坐标系之间的坐标转换矩阵由所述摄像机坐标系和所述校正用图案的图案坐标系之间的第一转换矩阵与所述图案坐标系和所述目标坐标系之间的第二转换矩阵的乘积表示，

所述摄像机校正执行部：

(a)根据在所述多个旋转位置中的一个特定旋转位置上拍摄到的所述校正用图案的图案图像来推定所述第一转换矩阵；

(b)根据在所述多个旋转位置拍摄到的所述校正用图案的图案图像来推定构成所述第二转换矩阵的平移矢量的三个分量中与各旋转轴正交的两个坐标轴方向上的两个平移矢量分量的平方和，并根据分别对所述三个旋转轴推定的所述平移矢量分量的平方和来算出构成所述第二转换矩阵的所述平移矢量；以及

(c)根据在所述特定旋转位置推定的所述第一转换矩阵和所述第二转换矩阵的平移矢量来算出构成所述坐标转换矩阵的平移矢量。

5.根据权利要求1或2所述的控制装置，其特征在于，

所述目标坐标系是具有与所述臂独立地相对于所述机器人的机器人坐标系固定的相对位置姿势的坐标系。

6.根据权利要求1或2所述的控制装置，其特征在于，

所述目标坐标系是所述臂的手末端坐标系。

7.一种机器人系统，其特征在于，具备：

机器人；以及

与所述机器人连接的权利要求1至6中任一项所述的控制装置。

8.一种校正方法，其特征在于，在具备机器人和摄像机的机器人系统中进行所述摄像机的校正，所述机器人具备构成为能够设置所述摄像机的校正用图案的臂，所述摄像机与所述臂独立地设置，所述校正方法包括：

使所述臂动作，以使所述校正用图案分别以彼此线性独立的三个旋转轴的各旋转轴为中心旋转，并在多个旋转位置停止；

使所述摄像机拍摄在所述多个旋转位置中的所述校正用图案的图案图像；以及

使用在所述多个旋转位置拍摄到的所述校正用图案的图案图像，确定能够计算目标坐标系和所述摄像机的摄像机坐标系之间的坐标转换的所述摄像机的参数，所述目标坐标系具有相对于所述机器人的机器人坐标系已知的相对位置姿势。

Images