CN104999453B - 机械手装置、机械手装置的控制方法以及程序 - Google Patents

Abstract

本发明涉及机械手装置、机械手装置的控制方法以及程序，该机械手装置具备：第一运算部，其根据致动器的角度传感器的旋转角度来检测数据，算出由具备所述角度传感器的所述致动器驱动的所述手臂的角速度；第二运算部，其根据借助所述连结装置连结的所述手臂中所具备的惯性传感器所检测的角速度检测数据，算出所述手臂以所述连结装置为轴的角速度，该连结装置包括所述第一运算部的运算对象的所述致动器；第三运算部，其算出除去了低频成分后的所述致动器与所述手臂之间的扭转角速度。

Description

机械手装置、机械手装置的控制方法以及程序

技术领域

本发明涉及机械手装置、机械手装置的控制方法以及程序。

背景技术

机械手装置在各种工业现场中被大量使用，其具有多关节结构，多数情况下被作为IC分选机或组装装置的一部分使用。因此，对于机械手装置而言，迄今为止如何能够快速并且准确地将手臂移动到所要求的位置为重要的性能指标、品质。

一般来说，为使手臂高速并且准确地移动关键在于：减小施加在手臂上的惯性力，不使驱动用的致动器的负荷增大。为减小施加在手臂上的惯性力，作为最有效的方法采用手臂的轻型化的。但是，由于使手臂轻型化会导致手臂的刚性降低，难以抑制手臂停止时产生的手臂的振动，即使使手臂前端部停止在目标的位置，也会产生手臂自身的振动的振幅大小的位置错移，因而存在着直到振动衰减的时刻之前无法开始下次的动作的问题。

针对该问题，提出过如下的方法等，即，在手臂前端设置加速度传感器，根据加速度信号使手臂作动，从而抑制振动(例如专利文献1)；在手臂前端及手臂上设置角速度传感器，根据角速度信号，控制手臂动作(例如专利文献2)。

专利文献1：日本特开平1-173116号公报

专利文献2：日本特开2005-242794号公报

但是，在以往的机械手装置的控制方法中，由于使用角速度传感器或者加速度传感器的某一种，生成抑制振动的控制信号，因此在传感器信号中包含零偏漂移等误差的情况下就会在控制信号中产生误差，有时无法进行准确的控制。

例如，在专利文献2中，为了降低传感器的误差的影响而使用了除去角度传感器的高频成分的低通滤波器和除去角速度传感器的低频成分的高通滤波器这2种滤波器，因此存在增加了控制装置的运算量，在处理中花费时间、或为了提高处理速度而增加运算机的成本的问题。

发明内容

本发明可以以下的方式或应用例来实现，以解决上述问题的至少一部分。

[应用例1]本应用例的机械手装置的特征在于，具备：手臂连结装置，其包括致动器、以预定的减速比传递所述致动器的转矩的转矩传递机构以及检测所述致动器的旋转角度的角度传感器；手臂体，其通过所述手臂连结装置串联地并且可以旋转地连结多个手臂而构成；基体，其利用基体连结装置可以旋转地与所述手臂体连结，该基体连结装置包括被设于所述手臂体的一端部的所述致动器、以预定的减速比传递所述致动器的转矩的转矩传递机构以及检测所述致动器的旋转角度的角度传感器；惯性传感器，其被安装于所述手臂中，且至少包括角速度传感器；第一运算部，其根据所述角度传感器的旋转角度检测数据，算出由具备所述角度传感器的所述致动器驱动的所述手臂的角速度；第二运算部，其根据借助所述连结装置连结的所述手臂中所具备的所述惯性传感器的角速度检测数据，算出以所述连结装置为轴的由所述致动器驱动的所述手臂的角速度，该连结装置包括所述第一运算部的运算对象的所述致动器；第三运算部，其算出所述致动器与所述手臂之间的扭转角速度，该所述致动器与所述手臂之间的扭转角速度被除去了由所述致动器驱动的所述手臂的所述角速度与根据所述惯性传感器的所述角速度检测数据所算出的所述手臂的所述角速度之差中所含的低频成分。

根据上述的应用例，通过将扭转角速度作为机械手装置的振动抑制控制的基础的数据使用，可以进行准确的控制。

