CN103213134B - 机械手的控制方法和机械手 - Google Patents

Abstract

本发明涉及机械手的控制方法和机械手。机械手的控制方法包括：利用由陀螺仪传感器检测的角速度和根据由第1编码器以及第2编码器检测的信息得到的陀螺仪传感器坐标系下的角速度之差来计算臂的扭转角速度的步骤；利用扭转角速度的变化值来计算陀螺仪传感器的灵敏度误差的修正量的步骤；和利用灵敏度误差的修正量来修正陀螺仪传感器的灵敏度的步骤。

Description

机械手的控制方法和机械手

技术领域

本发明涉及机械手的控制方法和利用该机械手的控制方法进行动作的机械手。

背景技术

在使机械手动作时，利用角度传感器检测电机的旋转角度，并根据旋转角度计算角速度来控制动作。但是，在有些情况下，被电机驱动的传递机构、臂并不是刚体，在动作时会产生振动或扭转，无法进行准确的控制。

于是，提出了一种利用由配置于臂的前端部的惯性传感器检测出的加速度来计算臂的弯曲量，并将其与臂动作的角度目标值相加来抑制臂的振动的机械手（例如，参照专利文献1）。

另外，也存在如下技术，即通过惯性传感器检测臂前端部的动作，利用该惯性传感器的检测信号来驱动臂，由此即使臂发生了振动也能够实现高精度的定位（例如参照专利文献2）。

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

【专利文献2】日本特开平7-9374号公报

在上述的专利文献1和专利文献2中，在臂前端部配置惯性传感器，利用惯性传感器的检测值来控制臂的动作（包含位置）。但是，会预想到惯性传感器的灵敏度的精度会因惯性传感器的个体差、温度变化的影响、老化等而下降。由于这样的惯性传感器的精度下降，会存在因为无法进行按照目标的控制，所以机械手的动作精度下降这样的问题。

发明内容

本发明是为了解决上述问题的至少一部分而完成的，能够实现为以下的方式或应用例。

［应用例1］本应用例涉及的机械手的控制方法是具备电机、检测所述电机的旋转角度的角度传感器、与所述电机连结的臂、和安装于所述臂的惯性传感器的机械手的控制方法，该机械手的控制方法的特征在于，包括：利用由所述惯性传感器检测的角速度和根据由所述角度传感器检测的角度信息得到的惯性传感器坐标下的角速度之差来计算所述臂的扭转角速度的步骤；利用所述扭转角速度来计算所述惯性传感器的灵敏度的修正量的步骤；和利用所述修正量来修正所述惯性传感器的灵敏度的步骤。

这里，作为惯性传感器具有陀螺仪传感器，作为角度传感器具有编码器、旋转变压器等。

另外，惯性传感器坐标是将惯性传感器被安装的位置表示为基准的坐标系。另外，以后有时将惯性传感器坐标单纯地表示为传感器坐标。由于惯性传感器的输出值是传感器坐标系的输出值，所以也将角度传感器的角速度信息转换为传感器坐标系来计算扭转角速度。

根据本应用例，利用惯性传感器所检测的角速度和根据由角度传感器所检测的信息得到的惯性传感器坐标下的角速度之差来决定惯性传感器的灵敏度修正量。该角速度差是扭转角速度。利用这样计算出的灵敏度修正量来修正惯性传感器的灵敏度，并使机械手进行动作。由此，能够降低因惯性传感器的个体差、温度变化引起的对精度的影响、因老化引起的对精度降低的影响，从而提高机械手动作的精度。

［应用例2］优选在上述应用例涉及的机械手的控制方法中，计算所述修正量的步骤在所述机械手的动作中执行。

若在机械手静止时执行灵敏度误差的计算，则需要考虑灵敏度检测时的噪声分量被累积从而导致灵敏度误差与实际不同的情况。因此，通过仅在进行动作时计算灵敏度误差的修正量，能够抑制噪声的影响，从而得到高精度的灵敏度修正量。

另外，在惯性传感器不具有角速度的动作、角速度不具有一定的大小的动作的情况下判定为处于静止时。

［应用例3］优选在上述应用例涉及的机械手的控制方法中，在计算所述修正量的步骤中，以所述机械手的动作中的所给定的时间间隔计算扭转角速度，在动作结束后，根据以所述时间间隔计算出的所述扭转角速度的总和来计算所述灵敏度误差的修正量。

例如，当以600mS结束1个动作时，在惯性传感器中每2ms检测角速度，在1个动作结束后，计算每2ms检测出的扭转角速度的和作为灵敏度误差的修正量。由此，能够基本实时地取得扭转角速度的变化，如果按每1个动作计算修正量，则能够降低计算步骤的计算负荷。

另外，由于一边使机械手动作一边更新惯性传感器的灵敏度，所以在反复执行动作的情况下，具有能够按每1个动作提高精度的效果。

［应用例4］优选在上述应用例涉及的机械手的控制方法中，在计算所述修正量的步骤中，在检测到所述电机的旋转方向后，根据所述旋转方向来切换所述扭转角速度的值的正负。

在扭转角速度的计算中没有旋转方向的信息。于是，根据电机的旋转方向的正逆来将计算出的扭转角速度的值切换为正或者负。由此，能够进行配置了惯性传感器的臂的正逆旋转双方的灵敏度修正。

［应用例5］优选在上述应用例涉及的机械手的控制方法中，还具有所述惯性传感器的偏移去除步骤，计算所述扭转角速度的步骤在所述偏移去除步骤后执行。

由此，能够除去惯性传感器的偏移的影响，从而进行高精度的灵敏度误差的修正。

