CN104097202A - 机械手、机械手控制装置以及机械手系统 - Google Patents

Abstract

本发明提供能够容易且可靠地抑制振动的机械手、机械手控制装置以及机械手系统。机械手（1）具备：基台（11）；第一臂部（12），其按照能够以第一转动轴为转动中心转动的方式连结于上述基台（11）；第二臂部（13），其按照能够以第二转动轴为转动中心转动的方式连结于上述第一臂部（12），该第二转动轴是与上述第一转动轴正交的轴或与正交于上述第一转动轴的轴平行的轴；第三臂部（14），其按照能够以第三转动轴为转动中心转动的方式连结于上述第二臂部（13），该第三转动轴是与上述第二转动轴平行的轴；角速度传感器，其设置于上述第一臂部（12）；以及加速度传感器，其设置于上述第三臂部（14）。

Description

机械手、机械手控制装置以及机械手系统

技术领域

本发明涉及机械手、机械手控制装置以及机械手系统。

背景技术

在专利文献1所记载的机械手中，在前端部即在最前端侧的第六连杆上设置六轴传感器，基于该六轴传感器的检测结果来针对各连杆分别求出绕目标轴的角速度的振动分量，从而进行抑制振动的控制，其中，上述六轴传感器对相互正交的X轴、Y轴、Z轴各自的方向的加速度、分别绕X轴、Y轴、Z轴的加速度进行检测。此外，连杆的角速度的振动分量被称为“扭转角速度”或“振动角速度”等。

专利文献1：日本特开2011－136395号公报

在专利文献1所记载的机械手中，由于六轴传感器的姿势因机械手的动作而改变，所以需要根据该六轴传感器的检测结果来进行被称为雅可比转换的坐标轴转换等，求出各连杆的角速度的振动分量。并且，需要相应于每时每刻变化的马达的转动角度来进行计算。

因此，需要复杂且庞大的运算处理，所以需要具有性能高、昂贵的CPU（Central Processing Unit：中央处理器）等的控制装置，因而存在成本增加的问题。

另外，由于需要复杂且庞大的运算处理，所以容易产生运算误差，从而存在因该运算误差而不能充分地抑制振动的问题。

发明内容

本发明的目的在于提供能够容易且可靠地抑制振动的机械手、机械手控制装置以及机械手系统。

这种目的能够通过下述的本发明而实现。

本发明的机械手的特征在于，具备：基台；第一臂部，其按照能够以第一转动轴为转动中心转动的方式连结于上述基台；第二臂部，其按照能够以第二转动轴为转动中心转动的方式连结于上述第一臂部，该第二转动轴是与上述第一转动轴正交的轴或与正交于上述第一转动轴的轴平行的轴；第三臂部，其按照能够以第三转动轴为转动中心转动的方式连结于上述第二臂部，该第三转动轴是与上述第二转动轴平行的轴；角速度传感器，其设置于上述第一臂部；加速度传感器，其设置于上述第三臂部，其加速度的检测轴与正交于上述第三转动轴的轴平行。

由此，能够容易且可靠地抑制振动。

即，首先，能够利用角速度传感器来检测第一臂部的角速度。另外，由于第三转动轴与第二转动轴平行，所以能够利用加速度传感器来包括第二臂部的转动量在内地对第三臂部的加速度进行检测而导出其角速度。而且，能够基于上述的检测结果来抑制振动。

此外，能够将由加速度传感器检测出的加速度直接用作角速度，在这种情况下能够进一步简化运算处理。另外，将由加速度传感器检测出的加速度直接用作角速度这在第三臂部的速度比较小时、第二臂部的长度比较短时尤其有效。以下，针对将由加速度传感器检测出的加速度直接用作角速度省略每次陈述的情况。

另外，即便机械手的姿势发生变化，角速度传感器的检测轴相对于第一转动轴的姿势也是一定的。因此，不需要对由角速度传感器检测出的第一臂部的角速度进行基于角速度传感器朝向的修正。

另外，第三转动轴以及第二转动轴与第一转动轴正交或与正交于第一转动轴的轴平行，所以能够不混入第一臂部的加速度分量地对包括第二臂部的转动量在内的第三臂部的加速度进行检测。另外，即便机械手的姿势发生变化，例如即便第一臂部、第二臂部、第三臂部发生了转动，也不需要对由加速度传感器检测出的第三臂部的加速度进行对应加速度传感器的姿势的修正。

由此，不需要复杂且庞大的运算，由此难以产生运算误差，从而能够可靠地抑制振动，另外还能够提高机械手控制中的响应速度。

另外，利用加速度传感器，不是对第二臂部的加速度，而是包括第二臂部的转动量在内地对第三臂部的加速度进行检测而导出角速度，所以能够更可靠地抑制振动。

另外，与还在第二臂部设置角速度传感器、加速度传感器的情况相比，能够减少传感器的个数，从而能够降低成本，另外能够简化结构。

另外，由于设置于第一臂部的惯性传感器不是加速度传感器，而是角速度传感器，所以难以受到噪声的影响，从而能够更可靠地检测角速度。其理由是：第一臂部上惯性传感器的设置位置多在第一转动轴的附近，因此若是加速度传感器的情况下，则输出变小，噪声影响变大。

在本发明的机械手中，优选上述加速度传感器的加速度的检测轴为与上述第三臂部的中心轴正交或与正交于上述第三臂部的中心轴的轴平行。

由此，能够更可靠地包括第二臂部的转动量在内地对第三臂部的加速度进行检测而导出其角速度。

在本发明的机械手中，优选上述角速度传感器的角速度的检测轴为与上述第一转动轴平行或与上述第一转动轴一致。

由此，能够更可靠地检测第一臂部的角速度。

在本发明的机械手中，优选为具备：角速度传感器单元，其具有第一壳体、上述角速度传感器以及电路部，上述角速度传感器以及上述电路部被设置在上述第一壳体内，上述电路部对从上述角速度传感器输出的信号进行AD转换并进行发送；以及加速度传感器单元，其具有第二壳体、上述加速度传感器以及电路部，上述加速度传感器以及上述电路部设置于上述第二壳体内，上述电路部对从上述加速度传感器输出的信号进行AD转换并进行发送，上述角速度传感器单元设置于上述第一臂部，上述加速度传感器单元设置于上述第三臂部。

由此，与另外设置电路部的情况相比，能够简化结构。

在本发明的机械手中，优选为上述第一壳体的外形是长方体，上述角速度传感器的角速度的检测轴为与上述第一壳体的上述长方体的最大面的法线一致。

由此，能够容易且可靠地对角速度传感器的角速度的检测轴的方向进行识别，从而能够容易地将角速度传感器调整为适当的姿势。

在本发明的机械手中，优选为上述第一壳体具有安装部，该安装部在上述第一壳体的角部安装于上述第一臂部，上述第二壳体具有安装部，该安装部在上述第二壳体的角部安装于上述第三臂部。

由此，能够可靠地将角速度传感器单元安装于第一臂部，能够可靠地将加速度传感器单元安装于第三臂部。

在本发明的机械手中，优选为具备具有导电性并将上述第一壳体的上述安装部固定于上述第一臂部的第一固定部件，通过上述第一固定部件将上述角速度传感器单元的上述电路部接地连接于上述第一臂部；具备具有导电性并将上述第二壳体的上述安装部固定于上述第三臂部的第二固定部件，通过上述第二固定部件将上述加速度传感器单元的上述电路部接地连接于上述第三臂部。

由此，能够减少部件件数，从而能够简化结构。

在本发明的机械手中，优选为上述第一臂部具有框体和与上述框体一体形成的臂部侧安装部，上述角速度传感器单元直接安装于上述臂部侧安装部。

由此，角速度传感器单元能够可靠地与第一臂部一体地转动。

在本发明的机械手中，优选为上述第三臂部具有框体和与上述壳体一体形成的臂部侧安装部，上述加速度传感器单元直接安装于上述臂部侧安装部。

由此，加速度传感器单元能够可靠地与第三臂部一体地转动。

在本发明的机械手中，优选为具有电缆，上述电缆被配设于上述第一臂部，对该机械手供给电力，上述角速度传感器被配置于上述第一臂部的与上述电缆相反一侧的端部。

由此，能够防止角速度传感器受到由电缆产生的噪声的影响的情况，另外能够防止角速度传感器侧的电路、配线因电缆而短路的情况。

在本发明的机械手中，优选为具有电缆，上述电缆被配设于上述第三臂部，对该机械手供给电力，上述加速度传感器被配置于上述第三臂部的与上述电缆相反一侧的端部。

由此，能够防止加速度传感器受到由电缆产生的噪声的影响的情况，另外能够防止加速度传感器侧的电路、配线因电缆而短路的情况。

在本发明的机械手中，优选为具备：第四臂部，其按照能够以第四转动轴为转动中心转动的方式连结于上述第三臂部，该第四转动轴是与上述第三转动轴正交的轴或与正交于上述第三转动轴的轴平行的轴；第五臂部，其按照能够以第五转动轴为转动中心转动的方式连结于上述第四臂部，该第五转动轴是与上述第四转动轴正交的轴或与正交于上述第四转动轴的轴平行的轴；以及第六臂部，其按照能够以第六转动轴为转动中心转动的方式连结于上述第五臂部，该第六转动轴是与上述第五转动轴正交的轴或与正交于上述第五转动轴的轴平行的轴。

