JPH02146602A

JPH02146602A - サーボモータにより駆動される被駆動体の衝突検出・停出方法

Info

Publication number: JPH02146602A
Application number: JP63328866A
Authority: JP
Inventors: Nobutoshi Torii; 信利鳥居; Yasuo Naito; 内藤　保雄; Masayuki Hamura; 羽村　雅之; Kazuhisa Otsuka; 和久大塚
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 1988-08-11
Filing date: 1988-12-28
Publication date: 1990-06-05
Also published as: EP0380707A4; WO1990001732A1; US5214362A; EP0380707A1; KR900702425A

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】産業上の利用分野本発明は、サーボモータで駆動される産業用ロボット舌
の被駆動体の衝突検出・停止方法に関する。

従来の技術産業用ＩＩボッ１〜等の１ノーボモータで駆動される被
駆動体が、伯の異物と衝突したことを検出する方法とし
て、従来、ロボットの表面にタッチセンサ等を取イｑす
、該タッチはン（）で異物と［１ポツトの接触を検出す
る方法が採用されている。

また、サーボモータの駆動電流値に上限値を設け、この
上限値を越えた場合、アラームを出し、被駆動体が異物
に衝突したことを検出する方法も採用されている。この
方法は、指令された位置よでサーボモータは被駆動体を
移動させ、被駆動体を位置決めさせるが、この移動途中
で、異物に被駆動体が衝突し被駆動体が移動しなかった
とき、位置偏差値、速度偏差値が大きくなるから、サー
ボモータへの駆動電流値も大きくなり被駆動体を大きな
力で駆動しようとする。そのため、この駆動電流値を監
視し、ある上限値を越えるど被駆動体が異物に衝突した
ものとして検出するものである。

発明が解決しようとする課題上述したタッチセンサを被駆動体に取イ」ｔ）て衝突を
検出する方法では、駆動装置とは別にセン４ノーを設け
る必要がありコスト１胃となる。また、センサを設置し
ていない部分に異物が衝突したとぎは、衝突を検出でき
ないという欠点がある。

また、上記モータの駆動電流値がある上限値以上になっ
たことによって衝突を検出する方法も、電流値が上限値
に達するまでの時間を要し、衝突検出に遅れが出て（る
。

また、上記駆ｉＩｉ／ｌ電流値を所定リンプリング周期
で検出するような場合、例えば、産業用ロボット等のよ
うに、コンピュータを右Ｊる制御装置によってロボット
アーム舌の被駆動体を制御する場合には、コンピュータ
は所定リンブリング周期で上記駆動電流値を検出するこ
とになり、さらに衝突検出が遅くなることとなる。衝突
検出が遅くなると、衝突による被害を大きくする可能性
があり、できるだけ遅れの少ない衝突検出が望まれる。

そこで、本発明の目的は、タッチセンサ等を必要とせず
、短時間で衝突を検出しサーボモータの回転を停止させ
るサーボモータにより駆動される被駆動体の衝突検出・
停止方法を提供することにある。

課題を解決するための手段本発明は、速度指令値と、サーボモータの回転速度を検
出する速度検出器で検出される実速度との差である速度
偏差値を、所定周期で検出し、前周期で検出された速度
偏差値と今周期で検出された速度偏差値との差が所定値
以上となったどき、サーボモータで駆動される被駆動体
が異物に衝突したとして検出し、サーボモータの回転を
停止させる。

また、サーボモータへ出力されるトルク指令値を所定周
期で検出し、前周期で検出されたトルク指令値と今周期
で検出されたトルク指令値の差が所定値以上となったと
き、サーボモータで駆動される被駆動体が異物に衝突し
たとして検出し、サーボモータの回転を停止させる。

衝突が検出されてサーボモータの回転を停止させるとき
、サーボモータへのトルク指令値を零にすることによっ
て回転駆動力を断ってもよく、または、サーボモータの
実速度に比例し、該実速度方向とは逆のトルク指令値を
出力する比例制御のみを各周期毎行い、サーボモータの
回転を停止させてもよい。

また、上記衝突検出の所定値は、サーボモータへの速度
指令値に応じて、または、動作指令の種類に応じて変え
るＪ：うにしてもよい。さらに、ザーボ七−夕を通常の
状態で駆動し、前周期と今周期の速度偏差値の差又はト
ルク指令値の差を順次検出し、この検出された速度偏差
値の差又はトルク指令値の差を求め、求められた差より
上記衝突検出の所定値を自動設定するようにしてもよい
。

さらに、衝突が検出されたときは、制御装置が有する、
位置偏差過大によるサーボモータの励磁を解く機能を無
効にさせるとよい。

