JPH033013A - サーボモータで駆動される被駆動体の衝突検出方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 産業上の利用分野 本発明は、数値制ｍ＋装置で制御される工作橢械や産業
用ロボット等におけるづ一ボモータによって被駆動体を
移動させるものにおいて、被駆動体が移動中に異物と衝
突したことを検出する検出方法に関する。

従来の技術 サーボモータで駆動される工作機械やロボットにおいて
、サーボモータの駆動により移動させられる被駆動体が
、その移動途中において異物と衝突した場合、この衝突
を検出する方法として、被駆動体が異物に衝突しその移
動が停止または低速となり位置偏差が増大することから
、この位置偏差過大によって衝突を検出する方法が一般
的である。

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

また、ロボットにおいては、ロボットの表面にタッチセ
ンサ等を取イ・４け、該タッチセンサで異物とロボット
の接触を検出し、衝突を検出する方法が採用されている
。

さらに、衝突が生じたとき、速度偏差値やトルク指令値
が増大することから、これら速度偏差値やトルク指令値
の増加度より衝突を検出する方法も、本願出願人によっ
て特願昭６３−３２８８６６号等によって提案されてい
る。

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

また、位置偏差過大や、上記モータの駆動電流値がある
上限ｆｆｉ以上になったことによって衝突を検出する方
法も、位置偏差値が設定所定値まで、または電流値が上
限値に達するまでの時間を要し、衝突検出に遅れが出て
くる。特に、位置偏差値が小さく、その結果、電流値も
小さい状態で衝突が生じた場合には、特に衝突検出が遅
れる。

衝突検出が遅れると、その検出遅れの間、サーボモータ
からトルクが出力され、被駆動体や機構部等、及び、衝
突した異物に破損が生じるおそれがあり、できるだけ早
く衝突を検出することが望ましい。

そこで、本発明の目的は、サーボモータで駆動される被
駆動体が異物に衝突したことを短時間で検出することが
できる衝突検出方法を提供することにある。

課題を解決するための手段 本発明は、サーボ回路への位置指令に対し、サーボモー
タが移動する位置を求めるモデルを作り、サーボ回路で
求められた位置偏差値と上記モデルで求められた位置偏
差値との差が所定値以上のとき、サーボモータで駆動さ
れる被駆動体が異物に衝突したとして衝突を検出する。

特に、上記モデルをサーボ回路に入力される位置指令か
らモデルで求められる位置を減じてモデルにおける位置
偏差値を求め、該位置偏差値にサーボ回路の位置ループ
ゲインを乗じてモデルにお番ノる速度指令値を求め、該
速度指令値を積分してモデルにおける位置を求めるモデ
ルとし、簡単な構成とする。即ち、サーボ系における速
度指令に対し、サーボモータの応答速度までの伝達関数
を「１」とした簡単なモデルで構成する。

さらに、実際のサーボ系においては、加速度に応じて定
常速度偏差が生じることがあるので、モデルにおける速
度指令を微分し、この微分値に加速度補償ゲインを乗じ
て加速度補償値を求め、サーボ回路から得られる位置偏
差値から、モデルから得られる位置偏差値及び加速度補
償値を減じて１ｑられる値が所定値以上になると衝突が
生じたとして検出する。

作　　用 サーボ回路に入力される位置指令に対し、サーボモータ
が応答し移動する位置を求める。サーボモータをも含め
たサーボ系のモデルを作れば、正常にサーボ系が駆動し
ている状態では、サーボ回路における位置偏差値とモデ
ルで求められる位置偏差値には格別差が生じない。しか
し、サーボモータで駆動される被駆動体が移動途中で異
物に衝突し、停止または低速になると、実際のか−水回
路における位置偏差値は増大し、モデルで求められる位
置偏差値との差が増大する。そこで、この差が所定値以
上になることによって衝突を検出するようにする。

また、実際のサーボ回路においては、加速度に応じて定
常偏差が生じることがあり、この加速度の影響を補償す
るためにモデル内にモデルにおける速度指令を微分し、
この微分値に加速度補償ゲインを乗じて加速度補償値を
求め、実際のサーボ回路で得られる位置偏差値からモデ
ルで求められた位置偏差値及び加速度補償値を減じた値
が所定値以上になることによって衝突を検出する。

実施例 第１図は、本発明の一実施例のブロック図である。

図中、ｐｃはＮＣ＠置等から構成される装置指令であり
、２は位置制御部で、Ｋは位置ループゲインを表わす。

４は速度制御部、６は電流制御部、８はモータ部であり
、Ｊはイナーシャである。

