(57)【要約】      （修正有）【課題】サーボモータあるいはサーボモータを備えた多関節ロボットが障害物等に接触して異常負荷を検知した時に、速やかに前記障害物等から離れて前記異常負荷を回避する方法を提供する。【解決手段】外乱推定オブザーバを用いてロボットが受ける外乱トルクを推定し、該外乱推定値が設定基準値以上になることによって異常負荷を検出するロボットの異常負荷検出制御方法において、前記ロボットが、プログラムにより作業を開始する前に前記設定基準値を所定値に設定し、前記作業が終了後、前記設定基準値を前記所定値と異なる値に設定するものである。【選択図】図１
2008253132.tif 000002
【特許請求の範囲】
【請求項１】
外乱推定オブザーバを用いてロボットが受ける外乱トルクを推定し、該外乱推定値が設定基準値以上になることによって異常負荷を検出するロボットの異常負荷検出制御方法において、
前記ロボットが、プログラムにより作業を開始する前に前記設定基準値を所定値に設定し、前記作業が終了後、前記設定基準値を前記所定値と異なる値に設定することを特徴とするロボットの異常負荷検出制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、ロボットや工作機械等のサーボモータを動力源とする機械において、工作機械のカッタやロボットのアーム等のサーボモータで駆動される可動部がワーク等の他の物体に衝突したとき等の異常負荷を検出した時のサーボモータの制御方法に関する。
【背景技術】
【０００２】
工作機械の可動部（カッタ若しくはテーブル等）やロボットのアーム等が障害物に衝突したとき、これら可動部やアームを駆動するサーボモータは移動指令に従って移動しようとして、大きなトルクを発生する。そのため、可動部やアームの機構部を破損させる場合がある。この衝突等を検出する方法として、外乱推定オブザーバによって外乱トルクを推定し、この推定外乱トルクが設定値以上になったとき、衝突等による異常負荷が生じているものとして検出する方法が公知である（例えば、電気学会研究会資料「産業電力電気応用研究会」ＩＥＡ−８９−１０〜１４  村上俊之他「多軸加速制御における自己診断機能」）。また、外乱推定オブザーバによって異常負荷を検出すると、異常負荷が生じたサーボモータの速度指令を「０」としてサーボモータを急停止させる方法（例えば特開平３−１９６３１３号公報）、あるいはサーボモータをそれまでの進行方向とは逆方向に所定量移動させて停止させる方法（例えば特開平６−２４５５６１号公報）などの異常負荷検出制御方法が提案されている。
【発明の開示】
【発明が解決しようとする課題】
【０００３】
ところが、速度指令を「０」としてサーボモータを急停止させる方法では、サーボモータは機械を押したまま停止するので、サーボモータの停止後も機械に負荷がかかったままであるという問題がある。特開平６−２４５５６１号公報で開示された発明はこの問題を解決するためになされたものであるが、この発明では位置・速度ループによるクローズドループ制御回路をそのまま使用して、所定の位置偏差を入力してサーボモータをそれまでの進行方向とは逆方向に動作させる方法をとっているので、逆方向に動作させるための位置偏差速度指令に対する実際のサーボモータへのトルク指令にも、位置・速度クローズドループ処理を施すことにより遅れが生じることになる。その結果、異常負荷検出時に速度が０になるまでの時間および、負荷がかからない状態に移動するまでの時間が遅れることになる。機械の受けるダメージは異常負荷がかかりはじめてから、速度が０になり、さらに負荷がかからない状態に移動するまでの時間に依存するので、ダメージを小さくするためには、できるだけその時間を短くすることが必要である。そこで本発明はサーボモータあるいはサーボモータを備えた多関節ロボットが障害物等に接触して異常負荷を検知した時に、速やかに停止して、さらに前記障害物等から離れて前記異常負荷を除去する方法を提供することを目的とする。
【課題を解決するための手段】
【０００４】
上記の目的を達成するために、本発明では、以下のようにしたものである。
請求項１記載の発明は、外乱推定オブザーバを用いてロボットが受ける外乱トルクを推定し、該外乱推定値が設定基準値以上になることによって異常負荷を検出するロボットの異常負荷検出制御方法において、前記ロボットが、プログラムにより作業を開始する前に前記設定基準値を所定値に設定し、前記作業が終了後、前記設定基準値を前記所定値と異なる値に設定するものである。
【発明の効果】
【０００５】
以上述べたように、本発明によれば、衝突検出後ただちに大きな逆転トルクを指令するので、衝突後速やかにロボットアーム等を停止させたうえ、さらに障害物から引き離して、障害物から受ける反力を速やかに除去できるので、ロボットアーム等および被衝突側の障害物の破損を最小限に抑えることが可能になるという効果がある。
【発明を実施するための最良の形態】
【０００６】
【実施例１】
【０００７】
以下、本発明の実施例を図に基づいて説明する。図１は本発明の実施例を示すブロック図である。図において、１はサーボモータであり、２はサーボモータ１で駆動されるロボットアームであり、３はサーボモータ１の回転速度を検出する速度検出器であり、４はサーボモータ１を駆動するディジタルサーボ回路であり、５はクローズドループ制御とオープンループ制御を切り替えるスイッチであり、図示しない上位の制御装置の指令によって作動する。スイッチ５が閉じている時はサーボモータ１はクローズドループで制御され、スイッチ５が開くとオープンループで制御される。次にこの実施例の作用を説明する。通常の制御においてはスイッチ５は閉じられてクローズドループによる制御が選択される。この時、前記上位の制御装置からディジタルサーボ回路４に位置指令Ｖ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       （数６）
【００１５】
サーボモータが低速で回転しているときは、もともとの速度が低いのでサーボモータを逆回転するためには大きなトルクや時間は不要であり、必要以上に大きなトルクを指令するとロボットアーム等が必要以上に移動するので、かえって危険な場合がある。そこでこのように速度に応じて設定値を変えるようにするのである。
【００１６】
一般に多関節ロボットは６軸の自由度を有し、エンドエフェクタの位置の決定する３軸を基本軸とよび、エンドエフェクタの姿勢を決定する３軸を手首軸とよび、各軸はサーボモータで駆動される。本発明をこのような多関節ロボットに適用する場合は、これらの軸の性格に応じてつぎのような応用が考えられる。
（１）ある軸のサーボモータについて異常負荷を検知したときに、該軸のサーボモータだけに逆転トルクを指令し、他の軸にはその場での停止を指令する。
（２）ある軸のサーボモータについて異常負荷を検知したときに、該軸のサーボモータだけに逆転トルクを指令し、加えて該軸以外の他の軸についても逆転トルクを指令する。
（３）ある軸のサーボモータについて異常負荷を検知したときに、手首軸のサーボモータだけに逆転トルクを指令し、基本軸にはその場での停止を指令する。
【００１７】
以上、サーボモータあるいはサーボモータで駆動される多関節ロボット等の工作機械に加わる異常負荷を検出したときに、前記サーボモータを速やかに停止させる異常負荷検出制御方法について説明した。次に、異常負荷を検出するための設定基準値を変更する方法を説明する。例えば、ロボットにグラインダを取り付けて研磨作業を行う際、ロボットは研磨対象のワークから大きな反力を受ける。この反力を異常負荷であると認識してロボットが停止すると困るので、このような場合は外乱推定オブザーバの設定基準値を大きく設定する必要がある。しかし、ロボットが研磨作業以外の動作を行う場合、つまり研磨対象のワークのある場所から別の場所に高速で移動する（いわゆるエアカット）場合は前記設定基準値が大きいと衝突を検出できない。そこで、ロボットあるいはサーボモータの動作を指令するプログラム中に研磨作業を開始する命令の前に前記設定基準値を大きくする命令を記載し、研磨作業を終了後に前記設定基準値を小さくする命令を記載すればよい。また、外乱推定オブザーバによる外乱の推定値はサーボモータが高速で回転している時に大きく、低速で回転している時に小さくなるので、高速回転時にあわせて設定基準値を決定すると低速回転時には衝突検出の感度が低下する。そこで、前記外乱推定オブザーバの設定基準値をサーボモータの動作速度指令値の大小に応じて変更するようにプログラムすればよい。さらにまた、前記の研磨作業開始の命令やサーボモータの動作速度指令値を変更する命令がプログラム中で読み込まれるタイミングと実際に研磨作業が始まるタイミングおよびサーボモータの動作速度が変わるタイミングにはずれがある。これは、サーボ系のゲインに依存する遅れに起因するものであるが、この遅れを考慮して、前記設定基準値を変更するタイミングを一定時間遅らせば、さらに適切な異常負荷検出制御が行える。
【図面の簡単な説明】
【００１８】
【図１】本発明の実施例を示すブロック図
【符号の説明】
【００１９】
１：サーボモータ  
２：ロボットアーム  
３：速度検出器  
４：ディジタルサーボ回路
５：スイッチ
【図１】
2008253132.tif 000003