JP2008183680A - 負荷機械の制御装置とその衝突検出しきい値更新方法 - Google Patents

Abstract

【課題】予め設定した衝突検出しきい値を、工作機械や産業用ロボット等のの負荷機械の状態に応じて自動更新し、機械に作用する摩擦等に変化が生じる場合でも、衝突検出の感度を低下させることなく、衝突時の負荷を最小限に抑える信頼性の高い機械の制御装置提供する。
【解決手段】衝突検出しきい値の初期値を設定する初期値設定モードと、負荷機械の通常行なう動作パターンで動作させる通常動作モードとを有し、初期値設定モードにおいて、動作パターンをＮ分割して各動作区間の動作を開始させ、サーボ制御部１２から現在の外乱推定最大値を保持する制御統括部１１と、各動作区間における外乱推定値を推定してその最大値を現在の外乱推定最大値として制御統括部１１に渡すサーボ制御部１２とを備え、通常動作モードにおいて、制御統括部１１が、外乱推定値と現在の衝突検出しきい値とに基づいて現在の衝突検出しきい値を自動的に更新する。
【選択図】図１

Description

本発明は、同じ動作パターンを延々と繰り返す産業用ロボット、液晶表示パネル用ガラス基板搬送ロボット、半導体ウェハ搬送用ロボット等の負荷機械が、その周辺の障害物等に衝突した場合に、衝突を瞬時に検出して衝突時の負荷を最小限に抑えることが可能な負荷機械の制御装置とその衝突検出しきい値更新方法に関する。

工作機械や産業用ロボットのように、サーボモータを介して負荷機械を駆動する場合において、教示位置の誤入力や作業を行うワークの誤設定により、負荷機械が周辺治具と衝突してしまう場合がある。このような衝突を瞬時に検出して衝突時の被害を低減する方法として、オブザーバで負荷機械に加わる外乱を推定し、この推定外乱が予め設定されたしきい値を越えることにより瞬時に衝突を検出し、回避手段をとって被害の低減を図る方法が知られている（例えば、非特許文献１参照）。
またこの方法に関して、予め設定されたしきい値を自動的に設定する、第１乃至４の従来技術がある。

第１の従来技術は、産業用ロボットを構成するアームあるいはエンドエフェクタが障害物などと衝突した場合に、衝突時の負荷を最小限に抑えることが可能な産業用ロボットの衝突検出方法に関するものであり、アーム自身あるいはこれに把持されたエンドエフェクタが障害物に衝突した際の、衝突が発生してから衝突を検出するまでのタイムラグを少なくし、その結果アームあるいはエンドエフェクタ及び減速機を含む駆動系にかかる衝突時の負荷を最小限に抑えることが可能な産業用ロボットの衝突検出方法を提供することを目的としており、オブザーバを用いることにより駆動軸モータが受ける推定外乱トルクを算出し、この推定外乱トルクから既知の外乱トルクを差し引くことにより外乱トルクの衝突成分を算出し、この外乱トルクの衝突成分が予め設定された第１の規定値を超えたとき、または外乱トルクの衝突成分の変化量が予め設定された第２の規定値を超えたときのいずれかの場合に衝突を検出したと判断するようにしている。また、第１の規定値は、産業用ロボットを衝突の発生がない状態で動作させ、このときの外乱トルクの衝突成分の最大値を算出し、この最大値に所定のマージン値を乗ずることにより自動的に設定するようにしている（例えば、特許文献１参照）。

第２の従来技術は、衝突の発生を検出するための適正な規定値を一義的に選定することにより、迅速かつ信頼性の高い産業用ロボットの衝突検出方法を提供することを目的としており、産業用ロボットを衝突の発生がない状態で動作させ、このときの前記外乱トルクの衝突成分の最大値を算出し、該最大値に所定のマージン値を乗ずることにより前記規定値を自動的に設定するようにしている。また、所定のマージン値を１．５以上かつ２．５以下の範囲内で設定するようにしている（例えば、特許文献２参照）。

