(57)【特許請求の範囲】
【請求項１】
関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、
動作指令の角度成分を近似微分して第１の角速度を算出する工程と、
前記第１の角速度を前記慣性センサーの取り付け位置における第２の角速度に変換する工程と、
前記慣性センサーによって第３の角速度を取得する工程と、
前記第２の角速度に基づき前記慣性センサーの異常を判定する閾値を決定する工程と、
前記第３の角速度と、前記閾値とを比較する工程と、
を含むことを特徴とするロボットの故障検出方法。
【請求項２】
関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットを制御する制御装置であって、
動作指令の角度成分から第１の角速度を算出する演算部と、
前記第１の角速度を前記慣性センサーの取り付け位置における第２の角速度に変換する変換部と、
前記第２の角速度に基づき前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、
前記慣性センサーによって取得される第３の角速度と、前記閾値とを比較する比較部と、
を含むことを特徴とする制御装置。
【請求項３】
前記慣性センサーは、ジャイロセンサーであることを特徴とする請求項２に記載の制御装置。
【請求項４】
前記第３の角速度が前記閾値よりも高い場合、前記ロボットの駆動を停止させることを特徴とする請求項２または３に記載の制御装置。
【請求項５】
請求項２〜４のいずれか一項に記載の制御装置によって制御されることを特徴とするロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの故障検出方法、制御装置およびロボットに関する。
【背景技術】
【０００２】
ワークの搬送や組み立て等の作業において用いられる多関節構造を有するロボットは、近年多方面で採用されている。最近では、これらのロボットは、より高速、かつ正確にアームを移動させることが要求されてきている。そこで、関節や関節に連結されるアームに、角度センサーおよび慣性センサーを配設して、動作指令に基づく移動経路を所定の速度で駆動している場合がある。しかし、角度センサーや慣性センサーが故障した場合には、正確な移動制御ができなくなる。
【０００３】
そこで、モーターに配設されるエンコーダーによって得た回転角度から算出した加速度および加速度センサーから得た加速度データと、加速度指令と、の差分を算出し、これらの差分が予め設定された閾値よりも大きくなった場合に、伝達系の異常があったと判断するロボットの故障検出方法が提案されている（たとえば、特許文献１参照）。
【０００４】
また、アームの運動状態量を検出するジャイロセンサーによって取得した状態量を直交座標系の状態量に変換して第１のセンサー値とし、関節角度を検出する角度センサーによって検出した関節角度を直交座標系の状態量に変換して第２のセンサー値とし、第１のセンサー値と第２のセンサー値とを合成して出力合成値を取得することによって、ロボットの状態量を把握するとともに、作業者は直交指令と出力合成値とを比較して故障の有無を判定するというロボットの評価方法が提案されている（たとえば、特許文献２参照）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１０−６４２３２号公報
【特許文献２】特表ＷＯ２００６／０２２２０１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上述した特許文献１では、エンコーダーから得た角度を２回微分して加速度を算出している。加速度センサーとしては、具体的にはジャイロセンサーを用いて得られた角速度を微分して加速度を算出する。このように、各センサーから得られたセンサーデータのそれぞれを微分して加速度を算出することから誤差が大きくなってしまう課題を有している。
【０００７】
また、特許文献２では、ジャイロセンサーによって得たセンサーデータと、角度センサーによって得たセンサーデータと、の両方を直交座標に変換し、かつ合成した出力合成値を評価しているため、直交座標への変換計算、合成計算等、演算量が多くなり、演算部の負荷が大きくなるという課題がある。
さらに、作業者が直交指令と出力合成値とを比較して故障の有無を判定し、ロボットを停止させることから、リアルタイムでの故障検出は困難である。
【課題を解決するための手段】
【０００８】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【０００９】
［適用例１］本適用例に係るロボットの故障検出方法は、関節と、前記関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、前記角度センサーまたは前記慣性センサーの異常を判定する閾値を決定する工程と、前記角度センサーによって関節角度を取得する工程と、前記関節角度を近似微分して関節角速度を算出する工程と、前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する工程と、前記慣性センサーによって角速度を取得する工程と、前記慣性センサーにより取得した角速度と前記座標系角速度との角速度差、または角速度比を演算する工程と、前記角速度差、または角速度比と、前記閾値とを比較する工程と、を含むことを特徴とする。
【００１０】
本適用例によれば、角度センサーによって取得した関節角度を関節角速度に変換した後、慣性センサーの取り付け位置における座標系角速度に変換し、この座標系角速度と、慣性センサーから得られた角速度との角速度差を、予め動作指令に基づき決定された閾値と比較し、たとえば、角速度差＜閾値である場合は、角度センサーおよび慣性センサーは正常であると判定し、所定の作業を継続する。また、角速度差＞閾値である場合には、角度センサーまたは慣性センサーのどちらかが故障したと判定して直ちにロボットの駆動を停止させる。従って、特許文献１にように、得られたセンサーデータを２回微分して加速度に変換する必要がないので誤差が小さいという効果がある。
【００１１】
また、関節角速度を慣性センサーの座標系角速度への変換処理は一度でよいので、前述した特許文献２よりも演算量が少なくなり、演算に係る時間も短縮できる。さらに、角速度差と閾値との差を比較し、角速度差＞閾値の場合は故障と判定し直ちにロボットの可動を停止すれば、品質の低下を抑え、安全性を高めることができる。
なお、慣性センサーによって取得した角速度と座標系角速度との角速度比（割合）と、閾値（割合）とを、比較する方法であっても、角速度差を比較する方法と同様な効果が得られる。
【００１２】
［適用例２］上記適用例に係るロボットの故障検出方法において、動作指令角速度または動作指令角加速度の大きさによって、前記閾値を切り替える、ことが好ましい。
【００１３】
アームの加減速が大きい場合にはアームの振動が大きくなり、加減速が小さい場合にはアームの振動が小さくなる。よって、動作指令角速度が大きい場合には閾値も大きくし、動作指令角速度が小さい場合には閾値も小さくすれば、アームの加減速に合わせた適切な故障検出を行うことができる。
【００１４】
［適用例３］上記適用例に係るロボットの故障検出方法において、前記アームに発生する振動の大きさと前記閾値の対照表を備え、前記対照表を参照し、振動の値に対応させて予め設定された前記閾値を選択し、前記閾値の切り替えを行う、ことが好ましい。
【００１５】
このように、アームの姿勢および角速度によってアームにどれくらいの大きさの振動が発生するかを予め測定し、振動の大きさと閾値の対照表を供え、振動の大きさによって適切な閾値を選択すれば、動作指令角速度の大小によって閾値を切り替える方法と、同様な効果が得られる。
【００１６】
［適用例４］本適用例に係るロボットの故障検出方法は、関節と、前記関節に連結されるアームと、前記アームに取り付けられた慣性センサー及び撮像手段と、を有するロボットの故障検出方法であって、前記座標系角速度を、前記アームに取り付けられた撮像手段が取得した画像の移動前と移動後の差分情報を用いて算出した画像取得位置の角速度を、前記慣性センサーの取り付け位置の角速度に座標変換して座標系角速度を算出する工程と、前記角速度と、前記座標系角速度とを比較する工程と、を有すること、を特徴とする。
【００１７】
アームに、たとえばカメラ等の撮像手段を配設し、撮像手段によってアームの移動前と移動後の画像の差分情報を用いて算出した角速度を、慣性センサー位置の角速度に座標変換して座標系角速度とし、慣性センサーが取得した角速度と座標系角速度との角速度と、閾値とを比較する。そして、角速度差＞閾値となった場合には、慣性センサーが異常であることを検出できる。
なお、本適用例には、慣性センサーによって取得した角速度と、撮像手段から取得した座標系角速度との比（割合と）閾値とを比較する方法も含まれる。
【００１８】
［適用例５］本適用例に係るロボットの故障検出方法は、関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、前記慣性センサーによって角速度を取得する工程と、動作指令の角度成分を近似微分して関節角速度を算出する工程と、前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する工程と、前記慣性センサーの異常を判定する閾値を決定する工程と、前記慣性センサーによって取得した角速度と、前記閾値とを比較する工程と、を含むことを特徴とする。
【００１９】
このように、慣性センサーから取得した角速度と、動作指令の角度成分から算出した慣性センサー位置の座標系角速度に基づき決定した閾値とを比較して、慣性センサーから取得した角速度が閾値よりも大きい場合には、慣性センサーが故障したと判定することができる。
【００２０】
［適用例６］本適用例に係るロボットは、関節と、前記関節に連結されるアームと、前記関節の関節角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、前記角度センサーまたは前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、前記関節角度を近似微分して関節角速度を算出する近似微分演算部と、前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する座標変換部と、前記慣性センサーによって取得した角速度と前記座標系角速度とを比較する比較部と、前記比較部の比較結果に基づき故障の有無を判定する故障判定部と、を有することを特徴とする。
【００２１】
本適用例によれば、角度センサーによって取得した関節角度を関節角速度に変換した後、慣性センサーの取り付け位置の座標系角速度に変換し、この座標系角速度と、慣性センサーから得られた角速度との角速度差を、予め決定されている閾値と比較し、たとえば、角速度差＜閾値である場合は、角度センサーおよび慣性センサーは正常であると判定し、所定の作業を継続する。また、角速度差＞閾値である場合には、角度センサーまたは慣性センサーのどちらかが故障したと判定して直ちにロボットの駆動を停止させる。従って、特許文献１にように、得られたセンサーデータを２回微分して加速度に変換する必要がないので誤差が小さいという効果がある。
【００２２】
また、関節角速度を慣性センサーの座標系角速度への変換処理は一度でよいので、前述した特許文献２よりも演算量が少なくなり、演算に係る時間も短縮できる。さらに、角速度差と閾値との差を比較し、角速度差＞閾値の場合は故障と判定し直ちにロボットの可動を停止すれば、品質の低下を抑え、安全性を高めることができる。
なお、慣性センサーによって取得した角速度と座標系角速度との角速度比（割合）と、閾値（割合）とを、比較する方法であっても、角速度差を比較する方法と同様な効果が得られる。
【００２３】
［適用例７］本記適用例に係るロボットは、関節と、前記関節に連結されるアームと、前記アームに取り付けられた慣性センサー及び撮像手段と、前記撮像手段が取得した画像の移動前と移動後の角度の差分情報を用いて、画像取得位置の角速度に変換する近似微分演算部と、前記角速度を前記慣性センサーの取り付け位置の座標系角速度に変換する座標変換部と、前記座標系角速度と前記慣性センサーが取得した角速度とを比較し、角速度差を算出する比較部と、前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、前記角速度差と前記閾値とを比較して故障の有無を判定する故障判定部と、を有することを特徴とする。
【００２４】
このような構成によれば、アームに、たとえばカメラ等の撮像手段を配設し、撮像手段によってアームの動きの前後の画像の差分情報を用いて移動角度を算出し、慣性センサーの取り付け位置の角速度に座標変換して座標系角速度とし、閾値との比較によって、角速度差＞閾値となった場合には、慣性センサーの故障を検出することができる。
なお、本適用例には、慣性センサーによって取得した角速度と、撮像手段から取得した座標系角速度との比（割合と）閾値とを比較する方法も含まれる。
【図面の簡単な説明】
【００２５】
【図１】実施形態１に係るロボットの１例を表す模式図。
【図２】制御装置の主要構成を示す構成説明図。
【図３】指令の角速度成分と、エンコーダーから算出した角速度と、ジャイロセンサーの角速度成分の出力信号を比較したグラフ。
【図４】図３の一部を拡大したグラフ。
【図５】実施形態に係るロボットの故障検出方法の主たる工程を示すフローチャート。
【図６】動作指令の角速度成分と、エンコーダーから算出した角速度と、ジャイロセンサーが出力した角速度を比較したグラフ。
【図７】角速度差と閾値との関係を表すグラフ。
【図８】動作指令の加減速が大きい場合を表し、（ａ）は動作指令の角速度成分、（ｂ）は角速度差。
【図９】動作指令の加減速が小さい場合を表し、（ａ）は動作指令の角速度成分、（ｂ）は角速度差。
【図１０】実施形態４に係るロボットの１例を表す模式図。
【図１１】実施形態４に係る制御装置の主要構成を示す構成説明図。
【発明を実施するための形態】
【００２６】
以下、本発明の実施形態を図面を参照して説明する。
（実施形態１）
【００２７】
図１は、実施形態１に係るロボットの１例を表す模式図である。ロボット１０は、Ｘ・Ｙ平面を有する基台１１上に、Ｘ・Ｙ平面に垂直に配置される基部１２を有し、基部１２の中心軸（Ｚ軸）を基本座標軸として、複数の関節と、関節間を連結するアームとから構成されている。ここで、図１に表記した円形と四角形は関節を表し、関節に付す矢印は回転方向を表している。基部１２の端部は基台に固定され、関節Ｊ1と関節Ｊ2の間はアーム１３で連結される。
【００２８】
また、関節Ｊ2と関節Ｊ3とはアーム１４で連結され、関節Ｊ3とＪ4とはアーム１５で連結され、関節Ｊ4と関節Ｊ5とはアーム１６で連結され、関節Ｊ5と関節Ｊ6とはアーム１７で連結されている。さらに、関節Ｊ6にはアーム１８が連結されており、アーム１８の終端部（Ｅｎｄｐｏｉｎｔ）には指部（図示せず）を有する。なお、アーム１３，１６，１８は回転軸である。
【００２９】
関節Ｊ1〜Ｊ6にはそれぞれ、モーターと、減速機と、モーターの回転角度（つまり、関節の回転角度：単に関節角度と表す）を検出する角度センサーとしてのエンコーダーと、が備えられている。また、アーム１６には慣性センサーとしてのジャイロセンサー３０が取り付けられており、慣性センサー３０の取り付け位置における状態量を検出する。この状態量とは、姿勢情報、移動方向、角速度を含む。よって、慣性センサー３０は、３軸ジャイロセンサーが用いられることが望ましい。
【００３０】
なお、ジャイロセンサー３０は、本実施形態では、アーム１６に配設しているが、この位置には限定されない。しかし、取り付け可能であれば、Ｅｎｄｐｏｉｎｔに近い位置に配設されることが望ましい。これは、先端部のＥｎｄｐｏｉｎｔでの振動が基部１２付近よりも大きくなるためである。
【００３１】
ロボット１０には、後述する各モーターの駆動制御、エンコーダーによる角度の検出、ジャイロセンサーによる角速度検出、各種演算処理を行う制御装置２０を備えている。そして、ロボット１０は、アーム１３〜１８と、関節Ｊ1〜Ｊ6のモーターおよび減速機と、によって相対的な運動をすることで、Ｅｎｄｐｏｉｎｔ（たとえば指部）において、所定の作業をさせる。
【００３２】
続いて、制御装置２０の構成について説明する。
図２は、制御装置の主要構成を示す構成説明図である。制御装置２０は、エンコーダー４０（ここでは、基部１２の基準座標軸からジャイロセンサー取り付け位置までのエンコーダーの総称）から取得した関節角度を関節角速度に変換する近似微分演算部２１と、得られた関節角速度をジャイロセンサー３０の取り付け位置の座標系角速度に変換する座標変換部２２と、この座標系角速度とジャイロセンサー３０から取得した角速度（ジャイロ角速度と表す）とを比較し、角速度差を算出する比較部２３と、を有する。さらに、動作指令の角速度成分に基づき閾値を決定する閾値決定部２５と、閾値と上記角速度差とを比較して故障の有無を判定する故障判定部２４と、が備えられている。
【００３３】
次に、上述したロボット１０を駆動したときのジャイロ角速度と、エンコーダー４０から取得し算出した角速度と、動作指令の角速度成分と、の関係を比較して説明する。
図３は、動作指令の角速度成分と、エンコーダーから算出した角速度と、ジャイロセンサーの角速度の出力信号と、を比較したグラフ、図４は、図３の一部を拡大したグラフである。ともに、横軸に時間［ｓｅｃ］、縦軸に角速度［ｄｅｇ／ｓ］を表している。なお、図３、図４ともにジャイロセンサー３０およびエンコーダー４０は正常な状態の場合を示している。
【００３４】
図３に示した動作指令の角速度成分は、０．１［ｓｅｃ］の間に＋４５０［ｄｅｇ／ｓ］まで加速させ、そこから０．０８［ｓｅｃ］の間は一定速度を維持し、さらに、０．１［ｓｅｃ］の間に−４５０［ｄｅｇ／ｓ］減速させて速度を０に戻して停止させるというものである。図３に示すように、動作指令の角速度成分に対して、ジャイロ角速度と、エンコーダー４０より算出した角速度（以降、エンコーダー角速度と表すことがある）とは、僅かな遅延はあるものの、ほぼ一致している。
【００３５】
また、図４の拡大図では、ジャイロ角速度は、駆動を停止したときにアーム１６が振動をしていることを示し、エンコーダー４０より算出した角速度とは微妙に異なるが、全体としての角速度の変化はほぼ一致している。
そこで、動作指令の角速度成分に基づき閾値を決定し、閾値とエンコーダー角速度とジャイロ角速度との角速度差を比較して、これら角速度差が閾値よりも大きい場合には故障したと判定する。よって、より適切なロボット１０の故障検出が可能になることが分かる。
（ロボットの故障検出方法）
続いて、上記考え方に基づくロボット１０の故障検出方法について説明する。
図５は、本実施形態に係るロボットの故障検出方法の主たる工程を示すフローチャートである。なお、図２も参照して説明する。まず、ロボット１０の動作を開始し、動作の指令角速度から閾値を決定する（Ｓ１）。次に、エンコーダー４０より関節角度を取得し（Ｓ２）、近似微分演算部２１で関節角度を近似微分して関節角速度を算出し（Ｓ３）、次に、得られた関節角速度を、座標変換部２２でジャイロセンサー３０の取り付け位置の座標系角速度に変換する（Ｓ４）。
【００３６】
次に、上記の座標系角速度の算出方法について説明する。
まず、ジャイロセンサー３０の取り付け位置における姿勢情報と、エンコーダー角速度（関節角度）を用いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列を算出する。
【００３７】
【数１】
0005849451.tif 000002
続いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列と、関節角度から算出した関節角速度を用いて、ジャイロセンサーの取り付け位置における座標系角速度を算出する。
【００３８】
【数２】
0005849451.tif 000003
【００３９】
そして、ジャイロセンサー３０より角速度（ジャイロ角速度または出力角速度））を取得し（Ｓ５）、このジャイロ角速度と、関節角速度から算出した角速度（つまり、座標系角速度）と、の角速度差を演算する（Ｓ６）。そして、この角速度差と動作指令の角速度成分に基づき予め設定した閾値（工程：Ｓ１）とを比較し（Ｓ７）、角速度差が閾値よりも小さい（｜角速度差｜＜｜閾値｜）場合（ＹＥＳ）には、ジャイロセンサー３０およびエンコーダー４０が正常動作であると判定し、モーター駆動を継続する（Ｓ１０）。そして所定の移動動作が終了した時点（Ｓ１１：ＹＥＳ）で動作を終了させる。所定の移動動作が終了しない（ＮＯ）場合には、エンコーダー４０の関節角度取得工程（Ｓ２）から、以降の工程を繰り返す。
【００４０】
工程Ｓ７において、角速度差が閾値よりも大きい（｜角速度差｜＞｜閾値｜）と判定した場合（ＮＯ）には、ジャイロセンサー３０またはエンコーダー４０のいずれかが故障したと判定し、モーターを非常停止させる（Ｓ８）。
【００４１】
以上説明したロボット１０の故障検出方法を用いて故障検出した場合の１例をあげ説明する。
図６は、動作指令の角速度成分と、エンコーダー角速度と、ジャイロ角速度を比較したグラフである。横軸に時間［ｓｅｃ］、縦軸に角速度［ｄｅｇ／ｓ］を表している。図６において、指令の角速度成分とエンコーダー情報から算出した角速度（座標系角速度）と動作指令の角速度成分に対して遅延はあるものの、ほぼ同じである。また、ジャイロ角速度は、０．２３「ｓｅｃ」の位置で急激に０［ｄｅｇ／ｓ］になっている。つまり、ジャイロセンサーの検出値に異常があったことが推定できる。
【００４２】
図７は、角速度差と閾値との関係を表すグラフである。ここで、仮にマイナス側の閾値を−４５［ｄｅｇ／ｓ］とする。なお、図中に記載のねじれ角速度は、ジャイロセンサーの角速度とエンコーダーの検出値から算出した角速度との差（角速度差）である。この例では、０．２３［ｓｅｃ］の位置で、ねじれ角速度の絶対値が閾値の絶対値４５［ｄｅｇ／ｓ］よりも大きくなっている。この位置で、ジャイロセンサー３０が故障したと判定し、モーターの駆動を停止させる。
【００４３】
従って、上述したロボット１０及びロボットの故障検出方法によれば、エンコーダー４０によって取得した関節角度を関節角速度に変換した後、ジャイロセンサー３０の取り付け位置における座標系角速度に変換し、この座標系角速度と、ジャイロセンサー３０から得られた角速度との角速度差を、予め動作指令の角速度成分に基づき決定した閾値と比較し、たとえば、｜角速度差｜＜閾値である場合は、エンコーダー４０およびジャイロセンサー３０は共に正常であると判定し、所定の作業を継続する。また、｜角速度差｜＞｜閾値｜である場合には、エンコーダー４０（基部１２からジャイロセンサー取り付け位置までに配置されたエンコーダーのいずれか）またはジャイロセンサー３０のどちらかが故障したと判定して直ちにロボット１０（モーター）の駆動を停止させる。従って、特許文献１にように、角度センサーから得られたセンサーデータを２回微分して加速度に変換する必要がないので誤差が小さいという効果がある。
【００４４】
また、関節角速度をジャイロセンサーの取り付け位置における座標系角速度への変換処理は一度でよいので、前述した特許文献２よりも演算量が少なくなり、演算に係る時間も短縮できる。さらに、角速度差と閾値との差を比較し、｜角速度差｜＞｜閾値｜の場合は故障と判定し直ちにロボット１０の駆動を停止すれば、品質の低下を抑え、また、安全性を高めることができる。
【００４５】
なお、前述した角速度差と閾値とを判定要素として用いる故障検出方法の他に、慣性センサーにより取得した角速度と座標系角速度との角速度比（割合）を算出し、動作指令から決定する閾値（割合）とを、比較する方法を用いてロボット１０の故障検出を行うことができる。具体的には、図５で表すフローチャートの工程Ｓ１において動作の指令加速度成分から許容できる閾値を割合で決定し、工程Ｓ６において角速度差の代わりに角速度比を演算し、工程Ｓ７において角速度比と閾値とを比較すればよい。このようにして、｜角速度比｜＜｜閾値｜の場合は正常、｜角速度比｜＞｜閾値｜の場合には故障判定とすることができる。
（実施形態２）
【００４６】
続いて、実施形態２について説明する。前述した実施形態１は、閾値を一定にしていることに対し、動作指令の角速度成分の大きさに対応して閾値を切り替えるところに特徴を有する。たとえば、加減速が大きい動作を指令する場合には、アームの振動が大きくなるので閾値を大きくする。一方、加減速が小さい動作を指令する場合には、アームの振動が小さくなるので閾値を小さくする。このことを実測例を図示し説明する。なお、ロボット１０の構成（図１、図２、参照）および故障検出のフローチャート（図５、参照）は実施形態１と同じなので、相違箇所を中心に説明する。
【００４７】
図８は、動作指令の加減速が大きい場合を表し、（ａ）は動作指令の角速度成分、（ｂ）は角速度差を表している。
動作指令の角速度成分は、図８（ａ）に表すように、０．１［ｓｅｃ］間、４５０［ｄｅｇ／ｓ］の角速度まで加速し、それから０．０８［ｓｅｃ］の一定速度域の後、やはり０．１［ｓｅｃ］間で４５０［ｄｅｇ／ｓ］の角速度を減速して、速度を０に戻した例である。
【００４８】
上記条件の動作指令によって動作させたときの角速度差の測定例を図８（ｂ）に示している。このように動作させた場合、アームの振動が大きくなることから角速度差は概ね±１６［ｄｅｇ／ｓ］の範囲で変動する。
次に、動作指令の加減速が小さい場合について説明する。
【００４９】
図９は、動作指令の加減速が小さい場合を表し、（ａ）は動作指令の角速度成分、（ｂ）は角速度差を表している。
動作指令の角速度成分は、図９（ａ）に表すように、０．１５［ｓｅｃ］間で４５０［ｄｅｇ／ｓ］の角速度まで加速し、それから０．０３［ｓｅｃ］の一定速度域の後、やはり０．１５［ｓｅｃ］で４５０［ｄｅｇ／ｓ］の角速度まで減速して、速度を０に戻した例である。
【００５０】
上記条件の動作指令によって動作させたときの角速度差の測定例を図９（ｂ）に示している。このように動作させた場合、アームの振動が小さくなることから角速度差は概ね±１１［ｄｅｇ／ｓ］の範囲で変動する。
【００５１】
図８、図９で表した例が、共に正常であるとした場合、加減速が大きい場合の閾値をたとえば、±２０［ｄｅｇ／ｓ］とし、加減速が小さい場合の閾値をたとえば、±１５［ｄｅｇ／ｓ］とすることができる。
【００５２】
従って、図８、図９に示したように、動作指令の加減速が大きい場合には閾値を大きくし、動作指令の加減速が小さい場合には、閾値を小さくすることができる。このようにすれば、動作指令の加減速の大小に合わせた適切な閾値の設定を行うことができる。このことによって、故障の有無の判定をするために、過剰な安全率を設定しなくてもよいという効果がある。
（実施形態３）
【００５３】
続いて、実施形態３に係るロボット１０の故障検出方法について説明する。前述した実施形態２が、動作指令の加減速の大小に合わせて閾値の切り替えをすることに対し、実施形態３は、予めアームに発生する振動の大きさと閾値の対照表を備え、この対照表を参照して、振動の大きさに合わせて閾値を選択して、閾値の切り替えを行うことに特徴を有する。なお、図示は省略する。
【００５４】
前述したように、アームには振動が発生する。図８（ｂ）、図９（ｂ）では、角速度差の変動を表しているが、この角速度差の変動はアームの振動に起因している。従って、アームの姿勢および角速度によってアームにどれくらい振動が発生するかを予め測定し、振動の大きさに対応する閾値の対照表を作成し、この対照表から適切な閾値を選択すれば、動作指令の角速度成分の大小によって閾値を切り替える方法と、同様な効果が得られる。
（実施形態４）
【００５５】
次に、実施形態４について説明する。前述した実施形態１〜実施形態３が、ジャイロセンサーとエンコーダーから得た角速度を用いていることに対し、実施形態４は、アームに取り付けられた撮像手段が取得した画像から角速度を算出してジャイロセンサーとの角速度差と閾値とを比較して故障を検出することに特徴を有する。
【００５６】
図１０は、実施形態４に係るロボットの１例を表す模式図、図１１は制御装置の主要構成を示す構成説明図である。図１０に示すように、本実施形態に係るロボット１０は、前述した実施形態１（図１、参照）と主要構成は同じであって、先端部のアーム１８に撮像手段としてのカメラ５０が取り付けられている。また、カメラ５０の視野領域の基台１１の延長上には、基準マーカー６０が設けられている。カメラ５０は、アーム１８の動きに連動する。
【００５７】
続いて、本実施形態の制御装置２０の構成について図１１を参照して説明する。制御装置２０は、カメラ５０（画像処理装置を含む）が取得した基準マーカー６０の画像の移動前と移動後の角度の差分情報を用いて、画像取得位置の角速度に変換する近似微分演算部２１と、得られた角速度をジャイロセンサー３０の取り付け位置の座標系角速度に変換する座標変換部２２と、この座標系角速度とジャイロセンサー３０から取得した角速度とを比較し、角速度差を算出する比較部２３と、を有する。さらに、動作指令の角速度成分に基づき閾値を決定する閾値決定部２５と、閾値と上記角速度差とを比較して故障の有無を判定する故障判定部２４と、が備えられている。
【００５８】
次に、本実施形態に係るロボット１０の故障検出方法を説明する。図面は省略するが、図５のフローチャートを参照して説明する。まず、動作の指令角速度から閾値を決定する（Ｓ１に相当）。次に、カメラ５０が取得した基準マーカー６０の画像の移動前と移動後の角度の差分情報（Ｓ２の関節角度に相当するので、以降、関節角度と表す）を取得し、この関節角度を近似微分演算部２１で近似微分して画像取得位置の角速度を算出し（Ｓ３相当）、次に、得られた角速度を、座標変換部２２でジャイロセンサー３０の取り付け位置の座標系角速度に変換する（Ｓ４）。
【００５９】
座標系角速度の算出は、以下の数式を用いて行う。
まず、画像取得位置姿勢情報と、カメラ５０から取得した関節角度を用いて、関節角度・画像取得位置姿勢間ヤコビ行列を算出する。
【００６０】
【数３】
0005849451.tif 000004
続いて、カメラ５０から取得した関節角度と、ジャイロセンサー３０の取り付け位置姿勢情報を用いて、関節角度・ジャイロセンサー３０の取り付け位置姿勢間ヤコビ行列を算出する。
【００６１】
【数４】
0005849451.tif 000005
そして、関節角度・ジャイロセンサー３０の取り付け位置姿勢間ヤコビ行列と、関節角度・画像取得位置姿勢間ヤコビ行列と、画像取得位置姿勢の角速度を用いて、ジャイロセンサー３０の取り付け位置の座標系角速度を算出する。
【００６２】
【数５】
0005849451.tif 000006
【００６３】
次に、ジャイロセンサー３０によって角速度を取得し（Ｓ５に相当）、このジャイロセンサー３０の角速度（出力角速度）と、上記数式で算出したジャイロセンサーの取り付け位置の座標系角速度と、の角速度差を演算する（Ｓ６に相当）。工程Ｓ６以降の工程、および考え方は、図５に表すＳ７以降の工程を実行する。
【００６４】
そして、角速度差が閾値よりも小さい（｜角速度差｜＜｜閾値））場合には、ジャイロセンサー３０が正常動作していると判定し、角速度差が閾値よりも大きい（｜角速度差｜＞｜閾値｜）と判定した場合には、ジャイロセンサー３０が故障したと判定することができる。
なお、本実施形態では、ジャイロセンサー３０によって取得した角速度と、カメラ５０から取得した座標系角速度との比（割合と）閾値（割合）とを比較する方法でもよい。
（実施形態５）
【００６５】
続いて、実施形態５について説明する。前述した実施形態１〜実施形態４では、ロボット１０の故障判定は、エンコーダー４０が取得した関節角度から算出した座標系角速度とジャイロセンサー３０から取得する角速度との角速度差と、閾値との差で行っているが、本実施形態では、ジャイロセンサー３０の角速度と、動作指令の角度成分から算出した座標系角速度に基づき決定された閾値とを比較することを特徴とする。
【００６６】
なお、本実施形態のロボットおよび制御装置の構成は、前述した実施形態１（図１、図２、参照）と同じであるため、説明を省略する。但し、本実施形態では、近似微分演算部２１に入力されるデータは、動作指令の角度成分である。
【００６７】
次に、本実施形態によるロボットの故障検出方法を説明する。図示は省略するが、図５を参照して説明する。
まず、動作指令の角度成分から算出した座標系角速度に基づき閾値を決定する。閾値は、図１に示す基部１２の基準座標軸からジャイロセンサー３０の取り付け位置までの各アームの動作指令の関節角度を近似微分して動作指令の関節角速度を演算し、この動作指令の関節角速度をジャイロセンサー３０の取り付け位置の座標系角速度に変換し、動作指令の座標系角速度に基づき閾値を決定する。
【００６８】
次に、上記の座標系角速度の算出方法について説明する。
まず、ジャイロセンサー３０の取り付け位置における姿勢情報と、動作指令の関節角度を用いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列を算出する。
【００６９】
【数６】
0005849451.tif 000007
続いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列と、動作指令の関節角速度を用いて、ジャイロセンサーの取り付け位置における座標系角速度を算出する。
【００７０】
【数７】
0005849451.tif 000008
【００７１】
そして、ジャイロセンサー３０が取得した角速度（ジャイロ角速度）と、上記数式を用いた動作指令の座標系角速度と、を比較する。なお、ジャイロ角速度と閾値の比較は、座標系角速度を一定時間遅延させたものと比較する。なお、一定時間とは、（１／（位置比例ゲイン×各軸の減速比））に比例した時間を関節ごとに設定する。これは、図３に示すように、動作指令の角速度成分と、ジャイロセンサー３０の角速度検出には遅延時間があるため、この遅延時間を補正するためである。
【００７２】
このようにすれば、ジャイロセンサー３０が取得した角速度が閾値よりも大きい場合には、ジャイロセンサーに異常があると判定して、ロボット１０の駆動を停止させることができる。
【符号の説明】
【００７３】
１…ロボット、１３〜１８…アーム、２０…制御装置、２１…近似微分演算部、２２…座標変換部、２３…比較部、２４…故障判定部、２５…閾値決定部。
【図１】
0005849451.tif 000009
【図２】
0005849451.tif 000010
【図３】
0005849451.tif 000011
【図４】
0005849451.tif 000012
【図５】
0005849451.tif 000013
【図６】
0005849451.tif 000014
【図７】
0005849451.tif 000015
【図８】
0005849451.tif 000016
【図９】
0005849451.tif 000017
【図１０】
0005849451.tif 000018
【図１１】
0005849451.tif 000019