变换系数计算装置、变换系数计算方法及变换系数计算程序与流程

本发明涉及对变换系数进行计算的变换系数计算装置、变换系数计算方法及变换系数计算程序，该变换系数在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用。

背景技术：

机器人系统是下述系统，其具备例如安装有对加工对象等物体进行抓持的机器人手的机器人臂部等。

另外，机器人系统具备对物体及机器人手的周围进行测量的传感器，使用由传感器测量出的传感器信息，对机器人臂部及机器人手的位置及姿态进行控制。

其中，由传感器测量出的传感器信息是表示物体等的传感器坐标系下的3维位置的信息。因此，为了使得机器人系统能够在进行机器人臂部等的位置及姿态的控制时使用传感器信息，需要将传感器信息所表示的3维位置从传感器坐标系变换为机器人坐标系。

传感器坐标系是将传感器作为中心的坐标系，机器人坐标系是将机器人作为中心的机器人坐标系。

在下面的专利文献1中公开了一种变换系数计算装置，其中，机器人手对图像识别用的标记进行抓持，作为传感器的照相机对标记进行拍摄，由此对坐标的变换系数进行计算。

该变换系数计算装置根据由照相机拍摄出的影像内的标记的传感器坐标系(照相机坐标系)下的位置和由机器人手抓持的标记的机器人坐标系下的位置，对表示作为传感器的照相机和机器人之间的相对位置关系的参数进行计算。

表示作为传感器的照相机和机器人之间的相对位置关系的参数，与在将传感器坐标系下的3维位置变换为机器人坐标系下的3维位置时使用的坐标的变换系数相当。

该变换系数计算装置如果计算出表示照相机和机器人之间的相对位置关系的参数，则此后使用表示相对的位置关系的参数，将由传感器测量出的传感器信息所表示的物体等的3维位置变换为机器人坐标系的3维位置。

专利文献1：日本特开2014-180720号公报

技术实现要素：

当前的变换系数计算装置以上述方式构成，因此如果传感器是照相机，机器人手对标记进行抓持，则能够对表示作为传感器的照相机和机器人之间的相对位置关系的参数进行计算。但是，在传感器例如是激光扫描器等3维传感器的情况下，无法对标记进行拍摄，因此，存在无法对表示传感器和机器人之间的相对位置关系的参数进行计算的课题。

本发明就是为了解决如上述的课题而提出的，其目的在于得到一种变换系数计算装置、变换系数计算方法及变换系数计算程序，即使传感器是激光扫描器等3维传感器也能够对在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用的变换系数进行计算。

本发明涉及的变换系数计算装置设置：位置姿态信息取得部，其取得表示固定有物体的机器人手的位置及姿态的位置姿态信息，根据位置姿态信息，对物体的机器人坐标系下的位置或物体的机器人坐标系下的平面的参数即平面参数进行确定；以及传感器信息取得部，其从对固定于机器人手的物体进行测量的传感器，取得表示物体的传感器坐标系下的位置或物体的传感器坐标系下的平面的参数即平面参数的传感器信息，变换系数计算部在每次对机器人手的位置或姿态进行切换时，对由位置姿态信息取得部确定出的机器人坐标系下的位置或平面参数和由传感器信息取得部取得的传感器信息所表示的传感器坐标系下的位置或平面参数的组进行保存，根据所保存的多个组，对在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用的变换系数进行计算。

发明的效果

根据本发明，构成为，变换系数计算部在每次对机器人手的位置或姿态进行切换时，对由位置姿态信息取得部确定出的机器人坐标系下的位置或平面参数和由传感器信息取得部取得的传感器信息所表示的传感器坐标系下的位置或平面参数的组进行保存，根据所保存的多个组，对在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用的变换系数进行计算，因此具有如下效果，即使传感器是激光扫描器等3维传感器，也能够对在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用的变换系数进行计算。

附图说明

图1是表示安装有本发明的实施方式1涉及的变换系数计算装置4的机器人系统的结构图。

图2是表示本发明的实施方式1涉及的变换系数计算装置4的硬件结构图。

图3是变换系数计算装置4由软件或固件等实现的情况下的计算机的硬件结构图。

图4是表示与变换系数计算装置4由软件或固件等实现的情况下的处理流程相当的变换系数计算方法的流程图。

图5是表示安装有本发明的实施方式2涉及的变换系数计算装置6的机器人系统的结构图。

图6是表示本发明的实施方式2涉及的变换系数计算装置6的硬件结构图。

图7是表示与变换系数计算装置6由软件或固件等实现的情况下的处理流程相当的变换系数计算方法的流程图。

图8是表示安装有本发明的实施方式4涉及的变换系数计算装置6的机器人系统的结构图。

图9是表示本发明的实施方式4涉及的变换系数计算装置6的硬件结构图。

具体实施方式

下面，为了更详细地说明本发明，根据附图对用于实施本发明的方式进行说明。

实施方式1.

图1是表示安装有本发明的实施方式1涉及的变换系数计算装置4的机器人系统的结构图。

在图1中，机器人手1安装于机器人臂部的前端等，例如对加工对象等物体10进行固定。

机器人臂部至少具备大于或等于1个关节，通过对大于或等于1个关节进行控制，从而对机器人手1的位置或姿态进行切换。

机器人手1具有对物体10进行抓持的功能。但是，机器人手1只要能够对物体10进行固定即可，对物体10进行抓持的方式是任意的。例如，存在通过对多个手指进行开闭而将物体10夹入的方式、通过磁力或吸引而对物体10进行抓持的方式等。

机器人控制部2是通过对机器人臂部或机器人手1的关节进行控制而对机器人手1的位置或姿态进行切换的控制装置。机器人手1的姿态例如相当于相对于设置有机器人的面的角度等。

