JPWO2014064926A1 - ロボットの異常表示方法 - Google Patents

Abstract

ロボットのセンサレス衝突検出において、従来のロボットの異常表示方法では、衝突誤検出が発生した際に「衝突検出」とのみ異常表示され、どのような状況で衝突誤検出が発生したのかが不明であった。ロボットの衝突を検出した場合、異常表示として衝突検出したことを表示するともに、衝突検出とは異なる複数の異常検出項目のうち少なくとも１つ以上の異常表示を選択して行うことで、衝突誤検出発生時の状況を把握するために有用な情報を得ることができる。

Description

本発明は、減速機を介してモータにより駆動されるロボットの異常表示方法に関する。

近年、ロボット市場がグローバルに拡大するにつれ、ロボットが故障した場合や異常が発生した場合の対応が大きな課題となっている。すなわち、ロボットの異常表示が、実際の故障状態を把握できない状態であると、異常発生現場へサービスマンが直接行って状況を確認し、その後に対策を実施する必要がある。この方法では、全世界への異常状態解消の迅速な対応が困難となる。

一方で、ロボット自体の故障ではなく、ユーザの教示ミス等が原因で発生する、ロボットのロボット周辺物との衝突への対応も必要である。ロボットや周辺物が衝突により破壊すると、修理等のためサービスマンの派遣が必要となり、復旧に時間を要する。すなわち、衝突時の破壊防止のため、衝突検出の高精度化が求められている。しかし、高精度の衝突センサをロボットに搭載すると、ロボットにとっては余分な重量負荷となり、かつ、コストも上昇する。そこで、近年では、センサレス衝突検出の高精度化が進展している。

しかしながら、センサレス衝突検出の高精度化により、実際の衝突ではない場合も、ロボットの異常状態を誤って衝突と検出する可能性が高くなる。すなわち、ロボットの故障発生時に、他の異常検出機能よりも先に衝突検出機能が異常を検出し、衝突が発生していないにもかかわらず「衝突発生」といった誤った表示を行う可能性が有る。このように、センサレス衝突検出の高精度化により、故障を速やかに検出するという利点がある反面、故障原因をわかり難くする原因ともなっている。誤った衝突検出が発生する理由を図１〜図１２を用いて、以下に説明する。

図１は、ロボットの代表例として垂直多関節６軸ロボットを示す概略構成図である。図１に示すように、垂直多関節６軸ロボットは、ロボットメカニズム（１０１）と、ロボット制御装置（１０２）と、操作表示装置（１０３）とを有する。

図２は、異常表示の全体処理を示すフローチャートである。異常表示の全体処理は、ロボット制御装置（１０２）内で行われる。図２では、衝突検出処理以外にも、ロボット故障発生時の異常を検出するモータロック検出処理や、過負荷検出処理や、位置偏差オーバー検出処理を加えている。図２に示される異常表示の全体処理は、周期ΔＴ毎に繰り返され、周期ΔＴは数ミリ秒を想定している。

図２において、ステップ２−１では、モータロック検出処理を行う。ステップ２−６では、モータロックが検出されると、図１の操作表示装置（１０３）に異常表示として「モータロック検出」を表示する。モータロックが検出された場合、既に異常状態が発生し異常表示もされているので、ステップ２−６で異常表示処理を終了させる。ステップ２−６でモータロックが検出されなかった場合、ステップ２−２に進む。

次に、ステップ２−２では、過負荷検出処理を行う。ステップ２−７では、過負荷が検出されると、図１の操作表示装置（１０３）に異常表示として「過負荷検出」を表示する。過負荷が検出された場合は、既に異常状態が発生し異常表示もされているので、ステップ２−７で異常表示処理を終了させる。ステップ２−７で過負荷が検出されなかった場合、ステップ２−３に進む。

次に、ステップ２−３では、位置偏差オーバー検出処理を行う。ステップ２−８では、位置偏差オーバーが検出されると、図１の操作表示装置（１０３）に異常表示として「位置偏差オーバー検出」を表示する。位置偏差オーバーが検出された場合は、既に異常状態が発生し異常表示もされているので、ステップ２−８で異常表示処理を終了させる。ステップ２−８で位置偏差オーバーが検出されなかった場合、ステップ２−４に進む。

次に、ステップ２−４では、図３を用いて下記で説明する衝突検出処理を実行する。

衝突力をセンサレスで求める方法としては、動力学演算方式と外乱推定オブザーバ方式がある。動力学演算方式は、まず、モータの駆動電流で発生したトルクからモータ及び減速機のイナーシャと摩擦で損失するトルクを差し引いて減速機出力トルクを求める。次に、減速機出力トルクから、ロボットの動力学演算で求めたロボットの動力学トルクを差し引いて衝突力を求める。（例えば、非特許文献１参照。）。外乱推定オブザーバ方式は、外乱推定オブザーバを用いて衝突力を求める。（例えば、特許文献１参照。）。以下、図３を用いて、推定精度に優れる動力学演算方式を例に説明する。

図３は、動力学演算方式の制御ブロック線図である。図３において、６は位置制御ブロックである。位置制御ブロック（６）には、位置指令の速度成分ｄθｃｏｍ（１）を積分要素４０によって積分した位置指令θｃｏｍ（３）と、モータ位置θｍ（４）とが入力される。そして、位置指令θｃｏｍ（３）とモータ位置θｍ（４）との差分値に基づいて、位置比例ゲイン（５）から速度ループ指令ωｃｏｍ（７）を出力する。

図３において、１０は速度制御ブロックである。速度制御ブロック（１０）には、速度ループ指令ωｃｏｍ（７）と、モータ位置θｍ（４）とが入力される。そして、速度ループ指令ωｃｏｍ（７）と、モータ位置θｍ（４）を微分要素（３２）によって微分したモータ速度ωｍ（２）との差分値を求め、速度比例ゲイン（８）および速度積分ゲイン（９）を介して、モータ電流Ｉｍ（１１）を出力する。

図３において、１８はモータと外力を示したブロックである。τｍ（１３）はモータ発生トルクであり、減速機が剛体であると仮定すると、モータ駆動側から見れば式（１）で表され、負荷側から見れば式（２）で表される。

τｍ＝Ｋｔ×Ｉｍ （１）
τｍ＝τｄｙｎ＋τｄｉｓ＋Ｊｍ×αｍ＋Ｄ×ωｍ＋τμ （２）
ただし、式（１）と式（２）における記号は以下の通りである。

Ｋｔ（１２）：モータトルク定数
Ｉｍ（１１）：モータ電流
αｍ（９０）：モータ加速度（ωｍの微分値）
ωｍ（２） ：モータ速度
Ｊｍ（８９）：モータイナーシャ（ロータ＋減速機１次側）
Ｄ（２０） ：粘性摩擦係数
τμ（１５）：動摩擦トルク
τｄｙｎ（１４）：動力学トルク（重力トルク、慣性力、遠心力、コリオリ力の和）
τｄｉｓ（１６）：衝突トルク
動摩擦トルクτμ（１５）は以下の式（３）で計算できる。

τμ＝Ｋμ×ｓｇｎ （３）
ただし、Ｋμは動摩擦の大きさであり、ｓｇｎは、ωｍ＞０の時は「１」、ωｍ＝０の時は「０」、ωｍ＜０の時は「−１」である。

衝突トルクτｄｉｓ（１６）は、式（１）と式（２）より、以下の式（４）に変形して求めることが出来る。

τｄｉｓ＝Ｋｔ×Ｉｍ−（Ｊｍ×αｍ＋Ｄ×ωｍ＋τμ＋τｄｙｎ） （４）
図３において、３０は、式（４）を衝突トルク推定ブロックとして表したものである。

衝突トルク推定ブロック（３０）において、動力学トルク推定値τｄｙｎｏ（２９）は、動力学トルク演算ブロック（２６）が動力学演算することで求められる。動力学トルク演算ブロック（２６）は、ロボットを構成する全軸のモータ位置θｍ（４）と、モータ位置θｍ（４）を微分したモータ速度ωｍ（２）と、モータ速度ωｍ（２）を微分したモータ加速度αｍ（９０）と、ロボットの機械パラメータ（アーム長、アーム質量、重心位置、重心位置周りのイナーシャ）を用いて、動力学演算を実行する。この動力学トルク推定値τｄｙｎｏ（２９）を用いて、衝突トルク推定ブロック（３０）は、衝突トルク推定値τｄｉｓｏ（２８）を衝突判定ブロック（３１）へ出力する。

衝突判定ブロック（３１）は、所定の衝突検出閾値τｔｈを用いて、以下の式（５）に従い衝突を検出する。

｜τｄｉｓｏ｜ ＞ τｔｈ （５）
ステップ２−５では、式（５）が成立すれば、図１のロボットの操作表示装置（１０３）に異常表示として「衝突検出」を表示し、異常表示処理を終了させる。また、ステップ２−５では、式（５）が成立していなければ、異常表示をせずに異常表示処理を終了させる。そして、また、周期ΔＴごとに異常表示の全体処理を開始する。

以上の衝突検出では、前述のように、動力学トルク推定値τｄｙｎｏ（２９）が正確に計算できれば、精度良く衝突トルク推定値τｄｉｓｏ（２８）を求めることができる。

動力学トルク演算ブロック（２６）で動力学トルクを求める方法としては、ラグランジュ法とニュートンオイラー法とがある。ラグランジュ法では、６軸の垂直多関節ロボットでは、１０万回弱の積和演算が必要である。しかし、この演算量は、現在のＣＰＵの処理能力からすれば、数ミリ秒以内で演算することも可能である。ニュートンオイラー法では、積和演算量が１万回弱で済むので、演算時間は問題無い。ニュートンオイラー法は、前式の結果を次式に用いる、いわゆる漸化式方式であるので、演算の累積誤差が発生する恐れがある。しかし、３２ｂｉｔ以上のデータで処理すれば演算の累積誤差も問題無い。よって、どちらの方法を用いたとしても、現在のＣＰＵでは、動力学トルクτｄｙｎ（１４）をモータ最大トルク比１０％以内の誤差で計算可能である。式（４）の右辺で動力学トルクτｄｙｎ（１４）以外の項も、モータ最大トルク比１０％以内の誤差で計算可能であれば、衝突トルク推定値τｄｉｓｏ（２８）は、モータ最大トルク比２０％以内の誤差で求めることができる。

また、式（５）による衝突検出では、式（４）で求めた衝突トルク推定値τｄｉｓｏ（２８）の瞬時値を用いている。そのため、計算時間である数ミリ秒経てば、動力学トルクτｄｙｎ（１４）の判定が可能である。すなわち、衝突検出機能では、モータ最大トルク比２０％程度の異常が数ミリ秒観測されれば、衝突を検出することが可能である。

しかし、衝突検出閾値τｔｈを低く（モータ最大トルク比２０％程度に）設定すれば、衝突以外の故障でもいち早く検出してしまう。例えば、モータ（ベアリング、ブレーキ）や減速機の故障により、実際の動摩擦トルクが増加した場合について、検討する。この場合、正常時と同じようにロボットを動作させようとすると、動摩擦トルクτμの増大分だけモータ発生トルクτｍを増大させる必要がある。モータ発生トルクτｍを増加させるため、式（４）の右辺のモータ電流Ｉｍが増加する。しかし、式（４）の右辺で用いる動摩擦トルクτμは、予め正常時に測定した値を用いて計算するため、式（４）の左辺である衝突トルクτｄｉｓは、衝突していないにも関わらず増大する。そして、衝突トルクτｄｉｓの大きさがモータ最大トルク比２０％を数ミリ秒でも越えると、衝突検出として誤検出してしまう。

衝突力をセンサレスで求める方法では、式（４）の右辺のモータ電流Ｉｍが増加し、衝突トルクτｄｉｓが増大することの原因が特定できない。すなわち、ロボットの実衝突に起因するのか、モータや減速機の故障による摩擦トルク増大に起因するのかが、区別ができない。そのため、ロボット衝突時の損傷を少しでも少なくするために、衝突検出閾値τｔｈを低く（衝突検出感度を高く）設定すると、衝突誤検出の発生確率も高めてしまう。

一方、衝突検出機能とは別に、モータ（ベアリング、ブレーキ）や減速機の故障によりモータの回転が重くなったことを検出する機能としては、モータロック検出機能や過負荷検出機能がある。

モータロック検出機能では、モータを動作させようとしているのに殆ど動かない状態が継続した状況を検出する。モータ発生トルクτｍ（式（１）で示す）が、所定のモータトルク閾値τｍｔｈ（９１）以上になっても、故障により増大した摩擦トルクが大きいと、モータ速度ωｍ（２）が所定のモータ速度閾値ωｍｔｈ以上に上がらない。この状態が、所定のモータロック検出時間閾値ＬＫｔｈ以上に継続した場合に、モータロック検出機能が異常と判定する。

モータロック検出機能について、図５Ａおよび図５Ｂを用いて説明する。図５Ａは、正常時のモータ速度ωｍ（２）とモータ発生トルクτｍ（１３）とを示すグラフである。モータ発生トルクτｍ（１３）がモータトルク閾値τｍｔｈ（９１）を越えてから、モータロック検出時間閾値ＬＫｔｈ（９３）より小さい時間ＬＫ０（９５）が経過した後は、モータ速度ωｍ（２）がモータ速度閾値ωｍｔｈ（９２）を越えている。この場合は、モータロックは検出されない。

