JP2015062994A - ロボット装置およびロボット装置の制御方法 - Google Patents

Abstract

【課題】実作動時において基準となるセンサー検出データと比較し、その検出データの差分から確実な故障判断を行うロボット装置および制御方法を提供する。【解決手段】アクチュエーターを含む連結装置によりアームが回転もしくは直線移動可能に連結されたロボット装置であって、アームに取り付けられた慣性センサーと、アクチュエーターに備える角度センサーからの回転角度データから、アクチュエーターの角速度および角加速度を演算する第１演算部と、慣性センサーにより検出された出力より、アームの角速度または角加速度を演算する第２演算部と、第１演算部により演算された角速度または角加速度と、第２演算部により演算された角速度または角加速度とを比較する比較部と、比較部においてアクチュエーターとアームの角速度または角加速度との差の絶対値が、閾値より大きい場合に慣性センサーが故障と判定する。【選択図】図２

Description

本発明は、ロボット装置、およびロボット装置の制御方法に関する。

ＩＣハンドラーや組立装置の一部として多く使われている多関節構造を有するロボット装置は、様々な作業現場の中で多用されてきている。それゆえに、ロボット装置には今まで以上に決められた位置にいかに早く且つ正確にアームを移動させることができるか、が重要な性能仕様、品質になってきている。

一般的にロボット装置のアームを高速且つ正確に移動させるには、アームに掛かる慣性力を低減し、アームを駆動するアクチュエーターへの負荷を大きくさせないことにある。アームに掛かる慣性力を小さくする手段の一つには、アームその物の軽量化が簡便且つ効果的な方法として用いられている。しかし、アームの軽量化によって、アームの剛性を低下させることになり、アーム停止時に発生するアームの撓みを原因とする振動の発生を増大させてしまい、アーム先端部を目標位置に停止させるように制御しても、アーム振動の振幅分の位置ずれを生じてしまい、振動が減衰する時間が、次の動作を開始するまでの待機時間になってしまい、高速動作への障害となっていた。

この問題に対して、アーム先端に加速度センサーを設置し、加速度信号を元にアームを作動させ、振動を抑制する方法（例えば、特許文献１）、アーム先端およびアームに角速度センサーを設置し、角速度信号を元にアーム動作を制御する方法（例えば、特許文献２）、アーム先端に設置した慣性センサーの信号に基づいて駆動体を駆動する方法（例えば、特許文献３）、などが開示されている。

しかし、これらの先行文献において、アーム制御の基準となる加速度センサーや角速度センサーなどの慣性センサーそのものが故障した場合には、得られるデータ信号が不良であっても、その不良信号に基づく制御が行われ、ロボット装置の暴走などによる危険の発生に繋がるものである。この、センサーその物の故障を検知する技術として、センサーの検出値に対する閾値を予め設定し、閾値と検出値との差が判定値を超えたときにセンサーが故障していると判断することが開示されている（特許文献４、５）。

特開平１−１７３１１６号公報 特開２００５−２４２７９４号公報 特開平７−９３７４号公報 特開２００９−８４１２号公報 特開２００９−１８４０３５号公報

しかし、上述の特許文献に開示された技術は、予め使用するセンサーの正常作動時の検出値を基準として閾値を設定しているために、実際のロボット装置の様々な動作状態において、センサーが故障していても閾値を超えず正確な故障判定がされないこともあった。

そこで、実作動時において基準となるセンサー検出データと比較し、その検出データの差分から確実な故障判断を行うロボット装置およびロボット装置の制御方法を提供する。

本発明は、少なくとも上述の課題の一つを解決するように、下記の形態または適用例として実現され得る。

〔適用例１〕本適用例によるロボット装置は、アクチュエーターと、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アクチュエーターの回転角度を検出する角度センサーと、を含むアーム連結装置と、複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アクチュエーターの回転角度を検出する角度センサーと、を含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、前記アームに取り付けられた慣性センサーと、前記角度センサーからの前記アクチュエーターの回転角度データから前記アクチュエーターの角速度および角加速度を演算する第１演算部と、前記慣性センサーにより検出された出力より、前記アームの角速度または角加速度を演算する第２演算部と、前記第１演算部により演算された前記アクチュエーターの角速度または角加速度と、前記第２演算部により演算された前記アームの角速度または角加速度とを比較する比較部と、前記比較部において前記アクチュエーターの角速度または角加速度と前記アームの角速度または角加速度との差の絶対値が、閾値Ωより大きい場合に前記慣性センサーが故障と判定し前記アクチュエーターの動作を停止させる信号を出力する故障判定部と、を備えることを特徴とする。

〔適用例２〕上述の適用例において、前記閾値Ωは、前記ロボット装置の最大負荷の１２０％の負荷を持たせ、最大負荷時の許容加速度１２０％の加速度で動作させた場合に、前記第１演算部により演算される角速度がω_S、前記第２演算部により演算される角速度がω_Lの場合、ω_Sとω_Lの差の絶対値の最大値に対して、
Ω＝２×（｜ω_S−ω_L｜）_max
であることを特徴とする。

上述の適用例によれば、閾値を決める基準値を、一定値とせずに動作状態におけるアクチュエーターの動作データを基準値として慣性センサーの故障を検知するように構成した。この構成により、一定値を用いた閾値では、例えば、動作速度もしくは加速度が小さい場合などの故障を検知しない場合も発生したが、動作中の逐次情報に基づき、基準となるアクチュエーターの基準となる動作データも逐次書き換えられることで、例えば、微小動作であっても、様々な動作モードにおいても確実に慣性センサーの故障を検知することができる。

〔適用例３〕本適用例におけるロボット装置は、アクチュエーターと、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、を含むアーム連結装置と、複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、を含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、前記アームに取り付けられた少なくとも２個の慣性センサーと、前記慣性センサーの検出した複数の出力値を記憶し、平均値を演算する演算部と、前記演算部により演算された前記出力値の平均値を比較する比較部と、前記比較部において、２個の前記出力値の平均値の差の絶対値の中に、閾値Ｓより大きい前記差が在る場合に、前記慣性センサーのいずれか１個以上が故障と判定し、前記アクチュエーターの動作を停止させる信号を出力する故障判定部と、を備えることを特徴とする。

〔適用例４〕上述の適用例において、前記閾値Ｓは、前記慣性センサーの白色雑音の標準偏差σであるとき、
Ｓ＝６σ
であることを特徴とする。

上述の適用例によれば、閾値を決める基準値を、一定値とせずに動作状態における２個以上備えた慣性センサーの検出値の差を求め、正常であればホワイトノイズの標準偏差を基準値として慣性センサーの故障を検知するように構成した。この構成により、一定値を用いた閾値では、例えば、動作速度もしくは加速度が小さい場合などの故障を検知しない場合も発生したが、動作中の逐次情報に基づき、基準値と比較する慣性センサーの動作データも逐次書き換えられることで、例えば、微小動作であっても、様々な動作モードにおいても確実に慣性センサーの故障を検知することができる。

