CN102653099A - 机械手位置检测装置以及机械手*** - Google Patents

Abstract

本发明涉及一种检测机械手位置的机械手位置检测装置以及具备该机械手位置检测装置的机械手***。位置算出部具备：触发信号输入部，被输入触发信号；位置数据输入部，延迟通信时间地被输入从编码器输出的位置数据；数据处理部，取得被输入的位置数据，算出检测出触发信号时的机械手的位置。数据处理部使用检测触发信号前所取得的第1位置数据和其定时、以及检测到触发信号后的第2位置数据和其定时，对机械手的位置进行线性插值，算出与被输入触发信号的定时相比滞后通信时间的位置，作为检测出触发信号时的机械手的位置。

Description

机械手位置检测装置以及机械手***

技术区域

本发明涉及一种检测机械手位置的机械手位置检测装置以及具备该机械手位置检测装置的机械手***。

背景技术

以往，作为对把持工件的机械手的目标位置进行修正的方法，已知有一种例如像专利文献1那样的技术。在专利文献1中，为了将机械手所把持的工件和设置于该机械手的基准标记配置到对工件进行拍摄的摄像装置的摄像范围内，使摄像装置以及机械手分别移动至规定的位置。而且，通过对由摄像装置拍摄到的图像进行图像处理，取得工件相对基准标记的中心坐标和倾斜度等，基于该取得的中心坐标等算出与机械手的目标位置相关的位置修正量。

专利文献1：日本专利第4539685号公报

不过，在专利文献1的方法中，虽然能够准确算出与机械手的目标位置相关的位置修正量，但在对用于算出位置修正量的图像进行拍摄时需要停止机械手。虽说是为了算出位置修正量，但若将机械手停止在规定的位置，则与其停止的时间量对应地装置的生产效率降低。为了抑制这种生产效率的降低，希望不停止机械手地对移动中的工件进行拍摄，基于该拍摄时的机械手的位置和拍摄后的工件的图像取得机械手的位置修正量。

另一方面，表示机械手的位置的来自编码器的数据通常通过串行通信被输入到机械手控制器。因此，与该数据被输出的定时相比，在滞后了与串行通信的通信周期以及数据量等对应的、编码器和机械手控制器之间的通信时间的定时，被输入到机械手控制器。此时，若像上述那样机械手不断地移动，则基于编码器算出的位置和从图像得到的位置在时间轴上相互不同。

发明内容

本发明是鉴于上述的实际状况而完成的发明，其目的在于，提供一种针对从移动中的机械手检测到的该机械手的位置，可提高其精度的机械手位置检测装置以及机械手***。

本发明的机械手位置检测装置具有：位置数据输入部，其被从编码器输入表示机械手的位置的位置数据；和位置算出部，其取得上述被输入的位置数据，算出机械手的位置；上述位置算出部接受用于检测上述机械手的位置的指令，使用在上述指令之前的第1定时取得的第1位置数据、在上述指令之后的第2定时取得的第2位置数据、和上述编码器与上述位置数据输入部之间的通信所需的通信时间，通过上述第1位置数据和上述第2位置数据的线性插值，算出与被输入上述指令的定时相比滞后上述通信时间的定时的位置，并将该算出结果作为被输入上述指令时的上述机械手位置而输出。

根据该机械手位置检测装置，由于基于与指令被输入的定时相比滞后了编码器和位置数据输入部之间的通信所需要的通信时间的定时，利用线性插值输出机械手的位置，所以线性插值所使用的各数据的定时成为大致相等的时间轴上的定时。因此，通过使用第1位置数据和第2位置数据的线性插值而算出的机械手的位置，成为以通信时间修正机械手移动的移动量后的位置。因此，能够更高精度地检测移动中的机械手的位置。

在该机械手位置检测装置中，上述位置算出部具备指令检测部，该指令检测部能够以比取得从一个编码器输入给上述位置输入部的上述位置数据的规定周期T短的周期Ts检测该指令。

根据该机械手位置检测装置，与被从一个编码器输入位置数据的定时相比，可检测指令的定时变多。结果，能够检测出与被输入指令的定时更接近的时期的机械手的位置。

在该机械手位置检测装置中，上述位置算出部按照每个周期T/n，取得来自相互不同的编码器的位置数据，按照每个上述编码器输出被输入上述指令时的上述机械手的位置，所述周期T/n是将上述规定的周期T以1以上的整数n进行n等分而得到的周期。

根据该机械手位置检测装置，由于按照相互不同的编码器算出机械手的位置，所以能够以更高的精度检测指令被输入时的机械手的位置。

在该机械手位置检测装置中，上述位置算出部对取得的上述位置数据是否正常进行判断，并且在判断为检测到上述指令时的前一上述位置数据不正常的情况下，将在该位置数据之前取得的前一位置数据作为上述第1位置数据使用。

由于从编码器输入的位置数据是电信号，所以存在例如受到噪音等的影响的问题。若使用这样的受到影响的位置数据算出机械手的位置，则由位置算出部算出的机械手的位置的可靠性降低。在这一点上，根据上述结构的机械手位置检测装置，由于位置算出部对取得的位置数据是否正常进行判断，并且在判断为检测到指令时的前一位置数据不正常的情况下，将在该位置数据之前取得的前一位置数据作为第1位置数据使用，所以，能够使用正常的位置数据来算出机械手的位置。

在该机械手位置检测装置中，上述位置算出部在判断为紧接着检测到上述指令后取得的上述位置数据不正常的情况下，将在紧接着该位置数据之后取得的位置数据作为上述第2位置数据使用。