[应用例2]在上述的应用例中还具有如下的特征，上述低频成分是相对于上述机械手装置的机械系统固有频率当中的反共振频率及共振频率中的最低的频率更低的频率。

根据上述的应用例，不去除反共振频率及共振频率而去除低频成分，由此即使在使用了包含误差的惯性传感器的情况下，也可以在振幅大的区域中获得接近利用不包含误差的惯性传感器所得到的真值的扭转角速度，从而可以获得利用扭转角速度准确地控制振动的机械手装置。

[应用例3]在上述的应用例中还具有如下的特征，还具备第四运算部，其将利用上述第一运算部算出的上述手臂的上述角速度与利用上述第三运算部算出的上述扭转角速度相加。

根据上述的应用例，在扭转角速度的低频成分中不含有实际动作。所以，扭转角速度的低频成分的除去即为惯性传感器的误差(噪声)的除去，通过将除去了低频成分的扭转角速度、或者对除去了低频成分的扭转角速度加上致动器的角速度而得的手臂角速度作为控制数据来使用，就可以进行准确的机械手装置的振动抑制控制。

[应用例4]本应用例的机械手装置的控制方法其特征在于，该机械手装置具备：手臂连结装置，其包括致动器、以预定的减速比传递所述致动器的转矩的转矩传递机构以及检测所述致动器的旋转角度的角度传感器；手臂体，其通过所述手臂连结装置串联地并且可以旋转地连结多个手臂而构成；基体，其利用基体连结装置可以旋转地与所述手臂体连结，该基体连结装置包括被设于所述手臂体的一端部的所述致动器、以预定的减速比传递所述致动器的转矩的转矩传递机构以及检测所述致动器的旋转角度的角度传感器；惯性传感器，其被安装于所述手臂中，且至少包括角速度传感器，该机械手装置的控制方法包括：第一运算步骤，其根据所述角度传感器的旋转角度检测数据，算出由具备所述角度传感器的所述致动器驱动的所述手臂的角速度；第二运算步骤，其根据借助所述连结装置连结的所述手臂中所具备的所述惯性传感器的角速度检测数据，算出以所述连结装置为轴的由所述致动器驱动的所述手臂的角速度，该连结装置包括所述第一运算步骤的运算对象的所述致动器；第三运算步骤，其算出所述致动器与所述手臂之间的扭转角速度，该所述致动器与所述手臂之间的扭转角速度被除去了由所述致动器驱动的所述手臂的所述角速度与根据所述惯性传感器的所述角速度检测数据所算出的所述手臂的所述角速度之差中所含的低频成分。

根据上述的应用例，通过将扭转角速度作为机械手装置的振动抑制控制的基础的数据使用，可以进行准确的控制。

[应用例5]在上述的应用例中还具有如下的特征，上述低频成分是相对于上述机械手装置的机械系统固有频率当中的反共振频率及共振频率的最低的频率更低的频率。

根据上述的应用例，不去除反共振频率及共振频率而去除低频成分，由此即使在使用了包含误差的惯性传感器的情况下，也可以在振幅大的区域中获得接近利用不包含误差的惯性传感器得到的真值的扭转角速度，从而可以获得利用扭转角速度准确地控制振动的机械手装置。

[应用例6]在上述的应用例中还具有如下的特征，还具备第四运算步骤，其将利用上述第一运算步骤算出的上述手臂的上述角速度与利用上述第三运算步骤算出的上述扭转角速度相加，算出上述手臂的角速度。

根据上述的应用例，在扭转角速度的低频成分中不含有实际动作。所以，扭转角速度的低频成分的除去即为惯性传感器的误差(噪声)的除去，通过将除去了低频成分的扭转角速度、或者对除去了低频成分的扭转角速度加上致动器的角速度而得的手臂角速度作为控制数据来使用，就可以进行准确的机械手装置的振动抑制控制。