〔適用例５〕本適用例におけるロボット装置は、基体アクチュエーターと、前記基体アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記基体アクチュエーターの回転角度を検出する基体角度センサーと、アーム連結装置を備え、前記トルク伝達機構に連結され、前記アーム連結装置を直線往復駆動させるアーム駆動装置と、を含む基体と、前記基体の前記アーム連結装置に連結され、アームアクチュエーターと、前記アームアクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アームアクチュエーターの回転角度を検出するアーム角度センサーと、ワーク保持装置を備え、前記アーム連結装置に連結され、前記ワーク保持装置を直線往復駆動させるワーク保持装置駆動装置と、を含むアームと、前記ワーク保持装置に取り付けられた少なくとも加速度センサーを含む２以上の慣性センサーと、前記基体角度センサーからの前記基体アクチュエーターの回転角度データから前記アーム駆動装置の加速度と、前記アーム角度センサーからの前記アームアクチュエーターの回転角度データから前記ワーク保持装置駆動装置の加速度と、を演算する第１演算部と、前記慣性センサーにより検出された出力から、前記ワーク保持装置の加速度を演算する第２演算部と、前記第１演算部により演算された前記加速度と、前記第２演算部により演算された前記加速度とを比較する比較部と、前記第１演算部により演算された前記加速度と、前記第２演算部により演算された前記加速度との差の絶対値が、閾値Ａより大きい場合に前記慣性センサーが故障と判定し前記基体アクチュエーターおよび前記アームアクチュエーターの動作を停止させる信号を出力する故障判定部と、を備えることを特徴とする。

〔適用例６〕上述の適用例において、前記閾値Ａは、前記ロボット装置の最大負荷の１２０％の負荷を持たせ、最大負荷時の許容加速度１２０％の加速度で動作させた場合に、前記第１演算部により演算される加速度がα_S、前記第２演算部により演算される加速度がα_Lの場合、α_Sとα_Lの差の絶対値の最大値に対して、
Ａ＝２×（｜α_S−α_L）｜ｍａｘ
であることを特徴とする。

上述の適用例によれば、閾値を決める基準値を、一定値とせずに動作状態におけるアクチュエーターの動作データを基準値として慣性センサーの故障を検知するように構成した。この構成により、一定値を用いた閾値では、例えば、動作速度もしくは加速度が小さい場合などの故障を検知しない場合も発生したが、動作中の逐次情報に基づき、基準となるアクチュエーターの基準となる動作データも逐次書き換えられることで、例えば、微小動作であっても、様々な動作モードにおいても確実に慣性センサーの故障を検知することができる。

〔適用例７〕本適用例のロボット装置の制御方法は、アクチュエーターと、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アクチュエーターの回転角度を検出する角度センサーと、を含むアーム連結装置と、複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アクチュエーターの回転角度を検出する角度センサーと、を含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、を備えるロボット装置の制御方法であって、前記アームに取り付けられた慣性センサーと、前記角度センサーからの前記アクチュエーターの回転角度データから前記アクチュエーターの角速度および角加速度と、前記慣性センサーにより検出された出力より、前記アームの角速度または角加速度と、を演算する演算工程と、前記演算工程による演算された前記アクチュエーターの角速度または角加速度と、前記アームの角速度または角加速度とを比較する比較工程と、前記比較工程において前記アクチュエーターの角速度または角加速度と前記アームの角速度または角加速度との差の絶対値が、閾値Ωより大きい場合に前記慣性センサーが故障と判定する判定工程と、前記判定工程において故障と判断した場合に、前記アクチュエーターの動作を停止させる信号を出力する停止指示工程と、を備えることを特徴とする。

〔適用例８〕上述の適用例において、前記閾値Ωは、前記ロボット装置の最大負荷の１２０％の負荷を持たせ、最大負荷時の許容加速度１２０％の加速度で動作させた場合に、前記演算工程において演算される前記アクチュエーターの角速度がω_S、前記アームの角速度がω_Lの場合、ω_Sとω_Lの差の絶対値の最大値に対して、
Ω＝２×（｜ω_S−ω_L｜）_max
であることを特徴とする。

上述の適用例によれば、閾値を決める基準値を、一定値とせずに動作状態におけるアクチュエーターの動作データを基準値として慣性センサーの故障を検知するように構成した。この構成により、一定値を用いた閾値では、例えば、動作速度もしくは加速度が小さい場合などの故障を検知しない場合も発生したが、動作中の逐次情報に基づき、基準となるアクチュエーターの基準となる動作データも逐次書き換えられることで、例えば、微小動作であっても、様々な動作モードにおいても確実に慣性センサーの故障を検知することができ、確実にロボット装置の動作を停止させることができ、安全な作業を保障することができる。

〔適用例９〕本適用例のロボット装置の制御方法は、アクチュエーターと、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、を含むアーム連結装置と、複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、を含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、を備えるロボット装置の制御方法であって、前記アームに取り付けられた少なくとも２個の慣性センサーと、前記慣性センサーの検出した複数の出力値を記憶し、平均値を演算する演算工程と、前記演算工程により演算された前記出力値の平均値を比較する比較工程と、前記比較工程において、２個の前記出力値の平均値の差の絶対値の中に、閾値Ｓより大きい前記差が在る場合に、前記慣性センサーのいずれか１個以上が故障と判定する判定工程と、前記判定工程において故障と判定された場合に、前記アクチュエーターの動作を停止させる信号を出力する停止指示工程と、を備えることを特徴とする。

〔適用例１０〕上述の適用例において、前記閾値Ｓは、前記慣性センサーの白色雑音の標準偏差σであるとき、
Ｓ＝６σ
であることを特徴とする。

上述の適用例によれば、閾値を決める基準値を、一定値とせずに動作状態における２個以上備えた慣性センサーの検出値の差を求め、正常であればホワイトノイズの標準偏差を基準値として慣性センサーの故障を検知するように構成した。この構成により、一定値を用いた閾値では、例えば、動作速度もしくは加速度が小さい場合などの故障を検知しない場合も発生したが、動作中の逐次情報に基づき、基準値と比較する慣性センサーの動作データも逐次書き換えられることで、例えば、微小動作であっても、様々な動作モードにおいても確実に慣性センサーの故障を検知することができ、確実にロボット装置の動作を停止させることができ、安全な作業を保障することができる。

〔適用例１１〕本適用例のロボット装置の制御方法は、基体アクチュエーターと、前記基体アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記基体アクチュエーターの回転角度を検出する基体角度センサーと、アーム連結装置を備え、前記トルク伝達機構に連結され、前記アーム連結装置を直線往復駆動させるアーム駆動装置と、を含む基体と、前記基体の前記アーム連結装置に連結され、アームアクチュエーターと、前記アームアクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アームアクチュエーターの回転角度を検出するアーム角度センサーと、ワーク保持装置を備え、前記アーム連結装置に連結され、前記ワーク保持装置を直線往復駆動させるワーク保持装置駆動装置と、を含むアームと、を備えるロボット装置の制御方法であって、前記基体角度センサーからの前記基体アクチュエーターの回転角度データから前記アーム駆動装置の加速度と、前記アーム角度センサーからの前記アームアクチュエーターの回転角度データから前記ワーク保持装置駆動装置の加速度と、前記ワーク保持装置に取り付けられた少なくとも加速度センサーを含む１以上の慣性センサーにより検出された出力から、前記ワーク保持装置の速度および加速度と、を演算する演算工程と、前記演算工程により演算された前記アーム駆動装置、前記ワーク保持装置駆動装置の加速度と、前記ワーク保持装置の加速度と、を比較する比較工程と、前記比較工程において、前記角度センサーの出力値より演算された前記アーム駆動装置、前記ワーク保持装置駆動装置の加速度と、前記慣性センサーの出力値より演算された前記ワーク保持装置の加速度との差の絶対値が、閾値Ａより大きい場合に前記慣性センサーが故障と判定する判定工程と、前記判定工程において故障と判定された場合に、前記基体アクチュエーターおよび前記アームアクチュエーターの動作を停止させる信号を出力する停止指示工程と、を備えることを特徴とする。

