JPH09305235A - サーボモータの位置制御方法および制御装置 - Google Patents

Abstract

(57)【要約】 【課題】 衝突時の可動部の破損を最小限におさえるた
めに、サーボモータの回転速度によらず、高速かつ高精
度の衝突検出を行なえるサーボモータの位置制御方法お
よび装置を提供する。 【解決手段】 サーボモータの回転軸が衝突時に被衝突
物に与えるパワーは高速回転時ほど大きく、可動部への
損傷も大きくなることに着目し、可動部（１６）の外乱
トルクを推定する推定手段（２０）と、この推定手段に
よって推定された推定外乱トルクとサーボモータの回転
速度を乗算して推定外乱パワー値を求める乗算手段（３
０）と、この推定外乱パワー値を予め定められた許容外
乱パワー値と比較して、推定外乱パワー値が許容外乱パ
ワー値よりも大きいときに可動部が障害物に衝突したと
判定する判定手段（３１）と、この判定手段が衝突と判
定した場合に停止命令を出力する速度制御手段（１３）
とを設けた。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、ＮＣ工作機械やロ
ボットなどの位置制御において、サーボモータによって
駆動される可動部を誤ってワーク等に衝突させたような
異常状態を推定外乱に基づいて検出し、衝突が検出され
たときに停止命令を実行するサーボモータの位置制御に
関する。

【０００２】

【従来の技術】数値制御工作機械（ＮＣ工作機械）やロ
ボットなどのサーボモータの制御において、制御軸を運
転中に誤ってワーク等に衝突させたような異常を外乱ト
ルクオブザーバによって観察された推定外乱トルクを用
いて検出する方法は、既に特開平３−１９６３１３号公
報や特開平４−２４２４０６号公報において提案されて
いる。

【０００３】図４は、従来の推定外乱による衝突検出方
法を用いたサーボモータの位置制御装置の一例を示すブ
ロック図である。ここで、位置制御装置は通常、数値制
御部４１の指示通りに可動部４６をモータ４５によって
駆動することを目的として以下のような制御を行う。す
なわち、数値制御部４１から可動部４６に対する位置指
令Ｐｃが位置制御部４２に与えられると、位置制御部４
２はこの位置指令Ｐｃと位置検出部４７からフィードバ
ックされる可動部４６の位置Ｐｆとの位置偏差をなくす
ように速度制御部４３に対してモータ４５の速度指令Ｖ
c を速度制御部４３に出力する。速度制御部４３は、速
度指令Ｖc と速度検出部４８よりフィードバックされる
モータ４５の回転速度Ｖf の偏差をなくすようにトルク
指令値としてのトルク電流指令Ｉc を電流制御部４４に
出力する。そして、電流制御部４４がモータ４５の巻線
に流れるトルク電流とトルク電流指令Ｉc との電流偏差
がなくなるように電流制御をする。

【０００４】この例においては推定外乱による衝突検出
を行うため、外乱トルクオブザーバ５０を用いている。
すなわち、可動部４６に外部から加えられる外乱トルク
を、外乱トルクオブザーバ５０が電流指令Ｉc とモータ
の回転速度Ｖf に基づいて推定する。

【０００５】図５は外乱トルクオブザーバ５０の一例を
示すブロック図である。ここで、ブロック５１はトルク
係数をＫt とするモータの伝達関数、ブロック５２、５
３は、それぞれモータの回転速度を微分する微分要素、
ならびにモータの軸イナーシャとモータ軸換算の負荷イ
ナーシャの和Ｊを表す伝達関数である。モータと可動部
に作用するトルク、すなわち全トルクＴa は、トルク電
流指令Ｉc にトルク係数Ｋt を乗じた量である。また、
モータ自身が発生するトルク（モータトルク）Ｔm はモ
ータの回転速度Ｖf の時間微分、すなわち回転加速度に
モータの軸イナーシャと負荷イナーシャの和Ｊを乗じた
量である。したがって、外乱トルクＴo は全トルクＴa
からモータトルクＴm を除いた値となり、これは式
（１）のように表すことができる。 Ｔo ＝ Ｔa − Ｔm ＝ Ｋt ×Ｉc − Ｊ×（ｄ／ｄｔ）Ｖf （１） 図５の加え合わせ点５４は、全トルクＴa からモータト
ルクＴm を引いて外乱トルクＴo を得る減算手段を表
す。なお、ここで外乱トルクオブザーバは、外乱トルク
Ｔo を一次遅れフィルタ５５に通して過渡的な推定誤差
を除去したものを推定外乱トルクＴL として出力する。