第３の従来技術または第４の従来技術は共に、衝突及び溶着の発生を検出するための適正な規定値を一義的に選定することにより、迅速かつ信頼性の高い溶接ロボットの動作異常検出方法を提供することを目的としており、各溶接点位置を基準にしたときの溶接ロボット本体の予め設定された移動範囲内を溶着検出範囲とし、一方、移動範囲外を衝突検出範囲とし、溶接ロボットを衝突の発生がなくかつ溶接電流を流さない状態にて動作させ、このときの溶着検出範囲と衝突検出範囲のそれぞれの区間における外乱トルクの衝突成分の最大値を算出し、これらそれぞれの区間における外乱トルクの衝突成分の最大値にそれぞれの区間毎に予め設定されたマージン値をそれぞれ乗ずることにより、溶着検出範囲及び衝突検出範囲のそれぞれにおける動作異常の規定値を自動的に設定している（例えば、特許文献３または４参照）。
特開平１１−０７０４９０号公報（第３−６頁、図１） 特開平１１−２５４３８０号公報（第３−５頁、図１） 特開平１１−２９１１９０号公報（第３−７頁、図１） 特開平１１−３２０４７７号公報（第４−８頁、図１） 村上俊之著「多軸加速制御における自己診断機能」、電気学会研究会資料「産業電力電気応用研究会」ＩＥＡ−８９−１０−１４、１９８９年２月２２日）

オブザーバにより負荷機械に作用する外乱を推定して衝突を検出する方法では、負荷機械をあらゆるパターンで動作させ、そのときの推定外乱の最大値にマージンを設けてしきい値を予め決定し、通常の動作でも誤検出しない様にする必要がある。一般に、推定外乱の最大値は、負荷機械が高速動作する程大きくなるため、最高速度で動作させて検出しない様にしきい値を決定している。
しかしながら、負荷機械に作用する摩擦等に経年変化が生じ、実際の外乱最大値が小さくなった場合、実際の外乱最大値と、推定外乱の最大値にマージンを設けて予め決定したしきい値との隔たりが大きくなり、衝突検出の感度が低下するという問題があった。また、通常の動作パターンで動かした時の実際の外乱最大値と、最高速度で動作させた時の実際の外乱最大値との隔たりが大きい場合も、衝突検出の感度が低下するという問題もあった。

第１乃至４の従来技術では、予め決定するしきい値を自動的に設定することが記載されているが、いずれも産業用ロボットを衝突の発生がない状態で動作させ、このときの前記外乱トルクの衝突成分の最大値を算出し、該最大値に所定のマージン値を乗ずることにより前記規定値を自動的に設定するようにしているに過ぎず、前述した問題と同様な問題があり、前記規定値を自動的に更新するという記載は一切ない。
本発明はこのような問題点に鑑みてなされたものであり、予め設定した衝突検出しきい値を、工作機械や産業用ロボット等の負荷機械の状態に応じて自動更新し、負荷機械に作用する摩擦等に経年変化が生じる場合でも、衝突検出の感度を低下させることなく衝突検出して、衝突時の負荷を最小限に抑えることができる信頼性の高い負荷機械の制御装置とその衝突検出しきい値更新方法を提供することを目的とする。