〔適用例１２〕上述の適用例において、前記閾値Ａは、前記ロボット装置の最大負荷の１２０％の負荷を持たせ、最大負荷時の許容加速度１２０％の加速度で動作させた場合に、前記演算工程により演算される前記アーム駆動装置もしくは前記ワーク保持装置駆動装置の加速度がα_S、前記ワーク保持装置の加速度がα_Lの場合、α_Sとα_Lの差の絶対値の最大値に対して、
Ａ＝２×（｜α_S−α_L）｜ｍａｘ
であることを特徴とする。

上述の適用例によれば、閾値を決める基準値を、一定値とせずに動作状態におけるアクチュエーターの動作データを基準値として慣性センサーの故障を検知するように構成した。この構成により、一定値を用いた閾値では、例えば、動作速度もしくは加速度が小さい場合などの故障を検知しない場合も発生したが、動作中の逐次情報に基づき、基準となるアクチュエーターの基準となる動作データも逐次書き換えられることで、例えば、微小動作であっても、様々な動作モードにおいても確実に慣性センサーの故障を検知することができ、確実にロボット装置の動作を停止させることができ、安全な作業を保障することができる。

第１実施形態に係るロボット装置の、（ａ）は概略平面図、（ｂ）は概略断面図。 第１実施形態に係るロボット装置の故障検出のブロック図。 第１実施形態に係るロボット装置の故障検出のフローチャート。 第２実施形態に係るロボット装置の、（ａ）は概略平面図、（ｂ）は概略断面図。 第２実施形態に係るロボット装置の故障検出のブロック図。 センサーのホワイトノイズの一例を示すグラフ。 第２実施形態に係るロボット装置の故障検出のフローチャート。 第３実施形態に係るロボット装置の概略平面図。 第３実施形態に係るロボット装置の概略斜視図。 第３実施形態に係るロボット装置の故障検出のブロック図。

以下、図面を参照して、本発明に係る実施形態を説明する。

（第１実施形態）
本発明に係る第１実施形態について説明する。図１は第１実施形態に係るロボット装置を示し、（ａ）は概略平面図、（ｂ）は概略断面図である。本実施形態のロボット装置は、水平方向に回転可能に３本のアームが連結された、いわゆる３軸水平多関節ロボット１００（以下、ロボット装置１００という）である。

ロボット装置１００は、第１アーム１１と第２アーム１２とが第１アーム連結装置２１、第２アーム１２と第３アーム１３とが第２アーム連結装置２２によって回転可能に直列的に連結されて構成されるアーム体１０を備えている。アーム体１０は、更に基体連結装置３０により、基盤に固定された基体４０と回転可能に連結され、ロボット装置１００を構成している。

第１アーム連結装置２１は、アクチュエーター５１と、アクチュエーター５１のトルクを所定の減速比で伝達するトルク伝達装置６１と、を含み、第２アーム連結装置２２も同様にアクチュエーター５２とトルク伝達装置６２と、を含む。また、基体連結装置３０は、アクチュエーター５３と、アクチュエーター５３のトルクを所定の減速比で伝達するトルク伝達装置６３と、を含む。アーム体１０の基体４０とは反対の端部となる第３アーム１３の端部には加工用ツールもしくは被加工物を保持するワーク保持装置７０が備えられている。

第１アーム連結装置２１に含まれるアクチュエーター５１には回転角度を検出する角度センサー８１が備えられ、同様に第２アーム連結装置にはアクチュエーター５２に角度センサー８２が備えられている。また、基体連結装置３０にも、アクチュエーター５３に角度センサー８３が備えられている。更に、第１アーム１１には慣性センサー９１、第２アーム１２には慣性センサー９２、第３アーム１３には慣性センサー９３が備えられている。慣性センサー９１、９２、９３は、少なくとも角速度センサーを含み、慣性センサー９１は第１アーム１１の、慣性センサー９２は第２アーム１２の、慣性センサー９３は第３アーム１３の各慣性センサー取り付け位置での角速度を検出可能としている。

図２は、本実施形態に係るブロック図である。角度センサー８１、８２、８３によって検出されたアクチュエーター５１、５２、５３の回転角度データは第１演算部１１００において、回転角度換算され、この回転角度を時間で１回微分し、角速度を演算する。

第２演算部１２００では、第１アーム１１、第２アーム１２、第３アーム１３に備えられた慣性センサー９１、９２、９３が検出したデータから第１アーム１１の角速度ω_a1、第２アームの角速度ω_a2、第３アームの角速度ω_a3を演算する。

比較部２０００は、第１演算部１１００および第２演算部１２００において演算された角速度を比較する。角速度の比較手法は、各アクチュエーターの角速度と、当該アクチュエーターに連結されるアームに備える慣性センサーの角速度成分から当該アクチュエーターを備える連結装置の角速度成分を除いた実質角速度と、の差を求める演算を行う。

例として、第２アーム１２によって説明する。第２アーム１２が第１アーム１１に連結される第１アーム連結装置２１の角度センサー８１の検出データから求められた角度をθ１、トルク伝達装置６１の減速比をＮ１、とし、第１アーム１１に備える慣性センサー９１の検出値から求められる角速度がω_a1、第２アーム１２に備える慣性センサー９２の検出値から求められる角速度がω_a2であることから、比較部では比較結果として、絶対値の、
｜（ω_a2−ω_a1）−ｄθ１／ｄｔ×１／Ｎ１）｜ （式１−１）
を求める。ここでｔは時間。微分値ｄθ１／ｄｔは第１演算部１１００において演算され、演算された第１アーム連結装置２１のアクチュエーター５１の角速度をω１とすると、式１は、
｜（ω_a2−ω_a1）−ω１×１／Ｎ１）｜ （式１−２）
と書き換えることができる。

比較部２０００で求められた比較結果、例えば第２アーム１２では（式１−１）の値から、対象となる慣性センサーが正常か故障かを判定部３０００において判定する。判定部３０００では、予め故障判定のための閾値（判断値）を図示しない記憶装置に格納し、閾値に対して比較結果の大小にて正常、故障の判断を行う。

上述に例示した第２アーム１２以外のアームについても同様に、第１アーム１１、第３アーム１３についても（式１−１）に相当する比較結果となる絶対値は以下の通りである。
第１アーム１１の場合、
｜ω_a1−ω３×１／Ｎ３｜ （式１−３）
ω_a1は第１アーム１１に備える慣性センサー９１の検出値から求められる角速度、ω３は基体連結装置３０のアクチュエーター５３に備える角度センサー８３の検出値から演算された角速度、Ｎ３は基体連結装置３０のトルク伝達装置６３の減速比である。

第３アーム１３の場合、
｜（ω_a3−ω_a2）−ω２×１／Ｎ２）｜ （式１−４）
ω_a3は第３アーム１３に備える慣性センサー９３の検出値から求められる角速度、ω_a2は第２アーム１２に備える慣性センサー９２の検出値から求められる角速度、ω２は第２アーム連結装置２２のアクチュエーター５２に備える角度センサー８２の検出値から演算された角速度、Ｎ２は第２アーム連結装置２２のトルク伝達装置６２の減速比である。

ここで閾値について説明する。閾値は、基準となる第１アーム連結装置２１、第２アーム連結装置２２、基体連結装置３０に備えるアクチュエーター５１、５２、５３の角度センサー８１、８２、８３が検出する角度データから算出される角速度を基準に設定される。