【０００６】図４のサーボモータの位置制御装置におい
て、外乱トルクオブザーバ５０によって得られた推定外
乱トルクＴL は判定部６０に入力される。そこで推定外
乱トルクＴL は、しきい値として予め設定された許容外
乱トルクＴLLと比較され、許容外乱トルクＴLLよりも推
定外乱トルクＴL が大きい場合に衝突と判断される。図
６（ａ）は、縦軸に推定外乱トルク値ＴL 、横軸に時間
ｔをとり、推定外乱トルク値に基づいて衝突検出を行っ
た場合の推定外乱トルク値の衝突の前後の時間推移を示
したグラフである。ここで、予め設定された許容外乱ト
ルクＴLLは衝突検出にあたってしきい値としての役割を
はたしていることがわかる。また、外乱トルクオブザー
バ５０の一次遅れフィルタ５５のために、衝突発生から
推定外乱トルク値ＴL が許容外乱トルク値ＴLLより大き
くなって衝突が検出されるまでの間に時間遅れが存在す
ることもわかる。

【０００７】以上のようにして衝突が検出された場合
に、判定部６０はアラーム信号ＡＬを数値制御部４１お
よび速度制御部４３に対して出力し、数値制御部４１は
所定のアラーム処理を実行する一方、速度制御部４３は
モータ４５を停止させるためにトルク電流指令Ｉc を０
として可動部４６の破損を回避する。

【０００８】

【発明が解決しようとする課題】しかしながら、上述の
衝突検出方法を用いた位置制御方法には次のような問題
があった。すなわち、従来技術ではサーボモータの回転
速度とは無関係に一定の許容外乱トルク値ＴLLをしきい
値として衝突の有無を判断しているが、可動部とサーボ
モータの回転軸がもつ運動エネルギーは回転速度の２乗
に比例するために、サーボモータの高速回転時には低速
回転時に比べ可動部への損傷を軽減できない。サーボモ
ータの回転軸が衝突時に被衝突物から受けるパワーは、
衝突により生じた外乱トルク値とモータ軸の微小時間に
おける移動量、すなわちモータ回転速度の積である。図
６（ｂ）および（ｃ）は、推定外乱パワー値ＰL の時間
推移を示している。この推定外乱パワー値ＰL は、図６
（ａ）に示すように許容外乱トルクＴLLに基づいて衝突
検出を行った場合の推定外乱トルクＴL と推定外乱トル
ク値ＴL とモータの回転速度Ｖf の積として表される。
ここで、図６（ｂ）、（ｃ）は、それぞれ高速回転時、
低速回転時における衝突前後の推定外乱パワー値ＰL の
時間推移を示したグラフである。これによれば、許容外
乱トルクＴLLに基づく衝突検出時の推定外乱パワーは、
低速回転時より高速回転時の方が大きいことを示してい
る。

【０００９】また、従来の衝突検出方法には、許容外乱
トルクほど大きなトルクを発生させることなく可動部を
破損させてしまうような衝突を検出することができない
という、衝突検出の精度の問題があった。一般的に工作
機械等の正常運転時においてサーボモータのモータ軸に
大きなトルクがかかるのは切削等の比較的低速で運転す
るときであって、早送りの位置決めなどの高速運転中に
は大きなトルクはかかっていない。したがって、工具の
早送り中など、モータが高速で回転しているときに衝突
が発生したときには、工具は折損してしまうにもかかわ
らず、この衝突によって発生する外乱トルクは許容外乱
トルクＴLLに達しないために衝突を検出できず、そのま
ま運転を継続してしまうことがある。

【００１０】さらに、上述の従来例のように式（１）か
ら得られる外乱トルクＴo を一次遅れフィルタ５５に通
して推定外乱トルクＴL を得る場合、図６（ａ）からも
わかるとおり、判定部６０が衝突と判定するまで、すな
わち推定外乱トルク値ＴL が許容外乱トルク値ＴLLより
大きくなるまで、時間遅れが存在する。したがって、特
にサーボモータの高速回転時には衝突検出が遅れてしま
い、その結果として可動部を破損させることがあった。