［应用例6］优选在上述应用例涉及的机械手的控制方法中，利用与所述机械手的动作速度的大小对应的所述惯性传感器的灵敏度的修正常数来计算所述修正量。

由此，能够在动作速度大时使修正常数变大来增大灵敏度修正量，在动作速度小时通过使修正常数变小来减小灵敏度修正量。另外，修正常数例如利用惯性传感器的灵敏度修正增益来计算。

［应用例7］本应用例涉及的机械手的特征在于，具备：电机；与所述电机连结而被驱动的臂；检测所述电机的旋转角度的角度传感器；安装于所述臂的惯性传感器；利用由所述惯性传感器检测的角速度和根据由所述角度传感器检测的信息得到的惯性传感器坐标下的角速度之差来计算所述臂的扭转角速度的扭转角速度计算部；计算所述惯性传感器的灵敏度修正量的灵敏度修正量计算部；利用所述灵敏度修正量计算部所计算的灵敏度修正量来修正灵敏度的灵敏度修正部；和判断所述臂的动作的动作判断部。

根据本应用例，利用由惯性传感器检测的角速度和根据由角度传感器检测的角度信息得到的惯性传感器坐标下的角速度之差来决定惯性传感器的灵敏度修正量。该角速度差是扭转角速度。利用这样算出的灵敏度修正量来修正惯性传感器的灵敏度，并使机械手动作。由此，能够排除因惯性传感器的个体差、温度变化引起的对精度的影响、因老化引起的对精度下降的影响，从而提高机械手动作的精度。

［应用例8］本应用例涉及的机械手的控制方法的特征在于，包括：利用由加速度传感器检测的加速度来计算传感器坐标系的速度的步骤；根据由速度传感器检测的传感器坐标系的速度和利用所述加速度计算的传感器坐标系的速度之差来计算歪斜速度的步骤；利用所述歪斜速度来计算所述加速度传感器的灵敏度修正量的步骤；和利用所述灵敏度修正量来修正所述加速度传感器的灵敏度的步骤。

相对于利用上述的扭转角速度来进行惯性传感器（例如陀螺仪传感器）的灵敏度修正，本应用例涉及的机械手的控制方法利用歪斜速度来进行加速度传感器的灵敏度修正，使机械手动作。由此，能够降低因加速度传感器的个体差、温度变化引起的对精度的影响、因老化引起的对精度降低的影响，从而提高机械手动作的精度。

［应用例9］优选在上述应用例涉及的机械手的控制方法中，计算所述修正量的步骤在所述机械手的动作中执行。

若在机械手静止时执行灵敏度误差的计算，则需要考虑灵敏度检测时的噪声分量被累积从而导致灵敏度误差与实际不同的情况。由此，通过仅在进行动作时计算灵敏度误差的修正量，能够抑制噪声的影响，从而得到高精度的灵敏度修正量。

另外，在加速度传感器不具有速度的动作、速度不具有一定的大小的动作的情况下判定为处于静止时。

［应用例10］优选在上述应用例涉及的机械手的控制方法中，在计算所述修正量的步骤中，以所述机械手的动作中的所给定的时间间隔计算歪斜速度，在动作结束后，根据以所述时间间隔计算出的歪斜速度的总和来计算所述灵敏度误差的修正量。

例如，在以600mS结束1个动作的情况下，在加速度传感器中每2ms检测加速度，在1个动作结束后，计算每2ms检测出的歪斜速度的总和作为灵敏度误差的修正量。由此，能够基本实时地取得歪斜速度的变化，如果按每1个动作计算修正量，则能够降低计算步骤的计算负荷。

另外，由于一边使机械手动作一边更新加速度传感器的灵敏度，所以在反复执行动作的情况下，具有能够按每1个动作提高精度这样的效果。

［应用例11］优选在上述应用例涉及的机械手的控制方法中，在计算所述修正量的步骤中，在检测出所述加速度传感器的安装位置的移动方向后，根据所述移动方向来切换所述歪斜速度的值的正负。

在歪斜速度的计算中没有移动方向的信息。于是，根据加速度传感器安装位置的移动方向的正逆来将计算出的歪斜速度的值切换为正或者负。由此，能够进行加速度传感器被配置的位置处的正逆移动双方的灵敏度修正。

［应用例12］优选在上述应用例涉及的机械手的控制方法中，还具有所述加速度传感器的偏移去除步骤，计算所述歪斜速度的步骤在所述偏移去除步骤后执行。

加速度传感器在初始状态下存在偏移误差。偏移误差是在静止时检测出加速度传感器的检测值为“0”以外的值时的误差，通过除去偏移误差，能够进行高精度的灵敏度误差的修正。

［应用例13］优选在上述应用例涉及的机械手的控制方法中，利用与所述机械手的动作速度的绝对值对应的所述加速度传感器的灵敏度的修正常数来计算所述修正量。

由此，能够在动作速度大时使修正常数变大来增大灵敏度修正量，在动作速度小时通过使修正常数变小来减小灵敏度修正量。

另外，修正常数例如利用加速度传感器的灵敏度修正增益来进行计算。

［应用例14］本应用例涉及的机械手的特征在于，具备：取得加速度传感器的检测值，来计算传感器坐标系下的速度的传感器坐标速度计算部；根据由速度传感器检测出的速度和所述传感器坐标系下的速度之差来计算歪斜速度的歪斜速度计算部；计算所述加速度传感器的灵敏度修正量的灵敏度修正量计算部；基于所述灵敏度修正量来修正所述加速度传感器的灵敏度的灵敏度计算部；和判断是处于动作还是处于静止中的动作判断部。