慣性センサー９１の故障判断の閾値をΩ１、慣性センサー９２の故障判断の閾値をΩ２、慣性センサー９３の故障判断の閾値をΩ３とする。例えば、第１アーム１１に備える慣性センサー９１の故障判断の閾値Ω１を決定する場合を説明する。

ロボット装置１００に対して最大負荷の１２０％の負荷を持たせ、最大負荷時の第１アーム１１の許容加速度の１２０％によって第１アーム１１を動作させる。動作開始からＴ時間後の、慣性センサー９１の出力値から求められる角速度ω_a1Aと、基体連結装置３０のアクチュエーター５３に備える角度センサー８３の検出値から求められる角速度ω_m1Aと、の差の絶対値の最大値に対して、閾値Ω１を、
Ω１＝２×｜ω_a1A―ω_m1A｜_max （式１−５）
とする。

同様に、ロボット装置１００に対して最大負荷の１２０％の負荷を持たせ、第２アーム１２の最大負荷時の許容加速度の１２０％によって第２アーム１２を動作させる。動作開始からＴ時間後の、慣性センサー９２の出力値から求められる角速度ω_a2Aと、慣性センサー９１の出力値から求められる角速度ω_a1Aとの差による第２アーム１２の角速度と、第１アーム連結装置２１のアクチュエーター５１に備える角度センサー８１の検出値から求められる角速度ω_m2Aと、の差の絶対値の最大値に対して、閾値Ω２を、
Ω２＝２×｜ω_a2A―ω_a1A−ω_m2A｜_max （式１−６）

また、ロボット装置１００に対して最大負荷の１２０％の負荷を持たせ、第３アーム１３の最大負荷時の許容加速度の１２０％によって第３アーム１３を動作させる。動作開始からＴ時間後の、慣性センサー９３の出力値から求められる角速度ω_a3Aと、慣性センサー９２の出力値から求められる角速度ω_a2Aとの差による第３アーム１３の角速度と、第２アーム連結装置２２のアクチュエーター５２に備える角度センサー８２の検出値から求められる角速度ω_m3Aと、の差の絶対値の最大値に対して、閾値Ω３を、
Ω３＝２×｜ω_a3A−ω_a2A−ω_m3A｜_max （式１−７）
とする閾値を得ることができる。

このように求めた閾値Ω１、Ω２、Ω３より、上述の（式１−１）（式１−２）（式１−３）と比較し、下記の条件の時、慣性センサーを故障と判断する。
慣性センサー９１故障判断
｜ω_a1−ω３×１／Ｎ３）｜＞Ω１ （式１−８）
慣性センサー９１、９２故障判断
｜（ω_a2−ω_a1）−ω１×１／Ｎ１）｜＞Ω２ （式１−９）
慣性センサー９２、９３故障判断
｜（ω_a3−ω_a2）−ω２×１／Ｎ２）｜＞Ω３ （式１−１０）

次に、第１実施形態に係る制御方法について説明する。図３は、第１実施形態に係る制御方法を説明するフローチャートである。

先ず、演算工程（Ｓ１１１、Ｓ１１２）において、角度センサー、慣性センサーの検出値を取得し角速度を求める演算を実行する。第１演算部１１００の演算工程（Ｓ１１１）では、アーム連結装置２１、２２および基体連結装置３０に備えるアクチュエーター５１、５２、５３に備える角度センサー８１、８２、８３の検出データを取得する。取得した検出データを角度データに変換し、変換された角度データを時間で微分し角速度を演算する。

第２演算部１２００の演算工程（Ｓ１１２）では、第１アーム連結装置２１および第２アーム連結装置２２に備える慣性センサー９１、９２の検出データと、ワーク保持装置７０に備える慣性センサー９３の検出データと、を取得する。取得した検出データを角速度に変換する。

次に、演算工程Ｓ１１１、Ｓ１１２で演算されたアクチュエーターの角速度と、アームの角速度とを比較する比較工程（Ｓ１２０）に移行する。比較工程（Ｓ１２０）では、上述の（式１−２）、（式１−３）、（式１−４）の演算結果を次の判定工程（Ｓ１３０）に出力する。

判定工程（Ｓ１３０）では、予め図示しない記憶装置に格納された閾値Ω１、Ω２、Ω３に対して、比較工程（Ｓ１２０）から入力した演算結果が大きいか、を判定する。判定結果が「ＹＥＳ」である場合、すなわち比較工程（Ｓ１２０）からの演算結果が閾値より大きい場合、該当する慣性センサーが故障と判定する。

第１アーム１１を例に説明する。第１アーム１１では基体連結装置３０に備える角度センサー８３の検出値を基準として、第１アーム１１に備える慣性センサー９１の故障判定を実行する。演算工程（Ｓ１１１、Ｓ１１２）において角度センサー８３の検出値から演算される第１アーム１１の角速度ωａ３／Ｎ３と、慣性センサー９１から得られる第１アーム１１の角速度ω１と、を比較工程（Ｓ１２０）において（式１−８）による比較結果を出力し、閾値Ω１より大きい値であるかを判定工程（Ｓ１３０）にて判定する。すなわち、判定結果が、
｜ω_a1−ω３×１／Ｎ３）｜＞Ω１ （式１−８）
となった場合に、慣性センサー９１が故障していると判定される。

同様に、第２アーム１２および第３アーム１３においても判定結果が、第２アーム１２の場合には、
｜（ω_a2−ω_a1）−ω１×１／Ｎ１）｜＞Ω２ （式１−９）
となった場合に、慣性センサー９１もしくは慣性センサー９２のどちらか一方、もしくは両方が故障していると判定する。

第３アームの場合には、
｜（ω_a3−ω_a2）−ω２×１／Ｎ２）｜＞Ω３ （式１−１０）
となった場合に、慣性センサー９２もしくは慣性センサー９３のどちらか一方、もしくは両方が故障していると判定する。

判定工程（Ｓ１３０）において、故障していない（ＮＯ）と判定された場合には、再び演算工程（Ｓ１１１、Ｓ１１２）に戻り、慣性センサーの故障判定を繰り返す。

判定工程（Ｓ１３０）において、故障（ＹＥＳ）と判定されると、停止指示工程（Ｓ１４０）に移行する。停止指示工程（Ｓ１４０）では、制御部４０００にアクチュエーター５１、５２、５３の作動を停止させる指令が送出され、制御部４０００よりアクチュエーター５１、５２、５３に停止信号が送られ、動作停止となる。すなわちロボット装置１００の作動が停止させられる。その後、故障判定された慣性センサーをロボット装置から取り外し、必要な修理またはセンサー交換が施される。

上述のように慣性センサーの故障の判定基準となるものは、各連結装置に備えるアクチュエーターの角度センサーの検出データから得られる角速度としているので、判定基準そのものもアーム体１０の動作に連動し変動する値であって、より実動作を基準として故障判定されるので、正確な慣性センサーの故障判定が可能となる。これにより、ロボット装置の暴走による危険を確実に回避し、早期の正常化による安定したロボット装置可動を保証し、製品品質を高く維持することができる。

（第２実施形態）
第２実施形態は、第１実施形態に対して慣性センサーの配置と、判定基準となる検出データとが異なる。従って、第１実施形態と共通な構成には同じ符号を付し、説明する。

図４は第２実施形態に係るロボット装置を示し、（ａ）は概略平面図、（ｂ）は概略断面図である。本実施形態のロボット装置は、水平方向に回転可能に３本のアームが連結された、いわゆる３軸水平多関節ロボット２００（以下、ロボット装置２００という）である。