上記問題を解決するため、本発明は次のように構成したのである。
請求項１に記載の発明は、モータの位置または速度制御を統括する制御統括部と、前記制御統括部からの指令と位置検出器からの検出位置信号に基づいてサーボ演算するサーボ制御部と、前記モータへ駆動電流を供給するＰＷＭアンプ部とを備え、前記位置検出器を備えた前記モータに連結された負荷機械を制御する負荷機械の制御装置であって、オブザーバにより前記モータまたは前記負荷機械に加わる外乱を推定し、その外乱推定値と予め設定された衝突検出しきい値との比較に基づいて前記負荷機械の衝突を検出する負荷機械の制御装置において、前記制御装置が、衝突検出しきい値の初期値を設定する初期値設定モードと、前記負荷機械の通常行なう一連の動作パターンで動作させる通常動作モードとを有し、前記初期値設定モードで、前記動作パターンをＮ分割して各動作区間の動作をさせ、各動作区間における前記外乱推定値の最大値を前記初期値として設定し、前記通常動作モードで、前記動作パターンで動作させ、前記外乱推定値と前記初期値との比較に基づいて、衝突検出しきい値を自動的に更新するものである。
また、請求項２に記載の発明において、請求項１記載の発明における前記初期値設定モードにおいて、前記動作パターンをＮ分割して各動作区間の動作を開始させると共に、前記サーボ制御部から現在の外乱推定最大値を保持する前記制御統括部と、各動作区間における前記外乱推定値を推定してその最大値を前記現在の外乱推定最大値として前記制御統括部に渡す前記サーボ制御部とを備え、前記通常動作モードにおいて、前記制御統括部が、前記外乱推定値と現在の衝突検出しきい値とに基づいて前記現在の衝突検出しきい値を自動的に更新するものである。
また、請求項３に記載の発明において、請求項１記載の発明における前記制御統括部が、更に、前記外乱推定値と前記現在の外乱推定最大値とに基づいて前記現在の衝突検出しきい値を自動的に更新するものである。
また、請求項４に記載の発明において、請求項２記載の発明における前記サーボ制御部が前記制御統括部へ渡す前記現在の外乱推定最大値は、前記初期値設定モードにおける各動作区間毎の推定外乱最大値、あるいは、各動作区間中の最大である１つの推定外乱最大値であるものである。
また、請求項５に記載の発明において、請求項２記載の発明における前記制御統括部が保持する前記現在の外乱推定最大値は、前記初期値設定モードにおける各動作区間毎の推定外乱最大値、あるいは、各動作区間中の最大である１つの推定外乱最大値であるものである。
また、請求項６に記載の発明において、請求項１記載の発明における前記初期値設定モードにおける前記外乱推定値の最大値は、各動作区間毎の動作を複数回させた際の各最大値を平均化したものである。
また、請求項７に記載の発明において、請求項１記載の発明における前記制御装置が、単数の前記制御統括部と複数の前記サーボ制御部と、あるいは、複数の前記制御統括部と複数の前記サーボ制御部と、を備えるものである。
請求項８に記載の発明において、モータの位置または速度制御を統括する制御統括部と、前記制御統括部からの指令と位置検出器からの検出位置信号に基づいてサーボ演算するサーボ制御部と、前記モータへ駆動電流を供給するＰＷＭアンプ部とを備え、前記位置検出器を備えた前記モータに連結された負荷機械を制御する負荷機械の制御装置の衝突検出しきい値更新方法であって、オブザーバにより前記モータまたは前記負荷機械に加わる外乱を推定し、その外乱推定値と予め設定された衝突検出しきい値との比較に基づいて前記負荷機械の衝突を検出する負荷機械の制御装置の衝突検出しきい値更新方法において、前記負荷機械の通常行なう一連の動作パターンをＮ分割して各動作区間の動作をさせ、各動作区間における前記外乱推定値の最大値を衝突検出しきい値の初期値として設定する、前記初期値設定モード処理をし、前記動作パターンで動作させ、前記外乱推定値と前記初期値との比較に基づいて、衝突検出しきい値を自動的に更新する、前記通常動作モード処理をするのである。
また、請求項９に記載の発明において、請求項８記載の発明における前記初期値設定モード処理において、前記制御統括部が、前記動作パターンをＮ分割して各動作区間の動作を開始させると共に、前記サーボ制御部から現在の外乱推定最大値を保持し、前記サーボ制御部が、各動作区間における前記外乱推定値を推定してその最大値を前記現在の外乱推定最大値として前記制御統括部に渡し、前記通常動作モード処理において、前記制御統括部が、前記外乱推定値と現在の衝突検出しきい値とに基づいて前記現在の衝突検出しきい値を自動的に更新するのである。
また、請求項１０に記載の発明において、請求項８記載の発明における前記通常動作モード処理において、前記制御統括部が、更に、前記外乱推定値と前記現在の外乱推定最大値とに基づいて前記現在の衝突検出しきい値を自動的に更新するのである。
また、請求項１１に記載の発明において、請求項８記載の発明における前記初期値設定モード処理における前記外乱推定値の最大値は、各動作区間毎の動作を複数回させた際の各最大値を平均化したものである。

