JP5375062B2 - ロボットシステムおよび制御方法 - Google Patents

Description

本発明はロボットや工作機械等のサーボモータを動力源とする機械において、工作機械のカッタやロボットアーム等のサーボモータで駆動される可動部がワーク等の他の物体に衝突したとき等の異常負荷を検出した時のサーボモータの制御方法に関し、特に、ロボットシステムの制御方法に関する。

産業用ロボットを構成するアーム自身あるいはこれに把持されたエンドエフェクタが障害物に衝突した場合、各アームを駆動するサーボモータは予め設定された移動指令に従ってなおも移動し続けようとし、この結果サーボモータは拘束状態となり、大きなトルクを発生し続けることになる。この状態が長く続くとサーボモータや減速機を含むアームの機構部が破損する可能性が生ずる。また、障害物に機械的な損傷を与えることになる。
従来より衝突の発生を検出し、サーボモータの移動指令を即時に中断する等の処置として、５つの方法が提案されていた（例えば、特許文献１から５参照のこと）。

第１の方法では、外乱推定オブザーバによって外乱トルクを推定し、この推定外乱トルクが規定値以上になったとき、負荷異常として衝突等が生じているものと判断するようにしていた。この方法は、衝突検出用の特別な検出器を使用することなく、ソフトウェア上での処理により衝突の発生を検出し、サーボモータへの供給動力を遮断し、これによりアームを即座に緊急停止させることができるというものである。
また、第２の方法では、外乱推定オブザーバによって異常負荷を検出すると、異常負荷が生じたサーボモータの速度指令を「０」としてサーボモータを急停止させていた。
また、第３の方法では、サーボモータをそれまでの進行方向とは逆方向に所定量移動させて停止させる方法などの異常負荷検出制御方法が提案されている。

ところが、速度指令を「０」としてサーボモータを急停止させる第２の方法では、サーボモータは機械を押したまま停止するので、サーボモータの停止後も機械に負荷がかかったままであるという問題がある。第３の方法はこの問題を解決するためになされたものであるが、この発明では位置・速度ループによるクローズドループ制御回路をそのまま使用して、所定の位置偏差を入力してサーボモータをそれまでの進行方向とは逆方向に動作させる方法をとっているので、逆方向に動作させるための位置偏差速度指令に対する実際のサーボモータへのトルク指令にも、位置・速度クローズドループ処理を施すことにより遅れが生じることになる。その結果、異常負荷検出時に速度が０になるまでの時間および、負荷がかからない状態に移動するまでの時間が遅れることになる。機械の受けるダメージは異常負荷がかかりはじめてから、速度が０になり、さらに負荷がかからない状態に移動するまでの時間に依存するので、ダメージを小さくするためには、できるだけその時間を短くすることが必要である。

また、第４の方法では、衝突を検出した際、衝突した方向とは逆方向にロボットを位置制御することにより、機械的な接触により生ずる準静的な応力を回避することで、ロボットアームと障害物との衝突による機械的損傷を最小限に抑えられるというものである。しかしながら，衝突検出後、モータ回転方向と衝突方向が同じ場合、衝突によりモータ回転方向と同方向の力、すなわちモータを加速させる方向に衝突トルクが加わることとなり、この場合、衝突した方向とは逆方向にロボットを位置制御するためには、衝突トルクに逆らった方向のトルクをモータに入力する必要があるため、かえって衝突トルクを増大させることになる．

上記問題を解決するために第５の方法では、モータ回転方向と衝突方向が同じ場合は、電流制御を介さずに、位置制御から直接、柔軟制御に切り換え、衝突力に倣って動作させることにより、衝突トルクを緩和し、一方、モータ回転方向と衝突方向が逆の場合は、位置制御から電流制御に切り換え、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度を減速し、衝突エネルギーを緩和し、その後、モータ回転速度が設定値以下になれば、柔軟制御に切り換え、衝突で生じた減速機等での歪みを解消するというものである。
特開平６−１３１０５０号公報 特開平３−１９６３１３号公報 特開平６−２４５５６１号公報 特開平６−２７８０８１号公報 国際公開Ｗ０２００５/００９６９２号公報