由此，能够容易地进行复杂的动作。

在本发明的机械手中，优选为上述第一转动轴与上述基台的设置面的法线一致。

由此，能够容易地进行机械手的控制。

本发明的机械手控制装置的特征在于，该机械手控制装置控制机械手的动作，该机械手具备：基台；第一臂部，其按照能够以第一转动轴为转动中心转动的方式连结于上述基台；第二臂部，其按照能够以第二转动轴为转动中心转动的方式连结于上述第一臂部，该第二转动轴是与上述第一转动轴正交的轴或与正交于上述第一转动轴的轴平行的轴；以及第三臂部，其按照能够以第三转动轴为转动中心转动的方式连结于上述第二臂部，该第三转动轴是与上述第二转动轴平行的轴，上述机械手控制装置具备：接收部，其接收第一信号和第二信号，上述第一信息是从设置于上述第一臂部的角速度传感器输出的信号，上述第二信号是从设置于上述第三臂部设置且加速度的检测轴是与正交于上述第三转动轴的轴平行的加速度传感器输出的信号；运算部，其基于由上述接收部接收到的上述第一信号以及上述第二信号来求出上述第一臂部的角速度的振动分量以及上述第三臂部的角速度的振动分量；以及控制部，其基于由上述运算部求出的上述第一臂部的角速度的振动分量以及上述第三臂部的角速度的振动分量来控制上述机械手的动作。

由此，能够容易且可靠地抑制振动。

即，首先能够利用运算部基于由角速度传感器检测出的第一臂部的角速度基来求出第一臂部的角速度的振动分量。另外，由于第三转动轴与第二转动轴平行，所以能够利用运算部基于由加速度传感器检测出的包括第二臂部的转动量在内的第三臂部的加速度来包括第二臂部的转动量在内地求出第三臂部的角速度的振动分量。而且，能够基于上述第一臂部的角速度的振动分量以及第三臂部的角速度的振动分量来抑制振动。

另外，即便机械手的姿势发生了变化，角速度传感器的检测轴相对于第一转动轴的姿势也是一定的。因此，不需要对由角速度传感器检测出的第一臂部的角速度进行基于角速度传感器朝向的修正。

另外，由于第三转动轴以及第二转动轴与第一转动轴正交或与正交于第一转动轴的轴平行，所以能够不混入第一臂部的加速度分量地对包括第二臂部的转动量在内的第三臂部的加速度进行检测。另外，即便机械手的姿势发生了变化，例如即便第一臂部、第二臂部、第三臂部发生了转动，也不需要对由加速度传感器检测出的第三臂部的加速度进行对应加速度传感器的姿势的修正。

由此，不需要复杂且庞大的运算，由此难以产生运算误差，从而能够可靠地抑制振动，另外还能够提高机械手控制中的响应速度。

另外，利用运算部，不是基于仅第二臂部的角速度的振动分量，而是基于由加速度传感器检测出的包括第二臂部的转动量在内的第三臂部的加速度来求出包括第二臂部的角速度的振动分量在内的第三臂部的角速度的振动分量，所以能够更可靠地抑制振动。

本发明的机械手系统的特征在于，具备：本发明的机械手；以及机械手控制装置，其控制上述机械手的动作。

由此，能够容易且可靠地抑制振动。

即，首先能够利用角速度传感器检测第一臂部的角速度。另外，由于第三转动轴与第二转动轴平行，所以能够利用加速度传感器包括第二臂部的转动量在内地对第三臂部的加速度进行检测而导出其角速度。而且，能够基于上述检测结果来抑制振动。

另外，即便机械手的姿势发生了变化，角速度传感器的检测轴相对于第一转动轴的姿势也是一定的。因此，不需要对由角速度传感器检测出的第一臂部的角速度进行角速度传感器的朝向的修正。

另外，第三转动轴以及第二转动轴由于与第一转动轴正交或与正交于第一转动轴的轴平行，所以能够不混入第一臂部的加速度分量地对包括第二臂部的转动量在内的第三臂部的加速度进行检测。另外，即便机械手的姿势发生了变化，例如即便第一臂部、第二臂部、第三臂部发生了转动，也不需要对由加速度传感器检测出的第三臂部的加速度进行对应加速度传感器的姿势的修正。

由此，不需要复杂且庞大的运算，由此难以产生运算误差，从而能够可靠地抑制振动，另外能够提高机械手控制中的响应速度。

另外，利用加速度传感器，不是对第二臂部的加速度，而是包括第二臂部的转动量在内地对第三臂部的加速度进行检测而导出角速度，所以能够更可靠地抑制振动。

另外，与还在第二臂部设置角速度传感器、加速度传感器的情况相比，能够减少传感器的个数，从而能够降低成本，另外能够简化结构。

另外，由于设置于第一臂部的惯性传感器不是加速传感器，而是角速度传感器，所以难以受到噪声的影响，从而能够更可靠地检测角速度。其理由是：第一臂部中惯性传感器的设置位置多在第一转动轴的附近，因此，若是加速度传感器的情况下，则输出变小，噪声的影响变大。

附图说明

图1是从正面侧观察本发明的机械手的第一实施方式的立体图。

图2是从背面侧观察图1示出的机械手的立体图。

图3是图1示出的机械手的简图。

图4是具有图1示出的机械手的机械手系统的主要部分的框图。

图5是图1示出的机械手的主视图。

图6是表示图1示出的机械手的第一臂部中的角速度传感器附近的图。

图7是表示图1示出的机械手的第三臂部中的加速度传感器附近的图。

图8是图1示出的机械手的角速度传感器单元的剖视图。

图9是图1示出的机械手的角速度传感器所具有的陀螺元件的俯视图。

图10是表示图9示出的陀螺元件的动作的图。

图11是图1示出的机械手的加速度传感器的图。

图12是图1示出的机械手的主要部分的框图。

图13是图1示出的机械手的主要部分的框图。

图14是图1示出的机械手的主要部分的框图。

图15是图1示出的机械手的主要部分的框图。

图16是图1示出的机械手的主要部分的框图。

具体实施方式

以下，根据附图示出的优选的实施方式详细地对本发明的机械手、机械手控制装置以及机械手系统进行说明。

第一实施方式

图1是从正面侧观察本发明的机械手的第一实施方式的立体图。图2是从背面侧观察图1示出的机械手的立体图。图3是图1示出的机械手的简图。图4是具有图1示出的机械手的机械手系统的主要部分的框图。图5是图1示出的机械手的主视图。图6是表示图1示出的机械手的第一臂部的角速度传感器附近的图。图7是表示图1示出的机械手的第三臂部的加速度传感器附近的图。图8是图1示出的机械手的角速度传感器单元的剖视图。图9是图1示出的机械手的角速度传感器所具有的陀螺元件的俯视图。图10是表示图9示出的陀螺元件的动作的图。图11是图1示出的机械手的加速度传感器的图。图12～图16分别是图1示出的机械手的主要部分的框图。

此外，以下，为了便于说明，将图1～图3、图5～图7中的上侧称为“上”或“上方”、下侧称为“下”或“下方”。另外，将图1～图3、图5～图7中的基台侧称为“基端”、其相反一侧称为“前端”。另外，在图8中，对应角速度传感器单元并以括弧书写来标注加速度传感器单元的各部分的附图标记，并省略加速度传感器单元的图示。

图1～图4示出的机械手系统（产业用机械手系统）10例如能够使用在制造手表那样的精密设备等的制造工序中，具有机械手（产业用机械手）1和控制机械手1的动作的机械手控制装置（控制机构）20（参照图4）。机械手1与机械手控制装置20电连接。另外，机械手控制装置20例如由内置有CPU（Central Processing Unit：中央处理器）的个人计算机（PC）等构成。此外，稍后对机械手控制装置20进行详述。

机械手1具备基台11；4个臂部（连杆）12、13、14、15；腕（连杆）16；以及6个驱动源401、402、403、404、405、406。该机械手1是将基台11、臂部12、13、14、15以及腕16从基端侧向前端侧按照该按顺序连结的垂直多关节（6轴）机械手（机械手主体）。在垂直多关节机械手中，还能够将基台11、臂部12～15以及腕16统称为“臂部”，能够分别将臂部12称为“第一臂部”、将臂部13称为“第二臂部”、将臂部14称为“第三臂部”、将臂部15称为“第四臂部”、将腕16称为“第五臂部、第六臂部”。此外，在本实施方式中，腕16具有第五臂部和第六臂部。能够在腕16安装终端执行器等。

臂部12～15、腕16被分别支承为能够相对于基台11独立地移位。该臂部12～15、腕16的长度各自未被特别限定，但在图示的结构中，与第三臂部14以及腕16相比，将第一臂部12、第二臂部13、第四臂部15的长度设定得较长。