請求項１乃至３、または、８乃至１０に記載の発明によると、設定された衝突検出しきい値を容易に自動更新することができる。また、負荷機械に作用する摩擦等に経年変化が生じる場合、または、通常の動作パターンで動かした時の実際の外乱最大値と、最高速度で動作させた時の実際の外乱最大値との隔たりが大きい場合でも、衝突検出の感度を低下させることなく衝突検出することができ、信頼性を向上させることができる。
請求項４または５に記載の発明によると、各動作区間毎の衝突検出しきい値を設定すること、それを容易に自動更新することができ、各動作区間毎に大きく異なる実際の外乱最大値の場合でも、衝突検出の感度を低下させることなく衝突検出することができ、信頼性を向上させることができる。また、制御装置のメモリ容量または各動作区間毎の実際の外乱最大値に応じて衝突検出しきい値を統一することができ、制御装置のメモリ容量の節約ができる。
請求項６または１１に記載の発明によると、外乱推定最大値に、各動作区間毎の動作におけるノイズの影響を極力少なくすることができ、更に、衝突検出の感度を向上させることができ、一層の信頼性の向上を図ることができる。
請求項７に記載の発明によると、様々な負荷機械への適用をさせることができ、複数軸における複数軸毎の衝突検出しきい値を設定すること、それを容易に自動更新することができる。

以下、本発明の実施の形態について図を参照して説明する。

図１は、本発明の負荷機械の制御装置の構成を示す概略ブロック図である。図において、１は制御装置、１１は制御統括部、１２はサーボ制御部、１３はＰＷＭアンプ部、２はモータ及び連結された負荷機械、３はエンコーダ等の位置検出器である。また、１１１は衝突検出しきい値一時保管エリア、１１２は衝突検出しきい値設定エリア、１２１はサーボ制御演算部、１２２は補償演算部である。
本発明が従来技術と異なる部分は、制御統括部１１が衝突検出しきい値一時保管エリア１１１と衝突検出しきい値設定エリア１１２とを備え、これらエリアを用いて、予め設定した衝突検出しきい値を工作機械や産業用ロボット等の負荷機械の状態に応じて自動更新する部分である。

制御装置１は、制御統括部１１とサーボ制御部１２とＰＷＭアンプ部１３とで構成され、モータ２に連結された負荷機械を駆動させる。また、負荷機械に連結された複数のモータ２を駆動する場合は、単数の制御統括部１１と、複数のサーボ制御部１２とＰＷＭアンプ部１３とで構成、あるいは、複数の制御統括部１１とサーボ制御部１２とＰＷＭアンプ部１３とで構成される。

制御統括部１１は、負荷機械への教示データに基づく位置または速度指令を、通信等のインタフェースを介してサーボ制御部１２へ送信したり、サーボ制御部１２からのサーボ状態データを受信したり、教示プログラム中の命令を解釈して単数あるいは複数のサーボ制御を統括する。また、制御統括部１１は、負荷機械の各動作区間毎に、衝突検出しきい値一時保管エリア１１１と衝突検出しきい値設定エリア１１２とを有しており、公知の衝突検出方法で、負荷機械の各動作区間における外乱推定値が、それに対応する動作区間の衝突検出しきい値設定エリア１１２に設定されたしきい値を越えた場合に衝突として検出し、また、後述する衝突検出しきい値更新方法で、予め設定した衝突検出しきい値を工作機械や産業用ロボット等の負荷機械の状態に応じて自動更新する。ここで、公知の衝突検出方法は、例えば、従来技術のようなオブザーバで負荷機械に加わる外乱を推定し、この推定外乱が予め設定されたしきい値を越えることにより瞬時に衝突を検出する方法等を用いる。

サーボ制御部１２は、負荷機械を駆動させるべく、連結されたモータをサーボ制御する。また、サーボ制御部１２は、制御統括部１１からの位置または速度指令と、位置検出器３からの検出位置信号とに基づいてサーボ制御演算するサーボ制御演算部１２１と、サーボ制御に対するオブザーバ等の補償演算を行なう補償演算部１２２とを有する。
ＰＷＭアンプ部１３は、サーボ制御部１２からのトルクまたは電流指令に基づいてモータ２へ駆動電流を流す。