通常，第1定时和第2定时间隔越长，越会失去相互之间的位置的线性。在该机械手位置检测装置中，由于使用这样的线性插值，所以，第1定时和第2定时在时间上间隔越长，由位置算出部算出的机械手的位置和实际的机械手的位置越容易产生大的误差。在这一点上，根据上述结构，例如当将输入位置数据的定时、即检测到指令后的定时作为基准定时时，即便是第1定时和第2定时在时间上间隔最长的情况，第1位置数据也是在比基准定时提前2个定时输入的位置数据。第2位置数据是在比基准定时滞后2个定时输入的位置数据。结果，与基于在该第1定时之前取得的位置数据和在该第2定时之后取得的位置数据算出机械手的位置的情况相比，能够提高算出的机械手的位置精度。

本发明的机械手***具备：搬运工件的机械手；编码器，其输出表示上述机械手位置的位置数据；指令输出部，其输出用于检测上述机械手的位置的指令；和机械手位置检测部，其被输入上述位置数据和上述指令，基于上述位置数据检测被输入上述指令时的上述机械手的位置；上述机械手位置检测部是上述的机械手位置检测装置。

根据本发明的机械手***，即便在机械手移动中指令输出部输出了指令，也能够高精度地检测该机械手的位置。

该机械手***还具备：照相机，其通过在被输入来自上述指令输出部的指令的定时，对预定的拍摄范围进行拍摄，来取得包含向目标位置移动过程中的上述机械手所搬运的工件的图像；工件位置取得部，其基于上述照相机取得的图像，取得上述工件的位置；以及目标位置修正部，其基于上述工件位置取得部取得的上述工件的位置、和上述机械手位置检测部检测到的上述机械手的位置，对上述目标位置进行修正。

根据该机械手***，能够基于被输入来自指令输出部的指令的定时的机械手的位置与工件的位置的相对位置，对机械手的目标位置进行修正。如上所述，由机械手位置检测部检测的机械手的位置是使机械手移动并对其进行检测的位置，并且其精度提高。即，无需为了修正目标位置而停止机械手，并且，针对机械手的目标位置的修正量的精度也能够提高。结果，能够在抑制机械手***的生产效率降低的同时，将工件搬运到应该搬运的位置。

图1是表示本发明的一个实施方式所涉及的机械手***的概要结构的立体图。

图2是表示机械手的正面结构的主视图。

图3是表示机械手***的***结构的功能框图。

图4是表示取得各编码器的位置数据的定时的时序图。

图5是表示数据处理部的存储部所存储的数据的图。

图6是使机械手的位置与时间对应的图，是用于概念性说明算出机械手的位置的计算过程的说明图。

图7是表示算出机械手位置的处理步骤的流程图。

图8是表示算出机械手位置的处理步骤的流程图。

具体实施方式

下面，参照图1～图8，对本发明所涉及的机械手位置检测装置以及机械手***的一个实施方式进行说明。首先，参照图1及图2对本发明所涉及的机械手***的概要结构进行说明。

如图1所示，机械手***10中在以规定的搬运速度沿搬运方向C_V搬运工件W的传送带11的侧方，配置有对被传送带11搬运的工件W进行选择性地把持、对其进行规定作业的水平多关节型机械手12。在搬运方向C_V的机械手12的上游侧，设置有从上方对在传送带11上被搬运的工件W进行拍摄的照相机13。在该机械手***10中，基于由照相机13拍摄到的图像来选择成为作业对象的工件W，并基于该图像和测定传送带11的搬运量的编码器14，对被选择为作业对象的工作W的位置进行把握。被选择为作业对象的工件W在通过机械手12而上升到规定高度的状态下，被搬运到设置于该机械手12的可动范围内的作业台15上的规定位置。

如图2所示，机械手12中在设置于地面等的基台21的上端部设置有以沿垂直方向的控制轴C1为中心相对基台21可转动的基台轴22。通过设置于基台21内的第1马达23正反旋转，基台轴22以控制轴C1为中心转动。

在基台轴22上连结固定有沿水平方向延伸的第1水平臂24的基端部。在第1水平臂24的前端部上连结固定有支承轴25。支承轴25支承第2水平臂26，该第2水平臂26以沿垂直方向的控制轴C2为中心相对支承轴25可转动。

在第2水平臂26的基端部配设有第2马达27。第2水平臂26借助因第2马达27正反旋转而使该第2马达27从支承轴25受到的反作用力，以控制轴C2为中心相对第1水平臂24在水平方向转动。

在第2水平臂26的前端部设置有贯通第2水平臂26的上下旋转轴29。上下旋转轴29被支承为可相对第2水平臂26旋转，且可在上下方向移动。而且，上下旋转轴29通过第2水平臂26所具备的升降马达30正反旋转，沿垂直方向的控制轴C3升降。上下旋转轴29通过第2水平臂26所具备的旋转马达31正反旋转，以沿垂直方向的自身的控制轴C4为中心正反旋转。能够在上下旋转轴29的作业部32上安装工具，例如把持被搬运物的手、加工被加工物的手等。

另外，在向作业台15搬运的工件W的搬运路径上，设置有检测工件W通过了检测线FL的检测传感器35。检测传感器35具备沿水平方向射出由指向性、收敛性高的激光构成的检测光LB的射出部36和接受由射出部36射出的检测光LB的受光部37。射出部36及受光部37被设置成隔着机械手12的可动范围，且检测光LB暂时被从传送带11向作业台15搬运的工件W遮挡。

在检测传感器35的下侧设置有与检测传感器35检测工件W同步地从下方对该工件W进行拍摄的照相机39。在该机械手***10中，基于由该照相机39拍摄到的图像和此时机械手12的位置，把握工件W相对机械手12的相对位置，来对机械手12的目标位置进行修正，以使工件W被搬运至预先设定的位置。

下面，参照图3～图8对上述结构的机械手***10的***结构进行说明。

如图3所示，上述的机械手***10通过控制器50被统一控制。控制器50是驱动控制机械手12的控制装置，对机械手12向目标位置移动的定位动作进行控制，或对机械手12追随在传送带11上移动的工件W的追随动作进行控制。