ところが、モータ回転方向と衝突方向が同じ場合、柔軟制御に切り換え、衝突力に倣って動作させることにより、衝突トルクを緩和することができるが、柔軟制御に切り換えるということは、ロボットは制御不能状態であり、衝突時のロボットの慣性が大きい場合、非常に危険である。特に、衝突時のロボット基本軸のサーボモータ回転速度が速い場合、柔軟制御に切り換るとことにより、慣性が０になるまでロボットは大きく動作することとなり、再衝突の可能性があり、ロボット及び障害物に与える機械的ダメージは大きくなる。また、ロボットに存在する関節全軸を柔軟制御とした場合、ロボットは指令軌跡上から大きくずれることとなり、衝突検出後、再プレイバックした際、ティーチング時とは違う軌跡上を移動することなり、障害物に衝突する可能性がある。
そこで、本発明はサーボモータあるいはサーボモータを備えた多関節ロボットが障害物等に衝突した際、衝突を検出する軸、モータ回転方向と衝突トルクの方向、モータ回転速度により、手首軸と基本軸の停止方法を切り換えることにより、ロボット及び障害物に与える機械的ダメージを最小限に抑えるロボットの停止方法、及び、衝突検出後、再プレイバックした際、再衝突を回避する復帰方法を提供することを目的とする。

上記問題を解決するため、本発明は、次のようにしたのである。
請求項１に記載の発明は、モータにより駆動されるロボットアームと、前記ロボットアームが障害物に衝突することを検知する衝突検出装置を備え、前記衝突検出装置からの情報をもとに前記ロボットアームを制御するロボットシステムにおいて、前記衝突検出装置からの前記情報をもとに複数の停止方法のうちから一つの停止方法を選択する停止方法選択処理部を備え、前記停止方法選択処理部は、前記衝突検出装置で衝突を検出した軸が前記ロボットアームの基本軸の場合、前記ロボットアームの全軸を引き戻して停止させる停止方法を選択する一方、前記衝突検出装置で衝突を検出した軸が前記ロボットアームの手首軸の場合、柔軟停止処理により前記手首軸を停止させつつ即停止処理により前記基本軸を停止させる停止方法と、前記全軸を引き戻して停止させる停止方法とのいずれかを選択するものである。
請求項２に記載の発明は、前記全軸を引き戻して停止させる場合、引き戻し速度は、衝突直前の前記モータの回転速度に応じて決定されるものである。
請求項３に記載の発明は、前記衝突検出装置からの前記情報をもとに複数の停止方法により停止したのちに、正常位置へ移動するための復帰位置記憶装置を備えたものである。
請求項４に記載の発明は、前記復帰位置記憶装置が、衝突を検出した瞬間の指令位置発生装置からの位置指令を保存するものである。
請求項５に記載の発明は、前記衝突検出装置が、前記ロボットアームの基本軸の全ておよび手首軸の全てに備えられたものである。
請求項６に記載の発明は、前記ロボットアームの前記手首軸に、停止方法選択処理部で選択された停止方法に応じて動作する重力補償する切替え装置を備えたものである。
請求項７に記載の発明は、モータの制御におけるロボットアームの衝突検出時のロボットシステムの制御方法において、複数の停止方法のうちから一つの停止方法を選択する停止方法選択処理部により、衝突検出軸が前記ロボットアームの基本軸の場合、前記ロボットアームの全軸を引き戻して停止させる停止方法を選択する一方、衝突検出軸が前記ロボットアームの手首軸の場合、柔軟停止処理により前記手首軸を停止させつつ即停止処理により前記基本軸を停止させる停止方法と、前記全軸を引き戻して停止させる停止方法とのいずれかを選択するものである。
請求項８に記載の発明は、前記停止方法選択処理部で、衝突検出軸が前記手首軸で、且つ、モータ回転方向と衝突トルクの方向が同じ場合、柔軟停止処理により前記手首軸を停止させ、即停止処理により、前記基本軸を停止させるものである。
請求項９に記載の発明は、前記停止方法選択処理部では、衝突検出軸が前記手首軸で、且つ、モータ回転方向と衝突トルクの方向が逆の場合、且つ、前記手首軸の衝突時のサーボモータの回転速度が、規定値以下の場合、柔軟停止処理により前記手首軸を停止させ、即停止処理により、前記基本軸を停止させるものである。
請求項１０に記載の発明は、前記停止方法選択処理部では、衝突検出軸が前記手首軸で、且つ、モータ回転方向と衝突トルクの方向が逆の場合、且つ、前記手首軸の衝突時のサーボモータの回転速度が、規定値以上の場合、前記全軸を引き戻しさせるものである。
請求項１１に記載の発明は、衝突を検出した瞬間の位置指令を記憶し、再プレイバック要求があった場合、前記記憶された位置に低速で前記全軸戻してから、プレイバック動作させるものである。