図５Ｂは、モータや減速機の故障による摩擦トルクの増大でモータロックが検出されるという異常時のモータ速度ωｍ（２）とモータ発生トルクτｍ（１３）とを示すグラフである。図５Ｂでは、モータ発生トルクτｍ（１３）がモータトルク閾値τｍｔｈ（９１）を越えてからも、モータや減速機の故障による摩擦トルクの増大のせいで、正常時（点線で示す）に比べモータ速度ωｍ（２）の増加が遅くなる。そのため、モータ速度ωｍ（２）は、モータロック検出時間閾値ＬＫｔｈ（９３）になっても、モータ速度閾値ωｍｔｈ（９２）に達しない。この場合、モータロックが検出される。

図４は、上記モータロック検出のフローチャートである。モータロック検出処理は一定周期ΔＴ毎に実行される。

ステップ４−１では、モータ発生トルクτｍがモータトルク閾値τｍｔｈ（９１）以上になっているか否かを判定する。モータ発生トルクτｍがモータトルク閾値τｍｔｈ（９１）以上になっていると、「Ｙ」と判定し、ステップ４−２へ進む。

ステップ４−２では、モータ速度ωｍ（２）がモータ速度閾値ωｍｔｈ（９２）以下であるか否かを判定する。モータ速度ωｍ（２）がモータ速度閾値ωｍｔｈ（９２）以下であると、「Ｙ」と判定し、ステップ４−３へ進む。

ステップ４−３では、式（５）によって、モータロック継続時間ＬＫｄｅｔを算出し、ステップ４−５に進む。

ＬＫｄｅｔ＝ＬＫｄｅｔ＋ΔＴ （６）
ここで、ΔＴは、モータロック検出処理周期である。

すなわち、ステップ４−３では、ステップ４−１およびステップ４−２がともに「Ｙ」と判定されている時間の長さを、モータロック検出処理周期ΔＴずつ加算して測定するものである。

なお、ステップ４−１あるいはステップ４−２の判定が「Ｎ」なら、ステップ４−４に進み、モータロック継続時間ＬＫｄｅｔを０にリセットし、モータロック検出の処理を終える。

ステップ４−５では、モータロック継続時間ＬＫｄｅｔがモータロック検出時間閾値ＬＫｔｈ（９３）以上になったか否かを判定する。

ＬＫｄｅｔ≧ＬＫｔｈ （７）
モータロック継続時間ＬＫｄｅｔがモータロック検出時間閾値ＬＫｔｈ（９３）以上であると、「Ｙ」と判定し、ステップ４−６へ進む。この場合、モータロック状態であると判定され、ステップ４−６でモータを停止する。続いて、ステップ４−７で「モータロック検出」の異常表示を行い、モータロック検出の処理を終了する。なお、異常表示は、図１に示す操作表示装置（１０３）に表示する。

ステップ４−５の判定が「Ｎ」なら、モータロック状態では無いと判定し、モータロック検出の処理を終了する。

このモータロック検出機能では、正常時は、モータトルク閾値τｍｔｈ（９１）以上のモータトルクで、モータ速度閾値ωｍｔｈ（９２）以上のモータ速度に到達するまでには、所要時間ＬＫ０（９５）を要する。そのためモータロック検出時間閾値ＬＫｔｈ（９３）は、正常時の所要時間ＬＫ０（９５）よりも大きくする必要が有る。ロボットの大きさにより異なるが、モータロック検出時間閾値ＬＫｔｈ（９３）は、数百ミリ秒から数秒であり、衝突検出機能が数ミリ秒内で検出することに比べれば遅い。すなわち、衝突検出閾値τｔｈを低く（衝突検出感度を高く）設定すると、実際に衝突していないにも関わらず、モータロックが検出される前に衝突が先に検出されてしまい、異常表示も「衝突検出」とのみ表示されることになる。図２において説明すると、ステップ２−６によるモータロック検出では「Ｎ」と判定されて続けている間に、ステップ２−５による衝突検出では「Ｙ」と判定される。

次に、過負荷検出機能について説明する。

過負荷検出機能では、モータを駆動させる電流の時間積算値が、モータ時限特性曲線の限界値を超えない様に、過負荷異常を検出する。モータ発生トルクはモータを駆動させる電流に比例し、モータ時限特性曲線の限界値とは、モータの巻線温度の上限を示す。図７は、モータを駆動させるモータ電流と時間の関係を示すグラフである。例えば、図７に示すモータ時限曲線の限界値（点線）は、最大電流比４０％の時は約１００秒、最大電流比５０％の時は２５秒、最大電流比７０％の時は約８秒である。この限界値以下になる様に過負荷検出を行う。

図６は、過負荷検出のフローチャートである。過負荷検出処理は一定周期ΔＴ毎に実行される。

ステップ６−１で、過負荷検出値ＯＬｄｅｔを以下の式（８）で求める。

ＯＬｄｅｔ＝ＯＬｄｅｔ＋（｜Ｉｍ｜−Ｉｍｔｈ）×ΔＴ （８）
ここで、Ｉｍｔｈは過負荷検出電流閾値であり、ΔＴは過負荷検出周期である。

次に、ステップ６−２では、過負荷検出値ＯＬｄｅｔが過負荷閾値ＯＬｔｈ以上であるか否かを判定する。すなわち、下記式（９）の条件が成立するかどうかを判定する。

ＯＬｄｅｔ ≧ ＯＬｔｈ （９）
式（９）が成立すると、「Ｙ」と判定し、過負荷状態であると判定され、ステップ６−３へ進む。ステップ６−３では、モータを停止し、続いて、ステップ６−４で「過負荷検出」であることを示す「異常表示」を行い、過負荷検出の処理を終了する。なお、「異常表示」は、図１に示す操作表示装置（１０３）に表示する。

ステップ６−２の判定が「Ｎ」なら、過負荷状態では無いとの判定し、過負荷検出の処理を終了する。

図８に、式（９）でＯＬｄｅｔ＝ＯＬｔｈとなる過負荷検出閾値特性を実線で示す。図８の過負荷検出閾値特性（実線）は、Ｉｍｔｈ＝３３、ＯＬｔｈ＝１００の場合を示している。過負荷検出閾値特性（実線）は限界値（点線）を下回る様に設定されており、式（８）で示す過負荷検出値ＯＬｄｅｔの演算方法が妥当である事を示している。

図８に示すように、モータ電流が最大電流比１００％でも過負荷検出には数秒かかることがわかる。この過負荷検出にかかる時間は、衝突検出機能が数ミリ秒内で検出することに比べれば遅い。つまり、過負荷異常が発生していない状態からモータや減速機の故障が発生すると、モータ回転が重くなり、モータ電流（モータ発生トルク）が最大電流比で２０％程度増加する。このようにモータや減速器の故障が発生しても、過負荷異常が検出されるには最低でも数秒以上かかり、先に衝突誤検出が発生する可能性が極めて高い。図２において説明すると、ステップ２−７による過負荷検出では「Ｎ」と判定されて続けている間に、ステップ２−５による衝突検出では「Ｙ」と判定される。

次に、図３に示す衝突検出処理における誤検出について説明する。

すなわち、モータ位置θｍ（４）を検出するモータ位置検出器（９９）に異常が有った場合でも、衝突検出として誤検出する可能性がある。

動力学トルク演算ブロック（２６）には、モータ位置θｍ（４）を２回微分して生成するモータ加速度αｍ（９０）を用いている。そのため、モータ位置θｍ（４）の変化は、モータ加速度αｍ（９０）に大きく影響する。

一般的に、デジタル制御における微分は、現サンプル値とサンプル１周期前の前サンプル値の差分によって行われることが一般的である。そこで、一例を示す。

図９は、モータ位置検出器に異常が発生した時のモータ位置波形を示すグラフである。図１０はモータ位置検出器に異常が発生した時の速度波形を示すグラフである。図１１はモータ位置検出器に異常が発生した時の加速度波形を示すグラフである。モータ位置θｍ（４）が０の位置での停止状態から、０．５秒でモータ速度ωｍ（２）が５０回転／秒（＝３０００ｒｐｍ）に等加速度（１００回転／秒^２）で到達する場合の波形を示している。ここで、サンプル周期は５ミリ秒であり、５００ミリ秒後のモータ位置θｍ（４）は図９に示すように、約１２．５回転である。図９は、３００ミリ秒の時点でモータ位置検出器（９９）に異常が発生し、モータ位置θｍ（４）の読み取りができなかった場合である。この場合、前回サンプル値（２９５ｍ秒時点）でのモータ位置θｍ（４）の値が更新されず、そのままの状態になっている。

５００ミリ秒後の値（１２．５回転）を基準に、異常が発生した３００ミリ秒時のモータ位置θｍ（４）の誤差ｅθｍ（８６）を計算すると、約４．７％（約０．１５回転分）である。図１０において、モータ速度ωｍ（２）の誤差ｅωｍ（８７）も、５００ミリ秒後の値（５０回転／秒）を基準に算出すると、約１２０％になる。さらに図１１において、モータ加速度αｍ（９０）の誤差ｅαｍ（８８）も、５００ミリ秒後の値（１００回転／秒^２）を基準に算出すると、約−３３３３％となる。従って、動力学トルク演算ブロック（２６）で求める動力学トルクτｄｙｎ（１４）の精度に大きな影響を与え、衝突誤検出を引き起こす可能性が高い。

衝突力をセンサレスで求める方法において、モータ位置θｍ（４）が変わらないことの原因は特定できない。すなわち、実際に衝突が発生してロボットが停止したことによるのか、モータ位置検出器（９９）に異常が有ってモータ位置θｍ（４）が更新されなかったからなのかは不明である。そのため、衝突検出機能を有効に働かせようとすると、衝突誤検出の発生を免れることはできない。

一方、衝突検出機能とは別に、モータ位置θｍ（４）を検出するモータ位置検出器（９９）に異常が有ったことを検出する機能としては、位置偏差オーバー検出機能がある。

次に、位置偏差オーバー検出機能について説明する。

図１２は、位置偏差オーバー検出のフローチャートである。位置偏差オーバー検出処理は一定周期ΔＴ毎に実行される。

ステップ１２−１では、下記の式（１０）に示すように、位置偏差オーバー検出値θｄｅｔを、位置指令θｃｏｍ（３）とモータ位置θｍ（４）の差分の絶対値として算出する。

θｄｅｔ＝｜θｃｏｍ−θｍ｜ （１０）
ステップ１２−２では、位置偏差オーバー検出値θｄｅｔと位置偏差オーバー検出閾値θｔｈとを比較し、下記の式（１１）の条件を満たすかどうかを判定する。

θｄｅｔ≧θｔｈ （１１）
位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出閾値θｔｈ以上であると、「Ｙ」と判定し、ステップ２−３へ進む。この場合、位置偏差オーバー状態であると判定され、ステップ１２−３でモータを停止する。続いて、ステップ１２−４で「位置偏差オーバー」の異常表示を行い、位置偏差オーバー検出の処理を終了する。なお、異常表示は、図１に示す操作表示装置（１０３）に表示する。

ステップ１２−２の判定が「Ｎ」なら、位置偏差オーバー状態では無いと判定し、位置偏差オーバー検出の処理を終了する。

この位置偏差オーバー検出において、制御遅れによる位置偏差を誤検出しないように、式（１１）における位置偏差オーバー検出閾値θｔｈは、数回転程度に設定される。

例えば、制御遅れによる位置偏差が１回転であって、位置偏差オーバー検出閾値θｔｈを２回転に設定していたとする。この時、図９で異常が発生した３００ミリ秒時のモータ位置θｍ（４）の誤差（約０．１５回転分）が加算されても、位置偏差オーバー検出値θｄｅｔは１．１５回転程度であり、式（１１）の条件は成立せず、位置偏差オーバーは検出されない。しかし、動力学トルク演算ブロック（２６）で求める動力学トルクτｄｙｎ（１４）の精度には大きな影響を与えるため、誤って衝突検出が検出される。すなわち、実際に衝突していないにも関わらず、衝突が先に検出されてしまい、異常表示も「衝突検出」とのみ表示されることになる。図２において説明すると、ステップ２−８による位置偏差オーバー検出では「Ｎ」と判定されて続けている間に、ステップ２−５による衝突検出では「Ｙ」と判定される。

図２のステップ２−５の衝突検出発生時の異常表示処理として、従来は、図２０で示す単一異常表示処理を実行する。

これまで説明した様に、図２に示すステップ２−１〜２−３における異常検出には時間がかかるため、ステップ２−４における衝突検出が先に検出され、ステップ２−５で「衝突検出」とのみ異常表示される。

特許第３３６７６４１号公報

小菅一弘、他１名、"マニピュレータの動的衝突検出"、日本機械学会［Ｎｏ．９９−９］ロボティクス・メカトロニクス講演会‘９９講演論文集 ２Ａ１−１１−０３０

従来の衝突力をセンサレスで求める方法では、衝突検出がロボットの実際の衝突によるのか、その他の原因によるものなのかが、区別できない。そして、衝突検出が実際の衝突によるものではない場合も「衝突検出」とのみ表示され、どのような状況で衝突誤検出が発生したのか不明である。

本発明は、上記課題を解決するものであり、衝突誤検出発生時の状況を把握するのに有用な情報を表示することができるロボットの異常表示方法を提供する。

