WO1990001732A1

WO1990001732A1 - Impingement detection/driving stop method in machine driven by servo motor

Info

Publication number: WO1990001732A1
Application number: PCT/JP1989/000821
Authority: WO
Inventors: Nobutoshi Torii; Yasuo Naito; Masayuki Hamura; Kazuhisa Otsuka
Original assignee: Fanuc Ltd
Priority date: 1988-08-11
Filing date: 1989-08-11
Publication date: 1990-02-22
Also published as: JPH02146602A; EP0380707A1; US5214362A; EP0380707A4; KR900702425A

Description

明細害

サーボモータにより駆動される機械における

衝突検出 ♦ 駆動停止方法

技術分野

本発明は、サーボモータにより駆動される機械作動部と異物等との衝突を迅速に検出可能でかつ衝突検出時に機械作動部の駆動を迅速に停止可能な衝突検出 ♦ 駆動停止方法に関する。

背景技術

機械の運転時、機械作動部とその作動領域に入り込んだ異物とが干渉し、機械の正常運転状態が阻害されあるいは機械が破損するに至ることがある。従来、種々の方法でこれを防止している。

たとえば、機械作動部の外面に取り付けたタツチセンサを用いて機械作動部と異物との衝突を検出している。しかし、センサの設置に伴って機械がコスト髙になり、しかも、センサの作動領域外における機械作動部と異物との衝突を検出できない。また、機械作動部をサーボモ一夕で駆動するタイブの機械において、機械作動部と異物との衝突に伴いサーポモータ駆動電流が所定の判別値を越えたとき両者の衝突を検出することが知られている。しかしながら、衝突時点からサーボモータ駆動電流が実際に当該判別値に到達するまでに時間を要するので、衝突検出上、時間的な遅れが生じ、衝突による被害が大きくなる。さらに、センサ出力あるいはサーボモータ駆動電流の検出値に基づいて内蔵のコンピュータで衝突判別処理を行うタイブの機械たとえば産業用ロポットが知られ、この種の機械は、衝突判別処理をロボット制御用コンビュータで実行するようになっている。すなわち、衝突判別処理と口ボット制御とを同一のコンピュータで実行している。ロボット制御は各種の複雑な情報処理を含み、従って、コンピュータの制御実行周期および衝突判別処理実行周期は比較的大きいものにならざるを得ず、結果として、衝突時点から衝突判別処理が完了するまでに時間を要し、衝突検出上の時間的な遅れがさらに増大する。

発明の開示

本発明の目的は、サーボモータにより駆動される機械作動部と異物等との衝突を迅速に検出可能かつ衝突検出時に檨械作動部の駆動を迅速に停止可能で、衝突による機械の破損等を防止もしくは軽減可能な衝突検出 ♦ 駆動停止方法を提供することにある。

上述の目的を達成するため、本発明の、サーボモータにより駆動される機械における衝突検出 ♦ 駆動停止方法は、サーポモータの駆動力に閬連する衝突判別パラメ一タの値を周期的に検出する工程（ a ) と、前記衝突判別パラメータ値の変化高を周期的に検出する工程（ b ) と、前記衝突判別パラメータ値の変化高が判別基準値を越えたとき衝突発生を判別する工程（ c ) と、衝突発生を判別したときモータ回転が停止するように前記サーボモータを制御する工程（ d ) とを備える。

本発明によれば、衝突判別パラメータ値の変化高が判別基準値を越えたとき、サーボモータにより駆動される機械における衝突発生を検出しかつサーボモータの回転を停止させるようにしたので、特別のセンサを用いずに衝突発生を迅速に検出しかつ機械の駆動を迅速に停止でき、結果として衝突による機械の破損等を防止もしくは軽減できる。 ''

図面の簡単な説明

第 1 図は本発明の第 1 の実施例による衝突検出 ♦ 駆動停止方法を実施するための制御ブログラムのフローチヤート、第 2図は第 2の実施例についての第 1 図と同様の図、第 3図は第 3の実施例についての第 1 図と同様の図、第 4図は第 4の実施例についての第 1 図と同様の図、第 5図は第 5の実施例についての第 1 図と同様の図、第 6 図は第 1 の実施例の方法が適用される産業用ロボットの要部を示す概略ブロック図、第 7図は第 6図のロボットのひとつの軸に閬連するサーボ系の制御特性を伝達関数を用いて示したブロック線図、および、第 8図は第 3の実施例の方法で用いられる衝突判別基準値を格納したルックアップチープルを示す図である。