在控制器50中预先设定有表示机械手12的结构、动作特性以及可动范围内的区域即能够追随传送带11上的工件W的追随区域等的机械手数据。而且，设定有表示传送带11和机械手12的相对位置、传送带11和照相机39的相对位置及机械手12和照相机39的相对位置等的与各种位置相关的数据；以及表示工件种类和形状的工件数据等。并且，这些机械手数据、与各种位置相关的数据以及工件数据被用于机械手12的驱动控制。

控制器50具备：存储用户程序的用户程序存储部51、和执行存储在该用户程序存储部51的用户程序的用户程序执行部52。而且具备：收纳由照相机13识别出的工件W的初始位置的工件位置存储部53、和基于传送带11的移动量算出该工件位置存储部53的工件W的当前位置的工件位置更新部54。并且，具备生成作业部32的轨道(各控制轴C1～C4的关节角度)，并将其变换成马达指令值(脉冲)，输出给后述的马达控制部57的指令值生成部55。另外，还具备根据追随区域和工件W的位置关系等检测成为作业对象的工件W的作业对象检测部56、和根据来自指令值生成部55的马达指令值对各控制轴C1～C4的马达23、27、30、31进行控制的马达控制部57。

用户程序存储部51的用户程序是执行为了使机械手12的作业部32移动来追随作业对象的工件W所必要的各种处理的程序。而且，是执行为了使作业部32所把持的工件W移动到作业台15的规定位置所必要的各种处理的程序。

用户程序执行部52执行用户程序。用户程序执行部52根据作业对象检测部56的检测结果，将被检测为作业对象的工件W作为作业对象进行处理。并且，用户程序执行部52算出为了使机械手12追随被选择的工件W所必要的信息，而且算出为了使机械手12移动到目标位置所必要的信息，并将这些算出的数据传送给指令值生成部55。

另外，从后述的工件位置取得部58向作为目标位置修正部的用户程序执行部52输入搬运中的工件W的位置，并且由位置算出部60算出的移动中的机械手12的位置被输入给用户程序执行部52。用户程序执行部52基于从工件位置取得部58输入的工件W的位置和从位置算出部60输入的机械手12的位置，对机械手12的目标位置进行修正，并且算出为了移动到该修正后的目标位置所必要的信息，并将该算出的数据传送给指令值生成部55。

工件位置更新部54根据照相机13拍摄到的传送带11上的工件W的拍摄图像，求出工件W的初始位置，并将该初始位置的信息保存到工件位置存储部53。另一方面，工件位置更新部54基于由从编码器14输入的传送带脉冲的积算而得到的传送带脉冲数，算出通过传送带11传送的工件W的搬运量。然后，求出拍摄工件W时(初始位置)的传送带脉冲数和当前的传送带脉冲数之差，并利用该脉冲数的差与预先设定的单位传送带脉冲的搬运量的乘积，求出相对工件W的初始位置的搬运量。工件位置更新部54例如通过对工件位置存储部53中存储的拍摄时算出的工件W的初始位置(X轴方向的坐标)加上如上所述算出的工件W的搬运量等，能够逐次计算在传送带11上搬运的工件W的当前位置。

作业对象检测部56以规定的执行周期对工件W的当前位置和追随区域进行比较。然后，当工件W的当前位置到达(进入)追随区域内时，将该工件W检测为作业对象，并且，当该工件W的当前位置不包含在追随区域内时，不将该工件W检测为作业对象。然后，将检测到的结果传送给用户程序执行部52和指令值生成部55。

指令值生成部55基于被输入的各种数据，生成用于使机械手12移动至目标位置的轨道，并且计算用于使机械手12追随该轨道的各控制轴C1～C4的关节角度。指令值生成部55将用于使机械手12沿着轨道的各控制轴C1～C4的关节角度作为目标关节角度输出给马达控制部57。在本实施方式中，马达控制部57以控制周期T控制1个马达，并且以不同的定时控制各个马达。本实施方式的马达控制部57以控制周期T除以控制轴数n(n为1以上的整数，在本实施方式中n＝4)得到的周期T/n，对各马达23、27、30、31依次进行控制，并且以周期T/n依次输出由指令值生成部55输出的与各马达的目标关节角度相应的输出。即，控制器50以周期T/n按照第1马达23、第2马达27、升降马达30、旋转马达31的顺序切换成为控制对象的马达。

并且，控制器50具有：工件位置取得部58，其取得由检测传感器35检测出工件W时的该工件W的位置；以及位置算出部60，其算出该检测时的机械手12的位置。

工件位置取得部58通过对从照相机39输入的图像数据进行图像处理来取得工件W的位置数据，该照相机39从下方拍摄到达了检测线FL的工件W。工件位置取得部58通过上述图像处理，求出拍摄范围中的工件W的中心位置的坐标值，并输出给用户程序执行部52。

照相机39与作为指令输出部的触发信号生成部59电连接，若从该触发信号生成部59被输入作为指令的触发信号，则在其摄像范围进行拍摄，并将其拍摄到的图像的图像数据输出给工件位置取得部58。

若由检测传感器35检测到工件W，则触发信号生成部59生成作为指令的触发信号。触发信号生成部59将其生成的触发信号同时输出给照相机39和后述的触发信号输入部61。

下面，对作为机械手位置检测装置以及机械手位置检测部的位置算出部60进行详细说明。

如图3所示，位置算出部60具有作为指令检测部的触发信号输入部61、位置数据输入部62和数据处理部63。

触发信号输入部61被输入触发信号生成部59所输出的触发信号。触发信号输入部61以触发信号的检测周期检测触发信号的状态，在数据处理部63中，以马达的控制周期T除以控制轴数n(在本实施方式中n＝4)而得到的检测周期Ts(＝T/n)检测触发信号输入部61所检测到的触发信号的状态。