基台11与第一臂部12经由关节（接头）171而连结。而且，第一臂部12能够相对于基台11以与竖直方向平行的第一转动轴O1为转动中心绕该第一转动轴O1转动。第一转动轴O1与基台11的设置面即地面101上表面的法线一致。绕该第一转动轴O1的转动是通过具有马达401M的第一驱动源401的驱动而进行的。另外，第一驱动源401通过马达401M和电缆（未图示）而被驱动，该马达401M经由电连接的马达驱动器301而被机械手控制装置20控制（参照图4）。此外，第一驱动源401可以构成为通过与马达401M一起设置的减速器（未图示）来传递来自马达401M的驱动力，另外，也可以省略减速器，但在本实施方式中，第一驱动源401具有减速器。

第一臂部12与第二臂部13经由关节（接头）172而连结。而且，第二臂部13能够相对于第一臂部12以与水平方向平行的第二转动轴O2为轴心转动。第二转动轴O2与第一转动轴O1正交。绕该第二转动轴O2的转动是通过具有马达402M的第二驱动源402的驱动而进行的。另外，第二驱动源402通过马达402M和电缆（未图示）而被驱动，该马达402M经由电连接的马达驱动器302而被机械手控制装置20控制（参照图4）。此外，第二驱动源402可以构成为通过与马达402M一起设置的减速器45（参照图5）来传递来自马达402M的驱动力，另外，也可以省略减速器，但在本实施方式中，第二驱动源402具有减速器45。另外，第二转动轴O2也可以与正交于第一转动轴O1的轴平行。

第二臂部13与第三臂部14经由关节（接头）173而连结。而且，第三臂部14能够相对于第二臂部13以与水平方向平行的第三转动轴O3为转动中心绕该第三转动轴O3转动。第三转动轴O3与第二转动轴O2平行。绕该第三转动轴O3的转动是通过第三驱动源403的驱动而进行的。另外，第三驱动源403通过马达403M和电缆（未图示）而被驱动，该马达403M经由电连接的马达驱动器303而被机械手控制装置20控制（参照图4）。此外，第三驱动源403可以构成为通过与马达403M一起设置的减速器（未图示）来传递来自马达403M的驱动力，另外，也可以省略减速器，但在本实施方式中，第三驱动源403具有减速器。

第三臂部14与第四臂部15经由关节（接头）174而连结。而且，第四臂部15能够相对于第三臂部14（基台11）以与第三臂部14的中心轴方向平行的第四转动轴O4为转动中心绕该第四转动轴O4转动。第四转动轴O4与第三转动轴O3正交。绕该第四转动轴O4的转动是通过第四驱动源404的驱动而进行的。另外，第四驱动源404通过马达404M和电缆（未图示）而被驱动，该马达404M经由电连接的马达驱动器304而被机械手控制装置20控制（参照图4）。此外，第四驱动源404可以构成为通过与马达404M一起设置的减速器（未图示）来传递来自马达404M的驱动力，另外，也可以省略减速器，但在在本实施方式中，第四驱动源404具有减速器。第四转动轴O4也可以与正交于第三转动轴O3的轴平行。

第四臂部15与第五腕16经由关节（接头）175而连结。而且，腕16能够相对于第四臂部15以与水平方向（y轴方向）平行的第五转动轴O5为转动中心绕该第五转动轴O5转动。第五回动轴O5与第四转动轴O4正交。绕该第五转动轴O5的转动是通过第五驱动源405的驱动而进行的。另外，第五驱动源405通过马达405M和电缆（未图示）而被驱动，该马达405M经由电连接的马达驱动器305而被机械手控制装置20控制（参照图4）。此外，第五驱动源405可以构成为通过与马达405M一起设置的减速器（未图示）来传递来自马达405M的驱动力，另外，也可以省略减速器，但在本实施方式中，第五驱动源405具有减速器。另外，腕16能够经由关节（接头）176而以与第五转动轴O5垂直的第六转动轴O6为转动中心绕该第六转动轴O6转动。第六转动轴O6与第五转动轴O5正交。绕该第六转动轴O6的转动是通过第六驱动源406的驱动而进行的。另外，第六驱动源406通过马达和电缆（未图示）而被驱动，该马达406M经由电连接的马达驱动器306而被机械手控制装置20控制（参照图4）。此外，第六驱动源406可以构成为通过与马达406M一起设置的减速器（未图示）来传递来自马达406M的驱动力，另外，也可以省略减速器，但在本实施方式中，第六驱动源406具有减速器。另外，第五转动轴O5也可以与正交于第四转动轴O4的轴平行，另外，第六转动轴O6也可以与正交于第五回动轴O5的轴平行。

另外，如图6所示，在第一臂部12设置有角速度传感器31，即设置有具有角速度传感器31的角速度传感器单元71。利用该角速度传感器31对第一臂部12绕第一转动轴O1的角速度进行检测。

另外，如图7所示，在第三臂部14设置有加速度传感器32，即设置有具有加速度传感器32的加速度传感器单元72。利用该加速度传感器32对第三臂部14绕第二转动轴O2的角速度进行检测。

此外，作为角速度传感器31、加速度传感器32分别未特别限定，稍后对其一个例子进行说明。

在此，在该机械手1中，通过抑制第一臂部12、第二臂部13以及第三臂部14的振动来抑制机械手1整体的振动。其中，为抑制第一臂部12、第二臂部13以及第三臂部的振动，不是在第一臂部12、第二臂部13以及第三臂部的全部设置角速度传感器、加速度传感器，而是如上述那样地仅在第一臂部12设置角速度传感器31、仅在第三臂部14设置加速度传感器32，基于该角速度传感器31、加速度传感器32的检测结果来控制驱动源401、402的动作。由此，与在第一臂部12、第二臂部13以及第三臂部14的全部设置角速度传感器、加速度传感器的情况相比，能够减少传感器的个数，从而能够降低成本，另外还能够简化电路结构。另外，通过加速度传感器32，不是对第二臂部13的加速度，而是包括第二臂部13的转动量在内地对第三臂部14的加速度进行检测，所以能够更可靠地抑制振动。另外，能够通过控制第二驱动源402的动作来提高抑制机械手1的振动的效果，其中，该第二驱动源402使比第三臂部14更靠基端侧的第二臂部13转动。

对驱动源401～406而言，在各个马达或减速器设置有第一位置传感器411、第二位置传感器412、第三位置传感器413、第四位置传感器414、第五位置传感器415以及第六位置传感器416。作为上述位置传感器分别未特别限定，例如，可以使用编码器、旋转编码器、解析器、电位器等。利用上述位置传感器411～416分别对驱动源401～406的马达或减速器的轴部的转动角度进行检测。作为该驱动源401～406的马达分别未特别限定，例如，优选为使用AC伺服马达、DC伺服马达等伺服马达。另外，上述各电缆可以分别插通机械手1。

如图4所示，机械手1与机械手控制装置20电连接。即，驱动源401～406、位置传感器411～416、角速度传感器31以及加速度传感器32分别与机械手控制装置20电连接。

而且，机械手控制装置20能够使臂部12～15、腕16分别独立地动作，即能够经由马达驱动器301～306来分别独立地控制驱动源401～406。在该情况下，机械手控制装置20利用位置传感器411～416、角速度传感器31以及加速度传感器32来进行检测，基于该检测结果来分别控制驱动源401～406的驱动，例如角速度、转动角度等。该控制程序预先存储于内置于机械手控制装置20的记录介质。

如图1、图2所示，当机械手1是垂直多关节机械手时，基台11位于该垂直多关节机械手的最下方，是固定于设置空间的地面101的部分。作为该固定方法未特别限定，例如，在图1、图2示出的本实施方式中，使用利用多个螺栓111的固定方法。此外，作为基台11的设置空间处的固定位置，除地面之外，也可以是设置空间的壁、天花板。

基台11具有中空的基台主体（壳体）112。基台主体112能够划分为呈圆筒状的圆筒状部113和一体地形成于该圆筒状部113的外周部的呈箱状的箱状部114。而且，在这种基台主体112中例如收纳有马达401M、马达驱动器301～306。

臂部12～15分别具有中空的臂部主体（框体）2、驱动机构3以及密封机构4。此外，以下为便于说明，将第一臂部12所具有的臂部主体2、驱动机构3以及密封机构4分别称为“臂部主体2a”、“驱动机构3a”以及“密封机构4a”，将第二臂部13所具有的臂部主体2、驱动机构3以及密封机构4分别称为“臂部主体2b”、“驱动机构3b”以及“密封机构4b”，将第三臂部14所具有的臂部主体2、驱动机构3以及密封机构4分别称为“臂部主体2c”、“驱动机构3c”以及“密封机构4c”，将第四臂部15所具有的臂部主体2、驱动机构3以及密封机构4分别称为“臂部主体2d”、“驱动机构3d”以及“密封机构4d”。

另外，关节171～176分别具有转动支承机构（未图示）。该转动支承机构是将相互连结的两个臂部中的一方支承为能够相对于另一方转动的机构、是将相互连结的基台11与第一臂部12中的一方支承为能够相对于另一方转动的机构、是将相互连结的第四臂部15与第五腕16中的一方支承为能够相对于另一方转动的机构。在以相互连结的第四臂部15与腕16为一个例子的情况下，转动支承机构能够使腕16相对于第四臂部15转动。另外，各转动支承机构分别具有减速器（未图示），上述减速器以规定的减速比对对应的马达的转动速度进行减速，并将驱动力传递至对应的臂部、腕16的腕主体161以及支承环162。此外，如上所述，在本实施方式中，包括该减速器与马达作为驱动源。

