JP3997201B2 - 産業用ロボットおよびその異常判断方法 - Google Patents

Description

本発明は、ロボットアームの駆動源にサーボモータを使用した産業用ロボットに関し、特に、ロボットアームの位置ずれや暴走等の異常の検出を精度よく行うことが可能な、産業用ロボット及びその異常判断方法に関する。

産業用ロボットにおいては、従来、サーボモータに取り付けられたエンコーダからの位置情報および所望の動作をさせるために計算された指令位置に基づいて位置制御量を計算する比例型フィードバック制御部を有し、この比例型フィードバック制御部からの制御量によってサーボモータおよびそれに連結されているロボットアームなどを駆動させるようにしている。また、サーボモータ速度およびトルク指令を入力する状態推定オブザーバによる振動抑制部も有している。さらに、産業用ロボットにおいては、ロボットの軌道逸脱や周辺機器との干渉を検知する機能も設けられている。

このロボットの軌道逸脱や周辺機器との干渉を検知する手段としては、従来は指令位置と現在位置との差である位置偏差が固定的な異常の閾値を超えたか否かにより判断する方法がとられてきた。ところで、フィードバック制御を用いた場合、その制御特性上、サーボ遅れが存在するので、ロボットが軌道逸脱や周辺機器との干渉を生じていない正常動作時においても、指令位置と現在位置とにはサーボ遅れとロボットの移動速度との積から定まる位置偏差が存在する。さらに、このサーボ遅れに起因する位置偏差は、その性質上、ロボットの動作速度に比例して大きくなる。そのため、ロボットが高速で正常に動作している場合に異常であると誤検知しないよう、異常の閾値は正常動作時に許容される位置偏差よりも一般に大きな値が設定される。したがって、従来は、サーボ遅れの存在により異常の検出精度が上げられないという問題があった。

このようなサーボ遅れの存在に起因して異常検出の精度が悪くなるという問題を解決する技術としては、例えば特許文献１に開示されているものがある。この特許文献１においては、サーボモータを具備する産業用ロボットや工作機に関して、位置フィードバック制御部を含む全体を２次モデルとしてモデル化し、そのモデル化されたものの出力（推定位置）とエンコーダ等により検出された実際のサーボモータの位置（実位置）とを比較し、その差が所定の閾値を超えていた場合に異常として判断する方法を開示している。

しかし、前述した特許文献１に開示されているような、２次モデルと産業用ロボットなどの制御対象の実モデルとの間には一般にモデル化誤差が存在するので、２次モデルのような高次のモデルを決定するパラメータの同定は、閾値の決定に伴う試行錯誤に比べて同等以上の困難さが伴うことが多いという問題があった。

係る問題を解決するものとして、出願人は、特願２００２−３４８４７６において、時定数が位置ループゲインの逆数によって与えられる１次のローパスフィルタによってモデル化するとともに、異常検知閾値の算出に際しては動作周波数によって変動するモデル化誤差を考慮する方法を提案した。これにより低次のモデルを使用しながらも異常の検出精度を確保することを可能にした。
特開平１０−３１５１７３号公報

ところで、１次のローパスフィルタの出力とサーボモータの実際の出力との差分は、ロボットの動作速度に比例する。前述の特願２００２−３４８４７６では異常検知閾値の算出に際してロボットの動作速度は考慮されていないので、ロボットの動作速度が低速の場合には、指令位置の軌道に対する実際のロボットの軌道逸脱といった動作異常の検出が、ロボットの動作速度が高速の場合に比して遅れることになる。

本発明の目的は、ロボットの動作速度の如何に関わらず、指令位置の軌道に対する実際のロボットの軌道逸脱といった動作異常を瞬時に検出することが可能な、産業用ロボットおよびその異常判断方法を提供することである。

前述した目的を達成するために、本発明では以下の知見を利用することにした。
すなわち、出願人は、理論的に推定される現在位置すなわち推定現在位置（ローパスフィルタ６の出力）とサーボモータにて検出される実際の位置（モータおよびロボットアーム３の出力）との差（差分器１０の出力）は、ロボットの理論的に推定される速度すなわち推定速度（微分器７の出力）および総オブザーバフィードバック量（状態推定オブザーバ４の出力）に基づいて算出される値（異常検出閾値算出部８の出力）と比例関係にあるとの知見を得た。