以上、述べたように、本発明によれば、サーボモータあるいはサーボモータを備えた多関節ロボットが障害物等に衝突した際、衝突を検出する軸、モータ回転方向と衝突トルクの方向、モータ回転速度により、手首軸と基本軸の停止方法を切り換えることにより、ロボット及び障害物に与える機械的ダメージを最小限に抑えることができ、更に、衝突検出後、再プレイバックした際、再衝突を回避出来るという効果がある。

以下、本発明の実施の形態について図を参照して説明する。

図１は本発明の衝突検出装置を用いたサーボモータの駆動システムを示すブロック図であり、関節軸がＮ個存在する場合、各関節軸に対して各１個の衝突検出装置を用いたサーボモータの駆動システムで、基本軸制御ブロック２０と手首軸制御ブロック２１から構成される。
基本軸および手首軸について説明すると、ロボットアームは、一般的な産業用ロボットでは、例えば特開２００７−３２６１５１号公報に見られるような６軸で構成されており、ロボットアームの姿勢を形成するＳ軸、Ｌ軸、Ｕ軸が基本軸と呼ばれ、ロボットアーム先端の姿勢を形成するＲ軸、Ｂ軸、Ｔ軸が手首軸と呼ばれている。
図において１は産業用ロボットが適正に動作するために求められた指令位置を出力する指令位置発生装置である。２は指令位置発生装置からの位置指令に対して比例制御をし、速度に対して比例積分制御して、トルク指令を指令する制御部である。３は制御部２が供給する電流により回転するサーボモータ、４はサーボモータ３によって駆動される制御対象のロボットアームである。５はサーボモータの位置を検出するエンコーダであり，６は制御部２のトルク指令とサーボモータ３に設けられたエンコーダ５の回転信号を受けてロボットアーム４が受ける外乱を推定する衝突検出装置、７は衝突検出装置監視部である。衝突検出装置６は、サーボモータ３のモータトルクと回転位置からロボットアーム４に作用する外乱を推定する外乱推定オブザーバを備えている。衝突検出装置監視部７では、この推定された外乱トルクが規定値以上になったとき、衝突が生じているものと判断するようにしている。衝突検出装置監視部７で衝突と判断されると、停止方法選択処理１５で、ロボットを停止するための手段を選択し、その後、ロボットはその選択方法に従い、ロボット停止処理９で停止することとなる。