発明を実施するための最良の形態

第 6図を参照すると、本発明の第 1 の実施例の方法が適用される産業用ロボットは、基本的には従来公知のように構成され、各種センサ（図示省略）を有する作業対象 1 4に対して所要の作業を行うためのロボット機構部 1 3と、該ロボット機構部 1 3を制御するための、後述の各種要素よりなる制御装置と、各種センサ（図示省略）とを備えている。

ロボット機構部 1 3は、アームを構成する一連のリンクおよびアーム先端に装着したエンドエフヱクタ等の作動部を有し、作動部の各々（その内のひとつを符号 1 3 aで示す）は、各該作動部の一つ以上の軸の各々に関してこれに対応するァクチユエータにより直線的に駆動ざれあるいは回転駆動されるようになっており、夫々のァクチユエータは好ましくは速度検出器 1 3 cを付設したサーボモータ（その内のひとつを符号 1 3 bで示す）より成る。

制御装置はロボット制御甩の主コンビュ一夕 1 を備え、主コンピュータ 1 は、マイクロプロセッサ，該マイクロプロセッサが実行する制御プログラムを格納した R O M, データの一時記憶等に利用される R A M等により構成され、 tiボットの各軸についての移動指令などの動作指令を送出するようになっている。

軸制御器 2は、ロボットの各軸のサーボモータおよびこれに鬨連するサーボアンプ（その内のひとつを符号 1 2で示す〉と協働して各軸のデジタルサーボ系すなわちソフトウェアサーボ系を構成している。すなわち、軸制御器 2は、第 7図に示す位置制御樣能 2 1，比例制御機能 2 2および積分制御機能 2 3， 2 5を備えるデジタルシグナルプロセッサ（図示省略）を有し、サーボモータと協働して第 7図に示す制御特性 2 4を現出させるサーボアンブに、主コンピュータ 1 からの移動指令に応じたトルク指令を送出するようになっている。第 7図において、符号 K p， Κ Vおよび Κ 1 は軸制御器 2の位 gルーブゲイン，比例ゲインおよび積分ゲインを、符号 K t はサーボモータのトルク定数を、符号 J はサーボモータの出力軸に加わる全イナ一シャをそれぞれ表す。そして、軸制御器 2は、後で詳述する衝突判別基準値 E R R , デジタルシグナルプロセッサのサーボ制御上の動作モードを表すフラグ情報 Fおよび衝突判別のための速度偏差情報 ε ν ，を記憶するためのメモリ（レジスタ〉を有している。

また、制御装置は、 Πポットの各種センサおよび各種ァクチユエータならびに作業対象 1 4の各種センサに接続された I / 0ユニット 3と、教示操作盤 7および操作パネル 9に接続されたシリアルポート 4と、 ) ッテリ 8 によりバックアップされたデータメモリ用 R A Μ 5と、入力トランス 1 1 および入力ユニット 1 0を介して外部電源（図示略）から辁電される電源回路 6 とを有し、教示操作盤 7および操作パネル 9には表示部（図示略）が夫々設けられている。そして、電源回路 6から主コンビユータ 1 およびこれに接続された上述の要素 2〜 5への耠電をバックパネル 1 6を介して行うと共に、入力ュニット 1 0を介してロボット機構部 1 3に耠電し、また、入力ユニット 1 0およびサーボトランス 1 5を介してサ一ボアンブ 1 2に辁電するようにしている。

以下、第 1図を参照して、上述のように構成したロボットの動作を説明する。

ロボットの運乾に先立ち、第 6図の制御装匱が初期化され、軸制御器 2のメモリ内のフラグ情報 Fが「 0」にリセットされる。その後、オペレータが教示操作盤 7および操作パネル 9を操作してロボットへの教示を従来公知のように行うと、教示データがデータメモリ用 R A M 5に格納される。又、オペレータは、操作パネル 9を操作して衝突判別基準値 E R Rを軸制御器 2のメモリ（レジスタ）に格納する。この値 E R Rは、正常な口ポット運転状態において、軸制御器 2の 1サ-ポ制御動作実行周期に対応する期間内に発生し得る速度偏差の最大変化高よりも所定値だけ大きい値に設定される。