传感器3是对由机器人手1抓持的物体10进行测量的激光扫描器等3维传感器，输出表示物体10的测量结果的传感器信息。

从传感器3输出的传感器信息是空间中的物体10的表面的3维坐标(X，Y，Z)的集合。作为通过传感器3实现的物体10的测量方法，已知基于立体视觉的方法、Time-of-Flight法等。

变换系数计算装置4具备位置姿态信息取得部11、传感器信息取得部12、变换系数计算部13及坐标变换部14。

图2是表示本发明的实施方式1涉及的变换系数计算装置4的硬件结构图。

位置姿态信息取得部11例如由图2所示的位置姿态信息取得电路21实现，实施如下处理，即，取得表示机器人手1的位置及姿态的位置姿态信息，根据位置姿态信息对物体10的机器人坐标系下的3维位置进行确定。

传感器信息取得部12例如由图2所示的传感器信息取得电路22实现，实施如下处理，即，从传感器3取得表示物体10的传感器坐标系下的3维位置的传感器信息。

此外，传感器坐标系是将传感器3作为中心的坐标系，机器人坐标系是将机器人手1作为中心的机器人坐标系。

变换系数计算部13例如由图2所示的变换系数计算电路23实现。

变换系数计算部13具备存储部13a，该存储部13a在每次由机器人控制部2对机器人手1的位置或姿态进行切换时，对由位置姿态信息取得部11确定出的机器人坐标系下的3维位置和由传感器信息取得部12取得的传感器信息表示的传感器坐标系下的3维位置的组进行保存。

变换系数计算部13实施如下处理，即，根据存储部13a所保存的多个组，对刚体变换的旋转矩阵R及平移矢量t进行计算，作为在将传感器坐标系下的3维位置变换为机器人坐标系下的3维位置时使用的变换系数。

另外，变换系数计算部13实施如下处理，即，在每次保存机器人坐标系下的3维位置和传感器坐标系下的3维位置的组时，将对机器人手1的位置或姿态的切换进行指示的控制信号输出至机器人控制部2。

坐标变换部14例如由图2所示的坐标变换电路24实现。

坐标变换部14实施如下处理，即，使用由换系数计算部13计算出的变换系数即旋转矩阵R及平移矢量t，将由传感器信息取得部12取得的传感器信息示出的传感器坐标系下的3维位置变换为机器人坐标系下的3维位置。

在图1中，假设作为变换系数计算装置4的结构要素的位置姿态信息取得部11、传感器信息取得部12、变换系数计算部13及坐标变换部14分别由图2所示的专用硬件、即位置姿态信息取得电路21、传感器信息取得电路22、变换系数计算电路23及坐标变换电路24实现。

这里，变换系数计算部13所具备的存储部13a例如由存储处理电路实现。

该存储处理电路例如属于RAM(Random Access Memory)、ROM(Read Only Memory)、闪存、EPROM(Erasable Programmable Read Only Memory)、EEPROM(Electrically Erasable Programmable Read Only Memory)等非易失性或易失性的半导体存储器、磁盘、软盘、光盘、压缩盘、迷你盘、DVD(Digital Versatile Disc)等。

另外，位置姿态信息取得电路21、传感器信息取得电路22、变换系数计算电路23及坐标变换电路24例如属于单一电路、复合电路、程序化后的处理器、并行程序化后的处理器、ASIC(Application Specific Integrated Circuit)、FPGA(Field-Programmable Gate Array)、或者它们的组合。

变换系数计算装置4的结构要素并不限定于由专用硬件实现，变换系数计算装置4也可以由软件、固件、或者软件和固件组合实现。

软件或固件作为程序而储存于计算机的存储器。计算机代表对程序进行执行的硬件，例如属于CPU(Central Processing Unit)、中央处理装置、处理装置、运算装置、微处理器、微机、处理器、DSP(Digital Signal Processor)等。

图3是变换系数计算装置4由软件或固件等实现的情况下的计算机的硬件结构图。

在变换系数计算装置4由软件或固件等实现的情况下，只要将变换系数计算部13所具备的存储部13a构成于计算机的存储器31上，并且将用于使计算机执行位置姿态信息取得部11的位置姿态信息取得处理流程、传感器信息取得部12的传感器信息取得处理流程、变换系数计算部13的变换系数计算处理流程及坐标变换部14的坐标变换处理流程的变换系数计算程序储存于存储器31，计算机的处理器32执行储存于存储器31的程序即可。

在图3中，通信电路33是如下电路，即，从机器人手1接收位置姿态信息，并且从传感器3接收传感器信息，另外，将控制信号输出至机器人控制部2。

显示装置34例如由液晶显示器实现，对处理器32的处理的情形等进行显示。

图4是表示与变换系数计算装置4由软件或固件等实现的情况下的处理流程相当的变换系数计算方法的流程图。

下面，对动作进行说明。

机器人手1对物体10进行抓持。在该实施方式1中，作为一个例子，设为对球体的物体10进行抓持。

机器人控制部2通过对机器人臂部或机器人手1的关节进行控制而对机器人手1的位置或姿态进行切换。

在该实施方式1中，机器人控制部2按照从变换系数计算部13输出的控制信号使机器人臂部或机器人手1的关节的角度改变，由此对机器人手1的位置或姿态进行切换。

另外，在该实施方式1中，为了方便说明，设为对机器人臂部或机器人手1的关节进行N(N为大于或等于3的整数)次控制。

通过机器人控制部2对机器人臂部或机器人手1的关节进行控制，由此在第n(n＝1，2，...，N)次的控制下，使得由机器人手1抓持的物体10在机器人坐标系中移动至Prⁿ的位置(图4的步骤ST1)。在该阶段n＝1，因此在机器人坐标系中移动至Pr¹的位置。