そこで、請求項１に係る発明では、ロボットアームの駆動源にサーボモータを使用した産業用ロボットの異常判断方法において、位置ループゲインの逆数を時定数とするローパスフィルタにより指令位置に基づいて理論的な現在位置としての推定現在位置を算出し、サーボモータに取り付けられたエンコーダにて検出された実際の現在位置としての実位置と前記推定現在位置との差の絶対値を算出し、サーボモータを駆動するためのトルク指令値、前記実位置の微分値、および既知の外乱トルクに基づいて、ロボットアームの推定アーム速度および推定ねじれ量を算出し、これら推定アーム速度および推定ねじれ量のそれぞれに対して所定のゲインを掛けた後これらを加え合わせることにより、トルク量としての総オブザーバフィードバック量を算出し、この総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて異常検知閾値を算出し、前記実位置と推定現在位置との差の絶対値が前記異常検知閾値よりも大きい場合に異常状態であるものと判断するようにしたことを特徴とする産業用ロボットの異常判断方法を提供した。

また、係る異常判断方法を実現するための産業用ロボットとして、請求項２に係る発明では、ロボットアームの駆動源にサーボモータを使用した産業用ロボットにおいて、指令位置に基づいてサーボモータを駆動するためのトルク指令値を生成するＰＩＤ制御部と、生成されたトルク指令値に基づいてロボットアームを駆動するサーボモータと、位置ループゲインの逆数を時定数とし、前記指令位置に基づいて理論的な現在位置としての推定現在位置を算出するローパスフィルタと、前記トルク指令値、前記サーボモータに取り付けられたエンコーダにて検出された実際の現在位置としての実位置の微分値、および既知の外乱トルクに基づいて、ロボットアームの推定アーム速度および推定ねじれ量を算出し、これら推定アーム速度および推定ねじれ量のそれぞれに対して所定のゲインを掛けた後これらを加え合わせることにより、トルク量としての総オブザーバフィードバック量を算出する状態推定オブザーバと、この総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて異常検知閾値を算出する異常検知閾値算出部と、前記実位置と前記推定現在位置との差の絶対値が前記異常検知閾値よりも大きい場合に異常状態であるものと判断する異常判断部と、を有することを特徴とする産業用ロボットを提供した。

係る構成としたことにより、位置ループゲインの逆数を時定数とするローパスフィルタから出力された理論的な現在位置としての推定現在位置と、サーボモータに取り付けられたエンコーダにて検出された実際の現在位置としての実位置との差の絶対値を求め、この差が状態推定オブザーバにより算出された総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて設定された異常検知閾値よりも大きい場合に異常状態であるものと判断される。異常状態であるか否かを判断するための異常検知閾値の設定においては、指令位置に基づいて得られるロボットの推定速度を利用している、すなわちロボットの動作速度が加味されているので、ロボットの動作速度の如何に関わらず最適な異常検知閾値を設定できる。

本発明によれば、ロボットの動作速度の如何に関わらず最適な異常検知閾値を設定できるので、ロボットが低速で動作している際の異常についても瞬時に検出することができるものとなった。そのため、ロボットの動作速度の如何に関わらず、指令位置の軌道に対する実際のロボットの軌道逸脱といった動作異常を瞬時に検出することが可能となった。

以下、本発明の一実施形態について、図面を参照して説明する。図１は、本発明を実現するためのシステムの一実施形態を示すブロック図である。図中の１は、指令値フィルタＦＦ演算部である。この指令値フィルタＦＦ演算部１は、指令位置を入力し、この指令位置に対して停止時の衝撃を緩和するためのフィルタ処理を施す指令値フィルタ部と、応答性をよくするために利用するフィードフォワード（ＦＦ）量を計算し、このフィードフォワード（ＦＦ）量を入力した指令位置に加算するＦＦ演算部とから構成され、これによりフィルタ処理を施された指令位置を生成する。この生成された指令位置は、後述するＰＩＤ制御部２およびローパスフィルタ６に出力される。

２は、比例、積分、微分といった制御を行うＰＩＤ制御部である。このＰＩＤ制御部２は、指令値フィルタＦＦ演算部１から出力されたフィルタ処理を施された指令位置およびフィードバック量を入力し、これらに基づいて後述するモータおよびロボットアーム３が具備するサーボモータを駆動するためのトルク指令値を生成する。なお、このＰＩＤ制御部２における前述のフィードバック量とは、後述するモータおよびロボットアーム３が具備するサーボモータに取り付けられた図示しないエンコーダの位置情報、および後述する状態推定オブザーバ４にて算出された総オブザーバフィードバック量である。このＰＩＤ制御部２にて生成されたトルク指令値は、後述するモータおよびロボットアーム３および後述する状態推定オブザーバ４に出力される。