以下に、図１および図２を用いて停止選択処理について詳しく説明する。衝突検出装置６では制御部２のトルク指令とサーボモータ３に設けられたエンコーダ５の回転信号を受けてロボットアーム４が受ける外乱を外乱推定オブザーバにより推定する。推定した外乱と、予め設定しておいた規定値を衝突検出装置監視部７で比較する。関節軸がＮ個存在する場合、Ｎ関節軸全ての軸で外乱が規定値以下であれば、ロボットは指令位置発生装置１から出力された指令に従い動作する。衝突検出装置監視部７で規定値以上となった関節軸が１関節でもあれば、衝突と判断され、Ｓ２の処理にすすむ。

衝突検出装置監視部７で衝突と判断すると、先ず衝突を検出した軸により、Ｓ２のステップで、ロボットを停止する方法を分別する。衝突検出装置監視部７で衝突と判断された関節が基本軸の場合、全ての関節軸を引き戻し方法で停止させる。衝突検出装置監視部７で衝突と判断された関節が手首軸の場合は、Ｓ３の処理にすすむ。

Ｓ３では、衝突を検出した軸の衝突時のモータ回転方向と衝突によりサーボモータに作用するトルク（＝衝突トルク）が発生する方向を比較し、モータ回転方向と衝突トルクの方向が逆の場合は、Ｓ４に進む。衝突トルクの方向は、衝突検出装置６で推定した外乱の符号により判断する。

衝突検出軸が手首軸で、且つ、モータ回転方向と衝突トルクの方向が同じ場合、手首軸に関しては柔軟停止処理により手首軸を停止させ、基本軸に関しては即停止処理により、基本軸を停止させる。

衝突検出軸が手首軸で、且つ、モータ回転方向と衝突トルクの方向が逆の場合、Ｓ４に進む。Ｓ４では、衝突を検出した手首軸の衝突時のサーボモータの回転速度と、予め設定した速度規定値とを比較する。手首軸の衝突時のサーボモータの回転速度が、規定値以上となれば、基本軸、手首軸、全ての関節軸を引き戻し処理によりロボットを停止させる。

衝突検出軸が手首軸で、且つ、モータ回転方向と衝突トルクの方向が逆、且つ、手首軸の衝突時のサーボモータの回転速度が規定値以下となれば、手首軸に関しては柔軟停止処により手首軸を停止させ、基本軸に関しては即停止処理により、基本軸を停止させる。

上記したように、衝突を検出した軸、衝突直前のサーボモータの回転方向、衝突トルクの方向、サーボモータの回転速度によりロボット停止方法を切り換える。以下に引き戻し停止、柔軟停止、即停止処理について詳しく説明する。

先ずは、図１を用いて引き戻し停止処理について説明する。指令位置発生装置１により産業用ロボットが適正に動作するために求められた各駆動軸の指令位置を出力され、制御部２により指令位置発生装置からの位置指令に対して比例制御をし、速度に対して比例積分制御してトルク指令を指令し、サーボモータ３により制御部２が供給する電流により回転し、ロボットアーム４が動作する。位置記憶バッファ８によりエンコーダ５から読み込まれたサーボモータ３の現在位置を記憶され、衝突検出器６によりロボットアーム４が受ける外乱を推定し、その推定した外乱と規定値を衝突検出装置監視部７で比較し、衝突検出装置監視部で規定値以上となった関節軸が１関節でもあれば、衝突と判断される。衝突と判断された関節が基本軸の場合、停止方法選択処理１５で引戻し処理が選択され、ロボット停止処理９によってスイッチ１２がクローズされ、引戻しを実行可能状態となり、ロボットに存在する全ての関節軸を引き戻し停止処理で停止させる。もしくは，衝突検出軸が手首軸で、且つ、モータ回転方向と衝突トルクの方向が逆の場合、且つ、手首軸の衝突時のサーボモータの回転速度が、規定値以上の場合、停止方法選択処理１５で引戻し処理が選択され、ロボット停止処理９によってスイッチ１２がクローズされ、引戻しを実行可能状態となり、基本軸、手首軸、全ての関節軸を引き戻し停止処理により停止させる。