位置Prⁿ如以下的式(1)所示，由机器人坐标系的X轴中的坐标Xr、Y轴中的坐标Yr、Z轴中的坐标Zr表示。

在式(1)中，T是表示转置的记号，例如x^T是矢量x的转置。

位置姿态信息取得部11取得表示机器人手1的位置及姿态的位置姿态信息，根据位置姿态信息，对物体10的机器人坐标系下的3维位置Prⁿ进行确定(图4的步骤ST2)。

根据位置姿态信息对机器人坐标系下的3维位置Prⁿ进行确定的处理是公知技术，因此省略详细的说明，例如可以通过以下所示的处理而对3维位置Prⁿ进行确定。

例如，事先准备表示机器人手1的位置及姿态和由机器人手1抓持的物体10的3维位置Prⁿ之间的对应关系的表格，位置姿态信息取得部11参照表格，由此对与位置姿态信息所表示的位置及姿态对应的3维位置Prⁿ进行确定。

传感器3通过机器人控制部2对在机器人坐标系中移动至Prⁿ(n＝1，2，...，N)的位置的物体10进行测量，作为物体10的测量结果，输出表示物体10的传感器坐标系下的3维位置Psⁿ(n＝1，2，...，N)的传感器信息。

在该实施方式1中，从传感器3输出的传感器信息是表示物体10的表面处的M点的位置的3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ。

传感器信息取得部12取得从传感器3输出的传感器信息，根据传感器信息对物体10的传感器坐标系下的3维位置Psⁿ(n＝1，2，...，N)进行确定(图4的步骤ST3)。

M是大于或等于1的整数，在M＝1的情况下Psⁿ＝p1ⁿ。

在M≥2的情况下，传感器信息取得部12使用表示物体10的形状的形状数据，根据3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ而对物体10的传感器坐标系下的3维位置Psⁿ进行计算。

在该实施方式1中，机器人手1对球体的物体10进行抓持，因此，3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ是球体的表面上的任意点。

因此，传感器信息取得部12使用表示球体的形状的球的方程式作为表示物体10的形状的形状数据，将球的方程式与3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ进行拟合，由此，能够得到3维位置Psⁿ作为物体10的中心的位置(X0，Y0，Z0)。

作为将球的方程式与3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ进行拟合的方法，可以想到多个方法，但这里作为一个例子，对通过最小二乘法实现的拟合方法进行说明。

首先，球的方程式由以下的式(3)表示。

(X-X0)²+(Y-Y0)²+(Z-Z0)²＝r² (3)

在式(3)中，r是球体的半径。

此时，M点的3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ的平方误差由以下的式(4)表示。

在式(4)中，||x||是表示矢量x的范数的记号。

传感器信息取得部12计算由式(4)表示的平方误差最小的Psⁿ作为传感器坐标系下的3维位置Psⁿ。

对于计算由式(4)表示的平方误差最小的Psⁿ的方法也存在多个，例如可以使用非专利文献1所公开的“Direct Least Squares Algorithm”等方法。

在该实施方式1中，对球体是纯球的例子进行说明，但物体10也可以不是纯球而是椭圆体，在该情况下，使用表示椭圆体的形状的方程式即可。

[非专利文献1]

David Eberly著“Least Squares Fitting of Data,”1999年7月15日发行

https://www.geometrictools.com/Documentation/LeastSquaresFitting.pdf

此外，有时在M点的3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ中还包含除了由机器人手1抓持的物体10的3维位置以外的不需要的位置的测量结果。

在该情况下，需要从M点的3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ中，将不需要的位置的测量结果去除。不需要的位置的测量结果的去除可以通过人手动去除，但也可以使用例如以下的非专利文献2所记载的方式进行去除。

在非专利文献2中记载了如下方法，即，从M点的3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ中，自动提取由机器人手1抓持的物体10的区域的测量结果。

[非专利文献2]

Anas Abuzaina等著“Sphere Detection in Kinect Point Clouds via the 3D Hough Transform,”Computer Analysis of Images and Patterns，vol.8048，pp.290～297，Springer，2013年发行

变换系数计算部13取得由位置姿态信息取得部11确定出的物体10的机器人坐标系下的3维位置Prⁿ、和由传感器信息取得部12确定出的物体10的传感器坐标系下的3维位置Psⁿ，将机器人坐标系下的3维位置Prⁿ和传感器坐标系下的3维位置Psⁿ的组保存于存储部13a(图4的步骤ST4)。

变换系数计算部13如果将机器人坐标系下的3维位置Prⁿ和传感器坐标系下的3维位置Psⁿ的组保存于存储部13a，则判定对机器人臂部或机器人手1的关节进行控制的次数n是否已达到N次(图4的步骤ST5)。

在该实施方式1中，N是大于或等于3的整数，在该阶段中n＝1，因此判定为对机器人臂部或机器人手1的关节进行控制的次数n未达到N次。

变换系数计算部13如果判定为对机器人臂部或机器人手1的关节进行控制的次数n未达到N次(图4的步骤ST5：NO的情况)，则将对机器人手1的位置或姿态的切换进行指示的控制信号输出至机器人控制部2(图4的步骤ST6)。

如果从变换系数计算部13输出对机器人手1的位置或姿态的切换进行指示的控制信号，则重复进行步骤ST1～ST5的处理。

即，机器人控制部2如果从变换系数计算部13接收到控制信号，则进行如下控制，即，按照该控制信号使机器人臂部或机器人手1的关节的角度改变，由此对机器人手1的位置及姿态进行切换。