根据本应用例，利用歪斜速度来进行加速度传感器的灵敏度修正，使机械手动作。由此，能够降低因加速度传感器的个体差、温度变化引起的对精度的影响、因老化引起的对精度下降的影响，从而提高机械手动作的精度。

另外，本应用例的机械手能够适用于直动机械手和3轴/6轴机械手。

附图说明

图1是例示实施方式1的机械手的概略构成的构成说明图。

图2是表示实施方式1的控制装置的主要构成的构成说明图。

图3是表示事例1的灵敏度误差和扭转角速度的关系的数据。

图4是表示事例2的灵敏度误差和扭转角速度的关系的数据。

图5是表示事例3的灵敏度误差和扭转角速度的关系的数据。

图6是表示事例4的灵敏度误差和扭转角速度的关系的数据。

图7是表示实施方式1的机械手的控制方法的主要步骤的流程图。

图8是示意性表示实施方式1的6轴机械手的构成说明图。

图9是例示实施方式2的直动机械手的概略构成的构成说明图。

图10是表示实施方式2的控制装置的主要构成的构成说明图。

图11是表示实施方式2的传感器坐标速度计算部的构成的构成说明图。

图12是示意性表示实施方式2的6轴机械手的构成说明图。

具体实施方式

以下，参照附图说明本发明的实施方式。

（实施方式1）

（机械手的构成）

图1是例示实施方式1的机械手的概略构成的构成说明图。另外，图1是为了使各部件成为能够识别的大小而将各部件乃至部分的纵横的比例设为与实际不同的示意图。另外，在本实施方式中例示了3轴机械手（选择顺应性装配机器手（SCARARobot））。在图1中，机械手10被构成为以平板状的基台38为基础将多个电机和臂连结。在基台38上，配置有形成了空间的支持框39。

在支持框39的下方空间配置电机41，在电机41的轴方向端部配置有作为角度传感器的第1编码器42。第1编码器42检测电机41的旋转角度。

在支持框39的上方空间配置有第1减速器43，电机41的旋转轴41a与第1减速器43连结。在第1减速器43的上侧突出设置有输出轴43a，输出轴43a按以规定的减速比使电机41的转速减速后的转速进行旋转。第1减速器43能够采用各种减速机构，在本实施方式中，采用波动齿轮。输出轴43a贯穿于支持框39的孔部39a而突出设置于支持框39的上方。

突出设置的输出轴43a与臂44的一端部连结，臂44以输出轴43a为旋转轴在XY平面内转动。在臂44的另一端部重叠连结有第2减速器46、电机47，在电机47的旋转轴方向端部配置有作为角度传感器的第2编码器48。第2编码器48检测电机47的旋转角度。并且，第2减速器46的输出轴46a贯穿臂44的孔部44a而从臂44突出设置。第2减速器46的输出轴46a按以规定的减速比使电机47的转速减速后的转速进行旋转。

如果电机41和电机47的旋转方向能够利用电信号来控制，则可以使用各种电机。在本实施方式中，例如采用直流电机。第1编码器42能够检测电机41的旋转角度即可，第2编码器48能够检测电机47的旋转角度即可，因此能够使用各种旋转编码器。在本实施方式中，例如，采用光学式的旋转编码器。臂49的一端部与输出轴46a连结，将输出轴46a作为旋转轴在XY平面内转动。

在臂49的另一端部配置有作为惯性传感器的陀螺仪传感器50。陀螺仪传感器50的种类没有特别限定，在本实施方式中，采用振动型陀螺仪传感器。另外，对于陀螺仪传感器50而言，在本实施例中优选采用3轴陀螺仪传感器。

在臂49的陀螺仪传感器50侧配置有电机51，电机51的旋转轴51a贯穿于臂49的孔部49a而在基台38侧突出设置。升降装置52与旋转轴51a连结，被构成为能够通过旋转轴51a进行旋转。

在升降装置52的下前端部配置有手部53。升降装置52具有沿上下（Z轴方向）移动的直动机构，能够使手部53升降。手部53具有多个指部53a和直动机构（未图示），直动机构能够变更多个指部53a的间隔。并且，手部53能够在指部53a之间夹持保持作业对象的工件（未图示）。

机械手10具备控制上述各驱动系统的控制装置20。控制装置20具有基于电机41、47、51的驱动控制、第1编码器42、第2编码器48以及陀螺仪传感器50的检测接口、第1编码器42、第2编码器48以及陀螺仪传感器50的检测值来计算扭转角速度、灵敏度修正量的计算部等。

接着，对控制装置20的构成进行说明。

图2是表示控制装置20的主要构成的构成说明图。控制装置20经由接口（未图示）获得陀螺仪传感器50的角速度的检测值、基于第1编码器42的电机41的旋转角度检测值以及基于第2编码器48的电机47的旋转角度检测值，并具备：计算扭转角速度的扭转角速度计算部21、计算陀螺仪传感器50的灵敏度修正量的灵敏度修正量计算部22、基于计算出的灵敏度修正量来修正陀螺仪传感器50的灵敏度的灵敏度修正部24、和判断机械手10是在动作中还是在静止中的动作判断部23。

另外，在图2中，将第1编码器42和第2编码器48总称表示为编码器40。

（机械手的控制方法）

接着，对机械手10的控制方法进行说明。首先，例示计算式来说明机械手10的控制方法的见解。

