JPH033687A - サーボモータにより駆動される被駆動体の衝突破損防止方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 産業上の利用分野 本発明は、産業用ロボットや産業機械等のサーボモータ
で駆動される機械において、被駆動体が異物と衝突した
とき、被駆動体及び衝突した異物の破損を極力小さくす
る方法に関する。

従来の技術 産業用ロボットや産業機械等のサーボモータで駆動され
る機械においては、サーボモータで駆動され、移動中の
被駆動体が、その移動途中において異物と衝突したこと
を検出する方法として、被駆動体表面にタッチセンサを
設け、該タッチセンサからの信号によって衝突を検出す
る方法、または、サーボモータの駆動電流値即ちトルク
指令値に上限値を設け、この上限値を越えた場合アラー
ムを出力し、被駆動体が異物に衝突したことを検出する
方法が採用されている。侵者のこの方法は、指令された
位置までサーボモータは被駆動体を移動させ被駆動体を
位置決めさせるが、この移動途中で、異物に被駆動体が
衝突し被駆動体が移動しなかったとき、位置指令と実位
置との差である位置偏差値、速度指令と実速度との差で
ある速度偏差値が大きくなるから、サーボモータへの駆
動電流値も大きくなり被駆動体を大きな力で駆動しよう
とする。そのため、この駆動電流値を監視し、ある上限
値を越えると被駆動体が異物に衝突したものとして検出
するものである。

また、サーボモータの駆動で移動する被駆動体が移動途
中で異物に衝突しその移動が停止することから、速度偏
差の変化呂、トルク指令値の変化量を検出し衝突を検出
する方法も、本願出願人によって特願昭６３−３２８８
６６号で提案されている。

衝突を上述した各方法で検出した後の被駆動体の停止方
法としては、衝突検出後サーボ駆′＠電源を切断し、サ
ーボモータに生じる逆起電力によるダイナミックブレー
キ、または、サーボモータに取付けられたブレーキや被
駆動体に取付けられたブレーキを利用して移動を停止さ
せる方法が一般的に採用されている。

また、衝突が検出されると、以後トルク指令値を「０」
にする方法や衝突検出侵、サーボモータに出力する速度
指令をｒＯＪにし、これにより衝突前の実速度とは逆方
向のトルク指令が出力されるように制御する方法が上記
特願昭６３−３２８８６６号等で提案されている。

発明が解決しようとする課題 被駆動体が移動中異物に衝突し、この衝突を検出してサ
ーボ駆動電源を切断しても、また、特願昭６３−３２８
８６６号で提案されたように、トルク指令値を「０」に
したり、速度指令値をｒＯＪにすることによって被駆動
体を停止させようとしても、衝突検出が所定周期毎に行
われるため、衝突時の被駆動体の実速度と衝突から衝突
検出までの時間遅れ分に応じて被駆動体の停止するまで
の移動距離が決まり、衝突点より行きすぎた点で停止す
ることとなり、被駆動体及び異物は互いに応力がかかり
、これらが損傷してしまうことがある。

また、上述したような、モータ駆動電流、即ち、トルク
指令値が設定された上限値を超えることによって、衝突
を検出する方法では、衝突面のサーボモータの出力トル
クが小さいとぎに（モータ駆動電流が小さいときに）、
衝突が生じた場合にはトルク指令値が上限値に達し、衝
突を検出するまでに時間を要し、かつ、設定された上限
値までトルク指令値（モータ駆動電流）が出力されるこ
とになるため、被駆動体及び衝突した異物には大きな応
力が加わることになる。

特に、被駆動体の駆動方向によってサーボモータの出力
トルクが異なる場合、例えば被駆動体を上下方向に駆動
する場合、被駆動体を上方向に駆動する場合には重力方
向とは逆に駆動することからサーボモータのトルク指令
値（駆動電流）は大きく、下方向に駆動する場合にはサ
ーボモータのトルク指令値（駆動電流）は上方向への駆
動と比べ小さなものとなる。

しかし、サーボモータへのトルク制限値は、被駆動体を
駆動するに必要な最大トルク指令値によって決定される
ため、上下方向に被駆動体を駆動する場合には、被駆動
体を上方向に駆動するとき必要な最大トルク指令値に若
干のマージンをとった値がトルク制限値として設定され
ている。そして、このトルク制限値はサーボモータの回
転方向に関係なく設定され、例えば、トルク制限値が△
であったとすると、プラス方向のトルク指令値をＡ以下
に制限し、マイナス方向のトルク指令値を−Ａ以上に制
限する。即ち、トルク指令値の絶％ｊ値がＡ以下になる
ように制限する。

