WO1989008538A1 - Method of detecting collision of moving portions driven by servo motor - Google Patents

Description

明 細 害

サーボモータによ り駆動される

機械作動部の衝突検出方法

技 術 分 野

本発明は、 サーボモータによ り駆動される機械作動部 と異物等との衝突を迅速に検出可能な衝突検出方法に関 背 景 技 術

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

たとえば、 機械作動部の外面に取り付けたタツチセン サを用いて機械作動部と異物との衝突を検出している。 しかし、 センサの設箧に伴って機械がコス ト高になり、 また、 センサの作動領域外における機械作動部と異物と の衝突を検出できない。

また、 機械作動部をサ―ボモータで駆動するタイプの 機械において、 機械作動部と異物との衝突に伴いサーボ モータ駆動電流が所定の判別値を越えたとき両者の衝突 を検出することが知られている。 しかしながら、 衝突時 点からサーボモータ駆動電流が実際に当該判別値に到達 するまでに時間を要するので、 衝突検出上、 時間的な遅 れが生じ、 衝突による被害が大き く なる。

さらに、 センサ出力あるいはサ―ボモータ駆動電流の 検出値に基づいて内蔵のコンビュー夕で衝突判別処理を 行うタィブの機械たとえば産業用ロボッ トが知られ、 こ の種の機械は、 衝突判別処理をロボッ ト制御用コンビュ —タで実行するようになっている。 すなわち、 衝突判別 処理とロボッ ト制御とを同一のコンビユータで実行して いる。 ロボッ ト制御は各種の複雑な情報処理を含み、 従 つて、 コンビュ —タの制御実行周期および衝突判別処理 実行周期は比較的大きいものにならざるを得ず、 結果と して、 衝突時点から衝突判別処理が完了するまでに時間 を要し、 衝突検出上の時間的な遅れがさらに増大する。

発 明 の 開 示

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

上述の目的を達成するため、 本発明の、 サーポモータ により駆動される機械作動部の衝突検出方法は、 サーボ モータの駆動力に閬連する所定の衝突判別パラメータを 第 1の所定の周期で検出する工程 （ a〉 と、 該衝突判別 パラメ一夕の変化高を第 2の所定 周期で検出する工程 ( b ) と、 前記工程 （ b ) において検出した衝突判別パ ラメ—タの変化高が所定値を越えたとき機械作動部の衝 突を判別する工程 （ c〉 とを備える。

好ま し くは、 サーポモータに出力される トルク指令値 をあるいはサ―ボモータに出力される速度指令とサ―ポ モータの実際速度との偏差を衝突判別パラメ—夕として 用い、 また、 該パラメータの変化高の検出に閬連する第

1 の所定の周期は機械制御実行周期よ り短く され、 ざら に、 前回の周期における衝突判別パラメ一タの検出値と 今回の周期における検出パラメータ値との差を好ましく は第 1 の所定の周期に等しい第 2の所定の周期で衝突判 別パラメータの変化高と して検出する。

本発明によれば、 所定の周期で検出される衝突判別パ ラメ—タの変化高が所定値を越えたとき、 サーボモータ により駆動される機械作動部の衝突を検出するようにし たので、 特別のセンサを用いずに機械作動部の衝突を迅 速に検出でき、 結果として衝突による機械の破損等を防 止もし く は軽減できる。

図 面 の 簡 単 な 説 明

第 1 図は、 本発明の一実施例による衝突検出方法が適 用される産業用ロボッ ト の要部を示すプロック図、 およ び、 第 2図は、 ロボッ ト のひとつの軸に関連するサーボ 系の制御特性を伝達閬数を用いて示したブロック線図で ある 0

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

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

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

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

軸制御器 2は、 口ポッ トの各軸のサ―ボモータおよび これに閬連するサーボアンプ （そ 0内のひとつを符号 1 2で示す） と協働して各軸のデジタルサーボ系すなわち ソフ ト ウェアサーポ系を構成している。 すなわち、 軸制 御器 2は、 第 2図に示す位置制御機能 2 1 , 速度制御機 能 2 2および積分機能 2 4を備えるデジタルシグナルプ 口セッサ （図示省略） を有し、 サーボモータと協働して 第 2図に示す制御特性 2 3を現出させるサーボアンブに 主コンピュータ 1 からの移動指令に応じた トルク指令を 送出するようになっている。 第 2図において、 符号 K p および Κ Vは軸制御器 2の位置ループゲインおよび速度 ループゲインを、 符号 K t はザーボモータの トルク定数 を、 符号 . J はサーボモータの出力軸に加わる全イナーシ ャをそれぞれ表す。

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

以下、 上述のように構成したロボッ トの動作を説明す る。 - ロボッ トの運転に先立ち、 オペレータが教示操作盤 7 および操作パネル 9を操作してロボッ トへの教示を従来 公知のよ うに行うと、 教示データがデータメモリ用 R A M 5に格納される。

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