[应用例7]本应用例的程序是对机械手装置进行控制的程序，该机械手装置具备：手臂连结装置，其包括致动器、以预定的减速比传递所述致动器的转矩的转矩传递机构以及检测所述致动器的旋转角度的角度传感器；手臂体，其通过所述手臂连结装置串联地并且可以旋转地连结多个手臂而构成；基体，其利用基体连结装置可以旋转地与所述手臂体连结，该基体连结装置包括被设于所述手臂体的一端部的所述致动器、以预定的减速比传递所述致动器的转矩的转矩传递机构以及检测所述致动器的旋转角度的角度传感器；惯性传感器，其被安装于所述手臂中，且至少包括角速度传感器，该程序的特征在于使计算机执行以下步序：第一运算步序，其根据所述角度传感器的旋转角度检测数据，算出由具备所述角度传感器的所述致动器驱动的所述手臂的角速度；第二运算步序，其根据借助所述连结装置连结的所述手臂中所具备的所述惯性传感器的角速度检测数据，算出以所述连结装置为轴的由所述致动器驱动的所述手臂的角速度，该连结装置包括所述第一运算步序的运算对象的所述致动器；第三运算步序，其算出所述致动器与所述手臂之间的扭转角速度，该所述致动器与所述手臂之间的扭转角速度被除去了由所述致动器驱动的所述手臂的所述角速度与根据所述惯性传感器的所述角速度检测数据所算出的所述手臂的所述角速度之差中所含的低频成分。

只要在机械手装置所具备的计算机中搭载本应用例的程序，就可以很容易地进行上述的机械手装置的振动抑制控制。

附图说明

图1表示实施方式的机械手装置的概况，(a)是示意性的俯视图，(b)是示意性的剖面图。

图2是实施方式的机械手装置的控制框图。

图3是致动器、手臂和扭转针对实施方式的机械手装置的致动器的转矩的扭转的角速度响应的波特图。

图4表示实施方式的动作时的扭转角速度频率成分的低频成分的除去前后，(a)是不带有误差的惯性传感器的情况的曲线图，(b)是带有低频误差的惯性传感器的情况的曲线图。

图5是实施方式的控制方法的流程图。

图6是表示实施例2的框图。

符号说明：10…手臂体，11、12、13…手臂，21、22…手臂连结装置，30…基体连结装置，40…基体，51、52、53…致动器，61、62、63…转矩传递装置，70…工件保持装置，81、82、83…角度传感器，91、92、93…惯性传感器，100…机械手装置。

具体实施方式

下面，参照附图，对本发明的实施方式进行说明。

(实施方式)

对本发明的实施方式进行说明。图1表示实施方式的机械手装置，(a)是概略俯视图，(b)是概略剖面图。本实施方式的机械手装置是连结有可以在水平方向旋转的3根手臂的所谓3轴水平多关节机械手100(以下称作机械手装置100)。

机械手装置100具备将第一手臂11与第二手臂12利用第一手臂连结装置21可以旋转地串联连结、将第二手臂12与第三手臂13利用第二手臂连结装置22可以旋转地串联连结而构成的手臂体10。手臂体10还利用基体连结装置30，可以旋转地与固定于底盘中的基体40连结，构成机械手装置100。

第一手臂连结装置21包括致动器51、以预定的减速比传递致动器51的转矩的转矩传递装置61，第二手臂连结装置22同样也包括致动器52和转矩传递装置62。另外，基体连结装置30包括致动器53和以预定的减速比传递致动器53的转矩的转矩传递装置63。在手臂体10的与基体40相反一侧的端部的第三手臂13的端部装备有保持加工用工具或被加工物的工件保持装置70。

在第一手臂连结装置21所包括的致动器51中装备有检测旋转角度的角度传感器81，同样地，在第二手臂连结装置22的致动器52中装备有角度传感器82。另外，在基体连结装置30的致动器53中也装备有角度传感器83。此外，在第一手臂11中装备有惯性传感器91，在第二手臂12中装备有惯性传感器92，在第三手臂13中装备有惯性传感器93。惯性传感器91、92、93至少包括角速度传感器，惯性传感器91可以检测出第一手臂11在惯性传感器安装位置的角速度，惯性传感器92可以检测出第二手臂12在惯性传感器安装位置的角速度，惯性传感器93可以检测出第三手臂13在惯性传感器安装位置的角速度。