その結果、被駆動体が上方向に駆動されているときに衝
突が生じた場合は、衝突からさらにサーボモータが、ト
ルク制限値までトルクを出力するまでの時間は短く、か
つ、衝突によって、衝突した異物及び被駆動体に生じる
応力も大きなものとはならない。しかし、下方向に被駆
動体が駆動されているときに衝突が生じると、下方向へ
駆動するサーボモータの出力トルクはもともと小さいの
で、衝突が生じてトルク制限値まで達するのに時間を要
し、かつ、摩擦抵抗等を差し引いたサーボモータの全ト
ルクが被駆動体に加わり、また、異物へも加わることに
なり、被駆動体及び異物を損傷させる可能性が大きくな
る。

そこで、本発明の目的は、被駆動体の移動途中において
異物への衝突が生じたとき、被駆動体及び異物に作用す
る外力（応力）を軽減させ、衝突破損を防止する方法を
提供することにある。

課題を解決するための手段 本発明は、サーボモータで被駆動体を駆動中、被駆動体
が異物と衝突したことを検出すると、該衝突検出時のサ
ーボモータの実速度とは逆方向の所定速度指令を所定時
間与えてサーボモータを逆転させることによって被駆動
体を異物から引き離し、衝突によって生じた外力を被駆
動体及び異物から取り除く。

また、駆動方向によって、サーボモータに必要な最大ト
ルク指令値（駆動°電流）が異なる場合、駆動方向に応
じて、その駆動方向に必要な最大トルク指令値に基きト
ルク制限値を夫々設定することにより、衝突が生じた場
合、過大なる力（応力）が被駆動体及び衝突した異物に
加わらないようにして破損を防止する。

作　　用 被駆動体と異物の衝突が検出された時点においては、衝
突検出遅れに応じた分の速度偏差値により出力されるサ
ーボモータのトルクによって、被駆動体及び異物は互い
に圧接し、応力が生じ、また、変形が生じている。そこ
で、衝突が検出されると、衝突前のり一ボモータの実速
度とは逆方向の所定速度指令を所定時間与えることによ
り、サーボモータを逆転させ、被駆動体を異物から引き
離す。これにより被駆動体及び異物の破損を防止する。

また、被駆動体の駆動方向によってサーボモータが出力
するに必要な最大トルク指令値が異なるような場合（例
えば被駆動体を上下方向に駆動するような場合）、被駆
動体の駆動方向に応じて、その駆動方向に必要な最大ト
ルク指令値に基いてサーボモータのトルク制限値を夫々
設定することによって、被駆動体がどちらの方向に駆動
されていようとも、その駆動途中において衝突が生じた
場合、直ちにトルク制限値にサーボモータの出力トルク
が達し、それ以上のトルクを出力しないから、被駆動体
及び異物には過大な力が作用することはない。

実施例 第２図は本発明を実施する一実施例のデジタルサーボ制
御におけるブロック図である。

、図中、符号２はサーボモータを有するロボットや産業
用機械を制御する数値制御装置（以下、ＮＣ装置という
）で、該ＮＣ装置２から各サーボ七−夕への移動指令を
共有メモリ４を介してデジタルサーボ回路６に出力し、
デジタルサーボ回路６はプロセッサ等で構成され、共有
メモリ４を介して入力された移動指令に対し、位置ルー
プ制御。

速度ループ制御等を行い、サーボアンプ８にトルク指令
値（駆動電流値）を出力し、サーボアンプ８はサーボモ
ータ１０を駆動する構成となっている。なお、符号１２
はサーボモータ１ｏの１回転当りに所定数のフィードバ
ックパルスを出力するパルスコーダである。

上記デジタルサーボ制御の構成は従来から公知の構成と
何ら変るものではない。

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

デジタルサーボ回路６は、共有メモリ４を介して受信す
るＮＣ装置２からの当該軸への位置指令Ｐｃと、当該軸
のサーボモータに取付けられたパルスコーダで検出され
る現在のモータ速度Ｖを積分処理（２８）Ｉ、て得られ
る現在位置Ｐｆとの差である位置偏差値εｐ（εｐ＝・
Ｐｃ−Ｐｆ）を求め、位置ループ制御処理（２０）を行
って速度指令値ｙｃを求め、速度ループ制御処理（２２
）。

