ロボットシステム
【課題】ロボット自体に異常がない状態でロボットを停止させた場合に、ロボットをその停止位置から所定の待機位置まで戻す動作を行う際における衝突のリスクを軽減する。
【解決手段】通常動作が行われる際、角度指令生成部27は、各モータMに対する回転角度指令を比較的短い生成周期毎に生成し、その生成した回転角度指令の一部を比較的長い出力周期毎に制御部26に出力する。角度指令記憶部28は、角度指令生成部27が生成した回転角度指令の全てを順次記憶する。遡り動作が行われる際、角度指令生成部27は、角度指令記憶部28に記憶されている回転角度指令を新しいものから古いものへと順に読み出すとともに、それら読み出した回転角度指令を遡り角度指令として出力周期毎に出力する。制御部26は、各軸の回転角度を遡り角度指令に一致させるようにモータMによる各軸の駆動をフィードバック制御する。
【解決手段】通常動作が行われる際、角度指令生成部27は、各モータMに対する回転角度指令を比較的短い生成周期毎に生成し、その生成した回転角度指令の一部を比較的長い出力周期毎に制御部26に出力する。角度指令記憶部28は、角度指令生成部27が生成した回転角度指令の全てを順次記憶する。遡り動作が行われる際、角度指令生成部27は、角度指令記憶部28に記憶されている回転角度指令を新しいものから古いものへと順に読み出すとともに、それら読み出した回転角度指令を遡り角度指令として出力周期毎に出力する。制御部26は、各軸の回転角度を遡り角度指令に一致させるようにモータMによる各軸の駆動をフィードバック制御する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、ロボット自体に異常がなくロボットを停止させた場合に、ロボットをその停止位置から所定の待機位置まで戻す動作を行う機能を備えたロボットシステムに関する。
【背景技術】
【0002】
例えば製造ラインなどにおいてロボットが用いられる場合、ロボットに異常がなくとも、ロボットと協調して動作する他の設備の異常(コンベアの停止、ワークとなる部品が流れてこない等)が原因でロボットの動作を停止せざるを得ない場合がある。このような場合、以下の理由からロボットを停止位置から所定の待機位置まで戻さなければならない。すなわち、他の設備をリセットした(所定の待機位置に戻した)にもかかわらず、ロボットだけが停止した位置まで進んだ状態になっていると、他の設備とロボットとの動作整合が取れなくなってしまう。
【0003】
現在、ロボットを停止位置から所定の待機位置まで戻す方法としては、ユーザの手動操作により移動させる方法、または、停止位置から待機位置へ戻すための動作プログラムを準備しておき、ロボット停止時にその停止位置に対応する動作プログラムを選択して起動させてロボットを移動させる方法などが用いられている。
【0004】
しかし、手動操作による方法では、ロボットの周辺に配置された設備等の物体に衝突することなく待機位置へ移動させるために正確な操作が要求される。このため、ユーザの作業負担が増大する。また、待機位置へ戻すための動作プログラムを用いる方法では、想定されるロボットの停止位置の数だけの動作プログラムを予め用意しておく必要がある。さらに、現実には想定外の位置でロボットが停止する場合も考えられ、この場合には、予め用意した動作プログラムでは、ロボットを待機位置へ戻すことができない可能性もある。
一方、特許文献1には、ロボットが異物と衝突したと判断した場合に、自動的に手先を異物と衝突しない位置まで戻す技術が開示されている。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特開平5−84681号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
設備の異常に伴ってロボットが停止した場合に、上記特許文献1記載の技術を用いれば、自動的にロボットの手先位置を所定の待機位置(待避位置)まで戻すことが可能となる。しかし、特許文献1記載の技術を用いただけでは、戻り動作時において衝突が生じる危険性がある。なぜなら、ロボットには姿勢という概念があり、単に手先位置が待機位置まで戻ればよいというわけではない。つまり、進み方向に所定動作を行った後に停止され、その後、単純に戻り方向に上記所定動作を行うと、進み方向に所定動作を行ったときとはロボットの姿勢が異なる可能性がある。例えば、進み方向に所定動作を行った際には、周辺の設備から離れるように肘の部分が折り畳まれた姿勢であったにもかかわらず、戻り方向に所定動作を行う際には、周辺の設備に接近するように肘の部分が張り出した姿勢となる場合も考えられる。このように姿勢が異なる場合、戻り方向に所定動作を行うときに、ロボットの肘の部分が周辺の設備に衝突する危険性(リスク)がある。
【0007】
本発明は上記事情に鑑みてなされたものであり、その目的は、ロボット自体に異常がない状態でロボットを停止させた場合に、ロボットをその停止位置から所定の待機位置まで戻す動作を行う際における衝突のリスクを軽減することができるロボットシステムを提供することにある。
【課題を解決するための手段】
【0008】
請求項1記載の手段によれば、通常の動作(進み動作)を行う場合、制御部は、ロボットに設けられた複数の軸の回転角度を角度指令生成部から出力される回転角度指令に一致させるように、駆動部による各軸の駆動をフィードバック制御する通常制御を実行する。ここで、複数の軸の回転角度は、回転角度検出部によって検出される。また、角度指令生成部は、回転角度指令を生成周期毎に生成し、その回転角度指令の一部を生成周期よりも長い出力周期毎に出力する。そして、角度指令記憶部は、上記通常制御が実行されている期間に角度指令生成部により生成される回転角度指令の全てを順次記憶する。
【0009】
この理由は以下のとおりである。すなわち、現在において、回転角度指令は、通常、ソフトウエアによる処理(電気的な動作のみに依存する処理)によって生成されることが多いため、極めて短い間隔である生成周期毎に生成することができる。一方、駆動部、回転角度検出部などは、ハードウエアによる処理(機械的な動作が含まれる処理)によって、ロボットの軸を駆動したり、軸の回転角度を検出したりするため、回転角度を回転角度指令に一致させるための動作間隔は、上記生成周期に比べて長い間隔にならざるを得ない。このため、回転角度指令の生成周期を上記動作間隔に合わせて長くするか、あるいは短い生成周期で生成した回転角度指令を上記動作間隔に合わせて間引いて出力するかのいずれかを採用する必要がある。本手段では、後述する遡り制御において短い生成周期で生成した回転角度指令を用いる必要があるため、後者の方法を採用している。
【0010】
さて、従来技術において説明したように、ロボットと協調して動作する他の設備に異常が生じた場合には、ロボットに異常がなくても、その通常制御の実行が停止されるとともにロボットを停止するための動作がなされる。このようにロボットが停止された場合には、その停止位置からロボットを所定の待機位置まで戻す動作(遡り動作)を行う必要がある。このような場合、例えば、ユーザによる所定の操作や、ロボットが上記したように停止すると自動的に起動するプログラムの動作などにより、遡り指令入力部を介して遡り開始指令が入力される。すると、角度指令生成部は、角度指令記憶部に記憶された回転角度指令を新しいものから古いものへと順に読み出すとともに、それら読み出した回転角度指令を遡り角度指令として出力周期毎に出力する。すなわち、進み動作時における全軸の回転角度のログを逆の順番に並べ替えたものが遡り角度指令となって出力される。
【0011】
制御部は、各軸の回転角度を上記遡り角度指令に一致させるように、駆動部による各軸の駆動をフィードバック制御する遡り制御を実行する。これにより、ロボットの全ての軸の回転角度が進み動作時と同じ値をとりつつ遡り動作が行われる。言い換えれば、ロボットが、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。つまり、いわゆるプレイバック動作が行われるので、進み動作時と同じ姿勢を保ちつつ遡り動作が行われることになる。このような遡り動作により、ロボットが所定の待機位置まで戻ることで、遡り指令入力部を介して遡り終了指令が入力される。すると、角度指令生成部は、遡り角度指令の出力を停止し、制御部は、遡り制御の実行を停止する。このように、本手段によれば、ロボットを停止位置から所定の待機位置まで戻す遡り動作が行われる際に、ロボットの姿勢が変化することで生じる衝突のリスクを軽減することができる。
【0012】
また、遡り動作の制御に用いる遡り角度指令は、生成周期毎に生成される全ての回転角度指令を出力周期毎に出力したものである。つまり、本来は比較的短い生成周期毎の回転角度を指令する回転角度指令を比較的長い周期毎に出力するので、遡り動作時における各軸の回転速度、ひいてはロボットの動作速度が通常動作時に比べて遅くなる。このように、遡り動作時には、通常動作時と比べてロボットの動作が遅くなる(スロー動作になる)ので、仮にロボット周辺の他の設備がユーザの予期せぬような動きを行った場合であっても、ユーザはその動きに対して余裕を持って対処することができる。このため、遡り動作が行われる際に初めて生じる衝突のリスクについても軽減することができる。
【0013】
さて、ロボットが例えば周辺設備に衝突したことで停止した場合、請求項1記載の手段における遡り制御を実行しようとすると以下の問題が生じる。すなわち、衝突により停止した場合、停止時点の各軸の回転角度が回転角度指令(遡り角度指令)とは、かけ離れた値となっていることが多い。このため、正常に遡り制御を実行することは難しい。また、仮に遡り制御がうまく実行できたとしても、それは、周辺の設備に衝突したときの軌跡をロボットが再び通ることになる。つまり、ロボットが周辺の設備に再度衝突してしまう可能性が非常に高くなる。従って、衝突による停止時には、上記遡り制御を実行することなく、ロボットを含む各設備の動作を完全に停止させた上で、衝突が生じた原因を取り除くことが望ましい。そこで、請求項2記載の手段を採用することが考えられる。
【0014】
請求項2記載の手段によれば、制御部は、遡り開始指令が入力されると、角度指令記憶部に記憶されている最新の回転角度指令と回転角度検出部により検出された現在の回転角度との差分を求める。そして、この差分の絶対値が所定の誤差値以下である場合には遡り制御を実行する。一方、上記差分の絶対値が所定の誤差値を超える場合には遡り制御は実行しない。ロボットが正常に動作している場合であっても、回転角度と回転角度指令とは、種々の誤差の影響によって完全に一致しない場合がある。これに対し、ロボットが衝突により停止した場合には、上記したとおり、回転角度と回転角度指令とは大きくかけ離れた値となる。従って、制御部は、回転角度と回転角度指令との差分の絶対値が所定の誤差値を超えた場合には、ロボットが衝突により停止した状態であると判断し、遡り制御を実行しないようにしている。これにより、遡り動作が行われることによってロボットが再衝突するリスクを抑制することができる。
【0015】
請求項3記載の手段によれば、ロボットの衝突を検出する衝突検出手段を備えている。ロボットの衝突としては、自動運転中の衝突や、教示(ティーチング)中または動作確認中の衝突などが考えられる。前者の場合、ロボットは最高速度で動作している可能性が高く、衝突時の速度(衝突速度)は比較的速い。このため、ロボットの機構部品や周辺設備が多大な損傷を受ける可能性が高く、その後の自動復旧が困難な状態となる。このような場合には、衝突により停止した時点の各軸の回転角度と回転角度指令との偏差が過大になることが多いため、遡り制御の実行自体が困難となる。
【0016】
一方、後者の場合、ロボットは低速で動作しているため、その衝突速度は比較的遅い。このため、ロボットの機構部品や周辺設備がほとんど損傷することなく、その後の自動復旧が可能な状態となる。このような場合には、衝突により停止した時点の各軸の回転角度と回転角度指令との偏差が比較的小さいことが多いため、遡り制御を実行することが可能となる。
【0017】
また、上記した2通りの衝突のうち、衝突速度の速い前者の衝突よりも衝突速度の遅い後者の衝突のほうが発生頻度は高い。なぜなら、自動運転中、ロボットは動作プログラムに基づいて定められた動作を繰り返し行う。そして、その繰り返される動作は、予め周辺設備などに衝突することがないように十分に検証されたものである。このため、自動運転中に衝突する可能性は極めて低い。これに対し、教示中や動作確認中においては、周辺設備との衝突検証を兼ねて教示や動作確認を行うようになっており、その最中に動作軌跡の修正が頻繁に行われることも多い。このため、教示中や動作確認中には、ロボットが毎回同じ動作を繰り返す可能性は低い(ロボットの動作の再現性は低い)。従って、新たな動作軌跡を生成した後や、動作軌跡を修正した後に行われる教示や確認動作において、周辺設備などへの衝突が生じる可能性は高くなる。このように、衝突速度が速く遡り制御を実行することができない重度の衝突よりも、衝突速度が遅く遡り制御を実行可能な軽微な衝突のほうが発生頻度は高い。このようなことから、軽微な衝突によりロボットが停止した場合にも、遡り制御を実行してロボットを待機位置まで移動させることが望ましい。
【0018】
ただし、軽微な衝突とは言っても、むやみにその衝突回数を増やすことは、精密動作が要求されるロボットにとっては好ましくない。なぜなら、軽微な衝突であっても、その衝突が繰り返されることにより、ロボットを精密に動作させるために行われるキャリブレーションやフレームの精度に狂いが生じる危険性があるからである。
【0019】
そこで、本手段では、軽微な衝突によりロボットが停止した場合、以下のように再衝突の発生を防止する工夫を加えた上で、遡り制御を実行するようにしている。すなわち、制御部は、衝突検出手段により衝突が検出されると、その衝突が発生した時点(衝突タイミング)と、その衝突時の位置(衝突位置)を特定する。制御部は、これら特定された衝突位置および衝突タイミングを用いて、球状をなす進入禁止空間を設定する。すなわち、進入禁止空間の中心は、衝突位置に設定する。また、進入禁止空間の半径は、衝突タイミングの前後における回転角度指令の出力タイミングのうち、衝突タイミングに近い方の出力タイミングに出力された回転角度指令と、衝突位置との間の距離に相当する長さに設定する。そして、制御部は、衝突が検出された状態で遡り開始指令が入力されると、遡り制御を実行する。
【0020】
さて、衝突によりロボットが停止された後に遡り制御を実行する際、角度指令生成部が出力する遡り角度指令に基づいてロボットを遡り動作させると、その動作の軌跡は、必ず衝突位置を通過することになる。従って、ロボットが再度衝突する可能性が高くなってしまう。そこで、角度指令生成部は、衝突が検出された状態で遡り開始指令が入力されると、角度指令記憶部から読み出した回転角度指令に基づくロボットの遡り動作の軌跡が、上記進入禁止空間の外郭に最初に重なる外郭位置と最後に重なる外郭位置との間では、動作軌跡が進入禁止空間の外であり、且つ、進入禁止空間の球の表面に沿う軌跡となるように、読み出した回転角度指令を変更し、その変更後の回転角度指令を遡り角度指令として出力する。
【0021】
このようにすれば、衝突により停止した後、ロボットは、進入禁止空間の外郭に達するまでの間は、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。そして、進入禁止空間の外郭を沿うようにして衝突位置を回避した後、再び進み動作の動作軌跡を逆向きに辿るように遡り動作が行われる。この進入禁止空間の半径は、上記したようにして設定されていることから、非常に小さいものであると言える。このため、進入禁止空間の外郭を沿う動作を行う際、ロボットの肘などの別部分が周辺設備などに衝突するといった新たな衝突が引き起こされる可能性が高まるほど、つまり、新たな別の衝突が発生するようなレベルまでロボットの姿勢が変化することはない。すなわち、進入禁止空間の外郭を沿う遡り動作を行う際におけるロボットの姿勢は、衝突が発生した進み動作時の姿勢とほとんど同じであり、新たな別の衝突を引き起こすような姿勢の変化はない。
【0022】
このように衝突後の遡り動作では衝突位置を回避するため、完全なプレイバック動作とはならない。しかし、上記したとおり進入禁止空間は小さい。また、元々、ロボットが通過する経路(動作軌跡)は、ロボットの体格を考慮した上で周辺設備などに衝突しないように所定のマージンが設けられている。そして、ロボットの体格は、進入禁止空間よりも遥かに大きいものとなっている。このため、本手段のような進入禁止空間の外郭を沿うように動作軌跡が変更された程度では、ロボットが上記マージンの範囲を越えて周辺設備に衝突する可能性は極めて低い。つまり、上記した若干の動作軌跡の変更により、ロボットが新たな衝突を生じることはほとんどあり得ない。従って、衝突後にロボットが遡り動作する際、その衝突位置を確実に避けるとともに新たな衝突の発生を未然に防止しつつ、進み動作時と同じ姿勢を保ちながら遡り動作することが可能となる。
【0023】
請求項4記載の手段によれば、請求項3記載の手段と同様に、ロボットの衝突を検出する衝突検出手段を備えている。制御部は、衝突検出手段により衝突が検出されると、請求項3記載の手段と同様に、衝突タイミングおよび衝突位置を特定するとともに、進入禁止空間を設定する。そして、制御部は、衝突が検出された状態で遡り開始指令が入力されると、遡り制御を実行する。また、角度指令生成部は、衝突が検出された状態で遡り開始指令が入力されると、請求項3記載の手段と同様に、読み出した回転角度指令を変更し、その変更後の回転角度指令を遡り角度指令として出力する。このため、本手段によっても、請求項3記載の手段と同様の作用および効果が得られる。
【0024】
ただし、本手段における進入禁止空間は、請求項3記載の手段における進入禁止空間とは若干異なっている。すなわち、本手段の進入禁止空間の半径は、衝突タイミングの前後における回転角度指令の出力タイミングのうち、衝突タイミングから遠い方の出力タイミングに出力された回転角度指令と、衝突位置との間の距離に相当する長さに設定されている。このため、進入禁止空間の半径(大きさ)が請求項3記載の手段のものと比べて若干大きくなっている。このようにすれば、例えば、ロボットが衝突した設備の表面が、その衝突によって多少変形した場合でも、その変形部分をも避けて遡り動作を行うことが可能になり、設備の変形に伴う衝突を未然に防止できるという効果が得られる。
【0025】
請求項5記載の手段によれば、角度指令生成部は、遡り開始指令が入力されると、角度指令記憶部に記憶された回転角度指令のうち最新のものを除く回転角度指令を新しいものから古いものへと順に読み出す。角度指令記憶部に記憶された回転角度指令のうち最新のものは、ロボットが停止した時点の現在位置を指令するためのものである。このため、請求項1記載の手段のように、最新のものから順に遡り角度指令とした場合には、実際にロボットが遡り動作を開始するのは、最新のものよりも1つ前の遡り角度指令が出力された時点となる。つまり、遡り動作の開始が若干遅れることになる。これに対し、本手段によれば、そのような若干の遅れもなく、素早く遡り動作を開始することができ、以って、遡り動作全体に要する時間を短縮することができる。
【図面の簡単な説明】
【0026】
【図1】本発明の第1の実施形態を示すロボットシステムの構成を概略的に示す図
【図2】ロボットシステムの電気構成図
【図3】通常動作時における回転角度指令の生成および出力タイミングを示す図
【図4】角度指令記憶部に記憶される回転角度指令の一例を示す図
【図5】遡り動作時における回転角度指令の出力タイミングを示す図
【図6】モータ制御の内容を等価的に示すブロック図
【図7】所定の動作プログラムに従って動作するロボットの状態を示す図
【図8】動作プログラムの一例を示す図
【図9】本発明の第2の実施形態を示す図1相当図
【図10】図6相当図
【図11】衝突前後における回転角度指令、回転角度および電流値を示す図
【図12】衝突前後におけるロボットの動作状態を示す図
【図13】衝突位置を通過する際のロボットの手先およびその動作軌跡示す図
【図14】衝突位置付近での進み動作時および遡り動作時の各動作軌跡を示す図
【発明を実施するための形態】
【0027】
(第1の実施形態)
以下、本発明の第1の実施形態について図1〜図8を参照しながら説明する。
図1は、一般的な産業用ロボットのシステム構成を示している。図1に示すロボットシステム1は、ロボット2と、ロボット2を制御するコントローラ3と、コントローラ3に接続されたティーチングペンダント4とから構成されている。
【0028】
ロボット2は、例えば4軸の水平多関節型ロボットとして構成されている。ロボット2は、設置面に固定されるベース5と、ベース5上に垂直軸(第1軸)を中心に旋回可能に連結された第1のアーム6と、第1のアーム6の先端部上に垂直軸(第2軸)を中心に旋回可能に連結された第2のアーム7と、第2のアーム7の先端部に上下動可能で且つ回転可能に設けられたシャフト8とから構成されている。なお、このシャフト8を上下動させる際の軸が第3軸に相当し、回転させる際の軸が第4軸に相当する。シャフト8の先端部(下端部)には、フランジ9が位置決めされて着脱可能に取り付けられる。
【0029】
ベース5、第1のアーム6、第2のアーム7、シャフト8およびフランジ9は、ロボット2のアームとして機能し、アーム先端であるフランジ9には、図示はしないが、エンドエフェクタ(手先)が取り付けられる。ロボット2に設けられる複数の軸はそれぞれに対応して設けられるモータ(図2に符号Mを付して示す)により駆動される。各モータの近傍には、それぞれの回転軸の回転角度を検出するための位置検出器(図示せず)が設けられている。
【0030】
ティーチングペンダント4は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント4は、表面部の中央部に例えば液晶ディスプレイからなる表示部10を有している。表示部10には、各種の画面が表示される。表示部10は、タッチパネルで構成されている。またティーチングペンダント4には、表示部10の周囲に各種のキースイッチ11が設けられており、使用者は、キースイッチ11や上記タッチパネルに設けられるタッチスイッチによって種々の入力操作を行う。キースイッチ11または上記タッチスイッチには、後述する遡り動作の開始を指示するための遡り開始スイッチが含まれている。ティーチングペンダント4は、ケーブルを経由してコントローラ3に接続され、通信インターフェイスを経由してコントローラ3との間で高速のデータ転送を実行するようになっており、キースイッチ11等の操作により入力された操作信号等の情報はティーチングペンダント4からコントローラ3へ送信される。
【0031】
図2は、ロボットシステム1の電気構成を概略的に示すブロック図である。ロボット2には、各軸をそれぞれ駆動するための複数のモータM(図2では1つのみ示す)が設けられている。モータM(駆動部に相当)は、例えばブラシレスDCモータである。コントローラ3には、交流電源21より供給される交流を整流および平滑して出力する直流電源装置22、モータMを駆動するインバータ装置23、電流検出部24、回転角度検出部25、これら各装置の制御などを行う制御部26、角度指令生成部27および角度指令記憶部28が設けられている。
【0032】
直流電源装置22は、整流回路30と平滑用のコンデンサ31とから構成されている。整流回路30は、ダイオードをブリッジの形態に接続してなる周知構成のものである。例えば3相200Vの交流電源21の各相出力は、整流回路30の交流入力端子に接続されている。整流回路30の直流出力端子は、それぞれ直流電源線32、33に接続されている。これら直流電源線32、33間にはコンデンサ31が接続されている。
【0033】
インバータ装置23は、直流電源線32、33間に6つのスイッチング素子例えばIGBT(図1には2つのみ示す)を三相フルブリッジ接続して構成されたインバータ主回路と、その駆動回路とを6組備えている(図2には1組のみ示す)。IGBTのコレクタ・エミッタ間には還流ダイオードが接続されている。また、IGBTのゲートには、駆動回路からゲート信号が与えられている。駆動回路は、制御部26から与えられる指令信号(通電指令Sc)に基づいてパルス幅変調されたゲート信号を出力して各IGBTを駆動する。
【0034】
制御部26は、CPU、ROM、RAM、I/Oなどを備えたマイクロコンピュータを主体として構成されている。電流検出部24は、モータMに流れる電流を検出する電流検出器(図示せず)からの検出信号を制御部26に入力可能なデータに変換して出力する。回転角度検出部25は、モータMの回転角度を検出する位置検出器(図示せず)からの検出信号を制御部26に入力可能なデータに変換して出力する。
【0035】
制御部26は、電流検出部24から出力されるデータを元にモータMに流れる電流の値を取得するとともに、回転角度検出部25から出力されるデータを元にモータMの回転角度および回転速度を取得する。詳細は後述するが、制御部26は、上記取得した回転角度を、角度指令生成部27から与えられる指令値(回転角度指令pc)に一致させるようにインバータ装置23によるモータMの駆動をフィードバック制御する。なお、このフィードバック制御の際に上記取得した電流値および回転速度も用いられる。
【0036】
さて、一般に産業用のロボット2は、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作する通常動作(進み動作)を行うようになっている。これに加えて、本実施形態のロボット2は、自身の異常以外の原因(設備異常などの原因)で停止した際に、各軸が進み動作時の動作軌跡を逆向きに辿るように動作する遡り動作を行うことが可能となっている。
【0037】
角度指令生成部27は、制御部26の上位の制御装置に相当するものであり、制御部26と同様にマイクロコンピュータを主体として構成されている。角度指令生成部27は、通常動作が行われる際には、上記動作プログラムを解釈し、ロボット2に動作プログラムに従った動作を行わせるように各モータMを制御するための回転角度指令を生成周期毎に生成する。角度指令生成部27は、生成した回転角度指令の一部を回転角度指令pcとして出力周期毎に制御部26に出力する。出力周期は、生成周期よりも長くなっている。
【0038】
図3は、通常動作時における角度指令生成部27による回転角度指令の生成および出力のタイミングを示している。図3に示すように、角度指令生成部27は、比較的短い生成周期(本実施形態では1ms)毎に生成した回転角度指令の一部を、比較的長い出力周期(本実施形態では8ms)毎に出力する。言い換えると、角度指令生成部27は、生成周期毎に生成した回転角度指令のうち、大部分は出力しない(間引いている)。本実施形態では、出力する回転角度指令と、出力しない(間引く)回転角度指令との割合は、1:7としている。このような構成とした理由は、以下のとおりである。
【0039】
すなわち、回転角度指令は、角度指令生成部27のソフトウエアによる処理によって生成されるため、比較的短い間隔である生成周期毎に生成することができる。一方、モータM、インバータ装置23、回転角度検出部25などは、ハードウエアによる処理によって、ロボット2の各軸を駆動したり、各軸の回転角度を検出したりする。従って、回転角度を回転角度指令に一致させるための動作間隔は、生成周期に比べて長い間隔にならざるを得ない。このため、回転角度指令の生成周期を動作間隔に合わせて長くするか、あるいは短い生成周期で生成した回転角度指令を動作間隔に合わせて間引いて出力するかのいずれかを採用する必要がある。本実施形態では、後述する遡り制御において短い生成周期で生成した回転角度指令を用いる必要があるため、後者の方法を採用している。
【0040】
角度指令記憶部28は、通常動作が行われる期間に角度指令生成部27が生成した回転角度指令の全て(=出力された回転角度指令と間引かれた回転角度指令との両方)を順次記憶する。例えば、通常動作が図3に示した期間だけ行われる場合、生成された回転角度指令pc1’〜pc12’をこの順番通りに記憶する。図4は、角度指令記憶部28に記憶される回転角度指令の一例を示している。図4に示すように、角度指令記憶部28には、ロボット2の全ての軸(第1軸〜第4軸)を駆動するための各モータMに対する回転角度指令(通常動作が開始される時点から終了される時点までの回転角度指令)が記憶される。記憶された回転角度指令のうち、最初の回転角度指令がロボット2の待機位置に相当し、最後の回転角度指令がロボット2の停止位置に相当している。すなわち、角度指令記憶部28に記憶された回転角度指令は、通常動作時における各軸の回転角度(軌跡)の生成周期毎のログであるとも言える。
【0041】
制御部26は、通常動作が行われていない期間にて、遡り開始指令および遡り終了指令を入力する機能(遡り指令入力部に相当)を備えている。つまり、制御部26は、通常動作が行われている期間にあっては、遡り開始指令または遡り終了指令が与えられても、それらの指令を無視する。遡り開始指令は、ロボット2が設備異常などの原因で停止された場合に与えられる。本実施形態では、遡り開始指令は、ティーチングペンダント4に設けられた遡り開始スイッチが操作されることで与えられる。なお、遡り開始指令は、上記スイッチの手動操作によって与えられる構成に限らずともよく、自動的に与えられる構成としてもよい。例えば、遡り開始指令を出力するためのプログラムを実装するとともに、ロボット2が上記原因で停止した場合に、そのプログラムが自動的に実行されるように設定すればよい。また、遡り終了指令は、遡り動作によってロボット2が所定の待機位置まで移動した時に与えられる。
【0042】
制御部26は、遡り開始指令が入力されると、角度指令記憶部28に記憶されている最新の各回転角度指令(図4におけるNo.Nの回転角度指令)と、回転角度検出部25を介して取得した各モータMの現在の回転角度を用いて、遡り可否判断処理を行う。この遡り可否判断処理は、ロボット2の最新の位置、つまりロボット2の停止位置が、最新の指令位置と一致しているか否かを確認するものである(詳細は後述する)。制御部26は、遡り可否判断処理によりロボット2の停止位置が指令位置と一致していると判断した場合、角度指令生成部27に遡り開始指令を出力し、ロボット2に対し遡り動作を行わせる遡り制御を実行する。また、制御部26は、停止位置が指令位置と一致しないと判断した場合、角度指令生成部27に遡り開始指令を出力せず、上記遡り制御を実行しない。制御部26は、遡り終了指令が与えられると、その遡り終了指令を角度指令生成部27に出力し、上記遡り制御の実行を終了する。
【0043】
角度指令生成部27は、通常動作を行っていない期間において、制御部26から遡り開始指令が与えられると、角度指令記憶部28に記憶されている回転角度指令を記憶された時期が2番目に新しいもの(図4におけるNo.N−1)から最も古いもの(図4におけるNo.1)へと順に読み出す。角度指令生成部27は、読み出した全ての回転角度指令を、読み出した順に、つまり、記憶された時期が新しいものから古いものへと順に、回転角度指令pcとして出力する。この場合における回転角度指令pcが遡り角度指令に相当する。図5は、遡り動作時における角度指令生成部27による回転角度指令pcの出力のタイミングを示している。図5に示すように、角度指令生成部27は、読み出した回転角度指令の全てを、出力周期毎に出力する。従って、比較的短い生成周期毎に生成された回転角度指令が、間引かれることなく、その生成された順番とは逆の順番で、比較的長い出力周期毎に出力されることになる。そして、角度指令生成部27は、制御部26から遡り終了指令が与えられると、上記遡り角度指令の出力動作を停止する。
【0044】
図6は、ロボットシステム1におけるモータ制御の内容を等価的に示したブロック図である。図6に示すように、制御部26は、位置制御部41、速度制御部42および電流制御部43を備えている。なお、図6では、1つのモータMの制御に係る構成のみを示しているが、実際には全てのモータMのそれぞれに対応して同様の構成が設けられている。
【0045】
位置制御部41は、角度指令生成部27から与えられる回転角度指令pcに対する現在の回転角度p*の偏差を求める減算器44と、この減算器44の出力(偏差)をゼロに近づけるように速度指令vcを出力する位置制御アンプ45とから構成されている。この位置制御アンプ45のゲインはKpとなっている。
【0046】
速度制御部42は、微分器46、減算器47および速度制御アンプ48により構成されている。微分器46は、現在の回転角度p*を微分して現在の回転速度v*に変換する。減算器47は、速度指令vcに対する現在の回転速度v*の偏差を求める。速度制御アンプ48は、この減算器47の出力(偏差)をゼロに近づけるように電流指令icを出力する。この速度制御アンプ48のゲインはKvとなっている。
【0047】
電流制御部43は、電流指令icに対する現在のモータMに流れる電流i*の偏差を求める減算器49と、この減算器49の出力(偏差)をゼロに近づけるようにインバータ装置23に対する指令信号(通電指令Sc)を出力する電流制御アンプ50とから構成されている。この電流制御アンプ50のゲインはKiとなっている。このような構成により、制御部26は、電流フィードバック制御、速度フィードバック制御および位置フィードバック制御を行い、モータMの駆動をフィードバック制御してロボット2のアームの動作制御を行う。
【0048】
次に、上記構成の作用について図7および図8も参照して説明する。
図7は、所定の動作プログラムに従って動作するロボットの状態を示している。図8は、上記所定の動作プログラムの概要を示している。図8に示す動作プログラム(pro2)の概要は以下のとおりである。すなわち、07行目は、ロボット2の手先を位置P1(待機位置)まで動作させる命令である。09行目は、ロボット2の動作に関係する部品の品番に応じて10行目または15行目に移行させる分岐命令である。10行目に移行した場合には、11〜13行目の命令が実行される。11〜13行目は、ロボット2の手先を位置P2、位置P3、位置P4という順に移動させる命令である。一方、15行目に移行した場合には、16行目以降の命令が実行される。
【0049】
図7では、09行目の分岐命令において10行目に移行した場合のロボット2の状態を示している。従って、この動作プログラムに従った通常動作が行われる際には、ロボット2の手先は、位置P1→位置P2→位置P3→位置P4という順に移動する。なお、図7では、手先が位置P1にあるときのロボット2を実線で示し、手先がそれぞれ位置P2、P3、P4にあるときのロボット2を二点鎖線で示している。
【0050】
この際、角度指令生成部27は、ロボット2が位置P1から位置P4まで順に移動するように各モータMを駆動させるための回転角度指令を生成周期毎に生成するとともに、その一部を出力周期毎に出力する。従って、角度指令生成部27は、25個(=1×4+7×3)の回転角度指令を生成し、そのうちの4個の回転角度指令(位置P1、P2、P3、P4をそれぞれ指令するもの)を出力する。この際、角度指令記憶部28は、角度指令生成部27が生成した25個の回転角度指令を順次記憶している。制御部26は、角度指令生成部27から与えられる回転角度指令pcに基づいて、モータMの駆動をフィードバック制御する(通常制御)。
【0051】
このような通常動作が行われている際、例えば設備などの異常によりロボット2が位置P4において停止された場合、本実施形態では、以下のような手順で遡り動作を実行することが可能となっている。すなわち、ティーチングペンダント4に設けられた遡り開始スイッチが操作されると、制御部26は、遡り可否判断処理を実行する。この遡り可否判断処理において、制御部26は、角度指令記憶部28に記憶された最新の回転角度指令(この場合、位置P4を示す指令)と、現在のモータMの回転角度との差分を各軸について求める。制御部26は、それら差分の絶対値のうち最も大きい値が所定の誤差値以下である場合には、角度指令生成部27に対し遡り開始指令を出力する。この誤差値は、ロボット2が正常に動作している際に生じる回転角度指令と回転角度との誤差(回転角度指令に対する回転角度の偏差)よりも十分に大きい値とすればよい。
【0052】
角度指令生成部27は、遡り開始指令が与えられると、角度指令記憶部28に記憶されている25個の回転角度指令のうち、位置P4よりも1つ前(位置P3寄り)の位置を示す指令から最も古いもの(位置P1=待機位置を示す指令)へと順に読み出す。角度指令生成部27は、読み出した24個の回転角度指令を、読み出した順に、つまり、記憶された時期が新しいものから古いものへと順に、回転角度指令pc(遡り角度指令)として出力周期毎に出力する。
【0053】
制御部26は、角度指令生成部27から与えられる回転角度指令pc(遡り角度指令)に基づいて、モータMの駆動をフィードバック制御する(遡り制御)。この結果、ロボット2の手先は、位置P4→位置P3→位置P2→位置P1(待機位置)という順に移動する。しかも、この際、各軸が通常動作(進み動作)時と同じ位置を通過しながら、言い換えると、ロボット2が通常動作時と同じ姿勢を保ちつつ移動する。さらに、1msの生成周期毎に生成した回転角度指令が8msの出力周期毎に遡り角度指令として出力されるため、遡り動作時のロボット2の移動速度は、通常動作時のロボット2の移動速度の約1/8となる。
【0054】
ロボット2の手先が位置P1(待機位置)に移動したことにより、制御部26に遡り終了指令が与えられる。これに伴い、制御部26は、角度指令生成部27に対し遡り終了指令を出力するとともに遡り制御の実行を終了する。また、角度指令生成部27は、制御部26から遡り終了指令が与えられたことにより、遡り指令の出力動作を停止する。
【0055】
以上説明したように、本実施形態によれば次のような効果が得られる。
ロボット2の遡り動作が行われる際、角度指令生成部27は、角度指令記憶部28に記憶されている回転角度指令を新しいものから古いものへと順に読み出すとともに、それら読み出した回転角度指令を遡り角度指令として出力周期毎に出力する。すなわち、進み動作時における各軸の回転角度のログを逆の順番に並べ替えたものが遡り角度指令となって出力される。制御部26は、各軸の回転角度を遡り角度指令に一致させるように、モータMによる各軸の駆動をフィードバック制御する遡り制御を実行する。これにより、ロボット2の全ての軸の回転角度が進み動作時と同じ値をとりつつ遡り動作が行われる。言い換えれば、ロボット2が、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。つまり、いわゆるプレイバック動作が行われるので、進み動作時と同じ姿勢を保ちつつ遡り動作が行われることになる。従って、ロボット2を停止位置から所定の待機位置まで戻す遡り動作が行われる際において、ロボット2の姿勢が進み動作と比較して変化することで生じる衝突のリスクを軽減することができる。
【0056】
このような本実施形態に対し、動作プログラムを停止位置への移動を命令する行から、所定の待機位置への移動を命令する行まで単純に遡って実行することにより、ロボット2を停止位置から所定の待機位置まで戻すという手法を採用することも考えられる。しかしながら、上記手法は以下の理由から採用することはできない。すなわち、動作プログラムには、条件毎に以降の動作が変化する分岐命令(例えば、図7の09行目)などが存在する。動作プログラムを単に逆に実行することを考えた場合、このような分岐部分において、どのように分岐させるべきかを決定することができない。従って、動作プログラムを逆に実行することができない可能性が高い。さらに、ロボット2の通常動作(進み動作9においては、所定の動作点から次の動作点に移動する際にパス動作が行われる場合がある。本実施形態によれば、進み動作時における各軸の回転角度のログを逆の順番に並べ替えたものが遡り角度指令として出力されるため、パス動作に伴う各軸の回転角度についても正確に再現される。一方、単純に動作プログラムを逆に実行するだけでは、このようなパス動作に伴う各軸の回転角度を求めることが不可能である。このようなことから、動作プログラムを単に逆に実行するだけでは、本実施形態と同様の効果は得られない。
【0057】
また、遡り角度指令は、生成周期毎に生成される全ての回転角度指令を長い出力周期毎に出力したものである。つまり、本来は比較的短い生成周期毎の回転角度を指令する回転角度指令を比較的長い周期毎に出力するので、遡り動作時における各軸の回転速度、ひいてはロボット2の動作速度が通常動作時に比べて遅くなる。このように、遡り動作時には、通常動作時と比べてロボット2の動作が遅くなる(スロー動作になる)ので、仮にロボット2周辺の他の設備がユーザの予期せぬような動きを行った場合であっても、ユーザはその動きに対して余裕を持って対処することができる。このため、遡り動作が行われる際に初めて生じる衝突のリスクについても軽減することができる。
【0058】
さて、ロボット2が例えば周辺設備に衝突したことで停止した場合、停止時点の各軸の回転角度が回転角度指令とは、かけ離れた値となっていることが多い。このため、正常に遡り制御を実行することは難しい。また、仮に遡り制御がうまく実行できたとしても、それは、周辺の設備に衝突したときの軌跡をロボット2が再び通ることになる。つまり、ロボット2が周辺の設備に再度衝突してしまう可能性が非常に高くなる。従って、衝突による停止時には、遡り制御を実行することなく、ロボット2を含む各設備の動作を完全に停止させた上で、衝突が生じた原因を取り除くことが望ましい。そこで、本実施形態の制御部26は、遡り開始指令が与えられると、以下の遡り可否判断処理を実行する。
【0059】
すなわち、制御部26は、角度指令記憶部28に記憶された最新の回転角度指令と現在のモータMの回転角度との差分を各軸について求め、それら差分の絶対値のうち最も大きい値が所定の誤差値以下である場合には、遡り制御を実行する。一方、差分の絶対値のうち最も大きい値が所定の誤差値以下である場合には遡り制御を実行しない。ロボット2が正常に動作している場合でも、回転角度と回転角度指令とは、種々の誤差の影響によって完全に一致しない場合がある。これに対し、ロボット2が衝突により停止した場合には、回転角度と回転角度指令とは大きくかけ離れた値となる。このことから、制御部26は、回転角度と回転角度指令との差分の絶対値が所定の誤差値を超えた場合には、ロボット2が衝突により停止した状態であると判断し、遡り制御を実行しないようにしている。これにより、遡り動作が行われることによってロボット2が再衝突するリスクを抑制することができる。
【0060】
また、角度指令生成部27は、遡り開始指令が与えられると、角度指令記憶部28に記憶されている回転角度指令を記憶された時期が2番目に新しいものから最も古いものへと順に読み出す。角度指令記憶部28に記憶された回転角度指令のうち最新のものは、ロボット2が停止した時点の現在位置を指令するためのものである。このため、最新のものから順に遡り角度指令とした場合には、実際にロボット2が遡り動作を開始するのは、最新のものよりも1つ前の遡り角度指令が出力された時点となる。つまり、遡り動作の開始が若干遅れることになる。これに対し、本実施形態の構成によれば、そのような若干の遅れもなく、素早く遡り動作を開始することができ、以って、遡り動作全体に要する時間を短縮することができる。
【0061】
(第2の実施形態)
以下、本発明の第2の実施形態について図9〜図14を参照しながら説明する。
図9および図10は、それぞれ第1の実施形態における図1および図6相当図であり、第1の実施形態と同一部分には同一符号を付して説明を省略する。図9に示す本実施形態のロボットシステム61は、図1に示した第1の実施形態のロボットシステム1に対し、ロボット2に代えてロボット62を備えている点が異なる。
【0062】
ロボット62は、例えば6軸の垂直多関節型ロボットとして構成されている。ロボット62は、ベース63と、このベース63に水平方向に回転可能に支持されたショルダ部64と、このショルダ部64に上下方向に回転可能に支持された下アーム65と、この下アーム65に上下方向に回転可能に支持された第1の上アーム66と、この第1の上アーム66に捻り回転可能に支持された第2の上アーム67と、この第2の上アーム67に上下方向に回転可能に支持された手首68と、この手首68に捻り回転可能に支持されたフランジ69とから構成されている。
【0063】
ベース63、ショルダ部64、下アーム65、第1の上アーム66、第2の上アーム67、手首68およびフランジ69は、ロボット62のアームとして機能し、アーム先端であるフランジ69には、図示はしないが、エンドエフェクタ(手先)が取り付けられる。ロボット62に設けられる複数の軸は、第1の実施形態のロボット2と同様、それぞれに対応して設けられるモータ(図2に符号Mを付して示す)により駆動される。また、各モータの近傍には、それぞれの回転軸の回転位置を検出するための位置検出器(図示せず)が設けられている。
【0064】
図示はしないが、本実施形態の角度指令記憶部28は、通常動作が行われる期間において、生成周期(例えば1ms)毎に電流i*の値(または電流指令icの値)を順次記憶するようになっている。すなわち、本実施形態の角度指令記憶部28には、生成周期毎の電流値のログデータが記憶される。
【0065】
図10は、ロボットシステム61におけるモータ制御の内容を等価的に示したブロック図である。図10に示すように、本実施形態の制御部71は、第1の実施形態の制御部26が備える構成に加え、さらに、トルク値推定部72、発生トルク値演算部73および外乱トルク値演算部74を備えている。
【0066】
トルク値推定部72(トルク値推定手段に相当)は、ロボット62が所定の動作プログラムに基づいて行う動作の開始から終了までの間にモータMに発生させる必要があるトルクの値(推定トルク値)を、回転角度指令pcを用いて一括して推定する。この推定方法としては、例えば運動方程式を用いて、ロボット62の軸を所定の回転位置からその次の回転位置まで回転させるためにモータMに発生させる必要があるトルク値を推定するという方法が挙げられる。なお、トルク値推定部72は、ロボット62が現在の指令位置からその次の指令位置まで移動するためにモータMに発生させる必要があるトルク値を逐次推定するように構成してもよい。発生トルク値演算部73(トルク値演算手段に相当)は、モータMに流れる現在の電流i*に対し、モータMのトルク定数を乗算することにより、モータMが発生する現在のトルクの値(発生トルク値)を求める。
【0067】
外乱トルク値演算部74(外乱演算手段に相当)は、推定トルク値および発生トルク値の差分から、モータMが駆動する対象となる軸に外部から加わるトルク値(外乱負荷トルク値)を求める。すなわち、外部からロボット62に対して負荷が加えられない限り、ロボット62の各軸には、回転角度指令pcに従いモータMが発生するトルクがそれぞれ生じることになるので、推定トルク値と発生トルク値とは一致する。これに対し、衝突が発生したことなどにより外部からロボット62に対して所定の負荷が加えられると、推定トルク値と発生トルク値との間には、その負荷に応じた分だけの差が生じる。そこで、外乱トルク値演算部74は、推定トルク値Teおよび発生トルク値Tgを用いて下記(1)式に基づき外乱負荷トルク値Tdを演算する。
Td=|Tg−Te| …(1)
【0068】
制御部71(衝突判断手段に相当)は、各軸に加わる外乱負荷トルク値Tdに基づいてロボット62に対する衝突を検出する。すなわち、制御部71は、所定の衝突検出周期毎に各軸に加わる外乱負荷トルク値Tdを求め、少なくともいずれか一つの軸に加わる外乱負荷トルク値Tdが予め定めたしきい値Tthを超えると衝突が発生したと判断する。しきい値Tthは、予めユーザによる設定が可能となっており、例えば1〜500[kgf・cm]の範囲の値(初期値は100[kgf・cm])としている。なお、しきい値Tthの設定値を小さくすると衝突検出の感度を高められるが、種々の演算誤差などに起因する誤検出の可能性も高まる。一方、しきい値Tthの設定値を大きくすると衝突検出の感度は低くなるが、誤検出の可能性が低減する。しきい値Tthは、このようなトレードオフの関係を踏まえた上で適用するシステムとして最適な値に設定すればよい。
【0069】
このように、本実施形態のロボットシステム61では、ロボット62が周辺機器と衝突した際、センサレスで瞬時に衝突を検出することを可能としている。すなわち、本実施形態では、制御部71が衝突検出手段として機能する。また、制御部71は、ロボット62の衝突を検出すると、ロボット62の各アームの動作を直ちに停止させる停止制御を行うようになっている。制御部71は、その停止制御において、ロボット62の各軸を駆動するモータMに逆転トルクを発生させつつ、その後の移動軌跡に沿ってアームの動作を停止させる。
【0070】
また、制御部71は、ロボット62の衝突を検出すると、衝突時の各軸の回転角度に基づいて衝突位置を特定する。図11は、衝突前後の所定の軸における回転角度指令pc、回転角度p*および電流i*を示している。図11における縦向きの実線は、動作指令出力ポイント(サンプリングタイミング)を示している。その動作指令出力ポイントの間隔(ロボット動作指令間隔)は、本実施形態では出力周期(例えば8ms)と同じになっている。また、図11における縦向きの点線は、上記した衝突検出動作の実行タイミングを示している。その実行タイミングの間隔(衝突検出周期)は、出力周期よりも短くなっており、本実施形態では生成周期(例えば1ms)と同じになっている。
【0071】
時刻t1の時点において、ロボット62に対する衝突が生じた場合、その後も回転角度指令pcは、次の動作指令出力ポイントの値(目標値)に向けて増加し続ける。一方、検出される実際の回転角度p*(エンコーダ値)は、衝突が生じているため、その値は変化せずほぼ一定を保っている。このため、回転角度指令pcに対する現在の回転角度p*の偏差(位置偏差)が急激に増加し、それに伴って電流i*も急激に増加する。その後、時刻t2の時点において、前述した衝突検出動作により衝突が検出されると、回転角度指令pcを回転角度p*に合わせるようにしてロボット62が停止制御される。この際、電流i*もゼロに向けて低下し、ロボット62が完全に停止した時点(時刻t3)でゼロになる。
【0072】
制御部71は、衝突を検出してロボット62を停止させた後、角度指令記憶部28に記憶された電流値のログデータを、停止させた時点の値から遡り、その値が急激に増加し始めたポイントを探す。図11の場合、時刻t1の時点において電流i*が急増し始めることになる。このため、制御部71は、その時刻t1を衝突が生じたタイミング(衝突タイミング)として特定するとともに、時刻t1における各軸の回転角度p*に基づいて衝突位置を特定する。この衝突位置の特定方法としては、例えば、加わった外乱負荷トルク値Tdが最も大きい軸に対して衝突が発生したと判断して衝突位置を特定する方法が挙げられる。その場合、各軸の外乱負荷トルク値Tdから、ヤコビ行列を用いてロボット62の手先に加わる外乱負荷トルク値を求め、手先への衝突も特定することが可能である。
【0073】
また、本実施形態では、制御部71は、衝突検出後においても遡り制御を実行することが可能となっている。制御部71は、衝突を検出すると、衝突後の遡り制御で用いるための進入禁止空間を設定する。進入禁止空間は球状をなしており、その中心は、特定した衝突位置(時刻t1の位置)に設定される。そして、例えば、特定した衝突位置がロボット62の手先である場合、進入禁止空間の半径は、衝突タイミングの前後における動作指令出力ポイントのうち、衝突タイミングに近い方のタイミング、つまり、衝突タイミングの直前の動作指令出力ポイント(時刻ta)時点の手先の位置と、衝突時点の手先の位置(衝突位置)との間の距離に相当する長さに設定される。なお、時刻ta時点の手先の位置は、時刻taの時点において出力された各軸の回転角度指令に基づいて求められる。
【0074】
次に、本実施形態の作用および効果について図12〜図14も参照して説明する。
さて、ロボット62の衝突としては、自動運転中の衝突や、教示中または動作確認中の衝突などが考えられる。前者の場合、ロボット62は最高速度で動作している可能性が高く、衝突速度は比較的速い。このため、ロボットの機構部品や周辺設備が多大な損傷を受ける可能性が高く、その後の自動復旧が困難な状態となる。このような場合には、衝突により停止した時点の位置偏差が過大になることが多いため、遡り制御の実行自体が困難となる。この場合には、衝突検出以前に、偏差過大、過電流、加速度限界値オーバーなどのエラーが発生してロボット62が緊急停止されるため、本実施形態のロボットシステム61においても遡り制御は実行されない。
【0075】
一方、後者の場合、ロボット62は低速で動作しているため、その衝突速度は比較的遅い。このため、ロボットの機構部品や周辺設備がほとんど損傷することなく、その後の自動復旧が可能な状態となる。このような場合には、衝突により停止した時点の位置偏差が比較的小さいことが多いため、遡り制御を実行することが可能となる。また、上記各衝突のうち、衝突速度の速い前者の衝突よりも衝突速度の遅い後者の衝突のほうが発生頻度は高い。なぜなら、自動運転中、ロボット62は動作プログラムに基づいて定められた動作を繰り返し行う。そして、その繰り返される動作は、予め周辺設備などに衝突することがないように十分に検証されたものである。このため、自動運転中に衝突する可能性は極めて低い。
【0076】
これに対し、教示中や動作確認中においては、周辺設備との衝突検証を兼ねて教示や動作確認を行うようになっており、その最中に動作軌跡の修正が頻繁に行われることも多い。このため、教示中や動作確認中には、ロボット62の動作の再現性は低い。従って、新たな動作軌跡を生成した後や、動作軌跡を修正した後に行われる教示や確認動作において、周辺設備などへの衝突が生じる可能性は高くなる。このように、衝突速度が速く遡り制御を実行することができない重度の衝突よりも、衝突速度が遅く遡り制御を実行可能な軽微な衝突のほうが発生頻度は高い。このようなことから、軽微な衝突によりロボット62が停止した場合にも、遡り制御を実行してロボットを待機位置まで移動させることが望ましい。
【0077】
ただし、軽微な衝突とは言っても、むやみにその衝突回数を増やすことは、精密動作が要求されるロボット62にとっては好ましくない。なぜなら、軽微な衝突であっても、その衝突が繰り返されることにより、ロボット62を精密に動作させるために行われるキャリブレーションやフレームの精度に狂いが生じる危険性があるからである。そこで、本実施形態では、軽微な衝突によりロボット62が停止した場合、以下のように再衝突の発生を防止する工夫を加えた上で、遡り制御を実行するようにしている。
【0078】
図12は、例えば教示中または動作確認中における衝突前後のロボットの動作状態を示している。まず、図12の(1)に示すように、ロボット62の手先は、設備81から少し離れた位置から設備81の方向に向けて移動している。そして、図12の(2)に示すように、ロボット62の手先が設備81に衝突する。この際、制御部71は、その衝突を検出し、ロボット62の停止制御を開始する。その後、図12の(3)に示すように、ロボット62の手先は、衝突の反動で設備81から僅かに離れた位置まで移動しながら上記停止制御により停止する。
【0079】
制御部71は、このように衝突発生に伴ってロボット62が停止した状態において、遡り開始指令が入力されると、遡り制御を実行する。ただし、衝突によりロボット62が停止された後に遡り制御を実行する際、第1の実施形態の遡り制御と同じように角度指令生成部27が出力する回転角度指令pc(遡り角度指令)に基づいてロボット62を遡り動作させると、その動作の軌跡は、必ず衝突位置を通過することになる。従って、ロボット62が再度衝突する可能性が高くなってしまう。
【0080】
そこで、本実施形態の角度指令生成部27は、衝突が検出された状態で遡り開始指令が入力されると、角度指令記憶部28から読み出した回転角度指令pcに以下のような変更を加える。すなわち、本実施形態の角度指令生成部27は、衝突が検出された状態で遡り指令が入力されると、角度指令記憶部28から読み出した回転角度指令pcに基づくロボット62の遡り動作の軌跡が、制御部71が設定した進入禁止空間の球の外郭に最初に重なる外郭位置と最後に重なる外郭位置との間では、その動作軌跡が進入禁止空間の外であり、且つ、進入禁止空間の球の表面に沿う軌跡となるように、読み出した回転角度指令pcを変更し、その変更後の回転角度指令pcを遡り角度指令として出力する。要するに、本実施形態では、進入禁止空間の内部を通過する動作軌跡となる回転角度指令pcを遡り角度指令として使用することなく、その代わりに、進入禁止空間の球の表面を這うような動作軌跡となる回転角度指令pcを新たに生成して遡り角度指令として使用する。
【0081】
このように角度指令生成部27による回転角度指令pcの変更に伴って動作軌跡が変更された状態で、制御部71による遡り制御が実行されると、ロボット62の手先は、図12の(3)に示した停止位置から、衝突位置を経由し、図12の(1)に示した位置に移動することになる。ただし、遡り制御において衝突位置を経由する際には、図13に示すように、ロボット62の手先が衝突位置を回避するように遡り動作が行われる。以下、遡り動作時の衝突位置の回避動作について、図14を参照して詳しく説明する。
【0082】
図14は、衝突位置付近における進み動作時の手先の動作軌跡と、衝突後の遡り動作時の手先の動作軌跡とを示している。図14では、進み動作時の動作軌跡は実線で示し、遡り動作時の動作軌跡は一点鎖線で示している。また、衝突位置は符号aを付した点で示し、設定した進入禁止空間は破線で表すとともに符号bを付している。さらに、進み動作時の動作軌跡が、最初に進入禁止空間bに重なる位置(脱出外郭位置)は符号x1を付した点で示し、最後に進入禁止空間bに重なる位置(進入外郭位置)は符号x2を付した点で示している。なお、図14に示す時刻t1、ta、tbは、いずれも図11に同じ符号を付して示した各時刻に対応している。
【0083】
図14に示すように、衝突により停止した後、ロボット62の手先は、その停止位置から進入外郭位置x2までの区間では、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。進入外郭位置x2から脱出外郭位置x1までの区間では、進入禁止空間bの球の表面(外郭)に沿うようにして衝突位置aを回避する。そして、脱出外郭位置x1以降の区間では、再び進み動作の動作軌跡を逆向きに辿るように遡り動作が行われることになる。なお、図14では、説明の都合上、停止位置から進入外郭位置x2までの区間および脱出外郭位置x1以降の区間において、進み動作時および遡り動作時の各動作軌跡を互いに若干ずらして描いているが、実際には各動作軌跡は一致している。
【0084】
進入禁止空間bの半径は、前述したように設定されていることから、非常に小さいものであると言える。このため、進入禁止空間bの外郭を沿う遡り動作を行う際、ロボット62の肘などの手先とは別の部分が周辺設備などに衝突するといった新たな衝突が引き起こされる可能性が高まるほど、つまり、新たな別の衝突が発生するようなレベルまでロボット62の姿勢が変化することはない。すなわち、進入禁止空間bの外郭を沿う遡り動作を行う際におけるロボット62の姿勢は、衝突が発生した進み動作時の姿勢とほとんど同じであり、新たな別の衝突を引き起こすような姿勢の変化はない。
【0085】
このように衝突後の遡り動作では衝突位置aを回避するため、完全なプレイバック動作とはならない。しかし、上記したとおり進入禁止空間bは小さい。また、元々、ロボット62が通過する経路(動作軌跡)は、ロボット62の体格を考慮した上で周辺設備などに衝突しないように所定のマージンが設けられている。そして、ロボット62の体格は、進入禁止空間bよりも遥かに大きい。このため、進入禁止空間bの外郭を沿うように動作軌跡が変更された程度では、ロボット62が上記マージンの範囲を越えて周辺設備に衝突する可能性は極めて低い。つまり、遡り制御における若干の動作軌跡の変更により、ロボット62が新たな衝突を生じることはほとんどあり得ない。従って、衝突後にロボット62が遡り動作する際、その衝突位置aを確実に避けるとともに新たな衝突の発生を未然に防止しつつ、進み動作時と同じ姿勢を保ちながら遡り動作することが可能となる。
【0086】
(その他の実施形態)
なお、本発明は上記し且つ図面に記載した各実施形態に限定されるものではなく、次のような変形または拡張が可能である。
角度指令生成部27は、遡り開始指令が入力されたとき、角度指令記憶部28に記憶されている回転角度指令を記憶された時期が3番目以降に新しいものから古いものへと順に読み出してもよい。また、角度指令生成部27は、遡り開始指令が入力されたとき、角度指令記憶部28に記憶されている回転角度指令を記憶された時期が最も新しいものから最も古いものへと順に読み出してもよい。
生成周期および出力周期は、遡り動作時のロボット2の動作速度を必要な速度に設定するように適宜変更可能である。すなわち、角度指令生成部27が出力する回転角度指令および出力しない(間引く)回転角度指令の割合(1:x)は、「x>1」という条件を満たす範囲内で適宜変更可能である。
遡り可否判断処理において、制御部26は、角度指令記憶部28に記憶された最新の回転角度指令と現在のモータMの回転角度との差分を、所定の軸について求めるように変更してもよい。また、制御部26は、ロボット2が衝突により停止した際に遡り開始指令が与えられないという条件であれば、遡り可否判断処理を行わなくてもよい。
【0087】
遡り制御の実行を終了する待機位置としては、角度指令記憶部28に記憶された最初の回転角度指令が表す位置に限らずともよく、任意の位置でよい。この任意の位置として、例えば、動作プログラムにおけるmove命令(所定の作業をするために手先を動かすなどの命令)があるところの位置(またはその前後の位置)にするとよい。move命令の位置およびその前後の位置では、手先に対しワークを掴むなどの指令が出されている可能性が高い。ロボット2がワークを掴んだまま、遡り動作が継続されると、そのワークが周辺設備(例えば狭い通路など)に衝突してしまう可能性がある。そこで、move命令の前後で遡り動作を終了させてロボット2を待機させることで、上記衝突が発生する可能性を低減することができる。
【0088】
第2の実施形態において、制御部71が設定する進入禁止空間は、衝突発生後にロボット62を遡り動作させる際に、ロボット62が最初に衝突した位置に再衝突することを回避するとともに、新たな別の衝突の発生を防止できるような空間であれば適宜変更可能である。例えば、特定した衝突位置がロボット62の手先である場合、進入禁止空間の半径は、衝突タイミングの前後における動作指令出力ポイントのうち、衝突タイミングから遠い方のタイミング、つまり、衝突タイミングの直後の動作指令出力ポイント(図11、図14の時刻tb)時点の手先の位置と、衝突時点の手先の位置(衝突位置)との間の距離に相当する長さに設定してもよい。なお、時刻tb時点の手先の位置は、時刻tbの時点において出力された各軸の回転角度指令に基づいて求められる。このように設定した進入禁止空間の半径は、第2の実施形態で設定したものと比べて若干大きくなる。このため、例えば、ロボット62が衝突した設備の表面が、その衝突によって多少変形した場合でも、その変形部分をも避けて遡り動作を行うことが可能になり、設備の変形に伴う衝突を未然に防止できるという効果が得られる。
【0089】
また、ロボット62の精度誤差(例えば、±0.075mm)に基づいて進入禁止空間の半径を設定してもよい。その場合、安全サイドへのマージンをとるため、精度誤差をN倍(例えば、N=2)したものを、進入禁止空間の半径に設定するとよい。または、衝突時の速度(衝突速度)に基づいて進入禁止空間の半径を設定してもよい。その場合、安全サイドへのマージンをとるため、衝突検出周期(例えば1ms)の間に衝突速度で移動する距離をN倍(例えば、N=2)したものを、進入禁止空間の半径に設定するとよい。あるいは、ロボット62の精度誤差に基づいて設定した半径と、衝突速度に基づいて設定した半径とのうち、大きい方を進入禁止空間の半径に設定するようにしてもよい。
【0090】
上記衝突速度は、例えば衝突時の各軸の回転速度に基づいて求めればよい。すなわち、制御部71は、衝突を検出した後、衝突タイミングの直前の動作指令出力ポイント(図11の時刻ta)と、その一つ前の動作指令出力ポイント(図11の時刻tc)との間の移動量、つまり、直前の回転角度指令pcと、その一つ前の回転角度指令pcとの差から、衝突直前の出力周期における移動距離(移動量)を求める。そして、制御部71は、出力周期の値(例えば8ms)と求めた移動距離とから、衝突直前の出力周期における回転速度を算出し、その値を衝突速度として取得すればよい。このようにすれば、衝突が生じた直前の時点の速度、ひいては衝突時点の速度を正確に求めることが可能となる。
【0091】
衝突検出手段としては、第2の実施形態で説明した構成に限らずともよく、ロボットに対する衝突を検出可能なものであれば適用することができる。例えば、他のセンサレス方式の衝突検出手段や、加速度センサなどのセンサを用いる衝突検出手段であってもよい。
第1の実施形態では、本発明を4軸の水平多関節型のロボット2に適用した例を説明し、第2の実施形態では、本発明を6軸の垂直多関節型のロボット62に適用した例を説明したが、本発明は、複数の軸を有する多軸型のロボット全般に適用可能である。
【符号の説明】
【0092】
図面中、1、61はロボットシステム、2、62はロボット、25は回転角度検出部、26は制御部(遡り指令入力部)、27は角度指令生成部、28は角度指令記憶部、71は制御部(衝突検出手段)、Mはモータ(駆動部)を示す。
【技術分野】
【0001】
本発明は、ロボット自体に異常がなくロボットを停止させた場合に、ロボットをその停止位置から所定の待機位置まで戻す動作を行う機能を備えたロボットシステムに関する。
【背景技術】
【0002】
例えば製造ラインなどにおいてロボットが用いられる場合、ロボットに異常がなくとも、ロボットと協調して動作する他の設備の異常(コンベアの停止、ワークとなる部品が流れてこない等)が原因でロボットの動作を停止せざるを得ない場合がある。このような場合、以下の理由からロボットを停止位置から所定の待機位置まで戻さなければならない。すなわち、他の設備をリセットした(所定の待機位置に戻した)にもかかわらず、ロボットだけが停止した位置まで進んだ状態になっていると、他の設備とロボットとの動作整合が取れなくなってしまう。
【0003】
現在、ロボットを停止位置から所定の待機位置まで戻す方法としては、ユーザの手動操作により移動させる方法、または、停止位置から待機位置へ戻すための動作プログラムを準備しておき、ロボット停止時にその停止位置に対応する動作プログラムを選択して起動させてロボットを移動させる方法などが用いられている。
【0004】
しかし、手動操作による方法では、ロボットの周辺に配置された設備等の物体に衝突することなく待機位置へ移動させるために正確な操作が要求される。このため、ユーザの作業負担が増大する。また、待機位置へ戻すための動作プログラムを用いる方法では、想定されるロボットの停止位置の数だけの動作プログラムを予め用意しておく必要がある。さらに、現実には想定外の位置でロボットが停止する場合も考えられ、この場合には、予め用意した動作プログラムでは、ロボットを待機位置へ戻すことができない可能性もある。
一方、特許文献1には、ロボットが異物と衝突したと判断した場合に、自動的に手先を異物と衝突しない位置まで戻す技術が開示されている。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特開平5−84681号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
設備の異常に伴ってロボットが停止した場合に、上記特許文献1記載の技術を用いれば、自動的にロボットの手先位置を所定の待機位置(待避位置)まで戻すことが可能となる。しかし、特許文献1記載の技術を用いただけでは、戻り動作時において衝突が生じる危険性がある。なぜなら、ロボットには姿勢という概念があり、単に手先位置が待機位置まで戻ればよいというわけではない。つまり、進み方向に所定動作を行った後に停止され、その後、単純に戻り方向に上記所定動作を行うと、進み方向に所定動作を行ったときとはロボットの姿勢が異なる可能性がある。例えば、進み方向に所定動作を行った際には、周辺の設備から離れるように肘の部分が折り畳まれた姿勢であったにもかかわらず、戻り方向に所定動作を行う際には、周辺の設備に接近するように肘の部分が張り出した姿勢となる場合も考えられる。このように姿勢が異なる場合、戻り方向に所定動作を行うときに、ロボットの肘の部分が周辺の設備に衝突する危険性(リスク)がある。
【0007】
本発明は上記事情に鑑みてなされたものであり、その目的は、ロボット自体に異常がない状態でロボットを停止させた場合に、ロボットをその停止位置から所定の待機位置まで戻す動作を行う際における衝突のリスクを軽減することができるロボットシステムを提供することにある。
【課題を解決するための手段】
【0008】
請求項1記載の手段によれば、通常の動作(進み動作)を行う場合、制御部は、ロボットに設けられた複数の軸の回転角度を角度指令生成部から出力される回転角度指令に一致させるように、駆動部による各軸の駆動をフィードバック制御する通常制御を実行する。ここで、複数の軸の回転角度は、回転角度検出部によって検出される。また、角度指令生成部は、回転角度指令を生成周期毎に生成し、その回転角度指令の一部を生成周期よりも長い出力周期毎に出力する。そして、角度指令記憶部は、上記通常制御が実行されている期間に角度指令生成部により生成される回転角度指令の全てを順次記憶する。
【0009】
この理由は以下のとおりである。すなわち、現在において、回転角度指令は、通常、ソフトウエアによる処理(電気的な動作のみに依存する処理)によって生成されることが多いため、極めて短い間隔である生成周期毎に生成することができる。一方、駆動部、回転角度検出部などは、ハードウエアによる処理(機械的な動作が含まれる処理)によって、ロボットの軸を駆動したり、軸の回転角度を検出したりするため、回転角度を回転角度指令に一致させるための動作間隔は、上記生成周期に比べて長い間隔にならざるを得ない。このため、回転角度指令の生成周期を上記動作間隔に合わせて長くするか、あるいは短い生成周期で生成した回転角度指令を上記動作間隔に合わせて間引いて出力するかのいずれかを採用する必要がある。本手段では、後述する遡り制御において短い生成周期で生成した回転角度指令を用いる必要があるため、後者の方法を採用している。
【0010】
さて、従来技術において説明したように、ロボットと協調して動作する他の設備に異常が生じた場合には、ロボットに異常がなくても、その通常制御の実行が停止されるとともにロボットを停止するための動作がなされる。このようにロボットが停止された場合には、その停止位置からロボットを所定の待機位置まで戻す動作(遡り動作)を行う必要がある。このような場合、例えば、ユーザによる所定の操作や、ロボットが上記したように停止すると自動的に起動するプログラムの動作などにより、遡り指令入力部を介して遡り開始指令が入力される。すると、角度指令生成部は、角度指令記憶部に記憶された回転角度指令を新しいものから古いものへと順に読み出すとともに、それら読み出した回転角度指令を遡り角度指令として出力周期毎に出力する。すなわち、進み動作時における全軸の回転角度のログを逆の順番に並べ替えたものが遡り角度指令となって出力される。
【0011】
制御部は、各軸の回転角度を上記遡り角度指令に一致させるように、駆動部による各軸の駆動をフィードバック制御する遡り制御を実行する。これにより、ロボットの全ての軸の回転角度が進み動作時と同じ値をとりつつ遡り動作が行われる。言い換えれば、ロボットが、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。つまり、いわゆるプレイバック動作が行われるので、進み動作時と同じ姿勢を保ちつつ遡り動作が行われることになる。このような遡り動作により、ロボットが所定の待機位置まで戻ることで、遡り指令入力部を介して遡り終了指令が入力される。すると、角度指令生成部は、遡り角度指令の出力を停止し、制御部は、遡り制御の実行を停止する。このように、本手段によれば、ロボットを停止位置から所定の待機位置まで戻す遡り動作が行われる際に、ロボットの姿勢が変化することで生じる衝突のリスクを軽減することができる。
【0012】
また、遡り動作の制御に用いる遡り角度指令は、生成周期毎に生成される全ての回転角度指令を出力周期毎に出力したものである。つまり、本来は比較的短い生成周期毎の回転角度を指令する回転角度指令を比較的長い周期毎に出力するので、遡り動作時における各軸の回転速度、ひいてはロボットの動作速度が通常動作時に比べて遅くなる。このように、遡り動作時には、通常動作時と比べてロボットの動作が遅くなる(スロー動作になる)ので、仮にロボット周辺の他の設備がユーザの予期せぬような動きを行った場合であっても、ユーザはその動きに対して余裕を持って対処することができる。このため、遡り動作が行われる際に初めて生じる衝突のリスクについても軽減することができる。
【0013】
さて、ロボットが例えば周辺設備に衝突したことで停止した場合、請求項1記載の手段における遡り制御を実行しようとすると以下の問題が生じる。すなわち、衝突により停止した場合、停止時点の各軸の回転角度が回転角度指令(遡り角度指令)とは、かけ離れた値となっていることが多い。このため、正常に遡り制御を実行することは難しい。また、仮に遡り制御がうまく実行できたとしても、それは、周辺の設備に衝突したときの軌跡をロボットが再び通ることになる。つまり、ロボットが周辺の設備に再度衝突してしまう可能性が非常に高くなる。従って、衝突による停止時には、上記遡り制御を実行することなく、ロボットを含む各設備の動作を完全に停止させた上で、衝突が生じた原因を取り除くことが望ましい。そこで、請求項2記載の手段を採用することが考えられる。
【0014】
請求項2記載の手段によれば、制御部は、遡り開始指令が入力されると、角度指令記憶部に記憶されている最新の回転角度指令と回転角度検出部により検出された現在の回転角度との差分を求める。そして、この差分の絶対値が所定の誤差値以下である場合には遡り制御を実行する。一方、上記差分の絶対値が所定の誤差値を超える場合には遡り制御は実行しない。ロボットが正常に動作している場合であっても、回転角度と回転角度指令とは、種々の誤差の影響によって完全に一致しない場合がある。これに対し、ロボットが衝突により停止した場合には、上記したとおり、回転角度と回転角度指令とは大きくかけ離れた値となる。従って、制御部は、回転角度と回転角度指令との差分の絶対値が所定の誤差値を超えた場合には、ロボットが衝突により停止した状態であると判断し、遡り制御を実行しないようにしている。これにより、遡り動作が行われることによってロボットが再衝突するリスクを抑制することができる。
【0015】
請求項3記載の手段によれば、ロボットの衝突を検出する衝突検出手段を備えている。ロボットの衝突としては、自動運転中の衝突や、教示(ティーチング)中または動作確認中の衝突などが考えられる。前者の場合、ロボットは最高速度で動作している可能性が高く、衝突時の速度(衝突速度)は比較的速い。このため、ロボットの機構部品や周辺設備が多大な損傷を受ける可能性が高く、その後の自動復旧が困難な状態となる。このような場合には、衝突により停止した時点の各軸の回転角度と回転角度指令との偏差が過大になることが多いため、遡り制御の実行自体が困難となる。
【0016】
一方、後者の場合、ロボットは低速で動作しているため、その衝突速度は比較的遅い。このため、ロボットの機構部品や周辺設備がほとんど損傷することなく、その後の自動復旧が可能な状態となる。このような場合には、衝突により停止した時点の各軸の回転角度と回転角度指令との偏差が比較的小さいことが多いため、遡り制御を実行することが可能となる。
【0017】
また、上記した2通りの衝突のうち、衝突速度の速い前者の衝突よりも衝突速度の遅い後者の衝突のほうが発生頻度は高い。なぜなら、自動運転中、ロボットは動作プログラムに基づいて定められた動作を繰り返し行う。そして、その繰り返される動作は、予め周辺設備などに衝突することがないように十分に検証されたものである。このため、自動運転中に衝突する可能性は極めて低い。これに対し、教示中や動作確認中においては、周辺設備との衝突検証を兼ねて教示や動作確認を行うようになっており、その最中に動作軌跡の修正が頻繁に行われることも多い。このため、教示中や動作確認中には、ロボットが毎回同じ動作を繰り返す可能性は低い(ロボットの動作の再現性は低い)。従って、新たな動作軌跡を生成した後や、動作軌跡を修正した後に行われる教示や確認動作において、周辺設備などへの衝突が生じる可能性は高くなる。このように、衝突速度が速く遡り制御を実行することができない重度の衝突よりも、衝突速度が遅く遡り制御を実行可能な軽微な衝突のほうが発生頻度は高い。このようなことから、軽微な衝突によりロボットが停止した場合にも、遡り制御を実行してロボットを待機位置まで移動させることが望ましい。
【0018】
ただし、軽微な衝突とは言っても、むやみにその衝突回数を増やすことは、精密動作が要求されるロボットにとっては好ましくない。なぜなら、軽微な衝突であっても、その衝突が繰り返されることにより、ロボットを精密に動作させるために行われるキャリブレーションやフレームの精度に狂いが生じる危険性があるからである。
【0019】
そこで、本手段では、軽微な衝突によりロボットが停止した場合、以下のように再衝突の発生を防止する工夫を加えた上で、遡り制御を実行するようにしている。すなわち、制御部は、衝突検出手段により衝突が検出されると、その衝突が発生した時点(衝突タイミング)と、その衝突時の位置(衝突位置)を特定する。制御部は、これら特定された衝突位置および衝突タイミングを用いて、球状をなす進入禁止空間を設定する。すなわち、進入禁止空間の中心は、衝突位置に設定する。また、進入禁止空間の半径は、衝突タイミングの前後における回転角度指令の出力タイミングのうち、衝突タイミングに近い方の出力タイミングに出力された回転角度指令と、衝突位置との間の距離に相当する長さに設定する。そして、制御部は、衝突が検出された状態で遡り開始指令が入力されると、遡り制御を実行する。
【0020】
さて、衝突によりロボットが停止された後に遡り制御を実行する際、角度指令生成部が出力する遡り角度指令に基づいてロボットを遡り動作させると、その動作の軌跡は、必ず衝突位置を通過することになる。従って、ロボットが再度衝突する可能性が高くなってしまう。そこで、角度指令生成部は、衝突が検出された状態で遡り開始指令が入力されると、角度指令記憶部から読み出した回転角度指令に基づくロボットの遡り動作の軌跡が、上記進入禁止空間の外郭に最初に重なる外郭位置と最後に重なる外郭位置との間では、動作軌跡が進入禁止空間の外であり、且つ、進入禁止空間の球の表面に沿う軌跡となるように、読み出した回転角度指令を変更し、その変更後の回転角度指令を遡り角度指令として出力する。
【0021】
このようにすれば、衝突により停止した後、ロボットは、進入禁止空間の外郭に達するまでの間は、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。そして、進入禁止空間の外郭を沿うようにして衝突位置を回避した後、再び進み動作の動作軌跡を逆向きに辿るように遡り動作が行われる。この進入禁止空間の半径は、上記したようにして設定されていることから、非常に小さいものであると言える。このため、進入禁止空間の外郭を沿う動作を行う際、ロボットの肘などの別部分が周辺設備などに衝突するといった新たな衝突が引き起こされる可能性が高まるほど、つまり、新たな別の衝突が発生するようなレベルまでロボットの姿勢が変化することはない。すなわち、進入禁止空間の外郭を沿う遡り動作を行う際におけるロボットの姿勢は、衝突が発生した進み動作時の姿勢とほとんど同じであり、新たな別の衝突を引き起こすような姿勢の変化はない。
【0022】
このように衝突後の遡り動作では衝突位置を回避するため、完全なプレイバック動作とはならない。しかし、上記したとおり進入禁止空間は小さい。また、元々、ロボットが通過する経路(動作軌跡)は、ロボットの体格を考慮した上で周辺設備などに衝突しないように所定のマージンが設けられている。そして、ロボットの体格は、進入禁止空間よりも遥かに大きいものとなっている。このため、本手段のような進入禁止空間の外郭を沿うように動作軌跡が変更された程度では、ロボットが上記マージンの範囲を越えて周辺設備に衝突する可能性は極めて低い。つまり、上記した若干の動作軌跡の変更により、ロボットが新たな衝突を生じることはほとんどあり得ない。従って、衝突後にロボットが遡り動作する際、その衝突位置を確実に避けるとともに新たな衝突の発生を未然に防止しつつ、進み動作時と同じ姿勢を保ちながら遡り動作することが可能となる。
【0023】
請求項4記載の手段によれば、請求項3記載の手段と同様に、ロボットの衝突を検出する衝突検出手段を備えている。制御部は、衝突検出手段により衝突が検出されると、請求項3記載の手段と同様に、衝突タイミングおよび衝突位置を特定するとともに、進入禁止空間を設定する。そして、制御部は、衝突が検出された状態で遡り開始指令が入力されると、遡り制御を実行する。また、角度指令生成部は、衝突が検出された状態で遡り開始指令が入力されると、請求項3記載の手段と同様に、読み出した回転角度指令を変更し、その変更後の回転角度指令を遡り角度指令として出力する。このため、本手段によっても、請求項3記載の手段と同様の作用および効果が得られる。
【0024】
ただし、本手段における進入禁止空間は、請求項3記載の手段における進入禁止空間とは若干異なっている。すなわち、本手段の進入禁止空間の半径は、衝突タイミングの前後における回転角度指令の出力タイミングのうち、衝突タイミングから遠い方の出力タイミングに出力された回転角度指令と、衝突位置との間の距離に相当する長さに設定されている。このため、進入禁止空間の半径(大きさ)が請求項3記載の手段のものと比べて若干大きくなっている。このようにすれば、例えば、ロボットが衝突した設備の表面が、その衝突によって多少変形した場合でも、その変形部分をも避けて遡り動作を行うことが可能になり、設備の変形に伴う衝突を未然に防止できるという効果が得られる。
【0025】
請求項5記載の手段によれば、角度指令生成部は、遡り開始指令が入力されると、角度指令記憶部に記憶された回転角度指令のうち最新のものを除く回転角度指令を新しいものから古いものへと順に読み出す。角度指令記憶部に記憶された回転角度指令のうち最新のものは、ロボットが停止した時点の現在位置を指令するためのものである。このため、請求項1記載の手段のように、最新のものから順に遡り角度指令とした場合には、実際にロボットが遡り動作を開始するのは、最新のものよりも1つ前の遡り角度指令が出力された時点となる。つまり、遡り動作の開始が若干遅れることになる。これに対し、本手段によれば、そのような若干の遅れもなく、素早く遡り動作を開始することができ、以って、遡り動作全体に要する時間を短縮することができる。
【図面の簡単な説明】
【0026】
【図1】本発明の第1の実施形態を示すロボットシステムの構成を概略的に示す図
【図2】ロボットシステムの電気構成図
【図3】通常動作時における回転角度指令の生成および出力タイミングを示す図
【図4】角度指令記憶部に記憶される回転角度指令の一例を示す図
【図5】遡り動作時における回転角度指令の出力タイミングを示す図
【図6】モータ制御の内容を等価的に示すブロック図
【図7】所定の動作プログラムに従って動作するロボットの状態を示す図
【図8】動作プログラムの一例を示す図
【図9】本発明の第2の実施形態を示す図1相当図
【図10】図6相当図
【図11】衝突前後における回転角度指令、回転角度および電流値を示す図
【図12】衝突前後におけるロボットの動作状態を示す図
【図13】衝突位置を通過する際のロボットの手先およびその動作軌跡示す図
【図14】衝突位置付近での進み動作時および遡り動作時の各動作軌跡を示す図
【発明を実施するための形態】
【0027】
(第1の実施形態)
以下、本発明の第1の実施形態について図1〜図8を参照しながら説明する。
図1は、一般的な産業用ロボットのシステム構成を示している。図1に示すロボットシステム1は、ロボット2と、ロボット2を制御するコントローラ3と、コントローラ3に接続されたティーチングペンダント4とから構成されている。
【0028】
ロボット2は、例えば4軸の水平多関節型ロボットとして構成されている。ロボット2は、設置面に固定されるベース5と、ベース5上に垂直軸(第1軸)を中心に旋回可能に連結された第1のアーム6と、第1のアーム6の先端部上に垂直軸(第2軸)を中心に旋回可能に連結された第2のアーム7と、第2のアーム7の先端部に上下動可能で且つ回転可能に設けられたシャフト8とから構成されている。なお、このシャフト8を上下動させる際の軸が第3軸に相当し、回転させる際の軸が第4軸に相当する。シャフト8の先端部(下端部)には、フランジ9が位置決めされて着脱可能に取り付けられる。
【0029】
ベース5、第1のアーム6、第2のアーム7、シャフト8およびフランジ9は、ロボット2のアームとして機能し、アーム先端であるフランジ9には、図示はしないが、エンドエフェクタ(手先)が取り付けられる。ロボット2に設けられる複数の軸はそれぞれに対応して設けられるモータ(図2に符号Mを付して示す)により駆動される。各モータの近傍には、それぞれの回転軸の回転角度を検出するための位置検出器(図示せず)が設けられている。
【0030】
ティーチングペンダント4は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント4は、表面部の中央部に例えば液晶ディスプレイからなる表示部10を有している。表示部10には、各種の画面が表示される。表示部10は、タッチパネルで構成されている。またティーチングペンダント4には、表示部10の周囲に各種のキースイッチ11が設けられており、使用者は、キースイッチ11や上記タッチパネルに設けられるタッチスイッチによって種々の入力操作を行う。キースイッチ11または上記タッチスイッチには、後述する遡り動作の開始を指示するための遡り開始スイッチが含まれている。ティーチングペンダント4は、ケーブルを経由してコントローラ3に接続され、通信インターフェイスを経由してコントローラ3との間で高速のデータ転送を実行するようになっており、キースイッチ11等の操作により入力された操作信号等の情報はティーチングペンダント4からコントローラ3へ送信される。
【0031】
図2は、ロボットシステム1の電気構成を概略的に示すブロック図である。ロボット2には、各軸をそれぞれ駆動するための複数のモータM(図2では1つのみ示す)が設けられている。モータM(駆動部に相当)は、例えばブラシレスDCモータである。コントローラ3には、交流電源21より供給される交流を整流および平滑して出力する直流電源装置22、モータMを駆動するインバータ装置23、電流検出部24、回転角度検出部25、これら各装置の制御などを行う制御部26、角度指令生成部27および角度指令記憶部28が設けられている。
【0032】
直流電源装置22は、整流回路30と平滑用のコンデンサ31とから構成されている。整流回路30は、ダイオードをブリッジの形態に接続してなる周知構成のものである。例えば3相200Vの交流電源21の各相出力は、整流回路30の交流入力端子に接続されている。整流回路30の直流出力端子は、それぞれ直流電源線32、33に接続されている。これら直流電源線32、33間にはコンデンサ31が接続されている。
【0033】
インバータ装置23は、直流電源線32、33間に6つのスイッチング素子例えばIGBT(図1には2つのみ示す)を三相フルブリッジ接続して構成されたインバータ主回路と、その駆動回路とを6組備えている(図2には1組のみ示す)。IGBTのコレクタ・エミッタ間には還流ダイオードが接続されている。また、IGBTのゲートには、駆動回路からゲート信号が与えられている。駆動回路は、制御部26から与えられる指令信号(通電指令Sc)に基づいてパルス幅変調されたゲート信号を出力して各IGBTを駆動する。
【0034】
制御部26は、CPU、ROM、RAM、I/Oなどを備えたマイクロコンピュータを主体として構成されている。電流検出部24は、モータMに流れる電流を検出する電流検出器(図示せず)からの検出信号を制御部26に入力可能なデータに変換して出力する。回転角度検出部25は、モータMの回転角度を検出する位置検出器(図示せず)からの検出信号を制御部26に入力可能なデータに変換して出力する。
【0035】
制御部26は、電流検出部24から出力されるデータを元にモータMに流れる電流の値を取得するとともに、回転角度検出部25から出力されるデータを元にモータMの回転角度および回転速度を取得する。詳細は後述するが、制御部26は、上記取得した回転角度を、角度指令生成部27から与えられる指令値(回転角度指令pc)に一致させるようにインバータ装置23によるモータMの駆動をフィードバック制御する。なお、このフィードバック制御の際に上記取得した電流値および回転速度も用いられる。
【0036】
さて、一般に産業用のロボット2は、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作する通常動作(進み動作)を行うようになっている。これに加えて、本実施形態のロボット2は、自身の異常以外の原因(設備異常などの原因)で停止した際に、各軸が進み動作時の動作軌跡を逆向きに辿るように動作する遡り動作を行うことが可能となっている。
【0037】
角度指令生成部27は、制御部26の上位の制御装置に相当するものであり、制御部26と同様にマイクロコンピュータを主体として構成されている。角度指令生成部27は、通常動作が行われる際には、上記動作プログラムを解釈し、ロボット2に動作プログラムに従った動作を行わせるように各モータMを制御するための回転角度指令を生成周期毎に生成する。角度指令生成部27は、生成した回転角度指令の一部を回転角度指令pcとして出力周期毎に制御部26に出力する。出力周期は、生成周期よりも長くなっている。
【0038】
図3は、通常動作時における角度指令生成部27による回転角度指令の生成および出力のタイミングを示している。図3に示すように、角度指令生成部27は、比較的短い生成周期(本実施形態では1ms)毎に生成した回転角度指令の一部を、比較的長い出力周期(本実施形態では8ms)毎に出力する。言い換えると、角度指令生成部27は、生成周期毎に生成した回転角度指令のうち、大部分は出力しない(間引いている)。本実施形態では、出力する回転角度指令と、出力しない(間引く)回転角度指令との割合は、1:7としている。このような構成とした理由は、以下のとおりである。
【0039】
すなわち、回転角度指令は、角度指令生成部27のソフトウエアによる処理によって生成されるため、比較的短い間隔である生成周期毎に生成することができる。一方、モータM、インバータ装置23、回転角度検出部25などは、ハードウエアによる処理によって、ロボット2の各軸を駆動したり、各軸の回転角度を検出したりする。従って、回転角度を回転角度指令に一致させるための動作間隔は、生成周期に比べて長い間隔にならざるを得ない。このため、回転角度指令の生成周期を動作間隔に合わせて長くするか、あるいは短い生成周期で生成した回転角度指令を動作間隔に合わせて間引いて出力するかのいずれかを採用する必要がある。本実施形態では、後述する遡り制御において短い生成周期で生成した回転角度指令を用いる必要があるため、後者の方法を採用している。
【0040】
角度指令記憶部28は、通常動作が行われる期間に角度指令生成部27が生成した回転角度指令の全て(=出力された回転角度指令と間引かれた回転角度指令との両方)を順次記憶する。例えば、通常動作が図3に示した期間だけ行われる場合、生成された回転角度指令pc1’〜pc12’をこの順番通りに記憶する。図4は、角度指令記憶部28に記憶される回転角度指令の一例を示している。図4に示すように、角度指令記憶部28には、ロボット2の全ての軸(第1軸〜第4軸)を駆動するための各モータMに対する回転角度指令(通常動作が開始される時点から終了される時点までの回転角度指令)が記憶される。記憶された回転角度指令のうち、最初の回転角度指令がロボット2の待機位置に相当し、最後の回転角度指令がロボット2の停止位置に相当している。すなわち、角度指令記憶部28に記憶された回転角度指令は、通常動作時における各軸の回転角度(軌跡)の生成周期毎のログであるとも言える。
【0041】
制御部26は、通常動作が行われていない期間にて、遡り開始指令および遡り終了指令を入力する機能(遡り指令入力部に相当)を備えている。つまり、制御部26は、通常動作が行われている期間にあっては、遡り開始指令または遡り終了指令が与えられても、それらの指令を無視する。遡り開始指令は、ロボット2が設備異常などの原因で停止された場合に与えられる。本実施形態では、遡り開始指令は、ティーチングペンダント4に設けられた遡り開始スイッチが操作されることで与えられる。なお、遡り開始指令は、上記スイッチの手動操作によって与えられる構成に限らずともよく、自動的に与えられる構成としてもよい。例えば、遡り開始指令を出力するためのプログラムを実装するとともに、ロボット2が上記原因で停止した場合に、そのプログラムが自動的に実行されるように設定すればよい。また、遡り終了指令は、遡り動作によってロボット2が所定の待機位置まで移動した時に与えられる。
【0042】
制御部26は、遡り開始指令が入力されると、角度指令記憶部28に記憶されている最新の各回転角度指令(図4におけるNo.Nの回転角度指令)と、回転角度検出部25を介して取得した各モータMの現在の回転角度を用いて、遡り可否判断処理を行う。この遡り可否判断処理は、ロボット2の最新の位置、つまりロボット2の停止位置が、最新の指令位置と一致しているか否かを確認するものである(詳細は後述する)。制御部26は、遡り可否判断処理によりロボット2の停止位置が指令位置と一致していると判断した場合、角度指令生成部27に遡り開始指令を出力し、ロボット2に対し遡り動作を行わせる遡り制御を実行する。また、制御部26は、停止位置が指令位置と一致しないと判断した場合、角度指令生成部27に遡り開始指令を出力せず、上記遡り制御を実行しない。制御部26は、遡り終了指令が与えられると、その遡り終了指令を角度指令生成部27に出力し、上記遡り制御の実行を終了する。
【0043】
角度指令生成部27は、通常動作を行っていない期間において、制御部26から遡り開始指令が与えられると、角度指令記憶部28に記憶されている回転角度指令を記憶された時期が2番目に新しいもの(図4におけるNo.N−1)から最も古いもの(図4におけるNo.1)へと順に読み出す。角度指令生成部27は、読み出した全ての回転角度指令を、読み出した順に、つまり、記憶された時期が新しいものから古いものへと順に、回転角度指令pcとして出力する。この場合における回転角度指令pcが遡り角度指令に相当する。図5は、遡り動作時における角度指令生成部27による回転角度指令pcの出力のタイミングを示している。図5に示すように、角度指令生成部27は、読み出した回転角度指令の全てを、出力周期毎に出力する。従って、比較的短い生成周期毎に生成された回転角度指令が、間引かれることなく、その生成された順番とは逆の順番で、比較的長い出力周期毎に出力されることになる。そして、角度指令生成部27は、制御部26から遡り終了指令が与えられると、上記遡り角度指令の出力動作を停止する。
【0044】
図6は、ロボットシステム1におけるモータ制御の内容を等価的に示したブロック図である。図6に示すように、制御部26は、位置制御部41、速度制御部42および電流制御部43を備えている。なお、図6では、1つのモータMの制御に係る構成のみを示しているが、実際には全てのモータMのそれぞれに対応して同様の構成が設けられている。
【0045】
位置制御部41は、角度指令生成部27から与えられる回転角度指令pcに対する現在の回転角度p*の偏差を求める減算器44と、この減算器44の出力(偏差)をゼロに近づけるように速度指令vcを出力する位置制御アンプ45とから構成されている。この位置制御アンプ45のゲインはKpとなっている。
【0046】
速度制御部42は、微分器46、減算器47および速度制御アンプ48により構成されている。微分器46は、現在の回転角度p*を微分して現在の回転速度v*に変換する。減算器47は、速度指令vcに対する現在の回転速度v*の偏差を求める。速度制御アンプ48は、この減算器47の出力(偏差)をゼロに近づけるように電流指令icを出力する。この速度制御アンプ48のゲインはKvとなっている。
【0047】
電流制御部43は、電流指令icに対する現在のモータMに流れる電流i*の偏差を求める減算器49と、この減算器49の出力(偏差)をゼロに近づけるようにインバータ装置23に対する指令信号(通電指令Sc)を出力する電流制御アンプ50とから構成されている。この電流制御アンプ50のゲインはKiとなっている。このような構成により、制御部26は、電流フィードバック制御、速度フィードバック制御および位置フィードバック制御を行い、モータMの駆動をフィードバック制御してロボット2のアームの動作制御を行う。
【0048】
次に、上記構成の作用について図7および図8も参照して説明する。
図7は、所定の動作プログラムに従って動作するロボットの状態を示している。図8は、上記所定の動作プログラムの概要を示している。図8に示す動作プログラム(pro2)の概要は以下のとおりである。すなわち、07行目は、ロボット2の手先を位置P1(待機位置)まで動作させる命令である。09行目は、ロボット2の動作に関係する部品の品番に応じて10行目または15行目に移行させる分岐命令である。10行目に移行した場合には、11〜13行目の命令が実行される。11〜13行目は、ロボット2の手先を位置P2、位置P3、位置P4という順に移動させる命令である。一方、15行目に移行した場合には、16行目以降の命令が実行される。
【0049】
図7では、09行目の分岐命令において10行目に移行した場合のロボット2の状態を示している。従って、この動作プログラムに従った通常動作が行われる際には、ロボット2の手先は、位置P1→位置P2→位置P3→位置P4という順に移動する。なお、図7では、手先が位置P1にあるときのロボット2を実線で示し、手先がそれぞれ位置P2、P3、P4にあるときのロボット2を二点鎖線で示している。
【0050】
この際、角度指令生成部27は、ロボット2が位置P1から位置P4まで順に移動するように各モータMを駆動させるための回転角度指令を生成周期毎に生成するとともに、その一部を出力周期毎に出力する。従って、角度指令生成部27は、25個(=1×4+7×3)の回転角度指令を生成し、そのうちの4個の回転角度指令(位置P1、P2、P3、P4をそれぞれ指令するもの)を出力する。この際、角度指令記憶部28は、角度指令生成部27が生成した25個の回転角度指令を順次記憶している。制御部26は、角度指令生成部27から与えられる回転角度指令pcに基づいて、モータMの駆動をフィードバック制御する(通常制御)。
【0051】
このような通常動作が行われている際、例えば設備などの異常によりロボット2が位置P4において停止された場合、本実施形態では、以下のような手順で遡り動作を実行することが可能となっている。すなわち、ティーチングペンダント4に設けられた遡り開始スイッチが操作されると、制御部26は、遡り可否判断処理を実行する。この遡り可否判断処理において、制御部26は、角度指令記憶部28に記憶された最新の回転角度指令(この場合、位置P4を示す指令)と、現在のモータMの回転角度との差分を各軸について求める。制御部26は、それら差分の絶対値のうち最も大きい値が所定の誤差値以下である場合には、角度指令生成部27に対し遡り開始指令を出力する。この誤差値は、ロボット2が正常に動作している際に生じる回転角度指令と回転角度との誤差(回転角度指令に対する回転角度の偏差)よりも十分に大きい値とすればよい。
【0052】
角度指令生成部27は、遡り開始指令が与えられると、角度指令記憶部28に記憶されている25個の回転角度指令のうち、位置P4よりも1つ前(位置P3寄り)の位置を示す指令から最も古いもの(位置P1=待機位置を示す指令)へと順に読み出す。角度指令生成部27は、読み出した24個の回転角度指令を、読み出した順に、つまり、記憶された時期が新しいものから古いものへと順に、回転角度指令pc(遡り角度指令)として出力周期毎に出力する。
【0053】
制御部26は、角度指令生成部27から与えられる回転角度指令pc(遡り角度指令)に基づいて、モータMの駆動をフィードバック制御する(遡り制御)。この結果、ロボット2の手先は、位置P4→位置P3→位置P2→位置P1(待機位置)という順に移動する。しかも、この際、各軸が通常動作(進み動作)時と同じ位置を通過しながら、言い換えると、ロボット2が通常動作時と同じ姿勢を保ちつつ移動する。さらに、1msの生成周期毎に生成した回転角度指令が8msの出力周期毎に遡り角度指令として出力されるため、遡り動作時のロボット2の移動速度は、通常動作時のロボット2の移動速度の約1/8となる。
【0054】
ロボット2の手先が位置P1(待機位置)に移動したことにより、制御部26に遡り終了指令が与えられる。これに伴い、制御部26は、角度指令生成部27に対し遡り終了指令を出力するとともに遡り制御の実行を終了する。また、角度指令生成部27は、制御部26から遡り終了指令が与えられたことにより、遡り指令の出力動作を停止する。
【0055】
以上説明したように、本実施形態によれば次のような効果が得られる。
ロボット2の遡り動作が行われる際、角度指令生成部27は、角度指令記憶部28に記憶されている回転角度指令を新しいものから古いものへと順に読み出すとともに、それら読み出した回転角度指令を遡り角度指令として出力周期毎に出力する。すなわち、進み動作時における各軸の回転角度のログを逆の順番に並べ替えたものが遡り角度指令となって出力される。制御部26は、各軸の回転角度を遡り角度指令に一致させるように、モータMによる各軸の駆動をフィードバック制御する遡り制御を実行する。これにより、ロボット2の全ての軸の回転角度が進み動作時と同じ値をとりつつ遡り動作が行われる。言い換えれば、ロボット2が、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。つまり、いわゆるプレイバック動作が行われるので、進み動作時と同じ姿勢を保ちつつ遡り動作が行われることになる。従って、ロボット2を停止位置から所定の待機位置まで戻す遡り動作が行われる際において、ロボット2の姿勢が進み動作と比較して変化することで生じる衝突のリスクを軽減することができる。
【0056】
このような本実施形態に対し、動作プログラムを停止位置への移動を命令する行から、所定の待機位置への移動を命令する行まで単純に遡って実行することにより、ロボット2を停止位置から所定の待機位置まで戻すという手法を採用することも考えられる。しかしながら、上記手法は以下の理由から採用することはできない。すなわち、動作プログラムには、条件毎に以降の動作が変化する分岐命令(例えば、図7の09行目)などが存在する。動作プログラムを単に逆に実行することを考えた場合、このような分岐部分において、どのように分岐させるべきかを決定することができない。従って、動作プログラムを逆に実行することができない可能性が高い。さらに、ロボット2の通常動作(進み動作9においては、所定の動作点から次の動作点に移動する際にパス動作が行われる場合がある。本実施形態によれば、進み動作時における各軸の回転角度のログを逆の順番に並べ替えたものが遡り角度指令として出力されるため、パス動作に伴う各軸の回転角度についても正確に再現される。一方、単純に動作プログラムを逆に実行するだけでは、このようなパス動作に伴う各軸の回転角度を求めることが不可能である。このようなことから、動作プログラムを単に逆に実行するだけでは、本実施形態と同様の効果は得られない。
【0057】
また、遡り角度指令は、生成周期毎に生成される全ての回転角度指令を長い出力周期毎に出力したものである。つまり、本来は比較的短い生成周期毎の回転角度を指令する回転角度指令を比較的長い周期毎に出力するので、遡り動作時における各軸の回転速度、ひいてはロボット2の動作速度が通常動作時に比べて遅くなる。このように、遡り動作時には、通常動作時と比べてロボット2の動作が遅くなる(スロー動作になる)ので、仮にロボット2周辺の他の設備がユーザの予期せぬような動きを行った場合であっても、ユーザはその動きに対して余裕を持って対処することができる。このため、遡り動作が行われる際に初めて生じる衝突のリスクについても軽減することができる。
【0058】
さて、ロボット2が例えば周辺設備に衝突したことで停止した場合、停止時点の各軸の回転角度が回転角度指令とは、かけ離れた値となっていることが多い。このため、正常に遡り制御を実行することは難しい。また、仮に遡り制御がうまく実行できたとしても、それは、周辺の設備に衝突したときの軌跡をロボット2が再び通ることになる。つまり、ロボット2が周辺の設備に再度衝突してしまう可能性が非常に高くなる。従って、衝突による停止時には、遡り制御を実行することなく、ロボット2を含む各設備の動作を完全に停止させた上で、衝突が生じた原因を取り除くことが望ましい。そこで、本実施形態の制御部26は、遡り開始指令が与えられると、以下の遡り可否判断処理を実行する。
【0059】
すなわち、制御部26は、角度指令記憶部28に記憶された最新の回転角度指令と現在のモータMの回転角度との差分を各軸について求め、それら差分の絶対値のうち最も大きい値が所定の誤差値以下である場合には、遡り制御を実行する。一方、差分の絶対値のうち最も大きい値が所定の誤差値以下である場合には遡り制御を実行しない。ロボット2が正常に動作している場合でも、回転角度と回転角度指令とは、種々の誤差の影響によって完全に一致しない場合がある。これに対し、ロボット2が衝突により停止した場合には、回転角度と回転角度指令とは大きくかけ離れた値となる。このことから、制御部26は、回転角度と回転角度指令との差分の絶対値が所定の誤差値を超えた場合には、ロボット2が衝突により停止した状態であると判断し、遡り制御を実行しないようにしている。これにより、遡り動作が行われることによってロボット2が再衝突するリスクを抑制することができる。
【0060】
また、角度指令生成部27は、遡り開始指令が与えられると、角度指令記憶部28に記憶されている回転角度指令を記憶された時期が2番目に新しいものから最も古いものへと順に読み出す。角度指令記憶部28に記憶された回転角度指令のうち最新のものは、ロボット2が停止した時点の現在位置を指令するためのものである。このため、最新のものから順に遡り角度指令とした場合には、実際にロボット2が遡り動作を開始するのは、最新のものよりも1つ前の遡り角度指令が出力された時点となる。つまり、遡り動作の開始が若干遅れることになる。これに対し、本実施形態の構成によれば、そのような若干の遅れもなく、素早く遡り動作を開始することができ、以って、遡り動作全体に要する時間を短縮することができる。
【0061】
(第2の実施形態)
以下、本発明の第2の実施形態について図9〜図14を参照しながら説明する。
図9および図10は、それぞれ第1の実施形態における図1および図6相当図であり、第1の実施形態と同一部分には同一符号を付して説明を省略する。図9に示す本実施形態のロボットシステム61は、図1に示した第1の実施形態のロボットシステム1に対し、ロボット2に代えてロボット62を備えている点が異なる。
【0062】
ロボット62は、例えば6軸の垂直多関節型ロボットとして構成されている。ロボット62は、ベース63と、このベース63に水平方向に回転可能に支持されたショルダ部64と、このショルダ部64に上下方向に回転可能に支持された下アーム65と、この下アーム65に上下方向に回転可能に支持された第1の上アーム66と、この第1の上アーム66に捻り回転可能に支持された第2の上アーム67と、この第2の上アーム67に上下方向に回転可能に支持された手首68と、この手首68に捻り回転可能に支持されたフランジ69とから構成されている。
【0063】
ベース63、ショルダ部64、下アーム65、第1の上アーム66、第2の上アーム67、手首68およびフランジ69は、ロボット62のアームとして機能し、アーム先端であるフランジ69には、図示はしないが、エンドエフェクタ(手先)が取り付けられる。ロボット62に設けられる複数の軸は、第1の実施形態のロボット2と同様、それぞれに対応して設けられるモータ(図2に符号Mを付して示す)により駆動される。また、各モータの近傍には、それぞれの回転軸の回転位置を検出するための位置検出器(図示せず)が設けられている。
【0064】
図示はしないが、本実施形態の角度指令記憶部28は、通常動作が行われる期間において、生成周期(例えば1ms)毎に電流i*の値(または電流指令icの値)を順次記憶するようになっている。すなわち、本実施形態の角度指令記憶部28には、生成周期毎の電流値のログデータが記憶される。
【0065】
図10は、ロボットシステム61におけるモータ制御の内容を等価的に示したブロック図である。図10に示すように、本実施形態の制御部71は、第1の実施形態の制御部26が備える構成に加え、さらに、トルク値推定部72、発生トルク値演算部73および外乱トルク値演算部74を備えている。
【0066】
トルク値推定部72(トルク値推定手段に相当)は、ロボット62が所定の動作プログラムに基づいて行う動作の開始から終了までの間にモータMに発生させる必要があるトルクの値(推定トルク値)を、回転角度指令pcを用いて一括して推定する。この推定方法としては、例えば運動方程式を用いて、ロボット62の軸を所定の回転位置からその次の回転位置まで回転させるためにモータMに発生させる必要があるトルク値を推定するという方法が挙げられる。なお、トルク値推定部72は、ロボット62が現在の指令位置からその次の指令位置まで移動するためにモータMに発生させる必要があるトルク値を逐次推定するように構成してもよい。発生トルク値演算部73(トルク値演算手段に相当)は、モータMに流れる現在の電流i*に対し、モータMのトルク定数を乗算することにより、モータMが発生する現在のトルクの値(発生トルク値)を求める。
【0067】
外乱トルク値演算部74(外乱演算手段に相当)は、推定トルク値および発生トルク値の差分から、モータMが駆動する対象となる軸に外部から加わるトルク値(外乱負荷トルク値)を求める。すなわち、外部からロボット62に対して負荷が加えられない限り、ロボット62の各軸には、回転角度指令pcに従いモータMが発生するトルクがそれぞれ生じることになるので、推定トルク値と発生トルク値とは一致する。これに対し、衝突が発生したことなどにより外部からロボット62に対して所定の負荷が加えられると、推定トルク値と発生トルク値との間には、その負荷に応じた分だけの差が生じる。そこで、外乱トルク値演算部74は、推定トルク値Teおよび発生トルク値Tgを用いて下記(1)式に基づき外乱負荷トルク値Tdを演算する。
Td=|Tg−Te| …(1)
【0068】
制御部71(衝突判断手段に相当)は、各軸に加わる外乱負荷トルク値Tdに基づいてロボット62に対する衝突を検出する。すなわち、制御部71は、所定の衝突検出周期毎に各軸に加わる外乱負荷トルク値Tdを求め、少なくともいずれか一つの軸に加わる外乱負荷トルク値Tdが予め定めたしきい値Tthを超えると衝突が発生したと判断する。しきい値Tthは、予めユーザによる設定が可能となっており、例えば1〜500[kgf・cm]の範囲の値(初期値は100[kgf・cm])としている。なお、しきい値Tthの設定値を小さくすると衝突検出の感度を高められるが、種々の演算誤差などに起因する誤検出の可能性も高まる。一方、しきい値Tthの設定値を大きくすると衝突検出の感度は低くなるが、誤検出の可能性が低減する。しきい値Tthは、このようなトレードオフの関係を踏まえた上で適用するシステムとして最適な値に設定すればよい。
【0069】
このように、本実施形態のロボットシステム61では、ロボット62が周辺機器と衝突した際、センサレスで瞬時に衝突を検出することを可能としている。すなわち、本実施形態では、制御部71が衝突検出手段として機能する。また、制御部71は、ロボット62の衝突を検出すると、ロボット62の各アームの動作を直ちに停止させる停止制御を行うようになっている。制御部71は、その停止制御において、ロボット62の各軸を駆動するモータMに逆転トルクを発生させつつ、その後の移動軌跡に沿ってアームの動作を停止させる。
【0070】
また、制御部71は、ロボット62の衝突を検出すると、衝突時の各軸の回転角度に基づいて衝突位置を特定する。図11は、衝突前後の所定の軸における回転角度指令pc、回転角度p*および電流i*を示している。図11における縦向きの実線は、動作指令出力ポイント(サンプリングタイミング)を示している。その動作指令出力ポイントの間隔(ロボット動作指令間隔)は、本実施形態では出力周期(例えば8ms)と同じになっている。また、図11における縦向きの点線は、上記した衝突検出動作の実行タイミングを示している。その実行タイミングの間隔(衝突検出周期)は、出力周期よりも短くなっており、本実施形態では生成周期(例えば1ms)と同じになっている。
【0071】
時刻t1の時点において、ロボット62に対する衝突が生じた場合、その後も回転角度指令pcは、次の動作指令出力ポイントの値(目標値)に向けて増加し続ける。一方、検出される実際の回転角度p*(エンコーダ値)は、衝突が生じているため、その値は変化せずほぼ一定を保っている。このため、回転角度指令pcに対する現在の回転角度p*の偏差(位置偏差)が急激に増加し、それに伴って電流i*も急激に増加する。その後、時刻t2の時点において、前述した衝突検出動作により衝突が検出されると、回転角度指令pcを回転角度p*に合わせるようにしてロボット62が停止制御される。この際、電流i*もゼロに向けて低下し、ロボット62が完全に停止した時点(時刻t3)でゼロになる。
【0072】
制御部71は、衝突を検出してロボット62を停止させた後、角度指令記憶部28に記憶された電流値のログデータを、停止させた時点の値から遡り、その値が急激に増加し始めたポイントを探す。図11の場合、時刻t1の時点において電流i*が急増し始めることになる。このため、制御部71は、その時刻t1を衝突が生じたタイミング(衝突タイミング)として特定するとともに、時刻t1における各軸の回転角度p*に基づいて衝突位置を特定する。この衝突位置の特定方法としては、例えば、加わった外乱負荷トルク値Tdが最も大きい軸に対して衝突が発生したと判断して衝突位置を特定する方法が挙げられる。その場合、各軸の外乱負荷トルク値Tdから、ヤコビ行列を用いてロボット62の手先に加わる外乱負荷トルク値を求め、手先への衝突も特定することが可能である。
【0073】
また、本実施形態では、制御部71は、衝突検出後においても遡り制御を実行することが可能となっている。制御部71は、衝突を検出すると、衝突後の遡り制御で用いるための進入禁止空間を設定する。進入禁止空間は球状をなしており、その中心は、特定した衝突位置(時刻t1の位置)に設定される。そして、例えば、特定した衝突位置がロボット62の手先である場合、進入禁止空間の半径は、衝突タイミングの前後における動作指令出力ポイントのうち、衝突タイミングに近い方のタイミング、つまり、衝突タイミングの直前の動作指令出力ポイント(時刻ta)時点の手先の位置と、衝突時点の手先の位置(衝突位置)との間の距離に相当する長さに設定される。なお、時刻ta時点の手先の位置は、時刻taの時点において出力された各軸の回転角度指令に基づいて求められる。
【0074】
次に、本実施形態の作用および効果について図12〜図14も参照して説明する。
さて、ロボット62の衝突としては、自動運転中の衝突や、教示中または動作確認中の衝突などが考えられる。前者の場合、ロボット62は最高速度で動作している可能性が高く、衝突速度は比較的速い。このため、ロボットの機構部品や周辺設備が多大な損傷を受ける可能性が高く、その後の自動復旧が困難な状態となる。このような場合には、衝突により停止した時点の位置偏差が過大になることが多いため、遡り制御の実行自体が困難となる。この場合には、衝突検出以前に、偏差過大、過電流、加速度限界値オーバーなどのエラーが発生してロボット62が緊急停止されるため、本実施形態のロボットシステム61においても遡り制御は実行されない。
【0075】
一方、後者の場合、ロボット62は低速で動作しているため、その衝突速度は比較的遅い。このため、ロボットの機構部品や周辺設備がほとんど損傷することなく、その後の自動復旧が可能な状態となる。このような場合には、衝突により停止した時点の位置偏差が比較的小さいことが多いため、遡り制御を実行することが可能となる。また、上記各衝突のうち、衝突速度の速い前者の衝突よりも衝突速度の遅い後者の衝突のほうが発生頻度は高い。なぜなら、自動運転中、ロボット62は動作プログラムに基づいて定められた動作を繰り返し行う。そして、その繰り返される動作は、予め周辺設備などに衝突することがないように十分に検証されたものである。このため、自動運転中に衝突する可能性は極めて低い。
【0076】
これに対し、教示中や動作確認中においては、周辺設備との衝突検証を兼ねて教示や動作確認を行うようになっており、その最中に動作軌跡の修正が頻繁に行われることも多い。このため、教示中や動作確認中には、ロボット62の動作の再現性は低い。従って、新たな動作軌跡を生成した後や、動作軌跡を修正した後に行われる教示や確認動作において、周辺設備などへの衝突が生じる可能性は高くなる。このように、衝突速度が速く遡り制御を実行することができない重度の衝突よりも、衝突速度が遅く遡り制御を実行可能な軽微な衝突のほうが発生頻度は高い。このようなことから、軽微な衝突によりロボット62が停止した場合にも、遡り制御を実行してロボットを待機位置まで移動させることが望ましい。
【0077】
ただし、軽微な衝突とは言っても、むやみにその衝突回数を増やすことは、精密動作が要求されるロボット62にとっては好ましくない。なぜなら、軽微な衝突であっても、その衝突が繰り返されることにより、ロボット62を精密に動作させるために行われるキャリブレーションやフレームの精度に狂いが生じる危険性があるからである。そこで、本実施形態では、軽微な衝突によりロボット62が停止した場合、以下のように再衝突の発生を防止する工夫を加えた上で、遡り制御を実行するようにしている。
【0078】
図12は、例えば教示中または動作確認中における衝突前後のロボットの動作状態を示している。まず、図12の(1)に示すように、ロボット62の手先は、設備81から少し離れた位置から設備81の方向に向けて移動している。そして、図12の(2)に示すように、ロボット62の手先が設備81に衝突する。この際、制御部71は、その衝突を検出し、ロボット62の停止制御を開始する。その後、図12の(3)に示すように、ロボット62の手先は、衝突の反動で設備81から僅かに離れた位置まで移動しながら上記停止制御により停止する。
【0079】
制御部71は、このように衝突発生に伴ってロボット62が停止した状態において、遡り開始指令が入力されると、遡り制御を実行する。ただし、衝突によりロボット62が停止された後に遡り制御を実行する際、第1の実施形態の遡り制御と同じように角度指令生成部27が出力する回転角度指令pc(遡り角度指令)に基づいてロボット62を遡り動作させると、その動作の軌跡は、必ず衝突位置を通過することになる。従って、ロボット62が再度衝突する可能性が高くなってしまう。
【0080】
そこで、本実施形態の角度指令生成部27は、衝突が検出された状態で遡り開始指令が入力されると、角度指令記憶部28から読み出した回転角度指令pcに以下のような変更を加える。すなわち、本実施形態の角度指令生成部27は、衝突が検出された状態で遡り指令が入力されると、角度指令記憶部28から読み出した回転角度指令pcに基づくロボット62の遡り動作の軌跡が、制御部71が設定した進入禁止空間の球の外郭に最初に重なる外郭位置と最後に重なる外郭位置との間では、その動作軌跡が進入禁止空間の外であり、且つ、進入禁止空間の球の表面に沿う軌跡となるように、読み出した回転角度指令pcを変更し、その変更後の回転角度指令pcを遡り角度指令として出力する。要するに、本実施形態では、進入禁止空間の内部を通過する動作軌跡となる回転角度指令pcを遡り角度指令として使用することなく、その代わりに、進入禁止空間の球の表面を這うような動作軌跡となる回転角度指令pcを新たに生成して遡り角度指令として使用する。
【0081】
このように角度指令生成部27による回転角度指令pcの変更に伴って動作軌跡が変更された状態で、制御部71による遡り制御が実行されると、ロボット62の手先は、図12の(3)に示した停止位置から、衝突位置を経由し、図12の(1)に示した位置に移動することになる。ただし、遡り制御において衝突位置を経由する際には、図13に示すように、ロボット62の手先が衝突位置を回避するように遡り動作が行われる。以下、遡り動作時の衝突位置の回避動作について、図14を参照して詳しく説明する。
【0082】
図14は、衝突位置付近における進み動作時の手先の動作軌跡と、衝突後の遡り動作時の手先の動作軌跡とを示している。図14では、進み動作時の動作軌跡は実線で示し、遡り動作時の動作軌跡は一点鎖線で示している。また、衝突位置は符号aを付した点で示し、設定した進入禁止空間は破線で表すとともに符号bを付している。さらに、進み動作時の動作軌跡が、最初に進入禁止空間bに重なる位置(脱出外郭位置)は符号x1を付した点で示し、最後に進入禁止空間bに重なる位置(進入外郭位置)は符号x2を付した点で示している。なお、図14に示す時刻t1、ta、tbは、いずれも図11に同じ符号を付して示した各時刻に対応している。
【0083】
図14に示すように、衝突により停止した後、ロボット62の手先は、その停止位置から進入外郭位置x2までの区間では、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。進入外郭位置x2から脱出外郭位置x1までの区間では、進入禁止空間bの球の表面(外郭)に沿うようにして衝突位置aを回避する。そして、脱出外郭位置x1以降の区間では、再び進み動作の動作軌跡を逆向きに辿るように遡り動作が行われることになる。なお、図14では、説明の都合上、停止位置から進入外郭位置x2までの区間および脱出外郭位置x1以降の区間において、進み動作時および遡り動作時の各動作軌跡を互いに若干ずらして描いているが、実際には各動作軌跡は一致している。
【0084】
進入禁止空間bの半径は、前述したように設定されていることから、非常に小さいものであると言える。このため、進入禁止空間bの外郭を沿う遡り動作を行う際、ロボット62の肘などの手先とは別の部分が周辺設備などに衝突するといった新たな衝突が引き起こされる可能性が高まるほど、つまり、新たな別の衝突が発生するようなレベルまでロボット62の姿勢が変化することはない。すなわち、進入禁止空間bの外郭を沿う遡り動作を行う際におけるロボット62の姿勢は、衝突が発生した進み動作時の姿勢とほとんど同じであり、新たな別の衝突を引き起こすような姿勢の変化はない。
【0085】
このように衝突後の遡り動作では衝突位置aを回避するため、完全なプレイバック動作とはならない。しかし、上記したとおり進入禁止空間bは小さい。また、元々、ロボット62が通過する経路(動作軌跡)は、ロボット62の体格を考慮した上で周辺設備などに衝突しないように所定のマージンが設けられている。そして、ロボット62の体格は、進入禁止空間bよりも遥かに大きい。このため、進入禁止空間bの外郭を沿うように動作軌跡が変更された程度では、ロボット62が上記マージンの範囲を越えて周辺設備に衝突する可能性は極めて低い。つまり、遡り制御における若干の動作軌跡の変更により、ロボット62が新たな衝突を生じることはほとんどあり得ない。従って、衝突後にロボット62が遡り動作する際、その衝突位置aを確実に避けるとともに新たな衝突の発生を未然に防止しつつ、進み動作時と同じ姿勢を保ちながら遡り動作することが可能となる。
【0086】
(その他の実施形態)
なお、本発明は上記し且つ図面に記載した各実施形態に限定されるものではなく、次のような変形または拡張が可能である。
角度指令生成部27は、遡り開始指令が入力されたとき、角度指令記憶部28に記憶されている回転角度指令を記憶された時期が3番目以降に新しいものから古いものへと順に読み出してもよい。また、角度指令生成部27は、遡り開始指令が入力されたとき、角度指令記憶部28に記憶されている回転角度指令を記憶された時期が最も新しいものから最も古いものへと順に読み出してもよい。
生成周期および出力周期は、遡り動作時のロボット2の動作速度を必要な速度に設定するように適宜変更可能である。すなわち、角度指令生成部27が出力する回転角度指令および出力しない(間引く)回転角度指令の割合(1:x)は、「x>1」という条件を満たす範囲内で適宜変更可能である。
遡り可否判断処理において、制御部26は、角度指令記憶部28に記憶された最新の回転角度指令と現在のモータMの回転角度との差分を、所定の軸について求めるように変更してもよい。また、制御部26は、ロボット2が衝突により停止した際に遡り開始指令が与えられないという条件であれば、遡り可否判断処理を行わなくてもよい。
【0087】
遡り制御の実行を終了する待機位置としては、角度指令記憶部28に記憶された最初の回転角度指令が表す位置に限らずともよく、任意の位置でよい。この任意の位置として、例えば、動作プログラムにおけるmove命令(所定の作業をするために手先を動かすなどの命令)があるところの位置(またはその前後の位置)にするとよい。move命令の位置およびその前後の位置では、手先に対しワークを掴むなどの指令が出されている可能性が高い。ロボット2がワークを掴んだまま、遡り動作が継続されると、そのワークが周辺設備(例えば狭い通路など)に衝突してしまう可能性がある。そこで、move命令の前後で遡り動作を終了させてロボット2を待機させることで、上記衝突が発生する可能性を低減することができる。
【0088】
第2の実施形態において、制御部71が設定する進入禁止空間は、衝突発生後にロボット62を遡り動作させる際に、ロボット62が最初に衝突した位置に再衝突することを回避するとともに、新たな別の衝突の発生を防止できるような空間であれば適宜変更可能である。例えば、特定した衝突位置がロボット62の手先である場合、進入禁止空間の半径は、衝突タイミングの前後における動作指令出力ポイントのうち、衝突タイミングから遠い方のタイミング、つまり、衝突タイミングの直後の動作指令出力ポイント(図11、図14の時刻tb)時点の手先の位置と、衝突時点の手先の位置(衝突位置)との間の距離に相当する長さに設定してもよい。なお、時刻tb時点の手先の位置は、時刻tbの時点において出力された各軸の回転角度指令に基づいて求められる。このように設定した進入禁止空間の半径は、第2の実施形態で設定したものと比べて若干大きくなる。このため、例えば、ロボット62が衝突した設備の表面が、その衝突によって多少変形した場合でも、その変形部分をも避けて遡り動作を行うことが可能になり、設備の変形に伴う衝突を未然に防止できるという効果が得られる。
【0089】
また、ロボット62の精度誤差(例えば、±0.075mm)に基づいて進入禁止空間の半径を設定してもよい。その場合、安全サイドへのマージンをとるため、精度誤差をN倍(例えば、N=2)したものを、進入禁止空間の半径に設定するとよい。または、衝突時の速度(衝突速度)に基づいて進入禁止空間の半径を設定してもよい。その場合、安全サイドへのマージンをとるため、衝突検出周期(例えば1ms)の間に衝突速度で移動する距離をN倍(例えば、N=2)したものを、進入禁止空間の半径に設定するとよい。あるいは、ロボット62の精度誤差に基づいて設定した半径と、衝突速度に基づいて設定した半径とのうち、大きい方を進入禁止空間の半径に設定するようにしてもよい。
【0090】
上記衝突速度は、例えば衝突時の各軸の回転速度に基づいて求めればよい。すなわち、制御部71は、衝突を検出した後、衝突タイミングの直前の動作指令出力ポイント(図11の時刻ta)と、その一つ前の動作指令出力ポイント(図11の時刻tc)との間の移動量、つまり、直前の回転角度指令pcと、その一つ前の回転角度指令pcとの差から、衝突直前の出力周期における移動距離(移動量)を求める。そして、制御部71は、出力周期の値(例えば8ms)と求めた移動距離とから、衝突直前の出力周期における回転速度を算出し、その値を衝突速度として取得すればよい。このようにすれば、衝突が生じた直前の時点の速度、ひいては衝突時点の速度を正確に求めることが可能となる。
【0091】
衝突検出手段としては、第2の実施形態で説明した構成に限らずともよく、ロボットに対する衝突を検出可能なものであれば適用することができる。例えば、他のセンサレス方式の衝突検出手段や、加速度センサなどのセンサを用いる衝突検出手段であってもよい。
第1の実施形態では、本発明を4軸の水平多関節型のロボット2に適用した例を説明し、第2の実施形態では、本発明を6軸の垂直多関節型のロボット62に適用した例を説明したが、本発明は、複数の軸を有する多軸型のロボット全般に適用可能である。
【符号の説明】
【0092】
図面中、1、61はロボットシステム、2、62はロボット、25は回転角度検出部、26は制御部(遡り指令入力部)、27は角度指令生成部、28は角度指令記憶部、71は制御部(衝突検出手段)、Mはモータ(駆動部)を示す。
【特許請求の範囲】
【請求項1】
ロボットに設けられた複数の軸と、
前記複数の軸をそれぞれ駆動するための駆動部と、
前記複数の軸の回転角度を検出する回転角度検出部と、
回転角度指令を生成周期毎に生成し、その回転角度指令の一部を前記生成周期よりも長い出力周期毎に出力する角度指令生成部と、
前記回転角度検出部により検出される回転角度を前記角度指令生成部から出力される前記回転角度指令に一致させるように前記駆動部の動作を制御する通常制御を実行する制御部と、
前記制御部により前記通常制御が実行されている期間に前記角度指令生成部により生成される前記回転角度指令の全てを順次記憶する角度指令記憶部と、
前記制御部により前記通常制御が実行されていない期間にて、遡り開始指令および遡り終了指令を入力する遡り指令入力部とを備え、
前記角度指令生成部は、
前記遡り開始指令が入力されると、前記角度指令記憶部に記憶された前記回転角度指令を新しいものから古いものへと順に読み出すとともに、当該読み出した回転角度指令を遡り角度指令として前記出力周期毎に出力し、
前記遡り終了指令が入力されると、前記遡り角度指令の出力を停止し、
前記制御部は、前記遡り開始指令が入力されると、前記回転角度検出部により検出される回転角度を前記角度指令生成部から出力される遡り角度指令に一致させるように前記駆動部の動作を制御する遡り制御の実行を開始し、
前記遡り終了指令が入力されると、前記遡り制御の実行を終了することを特徴とするロボットシステム。
【請求項2】
前記制御部は、前記遡り開始指令が入力されると、前記角度指令記憶部に記憶されている最新の前記回転角度指令と前記回転位置検出部により検出された現在の回転角度との差分を求め、その差分の絶対値が所定の誤差値以下である場合には前記遡り制御を実行し、前記差分の絶対値が前記誤差値を超える場合には前記遡り制御を実行しないことを特徴とする請求項1に記載のロボットシステム。
【請求項3】
前記ロボットの衝突を検出する衝突検出手段を備え、
前記制御部は、
前記衝突検出手段により衝突が検出されると、前記衝突が発生した時点である衝突タイミングおよび前記衝突タイミング時の位置である衝突位置を特定し、前記衝突位置が中心であり且つ前記衝突タイミングの前後における前記回転角度指令の出力タイミングのうち前記衝突タイミングに近い方の出力タイミングに出力された回転角度指令と前記衝突位置との間の長さの半径を持つ球状をなす進入禁止空間を設定し、
前記角度指令生成部は、
前記衝突が検出された状態で前記遡り開始指令が入力されると、前記角度指令記憶部から読み出した回転角度指令に基づく前記ロボットの動作軌跡が前記進入禁止空間の外郭に重なる2つの外郭位置の間では、前記ロボットの動作軌跡が前記進入禁止空間の外であり且つ前記進入禁止空間の球の表面に沿う軌跡となるように前記読み出した回転角度指令を変更したものを前記遡り角度指令として出力することを特徴とする請求項1にロボットシステム。
【請求項4】
前記ロボットの衝突を検出する衝突検出手段を備え、
前記制御部は、
前記衝突検出手段により衝突が検出されると、前記衝突が発生した時点である衝突タイミングおよび前記衝突タイミング時の位置である衝突位置を特定し、前記衝突位置が中心であり且つ前記衝突タイミングの前後における前記回転角度指令の出力タイミングのうち前記衝突タイミングから遠い方の出力タイミングに出力された回転角度指令と前記衝突位置との間の長さの半径を持つ球状をなす進入禁止空間を設定し、
前記角度指令生成部は、
前記衝突が検出された状態で前記遡り開始指令が入力されると、前記角度指令記憶部から読み出した回転角度指令に基づく前記ロボットの動作軌跡が前記進入禁止空間の外郭に重なる2つの外郭位置の間では、前記ロボットの動作軌跡が前記進入禁止空間の外であり且つ前記進入禁止空間の球の表面に沿う軌跡となるように前記読み出した回転角度指令を変更したものを前記遡り角度指令として出力することを特徴とする請求項1にロボットシステム。
【請求項5】
前記角度指令生成部は、前記遡り開始指令が入力されると、前記角度指令記憶部に記憶された前記回転角度指令のうち最新のものを除く回転角度指令を新しいものから古いものへと順に読み出すことを特徴とする請求項1〜4のいずれか一つに記載のロボットシステム。
【請求項1】
ロボットに設けられた複数の軸と、
前記複数の軸をそれぞれ駆動するための駆動部と、
前記複数の軸の回転角度を検出する回転角度検出部と、
回転角度指令を生成周期毎に生成し、その回転角度指令の一部を前記生成周期よりも長い出力周期毎に出力する角度指令生成部と、
前記回転角度検出部により検出される回転角度を前記角度指令生成部から出力される前記回転角度指令に一致させるように前記駆動部の動作を制御する通常制御を実行する制御部と、
前記制御部により前記通常制御が実行されている期間に前記角度指令生成部により生成される前記回転角度指令の全てを順次記憶する角度指令記憶部と、
前記制御部により前記通常制御が実行されていない期間にて、遡り開始指令および遡り終了指令を入力する遡り指令入力部とを備え、
前記角度指令生成部は、
前記遡り開始指令が入力されると、前記角度指令記憶部に記憶された前記回転角度指令を新しいものから古いものへと順に読み出すとともに、当該読み出した回転角度指令を遡り角度指令として前記出力周期毎に出力し、
前記遡り終了指令が入力されると、前記遡り角度指令の出力を停止し、
前記制御部は、前記遡り開始指令が入力されると、前記回転角度検出部により検出される回転角度を前記角度指令生成部から出力される遡り角度指令に一致させるように前記駆動部の動作を制御する遡り制御の実行を開始し、
前記遡り終了指令が入力されると、前記遡り制御の実行を終了することを特徴とするロボットシステム。
【請求項2】
前記制御部は、前記遡り開始指令が入力されると、前記角度指令記憶部に記憶されている最新の前記回転角度指令と前記回転位置検出部により検出された現在の回転角度との差分を求め、その差分の絶対値が所定の誤差値以下である場合には前記遡り制御を実行し、前記差分の絶対値が前記誤差値を超える場合には前記遡り制御を実行しないことを特徴とする請求項1に記載のロボットシステム。
【請求項3】
前記ロボットの衝突を検出する衝突検出手段を備え、
前記制御部は、
前記衝突検出手段により衝突が検出されると、前記衝突が発生した時点である衝突タイミングおよび前記衝突タイミング時の位置である衝突位置を特定し、前記衝突位置が中心であり且つ前記衝突タイミングの前後における前記回転角度指令の出力タイミングのうち前記衝突タイミングに近い方の出力タイミングに出力された回転角度指令と前記衝突位置との間の長さの半径を持つ球状をなす進入禁止空間を設定し、
前記角度指令生成部は、
前記衝突が検出された状態で前記遡り開始指令が入力されると、前記角度指令記憶部から読み出した回転角度指令に基づく前記ロボットの動作軌跡が前記進入禁止空間の外郭に重なる2つの外郭位置の間では、前記ロボットの動作軌跡が前記進入禁止空間の外であり且つ前記進入禁止空間の球の表面に沿う軌跡となるように前記読み出した回転角度指令を変更したものを前記遡り角度指令として出力することを特徴とする請求項1にロボットシステム。
【請求項4】
前記ロボットの衝突を検出する衝突検出手段を備え、
前記制御部は、
前記衝突検出手段により衝突が検出されると、前記衝突が発生した時点である衝突タイミングおよび前記衝突タイミング時の位置である衝突位置を特定し、前記衝突位置が中心であり且つ前記衝突タイミングの前後における前記回転角度指令の出力タイミングのうち前記衝突タイミングから遠い方の出力タイミングに出力された回転角度指令と前記衝突位置との間の長さの半径を持つ球状をなす進入禁止空間を設定し、
前記角度指令生成部は、
前記衝突が検出された状態で前記遡り開始指令が入力されると、前記角度指令記憶部から読み出した回転角度指令に基づく前記ロボットの動作軌跡が前記進入禁止空間の外郭に重なる2つの外郭位置の間では、前記ロボットの動作軌跡が前記進入禁止空間の外であり且つ前記進入禁止空間の球の表面に沿う軌跡となるように前記読み出した回転角度指令を変更したものを前記遡り角度指令として出力することを特徴とする請求項1にロボットシステム。
【請求項5】
前記角度指令生成部は、前記遡り開始指令が入力されると、前記角度指令記憶部に記憶された前記回転角度指令のうち最新のものを除く回転角度指令を新しいものから古いものへと順に読み出すことを特徴とする請求項1〜4のいずれか一つに記載のロボットシステム。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【公開番号】特開2011−201009(P2011−201009A)
【公開日】平成23年10月13日(2011.10.13)
【国際特許分類】
【出願番号】特願2010−190767(P2010−190767)
【出願日】平成22年8月27日(2010.8.27)
【出願人】(501428545)株式会社デンソーウェーブ (1,155)
【Fターム(参考)】
【公開日】平成23年10月13日(2011.10.13)
【国際特許分類】
【出願日】平成22年8月27日(2010.8.27)
【出願人】(501428545)株式会社デンソーウェーブ (1,155)
【Fターム(参考)】
[ Back to top ]