JP4240517B2

JP4240517B2 - サーボモータおよび多関節ロボットの異常負荷検出制御方法

Info

Publication number: JP4240517B2
Application number: JP27567499A
Authority: JP
Inventors: 正夫尾島; 泰資深田; 淳萩原; 康之井上
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 1999-05-20
Filing date: 1999-09-29
Publication date: 2009-03-18
Anticipated expiration: 2019-09-29
Also published as: JP2001037289A

Description

【０００１】
【発明の属する技術分野】
この発明は、ロボットや工作機械等のサーボモータを動力源とする機械において、工作機械のカッタやロボットのアーム等のサーボモータで駆動される可動部がワーク等の他の物体に衝突したとき等の異常負荷を検出した時のサーボモータの制御方法に関する。
【０００２】
【従来の技術】
工作機械の可動部（カッタ若しくはテーブル等）やロボットのアーム等が障害物に衝突したとき、これら可動部やアームを駆動するサーボモータは移動指令に従って移動しようとして、大きなトルクを発生する。そのため、可動部やアームの機構部を破損させる場合がある。この衝突等を検出する方法として、外乱推定オブザーバによって外乱トルクを推定し、この推定外乱トルクが設定値以上になったとき、衝突等による異常負荷が生じているものとして検出する方法が公知である（例えば、電気学会研究会資料「産業電力電気応用研究会」ＩＥＡ−８９−１０〜１４村上俊之他「多軸加速制御における自己診断機能」）。
また、外乱推定オブザーバによって異常負荷を検出すると、異常負荷が生じたサーボモータの速度指令を「０」としてサーボモータを急停止させる方法（例えば特開平３−１９６３１３号公報）、あるいはサーボモータをそれまでの進行方向とは逆方向に所定量移動させて停止させる方法（例えば特開平６−２４５５６１号公報）などの異常負荷検出制御方法が提案されている。
【０００３】
【発明が解決しようとする課題】
ところが、速度指令を「０」としてサーボモータを急停止させる方法では、サーボモータは機械を押したまま停止するので、サーボモータの停止後も機械に負荷がかかったままであるという問題がある。特開平６−２４５５６１号公報で開示された発明はこの問題を解決するためになされたものであるが、この発明では位置・速度ループによるクローズドループ制御回路をそのまま使用して、所定の位置偏差を入力してサーボモータをそれまでの進行方向とは逆方向に動作させる方法をとっているので、逆方向に動作させるための位置偏差速度指令に対する実際のサーボモータへのトルク指令にも、位置・速度クローズドループ処理を施すことにより遅れが生じることになる。その結果、異常負荷検出時に速度が０になるまでの時間および、負荷がかからない状態に移動するまでの時間が遅れることになる。
機械の受けるダメージは異常負荷がかかりはじめてから、速度が０になり、さらに負荷がかからない状態に移動するまでの時間に依存するので、ダメージを小さくするためには、できるだけその時間を短くすることが必要である。
そこで本発明はサーボモータあるいはサーボモータを備えた多関節ロボットが障害物等に接触して異常負荷を検知した時に、速やかに停止して、さらに前記障害物等から離れて前記異常負荷を除去する方法を提供することを目的とする。
【０００４】
【課題を解決するための手段】
そこで、本発明は、サーボモータに加わる異常負荷を検出した時に、前記異常負荷の検出直前のサーボモータの回転方向と逆方向のトルクであって前記サーボモータの最高回転速度と前記サーボモータの実動作速度値または速度指令値のいずれか一つの値との比に応じた所定の大きさのトルク指令を前記サーボモータに所定時間与えるものである。
また、オブザーバを用いてサーボモータが受ける外乱トルクを推定し、該外乱推定値が設定基準値以上になることによって異常負荷を検出するものである。
また、通常はクローズドループによるサーボモータ制御を行い、異常負荷が検出されると、オープンループによる制御に切り替えるものである。
また、異常負荷が検出される直前のサーボモータの実際の回転速度の大小に応じて、前記トルク指令の大きさを切り替えるものである。
また、異常負荷が検出される直前のサーボモータの回転速度の指令値の大小に応じて、前記トルク指令の大きさを切り替えるものである。
また、異常負荷が検出される直前のサーボモータの実際の回転速度の大小に応じて、前記トルク指令を与える前記所定時間の長さを切り替えるものである。
また、異常負荷が検出される直前のサーボモータの回転速度の指令値の大小に応じて、前記トルク指令を与える前記所定時間の長さを切り替えるものである。
また、前記トルク指令の大きさを前記サーボモータに重力によって負荷されるトルクと釣り合う大きさとするものである。
また、異常負荷が検出される直前のサーボモータの実際の回転速度の大小に応じて、前記設定基準値の大きさを切り替えるものである。
また、異常負荷が検出される直前のサーボモータの回転速度の指令値の大小に応じて、前記設定基準値の大きさを切り替えるものである。
また、オブザーバを用いて多関節ロボットのサーボモータが受ける外乱トルクを推定し、該外乱推定値が設定基準値以上になることによって異常負荷を検出し、前記異常負荷の検出直前のサーボモータの回転方向と逆方向のトルクであって前記サーボモータの最高回転速度と前記サーボモータの実動作速度値または速度指令値のいずれか一つの値との比に応じた所定の大きさのトルク指令を前記異常負荷を検出したサーボモータに所定時間与えるものである。
また、前記多関節ロボットのティーチング時とプレイバック時で前記設定基準値の大きさを切り替えるものである。
また、多関節ロボットのティーチング時とプレイバック時で前記トルク指令の大きさを切り替えるものである。
また、多関節ロボットのティーチング時とプレイバック時で前記トルク指令を与える前記所定時間の長さを切り替えるものである。
また、異常負荷を検出した多関節ロボットのサーボモータに加えて、前記異常負荷を検出していない他のサーボモータに、前記異常負荷の検出直前の前記他のサーボモータの回転方向と逆方向のトルクであって所定の大きさのトルク指令を所定時間与えるものである。
また、異常負荷を検出すると、多関節ロボットの特定の軸のサーボモータに対しては、前記異常負荷の検出直前の前記特定のサーボモータの回転方向と逆方向のトルクであって所定の大きさのトルク指令を所定時間与え、他の軸のサーボモータに対してはその場で停止する指令を与えるものである。
また、外乱推定オブザーバを用いてサーボモータが受ける外乱トルクを推定し、該外乱推定値が設定基準値以上になることによって異常負荷を検出するサーボモータの異常負荷検出制御方法において、前記サーボモータの最高回転速度と前記サーボモータの実動作速度値または速度指令値のいずれか一つの値との比に応じて前記設定基準値を変更するものである。
また、前記設定基準値の変更を、サーボ系の遅れを考慮したー定時間の遅れを持たせて行うものである。
【０００５】
【発明の実施の形態】
以下、本発明の実施例を図に基づいて説明する。
図１は本発明の実施例を示すブロック図である。図において、１はサーボモータであり、２はサーボモータ１で駆動されるロボットアームであり、３はサーボモータ１の回転速度を検出する速度検出器であり、４はサーボモータ１を駆動するディジタルサーボ回路であり、５はクローズドループ制御とオープンループ制御を切り替えるスイッチであり、図示しない上位の制御装置の指令によって作動する。スイッチ５が閉じている時はサーボモータ１はクローズドループで制御され、スイッチ５が開くとオープンループで制御される。
次にこの実施例の作用を説明する。通常の制御においてはスイッチ５は閉じられてクローズドループによる制御が選択される。この時、前記上位の制御装置からディジタルサーボ回路４に位置指令ＶSPが入力される。位置指令ＶSPはディジタルサーボ回路４の内部で位置ループ比例ゲインＫP 、速度ループ比例ゲインＫｖ，およびモータトルク定数ＫT を順に乗じて、サーボモータ１に指令するトルクに応じた電流指令値Ｉref に変換され、スイッチ５を通ってサーボモータ１に流れる。電流指令値Ｉref がサーボモータ１に流れるとサーボモータ１が回転し、ロボットアーム２を駆動する。サーボモータ１の回転速度は速度検出器３で検出され、速度フィードバック信号Ｖref として出力して、ディジタルサーボ回路４にフィードバックする。以上が通常のクローズドループによる制御である。
ここで、ロボットアーム２が障害物に衝突したとする。この衝突を図示しない外乱推定オブザーバが検出すると、前記外乱推定オブザーバの作用によって前記上位の制御装置からの信号でスイッチ５が開き、オープンループ制御に切り替えられる。次に前記上位の制御装置が所定のトルクＴｑを所定の時間Ｔｍの間だけサーボモータ１に指令する。通常、トルクＴｑの大きさはサーボモータ１の最大トルクを設定し、その方向はサーボモータ１の衝突直前の回転方向の逆方向に設定する。トルクＴｑの方向はロボットアーム２と障害物の接触によってサーボモータ１が受ける反力トルクと同じ方向であると言い換えることもできる。サーボモータ１はこのようなトルクＴｑを指令されるので、急激に減速し、さらに反転し、衝突前の位置にロボットアーム２を引き戻す。所定の時間Ｔｍが経過すると、再びスイッチ５を閉じてクローズドループ制御に切り替え、ロボットアーム２を停止させる。
ロボットアーム２が重力による負荷を受ける場合、トルクＴｑを前記重力による負荷と釣り合う大きさと方向に設定すれば、ロボットアーム２の重力による落下を防ぐことができる。この時、ロボットアーム２は障害物から受ける反力によって、衝突前の位置まで戻される。
また、トルクＴｑの大きさを、次のようなサーボモータ１の運転条件によって切り替えてもよい。
（１）異常負荷の検出すなわち衝突の直前のサーボモータの実際の回転速度に応じて、回転速度が大きいときはトルクＴｑの大きさを大きい値に、回転速度が小さいときはトルクＴｑの大きさを小さい値に設定する。
（２）異常負荷の検出すなわち衝突の直前のサーボモータの回転速度指令に応じて、回転速度指令が大きいときはトルクＴｑの大きさを大きい値に、回転速度指令が小さいときはトルクＴｑの大きさを小さい値に設定する。
（３）サーボモータが多関節ロボットの駆動用に用いられている場合、プレイバック動作時にはトルクＴｑの大きさを大きい値に、教示動作時にはトルクＴｑの大きさを小さい値に設定する。
（４）サーボモータが多関節ロボットの駆動用に用いられている場合、エンドエフェクタの姿勢に影響する軸についてはトルクＴｑの大きさを大きい値に、それ以外の軸についてはトルクＴｑの大きさを小さい値に設定する。
またトルクＴｑを指令する時間Ｔｍおよび外乱推定オブザーバにおいて異常を検出するための設定基準値Ｌｓについても大小２つの値を用意して、トルクＴｑと同様にサーボモータ１の運転条件によって切り替えてもよい。
さらにまた、トルクＴｑと時間Ｔｍおよび設定基準値Ｌｓをサーボモータの回転速度Ｖを基準にして次のような式で求めてもよい。
【０００６】
Ｔｑ＝（Ｖ／Ｖmax ）＊Ｔｑ＿max （式１）
【０００７】
Ｔｍ＝（Ｖ／Ｖmax ）＊Ｔｍ＿max （式２）
【０００８】
Ｌｓ＝（Ｖ／Ｖmax ）＊Ｌｓ＿max （式３）
【０００９】
ここで、Ｔｑ＿max 、Ｔｍ＿max およびＬｓ＿max はそれぞれトルク、時間および設定基準値の最大値である。また、Ｖmax はサーボモータの最高回転速度である。さらに、サーボモータの回転速度Ｖは実際の速度であっても速度指令値であってもよい。また、速度との関係は、状況に応じて任意の式を用いてよい。例えば、以下の様な式も有効である。
【００１０】
Ｔｑ＝sin （π／２＊（Ｖ／Ｖmax ））＊Ｔｑ＿max （式４）
【００１１】
Ｔｍ＝sin （π／２＊（Ｖ／Ｖmax ））＊Ｔｍ＿max （式５）
【００１２】
Ｌｓ＝sin （π／２＊（Ｖ／Ｖmax ））＊Ｌｓ＿max （式６）
【００１３】
サーボモータが低速で回転しているときは、もともとの速度が低いのでサーボモータを逆回転するためには大きなトルクや時間は不要であり、必要以上に大きなトルクを指令するとロボットアーム等が必要以上に移動するので、かえって危険な場合がある。そこでこのように速度に応じて設定値を変えるようにするのである。
【００１４】
一般に多関節ロボットは６軸の自由度を有し、エンドエフェクタの位置の決定する３軸を基本軸とよび、エンドエフェクタの姿勢を決定する３軸を手首軸とよび、各軸はサーボモータで駆動される。本発明をこのような多関節ロボットに適用する場合は、これらの軸の性格に応じてつぎのような応用が考えられる。
（１）ある軸のサーボモータについて異常負荷を検知したときに、該軸のサーボモータだけに逆転トルクを指令し、他の軸にはその場での停止を指令する。
（２）ある軸のサーボモータについて異常負荷を検知したときに、該軸のサーボモータだけに逆転トルクを指令し、加えて該軸以外の他の軸についても逆転トルクを指令する。
（３）ある軸のサーボモータについて異常負荷を検知したときに、手首軸のサーボモータだけに逆転トルクを指令し、基本軸にはその場での停止を指令する。
【００１５】
以上、サーボモータあるいはサーボモータで駆動される多関節ロボット等の工作機械に加わる異常負荷を検出したときに、前記サーボモータを速やかに停止させる異常負荷検出制御方法について説明した。次に、異常負荷を検出するための設定基準値を変更する方法を説明する。
例えば、ロボットにグラインダを取り付けて研磨作業を行う際、ロボットは研磨対象のワークから大きな反力を受ける。この反力を異常負荷であると認識してロボットが停止すると困るので、このような場合は外乱推定オブザーバの設定基準値を大きく設定する必要がある。しかし、ロボットが研磨作業以外の動作を行う場合、つまり研磨対象のワークのある場所から別の場所に高速で移動する（いわゆるエアカット）場合は前記設定基準値が大きいと衝突を検出できない。そこで、ロボットあるいはサーボモータの動作を指令するプログラム中に研磨作業を開始する命令の前に前記設定基準値を大きくする命令を記載し、研磨作業を終了後に前記設定基準値を小さくする命令を記載すればよい。
また、外乱推定オブザーバによる外乱の推定値はサーボモータが高速で回転している時に大きく、低速で回転している時に小さくなるので、高速回転時にあわせて設定基準値を決定すると低速回転時には衝突検出の感度が低下する。そこで、前記外乱推定オブザーバの設定基準値をサーボモータの動作速度指令値の大小に応じて変更するようにプログラムすればよい。
さらにまた、前記の研磨作業開始の命令やサーボモータの動作速度指令値を変更する命令がプログラム中で読み込まれるタイミングと実際に研磨作業が始まるタイミングおよびサーボモータの動作速度が変わるタイミングにはずれがある。これは、サーボ系のゲインに依存する遅れに起因するものであるが、この遅れを考慮して、前記設定基準値を変更するタイミングを一定時間遅らせば、さらに適切な異常負荷検出制御が行える。
【００１６】
【発明の効果】
以上述べたように、本発明によれば、衝突検出後ただちに大きな逆転トルクを指令するので、衝突後速やかにロボットアーム等を停止させたうえ、さらに障害物から引き離して、障害物から受ける反力を速やかに除去できるので、ロボットアーム等および被衝突側の障害物の破損を最小限に抑えることが可能になるという効果がある。
【図面の簡単な説明】
【図１】本発明の実施例を示すブロック図である。
【符号の説明】
１：サーボモータ２：ロボットアーム３：速度検出器４：ディジタルサーボ回路
５：スイッチ