（２４）では、この求められた速度指令［Ｖｃとパルス
コーダで検出される現在速度Ｖとの差である速度偏差値
εＶに比例する値を求め、該値と速度偏差値ε■を積分
（２２）ｔ、て得られる値を加算してトルク指令値ＴＣ
を求め、このトルク指令値ＴＣに基き、サーボアンプで
はインバータ制御を行い入力されたこのトルク指令ｉｃ
［Ｔｃに応じてサーボモータに駆動電流を流し、サーボ
モータを速度■で駆動する。

以上は、従来から行われているサーボ制御の動作である
が、本発明においては、さらにサーボモータ１０の駆動
中の被駆動体の異物への衝突検出処理をデジタルサーボ
回路６のプロセッサが行う。

第１図はこの衝突検出処理を含む制御処理の一実施例の
フローヂャートであり、デジタルサーボ回路６のプロセ
ッサは速度ループ制御処理周期毎に第１図に示す処理を
行う。

まず、フラグＦが「１」にセットされているか否か判断
する（ステップＳ１）。なお、このフラグＦはＮＣ装置
２がリセットされたとき「０」にリセットされている。

フラグＦが［０，１であると、位置ループ制御処理（２
０）で従来と同様にして求められた速度指令値ＶＣから
当該サーボモータに取付けられたバルスコーダ１２から
の検出速度、即ち、サーボモータの実速度Ｖを減じて速
度偏差値ε■を求める（ステップ８２）。この求められ
た速度偏差値ε■から前周期の速度偏差値εＶ′を減じ
、その絶対値が設定記憶されている所定値ＥＲＲより小
さいか否か判断する（ステップＳ３）。

ステップＳ３で所定値ＥＲＲより１ε■−εＶ゛が小さ
いとアキュムレータＡに速度指令＋ｔｎ　ｖ　ｃから実
速度■を減じた値（即ち、速度偏差値ε■）に積分ゲイ
ンに１を乗じた値（Ｖｃ−Ｖ）ＸＫ１を加算する（ステ
ップ８４）。なお、このステップＳ４の処理が第３図に
おける積分処理（２２）に対応する。

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

次に、速度指令値ＶＣから実速度■を減じた値をレジス
タに格納し、次周期のステップＳ３で使用する前周期の
速度偏差値εＶ′として記憶する（ステップ８６）。以
下、デジタルサーボ回路のプロセッサの速度周期毎上記
処即を行ってサーボモータを駆動制御する。

上述のように、サーボモータが駆動制御されている中で
、サーボモータで駆動される被駆動体が異物と衝突する
と、位置偏差εｐは増大し速度指令１ｆｉＶｃは増大す
ると共にサーボモータの実速度はｒＯＪまたは低速度と
なる。

その結果、ステップＳ２で求められる速度偏差値ε■の
値は大きくなり、ステップＳ３で、この速度偏差値ε■
と前周期での速度偏差値εＶ′の差の絶対１ｉｉｌεｖ
−εｖ’　　ｌは、所定値ＥＲＲに以上になったことが
検出される。すると、プロセッサはフラグＦを「１」に
セットし、アラーム表示しくステップＳ７）、位置ルー
プ制御処理によって出力されている現在の速度指令値Ｖ
ｃが正か負か判断しくステップＳ８）、正ならば設定さ
れている所定値■ａの負の値−Ｖａを速度指令値を記憶
するレジスタＲ（Ｖｃ）に格納しくステップＳ９）、速
度指令値ＶＣが負ならば、上記所定値■ａの正の値十Ｖ
ａを上記レジスタＲ（ＶＣ）に格納する（ステップ５１
０）。即ち、現在の速度指令値ＶＣとは逆方向の所定値
ｙａの速度指令をレジスタＲ（Ｖｃ）に格納する。そし
て、アキュムレータＡ及びカウンタｍの値を１゛０」に
しくステップ５１１）、カウンタｍを「１」インクリメ
ントし、レジスタＲ（ＶＣ）に格納された速度指令＋Ｖ
ａまたは−Ｖａを、速度指令ＶＣとして出力する（ステ
ップ５１３）。カウンタｍの値が設定値Ｍ以上でなけれ
ば（ステップ８１４）、ステップＳ５へ移行し、設定さ
れたアギュムレータＡの値（△−０）、速度指令Ｖｃ　
（＝＋Ｖａまたは−Ｖａ）と現右の実速度Ｖよりトルク
指令値ＴＣを求め出力する。その結果、サーボモータ１
０は衝突前までの回転方向とは逆方向に回転することと
なる。そして、速度指令ＶＣから実速度Ｖを減じて速度
偏差値ε■′を記憶しくステップ８５）、当該処理周期
の処理を終了する。