３は、モータおよびロボットアームである。このモータおよびロボットアーム３は、ＰＩＤ制御部２から出力されたトルク指令値を入力し、これに基づいて具備するサーボモータが駆動し、このサーボモータに連結されたロボットアームが動作する。このモータおよびロボットアーム３からは、サーボモータに取り付けられた図示しないエンコーダが検出した位置情報すなわち実位置（実際の位置）が出力される。

４は、状態推定オブザーバである。この状態推定オブザーバ４は、前述のＰＩＤ制御部２から出力されたトルク指令値、前述のモータおよびロボットアーム３から出力された実位置（実際の位置）を微分器５にて微分した値すなわち実速度（実際の速度）、および既知の外乱トルクを入力する。そして、これらの入力に基づいて、ロボットアームの推定アーム速度および推定ねじれ量を算出する。さらに、これら推定アーム速度および推定ねじれ量のそれぞれに対して所定のゲインを掛けた後、これらを加え合わせる。このようにして算出された値が前述のＰＩＤ制御部２へフィードバックすべき総オブザーバフィードバック量である。なお、この総オブザーバフィードバック量の次元はトルクである。この状態推定オブザーバ４にて算出された総オブザーバフィードバック量は、後述する異常検知閾値算出部８へ出力されるとともに、前述したＰＩＤ制御部２へフィードバックされる。

６は、ローパスフィルタである。このローパスフィルタ６は、時定数が位置ループゲインの逆数によって与えられる１次のローパスフィルタであり、前述したモータおよびロボットアーム３というロボット機構部を理論的にモデル化したものである。このローパスフィルタ６は、前述の指令値フィルタＦＦ演算部１から出力されたフィルタ処理を施された指令位置を入力し、これに基づいて理論的に推定される現在位置すなわち推定現在位置を算出する。

８は、異常検知閾値算出部である。この異常検知閾値算出部８は、前述の状態推定オブザーバ４から出力された総オブザーバフィードバック量、および前述のローパスフィルタ６から出力された推定現在位置を微分器７にて微分した値すなわち現在の推定速度を入力する。そして、これらの入力に基づいて、異常であるか否かを判断するための閾値としての異常検知閾値を算出する。ここでは、異常検知閾値を式（１）により算出する。

この式（１）において、Ｄは前述の状態推定オブザーバ４から出力された総オブザーバフィードバック量であり、状態推定オブザーバ４から逐次入力される。また、Ｖは前述のローパスフィルタ６から出力された推定現在位置を微分器７にて微分した値すなわち現在の推定速度であり、ローパスフィルタ６を経由して逐次入力される。Ｖ_ｍａｘはロボットの各関節軸の最高速度であり、予め設定しておく固定値である。Ｃ_０は前述の状態推定オブザーバ４から出力された総オブザーバフィードバック量に対する比例係数である。この比例係数については後述する。Ｇ_０は周波数０における動作ずれであり、Ｇ_ｍａｘは動作ずれの最大値であり、予め設定しておく固定値である。これらＧ_０およびＧ_ｍａｘは図２に基づいて設定する。すなわち、図２はローパスフィルタ６と実モデルとの誤差の周波数応答を示したものである。この図２を解析することにより、図２に示した周波数０における動作ずれＧ_０および動作ずれの最大値Ｇ_ｍａｘを算出することができる。

差分器１０は、モータおよびロボットアーム３から出力された実位置とローパスフィルタ６から出力された推定現在位置との差分を算出する。具体的には、差分器１０は、推定現在位置から実位置を差し引き、その絶対値を出力する。また、差分器１１は、異常検出閾値算出部８において算出された異常検出閾値から差分器１０において算出された実位置と推定現在位置との差分を差し引き、これを出力する。この差分器１１の出力は異常判断部９に入力される。

９は、異常判断部である。この異常判断部９は、前述の差分器１１より出力された異常検出閾値から実位置と推定現在位置との差分を差し引いた値を入力する。そしてこの値がマイナスであった場合、すなわち実位置と推定現在位置との差が異常検出閾値よりも大きい場合は、異常状態すなわち指令位置の軌道に対する実際のロボットの軌道逸脱といった動作異常の状態であるものと判断する。