図２は、本発明の制御装置において、衝突検出しきい値を更新する処理手順を示すフローチャートである。この図を用いて本発明の方法を順を追って説明する。
（ステップ１）
制御統括部１１で教示プログラム内の命令を解釈し、教示プログラム内に記載された計測開始命令が解釈されれば、衝突検出しきい値を更新する一連の処理を開始し、先ずは衝突検出しきい値の初期値設定モードを開始し、ステップ２に進む。また、計測開始命令が解釈されなければ、ステップ１１（通常動作）に進む。
ここで、初期値設定モードは、教示プログラム内に記載された計測開始命令（例えば、ＣＬＳＨＯＣＫ ＃（Ａ））により開始され、計測終了命令（例えば、ＳＥＴＳＨＯＣＫ ＃（Ａ））で解除される。Ａは、負荷機械の通常行なう動作パターンを複数区間に分けた区間数を示し、例えば通常行なう動作パターンをＮ区間に分割した場合、Ａには１からＮまでの正の整数値が入ることになる。
なお、通常行なう動作パターンの分割数は、負荷機械が通常行なう一連の動作パターンを、ある作業毎にあるいは区切りの作業毎に分割すればよい。
（ステップ２）
制御統括部１１で教示プログラム内に記載された計測開始命令が解釈された後、その命令はサーボ制御部１２に渡され、サーボ制御部１２は計測開始命令を受けた後、補償演算部１２２における保持されている推定外乱最大値を一旦クリアし、各区間の動作パターンを開始し、ステップ３に進む。

（ステップ３）
サーボ制御部１２は、各区間の動作パターンの動作中に補償演算部１２２でオブザーバにより負荷機械に加わる外乱を都度推定して推定外乱最大値を保持し、ステップ４に進む。
ここで、各区間の動作パターンは複数回繰り返され、その繰り返し回数は、負荷機械の種類等に応じて適宜決定すればよい。また、推定外乱最大値は、その複数回で平均化された推定外乱最大値として保持すればよい。
（ステップ４）
制御統括部１１で常時行なわれる教示プログラム内の命令の解釈の中で、教示プログラム内に記載された計測終了命令が解釈された後、その命令はサーボ制御部１２に渡され、ステップ５に進む。また、計測終了命令が解釈されない間は、ステップ３，４を繰り返す。
（ステップ５）
サーボ制御部１２は、制御統括部１１からの計測終了命令を受けた後、各動作区間毎の推定外乱最大値を制御統括部１１に渡し、各動作区間毎の推定外乱最大値を受けた制御統括部１１は、各動作区間毎に衝突検出しきい値一時保管エリア１１１に推定外乱最大値を保持し、衝突検出しきい値の初期値設定モードを終了し、ステップ１１（通常動作）に進む。
ここで、サーボ制御部１２から制御統括部１１に渡される推定外乱最大値は、各動作区間毎の推定外乱最大値としたが、各動作区間中の最大である推定外乱最大値のみを渡してもよい。また、衝突検出しきい値一時保管エリア１１１に保持する推定外乱最大値は、各動作区間毎に保持するようにしたが、各動作区間中の最大である推定外乱最大値のみを保持してもよい。
なお、衝突検出しきい値設定エリア１１２に値が設定されていない、負荷機械を動作させる前の初期状態においては、衝突検出しきい値一時保管エリア１１１に保持された推定外乱最大値は、衝突検出しきい値設定エリア１１２に初期値として保持する。

（ステップ１１）
負荷機械が通常行なう一連の動作パターンでの動作を開始し、ステップ１２に進む。
（ステップ１２）
サーボ制御部１２は、動作パターンの動作中に補償演算部１２２でオブザーバにより負荷機械に加わる外乱を都度推定して推定外乱最大値を算出し、推定外乱最大値に安全係数を乗じた値が式（１）を満たしていればステップ１３に進み、満たしていなければステップ１７に進む。
Ａ×０．８ ≦ 推定外乱最大値×安全係数 ≦ Ａ×１．２ ・・・（１）
ここで、Ａは衝突検出しきい値設定エリア１１２に設定されている値である。
また、安全係数は、初期値設定モードで初期閾値設定後および動作モードでの閾値更新後の環境の変化や摩擦の変化等を許容するためものであり、閾値設定後から次の閾値更新までの間、常に同じ条件（温度や粘性等）で動作できるわけではなく、閾値設定後、外乱推定値がその閾値を超えない保証はできないため、次の閾値が更新されるまでの環境の変化等を吸収する安全係数が必要となるのである。
また、式（１）においては、外乱推定値が正常な範囲内かどうかを判断している。そもそも、本発明における閾値更新の目的は、摩擦等の経年変化分の吸収であるため、緩やかに衝突検出閾値が低下する状況を想定している。この観点から、閾値更新の範囲を式（１）のように設定しており、Ａが０．８倍以下の場合は全く動作しない場合等により閾値を誤って更新しないためであり、Ａが１．２倍以上の場合は、接触等でより大きな検出値となった場合、閾値を誤って設定し感度の低下を招かないためである。