由此，由机器人手1抓持的物体10在机器人坐标系下移动至Prⁿ的位置(图4的步骤ST1)。例如，如果对机器人臂部或机器人手1的关节进行控制的次数n是2，则在机器人坐标系下移动至Pr²的位置。例如，如果对机器人臂部或机器人手1的关节进行控制的次数n是3，则在机器人坐标系下移动至Pr³的位置。

位置姿态信息取得部11取得表示机器人手1的位置及姿态的位置姿态信息，根据位置姿态信息，对物体10的机器人坐标系下的3维位置Prⁿ进行确定(图4的步骤ST2)。

传感器3通过机器人控制部2对在机器人坐标系下移动至Prⁿ的位置的物体10进行测量，作为物体10的测量结果，输出表示物体10的传感器坐标系下的3维位置Psⁿ的传感器信息。

传感器信息取得部12取得从传感器3输出的传感器信息，根据传感器信息，对物体10的传感器坐标系下的3维位置Psⁿ进行确定(图4的步骤ST3)。

变换系数计算部13取得由位置姿态信息取得部11确定出的物体10的机器人坐标系下的3维位置Prⁿ和由传感器信息取得部12确定出的物体10的传感器坐标系下的3维位置Psⁿ，将机器人坐标系下的3维位置Prⁿ和传感器坐标系下的3维位置Psⁿ的组保存于存储部13a(图4的步骤ST4)。

变换系数计算部13如果将机器人坐标系下的3维位置Prⁿ和传感器坐标系下的3维位置Psⁿ的组保存于存储部13a，则判定对机器人臂部或机器人手1的关节进行控制的次数n是否已达到N次(图4的步骤ST5)。

变换系数计算部13如果判定为对机器人臂部或机器人手1的关节进行控制的次数n达到N次(图4的步骤ST5：YES的情况)，则对旋转矩阵R及平移矢量t进行计算，作为变换系数(图4的步骤ST7)。

如果使用旋转矩阵R及平移矢量t，则将传感器坐标系下的3维位置Psⁿ变换为机器人坐标系下的3维位置Prⁿ时的刚体变换由以下的式(5)表示。

下面，具体说明变换系数计算部13涉及的旋转矩阵R及平移矢量t的计算处理。

变换系数计算部13取得存储部13a所保存的N个组。

即，变换系数计算部13如以下所示，取得机器人坐标系下的3维位置Prⁿ和传感器坐标系下的3维位置Psⁿ的N个组。n＝1，2，...，N。

Pr¹-Ps¹的组

Pr²-Ps²的组

:

Pr^N-Ps^N的组

变换系数计算部13如果取得机器人坐标系下的3维位置Prⁿ和传感器坐标系下的3维位置Psⁿ的N个组，则如以下所示，根据N个组，对旋转矩阵R及平移矢量t进行计算。

作为根据N个组对旋转矩阵R进行计算的方法，例如可以使用以下的非专利文献3所记载的方法。

非专利文献3所记载的方法是使N个组中的3维位置Prⁿ和3维位置Psⁿ尽量接近的方法。

即，非专利文献3所记载的方法是对使得以下的式(6)最小的旋转矩阵R进行计算的方法。

[非专利文献3]

八木康史等编“コンピュータビジョン最先端ガイド3”“アドコム·メディア株式会社”，2010年12月8日发行，p.36～37

变换系数计算部13对使得式(6)最小的旋转矩阵R进行计算，因此，首先对由以下的式(7)表示的协方差矩阵A进行计算。

在式(7)中，μr是机器人坐标系下的物体10的观测点的重心坐标，由以下的式(8)表示。

μs是传感器坐标系下的物体10的观测点的重心坐标，由以下的式(9)表示。

变换系数计算部13如果计算出协方差矩阵A，则如以下的式(10)所示，对协方差矩阵A进行奇异值分解。

A→USV^T (10)

在式(10)中，U、V是酉矩阵，酉矩阵U、V是通过对协方差矩阵A进行奇异值分解而得到的。

变换系数计算部13如以下的式(11)所示，使用酉矩阵U、V而对旋转矩阵R进行计算。

在式(11)中，det(X)表示矩阵X的矩阵式。

变换系数计算部13如果计算出旋转矩阵R，则如以下的式(12)所示，使用旋转矩阵R和物体10的观测点的重心坐标μr、μs，对平移矢量t进行计算。

t＝μs-Rμr (12)

坐标变换部14取得由变换系数计算部13计算出的旋转矩阵R及平移矢量t，对旋转矩阵R及平移矢量t进行存储。

然后，坐标变换部14如果从传感器信息取得部12接收到传感器信息，则将所存储的旋转矩阵R及平移矢量t和该传感器信息所表示的物体10的传感器坐标系下的3维位置Ps代入至以下的式(13)，由此将物体10的传感器坐标系下的3维位置Ps变换为机器人坐标系下的3维位置Pr(图4的步骤ST8)。

Pr＝RPs+t (13)

根据上述可明确，根据该实施方式1，构成为，变换系数计算部13在每次对机器人手1的位置或姿态进行切换时，对由位置姿态信息取得部11确定出的机器人坐标系下的位置和由传感器信息取得部12取得的传感器信息所表示的传感器坐标系下的位置的组进行保存，根据所保存的多个组，对在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用的变换系数进行计算，因此实现如下效果，即，即使传感器3是激光扫描器等3维传感器，也能够对在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用的变换系数进行计算。

实施方式2.

在上述实施方式1中，示出了由机器人手1抓持的物体10是球体的例子，在该实施方式2中，对由机器人手1抓持的物体10是平板的例子进行说明。

图5是表示安装有本发明的实施方式2涉及的变换系数计算装置6的机器人系统的结构图。在图5中，与图1相同的标号表示相同或相当的部分，因此省略说明。