上記課題を解決するために、本発明のロボットの異常表示方法は、ロボットの動作時に検出した値に基づいて少なくとも衝突検出を含む複数の異常を検出し、異常が発生したことを表示するロボットの異常表示方法である。さらに、ロボットの衝突を検出する衝突検出工程を有する。さらに、異常表示として衝突検出したことを表示する衝突検出表示工程を有する。さらに、衝突検出とは異なる複数の異常検出項目のうち少なくとも１つについても異常表示として異常が発生したことを表示する異常表示工程を有する。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、異常表示工程において、複数の異常検出項目の異常表示を選択して行うとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、複数の異常検出項目の各々には、異常と判定するための異常判定閾値と、異常判定閾値より小さくロボットの衝突を検出した場合に衝突検出したことを表示することと併せて異常が発生したことを表示するための表示可否閾値とが設定されている。さらに、複数の異常検出項目のうち、ロボットの動作時に検出した値に基づく異常判定値が表示可否閾値を超えたものは異常表示として異常が発生したことを表示するとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、複数の異常検出項目の各々には、異常と判定するための異常判定閾値と、異常判定閾値より小さくロボットの衝突を検出した場合に衝突検出したことを表示することと併せて異常が発生したことを表示するための表示可否閾値とが設定されているとよい。さらに、複数の異常検出項目には優先順位が設定されているとよい。さらに、優先順位が高いものから順にロボットの動作時に検出した値に基づく異常判定値が表示可否閾値を超えたか否かを判定するとよい。さらに、表示可否閾値を超えた異常検出項目のみ異常表示を行うとよい。さらに、異常表示を行った異常検出項目より優先順位が低い異常検出項目については、異常判定値が表示可否閾値を超えたか否かの判定および異常表示を行わないとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、設定された衝突検出閾値に基づいて各異常検出項目についての表示可否閾値を決定するとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、各異常検出項目について異常判定閾値に対するロボットの動作時に検出した値に基づく異常判定値の割合を求めるとよい。さらに、異常判定値の割合が最も高い異常検出項目のみ衝突検出とともに異常表示を行うとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、複数の異常検出項目の各々には、異常と判定するための異常判定閾値が設定されているとよい。さらに、複数の異常検出項目のそれぞれについて、異常判定閾値に対するロボットの動作時に検出した値に基づく異常判定値の割合を求めるとよい。さらに、ロボットの衝突を検出した場合に衝突検出したことを表示することと併せて各異常検出項目のそれぞれについて、異常判定値の割合を表示するとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、複数の異常検出項目の各々には、異常と判定するための異常判定閾値が設定されているとよい。さらに、複数の異常検出項目のそれぞれについて、異常判定閾値に対するロボットの動作時に検出した値に基づく異常判定値の割合を求めるとよい。さらに、ロボットの衝突を検出した場合に衝突検出したことを表示することと併せて、異常判定値が表示可否閾値を超えたもののみ各異常検出項目の異常判定値の割合を表示するとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、複数の異常検出項目として、ロボットを駆動するモータのロック検出とモータの過負荷検出とロボットの位置偏差検出との少なくとも１つを含むとよい。

以上のように、本発明のロボットの異常表示方法においては、衝突検出発生時、他の異常検出手段の状況も併せて表示することにより、衝突誤検出発生時のロボットの動作状況の把握に役立つものである。

図１は、本発明を適用する代表例としての垂直多関節６軸ロボットの概略構成を示す図である。 図２は、本発明の実施の形態における異常表示全体処理を示すフローチャートである。 図３は、本発明の実施の形態における動力学演算方式の制御ブロック線図である。 図４は、本発明の実施の形態におけるモータロック検出を示すフローチャートである。 図５Ａは、正常時のモータ速度とモータトルクの時間変化を示すグラフである。 図５Ｂは、モータロック検出時のモータ速度とモータトルクの時間変化を示すグラフである。 図６は、本発明の実施の形態における過負荷検出を示すフローチャートである。 図７は、モータ時限特性曲線（モータの巻線温度上昇の限界）を示すグラフである。 図８は、過負荷検出閾値特性を示すグラフである。 図９は、モータ位置検出器異常発生時のモータ位置波形を示す図である。 図１０は、モータ位置検出器異常発生時のモータ速度波形を示す図である。 図１１は、モータ位置検出器異常発生時のモータ加速度波形を示す図である。 図１２は、本発明の実施の形態における位置偏差オーバー検出を示すフローチャートである。 図１３は、本発明の実施の形態１における第１の複数異常表示処理を示すフローチャートである。 図１４は、本発明の実施の形態２における第２の複数異常表示処理を示すフローチャートである。 図１５は、本発明の実施の形態３における第３の複数異常表示処理を示すフローチャートである。 図１６は、本発明の実施の形態４における第４の複数異常表示処理を示すフローチャートである。 図１７は、本発明の実施の形態５における第５の複数異常表示処理を示すフローチャートである。 図１８は、本発明の実施の形態６における第６の複数異常表示処理を示すフローチャートである。 図１９は、本発明の実施の形態７における第７の複数異常表示処理を示すフローチャートである。 図２０は、従来の単一異常表示処理を示すフローチャートである。

（実施の形態１）
以下、本発明の実施の形態１について、図面を用いて説明する。なお、背景技術で説明したものと同様の事項については、同一の符号を付して詳細な説明を省略する。

本実施の形態１では、背景技術でも説明した図２のステップ２−５の衝突検出発生異常表示処理として、図１３のフローチャートに示す第１の複数異常表示処理を実行する。

図１３に示す第１の複数異常表示処理では、ロボットの衝突を検出した場合、異常表示として衝突検出したことを表示する。さらに、衝突検出とは異なる複数の異常検出項目のうち、少なくとも１つ以上の異常表示を選択して行う処理（ステップ１３−３〜ステップ１３−８）を備えている。

さらに、衝突検出とは異なる複数の異常検出ステップ（ステップ１３−３、ステップ１３−５、ステップ１３−７）の各々には、異常判定閾値よりも小さい表示可否閾値が設定されている。

そして、複数の異常検出項目のうち、ロボットの動作時に検出した値（例えば、モータ発生トルクτｍ、モータ速度ωｍ、モータ電流Ｉｍ、モータ位置θｍ）に基づく異常判定値（モータロック検出値ＬＫｄｅｔ、過負荷検出値ＯＬｄｅｔ、位置偏差オーバー検出値θｄｅｔ）が表示可否閾値を超えたものは、異常表示として異常が発生したことを表示する。（ステップ１３−４、ステップ１３−６、ステップ１３−８）
図１３において、ステップ１３−１では、ロボットの衝突検出が発生したか否かを判定する。ステップ１３−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１３−２に進む。ステップ１３−２では、「衝突検出」の異常表示を行い、ステップ１３−３へ進む。ステップ１３−１において、ロボットの衝突検出が発生しないと、「Ｎ」と判定し、その後は何も実施せず、第１の複数異常表示処理を終了する。

ステップ１３−３では、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っているか否かを判定する。ステップ１３−３において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていると「Ｙ」と判定し、ステップ１３−４に進む。ステップ１３−３において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていないと、「Ｎ」と判定し、ステップ１３−５に進む。

ここで、モータロック検出表示可否閾値ＬＫｔｈｄは、下記式（１２）で示すように、モータロック検出時間閾値ＬＫｔｈより小さく設定するものとする。

０ ＜ ＬＫｔｈｄ ＜ ＬＫｔｈ （１２）
ステップ１３−４では、異常表示として「モータロック検出」を追加して表示し、ステップ１３−５へ進む。

ステップ１３−５では、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っているか否かを判定する。ステップ１３−５において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていると「Ｙ」と判定し、ステップ１３−６に進む。ステップ１３−５において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１３−７に進む。

ここで、過負荷検出表示可否閾値ＯＬｔｈｄは、下記式（１３）で示すように、過負荷閾値ＯＬｔｈより小さく設定するものとする。

０ ＜ ＯＬｔｈｄ ＜ ＯＬｔｈ （１３）
ステップ１３−６では、異常表示として「過負荷検出」を追加して表示し、ステップ１３−７へ進む。

ステップ１３−７では、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っているか否かを判定する。ステップ１３−７において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていると「Ｙ」と判定し、ステップ１３−８に進む。ステップ１３−７において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていないと「Ｎ」と判定し、第１の複数異常表示処理を終了する。

ここで、位置偏差オーバー検出表示可否閾値θｔｈｄは、下記式（１４）で示すように、位置偏差オーバー検出閾値θｔｈより小さく設定するものとする。

０ ＜ θｔｈｄ ＜ θｔｈ （１４）
ステップ１３−８では、異常表示として「位置偏差オーバー検出」を追加して表示し、第１の複数異常表示処理を終了する。

このように、衝突検出時に、衝突検出とは異なる複数の異常表示をさせることにより、衝突誤検出発生時の状況を把握するために有用な情報を得ることができる。

例えば、実際に衝突していないのにロボットが異常停止し、その時の異常表示が「衝突検出」の他に「モータロック検出」が表示されているとする。この場合、モータブレーキや減速機の異常が原因でモータが殆ど動作しない現象であると推定できる。また、「衝突検出」の他に「過負荷検出」が表示されていれば、モータベアリングや減速機の異常が原因で摩擦が大きい状態で動作していたと推定できる。また、「衝突検出」の他に「位置偏差オーバー検出」が表示されていれば、モータ位置検出器の異常が発生したと推定できる。

（実施の形態２）
本実施の形態２では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１４のフローチャートに示す第２の複数異常表示処理を実行する。

図１４に示す第２の複数異常表示処理では、複数の異常検出項目には優先順位が設定されている。そして、優先順位が高いものから順に、ロボットの動作時に検出した値（例えば、モータトルクτｍ、モータ速度ωｍ、モータ電流Ｉｍ、モータ位置θｍ）に基づく異常判定値（モータロック検出値ＬＫｄｅｔ、過負荷検出値ＯＬｄｅｔ、位置偏差オーバー検出値θｄｅｔ）が表示可否閾値を超えたか否かを判定する。そして、表示可否閾値を超えた異常検出項目のみ異常表示を行う。そして、異常表示を行った異常検出項目より優先順位が低い異常検出項目については、異常判定値が表示可否閾値を超えたか否かの判定および異常表示を行わない（ステップ１４−３、ステップ１４−５、ステップ１４−７のいずれかへ移行することで処理を終了させる）。

図１４では、複数の異常検出項目は、優先順位の高い方から順に「モータロック検出」、「過負荷検出」、「位置偏差オーバー検出」としている。

図１４において、ステップ１４−１では、ロボットの衝突検出が発生したか否かを判定する。ステップ１４−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１４−２へ進む。ステップ１４−１において、ロボットの衝突検出が発生しないと「Ｎ」と判定し、その後は何も実施せず、第２の複数異常表示処理を終了する。

ステップ１４−２では、一番優先順位の高いモータロック検出について、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っているか否かを判定する。ステップ１４−２において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていると「Ｙ」と判定し、ステップ１４−３に進む。ステップ１４−２において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１４−４に進む。

ステップ１４−３では、「衝突検出」と「モータロック検出」を異常表示し、第２の複数異常表示処理を終了する。

ステップ１４−４では、次に優先順位の高い過負荷検出について、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っているか否かを判定する。ステップ１４−４において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていると「Ｙ」と判定し、ステップ１４−５に進む。ステップ１４−４において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１４−６に進む。

ステップ１４−５では、「衝突検出」と「過負荷検出」を異常表示し、第２の複数異常表示処理を終了する。

ステップ１４−６では、一番優先順位が低い位置偏差オーバー検出について、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っているか否かを判定する。ステップ１４−６において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていると「Ｙ」と判定し、ステップ１４−７に進む。ステップ１４−６において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１４−８に進む。

ステップ１４−７では、「衝突検出」と「位置偏差オーバー検出」を異常表示し、第２の複数異常表示処理を終了する。

ステップ１４−８では、「衝突検出」のみ異常表示し、第２の複数異常表示処理を終了する。

（実施の形態３）
本実施の形態３では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１５のフローチャートに示す第３の複数異常表示処理を実行する。

図１５の第３の複数異常表示処理では、実施の形態１における図１３の第１の複数異常表示処理に対し、設定された各異常検出項目についての衝突検出閾値に基づいて、各異常検出項目についての表示可否閾値を決定する処理を加えている。

図１５において、ステップ１５−１で、ロボットの衝突検出が発生したか否かを判定する。ステップ１５−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１５−２に進み「衝突検出」の異常表示を行い、さらにステップ１５−Ａへ進む。ステップ１５−１において、ロボットの衝突検出が発生しないと「Ｎ」と判定し、その後は何も実施せず、第３の複数異常表示処理を終了する。

ステップ１５−Ａは、設定された衝突検出閾値に基づいて各異常検出項目についての表示可否閾値を決定するために加えた処理である。ステップ１５−Ａでは、衝突検出閾値τｔｈに基づいて、下記式（１５−１）〜（１５−３）に示すように、モータロック検出表示可否閾値ＬＫｔｈｄと、過負荷検出表示可否閾値ＯＬｔｈｄと、位置偏差オーバー検出表示可否閾値θｔｈｄを算出する。

ＬＫｔｈｄ ＝ ｋｔｈＡ１×τｔｈ ＋ ｋｔｈＢ１ （１５−１）
ＯＬｔｈｄ ＝ ｋｔｈＡ２×τｔｈ ＋ ｋｔｈＢ２ （１５−２）
θｔｈｄ ＝ ｋｔｈＡ３×τｔｈ ＋ ｋｔｈＢ３ （１５−３）
ただし、ｋｔｈＡ１とｋｔｈＡ２とｋｔｈＡ３とは衝突検出閾値（τｔｈ）比例係数である。また、ｋｔｈＢ１とｋｔｈＢ２とｋｔｈＢ３とは加算係数である。