【００１１】上述のような問題を解決するために、本発
明はサーボモータの回転速度によらず、高速かつ高精度
の衝突検出を行えるサーボモータの位置制御方法および
装置を提供することを目的とする。

【００１２】

【課題を解決するための手段】上述の目的を達成するた
めに、本発明にかかるサーボモータの位置制御方法は、
サーボモータによって駆動される可動部の外乱トルクを
推定し、推定された外乱トルクとサーボモータの回転速
度を乗算して推定外乱パワー値を算出し、この推定外乱
パワー値を予め定められた許容外乱パワー値と比較し
て、推定外乱パワー値が前記許容外乱パワー値よりも大
きいときに可動部が障害物に衝突したと判定し、衝突と
判定された場合に停止命令を実行するようにしたもので
ある。

【００１３】また本発明は、上述のサーボモータの制御
方法において、外乱トルクの推定に外乱トルクオブザー
バを用いることを特徴とするサーボモータの位置制御方
法である。

【００１４】すなわち、本発明では、サーボモータの回
転軸が衝突時に被衝突物に与えるパワーは高速回転時ほ
ど大きく、可動部への損傷も大きくなることに着目し、
従来の推定外乱トルク値のかわりに、推定外乱トルク値
にサーボモータの回転速度を乗じて得られる推定外乱パ
ワーを、許容外乱パワーと比較することで衝突の有無を
判断し、サーボモータの位置制御を行っている。これに
よって許容外乱トルク値がサーボモータの回転数に応じ
て変化し、特に高速回転時には高速かつ高精度の衝突検
出を行ってサーボモータの位置制御を行って、衝突時の
可動部の破損を最小限におさえることができる。

【００１５】これをグラフに表すと図１のようになる。
図１（ａ）は、縦軸に許容外乱トルク値ＴLL、横軸にモ
ータの回転速度Ｖf をとり、許容外乱トルク値ＴLLとモ
ータの回転速度Ｖf の関係を示すグラフである。許容外
乱パワーＰLLを一定とした場合、許容外乱トルク値ＴLL
とモータの回転速度Ｖf は反比例することを示してい
る。したがって、実質的に高速回転時には小さく、低速
回転時には大きい許容外乱トルク値に基づいて精度の高
い衝突の検出を行って、サーボモータの位置制御を行う
ことができる。これを許容外乱トルク値の時間変化で考
えると、図１（ｂ）および（ｃ）のようになる。図１
（ｂ）および（ｃ）は、縦軸に推定外乱トルク値、横軸
に時間をとり、（ｂ）は高速回転時、（ｃ）は低速回転
時における衝突前後の推定外乱トルク値ＴL の時間変化
を表したグラフである。従来の衝突検出方法では許容外
乱トルク値が一定であったのに対して、本願発明で用い
る推定外乱パワーに基づく方法では、高速回転になるほ
ど許容外乱トルク値が小さくなっている。また、これら
二つのグラフは、外乱トルクオブザーバの一次遅れフィ
ルタの時定数Ｔが同じならば、高速回転時の検出遅れ時
間ｔb が低速回転時のｔc よりも短くなり、したがっ
て、高速回転時の衝突検出遅れ時間を短縮することがで
きることを示している。これによって、高速回転時にお
ける高精度、高速の衝突検出による位置制御ができる。

【００１６】また、本発明は、サーボモータによって駆
動される可動部の衝突を推定外乱によって検出するサー
ボモータの位置制御装置であって、可動部の外乱トルク
を推定する推定手段と、この推定手段によって推定され
た推定外乱トルクとサーボモータの回転速度を乗算して
推定外乱パワー値を求める乗算手段と、この推定外乱パ
ワー値を予め定められた許容外乱パワー値と比較して、
推定外乱パワー値が許容外乱パワー値よりも大きいとき
に可動部が障害物に衝突したと判定する判定手段と、こ
の判定手段が衝突と判定した場合に停止命令を出力する
速度制御手段とを有することを特徴とするサーボモータ
の位置制御装置である。