位置数据输入部62与基于各马达23、27、30、31的绝对位置来检测各控制轴C1、C2、C3、C4的关节角度的第1编码器64、第2编码器65、第3编码器66、第4编码器67连接。各编码器64～67每隔马达的控制周期T的一半时间(＝T/2)、即编码器和位置数据输入部62之间的通信所需的通信时间Td，检测所对应的关节角度，并将其检测到的表示关节角度的位置数据输出给位置数据输入部62。

另外，位置数据输入部62具有判定被输入的位置数据是否正常的判定部68。而且，位置数据输入部62使从各编码器64～67输入的位置数据与判定部68针对该位置数据的判定结果相对应。

数据处理部63取得被输入给位置数据输入部62的位置数据和与该位置数据对应的判定结果。其中，如果数据处理部63从位置数据输入部62同时取得与各编码器对应的位置数据，则需要将数据处理部63构成为与各位置数据对应，存在数据处理部63的结构变复杂的问题。鉴于此，数据处理部63中内置有每隔周期T/n便加一的计数器，即表示与成为控制对象的控制轴对应的计数值的计数器。而且，如图4所示，数据处理部63每隔控制周期T便依次取得来自各编码器64～67的位置数据，以使该计数器的计数值所表示的控制轴、和与被输入到位置数据输入部62的位置数据对应的控制轴匹配。

其中，来自编码器的位置数据通过通信时间Td的串行通信，被输入给位置数据输入部62。因此，数据处理部63所取得的位置数据是表示在比取得该位置数据的定时提前通信时间Td的定时，由编码器检测到的位置的位置数据。

如图5所示，数据处理部63具有存储部70，该存储部70中预先设置有存储与控制轴C1的关节角度、控制轴C2的关节角度、控制轴C3的关节角度、控制轴C4的关节角度相关的各种数据的区域。

在本实施方式中，将来自编码器的位置数据被输入给位置数据输入部62的定时中、检测到触发信号的下一定时作为该编码器的基准定时。存储部70中按照控制轴C1～C4每一个设置有对比上述基准定时靠前取得的位置数据、即第1位置进行存储的区域。第1位置73a是被从第1编码器64输入的位置数据。第1位置73b是被从第2编码器65输入的位置数据。第1位置73c是被从第3编码器66输入的位置数据。第1位置73d是从被第4编码器67输入的位置数据。而且，存储器70中按照控制轴C1～C4每一个设置有对比上述基准定时靠后取得的位置数据、即第2位置进行存储的区域。第2位置74a是被从第1编码器64输入的位置数据。第2位置74b是被从第2编码器65输入的位置数据。第2位置74c是被从第3编码器66输入的位置数据。第2位置74d是被从第4编码器输入的位置数据。

另外，在存储部70中按照控制轴C1～C4每一个设置有存储对于表示第1位置的位置数据，表示该位置数据是否正常的第1标志的区域。第1标志75a与被从第1编码器64输入的位置数据对应。第1标志75b与被从第2编码器65输入的位置数据对应。第1标志75c与被从第3编码器66输入的位置数据对应。第1标志75d与被从第4编码器67输入的位置数据对应。另外，在存储部70中按照控制轴C1～C4每一个设置有存储对于表示第2位置的位置数据，表示该位置数据是否正常的第2标志的区域。第2标志76a与被从第1编码器64输入的位置数据对应。第2标志76b与被从第2编码器65输入的位置数据对应。第2标志76c与和第3编码器66相关的位置数据对应。第2标志76d与被从第4编码器67输入的位置数据对应。

而且，数据处理部63每当取得位置数据时，便判断该位置数据是否正确。此时，若在比上述基准定时靠前的定时判断为位置数据正常，则数据处理部63将该位置数据更新为第1位置，且将第1标志设定为“0”。另一方面，若在比上述基准定时靠前的定时判断为位置数据异常，则数据处理部63不更新第1位置，将第1标志设定为“1”。其中，数据处理部63针对控制轴C1～C4依次执行上述的第1位置的更新以及第1标志的设定，直到检测到触发信号为止。然后，若检测到触发信号，则数据处理部63将该检测时的各第1位置以及第1标志作为第1定时数据77保存到存储部70的规定区域。

另外，若在上述基准定时之后的定时判断为位置数据正常，则数据处理部63将该位置数据作为第2位置进行更新，并将第2标志设定为“0”。另一方面，若在上述基准定时之后的定时判断为位置数据异常，则数据处理部63不将该位置数据作为第2位置进行更新，将第2标志设定为“1”。然后，数据处理部63取得上述之后定时的下一定时的位置数据，无论该位置数据是否正常，都将该位置数据作为第2位置进行更新。而且，若所有的第2位置全部被更新，则数据处理部63将各第2位置以及第2标志作为第2定时数据78保存到存储部70的规定区域。

另外，在存储部70中设置有对表示是否检测到触发信号的检测标志79进行存储的区域。若检测到触发信号，则数据处理部63将检测标志79设定为“1”，并且，若将算出的机械手12的位置输出给用户程序执行部52，则将检测标志79设定为“0”。

另外，如图5所示，在存储部70中设置有对表示检测出触发信号时作为控制对象的控制轴的数据、即控制轴数据80进行存储的区域。数据处理部63基于内置的计数器的计数值，将表示检测出触发信号时的控制轴的数据保存为控制轴数据80。然后，每当检测到触发信号时，数据处理部63便基于上述第1定时数据77、第2定时数据78、控制轴数据80，算出检测出触发信号时的各控制轴C1～C4的关节角度，来检测机械手12的位置。