ロボットの運転時、主コンピュータ 1 は所定の周期で各種制御動作を順次実行する。この制御動作には、例えば、データメモリ用 R A M 5に格納された教示データに従ってロボットの各軸に対する移動指令を演算する処理が含まれる。

軸制御器 2のデジタルシグナルブロセッサは、主コンビュータ 1 における制御動作実行処理周期より短い周期でロボットの各軸についての位！;，速度およびトルク制御動作（サーボ制御動作）ならびに衝突判別動作を実行する。すなわち、デジタルシグナルプロセッサは、或る制御動作実行周期において、軸制御器 2のメモリ内のフラグ Fが「 0 J にリセットされているか否かを判別する（第 1図のステップ S 1 ) 。ここではフラグ Fがリセットされており、ステップ S 1 の判別結果が否定（ N O) となる。そこで、デジタルシグナルブロセッサは、今回の制御動作実行周期に閬連するサーボモータたとえばサーボモータ 1 3 bに装着した速度検出器 1 3 cからの実際モータ回転速度 Vを表す出力を、第 7図に符号 25で示すように積分してサーボモータ 1 3 bの実際回転位置 P f を算出する。次いで、主コンピュータ 1からの、当該サーボモータに関連する指令信号が示す指令位置 P c と実際位 gP f との差（位箧偏差） ε ρ'に応じた指令速度 V cを算出する。そして、デジタルシグナルプロセッサは、第 7図に符号 2 6で示すように、算出指令速度 V c と実際モータ速度 Vとの差（速度偏差） ε νを算出する（スチップ S 2) 。次に、斯く箅出した値 ε Vから前回のサ -ボ制御動作実行周期での箅出速度偏差 ε ν ' (後述）を減じて得た 1制御周期に対応する期間内での速度偏差の変化高の絶対値 I ε V — ε V， I が衝突判別基準値 Ε R Rより小さいか否かを判別する（ステップ S 3 )。

スチップ S 3での判別結果が肯定（ Y E S ) であれば、デジタルシグナルプロセッサは正常なロボット運転状態にあると判別して通常の制御動作を実行する。即ち、第 7図に符号 23で示すように、速度偏差 ε ν ( = V c - V ) と積分ゲイン K 1 との積値を箅出し、当該積値 K 1 ♦ ( V c - V ) をアキュムレータの値 Αに加算する（スチップ S 4〉。次に、第 7図に符号 2 2で示すように、速度偏差 ε ν ( = V c - V ) と比例ゲイン Κ νとの積値を算出し、当該箅出値 Κ ν * ( V c - V ) をアキュムレ一タの値 Αに加算してトルク指令値 T q c を求め、この値 T q c を D Z A変換してサーボアンブ 1 2に出力する (ステップ S 5， S 6 ) 0

サーボアンブ 1 2は、このトルク指令値 T q cに応じた駆動電流をサーボモータ 1 3 に流して該サーボモータを速度 Vで回転させ、ロボット作動部 1 3 aを駆動する。デジタルシグナルプロセッサは、指令速度 V c と実際速度 Vとの偏差 V c— Vを次の制御周期での衝突判別ステップ S 3で用いる速度偏差 ε v，としてレジスタに格納し（ステップ S 7 ) 、今回の制御同期での処理を終了する。その後、上述の処理を所定の制御周期で繰り返し実行する。