这里，将陀螺仪传感器50检测出的角速度表示为陀螺仪输出，将根据第1编码器42和第2编码器48检测出的角度计算出的陀螺仪传感器50的配置位置处的角速度表示为编码器输出。陀螺仪输出和编码器输出的差为扭转角速度。另外，在“陀螺仪输出—编码器输出”中，包含动作的扭转角速度、陀螺仪传感器50的偏移误差、和陀螺仪传感器50自身的灵敏度误差。由于扭转角速度是振动分量，所以根据动作具有差异，但是若对1个动作进行平均则大致为“0”。

因此，若获取除去了偏移的影响的“陀螺仪输出—编码器输出”的和，则能够了解灵敏度误差的影响。如果对该灵敏度误差进行修正，则能够进行陀螺仪传感器50的灵敏度修正。另外，若将根据第1编码器42的信息计算出的电机41的角速度设为J1，将根据第2编码器48的信息计算出的电机47的角速度设为J2，则编码器输出是J1和J2之和，将“J1＋J2”表示为电机角速度来进行说明。

利用数学式来说明该内容。

陀螺仪角速度＝电机角速度（J1＋J2）＋扭转角速度……（1）

若对（1）式进行变形，则成为：

扭转角速度＝陀螺仪角速度—电机角速度（J1＋J2）……（2）

（2）式能够进行如下变形。

扭转角速度＝扭转角速度＋电机角速度（J1＋J2）—电机角速度（J1＋J2）…（3）

这里，计算出的扭转角速度能够按如下方式表示。

但是，陀螺仪灵敏度是将实际值标准化为1的值，（陀螺仪灵敏度-1）表示灵敏度误差。

计算出的扭转角速度＝陀螺仪角速度—电机角速度（J1＋J2）＝（扭转角速度＋电机角速度（J1＋J2））×陀螺仪灵敏度—电机角速度（J1＋J2）＝扭转角速度×陀螺仪灵敏度＋电机角速度（J1＋J2）×（陀螺仪灵敏度-1）……（4）

由于扭转角速度是振动分量，所以如果没有扭转分量，则1个动作中的平均值大致为“0”。因此，（3）式的第1项为“0”，仅剩余第2项。因此，如果按使得（陀螺仪灵敏度-1）接近“0”的方式进行修正，则能够得到正确的陀螺仪灵敏度的值。

接着，列举没有灵敏度误差时的事例和有灵敏度误差时的事例，并参照图3～图6，对灵敏度误差和扭转角速度的关系进行说明。另外，各图的（a）的实线表示根据来自编码器40的信息计算出的角速度（电机角速度（J1＋J2）），虚线表示陀螺仪传感器50检测出的角速度，各图的（b）表示计算出的扭转角速度，纵轴是角速度（deg／s），横轴是时间（sec）。另外，对于计算出的扭转角速度，利用曲线图来表示以所给定的时间间隔（在本实施例中各事例都设为2ms间隔）检测出的值。

另外，在各图中，都将从动作开始到大约0．13秒为止进行加速动作，之后大约0．13秒～0．2秒以恒定速度进行动作，大约0．2秒～0．3秒进行减速并静止的一系列动作设为1个动作。例如，将工件从A地点移动至B地点的动作设为1个动作。

（事例1）

图3是表示事例1的灵敏度误差和扭转角速度的关系的数据，表示灵敏度误差大约为“0”的情况。也就是说，如图3（a）所示那样由编码器40计算出的陀螺仪传感器坐标（有时表示为陀螺仪坐标）处的角速度和陀螺仪传感器50所检测出的角速度的值基本一致。此时，如图3（b）所示，利用上述（3）式计算出的扭转角速度的总和大约为“0”。

（事例2）

图4是表示事例2的灵敏度误差和扭转角速度的关系的数据，如图4（a）所示那样，示出由陀螺仪传感器50检测出的角速度相对于根据编码器40的检测值计算出的角速度大约存在＋10％的灵敏度误差。该灵敏度误差是扭转角速度，如图4（b）所示，利用上述（4）式计算出的扭转角速度的总和为正值，该值相当于＋10％的灵敏度误差。

（事例3）

图5是表示事例3的灵敏度误差和扭转角速度的关系的数据，表示灵敏度误差（扭转分量和灵敏度误差分量的和）为-10％的情况。如图5（b）所示，利用上述（4）式计算出的扭转角速度的总和为负值，如图5（a）所示，示出由陀螺仪传感器50检测出的角速度相对于根据编码器40的检测值计算出的角速度大约存在-10％的灵敏度误差。

（事例4）

图6是表示事例4的灵敏度误差和扭转角速度的关系的数据。上述事例2和事例3是进行正旋转的事例，相对于此，事例4是进行逆旋转的情况的事例，表示灵敏度误差（扭转分量和灵敏度误差分量的和）为＋10％的情况。如图6（b）所示，利用上述（4）式计算出的扭转角速度的总和为负值，如图6（a）所示，示出由陀螺仪传感器50检测出的角速度相对于根据编码器40的检测值计算出的角速度大约存在＋10％的灵敏度误差。

也就是说，如果对以上说明的灵敏度误差进行修正，则能够排除陀螺仪传感器50的因臂扭转引起的灵敏度误差。以下参照流程图说明利用了这样的见解的机械手的控制方法。

图7是表示机械手的控制方法的主要步骤的流程图。首先，进行陀螺仪传感器50的偏移去除（S10）。陀螺仪传感器50在初始状态下存在偏移误差。偏移误差是在静止时检测出陀螺仪传感器50的检测值为“0”以外的值时的误差，通过除去偏移误差来确保后续步骤的计算处理的准确性。接着，计算扭转角速度（S20）。对于扭转角速度而言，利用由陀螺仪传感器50检测出的角速度和电机角速度（J1＋J2）并根据上述的（2）式进行计算。