【００１７】本発明はまた、上述のサーボモータの位置
制御装置において、可動部の外乱トルクを推定する推定
手段として外乱トルクオブザーバを備え、衝突検出を推
定外乱パワーに基づいて行うことを特徴とする。

【００１８】この位置制御装置では、推定外乱トルクの
代わりに推定外乱パワーに基づいて衝突検出をおこなう
ために、推定手段が衝突によって生じる外乱トルクを推
定し、乗算手段がこの推定外乱トルクとモータの回転速
度から推定外乱パワーを算出しする。そして、判定部が
この推定外乱パワーを許容外乱パワーと比較することで
衝突を検出し、衝突を検出した場合に停止命令を速度制
御部に出力する。上述のような構成をとることで、許容
外乱パワー値が一定の場合、モータ回転速度に反比例し
た許容外乱トルク値で衝突の判定を行われる。その結
果、本発明は、サーボモータの高速回転時には小さい外
乱トルクを、低速回転時には大きい外乱トルクを衝突と
して検出し、高速かつ高精度の衝突検出機能を備えたサ
ーボモータの位置制御装置を提供することができる。

【００１９】

【発明の実施の形態】以下、本発明の実施の形態を図面
を参照して詳述する。図２は本発明のサーボモータの位
置制御装置の実施の一形態を表すブロック図である。こ
の位置制御装置は、モータ１５によって可動部１６が数
値制御部１１の指示通りに駆動されるようになってい
る。このとき、可動部１６の位置Ｐｆおよびモータ１５
の回転速度Ｖf は、それぞれ位置検出部１７、速度検出
部１８によって検出され、位置制御部１２および速度制
御部１３にフィードバックされている。数値制御部１１
から機械制御部１６の位置指令Ｐｃが与えられると、位
置制御部１２は、この位置指令Ｐｃと位置検出部１７か
ら得られる可動部１６の位置Ｐｆとの位置偏差をなくす
ようにこの位置に対し比例制御（Ｐ制御）を行い、速度
制御部１３にモータ１５に対する速度指令Ｖc を出力す
る。速度制御部１３は、速度検出部１８より得られるモ
ータ１５の回転速度Ｖf とこの速度指令Ｖc との速度偏
差をなくすように速度に対し比例・積分（ＰＩ）制御を
行って、トルク電流指令Ｉc を電流制御部１４に出力す
る。電流制御部１４はモータ１５の巻線を流れるトルク
電流とトルク電流指令Ｉc との電流偏差がなくなるよう
に電流制御をする。

【００２０】外乱トルク推定手段として作用する外乱ト
ルクオブザーバ２０は、先に図５に示したものと同じ構
成をとり、速度制御部１３から出力されるトルク電流指
令Ｉc とモータ１５の回転速度Ｖf を入力として推定外
乱トルク値ＴL を出力する。乗算器３０は、推定外乱ト
ルク値ＴL とモータ１５の回転速度Ｖf との乗算を行
い、推定外乱パワーＰL を判定部３１に出力する。判定
部３１は、推定外乱パワーＰL を可動部の機械的強度等
を考慮して決められた許容外乱パワーＰLLと比較し、推
定外乱パワーＰL が許容外乱パワーＰLLよりも大きい場
合に衝突があったものと判断してアラーム信号ＡＬを数
値制御部１１と速度制御部１３に出力する。アラーム信
号ＡＬが出力された場合、位置制御部１１はアラーム処
理を実行すると同時に、速度制御部１３はトルク電流指
令Ｉc を０として電流制御部１４に出力し、モータ１５
を停止させる。

【００２１】図３は、本発明のサーボモータの位置制御
方法の実施の形態を表すフローチャートである。モータ
の回転速度偏差に基づいてトルク電流指令Ｉc を計算す
る（ステップ１）。トルク電流指令Ｉc とモータの回転
速度Ｖf から可動部の外乱トルク値ＴL を推定し（ステ
ップ２）、この推定外乱トルク値にモータの回転速度を
掛けて推定外乱パワー値ＰL を算出する（ステップ
３）。そして、衝突を検出するために予め設定されたし
きい値、すなわち許容外乱パワー値ＰLLと推定外乱パワ
ー値ＰL とを比較し、推定外乱パワー値ＰL が許容外乱
パワー値ＰLLよりも大きい場合は衝突があったものと判
定し、ステップ５に進む（ステップ４）。衝突を検出し
た場合にはアラームを出力し、速度指令Ｖc を０にする
（ステップ５）。