下面，参照图6对位置算出部60计算机械手12的位置的算出方法进行说明。其中，在图6中将第1定时的关节角度、即第1位置表示为Pa，将第2定时的关节角度、即第2位置表示为Pb，将检测出触发信号时的关节角度表示为P(k)。而且，本实施方式中的通信时间Td为马达的控制周期T的一半的时间(T/2)。

若第2定时数据78被保存，则数据处理部63按照控制轴C1～C4每一个基于第1标志与第2标志的组合，从根据该组合而预先设定的4个算式中选择1个算式。然后，通过将第1位置、第2位置和基于控制轴数据80的常数k代入到该选择的算式，求出检测出触发信号时的各控制轴C1～C4的关节角度。这里所说的常数k是基于成为算出对象的控制轴与检测出触发信号时成为控制对象的控制轴之间的关系，按每个控制轴决定的常数，以周期T/n为基准单位，是以“T/n×k”表示检测到触发信号的定时至基准定时为止的期间的常数。

例如，在检测出触发信号时的控制对象为控制轴C1(第1马达23)的情况下，如果算出对象是控制轴C2(第2马达27)，则由于比与第2编码器65相关的基准定时提前“周期T/n”检测到触发信号，所以常数k为“1”。另外，如果算出对象是控制轴C3(升降马达30)，则由于比第3编码器66的基准定时提前“2×周期T/n”检测到触发信号，所以常数k为“2”。另外，如果算出对象是控制轴C4(旋转马达31)，则由于比基准定时提前“3×周期T/n”检测到触发信号，所以常数k为“3”。另外，如果算出对象是控制轴C1(第1马达23)，则由于比基准定时提前控制周期T检测到触发信号，所以常数k为“4”。

在成为算出对象的控制轴的第1标志以及第2标志均为“0”的情况下，即在第1定时与基准定时间隔了“控制周期T”、基准定时与第2定时间隔了“控制周期T”的情况下，数据处理部63利用以下所示的式(1)，算出检测出触发信号时的该控制轴的关节角度。

【式1】

$P\left(k\right)=\mathrm{Pa}+\frac{\mathrm{Pb}-\mathrm{Pa}}{T+T}\×(\mathrm{Td}+T-\frac{T}{n}k)$

$=\mathrm{Pa}+\frac{\mathrm{Pb}-\mathrm{Pa}}{4}\×(3-\frac{2k}{n})...\left(1\right)$

另外，在成为算出对象的控制轴的第1标志为“0”、第2标志为“1”的情况下，即在第1定时与基准定时间隔了“控制周期T”、基准定时与第2定时间隔了“2×控制周期T”的情况下，数据处理部63利用以下所示的式(2)，算出检测出触发信号时的该控制轴的关节角度。

【式2】

$P\left(k\right)=\mathrm{Pa}+\frac{\mathrm{Pb}-\mathrm{Pa}}{T+2T}\×(\mathrm{Td}+T-\frac{T}{n}k)$

$=\mathrm{Pa}+\frac{\mathrm{Pb}-\mathrm{Pa}}{6}\×(3-\frac{2k}{n})...\left(2\right)$

另外，在成为算出对象的控制轴的第1标志为“1”、第2标志为“0”的情况下，即在第1定时与基准定时间隔了“2×控制周期T”、基准定时与第2定时间隔了“控制周期T”的情况下，数据处理部63利用以下所示的式(3)，算出检测出触发信号时的该控制轴的关节角度。

【式3】

$P\left(k\right)=\mathrm{Pa}+\frac{\mathrm{Pb}-\mathrm{Pa}}{2T+T}\×(\mathrm{Td}+2T-\frac{T}{n}k)$

$=\mathrm{Pa}+\frac{\mathrm{Pb}-\mathrm{Pa}}{6}\×(5-\frac{2k}{n})...\left(3\right)$

另外，在成为算出对象的控制轴的第1标志和第2标志均为“1”的情况下，即在第1定时与基准定时间隔了“2×控制周期T”、基准定时与第2定时间隔了“2×控制周期T”的情况下，数据处理部63利用以下所示的式(4)，算出检测出触发信号时的该控制轴的关节角度。

【式4】

$P\left(k\right)=\mathrm{Pa}+\frac{\mathrm{Pb}-\mathrm{Pa}}{2T+2T}\×(\mathrm{Td}+2T-\frac{T}{n}k)$

$=\mathrm{Pa}+\frac{\mathrm{Pb}-\mathrm{Pa}}{8}\×(5-\frac{2k}{n})...\left(4\right)$

即，在上述式(1)～(4)中，通过使用了第1定时的位置数据和第2定时的位置数据的线性插值，算出了检测出触发信号时的关节角度。而且，利用基于该线性插值的关节角度的变化率，算出了上述通信时间Td的期间内的关节角度的变化量。然后，通过对根据上述线性插值而算出的检测出触发信号时的关节角度、和上述通信时间Td的期间内的关节角度的变化量相加，求出检测出触发信号时的实际的关节角度。

若详细叙述，则如图6所示，关节角度Pa实际是比第1定时提前通信时间Td的时刻的关节角度。而关节角度Pb也是实际比第2定时提前通信时间Td的时刻的关节角度。因此，通过上述线性插值所示的直线81表示了比第1定时至第2定时的期间提前通信时间Td的期间的关节角度。即，使直线81平行移动通信时间Td的量的直线82表示了通过第1定时及第2定时取得的关节角度、与该关节角度被具体化的实际的定时的关系。因此，由于该直线82和直线81的差是关节角度以上述关节角度的变化率变化了通信时间Td的量的量，所以，通过对以直线81表示的关节角度加上上述关节角度的变化量，可算出检测出触发信号时的实际的关节角度。换言之，在通过使用了第1定时的位置数据和第2定时的位置数据的线性插值而表示的直线81中，将与检测出触发信号时相比滞后通信时间Td的定时的关节角度作为检测出触发信号时的实际的关节角度算出。