上記サーボ制御に従うロボット逞耘中、ロボット作動部 1 3 aが例えば該作動部の作動領域内に入り込んだ異物と衝突すると、サーポモータ 1 3 bの実際回転速度 V が低下しまたはモータ回転が停止するに至る。斯かる場合、衝突発生直後のサーボ制御動作実行周期でのスチップ S 2において算出される速度偏差 ε V ( = V c - V ) が急増し、当該制御周期のステップ S 3において、前回の衝突判別時点から今回判別時点までの期間内での速度偏差の変化高の絶対値 I ε V — ε V， I が衝突判別基準値 E R Rよりも大きいと判別される。この場合、デジ夕ルシグナルプロセッサは、衝突発生と判別してフラグ F を値「 1 」にセットすると共に、衝突発生を告知するメッセージを例えば操作パネル 9の表示部に表示する（スチップ S 8 ) 。次に、トルク指令値 T q c を Γ 0 J に設定し（ステップ S 9 ) 、スチップ S 6に移行する。この結果、軸制御器 2からサーボアンブ 1 2に値「 0 J のトルク指令が送出され、サーボモータ 1 3 に駆動電流が流れなくなり、モータ回転が停止する。デジタルシグナルプロセッサは、上記速度偏差格納ステップ S 7をさらに実行して今回制御周期での処理を終了する。

今回制御周期のスチップ S 8でフラグ Fが値「 1 」にセットされたことから、次回およびそれ以降のサーボ制御周期ではステップ S l， S 9 , S 6および S 7よりなる'処理が繰り返し実行され、モータ回転停止状態が維持される。

.以上において、ひとつのロボット制御軸に関して作動説明したが、その他の軸についても同様である。

次に、第 2図を参照して、本発明の第 2の実施例による衝突検出 ♦ 停止方法を説明する。

この実施例の方法は、速度偏差の変化高に基づいて衝突判別を行う上記第 1 の実施例の方法と比較してトルク指令の変化高に基づいて衝突判別を行う点が相違する。なお、本実施例および後述の実施例の方法は例えば第 6 図のロボットに適用される。

正常なロボット運転時、軸制御器 2のデジタルシグナルプロセッサは、サーボ制御動作実行周期の各々においてステップ S 1 0 0でフラグ Fが値 Γ 1 J でないと判別し、第 1 図のステップ S 2の場合と同様の処理によって速度偏差 ε V ( = V c— V ) を求める。次いで、スチッブ S 4， S 5に夫々対応するトルク指令値算出スチップ S 1 0 1 , S 1 0 2を実行する。そして、この結果得たドルク指令値 T q cから前回のサーボ制御周期でのトルク指令値 T q c ' を減じてトルク指令値の変化高を求め、その絶対値 I T q c— T q c， I が衝突判別基準値 E R Rよりも小さいか否かを判別する（スチップ S 1 0 3 )。この値 E R Rは、正常なロボット運転状態において 1サーボ制御動作実行周期に対応する期間内に発生し得るトルク指令の最大変化高よりも所定値だけ大きい値に設定され、ロボヅト運転前に軸制御器 2内のレジスタに予め格納されている。ステップ S 1 0 3での判別結果が肯定ならば、正常なロボット運転状態にあると判別してステッブ S 6に対応するトルク指令値算出ステッブ S 1 0 6 を実行した後、ステップ S 1 0 2で算出した値 T q cを、次の制御周期での衝突判別処理（ S 1 0 3 ) のための値 T q c ，としてレジスタに格納し（ステップ S 1 0 7 )、今回周期での処理を終了する。

ロボット運転中、ロボット作動部 1 3 aが異物と衝突すると、サーボモータ 1 3 bの実際回転速度 Vが低下しまたはモータ回転が停止するに至る。斯かる場合、衝突直後の制御周期で箅出される速度偏差 ε V ( = V c - V ) が急増し、従って、当該制御周期のステップ S 1 0 2において箅出されるトルク指令値 T q cが急増する。この結果、衝突判別ステップ S 1 0 3の判別結果が否定になる。デジタルシグナルプロセッサは、スチップ S 8および S 9に夫々対応するステップ S 1 0 4および S 1 0 5 を実行して衝突発生を聱告すると共にサーボモータの回転を停止させる。そして、次回およびそれ以降の制御周期においてモータ回転停止状態を維持する。

次に、第 3図および第 8図を参照して、本発明の第 3 の実施例による衝突検出 ♦ 停止方法を説明する。

この実施例は基本的には上記第 1 の実施例と同一である。但し、衝突判別基準値を指令速度に応じて可変設定する点と、衝突発生時に実際モータ速度と大きさが比例しかつ実際モータ回転方向と逆方向に作用するトルク指令を送出する点とが第 1 の実施例と異なる。