【００２２】

【発明の効果】本発明によれば、推定外乱によって可動
部の衝突を検出する際に、推定外乱トルク値とモータの
回転速度の積である推定外乱パワー値に基づいて衝突の
判定を行うことで、許容外乱トルク値をサーボモータの
回転数に応じて連続的に変化させ、サーボモータの高速
回転時には小さい外乱トルク値で、低速回転時には大き
い外乱トルク値で衝突を検出することができる。その結
果、従来に比べて特に高速回転時における衝突検出機能
の高精度化を図ることができる。

【００２３】また、本発明では衝突の検出を推定外乱パ
ワー値に基づいて行うため、衝突時に被衝突物に与える
パワーを一定値以下にすることによって、衝突時の可動
部の破損を最小限におさえることができる。

【００２４】さらに、本発明においては、外乱トルクオ
ブザーバが一次遅れフィルタを含む場合であっても、サ
ーボモータの高速回転時には小さい外乱トルク値で衝突
を検出することとなるので、衝突検出機能の高速化に寄
与することができる。

【図面の簡単な説明】

【図１】 （ａ）は許容外乱トルク値ＴLLとモータの回
転速度Ｖf の関係を示すグラフ、（ｂ）および（ｃ）は
衝突前後の推定外乱トルク値ＴL の時間変化を表したグ
ラフである。

【図２】 本発明のサーボモータの位置制御装置の実施
の一形態を表すブロック図である。

【図３】 本発明のサーボモータの位置制御方法の実施
の形態を表すフローチャートである。

【図４】 従来の衝突検出方法によるサーボモータの位
置制御装置の一例を示すブロック図である。

【図５】 図４の外乱トルクオブザーバの一例を示すブ
ロック図である。

【図６】 （ａ）は衝突の前後の推定外乱トルク値の時
間推移を示したグラフ、（ｂ）および（ｃ）は、それぞ
れ高速回転時、低速回転時における衝突前後の推定外乱
パワー値ＰL の時間推移を示したグラフである。

【符号の説明】

１１…数値制御部、１２…位置制御部、１３…速度制御
部、１４…電流制御部、１５…モータ、１６…可動部、
１７…位置検出部、１８…速度検出部、２０…外乱トル
クオブザーバ、３０…乗算器、３１…判定部。

Claims (4)

【特許請求の範囲】
1. 【請求項１】 サーボモータによって駆動される可動部
   の衝突を推定外乱によって検出するサーボモータの位置
   制御方法であって、 前記可動部の外乱トルクを推定し、 推定された前記外乱トルクと前記サーボモータの回転速
   度を乗算して推定外乱パワー値を算出し、 前記推定外乱パワー値を許容外乱パワー値と比較して、
   前記推定外乱パワー値が前記許容外乱パワー値よりも大
   きいときに前記可動部が障害物に衝突したと判定し、 衝突と判定された場合に停止命令を実行することを特徴
   とするサーボモータの位置制御方法。
2. 【請求項２】 請求項１において、外乱トルクは外乱ト
   ルクオブザーバによって求めることを特徴とするサーボ
   モータの位置制御方法。
3. 【請求項３】 サーボモータによって駆動される可動部
   の衝突を推定外乱によって検出するサーボモータの位置
   制御装置であって、 前記可動部の外乱トルクを推定する推定手段と、 前記推定手段によって推定された前記推定外乱トルクと
   前記サーボモータの回転速度を乗算して推定外乱パワー
   値を求める乗算手段と、 前記推定外乱パワー値を許容外乱パワー値と比較して、
   前記推定外乱パワー値が前記許容外乱パワー値よりも大
   きいときに前記可動部が障害物に衝突したと判定する判
   定手段と、 前記判定手段が衝突と判定した場合に停止命令を出力す
   る速度制御手段とを有することを特徴とするサーボモー
   タの位置制御装置。
4. 【請求項４】 請求項３において、可動部の外乱トルク
   を推定する推定手段は外乱トルクオブザーバであること
   を特徴とするサーボモータの位置制御装置。