図１の停止方法選択処理１５にて引き戻し停止処理が選択されると、位置記憶バッファ８に記憶された位置データに基づき、引き戻し位置と引き戻し速度を決定する。引き戻し位置は、記憶バッファに記録されたデータの中で一番古い位置データに設定される。記憶バッファには予め設定されたＭ個の位置データが格納されているので、一番古いデータは記憶バッファにあるデータの中で衝突した障害物から一番離れた位置となる。
引き戻し位置をθ１、衝突検出時のサーボモータk３の位置をθ２とすると、引き戻し距離θは式１で表される。
θ=θ１-θ２ （１）

位置記憶バッファには同じ時間の間隔で記録されており，その周期をＴとする。引き戻し速度Ｖは、引き戻し量θ、位置記憶バッファに記憶する周期Ｔ、位置記憶バッファに記憶するデータ数Ｍを用いて式２で表される。
Ｖ=θ/(Ｔ*Ｍ) （２）

上式を用いて引き戻し速度を決定することにより、衝突直前のサーボモータの回転速度が速いときは、引き戻し速度は速く、かつ引き戻し量は長くなり、回転速度が遅いときは引き戻し速度は遅く、かつ引き戻し量は短くなる。

衝突検出時のサーボモータ３の位置θ２と、引き戻し速度Ｖを指令位置変更装置10に対して出力する。指令位置変更装置10は、位置記憶バッファ８から衝突検出時のサーボモータ３の位置θ２と、引き戻し速度Ｖが入力されると、次式により，指令位置θrを求め、指令位置発生装置１へ出力する。
θr =θ2+ （Ｖ*Ｔr *Ｉ） （Ｉ＝１、２…、Ｔ*Ｍ/Ｔｒ） （３）

ここでＴrは位置指令の更新周期である。指令位置変更装置10は数３により、位置指令の更新周期Ｔr 毎にＩを１からＴ*Ｍ/Ｔｒまで１づつ増やし、指令位置θrを求め、指令位置発生装置１へ出力する。位置指令の更新回数がＴ*Ｍ/Ｔｒの時、 指令位置θr はθ１と一致する。位置指令をθ２からθ１に向かって更新することにより、サーボモータには衝突直前の逆方向のトルクが発生し、その逆方向のトルクによりロボットは障害物から離れる方向に位置制御されることとなる。衝突直前のサーボモータの回転速度が速いときは、引き戻し速度は速く、かつ引き戻し量は長くなり、回転速度が遅いときは引き戻し速度は遅く、かつ引き戻し量は短くなる。

衝突時のサーボモータの回転速度が速ければ、ロボットの障害物への行き過ぎ量は長くなり、ロボットを確実に障害物から引き戻すためには、引き戻し量を長くする必要がある。また、ロボットと障害物の受けるダメージは衝突時の速度に比例することとなるので、速度に比例して引き戻す速度を速くする必要がある。

ロボットと障害物の受けるダメージは衝突時の速度に比例することとなる。本実施例を適用すれば衝突検出後、引き戻し速度を衝突前のサーボモータの回転速度に比例して決定するので、ロボットと障害物の受けるダメージを最小限に抑えることができる。また、衝突前のサーボモータの回転速度に比例して引き戻し量は長くなるので、ロボットを確実に障害物から引き戻すことができる。

衝突検出軸が手首軸ではなく基本軸の場合、モータ回転方向と衝突トルクの方向によらず、全軸引き戻し停止とする。モータ回転方向と衝突トルクが同じ場合、基本軸を柔軟制御に切り換え衝突力に倣って動作することとなり、衝突トルクを緩和することができるが、基本軸は手首軸と比べて慣性が大きく、大きな慣性力を持った基本軸を柔軟停止とした場合、ロボットは衝突検出後大きく動作することとなり、再衝突の危険がある。そこで、基本軸で検出した場合、モータ回転方向と衝突トルクの方向によらず、全軸引き戻し停止とすることにより、再衝突の危険性は柔軟制御と比べ低く、より安全である。また、引き戻し停止処理では、引き戻す位置はロボットの軌跡上に引き戻すため、引き戻し停止処理後、再プレイバックした場合、ロボットはティーチング時の指令軌跡上を動作することとなり、再衝突することはない。

