JPH03196313A

JPH03196313A - 外乱推定オブザーバによる衝突検出方法

Info

Publication number: JPH03196313A
Application number: JP33496789A
Authority: JP
Inventors: Souichi Arita; 創一有田; Tetsuro Kato; 哲朗加藤; Osamu Yoshida; 修吉田
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 1989-12-26
Filing date: 1989-12-26
Publication date: 1991-08-27
Anticipated expiration: 2012-10-22
Also published as: EP0464211A1; EP0464211B1; EP0464211A4; DE69021048D1; WO1991010181A1; JP2665984B2; DE69021048T2

Abstract

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

Description

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

従来の技術ロボットのアーム、または工作機械のテーブルやワーク
が障害物に衝突したとき、アームやテーブルの移動は停
止するが、サーボモータは指令位置まで移動させようと
して大きなトルクを発生する。そのため、機構部の破損
と２次災害を生じる危険性がある。

衝突検出機構のないサーボ系においては、誤差過大アラ
ームによって異常を検出し、これにより衝突を検出する
ようにしている。即ち、アームやテーブル等の被駆動体
が異物と衝突し、その移動が停止した場合、指令位置と
実際の移動位置の差である位置偏差は大きな値となる。

この位置偏差の過大を誤差過大としてアラームを発生さ
せ、このアラームによって衝突を検出するものである。

また、サーボモータの駆動電流値に上限値を設け、この
上限値を越えた場合、アラームを出し、被駆動体が異物
に衝突したことを検出する方法も公知である。

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

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

また、誤差過大アラームで衝突を検出する方法では、位
置偏差が誤差過大アラーム値に達するまでに、モータが
機構部が耐えられないくらいのトルクを発生し機構部を
破壊させる場合がある。さらに、上記モータの駆動電流
値がある上限値以上になったことによって衝突を検出す
る方法も、電流値が上限値に達するまでに時間を要し、
衝突検出が遅れ、機構部や衝突した異物を破損させる恐
れがある。

そこで、本願出願人は、被駆動体が異物に衝突したとき
、速度偏差が増大することから、速度偏差の微分値の大
きさによって衝突を検出する方法、及びトルク指令値の
微分値の大きさによって衝突を検出する方法を提案した
（特願昭６３−３２８８６６号参照）。

しかしながら、上述した速度偏差の微分値の大きさ、ト
ルク指令値の微分値の大きさによって衝突を検出する方
法においても、低速度でサーボモータが動作中に衝突が
生じると、速度指令自体が小さい値であるため速度偏差
はゆっくりと増加し、その結果、微分値は大きな値を示
さず、又、速度偏差の変化（微分値）が小さいからトル
ク指令値の微分値も大きな値とはならず、衝突を検出す
るには衝突を検出するしきい値を小さくしなければなら
ない。しきい値を小さくすれば、通常の動作のときにお
いて衝突が生じないにもかかわらず、衝突を検出するこ
とになり、結局、低速度での動作中は衝突を検出するこ
とができないという問題があった。

そこで、本発明の目的は、機構部の破壊限界に達する前
に確実に衝突を検出する衝突検出方法を提供することに
ある。

課題を解決するための手段本発明は、サーボ回路の速度ループに対し外乱推定オブ
ザーバを組み、該外乱推定オブザーバによって推定され
た外乱の大きさが設定値以上になったとき被駆動体が異
物に衝突したとして衝突を検出するするようにした。

作　　用外乱推定オブザーバを組めば、外乱トルクを推定するこ
とができる。そこで、衝突によるトルク変動を外乱トル
クとみなし、オブザーバで推定される外乱トルクが設定
値以上に達したとき衝突として検出する。この設定値を
機構部の耐強度に合わせて設定しておけば、機構部を破
損するトルクが発生する前に衝突を検出することができ
る。

実施例第２図は、位置に対し比例（Ｐ）制御を行い、速度に対
し比例、積分（Ｐｌ）制御を行うロボット等のサーボモ
ータ制御系のブロック線図であり、伝達関数１０のＫＰ
は位置ループにおける比例ゲイン、伝達関数１２は速度
ループにおける伝達関数で、Ｋ１は積分定数、Ｋ２は比
例定数である。