传感器5是对由机器人手1抓持的物体10进行测量的激光扫描器等3维传感器，输出表示物体10的测量结果的传感器信息。

从传感器5输出的传感器信息是空间中的物体10的表面即平面的参数(下面，称为“平面参数”)的集合。作为基于传感器5的物体10的测量方法，已知基于立体视觉的方法、Time-of-Flight法等。

此外，传感器5输出作为平面参数的集合的传感器信息，除此之外与图1的传感器3相同地，输出作为物体10的表面的3维坐标(X，Y，Z)的集合的传感器信息。

变换系数计算装置6具备位置姿态信息取得部41、传感器信息取得部42、变换系数计算部43及坐标变换部44。

图6是表示本发明的实施方式2涉及的变换系数计算装置6的硬件结构图。

位置姿态信息取得部41例如由图6所示的位置姿态信息取得电路51实现，实施如下处理，即，取得表示机器人手1的位置及姿态的位置姿态信息，根据位置姿态信息对物体10的机器人坐标系下的平面参数进行确定。

传感器信息取得部42例如由图6所示的传感器信息取得电路52实现，实施如下处理，即，从传感器5取得表示物体10的传感器坐标系下的平面参数的传感器信息。

变换系数计算部43例如由图6所示的变换系数计算电路53实现。

变换系数计算部43具备存储部43a，该存储部43a在每次通过机器人控制部2对机器人手1的位置或姿态进行切换时，对由位置姿态信息取得部41确定出的机器人坐标系下的平面参数和由传感器信息取得部42取得的传感器信息所表示的传感器坐标系下的平面参数的组进行保存。

变换系数计算部43实施如下处理，即，根据存储部43a所保存的多个组，计算出刚体变换中的旋转矩阵R及平移矢量t，作为在将传感器坐标系下的3维位置变换为机器人坐标系下的3维位置时使用的变换系数。

另外，变换系数计算部43实施如下处理，即，在每次对机器人坐标系下的平面参数和传感器坐标系下的平面参数的组进行保存时，将对机器人手1的位置或姿态的切换进行指示的控制信号输出至机器人控制部2。

坐标变换部44例如由图6所示的坐标变换电路54实现。

坐标变换部44实施如下处理，即，使用由变换系数计算部43计算出的变换系数即旋转矩阵R及平移矢量t，将由传感器信息取得部42取得的传感器信息所表示的传感器坐标系下的3维位置变换为机器人坐标系下的3维位置。

在图5中，假想变换系数计算装置6的结构要素由专门的硬件实现。

但是，变换系数计算装置6的结构要素并不限定于由专门的硬件实现，变换系数计算装置6也可以由软件、固件、或者软件和固件的组合实现。

在变换系数计算装置6由软件或固件等实现的情况下，将变换系数计算部45所具备的存储部45a构成于图3所示的计算机的存储器31上，并且将变换系数计算程序储存于存储器31，由计算机的处理器32执行存储器31所储存的程序即可，其中，变换系数计算程序用于使计算机执行位置姿态信息取得部41的位置姿态信息取得处理流程、传感器信息取得部42的传感器信息取得处理流程、变换系数计算部43的变换系数计算处理流程及坐标变换部44的坐标变换处理流程。

图7是表示与变换系数计算装置4由软件或固件等实现的情况下的处理流程相当的变换系数计算方法的流程图。

下面说明动作。

在该实施方式2中，作为物体10的平板，假定为是在机器人坐标系中无限扩大的平面，平板的平面设为由以下的式(14)所表示的平面参数πr表示。

πr＝(nr，dr) (14)

在式(14)中，nr是平面的法线矢量，dr是机器人坐标系的原点至平面的距离。机器人坐标系的原点例如是机器人手1的中心位置。

机器人手1对平板的物体10进行抓持。

机器人控制部2通过对机器人臂部或机器人手1的关节进行控制，从而对机器人手1的位置或姿态进行切换。

在该实施方式2中，也与上述实施方式1相同地，机器人控制部2通过按照控制信号使机器人臂部或机器人手1的关节的角度变化，从而对机器人手1的位置或姿态进行切换。

另外，在该实施方式2中，为了方便说明，设为对机器人臂部或机器人手1的关节进行N(N为大于或等于3的整数)次控制。

由机器人手1抓持的物体10的平面通过机器人控制部2对机器人臂部或机器人手1的关节进行控制，由此在进行了第n(n＝1，2，...，N)次的控制中，成为由以下的式(15)所示的平面参数πrⁿ表示的平面(图7的步骤ST11)。在该阶段中，由于n＝1，因此物体10的平面成为由平面参数πr¹表示的平面。

位置姿态信息取得部41取得表示机器人手1的位置及姿态的位置姿态信息，根据位置姿态信息，对物体10的机器人坐标系下的平面参数πrⁿ进行确定(图7的步骤ST12)。

根据位置姿态信息对机器人坐标系下的平面参数πrⁿ进行确定的处理是公知技术，因此省略详细说明，例如可以通过以下所示的处理对平面参数πrⁿ进行确定。

例如，事先准备表示机器人手1的位置及姿态和由机器人手1抓持的物体10的平面参数πrⁿ的对应关系的表格，位置姿态信息取得部41通过参照表格，对与位置姿态信息所表示的位置及姿态相对应的平面参数πrⁿ进行确定。

传感器5通过机器人控制部2而输出表示由切换了位置或姿态的机器人手1抓持的物体10的传感器坐标系下的平面参数πsⁿ(n＝1，2，...，N)的传感器信息。

在该实施方式2中，从传感器5输出的传感器信息是表示物体10的表面的M点的平面参数的3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ。