衝突検出軸が手首軸で、且つ、モータ回転方向と衝突トルクの方向が逆の場合、且つ、手首軸の衝突時のサーボモータの回転速度が規定値以上の場合、手首軸の慣性トルクが大きいので、モータ回転と逆方向のトルクをモータで発生させることによりモータ回転速度を減速し、衝突エネルギーを緩和し、ロボットと障害物の受けるダメージを最小限に抑えることができる。また、引き戻し停止処理では、引き戻す位置はロボットの軌跡上に引き戻すため、引き戻し停止処理後、再プレイバックした場合、ロボットはティーチング時の指令軌跡上を動作することとなり、再衝突することはない。

次に、手首軸柔軟停止処理、基本軸即停止処理について詳しく説明する。衝突検出軸が手首軸で、且つ、モータ回転方向と衝突トルクの方向が同じ場合、柔軟停止処により手首軸を停止させ、即停止処理により、基本軸を停止させる。もしくは，衝突検出軸が手首軸で、且つ、モータ回転方向と衝突トルクの方向が逆の場合、且つ、手首軸の衝突時のサーボモータの回転速度が、規定値以下の場合、柔軟停止処理により手首軸を停止させ、即停止処理により、基本軸を停止させる。

図１を用いて柔軟停止処理が選択された場合の手首軸の制御系を説明する。衝突検出器６により、ロボットアーム４が受ける外乱を推定し、その推定した外乱と規定値を衝突検出装置監視部７で比較し、衝突検出装置監視部で規定値以上となった関節軸が１関節でもあれば、衝突と判断される。停止方法選択処理１５にて柔軟停止処理が選択されると、衝突検出後ただちに、制御手段内で、スイッチ１３をオープンにして、強制的に制御部２からのトルク指令値を０にする。このままだと重力の作用するアームの場合、アームが落下するため、スイッチ１４をクローズし、サーボモータへのトルク指令値に重力補償値を加算する。重力補償値はロボットアームの姿勢および重量パラメータから計算するか、または計測で予め求めておく。これにより、衝突後、手首軸は衝突時に受ける衝突トルクに対して柔軟に倣う状態になる。また、衝突検出時、スイッチ１３をオープン、スイッチ１４をクローズにすると同時に衝突を検出した瞬間の指令位置発生装置からの位置指令を復帰位置記憶装置１１に出力する。

また、図１を用いて即時停止処理が選択された場合の基本軸の制御系を説明する。停止方法選択処理１５により即時停止処理が選択されると、ロボット停止処理９によりスイッチ１２をクローズ、即時停止処理を実行可能状態とする。位置記憶バッファ８にて、エンコーダ５から読み込まれたサーボモータ３の現在位置を記憶する。衝突検出器６により、ロボットアーム４が受ける外乱を推定し、その推定した外乱と規定値を衝突検出装置監視部７で比較し、衝突検出装置監視部で規定値以上となった関節軸が１関節でもあれば、衝突と判断される。停止方法選択処理にて即時停止処理が選択されると、位置記憶バッファに記憶された位置データに基づき、停止位置を決定する。衝突を検出した瞬間の位置記憶バッファ８に記録されたデータの中で一番新しい位置データを停止位置とする。記憶バッファ８には予め設定されたＭ個の位置データが格納されているので、一番新しいデータは衝突を検出した位置となる。位置記憶バッファ８に記録されたデータの中で一番新しい位置データを指令位置変更装置10に対して出力する。サーボモータ３には衝突直前の逆方向のトルクが発生し、その逆方向のトルクによりロボットアーム４は衝突検出時の位置に位置制御されることとなる。また、衝突を検出した瞬間の指令位置発生装置からの位置指令を復帰位置記憶装置１１に出力する。