第２実施形態に係るロボット装置２００は、第１実施形態に係るロボット装置１００に対して、慣性センサーが各アームに２個備えられている点で異なる。第１アーム１１には慣性センサー９１ａと９１ｂ、第２アーム１２には慣性センサー９２ａと９２ｂ、第３アーム１３には慣性センサー９３ａと９３ｂが備えられている。これらの慣性センサーは、互いに近接するように配置され、更に規格性能の同等のものが用いられている。

図５は第２実施形態に係るブロック図である。演算部１３００において、アーム体１０に備えられた慣性センサー９１ａ、９１ｂ、９２ａ、９２ｂ、９３ａ、９３ｂから、ｔ１〜ｔｎ時間毎に検出データを取得する。すなわち、
慣性センサー９１ａにおけるｔ１〜ｔｎ時間までのｎ個の検出データ
α_1a1、α_1a2、α_1a3、〜_、α_1an
慣性センサー９１ｂにおけるｔ１〜ｔｎ時間までのｎ個の検出データ
α_1b1、α_1b2、α_1b3、〜_、α_1bn
慣性センサー９２ａにおけるｔ１〜ｔｎ時間までのｎ個の検出データ
α_2a1、α_2a2、α_2a3、〜_、α_2an
慣性センサー９２ｂにおけるｔ１〜ｔｎ時間までのｎ個の検出データ
α_2b1、α_2b2、α_2b3、〜_、α_2bn
慣性センサー９３ａにおけるｔ１〜ｔｎ時間までのｎ個の検出データ
α_3a1、α_3a2、α_3a3、〜_、α_3an
慣性センサー９３ｂにおけるｔ１〜ｔｎ時間までのｎ個の検出データ
α_3ba1、α_3b2、α_3b3、〜_、α_3bnを作成する。

取得された各慣性センサー９１ａ、９１ｂ、９２ａ、９２ｂ、９３ａ、９３ｂの各ｎ個の検出データの平均値を計算する。すなわち、
慣性センサー９１ａの検出データ平均α_1am
α_1am＝（α_1a1＋α_1a2＋α_1a3＋〜＋α_1an）／ｎ
慣性センサー９１ｂの検出データ平均α_1bm
α_1bm＝（α_1b1＋α_1b2＋α_1b3＋〜＋α_1bn）／ｎ
慣性センサー９２ａの検出データ平均α_2am
α_2am＝（α_2a1＋α_2a2＋α_2a3＋〜＋α_2an）／ｎ
慣性センサー９２ｂの検出データ平均α_2bm
α_2bm＝（α_2b1＋α_2ba2＋α_2b3＋〜＋α_2bn）／ｎ
慣性センサー９３ａの検出データ平均α_3am
α_3am＝（α_3a1＋α_3a2＋α_3a3＋〜＋α_3an）／ｎ
慣性センサー９３ｂの検出データ平均α_3bm
α_3bm＝（α_3b1＋α_3b2＋α_3b3＋〜＋α_3bn）／ｎ
となる。なお、データ数ｎは、１００以上を取得することが好ましく、演算のＣＰＵの性能などの条件から適宜、データ数ｎを決定すれば良い。

比較部２１００では、得られた各検出データ平均値より、近接する慣性センサーの検出データ平均値の差の絶対値を求める。例えば、第１アーム１１においては慣性センサー９１ａと９１ｂが近接する慣性センサーであり、その検出データ平均値の差の絶対値α_1d、すなわち、
｜α_1am―α_1bm｜＝α_1d （式２−１）
を求める。同様に第２アーム１２、第３アーム１３も近接する慣性センサーの検出データ平均値の差の絶対値α_2d、α_3dが、
｜α_2am―α_2bm｜＝α_2d （式２−２）
｜α_3am―α_3bm｜＝α_3d （式２−３）
と求められる。

次に、判定部３１００において、比較部２１００において求められた、比較結果としての各慣性センサーの検出データ平均値の差の絶対値α_1d、α_2d、α_3dから、対象となる慣性センサーが正常か故障かを判定する。判定部３１００では、予め故障判定のための閾値を図示しない記憶装置に格納し、閾値に対して比較結果の大小にて、正常、故障の判断を行う。

閾値について説明する。閾値は、各慣性センサーの特性から次のように決定される。すなわち、ロボット装置２００に取付ける前の慣性センサー９１ａ、９１ｂ、９２ａ、９２ｂ、９３ａ、９３ｂ、もしくは取り付ける慣性センサー９１ａ、９１ｂ、９２ａ、９２ｂ、９３ａ、９３ｂと同じ規格の未使用の慣性センサーを用いて、各慣性センサーのホワイトノイズ（白色雑音）を測定する。ホワイトノイズは、例えば、図６に示すように不規則に振動する波形を有している。測定される各慣性センサーのホワイトノイズの標準偏差を求める。

例えば、第１アーム１１に備える慣性センサー９１ａ、９１ｂに対する閾値は、ロボット装置２００に取り付ける前の慣性センサー９１ａ、９１ｂ、もしくは慣性センサー９１ａ、９１ｂと同じ規格の未使用の慣性センサーを用いて計測されたホワイトノイズの値から、標準偏差σ１を求める。求められた標準偏差σ１を基準として、第１アーム１１に備える慣性センサー９１ａ、９１ｂの故障判定の閾値Ｓ１を、
Ｓ１＝６×σ１
に設定する。

同様に、第２アーム１２に備える慣性センサー９２ａ、９２ｂのホワイトノイズの標準偏差σ２から閾値Ｓ２を、第３アーム１３に備える慣性センサー９３ａ、９３ｂのホワイトノイズの標準偏差σ３から閾値Ｓ３を、
Ｓ２＝６×σ２
Ｓ３＝６×σ３
に設定する。

このように設定される閾値Ｓ１、Ｓ２、Ｓ３と比較部２１００によって得られた（式２−１）（式２−２）（式２−３）で求められる、各慣性センサーの検出データ平均値の差の絶対値α_1d、α_2d、α_3dの大小により故障判定する。すなわち、α_1d＞Ｓ１である場合には、慣性センサー９１ａ、９１ｂのどちらか一方もしくは両方が故障と判断し、α_2d＞Ｓ２である場合には、慣性センサー９２ａ、９２ｂのどちらか一方もしくは両方が故障と判断し、α_3d＞Ｓ３である場合には、慣性センサー９３ａ、９３ｂのどちらか一方もしくは両方が故障と判断する。

次に、第２実施形態に係る制御方法について説明する。図７は、第２実施形態に係る制御方法を説明するフローチャートである。先ず、演算工程（Ｓ２１０）において、慣性センサーからの検出データを取得し、検出データの平均値α_1am、α_1bm、α_2am、α_2bam、α_3am、α_3bmを演算する。

次に、比較工程（Ｓ２２０）に移行する。比較工程（Ｓ２２０）では、演算工程（Ｓ２１０）により演算された検出データの平均値α_1am、α_1bm、α_2am、α_2bam、α_3am、α_3bmより、比較値として、近接する慣性センサー９１ａと９１ｂとの検出データの平均値α_1amとα_1bmとの差の絶対値α_1dを演算する（式２−１）。同様に、近接する慣性センサー９２ａと９２ｂとの検出データの平均値α_2amとα_2bmとの差の絶対値α_2d（式２−２）、近接する慣性センサー９３ａと９３ｂとの検出データの平均値α_3amとα_3bmとの差の絶対値α_3d（式２−３）、とを比較値として演算する。