图2是表示本实施方式的机械手装置100的构成的框图。CPU200包括后述的第一运算部510、第二运算部520、第三运算部530、第四运算部540以及控制部600，读出存储在ROM300中的程序而执行。另外，RAM400保存利用CPU200的程序执行而得到的数据，从保存的数据中向CPU200送出必需的数据。

由装备在机械手装置100中的角度传感器81、82、83检测出的致动器51、52、53的旋转角度数据在第一运算部510中被换算为致动器51的旋转角度θ1、致动器52的旋转角度θ2、致动器53的旋转角度θ3，将换算出的各个旋转角度θ1、θ2、θ3用时间进行1次微分，算出致动器的旋转角速度。

根据所得的致动器的旋转角速度，求出致动器所驱动的手臂的旋转角速度。在第一手臂11的情况下，由于是由致动器53利用具有减速比1/N3的转矩传递装置63驱动，因此基体连结装置30的输出部的旋转角速度ω3为：

ω3＝(dθ3/dt)×(1/N3)

同样地，包括对第二手臂12进行驱动的致动器51的第一手臂连结装置21的输出部的旋转角速度ω1为：

ω1＝(dθ1/dt)×(1/N1)

N1：转矩传递装置61的减速比

包括对第三手臂13进行驱动的致动器53的第二手臂连结装置22的输出部的旋转角速度ω2为：

ω2＝(dθ2/dt)×(1/N2)

N2：转矩传递装置62的减速比

第二运算部520中，根据在第一手臂11、第二手臂12、第三手臂13中所装备的惯性传感器91、92、93检测出的数据，算出第一手臂11的角速度、第二手臂的角速度、第三手臂的角速度。根据检测数据算出的角速度是以基体连结装置30作为旋转轴的角速度。所以，在第二手臂12的情况下，根据所得的角速度还算出以第一手臂连结装置21作为旋转轴的手臂的角速度，在第三手臂13的情况下，根据所得的角速度还算出以第二手臂连结装置22作为旋转轴的手臂的角速度。

即，在第二手臂12的情况下，根据第一手臂11中所装备的惯性传感器91所得到的检测数据所换算出的角速度ω_a1和在第二手臂中所装备的惯性传感器92的检测数据所换算出的角速度ω_a2，可以求出第二手臂12以第一手臂连结装置21作为旋转轴的角速度ω_j2为：

ω_j2＝ω_a2-ω_a1

同样地，在第三手臂13的情况下，根据在第二手臂12中所装备的惯性传感器92所得到的检测数据所换算出的角速度ω_a2和在第三手臂中所装备的惯性传感器93的检测数据所换算的角速度ω_a3，可以求出第三手臂13以第二手臂连结装置22作为旋转轴的角速度ω_j3为：

ω_j3＝ω_a3-ω_a2

在第一手臂11的情况下，由于第一手臂11以在基体40中装备的基体连结装置30为中心转动，因此由惯性传感器91得到的角速度ω_a1就为第一手臂11的角速度。

在第三运算部530中，算出作为如上所述算出的由致动器的旋转造成的以包括致动器的连结装置作为旋转轴的第二手臂12的角速度ω1、第三手臂的角速度ω2、第一手臂11的角速度ω3与从安装于各手臂中的惯性传感器得到的以连结装置作为旋转轴的第二手臂的角速度ω_j2、第三手臂的角速度ω_j3、第一手臂的角速度ω_a1的差的扭转角速度。

即，求出基体连结装置30与第一手臂11的扭转角速度ω_t1为：

ω_t1＝ω_a1-ω1

同样地，求出第一手臂连结装置21与第二手臂12的扭转角速度ω_t2为：

ω_t2＝ω_j2-ω2

求出第二手臂连结装置22与第三手臂13的扭转角速度ω_t3为：

ω_t3＝ω_j3-ω3

图3是说明对于机械手装置100的致动器的转矩，从致动器经由转矩传递装置驱动手臂的角速度Ω_act、利用惯性传感器得到的手臂的角速度Ω_arm、扭转角速度Ω_tor的响应的波特图。如图3所示，在Ω_act、Ω_arm中，在低频成分中也包含本来的动作成分，然而在Ω_tor中实际的动作成分基本上不包含于低频成分中。着眼于这一点，本发明仅将扭转角速度Ω_tor的低频成分看作由传感器的零偏漂移等误差造成的影响，通过将其除去而除去机械手装置100的低频噪声，从而可以准确地控制机械手装置100的振动。