下面，参照图7及图8对在上述结构的数据处理部63中算出机械手位置时的处理步骤进行说明。其中，该处理被反复执行。

如图7所示，首先对于被内置的计数器的计数值所表示的控制轴，取得与判定部68的判定结果对应的位置数据，作为第1定时的位置数据(步骤S10)。接下来，基于在上述步骤S10中取得的位置数据，判断该位置数据是否正常(步骤S11)。

当在上述步骤S11中判断为位置数据正常时(步骤S11：是)，在第1标志被设定为“0”的同时，第1位置被更新成该位置数据(步骤S12)，然后进入到下一步骤S14。另一方面，当在步骤S11中判断为位置数据异常时(步骤S11：否)，第1标志被设定成“1”，同时第1位置不被更新(步骤S13)，进入到下一步骤S14。

在下一步骤S14中，借助触发信号输入部61，判断在此次的检测定时是否检测到触发信号。

当在上述步骤S14中未检测到触发信号时(步骤S14：否)，再次转移到步骤S10，经由位置数据输入部62取得与由计数器的计数值接下来表示的控制轴相关的第1定时的位置数据。即，在步骤S14中，针对控制轴C1～C4依次执行第1位置的更新以及第1标志的设定，直至检测到触发信号。

当在上述步骤S14中检测到触发信号时(步骤S14：是)，检测标志79被设定成“1”(步骤S15)，且基于计数器的计数值，将表示检测出触发信号时的控制轴的数据保存到控制轴数据80(步骤S16)。而且，表示检测出触发信号时所存储的各第1位置以及第1标志的状态的数据被作为第1定时数据77保存在存储部70的规定区域(步骤S17)。

接下来，依次取得基准定时的各控制轴C1～C4的位置数据(步骤S18)。然后，依次取得紧接着基准定时之后的定时的各控制轴的位置数据作为第2定时的位置数据(步骤S19)。

在下一步骤S20中，基于紧接着基准定时之后的定时取得的位置数据，判断该位置数据是否正常。在上述步骤S20中，对于判断为所取得的位置数据为正常的控制轴而言，第2标志被设定为“0”(步骤S21)，并且第2位置被更新成所取得的位置数据(步骤S24)。

另一方面，在上述步骤S20中，对于判断为所取得的位置数据为异常的控制轴而言，第2标志被设定成“1”(步骤S22)。然后，取得下一定时的位置数据作为第2定时的位置数据(步骤S23)，在第2标志的状态被维持为“1”的状态下，第2位置被更新成该取得的位置数据(步骤S24)。

接着，如图8所示，对于控制轴C1～C4言，若第2位置的更新全部结束，则将表示各第2位置以及第2标志的状态的数据作为第2定时数据78保存在存储部70的规定区域(步骤S25)。

若第2定时数据78被保存，则可确认与各控制轴C1～C4相关的第1及第2标志的状态(步骤S26)。而且，可根据第1以及第2标志选择算式。即，对于第1标志和第2标志都为“0”的控制轴而言(步骤S27：是，步骤S28：是)，选择上述数式(1)(步骤S30)。另外，对于第1标志为“0”(步骤S27：是)，且第2标志为“1”(步骤S28：否)的控制轴而言，选择数式(2)(步骤S31)。另外，对于第1标志为“1”(步骤S27：否)，且第2标志为“0”(步骤S28：是)的控制轴而言，选择数式(3)(步骤S32)。另外，对于第1标志为“1”(步骤S27：否)，且第2标志为“1”(步骤S28：否)的控制轴而言，选择数式(4)(步骤S33)。

若针对各控制轴C1～C4选择出算式，则第1位置、第2位置、基于控制轴数据80的常数k被代入到所选择的各式(步骤S34)。由此，可算出由各控制轴C1～C4的关节角度表示的机械手12的位置(步骤S35)。然后，通过将算出的机械手12的位置输出给用户程序执行部52(步骤S36)，在检测标志79被设定为“0”的基础上，结束一系列的处理。

下面，对上述结构的机械手***10的作用进行说明。

在上述结构的机械手***10中，通过传送带11搬运的工件W被机械手12选择性把持。而且，在将该工件W搬运至作业台15上的规定位置时，由检测传感器35检测工件W。即，在使机械手12移动至该机械手12的目标位置时，工件W被检测传感器35检测。若检测到工件W，则基于利用照相机39拍摄到的图像数据，取得检测出工件时的工件W的位置。另外，由位置算出部60基于检测出工件时的前后的机械手12的位置，算出该检测时的机械手12的实际位置。然后，基于该工件W的位置和机械手12的实际的位置，修正机械手12的目标位置。

如上述说明那样，根据本实施方式涉及的机械手***10，能够得到以下列举的效果。

(1)在上述实施方式的位置算出部60中，对于各控制轴而言，针对通过使用了第1定时的位置数据、第2定时的位置数据的线性插值而取得的检测出触发信号时的关节角度，加上使用基于该线性插值的关节角度的变化率而算出的、通信时间Td中的关节角度的变化量，从而求出检测出触发信号时的实际的关节角度。换言之，对于通过使用了第1定时的位置数据和第2定时的位置数据的线性插值而取得的关节角度而言，将比检测出触发信号时滞后通信时间Td的定时的关节角度，作为检测出触发信号时的实际的关节角度算出。根据这种结构，即便机械手12在移动中，也能够高精度地对检测出触发信号时的各控制轴C1～C4的关节角度进行检测。即，能够高精度检测移动中的机械手12的位置。

(2)位置算出部60的数据处理部63分别在不同的定时取得与控制轴C1的关节角度相关的位置数据、与控制轴C2的关节角度相关的位置数据、与控制轴C3的关节角度相关的位置数据以及与控制轴C4的关节角度相关的位置数据。然后，位置算出部60针对各控制轴C1～C4，算出检测出触发信号时的实际的关节角度。结果，由于分别算出控制轴C1的关节角度、控制轴C2的关节角度、控制轴C3的关节角度以及控制轴C4的关节角度，所以能够以更高的精度对检测出触发信号时的机械手的实际的位置进行检测。