接着，判定机械手10是在动作中还是在静止中（S30）。这里，在陀螺仪传感器50、以及第1编码器42和第2编码器48这两方不具有角速度的状态、以及电机41的角速度J1和利用第2编码器48检测出的电机47的角速度J2向相反方向旋转相同角度这样的动作的情况下，判断为机械手10处于静止中。在判定为机械手10处于静止中的情况下，设定灵敏度修正量＝0（S35），并向计算修正常数的步骤（S60）转移。此时，由于灵敏度修正量＝0，所以不进行灵敏度修正。

在步骤S30中，在判断为机械手10处于动作中的情况下，判断相对于陀螺仪传感器坐标，陀螺仪传感器50的旋转是正（正旋转）还是负（逆旋转）（S40）。在逆旋转的情况下，对扭转角速度乘以（-1）（S45）。因此，在逆旋转的情况下，设定为灵敏度修正量＝扭转角速度×（-1）并转移到下一个计算修正常数的步骤（S60）。

在步骤S40中，在判定为陀螺仪传感器50是正旋转的情况下，设定为灵敏度修正量＝扭转角速度并转移到下一个计算修正常数的步骤（S60）。也就是说，在存在动作指令，且由陀螺仪传感器50和编码器40取得的角速度在一定的值以上的情况下判定为处于动作中。修正常数是对步骤S45或者步骤S50中计算出的灵敏度修正量乘以陀螺仪传感器50的灵敏度修正增益而计算出的值，在旋转角速度大时变大，在旋转角速度小时变小。

基于这样计算出的灵敏度修正量和修正常数来执行陀螺仪传感器50的灵敏度修正（S70）。并且，在判定为基于动作指令的动作结束时（是），停止上述的各种计算步骤。此时，在该一系列控制步骤中计算出的数据反馈至控制装置20，下一动作命令以基于这些计算并修正后的数据的灵敏度而被输出。并且，每当这样的1个动作结束，则一边使机械手10动作一边对陀螺仪传感器50的灵敏度进行更新。

当在步骤S80中基于动作指令的动作未结束（否）时，继续进行步骤S20以后的步骤直至动作结束。

另外，偏移去除步骤（S10）也可以在灵敏度修正量计算步骤（S50或者S45）中执行。

根据上述的机械手的控制方法，利用陀螺仪传感器50所检测的角速度、和根据编码器40所检测的信息（综合了第1编码器42的检测值和第2编码器48的检测值的信息）得到的陀螺仪传感器坐标中的角速度之差来决定陀螺仪传感器50的灵敏度修正量。该角速度差是扭转角速度。利用这样计算出的灵敏度修正量来修正陀螺仪传感器50的灵敏度，并使机械手10进行动作。由此，能够排除因陀螺仪传感器50的个体差、因温度变化而引起的对精度的影响、因老化而引起的对精度降低的影响，从而提高机械手动作的精度。

另外，由于一边使机械手10动作一边对陀螺仪传感器50的灵敏度进行更新，所以在反复执行动作的情况下，具有能够按每1个动作提高精度的效果。

另外，在实施方式1的机械手控制方法中，计算陀螺仪传感器50的灵敏度误差的修正量的步骤（S50或者S45）在机械手10的动作中执行。若在机械手10静止时执行灵敏度误差的计算，则需要考虑噪声分量被累积从而导致灵敏度误差成为与实际不同的值的情况。因此，通过仅在进行动作时计算灵敏度误差的修正量，能够抑制噪声的影响，从而得到高精度的灵敏度修正量。

在计算陀螺仪传感器50的灵敏度误差的修正量的步骤（S50或者S45）中，以机械手10的动作中的所给定的时间间隔计算扭转角速度，并在动作结束后综合以上述的时间间隔计算出的扭转角速度来计算出灵敏度误差的修正量。由此，能够大致实时地取得扭转角速度的变化，如果每当1个动作结束时计算修正量，则能够降低计算步骤的计算负荷。

另外，在计算陀螺仪传感器50的灵敏度误差的修正量的步骤（S50或者S45）中，在检测出陀螺仪传感器50的旋转方向后，与旋转方向对应地切换扭转角速度的值的正负。在扭转角速度的计算中不存在直接的旋转方向的信息。于是，根据陀螺仪传感器50的旋转方向将计算出的扭转角速度的值切换为正或者负。由此，能够进行陀螺仪传感器50的正逆旋转双方的灵敏度修正。

另外，计算扭转角速度的步骤（S20）在陀螺仪传感器50的偏移去除后执行，从而能够除去偏移的影响，进行高精度的灵敏度误差的修正。

陀螺仪传感器50的灵敏度误差的修正量使用与机械手10的动作速度的大小对应的修正常数而被计算。修正常数是对灵敏度修正量乘以灵敏度修正增益而得的值。因此，能够在动作速度大时使修正常数变大来使灵敏度修正量变大，在动作速度小时通过使修正常数变小来使灵敏度修正量变小。

利用了以上说明的机械手的控制方法的机械手10一边修正陀螺仪传感器50的灵敏度一边进行动作，因此能够排除因陀螺仪传感器50的个体差、温度变化引起的对精度的影响、因老化引起的精度低下、因扭转引起的对动作精度的影响，从而提高动作精度。

另外，虽然例示了上述的机械手10使用了3轴机械手（选择顺应性装配机器手）的情况进行了说明，但是在6轴机械手中也能够应用上述的机械手的控制方法。参照附图对该情况进行说明。

（6轴机械手）

