机械手装置以及机械手装置的控制方法 【技术领域】
本发明涉及机械手装置以及机械手装置的控制方法。背景技术 作为 IC 分类机和组装装置的一部分被经常使用的具有多关节构造的机械手装 置， 逐渐被经常用于各种操作现场中。 因此， 怎样能够让机械手装置更迅速且更准确地将手 臂移动到规定位置， 已成为重要的性能标准和品质。
通常为了使机械手装置的手臂高速且准确地移动， 需要减少施加于手臂的惯性 力， 且不增大对驱动手臂的致动器施加的负荷。 作为减少施加到手臂的惯性力的方法之一， 手臂本身的轻型化作为简便且有效的方法被使用。 但是， 通过手臂的轻型化， 会导致手臂的 刚性降低， 从而会使因在手臂停止时产生的手臂的挠曲而产生振动的情况增加， 即使进行 控制以使得手臂前端部停止在目标位置， 也会产生手臂振动的振幅量的位置偏移， 振动衰 减的时间变成到开始下一动作为止的待机时间， 从而成为高速动作的障碍。
针对该问题， 公开有如下方法 ： 在手臂前端设置加速度传感器， 以加速度信号为基 础使手臂工作， 并抑制振动的方法 ( 例如， 专利文献 1)， 在手臂前端以及手臂上设置角速度 传感器， 以角速度信号为基础控制手臂动作的方法 ( 例如， 专利文献 2)， 和基于设置在手臂 前端的惯性传感器的信号来驱动驱动体的方法 ( 例如， 专利文献 3) 等。
但是， 在这些现有文献中， 在作为手臂控制的基准的加速度传感器或角速度传感 器等惯性传感器本身发生了故障的情况下， 即使所得到的数据信号不好， 也进行基于该不 良信号的控制， 会导致因机械手装置的失控等带来的危险的发生。作为检测该传感器自身 的故障的技术， 公开有如下技术 ： 预先设定针对传感器的检测值的阈值， 在阈值和检测值之 差超过了判断值时判断为传感器发生了故障 ( 专利文献 4、 5)。
【专利文献 1】 日本特开平 1-173116 号公报
【专利文献 2】 日本特开 2005-242794 号公报
【专利文献 3】 日本特开平 7-9374 号公报
【专利文献 4】 日本特开 2009-8412 号公报
【专利文献 5】 日本特开 2009-184035 号公报
但是， 就上述专利文献所公开的技术来说， 由于以使用的传感器的正常工作时的 检测值作为基准来预先设定阈值， 因此在实际的机械手装置的各种动作状态下， 有时即使 传感器发生故障， 在不超过阈值的情况下也无法进行正确的故障判断。
发明内容
因此， 提供一种在实际工作时与作为基准的传感器检测数据进行比较， 根据该检 测数据的差值来进行可靠的故障判断的机械手装置以及机械手装置的控制方法。
本发明为了至少解决上述的课题之一， 能够作为下述的方式或者应用例被实现。
〔应用例 1〕 本应用例的机械手装置， 其特征在于， 具备 ： 手臂连结装置， 其具有致动器、 将上述致动器的转矩以规定的减速比进行传递的转矩传递机构、 和检测上述致动器的 旋转角度的角度传感器 ； 手臂体， 其是由多个手臂通过上述手臂连结装置串联且可旋转地 被连结而形成的 ； 基体， 上述手臂体通过设置在上述手臂体的一个端部的基体连结装置被 可旋转地连结在该基体上， 该基体连结装置具备上述致动器、 将上述致动器的转矩以规定 的减速比进行传递的转矩传递机构、 和检测上述致动器的旋转角度的角度传感器 ； 惯性传 感器， 其安装在上述手臂上 ； 第一计算部， 其根据来自上述角度传感器的上述致动器的旋转 角度数据来计算上述致动器的角速度以及角加速度 ； 第二计算部， 其根据由上述惯性传感 器检测到的输出， 计算上述手臂的角速度或者角加速度 ； 比较部， 其对由上述第一计算部计 算出的上述致动器的角速度或者角加速度、 和由上述第二计算部计算出的上述手臂的角速 度或者角加速度进行比较 ； 故障判断部， 当在上述比较部中， 上述致动器的角速度或者角加 速度与上述手臂的角速度或者角加速度之差的绝对值大于阈值 Ω 时， 该故障判断部判断 为上述惯性传感器发生了故障， 并输出使上述致动器的动作停止的信号。
〔应用例 2〕 在上述的应用例中， 其特征在于， 在承受上述机械手装置的最大负荷的 120％的负荷、 且以最大负荷时的允许加速度的 120％的加速度进行动作的情况下， 当由上 述第一计算部计算的角速度为 ωS、 由上述第二计算部计算的角速度为 ωL 时， 相对于 ωS 和 ωL 之差的绝对值的最大值， 上述阈值 Ω 是 ： 。 Ω ＝ 2×(|ωS-ωL|)max
根据上述的应用例， 构成为不将决定阈值的基准值设为固定值而是以动作状态下 的致动器的动作数据作为基准值来检测惯性传感器的故障。根据该构成， 对于使用了固定 值的阈值， 例如， 也发生过没有检测到动作速度或加速度较小的情况下等的故障的情况， 而 基于动作中的依次信息， 成为作为基准的致动器的基准的动作数据也被依次改写， 由此例 如即使是微小动作， 也能够在各种动作模式下可靠地检测惯性传感器的故障。
〔应用例 3〕 本应用例的机械手装置， 其特征在于， 具备 ： 手臂连结装置， 其具有致 动器、 和将上述致动器的转矩以规定的减速比进行传递的转矩传递机构 ； 手臂体， 其是由多 个手臂通过上述手臂连结装置串联且可旋转地被连结而形成的 ； 基体， 上述手臂体通过设 置在上述手臂体的一个端部的基体连结装置被可旋转地连结在该基体上， 该基体连结装置 具备上述致动器、 和将上述致动器的转矩以规定的减速比进行传递的转矩传递机构 ； 至少 2 个惯性传感器， 被安装在上述手臂上 ； 计算部， 其存储上述惯性传感器检测出的多个输出 值， 并计算平均值 ； 比较部， 其对由上述计算部计算出的上述输出值的平均值进行比较 ； 故 障判断部， 当在上述比较部中， 2 个上述输出值的平均值之差的绝对值中存在大于阈值 S 的 上述差时， 该故障判断部判断为上述惯性传感器中的任意一个以上发生了故障， 并输出使 上述致动器的动作停止的信号。
〔应用例 4〕 在上述的应用例中， 其特征在于， 在上述惯性传感器的白噪声的标准偏 差是 σ 时， 上述阈值 S 为 ：
S ＝ 6σ
根据上述应用例， 构成为不将决定阈值的基准值设为固定值而是求出动作状态下 的具备的 2 个以上的惯性传感器的检测值之差， 如果是正常则以白噪声的标准偏差作为基 准值来检测惯性传感器的故障。 根据该构成， 对于使用了固定值的阈值， 例如发生过没有检 测到动作速度或加速度较小的情况下等的故障的情况， 而基于动作中的依次信息， 与基准
值进行比较的惯性传感器的动作数据也被依次改写， 由此例如即使是微小动作， 也能够在 各种动作模式下可靠地检测惯性传感器的故障。
〔应用例 5〕 本应用例的机械手装置， 其特征在于， 具备 ： 基体， 其具备基体致动器、 将上述基体致动器的转矩以规定的减速比进行传递的转矩传递机构、 检测上述基体致动器 的旋转角度的基体角度传感器、 和手臂驱动装置， 该手臂驱动装置具有手臂连结装置， 与上 述转矩传递机构连结， 并直线往返驱动上述手臂连结装置 ； 手臂， 其与上述基体的上述手臂 连结装置连结， 具有手臂致动器、 将上述手臂致动器的转矩以规定的减速比进行传递的转 矩传递机构、 检测上述手臂致动器的旋转角度的手臂角度传感器、 和工件保持装置驱动装 置， 该工件保持装置驱动装置具有工件保持装置， 与上述手臂连结装置连结， 并直线往返驱 动上述工件保持装置 ； 2 个以上惯性传感器， 其安装在上述工件保持装置上， 至少包含加速 度传感器 ； 第一计算部， 其根据来自上述基体角度传感器的上述基体致动器的旋转角度数 据来计算上述手臂驱动装置的加速度， 并根据来自上述手臂角度传感器的上述手臂致动器 的旋转角度数据来计算上述工件保持装置驱动装置的加速度 ； 第二计算部， 其根据由上述 惯性传感器检测到的输出， 计算上述工件保持装置的加速度 ； 比较部， 其对由上述第一计算 部计算出的上述加速度和由上述第二计算部计算出的上述加速度进行比较 ； 故障判断部， 其在由上述第一计算部计算出的上述加速度和由上述第二计算部计算出的上述加速度之 差的绝对值大于阈值 A 的情况下， 判断为上述惯性传感器发生了故障， 并输出使上述基体 致动器以及上述手臂致动器的动作停止的信号。 〔应用例 6〕 在上述的应用例中， 其特征在于， 在承受上述机械手装置的最大负荷的 120％的负荷， 并以最大负荷时的允许加速度的 120％的加速度进行动作的情况下， 当由上 述第一计算部计算的加速度为 αS、 由上述第二计算部计算的加速度为 αL 时， 相对于 αS 和 αL 之差的绝对值的最大值， 上述阈值 A 为 ： 。
A ＝ 2×(|αS-αL|)max
根据上述应用例， 构成为不将决定阈值的基准值设为固定值而是将动作状态下的 致动器的动作数据作为基准值来检测惯性传感器的故障。根据该构成， 对于使用了固定值 的阈值， 例如发生过没有检测到动作速度或加速度较小的情况下等的故障的情况， 而基于 动作中的依次信息， 成为作为基准的致动器的基准的动作数据也被依次改写， 由此例如即 使是微小动作， 也能够在各种动作模式下可靠地检测惯性传感器的故障。
〔应用例 7〕 本应用例的机械手装置的控制方法， 其特征在于， 该机械手装置具备 ： 手臂连结装置， 其具有致动器、 将上述致动器的转矩以规定的减速比进行传递的转矩传递 机构、 和检测上述致动器的旋转角度的角度传感器 ； 手臂体， 其是由多个手臂通过上述手臂 连结装置串联且可旋转地被连结而形成的 ； 基体， 上述手臂体通过设置在上述手臂体的一 个端部的基体连结装置被可旋转地连结在该基体上， 该基体连结装置具备上述致动器、 将 上述致动器的转矩以规定的减速比进行传递的转矩传递机构、 和检测上述致动器的旋转角 度的角度传感器 ； 和安装在上述手臂上的惯性传感器， 该机械手装置的控制方法包括 ： 计 算步骤， 根据来自上述角度传感器的上述致动器的旋转角度数据来计算上述致动器的角速 度以及角加速度， 并根据由上述惯性传感器检测到的输出来计算上述手臂的角速度或者角 加速度 ； 比较步骤， 对通过上述计算步骤而计算出的上述致动器的角速度或者角加速度、 与 上述手臂的角速度或者角加速度进行比较 ； 判断步骤， 当在上述比较步骤中上述致动器的
角速度或者角加速度与上述手臂的角速度或者角加速度之差的绝对值大于阈值 Ω 时， 判 断为上述惯性传感器发生了故障 ； 停止指示步骤， 当在上述判断步骤中判断为发生了故障 时， 输出使上述致动器的动作停止的信号。
〔应用例 8〕 在上述的应用例中， 其特征在于， 在承受上述机械手装置的最大负荷的 120％的负荷， 并以最大负荷时的允许加速度的 120％的加速度进行动作的情况下， 当在上 述计算步骤中计算的上述致动器的角速度为 ωS、 上述手臂的角速度为 ωL 时， 相对于 ωS 和 ωL 之差的绝对值的最大值， 上述阈值 Ω 为 ：
Ω ＝ 2×(|ωS-ωL|)max
根据上述的应用例， 构成为不将决定阈值的基准值设为固定值而是将动作状态下 的致动器的动作数据作为基准值来检测惯性传感器的故障。根据该构成， 对于利用了固定 值的阈值， 例如也发生过没有检测到动作速度或加速度较小的情况下等的故障的情况， 而 基于动作中的依次信息， 成为作为基准的致动器的基准的动作数据也被依次改写， 由此例 如即使是微小动作， 也能够在各种动作模式下可靠地检测惯性传感器的故障， 能够可靠地 使机械手装置的动作停止， 能够保障安全的操作。
〔应用例 9〕 本应用例的机械手装置的控制方法， 其特征在于， 该机械手装置具备 ： 手臂连结装置， 其具有致动器、 和将上述致动器的转矩以规定的减速比进行传递的转矩传 递机构 ； 手臂体， 其是由多个手臂通过上述手臂连结装置串联且可旋转地被连结而形成的 ； 基体， 上述手臂体通过设置在上述手臂体的一个端部的基体连结装置被可旋转地连结在该 基体上， 该基体连结装置具备上述致动器、 和将上述致动器的转矩以规定的减速比进行传 递的转矩传递机构 ； 和至少 2 个惯性传感器， 其安装在上述手臂上， 该机械手装置的控制方 法包括 ： 计算步骤， 存储上述惯性传感器检测到的多个输出值， 并计算平均值 ； 比较步骤， 对由上述计算步骤计算出的上述输出值的平均值进行比较 ； 判断步骤， 当在上述比较步骤 中， 2 个上述输出值的平均值之差的绝对值中存在大于阈值 S 的上述差时， 判断为上述惯性 传感器中的任意一个以上发生了故障 ； 停止指示步骤， 当在上述判断步骤中判断为故障时， 输出使上述致动器的动作停止的信号。
〔应用例 10〕 上述的应用例中， 其特征在于， 在上述惯性传感器的白噪声的标准偏 差是 σ 时， 上述阈值 S 为 ：
S ＝ 6σ
根据上述的应用例， 构成为不将决定阈值的基准值设为固定值而是求出动作状态 下的具有 2 个以上的惯性传感器的检测值之差， 如果是正常则以白噪声的标准偏差作为基 准值来检测惯性传感器的故障。 根据该构成， 对于利用了固定值的阈值， 例如也发生过没有 检测到动作速度或加速度较小的情况下等的故障的情况， 而基于动作中的依次信息， 与基 准值进行比较的惯性传感器的动作数据也被依次改写， 由此例如即使是微小动作， 也能够 在各种动作模式下可靠地检测惯性传感器的故障， 能够可靠地使机械手装置的动作停止， 能够保障安全的操作。
〔应用例 11〕 本应用例的机械手装置的控制方法， 其特征在于， 该机械手装置具 备： 基体， 其具备基体致动器、 将上述基体致动器的转矩以规定的减速比进行传递的转矩传 递机构、 检测上述基体致动器的旋转角度的基体角度传感器、 和手臂驱动装置， 该手臂驱动 装置具有手臂连结装置， 与上述转矩传递机构连结， 并直线往返驱动上述手臂连结装置 ； 手臂， 其与上述基体的上述手臂连结装置连结， 具有手臂致动器、 将上述手臂致动器的转矩以 规定的减速比进行传递的转矩传递机构、 检测上述手臂致动器的旋转角度的手臂角度传感 器、 和工件保持装置驱动装置， 该工件保持装置驱动装置具有工件保持装置， 与上述手臂连 结装置连结， 并直线往返驱动上述工件保持装置， 该机械手装置的控制方法包括 ： 计算步 骤， 根据来自上述基体角度传感器的上述基体致动器的旋转角度数据计算上述手臂驱动装 置的加速度， 根据来自上述手臂角度传感器的上述手臂致动器的旋转角度数据计算上述工 件保持装置驱动装置的加速度， 并根据由安装在上述工件保持装置上、 并至少具有加速度 传感器的一个以上惯性传感器检测到的输出， 计算上述工件保持装置的速度以及加速度 ； 比较步骤， 对由上述计算步骤计算出的上述手臂驱动装置、 上述工件保持装置驱动装置的 加速度、 和上述工件保持装置的加速度进行比较 ； 判断步骤， 当在上述比较步骤中， 根据上 述角度传感器的输出值计算出的上述手臂驱动装置、 上述工件保持装置驱动装置的加速度 与根据上述惯性传感器的输出值计算出的上述工件保持装置的加速度之差的绝对值大于 阈值 A 时， 判断为上述惯性传感器发生了故障 ； 停止指示步骤， 当在上述判断步骤中判断为 发生了故障时， 输出使上述基体致动器以及上述手臂致动器的动作停止的信号。
〔应用例 12〕 在上述的应用例中， 其特征在于， 在承受上述机械手装置的最大负荷 的 120％的负荷， 并以最大负荷时的允许加速度的 120％的加速度进行动作的情况下， 当由 上述计算步骤计算的上述手臂驱动装置或上述工件保持装置驱动装置的加速度为 αS、 上 述工件保持装置的加速度为 αL 时， 相对于 αS 和 αL 之差的绝对值的最大值， 上述阈值 A 为：
A ＝ 2×(|αS-αL|)max
根据上述的应用例， 构成为不将决定阈值的基准值设为固定值而是将动作状态下 的致动器的动作数据作为基准值来检测惯性传感器的故障。根据该构成， 对于利用了固定 值的阈值， 例如也发生过没有检测到动作速度或加速度较小的情况下等的故障的情况， 而 基于动作中的依次信息， 成为作为基准的致动器的基准的动作数据也被依次改写， 由此例 如即使是微小动作， 也能够在各种动作模式下可靠地检测惯性传感器的故障， 能够可靠地 使机械手装置的动作停止， 能够保障安全的操作。 附图说明
图 1 的 (a) 是第一实施方式涉及的机械手装置的概略俯视图， (b) 是其概略剖视 图。
图 2 是第一实施方式涉及的机械手装置的故障检测的框图。
图 3 是第一实施方式涉及的机械手装置的故障检测的流程图。
图 4 的 (a) 是第二实施方式涉及的机械手装置的概略俯视图， (b) 是其概略剖视 图。
图 5 是第二实施方式涉及的机械手装置的故障检测的框图。
图 6 是表示传感器的白噪声的一个例子的图表。
图 7 是第二实施方式涉及的机械手装置的故障检测的流程图。
图 8 是第三实施方式涉及的机械手装置的概略俯视图。
图 9 是第三实施方式涉及的机械手装置的概略立体图。图 10 是第三实施方式涉及的机械手装置的故障检测的框图。
图中符号说明 ：
11... 第一手臂 ； 12... 第二手臂 ； 13... 第三手臂 ； 40... 基体 ； 51、 52、 53... 致动 器， 81、 82、 83... 角度传感器 ； 91、 92、 93... 惯性传感器 ； 1100... 第一计算部 ； 1200... 第 二计算部 ； 2000... 比较部 ； 3000... 判断部， 4000... 控制部 具体实施方式
下面参照附图说明本发明涉及的实施方式。
( 第一实施方式 )
针对本发明涉及的第一实施方式进行说明。图 1 表示了第一实施方式涉及的机械 手装置， 其中， (a) 是概略俯视图， (b) 是概略剖视图。本实施方式的机械手装置是在水平方 向上能够旋转地连结有 3 个手臂的、 所谓的 3 轴水平多关节机械手 100( 以下， 称为机械手 装置 100)。
机械手装置 100 具备手臂体 10， 该手臂体 10 构成为将第一手臂 11 和第二手臂 12 通过第一手臂连结装置 21 可旋转地串联连结， 并将第二手臂 12 和第三手臂 13 通过第二手 臂连结装置 22 可旋转地串联连结。手臂体 10 进一步通过基体连结装置 30 可旋转地与固 定在底座的基体 40 连结， 从而构成机械手装置 100。 第一手臂连结装置 21 具备致动器 51 和将致动器 51 的转矩以规定的减速比进行 传递的转矩传递装置 61， 第二手臂连结装置 22 也同样地具备致动器 52 和转矩传递装置 62。另外， 基体连结装置 30 具备致动器 53 和将致动器 53 的转矩以规定的减速比进行传递 的转矩传递装置 63。在作为手臂体 10 的与基体 40 相反的端部的第三手臂 13 的端部具有 保持加工用工具或被加工物的工件保持装置 70。
在第一手臂连结装置 21 所具备的致动器 51 上具备检测旋转角度的角度传感器 81， 同样地在第二手臂连结装置的致动器 52 上具备角度传感器 82。另外， 在基体连结装置 30 的致动器 53 上也具备角度传感器 83。而且， 在第一手臂 11 上具有惯性传感器 91， 在第 二手臂 12 上具有惯性传感器 92， 在第三手臂 13 上具有惯性传感器 93。惯性传感器 91、 92、 93 至少包括角速度传感器， 惯性传感器 91 能够检测第一手臂 11 的惯性传感器安装位置处 的角速度， 惯性传感器 92 能够检测第二手臂 12 的惯性传感器安装位置处的角速度， 惯性传 感器 93 能够检测第三手臂 13 的惯性传感器安装位置处的角速度。
图 2 是本实施方式涉及的框图。由角度传感器 81、 82、 83 检测到的致动器 51、 52、 53 的旋转角度数据被在第一计算部 1100 中进行旋转角度换算， 将该旋转角度以时间进行 1 次微分， 来计算角速度。
在第二计算部 1200 中， 根据第一手臂 11、 第二手臂 12、 第三手臂 13 所具备的惯性 传感器 91、 92、 93 检测到的数据来计算第一手臂 11 的角速度 ωa1、 第二手臂的角速度 ωa2、 和第三手臂的角速度 ωa3。
比较部 2000 对在第一计算部 1100 以及第二计算部 1200 中计算出的角速度进行 比较。 角速度的比较方法是进行求出下述差值的计算， 即求出各致动器的角速度、 和从与该 致动器连结的手臂所具备的惯性传感器的角速度分量中去除了具备该致动器的连结装置 的角速度分量而得的实质角速度之差。
作为例子， 利用第二手臂 12 进行说明。将根据连结第二手臂 12 与第一手臂 11 的 第一手臂连结装置 21 的角度传感器 81 的检测数据而求出的角度设为 θ1， 将转矩传递装置 61 的减速比设为 N1， 根据第一手臂 11 所具备的惯性传感器 91 的检测值而求出的角速度为 ωa1， 根据第二手臂 12 所具备的惯性传感器 92 的检测值而求出的角速度为 ωa2， 由此在比 较部中作为比较结果求出绝对值， 即下式。
|(ωa2-ωa1)-dθ1/dt×1/N1)| ( 式 1-1)
在此 t 表示时间。当微分值 dθ1/dt 在第一计算部 1100 中被计算， 并将计算出的 第一手臂连结装置 21 的致动器 51 的角速度设为 ω1 时， 式 1 能够改写成下式。
|(ωa2-ωa1)-ω1×1/N1)| ( 式 1-2)
根据在比较部 2000 求出的比较结果、 例如对于第二手臂 12 是根据 ( 式 1-1) 的 值， 而在判断部 3000 中对作为对象的惯性传感器是正常还是故障进行判断。 在判断部 3000 中， 预先将用于进行故障判断的阈值 ( 判断值 ) 储存到未图示的存储装置中， 根据相对于阈 值比较结果的大小来进行正常、 故障的判断。
对于除上述例示的第二手臂 12 以外的手臂也同样， 对于第一手臂 11、 第三手臂 13 来说， 作为与 ( 式 1-1) 相当的比较结果的绝对值如下述那样。 在第一手臂 11 的情况下， 比较结果如下式。
|ωa1-ω3×1/N3| ( 式 1-3)
ωa1 是根据第一手臂 11 所具备的惯性传感器 91 的检测值而求出的角速度， ω3 是 根据基体连结装置 30 的致动器 53 所具备的角度传感器 83 的检测值而计算出的角速度， N3 是基体连结装置 30 的转矩传递装置 63 的减速比。
在第三手臂 13 的情况下， 比较结果如下式。
|(ωa3-ωa2)-ω2×1/N2)| ( 式 1-4)
ωa3 是根据第三手臂 13 所具备的惯性传感器 93 的检测值而求出的角速度， ωa2 是 根据第二手臂 12 所具备的惯性传感器 92 的检测值而求出的角速度， ω2 是根据第二手臂 连结装置 22 的致动器 52 所具备的角度传感器 82 的检测值而计算出的角速度， N2 是第二 手臂连结装置 22 的转矩传递装置 62 的减速比。
在此对阈值进行说明。阈值是以根据作为基准的第一手臂连结装置 21、 第二手臂 连结装置 22、 基体连结装置 30 所具备的致动器 51、 52、 53 的角度传感器 81、 82、 83 检测到的 角度数据而计算出的角速度为基准而设定的。
将惯性传感器 91 的故障判断的阈值设为 Ω1， 惯性传感器 92 的故障判断的阈值设 为 Ω2， 惯性传感器 93 的故障判断的阈值设为 Ω3。例如， 对决定第一手臂 11 所具备的惯 性传感器 91 的故障判断的阈值 Ω1 的情况进行说明。
使机械手装置 100 持有最大负荷的 120％的负荷， 并以第一手臂 11 在最大负荷时 的允许加速度的 120％使第一手臂 11 动作。 相对于根据从动作开始起经过 T 时间后的惯性 传感器 91 的输出值而求出的角速度 ωa1A 与根据从动作开始起经过 T 时间后的基体连结装 置 30 的致动器 53 所具备的角度传感器 83 的检测值而求出的角速度 ωm1A 之差的绝对值的 最大值， 将阈值 Ω1 设为下式。
Ω1 ＝ 2×|ωa1A-ωm1A|max ( 式 1-5)
同样地， 使机械手装置 100 持有最大负荷的 120％的负荷， 并以第二手臂 12 在最大
负荷时的允许加速度的 120％使第二手臂 12 动作。 相对于第二手臂 12 的角速度、 与根据从 动作开始起经过 T 小时后的第一手臂连结装置 21 的致动器 51 所具备的角度传感器 81 的 检测值而求出的角速度 ωm2A 之差的绝对值的最大值， 能够得到阈值 Ω2、 即下式 ( 式 1-6)， 其中， 上述第二手臂 12 的角速度基于根据从动作开始起经过 T 时间后的惯性传感器 92 的 输出值而求出的角速度 ωa2A 和、 根据从动作开始起经过 T 小时后的惯性传感器 91 的输出 值而求出的角速度 ωa1A 之间的差值。
Ω2 ＝ 2×|ωa2A-ωa1A-ωm2A|max ( 式 1-6)
另外， 使机械手装置 100 持有最大负荷的 120％的负荷， 并以第三手臂 13 在最大负 荷时的允许加速度的 120％来使第三手臂 13 动作。 相对于第三手臂 13 的角速度、 与根据从 动作开始起经过 T 小时后的第 2 手臂连结装置 22 的致动器 52 所具备的角度传感器 82 的 检测值而求出的角速度 ωm3A 之差的绝对值的最大值， 能够得到阈值 Ω3、 即下式 ( 式 1-7)， 其中， 上述第三手臂 13 的角速度基于根据从动作开始起经过 T 小时后的惯性传感器 93 的 输出值而求出的角速度 ωa3A 和、 根据从动作开始起经过 T 小时后的惯性传感器 92 的输出 值而求出的角速度 ωa2A 之间的差值。
Ω3 ＝ 2×|ωa3A-ωa2A-ωm3A|max ( 式 1-7)
由这样求出的阈值 Ω1、 Ω2、 Ω3 与上述的 ( 式 1-1)、 ( 式 1-2)、 ( 式 1-3) 进行比 较， 在满足下述的条件时， 将惯性传感器判断为发生了故障。
惯性传感器 91 故障判断如下 ：
|ωa1-ω3×1/N3)| ＞ Ω1 ( 式 1-8)
惯性传感器 91、 92 故障判断如下 ：
|(ωa2-ωa1)-ω1×1/N1)| ＞ Ω2 ( 式 1-9)
惯性传感器 92、 93 故障判断如下 ：
|(ωa3-ωa2)-ω2×1/N2)| ＞ Ω3 ( 式 1-10)
接下来， 对第一实施方式涉及的控制方法进行说明。图 3 是说明第一实施方式涉 及的制御方法的流程图。
首先， 在计算步骤 (S111， S112) 中， 执行取得角度传感器、 惯性传感器的检测值并 求出角速度的计算。在第一计算部 1100 的计算步骤 (S111) 中， 取得手臂连结装置 21、 22 以及基体连结装置 30 所具备的致动器 51、 52、 53 中具备的角度传感器 81、 82、 83 的检测数 据。将所取得的检测数据转换为角度数据， 并将转换后的角度数据以时间进行微分来计算 角速度。
在第二计算部 1200 的计算步骤 (S112) 中， 取得第一手臂连结装置 21 以及第二手 臂连结装置 22 所具备的惯性传感器 91、 92 的检测数据和， 工件保持装置 70 所具备的惯性 传感器 93 的检测数据。将所取得的检测数据转换为角速度。
接下来， 转移至对在计算步骤 S111、 S112 中计算出的致动器的角速度和手臂的角 速度进行比较的比较步骤 (S120)。在比较步骤 (S120) 中， 将上述的 ( 式 1-2)、 ( 式 1-3)、 ( 式 1-4) 的计算结果向接下来的判断步骤 (S130) 输出。
在判断步骤 (S130) 中， 判断从比较步骤 (S120) 输入的计算结果是否大于预先储 存在未图示的存储装置中的阈值 Ω1、 Ω2、 Ω3。在判断结果为 “是” 的情况下， 即在来自比 较步骤 (S120) 的计算结果大于阈值的情况下， 判断为相应的惯性传感器发生了故障。以第一手臂 11 为例进行说明。对于第一手臂 11， 以基体连结装置 30 所具备的角 度传感器 83 的检测值为基准， 执行第一手臂 11 所具备的惯性传感器 91 的故障判断。将在 计算步骤 (S111、 S112) 中根据角度传感器 83 的检测值计算出的第一手臂 11 的角速度 ωa3/ N3 和、 由惯性传感器 91 得到的第一手臂 11 的角速度 ω1 在比较步骤 (S120) 中通过 ( 式 1-8) 而得到的比较结果进行输出， 在判断步骤 (S130) 中判断其是否是大于阈值 Ω1 的值。 即， 在判断结果为下式的情况下， 判断为惯性传感器 91 发生了故障。
|ωa1-ω3×1/N3)| ＞ Ω1 ( 式 1-8)
同样地， 对于第二手臂 12 以及第三手臂 13， 当判断结果在第二手臂 12 的情况下成 为下式时， 判断为惯性传感器 91 或惯性传感器 92 的其中一方、 或双方发生了故障。
|(ωa2-ωa1)-ω1×1/N1)| ＞ Ω2 ( 式 1-9)
当判断结果在第三手臂的情况下成为下式时， 判断为惯性传感器 92 或惯性传感 器 93 的其中一方、 或双方发生了故障。
|(ωa3-ωa2)-ω2×1/N2)| ＞ Ω3( 式 1-10)
在判断步骤 (S130) 中， 在判断为没有发生故障 ( 否 ) 的情况下， 再次返回计算步 骤 (S111、 S112)， 并反复进行惯性传感器的故障判断。
当 在 判 断 步 骤 (S130) 中 判 断 为 发 生 了 故 障 ( 是 ) 时， 转移至停止指示步骤 (S140)。在停止指示步骤 (S140) 中， 向控制部 4000 发送出使致动器 51、 52、 53 的动作停止 的指令， 由控制部 4000 向致动器 51、 52、 53 发送停止信号， 动作从而停止。即、 机械手装置 100 的动作被停止。然后， 将被判断为故障的惯性传感器从机械手装置卸下， 实施必要的修 理或者传感器更换。
由于如上述那样惯性传感器的故障的判断基准是根据各连结装置所具备的致动 器的角度传感器的检测数据而得到的角速度， 所以判断基准本身也是与手臂体 10 的动作 连动地发生变动的值， 由于以更实际的动作为基准进行故障判断， 因此能够进行准确的惯 性传感器的故障判断。 由此， 能够可靠地避免因机械手装置的失控带来的危险， 保证因早期 的正常化而得到的稳定的机械手装置动作， 并维持较高的产品品质。
( 第二实施方式 )
第二实施方式相对于第一实施方式的不同之处在于 ： 惯性传感器的配置和成为判 断基准的检测数据。 因此， 对与第一实施方式共同的构成标注相同的附图标记来进行说明。
图 4 表示了第二实施方式涉及的机械手装置， 其中， (a) 是概略俯视图， (b) 是概略 剖视图。本实施方式的机械手装置是， 在水平方向上能够旋转地连结有 3 个手臂的、 所谓的 3 轴水平多关节机械手 200( 以下， 称为机械手装置 200)。
第二实施方式涉及的机械手装置 200 相对于第一实施方式涉及的机械手装置 100 的不同之处在于 ： 在各手臂上具备 2 个惯性传感器。在第一手臂 11 上具有惯性传感器 91a 和 91b， 在第二手臂 12 上具有惯性传感器 92a 和 92b， 在第三手臂 13 上具有惯性传感器 93a 和 93b。这些惯性传感器被配置成相互接近， 并且使用标准性能相同的惯性传感器。
图 5 是第二实施方式涉及的框图。在计算部 1300 中， 从手臂体 10 所具备的惯性 传感器 91a、 91b、 92a、 92b、 93a、 93b 中， 按 t1 ～ tn 的每个时间取得检测数据。即，
惯性传感器 91a 中的 t1 ～ tn 时间的 n 个检测数据生成为 ：
α1a1、 α1a2、 α1a3、 ～、 α1an惯性传感器 91b 中的 t1 ～ tn 时间的 n 个检测数据生成为 ： α1b1、 α1b2、 α1b3、 ～、 α1bn 惯性传感器 92a 中的 t1 ～ tn 时间的 n 个检测数据生成为 ： α2a1、 α2a2、 α2a3、 ～、 α2an 惯性传感器 92b 中的 t1 ～ tn 时间的 n 个检测数据生成为 ： α2b1、 α2b2、 α2b3、 ～、 α2bn 惯性传感器 93a 中的 t1 ～ tn 时间的 n 个检测数据生成为 ： α3a1、 α3a2、 α3a3、 ～、 α3an 惯性传感器 93b 中的 t1 ～ tn 时间的 n 个检测数据生成为 ： α3b1、 α3b2、 α3b3、 ～、 α3bn 计算所取得的各惯性传感器 91a、 91b、 92a、 92b、 93a、 93b 的各 n 个检测数据的平均 惯性传感器 91a 的检测数据平均值 α1am 为 ： α1am ＝ (α1a1+α1a2+α1a3+ ～ +α1an)/n 惯性传感器 91b 的检测数据平均值 α1bm 为 ：值。即，
α1bm ＝ (α1b1+α1b2+α1b3+ ～ +α1bn)/n
惯性传感器 92a 的检测数据平均值 α2am 为 ：
α2am ＝ (α2a1+α2a2+α2a3+ ～ +α2an)/n
惯性传感器 92b 的检测数据平均值 α2bm 为 ：
α2bm ＝ (α2b1+α2b2+α2b3+ ～ +α2bn)/n
惯性传感器 93a 的检测数据平均值 α3am 为 ：
α3am ＝ (α3a1+α3a2+α3a3+ ～ +α3an)/n
惯性传感器 93b 的检测数据平均值 α3bm 为 ：
α3bm ＝ (α3b1+α3b2+α3b3+ ～ +α3bn)/n
此外， 数据个数 n 优选取 100 以上， 根据进行计算的 CPU 的性能等条件适当地决定 数据个数 n 即可。
在比较部 2100 中， 根据所得到的各检测数据平均值， 求出接近的惯性传感器的检 测数据平均值之差的绝对值。例如， 在第一手臂 11 中惯性传感器 91a 和 91b 是接近的惯性 传感器， 求出其检测数据平均值之差的绝对值 α1d， 即为下式。
|α1am-α1bm| ＝ α1d ( 式 2-1)
同样地， 对于第二手臂 12、 第三手臂 13， 也求出接近的惯性传感器的检测数据平 均值之差的绝对值 α2d、 α3d、 即为下式。
|α2am-α2bm| ＝ α2d ( 式 2-2)
|α3am-α3bm| ＝ α3d ( 式 2-3)
接下来， 在判断部 3100 中， 根据在比较部 2100 中求出的、 作为比较结果的各惯性 传感器的检测数据平均值之差的绝对值 α1d、 α2d、 α3d， 对作为对象的惯性传感器是正常还 是故障进行判断。在判断部 3100 中， 预先将用于进行故障判断的阈值储存到未图示的存储 装置中， 根据相对于阈值比较结果的大小来进行正常、 故障的判断。
接着对阈值进行说明。阈值是根据各惯性传感器的特性如下述那样决定的。即，利用在安装到机械手装置 200 之前的惯性传感器 91a、 91b、 92a、 92b、 93a、 93b 或与安装的惯 性传感器 91a、 91b、 92a、 92b、 93a、 93b 相同标准的未使用的惯性传感器， 测量各惯性传感器 的白噪声 ( 白色噪声 )。白噪声例如如图 6 所示那样具有不规则地振动的波形。求出所测 量的各惯性传感器的白噪声的标准偏差。
例如， 对于针对第一手臂 11 所具备的惯性传感器 91a、 91b 的阈值， 根据利用在安 装到机械手装置 200 之前的惯性传感器 91a、 91b， 或与惯性传感器 91a、 91b 标准相同的未使 用的惯性传感器而测量出的白噪声的值， 求出标准偏差 σ1。 以所求出的标准偏差 σ1 为基 准， 将第一手臂 11 所具备的惯性传感器 91a、 91b 的故障判断的阈值 S1 设定为下式。
S1 ＝ 6×σ1
同样地， 根据第二手臂 12 所具备的惯性传感器 92a、 92b 的白噪声的标准偏差 σ2 设定阈值 S2， 根据第三手臂 13 所具备的惯性传感器 93a、 93b 的白噪声的标准偏差 σ3 设定 阈值 S3， 阈值 S2 和阈值 S3 分别为下式。
S2 ＝ 6×σ2
S3 ＝ 6×σ3
根据这样设定的阈值 S1、 S2、 S3 和在比较部 2100 得到的利用 ( 式 2-1)( 式 2-2) ( 式 2-3) 求出的、 各惯性传感器的检测数据平均值之差的绝对值 α1d、 α2d、 α3d 的大小来进 行故障判断。即， 在 α1d ＞ S1 的情况下， 判断为惯性传感器 91a、 91b 的其中一方或双方发 生了故障， 在 α2d ＞ S2 的情况下， 判断为惯性传感器 92a、 92b 的其中一方或双方发生了故 障， 在 α3d ＞ S3 的情况下， 判断为惯性传感器 93a、 93b 的其中一方或双方发生了故障。
接下来， 对第二实施方式涉及的控制方法进行说明。图 7 是说明第二实施方式涉 及的制御方法的流程图。首先， 在计算步骤 (S210) 中， 取得来自惯性传感器的检测数据， 并 计算检测数据的平均值 α1am、 α1bm、 α2am、 α2bm、 α3am、 α3bm。
接下来， 转移至比较步骤 (S220)。在比较步骤 (S220) 中， 根据在计算步骤 (S210) 计算出的检测数据的平均值 α1am、 α1bm、 α2am、 α2bm、 α3am、 α3bm， 计算接近的惯性传感器 91a 和 91b 的检测数据的平均值 α1am 和 α1bm 之差的绝对值 α1d( 式 2-1) 作为比较值。同样地， 将接近的惯性传感器 92a 和 92b 的检测数据的平均值 α2am 和 α2bm 之差的绝对值 α2d( 式 2-2)、 接近的惯性传感器 93a 和 93b 的检测数据的平均值 α3am 和 α3bm 之差的绝对值 α3d( 式 2-3) 作为比较值进行计算。
接下来， 转移至判断步骤 (S230)。在判断步骤 (S230) 中， 对来自比较步骤 (S220) 的计算结果是否大于预先储存在未图示的存储装置中的阈值 S1、 S2、 S3 进行判断。在判断 结果为 “是” 的情况下， 即在来自比较步骤 (S220) 的计算结果大于阈值的情况下， 判断为相 应的惯性传感器发生了故障。
以第一手臂 11 为例进行说明。在第一手臂 11 中， 惯性传感器 91a、 91b 成为判断 的对象， 在检测数据平均值之差的绝对值 α1d 相对于阈值 S1 成为下式的情况下， 判断为惯 性传感器 91a、 91b 的其中一方或双方发生了故障。
α1d ＞ S1
同样地， 在第二手臂 12 中， 在成为下式的情况下， 判断为惯性传感器 92a、 92b 中的 其中一方或双方发生了故障，
α2d ＞ S2在第三手臂 13 中， 在成为下式的情况下， 判断为惯性传感器 93a、 93b 中的其中一 方或双方发生了故障。
α3d ＞ S3
在判断步骤 (S230) 中， 在判断为没有发生故障、 即 “否” 的情况下， 再次返回计算 步骤 (S210)， 反复进行惯性传感器的故障判断。
在判断步骤 (S230) 中， 当判断为发生了故障 ( 是 ) 时， 转移至停止指示步骤 (S240)。在停止指示步骤 (S240) 中， 向控制部 4000 发送出使致动器 51、 52、 53 的动作停止 的指令， 由控制部 4000 向致动器 51、 52、 53 发送停止信号， 动作从而停止。即机械手装置 200 的动作被停止。然后， 将判断为故障的惯性传感器从机械手装置卸下， 实施必要的修理 或者传感器更换。
如上述那样， 成为惯性传感器的故障的判断基准的是， 在手臂上至少具备 2 个以 上的多个惯性传感器， 对相同的手臂上所具备的惯性传感器的输出进行比较， 通过将传感 器具有的白噪声的标准偏差作为判断基准， 能够进行准确的惯性传感器的故障判断。 由此， 能够可靠地避免因机械手装置的失控带来的危险， 保证因早期的正常化而得到的稳定的机 械手装置动作， 并维持较高的产品品质。
( 第三实施方式 )
在第一实施方式以及第二实施方式中， 通过所谓的水平多关节机械手装置进行了 说明， 而在第三实施方式中， 说明直动式机械手装置中的传感器的故障判断。
图 8 是第三实施方式涉及的机械手装置 1000 的概略俯视图。另外， 图 9 是从图 8 中的箭头 P 方向观察的剖视图外观的概略图。图 8 中为了明确机械手装置 1000 的构成， 而 省略了覆盖主要部分的框体等的图示。 机械手装置 1000 具备 ： 固定于操作台或者地板等的 基体 300、 可移动地被固定在基体 300 上的手臂 400、 和可移动地固定在手臂 400 上且具备 保持加工用工具或被加工物的工件保持部 520 的工件保持装置 500。
基体 300 具备 ： 基体致动器 320、 将基体致动器 320 的转矩以规定的减速比进行传 递的基体转矩传递装置 310、 与基体转矩传递装置 310 的旋转轴连结的手臂驱动带 340、 将 手臂驱动带 340 可拉动旋转地固定的基体滑轮 350。 另外， 在基体致动器 320 上具有检测旋 转角度的角度传感器 330。
手臂 400 具备手臂致动器 420、 将手臂致动器 420 的转矩以规定的减速比进行传 递的手臂转矩传递装置 410、 与手臂转矩传递装置 410 的旋转轴连结的工件保持装置驱动 带 440、 将工件保持装置驱动带 440 可拉动旋转地固定的手臂滑轮 450。另外， 在手臂致动 器 420 上具备检测旋转角度的角度传感器 430。
可移动地固定在手臂 400 上的工件保持装置 500 中， 具备至少具有 2 个加速度传 感器 530a、 530b 的惯性传感器 530， 通过未图示的驱动装置来驱动工件保持部 520。
另外， 手臂 400 上具备固定在基体 300 所具备的手臂驱动带 340 上的手臂固定装 置 460， 通过手臂驱动带 340 的动作而使手臂 400 动作。另外， 工件保持装置 500 上具备固 定在手臂 400 所具备的工件保持装置驱动带 440 上的工件保持装置固定装置 510， 通过工件 保持装置驱动带 440 的动作而使工件保持装置 500 动作。
这样构成的机械手装置 1000 能够使手臂 400 以及工件保持装置 500 在图 8 以及 图 9 的箭头 Q、 R 所示的方向上移动， 并在规定的位置进行操作。图 10 是第三实施方式涉及的框图。 在第一计算部 1400 中， 将通过角度传感器 330、 430 检测到的基体致动器 320、 手臂致动器 420 的检测数据换算为基体致动器 320 的旋转角 度 θa、 手臂致动器的旋转角度 θb。对所得到的旋转角度 θa、 θb 以时间进行 2 次微分， 来 计算下式的旋转角加速度。
d2θa/dt2 ＝ ωa’ ( 式 3-1) 2 2
d θb/dt ＝ ωb’ ( 式 3-2)
在将把基体致动器 320 的旋转角度经由基体转矩传递装置 310 而转换为手臂驱动 带 340 的移动长度的转换系数设为 K1 的情况下， 手臂驱动带 340 的移动加速度 βa 通过下 式被求出。
βa ＝ ωa’ /K1 ( 式 3-3)
同样地， 在将把手臂致动器 420 的旋转角度经由手臂转矩传递装置 410 而转换为 工件保持装置驱动带 440 的移动长度的转换系数设为 K2 的情况下， 工件保持装置驱动带 440 的移动加速度 βb 通过下式求出。
βb ＝ ωb’ /K2 ( 式 3-4)
在第二计算部 1500 中， 根据工件保持装置 500 所具备的惯性传感器 530 中包含的 加速度传感器 530a、 530b 检测出的数据来计算加速度。在此， 配置成在加速度传感器 530a 例如检测图 9 所示的 Q 方向的加速度分量的情况下， 另一个加速度传感器 530b 检测 R 方向 的加速度。或者也可以利用 3 轴加速度传感器、 从而使用正交的 2 轴方向的检测数据。
得到根据加速度传感器 530a 的 Q 方向的检测数据换算出的加速度 αa、 和根据加 速度传感器 530b 的 R 方向的检测数据而换算出的加速度 αb。
在比较部 2200 中， 对在第一计算部 1400 中得到的驱动带的移动加速度 βa、 βb 和 在第二计算部 1500 中得到的工件保持装置 500 的加速度 αa、 αb 进行比较。比较部 2200 中的比较是指计算加速度差的绝对值， 图 8 所示的 Q 方向为下式 ：
|αa-βa|
而 R 方向为下式 ：
|αb-βb|
在判断部 3200 中， 在从上述的比较部 2200 输出的比较结果、 即加速度差的绝对值 大于阈值的情况下， 判断为成为对象的惯性传感器发生了故障。在此， 对阈值进行说明。
首先， 加速度传感器 530a 的故障判断的阈值 A1 如下述那样被设定。使机械手装 置 1000 持有最大负荷的 120％的负荷， 并以工件保持装置的向着图 8 所示的 Q 方向的最大 负荷时的允许加速度的 120％的加速度， 使工件保持装置 500 在 Q 方向上动作。求出根据 从动作开始经过 T 时间后的加速度传感器 530a 的输出值而求出的工件保持装置 500 的加 速度 αat， 和根据从动作开始经过 T 时间后的使工件保持装置 500 在 Q 方向上移动的基体 致动器 320 所具备的角度传感器 330 的旋转角度而计算出的手臂驱动带 340 的移动加速度 βat 之差。该 αat 和 βat 之差的绝对值的最大值的 2 倍成为阈值 A1。即， 通过下式得到阈 值 A1。
A1 ＝ 2×(|αat-βat|)max ( 式 3-5)
同样地， 对于加速度传感器 530b 的故障判断的阈值 A2， 使机械手装置 1000 持有最 大负荷的 120％的负荷， 以工件保持装置的向着图 8 所示的 R 方向的最大负荷时的允许加速度的 120％的加速度， 使工件保持装置 500 在 R 方向上动作。求出根据从动作开始经过 T 时间后的加速度传感器 530b 的输出值而求出的工件保持装置 500 的加速度 αbt、 和根据从 动作开始经过 T 时间后的使工件保持装置 500 在 R 方向上移动的手臂致动器 420 所具备的 角度传感器 430 的旋转角度而计算出的工件保持装置驱动带 440 的移动加速度 βbt 之差。 该 αbt 和 βbt 之差的绝对值的最大值的 2 倍成为阈值 A2。即， 通过下式得到阈值 A2。
A2 ＝ 2×(|αbt-βbt|)max ( 式 3-6)
判断部 3200 利用由 ( 式 3-5)、 ( 式 3-6) 求出的阈值 A1、 A2 和从比较部 2200 得到 的比较结果来执行故障判断。 对于加速度传感器 530a， 在成为如下情况时， 判断为加速度传 感器 530a 发生了故障。
|αa-βa| ＞ A1
同样地， 对于加速度传感器 530b， 在成为如下情况时， 判断为加速度传感器 530b 发生了故障。
|αb-βb| ＞ A2
在判断部 3200 中， 在判断为加速度传感器 530a 或 530b 发生了故障的情况下， 向 控制部 4000 发送使基体致动器 320 或手臂致动器 420 停止的指示， 控制部 4000 使基体致 动器 320 或者手臂致动器 420 停止。由此， 能够使机械手装置 1000 安全地停止。
接下来， 对第三实施方式涉及的控制方法进行说明。表示第三实施方式涉及的控 制方法的流程图与第一实施方式相同， 可以使用图 3 所示的流程图。因此， 省略与第一实施 方式相同的部分的详细说明， 对与第一实施方式的不同点进行说明。
在第三实施方式涉及的控制方法中， 在第一计算步骤 (S111) 中， 对来自基体致动 器 320 所具备的角度传感器 330 的旋转角度数据进行计算， 计算手臂驱动带 340 的移动加 速度 βa、 和来自手臂致动器 420 所具备的角度传感器 430 的旋转角度， 计算工件保持装置 驱动带 440 的移动加速度 βb， 并转移至比较步骤 (S120)。
在第二计算步骤 (S112) 中， 根据工件保持装置 500 所具备的惯性传感器 530 中 包含的加速度传感器 530a、 530b 检测出的数据计算加速度 αa、 αb， 并转移至比较步骤 (S120)。
接下来， 转移至比较步骤 (S120)。在比较步骤 (S120) 中， 使用在第一计算步骤 (S111) 以及第二计算步骤 (S112) 中得到的加速度值， 将下述的值作为比较结果向判断步 骤 (S130) 输出， 并转移至判断步骤 (S130)。
|αa-βa|
|αb-βb|
接下来， 转移至判断步骤 (S130)。在判断步骤 (S130) 中， 判断从比较步骤 (S120) 输入的比较结果是否大于预先储存在未图示的存储装置中的阈值 A1， A2。在判断结果是 “是” 的情况下， 即在来自比较步骤 (S120) 的比较结果大于阈值的情况下， 判断为相应的加 速度传感器发生了故障。
即， 在
|αa-βa| ＞ A1
或者，
|αb-βb| ＞ A2 的情况下， 判断为加速度传感器 530a 或 530b 发生了故障。在判断步骤 (S130) 中， 在判断为是故障判定、 即是 “是” 的情况下， 转移至停止指 示步骤 (S140)。在停止指示步骤 (S140) 中， 从控制部 4000 向基体致动器 320、 手臂致动器 420 输出停止动作的指示， 机械手装置 1000 从而停止。
如上述那样， 成为惯性传感器的故障的判断基准是， 根据基体以及手臂所具备的 致动器的角度传感器的检测数据而得到的驱动带的移动加速度， 所以判断基准本身也是 与工件保持装置 500 的动作连动地发生变动的值， 由于以更实际的动作为基准进行故障判 断， 因此能够进行准确的加速度传感器的故障判断。 由此， 能够可靠地避免因机械手装置的 失控而带来的危险， 保证因早期的正常化而得到的稳定的机械手装置动作， 并维持较高的 产品品质。