第一臂部12以相对于水平方向倾斜的姿势连结于基台11的上端部（前端部）。在该第一臂部12中，驱动机构3a具有马达402M，并收纳于臂部主体2a内。另外，臂部主体2a内被密封机构4a气密密封。臂部主体2a具有1对前端侧的舌片部241a、241b和基端侧的根部251。舌片部241a与舌片部241b分离并相互对置。另外，舌片部241a、241b相对于根部251倾斜，由此，第一臂部12相对于水平方向倾斜。而且，在舌片部241a与舌片部241b之间配置有第二臂部13的基端部。

另外，第一臂部12中角速度传感器31的设置位置未特别限定，但在本实施方式中，如图6所示，角速度传感器31即角速度传感器单元71设置于第一臂部12的臂部主体2a的根部251的内部与电缆85相反一侧的端部。电缆85是向机械手1的各马达401M～406M供给电力的电缆。由此，能够防止角速度传感器31受到从电缆85发出的噪声的影响的情况，另外，还能够防止角速度传感器单元71的后述的电路部713、配线、角速度传感器31由于电缆85而短路的情况。

在此，针对驱动机构3以及减速器，以设置于第一臂部12的臂部主体2a内并使第二臂部13转动的驱动机构3为代表进行说明。

如图1以及图5所示，驱动机构3具有：第一带轮91，其连结于马达402M的轴部；第二带轮92，其与第一带轮91分离配置；以及带（正时带）93，其架设于第一带轮91与第二带轮92。而且，第二带轮92与第二臂部13的轴部利用减速器45连结。

作为减速器45，未特别限定，例如能够举例由多个齿轮构成的减速器、被称为谐波驱动器（“谐波驱动器”是注册商标）的减速器等。

作为机械手1的臂部12～15、腕16的振动的主要原因，例如能够举例减速器45的扭转、挠曲、带93的伸缩、臂部12～15以及腕16的挠曲等。

第二臂部13连结于第一臂部12的前端部。在该第二臂部13中，驱动机构3b具有马达403M并收纳于臂部主体2b内。另外，臂部主体2a内被密封机构4b气密密封。臂部主体2b具有前端侧的1对舌片部242a、242b和基端侧的根部252。舌片部242a与舌片部242b分离并相互对置。而且，在舌片部242a与舌片部242b之间配置有第三臂部14的基端部。

第三臂部14连结于第二臂部13的前端部。在该第三臂部14中，驱动机构3c具有马达404M并收纳于臂部主体2c内。另外，臂部主体2c内被密封机构4c气密密封。此外，臂部主体2c由相当于上述臂部主体2a的根部251、上述臂部主体2b的根部252的部件构成。

另外，第三臂部14中加速度传感器32的设置位置未特别限定，但在本实施方式中，如图7所示，加速度传感器32即加速度传感器单元72设置于第三臂部14的臂部主体2c的内部与电缆85相反的一侧的端部。由此，能够防止加速度传感器32受到从电缆85发出的噪声的影响的情况，另外，还能够防止加速度传感器单元72的电路部723、配线、加速度传感器32由于电缆85而短路的情况。

第四臂部15以与第三臂部14的中心轴方向平行的方式连结于第三臂部14的前端部。在该臂部15中，驱动机构3d具有马达405M、406M，并收纳于臂部主体2d内。另外，臂部主体2d内被密封机构4d气密密封。臂部主体2d具有前端侧的1对舌片部244a、244b和基端侧的根部254。舌片部244a与舌片部244b分离并相互对置。而且，在舌片部244a与舌片部244b之间配置有腕16的支承环162。

在第四臂部15的前端部（与基台11相反的一侧的端部）连结有腕16。在该腕16，在其前端部（与第四臂部15相反一侧的端部），作为功能部（终端执行器），例如可拆装地安装有把持手表等精密设备的机械手（未图示）。此外，作为机械手，未特别限定，例如，能够举例具有多个指部（finger）的结构的机械手。而且，该机械手1保持用机械手把持精密设备不变，通过控制臂部12～15、腕16等的动作来输送精密设备。

腕16具有呈圆筒状的腕主体（第六臂部）161和呈环状的支承环（第五臂部）162，其中，该支承环（第五臂部）162与腕主体161独立构成，并设置于该腕主体161的基端部。

腕主体161的前端面163是平坦的面，成为安装机械手的安装面。另外，腕主体161经由关节176连结于第四臂部15的驱动机构3d，并被该驱动机构3d的马达406M驱动而绕第六转动轴O6转动。

支承环162经由关节175连结于第四臂部15的驱动机构3d，并被该驱动机构3d的马达405M驱动而随腕主体161一起绕第五转动轴O5转动。

作为臂部主体2的构成材料，未特别限定，例如可以使用各种金属材料，在上述金属材料中特别优选铝或铝合金。在臂部主体2是使用金属模而成型的铸件的情况下，由于对于该臂部主体2的构成材料使用铝或铝合金，从而能够容易地进行金属模成型。

另外，作为基台11的基台主体112、腕16的腕主体161以及支承环162的构成材料，分别未特别限定，例如能够举例与上述臂部主体2的构成材料相同的材料等。此外，腕16的腕主体161的构成材料优选为使用不锈钢。

另外，作为密封机构4的构成材料，未特别限定，例如能够使用各种树脂材料、各种金属材料。此外，作为密封机构4的构成材料，能够通过使用树脂材料来实现轻型化。

接下来，对角速度传感器单元71、加速度传感器单元72进行说明。

如图8所示，角速度传感器单元71具有：第一壳体711；电路基板712，其设置于第一壳体711内，并具有配线；以及角速度传感器31和电路部713，它们电连接于电路基板712上。在本实施方式中，第一壳体711由密封件构成，利用该密封件将角速度传感器31、电路部713以及电路基板712整体密封。

同样，加速度传感器单元72具有：第二壳体721；电路基板722，其设置于第二壳体721内，并具有配线；以及加速度传感器32和电路部723，它们电连接于电路基板722上。在本实施方式中，第二壳体721由密封件构成，利用该密封件将加速度传感器32、电路部723以及电路基板722整体密封。

这样，通过封装角速度传感器31以及电路部713、加速度传感器32以及电路部723能够简化结构。

首先，对角速度传感器单元71进行说明。

电路部713具有：AD转换部，其对从角速度传感器31输出的信号进行AD转换，即、将模拟信号转换为数字信号；以及发送部，其将上述转换出的信号发送至机械手控制装置20。

另外，第一壳体711的外形是立方体。

另外，作为角速度传感器31，只要能检测绕检测轴的角速度即可，而未特别限定，但例如可以使用陀螺元件。作为陀螺元件的结构，未特别限定，以下对该陀螺元件的一个例子进行说明。另外，以下如图9以及图10所示，将相互正交的3个轴设为X轴、Y轴以及Z轴。

如图9所示，陀螺元件33具有：水晶基板；未图示的检测用电极，其设置于各检测用振动臂332a、332b；以及未图示的驱动用电极，其设置于各驱动用振动臂334a、334b、334c、334d，其中，水晶基板具有：基部331；1对检测用振动臂332a、332b，它们从基部331的两侧向Y轴方向且向相互相反的一侧延伸；1对连结臂部333a、333b，它们从基部331的两侧向X轴方向且向相互相反的一侧延伸；1对驱动用振动臂334a、334b，它们从连结臂部333a的前端部的两侧向Y轴方向且向相互相反的一侧延伸；以及1对驱动用振动臂334c、334d，它们从连结臂部333b的前端部的两侧向Y轴方向且向相互相反的一侧延伸。

这种陀螺元件33以如下的方式动作。此外，在图10（a）、10（b）中，为便于表现振动方式而用线来表示各振动臂。

首先，如图10（a）所示，在未对陀螺元件33施加角速度的状态下，通过对驱动用电极施加电压来使各驱动用振动臂334a、334b、334c、334d向箭头E示出的方向弯曲振动。该弯曲振动按照规定频率重复实线所示的振动姿态和双点划线所示的振动姿态。此时，驱动用振动臂334a、334b和驱动用振动臂334c、334d进行相对于通过重心G的Y轴线对称的振动。

如图10（b）所示，在进行该振动的状态下，若对陀螺元件33施加绕Z轴（检测轴）的角速度ω，则对驱动用振动臂334a、334b、334c、334d以及连结臂部333a、333b作用箭头B方向的科里奥利力，从而对上述臂部激发新的振动。另外，与此同时，对检测用振动臂332a、332b激发与箭头B的振动呼应的箭头C方向的振动。而且，从检测用电极输出与由该检测用振动臂332a、332b的振动产生的检测用振动臂332a、332b的变形对应的信号（电压）。

另外，角速度传感器31具有角速度的检测轴（以下，还简称为“检测轴”）311，并构成为检测绕该检测轴311的角速度。另外，角速度传感器31的检测轴311与第一壳体711的长方体的最大面的法线一致。由此，能够容易且可靠地识别角速度传感器31的检测轴311的方向，从而能够容易地将角速度传感器31调整至适当的姿势。而且，角速度传感器31即角速度传感器单元71设置为角速度传感器31的检测轴311与第一转动轴O1平行。此外，角速度传感器单元71也可以设置为角速度传感器31的检测轴311与第一转动轴O1一致。