图8是示意性表示6轴机械手100的构成说明图。6轴机械手100在具有XY平面的支持台11上具有与XY平面垂直配置的基部12，将基部12的中心轴（Z轴）作为基本坐标轴，由多个关节和连结关节间的臂构成。在此，图8所示的圆形和四边形表示关节，对关节标注的箭头表示旋转方向。基部12的端部固定于支持台11，关节J₁和关节J₂之间由臂13连结。

另外，关节J₂和关节J₃由臂14连结，关节J₃和J₄由臂15连结，关节J₄和关节J₅由臂16连结，关节J₅和关节J₆由臂17连结。并且，在关节J₆上连结有臂18，臂18的终端部（Endpoint）具有指部（未图示）。

此处省略了图示，在关节J₁～J₆处分别设置有电机、减速器、作为检测电机的旋转角度（即关节的旋转角度）的角度传感器的编码器。另外，在臂16上安装有作为惯性传感器的陀螺仪传感器50，检测陀螺仪传感器50的安装位置处的角速度。

另外，陀螺仪传感器50在本实施例中配设于臂16，但不限于该位置。但是，如果能够安装，则优选配设于与Endpoint接近的位置。其原因在于，顶端部的Endpoint处的振动比基部12附近大。

6轴机械手100具备进行各电机的驱动控制、基于编码器的角速度的计算、基于陀螺仪传感器50的角速度检测、上述的各种计算处理等的控制装置20。并且，6轴机械手100通过利用臂13～18、和关节J₁～J₆的电机以及减速器来进行相对运动，在Endpoint（例如指部）处进行规定的作业。

这里，对6轴机械手100的陀螺仪传感器50的角速度的计算方法进行说明。首先，利用陀螺仪传感器50的安装位置处的姿势信息和编码器角速度（关节角度）来计算陀螺仪传感器的安装位置姿势间雅可比矩阵。

【数式5】

$J_s=\frac{\∂p}{\∂\mathrm{\θ}}...\left(5\right)$

Js:关节角度/陀螺仪传感器的安装位置姿势间雅可比矩阵

p:陀螺仪传感器的安装位置处的姿势

θ:关节角度(编码器的信息)

接着，利用陀螺仪传感器50的安装位置姿势间雅可比矩阵Js和根据关节角度计算出的关节角速度来计算陀螺仪传感器的安装位置处的坐标系角速度。

【数式6】

$\stackrel{\·}{p}=J_s\stackrel{\·}{\mathrm{\θ}}...\left(6\right)$

陀螺仪传感器的安装位置处的坐标系角速度

根据关节角度计算出的关节角速度

通过基于数式5和数式6的计算，根据通过电机角速度（编码器的角速度）计算出的陀螺仪传感器50的安装位置处的坐标系角速度和陀螺仪传感器50的输出角速度的差值计算扭转角速度，执行与上述3轴的机械手10（选择顺应性装配机器手）的控制方法（参照图7）同样的灵敏度修正。也就是说，利用上述数式5和数式6进行图7的扭转角速度的计算（S20）即可。

由此，在6轴机械手100的情况下，也与3轴的机械手10同样，由于一边修正陀螺仪传感器50的灵敏度误差一边进行动作，所以能够排除因陀螺仪传感器50的个体差、温度变化引起的对精度的影响、因老化引起的精度下降、因扭转引起的对动作精度的影响，从而提高动作精度。

另外，也能够在上述的3轴机械手、6轴机械手中同样地进行控制。

（实施方式2）

接着，对实施方式2的机械手以及机械手的控制方法进行说明。在上述的实施方式1中，对使用陀螺仪传感器作为惯性传感器的情况进行了说明，但是实施方式2的特征在于使用加速度传感器。以直动机械手和6轴机械手为例进行说明。

首先，对直动机械手进行说明。

图9是例示实施方式2涉及的直动机械手200的概略构成的构成说明图。机械手200构成为具有：在X方向（图示Vx方向）延伸的固定轨210、安装于固定轨210并在X方向（沿着固定轨210的Vx方向）能够移动的X方向移动装置220、安装于X方向移动装置220并在Y方向（图示Vy方向）能够移动的Y方向移动装置230、安装于Y方向移动装置230并在Z方向（图示Vz方向）能够移动的Z方向移动装置240。Z方向移动装置240的顶端部是抓取工件的把持部。在该Z方向移动装置240上配置有作为惯性传感器的加速度传感器60。加速度传感器60是能够检测X方向、Y方向、Z方向各自的加速度、姿势的3轴加速度传感器。

在X方向移动装置220、Y方向移动装置230以及Z方向移动装置240上具有线性致动器，分别具备检测X方向、Y方向、Z方向的移动速度的检测器。作为检测器具备直动系统或者旋转系统检测用的编码器40（都省略图示）。

接着，对实施方式2涉及的控制装置120的构成进行说明。

图10是表示实施方式2涉及的控制装置120的主要构成的构成说明图。控制装置120取得加速度传感器60的检测值，并具备：计算速度和传感器坐标的传感器坐标速度计算部121；计算由编码器40检测出的速度和由传感器坐标速度计算部121计算出的速度之差（称为歪斜速度）的歪斜速度计算部122、计算加速度传感器60的灵敏度修正量的灵敏度修正量计算部123、基于计算出的灵敏度修正量来修正加速度传感器60的灵敏度的灵敏度计算部125、和判断机械手200是在动作中还是在静止中的动作判断部124。

接着，对传感器坐标速度计算部121的构成进行说明。