上記のように衝突検出閾値τｔｈに基づいて異常検出表示可否閾値を求める理由は、衝突検出閾値τｔｈが大きい（衝突検出感度が低い）時は、衝突誤検出の可能性が低下するからである。衝突誤検出をしなければ、他の異常検出表示はかえって衝突検出発生時の状況判断を複雑にする恐れが有る。すなわち、衝突検出閾値τｔｈが高くなるほど、異常検出表示可否閾値も高くし、複数異常表示をさせないようにする。

そして、ステップ１５−Ａで算出したモータロック検出表示可否閾値ＬＫｔｈｄと、過負荷検出表示可否閾値ＯＬｔｈｄと、位置偏差オーバー検出表示可否閾値θｔｈｄを、ステップ１５−３、ステップ１５−５、ステップ１５−７の判定に用いる。

なお、ステップ１５−３以後の処理は、実施の形態１における図１３のステップ１３−３以後の処理と同様である。

（実施の形態４）
本実施の形態４では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１６のフローチャートに示す第４の複数異常表示処理を実行する。

図１６の第４の複数異常表示処理では、実施の形態２における図１４の第２の複数異常表示処理に対し、設定された衝突検出閾値に基づいて各異常検出項目についての表示可否閾値を決定する処理（ステップ１６−Ａ）を加えている。

なお、ステップ１６−Ａの処理は、実施の形態３における図１５のステップ１５−Ａの処理と同様である。ステップ１６−３以後の処理は、実施の形態２における図１４のステップ１４−３以後の処理と同様である。

（実施の形態５）
本実施の形態５では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１７のフローチャートに示す第５の複数異常表示処理を実行する。

図１７の第５の複数異常表示処理では、各異常検出項目について、異常判定閾値に対するロボットの動作時に検出した値（例えば、モータトルクτｍ、モータ速度ωｍ、モータ電流Ｉｍ、モータ位置θｍ）に基づく異常判定値（モータロック検出値ＬＫｄｅｔ、過負荷検出値ＯＬｄｅｔ、位置偏差オーバー検出値θｄｅｔ）の割合を求める。異常判定値の割合が最も高い異常検出項目のみを衝突検出とともに異常表示を行う処理を有している。

図１７において、ステップ１７−１で、ロボットの衝突検出が発生したか否かを判定している。ステップ１７−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１７−２へ進む。ステップ１７−１において、ロボットの衝突検出が発生しないと「Ｎ」と判定し、第５の複数異常表示処理を終了する。

ステップ１７−２では、異常判定閾値に対する異常判定値の割合を計算する。ステップ１７−２では、モータロック検出異常判定値割合ＬＫｒａｔと、過負荷検出異常判定値割合ＯＬｒａｔと、位置偏差オーバー検出異常判定値割合θｒａｔを、下記式（１６−１）〜（１６−３）で算出する。

ＬＫｒａｔ ＝ ＬＫｄｅｔ ÷ ＬＫｔｈ （１６−１）
ＯＬｒａｔ ＝ ＯＬｄｅｔ ÷ ＯＬｔｈ （１６−２）
θｒａｔ ＝ θｄｅｔ ÷ θｔｈ （１６−３）
なお、ＬＫｄｅｔはモータロック検出値であり、ＬＫｔｈはモータロック検出閾値である。また、ＯＬｄｅｔは過負荷検出値であり、ＯＬｔｈは過負荷検出閾値である。また、θｄｅｔは位置偏差オーバー検出値であり、θｔｈは位置偏差オーバー検出閾値である。

ステップ１７−３では、ステップ１７−２で求めた異常判定値割合の内、モータロック検出異常判定値割合ＬＫｒａｔが最大かどうかを判定する。ステップ１７−３において、モータロック検出異常判定値割合ＬＫｒａｔが最大であると「Ｙ」と判定し、ステップ１７−４へ進む。ステップ１７−３において、モータロック検出異常判定値割合ＬＫｒａｔが最大でないと「Ｎ」と判定し、ステップ１７−５へ進む。

ステップ１７−４では、「衝突検出」と「モータロック検出」を異常表示し、第５の複数異常表示処理を終了する。

ステップ１７−５では、ステップ１７−２で求めた異常判定値割合の内、過負荷検出異常判定値割合ＯＬｒａｔが最大かどうかを判定する。ステップ１７−５において、過負荷検出異常判定値割合ＯＬｒａｔが最大であると「Ｙ」と判定し、ステップ１７−６へ進む。ステップ１７−５において、過負荷検出異常判定値割合ＯＬｒａｔが最大でないと「Ｎ」と判定し、ステップ１７−７へ進む。

ステップ１７−６では、「衝突検出」と「過負荷検出」を異常表示し、第５の複数異常表示処理を終了する。

ステップ１７−７では、「衝突検出」と「位置偏差オーバー検出」を異常表示し、第５の複数異常表示処理を終了する。

異常判定値割合が最大の項目のみ表示する理由は、実際の衝突を検出しなければ、異常判定値割合が最大の項目における異常発生の可能性が一番高いからである。

（実施の形態６）
本実施の形態６では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１８のフローチャートに示す第６の複数異常表示処理を実行する。

図１８において、ステップ１８−１で、ロボットの衝突検出が発生したか否かを判定している。ステップ１８−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１８−２に進み「衝突検出」の異常表示を行い、さらにステップ１８−３へ進む。ステップ１８−１において、ロボットの衝突検出が発生しないと「Ｎ」と判定し、その後は何も実施せず、第６の複数異常表示処理を終了する。

ステップ１８−３では、異常判定閾値に対する異常判定値割合を計算する。なお、モータロック検出異常判定値割合ＬＫｒａｔと、過負荷検出異常判定値割合ＯＬｒａｔと、位置偏差オーバー検出異常判定値割合θｒａｔは、実施の形態５で説明した既出の式（１６−１）〜（１６−３）により算出する。その後ステップ１８−４へ進む。

ステップ１８−４では、異常表示に「モータロック検出割合 ＬＫｒａｔ」、「過負荷検出割合 ＯＬｒａｔ」、「位置偏差オーバー検出割合 θｒａｔ」を追加して表示し、第６の複数異常表示処理を終了する。なお、各異常判定値割合の表示は、実際の計算数字である。

衝突検出発生時に、他の異常判定値割合を表示することで、衝突を検出しなければ、どの項目の異常発生の可能性がどの程度高いかを知ることができる。

（実施の形態７）
本実施の形態７では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１９のフローチャートに示す第７の複数異常表示処理を実行する。

図１９の第７の複数異常表示処理では、実施の形態１における図１３の第１の複数異常表示処理に対し、各異常検出項目について異常判定閾値に対する異常判定値の割合を求める処理を加えたものである。ロボットの衝突を検出した場合に、衝突検出したことを表示することと併せて各異常検出項目を表示する処理を、異常判定閾値に対する異常判定値の割合を表示する処理に変更している。

図１９において、ステップ１９−１で、ロボットの衝突検出が発生したか否かを判定している。ステップ１９−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１９−２に進み「衝突検出」の異常表示を行い、さらにステップ１９−Ａへ進む。ステップ１９−１において、ロボットの衝突検出が発生しないと「Ｎ」と判定し、その後は何も実施せず、第６の複数異常表示処理を終了する。

ステップ１９−Ａでは、異常判定閾値に対する異常判定値割合を計算する。なお、モータロック検出異常判定値割合ＬＫｒａｔと、過負荷検出異常判定値割合ＯＬｒａｔと、位置偏差オーバー検出異常判定値割合θｒａｔは、実施の形態５で説明した既出の式（１６−１）〜（１６−３）により算出する。

ステップ１９−３では、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っているか否かを判定する。ステップ１９−３において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていると「Ｙ」と判定し、ステップ１９−４に進む。ステップ１９−３において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１９−５に進む。

ステップ１９−４では、異常表示に「モータロック検出割合 ＬＫｒａｔ」を追加して表示し、ステップ１９−５へ進む。なお、モータロック検出割合ＬＫｒａｔの表示は、実際の計算数字である。

ステップ１９−５では、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っているか否かを判定する。ステップ１９−５において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていると「Ｙ」と判定し、ステップ１９−６に進む。ステップ１９−５において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１９−７に進む。

ステップ１９−６では、異常表示に「過負荷検出割合 ＯＬｒａｔ」を追加して表示し、ステップ１９−７へ進む。なお、過負荷検出割合ＯＬｒａｔの表示は、実際の計算数字である。

ステップ１９−７では、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っているか否かを判定する。ステップ１９−７において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていると「Ｙ」と判定し、ステップ１９−８に進む。ステップ１９−７において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていないと「Ｎ」と判定し、第１の複数異常表示処理を終了する。

ステップ１９−８では、異常表示に「位置偏差オーバー検出割合 θｒａｔ」を追加して表示し、第６の複数異常表示処理を終了する。なお、位置偏差オーバー検出割合θｒａｔの表示は、実際の計算数字である。

衝突検出発生時に、検出表示可否閾値を越えた異常のみ異常判定値割合を表示することで、衝突を検出しなければ、どの項目の異常発生の可能性がどの程度高いかを絞り込んで表示することができる。

本発明のロボットの異常表示方法は、ロボットの衝突を検出した場合、異常表示として衝突検出したことを表示するとともに、衝突検出とは異なる複数の異常検出項目のうち少なくとも１つ以上の異常表示を選択して行う処理を備えているので、衝突誤検出発生時の状況を把握するのに有用な情報を得ることができ、産業上有用である。

１ 位置指令の速度成分 ｄθｃｏｍ
２ モータ速度 ωｍ
３ 位置指令 θｃｏｍ
４ モータ位置 θｍ
５ 位置比例ゲイン ＫＰＰ
６ 位置制御ブロック
７ 速度ループ指令 ωｃｏｍ
８ 速度比例ゲイン ＫＰ
９ 速度積分ゲイン ＫＩ／ｓ
１０ 速度制御ブロック
１１ モータ電流 Ｉｍ
１２ モータトルク定数 Ｋｔ
１３ モータ発生トルク τｍ
１４ 動力学トルク（重力トルク、慣性力、遠心力、コリオリ力の和） τｄｙｎ
１５ 動摩擦トルク τμ
１６ 衝突トルク τｄｉｓ
１７ モータ伝達関数ブロック
１８ モータ及び外力を示すブロック
１９ モータトルク定数 Ｋｔ
２０ 粘性摩擦係数 Ｄ
２１ モータ回転方向判定ブロック
２２ モータ方向信号 ｓｇｎ
２３ 動摩擦係数 Ｋμ
２４ 動摩擦トルク推定値 τμｏ
２５ 他軸モータ位置
２６ 動力学トルク演算ブロック
２７ 他軸動力学トルク推定値
２８ 衝突トルク推定値 τｄｉｓｏ
２９ 動力学トルク推定値 τｄｙｎｏ
３０ 衝突トルク推定ブロック
３１ 衝突判定ブロック
３２ 微分要素
４０ 積分要素
８６ モータ位置の誤差 ｅθｍ
８７ モータ速度の誤差 ｅωｍ
８８ モータ加速度の誤差 ｅαｍ
８９ モータイナーシャ Ｊｍ
９０ モータ加速度 αｍ
９１ モータトルク閾値 τｍｔｈ
９２ モータ速度閾値 ωｍｔｈ
９３ モータロック検出時間閾値 ＬＫｔｈ
９５ モータ速度に到達するまでの正常な所用時間 ＬＫ０
９９ モータ位置検出器

本発明は、減速機を介してモータにより駆動されるロボットの異常表示方法に関する。

近年、ロボット市場がグローバルに拡大するにつれ、ロボットが故障した場合や異常が発生した場合の対応が大きな課題となっている。すなわち、ロボットの異常表示が、実際の故障状態を把握できない状態であると、異常発生現場へサービスマンが直接行って状況を確認し、その後に対策を実施する必要がある。この方法では、全世界への異常状態解消の迅速な対応が困難となる。

一方で、ロボット自体の故障ではなく、ユーザの教示ミス等が原因で発生する、ロボットのロボット周辺物との衝突への対応も必要である。ロボットや周辺物が衝突により破壊すると、修理等のためサービスマンの派遣が必要となり、復旧に時間を要する。すなわち、衝突時の破壊防止のため、衝突検出の高精度化が求められている。しかし、高精度の衝突センサをロボットに搭載すると、ロボットにとっては余分な重量負荷となり、かつ、コストも上昇する。そこで、近年では、センサレス衝突検出の高精度化が進展している。

しかしながら、センサレス衝突検出の高精度化により、実際の衝突ではない場合も、ロボットの異常状態を誤って衝突と検出する可能性が高くなる。すなわち、ロボットの故障発生時に、他の異常検出機能よりも先に衝突検出機能が異常を検出し、衝突が発生していないにもかかわらず「衝突発生」といった誤った表示を行う可能性が有る。このように、センサレス衝突検出の高精度化により、故障を速やかに検出するという利点がある反面、故障原因をわかり難くする原因ともなっている。誤った衝突検出が発生する理由を図１〜図１２を用いて、以下に説明する。

図１は、ロボットの代表例として垂直多関節６軸ロボットを示す概略構成図である。図１に示すように、垂直多関節６軸ロボットは、ロボットメカニズム（１０１）と、ロボット制御装置（１０２）と、操作表示装置（１０３）とを有する。