(3)位置算出部60判断被输入到位置数据输入部62的位置数据是否正常，并使用根据该判断而选择的算式算出各控制轴C1～C4的关节角度。由此，能够抑制利用例如受到噪声等与位置数据的传送相关的不良影响的位置数据，检测机械手的位置。结果，对于由位置算出部60检测的机械手12的位置而言，能够提高其精度。

(4)这里，在机械手***中，通常判断为位置数据异常的机会连续的情况非常少，例如，有时将位置数据输入部62和编码器连接的电缆和编码器本身产生不良的状况。因此，若对从同一编码器向位置数据输入部62连续输入异常的位置数据的情况等执行特殊处理，则另外需要用于执行该处理的新的结构，存在位置算出部60的结构变复杂的问题。针对这一点，在上述实施方式中以从同一编码器输出的位置数据且连续的2个位置数据中至少一个为正常的数据为前提构成。结果，能够使位置算出部60为更加简易的结构。

(5)另外，由于使用线性插值算出机械手12的位置，所以第1定时和第2定时在时间上间隔越长，算出的机械手12的位置和实际的机械手12的位置越容易产生大的误差。这一点对于上述位置算出部60而言，即便在第1定时和第2定时在时间上间隔最长的情况下，第1位置数据也是在比基准定时提前2个定时输入的位置数据，第2位置数据是比基准定时滞后2个定时输入的位置数据。结果，与基于在该第1定时前取得的位置数据、和在该第2定时后取得的位置数据来算出机械手12的位置的情况相比，能够提高算出的机械手12的位置的精度。

(6)在上述实施方式中，将通信时间Td设为马达的控制周期T的一半的时间，将数据处理部63取得来自各编码器的位置数据的周期设为马达的控制周期T。这里，在各编码器的输出周期比通信期间Td小的情况下，有时导致第2定时成为比从基准定时经过通信时间Td的定时靠前的定时。该情况下，不能使将图6所示的直线81平行移动通信时间Td的直线82与从第1定时至基准定时的整个期间对应，存在无法算出基准时间之前的机械手12的位置的问题。对于这点，在上述实施方式中，由于数据处理部63取得来自各编码器的位置数据的周期是比通信周期Td大的控制周期T，所以能够使在图6中将直线81平行移动通信时间Td的直线82与从第1定时至基准定时的整个期间对应。结果，能够可靠地算出检测出触发信号时的机械手12的位置。

(7)在机械手***10中，能够一边使工件W移动一边对机械手12的目标位置进行修正。即，无需为了取得目标位置的修正量而停止机械手12。结果，能够抑制机械手***10的生产效率降低。

(8)并且，基于检测出触发信号时的机械手12的实际的位置，对机械手12的目标位置进行修正。由此，能够以高精度将工件W配置在作业台15的规定位置。

另外，上述实施方式也可以适当进行如下变更来实施。

如上述实施方式所示，位置算出部60可适用于一边使机械手移动一边检测该机械手的位置的机械手***。

如上所述，在机械手***中，由于判断为位置数据异常的机会连续的情况非常少，所以上述实施方式的位置算出部60以从同一编码器连续取得的2个位置数据中至少一个为正常的数据为前提构成。但是，也可以对其进行变更，考虑到连续输入异常位置数据的情况来构成位置算出部60。

例如，在数据处理部63中按照控制轴C1～C4每一个设置关于第1定时的位置数据和第2定时的位置数据，对位置数据异常连续的次数进行计数的计数器。而且，数据处理部63在取得正常的位置数据之前对上次取得的正常位置数据进行存储，作为第1定时的位置数据。另外，数据处理部63在基准定时之后的定时第一次取得正常的数据时将第2位置作为第2定时的位置数据进行更新。在这种结构下，基于上述各计数器的计数值求出从第1定时至基准定时的期间以及从基准定时至第2定时的期间。然后，可基于该求出的期间、第1定时的正常的位置数据以及第2定时的正常的位置数据进行线性插值，算出检测出触发信号时的机械手12的位置。

另外，根据上述结构，在基于计数器的计数值，连续取得了异常的位置数据的情况下，算出的机械手12的位置精度低，也能够结束一系列的处理。另外，通过将连续取得异常的位置数据的情况通知给用户，能够促使机械手12的维护。

在上述实施方式的位置算出部60中，也可以是在判断为紧接着基准定时之前或之后的定时取得的位置数据为异常的情况下，不算出检测出触发信号时的机械手12的位置的结构。

上述实施方式的位置算出部60基于从检测对象相互不同的编码器输入的位置数据，算出控制轴C1～C4的各个关节角度。但不限于此，根据上述实施方式，例如也可以只算出控制轴C1的关节角度以及控制轴C2的关节角度。根据这种结构，由于对机械手12的目标位置中、与X轴方向以及Y轴方向相关的关节角度进行修正，所以，例如当配置圆盘状的工件等工件时，在不重视其朝向的情况下，能够降低对位置算出部60的负荷。

上述实施方式的触发信号输入部61构成为能够以马达的控制周期T除以控制轴数n(n为1以上的整数，且在本实施方式中n＝4)而得到的检测周期Ts(＝T/n)检测上述触发信号。但不限于此，触发信号输入部61也可以构成为能够以比上述周期T/n短的周期检测触发信号。根据这种结构，由于可使由检测传感器35检测到工件W的时刻、与由数据处理部63检测到触发信号的时刻之差变小，所以能够以更高的精度算出检测出触发信号时的机械手12的位置。另外，也可以构成为能够以比上述周期T/s长的周期检测触发信号。对这种结构而言，与上述的结构相比，由检测传感器35检测到工件W的时刻和由数据处理部63检测到触发信号的时刻之差变大，导致算出的机械手12的位置的精度降低。虽然如此，由于通过考虑了通信时间Td的线性插值算出机械手12的位置，所以与只以来自编码器的位置数据检测机械手12的位置的结构相比，能够得到与实际的位置接近的位置。