图11是表示传感器坐标速度计算部121的构成的构成说明图。传感器坐标速度计算部121由利用加速度传感器60的检测值来向机械手200的基准坐标进行转换的变为基准坐标的坐标转换部130、从加速度传感器60的检测值（加速度和移动方向）除去重力加速度的影响的重力加速度去除计算部131、将除去了重力加速度的加速度转换为速度的积分计算部132、和变为传感器坐标的坐标转换部133构成。这里，传感器坐标相当于上述的实施方式1所记载的陀螺仪传感器坐标。

接着，对歪斜速度进行说明。将根据编码器40的角度信息计算出的X方向、Y方向、z方向各自的直动速度设为Vmx、Vmy、Vmz，将由加速度传感器60检测出的X方向加速度设为ax，将Y方向加速度设为ay，将Z方向加速度设为az。若对从该加速度传感器60检测出的加速度除去重力加速度分量而得的值进行积分，则得到X方向速度Vsx、Y方向速度Vsy、Z方向速度Vsz。于是，根据由编码器40得到的速度（Vmx，Vmy，Vmz）和由加速度传感器60得到的速度（Vsx，Vsy，Vsz）之差计算歪斜速度。

接着，对实施方式2的机械手200的控制方法进行说明。

机械手200的控制方法具有：利用由加速度传感器60检测出的加速度来计算传感器坐标系的速度的步骤、根据由编码器40检测的传感器坐标系的速度和利用加速度计算的传感器坐标系的速度之差计算歪斜速度的步骤、利用歪斜速度来计算加速度传感器60的灵敏度修正量的步骤、和利用灵敏度修正量来修正加速度传感器60的灵敏度的步骤。在图7所示的表示控制方法的主要步骤的流程图的各步骤中，能够将旋转系统的“扭转角速度计算（S20）”置换成上述的计算方法中计算出的直动系统的“歪斜速度计算”来进行说明。另外，将“旋转方向的判定（S40）”置换成“移动方向的判定”即可。

接着，对6轴机械手进行说明。

图12是示意性表示实施方式2涉及的6轴机械手300的构成说明图。6轴机械手的构成相对于实施方式1中的6轴机械手100（参照图8），除了将陀螺仪传感器50和加速度传感器60置换以外，其他构成相同，控制装置和传感器坐标速度计算部的构成与上述的直动机械手相同，因此省略详细说明。

对6轴机械手300的速度的计算方法进行说明。首先，利用加速度传感器60的安装位置处的姿势信息和关节角度（基于编码器40的角度信息）来计算关节角度/安装位置姿势间雅可比矩阵。

【数式7】

$J_T=\frac{\∂p}{\∂\mathrm{\θ}}...\left(7\right)$

JT:关节角度/传感器安装位置姿势间雅可比矩阵

p:加速度传感器安装位置处的姿势

θ:关节角度(编码器的信息)

接着，利用关节角度/传感器安装位置姿势间雅可比矩阵J_T和根据关节角度计算出的关节角速度来计算加速度传感器60的安装位置处的坐标系加速度。

【数式8】

$\stackrel{\·}{p}=J_T\stackrel{\·}{\mathrm{\θ}}...\left(8\right)$

传感器安装位置处的坐标系加速度

根据关节角度计算出的关节加速度

将通过基于数式7和数式8的计算算出的传感器安装位置处的3轴的传感器坐标系加速度分别表示为ax，ay，az。并且通过对加速度进行积分，来计算传感器坐标系的速度。

另外，由于加速度传感器60的输出值是传感器坐标系的输出值，所以暂时转换成以机械手基部为基准的基准坐标系（机械手坐标系）的加速度。之后，除去重力加速度分量，进行积分来计算基准坐标系下的速度，然后转换成传感器坐标系下的速度。

然后，根据由编码器计算出的传感器坐标系下的速度和由加速度传感器计算出的传感器坐标系下的速度的差值来计算歪斜速度。

之后的灵敏度修正的流程与实施方式1（参照图7）同样地进行即可。也就是说，在各步骤中，能够将旋转系统的“扭转角速度”置换成上述的计算方法中算出的直动系统的“歪斜速度”来进行说明。

根据以上说明的实施方式2的机械手控制方法，利用加速度传感器60所检测的速度和根据编码器40所检测的信息得到的加速度传感器坐标下的速度之差来决定加速度传感器60的灵敏度修正量。该速度差是歪斜速度。利用这样计算出的灵敏度修正量来修正加速度传感器60的灵敏度，并使直动机械手200或者6轴机械手300动作。由此，能够排除因加速度传感器60的个体差、温度变换引起的对精度的影响、因老化引起的对精度降低的影响，从而提高机械手动作的精度。

另外，由于一边使直动机械手200或者6轴机械手300动作一边更新加速度传感器60的灵敏度，所以在反复执行动作的情况下，具有能够按每1个动作来提高精度这样的效果。

另外，在实施方式2的机械手控制方法中，计算加速度传感器60的灵敏度误差的修正量的步骤在机械手200、300的动作中被执行。若直动机械手200或者6轴机械手300在静止时执行灵敏度误差的计算，则需要考虑噪声分量被累积从而导致灵敏度误差成为与实际不同的值的情况。因此，通过仅在进行动作时计算灵敏度误差的修正量，能够抑制噪声的影响，从而得到高精度的灵敏度修正量。

在计算加速度传感器60的灵敏度误差的修正量的步骤中，以直动机械手200或者6轴机械手300的动作中的所给定的时间间隔来计算扭转角速度，并在动作结束后综合以上述的时间间隔计算出的歪斜速度来计算灵敏度误差的修正量。由此，能够基本实时地取得歪斜速度的变化，若每当1个动作结束时计算修正量则能够降低计算步骤的计算负荷。