図２は、異常表示の全体処理を示すフローチャートである。異常表示の全体処理は、ロボット制御装置（１０２）内で行われる。図２では、衝突検出処理以外にも、ロボット故障発生時の異常を検出するモータロック検出処理や、過負荷検出処理や、位置偏差オーバー検出処理を加えている。図２に示される異常表示の全体処理は、周期ΔＴ毎に繰り返され、周期ΔＴは数ミリ秒を想定している。

図２において、ステップ２−１では、モータロック検出処理を行う。ステップ２−６では、モータロックが検出されると、図１の操作表示装置（１０３）に異常表示として「モータロック検出」を表示する。モータロックが検出された場合、既に異常状態が発生し異常表示もされているので、ステップ２−６で異常表示処理を終了させる。ステップ２−６でモータロックが検出されなかった場合、ステップ２−２に進む。

次に、ステップ２−２では、過負荷検出処理を行う。ステップ２−７では、過負荷が検出されると、図１の操作表示装置（１０３）に異常表示として「過負荷検出」を表示する。過負荷が検出された場合は、既に異常状態が発生し異常表示もされているので、ステップ２−７で異常表示処理を終了させる。ステップ２−７で過負荷が検出されなかった場合、ステップ２−３に進む。

次に、ステップ２−３では、位置偏差オーバー検出処理を行う。ステップ２−８では、位置偏差オーバーが検出されると、図１の操作表示装置（１０３）に異常表示として「位置偏差オーバー検出」を表示する。位置偏差オーバーが検出された場合は、既に異常状態が発生し異常表示もされているので、ステップ２−８で異常表示処理を終了させる。ステップ２−８で位置偏差オーバーが検出されなかった場合、ステップ２−４に進む。

次に、ステップ２−４では、図３を用いて下記で説明する衝突検出処理を実行する。

衝突力をセンサレスで求める方法としては、動力学演算方式と外乱推定オブザーバ方式がある。動力学演算方式は、まず、モータの駆動電流で発生したトルクからモータ及び減速機のイナーシャと摩擦で損失するトルクを差し引いて減速機出力トルクを求める。次に、減速機出力トルクから、ロボットの動力学演算で求めたロボットの動力学トルクを差し引いて衝突力を求める（例えば、非特許文献１参照）。外乱推定オブザーバ方式は、外乱推定オブザーバを用いて衝突力を求める（例えば、特許文献１参照）。以下、図３を用いて、推定精度に優れる動力学演算方式を例に説明する。

図３は、動力学演算方式の制御ブロック線図である。図３において、６は位置制御ブロックである。位置制御ブロック（６）には、位置指令の速度成分ｄθｃｏｍ（１）を積分要素４０によって積分した位置指令θｃｏｍ（３）と、モータ位置θｍ（４）とが入力される。そして、位置指令θｃｏｍ（３）とモータ位置θｍ（４）との差分値に基づいて、位置比例ゲイン（５）から速度ループ指令ωｃｏｍ（７）を出力する。

図３において、１０は速度制御ブロックである。速度制御ブロック（１０）には、速度ループ指令ωｃｏｍ（７）と、モータ位置θｍ（４）とが入力される。そして、速度ループ指令ωｃｏｍ（７）と、モータ位置θｍ（４）を微分要素（３２）によって微分したモータ速度ωｍ（２）との差分値を求め、速度比例ゲイン（８）および速度積分ゲイン（９）を介して、モータ電流Ｉｍ（１１）を出力する。

図３において、１８はモータと外力を示したブロックである。τｍ（１３）はモータ発生トルクであり、減速機が剛体であると仮定すると、モータ駆動側から見れば式（１）で表され、負荷側から見れば式（２）で表される。

τｍ＝Ｋｔ×Ｉｍ （１）
τｍ＝τｄｙｎ＋τｄｉｓ＋Ｊｍ×αｍ＋Ｄ×ωｍ＋τμ （２）
ただし、式（１）と式（２）における記号は以下の通りである。

Ｋｔ（１２）：モータトルク定数
Ｉｍ（１１）：モータ電流
αｍ（９０）：モータ加速度（ωｍの微分値）
ωｍ（２）：モータ速度
Ｊｍ（８９）：モータイナーシャ（ロータ＋減速機１次側）
Ｄ（２０）：粘性摩擦係数
τμ（１５）：動摩擦トルク
τｄｙｎ（１４）：動力学トルク（重力トルク、慣性力、遠心力、コリオリ力の和）
τｄｉｓ（１６）：衝突トルク
動摩擦トルクτμ（１５）は以下の式（３）で計算できる。

τμ＝Ｋμ×ｓｇｎ （３）
ただし、Ｋμは動摩擦の大きさであり、ｓｇｎは、ωｍ＞０の時は「１」、ωｍ＝０の時は「０」、ωｍ＜０の時は「−１」である。

衝突トルクτｄｉｓ（１６）は、式（１）と式（２）より、以下の式（４）に変形して求めることが出来る。

τｄｉｓ＝Ｋｔ×Ｉｍ−（Ｊｍ×αｍ＋Ｄ×ωｍ＋τμ＋τｄｙｎ） （４）
図３において、３０は、式（４）を衝突トルク推定ブロックとして表したものである。

衝突トルク推定ブロック（３０）において、動力学トルク推定値τｄｙｎｏ（２９）は、動力学トルク演算ブロック（２６）が動力学演算することで求められる。動力学トルク演算ブロック（２６）は、ロボットを構成する全軸のモータ位置θｍ（４）と、モータ位置θｍ（４）を微分したモータ速度ωｍ（２）と、モータ速度ωｍ（２）を微分したモータ加速度αｍ（９０）と、ロボットの機械パラメータ（アーム長、アーム質量、重心位置、重心位置周りのイナーシャ）を用いて、動力学演算を実行する。この動力学トルク推定値τｄｙｎｏ（２９）を用いて、衝突トルク推定ブロック（３０）は、衝突トルク推定値τｄｉｓｏ（２８）を衝突判定ブロック（３１）へ出力する。

衝突判定ブロック（３１）は、所定の衝突検出閾値τｔｈを用いて、以下の式（５）に従い衝突を検出する。

｜τｄｉｓｏ｜ ＞ τｔｈ （５）
ステップ２−５では、式（５）が成立すれば、図１のロボットの操作表示装置（１０３）に異常表示として「衝突検出」を表示し、異常表示処理を終了させる。また、ステップ２−５では、式（５）が成立していなければ、異常表示をせずに異常表示処理を終了させる。そして、また、周期ΔＴごとに異常表示の全体処理を開始する。

以上の衝突検出では、前述のように、動力学トルク推定値τｄｙｎｏ（２９）が正確に計算できれば、精度良く衝突トルク推定値τｄｉｓｏ（２８）を求めることができる。

動力学トルク演算ブロック（２６）で動力学トルクを求める方法としては、ラグランジュ法とニュートンオイラー法とがある。ラグランジュ法では、６軸の垂直多関節ロボットでは、１０万回弱の積和演算が必要である。しかし、この演算量は、現在のＣＰＵの処理能力からすれば、数ミリ秒以内で演算することも可能である。ニュートンオイラー法では、積和演算量が１万回弱で済むので、演算時間は問題無い。ニュートンオイラー法は、前式の結果を次式に用いる、いわゆる漸化式方式であるので、演算の累積誤差が発生する恐れがある。しかし、３２ｂｉｔ以上のデータで処理すれば演算の累積誤差も問題無い。よって、どちらの方法を用いたとしても、現在のＣＰＵでは、動力学トルクτｄｙｎ（１４）をモータ最大トルク比１０％以内の誤差で計算可能である。式（４）の右辺で動力学トルクτｄｙｎ（１４）以外の項も、モータ最大トルク比１０％以内の誤差で計算可能であれば、衝突トルク推定値τｄｉｓｏ（２８）は、モータ最大トルク比２０％以内の誤差で求めることができる。

また、式（５）による衝突検出では、式（４）で求めた衝突トルク推定値τｄｉｓｏ（２８）の瞬時値を用いている。そのため、計算時間である数ミリ秒経てば、動力学トルクτｄｙｎ（１４）の判定が可能である。すなわち、衝突検出機能では、モータ最大トルク比２０％程度の異常が数ミリ秒観測されれば、衝突を検出することが可能である。

しかし、衝突検出閾値τｔｈを低く（モータ最大トルク比２０％程度に）設定すれば、衝突以外の故障でもいち早く検出してしまう。例えば、モータ（ベアリング、ブレーキ）や減速機の故障により、実際の動摩擦トルクが増加した場合について、検討する。この場合、正常時と同じようにロボットを動作させようとすると、動摩擦トルクτμの増大分だけモータ発生トルクτｍを増大させる必要がある。モータ発生トルクτｍを増加させるため、式（４）の右辺のモータ電流Ｉｍが増加する。しかし、式（４）の右辺で用いる動摩擦トルクτμは、予め正常時に測定した値を用いて計算するため、式（４）の左辺である衝突トルクτｄｉｓは、衝突していないにも関わらず増大する。そして、衝突トルクτｄｉｓの大きさがモータ最大トルク比２０％を数ミリ秒でも越えると、衝突検出として誤検出してしまう。

衝突力をセンサレスで求める方法では、式（４）の右辺のモータ電流Ｉｍが増加し、衝突トルクτｄｉｓが増大することの原因が特定できない。すなわち、ロボットの実衝突に起因するのか、モータや減速機の故障による摩擦トルク増大に起因するのかが、区別ができない。そのため、ロボット衝突時の損傷を少しでも少なくするために、衝突検出閾値τｔｈを低く（衝突検出感度を高く）設定すると、衝突誤検出の発生確率も高めてしまう。

一方、衝突検出機能とは別に、モータ（ベアリング、ブレーキ）や減速機の故障によりモータの回転が重くなったことを検出する機能としては、モータロック検出機能や過負荷検出機能がある。

モータロック検出機能では、モータを動作させようとしているのに殆ど動かない状態が継続した状況を検出する。モータ発生トルクτｍ（式（１）で示す）が、所定のモータトルク閾値τｍｔｈ（９１）以上になっても、故障により増大した摩擦トルクが大きいと、モータ速度ωｍ（２）が所定のモータ速度閾値ωｍｔｈ以上に上がらない。この状態が、所定のモータロック検出時間閾値ＬＫｔｈ以上に継続した場合に、モータロック検出機能が異常と判定する。

モータロック検出機能について、図５Ａおよび図５Ｂを用いて説明する。図５Ａは、正常時のモータ速度ωｍ（２）とモータ発生トルクτｍ（１３）とを示すグラフである。モータ発生トルクτｍ（１３）がモータトルク閾値τｍｔｈ（９１）を越えてから、モータロック検出時間閾値ＬＫｔｈ（９３）より小さい時間ＬＫ０（９５）が経過した後は、モータ速度ωｍ（２）がモータ速度閾値ωｍｔｈ（９２）を越えている。この場合は、モータロックは検出されない。

図５Ｂは、モータや減速機の故障による摩擦トルクの増大でモータロックが検出されるという異常時のモータ速度ωｍ（２）とモータ発生トルクτｍ（１３）とを示すグラフである。図５Ｂでは、モータ発生トルクτｍ（１３）がモータトルク閾値τｍｔｈ（９１）を越えてからも、モータや減速機の故障による摩擦トルクの増大のせいで、正常時（点線で示す）に比べモータ速度ωｍ（２）の増加が遅くなる。そのため、モータ速度ωｍ（２）は、モータロック検出時間閾値ＬＫｔｈ（９３）になっても、モータ速度閾値ωｍｔｈ（９２）に達しない。この場合、モータロックが検出される。

図４は、上記モータロック検出のフローチャートである。モータロック検出処理は一定周期ΔＴ毎に実行される。

ステップ４−１では、モータ発生トルクτｍがモータトルク閾値τｍｔｈ（９１）以上になっているか否かを判定する。モータ発生トルクτｍがモータトルク閾値τｍｔｈ（９１）以上になっていると、「Ｙ」と判定し、ステップ４−２へ進む。

ステップ４−２では、モータ速度ωｍ（２）がモータ速度閾値ωｍｔｈ（９２）以下であるか否かを判定する。モータ速度ωｍ（２）がモータ速度閾値ωｍｔｈ（９２）以下であると、「Ｙ」と判定し、ステップ４−３へ進む。

ステップ４−３では、式（６）によって、モータロック継続時間ＬＫｄｅｔを算出し、ステップ４−５に進む。

ＬＫｄｅｔ＝ＬＫｄｅｔ＋ΔＴ （６）
ここで、ΔＴは、モータロック検出処理周期である。

すなわち、ステップ４−３では、ステップ４−１およびステップ４−２がともに「Ｙ」と判定されている時間の長さを、モータロック検出処理周期ΔＴずつ加算して測定するものである。

なお、ステップ４−１あるいはステップ４−２の判定が「Ｎ」なら、ステップ４−４に進み、モータロック継続時間ＬＫｄｅｔを０にリセットし、モータロック検出の処理を終える。

ステップ４−５では、モータロック継続時間ＬＫｄｅｔがモータロック検出時間閾値ＬＫｔｈ（９３）以上になったか否かを、以下の式（７）に従い、判定する。

ＬＫｄｅｔ≧ＬＫｔｈ （７）
モータロック継続時間ＬＫｄｅｔがモータロック検出時間閾値ＬＫｔｈ（９３）以上であると、「Ｙ」と判定し、ステップ４−６へ進む。この場合、モータロック状態であると判定され、ステップ４−６でモータを停止する。続いて、ステップ４−７で「モータロック検出」の異常表示を行い、モータロック検出の処理を終了する。なお、異常表示は、図１に示す操作表示装置（１０３）に表示する。