次の速度ループ制御処理周期からは、フラグＦが１１１
にセットされていることから、ステップＳ１からステッ
プ８１２へ移行し、カウンタｍを「１」インクリメント
し、速度指令（ｉＱＶｃをレジスタＲ（Ｖｃ）に格納さ
れた値としてカウンタｍが設定値Ｍに達するまで、レジ
スタＲ（Ｖｃ）に格納された速度指令値（Ｖｃ＝＋Ｖａ
又は−Ｖａ）によりトルク指令値Ｔｃを求め出力し、速
度偏差値εＶ′を記憶する処理を繰返すこととなる（ス
テップＳｌ、８１２〜８１４．Ｓ５．Ｓ６）。そして、
ステップＳ１４でカウンタｍの値が設定値Ｍ以上になる
と、従来と同様の非常停止処理を行ってサーボモータの
駆動を停止する（ステップＳ１５）。

なお、本実施例はステップ８１４からステップＳ５へ移
行するようにしたが、ステップ３１４からステップＳ４
へ移行し衝突が生じ、サーボモータの回転方向を逆転さ
せたときにも積分制御を行うようにしてもよい。また、
このときは、アキュムレータＡをステップＳ１１で１゛
０」にセットしなくてもよい。

第５図（ａ）〜（Ｃ）は本実施例の動作状態の一例を示
す説明図で、サーボモータ１０の駆動力をプーリ５１．
タイミングベルト５２．プーリ５３を介して軸受５７．
５８で軸支されたボールネジ５４へ伝達し、ボールネジ
５４と螺合するポールナツト５５に取付けられた被駆動
体７を上下動させる機構を示すもので、サーボモータ１
０を駆動し被駆動体７を速度■で下降させているとき、
その移動途中で第５図（ｂ）に示すように異物５９に衝
突すると、この衝突を検出してサーボモータ１０は逆転
し、被駆動体５６を設定速度Ｖａで所定時間（カウンタ
ｍの値が設定値Ｍに達するまで）駆動する。その結果被
駆動体５６と異物５９は離れ、第５図（Ｃ）に示す状態
で停止することになる。

なお、衝突が検出されてサーボモータ１０を逆転させず
に、従来のように衝突を検出して単にサーボモータ１０
の駆動を停止させる方法では、衝突検出遅れをｔＬ笥突
を検出してから被駆動体の速度が「０」になるまでの時
間をｔ２とすると、異物５９への衝突から被駆動体５６
の移動路ＭＬは次式のようになる。

その結果、衝突検出後、停止した状態は第５図（ｄ）に
示すように、被駆動体５６．異物５９には互いに外力が
加わり、被駆動体５６及び異物５９は変形し損傷、破損
を生じることとなる。

また、第５図に示すように被駆動体５６が上下方向に駆
動される場合、被駆動体５６を上方向に駆動する場合と
下方向に駆動する場合では、サーボモータ１０が出力す
るのに必要な上方向の最大トルクと下方向の最大トルク
は異なる。被駆動体５６の移動を停止させている状態に
おいてでも、被駆動体５６を停止状態に保持するため、
サーボモータ１０は上方向へのトルクを出力し続ける必
要がある。

そして、重力に逆らって上方向に被駆動体５６を駆動す
るのに必要なトルクは、下方向に駆動させるときに必要
なトルクより大きなトルクを必要とする。

そのため、従来のように、被駆動体５６を移動させるの
に必要な最大トルク値をトルク制限として設定すると、
被駆動体５６を下降させるとき異物５９への衝突が生じ
ると上行時に必要なトルクに合せて設定されたトルク制
限値までサーボモータ１０がトルクを出力するおそれが
あり、被駆動体５６．異物５９を損傷させる可能性が大
きい。

そこで本発明は、このように被駆動体の移動方向によっ
て必要とする最大トルクが異なる場合、移動方向毎にト
ルク制限を行う。

第４図は、このようなトルク制限を行うときのデジタル
サーボ回路６が行うサーボ制御のブロック線図で、第３
図の従来のブロック線図と比較し、トルク制限を行う項
３０が付加されている。