このため、軸制御器 2のメモリは、 N + 1個の指令速度区間の夫々に対応する衝突判別基準値が格納されたルックアップチープル T a bを備えている。第 8図に示すように、テーブル T a bは N + 1 個のアドレス I ( = 0， 1， ♦ ♦ ♦ , N ) を備え、第 0番目のアドレスは指令速度 V c が「 0 J である第 0番目の指令速度区間に対応し、当該ァドレスには衝突判別基準値 E R R oが格納されている。値 E R R 0はサーボモータ駆動開始時の衝突判別に用いられる。そして、第 i番目（ i = 1， 2 , · ♦ ♦， N ) のアドレスは、指令速度 V cの絶対値が（ i 一 1 ) ♦ V c max/ Nよりも大きくかつ i * V c max/ N以下の値をとる第 i番目の区間に対応し、このアドレスには基準値 E R R iが格納されている。ここで、 V c maxは最大指令速度を表し、 Nは指令速度区間の数から Γ ΐ 」を減じた整数値である。値 E R R iは、 E R R iく E R R i + 1 なる関係が成立するように設定され、指令速度が大きいほど大きな速度偏差変化を許容するようにしている。換言すれば、衝突時のモータ回転速度が大きいほどモータ回転速度の婊少変化高が大きい点を考慮して、衝突発生判別上の誤検出を防止しつつ衝突発生を可能な限り迅速に検出可能なようにしている。

第 3図を参照すると、ロボット運転時、軸制御器 2のデジタルシグナルブロセッサは、第 1図のステップ S 1 〜 S 5に夫々対応しかつ衝突判別，トルク指令値算出などに関連するスチッブ S 200〜 S 204を順次実行し、その結果得たトルク指令値 T q cをサーポアンプ 1 2に送出する。そして、ステップ S 7に対応する速度偏差格納ステップ S 20 5を実行した後、指令速度 V cが N + 1個の指令速度区間のいずれに属するかを判別する（スチヅブ S 206 ) 。すなわち、デジタルシグナルブロセッサは、指令速度 V cを演算定数 K ( = V c max/N) で除して値 V c /Kを求め、次いで、箅出値 V c /Kの小数 1位以下の端数を切上げて整数を求め、さらに、その絶対値（以下、 1 ^[丁 1 ¥ (：ノ 1{ | とぃぅ）を算出する。値 I N T I V c / K I は、指令速度 V cが属する指令速度区間に対応するルックアップチープルのァドレス I ( 0， 1 , ♦ ♦ ♦， N ) を表す。即ち、下記第（ 1 〉式が成立する。

I = I N T I V c / K I

次に、デジタルシグナルプロセッサは、ルックァッブテーブル T a bの第 I番目のァドレスから所要の衝突判別基準値 E R R i ( i = 0， 1 ， N) を読出し、この判別基準値を軸制御器 2の所定のレジスタに格納する（ステップ S 2 06 ) 。

ロボット運転中、衝突判別ステップ S 2 02での判別結果が否定であると、デジタルシグナルプロセッサは、ステップ S 8に対応するステップ 207においてフラグ Fを値「 1」にセットすると共にアラーム表示を行う。上述のようにステップ S 202で用いられる衝突判別基準値 E R Rは指令速度に依存して変化する最大許容モータ速度減少変化高に対応し、従って、衝突判別は的確かつ迅速に行われる。次いで、ステップ S 2 08において指令速度値 V cおよびアキュムレータの値 Aを夫々「 0」にセットした後、上述のステップ S 204に移行してトルク指令 T q C を箅出する。この箅出トルク指令 T q c は下記第（ 2) 式で表される。

T q c =— V * K v ♦ » ♦ ( 2 ) 次に、デジタルシグナルプロセッサは、上述の速度偏差格納/衝突判別基準値選択ステップ S 2 0 5 , S 2 0 6を実行して今回の制御周期での処理を終了し、次回及びそれ以降の制御周期において第（ 2 ) 式に示すトルク指令値 T q cを送出する。この様に、衝突判別後は、実際サーボモータ速度 Vに比例する大きさを有しかつ実際モータ回転方向と逆方向に作用するトルク指令値 T q c がサーボアンブ 1 2に送出される。この結果、サーボモータ 1 3 bが急激にかつ円滑に制動されて短時間内に回転停止し、ロボット作動部 1 3 aが駆動停止する。なお、衝突発生以降の実際モータ速度の減少に伴ってトルク指令値 T q cは潮減する。