次に、判定工程（Ｓ２３０）に移行する。判定工程（Ｓ２３０）では、予め図示しない記憶装置に格納されている閾値Ｓ１、Ｓ２、Ｓ３に対して、比較工程（Ｓ２２０）からの演算結果が大きいか、を判定する。判定結果が「ＹＥＳ」である場合、すなわち比較工程（Ｓ２２０）からの演算結果が閾値より大きい場合、該当する慣性センサーが故障と判定する。

第１アーム１１を例に説明する。第１アーム１１では、慣性センサー９１ａ、９１ｂが判定の対象となり、検出データ平均値の差の絶対値α_1dが閾値Ｓ１に対して、
α_1d＞Ｓ１
となった場合に、慣性センサー９１ａ、９１ｂのどちらか一方もしくは両方が故障していると判定される。

同様に、第２アーム１２では、
α_2d＞Ｓ２
となった場合に、慣性センサー９２ａ、９２ｂのどちらか一方もしくは両方が故障していると判定され、第３アーム１３では、
α_3d＞Ｓ３
となった場合に、慣性センサー９３ａ、９３ｂのどちらか一方もしくは両方が故障していると判定される。

判定工程（Ｓ２３０）において、故障していない「ＮＯ」と判定された場合には、再び演算工程（Ｓ２１０）に戻り、慣性センサーの故障判定を繰り返す。

判定工程（Ｓ２３０）において、故障（ＹＥＳ）と判定されると、停止指示工程（Ｓ２４０）に移行する。停止指示工程（Ｓ２４０）では、制御部４０００にアクチュエーター５１、５２、５３の作動を停止させる指令が送出され、制御部４０００よりアクチュエーター５１、５２、５３に停止信号が送られ、動作停止となる。すなわちロボット装置２００の作動が停止させられる。その後、故障判定された慣性センサーをロボット装置から取り外し、必要な修理またはセンサー交換が施される。

上述のように慣性センサーの故障の判定基準となるものは、アームに少なくとも２以上の複数個の慣性センサーを備え、同じアームに備える慣性センサーの出力を比較し、センサーの持つホワイトノイズの標準偏差を判定基準にすることで、正確な慣性センサーの故障判定が可能となる。これにより、ロボット装置の暴走による危険を確実に回避し、早期の正常化による安定したロボット装置可動を保証し、製品品質を高く維持することができる。

（第３実施形態）
第１実施形態および第２実施形態では、いわゆる水平多関節ロボット装置によって説明したが、第３実施形態では、直動式ロボット装置におけるセンサーの故障判定を説明する。

図８は、第３実施形態に係るロボット装置１０００の概略平面図である。また、図９は図８における矢印Ｐ方向からの矢視外観の概略図である。図８はロボット装置１０００の構成を明確にするため、主要部を覆う筐体等の図示を省略してある。ロボット装置１０００は、作業テーブルあるいは床などに固定される基体３００と、基体３００に対して移動可能に固定されたアーム４００と、アーム４００に対して移動可能に固定され加工用ツールもしくは被加工物を保持するワーク保持部５２０を備えるワーク保持装置５００と、を備える。

基体３００は、基体アクチュエーター３２０と、基体アクチュエーター３２０のトルクを所定の減速比で伝達する基体トルク伝達装置３１０と、基体トルク伝達装置３１０の回転軸に連結されるアーム駆動ベルト３４０と、アーム駆動ベルト３４０を引っ張り回転可能に固定する基体プーリー３５０と、を備える。また、基体アクチュエーター３２０には回転角度を検出する角度センサー３３０を備えている。

アーム４００は、アームアクチュエーター４２０と、アームアクチュエーター４２０のトルクを所定の減速比で伝達するアームトルク伝達装置４１０と、アームトルク伝達装置４１０の回転軸に連結されるワーク保持装置駆動ベルト４４０と、ワーク保持装置駆動ベルト４４０を引っ張り回転可能に固定するアームプーリー４５０と、を備える。また、アームアクチュエーター４２０には回転角度を検出する角度センサー４３０を備えている。

アーム４００に移動可能に固定されているワーク保持装置５００には、少なくとも２個の加速度センサー５３０ａ、５３０ｂを備える慣性センサー５３０を備え、図示しない駆動装置によってワーク保持部５２０を駆動する。

また、アーム４００には、基体３００に備えるアーム駆動ベルト３４０に固定されるアーム固定装置４６０を備え、アーム駆動ベルト３４０の作動によってアーム４００を動作させる。また、ワーク保持装置５００には、アーム４００に備えるワーク保持装置駆動ベルト４４０に固定されるワーク保持装置固定装置５１０を備えて、ワーク保持装置駆動ベルト４４０の作動によってワーク保持装置５００を動作させる。

このように構成されたロボット装置１０００は、図８および図９の矢印Ｑ、Ｒで示す方向にアーム４００およびワーク保持装置５００を移動させ、所定の位置で作業させることができる。

図１０は、第３実施形態に係るブロック図である。第１演算部１４００では、角度センサー３３０、４３０によって検出された基体アクチュエーター３２０、アームアクチュエーター４２０の検出データを、基体アクチュエーター３２０の回転角度θａ、アームアクチュエーターの回転角度θ_bに換算する。得られた回転角度θ_a、θ_bを時間で２回微分し、
ｄ²θ_a／ｄｔ²＝ω_a’ （式３−１）
ｄ²θ_b／ｄｔ²＝ω_b’ （式３−２）
の回転角加速度を演算する。

基体アクチュエーター３２０の回転角度を、基体トルク伝達装置３１０を介してアーム駆動ベルト３４０の移動長さに変換する変換係数をＫ１とした場合、アーム駆動ベルト３４０の移動加速度β_aは、
β_a＝ω_a’／Ｋ１ （式３−３）
により求められる。同様に、アームアクチュエーター４２０の回転角度を、アームトルク伝達装置４１０を介してワーク保持装置駆動ベルト４４０の移動長さに変換する変換係数をＫ２とした場合、ワーク保持装置駆動ベルト４４０の移動加速度β_bは、
β_b＝ω_b’／Ｋ２ （式３−４）
により求められる。

第２演算部１５００では、ワーク保持装置５００に備える慣性センサー５３０に含む加速度センサー５３０ａ、５３０ｂが検出したデータから加速度を演算する。ここで、加速度センサー５３０ａが、例えば、図９に示すＱ方向の加速度成分を検出する場合には、他の加速度センサー５３０ｂはＲ方向の加速度を検出する配置とする。もしくは３軸加速度センサーを用いて、直交する２軸方向の検出データを用いることでもよい。

加速度センサー５３０ａのＱ方向の検出データから換算された加速度α_a、加速度センサー５３０ｂのＲ方向の検出データから換算された加速度α_bを得る。

比較部２２００では、第１演算部１４００において得られた駆動ベルトの移動加速度β_a、β_bと、第２演算部１５００において得られたワーク保持装置５００の加速度α_a、α_bと、を比較する。比較部２２００における比較とは、加速度差の絶対値を算出するもので、図８に示すＱ方向は、
｜α_a−β_a｜
となり、Ｒ方向は、
｜α_b−β_b｜となる。

判定部３２００では、上述の比較部２２００から出力される比較結果、すなわち加速度差の絶対値が閾値より大きい場合に対象となる慣性センサーが故障であると判定する。ここで、閾値について説明する。