軸制御器 2のデジタルシグナルプロセッサは、 主コン ビュータ 1 における制御動作実行処理周斯ょり短い周期 でロボッ トの各軸についての位萤， 速度およびトルク制 御動作 （サ—ボ制御動作） ならびに後述の衝突判別動作 を実行する。 すなわち、 デジタルシグナルブロセッサは、 或る制御動作実行周期において当該周期に鬨連するサ一 ボモータ 1 3 bに装着した速度検出器 1 3 cからの、 該 サ―ボモータの実際速度 Vを表す出力を第 2図に符号 2 4で示すように積分してサ―ボモータ 1 3 bの実際回転 位置 P f を算出し、 次に、 このサーボモ—タに閬達する、 主コンビユータ 1 からの指令位匿 （移動指令） P c と実 際モータ位 g P f との差 （位置偏差） s に応じた速度 指令 V c を算出する。 さらに、 算出した速度指令 V と 実際モータ速度 Vとの差 （速度偏差） ε νに応じたトル ク指令 T q Cを箅出し、 この算出 トルク指令 T q cを D / A変換してサーボアンブ 1 2に出力する。 サーボアン ブ 1 2は、 この トルク指令 T q c に応じた駆動電流をサ ボモータ 1 3 bに流して該サーボモータを速度 Vで回 転させ、 対応するロボッ ト作動部 1 3 aを駆動する。

ロボッ ト の運転中、 軸制御器 2のデジタルシグナルブ 口セッサは、 好ま しくは、 上述のサーボ制御動作実行周 期の各々において （よ り一般的には所定数のサーボ制御 動作実行周期度に） 衝突判別動作を実行する。 すなわち、 前回の周期 （一般的には所定回数前の周期） において算 出した速度偏差 ε ν， と今回の周期において算出した速 度偏差 ε Vとの差 I ε V— ε V， I 又は前回の周期にお いて算出した トルク指令 T q c ' と今回の周期において 算出した トルク指令 T q cとの差 I T q c — T q c： ， I (一般的には複数のサ -ボ制御動作実行周期における速 度偏差の変化高又は同様のトルク指令の変化高） を求め、 各々の差 （変化高） がこれに対応する所定値よ り大きい か否かを判別する。 各々の所定値は正常なロボッ ト運転 時に発生し得る値よりも大きい値に設定されている。 各 々の差 （変化高） Ι ε ν— ε ν， I 又は I T q c — T q c ' I が所定値よ りも小さければ、 ロボッ ト作動部 1 3 aと異物等との衝突が生じていないと判別し、 今回の周 期において算出した速度偏差 ε V又は T q cをデジタル シグナルプロセッサ内の対応する レジスタに格納して今 回周期での衝突判別動作を終了する。

ロボッ ト作動部 1 3 aが例えば該作動部の作動領域内 に入り込んだ異物と衝突すると、 サーボモータ 1 3 bの 実際回転速度 V'が低下する。 斯かる場合、 衝突直後のサ 一ボ制御動作実行周期においてデジタルシグナルブロセ ッサによ り箅出される速度偏差 ε V ( = V c - V ) が急 増し、 また、 該プロセッサから送出される トルク指令 Τ q cが急増する。 従って、 当該サーボ制御動作実行周期 (一般的には 1 回乃至所定回数後のサーボ制御動作実行 周期〉 における衝突判別動作において差 （変化高） I ε V - ε V ' I 又は I T q c— T q c， I が所定値よりも 大き く なり、 衝突発生がデジタルシグナルプロセッサに よ り判別される。 この場合、 ブロセッサは、 たとえばト ルク指令レジス夕の値 T q Cを Γ 0 J に設定する。 この 結果、 サーボモータ 1 3 bに駆動電流が流れなくなり、 モータ回転が停止する。

上述の衝突検出動作が実行される結果、 衝突発生は、 当該衝突発生直後のサーボ制御動作実行周期 （一般には 所定回数後のサ―ボ制御動作実行周期〉 において検出さ れ、 従って、 衝突検出上の時間的遅れは、 最大でデジタ ルシグナルブ口セッサの 1サーボ制御動作実行周期 （一 般には所定数のサーボ制御動作実行周期） に等しく、 主 コンピュータ 1 によ り衝突検出を行う場合に比べて、 衝 突検出は迅速に行われる。

Claims

請 求 の 範 囲

1 . サーボモータの駆動力に閲連する所定の衝突判別パ ラメータを第 1 の所定の周期で検出する工程 （ a ) と、 前記衝突判別パラメ一タの変化高を第 2の所定の周期 で検出する工程 （ b ) と、 前記工程 （ b〉 において検 出した衝突判別パラメ一夕の変化高が所定値を越えた とき機械作動部の衝突を判別する工程 （ c ) とを備え る、 サーボモータによ り駆動される機械作動部の衝突 検出方法。

2。 前記第 1の所定の周期を、 前記衝突検出方法が適用 される機械の主コンピュータによる前記サーボモータ への移動指令送出動作を含む各種制御動作の実行周期 より短く設定した請求の範囲第 1項記載のサーボモー タによ り駆動される機械作動部の衝突検出方法。

3 . 前記第 2の所定の周期を前記第 1 の所定の周期と等 しく設定し、 前記工程 （ b ) において前回の周期にお ける衝突判別パラメータの検出値と今回の周期におけ る検出パラメータ値との差を検出する請求の範囲第 1 項記載のサーボモータにより駆動される機械作動部の 衝突検出方法。

4. 前記衝突判別パラメータは前記サ―ボモータに出力 される トルク指令値である請求の範囲第 1項， 第 2項 または第 3項記載のサーボモータによ り駆動される機 械作動部の衝突検出方法。

5 . 前記衝突判別パラメータは前記サ―ボモータに出力 される速度指令と前記サーボモ一タの実際速度との偏 差である請求の範囲第 1項， 第 2項または第 3項記載 のサーボモータにより駆動される機械作動部の衝突検 出方法。