图4是概念性地表示动作时的扭转角速度的频率成分的图，表示将低频成分利用高通滤波器等机构除去的情况。图4(a)表示没有误差的惯性传感器的情况，(b)表示具有零偏漂移等低频的误差的惯性传感器的情况，(a)与(b)的差表示由惯性传感器的误差造成的成分。

通过将图4(a)所示的低频成分除去前的扭转角速度Ω_tor利用高通滤波器除去低频成分，就会变为Ω_tor′。图4(b)中，通过将低频成分除去前的扭转角速度Ω′_tor同样地除去低频成分，就会变为Ω′_tor′。如图4(a)所示，除去了低频成分的扭转角速度Ω_tor′相对于除去前的扭转角速度Ω_tor来说，在所除去的频率以下振幅变小。但是，该区域S如图所示，是很微小的值，例如是10^-5以下的区域，除去量是可以忽略的水平。

另一方面，在图4(b)所示的带有误差的惯性传感器的情况下，借助高通滤波器的低频成分除去前后产生的差异区域T处于振幅大的区域。所以，通过除去低频成分，就可以接近图4(b)的低频成分除去前的不包含误差的值U。由此，通过除去扭转角速度的低频成分，即使使用带有零偏漂移等低频成分的误差的惯性传感器，也可以获得没有误差的，或者说是接近真值的扭转角速度，从而可以准确地控制由扭转角速度造成的机械手装置100的振动。

即，针对如上所述求出的扭转角速度ω_t1、ω_t2、ω_t3，算出将其频率成分中的低频成分利用未图示的高通滤波器(以下称作HPF)除去而得的扭转角速度Ω_t1、Ω_t2、Ω_t3。这里，优选使用HPF的滤波频率相对于机械手装置100的机械系统固有频率中的反共振频率及共振频率的最小的频率来说是更低的频率的HPF。

使用除去了低频成分的扭转角速度Ω_t1、Ω_t2、Ω_t3，在控制部600中生成控制信号，就可以控制各致动器51、52、53。该情况下，也可以具备第四运算部540，对除去了低频成分的扭转角速度Ω_t1、Ω_t2、Ω_t3加上经由各连结装置21、22、30的转矩传递装置61、62、63向第二手臂12、第三手臂13、第一手臂11传递的角速度ω2、ω3、ω1，使用该结果在控制部600中生成控制信号。

图5是表示本实施方式的机械手装置100的控制方法的流程图。在机械手装置100的工作状态下，首先在第一运算步骤(S111)中，第一运算部从在致动器中所装备的角度传感器81、82、83获得旋转角度数据。根据所得的角度数据，进行向旋转角度的换算和将旋转角度用时间进行1次微分，算出包括对第二手臂12进行驱动的致动器51的第一手臂连结装置21的输出部的旋转角速度ω1、包括对第三手臂13进行驱动的致动器52的第二手臂连结装置22的输出部的旋转角速度ω2、基体连结装置30的输出部的旋转角速度ω3。

此外，同时在第二运算步骤(S112)中，第二运算部从在各手臂中装备的惯性传感器91、92、93获得角速度数据，算出以连结驱动各手臂的连结装置作为旋转轴的角速度。即，第二手臂12中，根据从在第一手臂11中所装备的惯性传感器91中得到的检测数据所换算出的角速度ω_a1和在第二手臂中所装备的惯性传感器92的检测数据所换算出的角速度ω_a2，利用

ω_j2＝ω_a2-ω_a1

算出第二手臂12以第一手臂连结装置21作为旋转轴的角速度ω_j2。

同样地，在第三手臂13中，根据第二手臂12中所装备的惯性传感器92得到的检测数据所换算出的角速度ω_a2和在第三手臂中所装备的惯性传感器93的检测数据所换算出的角速度ω_a3，利用

ω_j3＝ω_a3-ω_a2

算出第三手臂13以第二手臂连结装置22作为旋转轴的角速度ω_j3。而且，在第一手臂11中，由于第一手臂11以装备在基体40中的基体连结装置30为中心转动，因此由惯性传感器91得到的角速度ω_a1就成为第一手臂11的角速度。

