(57)【要約】  【課題】ロボットの動作速度の如何に関わらず、動作異常を瞬時に検出することが可能な、産業用ロボットおよびその異常判断方法を提供する。  【解決手段】位置ループゲインの逆数を時定数とするローパスフィルタ６により指令位置に基づいて推定現在位置を算出し、サーボモータ３に取り付けられたエンコーダにて検出された実際の現在位置としての実位置と前記推定現在位置との差の絶対値を算出し、サーボモータ３を駆動するためのトルク指令値、前記実位置の微分値、および既知の外乱トルクに基づいて、総オブザーバフィードバック量を算出し、この総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて異常検知閾値を算出し、前記実位置と推定現在位置との差の絶対値が前記異常検知閾値よりも大きい場合に異常状態であるものと判断するようにした。  【選択図】図１
2005186183.tif 000002
【特許請求の範囲】
【請求項１】
ロボットアームの駆動源にサーボモータを使用した産業用ロボットの異常判断方法において、
位置ループゲインの逆数を時定数とするローパスフィルタにより指令位置に基づいて理論的な現在位置としての推定現在位置を算出し、
前記サーボモータに取り付けられたエンコーダにて検出された実際の現在位置としての実位置と前記推定現在位置との差の絶対値を算出し、
前記サーボモータを駆動するためのトルク指令値、前記実位置の微分値、および既知の外乱トルクに基づいて、総オブザーバフィードバック量を算出し、
該総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて異常検知閾値を算出し、
前記実位置と推定現在位置との差の絶対値が前記異常検知閾値よりも大きい場合に異常状態であるものと判断するようにしたことを特徴とする産業用ロボットの異常判断方法。
【請求項２】
ロボットアームの駆動源にサーボモータを使用した産業用ロボットにおいて、
指令位置に基づいてサーボモータを駆動するためのトルク指令値を生成するＰＩＤ制御部と、
前記トルク指令値に基づいてロボットアームを駆動するサーボモータと、
位置ループゲインの逆数を時定数とし、前記指令位置に基づいて理論的な現在位置としての推定現在位置を算出するローパスフィルタと、
前記トルク指令値、前記サーボモータに取り付けられたエンコーダにて検出された実際の現在位置としての実位置の微分値、および既知の外乱トルクに基づいて、総オブザーバフィードバック量を算出する状態推定オブザーバと、
前記総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて異常検知閾値を算出する異常検知閾値算出部と、
前記実位置と前記推定現在位置との差の絶対値が前記異常検知閾値よりも大きい場合に異常状態であるものと判断する異常判断部と、
を有することを特徴とする産業用ロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットアームの駆動源にサーボモータを使用した産業用ロボットに関し、特に、ロボットアームの位置ずれや暴走等の異常の検出を精度よく行うことが可能な、産業用ロボット及びその異常判断方法に関する。
【背景技術】
【０００２】
産業用ロボットにおいては、従来、サーボモータに取り付けられたエンコーダからの位置情報および所望の動作をさせるために計算された指令位置に基づいて位置制御量を計算する比例型フィードバック制御部を有し、この比例型フィードバック制御部からの制御量によってサーボモータおよびそれに連結されているロボットアームなどを駆動させるようにしている。また、サーボモータ速度およびトルク指令を入力する状態推定オブザーバによる振動抑制部も有している。さらに、産業用ロボットにおいては、ロボットの軌道逸脱や周辺機器との干渉を検知する機能も設けられている。
【０００３】
このロボットの軌道逸脱や周辺機器との干渉を検知する手段としては、従来は指令位置と現在位置との差である位置偏差が固定的な異常の閾値を超えたか否かにより判断する方法がとられてきた。ところで、フィードバック制御を用いた場合、その制御特性上、サーボ遅れが存在するので、ロボットが軌道逸脱や周辺機器との干渉を生じていない正常動作時においても、指令位置と現在位置とにはサーボ遅れとロボットの移動速度との積から定まる位置偏差が存在する。さらに、このサーボ遅れに起因する位置偏差は、その性質上、ロボットの動作速度に比例して大きくなる。そのため、ロボットが高速で正常に動作している場合に異常であると誤検知しないよう、異常の閾値は正常動作時に許容される位置偏差よりも一般に大きな値が設定される。したがって、従来は、サーボ遅れの存在により異常の検出精度が上げられないという問題があった。
【０００４】
このようなサーボ遅れの存在に起因して異常検出の精度が悪くなるという問題を解決する技術としては、例えば特許文献１に開示されているものがある。この特許文献１においては、サーボモータを具備する産業用ロボットや工作機に関して、位置フィードバック制御部を含む全体を２次モデルとしてモデル化し、そのモデル化されたものの出力（推定位置）とエンコーダ等により検出された実際のサーボモータの位置（実位置）とを比較し、その差が所定の閾値を超えていた場合に異常として判断する方法を開示している。
【０００５】
しかし、前述した特許文献１に開示されているような、２次モデルと産業用ロボットなどの制御対象の実モデルとの間には一般にモデル化誤差が存在するので、２次モデルのような高次のモデルを決定するパラメータの同定は、閾値の決定に伴う試行錯誤に比べて同等以上の困難さが伴うことが多いという問題があった。
【０００６】
係る問題を解決するものとして、出願人は、特願２００２−３４８４７６において、時定数が位置ループゲインの逆数によって与えられる１次のローパスフィルタによってモデル化するとともに、異常検知閾値の算出に際しては動作周波数によって変動するモデル化誤差を考慮する方法を提案した。これにより低次のモデルを使用しながらも異常の検出精度を確保することを可能にした。
【特許文献１】特開平１０−３１５１７３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００７】
ところで、１次のローパスフィルタの出力とサーボモータの実際の出力との差分は、ロボットの動作速度に比例する。前述の特願２００２−３４８４７６では異常検知閾値の算出に際してロボットの動作速度は考慮されていないので、ロボットの動作速度が低速の場合には、指令位置の軌道に対する実際のロボットの軌道逸脱といった動作異常の検出が、ロボットの動作速度が高速の場合に比して遅れることになる。
【０００８】
本発明の目的は、ロボットの動作速度の如何に関わらず、指令位置の軌道に対する実際のロボットの軌道逸脱といった動作異常を瞬時に検出することが可能な、産業用ロボットおよびその異常判断方法を提供することである。
【課題を解決するための手段】
【０００９】
前述した目的を達成するために、本発明では以下の知見を利用することにした。
すなわち、出願人は、理論的に推定される現在位置すなわち推定現在位置（ローパスフィルタ６の出力）とサーボモータにて検出される実際の位置（モータおよびロボットアーム３の出力）との差（差分器１０の出力）は、ロボットの理論的に推定される速度すなわち推定速度（微分器７の出力）および総オブザーバフィードバック量（状態推定オブザーバ４の出力）に基づいて算出される値（異常検出閾値算出部８の出力）と比例関係にあるとの知見を得た。
【００１０】
そこで、請求項１に係る発明では、ロボットアームの駆動源にサーボモータを使用した産業用ロボットの異常判断方法において、位置ループゲインの逆数を時定数とするローパスフィルタにより指令位置に基づいて理論的な現在位置としての推定現在位置を算出し、サーボモータに取り付けられたエンコーダにて検出された実際の現在位置としての実位置と前記推定現在位置との差の絶対値を算出し、サーボモータを駆動するためのトルク指令値、前記実位置の微分値、および既知の外乱トルクに基づいて、総オブザーバフィードバック量を算出し、この総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて異常検知閾値を算出し、前記実位置と推定現在位置との差の絶対値が前記異常検知閾値よりも大きい場合に異常状態であるものと判断するようにしたことを特徴とする産業用ロボットの異常判断方法を提供した。
【００１１】
また、係る異常判断方法を実現するための産業用ロボットとして、請求項２に係る発明では、ロボットアームの駆動源にサーボモータを使用した産業用ロボットにおいて、指令位置に基づいてサーボモータを駆動するためのトルク指令値を生成するＰＩＤ制御部と、生成されたトルク指令値に基づいてロボットアームを駆動するサーボモータと、位置ループゲインの逆数を時定数とし、前記指令位置に基づいて理論的な現在位置としての推定現在位置を算出するローパスフィルタと、前記トルク指令値、前記サーボモータに取り付けられたエンコーダにて検出された実際の現在位置としての実位置の微分値、および既知の外乱トルクに基づいて、総オブザーバフィードバック量を算出する状態推定オブザーバと、この総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて異常検知閾値を算出する異常検知閾値算出部と、前記実位置と前記推定現在位置との差の絶対値が前記異常検知閾値よりも大きい場合に異常状態であるものと判断する異常判断部と、を有することを特徴とする産業用ロボットを提供した。
【００１２】
係る構成としたことにより、位置ループゲインの逆数を時定数とするローパスフィルタから出力された理論的な現在位置としての推定現在位置と、サーボモータに取り付けられたエンコーダにて検出された実際の現在位置としての実位置との差の絶対値を求め、この差が状態推定オブザーバにより算出された総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて設定された異常検知閾値よりも大きい場合に異常状態であるものと判断される。異常状態であるか否かを判断するための異常検知閾値の設定においては、指令位置に基づいて得られるロボットの推定速度を利用している、すなわちロボットの動作速度が加味されているので、ロボットの動作速度の如何に関わらず最適な異常検知閾値を設定できる。
【発明の効果】
【００１３】
本発明によれば、ロボットの動作速度の如何に関わらず最適な異常検知閾値を設定できるので、ロボットが低速で動作している際の異常についても瞬時に検出することができるものとなった。そのため、ロボットの動作速度の如何に関わらず、指令位置の軌道に対する実際のロボットの軌道逸脱といった動作異常を瞬時に検出することが可能となった。
【発明を実施するための最良の形態】
【００１４】
以下、本発明の一実施形態について、図面を参照して説明する。図１は、本発明を実現するためのシステムの一実施形態を示すブロック図である。図中の１は、指令値フィルタＦＦ演算部である。この指令値フィルタＦＦ演算部１は、指令位置を入力し、この指令位置に対して停止時の衝撃を緩和するためのフィルタ処理を施す指令値フィルタ部と、応答性をよくするために利用するフィードフォワード（ＦＦ）量を計算し、このフィードフォワード（ＦＦ）量を入力した指令位置に加算するＦＦ演算部とから構成され、これによりフィルタ処理を施された指令位置を生成する。この生成された指令位置は、後述するＰＩＤ制御部２およびローパスフィルタ６に出力される。
【００１５】
２は、比例、積分、微分といった制御を行うＰＩＤ制御部である。このＰＩＤ制御部２は、指令値フィルタＦＦ演算部１から出力されたフィルタ処理を施された指令位置およびフィードバック量を入力し、これらに基づいて後述するモータおよびロボットアーム３が具備するサーボモータを駆動するためのトルク指令値を生成する。なお、このＰＩＤ制御部２における前述のフィードバック量とは、後述するモータおよびロボットアーム３が具備するサーボモータに取り付けられた図示しないエンコーダの位置情報、および後述する状態推定オブザーバ４にて算出された総オブザーバフィードバック量である。このＰＩＤ制御部２にて生成されたトルク指令値は、後述するモータおよびロボットアーム３および後述する状態推定オブザーバ４に出力される。
【００１６】
３は、モータおよびロボットアームである。このモータおよびロボットアーム３は、ＰＩＤ制御部２から出力されたトルク指令値を入力し、これに基づいて具備するサーボモータが駆動し、このサーボモータに連結されたロボットアームが動作する。このモータおよびロボットアーム３からは、サーボモータに取り付けられた図示しないエンコーダが検出した位置情報すなわち実位置（実際の位置）が出力される。
【００１７】
４は、状態推定オブザーバである。この状態推定オブザーバ４は、前述のＰＩＤ制御部２から出力されたトルク指令値、前述のモータおよびロボットアーム３から出力された実位置（実際の位置）を微分器５にて微分した値すなわち実速度（実際の速度）、および既知の外乱トルクを入力する。そして、これらの入力に基づいて、ロボットアームの推定アーム速度および推定ねじれ量を算出する。さらに、これら推定アーム速度および推定ねじれ量のそれぞれに対して所定のゲインを掛けた後、これらを加え合わせる。このようにして算出された値が前述のＰＩＤ制御部２へフィードバックすべき総オブザーバフィードバック量である。なお、この総オブザーバフィードバック量の次元はトルクである。この状態推定オブザーバ４にて算出された総オブザーバフィードバック量は、後述する異常検知閾値算出部８へ出力されるとともに、前述したＰＩＤ制御部２へフィードバックされる。
【００１８】
６は、ローパスフィルタである。このローパスフィルタ６は、時定数が位置ループゲインの逆数によって与えられる１次のローパスフィルタであり、前述したモータおよびロボットアーム３というロボット機構部を理論的にモデル化したものである。このローパスフィルタ６は、前述の指令値フィルタＦＦ演算部１から出力されたフィルタ処理を施された指令位置を入力し、これに基づいて理論的に推定される現在位置すなわち推定現在位置を算出する。
【００１９】
８は、異常検知閾値算出部である。この異常検知閾値算出部８は、前述の状態推定オブザーバ４から出力された総オブザーバフィードバック量、および前述のローパスフィルタ６から出力された推定現在位置を微分器５にて微分した値すなわち現在の推定速度を入力する。そして、これらの入力に基づいて、異常であるか否かを判断するための閾値としての異常検知閾値を算出する。ここでは、異常検知閾値を式（１）により算出する。
【００２０】
【数１】
2005186183.tif 000003
【００２１】
この式（１）において、Ｄは前述の状態推定オブザーバ４から出力された総オブザーバフィードバック量であり、状態推定オブザーバ４から逐次入力される。また、Ｖは前述のローパスフィルタ６から出力された推定現在位置を微分器５にて微分した値すなわち現在の推定速度であり、ローパスフィルタ６を経由して逐次入力される。Ｖｍａｘはロボットの各関節軸の最高速度であり、予め設定しておく固定値である。Ｃ０は前述の状態推定オブザーバ４から出力された総オブザーバフィードバック量に対する比例係数である。この比例係数については後述する。Ｇ０は周波数０における動作ずれであり、Ｇｍａｘは動作ずれの最大値であり、予め設定しておく固定値である。これらＧ０およびＧｍａｘは図２に基づいて設定する。すなわち、図２はローパスフィルタ６と実モデルとの誤差の周波数応答を示したものである。この図２を解析することにより、図２に示した周波数０における動作ずれＧ０および動作ずれの最大値Ｇｍａｘを算出することができる。
【００２２】
差分器１０は、モータおよびロボットアーム３から出力された実位置とローパスフィルタ６から出力された推定現在位置との差分を算出する。具体的には、差分器１０は、推定現在位置から実位置を差し引き、その絶対値を出力する。また、差分器１１は、異常検出閾値算出部８において算出された異常検出閾値から差分器１０において算出された実位置と推定現在位置との差分を差し引き、これを出力する。この差分器１１の出力は異常判断部９に入力される。
【００２３】
９は、異常判断部である。この異常判断部９は、前述の差分器１１より出力された異常検出閾値から実位置と推定現在位置との差分を差し引いた値を入力する。そしてこの値がマイナスであった場合、すなわち実位置と推定現在位置との差が異常検出閾値よりも大きい場合は、異常状態すなわち指令位置の軌道に対する実際のロボットの軌道逸脱といった動作異常の状態であるものと判断する。
【００２４】
以上、図１に示したブロック図における各ブロックの機能および処理について説明した。前述した異常判断部９における異常判断処理とした理由は、理論的に推定される現在位置すなわち推定現在位置（ローパスフィルタ６の出力）とサーボモータにて検出される実際の位置（モータおよびロボットアーム３の出力）との差（差分器１０の出力）は、ロボットの理論的に推定される速度すなわち推定速度（微分器７の出力）および前述した総オブザーバフィードバック量（状態推定オブザーバ４の出力）に基づいて算出される値（異常検出閾値算出部８の出力）と比例関係にあるとの知見を出願人が得たことによるものである。
【００２５】
本実施形態によれば、異常状態であるか否かを判断するための異常検知閾値の設定においては、指令位置に基づいて得られるロボットの推定速度を利用している、すなわちロボットの動作速度が加味されているので、ロボットの動作速度の如何に関わらず最適な異常検知閾値を設定できる。そのため、ロボットの動作速度の如何に関わらず、指令位置の軌道に対する実際のロボットの軌道逸脱といった動作異常を瞬時に検出することが可能となる。
【００２６】
なお、前述の式（１）に示した総オブザーバフィードバック量に対する比例係数Ｃ０は、例えば式（２）により算出することができる。
【００２７】
【数２】
2005186183.tif 000004
【００２８】
すなわち、総オブザーバフィードバック量に対する比例係数Ｃ０は、自身の関節軸の指令速度（指令位置の変化量）がゼロであるにも関わらず他の関節軸の動作によって自身の関節軸が振られて動作するような場合の最大偏差、およびこの最大偏差時の状態推定オブザーバ４からの出力（総オブザーバフィードバック量）により設定することができる。これにより、異常検知閾値算出部８にて算出される異常検知閾値には他の関節軸の動作が加味されることになるので、他の関節軸が動作することによる自身の関節軸の異常の誤検出が回避されることになり、その結果、異常の検出精度を向上させることができる。
【図面の簡単な説明】
【００２９】
【図１】本発明を実施するための一実施形態を示すブロック図である。
【図２】ローパスフィルタ６と実モデルとの誤差の周波数応答を示すグラフである。
【符号の説明】
【００３０】
１  指令値フィルタＦＦ演算部
２  ＰＩＤ制御部
３  モータおよびロボットアーム
４  オブザーバ
６  ローパスフィルタ
８  異常検知閾値算出部
９  異常判断部
【図１】
2005186183.tif 000005
【図２】
2005186183.tif 000006