" 次に、第 4図を参照して、第 4の実施例による衝突検出 ♦停止方法を説明する。

この実施例は基本的には上記第 2の実旛例と同一である。但し、衝突判別基準値の大きさを指令速度に応じて変化させる点と、衝突発生時に実際モータ速度と大きさが比例しかつ実際モータ回転方向と逆方向に作用するトルク指令を送出する点とが第 2の実施例と異なり、また、これらの点で第 3の実施例に類似している。このため、第 8図に示すルックアップチープル T a b に類似の、 N + 1個の指令速度区間の夫々に対応しかつ指令速度大きいほど大きい値をとる衝突判別基準値が格納されたチ一プル（図示略）を用いる。但し、本実施例における衝突判別基準値は、 1制御周期に対応する期間内でのトルク指令の許容変化高を表すもので、速度偏差の許容変化高を表す第 8図の衝突判別基準値とは大きさが異なる。第 4図を参照すると、正常なロボット運転時、デジタルシグナルブロセッサは、第 2図のスチッブ S 1 00〜 S 1 0 3, S 1 0 6および S 1 0 7に夫々対応しプロセッサ動作モードの確認，トルク指令値の算出，衝突判別およびトルク指令値の格納に係るスチップ S 300〜 S 303, S 30マおよび S 308と、第 3図のステップ S 20 6に対応し衝突判別基準値の選択に係るステップ S 30 9をこの順序で実行する。

ロボット運転中に衝突が発生すると、第 2図のステツブ S 1 0 3に対応する衝突判別ステップ S 303での判別結果が否定になり、デジタルシグナルブロセッサは、第 2図のステップ S 1 04に対応するプロセッサ動作モ一ド切換アラーム表示ステップ S 304を実行した後、第 3図のステップ S 20 8および S 204に夫々対応し衝突発生時のトルク指令値算出に係るステップ S 305 および S 306を実行し、さらに、上述のステップ S 3 07〜 S 309を実行する。この結果、第 3の実旅例の場合と同様、衝突判別後は、実際サーボモータ速度 Vの大きさに比例する大きさを有しかつ実際モータ回転方向と逆方向に作用するトルク指令値 T q cに従ってサーポモータ 1 3 bが急制動ざれる。

次に、第 5図を参照して、第 5の実施例による衝突検出 ♦ 停止方法を説明する。

この実施例は基本的には上記第 1の実施例と同一である。但し、第 6図の制御装置によりプログラムから読み出される動作指令の種別に応じて衝突判別基準値の大きさを変化させる点と、衝突発生時に第 6図の制御装置の誤差過大ァラーム機能を無効にするようにした点とが第 1 の実施例と異なる。

このため、本実施例では、第 8図のチープル T a bに類似の、所要数の衝突判別基準値が格納されたルックァップチ一プル（図示略）を用いている。そして、動作指令の種別、例えば、直線補間を表す G 0 1 コード，円弧補間を表す G 0 2コード等の夫々に対応づけて衝突判別基準値を設定し、実行中の動作指令の種別に依存して変化する棲械作動部の動作状態に即した最適な衝突判別を行えるようにしている。また、本実施例において衝突判別時に誤差過大アラーム檨能を無効にする理由は、サーポ制御系での制御パラメータである位 g傭差（指令位置と実際位箧との差〉が過大になったときにサーポモータの励磁を解くように作用する誤差過大アラーム檨能が位 g偏差が増大する衝突発生時に作動可能であり、斯かる場合にはサーボモータを軸制御器 2の制御下で作動させることができなくなり、衝突検出以降にサーポモータの回転停止制御を行う本発明の方法を実施する上で障害となり得るからである。