作　　用サーボモータが駆動され被駆動体を所定位置まで移動さ
せる途中において、被駆動体が異物に衝突すると、被駆
動体の移動及びサーボモータの回転は、停止又はその速
度を低下させる。そのため、サーボモータに指令されて
いる速度指令値と実際のサーボモータの速度、即ち、実
速度との差は増大し、速度偏差値は増大する。速度偏差
値が増大する結果、サーボモータへ指令されるトルク指
令値も増大し被駆動体を大きなトルクで移動させようと
する。そこで、所定周期で速度偏差値又はトルク指令値
を検出し、前周期と今周期で検出した速度偏差値の差又
はトルク指令値の差が所定値以上になったことを検出す
ることにＪｚっで、被駆動体が異物に衝突した時点を、
最大で検出周期の一周期遅れで検出する。すなわち、前
周期と今周期で検出した速度偏差値の差又はトルク指令
値の差を検出することは、速度偏差値又はトルク指令値
の微分値を求めることを意味し、被駆動体が異物に衝突
した時点においては、速度偏差値又はトルク指令値は前
述したように大きく変化するため、その微分値は大きく
なるので、この微分値の値を検出することによって衝突
時を検出する。そして、衝突が検出されると、その後は
、トルク指令値を零にするか又はサーボモータの実速度
に比例し、該実速度方向とは逆方向のトルク指令値を出
力する比例制御のみを各周期毎行う。その結果、トルク
指令値を零にすれば、サーボモータに流れる電流がＯと
なり、回転駆動力が断たれる。また、実速度方向とは逆
方向のトルク指令値を出力する場合には、トルク指令値
は衝突時のリーボを一タの速度に比例し、かつ、逆回転
方向のトルク指令値となるからサーボモータは急速に停
止する。また、衝突を検出するための上記所定値を指令
速度又は動作指令の種類に応じて変えることにより、低
速での衝突でも高速での衝突でも、さらには、動作指令
の種類に応じて正確に衝突を検出し、サーボモータを停
止させることができる。

また、サーボモータを通常の状態で駆動して、前周期と
今周期の速度偏差値の差又はトルク指令値の差を順次検
出し、検出された速度指令値の差又は１−ルク指令値の
差より、上記所定値を自動的に決定し設定するようにす
れば、最適な衝突検出のための上記所定値が設定できる
。

さらに、衝突を検出したとき、勺−ボモータの回転が停
止Ｊ−Ｌ、でいることから、位置偏差値が増大するが、
制御装置に、位置偏差過大によるサーボモータ励磁停止
機能がある場合には、この機能を停止させ、回転駆動力
を断つ。または、上述した方法により逆トルクが生じる
ようにＪｚする。

実施例本発明の方法の一実施例として、産業用ロボットに適用
した例を説明する。第６図は、［１ボツ］〜の制御装置
のブロック図であり、１はメインＣＰＵで、マイクロプ
ロセラυと該マイクロプロセッサが実行する制御プログ
ラムを格納したＲＯＭ及びデータの一時記憶等に利用さ
れるＲＡＭＩによって構成されている。２は軸制御器で
、ロボッ１−ｍ５部１３の各軸を駆動する各υ−ポを一
タのリーボ系の位置制御部、速度制御部、Ｖ−ボアンプ
に対する電圧制御部を有する各軸角のデジタル（ソフト
ウェア）ザーボ系を構成している。そして、該軸制御器
には各軸のサーボモータのサーボアンプ１２（図には１
つのみ示している）が接続され、各軸のサーボモータを
駆動制御するようになっている。また、各サーボモータ
に取イＮＪ−ｔノられた速度検出器からの速度フィード
バック信号が入力されている。３はＩ１０ユニツ１〜で
ロボッ１〜の作業対象１４及びロボットに設けられたゼ
ンザ。

アクチュエータ等に接続されている。４はシリアルボー
トでロボットに動作を教示するための教示操作盤７及び
パネル９が接続されている。５はブタメモリ用のＲＡＭ
で、バッテリー８によってバックアップされている。そ
して、これらメインＣＰＵ１．軸制御器２．Ｉ１０コニ
ット３．シリアルポート４．データメ王り用ＲΔＭ５は
バックパネル１６を介してバス結合されており、さらに
、入力１〜ランス１１．入力コニット１０を介して電源
回路６に電力が供給され、電源回路６からバックパネル
１６を介して上記各要素に電力が供給されるようになっ
ている。また、ロボットには入カニニット１０を介して
電力が供給されるようになっている。なお、１５は勺−
ホトランスでシーボアンブ用メイン電源を供給するもの
である。

メインｃｐｕｉは教示操作盤７舌から教示され、データ
メモリ用ＲＡＭ５に格納されてる教示データに従ってロ
ボッ］〜の各軸に対し移動指令を出力し、軸制御器２内
のデジタルシグナルプロセッザは、この移動指令位置デ
ータを受けて位置、速度。

電流制御を行い、各軸のサーボアンプ１２にトルク指令
を出力し、サーボモータを駆動し、［１ボットの各軸を
教示データどおりに駆動する。

上記制御装置の構成、動作は、従来のデジタル（ソフト
ウェア）サーボを使用した制御装置と何ら変わるところ
はなく、本実施例においては、従来の制御装置のハード
ウェアを何ら変更することなく、被駆動体、即ちロボッ
トのアーム等が異物に衝突したことを検出し、サーボモ
ータを停止させようとするものである。

第７図は、軸制御器２．サーボアンプ１２，１ノーボモ
ータで構成される一軸のサーボ制御系のブロック線図で
あり、伝達関数２１のＫｐは軸制御器２で実行される位
置制御処理の位置ループ・ゲイン、伝達関数２２のＫＶ
は軸制御器２で実行される速度制御処理の速度ループ・
ゲイン、また、伝達関数２３は速度偏差値ε■を積分す
る積分手段の伝達関数で、Ｋ１はその積分ゲイン、伝達
関数２４はサーボモータに対するもので、Ｋｔはモータ
のトルク定数、Ｊはモータ軸にかかる全イナーシャであ
る。また、伝達関数２５は軸制御器２内で行われるサー
ボモータの速度Ｖを積分して位置を求めるための積分手
段の伝達関数を意味している。