然后，转移到第三运算步骤(S120)。第三运算步骤(S120)中，根据第一运算步骤(S111)中算出的角速度ω1、ω2、ω3与第二运算步骤(S112)中算出的角速度ω_a1、ω_j2、ω_j3的差，算出扭转角速度

ω_t1＝ω_a1-ω1

ω_t2＝ω_j2-ω2

ω_t3＝ω_j3-ω3。

将所算出的扭转角速度ω_t1、ω_t2、ω_t3，利用除去相对于机械手装置100的机械系统固有频率中的反共振频率及共振频率的最低的频率来说更低的频率的高通滤波器进行滤波，算出除去了低频成分的扭转角速度Ω_t1、Ω_t2、Ω_t3。

然后，转移到第四运算步骤(S130)。第四运算步骤(S130)中，对在第三运算步骤(S120)中运算得到的利用高通滤波器除去了低频成分的扭转角速度Ω_t1、Ω_t2、Ω_t3加上经由各连结装置21、22、30的转矩传递装置61、62、63向第二手臂12、第三手臂13、第一手臂11传递的角速度ω2、ω3、ω1，将其结果送向控制信号生成步骤(S140)。

控制信号生成步骤(S140)在控制部600中，以所得的第四运算步骤(S130)的运算结果为基础，生成各致动器的控制信号。即，以对利用高通滤波器除去了低频成分的扭转角速度Ω_t1、Ω_t2、Ω_t3加上经由各连结装置21、22、30的转矩传递装置61、62、63向第二手臂12、第三手臂13、第一手臂11传递的角速度ω2、ω3、ω1而得的角速度为基础，生成用于控制各致动器的信号。

然后，转移到机械手装置动作停止确认步骤(S150)。在机械手装置动作停止确认步骤(S150)中，确认机械手装置100是否是动作状态，在是动作状态为“否”的情况下，回到第一运算步骤(S111)、第二运算步骤(S112)，反复进行控制。在是动作停止状态为“是”的情况下，结束控制。

如上所述，通过将扭转角速度作为控制数据，并将除去了低频成分的扭转角速度作为控制数据，就可以进行除去了惯性传感器的误差的准确的机械手装置的振动抑制控制。

(实施例1)

对实施方式的机械手装置的以扭转角速度作为基础的控制的实施例进行说明。在算出针对致动器的转矩指令值τ的方程式中，使用致动器角度、手臂角度(连杆角度)、致动器角速度、手臂角速度4个状态量，在其中的手臂角速度中使用上述的实施方式中说明过的“除去了低频成分的扭转角速度(Ω_t1、Ω_t2、Ω_t3)+经由连结装置的转矩传递装置向手臂传递的角速度(ω2、ω3、ω1)”。

测定作为上述4个状态量的致动器角度、手臂角度(连杆角度)、致动器角速度、对除去了低频成分的扭转角速度加上经由连结装置的转矩传递装置向手臂传递的角速度而得到的手臂角速度，以所测定出的4个状态量为基础，利用下述(式1)，构建状态反馈控制系统。

τ_i：致动器转矩指令

θ_mr：转矩传递装置(致动器)角度目标值

θ_m：转矩传递装置(致动器)角度＝根据角度传感器信息算出

θ_a：手臂角度＝手臂角速度(扭转角速度+转矩传递装置角速度)的近似积分

θ_m的1次微分＝转矩传递装置所驱动的角速度

θ_a的1次微分＝手臂角速度(扭转角速度+转矩传递装置角速度)

k₁，k₂，k₃，k₄，k₅：控制增益

式(1)中的k₁～k₅是控制装置的内部的控制增益，可以利用极点配置法或最佳调节器等来决定。通过使用如此得到的转矩指令值τ来控制，就可以抑制机械手装置的振动，执行准确的动作。

虽然在上述的实施例中，对使用在上述的实施方式中说明过的“除去了低频成分的扭转角速度(Ω_t1、Ω_t2、Ω_t3)+经由连结装置的转矩传递装置向手臂传递的角速度(ω2+ω3+ω1)作为手臂角速度的例子进行了说明，然而通过使用“扭转角速度”替代手臂角速度来作为状态量，也可以构建运算量少的控制系统。