Claims

サーボモータの制御における異常負荷検出時の制御方法において、前記異常負荷の検出直前のサーボモータの回転方向と逆方向のトルクであって前記サーボモータの最高回転速度と前記サーボモータの実動作速度値または速度指令値のいずれか一つの値との比に応じたトルク指令を前記サーボモータに所定時間与えることを特徴とするサーボモータの異常負荷検出制御方法。
外乱推定オブザーバを用いてサーボモータが受ける外乱トルクを推定し、該外乱推定値が設定基準値以上になることによって異常負荷を検出することを特徴とする請求項１に記載のサーボモータの異常負荷検出制御方法。
通常はクローズドループによるサーボモータ制御を行い、異常負荷が検出されると、オープンループによる制御に切り替えることを特徴とする請求項１または請求項２に記載のサーボモータの異常負荷検出制御方法。
異常負荷が検出される直前のサーボモータの実際の回転速度の大小に応じて、前記トルク指令の大きさを切り替えることを特徴とする請求項１から請求項３のいずれかに記載のサーボモータの異常負荷検出制御方法。
異常負荷が検出される直前のサーボモータの回転速度の指令値の大小に応じて、前記トルク指令の大きさを切り替えることを特徴とする請求項１から請求項３のいずれかに記載のサーボモータの異常負荷検出制御方法。
異常負荷が検出される直前のサーボモータの実際の回転速度の大小に応じて、前記トルク指令を与える前記所定時間の長さを切り替えることを特徴とする請求項１から請求項５のいずれかに記載のサーボモータの異常負荷検出制御方法。
異常負荷が検出される直前のサーボモータの回転速度の指令値の大小に応じて、前記トルク指令を与える前記所定時間の長さを切り替えることを特徴とする請求項１から請求項５のいずれかに記載のサーボモータの異常負荷検出制御方法。
前記トルク指令の大きさが、前記サーボモータに重力によって負荷されるトルクと釣り合う大きさであることを特徴とする請求項１から請求項７のいずれかに記載のサーボモータの異常負荷検出制御方法。
異常負荷が検出される直前のサーボモータの実際の回転速度の大小に応じて、前記設定基準値の大きさを切り替えることを特徴とする請求項２に記載のサーボモータの異常負荷検出制御方法。
異常負荷が検出される直前のサーボモータの回転速度の指令値の大小に応じて、前記設定基準値の大きさを切り替えることを特徴とする請求項２に記載のサーボモータの異常負荷検出制御方法。
外乱推定オブザーバを用いてサーボモータが受ける外乱トルクを推定し、該外乱推定値が設定基準値以上になることによって異常負荷を検出する多関節ロボットの異常負荷検出時の制御方法において、前記異常負荷の検出直前のサーボモータの回転方向と逆方向のトルクであって前記サーボモータの最高回転速度と前記サーボモータの実動作速度値または速度指令値のいずれか一つの値との比に応じたトルク指令を、前記異常負荷を検出したサーボモータに所定時間与えることを特徴とする多関節ロボットの異常負荷検出制御方法。
ティーチング時とプレイバック時で前記設定基準値の大きさを切り替えることを特徴とする請求項１１に記載の多関節ロボットの異常負荷検出制御方法。
ティーチング時とプレイバック時で前記トルク指令の大きさを切り替えることを特徴とする請求項１１または請求項１２に記載の多関節ロボットの異常負荷検出制御方法。
ティーチング時とプレイバック時で前記トルク指令を与える前記所定時間の長さを切り替えることを特徴とする請求項１１から請求項１３のいずれかに記載の多関節ロボットの異常負荷検出制御方法。
異常負荷を検出したサーボモータに加えて、前記異常負荷を検出していない他のサーボモータに、前記異常負荷の検出直前の前記他のサーボモータの回転方向と逆方向のトルクであって所定の大きさのトルク指令を所定時間与えることを特徴とする請求項１１から請求項１４のいずれかに記載の多関節ロボットの異常負荷検出制御方法。
異常負荷を検出すると、特定の軸のサーボモータに対しては、前記異常負荷の検出直前の前記特定のサーボモータの回転方向と逆方向のトルクであって所定の大きさのトルク指令を所定時間与え、他の軸のサーボモータに対してはその場で停止する指令を与えることを特徴とする請求項１１から請求項１５のいずれかに記載の多関節ロボットの異常負荷検出制御方法。
外乱推定オブザーバを用いてサーボモータが受ける外乱トルクを推定し、該外乱推定値が設定基準値以上になることによって異常負荷を検出するサーボモータの異常負荷検出制御方法において、前記サーボモータの最高回転速度と前記サーボモータの実動作速度値または速度指令値のいずれか一つの値との比に応じて前記設定基準値を変更することを特徴とするサーボモータの異常負荷検出制御方法。
前記設定基準値の変更を、サーボ系の遅れを考慮したー定時間の遅れを持たせて行うことを特徴とする請求項１７に記載のサーボモータの異常負荷検出制御方法。