軸制御器２は、メインＣＰＵ１から出力された当該軸へ
の指令位置ｐｃと、当該軸のサーボモータに取付（）ら
れた速度検出器で検出される現在のモータ速度Ｖを積分
処理（２５）Ｌ、て得られる現在位置Ｐ　ｆとの差であ
る位置偏差値εｐ（εｐ＝ＰＣ−Ｐｆ）に応じて、位置
制御処理（２１）で速度指令値Ｖｃを求め、速度制御部
１！ｌ’！　（２２）（２３）では、この求められた速
度指令値Ｖｃと速度検出器で検出される現在速度Ｖとの
差である速Ｉ哀偏差値ε■に比例する値を求め、該値と
速度偏差値ε■を積分（２３）して得られる値を加算し
てトルク指令値Ｔａｃを求め、軸制御器２は、このトル
ク指令値ＴｑｃをＤ／△変換してサーボアンプ１２へ出
力する。勺−ボアン１１２は入力されたこのトルク指令
ｆ＋１’ｆ　Ｔ　ｑｃに応じてザーボモタに駆動電流を
流し、サーボモータは速度Ｖで駆動される。

以上のようにして、各軸のサーボモータは駆動されるが
、サーボモータが速度Ｖで回転中、該υ−ボモータで駆
動されるロボットアーム等の被駆動体が異物に衝突し、
回転が停止又は極痘に速度Ｖが低下する間に速度指令値
Ｖｃと現実の速度Ｖとの差である速度偏差値εｖ　（＝
Ｖｃ−Ｖ）は急激に増大することとなる。又、速度偏差
値εＶが増大すれば、トルク指令値Ｔｑｃも急激に増大
し、サーボモータを大きなトルクで駆動しようとする。

そこで、軸制御器２内の位置、速麿、電流（トルク）の
制御、即ち、サーボ制御を行う軸制御器２内のデジタル
シグナルプロセッサは、所定周期で上記トルク指令値Ｔ
ＱＣ又は速度偏差値を検出し、前回の周期で検出した値
と今回の周期で検出した値との差（絶対値）が所定値以
上になったが否かによって衝突時を検出し、サーボモー
タの回転を停止させるようにする。

第１図は本発明の第１の実施例にお＋Ｊる軸制ｉｌｌ器
２内のデジタルシグナルプロセッサが行う動作処理フロ
ーチャートであり、以下、このフローチャートと共に第
１の実施例について説明する。

この実施例は、軸制御器２の各軸のサーボモータの速度
制御処理において、従来は、第４図のブロック線図にお
いて、単に速度指令値ＶＣからサーボモータの実速度Ｖ
を減じ速度偏差値ε■を求め、この速度偏差値εＶに速
度ループゲインＫＶを乗じたものと、速度偏差値εＶを
積分し積分ゲインを乗じたものを加算してトルク指令値
ＴｑＣを求めたが、本実施例においては、速度偏差値ε
Ｖを求める第７図中２６における処理を変えて速度偏差
値ε■の微分値の大きさによって衝突を検出し、サーボ
モータを停止させるようにしたものである。

まず、本実施例においては、軸制御器２内のメｔり内に
衝突検出のための基準値となる所定値ＥＲＲを記憶させ
ておく。

そこで、軸制御器２内のデジタルシブプルプロセラｑは
、メインＣＰＵＩから移動指令を受けて、指令位置Ｐｃ
とサーボモータの実速度を積分して得られた現在位置ど
の差εｐにより位置制御処理（２１）を行って速度指令
値Ｖｃを求める処理は、従来と同様で、その後、デジタ
ルシグナルプロセッサは第１図のフローチャートが示す
動作部外を行う。

まず、フラグＦが「１」にセットされているが否か判断
する（ステップ８１）。なお、このフラグＦは該ロボッ
ト制御装置がリセットされたとき［０］にリセットされ
ている。フラグＦがｌ−ＯＪであると、位置制御処理（
２１）で求められた速度指令値Ｖｃから当該サーボ上＝
夕に数個けられた速度検出器からの検出速度、即ち、サ
ーボ−［−タの実速度Ｖを減じて速度偏差値εＶを求め
る（ステップＳ２）。この求められた速度偏差値ε■か
ら前周期の速度偏差値ε■′を減じ、その絶対値が設定
記憶されているは所定値ＦＲＲより小さいか否か判断す
る（ステップ８３）。

ステップＳ３で所定値ＥＲＲより１εＶ−εＶ。

が小さいと、アキュムレータＡに速度指令値Ｖｃから実
速度Ｖを減じた値（即ち、速度偏差値εＶ）に積分ゲイ
ンに１を乗じた値（Ｖｃ−Ｖ）ｘＫｌを加算する（ステ
ップ３４）。なお、このスアップＳ４の処理が第７図に
おける積分処理（２３）に対応する。

次に、速度指令値ＶＣから実速度Ｖを減じた値に比例ゲ
インＫｖを乗じ（この処理が第７図の比例処］！ｌ！　
（２２）に対応する）、アキュムレータＡの値を加算し
トルク指令値ＴｑＣを求め、サーボアンプ１２へ出力す
る（ステップＳ５．Ｓ６）。

次に、速度指令値Ｖｃから実速度Ｖを減じた値をレジス
タに格納し、次周期のステップＳ３で使用する前周期の
速度偏差値εＶ′として記憶する（ステップＳ７）。以
下、デジタルシグナルプロセラ月の基本同期毎上記処理
を行って９−ボモタを駆動制御する。

上述のように、リーーボモータが駆動制御されている中
で、サーボモータで駆動される被駆動体が異物と衝突す
ると、勺−ボー［−りの実速度はｒＯＪ又は低速度とな
る。