即，将(式1)中的手臂角度θ_a以及其1次微分的手臂角速度dθ_a/dt作为扭转角速度(实施方式的Ω_t1、Ω_t2、Ω_t3)使用，利用下式(式2)来构建控制系统。

τ_i：致动器转矩指令

θ_mr：转矩传递装置(致动器)角度目标值

θ_m：转矩传递装置(致动器)角度＝根据角度传感器信息算出

θ_t：旋转角度＝扭转角速度的近似积分

θ_m的1次微分＝转矩传递装置所驱动的角速度

θ_t的1次微分＝扭转角速度

k_t1，k_t2，k_t3，k_t4，k_t5：控制增益

式(2)中的k_t1～k_t5是控制装置的内部的控制增益，可以利用极点配置法或最佳调节器等来决定。通过使用如此得到的转矩指令值τ来控制，就可以抑制机械手装置的振动，执行准确的动作。

(实施例2)

作为其他的实施例，利用图6所示的框图进行说明。图6相对于上述的实施方式的图2所示的框图不同点在于示出了控制部610的位置控制系统610a、速度控制系统610b取得运算结果、进行控制的例子。所以，对于与实施方式相同的构成使用相同的符号，省略说明。

在图6所示的本实施例中，也是CPU210包括后述的第一运算部510、第二运算部520、第三运算部530及控制部610，读出存储于ROM310中的程序而执行。另外，RAM410保存执行CPU210的程序而得到的数据，并从已保存的数据中向CPU210送出必需的数据。

如图6的框图所示，本实施例的控制由位置控制系统610a取得在第一运算部510中所换算的致动器的旋转角度数据。另外，同样由速度控制系统610b取得在第一运算部中所算出的致动器的角速度数据。速度控制系统610b取得在第三运算部530中算出的扭转角速度数据，与从位置控制系统610a输入的角速度指令一起，由速度控制系统610b向机械手装置100输出转矩指令。通过如此控制，就可以抑制机械手装置的振动，执行准确的动作。

虽然上述的实施方式以及实施例是利用如图1所示的具备3根手臂的水平多关节型的手臂体10进行的说明，然而并不限定于此，也可以适用于具备2根手臂的机械手装置或者具备4根以上的手臂的机械手装置。另外，在机械手装置型号方面，也可以适用于直动式、垂直多关节型等具备可动手臂的机械手装置。

Claims (6)

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

手臂；

检测所述手臂的旋转角度的角度传感器；

惯性传感器，其被设置于所述手臂；

第一运算部，其根据来自所述角度传感器的输出，算出所述手臂的角速度；

第二运算部，其根据来自所述惯性传感器的输出，算出所述手臂的角速度；

第三运算部，其将由所述第一运算部算出的所述角速度和由所述第二运算部算出的所述角速度的差所包含的低频成分除去。

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

所述低频成分是比所述机械手装置的机械系统固有频率当中的反共振频率及共振频率中的最低的频率低的频率。

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

还具备第四运算部，其将利用所述第一运算部算出的所述角速度与来自所述第三运算部的输出相加。

4.一种机械手装置的控制方法，其特征在于，

该机械手装置具备：手臂；检测所述手臂的旋转角度的角度传感器；惯性传感器，其被设置于所述手臂，

该机械手装置的控制方法包括：

第一运算步骤，其根据从所述角度传感器输出的信息，算出所述手臂的角速度；

第二运算步骤，其根据从所述惯性传感器输出的信息，算出所述手臂的角速度；

第三运算步骤，其将由所述第一运算步骤算出的所述角速度和由所述第二运算步骤算出的所述角速度的差所包含的低频成分除去。

5.根据权利要求4所述的机械手装置的控制方法，其特征在于，

所述低频成分是比所述机械手装置的机械系统固有频率当中的反共振频率及共振频率中的最低的频率低的频率。

6.根据权利要求4或5所述的机械手装置的控制方法，其特征在于，

还具备第四运算步骤，其将利用所述第一运算步骤算出的所述角速度与来自所述第三运算步骤的输出相加，算出所述手臂的角速度。