１０はサーボモータの速度■を積分して位置を算出する
積分器である。

位置指令ｐｃが入力されると、該位置指令ｐｃから位置
検出器等で検出される現在位置のフィードバック値Ｐｆ
を減じて位置偏差値εＣを求め、該位置偏差値εＣに位
置ループゲインＫを乗じて速度指令値ＶＣを求める。速
度制御部４では速度指令値ＶＣと速度検出器で検出され
るサーボモータの実速度Ｖにより速度ループ制御を行い
、トルク指令１ｉａ（駆動電流値）を出力する。通常、
この速度制御部は比例・積分制御（ＰＩ副制御または積
分・比例制御（ＩＰｔ、ｆｌａｌｌ）が行われる。次に
、電流制御部６ではトルク指令値に基いてトランジスタ
インバータ制御等を行い、サーボモータに流す電流の制
御を行いサーボモータを駆動する。

以上述べたサーボ系は従来のサーボ系と構成。

作用は同一であり、詳細は省略する。

本発明においては、上記サーボ系のモデルを作る。本実
施例においては、第１図に符号１２で示す上記サーボ系
の速度指令ＶＣを入力し、サーボモータが速度■で応答
するまでの伝達関数を「１」とした位置制御部のゲイン
にと積分器で構成される伝達関数１６．１８からなるサ
ーボ系のモデル１４を作る。

即ち、サーボ回路に入力される位置指令ｐｃを入力し、
当該モデル１４で得られる位ｉｐｍを位置指令Ｐｃから
減じてモデル１４における位置偏差値εｍを求め、該位
置偏差値εｍにサーボ回路の位置ゲインＫを乗じてモデ
ルにおける速度指令値ｖｍを求め、該速度指令ｆｉｎＶ
ｍを積分器１８で積分して、モデル１４における位置Ｐ
ｍを求めるモデル１４を作る。そして、実際の勺−水回
路で得られた位置偏差値εＣからモデル１４で求められ
た位置偏差値εｍを減算器２０で減じ、その値ｙ（−ε
Ｃ−８ｍ）の絶対値が設定所定値より大きいか否か比較
器２２で判断し、大きい場合、衝突検出信号を出力する
ようにする。

サーボモータによって駆動される被駆動体が正常に移動
していれば、実際のサーボ回路における位置偏差値εＣ
とモデル１４における位置偏差値εｍには大きな差はな
く、その差ｙの値は比較器２２で比較される基準値より
も小さく、衝突検出信号は出力されない。

しかし、被駆動体が移動途中において異物と衝突すると
、被駆動体はその移動を停止するか、移動速度が非常に
遅くなる。しかし、位置指令Ｐｃは順次入力されるので
、実際のり−水回路の位置偏差値εＣは増大する。一方
、モデル１４においては、通常どおり位置ｐｍを篩出し
出力するので、モデル１４における位置偏差値εｍは正
常どおりの値である。その結果、実際のサーボ回路の位
置偏差値εＣとモデル１４の位置偏差値εｍとの差の絶
対値１ｙｌ＝ｌεＣ−εｍｌは増大し、比較器２２に設
定された基準値以上になると、衝突検出信号を出力する
こととなる。

上記実施例では、実際のサーボ系における速度指令ｖｃ
から、サーボモータが駆動され実速Ｉｎ　Ｖが得られる
までの伝達関数を１゛１」と仮定してモデルを作成して
いる。しかし、実際には上記伝達関数はｌ−１Ｊではな
い。例えば、速度制御部４ではＰＩ副制御たはＩＰ制御
が行われ、伝達関数は１１」ではない。そのため、実際
には加速度に応じて定常偏差が生じることがある。そこ
で、この加速度の影響を補償して衝突を検出する第２の
実施例の方法の７０ツク図を第２図に示す。

第２図において、第１図と相違する点は、モデル２４に
おける速度指令ｖｍを微分し、その微分値に加速度補償
ゲインαを乗じて加速度補償値εαを求める加速度補償
の項２６が加わった点、及び、実際のサーボ回路の位置
偏差値εＣからモデル２４における位置偏差値εｍ及び
加速度補償値εαを減算器２８で減じて得られる値１ｙ
１＝εＣ−εｍ−εα１が基準値以上か比較器２２で判
断し、基準値以上のときに衝突検出信号を出力する点が
第１図に示す第１の実施例と相違するのみであり、同一
の構成は同一符号を付している。

上記第１．第２の実施例の回路は専用回路で構成しても
よいが、上記サーボ制御をデジタル勺−ボ制御するプロ
セッサによって上記モデル１４゜２４等の処理をｔｌっ
て衝突を検出する方法について、以下、説明する。