また、伝達関数１４．１６はモータの伝達関数で１、Ｋ
ｔはトルク定数、Ｊはイナーシャであり、伝達関数１８
が速度υを積分して位置θを算出する伝達関数である。

また、ＴＬは外乱トルクである。

位置指令値θｒから現在位置θのフィードバック値を減
算し、その差の位置偏差ε（＝θｒ−〇）に比例定数Ｋ
Ｐを乗じ、速度指令値を求め、該速度指令値と実速度θ
との差（速度偏差）によってＰ１制御を行ってトルク指
令値としての電流値Ｉを求め、該電流Ｉをモータの巻線
に流し、モータを駆動する。モータは速度θで回転し、
この速度θを積分して位置θが求められる。

この第２図に示すサーボモータに対し、外乱推定オブザ
ーバを組む。まず、サーボモータのサーボ制御をプロセ
ッサで行うデジタルサーボ制御において、従来から行わ
れている外乱推定オブザーバについて説明する。

第３図は、サーボモータにおけるオブザーバ対象のモデ
ルのブロック図で、符号１４は第２図に示すサーボモー
タのトルク定数Ｋｔの伝達関数、１６ａは第２図におけ
る伝達関数１６を分割し、イナーシャＪの伝達関数１６
ａと積分項１６ｂに分けたものである。第３図におて、
■は入力とじてのトルク指令、６．ＴＬは状態変数とし
ての速度、外乱トルクを意味する。

この第３図のモデルにおいて、状態変数θ。

ＴＬに対する状態方程式は次の第（１）式のようになる
。

なお、上記第（１）式においてθは加速度、ＴＬは外乱
トルクの変化度を意味するが、短い時間では外乱トルク
ＴＬには変化がないとして↑ＬＯと仮定している。

の第（１）式で示す方程式より、オブザーバを組む一般
的な手法により、速度θ、外乱ＴＬを推定する同一次元
オブザーバを組むと第１図に符号５０で示すオブザーバ
となる。外乱推定オブザーバ５０の項５２．５３のに３
．に４は外乱推定オブザーバのパラメータであり、項５
１は実際にサーボモータに出力されるトルク指令として
の電流値■に乗じるパラメータの値で、モータのトルク
定数　ＫｔをイナーシャＪで除した値である。

５４は積分項である。

この第１図のブロック図を解析すると、＋Ｉ　−Ｋｊ　
＋ＴＬ　　ｌ　（１／Ｊ−８）　＝１＞（２）（Ｉ・　（Ｋｌ　／Ｊ）　　＋　　（ｆｊ−ｖ）　Ｋ３
＋　　（（９−ｖ）（Ｋ４　／Ｓ）ｌ　　（１／５）＝
ｖ（３）（なお、■は積分項５４の出力で推定速度）第（２）式
よりｒ＝（θ・Ｊ−８−ＴＬ　）　／Ｋｔ　　・・・（４）
第（３）式に第（４）式を代入し整理すると、（６・Ｊ
−３−ＴＬ　）　／Ｊ　十（６−ｖ）　Ｋ３＋（θ−■
）　（Ｋ４／５）＝ｖ・５（５）Ｓ（θ−ｖ）＋（θ−Ｖ）・Ｋ３＋　（θ−ｖ）　　（Ｋ４　／Ｓ）　−ＴＬ　／Ｊ・・
・　（６）第（６）式より・・・　（７）上記第（７）式より項５３の出力Ｘは次の第（８）式で
示される。

（θ−■）・　（Ｋ４　／Ｓ）第（８）式において、パラメータに３．に４を極が安定
するように選択すると、ＸξＴＬ　／Ｊ・・・　（９）となり、外乱トルクＴＬをイナーシャＪで除した値が推
定される。

こうして、外乱推定オブザーバ５０によって得られた外
乱ＴＬに比例した値ｘ（＝ＴＬ／Ｊ）に項６０のパラメ
ータＪ−Ａ（Ｊはイナーシャ、Ａは単位系を合わせるた
めの定数）を乗じれば外乱トルクｙξＴＬが推定される
。この推定外乱トルクＹ＃ＹＬと衝突を検出するために
設定された設定値Ｔｓを比較器６２で比較し、推定外乱
トルクｙが大きければ、アラームを出力し速度ループの
積分器の値を「０」とすると共に以後の速度指令を「０
」にしてモータを停止させる。