ステップ４−５の判定が「Ｎ」なら、モータロック状態では無いと判定し、モータロック検出の処理を終了する。

このモータロック検出機能では、正常時は、モータトルク閾値τｍｔｈ（９１）以上のモータトルクで、モータ速度閾値ωｍｔｈ（９２）以上のモータ速度に到達するまでには、所要時間ＬＫ０（９５）を要する。そのためモータロック検出時間閾値ＬＫｔｈ（９３）は、正常時の所要時間ＬＫ０（９５）よりも大きくする必要が有る。ロボットの大きさにより異なるが、モータロック検出時間閾値ＬＫｔｈ（９３）は、数百ミリ秒から数秒であり、衝突検出機能が数ミリ秒内で検出することに比べれば遅い。すなわち、衝突検出閾値τｔｈを低く（衝突検出感度を高く）設定すると、実際に衝突していないにも関わらず、モータロックが検出される前に衝突が先に検出されてしまい、異常表示も「衝突検出」とのみ表示されることになる。図２において説明すると、ステップ２−６によるモータロック検出では「Ｎ」と判定されて続けている間に、ステップ２−５による衝突検出では「Ｙ」と判定される。

次に、過負荷検出機能について説明する。

過負荷検出機能では、モータを駆動させる電流の時間積算値が、モータ時限特性曲線の限界値を超えない様に、過負荷異常を検出する。モータ発生トルクはモータを駆動させる電流に比例し、モータ時限特性曲線の限界値とは、モータの巻線温度の上限を示す。図７は、モータを駆動させるモータ電流と時間の関係を示すグラフである。例えば、図７に示すモータ時限曲線の限界値（点線）は、最大電流比４０％の時は約１００秒、最大電流比５０％の時は２５秒、最大電流比７０％の時は約８秒である。この限界値以下になる様に過負荷検出を行う。

図６は、過負荷検出のフローチャートである。過負荷検出処理は一定周期ΔＴ毎に実行される。

ステップ６−１で、過負荷検出値ＯＬｄｅｔを以下の式（８）で求める。

ＯＬｄｅｔ＝ＯＬｄｅｔ＋（｜Ｉｍ｜−Ｉｍｔｈ）×ΔＴ （８）
ここで、Ｉｍｔｈは過負荷検出電流閾値であり、ΔＴは過負荷検出周期である。

次に、ステップ６−２では、過負荷検出値ＯＬｄｅｔが過負荷閾値ＯＬｔｈ以上であるか否かを判定する。すなわち、下記式（９）の条件が成立するかどうかを判定する。

ＯＬｄｅｔ ≧ ＯＬｔｈ （９）
式（９）が成立すると、「Ｙ」と判定し、過負荷状態であると判定され、ステップ６−３へ進む。ステップ６−３では、モータを停止し、続いて、ステップ６−４で「過負荷検出」であることを示す「異常表示」を行い、過負荷検出の処理を終了する。なお、「異常表示」は、図１に示す操作表示装置（１０３）に表示する。

ステップ６−２の判定が「Ｎ」なら、過負荷状態では無いとの判定し、過負荷検出の処理を終了する。

図８に、式（９）でＯＬｄｅｔ＝ＯＬｔｈとなる過負荷検出閾値特性を実線で示す。図８の過負荷検出閾値特性（実線）は、Ｉｍｔｈ＝３３、ＯＬｔｈ＝１００の場合を示している。過負荷検出閾値特性（実線）は限界値（点線）を下回る様に設定されており、式（８）で示す過負荷検出値ＯＬｄｅｔの演算方法が妥当である事を示している。

図８に示すように、モータ電流が最大電流比１００％でも過負荷検出には数秒かかることがわかる。この過負荷検出にかかる時間は、衝突検出機能が数ミリ秒内で検出することに比べれば遅い。つまり、過負荷異常が発生していない状態からモータや減速機の故障が発生すると、モータ回転が重くなり、モータ電流（モータ発生トルク）が最大電流比で２０％程度増加する。このようにモータや減速器の故障が発生しても、過負荷異常が検出されるには最低でも数秒以上かかり、先に衝突誤検出が発生する可能性が極めて高い。図２において説明すると、ステップ２−７による過負荷検出では「Ｎ」と判定され続けている間に、ステップ２−５による衝突検出では「Ｙ」と判定される。

次に、図３に示す衝突検出処理における誤検出について説明する。

すなわち、モータ位置θｍ（４）を検出するモータ位置検出器（９９）に異常が有った場合でも、衝突検出として誤検出する可能性がある。

動力学トルク演算ブロック（２６）には、モータ位置θｍ（４）を２回微分して生成するモータ加速度αｍ（９０）を用いている。そのため、モータ位置θｍ（４）の変化は、モータ加速度αｍ（９０）に大きく影響する。

一般的に、デジタル制御における微分は、現サンプル値とサンプル１周期前の前サンプル値の差分によって行われることが一般的である。そこで、一例を示す。

図９は、モータ位置検出器に異常が発生した時のモータ位置波形を示すグラフである。図１０はモータ位置検出器に異常が発生した時の速度波形を示すグラフである。図１１はモータ位置検出器に異常が発生した時の加速度波形を示すグラフである。モータ位置θｍ（４）が０の位置での停止状態から、０．５秒でモータ速度ωｍ（２）が５０回転／秒（＝３０００ｒｐｍ）に等加速度（１００回転／秒２）で到達する場合の波形を示している。ここで、サンプル周期は５ミリ秒であり、５００ミリ秒後のモータ位置θｍ（４）は図９に示すように、約１２．５回転である。図９は、３００ミリ秒の時点でモータ位置検出器（９９）に異常が発生し、モータ位置θｍ（４）の読み取りができなかった場合である。この場合、前回サンプル値（２９５ｍ秒時点）でのモータ位置θｍ（４）の値が更新されず、そのままの状態になっている。

５００ミリ秒後の値（１２．５回転）を基準に、異常が発生した３００ミリ秒時のモータ位置θｍ（４）の誤差ｅθｍ（８６）を計算すると、約４．７％（約０．１５回転分）である。図１０において、モータ速度ωｍ（２）の誤差ｅωｍ（８７）も、５００ミリ秒後の値（５０回転／秒）を基準に算出すると、約１２０％になる。さらに図１１において、モータ加速度αｍ（９０）の誤差ｅαｍ（８８）も、５００ミリ秒後の値（１００回転／秒２）を基準に算出すると、約−３３３３％となる。従って、動力学トルク演算ブロック（２６）で求める動力学トルクτｄｙｎ（１４）の精度に大きな影響を与え、衝突誤検出を引き起こす可能性が高い。

衝突力をセンサレスで求める方法において、モータ位置θｍ（４）が変わらないことの原因は特定できない。すなわち、実際に衝突が発生してロボットが停止したことによるのか、モータ位置検出器（９９）に異常が有ってモータ位置θｍ（４）が更新されなかったからなのかは不明である。そのため、衝突検出機能を有効に働かせようとすると、衝突誤検出の発生を免れることはできない。

一方、衝突検出機能とは別に、モータ位置θｍ（４）を検出するモータ位置検出器（９９）に異常が有ったことを検出する機能としては、位置偏差オーバー検出機能がある。

次に、位置偏差オーバー検出機能について説明する。

図１２は、位置偏差オーバー検出のフローチャートである。位置偏差オーバー検出処理は一定周期ΔＴ毎に実行される。

ステップ１２−１では、下記の式（１０）に示すように、位置偏差オーバー検出値θｄｅｔを、位置指令θｃｏｍ（３）とモータ位置θｍ（４）の差分の絶対値として算出する。

θｄｅｔ＝｜θｃｏｍ−θｍ｜ （１０）
ステップ１２−２では、位置偏差オーバー検出値θｄｅｔと位置偏差オーバー検出閾値θｔｈとを比較し、下記の式（１１）の条件を満たすかどうかを判定する。

θｄｅｔ≧θｔｈ （１１）
位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出閾値θｔｈ以上であると、「Ｙ」と判定し、ステップ１２−３へ進む。この場合、位置偏差オーバー状態であると判定され、ステップ１２−３でモータを停止する。続いて、ステップ１２−４で「位置偏差オーバー」の異常表示を行い、位置偏差オーバー検出の処理を終了する。なお、異常表示は、図１に示す操作表示装置（１０３）に表示する。

ステップ１２−２の判定が「Ｎ」なら、位置偏差オーバー状態では無いと判定し、位置偏差オーバー検出の処理を終了する。

この位置偏差オーバー検出において、制御遅れによる位置偏差を誤検出しないように、式（１１）における位置偏差オーバー検出閾値θｔｈは、数回転程度に設定される。

例えば、制御遅れによる位置偏差が１回転であって、位置偏差オーバー検出閾値θｔｈを２回転に設定していたとする。この時、図９で異常が発生した３００ミリ秒時のモータ位置θｍ（４）の誤差（約０．１５回転分）が加算されても、位置偏差オーバー検出値θｄｅｔは１．１５回転程度であり、式（１１）の条件は成立せず、位置偏差オーバーは検出されない。しかし、動力学トルク演算ブロック（２６）で求める動力学トルクτｄｙｎ（１４）の精度には大きな影響を与えるため、誤って衝突検出が検出される。すなわち、実際に衝突していないにも関わらず、衝突が先に検出されてしまい、異常表示も「衝突検出」とのみ表示されることになる。図２において説明すると、ステップ２−８による位置偏差オーバー検出では「Ｎ」と判定されて続けている間に、ステップ２−５による衝突検出では「Ｙ」と判定される。

図２のステップ２−５の衝突検出発生時の異常表示処理として、従来は、図２０で示す単一異常表示処理を実行する。

これまで説明した様に、図２に示すステップ２−１〜２−３における異常検出には時間がかかるため、ステップ２−４における衝突検出が先に検出され、ステップ２−５で「衝突検出」とのみ異常表示される。

特許第３３６７６４１号公報

小菅一弘、他１名、"マニピュレータの動的衝突検出"、日本機械学会［Ｎｏ．９９−９］ロボティクス・メカトロニクス講演会‘９９講演論文集２Ａ１−１１−０３０

従来の衝突力をセンサレスで求める方法では、衝突検出がロボットの実際の衝突によるのか、その他の原因によるものなのかが、区別できない。そして、衝突検出が実際の衝突によるものではない場合も「衝突検出」とのみ表示され、どのような状況で衝突誤検出が発生したのか不明である。

本発明は、上記課題を解決するものであり、衝突誤検出発生時の状況を把握するのに有用な情報を表示することができるロボットの異常表示方法を提供する。

上記課題を解決するために、本発明のロボットの異常表示方法は、ロボットの動作時に検出した値に基づいて少なくとも衝突検出を含む複数の異常を検出し、異常が発生したことを表示するロボットの異常表示方法である。さらに、ロボットの衝突を検出する衝突検出工程を有する。さらに、異常表示として衝突検出したことを表示する衝突検出表示工程を有する。さらに、衝突検出とは異なる複数の異常検出項目のうち少なくとも１つについても異常表示として異常が発生したことを表示する異常表示工程を有する。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、異常表示工程において、複数の異常検出項目の異常表示を選択して行うとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、複数の異常検出項目の各々には、異常と判定するための異常判定閾値と、異常判定閾値より小さくロボットの衝突を検出した場合に衝突検出したことを表示することと併せて異常が発生したことを表示するための表示可否閾値とが設定されている。さらに、複数の異常検出項目のうち、ロボットの動作時に検出した値である異常判定値が表示可否閾値を超えたものは異常表示として異常が発生したことを表示するとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、複数の異常検出項目の各々には、異常と判定するための異常判定閾値と、異常判定閾値より小さくロボットの衝突を検出した場合に衝突検出したことを表示することと併せて異常が発生したことを表示するための表示可否閾値とが設定されているとよい。さらに、複数の異常検出項目には優先順位が設定されているとよい。さらに、優先順位が高いものから順にロボットの動作時に検出した値である異常判定値が表示可否閾値を超えたか否かを判定するとよい。さらに、表示可否閾値を超えた異常検出項目のみ異常表示を行うとよい。さらに、異常表示を行った異常検出項目より優先順位が低い異常検出項目については、異常判定値が表示可否閾値を超えたか否かの判定および異常表示を行わないとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、設定された衝突検出閾値に基づいて各異常検出項目についての表示可否閾値を決定するとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、各異常検出項目について異常判定閾値に対するロボットの動作時に検出した値である異常判定値の割合を求めるとよい。さらに、異常判定値の割合が最も高い異常検出項目のみ衝突検出とともに異常表示を行うとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、複数の異常検出項目の各々には、異常と判定するための異常判定閾値が設定されているとよい。さらに、複数の異常検出項目のそれぞれについて、異常判定閾値に対するロボットの動作時に検出した値である異常判定値の割合を求めるとよい。さらに、ロボットの衝突を検出した場合に衝突検出したことを表示することと併せて各異常検出項目のそれぞれについて、異常判定値の割合を表示するとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、複数の異常検出項目の各々には、異常と判定するための異常判定閾値が設定されているとよい。さらに、複数の異常検出項目のそれぞれについて、異常判定閾値に対するロボットの動作時に検出した値である異常判定値の割合を求めるとよい。さらに、ロボットの衝突を検出した場合に衝突検出したことを表示することと併せて、異常判定値が表示可否閾値を超えたもののみ各異常検出項目の異常判定値の割合を表示するとよい。

