ロボットの故障検出方法、ロボット
【課題】ロボットの故障検出方法を提供する。
【解決手段】ロボット1の故障検出方法は、関節J1〜J6と、これら関節に連結されるアーム13〜18と、関節J1〜J6それぞれの回転角度を検出するエンコーダー40と、アームのいずれかに取り付けられたジャイロセンサー30と、を有するロボットの故障検出方法であって、エンコーダー40またはジャイロセンサー30の異常を判定する閾値を決定する工程と、エンコーダー40によって関節角度を取得する工程と、関節角度を近似微分して関節角速度を算出する工程と、関節角速度をジャイロセンサー位置の座標系角速度に変換する工程と、ジャイロセンサーによって角速度を取得する工程と、ジャイロセンサーにより取得した角速度と座標系角速度との角速度差、または角速度比を演算する工程と、角速度差、または角速度比と、閾値とを比較する工程と、を含む。
【解決手段】ロボット1の故障検出方法は、関節J1〜J6と、これら関節に連結されるアーム13〜18と、関節J1〜J6それぞれの回転角度を検出するエンコーダー40と、アームのいずれかに取り付けられたジャイロセンサー30と、を有するロボットの故障検出方法であって、エンコーダー40またはジャイロセンサー30の異常を判定する閾値を決定する工程と、エンコーダー40によって関節角度を取得する工程と、関節角度を近似微分して関節角速度を算出する工程と、関節角速度をジャイロセンサー位置の座標系角速度に変換する工程と、ジャイロセンサーによって角速度を取得する工程と、ジャイロセンサーにより取得した角速度と座標系角速度との角速度差、または角速度比を演算する工程と、角速度差、または角速度比と、閾値とを比較する工程と、を含む。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、ロボットの故障検出方法、このロボットの故障検出方法を用いたロボットに関する。
【背景技術】
【0002】
ワークの搬送や組み立て等の作業において用いられる多関節構造を有するロボットは、近年多方面で採用されている。最近では、これらのロボットは、より高速、かつ正確にアームを移動させることが要求されてきている。そこで、関節や関節に連結されるアームに、角度センサーおよび慣性センサーを配設して、動作指令に基づく移動経路を所定の速度で駆動している場合がある。しかし、角度センサーや慣性センサーが故障した場合には、正確な移動制御ができなくなる。
【0003】
そこで、モーターに配設されるエンコーダーによって得た回転角度から算出した加速度および加速度センサーから得た加速度データと、加速度指令と、の差分を算出し、これらの差分が予め設定された閾値よりも大きくなった場合に、伝達系の異常があったと判断するロボットの故障検出方法が提案されている(たとえば、特許文献1参照)。
【0004】
また、アームの運動状態量を検出するジャイロセンサーによって取得した状態量を直交座標系の状態量に変換して第1のセンサー値とし、関節角度を検出する角度センサーによって検出した関節角度を直交座標系の状態量に変換して第2のセンサー値とし、第1のセンサー値と第2のセンサー値とを合成して出力合成値を取得することによって、ロボットの状態量を把握するとともに、作業者は直交指令と出力合成値とを比較して故障の有無を判定するというロボットの評価方法が提案されている(たとえば、特許文献2参照)。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特開2010−64232号公報
【特許文献2】特表WO2006/022201号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
上述した特許文献1では、エンコーダーから得た角度を2回微分して加速度を算出している。加速度センサーとしては、具体的にはジャイロセンサーを用いて得られた角速度を微分して加速度を算出する。このように、各センサーから得られたセンサーデータのそれぞれを微分して加速度を算出することから誤差が大きくなってしまう課題を有している。
【0007】
また、特許文献2では、ジャイロセンサーによって得たセンサーデータと、角度センサーによって得たセンサーデータと、の両方を直交座標に変換し、かつ合成した出力合成値を評価しているため、直交座標への変換計算、合成計算等、演算量が多くなり、演算部の負荷が大きくなるという課題がある。
さらに、作業者が直交指令と出力合成値とを比較して故障の有無を判定し、ロボットを停止させることから、リアルタイムでの故障検出は困難である。
【課題を解決するための手段】
【0008】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【0009】
[適用例1]本適用例に係るロボットの故障検出方法は、関節と、前記関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、前記角度センサーまたは前記慣性センサーの異常を判定する閾値を決定する工程と、前記角度センサーによって関節角度を取得する工程と、前記関節角度を近似微分して関節角速度を算出する工程と、前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する工程と、前記慣性センサーによって角速度を取得する工程と、前記慣性センサーにより取得した角速度と前記座標系角速度との角速度差、または角速度比を演算する工程と、前記角速度差、または角速度比と、前記閾値とを比較する工程と、を含むことを特徴とする。
【0010】
本適用例によれば、角度センサーによって取得した関節角度を関節角速度に変換した後、慣性センサーの取り付け位置における座標系角速度に変換し、この座標系角速度と、慣性センサーから得られた角速度との角速度差を、予め動作指令に基づき決定された閾値と比較し、たとえば、角速度差<閾値である場合は、角度センサーおよび慣性センサーは正常であると判定し、所定の作業を継続する。また、角速度差>閾値である場合には、角度センサーまたは慣性センサーのどちらかが故障したと判定して直ちにロボットの駆動を停止させる。従って、特許文献1にように、得られたセンサーデータを2回微分して加速度に変換する必要がないので誤差が小さいという効果がある。
【0011】
また、関節角速度を慣性センサーの座標系角速度への変換処理は一度でよいので、前述した特許文献2よりも演算量が少なくなり、演算に係る時間も短縮できる。さらに、角速度差と閾値との差を比較し、角速度差>閾値の場合は故障と判定し直ちにロボットの可動を停止すれば、品質の低下を抑え、安全性を高めることができる。
なお、慣性センサーによって取得した角速度と座標系角速度との角速度比(割合)と、閾値(割合)とを、比較する方法であっても、角速度差を比較する方法と同様な効果が得られる。
【0012】
[適用例2]上記適用例に係るロボットの故障検出方法において、動作指令角速度または動作指令角加速度の大きさによって、前記閾値を切り替える、ことが好ましい。
【0013】
アームの加減速が大きい場合にはアームの振動が大きくなり、加減速が小さい場合にはアームの振動が小さくなる。よって、動作指令角速度が大きい場合には閾値も大きくし、動作指令角速度が小さい場合には閾値も小さくすれば、アームの加減速に合わせた適切な故障検出を行うことができる。
【0014】
[適用例3]上記適用例に係るロボットの故障検出方法において、前記アームに発生する振動の大きさと前記閾値の対照表を備え、前記対照表を参照し、振動の値に対応させて予め設定された前記閾値を選択し、前記閾値の切り替えを行う、ことが好ましい。
【0015】
このように、アームの姿勢および角速度によってアームにどれくらいの大きさの振動が発生するかを予め測定し、振動の大きさと閾値の対照表を供え、振動の大きさによって適切な閾値を選択すれば、動作指令角速度の大小によって閾値を切り替える方法と、同様な効果が得られる。
【0016】
[適用例4]本適用例に係るロボットの故障検出方法は、関節と、前記関節に連結されるアームと、前記アームに取り付けられた慣性センサー及び撮像手段と、を有するロボットの故障検出方法であって、前記座標系角速度を、前記アームに取り付けられた撮像手段が取得した画像の移動前と移動後の差分情報を用いて算出した画像取得位置の角速度を、前記慣性センサーの取り付け位置の角速度に座標変換して座標系角速度を算出する工程と、前記角速度と、前記座標系角速度とを比較する工程と、を有すること、を特徴とする。
【0017】
アームに、たとえばカメラ等の撮像手段を配設し、撮像手段によってアームの移動前と移動後の画像の差分情報を用いて算出した角速度を、慣性センサー位置の角速度に座標変換して座標系角速度とし、慣性センサーが取得した角速度と座標系角速度との角速度と、閾値とを比較する。そして、角速度差>閾値となった場合には、慣性センサーが異常であることを検出できる。
なお、本適用例には、慣性センサーによって取得した角速度と、撮像手段から取得した座標系角速度との比(割合と)閾値とを比較する方法も含まれる。
【0018】
[適用例5]本適用例に係るロボットの故障検出方法は、関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、前記慣性センサーによって角速度を取得する工程と、動作指令の角度成分を近似微分して関節角速度を算出する工程と、前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する工程と、前記慣性センサーの異常を判定する閾値を決定する工程と、前記慣性センサーによって取得した角速度と、前記閾値とを比較する工程と、を含むことを特徴とする。
【0019】
このように、慣性センサーから取得した角速度と、動作指令の角度成分から算出した慣性センサー位置の座標系角速度に基づき決定した閾値とを比較して、慣性センサーから取得した角速度が閾値よりも大きい場合には、慣性センサーが故障したと判定することができる。
【0020】
[適用例6]本適用例に係るロボットは、関節と、前記関節に連結されるアームと、前記関節の関節角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、前記角度センサーまたは前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、前記関節角度を近似微分して関節角速度を算出する近似微分演算部と、前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する座標変換部と、前記慣性センサーによって取得した角速度と前記座標系角速度とを比較する比較部と、前記比較部の比較結果に基づき故障の有無を判定する故障判定部と、を有することを特徴とする。
【0021】
本適用例によれば、角度センサーによって取得した関節角度を関節角速度に変換した後、慣性センサーの取り付け位置の座標系角速度に変換し、この座標系角速度と、慣性センサーから得られた角速度との角速度差を、予め決定されている閾値と比較し、たとえば、角速度差<閾値である場合は、角度センサーおよび慣性センサーは正常であると判定し、所定の作業を継続する。また、角速度差>閾値である場合には、角度センサーまたは慣性センサーのどちらかが故障したと判定して直ちにロボットの駆動を停止させる。従って、特許文献1にように、得られたセンサーデータを2回微分して加速度に変換する必要がないので誤差が小さいという効果がある。
【0022】
また、関節角速度を慣性センサーの座標系角速度への変換処理は一度でよいので、前述した特許文献2よりも演算量が少なくなり、演算に係る時間も短縮できる。さらに、角速度差と閾値との差を比較し、角速度差>閾値の場合は故障と判定し直ちにロボットの可動を停止すれば、品質の低下を抑え、安全性を高めることができる。
なお、慣性センサーによって取得した角速度と座標系角速度との角速度比(割合)と、閾値(割合)とを、比較する方法であっても、角速度差を比較する方法と同様な効果が得られる。
【0023】
[適用例7]本記適用例に係るロボットは、関節と、前記関節に連結されるアームと、前記アームに取り付けられた慣性センサー及び撮像手段と、前記撮像手段が取得した画像の移動前と移動後の角度の差分情報を用いて、画像取得位置の角速度に変換する近似微分演算部と、前記角速度を前記慣性センサーの取り付け位置の座標系角速度に変換する座標変換部と、前記座標系角速度と前記慣性センサーが取得した角速度とを比較し、角速度差を算出する比較部と、前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、前記角速度差と前記閾値とを比較して故障の有無を判定する故障判定部と、を有することを特徴とする。
【0024】
このような構成によれば、アームに、たとえばカメラ等の撮像手段を配設し、撮像手段によってアームの動きの前後の画像の差分情報を用いて移動角度を算出し、慣性センサーの取り付け位置の角速度に座標変換して座標系角速度とし、閾値との比較によって、角速度差>閾値となった場合には、慣性センサーの故障を検出することができる。
なお、本適用例には、慣性センサーによって取得した角速度と、撮像手段から取得した座標系角速度との比(割合と)閾値とを比較する方法も含まれる。
【図面の簡単な説明】
【0025】
【図1】実施形態1に係るロボットの1例を表す模式図。
【図2】制御装置の主要構成を示す構成説明図。
【図3】指令の角速度成分と、エンコーダーから算出した角速度と、ジャイロセンサーの角速度成分の出力信号を比較したグラフ。
【図4】図3の一部を拡大したグラフ。
【図5】実施形態に係るロボットの故障検出方法の主たる工程を示すフローチャート。
【図6】動作指令の角速度成分と、エンコーダーから算出した角速度と、ジャイロセンサーが出力した角速度を比較したグラフ。
【図7】角速度差と閾値との関係を表すグラフ。
【図8】動作指令の加減速が大きい場合を表し、(a)は動作指令の角速度成分、(b)は角速度差。
【図9】動作指令の加減速が小さい場合を表し、(a)は動作指令の角速度成分、(b)は角速度差。
【図10】実施形態4に係るロボットの1例を表す模式図。
【図11】実施形態4に係る制御装置の主要構成を示す構成説明図。
【発明を実施するための形態】
【0026】
以下、本発明の実施形態を図面を参照して説明する。
(実施形態1)
【0027】
図1は、実施形態1に係るロボットの1例を表す模式図である。ロボット10は、X・Y平面を有する基台11上に、X・Y平面に垂直に配置される基部12を有し、基部12の中心軸(Z軸)を基本座標軸として、複数の関節と、関節間を連結するアームとから構成されている。ここで、図1に表記した円形と四角形は関節を表し、関節に付す矢印は回転方向を表している。基部12の端部は基台に固定され、関節J1と関節J2の間はアーム13で連結される。
【0028】
また、関節J2と関節J3とはアーム14で連結され、関節J3とJ4とはアーム15で連結され、関節J4と関節J5とはアーム16で連結され、関節J5と関節J6とはアーム17で連結されている。さらに、関節J6にはアーム18が連結されており、アーム18の終端部(Endpoint)には指部(図示せず)を有する。なお、アーム13,16,18は回転軸である。
【0029】
関節J1〜J6にはそれぞれ、モーターと、減速機と、モーターの回転角度(つまり、関節の回転角度:単に関節角度と表す)を検出する角度センサーとしてのエンコーダーと、が備えられている。また、アーム16には慣性センサーとしてのジャイロセンサー30が取り付けられており、慣性センサー30の取り付け位置における状態量を検出する。この状態量とは、姿勢情報、移動方向、角速度を含む。よって、慣性センサー30は、3軸ジャイロセンサーが用いられることが望ましい。
【0030】
なお、ジャイロセンサー30は、本実施形態では、アーム16に配設しているが、この位置には限定されない。しかし、取り付け可能であれば、Endpointに近い位置に配設されることが望ましい。これは、先端部のEndpointでの振動が基部12付近よりも大きくなるためである。
【0031】
ロボット10には、後述する各モーターの駆動制御、エンコーダーによる角度の検出、ジャイロセンサーによる角速度検出、各種演算処理を行う制御装置20を備えている。そして、ロボット10は、アーム13〜18と、関節J1〜J6のモーターおよび減速機と、によって相対的な運動をすることで、Endpoint(たとえば指部)において、所定の作業をさせる。
【0032】
続いて、制御装置20の構成について説明する。
図2は、制御装置の主要構成を示す構成説明図である。制御装置20は、エンコーダー40(ここでは、基部12の基準座標軸からジャイロセンサー取り付け位置までのエンコーダーの総称)から取得した関節角度を関節角速度に変換する近似微分演算部21と、得られた関節角速度をジャイロセンサー30の取り付け位置の座標系角速度に変換する座標変換部22と、この座標系角速度とジャイロセンサー30から取得した角速度(ジャイロ角速度と表す)とを比較し、角速度差を算出する比較部23と、を有する。さらに、動作指令の角速度成分に基づき閾値を決定する閾値決定部25と、閾値と上記角速度差とを比較して故障の有無を判定する故障判定部24と、が備えられている。
【0033】
次に、上述したロボット10を駆動したときのジャイロ角速度と、エンコーダー40から取得し算出した角速度と、動作指令の角速度成分と、の関係を比較して説明する。
図3は、動作指令の角速度成分と、エンコーダーから算出した角速度と、ジャイロセンサーの角速度の出力信号と、を比較したグラフ、図4は、図3の一部を拡大したグラフである。ともに、横軸に時間[sec]、縦軸に角速度[deg/s]を表している。なお、図3、図4ともにジャイロセンサー30およびエンコーダー40は正常な状態の場合を示している。
【0034】
図3に示した動作指令の角速度成分は、0.1[sec]の間に+450[deg/s]まで加速させ、そこから0.08[sec]の間は一定速度を維持し、さらに、0.1[sec]の間に−450[deg/s]減速させて速度を0に戻して停止させるというものである。図3に示すように、動作指令の角速度成分に対して、ジャイロ角速度と、エンコーダー40より算出した角速度(以降、エンコーダー角速度と表すことがある)とは、僅かな遅延はあるものの、ほぼ一致している。
【0035】
また、図4の拡大図では、ジャイロ角速度は、駆動を停止したときにアーム16が振動をしていることを示し、エンコーダー40より算出した角速度とは微妙に異なるが、全体としての角速度の変化はほぼ一致している。
そこで、動作指令の角速度成分に基づき閾値を決定し、閾値とエンコーダー角速度とジャイロ角速度との角速度差を比較して、これら角速度差が閾値よりも大きい場合には故障したと判定する。よって、より適切なロボット10の故障検出が可能になることが分かる。
(ロボットの故障検出方法)
続いて、上記考え方に基づくロボット10の故障検出方法について説明する。
図5は、本実施形態に係るロボットの故障検出方法の主たる工程を示すフローチャートである。なお、図2も参照して説明する。まず、ロボット10の動作を開始し、動作の指令角速度から閾値を決定する(S1)。次に、エンコーダー40より関節角度を取得し(S2)、近似微分演算部21で関節角度を近似微分して関節角速度を算出し(S3)、次に、得られた関節角速度を、座標変換部22でジャイロセンサー30の取り付け位置の座標系角速度に変換する(S4)。
【0036】
次に、上記の座標系角速度の算出方法について説明する。
まず、ジャイロセンサー30の取り付け位置における姿勢情報と、エンコーダー角速度(関節角度)を用いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列を算出する。
【0037】
【数1】
続いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列と、関節角度から算出した関節角速度を用いて、ジャイロセンサーの取り付け位置における座標系角速度を算出する。
【0038】
【数2】
【0039】
そして、ジャイロセンサー30より角速度(ジャイロ角速度または出力角速度))を取得し(S5)、このジャイロ角速度と、関節角速度から算出した角速度(つまり、座標系角速度)と、の角速度差を演算する(S6)。そして、この角速度差と動作指令の角速度成分に基づき予め設定した閾値(工程:S1)とを比較し(S7)、角速度差が閾値よりも小さい(|角速度差|<|閾値|)場合(YES)には、ジャイロセンサー30およびエンコーダー40が正常動作であると判定し、モーター駆動を継続する(S10)。そして所定の移動動作が終了した時点(S11:YES)で動作を終了させる。所定の移動動作が終了しない(NO)場合には、エンコーダー40の関節角度取得工程(S2)から、以降の工程を繰り返す。
【0040】
工程S7において、角速度差が閾値よりも大きい(|角速度差|>|閾値|)と判定した場合(NO)には、ジャイロセンサー30またはエンコーダー40のいずれかが故障したと判定し、モーターを非常停止させる(S8)。
【0041】
以上説明したロボット10の故障検出方法を用いて故障検出した場合の1例をあげ説明する。
図6は、動作指令の角速度成分と、エンコーダー角速度と、ジャイロ角速度を比較したグラフである。横軸に時間[sec]、縦軸に角速度[deg/s]を表している。図6において、指令の角速度成分とエンコーダー情報から算出した角速度(座標系角速度)と動作指令の角速度成分に対して遅延はあるものの、ほぼ同じである。また、ジャイロ角速度は、0.23「sec」の位置で急激に0[deg/s]になっている。つまり、ジャイロセンサーの検出値に異常があったことが推定できる。
【0042】
図7は、角速度差と閾値との関係を表すグラフである。ここで、仮にマイナス側の閾値を−45[deg/s]とする。なお、図中に記載のねじれ角速度は、ジャイロセンサーの角速度とエンコーダーの検出値から算出した角速度との差(角速度差)である。この例では、0.23[sec]の位置で、ねじれ角速度の絶対値が閾値の絶対値45[deg/s]よりも大きくなっている。この位置で、ジャイロセンサー30が故障したと判定し、モーターの駆動を停止させる。
【0043】
従って、上述したロボット10及びロボットの故障検出方法によれば、エンコーダー40によって取得した関節角度を関節角速度に変換した後、ジャイロセンサー30の取り付け位置における座標系角速度に変換し、この座標系角速度と、ジャイロセンサー30から得られた角速度との角速度差を、予め動作指令の角速度成分に基づき決定した閾値と比較し、たとえば、|角速度差|<閾値である場合は、エンコーダー40およびジャイロセンサー30は共に正常であると判定し、所定の作業を継続する。また、|角速度差|>|閾値|である場合には、エンコーダー40(基部12からジャイロセンサー取り付け位置までに配置されたエンコーダーのいずれか)またはジャイロセンサー30のどちらかが故障したと判定して直ちにロボット10(モーター)の駆動を停止させる。従って、特許文献1にように、角度センサーから得られたセンサーデータを2回微分して加速度に変換する必要がないので誤差が小さいという効果がある。
【0044】
また、関節角速度をジャイロセンサーの取り付け位置における座標系角速度への変換処理は一度でよいので、前述した特許文献2よりも演算量が少なくなり、演算に係る時間も短縮できる。さらに、角速度差と閾値との差を比較し、|角速度差|>|閾値|の場合は故障と判定し直ちにロボット10の駆動を停止すれば、品質の低下を抑え、また、安全性を高めることができる。
【0045】
なお、前述した角速度差と閾値とを判定要素として用いる故障検出方法の他に、慣性センサーにより取得した角速度と座標系角速度との角速度比(割合)を算出し、動作指令から決定する閾値(割合)とを、比較する方法を用いてロボット10の故障検出を行うことができる。具体的には、図5で表すフローチャートの工程S1において動作の指令加速度成分から許容できる閾値を割合で決定し、工程S6において角速度差の代わりに角速度比を演算し、工程S7において角速度比と閾値とを比較すればよい。このようにして、|角速度比|<|閾値|の場合は正常、|角速度比|>|閾値|の場合には故障判定とすることができる。
(実施形態2)
【0046】
続いて、実施形態2について説明する。前述した実施形態1は、閾値を一定にしていることに対し、動作指令の角速度成分の大きさに対応して閾値を切り替えるところに特徴を有する。たとえば、加減速が大きい動作を指令する場合には、アームの振動が大きくなるので閾値を大きくする。一方、加減速が小さい動作を指令する場合には、アームの振動が小さくなるので閾値を小さくする。このことを実測例を図示し説明する。なお、ロボット10の構成(図1、図2、参照)および故障検出のフローチャート(図5、参照)は実施形態1と同じなので、相違箇所を中心に説明する。
【0047】
図8は、動作指令の加減速が大きい場合を表し、(a)は動作指令の角速度成分、(b)は角速度差を表している。
動作指令の角速度成分は、図8(a)に表すように、0.1[sec]間、450[deg/s]の角速度まで加速し、それから0.08[sec]の一定速度域の後、やはり0.1[sec]間で450[deg/s]の角速度を減速して、速度を0に戻した例である。
【0048】
上記条件の動作指令によって動作させたときの角速度差の測定例を図8(b)に示している。このように動作させた場合、アームの振動が大きくなることから角速度差は概ね±16[deg/s]の範囲で変動する。
次に、動作指令の加減速が小さい場合について説明する。
【0049】
図9は、動作指令の加減速が小さい場合を表し、(a)は動作指令の角速度成分、(b)は角速度差を表している。
動作指令の角速度成分は、図9(a)に表すように、0.15[sec]間で450[deg/s]の角速度まで加速し、それから0.03[sec]の一定速度域の後、やはり0.15[sec]で450[deg/s]の角速度まで減速して、速度を0に戻した例である。
【0050】
上記条件の動作指令によって動作させたときの角速度差の測定例を図9(b)に示している。このように動作させた場合、アームの振動が小さくなることから角速度差は概ね±11[deg/s]の範囲で変動する。
【0051】
図8、図9で表した例が、共に正常であるとした場合、加減速が大きい場合の閾値をたとえば、±20[deg/s]とし、加減速が小さい場合の閾値をたとえば、±15[deg/s]とすることができる。
【0052】
従って、図8、図9に示したように、動作指令の加減速が大きい場合には閾値を大きくし、動作指令の加減速が小さい場合には、閾値を小さくすることができる。このようにすれば、動作指令の加減速の大小に合わせた適切な閾値の設定を行うことができる。このことによって、故障の有無の判定をするために、過剰な安全率を設定しなくてもよいという効果がある。
(実施形態3)
【0053】
続いて、実施形態3に係るロボット10の故障検出方法について説明する。前述した実施形態2が、動作指令の加減速の大小に合わせて閾値の切り替えをすることに対し、実施形態3は、予めアームに発生する振動の大きさと閾値の対照表を備え、この対照表を参照して、振動の大きさに合わせて閾値を選択して、閾値の切り替えを行うことに特徴を有する。なお、図示は省略する。
【0054】
前述したように、アームには振動が発生する。図8(b)、図9(b)では、角速度差の変動を表しているが、この角速度差の変動はアームの振動に起因している。従って、アームの姿勢および角速度によってアームにどれくらい振動が発生するかを予め測定し、振動の大きさに対応する閾値の対照表を作成し、この対照表から適切な閾値を選択すれば、動作指令の角速度成分の大小によって閾値を切り替える方法と、同様な効果が得られる。
(実施形態4)
【0055】
次に、実施形態4について説明する。前述した実施形態1〜実施形態3が、ジャイロセンサーとエンコーダーから得た角速度を用いていることに対し、実施形態4は、アームに取り付けられた撮像手段が取得した画像から角速度を算出してジャイロセンサーとの角速度差と閾値とを比較して故障を検出することに特徴を有する。
【0056】
図10は、実施形態4に係るロボットの1例を表す模式図、図11は制御装置の主要構成を示す構成説明図である。図10に示すように、本実施形態に係るロボット10は、前述した実施形態1(図1、参照)と主要構成は同じであって、先端部のアーム18に撮像手段としてのカメラ50が取り付けられている。また、カメラ50の視野領域の基台11の延長上には、基準マーカー60が設けられている。カメラ50は、アーム18の動きに連動する。
【0057】
続いて、本実施形態の制御装置20の構成について図11を参照して説明する。制御装置20は、カメラ50(画像処理装置を含む)が取得した基準マーカー60の画像の移動前と移動後の角度の差分情報を用いて、画像取得位置の角速度に変換する近似微分演算部21と、得られた角速度をジャイロセンサー30の取り付け位置の座標系角速度に変換する座標変換部22と、この座標系角速度とジャイロセンサー30から取得した角速度とを比較し、角速度差を算出する比較部23と、を有する。さらに、動作指令の角速度成分に基づき閾値を決定する閾値決定部25と、閾値と上記角速度差とを比較して故障の有無を判定する故障判定部24と、が備えられている。
【0058】
次に、本実施形態に係るロボット10の故障検出方法を説明する。図面は省略するが、図5のフローチャートを参照して説明する。まず、動作の指令角速度から閾値を決定する(S1に相当)。次に、カメラ50が取得した基準マーカー60の画像の移動前と移動後の角度の差分情報(S2の関節角度に相当するので、以降、関節角度と表す)を取得し、この関節角度を近似微分演算部21で近似微分して画像取得位置の角速度を算出し(S3相当)、次に、得られた角速度を、座標変換部22でジャイロセンサー30の取り付け位置の座標系角速度に変換する(S4)。
【0059】
座標系角速度の算出は、以下の数式を用いて行う。
まず、画像取得位置姿勢情報と、カメラ50から取得した関節角度を用いて、関節角度・画像取得位置姿勢間ヤコビ行列を算出する。
【0060】
【数3】
続いて、カメラ50から取得した関節角度と、ジャイロセンサー30の取り付け位置姿勢情報を用いて、関節角度・ジャイロセンサー30の取り付け位置姿勢間ヤコビ行列を算出する。
【0061】
【数4】
そして、関節角度・ジャイロセンサー30の取り付け位置姿勢間ヤコビ行列と、関節角度・画像取得位置姿勢間ヤコビ行列と、画像取得位置姿勢の角速度を用いて、ジャイロセンサー30の取り付け位置の座標系角速度を算出する。
【0062】
【数5】
【0063】
次に、ジャイロセンサー30によって角速度を取得し(S5に相当)、このジャイロセンサー30の角速度(出力角速度)と、上記数式で算出したジャイロセンサーの取り付け位置の座標系角速度と、の角速度差を演算する(S6に相当)。工程S6以降の工程、および考え方は、図5に表すS7以降の工程を実行する。
【0064】
そして、角速度差が閾値よりも小さい(|角速度差|<|閾値))場合には、ジャイロセンサー30が正常動作していると判定し、角速度差が閾値よりも大きい(|角速度差|>|閾値|)と判定した場合には、ジャイロセンサー30が故障したと判定することができる。
なお、本実施形態では、ジャイロセンサー30によって取得した角速度と、カメラ50から取得した座標系角速度との比(割合と)閾値(割合)とを比較する方法でもよい。
(実施形態5)
【0065】
続いて、実施形態5について説明する。前述した実施形態1〜実施形態4では、ロボット10の故障判定は、エンコーダー40が取得した関節角度から算出した座標系角速度とジャイロセンサー30から取得する角速度との角速度差と、閾値との差で行っているが、本実施形態では、ジャイロセンサー30の角速度と、動作指令の角度成分から算出した座標系角速度に基づき決定された閾値とを比較することを特徴とする。
【0066】
なお、本実施形態のロボットおよび制御装置の構成は、前述した実施形態1(図1、図2、参照)と同じであるため、説明を省略する。但し、本実施形態では、近似微分演算部21に入力されるデータは、動作指令の角度成分である。
【0067】
次に、本実施形態によるロボットの故障検出方法を説明する。図示は省略するが、図5を参照して説明する。
まず、動作指令の角度成分から算出した座標系角速度に基づき閾値を決定する。閾値は、図1に示す基部12の基準座標軸からジャイロセンサー30の取り付け位置までの各アームの動作指令の関節角度を近似微分して動作指令の関節角速度を演算し、この動作指令の関節角速度をジャイロセンサー30の取り付け位置の座標系角速度に変換し、動作指令の座標系角速度に基づき閾値を決定する。
【0068】
次に、上記の座標系角速度の算出方法について説明する。
まず、ジャイロセンサー30の取り付け位置における姿勢情報と、動作指令の関節角度を用いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列を算出する。
【0069】
【数6】
続いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列と、動作指令の関節角速度を用いて、ジャイロセンサーの取り付け位置における座標系角速度を算出する。
【0070】
【数7】
【0071】
そして、ジャイロセンサー30が取得した角速度(ジャイロ角速度)と、上記数式を用いた動作指令の座標系角速度と、を比較する。なお、ジャイロ角速度と閾値の比較は、座標系角速度を一定時間遅延させたものと比較する。なお、一定時間とは、(1/(位置比例ゲイン×各軸の減速比))に比例した時間を関節ごとに設定する。これは、図3に示すように、動作指令の角速度成分と、ジャイロセンサー30の角速度検出には遅延時間があるため、この遅延時間を補正するためである。
【0072】
このようにすれば、ジャイロセンサー30が取得した角速度が閾値よりも大きい場合には、ジャイロセンサーに異常があると判定して、ロボット10の駆動を停止させることができる。
【符号の説明】
【0073】
1…ロボット、13〜18…アーム、20…制御装置、21…近似微分演算部、22…座標変換部、23…比較部、24…故障判定部、25…閾値決定部。
【技術分野】
【0001】
本発明は、ロボットの故障検出方法、このロボットの故障検出方法を用いたロボットに関する。
【背景技術】
【0002】
ワークの搬送や組み立て等の作業において用いられる多関節構造を有するロボットは、近年多方面で採用されている。最近では、これらのロボットは、より高速、かつ正確にアームを移動させることが要求されてきている。そこで、関節や関節に連結されるアームに、角度センサーおよび慣性センサーを配設して、動作指令に基づく移動経路を所定の速度で駆動している場合がある。しかし、角度センサーや慣性センサーが故障した場合には、正確な移動制御ができなくなる。
【0003】
そこで、モーターに配設されるエンコーダーによって得た回転角度から算出した加速度および加速度センサーから得た加速度データと、加速度指令と、の差分を算出し、これらの差分が予め設定された閾値よりも大きくなった場合に、伝達系の異常があったと判断するロボットの故障検出方法が提案されている(たとえば、特許文献1参照)。
【0004】
また、アームの運動状態量を検出するジャイロセンサーによって取得した状態量を直交座標系の状態量に変換して第1のセンサー値とし、関節角度を検出する角度センサーによって検出した関節角度を直交座標系の状態量に変換して第2のセンサー値とし、第1のセンサー値と第2のセンサー値とを合成して出力合成値を取得することによって、ロボットの状態量を把握するとともに、作業者は直交指令と出力合成値とを比較して故障の有無を判定するというロボットの評価方法が提案されている(たとえば、特許文献2参照)。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特開2010−64232号公報
【特許文献2】特表WO2006/022201号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
上述した特許文献1では、エンコーダーから得た角度を2回微分して加速度を算出している。加速度センサーとしては、具体的にはジャイロセンサーを用いて得られた角速度を微分して加速度を算出する。このように、各センサーから得られたセンサーデータのそれぞれを微分して加速度を算出することから誤差が大きくなってしまう課題を有している。
【0007】
また、特許文献2では、ジャイロセンサーによって得たセンサーデータと、角度センサーによって得たセンサーデータと、の両方を直交座標に変換し、かつ合成した出力合成値を評価しているため、直交座標への変換計算、合成計算等、演算量が多くなり、演算部の負荷が大きくなるという課題がある。
さらに、作業者が直交指令と出力合成値とを比較して故障の有無を判定し、ロボットを停止させることから、リアルタイムでの故障検出は困難である。
【課題を解決するための手段】
【0008】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【0009】
[適用例1]本適用例に係るロボットの故障検出方法は、関節と、前記関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、前記角度センサーまたは前記慣性センサーの異常を判定する閾値を決定する工程と、前記角度センサーによって関節角度を取得する工程と、前記関節角度を近似微分して関節角速度を算出する工程と、前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する工程と、前記慣性センサーによって角速度を取得する工程と、前記慣性センサーにより取得した角速度と前記座標系角速度との角速度差、または角速度比を演算する工程と、前記角速度差、または角速度比と、前記閾値とを比較する工程と、を含むことを特徴とする。
【0010】
本適用例によれば、角度センサーによって取得した関節角度を関節角速度に変換した後、慣性センサーの取り付け位置における座標系角速度に変換し、この座標系角速度と、慣性センサーから得られた角速度との角速度差を、予め動作指令に基づき決定された閾値と比較し、たとえば、角速度差<閾値である場合は、角度センサーおよび慣性センサーは正常であると判定し、所定の作業を継続する。また、角速度差>閾値である場合には、角度センサーまたは慣性センサーのどちらかが故障したと判定して直ちにロボットの駆動を停止させる。従って、特許文献1にように、得られたセンサーデータを2回微分して加速度に変換する必要がないので誤差が小さいという効果がある。
【0011】
また、関節角速度を慣性センサーの座標系角速度への変換処理は一度でよいので、前述した特許文献2よりも演算量が少なくなり、演算に係る時間も短縮できる。さらに、角速度差と閾値との差を比較し、角速度差>閾値の場合は故障と判定し直ちにロボットの可動を停止すれば、品質の低下を抑え、安全性を高めることができる。
なお、慣性センサーによって取得した角速度と座標系角速度との角速度比(割合)と、閾値(割合)とを、比較する方法であっても、角速度差を比較する方法と同様な効果が得られる。
【0012】
[適用例2]上記適用例に係るロボットの故障検出方法において、動作指令角速度または動作指令角加速度の大きさによって、前記閾値を切り替える、ことが好ましい。
【0013】
アームの加減速が大きい場合にはアームの振動が大きくなり、加減速が小さい場合にはアームの振動が小さくなる。よって、動作指令角速度が大きい場合には閾値も大きくし、動作指令角速度が小さい場合には閾値も小さくすれば、アームの加減速に合わせた適切な故障検出を行うことができる。
【0014】
[適用例3]上記適用例に係るロボットの故障検出方法において、前記アームに発生する振動の大きさと前記閾値の対照表を備え、前記対照表を参照し、振動の値に対応させて予め設定された前記閾値を選択し、前記閾値の切り替えを行う、ことが好ましい。
【0015】
このように、アームの姿勢および角速度によってアームにどれくらいの大きさの振動が発生するかを予め測定し、振動の大きさと閾値の対照表を供え、振動の大きさによって適切な閾値を選択すれば、動作指令角速度の大小によって閾値を切り替える方法と、同様な効果が得られる。
【0016】
[適用例4]本適用例に係るロボットの故障検出方法は、関節と、前記関節に連結されるアームと、前記アームに取り付けられた慣性センサー及び撮像手段と、を有するロボットの故障検出方法であって、前記座標系角速度を、前記アームに取り付けられた撮像手段が取得した画像の移動前と移動後の差分情報を用いて算出した画像取得位置の角速度を、前記慣性センサーの取り付け位置の角速度に座標変換して座標系角速度を算出する工程と、前記角速度と、前記座標系角速度とを比較する工程と、を有すること、を特徴とする。
【0017】
アームに、たとえばカメラ等の撮像手段を配設し、撮像手段によってアームの移動前と移動後の画像の差分情報を用いて算出した角速度を、慣性センサー位置の角速度に座標変換して座標系角速度とし、慣性センサーが取得した角速度と座標系角速度との角速度と、閾値とを比較する。そして、角速度差>閾値となった場合には、慣性センサーが異常であることを検出できる。
なお、本適用例には、慣性センサーによって取得した角速度と、撮像手段から取得した座標系角速度との比(割合と)閾値とを比較する方法も含まれる。
【0018】
[適用例5]本適用例に係るロボットの故障検出方法は、関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、前記慣性センサーによって角速度を取得する工程と、動作指令の角度成分を近似微分して関節角速度を算出する工程と、前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する工程と、前記慣性センサーの異常を判定する閾値を決定する工程と、前記慣性センサーによって取得した角速度と、前記閾値とを比較する工程と、を含むことを特徴とする。
【0019】
このように、慣性センサーから取得した角速度と、動作指令の角度成分から算出した慣性センサー位置の座標系角速度に基づき決定した閾値とを比較して、慣性センサーから取得した角速度が閾値よりも大きい場合には、慣性センサーが故障したと判定することができる。
【0020】
[適用例6]本適用例に係るロボットは、関節と、前記関節に連結されるアームと、前記関節の関節角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、前記角度センサーまたは前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、前記関節角度を近似微分して関節角速度を算出する近似微分演算部と、前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する座標変換部と、前記慣性センサーによって取得した角速度と前記座標系角速度とを比較する比較部と、前記比較部の比較結果に基づき故障の有無を判定する故障判定部と、を有することを特徴とする。
【0021】
本適用例によれば、角度センサーによって取得した関節角度を関節角速度に変換した後、慣性センサーの取り付け位置の座標系角速度に変換し、この座標系角速度と、慣性センサーから得られた角速度との角速度差を、予め決定されている閾値と比較し、たとえば、角速度差<閾値である場合は、角度センサーおよび慣性センサーは正常であると判定し、所定の作業を継続する。また、角速度差>閾値である場合には、角度センサーまたは慣性センサーのどちらかが故障したと判定して直ちにロボットの駆動を停止させる。従って、特許文献1にように、得られたセンサーデータを2回微分して加速度に変換する必要がないので誤差が小さいという効果がある。
【0022】
また、関節角速度を慣性センサーの座標系角速度への変換処理は一度でよいので、前述した特許文献2よりも演算量が少なくなり、演算に係る時間も短縮できる。さらに、角速度差と閾値との差を比較し、角速度差>閾値の場合は故障と判定し直ちにロボットの可動を停止すれば、品質の低下を抑え、安全性を高めることができる。
なお、慣性センサーによって取得した角速度と座標系角速度との角速度比(割合)と、閾値(割合)とを、比較する方法であっても、角速度差を比較する方法と同様な効果が得られる。
【0023】
[適用例7]本記適用例に係るロボットは、関節と、前記関節に連結されるアームと、前記アームに取り付けられた慣性センサー及び撮像手段と、前記撮像手段が取得した画像の移動前と移動後の角度の差分情報を用いて、画像取得位置の角速度に変換する近似微分演算部と、前記角速度を前記慣性センサーの取り付け位置の座標系角速度に変換する座標変換部と、前記座標系角速度と前記慣性センサーが取得した角速度とを比較し、角速度差を算出する比較部と、前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、前記角速度差と前記閾値とを比較して故障の有無を判定する故障判定部と、を有することを特徴とする。
【0024】
このような構成によれば、アームに、たとえばカメラ等の撮像手段を配設し、撮像手段によってアームの動きの前後の画像の差分情報を用いて移動角度を算出し、慣性センサーの取り付け位置の角速度に座標変換して座標系角速度とし、閾値との比較によって、角速度差>閾値となった場合には、慣性センサーの故障を検出することができる。
なお、本適用例には、慣性センサーによって取得した角速度と、撮像手段から取得した座標系角速度との比(割合と)閾値とを比較する方法も含まれる。
【図面の簡単な説明】
【0025】
【図1】実施形態1に係るロボットの1例を表す模式図。
【図2】制御装置の主要構成を示す構成説明図。
【図3】指令の角速度成分と、エンコーダーから算出した角速度と、ジャイロセンサーの角速度成分の出力信号を比較したグラフ。
【図4】図3の一部を拡大したグラフ。
【図5】実施形態に係るロボットの故障検出方法の主たる工程を示すフローチャート。
【図6】動作指令の角速度成分と、エンコーダーから算出した角速度と、ジャイロセンサーが出力した角速度を比較したグラフ。
【図7】角速度差と閾値との関係を表すグラフ。
【図8】動作指令の加減速が大きい場合を表し、(a)は動作指令の角速度成分、(b)は角速度差。
【図9】動作指令の加減速が小さい場合を表し、(a)は動作指令の角速度成分、(b)は角速度差。
【図10】実施形態4に係るロボットの1例を表す模式図。
【図11】実施形態4に係る制御装置の主要構成を示す構成説明図。
【発明を実施するための形態】
【0026】
以下、本発明の実施形態を図面を参照して説明する。
(実施形態1)
【0027】
図1は、実施形態1に係るロボットの1例を表す模式図である。ロボット10は、X・Y平面を有する基台11上に、X・Y平面に垂直に配置される基部12を有し、基部12の中心軸(Z軸)を基本座標軸として、複数の関節と、関節間を連結するアームとから構成されている。ここで、図1に表記した円形と四角形は関節を表し、関節に付す矢印は回転方向を表している。基部12の端部は基台に固定され、関節J1と関節J2の間はアーム13で連結される。
【0028】
また、関節J2と関節J3とはアーム14で連結され、関節J3とJ4とはアーム15で連結され、関節J4と関節J5とはアーム16で連結され、関節J5と関節J6とはアーム17で連結されている。さらに、関節J6にはアーム18が連結されており、アーム18の終端部(Endpoint)には指部(図示せず)を有する。なお、アーム13,16,18は回転軸である。
【0029】
関節J1〜J6にはそれぞれ、モーターと、減速機と、モーターの回転角度(つまり、関節の回転角度:単に関節角度と表す)を検出する角度センサーとしてのエンコーダーと、が備えられている。また、アーム16には慣性センサーとしてのジャイロセンサー30が取り付けられており、慣性センサー30の取り付け位置における状態量を検出する。この状態量とは、姿勢情報、移動方向、角速度を含む。よって、慣性センサー30は、3軸ジャイロセンサーが用いられることが望ましい。
【0030】
なお、ジャイロセンサー30は、本実施形態では、アーム16に配設しているが、この位置には限定されない。しかし、取り付け可能であれば、Endpointに近い位置に配設されることが望ましい。これは、先端部のEndpointでの振動が基部12付近よりも大きくなるためである。
【0031】
ロボット10には、後述する各モーターの駆動制御、エンコーダーによる角度の検出、ジャイロセンサーによる角速度検出、各種演算処理を行う制御装置20を備えている。そして、ロボット10は、アーム13〜18と、関節J1〜J6のモーターおよび減速機と、によって相対的な運動をすることで、Endpoint(たとえば指部)において、所定の作業をさせる。
【0032】
続いて、制御装置20の構成について説明する。
図2は、制御装置の主要構成を示す構成説明図である。制御装置20は、エンコーダー40(ここでは、基部12の基準座標軸からジャイロセンサー取り付け位置までのエンコーダーの総称)から取得した関節角度を関節角速度に変換する近似微分演算部21と、得られた関節角速度をジャイロセンサー30の取り付け位置の座標系角速度に変換する座標変換部22と、この座標系角速度とジャイロセンサー30から取得した角速度(ジャイロ角速度と表す)とを比較し、角速度差を算出する比較部23と、を有する。さらに、動作指令の角速度成分に基づき閾値を決定する閾値決定部25と、閾値と上記角速度差とを比較して故障の有無を判定する故障判定部24と、が備えられている。
【0033】
次に、上述したロボット10を駆動したときのジャイロ角速度と、エンコーダー40から取得し算出した角速度と、動作指令の角速度成分と、の関係を比較して説明する。
図3は、動作指令の角速度成分と、エンコーダーから算出した角速度と、ジャイロセンサーの角速度の出力信号と、を比較したグラフ、図4は、図3の一部を拡大したグラフである。ともに、横軸に時間[sec]、縦軸に角速度[deg/s]を表している。なお、図3、図4ともにジャイロセンサー30およびエンコーダー40は正常な状態の場合を示している。
【0034】
図3に示した動作指令の角速度成分は、0.1[sec]の間に+450[deg/s]まで加速させ、そこから0.08[sec]の間は一定速度を維持し、さらに、0.1[sec]の間に−450[deg/s]減速させて速度を0に戻して停止させるというものである。図3に示すように、動作指令の角速度成分に対して、ジャイロ角速度と、エンコーダー40より算出した角速度(以降、エンコーダー角速度と表すことがある)とは、僅かな遅延はあるものの、ほぼ一致している。
【0035】
また、図4の拡大図では、ジャイロ角速度は、駆動を停止したときにアーム16が振動をしていることを示し、エンコーダー40より算出した角速度とは微妙に異なるが、全体としての角速度の変化はほぼ一致している。
そこで、動作指令の角速度成分に基づき閾値を決定し、閾値とエンコーダー角速度とジャイロ角速度との角速度差を比較して、これら角速度差が閾値よりも大きい場合には故障したと判定する。よって、より適切なロボット10の故障検出が可能になることが分かる。
(ロボットの故障検出方法)
続いて、上記考え方に基づくロボット10の故障検出方法について説明する。
図5は、本実施形態に係るロボットの故障検出方法の主たる工程を示すフローチャートである。なお、図2も参照して説明する。まず、ロボット10の動作を開始し、動作の指令角速度から閾値を決定する(S1)。次に、エンコーダー40より関節角度を取得し(S2)、近似微分演算部21で関節角度を近似微分して関節角速度を算出し(S3)、次に、得られた関節角速度を、座標変換部22でジャイロセンサー30の取り付け位置の座標系角速度に変換する(S4)。
【0036】
次に、上記の座標系角速度の算出方法について説明する。
まず、ジャイロセンサー30の取り付け位置における姿勢情報と、エンコーダー角速度(関節角度)を用いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列を算出する。
【0037】
【数1】
続いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列と、関節角度から算出した関節角速度を用いて、ジャイロセンサーの取り付け位置における座標系角速度を算出する。
【0038】
【数2】
【0039】
そして、ジャイロセンサー30より角速度(ジャイロ角速度または出力角速度))を取得し(S5)、このジャイロ角速度と、関節角速度から算出した角速度(つまり、座標系角速度)と、の角速度差を演算する(S6)。そして、この角速度差と動作指令の角速度成分に基づき予め設定した閾値(工程:S1)とを比較し(S7)、角速度差が閾値よりも小さい(|角速度差|<|閾値|)場合(YES)には、ジャイロセンサー30およびエンコーダー40が正常動作であると判定し、モーター駆動を継続する(S10)。そして所定の移動動作が終了した時点(S11:YES)で動作を終了させる。所定の移動動作が終了しない(NO)場合には、エンコーダー40の関節角度取得工程(S2)から、以降の工程を繰り返す。
【0040】
工程S7において、角速度差が閾値よりも大きい(|角速度差|>|閾値|)と判定した場合(NO)には、ジャイロセンサー30またはエンコーダー40のいずれかが故障したと判定し、モーターを非常停止させる(S8)。
【0041】
以上説明したロボット10の故障検出方法を用いて故障検出した場合の1例をあげ説明する。
図6は、動作指令の角速度成分と、エンコーダー角速度と、ジャイロ角速度を比較したグラフである。横軸に時間[sec]、縦軸に角速度[deg/s]を表している。図6において、指令の角速度成分とエンコーダー情報から算出した角速度(座標系角速度)と動作指令の角速度成分に対して遅延はあるものの、ほぼ同じである。また、ジャイロ角速度は、0.23「sec」の位置で急激に0[deg/s]になっている。つまり、ジャイロセンサーの検出値に異常があったことが推定できる。
【0042】
図7は、角速度差と閾値との関係を表すグラフである。ここで、仮にマイナス側の閾値を−45[deg/s]とする。なお、図中に記載のねじれ角速度は、ジャイロセンサーの角速度とエンコーダーの検出値から算出した角速度との差(角速度差)である。この例では、0.23[sec]の位置で、ねじれ角速度の絶対値が閾値の絶対値45[deg/s]よりも大きくなっている。この位置で、ジャイロセンサー30が故障したと判定し、モーターの駆動を停止させる。
【0043】
従って、上述したロボット10及びロボットの故障検出方法によれば、エンコーダー40によって取得した関節角度を関節角速度に変換した後、ジャイロセンサー30の取り付け位置における座標系角速度に変換し、この座標系角速度と、ジャイロセンサー30から得られた角速度との角速度差を、予め動作指令の角速度成分に基づき決定した閾値と比較し、たとえば、|角速度差|<閾値である場合は、エンコーダー40およびジャイロセンサー30は共に正常であると判定し、所定の作業を継続する。また、|角速度差|>|閾値|である場合には、エンコーダー40(基部12からジャイロセンサー取り付け位置までに配置されたエンコーダーのいずれか)またはジャイロセンサー30のどちらかが故障したと判定して直ちにロボット10(モーター)の駆動を停止させる。従って、特許文献1にように、角度センサーから得られたセンサーデータを2回微分して加速度に変換する必要がないので誤差が小さいという効果がある。
【0044】
また、関節角速度をジャイロセンサーの取り付け位置における座標系角速度への変換処理は一度でよいので、前述した特許文献2よりも演算量が少なくなり、演算に係る時間も短縮できる。さらに、角速度差と閾値との差を比較し、|角速度差|>|閾値|の場合は故障と判定し直ちにロボット10の駆動を停止すれば、品質の低下を抑え、また、安全性を高めることができる。
【0045】
なお、前述した角速度差と閾値とを判定要素として用いる故障検出方法の他に、慣性センサーにより取得した角速度と座標系角速度との角速度比(割合)を算出し、動作指令から決定する閾値(割合)とを、比較する方法を用いてロボット10の故障検出を行うことができる。具体的には、図5で表すフローチャートの工程S1において動作の指令加速度成分から許容できる閾値を割合で決定し、工程S6において角速度差の代わりに角速度比を演算し、工程S7において角速度比と閾値とを比較すればよい。このようにして、|角速度比|<|閾値|の場合は正常、|角速度比|>|閾値|の場合には故障判定とすることができる。
(実施形態2)
【0046】
続いて、実施形態2について説明する。前述した実施形態1は、閾値を一定にしていることに対し、動作指令の角速度成分の大きさに対応して閾値を切り替えるところに特徴を有する。たとえば、加減速が大きい動作を指令する場合には、アームの振動が大きくなるので閾値を大きくする。一方、加減速が小さい動作を指令する場合には、アームの振動が小さくなるので閾値を小さくする。このことを実測例を図示し説明する。なお、ロボット10の構成(図1、図2、参照)および故障検出のフローチャート(図5、参照)は実施形態1と同じなので、相違箇所を中心に説明する。
【0047】
図8は、動作指令の加減速が大きい場合を表し、(a)は動作指令の角速度成分、(b)は角速度差を表している。
動作指令の角速度成分は、図8(a)に表すように、0.1[sec]間、450[deg/s]の角速度まで加速し、それから0.08[sec]の一定速度域の後、やはり0.1[sec]間で450[deg/s]の角速度を減速して、速度を0に戻した例である。
【0048】
上記条件の動作指令によって動作させたときの角速度差の測定例を図8(b)に示している。このように動作させた場合、アームの振動が大きくなることから角速度差は概ね±16[deg/s]の範囲で変動する。
次に、動作指令の加減速が小さい場合について説明する。
【0049】
図9は、動作指令の加減速が小さい場合を表し、(a)は動作指令の角速度成分、(b)は角速度差を表している。
動作指令の角速度成分は、図9(a)に表すように、0.15[sec]間で450[deg/s]の角速度まで加速し、それから0.03[sec]の一定速度域の後、やはり0.15[sec]で450[deg/s]の角速度まで減速して、速度を0に戻した例である。
【0050】
上記条件の動作指令によって動作させたときの角速度差の測定例を図9(b)に示している。このように動作させた場合、アームの振動が小さくなることから角速度差は概ね±11[deg/s]の範囲で変動する。
【0051】
図8、図9で表した例が、共に正常であるとした場合、加減速が大きい場合の閾値をたとえば、±20[deg/s]とし、加減速が小さい場合の閾値をたとえば、±15[deg/s]とすることができる。
【0052】
従って、図8、図9に示したように、動作指令の加減速が大きい場合には閾値を大きくし、動作指令の加減速が小さい場合には、閾値を小さくすることができる。このようにすれば、動作指令の加減速の大小に合わせた適切な閾値の設定を行うことができる。このことによって、故障の有無の判定をするために、過剰な安全率を設定しなくてもよいという効果がある。
(実施形態3)
【0053】
続いて、実施形態3に係るロボット10の故障検出方法について説明する。前述した実施形態2が、動作指令の加減速の大小に合わせて閾値の切り替えをすることに対し、実施形態3は、予めアームに発生する振動の大きさと閾値の対照表を備え、この対照表を参照して、振動の大きさに合わせて閾値を選択して、閾値の切り替えを行うことに特徴を有する。なお、図示は省略する。
【0054】
前述したように、アームには振動が発生する。図8(b)、図9(b)では、角速度差の変動を表しているが、この角速度差の変動はアームの振動に起因している。従って、アームの姿勢および角速度によってアームにどれくらい振動が発生するかを予め測定し、振動の大きさに対応する閾値の対照表を作成し、この対照表から適切な閾値を選択すれば、動作指令の角速度成分の大小によって閾値を切り替える方法と、同様な効果が得られる。
(実施形態4)
【0055】
次に、実施形態4について説明する。前述した実施形態1〜実施形態3が、ジャイロセンサーとエンコーダーから得た角速度を用いていることに対し、実施形態4は、アームに取り付けられた撮像手段が取得した画像から角速度を算出してジャイロセンサーとの角速度差と閾値とを比較して故障を検出することに特徴を有する。
【0056】
図10は、実施形態4に係るロボットの1例を表す模式図、図11は制御装置の主要構成を示す構成説明図である。図10に示すように、本実施形態に係るロボット10は、前述した実施形態1(図1、参照)と主要構成は同じであって、先端部のアーム18に撮像手段としてのカメラ50が取り付けられている。また、カメラ50の視野領域の基台11の延長上には、基準マーカー60が設けられている。カメラ50は、アーム18の動きに連動する。
【0057】
続いて、本実施形態の制御装置20の構成について図11を参照して説明する。制御装置20は、カメラ50(画像処理装置を含む)が取得した基準マーカー60の画像の移動前と移動後の角度の差分情報を用いて、画像取得位置の角速度に変換する近似微分演算部21と、得られた角速度をジャイロセンサー30の取り付け位置の座標系角速度に変換する座標変換部22と、この座標系角速度とジャイロセンサー30から取得した角速度とを比較し、角速度差を算出する比較部23と、を有する。さらに、動作指令の角速度成分に基づき閾値を決定する閾値決定部25と、閾値と上記角速度差とを比較して故障の有無を判定する故障判定部24と、が備えられている。
【0058】
次に、本実施形態に係るロボット10の故障検出方法を説明する。図面は省略するが、図5のフローチャートを参照して説明する。まず、動作の指令角速度から閾値を決定する(S1に相当)。次に、カメラ50が取得した基準マーカー60の画像の移動前と移動後の角度の差分情報(S2の関節角度に相当するので、以降、関節角度と表す)を取得し、この関節角度を近似微分演算部21で近似微分して画像取得位置の角速度を算出し(S3相当)、次に、得られた角速度を、座標変換部22でジャイロセンサー30の取り付け位置の座標系角速度に変換する(S4)。
【0059】
座標系角速度の算出は、以下の数式を用いて行う。
まず、画像取得位置姿勢情報と、カメラ50から取得した関節角度を用いて、関節角度・画像取得位置姿勢間ヤコビ行列を算出する。
【0060】
【数3】
続いて、カメラ50から取得した関節角度と、ジャイロセンサー30の取り付け位置姿勢情報を用いて、関節角度・ジャイロセンサー30の取り付け位置姿勢間ヤコビ行列を算出する。
【0061】
【数4】
そして、関節角度・ジャイロセンサー30の取り付け位置姿勢間ヤコビ行列と、関節角度・画像取得位置姿勢間ヤコビ行列と、画像取得位置姿勢の角速度を用いて、ジャイロセンサー30の取り付け位置の座標系角速度を算出する。
【0062】
【数5】
【0063】
次に、ジャイロセンサー30によって角速度を取得し(S5に相当)、このジャイロセンサー30の角速度(出力角速度)と、上記数式で算出したジャイロセンサーの取り付け位置の座標系角速度と、の角速度差を演算する(S6に相当)。工程S6以降の工程、および考え方は、図5に表すS7以降の工程を実行する。
【0064】
そして、角速度差が閾値よりも小さい(|角速度差|<|閾値))場合には、ジャイロセンサー30が正常動作していると判定し、角速度差が閾値よりも大きい(|角速度差|>|閾値|)と判定した場合には、ジャイロセンサー30が故障したと判定することができる。
なお、本実施形態では、ジャイロセンサー30によって取得した角速度と、カメラ50から取得した座標系角速度との比(割合と)閾値(割合)とを比較する方法でもよい。
(実施形態5)
【0065】
続いて、実施形態5について説明する。前述した実施形態1〜実施形態4では、ロボット10の故障判定は、エンコーダー40が取得した関節角度から算出した座標系角速度とジャイロセンサー30から取得する角速度との角速度差と、閾値との差で行っているが、本実施形態では、ジャイロセンサー30の角速度と、動作指令の角度成分から算出した座標系角速度に基づき決定された閾値とを比較することを特徴とする。
【0066】
なお、本実施形態のロボットおよび制御装置の構成は、前述した実施形態1(図1、図2、参照)と同じであるため、説明を省略する。但し、本実施形態では、近似微分演算部21に入力されるデータは、動作指令の角度成分である。
【0067】
次に、本実施形態によるロボットの故障検出方法を説明する。図示は省略するが、図5を参照して説明する。
まず、動作指令の角度成分から算出した座標系角速度に基づき閾値を決定する。閾値は、図1に示す基部12の基準座標軸からジャイロセンサー30の取り付け位置までの各アームの動作指令の関節角度を近似微分して動作指令の関節角速度を演算し、この動作指令の関節角速度をジャイロセンサー30の取り付け位置の座標系角速度に変換し、動作指令の座標系角速度に基づき閾値を決定する。
【0068】
次に、上記の座標系角速度の算出方法について説明する。
まず、ジャイロセンサー30の取り付け位置における姿勢情報と、動作指令の関節角度を用いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列を算出する。
【0069】
【数6】
続いて、関節角度・ジャイロセンサーの取り付け位置姿勢間ヤコビ行列と、動作指令の関節角速度を用いて、ジャイロセンサーの取り付け位置における座標系角速度を算出する。
【0070】
【数7】
【0071】
そして、ジャイロセンサー30が取得した角速度(ジャイロ角速度)と、上記数式を用いた動作指令の座標系角速度と、を比較する。なお、ジャイロ角速度と閾値の比較は、座標系角速度を一定時間遅延させたものと比較する。なお、一定時間とは、(1/(位置比例ゲイン×各軸の減速比))に比例した時間を関節ごとに設定する。これは、図3に示すように、動作指令の角速度成分と、ジャイロセンサー30の角速度検出には遅延時間があるため、この遅延時間を補正するためである。
【0072】
このようにすれば、ジャイロセンサー30が取得した角速度が閾値よりも大きい場合には、ジャイロセンサーに異常があると判定して、ロボット10の駆動を停止させることができる。
【符号の説明】
【0073】
1…ロボット、13〜18…アーム、20…制御装置、21…近似微分演算部、22…座標変換部、23…比較部、24…故障判定部、25…閾値決定部。
【特許請求の範囲】
【請求項1】
関節と、前記関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、
前記角度センサーまたは前記慣性センサーの異常を判定する閾値を決定する工程と、
前記角度センサーによって関節角度を取得する工程と、
前記関節角度を近似微分して関節角速度を算出する工程と、
前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する工程と、
前記慣性センサーによって角速度を取得する工程と、
前記慣性センサーにより取得した角速度と前記座標系角速度との角速度差、または角速度比を演算する工程と、
前記角速度差、または角速度比と、前記閾値とを比較する工程と、
を含むことを特徴とするロボットの故障検出方法。
【請求項2】
動作指令角速度または動作指令角加速度の大きさによって、前記閾値を切り替える、
ことを特徴とする請求項1に記載のロボットの故障検出方法。
【請求項3】
前記アームに発生する振動の大きさと前記閾値との対照表を備え、
前記対照表を参照し、前記アームの振動の値に対応させて予め設定された前記閾値を選択して、前記閾値を切り替える、
ことを特徴とする請求項2に記載のロボットの故障検出方法。
【請求項4】
関節と、前記関節に連結されるアームと、前記アームに取り付けられた慣性センサー及び撮像手段と、を有するロボットの故障検出方法であって、
前記慣性センサーによって角速度を取得する工程と
前記アームに取り付けられた撮像手段が取得した画像の移動前と移動後の差分情報を用いて算出した画像取得位置の角速度を、前記慣性センサーの取り付け位置の角速度に座標変換して座標系角速度を算出する工程と、
前記角速度と、前記座標系角速度とを比較する工程と
を含むこと特徴とするロボットの故障検出方法。
【請求項5】
関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、
前記慣性センサーによって角速度を取得する工程と、
動作指令の角度成分を近似微分して関節角速度を算出する工程と、
前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する工程と、
前記座標系角速度に基づき前記慣性センサーの異常を判定する閾値を決定する工程と、
前記慣性センサーによって取得した角速度と、前記閾値とを比較する工程と、
を含むことを特徴とするロボットの故障検出方法。
【請求項6】
関節と、前記関節に連結されるアームと、
前記関節の関節角度を検出する角度センサーと、
前記アームに取り付けられた慣性センサーと、
前記角度センサーまたは前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、
前記関節角度を近似微分して関節角速度を算出する近似微分演算部と、
前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する座標変換部と、
前記慣性センサーによって取得した角速度と前記座標系角速度とを比較する比較部と、
前記比較部の比較結果に基づき故障の有無を判定する故障判定部と、
を有することを特徴とするロボット。
【請求項7】
関節と、前記関節に連結されるアームと、
前記アームに取り付けられた慣性センサー及び撮像手段と、
前記撮像手段が取得した画像の移動前と移動後の角度の差分情報を用いて、画像取得位置の角速度に変換する近似微分演算部と、
前記角速度を前記慣性センサーの取り付け位置の座標系角速度に変換する座標変換部と、
前記座標系角速度と前記慣性センサーが取得した角速度とを比較し、角速度差を算出する比較部と、
前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、
前記角速度差と前記閾値とを比較して故障の有無を判定する故障判定部と、
を有することを特徴とするロボット。
【請求項1】
関節と、前記関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、
前記角度センサーまたは前記慣性センサーの異常を判定する閾値を決定する工程と、
前記角度センサーによって関節角度を取得する工程と、
前記関節角度を近似微分して関節角速度を算出する工程と、
前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する工程と、
前記慣性センサーによって角速度を取得する工程と、
前記慣性センサーにより取得した角速度と前記座標系角速度との角速度差、または角速度比を演算する工程と、
前記角速度差、または角速度比と、前記閾値とを比較する工程と、
を含むことを特徴とするロボットの故障検出方法。
【請求項2】
動作指令角速度または動作指令角加速度の大きさによって、前記閾値を切り替える、
ことを特徴とする請求項1に記載のロボットの故障検出方法。
【請求項3】
前記アームに発生する振動の大きさと前記閾値との対照表を備え、
前記対照表を参照し、前記アームの振動の値に対応させて予め設定された前記閾値を選択して、前記閾値を切り替える、
ことを特徴とする請求項2に記載のロボットの故障検出方法。
【請求項4】
関節と、前記関節に連結されるアームと、前記アームに取り付けられた慣性センサー及び撮像手段と、を有するロボットの故障検出方法であって、
前記慣性センサーによって角速度を取得する工程と
前記アームに取り付けられた撮像手段が取得した画像の移動前と移動後の差分情報を用いて算出した画像取得位置の角速度を、前記慣性センサーの取り付け位置の角速度に座標変換して座標系角速度を算出する工程と、
前記角速度と、前記座標系角速度とを比較する工程と
を含むこと特徴とするロボットの故障検出方法。
【請求項5】
関節に連結されるアームと、前記関節の回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、を有するロボットの故障検出方法であって、
前記慣性センサーによって角速度を取得する工程と、
動作指令の角度成分を近似微分して関節角速度を算出する工程と、
前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する工程と、
前記座標系角速度に基づき前記慣性センサーの異常を判定する閾値を決定する工程と、
前記慣性センサーによって取得した角速度と、前記閾値とを比較する工程と、
を含むことを特徴とするロボットの故障検出方法。
【請求項6】
関節と、前記関節に連結されるアームと、
前記関節の関節角度を検出する角度センサーと、
前記アームに取り付けられた慣性センサーと、
前記角度センサーまたは前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、
前記関節角度を近似微分して関節角速度を算出する近似微分演算部と、
前記関節角速度を前記慣性センサーの取り付け位置における座標系角速度に変換する座標変換部と、
前記慣性センサーによって取得した角速度と前記座標系角速度とを比較する比較部と、
前記比較部の比較結果に基づき故障の有無を判定する故障判定部と、
を有することを特徴とするロボット。
【請求項7】
関節と、前記関節に連結されるアームと、
前記アームに取り付けられた慣性センサー及び撮像手段と、
前記撮像手段が取得した画像の移動前と移動後の角度の差分情報を用いて、画像取得位置の角速度に変換する近似微分演算部と、
前記角速度を前記慣性センサーの取り付け位置の座標系角速度に変換する座標変換部と、
前記座標系角速度と前記慣性センサーが取得した角速度とを比較し、角速度差を算出する比較部と、
前記慣性センサーの異常を判定する閾値を決定する閾値決定部と、
前記角速度差と前記閾値とを比較して故障の有無を判定する故障判定部と、
を有することを特徴とするロボット。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【公開番号】特開2013−833(P2013−833A)
【公開日】平成25年1月7日(2013.1.7)
【国際特許分類】
【出願番号】特願2011−133939(P2011−133939)
【出願日】平成23年6月16日(2011.6.16)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
【公開日】平成25年1月7日(2013.1.7)
【国際特許分類】
【出願日】平成23年6月16日(2011.6.16)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
[ Back to top ]