传感器信息取得部42取得从传感器5输出的传感器信息，根据传感器信息对物体10的传感器坐标系下的平面参数πsⁿ(n＝1，2，...，N)进行确定(图7的步骤ST13)。

M是大于或等于1的整数，在M＝1的情况下πsⁿ＝p1ⁿ。

在M≥2的情况下，传感器信息取得部42使用表示物体10的形状的形状数据，根据3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ而对物体10的传感器坐标系下的平面参数πsⁿ进行计算。

在该实施方式2中，机器人手1对平板的物体10进行抓持，因此，3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ是平板的表面上的任意点。

因此，传感器信息取得部42使用表示平板的形状的平面的方程式作为表示物体10的形状的形状数据，将平面的方程式与3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ进行拟合，由此，能够得到物体10的传感器坐标系下的平面参数πsⁿ。

作为将平面的方程式与3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ进行拟合的方法，可以想到多个方法，与上述实施方式1相同地，可以使用通过最小二乘法实现的拟合方法。平面的方程式例如由以下的式(17)表示。

a(X-X0)+b(Y-Y0)+c(Z-Z0)＝0 (17)

在式(17)中，(a，b，c)是平面的法线矢量。

变换系数计算部43取得由位置姿态信息取得部41确定出的物体10的机器人坐标系下的平面参数πrⁿ、由传感器信息取得部42确定出的物体10的传感器坐标系下的平面参数πsⁿ，将机器人坐标系下的平面参数πrⁿ和传感器坐标系下的平面参数πsⁿ的组保存于存储部43a(图7的步骤ST14)。

变换系数计算部43如果将机器人坐标系下的平面参数πrⁿ和传感器坐标系下的平面参数πsⁿ的组保存于存储部43a，则判定对机器人臂部或机器人手1的关节进行控制的次数n是否已达到N次(图7的步骤ST15)。

在该实施方式2中，N是大于或等于3的整数，在该阶段中，由于n＝1，因此判定为对机器人臂部或机器人手1的关节进行控制的次数n未达到N次。

变换系数计算部43如果判定为对机器人臂部或机器人手1的关节进行控制的次数n未达到N次(图7的步骤ST15：NO的情况)，则将对机器人手1的位置或姿态的切换进行指示的控制信号输出至机器人控制部2(图7的步骤ST16)。

如果从变换系数计算部43输出对机器人手1的位置或姿态的切换进行指示的控制信号，则重复进行步骤ST11～ST15的处理。

即，机器人控制部2如果从变换系数计算部43接收到控制信号，则进行如下控制，即，按照该控制信号使机器人臂部或机器人手1的关节的角度改变，由于对机器人手1的位置及姿态进行切换。

由此，由机器人手1抓持的物体10的平面成为由平面参数πrⁿ表示的平面(图7的步骤ST11)。例如，如果对机器人臂部或机器人手1的关节进行控制的次数n是2，则物体10的平面成为由平面参数πr²表示的平面，例如，如果对机器人臂部或机器人手1的关节进行控制的次数n是3，则物体10的平面成为由平面参数πr³表示的平面。

位置姿态信息取得部41取得表示机器人手1的位置及姿态的位置姿态信息，根据位置姿态信息，对物体10的机器人坐标系下的平面参数πrⁿ进行确定(图7的步骤ST12)。

传感器5通过机器人控制部2而输出表示由切换了位置或姿态的机器人手1抓持的物体10的传感器坐标系下的平面参数πsⁿ(n＝1，2，...，N)的传感器信息。

传感器信息取得部42取得从传感器5输出的传感器信息，根据传感器信息对物体10的传感器坐标系下的平面参数πsⁿ进行确定(图7的步骤ST13)。

变换系数计算部43取得由位置姿态信息取得部41确定出的物体10的机器人坐标系下的平面参数πrⁿ、由传感器信息取得部42确定出的物体10的传感器坐标系下的平面参数πsⁿ，将机器人坐标系下的平面参数πrⁿ和传感器坐标系下的平面参数πsⁿ的组保存于存储部43a(图7的步骤ST14)。

变换系数计算部43如果将机器人坐标系下的平面参数πrⁿ和传感器坐标系下的平面参数πsⁿ的组保存于存储部43a，则判定对机器人臂部或机器人手1的关节进行控制的次数n是否已达到N次(图7的步骤ST15)。

变换系数计算部43如果判定为对机器人臂部或机器人手1的关节进行控制的次数n已达到N次(图7的步骤ST15：YES的情况)，则作为变换系数而对旋转矩阵R及平移矢量t进行计算(图7的步骤ST17)。

对于机器人坐标系下的平面参数πrⁿ和传感器坐标系下的平面参数πsⁿ的关系，如果使用旋转矩阵R及平移矢量t，则由以下的式(18)表示。

下面，具体说明变换系数计算部13涉及的旋转矩阵R及平移矢量t的计算处理。

变换系数计算部43取得存储部43a所保存的N个组。

即，变换系数计算部43如以下所示，取得机器人坐标系下的平面参数πrⁿ和传感器坐标系下的平面参数πsⁿ的N个组。n＝1，2，...，N。

πr¹-πs¹的组

πr²-πs²的组

:

πr^N-πs^N的组

变换系数计算部43如果取得机器人坐标系下的平面参数πrⁿ和传感器坐标系下的平面参数πsⁿ的N个组，则如以下所示，根据N个组而对旋转矩阵R及平移矢量t进行计算。

作为根据N个组对旋转矩阵R进行计算的方法，例如可以使用以下的非专利文献4所记载的方法。

非专利文献4所记载的方法是对使得以下的式(19)最小的旋转矩阵R进行计算的方法。

[非专利文献4]