另外，如图6以及图8所示，第一壳体711在其4个角部具有安装于第一臂部12的安装部7111。在各安装部7111分别形成有供阳螺纹件（固定部件）81插入的孔7112。

另一方面，第一臂部12具有3个臂部侧安装部121，它们与臂部主体2a一体形成，并安装有角速度传感器单元71（第一壳体711）。各臂部侧安装部121分别由在臂部主体2a突出形成的支柱构成。另外，各臂部侧安装部121分别配置于与第一壳体711的取付部7111对应的位置。另外，在各臂部侧安装部121的前端部分别形成有供阳螺纹件81旋合的阴螺纹部122。

此外，与上述臂部主体2a一体形成的臂部侧安装部121中的“一体”，不是指分别形成部件将它们接合的情况，而是指例如通过压铸等同时形成臂部主体2a与臂部侧安装部121的情况。后述的与臂部主体2c一体形成的臂部侧安装部141中的“一体”也同样。

在将角速度传感器单元71安装（设置）于第一臂部12时，将三个阳螺纹件81分别插入第一壳体711的孔7112，使之旋合于第一臂部12的臂部侧安装部121的前端部的阴螺纹部122。由此，利用各阳螺纹件81将第一壳体711的3个安装部7111分别固定于第一臂部12所对应的臂部侧安装部121。即，将角速度传感器单元71安装于第一臂部12的臂部侧安装部121。在该情况下，在臂部侧安装部121与角速度传感器单元71之间未夹设任何部件，即角速度传感器单元71直接安装于臂部侧安装部121。由此，能够将角速度传感器单元71可靠地安装于第一臂部12，另外，角速度传感器单元71能够可靠地与第一臂部12一体地转动。

此外，角速度传感器单元71直接安装于臂部侧安装部121中的“直接”，不是指将角速度传感器单元71安装于其它基板等中间体，并将该中间体安装于臂部侧安装部121的情况。即，是指在臂部侧安装部121与角速度传感器单元71之间，除粘合剂等之外未夹设任何部件的情况。后述的加速度传感器单元72直接安装于臂部侧安装部141中的“直接”也同样。

另外，阳螺纹件81具有导电性，例如由各种金属材料形成。该阳螺纹件81在插入第一壳体711的孔7112，并旋合于臂部侧安装部121的前端部的阴螺纹部122时，电连接于电路基板712的配线，该电路基板712电连接于电路部713的接地用端子，另外，阳螺纹件81的前端部电连接于臂部侧安装部121。由此，电路部713的接地用端子经由配线以及阳螺纹件81电连接于第一臂部12的臂部主体2a，从而接地。由此，能够减少接地所需要的部件件数，从而能够简化结构。

接下来，对加速度传感器单元72进行说明，但以下，以与角速度传感器单元71的不同点为中心进行说明，对于相同的事项，省略其说明。

电路部723具有：AD转换部，其对从加速度传感器32输出的信号进行AD转换，即，将模拟信号转换为数字信号；以及发送部，其将上述转换后的信号发送至机械手控制装置20。

另外，第二壳体721的外形是立方体。

另外，作为加速度传感器32，只要能检测检测轴方向的加速度即可，而未特别限定，以下对其一个例子进行说明。另外，以下，如图11所示，将相互正交的3个轴设为X轴、Y轴以及Z轴。

如图11所示，加速度传感器32具有压电振动片34和振荡器35。此外，振荡器35可以设置于电路部723。压电振动片34具有由压电材料形成的矩形的基部343和1对以该基部343为基端沿相同方向延伸的直线状的振动臂341、342。振动臂341的轴与振动臂342的轴平行，并且该振动臂341、342的轴的方向与检测轴的方向一致。

作为压电材料，例如能够使用水晶晶片。在该情况下，使用将水晶Z板切断研磨成规定厚度而得到的水晶晶片，其中，该水晶Z板是在从水晶的单结晶切出时，以使X轴为电轴、Y轴为机械轴、Z轴为光轴的方式，以Z轴为中心在图11中0°以上、5°以下的范围顺时针转动切出的。而且，通过对这种水晶晶片进行例如蚀刻来形成压电振动片34的外形。

在压电振动片34的振动臂341、342的主表面，即上表面和下表面形成有未图示的电极，上述电极电连接于基部343的引出电极344、345。

若对该引出电极344、345施加驱动电压，则产生振动臂341、342的前端部相互接近、分离的弯曲振动（弯曲运动）。由该弯曲振动引起的振动臂341、342的变形作为压电作用而产生的交流电压（交流电流）的频率f，在将振动臂341、342的宽度设为W、长度设为L时，用下述（1）式表示。

f＝W/L²…（1）

另外，在基部343的引出电极344、345电连接有振荡器35。振荡器35具有将规定频率的驱动电压施加于压电振动片34的功能。若利用振荡器35对压电振动片34施加规定频率的驱动电压，则压电振动片34以特定振动模式弯曲振动。

将由该弯曲振动产生并具有该弯曲振动的频率的信号经由振荡器35输出。

在此，若对压电振动片34沿振动臂341、342的轴向即检测轴的方向作用加速度，则振动臂341、342沿其轴向略微变形（伸缩）。即，振动臂341、342的长度L变小。如上述（1）式所示，因该振动臂341、342的伸缩引起压电振动片34的弯曲振动的频率f发生变化。

具体而言，在图11中，在加速度向G1方向作用的情况下，振动臂341、342略微伸长。在该情况下，弯曲振动的频率f减小。另外，在加速度向G2方向作用的情况下，振动臂341、342略微收缩。在该情况下，弯曲振动的频率f增大。由于该弯曲振动的频率f与从加速度传感器32输出的信号的频率相等，所以从加速度传感器32输出的信号表示加速度传感器32的检测轴的方向的加速度。因此，能够根据从加速度传感器32输出的信号来求出该检测轴的方向的加速度。

另外，加速度传感器32具有加速度的检测轴（以下，还简称为“检测轴”）321，构成为检测该检测轴321的方向的加速度。而且，加速度传感器32即加速度传感器单元72设置为加速度传感器32的检测轴321与正交于第三转动轴O3的轴147平行，并且与正交于第三臂部14的中心轴145的轴146平行。此外，加速度传感器单元72也可以设置为加速度传感器32的检测轴321与正交于第三转动轴O3的轴147平行，并且与第三臂部14的中心轴145正交。另外，加速度传感器32的检测轴321也可以不与第三臂部14的中心轴145正交，另外也可以不与正交于第三臂部14的中心轴145的轴146平行。

另外，如图7以及图8所示，第二壳体721在其4个角部具有安装于第三臂部14的安装部7211。在各安装部7211分别形成有供阳螺纹件81插入的孔7212。

另外，如图7所示，第三臂部14具有臂部侧安装部141，该臂部侧安装部141与臂部主体2c一体形成，并安装有加速度传感器单元72（第二壳体721）。臂部侧安装部141呈与第二壳体721对应的形状。即，臂部侧安装部141呈板状，俯视的形状呈四边形，在本实施方式中，呈长方形。另外，在臂部侧安装部141的各角部分别形成有供阳螺纹件81旋合的阴螺纹部。

在将加速度传感器单元72安装于第三臂部14时，将4个阳螺纹件81分别插入第二壳体721的孔7212，使之旋合于第三臂部14的臂部侧安装部141的前端部的阴螺纹部。由此，利用各阳螺纹件81将第二壳体721的4个安装部7211分别固定于第二臂部14的臂部侧安装部141。即，将加速度传感器单元72安装于第二臂部14的臂部侧安装部141。在该情况下，在臂部侧安装部141与加速度传感器单元72之间未夹设任何部件，即加速度传感器单元72直接安装于臂部侧安装部141。由此，能够将加速度传感器单元72可靠地安装于第三臂部14，另外加速度传感器单元72能够可靠地与第三臂部14一体地转动。

另外，阳螺纹件81在插入第二壳体721的孔7212并旋合于臂部侧安装部141的阴螺纹部时，电连接于电路基板722的配线，该电路基板722电连接于电路部723的接地用的端子，另外阳螺纹件81的前端部电连接于臂部侧安装部141。由此，电路部723的接地用端子经由配线以及阳螺纹件81电连接于第二臂部14的臂部主体2c，从而接地。由此，能够减少接地所需要的部件件数，从而能够简化结构。

接下来，参照图4、图12～图16对机械手控制装置20的结构进行说明。

机械手控制装置20具有：接收部，其对从角速度传感器31输出的第一信号、从加速度传感器32输出的第二信号以及从位置传感器411～416输出的各信号分别进行接收；运算部，其基于由上述接收部接收的第一信号以及第二信号来求出第一臂部12的角速度的振动分量以及第三臂部14的角速度的振动分量；以及控制部，其基于由上述运算部求出的第一臂部12的角速度的振动分量以及第三臂部14的角速度的振动分量来控制机械手1的动作。