另外，在计算加速度传感器60的灵敏度误差的修正量的步骤中，在检测出加速度传感器60的安装位置的移动方向后，与移动方向对应地切换歪斜速度的值的正负。在歪斜速度的计算中不存在直接的移动方向的信息。于是，根据加速度传感器60的安装位置的移动方向将计算出的歪斜速度的值切换为正或者负。由此，能够进行加速度传感器60的正逆移动方向双方的灵敏度修正。

另外，计算歪斜速度的步骤在加速度传感器60的偏移去除后执行，由此能够除去偏移误差的影响，从而进行高精度的灵敏度误差的修正。

加速度传感器60的灵敏度误差的修正量是利用与直动机械手200或者6轴机械手300的动作速度的大小对应的修正常数来计算的。修正常数是对灵敏度修正量乘以灵敏度修正增益而得到的值。因此，能够在动作速度大时使修正常数变大来增大灵敏度修正量，在动作速度小时通过使修正常数变小来减小灵敏度修正量。

另外，根据以上说明的实施方式2，利用根据歪斜速度计算出的灵敏度修正量来修正加速度传感器60的灵敏度，使直动机械手200或者6轴机械手300进行动作。由此，能够降低因加速度传感器60的个体差、温度变化引起的对精度的影响、因老化引起的对精度下降的影响，从而提高机械手动作的精度。

附图标记的说明：

10…机械手（3轴），20…控制装置，41、51…电机，42…第1编码器，44、49…臂，48…第2编码器，50…陀螺仪传感器。

Claims (14)

1.一种机械手的控制方法，其特征在于，所述机械手具备电机、检测所述电机的旋转角度的角度传感器、与所述电机连结的臂、和安装于所述臂的惯性传感器，

所述机械手的控制方法包括：

利用由所述惯性传感器检测的角速度和根据由所述角度传感器检测的角度信息得到的惯性传感器坐标下的角速度之差计算所述臂的扭转角速度的步骤；

利用所述扭转角速度来计算所述惯性传感器的灵敏度的修正量的步骤；以及

利用所述修正量来修正所述惯性传感器的灵敏度的步骤。

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

计算所述修正量的步骤在所述机械手的动作中执行。

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

在计算所述修正量的步骤中，以所述机械手的动作中的所给定的时间间隔计算扭转角速度，在动作结束后，根据以所述时间间隔计算出的扭转角速度的总和来计算所述灵敏度误差的修正量。

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

在计算所述修正量的步骤中，在检测出所述电机的旋转方向后，根据所述旋转方向来切换所述扭转角速度的值的正负。

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

所述机械手的控制方法还具有所述惯性传感器的偏移去除步骤，

计算所述扭转角速度的步骤在所述偏移去除步骤后执行。

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

利用与所述机械手的动作速度的绝对值对应的所述惯性传感器的灵敏度的修正常数来计算所述修正量。

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

电机；

臂，其与所述电机连结而被驱动；

角度传感器，其检测所述电机的旋转角度；

惯性传感器，其安装于所述臂；

扭转角速度计算部，其利用由所述惯性传感器检测的角速度和根据由所述角度传感器检测的信息得到的惯性传感器坐标下的角速度之差来计算所述臂的扭转角速度；

灵敏度修正量计算部，其计算所述惯性传感器的灵敏度修正量；

灵敏度修正部，其利用所述灵敏度修正量计算部所计算的灵敏度修正量来修正灵敏度；和

动作判断部，其判断所述臂的动作。

8.一种机械手的控制方法，其特征在于，包括：

利用由加速度传感器检测的加速度来计算传感器坐标系的速度的步骤；

根据由速度传感器检测的传感器坐标系的速度和利用所述加速度计算的传感器坐标系的速度之差来计算歪斜速度的步骤；

利用所述歪斜速度来计算所述加速度传感器的灵敏度修正量的步骤；以及

利用所述灵敏度修正量来修正所述加速度传感器的灵敏度的步骤。

9.根据权利要求8所述的机械手的控制方法，其特征在于，

计算所述修正量的步骤在所述机械手的动作中执行。

10.根据权利要求8或9所述的机械手的控制方法，其特征在于，

在计算所述修正量的步骤中，以所述机械手的动作中的所给定的时间间隔计算歪斜速度，在动作结束后，根据以所述时间间隔计算出的歪斜速度的总和来计算所述灵敏度误差的修正量。

11.根据权利要求8或9所述的机械手的控制方法，其特征在于，

在计算所述修正量的步骤中，在检测出所述加速度传感器的安装位置的移动方向后，根据所述移动方向来切换所述歪斜速度的值的正负。

12.根据权利要求8或9所述的机械手的控制方法，其特征在于，

所述机械手的控制方法还具有所述加速度传感器的偏移去除步骤，

计算所述歪斜速度的步骤在所述偏移去除步骤后执行。

13.根据权利要求8或9所述的机械手的控制方法，其特征在于，

利用与所述机械手的动作速度的绝对值对应的所述加速度传感器的灵敏度的修正常数来计算所述修正量。

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

传感器坐标速度计算部，其获取加速度传感器的检测值，来计算传感器坐标系下的速度；

歪斜速度计算部，其根据由速度传感器检测出的速度和所述传感器坐标系下的速度之差来计算歪斜速度；

灵敏度修正量计算部，其计算所述加速度传感器的灵敏度修正量；

灵敏度计算部，其基于所述灵敏度修正量来修正所述加速度传感器的灵敏度；以及

动作判断部，其判断是处于动作中还是处于静止中。