衝突を検出するために設定する上記設定値Ｔｓは機械の
静摩擦、バネ系の反力２重力項よりも大きく、かつ、機
構部の破壊限界よりも小さい値に設定する。これに寄り
、機構部が破壊される前に確実に衝突を検出し、モータ
を停止させることができる。

第４図は、デジタルサーボ（ソフトウェアサーボ）のプ
ロセッサが位置、速度ループ処理周期毎−０実施する処理のフローチャートである。

数値制御装置から構成される装置指令とサーボモータに
取付けられたパルスコーダ等によって検出される現在位
置（位置のフィードバック値）より、従来と同様に位置
ループ処理を行って速度指令を算出する（ステップ１０
０）。次に、フラグＦが「１」か否か判断する（ステッ
プ１０１）。

なお、このフラグＦは初期設定で「０」に設定されてお
り、衝突が検出されなければ「０」の状態を維持してい
る。該フラグＦが１１」でなければ、第１図に示す外乱
推定オブザーバ５０及びブロック６１の処理を実行し、
推定外乱トルクｙを求める（ステップ１０２）。そして
、求められた推定外乱トルクｙが衝突を検出するために
設定されたしきい値１８以上か否か判断しくステップ１
０３）、しきい値Ｔｓより小さければ、ステップ１００
で求められた速度指令とサーボモータに取付けられたパ
ルスコーダ等の速度検出器で検出されるサーボモータの
実速度より、従来と同様に速度ループ処理を行い電流指
令（トルク指令）を算出し、電流ループに該電流指令を
引き渡し、当該周期の処理を終了する（ステップ１０６
，１０７）。

一方、ステップ１０３で算出された推定外乱トルクｙが
しきい値１８以上のときには、衝突が検出されたとして
アラームを出力し、フラグＦを「１」にセットし、速度
指令を「０」、速度ループの積分器を「、０」にする（
ステップ１０４，１０５）。そして、ステップ１０６の
速度ループ処理では速度指令「０」、速度ループの積分
器が「０」として速度ループ処理を行い、電流指令を求
め、電流ループに引渡す。

次の周期からはフラグＦが「１」にセットされているか
ら、ステップ１０１からステップ１０５へ移行し、速度
指令を「０」、速度ループの積分器を「０」として速度
ループ処理を行って電流指令を求め、電流ループに引渡
すこととなる。

なお、上記実施例では衝突検出について説明したが、過
負荷時のアラームも本方式によって検出可能である。

発明の効果１２本発明は、外乱推定オブザーバによって外乱トルクを推
定し、該推定された外乱トルクが設定されたしきい値以
上になると衝突発生として検出するようにしたので、上
記設定するしきい値を機械の摩擦力、バネ系の反力１重
力項よりも大きくし、かつ、機構部の耐強度より小さい
値に設定することにより、確実に機構部の破損を防止す
ることができる。

【図面の簡単な説明】

第１図は本発明の一実施例の外乱推定オブザーバのブロ
ック図、第２図はサーボモータ制御系のブロック図、第
３図は外乱推定オブザーバを組むモデルのブロック図、
第４図はデジタルサーボのプロセッサが行う本実施例の
位置、速度ループ処理及び外乱推定オブザーバ処理のフ
ローチャートである。５０・・・外乱推定オブザーバ、■・・・トルク指令、
ＴＬ・・・外乱トルク、θ・・・サーボモータの実速度
、■・・・推定速度、ｙ・・・推定外乱トルク。３

Claims

【特許請求の範囲】

少なくとも位置ループと該位置ループのマイナーループ
として速度ループを有するサーボ回路によってサーボモ
ータを駆動制御し該サーボモータで駆動される被駆動体
の衝突検出方法において、速度ループに対し外乱推定オ
ブザーバを構成し、推定された外乱の大きさが設定値以
上になったとき、被駆動体が異物に衝突したとして衝突
を検出するようにした、外乱推定オブザーバによる衝突
検出方法。