その結果、ステップＳ２で求められる速度偏差値εＶの
値は大きくなり、ステップＳ３で、この速度偏差値ε■
ど前周期での速度偏差値εＶ′の差の絶対値１ε■−ε
ｖ／　　ｌは、所定値ＥＲＲ以上になったことが検出さ
れる。すると、プ［ｌセッサはフラグＦを「１」にセッ
トし、アラーム表示しくステップＳ８）、トルク指令値
ＴＱＣをｌ’　ＯＪに゛して（ステップＳ９）、ステッ
プＳ６へ移行し、該トルク指令値Ｔｑｃの１”０］を出
ツノする。トルク指令値ＴｑＣが１０」になると、サー
ボモータには電流が流れなくなり、回転駆動力が断たれ
る。

そして、前述したと同様のステップＳ７の処理を行い、
当該周期の処理を終え、次周期以降は、フラグＦが「１
」にセットされているから、ステップＳ１からステップ
Ｓ９に移行し、トルク指令値ＴｑｃとしてｒＯＪが出力
されることから、（Ｊ−ボモータの回転駆動力は断たれ
たままとなる。

次に、トルク指令値ＴＱＣの変化にＪ：って衝突を検出
する第２の実施例について、第２図の動作処理フローチ
ャートと共に説明する。

この第２実施例においても、前述した速度偏差値　εＶ
の変化によって衝突を検出した実施例ど同様に、衝突を
検出するための基準値となる所定値ＥＲＲを軸制御器２
内のメモリ内に記憶させておく。この場合、所定値ＥＲ
Ｒの値は今周期と前周期のトルク指令値の差に対応する
ものであるから、速度偏差値εＶの差に対応させた前述
の実施例のときの値とは異なる。

さて、速度指令値ＶＣを得るまでは、この実施例におい
ても同様であり、速度指令値ＶＣを得た後、軸制御器２
のデジタルシグブルプ［１セツザはフラグ′Ｆが「１」
にゼッｉ・されているか否か判断しくステップ８１００
）（このフラグＦは装置がリセットされた状態で「０」
にリセットされている）、セットされてなければアキコ
ムレータＡに速度指令値ＶＣから実速度Ｖを減じた値に
積分ゲインに１を乗じた値を加算しくステップ８１０１
）（第７図の積分処理（２３＞）、該アキコムレータＡ
の値に速度指令値Ｖｃから実速度Ｖを減じた値に比例ゲ
インＫＶを乗じた値（第７図の比例処理（２２＞）を加
算してトルク指令値ＴＱＣを求め（ステップ５１０２）
、このトルク指令値ＴＱＣから前周期で求められたレジ
スタに記憶されている前周期のトルク指令値ＴｑＣ′を
減じた値の絶対値が設定所定値ＥＲＲより小さいか否か
判断しくステップ５１０３）、小さければ、該トルク指
令値ＴｑＣを出力してサーボモータを駆動する（ステッ
プ５１０６）。そして、該トルク指令値ＴｑＣをレジス
タに格納し、次周期のステップ５１０３で判断するため
の前周期のトルク指令値Ｔｑｃ’　として記憶しくステ
ップ５１０７）、当該周期の処理を終了する。以下、基
本周期毎、上述したステップ８１００−８１０３．８１
０６゜５１０７の処理を繰返しサーボモータを駆動力る
。

こうして、サーボモータが駆動中、サーボモータで駆動
される被駆動体が異物と衝突するとサーボモータの回転
が阻止され、回転は停止又は低速となる。その結果、速
度偏差値（ＶＣ−Ｖ）は増大し、前周期のトルク指令Ｔ
ｑｃ’　と比べ今周期のトルク指令値ＴｑＣは大きな値
となるから、ステップ５１０３で今周期と前周期のトル
ク指令値の差ＩＴｑｃ−ＴＱＣ’　　ｌが所定値ＥＲＲ
を超えたことが検出される。そこで、プロレッ勺はフラ
グＦを「１」にセラ１〜し、アラーム表示を行い（ステ
ップ５１０４）、トルク指令値Ｔｑｃを１−Ｏ」にして
出ツノする（ステップ５１０５．８１０６）。

そして、前述したように、今周期のトルク指令値Ｔ（Ｉ
Ｃ（＝Ｏ）をレジスタに格納しくステップ５１０７）、
当該周期の処理は終了する。そして、次周期以降はフラ
グＦが「１」にセットされているから、ステップ８１０
０よりステップ５１０５へ移行し、ステップ５１０６，
５１０７の処理を行うこととなる。その結束、サーボモ
ータは回転駆動力が断たれたままになる。

上記第１．第２の実施例においては、衝突を検出するだ
めの所定値ＥＲＲを、設定された一定な値にしたが、動
作速度が大きく変動するような場合には上記所定値も変
える必要がある。即ち、速度偏差値の差又はトルク指令
値の差は、同一の動作でも動作速度ににって異なる。動
作速度が早い状態で衝突が生じると速度偏差値の差又は
トルク指令値の差は大きくなり、動作速度が遅い状態で
の衝突で１．Ｌ小さくなる。そこで、動作速度、即ち、
指令速度に応じて衝突を検出するための上記所定値ＥＲ
Ｒを変えて、速度偏差値によって衝突を検出し、サーボ
モータを停止させる第３の実施例について説明する。ま
た、この第３の実施例は衝突時に、サーボモータの実速
度に比例し、該実速度方向とは逆方向のトルク指令値を
出力する比例制御のみ各周期毎行い、サーボモータを強
制的に停止させる点が第１の実施例とは相違する。