Yuichi Taguchi等著、“Point-Plane SLAM for Hand-Held 3D Sensors,”Proceedings of IEEE International Conference on Robotics and Automation，pp.5182-5189，2013年发行

变换系数计算部43如果对旋转矩阵R进行计算，则对以下的式(20)所示的联立1次方程式进行求解，由此求出平移矢量t。

坐标变换部44取得由变换系数计算部43计算出的旋转矩阵R及平移矢量t，对旋转矩阵R及平移矢量t进行存储。

然后，坐标变换部44如果从传感器信息取得部12接收到表示物体10的传感器坐标系下的3维位置Ps的传感器信息，则将所存储的旋转矩阵R及平移矢量t、和传感器信息所表示的3维位置Ps代入至以下的式(21)，由此将物体10的传感器坐标系下的3维位置Ps变换为机器人坐标系下的3维位置Pr(图7的步骤ST18)。

Pr＝RPs+t (21)

根据上述可明确，根据该实施方式2，构成为，变换系数计算部43在每次对机器人手1的位置或姿态进行切换时，对由位置姿态信息取得部41确定出的机器人坐标系下的平面参数和由传感器信息取得部42取得的传感器信息所表示的传感器坐标系下的平面参数的组进行保存，根据所保存的多个组，对在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用的变换系数进行计算，因此实现如下效果，即，即使传感器5是激光扫描器等3维传感器，也能够对在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用的变换系数进行计算。

实施方式3.

在上述实施方式1中，示出了由机器人手1抓持的物体10是球体的例子，在上述实施方式2中，示出了由机器人手1抓持的物体10是平板的例子。由机器人手1抓持的物体10并不限定于球体或平板。

在该实施方式3中，如果物体10的形状是由3维模型表示的形状，则作为表示物体10的形状的形状数据，使用3维模型。

例如对于如具有角等的立方体这样，在形状上具有特征的物体10，可以由3维模型来表示形状。另外，也可以从传感器信息所表示的3维点阵数据提取出角等具有特征的点。

作为3维模型，可以使用由机器人手1抓持的物体10的CAD模型或多边形数据等。

在上述实施方式1中，传感器信息取得部12在对物体10的传感器坐标系下的3维位置Psⁿ进行计算时，作为表示物体10的形状的形状数据而使用表示球体的形状的球的方程式，与此相对，不同点在于，在该实施方式3中使用物体10的3维模型。

因此，在该实施方式3中，传感器信息取得部12使用物体10的3维模型作为表示物体10的形状的形状数据，将物体10的3维模型与表示物体10的角等具有特征的点的位置的3维点阵数据p1ⁿ、p2ⁿ、...、pMⁿ进行拟合，由此，可以得到3维位置Psⁿ作为物体10的中心的位置。

其他与上述实施方式1相同，因此省略详细说明。

实施方式4.

在上述实施方式1、2中，示出了变换系数计算部13或变换系数计算部43对在将传感器坐标系下的3维位置变换为机器人坐标系下的3维位置时使用的变换系数进行计算的例子。

但是，根据由机器人手1抓持的物体10的姿态，有时传感器3或传感器5的测量结果会包含大的误差。如果传感器3或传感器5的测量结果包含大的误差，则从传感器3或传感器5将包含大的误差的传感器信息输出至变换系数计算部13或变换系数计算部43，因此，由变换系数计算部13或变换系数计算部43计算出的变换系数的精度变差。

例如在物体10是平板的情况下，如果对平板与传感器5正对的状况和平板与传感器5不正对的状况进行比较，则在不正对的状况下测量出的传感器信息包含大的误差。

但是，根据形状、材质等物体10的特性，还有时即使物体10的姿态变化而传感器信息所包含的误差也不变化。

在该实施方式4中，对如下例子进行说明，即，能够避免根据物体10的姿态而由变换系数计算部13或变换系数计算部43计算出的变换系数的精度变差的状况。

图8是表示安装有本发明的实施方式4涉及的变换系数计算装置6的机器人系统的结构图。在图8中，与图5相同的标号表示相同或相当的部分。

图9是表示本发明的实施方式4涉及的变换系数计算装置6的硬件结构图。在图9中，与图6相同的标号表示相同或相当的部分。

变换系数计算装置6具备位置姿态信息取得部41、传感器信息取得部42、变换系数计算部45、坐标变换部44及位置姿态调整部46。

变换系数计算部45例如由图9所示的变换系数计算电路55实现。

变换系数计算部45与图5的变换系数计算部43相同地，具备存储部45a，该存储部45a在每次由机器人控制部2对机器人手1的位置或姿态进行切换时，对由位置姿态信息取得部41确定出的机器人坐标系下的平面参数和由传感器信息取得部42取得的传感器信息所表示的传感器坐标系下的平面参数的组进行保存。

变换系数计算部45与图5的变换系数计算部43相同地，实施如下处理，即，根据存储部45a所保存的多个组，计算出刚体变换中的旋转矩阵R及平移矢量t，作为在将传感器坐标系下的3维位置变换为机器人坐标系下的3维位置时使用的变换系数。

另外，变换系数计算部45与图5的变换系数计算部43相同地，实施如下处理，即，在每次对机器人坐标系下的平面参数和传感器坐标系下的平面参数的组进行保存时，将对机器人手1的位置或姿态的切换进行指示的控制信号输出至机器人控制部2。

变换系数计算部45与图5的变换系数计算部43不同，如果由位置姿态调整部46对机器人手1的位置或姿态进行调整，则之后在每次由机器人控制部2对机器人手1的位置或姿态进行切换时，将机器人坐标系下的平面参数和传感器坐标系下的平面参数的组保存于存储部45a。