先ず、加速度センサー５３０ａの故障判断の閾値Ａ１は次のように設定される。ロボット装置１０００に対して最大負荷の１２０％の負荷を持たせ、ワーク保持装置の図８に示すＱ方向への最大負荷時の許容加速度の１２０％の加速度によって、ワーク保持装置５００をＱ方向に動作させる。動作開始からＴ時間後の、加速度センサー５３０ａの出力値から求められるワーク保持装置５００の加速度α_atと、ワーク保持装置５００をＱ方向に移動させる基体アクチュエーター３２０に備える角度センサー３３０の回転角度から演算されるアーム駆動ベルト３４０の移動加速度β_atとの差を求める。このα_atとβ_atとの差の絶対値の最大値の２倍が閾値Ａ１となる。すなわち、
Ａ１＝２×（｜α_at−β_at｜）_max （式３−５）
が、閾値Ａ１として得られる。

同様に、加速度センサー５３０ｂの故障判断の閾値Ａ２は、ロボット装置１０００に対して最大負荷の１２０％の負荷を持たせ、ワーク保持装置の図８に示すＲ方向への最大負荷時の許容加速度の１２０％の加速度によって、ワーク保持装置５００をＲ方向に動作させる。動作開始からＴ時間後の、加速度センサー５３０ｂの出力値から求められるワーク保持装置５００の加速度α_btと、ワーク保持装置５００をＲ方向に移動させるアームアクチュエーター４２０に備える角度センサー４３０の回転角度から演算されるワーク保持装置駆動ベルト４４０の移動加速度β_btとの差を求める。このα_btとβ_btとの差の絶対値の最大値の２倍が閾値Ａ２となる。すなわち、
Ａ２＝２×（｜α_bt−β_bt｜）_max （式３−６）
が、閾値Ａ２として得られる。

判定部３２００は、（式３−５）、（式３−６）で求めた閾値Ａ１、Ａ２と、比較部２２００から得られる比較結果と、を用いて故障判定を実行する。加速度センサー５３０ａに対しては、
｜α_a−β_a｜＞Ａ１
となった時、加速度センサー５３０ａが故障と判定する。同様に、加速度センサー５３０ｂに対しては、
｜α_b−β_b｜＞Ａ２
となった時に加速度センサー５３０ｂが故障と判定する。

判定部３２００において、加速度センサー５３０ａもしくは５３０ｂが故障と判定され場合には、制御部４０００に対して基体アクチュエーター３２０もしくはアームアクチュエーター４２０の停止の指示が送られ、制御部４０００は基体アクチュエーター３２０もしくはアームアクチュエーター４２０を停止させる。これにより、ロボット装置１０００は安全に停止させることができる。

次に、第３実施形態に係る制御方法について説明する。第３実施形態に係る制御方法を示すフローチャートは、第１実施形態と同様であり、図３に示すフローチャートが適用できる。従って、第１実施形態と同じ部分の詳細説明は省略し、第１実施形態と異なる点を説明する。

第３実施形態に係る制御方法では、第１演算工程（Ｓ１１１）においては、基体アクチュエーター３２０に備えた角度センサー３３０からの回転角度データを演算し、アーム駆動ベルト３４０の移動加速度β_aと、アームアクチュエーター４２０に備えた角度センサー４３０からの回転角度を演算し、ワーク保持装置駆動ベルト４４０の移動加速度β_bと、を演算し、比較工程（Ｓ１２０）に移行する。

第２演算工程（Ｓ１１２）においては、ワーク保持装置５００に備える慣性センサー５３０に含む加速度センサー５３０ａ、５３０ｂが検出したデータから加速度α_a、α_bを演算し、比較工程（Ｓ１２０）に移行する。

次に、比較工程（Ｓ１２０）に移行する。比較工程（Ｓ１２０）では、第１演算工程（Ｓ１１１）および第２演算工程（Ｓ１１２）において得られた加速度値を用いて、比較結果として、
｜α_a−β_a｜
｜α_b−β_b｜
の値を、判定工程（Ｓ１３０）へ出力、移行する。

次に、判定工程（Ｓ１３０）へ移行する。判定工程（Ｓ１３０）では、予め図示しない記憶装置に格納された閾値Ａ１、Ａ２に対して、比較工程（Ｓ１２０）から入力した比較結果が大きいか、を判定する。判定結果が「ＹＥＳ」である場合、すなわち比較工程（Ｓ１２０）からの比較結果が閾値より大きい場合、該当する加速度センサーが故障と判定する。

すなわち、
｜α_a−β_a｜＞Ａ１
もしくは、
｜α_b−β_b｜＞Ａ２
である時、加速度センサー５３０ａもしくは５３０ｂは故障と判定する。

判定工程（Ｓ１３０）において、交渉判定すなわち「ＹＥＳ」と判定された場合、停止指示工程（Ｓ１４０）に移行する。停止指示工程（Ｓ１４０）では、制御部４０００から基体アクチュエーター３２０、アームアクチュエーター４２０に対して動作を停止させる指示が出力され、ロボット装置１０００は停止する。

上述のように慣性センサーの故障の判定基準となるものは、基体およびアームに備えるアクチュエーターの角度センサーの検出データから得られる駆動ベルトの移動加速度としているので、判定基準そのものもワーク保持装置５００の動作に連動し変動する値であって、より実動作を基準として故障判定されるので、正確な加速度センサーの故障判定が可能となる。これにより、ロボット装置の暴走による危険を確実に回避し、早期の正常化による安定したロボット装置可動を保証し、製品品質を高く維持することができる。

１１…第１アーム、１２…第２アーム、１３…第３アーム、４０…基体、５１，５２，５３…アクチュエーター、８１，８２，８３…角度センサー、９１，９２，９３…慣性センサー、１１００…第１演算部、１２００…第２演算部、２０００…比較部、３０００…判定部、４０００…制御部。

Claims (12)

1. アクチュエーターと、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アクチュエーターの回転角度を検出する角度センサーと、を含むアーム連結装置と、
   複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
   前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アクチュエーターの回転角度を検出する角度センサーと、を含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、
   前記アームに取り付けられた慣性センサーと、
   前記角度センサーからの前記アクチュエーターの回転角度データから前記アクチュエーターの角速度および角加速度を演算する第１演算部と、
   前記慣性センサーにより検出された出力より、前記アームの角速度または角加速度を演算する第２演算部と、
   前記第１演算部により演算された前記アクチュエーターの角速度または角加速度と、前記第２演算部により演算された前記アームの角速度または角加速度とを比較する比較部と、
   前記比較部において前記アクチュエーターの角速度または角加速度と前記アームの角速度または角加速度との差の絶対値が、閾値Ωより大きい場合に前記慣性センサーが故障と判定し前記アクチュエーターの動作を停止させる信号を出力する故障判定部と、を備える、
   ことを特徴とするロボット装置。
2. 前記閾値Ωは、前記ロボット装置の最大負荷の１２０％の負荷を持たせ、最大負荷時の許容加速度１２０％の加速度で動作させた場合に、前記第１演算部により演算される角速度がω_S、前記第２演算部により演算される角速度がω_Lの場合、ω_Sとω_Lの差の絶対値の最大値に対して、
   Ω＝２×（｜ω_S−ω_L｜）_max
   である、
   ことを特徴とする請求項１に記載のロボット装置。
3. アクチュエーターと、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、を含むアーム連結装置と、
   複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
   前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、を含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、
   前記アームに取り付けられた少なくとも２個の慣性センサーと、
   前記慣性センサーの検出した複数の出力値を記憶し、平均値を演算する演算部と、
   前記演算部により演算された前記出力値の平均値を比較する比較部と、
   前記比較部において、２個の前記出力値の平均値の差の絶対値の中に、閾値Ｓより大きい前記差が在る場合に、前記慣性センサーのいずれか１個以上が故障と判定し、前記アクチュエーターの動作を停止させる信号を出力する故障判定部と、を備える、
   ことを特徴とするロボット装置。