以上、図１に示したブロック図における各ブロックの機能および処理について説明した。前述した異常判断部９における異常判断処理とした理由は、理論的に推定される現在位置すなわち推定現在位置（ローパスフィルタ６の出力）とサーボモータにて検出される実際の位置（モータおよびロボットアーム３の出力）との差（差分器１０の出力）は、ロボットの理論的に推定される速度すなわち推定速度（微分器７の出力）および前述した総オブザーバフィードバック量（状態推定オブザーバ４の出力）に基づいて算出される値（異常検出閾値算出部８の出力）と比例関係にあるとの知見を出願人が得たことによるものである。

本実施形態によれば、異常状態であるか否かを判断するための異常検知閾値の設定においては、指令位置に基づいて得られるロボットの推定速度を利用している、すなわちロボットの動作速度が加味されているので、ロボットの動作速度の如何に関わらず最適な異常検知閾値を設定できる。そのため、ロボットの動作速度の如何に関わらず、指令位置の軌道に対する実際のロボットの軌道逸脱といった動作異常を瞬時に検出することが可能となる。

なお、前述の式（１）に示した総オブザーバフィードバック量に対する比例係数Ｃ_０は、例えば式（２）により算出することができる。

すなわち、総オブザーバフィードバック量に対する比例係数Ｃ_０は、自身の関節軸の指令速度（指令位置の変化量）がゼロであるにも関わらず他の関節軸の動作によって自身の関節軸が振られて動作するような場合の最大偏差、およびこの最大偏差時の状態推定オブザーバ４からの出力（総オブザーバフィードバック量）により設定することができる。これにより、異常検知閾値算出部８にて算出される異常検知閾値には他の関節軸の動作が加味されることになるので、他の関節軸が動作することによる自身の関節軸の異常の誤検出が回避されることになり、その結果、異常の検出精度を向上させることができる。

本発明を実施するための一実施形態を示すブロック図である。 ローパスフィルタ６と実モデルとの誤差の周波数応答を示すグラフである。

符号の説明

１ 指令値フィルタＦＦ演算部
２ ＰＩＤ制御部
３ モータおよびロボットアーム
４ オブザーバ
６ ローパスフィルタ
８ 異常検知閾値算出部
９ 異常判断部

Claims (2)

1. ロボットアームの駆動源にサーボモータを使用した産業用ロボットの異常判断方法において、
   位置ループゲインの逆数を時定数とするローパスフィルタにより指令位置に基づいて理論的な現在位置としての推定現在位置を算出し、
   前記サーボモータに取り付けられたエンコーダにて検出された実際の現在位置としての実位置と前記推定現在位置との差の絶対値を算出し、
   前記サーボモータを駆動するためのトルク指令値、前記実位置の微分値、および既知の外乱トルクに基づいて、ロボットアームの推定アーム速度および推定ねじれ量を算出し、該推定アーム速度および推定ねじれ量のそれぞれに対して所定のゲインを掛けた後これらを加え合わせることにより、トルク量としての総オブザーバフィードバック量を算出し、
   該総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて異常検知閾値を算出し、
   前記実位置と推定現在位置との差の絶対値が前記異常検知閾値よりも大きい場合に異常状態であるものと判断するようにしたことを特徴とする産業用ロボットの異常判断方法。
2. ロボットアームの駆動源にサーボモータを使用した産業用ロボットにおいて、
   指令位置に基づいてサーボモータを駆動するためのトルク指令値を生成するＰＩＤ制御部と、
   前記トルク指令値に基づいてロボットアームを駆動するサーボモータと、
   位置ループゲインの逆数を時定数とし、前記指令位置に基づいて理論的な現在位置としての推定現在位置を算出するローパスフィルタと、
   前記トルク指令値、前記サーボモータに取り付けられたエンコーダにて検出された実際の現在位置としての実位置の微分値、および既知の外乱トルクに基づいて、ロボットアームの推定アーム速度および推定ねじれ量を算出し、該推定アーム速度および推定ねじれ量のそれぞれに対して所定のゲインを掛けた後これらを加え合わせることにより、トルク量としての総オブザーバフィードバック量を算出する状態推定オブザーバと、
   前記総オブザーバフィードバック量と前記推定現在位置を微分することにより得られる推定速度とに基づいて異常検知閾値を算出する異常検知閾値算出部と、
   前記実位置と前記推定現在位置との差の絶対値が前記異常検知閾値よりも大きい場合に異常状態であるものと判断する異常判断部と、
   を有することを特徴とする産業用ロボット。

Images