具体而言，如图4、图12～图16所示，机械手控制装置20具有：上述接收部；第一驱动源控制部201，其控制第一驱动源401的动作；第二驱动源控制部202，其控制第二驱动源402的动作；第三驱动源控制部203，其控制第三驱动源403的动作；第四驱动源控制部204，其控制第四驱动源404的动作；第五驱动源控制部205，其控制第五驱动源405的动作；以及第六驱动源控制部206，其控制第六驱动源406的动作。

此外，上述运算部由第一驱动源控制部201的后述的角速度计算部561、减法器571、第二驱动源控制部202的后述的角速度计算部562、加减法器622以及第三驱动源控制部203的后述的角速度计算部563构成。

如图12所示，第一驱动源控制部201具有减法器511、位置控制部521、减法器531、角速度控制部541、转动角度计算部551、角速度计算部561、减法器571、转换部581、修正值计算部591以及加法器601。

如图13所示，第二驱动源控制部202具有减法器512、位置控制部522、减法器532、角速度控制部542、转动角度计算部552、角速度计算部562、632、加减法器622、转换部582、修正值计算部592以及加法器602。

如图13所示，第三驱动源控制部203具有减法器513、位置控制部523、减法器533、角速度控制部543、转动角度计算部553以及角速度计算部563。

如图14所示，第四驱动源控制部204具有减法器514、位置控制部524、减法器534、角速度控制部544、转动角度计算部554以及角速度计算部564。

如图15所示，第五驱动源控制部205具有减法器515、位置控制部525、减法器535、角速度控制部545、转动角度计算部555以及角速度计算部565。

如图16所示，第六驱动源控制部206具有减法器516、位置控制部526、减法器536、角速度控制部546、转动角度计算部556以及角速度计算部566。

在此，机械手控制装置20基于机械手1进行的处理的内容来对腕16的目标位置进行运算，生成用于使腕16移动至该目标位置的轨道。而且，为使腕16沿上述生成的轨道移动，机械手控制装置20按规定的控制周期测定各驱动源401～406的转动角度，并将基于该测定结果运算出的值分别作为各驱动源401～406的位置指令Pc而输出至驱动源控制部201～206（参照图12～图16）。此外，上述以及以下，虽然记作“输入、输出值”但这是“输入、输出与该值对应的信号”的意思。

如图12所示，对第一驱动源控制部201，除第一驱动源401的位置指令Pc之外，还从第一位置传感器411、角速度传感器31分别输入检测信号。第一驱动源控制部201以使根据第一位置传感器411的检测信号计算的第一驱动源的转动角度（位置反馈值Pfb）成为位置指令Pc，并且后述的角速度反馈值ωfb成为后述的角速度指令ωc的方式通过使用了各检测信号的反馈控制来驱动第一驱动源401。

即，对第一驱动源控制部201的减法器511输入位置指令Pc，另外从转动角度计算部551输入后述的位置反馈值Pfb。在转动角度计算部551中，计数从第一位置传感器411输入的脉冲数，并将与该计数值对应的第一驱动源401的转动角度作为位置反馈值Pfb输出至减法器511。减法器511将上述位置指令Pc与位置反馈值Pfb之间的偏差（从第一驱动源401的转动角度的目标值减去位置反馈值Pfb得到的值）输出至位置控制部521。

位置控制部521通过进行使用从减法器511输入的偏差和预先设定的系数即比例增益等的规定的运算处理来对与该偏差对应的第一驱动源401的角速度的目标值进行运算。位置控制部521将表示该第一驱动源401的角速度的目标值（指令值）的信号作为角速度指令ωc输出至减法器531。此外，在此，在本实施方式中，作为反馈控制而进行比例控制（P控制），但并不局限于此。

对减法器531输入角速度指令ωc，另外输入后述的角速度反馈值ωfb。减法器531将上述角速度指令ωc与角速度反馈值ωfb之间的偏差（从第一驱动源401的角速度的目标值减去角速度反馈值ωfb得到的值）输出至角速度控制部541。

角速度控制部541通过使用从减法器531输入的偏差和预先设定的系数即比例增益、积分增益等，进行包括积分在内的规定的运算处理，来生成与该偏差对应的第一驱动源401的驱动信号（驱动电流），并将其经由马达驱动器301供给至马达401M。此外，在此，在本实施方式中，作为反馈控制而进行PI控制，但并不局限于此。

如此，为使位置反馈值Pfb尽可能地与位置指令Pc相等，并且使角速度反馈值ωfb尽可能地与角速度指令ωc相等，进行反馈控制，控制第一驱动源401的驱动电流。

接下来，对第一驱动源控制部201中的角速度反馈值ωfb进行说明。

在角速度计算部561中，基于从第一位置传感器411输入的脉冲信号的频率来计算第一驱动源401的角速度ωm1，将该角速度ωm1输出至加法器601。

另外，在角速度计算部561中，基于从第一位置传感器411输入的脉冲信号的频率来计算第一臂部12绕第一转动轴O1的角速度ωA1m，将该角速度ωA1m输出至减法器571。此外，角速度ωA1m是将角速度ωm1除以第一驱动源401的马达401M与第一臂部12之间即关节171的减速比得到的值。

另外，利用角速度传感器31来检测第一臂部12绕第一转动轴O1的角速度。而且，将该角速度传感器31的检测信号即由角速度传感器31检测出的第一臂部12绕第一转动轴O1的角速度ωA1输出至减法器571。

对减法器571输入角速度ωA1以及角速度ωA1m，减法器571将从该角速度ωA1减去角速度ωA1m得到的值ωA1s（＝ωA1－ωA1m）输出至转换部581。该值ωA1s相当于第一臂部12绕第一转动轴O1的角速度的振动分量（振动角速度）。以下，将ωA1s称为振动角速度。在本实施方式中，进行将该振动角速度ωA1s（具体而言，是基于振动角速度ωA1s生成的值即马达401M的角速度ωm1s）放大后述的增益Ka倍并返回驱动源401的输入侧的反馈控制。具体而言，为使振动角速度ωA1s尽可能地为0，对驱动源401进行反馈控制。由此，能够抑制机械手1的振动。此外，在该反馈控制中，控制驱动源401的角速度。

转换部581将振动角速度ωA1s转换为第一驱动源401的角速度ωm1s，然后将该角速度ωm1s输出至修正值计算部591。该转换能够通过对振动角速度ωA1s乘以第一驱动源401的马达401M与第一臂部12之间即关节171的减速比来实现。

修正值计算部591对角速度ωm1s乘以预先设定的系数即增益（反馈增益）Ka来求出修正值Ka·ωm1s，并将该修正值Ka·ωm1s输出至加法器601。

对加法器601输入角速度ωm1，另外输入修正值Ka·ωm1s。加法器601将角速度ωm1与修正值Ka·ωm1s的加法值作为角速度反馈值ωfb输出至减法器531。此外，以下的动作与上述相同。

如图13所示，对第二驱动源控制部202，除第二驱动源402的位置指令Pc之外，还从第二位置传感器412、加速度传感器32分别输入检测信号。另外，对第二驱动源控制部202，还从第三驱动源控制部203输入臂部15绕第三回动轴O3的角速度ωA3m。第二驱动源控制部202以使根据第二位置传感器412的检测信号计算出的第二驱动源402的转动角度（位置反馈值Pfb）成为位置指令Pc，并且使后述的角速度反馈值ωfb成为后述的角速度指令ωc的方式通过使用了各检测信号的反馈控制来驱动第二驱动源402。

即，对第二驱动源控制部202的减法器512输入位置指令Pc，另外从转动角度计算部552输入后述的位置反馈值Pfb。在转动角度计算部552中，计数从第二位置传感器412输入的脉冲数，并将与该计数值对应的第二驱动源402的转动角度作为位置反馈值Pfb输出至减法器512。减法器512将上述位置指令Pc与位置反馈值Pfb之间的偏差（从第二驱动源402的转动角度的目标值减去位置反馈值Pfb得到的值）输出至位置控制部522。

位置控制部522通过进行使用从减法器512输入的偏差和预先设定的系数即比例增益等的规定的运算处理来对与该偏差对应的第二驱动源402的角速度的目标值进行运算。位置控制部522将表示该第二驱动源402的角速度的目标值（指令值）的信号作为角速度指令ωc输出至减法器532。此外，在此，在本实施方式中，作为反馈控制而进行比例控制（P控制），但并不局限于此。

对减法器532输入角速度指令ωc，另外，输入后述的角速度反馈值ωfb。减法器532将上述角速度指令ωc与角速度反馈值ωfb之间的偏差（从第二驱动源402的角速度的目标值减去角速度反馈值ωfb得到的值）输出至角速度控制部542。

角速度控制部542通过使用从减法器532输入的偏差和预先设定的系数即比例增益、积分增益等，进行包括积分在内的规定的运算处理，来生成与该偏差对应的第二驱动源402的驱动信号（驱动电流），并将其经由马达驱动器302供给至马达402M。此外，在此，在本实施方式中，作为反馈控制而进行PI控制，但并不局限于此。

如此，以使位置反馈值Pfb尽可能地与位置指令Pc相等，并且使角速度反馈值ωfb尽可能地与角速度指令ωc相等的方式，进行反馈控制，控制第二驱动源402的驱动电流。此外，第二转动轴O2由于与第一转动轴O1正交，所以不会受到第一臂部12的动作、振动的影响，从而能够相对于第一驱动源401独立地控制第二驱动源402的动作。