（ステップ１３）
推定外乱最大値に安全係数を乗じた値が式（２）を満たしていればステップ１４に進み、満たしていなければステップ１７に進む。
Ｃ ≦ 推定外乱最大値×安全係数 ・・・（２）
ここで、Ｃは衝突検出しきい値一時保管エリア１１１に保持されている値である。
また、安全係数は式（１）におけるものと同じであり、式（２）においては、閾値更新のための動作パターンを規定回数動作させた中での最大値を判断している。
なお、あくまでも閾値更新の可否を判断する条件式は式（１）となる。式（２）では、その上で動作パターンを規定回数動作させた中での最大値で更新を行っているものである。粘性摩擦等の経年変化を考えた場合、緩やかに閾値が低下していくと考えられるが、１回動作しただけで、毎回閾値を更新していくと、誤って低すぎる値に更新してしまう可能性も大きくなる。そのため、規定回数動作させ、その最大値でも従来の閾値の例えば０．９倍の値となれば、その値に閾値を更新するようにしている。

（ステップ１４）
サーボ制御部１２が、推定外乱最大値に安全係数を乗じた値を制御統括部１１に渡し、制御統括部１１では、衝突検出しきい値一時保管エリア１１１に保持されている値を、推定外乱最大値に安全係数を乗じた値に更新し、ステップ１５に進む。
（ステップ１５）
先の初期値設定モードにおける複数回繰り返される各区間の動作パターンの、通常動作モードにおける動作回数を制御統括部１１においてカウントしておき、この動作回数が予め規定されている規定値に達したかどうかを判断し、達した場合はステップ１６、達していない場合はステップ１２，１３，１４を繰り返す。
ステップ１２およびステップ１３の説明に記載した通り、基本的に、式（１）を満足していれば閾値を更新するが、１回のみの動作であると、誤った測定値でも式（１）を満足した場合、閾値として更新されてしまうため、この現象を防止するために、規定回数動かした時の式（１）を満足した中での最大値で更新するようにしている。よって、予め規定されている規定値は、適宜決定すればよいのであるが、大体の目安として３〜１０回程度だと考えられる。

（ステップ１６）
制御統括部１１の衝突検出しきい値一時保管エリア１１１に保持されている値を、衝突検出しきい値設定エリア１１２に更新し、衝突検出しきい値を更新する一連の処理を終了する。
最終的に、この衝突検出しきい値設定エリア１１２に設定された値が、公知の衝突検出方法における負荷機械の各動作区間における外乱推定値との比較に用いられ、衝突検出を行なう。
（ステップ１７）
先の初期値設定モードにおける複数回繰り返される各区間の動作パターンの、通常動作モードにおける動作回数を制御統括部１１においてカウントしておき、この動作回数が予め規定されている規定値に達したかどうかを判断し、達した場合は衝突検出しきい値を更新する一連の処理を終了し、達していない場合はステップ１２からの処理を繰り返す。

次に、負荷機械を産業用ロボットとした場合の教示プログラムの一例を示す。前述の通り、教示プログラムは、制御統括部１１において、その命令を一つひとつ解釈してサーボ制御部１２に渡されるものである。
負荷機械の通常行なう一連の動作パターンを教示プログラムで示すと、次のように示される。
ＮＯＰ
（１）ＭＯＶＪ Ｖ＝５０ （％）
（２）ＭＯＶＪ Ｖ＝５０ （％）
（３）ＭＯＶＬ Ｖ＝２０ （ｍｍ／ｓ）
（４）ＭＯＶＬ Ｖ＝２０ （ｍｍ／ｓ）
（５）ＭＯＶＪ Ｖ＝５０ （％）
（６）ＭＯＶＬ Ｖ＝２０ （ｍｍ／ｓ）
（７）ＭＯＶＬ Ｖ＝２０ （ｍｍ／ｓ）
（８）ＭＯＶＪ Ｖ＝５０ （％）
ＥＮＤ