4. 前記閾値Ｓは、前記慣性センサーの白色雑音の標準偏差σであるとき、
   Ｓ＝６σ
   であることを特徴とする請求項３に記載のロボット装置。
5. 基体アクチュエーターと、前記基体アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記基体アクチュエーターの回転角度を検出する基体角度センサーと、
   アーム連結装置を備え、前記トルク伝達機構に連結され、前記アーム連結装置を直線往復駆動させるアーム駆動装置と、を含む基体と、
   前記基体の前記アーム連結装置に連結され、アームアクチュエーターと、前記アームアクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アームアクチュエーターの回転角度を検出するアーム角度センサーと、
   ワーク保持装置を備え、前記アーム連結装置に連結され、前記ワーク保持装置を直線往復駆動させるワーク保持装置駆動装置と、を含むアームと、
   前記ワーク保持装置に取り付けられた少なくとも加速度センサーを含む２以上の慣性センサーと、
   前記基体角度センサーからの前記基体アクチュエーターの回転角度データから前記アーム駆動装置の加速度と、前記アーム角度センサーからの前記アームアクチュエーターの回転角度データから前記ワーク保持装置駆動装置の加速度と、を演算する第１演算部と、
   前記慣性センサーにより検出された出力から、前記ワーク保持装置の加速度を演算する第２演算部と、
   前記第１演算部により演算された前記加速度と、前記第２演算部により演算された前記加速度とを比較する比較部と、
   前記第１演算部により演算された前記加速度と、前記第２演算部により演算された前記加速度との差の絶対値が、閾値Ａより大きい場合に前記慣性センサーが故障と判定し前記基体アクチュエーターおよび前記アームアクチュエーターの動作を停止させる信号を出力する故障判定部と、を備える、
   ことを特徴とするロボット装置。
6. 前記閾値Ａは、前記ロボット装置の最大負荷の１２０％の負荷を持たせ、最大負荷時の許容加速度１２０％の加速度で動作させた場合に、前記第１演算部により演算される加速度がα_S、前記第２演算部により演算される加速度がα_Lの場合、α_Sとα_Lの差の絶対値の最大値に対して、
   Ａ＝２×（｜α_S−α_L）｜ｍａｘ
   である、
   ことを特徴とする請求項５に記載のロボット装置。
7. アクチュエーターと、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アクチュエーターの回転角度を検出する角度センサーと、を含むアーム連結装置と、
   複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
   前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アクチュエーターの回転角度を検出する角度センサーと、を含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、を備えるロボット装置の制御方法であって、
   前記アームに取り付けられた慣性センサーと、
   前記角度センサーからの前記アクチュエーターの回転角度データから前記アクチュエーターの角速度および角加速度と、前記慣性センサーにより検出された出力より、前記アームの角速度または角加速度と、を演算する演算工程と、
   前記演算工程による演算された前記アクチュエーターの角速度または角加速度と、前記アームの角速度または角加速度とを比較する比較工程と、
   前記比較工程において前記アクチュエーターの角速度または角加速度と前記アームの角速度または角加速度との差の絶対値が、閾値Ωより大きい場合に前記慣性センサーが故障と判定する判定工程と、
   前記判定工程において故障と判断した場合に、前記アクチュエーターの動作を停止させる信号を出力する停止指示工程と、を備える、
   ことを特徴とするロボット装置の制御方法。
8. 前記閾値Ωは、前記ロボット装置の最大負荷の１２０％の負荷を持たせ、最大負荷時の許容加速度１２０％の加速度で動作させた場合に、前記演算工程において演算される前記アクチュエーターの角速度がω_S、前記アームの角速度がω_Lの場合、ω_Sとω_Lの差の絶対値の最大値に対して、
   Ω＝２×（｜ω_S−ω_L｜）_max
   である、
   ことを特徴とする請求項７に記載のロボット装置の制御方法。
9. アクチュエーターと、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、を含むアーム連結装置と、
   複数のアームが前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
   前記アーム体の一方の端部に、前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、を含む基体連結装置が設けられ、前記基体連結装置により前記アーム体が回転可能に連結された基体と、を備えるロボット装置の制御方法であって、
   前記アームに取り付けられた少なくとも２個の慣性センサーと、
   前記慣性センサーの検出した複数の出力値を記憶し、平均値を演算する演算工程と、
   前記演算工程により演算された前記出力値の平均値を比較する比較工程と、
   前記比較工程において、２個の前記出力値の平均値の差の絶対値の中に、閾値Ｓより大きい前記差が在る場合に、前記慣性センサーのいずれか１個以上が故障と判定する判定工程と、
   前記判定工程において故障と判定された場合に、前記アクチュエーターの動作を停止させる信号を出力する停止指示工程と、を備える、
   ことを特徴とするロボット装置の制御方法。
10. 前記閾値Ｓは、前記慣性センサーの白色雑音の標準偏差σであるとき、
    Ｓ＝６σ
    であることを特徴とする請求項９に記載のロボット装置の制御方法。
11. 基体アクチュエーターと、前記基体アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記基体アクチュエーターの回転角度を検出する基体角度センサーと、
    アーム連結装置を備え、前記トルク伝達機構に連結され、前記アーム連結装置を直線往復駆動させるアーム駆動装置と、を含む基体と、
    前記基体の前記アーム連結装置に連結され、アームアクチュエーターと、前記アームアクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構と、前記アームアクチュエーターの回転角度を検出するアーム角度センサーと、
    ワーク保持装置を備え、前記アーム連結装置に連結され、前記ワーク保持装置を直線往復駆動させるワーク保持装置駆動装置と、を含むアームと、を備えるロボット装置の制御方法であって、
    前記基体角度センサーからの前記基体アクチュエーターの回転角度データから前記アーム駆動装置の加速度と、前記アーム角度センサーからの前記アームアクチュエーターの回転角度データから前記ワーク保持装置駆動装置の加速度と、
    前記ワーク保持装置に取り付けられた少なくとも加速度センサーを含む１以上の慣性センサーにより検出された出力から、前記ワーク保持装置の速度および加速度と、を演算する演算工程と、
    前記演算工程により演算された前記アーム駆動装置、前記ワーク保持装置駆動装置の加速度と、前記ワーク保持装置の加速度と、を比較する比較工程と、
    前記比較工程において、前記角度センサーの出力値より演算された前記アーム駆動装置、前記ワーク保持装置駆動装置の加速度と、前記慣性センサーの出力値より演算された前記ワーク保持装置の加速度との差の絶対値が、閾値Ａより大きい場合に前記慣性センサーが故障と判定する判定工程と、
    前記判定工程において故障と判定された場合に、前記基体アクチュエーターおよび前記アームアクチュエーターの動作を停止させる信号を出力する停止指示工程と、を備える、
    ことを特徴とするロボット装置の制御方法。
12. 前記閾値Ａは、前記ロボット装置の最大負荷の１２０％の負荷を持たせ、最大負荷時の許容加速度１２０％の加速度で動作させた場合に、前記演算工程により演算される前記アーム駆動装置もしくは前記ワーク保持装置駆動装置の加速度がα_S、前記ワーク保持装置の加速度がα_Lの場合、α_Sとα_Lの差の絶対値の最大値に対して、
    Ａ＝２×（｜α_S−α_L）｜ｍａｘ
    である、
    ことを特徴とする請求項１１に記載のロボット装置の制御方法。

Images