まず、軸制御器２内のメモリ内に指令速度値ＶＣに応じ
た上記衝突を検出するための基準値（所定値）ＥＲＲを
テーブルとして記憶させておく。第８図はこのテーブル
１’−ａｂの一例で、最高指令速度Ｖ　ｃｍａｘをＮ等
分して速度指令値上ＶＣが低速の方から高速の方へ順に
１へ・Ｎ区間に分り、各区間における上記基準値ＥＲＲ
１，ＥＲＲ２・・・・・・ＥＲＲＮを１番目（１＝１−
Ｎ）のアドレスに設定すると共に速度指令ＶＣが０のと
きのＭ半値ＥＲＲＯもＩ＝０番目のアドレスに記憶させ
ておく。すなわち、Ｋ　＝　Ｖ　ｃｍａｘ／　Ｎとし、
速度指令値がＶｃであると、該速度指令値ＶＣをＫで除
し、小数を切上げて得られる整数の絶対値（これを、Ｉ
　Ｎ　Ｔ　Ｉ　　Ｖ０＋として表わす）を得ると、この
整数がその指令速度ＶＣにおける基準値が得られるアド
レスを示すことになる。

この関係は次のとおりである。

ｌ−ｌＮＴ１−■旦−１・・・・・・（１）軸制御器２
内のデジタルジグノルプロセッサが移動指令を受けて速
度指令値ＶＣを求めるまでの処理は従来と同様であり、
その後、デジタルシグナルプロセッサは第３図のフロー
チャートで示す動作部外を行う。

まず、第１の実施例と同様に、フラグＦが「１」にセッ
トされているか否か判断する（ステップ５２００）。フ
ラグＦが「０」であると、位置制御処理（２１）で求め
られた速度指令値ＶＣから当該サーボモータに取付けら
れた速度検出器からの速度、即ち、サーボモータの実速
度Ｖを減じて速度偏差値ε■を求める（ステップ５２０
１　）。この求められた速度偏差値εＶから前周期の速
度偏差値ε■′を減じ、その絶対値がレジスタに記憶さ
れている基準値ＥＲＲより小さいか否か判断りる（ステ
ップ３２０２）。なお、サーボ玉−夕の駆動開始時にお
いては、前周期での速度偏差値ε■′はｒＯＪであり、
また、レジスタに記憶する基準値ＥＲＲは前周期では速
度指令値ＶＣが「０」であるから第（１）式よりＩ＝Ｏ
であり、テーブルＴａｂの０番目のアドレスに記憶され
た速度指令値Ｖｃ＝Ｏに対応するＷＱ−値ＥＲＲＯであ
る。

ステップ５２０２で基準値ＥＲＲより１εＶεｖ／　　
ｌが小さいと、アキュムレータＡに速度指令値ＶＣから
実速度Ｖを減じた値（即ち、速度偏差値ε■）に積分ゲ
インに１を乗じた値（ＶｃＶ）ＸＫ１を加算する（ステ
ップ５２０３）。なお、このステップ８２０３の処理は
第１の実施例のステップＳ４の処理と同様、第７図にお
ける積分処理（２３）に対応する。

次に、速度指令値ＶＣから実速度Ｖを減じた値に比例ゲ
インＫＶを乗じ、アキュムレータＡの値を加算しトルク
指令値ＴＱＣを求め、勺−ボアンプ１２へ出力する（ス
テップ８２０５）。次に、速度指令値ＶＣから実速度Ｖ
を減じた値をレジスタに格納し、次周期のステップ５２
０２で使用する前周期の速度偏差値ε■′として記憶し
くステップ５２０５）、速度指令値ＶＣに対し第（１）
式の処理を行い、該速度指令値ＶＣに対する基準値ＥＲ
Ｒを記憶するテーブルＴａｂのアドレス■を求め、この
１番目のアドレスより基準値ＥＲＲを読み取り、レジス
タに格納する（ステップ８２０６）。以下、デジタルシ
グナルプロセッサの基本周期毎上記処理を行ってサーボ
モータを駆動制御する。

上述のように、サーボ王−夕が駆動制御されている中で
、サーボモータで駆動される被駆動体が異物と衝突Ｊ−
ると、サーボモータの実速度は［０１又は低速度となる
。

その結束、ステップ５２０１で求められる速度偏差値ε
Ｖの値は大きくなり、ステップ８２０２で、この速度偏
差値ε■と前周期での速度偏差値ε■′の差の絶対値１
εＶ−εｖＪ　　ｌは、レジスタに格納された基準値Ｅ
ＲＲ以上になったことが検出される。すると、プロセラ
υはフラグＦを「１」にセットし、アラーム表示しくス
テップ５２０７＞、速度指令値ＶＣ及びアキュｌ＼レー
タＡの値を「０」にして（ステップ８２０８）、ステッ
プ８２０４へ移行し、トルク指令値Ｔｑｃの計算を行う
。即ち、Ａ＝Ｏ，Ｖｃ＝Ｏであるから、トルク指令値Ｔ
ｑｃはＴＱＣ−Δ十（Ｖｃ−Ｖ）　・Ｋｖ＝−Ｖ−Ｋｖ・・・
・・・（２）となり、実速度Ｖに比例し、かつ、現時点におりる回転
方向とは逆方向のトルク指令値ｒ　ｑ　ｃを出力するこ
ととなる。その結果、サーボモータには急速にブレーキ
がかかる。そして、前述したと同様のステップ８２０５
，５２０６の処理を行い、当該周期の処理を終え、次周
期以降は、フラグＦが「１」にセットされているから、
ステップ５２００からステップ８２０８に移行し、トル
ク指令値ＴｑＣは上記第〈２）式で示される値が出力さ
れることとなるから、サーボｒ−夕の回転は急速に停止
づることとなる。