また、好ましくは、本発明のロボットの異常表示方法は、上記に加えて、複数の異常検出項目として、ロボットを駆動するモータのロック検出とモータの過負荷検出とロボットの位置偏差検出との少なくとも１つを含むとよい。

以上のように、本発明のロボットの異常表示方法においては、衝突検出発生時、他の異常検出手段の状況も併せて表示することにより、衝突誤検出発生時のロボットの動作状況の把握に役立つものである。

図１は、本発明を適用する代表例としての垂直多関節６軸ロボットの概略構成を示す図である。 図２は、本発明の実施の形態における異常表示全体処理を示すフローチャートである。 図３は、本発明の実施の形態における動力学演算方式の制御ブロック線図である。 図４は、本発明の実施の形態におけるモータロック検出を示すフローチャートである。 図５Ａは、正常時のモータ速度とモータトルクの時間変化を示すグラフである。 図５Ｂは、モータロック検出時のモータ速度とモータトルクの時間変化を示すグラフである。 図６は、本発明の実施の形態における過負荷検出を示すフローチャートである。 図７は、モータ時限特性曲線（モータの巻線温度上昇の限界）を示すグラフである。 図８は、過負荷検出閾値特性を示すグラフである。 図９は、モータ位置検出器異常発生時のモータ位置波形を示す図である。 図１０は、モータ位置検出器異常発生時のモータ速度波形を示す図である。 図１１は、モータ位置検出器異常発生時のモータ加速度波形を示す図である。 図１２は、本発明の実施の形態における位置偏差オーバー検出を示すフローチャートである。 図１３は、本発明の実施の形態１における第１の複数異常表示処理を示すフローチャートである。 図１４は、本発明の実施の形態２における第２の複数異常表示処理を示すフローチャートである。 図１５は、本発明の実施の形態３における第３の複数異常表示処理を示すフローチャートである。 図１６は、本発明の実施の形態４における第４の複数異常表示処理を示すフローチャートである。 図１７は、本発明の実施の形態５における第５の複数異常表示処理を示すフローチャートである。 図１８は、本発明の実施の形態６における第６の複数異常表示処理を示すフローチャートである。 図１９は、本発明の実施の形態７における第７の複数異常表示処理を示すフローチャートである。 図２０は、従来の単一異常表示処理を示すフローチャートである。

（実施の形態１）
以下、本発明の実施の形態１について、図面を用いて説明する。なお、背景技術で説明したものと同様の事項については、同一の符号を付して詳細な説明を省略する。

本実施の形態１では、背景技術でも説明した図２のステップ２−５の衝突検出発生異常表示処理として、図１３のフローチャートに示す第１の複数異常表示処理を実行する。

図１３に示す第１の複数異常表示処理では、ロボットの衝突を検出した場合、異常表示として衝突検出したことを表示する。さらに、衝突検出とは異なる複数の異常検出項目のうち、少なくとも１つ以上の異常表示を選択して行う処理（ステップ１３−３〜ステップ１３−８）を備えている。

さらに、衝突検出とは異なる複数の異常検出ステップ（ステップ１３−３、ステップ１３−５、ステップ１３−７）の各々には、異常判定閾値よりも小さい表示可否閾値が設定されている。

そして、複数の異常検出項目のうち、ロボットの動作時に検出した値（例えば、モータ発生トルクτｍ、モータ速度ωｍ、モータ電流Ｉｍ、モータ位置θｍ）である異常判定値（モータロック検出値ＬＫｄｅｔ、過負荷検出値ＯＬｄｅｔ、位置偏差オーバー検出値θｄｅｔ）が表示可否閾値を超えたものは、異常表示として異常が発生したことを表示する（ステップ１３−４、ステップ１３−６、ステップ１３−８）。

図１３において、ステップ１３−１では、ロボットの衝突検出が発生したか否かを判定する。ステップ１３−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１３−２に進む。ステップ１３−２では、「衝突検出」の異常表示を行い、ステップ１３−３へ進む。ステップ１３−１において、ロボットの衝突検出が発生しないと、「Ｎ」と判定し、その後は何も実施せず、第１の複数異常表示処理を終了する。

ステップ１３−３では、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っているか否かを判定する。ステップ１３−３において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていると「Ｙ」と判定し、ステップ１３−４に進む。ステップ１３−３において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていないと、「Ｎ」と判定し、ステップ１３−５に進む。

ここで、モータロック検出表示可否閾値ＬＫｔｈｄは、下記式（１２）で示すように、モータロック検出時間閾値ＬＫｔｈより小さく設定するものとする。

０ ＜ ＬＫｔｈｄ ＜ ＬＫｔｈ （１２）
ステップ１３−４では、異常表示として「モータロック検出」を追加して表示し、ステップ１３−５へ進む。

ステップ１３−５では、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っているか否かを判定する。ステップ１３−５において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていると「Ｙ」と判定し、ステップ１３−６に進む。ステップ１３−５において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１３−７に進む。

ここで、過負荷検出表示可否閾値ＯＬｔｈｄは、下記式（１３）で示すように、過負荷閾値ＯＬｔｈより小さく設定するものとする。

０ ＜ ＯＬｔｈｄ ＜ ＯＬｔｈ （１３）
ステップ１３−６では、異常表示として「過負荷検出」を追加して表示し、ステップ１３−７へ進む。

ステップ１３−７では、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っているか否かを判定する。ステップ１３−７において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていると「Ｙ」と判定し、ステップ１３−８に進む。ステップ１３−７において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていないと「Ｎ」と判定し、第１の複数異常表示処理を終了する。

ここで、位置偏差オーバー検出表示可否閾値θｔｈｄは、下記式（１４）で示すように、位置偏差オーバー検出閾値θｔｈより小さく設定するものとする。

０ ＜ θｔｈｄ ＜ θｔｈ （１４）
ステップ１３−８では、異常表示として「位置偏差オーバー検出」を追加して表示し、第１の複数異常表示処理を終了する。

このように、衝突検出時に、衝突検出とは異なる複数の異常表示をさせることにより、衝突誤検出発生時の状況を把握するために有用な情報を得ることができる。

例えば、実際に衝突していないのにロボットが異常停止し、その時の異常表示が「衝突検出」の他に「モータロック検出」が表示されているとする。この場合、モータブレーキや減速機の異常が原因でモータが殆ど動作しない現象であると推定できる。また、「衝突検出」の他に「過負荷検出」が表示されていれば、モータベアリングや減速機の異常が原因で摩擦が大きい状態で動作していたと推定できる。また、「衝突検出」の他に「位置偏差オーバー検出」が表示されていれば、モータ位置検出器の異常が発生したと推定できる。

（実施の形態２）
本実施の形態２では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１４のフローチャートに示す第２の複数異常表示処理を実行する。

図１４に示す第２の複数異常表示処理では、複数の異常検出項目には優先順位が設定されている。そして、優先順位が高いものから順に、ロボットの動作時に検出した値（例えば、モータトルクτｍ、モータ速度ωｍ、モータ電流Ｉｍ、モータ位置θｍ）である異常判定値（モータロック検出値ＬＫｄｅｔ、過負荷検出値ＯＬｄｅｔ、位置偏差オーバー検出値θｄｅｔ）が表示可否閾値を超えたか否かを判定する。そして、表示可否閾値を超えた異常検出項目のみ異常表示を行う。そして、異常表示を行った異常検出項目より優先順位が低い異常検出項目については、異常判定値が表示可否閾値を超えたか否かの判定および異常表示を行わない（ステップ１４−３、ステップ１４−５、ステップ１４−７のいずれかへ移行することで処理を終了させる）。

図１４では、複数の異常検出項目は、優先順位の高い方から順に「モータロック検出」、「過負荷検出」、「位置偏差オーバー検出」としている。

図１４において、ステップ１４−１では、ロボットの衝突検出が発生したか否かを判定する。ステップ１４−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１４−２へ進む。ステップ１４−１において、ロボットの衝突検出が発生しないと「Ｎ」と判定し、その後は何も実施せず、第２の複数異常表示処理を終了する。

ステップ１４−２では、一番優先順位の高いモータロック検出について、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っているか否かを判定する。ステップ１４−２において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていると「Ｙ」と判定し、ステップ１４−３に進む。ステップ１４−２において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１４−４に進む。

ステップ１４−３では、「衝突検出」と「モータロック検出」を異常表示し、第２の複数異常表示処理を終了する。

ステップ１４−４では、次に優先順位の高い過負荷検出について、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っているか否かを判定する。ステップ１４−４において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていると「Ｙ」と判定し、ステップ１４−５に進む。ステップ１４−４において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１４−６に進む。

ステップ１４−５では、「衝突検出」と「過負荷検出」を異常表示し、第２の複数異常表示処理を終了する。

ステップ１４−６では、一番優先順位が低い位置偏差オーバー検出について、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っているか否かを判定する。ステップ１４−６において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていると「Ｙ」と判定し、ステップ１４−７に進む。ステップ１４−６において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１４−８に進む。

ステップ１４−７では、「衝突検出」と「位置偏差オーバー検出」を異常表示し、第２の複数異常表示処理を終了する。

ステップ１４−８では、「衝突検出」のみ異常表示し、第２の複数異常表示処理を終了する。

（実施の形態３）
本実施の形態３では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１５のフローチャートに示す第３の複数異常表示処理を実行する。

図１５の第３の複数異常表示処理では、実施の形態１における図１３の第１の複数異常表示処理に対し、設定された各異常検出項目についての衝突検出閾値に基づいて、各異常検出項目についての表示可否閾値を決定する処理を加えている。

図１５において、ステップ１５−１で、ロボットの衝突検出が発生したか否かを判定する。ステップ１５−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１５−２に進み「衝突検出」の異常表示を行い、さらにステップ１５−Ａへ進む。ステップ１５−１において、ロボットの衝突検出が発生しないと「Ｎ」と判定し、その後は何も実施せず、第３の複数異常表示処理を終了する。

ステップ１５−Ａは、設定された衝突検出閾値に基づいて各異常検出項目についての表示可否閾値を決定するために加えた処理である。ステップ１５−Ａでは、衝突検出閾値τｔｈに基づいて、下記式（１５−１）〜（１５−３）に示すように、モータロック検出表示可否閾値ＬＫｔｈｄと、過負荷検出表示可否閾値ＯＬｔｈｄと、位置偏差オーバー検出表示可否閾値θｔｈｄを算出する。

ＬＫｔｈｄ ＝ ｋｔｈＡ１×τｔｈ ＋ ｋｔｈＢ１ （１５−１）
ＯＬｔｈｄ ＝ ｋｔｈＡ２×τｔｈ ＋ ｋｔｈＢ２ （１５−２）
θｔｈｄ ＝ ｋｔｈＡ３×τｔｈ ＋ ｋｔｈＢ３ （１５−３）
ただし、ｋｔｈＡ１とｋｔｈＡ２とｋｔｈＡ３とは衝突検出閾値（τｔｈ）比例係数である。また、ｋｔｈＢ１とｋｔｈＢ２とｋｔｈＢ３とは加算係数である。

上記のように衝突検出閾値τｔｈに基づいて異常検出表示可否閾値を求める理由は、衝突検出閾値τｔｈが大きい（衝突検出感度が低い）時は、衝突誤検出の可能性が低下するからである。衝突誤検出をしなければ、他の異常検出表示はかえって衝突検出発生時の状況判断を複雑にする恐れが有る。すなわち、衝突検出閾値τｔｈが高くなるほど、異常検出表示可否閾値も高くし、複数異常表示をさせないようにする。

そして、ステップ１５−Ａで算出したモータロック検出表示可否閾値ＬＫｔｈｄと、過負荷検出表示可否閾値ＯＬｔｈｄと、位置偏差オーバー検出表示可否閾値θｔｈｄを、ステップ１５−３、ステップ１５−５、ステップ１５−７の判定に用いる。

なお、ステップ１５−３以後の処理は、実施の形態１における図１３のステップ１３−３以後の処理と同様である。

（実施の形態４）
本実施の形態４では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１６のフローチャートに示す第４の複数異常表示処理を実行する。

図１６の第４の複数異常表示処理では、実施の形態２における図１４の第２の複数異常表示処理に対し、設定された衝突検出閾値に基づいて各異常検出項目についての表示可否閾値を決定する処理（ステップ１６−Ａ）を加えている。

なお、ステップ１６−Ａの処理は、実施の形態３における図１５のステップ１５−Ａの処理と同様である。ステップ１６−３以後の処理は、実施の形態２における図１４のステップ１４−３以後の処理と同様である。

（実施の形態５）
本実施の形態５では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１７のフローチャートに示す第５の複数異常表示処理を実行する。

図１７の第５の複数異常表示処理では、各異常検出項目について、異常判定閾値に対するロボットの動作時に検出した値（例えば、モータトルクτｍ、モータ速度ωｍ、モータ電流Ｉｍ、モータ位置θｍ）である異常判定値（モータロック検出値ＬＫｄｅｔ、過負荷検出値ＯＬｄｅｔ、位置偏差オーバー検出値θｄｅｔ）の割合を求める。異常判定値の割合が最も高い異常検出項目のみを衝突検出とともに異常表示を行う処理を有している。

図１７において、ステップ１７−１で、ロボットの衝突検出が発生したか否かを判定している。ステップ１７−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１７−２へ進む。ステップ１７−１において、ロボットの衝突検出が発生しないと「Ｎ」と判定し、第５の複数異常表示処理を終了する。

