JPH01230107A

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

Info

Publication number: JPH01230107A
Application number: JP63054834A
Authority: JP
Inventors: Nobutoshi Torii; 信利鳥居; Yasuo Naito; 内藤　保雄; Masayuki Hamura; 羽村　雅之; Tetsuro Kato; 哲朗加藤
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 1988-03-10
Filing date: 1988-03-10
Publication date: 1989-09-13
Also published as: KR900700247A; KR960005985B1; WO1989008538A1; DE68919801D1; EP0365681A4; EP0365681A1; US5119006A; EP0365681B1; DE68919801T2

Abstract

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

Description

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

従来の技術産業用ロボット等のサーボモータで駆動される被駆動体
が、他の異物と衝突したことを検出する方法として、従
来、ロボットの表面にタップヒンザ等を取付け、該タッ
チレンチでテシ物とロボットの接触を検出する方法が採
用されている。

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

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

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

又、上記駆動電流値を所定サンプリング周期で検出する
ような場合、例えば、産業用ロボット等のように、コン
ピュータを右するａｉｌｌ　ｔＩｌ装置によってロボッ
トアーム等の被駆動体を制御する場合には、コンピュー
タは所定サンプリング周期で上記駆ｖＪ電流値を検出す
ることになり、さらに衝突検出が遅くなることとなる。

衝突検出が遅くなると、衝突による被害を大きくする可
能性があり、できるだけ遅れの少ない衝突検出が望まれ
る。

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

課題を解決するための手段本発明は、サーボモータへ出力されるトルク指令値を所
定周期で検出し、前周期で検出されたトルク指令値と分
周期で検出されたトルク指令値の差が所定Ｗｉ以上とな
ったとき、サーボモータで駆動される被駆動体が異物に
衝突したとして検出し、サーボモータの回転を停止させ
る。

又は、サーボモータへ指令される速度指令値とサーボモ
ータの回転速度を検出する速度検出器からの検出速度と
の差である速度偏差値を、所定周期で検出し、前周期で
検出された速度偏差値と分周期で検出された速度偏差値
との差が所定値以上となるとサーボ七−夕で駆動される
被駆動体が異物に衝突したとして検出し、サーボモータ
の回転を停止させる。

作　　用サーボモータが駆動され被駆動体を所定位置まで移動さ
せる途中においで、被駆動体が異物に衝突すると、被駆
動体の移動及びサーボモータの回転は、停止又はその速
度を低下させる。そのため、サーボモータに指令されて
いる速度指令値と実際のサーボモータの速度との差は増
大し、速度偏差値は増大する。速度偏差値が増大する結
果、サーボモータへ指令されるトルク指令値も増大し被
駆動体を大きなトルクで移動させようとする。そこで、
所定周期でトルク指令値又は速度偏差値を検出し、前周
期と分周期で検出したトルク指令値の差、又は速度偏差
値の差が所定値以上になったことを検出することによっ
て、被駆動体が異物に衝突した時点を、最大で検出周期
の一周期遅れで検出できる。即ち、前周期と分周期で検
出したトルク指令値の差又は速度偏差値の差を検出する
ことは、トルク指令値又は速度偏差値の微分値を求める
ことを意味し、被駆動体が異物に衝突した時点において
は、トルク指令値又は速度ｇａ差値は前述したように大
きく変化するため、その微分値は大きくなるので、この
微分値の値を検出することによって衝突時を検出するも
のである。

実施例本発明の方法の一実施例どして、産業用ロボットに適用
した例を説明する。第２図は、τロボットの制御装置の
ブロックであり、１はメインＣＰＵで、マイクロプロセ
ツサと該マイクロプロセツサが実行する制御ブＯグラム
を格納したＲＯＭ及びデータの一時記憶等に利用される
Ｒ　Ａ　Ｍ等によって構成されている。２は軸制ｔＩｌ
器で、１ロボット機構部１３の各軸を駆動する各り一−
ボ七−タのサーボ系の位置制御部、速度制御部、サーボ
アンプ対する電圧制御部を有す、る各軸用のデジタル（
ソフトウェア）サーボ系を構成している。そして、該軸
制御器には各軸のサーボモータのサーボアンプ１２（図
には１つのみ示している）が接続され、各軸のサーボモ
ータを駆動制ｍａするようになっている。また、各サー
ボモータに取付けられた速度検出器からの速度フィード
バック信号が入力されている。３はＩ１０ユニットでロ
ボットの作業対象１４及びロボットに設けられたセンサ
、アクチュエータ等に接続されでいる。４はシリアルボ
ートで１］ボツトに動作を教示づ−るための教示操作盤
７及びパネル９が接続されている。５はデータメモリ用
のＲＡ　Ｍで、バッテリー８によってバックアップされ
ている。そして、これらメインＣＰＵ１、軸制御器２．
１１０ユニット３．シリアルボート４．データメ［り用
ＲＡＭ５はバックパネル１６を介してバス結合されてお
り、さらに、入力１ヘランス１１．入カニニット１０を
介し′Ｃ電源回路６に電力が供給され、電源回路６から
バックパネル１６を介して上記各要素に′電力が供給さ
れるようになっている。また、ロボットには入カニニッ
ト１０を介して電力が供給されるようになっている。な
お、１５はサーボトランスでサーボアンプ用メイン電源
を供給するものである。

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

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

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

第１図は、軸制″ａ器２．サーボアンプ１２．リーボモ
ータで構成される一軸の４１−水制御系のブロック線図
であり、伝達関数２１のＫｐは軸制御器２で実行される
位置制御処理の位置ループ・ゲイン、伝達関数２２のＫ
Ｖは軸制御器２で実行される速度制御処理の速度ループ
・ゲインで、伝達関数２３はサーボアンプ、サーボモー
タに対するもので、Ｋｔはモータのトルク定数、Ｊはモ
ータ軸にかかる全イナーシャである。又、伝達関数２４
は軸制御器２内で行われる積分を意味している。