次に、トルク指令値ＴｑＣの変化によって衝突を検出し
、かつ、衝突検出のための基準値（所定値）を指令速度
に応じて変えた第４の実施例について、第４図の動作処
理フローチャートと共に説明する。

この第４の実施例においても、衝突を検出するための基
準値ＥＲＲを指令速度に応じて記憶した第８図に示すテ
ーブルと同様なテーブルを軸制御器２内のメｔり内に記
憶させておく。この場合、基準値ＦＲＲの値は今周期と
前周期のトルク指令値の差に対応するものであるから、
速度偏差値ε■の差に対応させ−た前述の第３の実施例
のときの値とは異なる。

さて、速度指令値ＶＣを得るまでは、この実施例におい
ても同様であり、速度指令値ＶＣを得た後、軸制御器２
のデジタルシグナルプロセッサはフラグＦが１１」にセ
ットされているか否か判断しくステップ５３００）（こ
のフラグＦはＨＺｉがリセットされた状態で１”Ｏ」に
リセットされている）、セットされてなければアキュム
レータ八に速度指令値Ｖｃから実速度Ｖを減じた値に積
分ゲインに１を乗じた値を加算しくステップ５３０１）
（第７図の積分処理（２３））、該ア：１ユムレータＡ
の値に速度指令値ＶＣから実速度Ｖを減じた値に比例ゲ
インＫｖを乗じた値（第７図の比例処理（２２））を加
算して１−ルク指令値ｒｑｃを求め（ステップ５３０２
）、このトルク指令値ＴｑＣから前周期で求められたレ
ジスタに記憶されている前周期のトルク指令値Ｔｑｃ′
を減じた値の絶対値がレジスタに記憶された基準値Ｅ　
ＲＲＪ：り小さいか否か判断しくステップ８３０３）　
、小さければ、該トルク指令値ＴｑＣを出力してサーボ
モータを駆動する（ステップ８３０７）。そして、該ト
ルク指令値ＴＱＣをレジスタに格納し、次周期のステッ
プ８３０３で判断するための前周期のトルク指令値Ｔ（
１ｃ’　として記憶する（ステップ８３０８）。次に、
第（１）式の演算を行って、当該速度指令値ＶＣに対す
る基準値ＥＲＲを記憶するアドレスＩを求め、テーブル
より求められた１番目のアドレスに格納されている基準
値ＥＲＲをレジスタに格納しくステップ５３０９）　、
当該周期の処理を終了する。以下、基本同期毎上述した
ステップ８３００へ・５３０３，５３０７へ・５３０９
の処理を繰返し、ナーボモータを駆動する。こうして、
す゛−ボモータが駆動中、サーボモータで駆動される被
駆動体が安物と衝突するとサーボモータの回転が阻止さ
れ、回転は停止又は低速となる。その結果、速度偏差値
（Ｖｃ−Ｖ）は増大し、前周期のトルク指令ＴＱＣ’　
と比べ今周期のトルク指令値Ｔ（ＩＣは大きな値となる
から、ステップ５３０３で今周期と前周期のトルク指令
値の差Ｔｑｃ−Ｔｑｃ’　　ｌが基準値ＥＲＲを超えた
ことか検出される。そこで、プロゼツ勺はフラグＦを「
１」にセラ１へし、アラーム表示を行い（ステップ５３
０４）、速麿指令値ＶＣ，アキュムレータへの値を「０
」にセラ１〜し、トルク指令値「ｑＣの語算を再度行う
（スアップ５３０５．Ｓ３０６）。この場合、Ｔｑｃ＝Ａ＋　（Ｖｃ−Ｖ）　・Ｋｖ＝−Ｖ−Ｋｖとな
り、トルク指令値は実速度Ｖに比例し、現時点における
回転方向とは逆方向のトルク指令値ＴｑＣとなる。なお
、ステップ５３０５，８３０６で、トルク指令値Ｔｑｃ
をＴＱＣ＝−Ｖ−ＫＶの演算を行って求めてもよいが、
トルク指令稙ＴｑＣの演算、Ｔ（ｌＣ＝Ａ＋（Ｖｃ−Ｖ
）　・Ｋｖはルーチン化されているため、Ａ−１Ｏ，Ｖ
ｃ＝Ｏとして計算を行うものである。

次に、プロセッサはトルク指令値Ｔ　ｑｃ　−−Ｖ・Ｋ
Ｖを出力しくステップ５３０７）、その結果、サーボモ
ータには速度方向とは逆方向の駆動電流が流れるからサ
ーボモータには急速にブレーキがかかる。そして、前述
したように、今周期のトルク指令値ＴｑＣをレジスタに
格納しくステップ８３０８）、速度指令値Ｖｃに対応Ｊ
る基準値ＥＲＲをテーブルより求めてレジスタに格納し
、当該周期の処理は終了する。そして、次周期以降はフ
ラグＦが「１」にセットされているから、ステラプ５３
００よりステップ５３０５へ移行し、ステップ８３０６
〜５３０９の処理を行うこととなる。

その結果、ザーボーし一タの実速度に比例し、サーボモ
ータの回転方向とは逆方向へのトルク指令値Ｔ　ｑ　ｃ
が毎周期出力されることとなるから、サーボモータは急
速にその回転を停止する。