上記の教示プログラム中、「ＮＯＰ」はプログラムの開始、「ＥＮＤ」はプログラムの終了、「ＭＯＶＪ」はロボットの各軸を独立に指定した速度で動作させる命令、「ＭＯＶＬ」はロボット先端の作業点を指定した速度で直線動作させる命令である。また、（１）から（８）はそれぞれ、ロボットの教示ポイントであり、（１）はロボットの退避点、（２）および（５）は作業開始点、（３）および（６）は作業中間点、（４）および（７）は作業終了点、（８）は（１）と同一点であってロボットの退避点あり、上記の教示プログラムは各ポイントを教示したものである。
例えば、（２）「ＭＯＶＪ Ｖ＝５０」はロボットの各軸を、独立に定格の５０％速度で作業開始点（２）に動作させることを意味し、（６）「ＭＯＶＬ Ｖ＝２０」はロボット先端の作業点を、２０ｍｍ／ｓの速度で作業中間点（６）に直線動作させることを意味する。

次に、上記の教示プログラムに対し、本発明の衝突検出しきい値更新方法を適用した場合の教示プログラムを示すと、次のように示される。
ＮＯＰ
ＣＬＳＨＯＣＫ ＃（１）
（１）ＭＯＶＪ Ｖ＝５０ （％）
（２）ＭＯＶＪ Ｖ＝５０ （％）
（３）ＭＯＶＬ Ｖ＝２０ （ｍｍ／ｓ）
（４）ＭＯＶＬ Ｖ＝２０ （ｍｍ／ｓ）
ＳＥＴＳＨＯＣＫ ＃（１）
ＣＬＳＨＯＣＫ ＃（２）
（５）ＭＯＶＪ Ｖ＝５０ （％）
（６）ＭＯＶＬ Ｖ＝２０ （ｍｍ／ｓ）
（７）ＭＯＶＬ Ｖ＝２０ （ｍｍ／ｓ）
（８）ＭＯＶＪ Ｖ＝５０ （％）
ＳＥＴＳＨＯＣＫ ＃（２）
ＥＮＤ

これは、負荷機械の通常行なう一連の動作パターンを２分割した場合の一例であり、「ＣＬＳＨＯＣＫ ＃（１）」は初期値設定モードにおける教示プログラム内に記載された計測開始命令、「ＳＥＴＳＨＯＣＫ ＃（１）」は初期値設定モードにおける教示プログラム内に記載された計測終了命令である。
図２に示した、本発明の制御装置において、衝突検出しきい値を更新する処理手順を示すフローチャートに従い、各動作区間の推定外乱最大値を推定、保持し、最終的に衝突検出しきい値設定エリア１１２に設定された値を自動更新し、公知の衝突検出方法における負荷機械の各動作区間における外乱推定値との比較に用いられ、衝突検出を行なうのである。

本発明の負荷機械の制御装置の構成を示す概略ブロック図 本発明の制御装置において、衝突検出しきい値を更新する処理手順を示すフローチャート

符号の説明

１ 制御装置
２ モータ及び連結された負荷機械
３ エンコーダ等の位置検出器
１１ 制御統括部
１２ サーボ制御部
１３ ＰＷＭアンプ部
１１１ 衝突検出しきい値一時保管エリア
１１２ 衝突検出しきい値設定エリア
１２１ サーボ制御演算部
１２２ 補償演算部

Claims (11)

1. モータの位置または速度制御を統括する制御統括部と、前記制御統括部からの指令と位置検出器からの検出位置信号に基づいてサーボ演算するサーボ制御部と、前記モータへ駆動電流を供給するＰＷＭアンプ部とを備え、前記位置検出器を備えた前記モータに連結された負荷機械を制御する負荷機械の制御装置であって、オブザーバにより前記モータまたは前記負荷機械に加わる外乱を推定し、その外乱推定値と予め設定された衝突検出しきい値との比較に基づいて前記負荷機械の衝突を検出する負荷機械の制御装置において、
   前記制御装置が、衝突検出しきい値の初期値を設定する初期値設定モードと、前記負荷機械の通常行なう一連の動作パターンで動作させる通常動作モードとを有し、
   前記初期値設定モードで、前記動作パターンをＮ分割して各動作区間の動作をさせ、各動作区間における前記外乱推定値の最大値を前記初期値として設定し、
   前記通常動作モードで、前記動作パターンで動作させ、前記外乱推定値と前記初期値との比較に基づいて、衝突検出しきい値を自動的に更新することを特徴とする負荷機械の制御装置。