上述实施方式的位置算出部60对从编码器输入的位置数据是否正常进行判断，在此基础上算出了检测出触发信号时的各关节角度。但不限于此，例如若能充分保证位置数据的可靠性，则位置算出部60可以不判断位置数据是否正常地算出各关节角度。根据这种结构，由于可省略位置数据输入部62的判定部68，所以可使位置算出部60的结构更加简易。

上述实施方式的位置算出部60以检测出触发信号为契机，算出了检测出触发信号时的机械手12的位置。但不限于此，位置算出部60也可以通过设置例如以把持到所搬运的工件W为契机而开始计时的计时器，检测从把持该工件W经过规定期间后的机械手12的位置。即，用于检测机械手的位置的指令不限于从位置算出部60的外部输入，也可以构成为基于预定的控制程序在位置算出部60的内部生成。

在上述实施方式中，作为基于触发信号的输入的基准定时，将紧接着检测到该触发信号之后的定时作为基准定时。但不限于此，基准定时只要是通过触发信号的检测而规定的定时即可，例如也可以将检测出触发信号时作为基准定时。

在上述实施方式中，将作为机械手位置检测装置的位置算出部60具体化为具备具有4个控制轴的水平多关节型机械手12的机械手***10。但不限于此，机械手位置检测装置也可具体化为具备搬运工件W的机械手的机械手***，例如可以具体化为具有6个控制轴的多关节机械手***。

符号说明：

T…控制周期；W…工件；C1、C2、C3、C4…控制轴；FL…检测线；LB…检测光；Pa、Pb…关节角度；Td…通信时间；Ts…检测周期；10…机械手***；11…传送带；12…机械手；13…照相机；14…编码器；15…作业台；21…基台；22…基台轴；23…第1马达；24…第1水平臂；25…支承轴；26…第2水平臂；27…第2马达；29…上下旋转轴；30…升降马达；31…旋转马达；32…作业部；35…检测传感器；36…射出部；37…受光部；39…照相机；50…控制器；51…用户程序存储部；52…用户程序执行部；53…工件位置存储部；54…工件位置更新部；55…指令值生成部；56…作业对象检测部；57…马达控制部；58…工件位置取得部；59…触发信号生成部；60…位置算出部；61…触发信号输入部；62…位置数据输入部；63…数据处理部；64…第1编码器；65…第2编码器；66…第3编码器；67…第4编码器；68…判定部；70…存储部；73a、73b、73c、73d…第1位置；74a、74b、74c、74d…第2位置；75a、75b、75c、75d…第1标志；76a、76b、76c、76d…第2标志；77…第1定时数据；78…第2定时数据；79…检测标志；80…控制轴数据；81、82…直线。

Claims (8)

1.一种机械手位置检测装置，其特征在于，具备：

位置数据取得部，其取得表示实际的机械手的位置的位置数据；

位置数据输入部，其被输入从上述位置数据取得部输出的上述位置数据；和

位置算出部，其通过线性插值来算出计算上的机械手的位置，该线性插值使用了在不同的定时被输入给上述位置数据输入部的第1以及第2位置数据。

2.根据权利要求1所述的机械手位置检测装置，其特征在于，

上述位置数据取得部是设置于上述机械手的编码器，

上述位置算出部接受用于检测上述机械手的位置的指令，算出上述计算上的机械手的位置，

上述第1位置数据是在上述指令之前的定时取得的数据，

上述第2位置数据是在上述指令之后的定时取得的数据。

3.根据权利要求2所述的机械手位置检测装置，其特征在于，

上述位置算出部具备指令检测部，该指令检测部能够以比取得从1个编码器输入给上述位置数据输入部的上述位置数据的规定周期T短的周期Ts，检测上述指令。

4.根据权利要求3所述的机械手位置检测装置，其特征在于，

上述位置算出部按照每个周期T/n，取得来自相互不同的编码器的位置数据，并按照每一个上述编码器输出被输入上述指令时的上述机械手的位置，所述周期T/n是将上述规定周期T以1以上的整数n进行n等分而得到的周期。

5.根据权利要求1～4中任意一项所述的机械手位置检测装置，其特征在于，

上述位置算出部对所取得的上述位置数据是否正常进行判断，

在判断为检测到上述指令时的前一上述位置数据不正常的情况下，将在该位置数据之前取得的前一位置数据作为上述第1位置数据使用。

6.根据权利要求5所述的机械手位置检测装置，其特征在于，

上述位置算出部在判断为紧接着检测到上述指令后取得的上述位置数据不正常的情况下，将紧接着该位置数据后取得的位置数据作为上述第2位置数据使用。

7.一种机械手***，其特征在于，具备：

搬运工件的机械手；

机械手控制器，其具备输出用于检测上述机械手的位置的指令的指令输出部，使上述机械手动作；

还具备权利要求1～6中任意一项所述的机械手位置检测装置。

8.根据权利要求7所述的机械手***，其特征在于，还具备：

照相机，其通过在被输入来自上述指令输出部的上述指令的定时，对预定的拍摄范围进行拍摄，来取得包含向目标位置移动过程中的上述机械手所搬运的工件的图像；

工件位置取得部，其基于上述照相机所取得的图像，取得上述工件的位置；和

目标位置修正部，其基于上述工件位置取得部取得的上述工件的位置、和由上述机械手位置检测装置算出的上述计算上的机械手的位置，对上述目标位置进行修正。

Images