第５図は、本発明の第５の実施例のデジタルシグブルブ
ロセッりの動伯処理フローチャートである。この第５の
実施例におけるフ［１−ヂャートは第１の実施例の第１
図で示すフローチャートと比較し、第５の実施例のステ
ップ５５００，５５０２．５５０４，５５０５，８５０
６，５５０７゜８５０９．５５１０は第１の実施例ノス
テップＳ′１．８２．Ｓ／４．Ｓ５，８６，８７．Ｓ８
．Ｓ９どそれぞれ対応し同一処理である。そして、異な
る点は、第５の実施例はステップ８５０１．８５０８が
加わっていることど、ステップ５５０３で前周期と今周
期の速度偏差値の差１εＶ−εＶ′と基準値ＥＲＲを比
較するが、この基準値の値が第１の実施例では一定な値
であった点を、この第５実施例では当該サーボモータの
動作性質に応じて、即ち、当該サーボモータにどのよう
な動作指令を出力しているかによって、この基準値ＥＲ
Ｒの値を決める。

これは、このサーボモータを使用しているＴ作機械やロ
ボットの動作指令に応じ基準値を決め、テーブルとして
記憶させておくもので、テーブルには第８図と同様に、
例えば、動作指令がＧＯＩ（直線制御）ならばＥＲＲＩ
、ＧＯ２（円弧制御）ならばＥＲＲ２、・・・・・・Ｇ
ｎならばＥＲＲｎどして記憶させておく。そして、ステ
ップ８５０１で、該テーブルより現在の動作指令に対応
する基準値ＥＲＲｎを読出し、現時点における衝突検出
のための基準値ＥＲＲとしてレジスタに格納し、ステッ
プ５５０３ではこのレジスタに格納された基準値ＥＲＲ
に基いて判断する。なお、ステップ５５０１の動作指令
に応じて、基準値ＥＲＲをテーブルから読出す処理は、
この速度制御処理を行うデジタルシグナルプロセッサが
行わず、数値制御装置のメインプロセラりがレジスタの
値を動作指令を変える毎にテーブルから読出し、書き変
えるようにしてｂよい。

また、第５の実施例には、ステップ５５０８に誤差過大
アラーム機能停止処理が加わっている。

誤差過大アラーム機能とは、数値制御装置において、位
置偏差吊即ち指令位置と現在位置との差が過大になると
く位置偏差を記憶するレジスタの値が過大になると）、
サーボモータの励磁を解き、数値制御装置及び数値制御
装置で制御される機械の動作を停止させる機能である。

そのため、この機能が作動すると、サーボモータの励磁
が解かれ、サーボモータの制御が作動しなくなる。前周
期と今周期の速度偏差値の差が基準値ＥＲＲを超えて衝
突が検出された場合には、当然位置偏差量も増大するも
のであるから、誤差過大アラーム機能が作動するおそれ
があるので、この機能を停止させてサーボモータを制御
可能にさせたものである。

この第５の実施例の他の動作は第２の実施例と同一であ
るので、説明を省略する。

なお、第５の実施例では、前周期と分周期の速度偏差値
の差より衝突を検出するための基準値を動作指令の種類
に応じて変える例を示したが、第２図に示す第２の実施
例にもこれを適用し、前周期と分周期のトルク指令値の
差によって衝突を検出するための基準値を動作指令の種
類によって変えてもよい。

また、第５の実施例に加えた誤差過大アラーム機能停止
処理（ステップ８５０８）は、伯の第１〜第４の実施例
にも加えてもよいことはもちろんである。即ち、第１の
実施例ではステップ８８゜第２の実施例においてはステ
ップ５１０４．第３の実施例ではステップ８２０７．第
４の実施例ではステップ５３０４の各処理と共に、この
誤差過大アラーム機能停止処理を行うようにすればよい
。

また、衝突検出のための基準値ＦＲＲを一定値（第１．
第２の実施例）、または、速度指令値に応じて変える場
合（第３．第４の実施例）、さらに、動作種類に応じて
変える（第５の実施例）ようにしたが、この基準値を動
作位置によって変えるようにしてもよい。即ち、【］ボ
ットＦＩ作機械を駆動制御するプログラムのプログラム
番号に応じて基準値ＥＲＲを設定し、設定された基準値
ＥＲＲに基いて衝突検出を行うようにしてもよい。

さらに、この基へ（値ＥＲＲの設定も自動設定するよう
にしてムよい。（ＩＩＩら、サーボモータを通常使用す
る状態で駆動し、各周期毎に前周期と分周期の速度偏差
値の差又はトルク指令値の差を検出記憶し、動作プログ
ラム終了後、記憶している速度偏差値の差又はトルク指
令値の差の最大値に安全係数を乗じた値を基準値どして
自動設定する。

または、速度指令値に応じて各速度指令値毎にこの基準
値を自動設定したり、さらには、動作種類。

動作位置に応じてこの基準値を自動設定するようにして
もよい。

さらには、上記基準値ＥＲＲをテーブルにより求める方
式ではなく、速度指令値ＶＣを変数として基準値ＥＲＲ
を求める演算式を実験的に求め、この演算式によって基
準値を求めるようにしてもよい。

また、上記基準値ＥＲＲを、分周期と前周期の速度偏差
の差またはトルク指令値の差に設定所定ＩＢを加え、こ
れを基準値ＥＲＲどしてレジスタに記憶しておき、次の
周期では、この基準値によって衝突を検出するようにし
てもよい。即ら、速度偏差の差による場合は、ＥＲＲ−６Ｖ−εＶ’　　＋Ｂトルク指令値の差による場合は、ＥＲＲ＝　ｌ　ＴＱＣ−Ｔｑｃ’　　ｌ　＋８と、各周
期毎基準値を更新し、各時点の動作特性に応じた上記基
準値としてもよい。