移動方向に応じてトルク制限値＋Ｔａ、−Ｔｂを設定し
、速度ループ制御処理によって算出されるトルク指令値
ＴＣが設定値＋Ｔａを超えるならば、トルク指令（ａＴ
ｃを設定値＋Ｔａとし、設定値−Ｔｂより負の値が人ぎ
ければトルク指令値をＴｂとし、設定値−Ｔｂと＋Ｔａ
間であれば、速度ループ制御処理で算出されたトルク指
令値Ｔｃをそのまま出力するようにする。

例えば、第５図に示す例において、被駆動体５６を上昇
させる方向がサーボモータ１０をプラス方向に回転する
方向であれば、１冒させる方向に被駆動体５６を駆動す
るのに必要な最大トルクに若干のマージンを付加したト
ルク制限値＋Ｔａを設定し、被駆動体５６を下降させる
のに必要な負の最大トルクに若干のマージンを付加して
トルク制限値−Ｔｂとして設定する。この場合、Ｔａ＞
Ｔｂとなる。また逆に、サーボモータ１０をプラス方向
に回転する方向が被駆動体５６を下降させる場合には、
Ｔａ＜Ｔｂとなる。

なお、トルク制限を行うデジタルサーボ回路６のブロセ
ッリの処理については省略するが、第１図に示す速度ル
ープ制御処理において、ステップＳ５で求められたトル
ク指令値Ｔｃに対し、該トルク指令値ＴＣｆｆｉＴＣ＞
　Ｔａならば設定値Ｔａをトルク指令値として出力し、
Ｔｃ　　＜　　−Ｔｂならば設定値−Ｔｂをトルク指令
値として出力し、−Ｔｌ）　　≦　ＴＯ≦　Ｔａであれ
ば、弾出されたトルク指令値ＴＣをそのまま出力すれば
よく、上記判断出力処理をステップ＄５とステップ８６
間に入れればよい。

発明の効果 本発明では、被駆動体が移動途中において異物と衝突し
た際、被駆動体を駆動するり一ボを−９の駆動を逆転さ
せ、被駆動体を異物から引き離して停止させるようにし
たから、被駆動体及び遺物に長時間衝突による外力が加
わることがなく、被駆動体及び異物を損傷させることが
少なくなる。

また、被駆動体の移動方向に応じて、必要とする最大ト
ルクが異なる場合、移動方向に応じてその移動方向の必
要最大トルクに応じて夫々トルク制限を行うようにした
から、衝突が生じても、被駆動体及び異物に加わる力は
大きくならず（被駆動体を移動させる力士衝突により移
動を阻止する力＝サーボモータの出力トルク）、被駆動
体及び異物を損傷させることは少なくなる。

【図面の簡単な説明】

第１図は、本発明の一実施例を実施したデジタルサーボ
サーボ回路のブロセッ１ノの速度ループ制御処理のフロ
ーヂャート、 第２図は、本発明を実施する一実施例のデジタルサーボ
制御のブロック図、 第３図は、サーボ制御系のブロック線図、第４図は、本
発明の駆動方向に応じてトルク制限値を変える一実施例
のサーボ制御系のブロック線図、 第５図（ａ）〜（Ｃ）は、本発明の動作状態の一例を示
す図、 第５図（（ｊ）は、従来の衝突停止状態を示す図である
。 ２・・・数値制御装置（ＮＧ装置）、４・・・共有メモ
リ、６・・・デジタルサーボ回路、８・・・リーボアン
ブ、１０・・・サーボモータ、１２・・・パルスコーダ
、５１．５３・・・プーリ、５２・・・タイミングベル
ト、５６・・・被駆動体、５９・・・異物。 第 ５ ０（ｄ）

Claims (2)

【特許請求の範囲】
1. （１）サーボモータで被駆動体を駆動中、被駆動体が異
   物と衝突したことを検出すると、該衝突検出時のサーボ
   モータの実速度とは逆方向の所定速度指令を所定時間与
   えてサーボモータを逆転させることを特徴とするサーボ
   モータにより駆動される被駆動体の衝突破損防止方法。
2. （２）被駆動体の駆動方向によつて、該被駆動体を駆動
   するサーボモータが出力する最大トルク指令値が異なる
   場合、被駆動体の駆動方向に応じ、各駆動方向に必要な
   最大トルク指令値に基いて、サーボモータの出力トルク
   を制限するトルク制限値を設定することを特徴とするサ
   ーボモータにより駆動される被駆動体の衝突破損防止方
   法。