プロセッサで構成されるデジタルサーボ回路の構成はす
でに公知であるので、その構成の説明は省略する。通常
、デジタルサーボ回路においては、ＮＣ装置等から所定
同期毎位置指令Ｐｃを共有メモリを介して受信し、この
受信した位置指令Ｐｃに基いて、位置ループ処理、速度
ループ処理等を行いサーボモータを駆動している。本発
明においては、デジタルサーボ回路のプロセッサは、こ
のサーボ制御処理の外に、上記したモデル１４゜２４の
処理及び衝突検出判断処理をも実行するものである。

第３図は、第１図に示す第１の実施例を実行するデジタ
ルサーボ回路のプロセッサの処理を示すフローチャート
であり、該プロセッサは所定周期毎、この７０−チャー
トで示す処理を実行する。

まず、フラグＦが「１」か否か判断しくステップＳ１）
　〈初めは初期設定でＩ−ＯＪがセットされている）、
「１」でなければ、共有メモリを介してＮＣ装置から送
られてくる位置指令Ｐｃを読み（ステップＳ２）、該位
置指令ｐｃと位置検出器から構成される装置フィードバ
ック信号とにより、通常行われる位置ループ処理を行い
、速度指令値Ｖｃ及び位置偏差値εＣを求める（ステッ
プ８３）。そして、次のステップ８４〜Ｓ７によるモデ
ルの処理を実行する。まず、位置指令ｐｃからレジスタ
Ｒ（Ｐｍ）に記憶する前周期のモデル処理における位ｅ
Ｐｍを減じてモデルにおける位置偏差値εｍ　（＝Ｐｃ
−Ｐｍ）を求め（ステップＳ４）、該位置偏差値εｍに
実際のサーボ回路における位置ループゲインＫを乗じて
、モデルにおける速度指令値Ｖｍ　（−Ｋ・εｍ）を算
出しくステップＳ５）、算出された速度指令値ｖｍをレ
ジスタＲ（Ｐｍ）の値に加算してモデルにおける位！１
ＰｍをレジスタＲ（Ｐｍ）に記憶する（ステップ８６）
。そして、ステップＳ３で求めた実際の位置偏差値εＣ
からステップＳ４で求めたモデルにおける位置偏差値ε
ｍを減じてその誤差ｙを求め（ステップＳ７）、この誤
差ｙの絶対値が設定所定値Ｅよりも大きいか否か判断す
る（ステップ３８）。被駆動体がサーボモータによって
正常に駆動され移動していれば、上記誤差ｙの絶対値ｌ
ｙｌは所定値Ｅより小さいので、この場合はステップＳ
３で求めた速度指令ｆＩｉｖｃ１．：基いて従来と同様
に速度ループ処理を行い（ステップ５１１）トルク指令
値を求めて電流ループ処理へ出力し、当該周期の位茸、
速度の処理は終了する。

以下、被駆動体が異物等に衝突ゼす、上記誤差ｙの絶対
値が所定値Ｅ以上にならなければ、プロセッサはステッ
プ８１〜Ｓ８．Ｓ１１の処理を所定同期毎繰返し、従来
と同様な位置、速度ループ処理を行う。

しかし、被駆動体が異物と衝突し、被駆動体の移動が停
止または速度が低下すると、実際のサーボ回路の位置偏
差値εＣが増大し、上記誤差ｙも増大する結果、誤差ｙ
の絶対値は所定値Ｅ以上となり、ステップＳ８からステ
ップＳ９へ移行し、速度ループ処理における積分器の処
理を行うアキュムレータＡの値を「０」にセットし、フ
ラグＦを「１」にセットする（ステップ８９）。そして
、速度指令値Ｖｇを「０」にセットしくステップ５１０
）、該速度指令値Ｖｃ　（−０）によって速度ループ処
理を実行する（ステップ５１１）。その結果、速度指令
値ＶＣが１“Ｏ」で、積分器（アキュムレータＡ）の出
力がｒＯＪであるので、速度ループ処理では現在の実速
度Ｖとは逆方向のトルク指令値を出力することとなり、
サーボモータは停止することとなる。そして、フラグＦ
が１−１」にセットされることから、次の周期からはス
テップＳ１からステップＳ１０へ移行し、速度指令値Ｖ
Ｃを「０」として速度ループ処理が行われ、サーボモー
タは停止する。

なお、ステップＳ８の処理によって衝突が検出されたと
きアラームを表示するようにするとよい。