停止方法選択処理１５にて手首軸柔軟停止処理、基本軸即停止処理が選択されるのは、衝突検出軸が手首軸で、且つ、モータ回転方向と衝突トルクの方向が同じ場合、もしくは，衝突検出軸が手首軸で、且つ、モータ回転方向と衝突トルクの方向が逆の場合、且つ、手首軸の衝突時のサーボモータの回転速度が、規定値以下の場合である。衝突検出軸が手首軸で、且つ、モータ回転方向と衝突トルクの方向が同じ場合、手首軸を柔軟停止とすることにより、手首軸は衝突力に倣って動作することとなり、衝突トルクを緩和することができる。また、この際、基本軸についても柔軟停止をすることが考えられるが、基本軸は手首軸と比べて慣性が大きく、大きな慣性力を持った基本軸を柔軟停止とした場合、ロボットは衝突検出後大きく動作することとなり、再衝突の危険がある。よって、基本軸については柔軟停止ではなく、衝突を検出した位置に停止させる即停止処理とする。

手首軸柔軟停止処理、基本軸即停止処理後、手首軸に関しては制御不能状態となるので、指令軌跡上から外れることとなり，ロボットアーム停止後、再プレイバックをした場合、ロボットはティーチング時とは違う軌跡上を移動することとなり、障害物に衝突する可能性がある。そこで、再プレイバック時の衝突を避けるため、再プレイバック要求があった場合、復帰位置記憶装置１１に記憶された位置に低速で全軸戻してから、プレイバック動作に入ることとする。復帰位置記憶装置１１に記憶された位置は、指令軌跡上の位置であり、障害物に再衝突する可能性は低く、安全である。また低速で指令軌跡上に戻してからプレイバック動作に入るため、指令軌跡上に戻るまでに、再衝突したとしても、ロボットアーム、及び障害物に与える機械的ダメージを最小限に抑えることが出来る。

衝突検出軸が手首軸、且つ、モータ回転方向と衝突トルクの方向が逆の場合、且つ、手首軸の衝突時のサーボモータの回転速度が規定値以下の場合、手首軸を柔軟停止、基本軸を即停止させる。サーボモータの回転速度の規定値は、実機により予め決めておく。衝突検出軸が手首軸で、且つ、手首軸の衝突時のサーボモータの回転速度が規定値以下の場合、手首軸の慣性は小さいので、ロボット及び障害物に与える機械的ダメージは小さい。よって、引き戻し動作をさせるより、手首軸を柔軟停止とすることにより、手首軸は外力に倣って動作することとなり、衝突で生じた減速機等の歪みを素早く解消することが出来る。また、この場合についても再プレイバック要求があった場合、復帰位置記憶装置１１に記憶された位置に低速で全軸戻してから、プレイバック動作に入ることとする。復帰位置記憶装置１１に記憶された位置は、指令軌跡上の位置であり、障害物に再衝突する可能性は低く、安全である。また低速で指令軌跡上に戻してからプレイバック動作に入るため、指令軌跡上に戻るまでに、再衝突したとしても、ロボットアーム及び障害物に与える機械的ダメージを最小限に抑えることが出来る。

本発明の実施例を示すブロック図 本発明による衝突検出後の停止方法を決定するフローチャート図

符号の説明

１ 指令発生装置
２ 制御部
３ サーボモータ
４ ロボットアーム
５ エンコーダ
６ 衝突検出装置
７ 衝突検出装置監視部
８ 位置記憶バッファ
９ ロボット停止処理
１０ 指令位置変更装置
１１ 復帰位置記憶装置
１２ スイッチ
１３ スイッチ
１４ スイッチ
１５ 停止方法選択処理
２０ 基本軸制御ブロック
２１ 手首軸制御ブロック