2. 前記初期値設定モードにおいて、前記動作パターンをＮ分割して各動作区間の動作を開始させると共に、前記サーボ制御部から現在の外乱推定最大値を保持する前記制御統括部と、
   各動作区間における前記外乱推定値を推定してその最大値を前記現在の外乱推定最大値として前記制御統括部に渡す前記サーボ制御部とを備え、
   前記通常動作モードにおいて、前記制御統括部が、前記外乱推定値と現在の衝突検出しきい値とに基づいて前記現在の衝突検出しきい値を自動的に更新することを特徴とする請求項１に記載の負荷機械の制御装置。
3. 前記制御統括部が、更に、前記外乱推定値と前記現在の外乱推定最大値とに基づいて前記現在の衝突検出しきい値を自動的に更新することを特徴とする請求項１に記載の負荷機械の制御装置。
4. 前記サーボ制御部が前記制御統括部へ渡す前記現在の外乱推定最大値は、前記初期値設定モードにおける各動作区間毎の推定外乱最大値、あるいは、各動作区間中の最大である１つの推定外乱最大値であることを特徴とする請求項２に記載の負荷機械の制御装置。
5. 前記制御統括部が保持する前記現在の外乱推定最大値は、前記初期値設定モードにおける各動作区間毎の推定外乱最大値、あるいは、各動作区間中の最大である１つの推定外乱最大値であることを特徴とする請求項２に記載の負荷機械の制御装置。
6. 前記初期値設定モードにおける前記外乱推定値の最大値は、各動作区間毎の動作を複数回させた際の各最大値を平均化したものであることを特徴とする請求項１に記載の負荷機械の制御装置。
7. 前記制御装置が、単数の前記制御統括部と複数の前記サーボ制御部と、あるいは、複数の前記制御統括部と複数の前記サーボ制御部と、を備えることを特徴とする請求項１記載の負荷機械の制御装置。
8. モータの位置または速度制御を統括する制御統括部と、前記制御統括部からの指令と位置検出器からの検出位置信号に基づいてサーボ演算するサーボ制御部と、前記モータへ駆動電流を供給するＰＷＭアンプ部とを備え、前記位置検出器を備えた前記モータに連結された負荷機械を制御する負荷機械の制御装置の衝突検出しきい値更新方法であって、オブザーバにより前記モータまたは前記負荷機械に加わる外乱を推定し、その外乱推定値と予め設定された衝突検出しきい値との比較に基づいて前記負荷機械の衝突を検出する負荷機械の制御装置の衝突検出しきい値更新方法において、
   前記負荷機械の通常行なう一連の動作パターンをＮ分割して各動作区間の動作をさせ、各動作区間における前記外乱推定値の最大値を衝突検出しきい値の初期値として設定する、前記初期値設定モード処理をし、
   前記動作パターンで動作させ、前記外乱推定値と前記初期値との比較に基づいて、衝突検出しきい値を自動的に更新する、前記通常動作モード処理をすることを特徴とする負荷機械の制御装置の衝突検出しきい値更新方法。
9. 前記初期値設定モード処理において、前記制御統括部が、前記動作パターンをＮ分割して各動作区間の動作を開始させると共に、前記サーボ制御部から現在の外乱推定最大値を保持し、
   前記サーボ制御部が、各動作区間における前記外乱推定値を推定してその最大値を前記現在の外乱推定最大値として前記制御統括部に渡し、
   前記通常動作モード処理において、前記制御統括部が、前記外乱推定値と現在の衝突検出しきい値とに基づいて前記現在の衝突検出しきい値を自動的に更新することを特徴とする請求項８に記載の負荷機械の制御装置の衝突検出しきい値更新方法。
10. 前記通常動作モード処理において、前記制御統括部が、更に、前記外乱推定値と前記現在の外乱推定最大値とに基づいて前記現在の衝突検出しきい値を自動的に更新することを特徴とする請求項８に記載の負荷機械の制御装置の衝突検出しきい値更新方法。
11. 前記初期値設定モード処理における前記外乱推定値の最大値は、各動作区間毎の動作を複数回させた際の各最大値を平均化したものであることを特徴とする請求項８に記載の負荷機械の制御装置の衝突検出しきい値更新方法。

Images