以上が第１の実施例のデジタルサーボ回路のプロセッサ
の処理であるが、次に、第２の実施例におｔプるブロセ
ッ１ノの処理を、第１の実施例と相違する点を中心に第
４図で示すフローチャートと共に説明する。

この第２の実施例の処理は第３図に示す第１の実施例の
処理において、ステップ８４〜Ｓ７の処理が第４図に示
すステップ８１００〜５１０５の処理（モデル２４の処
理）に置き換わったものである。

即ち、ステップ８１〜Ｓ３の処理を行い、位置指令ＰＣ
から前周期の処理で得られた［デルにおける位置を減じ
てモデルにおける位置偏差値εｍを求め（ステップ５１
００）、この位置偏差値εｍに位置ループゲインＫを乗
じて速度指令値ｖｍを求め（ステップ８１０１）、この
速度指令値ＶｍからレジスタＲ（Ｖｍ）に記憶する眞周
期の速度指令値を減じた値に加速度補償ゲインαを乗じ
て加速度補償値εαを求め（ステップ５１０２）、レジ
スタＲ（Ｖｍ）にステップ５１０１で求めた今周期の速
度指令値Ｖｍを格納しくステップ３１０３）　、位置Ｐ
ｍを記憶するレジスタＲ（Ｐｍ）にこの速度指令値ｖｍ
を加算しくステップ８１０４）、実際のサーボ回路の位
置偏差値εＣ（ステップＳ３で求めたもの）から、ステ
ップ８１００．ステップ５１０２で求めたモデルの位置
偏差値εｍ、加速度補償値εαを減じて誤差ｙを求め（
ステップ５１０５）、ステップＳ８へ移行する。この処
理により、加速度による定常偏差の影響は除去され、衝
突をより正確に（所定値Ｅの値を小さくすることができ
る）検出することができる。

発明の効果 本発明は、サーボ系のモデルを組み、該モデルによって
得られる位置偏差値と実際のサーボ回路によって得られ
る位置偏差値との差によって、さらには加速度の影響を
補償して、サーボモータで駆動される被駆動体の異物へ
の検出するようにしたので、衝突検出が早く、衝突によ
り被駆動体の機構部や異物を破損させる可能性は少なく
なる。

【図面の簡単な説明】

第１図は、本発明の第１の実施例のブロック図、第２図
は、本発明の第２の実施例のブロック図の要部図、 第３図は、同第１の実施例のデジタルサーボ回路のプロ
セッサによるフローチャート、第４図は、同第２の実施
例のデジタルサーボ回路のプロセッサによる処理のフロ
ーチャートの一部を示す図である。 ２・・・位置制御部、４・・・速度制御部、６・・・電
流制御部、８・・・モータ部、１０・・・積分器、１４
．２４・・・モデル、２２・・・比較器、Ｋ・・・位置
ループゲイン、Ｐｃ・・・位置指令、εＣ・・・位置偏
差、ＶＣ・・・速度指令、εｍ・・・モデルにおける位
置偏差、Ｖｍ・・・モデルにおける速度指令、Ｐｍ・・
・モデルにおける位置。 第 呪 第 図 スｔ−，プＳ８へ

Claims (3)

【特許請求の範囲】
1. （１）サーボ回路への位置指令に対し、サーボモータが
   移動する位置を求めるモデルを作り、サーボ回路で求め
   られた位置偏差値と上記モデルで求められた位置偏差値
   との差が所定値以上のとき、サーボモータで駆動される
   被駆動体が異物に衝突したとして検出するサーボモータ
   で駆動される被駆動体の衝突検出方法。
2. （２）上記モデルは、サーボ回路に入力される位置指令
   からモデルで求められる位置を減じてモデルにおける位
   置偏差値を求め、該位置偏差値にサーボ回路の位置ルー
   プゲインを乗じてモデルにおける速度指令値を求め、該
   速度指令値を積分してモデルにおける位置を求めること
   を特徴とする請求項１記載のサーボモータで駆動される
   被駆動体の衝突検出方法。
3. （３）サーボ回路に入力される位置指令からモデルで求
   められる位置を減じて位置偏差値を求め、該位置偏差値
   にサーボ回路の位置ループゲインを乗じて速度指令値を
   求め、該速度指令値を積分して位置を求めると共に、上
   記速度指令値を微分して加速度補償ゲインを乗じて加速
   度補償値を求めるサーボ系のモデルを作り、サーボ回路
   の位置偏差値から上記モデルで求められる位置偏差値及
   び加速度補償値を減じて得られる値が所定値以上のとき
   、サーボモータで駆動される駆動体が異物に衝突したと
   して検出するサーボモータで駆動される被駆動体の衝突
   検出方法。