另外，变换系数计算部45实施如下处理，即，根据存储部45a所保存的多个组，再次计算出刚体变换中的旋转矩阵R及平移矢量t，作为在将传感器坐标系下的3维位置变换为机器人坐标系下的3维位置时使用的变换系数。

位置姿态调整部46例如由图9所示的位置姿态调整电路56实现。

位置姿态调整部46在由变换系数计算部45最初计算出变换系数之后，对机器人控制部2进行控制，由此对机器人手1的位置或姿态进行调整。

在该实施方式4中，说明了将变换系数计算部45及位置姿态调整部46应用于图5的变换系数计算装置6的例子，但也可以应用于图1的变换系数计算装置4。

在应用于图1的变换系数计算装置4的情况下，变换系数计算部45与图1的变换系数计算部13相同地，对由位置姿态信息取得部11确定出的机器人坐标系下的3维位置和由传感器信息取得部12取得的传感器信息所表示的传感器坐标系下的3维位置的组进行保存。

另外，变换系数计算部45与图1的变换系数计算部13相同地，根据存储部45a所保存的多个组，计算出刚体变换中的旋转矩阵R及平移矢量t，作为在将传感器坐标系下的3维位置变换为机器人坐标系下的3维位置时使用的变换系数。

在图8中，假设变换系数计算装置6的结构要素由专门的硬件实现。

但是，变换系数计算装置6的结构要素并不限定于由专门的硬件实现，变换系数计算装置6也可以由软件、固件、或者软件和固件的组合来实现。

在变换系数计算装置6由软件或固件等实现的情况下，将变换系数计算部45所具备的存储部45a构成于图3所示的计算机的存储器31上，并且将用于使计算机执行位置姿态信息取得部41的位置姿态信息取得处理流程、传感器信息取得部42的传感器信息取得处理流程、变换系数计算部45的变换系数计算处理流程、坐标变换部44的坐标变换处理流程及位置姿态调整部46的处理流程的变换系数计算程序储存于存储器31，由计算机的处理器32执行储存于存储器31的程序即可。

下面说明动作。

在该实施方式4中，说明了物体10是平板的例子。

变换系数计算部45与上述实施方式2中的图5的变换系数计算部43相同地，通过执行图7的步骤ST11～ST17而计算出旋转矩阵R及平移矢量t，作为变换系数。

变换系数计算部45在计算出旋转矩阵R及平移矢量t之后，设定使得成为平板大致与传感器5正对的状况的平面参数πs＝(ns，ds)。

即，变换系数计算部45对使得表示传感器5的光轴的矢量na和平板的物体10的平面的法线矢量nr所成的角处于例如5度以内、且从机器人坐标系的原点至平面的距离dr处于传感器5的能够进行测距的范围内的、传感器坐标系下的平面参数πs＝(ns，ds)进行设定。

变换系数计算部45使用已计算出的旋转矩阵R及平移矢量t，将已设定的传感器坐标系下的平面参数πs＝(ns，ds)变换为机器人坐标系下的平面参数πr＝(nr，dr)。

位置姿态调整部46以使得物体10的平面成为通过由变换系数计算部45变换后的平面参数πr＝(nr，dr)而表示的平面的方式，对机器人控制部2进行控制，由此对机器人手1的位置或姿态进行调整。

如果通过位置姿态调整部46对机器人手1的位置或姿态进行调整，则变换系数计算部45将图7的步骤ST11～ST15执行N次，由此将机器人坐标系下的平面参数πrⁿ和传感器坐标系下的平面参数πsⁿ的组分别保存于存储部45a。

变换系数计算部45从存储部45a取得机器人坐标系下的平面参数πrⁿ和传感器坐标系下的平面参数πsⁿ的N个组，根据N个组再次对旋转矩阵R进行计算。

变换系数计算部45如果再次计算出旋转矩阵R，则使用该旋转矩阵R而再次对平移矢量t进行计算。

根据上述可明确，根据该实施方式4，构成为，具备位置姿态调整部46，该位置姿态调整部46在由变换系数计算部45计算出变换系数之后，对机器人手1的位置或姿态进行调整，变换系数计算部45在由位置姿态调整部46对机器人手1的位置或姿态进行调整之后，再次在每次对机器人手1的位置或姿态进行切换时对由位置姿态信息取得部41确定出的机器人坐标系下的平面参数、和由传感器信息取得部42取得的传感器信息所表示的传感器坐标系下的平面参数的组进行保存，根据所保存的多个组，对在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用的变换系数进行计算，因此，实现如下效果，即，能够避免根据物体10的姿态而由变换系数计算部45计算出的变换系数的精度变差的状况。

此外，本发明在其发明的范围内，能够进行各实施方式的自由组合，或者各实施方式的任意的结构要素的变形，或者在各实施方式中能够省略任意的结构要素。

工业实用性

本发明适于对在将传感器坐标系下的位置变换为机器人坐标系下的位置时使用的变换系数进行计算的变换系数计算装置、变换系数计算方法及变换系数计算程序。

标号的说明

1机器人手，2机器人控制部，3、5传感器，4、6变换系数计算装置，10物体，11位置姿态信息取得部，12传感器信息取得部，13变换系数计算部，13a存储部，14坐标变换部，21位置姿态信息取得电路，22传感器信息取得电路，23变换系数计算电路，24坐标变换电路，31存储器，32处理器，33通信电路，34显示装置，41位置姿态信息取得部，42传感器信息取得部，43变换系数计算部，43a存储部，44坐标变换部，45变换系数计算部，45a存储部，46位置姿态调整部，51位置姿态信息取得电路，52传感器信息取得电路，53变换系数计算电路，54坐标变换电路，55变换系数计算电路，56位置姿态调整电路。