軸制ｔｌ１３２はメインＣＰＵ１から出力された当該軸
への指令位置Ｐｃと当該軸のす〜ボモータに取付けられ
た速度検出器で検出される現在のモータ速度Ｖを積分（
２４）　しで得られる現在位置Ｐｆとの差である位置偏
差値εｐに応じて位置制御処理（２１）では速度指令値
Ｖｃを求め、速度制御処理（２２）では、この求められ
た速度指令値ＶＣとの速度検出器で検出される現在速度
Ｖとの差である速度偏差値ε■に応じてトルク指令値Ｔ
ＱＣを求め、軸制御器２は、このトルク指令値ＴｑＣｊ
Ｆｉ−Ｄ／△変換しＣサーボアンプ１２へ出力する。サ
ーボアンプ１２は入力されたこのトルク指令値ＴＣＩＣ
に応じてＩＪ′−ボモータに駆動電流を流し、サーボモ
ータは速度ＶＣ駆動される。

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

そこで、軸制御ＰＪ２の位置、速度、電流（トルク）を
制御する、即ち、サーボ制御を行うデジタルシグナルプ
ロセッサは、所定ＩＰｉｌｌｆｆｉで４−記トルク指令
値Ｔｑｃ又は速度偏差値を検出し、前回の周期で検出し
た値と今回の周期で検出した値との差（絶対値）が所定
値以上になったか否かによって衝突時を検出し、所定値
以上になっているとトルク指令値Ｔｑｃを「０」にする
。その結果、サーボモータにはダイナミックブレーキが
かかり、リーーボモータの回転は停止する。

この衝突検出のためのデジタルシグナルプロセッサ処理
フローは図示しないが、デジタルジグカルプロセッサは
速度制御のために、速度指令値ＶＣと現在速度Ｖとの差
の速度偏差値εＶを求め、トルク指令値ＴｑＣを出力し
ているものであるから、デジタルシグナルプロセッサの
処理１′Ａ本周期毎にこれらの値εｖ、Ｔａｃを検出し
ており、各周期毎にこの検出した速度偏差１ｆ［εまた
はトルク指令値ＴｑＣと前回の周期で検出した速痘偏Ｘ
値εｖ′、トルク指令値ＴｑＣ’　との差１εＶ−εｖ
’　　ｌ又はＩＴｑｃ−Ｔｑｃ’　　Ｉを求め、大々通
常の動作時において発生する値より大きなある所定値よ
り大きいか否か判断し、小さければ、今周期で検出した
速度偏差値ε■又はトルク指令値王ｑｃをレジスタに格
納し、当該周期の処理を終了する。そして、次の周期で
は、当該内＋１で検出した速度偏差値ε■又はトルク指
令値ＴｑＣと上記レジスタに格納している前回の周期の
速度偏差値εＶ′又はトルク指令値ＴｑＣ’　と夫々比
較し、前述と同様な処理を順次各周期に行う。

そして、速度偏差値の差１εＶ−ε■′　ｌまたはトル
ク指令値の差＋’ｒｑｃ−Ｔｑｃ’　　ｌの値が大々所
定値以上になったときには、次周期以降のトルク指令［
１Ｔｑｃを「０」とし、サーボモータの回転を停止させ
ロボットの動作を停止させる。

以上のように、前周期と今周期における速度偏差値の差
１ε■−εｖ’　　ｌが所定値以上になったか、又は、
前周期と今周期におけるトルク指令値の差ＩＴｑＣ−Ｔ
ＱＣ’　　Ｉが所定値以上になったか否かによって、即
ち、速度偏差値の微分値、又はトルク指令値の微分値に
よってアーム等の被駆動体が異物に衝突したか否かを検
出りるものである。

その結果、衝突検出の遅れは、最大でデジタルシグナル
ブロセッリ“の基本処理周期の遅れとなるだけであり、
極めて早く衝突を検出できる。

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

発明の効果本発明は、サーボモータで駆動される被駆動体が異物に
衝突したとぎ生じるトルク指令値の変化値（微分値）又
は速度偏差値の変化値（微分値）の増大を検出して、衝
突を検出するようにしたから、衝突を検出する遅れは、
トルク指令値又は速度偏差値を検出する周期の一周期が
最大の遅れとなり、きわめて〒く衝突を検出することが
できる。

【図面の簡単な説明】

第１図は本発明の詳細な説明するためのサーボ系のブロ
ック線図、第２図は本発明の一実施例として適用する産
業用ロボットの制御装置のブロック図である。 ε■・・・速度偏差値、ＴｑＣ・・・トルク指令値、。ｐｃ・・・指令位置、Ｐｆ・・・現在位置、εｐ・・・
位置偏差値、ＶＣ・・・速度偏差値、■・・・現在速度
。 −４■

Claims

【特許請求の範囲】

（１）サーボモータへ出力されるトルク指令値を所定周
期で検出し、前周期で検出されたトルク指令値と今周期
で検出されたトルク指令値の差が所定値以上となつたと
き、サーボモータの回転を停止させるサーボモータによ
り駆動される被駆動体の衝突検出方法。
（２）サーボモータへ指令される速度指令値とサーボモ
ータの回転速度を検出する速度検出器からの検出速度と
の差である速度偏差値を、所定周期で検出し、前周期で
検出された速度偏差値と今周期で検出された速度偏差値
との差が所定値以上となつたとき、サーボモータの回転
を停止させるサーボモータにより駆動される被駆動体の
衝突検出方法。