以上に述べたように、本発明においては、衝突検出の遅
れは、最大でデジタルシグナルプロセッサの基本処理周
期の遅れとなるだけであり、極めて早く衝突を検出でき
る。

しかも、衝突検出後、サーボモータの回転駆動力を断つ
ことができる。または、！！突突点点お（プる回転方向
とは逆方向への回転トルク指令が出力されるから、サー
ボモータは急速にその回転を停止させることができる。

また、サーボ制御をデジタルシグナルプロセッサで行わ
れない場合は、トルク指令値ＴｑＣ又は速度偏差値εＶ
を例えばメインＣＰＵ１で検出してもよく、そのときの
最大検出遅れはメインＣＰ１］１の一検出周期分となる
。

発明の効果本発明は、サーボモータで駆動される被駆動体が異物に
衝突したとき伴しるトルク指令値の変化値（微分値）又
は速度偏差値の変化値（微分値）の増大を検出して、衝
突を検出するようにしたから、衝突を検出する遅れは、
トルク指令値又は速度偏差値を検出する周期の一周期が
最大の遅れとなり、極めて早く衝突を検出することがで
き、がっ、衝突が検出されると、サーボモータの回転駆
動力を断つことができる。若しくは、衝突時以後のサー
ボモータの回転速度に比例し、その回転方向とは逆方向
のトルク指令値によって駆動されるから急激にブレーキ
がかかり、サーボモータは急速に停止するため、異物へ
の衝突による被害を最小限にすることがでる。

また、衝突を検出するための基準値を速度指令値の大き
さ、動作の種類に応じて変えるようにすると、動作速度
が変っても正確に衝突を検出することができ、低速時で
も高速時でも極めて甲く衝突を検出することかできる。

【図面の簡単な説明】

第１図は、本発明の第１の実施例のフローチャート、第
２図は、本発明の第２の実施例の７０チヤート、第３図
は、本発明の第３の実施例の７０−ヂヤート、第４図は
、本発明の第４の実施例のフローチャート、第５図は、
本発明の第５の実施例のフローチャート、第６図は、本
発明の各実施例を適用する産業用ロボッ１〜の制御装置
のブロック図、第７図は、サーボ制御系のブロック図、
第８図は、各実施例で使用するテーブルの説明図である
。 εＶ・・・速度偏差値、ＴｑＣ・・・トルク指令値、ｐ
ｃ・・・指令位置、Ｐｆ・・・現在位置、εｐ・・・位
置偏差値、Ｖｃ・・・速度指令値、■・・・現在速度、
Ｔａｂ・・・テーブル、ＥＲＲ・・・基準値。第

Claims

【特許請求の範囲】

（１）サーボモータへ指令される速度指令値と、サーボ
モータの回転速度を検出する速度検出器で検出される実
速度との差である速度偏差値を、所定周期で検出し、前
周期で検出された速度偏差値と今周期で検出された速度
偏差値との差が所定値以上となったとき、衝突検出とし
てサーボモータの回転を停止させることを特徴とするサ
ーボモータにより駆動される被駆動体の衝突検出・停止
方法。
（２）サーボモータへ出力されるトルク指令値を所定周
期で検出し、前周期で検出されたトルク指令値と今周期
で検出されたトルク指令値の差が所定値以上となつたと
き衝突検出としてサーボモータの回転を停止させること
を特徴とするサーボモータにより駆動される被駆動体の
衝突検出・停止方法。
（３）サーボモータへ出力されるトルク指令値を零にす
ることによってサーボモータの回転駆動力を断つ請求項
１または請求項２記載のサーボモータにより駆動される
被駆動体の衝突検出・停止方法。
（４）サーボモータの実速度に比例し、該実速度方向と
は逆方向のトルク指令値を出力する比例制御のみ各周期
毎行い、サーボモータの回転を停止させる請求項１また
は請求項２記載のサーボモータにより駆動される被駆動
体の衝突検出・停止方法。
（５）上記所定値は速度指令値に応じて変えるようにし
た請求項１、請求項２、請求項３または請求項４記載の
サーボモータにより駆動される被駆動体の衝突検出・停
止方法。
（６）上記所定値は動作指令の種類に応じて設定されて
いる請求項１、請求項２、請求項３または請求項４記載
のサーボモータにより駆動される被駆動体の衝突検出・
停止方法。
（７）衝突検出時には、制御装置の位置偏差過大による
サーボモータの励磁を解く機能を無効にさせる請求項１
、請求項２、請求項３、請求項４、請求項５または請求
項６記載のサーボモータにより駆動される被駆動体の衝
突検出・停止方法。
（８）サーボモータの通常の状態で駆動し、前周期と今
周期の速度偏差値の差を順次検出し、検出された速度偏
差値の差より上記所定値を自動設定するようにした請求
項１記載のサーボモータにより駆動される被駆動体の衝
突検出・停止方法。
（９）サーボモータを通常の状態で駆動し、前周期と今
周期のトルク指令値の差を順次検出し、検出されたトル
ク指令値の差より上記所定値を自動設定するようにした
請求項２記載のサーボモータにより駆動される被駆動体
の衝突検出・停止方法。
（１０）今周期と前周期の速度偏差の差に設定所定量加
算した値を次周期の上記所定値として順次各周期毎更新
するようにした請求項１記載のサーボモータにより駆動
される被駆動体の衝突検出・停止方法。
（１１）今周期と前周期のトルク指令値の差に設定所定
量加算した値を次周期の上記所定値として順次各周期毎
更新するようにした請求項２記載のサーボモータにより
駆動される被駆動体の衝突検出・停止方法。