Claims (11)

1. モータにより駆動されるロボットアームと、前記ロボットアームが障害物に衝突することを検知する衝突検出装置を備え、前記衝突検出装置からの情報をもとに前記ロボットアームを制御するロボットシステムにおいて、
   前記衝突検出装置からの前記情報をもとに複数の停止方法のうちから一つの停止方法を選択する停止方法選択処理部を備え、
   前記停止方法選択処理部は、前記衝突検出装置で衝突を検出した軸が前記ロボットアームの基本軸の場合、前記ロボットアームの全軸を引き戻して停止させる停止方法を選択する一方、前記衝突検出装置で衝突を検出した軸が前記ロボットアームの手首軸の場合、柔軟停止処理により前記手首軸を停止させつつ即停止処理により前記基本軸を停止させる停止方法と、前記全軸を引き戻して停止させる停止方法とのいずれかを選択することを特徴とするロボットシステム。
2. 前記全軸を引き戻して停止させる場合、引き戻し速度は、衝突直前の前記モータの回転速度に応じて決定されることを特徴とする請求項１記載のロボットシステム。
3. 前記衝突検出装置からの前記情報をもとに複数の停止方法により停止したのちに、正常位置へ移動するための復帰位置記憶装置を備えたことを特徴とする請求項１または２記載のロボットシステム。
4. 前記復帰位置記憶装置は、衝突を検出した瞬間の指令位置発生装置からの位置指令を保存することを特徴とする請求項３記載のロボットシステム。
5. 前記衝突検出装置は、前記ロボットアームの前記基本軸の全ておよび前記手首軸の全てに備えられたことを特徴とする請求項１乃至４のいずれか１つに記載のロボットシステム。
6. 前記ロボットアームの前記手首軸には、停止方法選択処理部で選択された停止方法に応じて動作する重力補償する切替え装置を備えられたことを特徴とする請求項５記載のロボットシステム。
7. モータの制御におけるロボットアームの衝突検出時のロボットシステムの制御方法において、
   複数の停止方法のうちから一つの停止方法を選択する停止方法選択処理部により、衝突検出軸が前記ロボットアームの基本軸の場合、前記ロボットアームの全軸を引き戻して停止させる停止方法を選択する一方、衝突検出軸が前記ロボットアームの手首軸の場合、柔軟停止処理により前記手首軸を停止させつつ即停止処理により前記基本軸を停止させる停止方法と、前記全軸を引き戻して停止させる停止方法とのいずれかを選択することを特徴とするロボットシステムの制御方法。
8. 前記停止方法選択処理部では、衝突検出軸が前記手首軸で、且つ、モータ回転方向と衝突トルクの方向が同じ場合、柔軟停止処理により前記手首軸を停止させ、即停止処理により、前記基本軸を停止させることを特徴とする請求項７記載のロボットシステムの制御方法。
9. 前記停止方法選択処理部では、衝突検出軸が前記手首軸で、且つ、モータ回転方向と衝突トルクの方向が逆の場合、且つ、前記手首軸の衝突時のサーボモータの回転速度が、規定値以下の場合、柔軟停止処理により前記手首軸を停止させ、即停止処理により、前記基本軸を停止させることを特徴とする請求項７または８記載のロボットの制御方法。
10. 前記停止方法選択処理部では、衝突検出軸が前記手首軸で、且つ、モータ回転方向と衝突トルクの方向が逆の場合、且つ、前記手首軸の衝突時のサーボモータの回転速度が、規定値以上の場合、前記全軸を引き戻しさせることを特徴とする請求項７乃至９のいずれか１つに記載のロボットの制御方法。
11. 衝突を検出した瞬間の位置指令を記憶し、再プレイバック要求があった場合、前記記憶された位置に低速で前記全軸戻してから、プレイバック動作させることを特徴とする請求項７乃至１０のいずれか１つに記載のロボットの制御方法。

Images