ステップ１７−２では、異常判定閾値に対する異常判定値の割合を計算する。ステップ１７−２では、モータロック検出異常判定値割合ＬＫｒａｔと、過負荷検出異常判定値割合ＯＬｒａｔと、位置偏差オーバー検出異常判定値割合θｒａｔを、下記式（１６−１）〜（１６−３）で算出する。

ＬＫｒａｔ ＝ ＬＫｄｅｔ ÷ ＬＫｔｈ （１６−１）
ＯＬｒａｔ ＝ ＯＬｄｅｔ ÷ ＯＬｔｈ （１６−２）
θｒａｔ ＝ θｄｅｔ ÷ θｔｈ （１６−３）
なお、ＬＫｄｅｔはモータロック検出値であり、ＬＫｔｈはモータロック検出閾値である。また、ＯＬｄｅｔは過負荷検出値であり、ＯＬｔｈは過負荷検出閾値である。また、θｄｅｔは位置偏差オーバー検出値であり、θｔｈは位置偏差オーバー検出閾値である。

ステップ１７−３では、ステップ１７−２で求めた異常判定値割合の内、モータロック検出異常判定値割合ＬＫｒａｔが最大かどうかを判定する。ステップ１７−３において、モータロック検出異常判定値割合ＬＫｒａｔが最大であると「Ｙ」と判定し、ステップ１７−４へ進む。ステップ１７−３において、モータロック検出異常判定値割合ＬＫｒａｔが最大でないと「Ｎ」と判定し、ステップ１７−５へ進む。

ステップ１７−４では、「衝突検出」と「モータロック検出」を異常表示し、第５の複数異常表示処理を終了する。

ステップ１７−５では、ステップ１７−２で求めた異常判定値割合の内、過負荷検出異常判定値割合ＯＬｒａｔが最大かどうかを判定する。ステップ１７−５において、過負荷検出異常判定値割合ＯＬｒａｔが最大であると「Ｙ」と判定し、ステップ１７−６へ進む。ステップ１７−５において、過負荷検出異常判定値割合ＯＬｒａｔが最大でないと「Ｎ」と判定し、ステップ１７−７へ進む。

ステップ１７−６では、「衝突検出」と「過負荷検出」を異常表示し、第５の複数異常表示処理を終了する。

ステップ１７−７では、「衝突検出」と「位置偏差オーバー検出」を異常表示し、第５の複数異常表示処理を終了する。

異常判定値割合が最大の項目のみ表示する理由は、実際の衝突を検出しなければ、異常判定値割合が最大の項目における異常発生の可能性が一番高いからである。

（実施の形態６）
本実施の形態６では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１８のフローチャートに示す第６の複数異常表示処理を実行する。

図１８において、ステップ１８−１で、ロボットの衝突検出が発生したか否かを判定している。ステップ１８−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１８−２に進み「衝突検出」の異常表示を行い、さらにステップ１８−３へ進む。ステップ１８−１において、ロボットの衝突検出が発生しないと「Ｎ」と判定し、その後は何も実施せず、第６の複数異常表示処理を終了する。

ステップ１８−３では、異常判定閾値に対する異常判定値割合を計算する。なお、モータロック検出異常判定値割合ＬＫｒａｔと、過負荷検出異常判定値割合ＯＬｒａｔと、位置偏差オーバー検出異常判定値割合θｒａｔは、実施の形態５で説明した既出の式（１６−１）〜（１６−３）により算出する。その後ステップ１８−４へ進む。

ステップ１８−４では、異常表示に「モータロック検出割合 ＬＫｒａｔ」、「過負荷検出割合 ＯＬｒａｔ」、「位置偏差オーバー検出割合 θｒａｔ」を追加して表示し、第６の複数異常表示処理を終了する。なお、各異常判定値割合の表示は、実際の計算数字である。

衝突検出発生時に、他の異常判定値割合を表示することで、衝突を検出しなければ、どの項目の異常発生の可能性がどの程度高いかを知ることができる。

（実施の形態７）
本実施の形態７では、図２のステップ２−５の衝突検出発生時異常表示処理として、図１９のフローチャートに示す第７の複数異常表示処理を実行する。

図１９の第７の複数異常表示処理では、実施の形態１における図１３の第１の複数異常表示処理に対し、各異常検出項目について異常判定閾値に対する異常判定値の割合を求める処理を加えたものである。ロボットの衝突を検出した場合に、衝突検出したことを表示することと併せて各異常検出項目を表示する処理を、異常判定閾値に対する異常判定値の割合を表示する処理に変更している。

図１９において、ステップ１９−１で、ロボットの衝突検出が発生したか否かを判定している。ステップ１９−１において、ロボットの衝突検出が発生すると「Ｙ」と判定し、ステップ１９−２に進み「衝突検出」の異常表示を行い、さらにステップ１９−Ａへ進む。ステップ１９−１において、ロボットの衝突検出が発生しないと「Ｎ」と判定し、その後は何も実施せず、第７の複数異常表示処理を終了する。

ステップ１９−Ａでは、異常判定閾値に対する異常判定値割合を計算する。なお、モータロック検出異常判定値割合ＬＫｒａｔと、過負荷検出異常判定値割合ＯＬｒａｔと、位置偏差オーバー検出異常判定値割合θｒａｔは、実施の形態５で説明した既出の式（１６−１）〜（１６−３）により算出する。

ステップ１９−３では、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っているか否かを判定する。ステップ１９−３において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていると「Ｙ」と判定し、ステップ１９−４に進む。ステップ１９−３において、モータロック検出値ＬＫｄｅｔがモータロック検出表示可否閾値ＬＫｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１９−５に進む。

ステップ１９−４では、異常表示に「モータロック検出割合 ＬＫｒａｔ」を追加して表示し、ステップ１９−５へ進む。なお、モータロック検出割合ＬＫｒａｔの表示は、実際の計算数字である。

ステップ１９−５では、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っているか否かを判定する。ステップ１９−５において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていると「Ｙ」と判定し、ステップ１９−６に進む。ステップ１９−５において、過負荷検出値ＯＬｄｅｔが過負荷検出表示可否閾値ＯＬｔｈｄを上回っていないと「Ｎ」と判定し、ステップ１９−７に進む。

ステップ１９−６では、異常表示に「過負荷検出割合 ＯＬｒａｔ」を追加して表示し、ステップ１９−７へ進む。なお、過負荷検出割合ＯＬｒａｔの表示は、実際の計算数字である。

ステップ１９−７では、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っているか否かを判定する。ステップ１９−７において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていると「Ｙ」と判定し、ステップ１９−８に進む。ステップ１９−７において、位置偏差オーバー検出値θｄｅｔが位置偏差オーバー検出表示可否閾値θｔｈｄを上回っていないと「Ｎ」と判定し、第７の複数異常表示処理を終了する。

ステップ１９−８では、異常表示に「位置偏差オーバー検出割合 θｒａｔ」を追加して表示し、第７の複数異常表示処理を終了する。なお、位置偏差オーバー検出割合θｒａｔの表示は、実際の計算数字である。

衝突検出発生時に、検出表示可否閾値を越えた異常のみ異常判定値割合を表示することで、衝突を検出しなければ、どの項目の異常発生の可能性がどの程度高いかを絞り込んで表示することができる。

本発明のロボットの異常表示方法は、ロボットの衝突を検出した場合、異常表示として衝突検出したことを表示するとともに、衝突検出とは異なる複数の異常検出項目のうち少なくとも１つ以上の異常表示を選択して行う処理を備えているので、衝突誤検出発生時の状況を把握するのに有用な情報を得ることができ、産業上有用である。

１ 位置指令の速度成分 ｄθｃｏｍ
２ モータ速度 ωｍ
３ 位置指令 θｃｏｍ
４ モータ位置 θｍ
５ 位置比例ゲイン ＫＰＰ
６ 位置制御ブロック
７ 速度ループ指令 ωｃｏｍ
８ 速度比例ゲイン ＫＰ
９ 速度積分ゲイン ＫＩ／ｓ
１０ 速度制御ブロック
１１ モータ電流 Ｉｍ
１２ モータトルク定数 Ｋｔ
１３ モータ発生トルク τｍ
１４ 動力学トルク（重力トルク、慣性力、遠心力、コリオリ力の和） τｄｙｎ
１５ 動摩擦トルク τμ
１６ 衝突トルク τｄｉｓ
１７ モータ伝達関数ブロック
１８ モータ及び外力を示すブロック
１９ モータトルク定数 Ｋｔ
２０ 粘性摩擦係数 Ｄ
２１ モータ回転方向判定ブロック
２２ モータ方向信号 ｓｇｎ
２３ 動摩擦係数 Ｋμ
２４ 動摩擦トルク推定値 τμｏ
２５ 他軸モータ位置
２６ 動力学トルク演算ブロック
２７ 他軸動力学トルク推定値
２８ 衝突トルク推定値 τｄｉｓｏ
２９ 動力学トルク推定値 τｄｙｎｏ
３０ 衝突トルク推定ブロック
３１ 衝突判定ブロック
３２ 微分要素
４０ 積分要素
８６ モータ位置の誤差 ｅθｍ
８７ モータ速度の誤差 ｅωｍ
８８ モータ加速度の誤差 ｅαｍ
８９ モータイナーシャ Ｊｍ
９０ モータ加速度 αｍ
９１ モータトルク閾値 τｍｔｈ
９２ モータ速度閾値 ωｍｔｈ
９３ モータロック検出時間閾値 ＬＫｔｈ
９５ モータ速度に到達するまでの正常な所用時間 ＬＫ０
９９ モータ位置検出器

上記課題を解決するために、本発明のロボットの異常表示方法は、ロボットの動作時に検出した値に基づいて少なくとも衝突検出を含む複数の異常を検出し、異常が発生したことを表示するロボットの異常表示方法である。さらに、ロボットの衝突をセンサレスで検出する衝突検出工程を有する。さらに、衝突検出工程の後であって、異常表示として衝突検出したことを表示する衝突検出表示工程を有する。さらに、衝突検出工程の後であって、衝突検出とは異なる複数の異常検出項目のうち少なくとも１つについても異常表示として異常が発生したことを表示する異常表示工程を有する。

Claims (9)

1. ロボットの動作時に検出した値に基づいて少なくとも衝突検出を含む複数の異常を検出し、異常が発生したことを表示するロボットの異常表示方法であって、
   前記ロボットの衝突を検出する衝突検出工程と、
   異常表示として衝突検出したことを表示する衝突検出表示工程と、
   前記衝突検出とは異なる複数の異常検出項目のうち少なくとも１つについても異常表示として異常が発生したことを表示する異常表示工程と、を備えたロボットの異常表示方法。
2. 前記異常表示工程において、前記複数の異常検出項目の異常表示を選択して行う請求項１記載のロボットの異常表示方法。
3. 前記複数の異常検出項目の各々には、異常と判定するための異常判定閾値と、前記異常判定閾値より小さくロボットの衝突を検出した場合に衝突検出したことを表示することと併せて異常が発生したことを表示するための表示可否閾値とが設定されており、
   前記複数の異常検出項目のうち、ロボットの動作時に検出した値に基づく異常判定値が前記表示可否閾値を超えたものは異常表示として異常が発生したことを表示する請求項２記載のロボットの異常表示方法。
4. 前記複数の異常検出項目の各々には、異常と判定するための異常判定閾値と、前記異常判定閾値より小さくロボットの衝突を検出した場合に衝突検出したことを表示することと併せて異常が発生したことを表示するための表示可否閾値とが設定されており、
   前記複数の異常検出項目には優先順位が設定されており、
   前記優先順位が高いものから順にロボットの動作時に検出した値に基づく前記異常判定値が前記表示可否閾値を超えたか否かを判定し、
   前記表示可否閾値を超えた異常検出項目のみ異常表示を行い、
   前記異常表示を行った異常検出項目より前記優先順位が低い異常検出項目については、前記異常判定値が前記表示可否閾値を超えたか否かの判定および異常表示を行わない請求項２記載のロボットの異常表示方法。
5. 設定された衝突検出閾値に基づいて各異常検出項目についての前記表示可否閾値を決定する請求項３または４記載のロボットの異常表示方法。
6. 前記複数の異常検出項目のそれぞれについて、異常判定閾値に対するロボットの動作時に検出した値に基づく異常判定値の割合を求め、
   前記異常判定値の割合が最も高い異常検出項目のみ衝突検出とともに異常表示を行う請求項２記載のロボットの異常表示方法。
7. 前記複数の異常検出項目の各々には、異常と判定するための異常判定閾値が設定されており、
   前記複数の異常検出項目のそれぞれについて、異常判定閾値に対するロボットの動作時に検出した値に基づく異常判定値の割合を求め、
   ロボットの衝突を検出した場合に衝突検出したことを表示することと併せて、前記複数の異常検出項目のそれぞれについて、前記異常判定値の割合を表示する請求項１記載のロボットの異常表示方法。
8. 前記複数の異常検出項目の各々には、異常と判定するための異常判定閾値が設定されており、
   前記複数の異常検出項目のそれぞれについて、異常判定閾値に対するロボットの動作時に検出した値に基づく異常判定値の割合を求め、
   ロボットの衝突を検出した場合に衝突検出したことを表示することと併せて、前記異常判定値が表示可否閾値を超えたもののみ各異常検出項目の前記異常判定値の割合を表示する請求項１記載のロボットの異常表示方法。
9. 前記複数の異常検出項目として、ロボットを駆動するモータのロック検出と前記モータの過負荷検出と前記ロボットの位置偏差検出との少なくとも１つを含む請求項１から８のいずれか１項に記載のロボットの異常表示方法。

Images