本実施例が適用されるロボットの作動は第 1，第 3の実施例の場合と略同様で、ロボット正常運転時、第 1図のステップ S 1 〜 S 7に夫々対応するスチップ S 5 0 0 , S 5 0 2〜 S 5 0 7が実行される。但し、衝突判別ステッブ S 5 0 3に先立つステップ S 5 0 1 において、今回制御周期に関連する動作指令の種別に対応する衝突判別基準値 E R Rをメモリから読み出す点が異なる。そして、衝突発生時にはステップ S 5 0 8においてロボット制御装置の誤差過大アラーム機能を無効にし、衝突発生後においてもサーボモータを軸制御器 2の制御下で作動させる。また、衝突発生直後は第 1 図のステップ S 8, S 9. S 6及び S 7に夫々対応するステップ S 5 0 9， S 5 1 0， S 5 0 6及び S 5 0 7を、その後はステップ S l，

S 9, S 6および S 7に夫々対応するスチップ S 5 0 0: S 5 1 0, S 5 0 6および S 5 0 7を実行する。

本発明は上述の第 1 〜第 5の実施例に限定されるものではなく、ロボット以外の各種機械に適用可能であると共に種々の変形が可能である。例えば、上記各実施例では、衝突判別パラメータ（速度偏差又はトルク指令値）：衝突判別基準値（一定或は指令速度または動作指令の種別に応じて可変），衝突判別後に送出するトルク指令値 ( 「 0」または「一 K V J ) および誤差過大アラーム機能の無効化処理を特定の態様で組合せた場合を説明したが、別の紐合せも可能である。例えば、第 5の実施例に示す誤差過大アラ -ム機能の無効化処理を第 1 〜第 4の実施例に適用しても良い。また、第 2の実施例において- 第 5の実施例と同様、衝突判別基準値を動作指令の種別に応じて可変設定しても良い。第 3，第 4の実施例ではルックアップテーブルから指令速度に応じた衝突判別基準値を求めたが、予め実験的に決定されかつ指令速度の関数である演箅式に基づいて所要の衝突判別基準値を箅出しても良い。また、上記各実施例では、一定の或は指令速度または動作指令の種別に応じて可変の衞突判別基準値を用いたが、衝突判別基準値を機械作動部の動作位 gに応じて可変設定にしても良い。この場合、例えば、各種機械の制御プログラムの一連のブロックを表し、従って、動作位 g領域を表すシ一ケンス番号の夫々に対応づけて所要数の衝突判別基準値をルックアップテーブルに格納しておく。

さらに、 1サーボ制御周期に対応する期間内での速度偏差の最大変化高またはトルク指令値の最大変化高と安全係数との積値を衝突判別基準値として用いても良い。この場合、機械を正常運転させつつ周期的に検出した速度偏差またはトルク指令値に基づいて、夫々の検出周期における前回検出時点から今回検出時点までの速度偏差またはトルク指令値の変化高を順次算出しかつ記憶し、運転終了後に変化高の最大値を求める。この場合、変化高の最大値を動作指令の種別毎に又は檨械作動部の動作位置領域毎に求めても良い。

また、各々のサーボ制御実行周期において、前回の速度偏差 ε ν ' と今回の速度偏差 ε Vとの差の絶対値または前回のトルク指令値 T q c ' と今回のトルク指令値 T q c との差の絶対値と所定値 Bとの和 I ε V— ε V， I + Bまたは I T q c — T q c ， Ι + Βを算出し、この加算値を次の制御周期で衝突判別基準値として用いても良い。

第 4の実施例においてはトルク指令値 T q cを算出式 T q c = A + ( V c— V ) * K vに従うルーチン化した演箅処理により求める点に関連して衝突検出後のトルク指令値をこの算,出式に V C = 0， A= 0を代入して求めるようにしたが、衝突検出後の値 T q cを式 T q c = - V ♦ K vに従って箅出しても良い。

本発明によれば、下記に列記する利点を得ることができる。

( i ) デジタルシグナルプロセッサのサーボ制御動作実行周期の各々において衝突の有無を判別するようにしたので、衝突発生をその直後のサーボ制御周期において検出でき、従って、衝突検出上の時間的遅れは、最大でデジタルシグナルプロセッサの 1サーボ制御動作実行周期に等しく、各種機械に装備される制御装置の主コンビユータにより衝突検出を行う場合に比べて、衝突検出を迅速に行える。