接下来，对第二驱动源控制部202的角速度反馈值ωfb进行说明。

在角速度计算部562中，基于从第二位置传感器412输入的脉冲信号的频率来计算第二驱动源402的角速度ωm2，并将该角速度ωm2输出至加法器602。

另外，在角速度计算部562中，基于从第二位置传感器412输入的脉冲信号的频率来计算第二臂部13绕第二转动轴O2的角速度ωA2m，并将该角速度ωA2m输出至加减法器622。此外，角速度ωA2m是将角速度ωm2除以第二驱动源402的马达402M与第二臂部13之间即关节172的减速比得到的值。

另外，在第三驱动源控制部203的角速度计算部563中，基于从第三位置传感器413输入的脉冲信号的频率来计算第三臂部14绕第三转动轴O3的角速度ωA3m，并将该角速度ωA3m输出至加减法器622。此外，角速度ωA3m是将角速度ωm3除以第三驱动源403的马达403M与第三臂部14之间即关节173的减速比得到的值。

另外，利用加速度传感器32来检测第三臂部14的加速度。而且，将该加速度传感器32的检测信号即由加速度传感器32检测的第三臂部14的加速度输出至第二驱动源控制部202的角速度计算部632。在角速度计算部632中，基于由加速度传感器32检测出的加速度来求出第三臂部14的角速度。即，角速度计算部632对由加速度传感器32检测出的加速度进行积分等来求出第三臂部14绕第二转动轴O2的角速度ωA3，并将该角速度ωA3输出至加减法器622。此外，第二转动轴O2、第三转动轴O3由于与第一转动轴O1正交，所以不会受到第一臂部12的动作、振动的影响，从而能够容易且可靠地求出第三臂部14绕第二转动轴O2的角速度。

对加减法器622输入角速度ωA3、角速度ωA2m以及角速度ωA3m，加减法器622将从角速度ωA3减去角速度ωA2m以及角速度ωA3m得到的值ωA2s（＝ωA3－ωA2m－ωA3m）输出至转换部582。该值ωA2s相当于第二臂部13与第三臂部14绕第二转动轴O2的总的角速度的振动分量（振动角速度）。以下，将ωA2s称为振动角速度。在本实施方式中，进行将该振动角速度ωA2s（具体而言，是基于振动角速度ωA2s生成的值即马达402M的角速度ωm2s）放大后述的增益Ka倍并返回第二驱动源402的输入侧的反馈控制。具体而言，为使振动角速度ωA2s尽可能地为0，对第二驱动源402进行反馈控制。由此，能够抑制机械手1的振动。此外，在该反馈控制中，控制第二驱动源402的角速度。

转换部582将振动角速度ωA2s转换为第二驱动源402的角速度ωm2s，并将该角速度ωm2s输出至修正值计算部592。该转换能够通过对振动角速度ωA2s乘以第二驱动源402的马达402M与第二臂部13之间即关节172的减速比来实现。

修正值计算部592对角速度ωm2s乘以预先设定的系数即增益（反馈增益）Ka来求出修正值Ka·ωm2s，并将该修正值Ka·ωm2s输出至加法器602。此外，该第二驱动源控制部202的增益Ka与第一驱动源控制部201的增益Ka可以相同，另外也可以不同。

对加法器602输入角速度ωm2，另外输入修正值Ka·ωm2s。加法器602将角速度ωm2与修正值Ka·ωm2s的加法值作为角速度反馈值ωfb输出至减法器532。此外，以下的动作如上述那样。

如图13所示，对第三驱动源控制部203，除第三驱动源403的位置指令Pc之外，还从第三位置传感器413输入检测信号。第三驱动源控制部203以使根据第三位置传感器413的检测信号计算出的第三驱动源403的转动角度（位置反馈值Pfb）成为位置指令Pc，并且使后述的角速度反馈值ωfb成为后述的角速度指令ωc的方式通过使用了各检测信号的反馈控制来驱动第三驱动源403。

即，对第三驱动源控制部203的减法器513输入位置指令Pc，另外从转动角度计算部553输入后述的位置反馈值Pfb。在转动角度计算部553中，计数从第三位置传感器413输入的脉冲数，并将与该计数值对应的第三驱动源403的转动角度作为位置反馈值Pfb输出至减法器513。减法器513将上述位置指令Pc与位置反馈值Pfb之间的偏差（从第三驱动源403的转动角度的目标值减去位置反馈值Pfb得到的值）输出至位置控制部523。

位置控制部523通过进行使用从减法器512输入的偏差和预先设定的系数即比例增益等的规定的运算处理来对与该偏差对应的第三驱动源403的角速度的目标值进行运算。位置控制部522将表示该第三驱动源403的角速度的目标值（指令值）的信号作为角速度指令ωc输出至减法器533。此外，在此，在本实施方式中，作为反馈控制而进行比例控制（P控制），但并不局限于此。

另外，在角速度计算部563中，基于从第三位置传感器413输入的脉冲信号的频率来计算第三驱动源403的角速度，并将该角速度作为角速度反馈值ωfb输入至减法器533。

对减法器533输入角速度指令ωc，另外输入角速度反馈值ωfb。减法器533将上述角速度指令ωc与角速度反馈值ωfb之间的偏差（从第三驱动源403的角速度的目标值减去角速度反馈值ωfb得到的值）输出至角速度控制部543。

角速度控制部543通过使用从减法器533输入的偏差和预先设定的系数即比例增益、积分增益等，进行包括积分在内的规定的运算处理，来生成与该偏差对应的第三驱动源403的驱动信号（驱动电流），并将其经由马达驱动器303供给至马达403M。此外，在此，在本实施方式中，作为反馈控制作为而进行PI控制，但并不局限于此。

如此，以使位置反馈值Pfb尽可能地与位置指令Pc相等，并且使角速度反馈值ωfb尽可能地与角速度指令ωc相等的方式，进行反馈控制，控制第三驱动源403的驱动电流。

此外，对于驱动源控制部204～206，由于分别与上述第三驱动源控制部203相同，所以省略其说明。

如上说明，在该机械手1以及机械手系统10中，能够利用角速度传感器31来检测第一臂部12的角速度，另外第三转动轴O3由于与第二转动轴O2平行，所以能够利用加速度传感器32包括第二臂部13的转动量（加速度）在内地对第三臂部14的加速度进行检测而导出其角速度。而且，能够基于上述检测结果抑制振动。

另外，即便机械手1的姿势发生变化，角速度传感器31的检测轴311相对于第一转动轴O1的姿势也是一定的。因此，不需要对由角速度传感器31检测出的第一臂部12的角速度进行与角速度传感器31的姿势对应的修正。

另外，第三转动轴O3以及第二转动轴O2由于与第一转动轴O1正交或与正交于第一转动轴O1的轴平行，所以能够对包括第二臂部13的转动量在内的第三臂部14的加速度进行检测而不混入第一臂部的加速度分量。另外，即便机械手1的姿势发生变化，例如第一臂部12、第二臂部13、第三臂部14转动，也不需要对由加速度传感器32检测出的第三臂部14的加速度进行与加速度传感器32的姿势对应的修正。

由此，不需要复杂且庞大的运算，由此，难以产生运算误差，从而能够可靠地抑制振动，另外，还能够提高机械手1控制中的响应速度。

另外，利用加速度传感器32，不是对第二臂部13的加速度，而是包括第二臂部13的转动量（加速度）在内地对第三臂部14的加速度进行检测而导出角速度，所以能够更可靠地抑制机械手1的振动。

另外，与还在第二臂部13设置角速度传感器、加速度传感器的情况相比，能够减少传感器的个数，从而能够降低成本，另外，能够简化结构。

另外，在抑制机械手1的振动的控制中，不是控制使第三臂部14转动的第三驱动源403，而是控制使比该第三臂部14更靠基端侧的第二臂部13转动的第二驱动源402的动作，由此提高抑制机械手1的振动的效果。

另外，由于设置于第一臂部12的惯性传感器不是加速度传感器，而是角速度传感器31，所以难以受到噪声的影响，从而能够更可靠地检测角速度。其理由是：第一臂部12的惯性传感器的设置位置在第一转动轴O1的附近，因此，若是加速度传感器的情况下，则输出变小，噪声的影响变大。

第二实施方式

以下，对第二实施方式以与上述的第一实施方式的不同点为中心进行说明，对于相同的事项，省略其说明。

在第二实施方式的机械手1中，能够将由加速度传感器32检测出的包括第二臂部13的转动量（加速度）在内的第三臂部14的加速度直接用作角速度。由此，能够进一步简化运算处理。

在此，“将加速度直接用作角速度”不是指对由加速度传感器32检测出的加速度进行积分等运算来求出角速度的情况，而是指将检测出的加速度作为角速度处理的情况。但不是排除设置比例系数等的情况。

此外，将由加速度传感器32检测出的加速直接用作角速度这在第三臂部14的速度比较小时、第二臂部13的长度比较短时，特别有效。其理由是：在第三臂部14的速度比较小时、第二臂部13的长度比较短时，由加速度传感器32检测出的加速度与角速度大致相等。

根据该机械手1，能够得到与上述的第一实施方式相同的效果。