( i i ) 衝突発生を検出した直後およびそれ以降において、値「 0」のトルク指令または衝突発生後の実際モータ速度に比例する大きざでかつ実際モータ回転方向とは逆方向に作用するトルク指令を送出するようにしたので、衝突発生時にモータ回転を迅速かつ円滑に停止でき、衝突による機械各部の損傷を最小限にとどめることができる。

( i i i ) 衝突判別基準値を指令速度の大きさ，機械作動部の動作の種別などに即して可変設定するようにしたので、各種機械が如何なる運転状態で運転されている場合にも迅速に衝突発生を検出でき、しかも誤検出を来すことがない。

Claims

請求の範囲

1. サーボモータの駆動力に関連する衝突判別パラメ一タを周期的に検出する工程（ a ) と、前記衝突判別パラメータの変化高を周期的に検出する工程（ b ) と、前記検出した衝突判別バラメータの変化高が判別基準値を越えたとき衝突発生を判別する工程（ c ) と、衝突発生を判別したとき前記サーボモータの回耘を停止させる工程（ d〉とを備えるサーボモータにより駆動される機械における衝突検出 ♦ 駆動停止方法。

2. 各種制御動作を周期的に実行するための主コンビュータを有する制御装箧を含む機械に適用されるもので、前記工程（ a ) ，（ b〉を前記主コンピュータの制御動作実行周期より短くかつ互いに同一の検出周期で実行し、前記工程（ b ) において前回および今回の検出周期での衝突判別パラメータの検出値の偏差を検出する請求の範囲第 1項記載の衝突検出 * 駆動停止方法。

3. 前記衝突判別パラメータは、前記サーボモータに送出される指令速度と前記サーボモータの実際回転速度との偏差と、前記サーポモータに送出ざれるトルク指令値とのいずれかである請求の範囲第 1 項記載の衝突検出 ♦ 駆動停止方法。

4. 前記工程（ d ) において、値が 0のトルク指令を送出する請求の範囲第 1 項記載の衝突検出 ♦ 駆動停止方法。

5. 前記工程（ d ) において、大きさが実際サーボモータ回転速度に比例しかつ実際サーボモータ回転方向とは逆方向に作用するトルク指令を送出する請求の範囲第 1項記载の衝突検出 ♦ 駆動停止方法。

前記判別基準値は指令速度に応じて可変設定される請求の範囲第 1 項記載の衝突検出 ♦駆動停止方法。. 前記判別基準値は動作指令の種別に応じて可変設定される請求の範囲第 1項記載の衝突検出 ♦ 駆動停止方法。

. 前記サーボモータを駆動制御するためのサーボ制御系における位笸偏差が過大になったときに前記サーポモータの励磁を解く誤差過大ァラ一ム檨能を備えた制御装置を含む機械に適用されるものであって、前記ェ程（ c ) において衝突を判別したとき前記誤差過大ァラーム機能を無効にする工程（ e ) をさらに含む請求の範囲第 1項記載の衝突検出 ♦ 駆動停止方法。

. 前記サーボモータを正常逞耘しつつ前記衝突判別パラメータの値を周期的に検出し、夫々の検出周期におい t前回検出時点から今回検出時点までの前記衝突判別パラメータ値の変化高を算出しかつ記憶し、前記モータ運転の終了後に求めた前記パラメータ変化高の最大値を前記判別墓準値として設定する工程（ f ) をさらに含む請求の範囲第 1項記載の衝突検出 ♦ 駆動停止方法。

0 . 前記工程（ c〉は前記衝突判別パラメータ値の変化高が前記判別基準値を越えたか否かを周期的に判別する工程（ c 1 ) を含み、前記方法は、前記工程（ a ) で検出した前記衝突判別パラメータ値と所定値との和を算出する工程（ g ) をさらに舍み、前記工程（ g ) で算出した前記和を次の周期での工程（ c 1 ) での前記判別基準値として用いる請求の範囲第 1項記载の衝突検出 ♦ 駆動停止方法。