以上，基于图示的实施方式对本发明的机械手、机械手控制装置以及机械手系统进行了说明，但并不局限于本发明，各部分的结构能够替换为具有相同的功能的任意的结构。另外，也可以对本发明附加其它的任意的构成件。

此外，作为各驱动源的马达，分别除了上述伺服马达之外，例如能够举例步进电机等。另外，在作为马达而使用步进电机的情况下，作为位置传感器，例如使用通过计测向步进电机输入的驱动脉冲的个数来检测马达的转动角度的位置传感器。

另外，各位置传感器、角速度传感器、加速度传感器的方式分别未特别限定，例如，能够举例光学式、磁式、电磁式、电式等。

另外，在上述实施方式中，形成为基于加速度传感器的检测结果对使第二臂部转动的第二驱动源的动作进行控制，但并不局限于此，例如，也可以基于加速度传感器的检测结果对使第三臂部转动的第三驱动源的动作进行控制。

另外，在上述实施方式中，机械手的转动轴的个数是6个，但在本发明中并不局限于此，机械手的转动轴的个数也可以是3个、4个、5个或7个以上。

即，在上述实施方式中，由于腕具有两个臂部，所以机械手的臂部的个数是6个，但在本发明中，并不局限于此，机械手的臂部的个数也可以是3个、4个、5个或7个以上。

另外，在上述实施方式中，机械手是具有一个将多个臂部能够转动地连结而构成的臂部连结体的单臂部机械手，但在本发明中，并不局限于此，例如，可以是具有两个将多个臂部能够转动地连结而构成的臂部连结体的双臂部机械手等，也可以是具有多个上述臂部连结体的机械手。

附图标记说明：

1…机械手（产业用机械手）；10…机械手系统；11…基台；12、13、14、15…臂部（连杆）；145…中心轴；146、147…轴；16…腕（连杆）；161…腕主体；162…支承环；163…前端面；171、172、173、174、175、176…关节（接头）；2、2a、2b、2c、2d…臂部主体；3、3a、3b、3c、3d…驱动机构；31…角速度传感器；311…检测轴；32…加速度传感器；321…检测轴；33…陀螺元件；331…基部；332a、332b…检测用振动臂；333a、333b…连结臂部；334a、334b、334c、334d…驱动用振动臂；34…压电振动片；341、342…振动臂；343…基部；344、345…引出电极；35…振荡器；4、4a、4b、4c、4d…密封机构；20…机械手控制装置；201、202、203、204、205、206…驱动源控制部；301、302、303、304、305、306…马达驱动器；401、402、403、404、405、406…驱动源；401M、402M、403M、404M、405M、406M…马达；411、412、413、414、415、416…位置传感器；511、512、513、514、515、516…减法器；521、522、523、524、525、526…位置控制部；531、532、533、534、535、536…减法器；541、542、543、544、545、546…角速度控制部；551、552、553、554、555、556…转动角度计算部；561、562、563、564、565、566、632…角速度计算部；571…减法器；581、582…转换部；591、592…修正值计算部；601、602…加法器；622…加减法器；45…减速器；71…角速度传感器传感器单元；72…加速度传感器传感器单元；711、721…壳体；7111、7211…安装部；712、722…电路基板；713、723…电路部；7112、7212…孔；81…阳螺纹件；85…电缆；91、92…带轮；93…带；101…地面；111…螺栓；112…基台主体；113…圆筒状部；114…箱状部；121、141臂部侧安装部；122…阴螺纹部；241a、241b、242a、242b、244a、244b…舌片部；251、252、254…根部；G…重心；O1、O2、O3、O4、O5、O6…转动轴。

Claims (15)

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

基台；

第一臂部，其按照能够以第一转动轴为转动中心转动的方式连结于所述基台；

第二臂部，其按照能够以第二转动轴为转动中心转动的方式连结于所述第一臂部，该第二转动轴是与所述第一转动轴正交的轴或与正交于所述第一转动轴的轴平行的轴；

第三臂部，其按照能够以第三转动轴为转动中心转动的方式连结于所述第二臂部，该第三转动轴是与所述第二转动轴平行的轴；

角速度传感器，其设置于所述第一臂部；以及

加速度传感器，其设置于所述第三臂部，并且加速度的检测轴与正交于所述第三转动轴的轴平行。

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

所述加速度传感器的所述加速度的检测轴与所述第三臂部的中心轴正交或与正交于所述第三臂部的中心轴的轴平行。

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

所述角速度传感器的角速度的检测轴与所述第一转动轴平行或与所述第一转动轴一致。

4.权利要求1～3中任一项所述的机械手，其特征在于，具备：

角速度传感器单元，其具有第一壳体、所述角速度传感器以及电路部，所述角速度传感器以及所述电路部被设置在所述第一壳体内，所述电路部对从以及所述角速度传感器输出的信号进行AD转换并进行发送；以及

加速度传感器单元，其具有第二壳体、所述加速度传感器以及电路部，所述加速度传感器以及所述电路部被设置于所述第二壳体内，所述电路部对从以及所述加速度传感器输出的信号进行AD转换并进行发送，

所述角速度传感器单元被设置于所述第一臂部，所述加速度传感器单元设置于所述第三臂部。

5.根据权利要求4所述的机械手，其特征在于，

所述第一壳体的外形是长方体，

所述角速度传感器的角速度的检测轴与所述第一壳体的所述长方体的最大面的法线一致。

6.根据权利要求4或5所述的机械手，其特征在于，

所述第一壳体在所述第一壳体的角部具有被安装于所述第一臂部的安装部，

所述第二壳体在所述第二壳体的角部具有被安装于所述第三臂部的安装部。

7.根据权利要求6所述的机械手，其特征在于，

具备具有导电性并将所述第一壳体的所述安装部固定于所述第一臂部的第一固定部件，通过所述第一固定部件将所述角速度传感器单元的所述电路部接地连接于所述第一臂部，

具备具有导电性并将所述第二壳体的所述安装部固定于所述第三臂部的第二固定部件，通过所述第二固定部件将所述加速度传感器单元的所述电路部接地连接于所述第三臂部。

8.根据权利要求4～7中任一项所述的机械手，其特征在于，

所述第一臂部具有框体和与所述框体一体形成的臂部侧安装部，

所述角速度传感器单元被直接安装于所述臂部侧安装部。

9.根据权利要求4～8中任一项所述的机械手，其特征在于，

所述第三臂部具有框体和与所述框体一体形成的臂部侧安装部，

所述加速度传感器单元被直接安装于所述臂部侧安装部。

10.根据权利要求1～9中任一项所述的机械手，其特征在于，

具有电缆，所述电缆被配设于所述第一臂部，对该机械手供给电力，

所述角速度传感器被配置于所述第一臂部的与所述电缆相反一侧的端部。

11.根据权利要求1～10中任一项所述的机械手，其特征在于，

具有电缆，所述电缆被配设于所述第三臂部，对该机械手供给电力，

所述加速度传感器被配置于所述第三臂部的与所述电缆相反一侧的端部。

12.根据权利要求1～11中任一项所述的机械手，其特征在于，具备：

第四臂部，其按照能够以第四转动轴为转动中心转动的方式连结于所述第三臂部，该第四转动轴是与所述第三转动轴正交的轴或与正交于所述第三转动轴的轴平行的轴；

第五臂部，其按照能够以第五转动轴为转动中心转动的方式连结于所述第四臂部，该第五转动轴是与所述第四转动轴正交的轴或与正交于所述第四转动轴的轴平行的轴；以及

第六臂部，其按照能够以第六转动轴为转动中心转动的方式连结于所述第五臂部，该第六转动轴是与所述第五转动轴正交的轴或与正交于所述第五转动轴的轴平行的轴。

13.根据权利要求1～12中任一项所述的机械手，其特征在于，

所述第一转动轴与所述基台的设置面的法线一致。

14.一种机械手控制装置，其特征在于，

该机械手控制装置控制机械手的动作，所述机械手具备：基台；第一臂部，其按照能够以第一转动轴为转动中心转动的方式连结于所述基台；第二臂部，其按照能够以第二转动轴为转动中心转动的方式连结于所述第一臂部，该第二转动轴是与所述第一转动轴正交的轴或与正交于所述第一转动轴的轴平行的轴；以及第三臂部，其按照能够以第三转动轴为转动中心转动的方式连结于所述第二臂部，该第三转动轴是与所述第二转动轴平行的轴，

所述机械手控制装置具备：

接收部，其接收第一信号和第二信号，所述第一信号是从设置于所述第一臂部的角速度传感器输出的信号，所述第二信号是从设置于所述第三臂部且加速度的检测轴与正交于所述第三转动轴的轴平行的加速度传感器输出的信号；

运算部，其基于由所述接收部接收到的所述第一信号以及所述第二信号来求出所述第一臂部的角速度的振动分量以及所述第三臂部的角速度的振动分量；以及

控制部，其基于由所述运算部求出的所述第一臂部的角速度的振动分量以及所述第三臂部的角速度的振动分量来控制所述机械手的动作。

15.一种机械手系统，其特征在于，具备：

权利要求1～13中任一项所述的机械手；以及

机械手控制装置，其控制所述机械手的动作。