JP5593968B2 - ロボットシステム - Google Patents

Description

本発明は、ロボット自体に異常がなくロボットを停止させた場合に、ロボットをその停止位置から所定の待機位置まで戻す動作を行う機能を備えたロボットシステムに関する。

例えば製造ラインなどにおいてロボットが用いられる場合、ロボットに異常がなくとも、ロボットと協調して動作する他の設備の異常（コンベアの停止、ワークとなる部品が流れてこない等）が原因でロボットの動作を停止せざるを得ない場合がある。このような場合、以下の理由からロボットを停止位置から所定の待機位置まで戻さなければならない。すなわち、他の設備をリセットした（所定の待機位置に戻した）にもかかわらず、ロボットだけが停止した位置まで進んだ状態になっていると、他の設備とロボットとの動作整合が取れなくなってしまう。

現在、ロボットを停止位置から所定の待機位置まで戻す方法としては、ユーザの手動操作により移動させる方法、または、停止位置から待機位置へ戻すための動作プログラムを準備しておき、ロボット停止時にその停止位置に対応する動作プログラムを選択して起動させてロボットを移動させる方法などが用いられている。

しかし、手動操作による方法では、ロボットの周辺に配置された設備等の物体に衝突することなく待機位置へ移動させるために正確な操作が要求される。このため、ユーザの作業負担が増大する。また、待機位置へ戻すための動作プログラムを用いる方法では、想定されるロボットの停止位置の数だけの動作プログラムを予め用意しておく必要がある。さらに、現実には想定外の位置でロボットが停止する場合も考えられ、この場合には、予め用意した動作プログラムでは、ロボットを待機位置へ戻すことができない可能性もある。
一方、特許文献１には、ロボットが異物と衝突したと判断した場合に、自動的に手先を異物と衝突しない位置まで戻す技術が開示されている。

特開平５−８４６８１号公報

設備の異常に伴ってロボットが停止した場合に、上記特許文献１記載の技術を用いれば、自動的にロボットの手先位置を所定の待機位置（待避位置）まで戻すことが可能となる。しかし、特許文献１記載の技術を用いただけでは、戻り動作時において衝突が生じる危険性がある。なぜなら、ロボットには姿勢という概念があり、単に手先位置が待機位置まで戻ればよいというわけではない。つまり、進み方向に所定動作を行った後に停止され、その後、単純に戻り方向に上記所定動作を行うと、進み方向に所定動作を行ったときとはロボットの姿勢が異なる可能性がある。例えば、進み方向に所定動作を行った際には、周辺の設備から離れるように肘の部分が折り畳まれた姿勢であったにもかかわらず、戻り方向に所定動作を行う際には、周辺の設備に接近するように肘の部分が張り出した姿勢となる場合も考えられる。このように姿勢が異なる場合、戻り方向に所定動作を行うときに、ロボットの肘の部分が周辺の設備に衝突する危険性（リスク）がある。

本発明は上記事情に鑑みてなされたものであり、その目的は、ロボット自体に異常がない状態でロボットを停止させた場合に、ロボットをその停止位置から所定の待機位置まで戻す動作を行う際における衝突のリスクを軽減することができるロボットシステムを提供することにある。

請求項１記載の手段によれば、通常の動作（進み動作）を行う場合、制御部は、ロボットに設けられた複数の軸の回転角度を角度指令生成部から出力される回転角度指令に一致させるように、駆動部による各軸の駆動をフィードバック制御する通常制御を実行する。ここで、複数の軸の回転角度は、回転角度検出部によって検出される。また、角度指令生成部は、回転角度指令を生成周期毎に生成し、その回転角度指令の一部を生成周期よりも長い出力周期毎に出力する。そして、角度指令記憶部は、上記通常制御が実行されている期間に角度指令生成部により生成される回転角度指令の全てを順次記憶する。

この理由は以下のとおりである。すなわち、現在において、回転角度指令は、通常、ソフトウエアによる処理（電気的な動作のみに依存する処理）によって生成されることが多いため、極めて短い間隔である生成周期毎に生成することができる。一方、駆動部、回転角度検出部などは、ハードウエアによる処理（機械的な動作が含まれる処理）によって、ロボットの軸を駆動したり、軸の回転角度を検出したりするため、回転角度を回転角度指令に一致させるための動作間隔は、上記生成周期に比べて長い間隔にならざるを得ない。このため、回転角度指令の生成周期を上記動作間隔に合わせて長くするか、あるいは短い生成周期で生成した回転角度指令を上記動作間隔に合わせて間引いて出力するかのいずれかを採用する必要がある。本手段では、後述する遡り制御において短い生成周期で生成した回転角度指令を用いる必要があるため、後者の方法を採用している。

さて、従来技術において説明したように、ロボットと協調して動作する他の設備に異常が生じた場合には、ロボットに異常がなくても、その通常制御の実行が停止されるとともにロボットを停止するための動作がなされる。このようにロボットが停止された場合には、その停止位置からロボットを所定の待機位置まで戻す動作（遡り動作）を行う必要がある。このような場合、例えば、ユーザによる所定の操作や、ロボットが上記したように停止すると自動的に起動するプログラムの動作などにより、遡り指令入力部を介して遡り開始指令が入力される。すると、角度指令生成部は、角度指令記憶部に記憶された回転角度指令を新しいものから古いものへと順に読み出すとともに、それら読み出した回転角度指令を遡り角度指令として出力周期毎に出力する。すなわち、進み動作時における全軸の回転角度のログを逆の順番に並べ替えたものが遡り角度指令となって出力される。

制御部は、各軸の回転角度を上記遡り角度指令に一致させるように、駆動部による各軸の駆動をフィードバック制御する遡り制御を実行する。これにより、ロボットの全ての軸の回転角度が進み動作時と同じ値をとりつつ遡り動作が行われる。言い換えれば、ロボットが、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。つまり、いわゆるプレイバック動作が行われるので、進み動作時と同じ姿勢を保ちつつ遡り動作が行われることになる。このような遡り動作により、ロボットが所定の待機位置まで戻ることで、遡り指令入力部を介して遡り終了指令が入力される。すると、角度指令生成部は、遡り角度指令の出力を停止し、制御部は、遡り制御の実行を停止する。このように、本手段によれば、ロボットを停止位置から所定の待機位置まで戻す遡り動作が行われる際に、ロボットの姿勢が変化することで生じる衝突のリスクを軽減することができる。

また、遡り動作の制御に用いる遡り角度指令は、生成周期毎に生成される全ての回転角度指令を出力周期毎に出力したものである。つまり、本来は比較的短い生成周期毎の回転角度を指令する回転角度指令を比較的長い周期毎に出力するので、遡り動作時における各軸の回転速度、ひいてはロボットの動作速度が通常動作時に比べて遅くなる。このように、遡り動作時には、通常動作時と比べてロボットの動作が遅くなる（スロー動作になる）ので、仮にロボット周辺の他の設備がユーザの予期せぬような動きを行った場合であっても、ユーザはその動きに対して余裕を持って対処することができる。このため、遡り動作が行われる際に初めて生じる衝突のリスクについても軽減することができる。

さて、ロボットが例えば周辺設備に衝突したことで停止した場合、請求項１記載の手段における遡り制御を実行しようとすると以下の問題が生じる。すなわち、衝突により停止した場合、停止時点の各軸の回転角度が回転角度指令（遡り角度指令）とは、かけ離れた値となっていることが多い。このため、正常に遡り制御を実行することは難しい。また、仮に遡り制御がうまく実行できたとしても、それは、周辺の設備に衝突したときの軌跡をロボットが再び通ることになる。つまり、ロボットが周辺の設備に再度衝突してしまう可能性が非常に高くなる。従って、衝突による停止時には、上記遡り制御を実行することなく、ロボットを含む各設備の動作を完全に停止させた上で、衝突が生じた原因を取り除くことが望ましい。そこで、請求項２記載の手段を採用することが考えられる。

請求項２記載の手段によれば、制御部は、遡り開始指令が入力されると、角度指令記憶部に記憶されている最新の回転角度指令と回転角度検出部により検出された現在の回転角度との差分を求める。そして、この差分の絶対値が所定の誤差値以下である場合には遡り制御を実行する。一方、上記差分の絶対値が所定の誤差値を超える場合には遡り制御は実行しない。ロボットが正常に動作している場合であっても、回転角度と回転角度指令とは、種々の誤差の影響によって完全に一致しない場合がある。これに対し、ロボットが衝突により停止した場合には、上記したとおり、回転角度と回転角度指令とは大きくかけ離れた値となる。従って、制御部は、回転角度と回転角度指令との差分の絶対値が所定の誤差値を超えた場合には、ロボットが衝突により停止した状態であると判断し、遡り制御を実行しないようにしている。これにより、遡り動作が行われることによってロボットが再衝突するリスクを抑制することができる。

請求項３記載の手段によれば、ロボットの衝突を検出する衝突検出手段を備えている。ロボットの衝突としては、自動運転中の衝突や、教示（ティーチング）中または動作確認中の衝突などが考えられる。前者の場合、ロボットは最高速度で動作している可能性が高く、衝突時の速度（衝突速度）は比較的速い。このため、ロボットの機構部品や周辺設備が多大な損傷を受ける可能性が高く、その後の自動復旧が困難な状態となる。このような場合には、衝突により停止した時点の各軸の回転角度と回転角度指令との偏差が過大になることが多いため、遡り制御の実行自体が困難となる。

一方、後者の場合、ロボットは低速で動作しているため、その衝突速度は比較的遅い。このため、ロボットの機構部品や周辺設備がほとんど損傷することなく、その後の自動復旧が可能な状態となる。このような場合には、衝突により停止した時点の各軸の回転角度と回転角度指令との偏差が比較的小さいことが多いため、遡り制御を実行することが可能となる。

また、上記した２通りの衝突のうち、衝突速度の速い前者の衝突よりも衝突速度の遅い後者の衝突のほうが発生頻度は高い。なぜなら、自動運転中、ロボットは動作プログラムに基づいて定められた動作を繰り返し行う。そして、その繰り返される動作は、予め周辺設備などに衝突することがないように十分に検証されたものである。このため、自動運転中に衝突する可能性は極めて低い。これに対し、教示中や動作確認中においては、周辺設備との衝突検証を兼ねて教示や動作確認を行うようになっており、その最中に動作軌跡の修正が頻繁に行われることも多い。このため、教示中や動作確認中には、ロボットが毎回同じ動作を繰り返す可能性は低い（ロボットの動作の再現性は低い）。従って、新たな動作軌跡を生成した後や、動作軌跡を修正した後に行われる教示や確認動作において、周辺設備などへの衝突が生じる可能性は高くなる。このように、衝突速度が速く遡り制御を実行することができない重度の衝突よりも、衝突速度が遅く遡り制御を実行可能な軽微な衝突のほうが発生頻度は高い。このようなことから、軽微な衝突によりロボットが停止した場合にも、遡り制御を実行してロボットを待機位置まで移動させることが望ましい。

ただし、軽微な衝突とは言っても、むやみにその衝突回数を増やすことは、精密動作が要求されるロボットにとっては好ましくない。なぜなら、軽微な衝突であっても、その衝突が繰り返されることにより、ロボットを精密に動作させるために行われるキャリブレーションやフレームの精度に狂いが生じる危険性があるからである。

そこで、本手段では、軽微な衝突によりロボットが停止した場合、以下のように再衝突の発生を防止する工夫を加えた上で、遡り制御を実行するようにしている。すなわち、制御部は、衝突検出手段により衝突が検出されると、その衝突が発生した時点（衝突タイミング）と、その衝突時の位置（衝突位置）を特定する。制御部は、これら特定された衝突位置および衝突タイミングを用いて、球状をなす進入禁止空間を設定する。すなわち、進入禁止空間の中心は、衝突位置に設定する。また、進入禁止空間の半径は、衝突タイミングの前後における回転角度指令の出力タイミングのうち、衝突タイミングに近い方の出力タイミングに出力された回転角度指令と、衝突位置との間の距離に相当する長さに設定する。そして、制御部は、衝突が検出された状態で遡り開始指令が入力されると、遡り制御を実行する。

さて、衝突によりロボットが停止された後に遡り制御を実行する際、角度指令生成部が出力する遡り角度指令に基づいてロボットを遡り動作させると、その動作の軌跡は、必ず衝突位置を通過することになる。従って、ロボットが再度衝突する可能性が高くなってしまう。そこで、角度指令生成部は、衝突が検出された状態で遡り開始指令が入力されると、角度指令記憶部から読み出した回転角度指令に基づくロボットの遡り動作の軌跡が、上記進入禁止空間の外郭に最初に重なる外郭位置と最後に重なる外郭位置との間では、動作軌跡が進入禁止空間の外であり、且つ、進入禁止空間の球の表面に沿う軌跡となるように、読み出した回転角度指令を変更し、その変更後の回転角度指令を遡り角度指令として出力する。

このようにすれば、衝突により停止した後、ロボットは、進入禁止空間の外郭に達するまでの間は、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。そして、進入禁止空間の外郭を沿うようにして衝突位置を回避した後、再び進み動作の動作軌跡を逆向きに辿るように遡り動作が行われる。この進入禁止空間の半径は、上記したようにして設定されていることから、非常に小さいものであると言える。このため、進入禁止空間の外郭を沿う動作を行う際、ロボットの肘などの別部分が周辺設備などに衝突するといった新たな衝突が引き起こされる可能性が高まるほど、つまり、新たな別の衝突が発生するようなレベルまでロボットの姿勢が変化することはない。すなわち、進入禁止空間の外郭を沿う遡り動作を行う際におけるロボットの姿勢は、衝突が発生した進み動作時の姿勢とほとんど同じであり、新たな別の衝突を引き起こすような姿勢の変化はない。

このように衝突後の遡り動作では衝突位置を回避するため、完全なプレイバック動作とはならない。しかし、上記したとおり進入禁止空間は小さい。また、元々、ロボットが通過する経路（動作軌跡）は、ロボットの体格を考慮した上で周辺設備などに衝突しないように所定のマージンが設けられている。そして、ロボットの体格は、進入禁止空間よりも遥かに大きいものとなっている。このため、本手段のような進入禁止空間の外郭を沿うように動作軌跡が変更された程度では、ロボットが上記マージンの範囲を越えて周辺設備に衝突する可能性は極めて低い。つまり、上記した若干の動作軌跡の変更により、ロボットが新たな衝突を生じることはほとんどあり得ない。従って、衝突後にロボットが遡り動作する際、その衝突位置を確実に避けるとともに新たな衝突の発生を未然に防止しつつ、進み動作時と同じ姿勢を保ちながら遡り動作することが可能となる。

請求項４記載の手段によれば、請求項３記載の手段と同様に、ロボットの衝突を検出する衝突検出手段を備えている。制御部は、衝突検出手段により衝突が検出されると、請求項３記載の手段と同様に、衝突タイミングおよび衝突位置を特定するとともに、進入禁止空間を設定する。そして、制御部は、衝突が検出された状態で遡り開始指令が入力されると、遡り制御を実行する。また、角度指令生成部は、衝突が検出された状態で遡り開始指令が入力されると、請求項３記載の手段と同様に、読み出した回転角度指令を変更し、その変更後の回転角度指令を遡り角度指令として出力する。このため、本手段によっても、請求項３記載の手段と同様の作用および効果が得られる。

ただし、本手段における進入禁止空間は、請求項３記載の手段における進入禁止空間とは若干異なっている。すなわち、本手段の進入禁止空間の半径は、衝突タイミングの前後における回転角度指令の出力タイミングのうち、衝突タイミングから遠い方の出力タイミングに出力された回転角度指令と、衝突位置との間の距離に相当する長さに設定されている。このため、進入禁止空間の半径（大きさ）が請求項３記載の手段のものと比べて若干大きくなっている。このようにすれば、例えば、ロボットが衝突した設備の表面が、その衝突によって多少変形した場合でも、その変形部分をも避けて遡り動作を行うことが可能になり、設備の変形に伴う衝突を未然に防止できるという効果が得られる。

請求項５記載の手段によれば、角度指令生成部は、遡り開始指令が入力されると、角度指令記憶部に記憶された回転角度指令のうち最新のものを除く回転角度指令を新しいものから古いものへと順に読み出す。角度指令記憶部に記憶された回転角度指令のうち最新のものは、ロボットが停止した時点の現在位置を指令するためのものである。このため、請求項１記載の手段のように、最新のものから順に遡り角度指令とした場合には、実際にロボットが遡り動作を開始するのは、最新のものよりも１つ前の遡り角度指令が出力された時点となる。つまり、遡り動作の開始が若干遅れることになる。これに対し、本手段によれば、そのような若干の遅れもなく、素早く遡り動作を開始することができ、以って、遡り動作全体に要する時間を短縮することができる。

本発明の第１の実施形態を示すロボットシステムの構成を概略的に示す図 ロボットシステムの電気構成図 通常動作時における回転角度指令の生成および出力タイミングを示す図 角度指令記憶部に記憶される回転角度指令の一例を示す図 遡り動作時における回転角度指令の出力タイミングを示す図 モータ制御の内容を等価的に示すブロック図 所定の動作プログラムに従って動作するロボットの状態を示す図 動作プログラムの一例を示す図 本発明の第２の実施形態を示す図１相当図 図６相当図 衝突前後における回転角度指令、回転角度および電流値を示す図 衝突前後におけるロボットの動作状態を示す図 衝突位置を通過する際のロボットの手先およびその動作軌跡示す図 衝突位置付近での進み動作時および遡り動作時の各動作軌跡を示す図

（第１の実施形態）
以下、本発明の第１の実施形態について図１〜図８を参照しながら説明する。
図１は、一般的な産業用ロボットのシステム構成を示している。図１に示すロボットシステム１は、ロボット２と、ロボット２を制御するコントローラ３と、コントローラ３に接続されたティーチングペンダント４とから構成されている。

ロボット２は、例えば４軸の水平多関節型ロボットとして構成されている。ロボット２は、設置面に固定されるベース５と、ベース５上に垂直軸（第１軸）を中心に旋回可能に連結された第１のアーム６と、第１のアーム６の先端部上に垂直軸（第２軸）を中心に旋回可能に連結された第２のアーム７と、第２のアーム７の先端部に上下動可能で且つ回転可能に設けられたシャフト８とから構成されている。なお、このシャフト８を上下動させる際の軸が第３軸に相当し、回転させる際の軸が第４軸に相当する。シャフト８の先端部（下端部）には、フランジ９が位置決めされて着脱可能に取り付けられる。

ベース５、第１のアーム６、第２のアーム７、シャフト８およびフランジ９は、ロボット２のアームとして機能し、アーム先端であるフランジ９には、図示はしないが、エンドエフェクタ（手先）が取り付けられる。ロボット２に設けられる複数の軸はそれぞれに対応して設けられるモータ（図２に符号Ｍを付して示す）により駆動される。各モータの近傍には、それぞれの回転軸の回転角度を検出するための位置検出器（図示せず）が設けられている。

ティーチングペンダント４は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント４は、表面部の中央部に例えば液晶ディスプレイからなる表示部１０を有している。表示部１０には、各種の画面が表示される。表示部１０は、タッチパネルで構成されている。またティーチングペンダント４には、表示部１０の周囲に各種のキースイッチ１１が設けられており、使用者は、キースイッチ１１や上記タッチパネルに設けられるタッチスイッチによって種々の入力操作を行う。キースイッチ１１または上記タッチスイッチには、後述する遡り動作の開始を指示するための遡り開始スイッチが含まれている。ティーチングペンダント４は、ケーブルを経由してコントローラ３に接続され、通信インターフェイスを経由してコントローラ３との間で高速のデータ転送を実行するようになっており、キースイッチ１１等の操作により入力された操作信号等の情報はティーチングペンダント４からコントローラ３へ送信される。

図２は、ロボットシステム１の電気構成を概略的に示すブロック図である。ロボット２には、各軸をそれぞれ駆動するための複数のモータＭ（図２では１つのみ示す）が設けられている。モータＭ（駆動部に相当）は、例えばブラシレスＤＣモータである。コントローラ３には、交流電源２１より供給される交流を整流および平滑して出力する直流電源装置２２、モータＭを駆動するインバータ装置２３、電流検出部２４、回転角度検出部２５、これら各装置の制御などを行う制御部２６、角度指令生成部２７および角度指令記憶部２８が設けられている。

直流電源装置２２は、整流回路３０と平滑用のコンデンサ３１とから構成されている。整流回路３０は、ダイオードをブリッジの形態に接続してなる周知構成のものである。例えば３相２００Ｖの交流電源２１の各相出力は、整流回路３０の交流入力端子に接続されている。整流回路３０の直流出力端子は、それぞれ直流電源線３２、３３に接続されている。これら直流電源線３２、３３間にはコンデンサ３１が接続されている。

インバータ装置２３は、直流電源線３２、３３間に６つのスイッチング素子例えばＩＧＢＴ（図１には２つのみ示す）を三相フルブリッジ接続して構成されたインバータ主回路と、その駆動回路とを６組備えている（図２には１組のみ示す）。ＩＧＢＴのコレクタ・エミッタ間には還流ダイオードが接続されている。また、ＩＧＢＴのゲートには、駆動回路からゲート信号が与えられている。駆動回路は、制御部２６から与えられる指令信号（通電指令Ｓｃ）に基づいてパルス幅変調されたゲート信号を出力して各ＩＧＢＴを駆動する。

制御部２６は、ＣＰＵ、ＲＯＭ、ＲＡＭ、Ｉ／Ｏなどを備えたマイクロコンピュータを主体として構成されている。電流検出部２４は、モータＭに流れる電流を検出する電流検出器（図示せず）からの検出信号を制御部２６に入力可能なデータに変換して出力する。回転角度検出部２５は、モータＭの回転角度を検出する位置検出器（図示せず）からの検出信号を制御部２６に入力可能なデータに変換して出力する。

制御部２６は、電流検出部２４から出力されるデータを元にモータＭに流れる電流の値を取得するとともに、回転角度検出部２５から出力されるデータを元にモータＭの回転角度および回転速度を取得する。詳細は後述するが、制御部２６は、上記取得した回転角度を、角度指令生成部２７から与えられる指令値（回転角度指令ｐｃ）に一致させるようにインバータ装置２３によるモータＭの駆動をフィードバック制御する。なお、このフィードバック制御の際に上記取得した電流値および回転速度も用いられる。

さて、一般に産業用のロボット２は、予めティーチングなどを実施することにより作成される所定の動作プログラムに従って動作する通常動作（進み動作）を行うようになっている。これに加えて、本実施形態のロボット２は、自身の異常以外の原因（設備異常などの原因）で停止した際に、各軸が進み動作時の動作軌跡を逆向きに辿るように動作する遡り動作を行うことが可能となっている。

角度指令生成部２７は、制御部２６の上位の制御装置に相当するものであり、制御部２６と同様にマイクロコンピュータを主体として構成されている。角度指令生成部２７は、通常動作が行われる際には、上記動作プログラムを解釈し、ロボット２に動作プログラムに従った動作を行わせるように各モータＭを制御するための回転角度指令を生成周期毎に生成する。角度指令生成部２７は、生成した回転角度指令の一部を回転角度指令ｐｃとして出力周期毎に制御部２６に出力する。出力周期は、生成周期よりも長くなっている。

図３は、通常動作時における角度指令生成部２７による回転角度指令の生成および出力のタイミングを示している。図３に示すように、角度指令生成部２７は、比較的短い生成周期（本実施形態では１ｍｓ）毎に生成した回転角度指令の一部を、比較的長い出力周期（本実施形態では８ｍｓ）毎に出力する。言い換えると、角度指令生成部２７は、生成周期毎に生成した回転角度指令のうち、大部分は出力しない（間引いている）。本実施形態では、出力する回転角度指令と、出力しない（間引く）回転角度指令との割合は、１：７としている。このような構成とした理由は、以下のとおりである。

すなわち、回転角度指令は、角度指令生成部２７のソフトウエアによる処理によって生成されるため、比較的短い間隔である生成周期毎に生成することができる。一方、モータＭ、インバータ装置２３、回転角度検出部２５などは、ハードウエアによる処理によって、ロボット２の各軸を駆動したり、各軸の回転角度を検出したりする。従って、回転角度を回転角度指令に一致させるための動作間隔は、生成周期に比べて長い間隔にならざるを得ない。このため、回転角度指令の生成周期を動作間隔に合わせて長くするか、あるいは短い生成周期で生成した回転角度指令を動作間隔に合わせて間引いて出力するかのいずれかを採用する必要がある。本実施形態では、後述する遡り制御において短い生成周期で生成した回転角度指令を用いる必要があるため、後者の方法を採用している。

角度指令記憶部２８は、通常動作が行われる期間に角度指令生成部２７が生成した回転角度指令の全て（＝出力された回転角度指令と間引かれた回転角度指令との両方）を順次記憶する。例えば、通常動作が図３に示した期間だけ行われる場合、生成された回転角度指令ｐｃ１’〜ｐｃ１２’をこの順番通りに記憶する。図４は、角度指令記憶部２８に記憶される回転角度指令の一例を示している。図４に示すように、角度指令記憶部２８には、ロボット２の全ての軸（第１軸〜第４軸）を駆動するための各モータＭに対する回転角度指令（通常動作が開始される時点から終了される時点までの回転角度指令）が記憶される。記憶された回転角度指令のうち、最初の回転角度指令がロボット２の待機位置に相当し、最後の回転角度指令がロボット２の停止位置に相当している。すなわち、角度指令記憶部２８に記憶された回転角度指令は、通常動作時における各軸の回転角度（軌跡）の生成周期毎のログであるとも言える。

制御部２６は、通常動作が行われていない期間にて、遡り開始指令および遡り終了指令を入力する機能（遡り指令入力部に相当）を備えている。つまり、制御部２６は、通常動作が行われている期間にあっては、遡り開始指令または遡り終了指令が与えられても、それらの指令を無視する。遡り開始指令は、ロボット２が設備異常などの原因で停止された場合に与えられる。本実施形態では、遡り開始指令は、ティーチングペンダント４に設けられた遡り開始スイッチが操作されることで与えられる。なお、遡り開始指令は、上記スイッチの手動操作によって与えられる構成に限らずともよく、自動的に与えられる構成としてもよい。例えば、遡り開始指令を出力するためのプログラムを実装するとともに、ロボット２が上記原因で停止した場合に、そのプログラムが自動的に実行されるように設定すればよい。また、遡り終了指令は、遡り動作によってロボット２が所定の待機位置まで移動した時に与えられる。

制御部２６は、遡り開始指令が入力されると、角度指令記憶部２８に記憶されている最新の各回転角度指令（図４におけるＮｏ．Ｎの回転角度指令）と、回転角度検出部２５を介して取得した各モータＭの現在の回転角度を用いて、遡り可否判断処理を行う。この遡り可否判断処理は、ロボット２の最新の位置、つまりロボット２の停止位置が、最新の指令位置と一致しているか否かを確認するものである（詳細は後述する）。制御部２６は、遡り可否判断処理によりロボット２の停止位置が指令位置と一致していると判断した場合、角度指令生成部２７に遡り開始指令を出力し、ロボット２に対し遡り動作を行わせる遡り制御を実行する。また、制御部２６は、停止位置が指令位置と一致しないと判断した場合、角度指令生成部２７に遡り開始指令を出力せず、上記遡り制御を実行しない。制御部２６は、遡り終了指令が与えられると、その遡り終了指令を角度指令生成部２７に出力し、上記遡り制御の実行を終了する。

角度指令生成部２７は、通常動作を行っていない期間において、制御部２６から遡り開始指令が与えられると、角度指令記憶部２８に記憶されている回転角度指令を記憶された時期が２番目に新しいもの（図４におけるＮｏ．Ｎ−１）から最も古いもの（図４におけるＮｏ．１）へと順に読み出す。角度指令生成部２７は、読み出した全ての回転角度指令を、読み出した順に、つまり、記憶された時期が新しいものから古いものへと順に、回転角度指令ｐｃとして出力する。この場合における回転角度指令ｐｃが遡り角度指令に相当する。図５は、遡り動作時における角度指令生成部２７による回転角度指令ｐｃの出力のタイミングを示している。図５に示すように、角度指令生成部２７は、読み出した回転角度指令の全てを、出力周期毎に出力する。従って、比較的短い生成周期毎に生成された回転角度指令が、間引かれることなく、その生成された順番とは逆の順番で、比較的長い出力周期毎に出力されることになる。そして、角度指令生成部２７は、制御部２６から遡り終了指令が与えられると、上記遡り角度指令の出力動作を停止する。

図６は、ロボットシステム１におけるモータ制御の内容を等価的に示したブロック図である。図６に示すように、制御部２６は、位置制御部４１、速度制御部４２および電流制御部４３を備えている。なお、図６では、１つのモータＭの制御に係る構成のみを示しているが、実際には全てのモータＭのそれぞれに対応して同様の構成が設けられている。

位置制御部４１は、角度指令生成部２７から与えられる回転角度指令ｐｃに対する現在の回転角度ｐ＊の偏差を求める減算器４４と、この減算器４４の出力（偏差）をゼロに近づけるように速度指令ｖｃを出力する位置制御アンプ４５とから構成されている。この位置制御アンプ４５のゲインはＫｐとなっている。

速度制御部４２は、微分器４６、減算器４７および速度制御アンプ４８により構成されている。微分器４６は、現在の回転角度ｐ＊を微分して現在の回転速度ｖ＊に変換する。減算器４７は、速度指令ｖｃに対する現在の回転速度ｖ＊の偏差を求める。速度制御アンプ４８は、この減算器４７の出力（偏差）をゼロに近づけるように電流指令ｉｃを出力する。この速度制御アンプ４８のゲインはＫｖとなっている。

電流制御部４３は、電流指令ｉｃに対する現在のモータＭに流れる電流ｉ＊の偏差を求める減算器４９と、この減算器４９の出力（偏差）をゼロに近づけるようにインバータ装置２３に対する指令信号（通電指令Ｓｃ）を出力する電流制御アンプ５０とから構成されている。この電流制御アンプ５０のゲインはＫｉとなっている。このような構成により、制御部２６は、電流フィードバック制御、速度フィードバック制御および位置フィードバック制御を行い、モータＭの駆動をフィードバック制御してロボット２のアームの動作制御を行う。

次に、上記構成の作用について図７および図８も参照して説明する。
図７は、所定の動作プログラムに従って動作するロボットの状態を示している。図８は、上記所定の動作プログラムの概要を示している。図８に示す動作プログラム（ｐｒｏ２）の概要は以下のとおりである。すなわち、０７行目は、ロボット２の手先を位置Ｐ１（待機位置）まで動作させる命令である。０９行目は、ロボット２の動作に関係する部品の品番に応じて１０行目または１５行目に移行させる分岐命令である。１０行目に移行した場合には、１１〜１３行目の命令が実行される。１１〜１３行目は、ロボット２の手先を位置Ｐ２、位置Ｐ３、位置Ｐ４という順に移動させる命令である。一方、１５行目に移行した場合には、１６行目以降の命令が実行される。

図７では、０９行目の分岐命令において１０行目に移行した場合のロボット２の状態を示している。従って、この動作プログラムに従った通常動作が行われる際には、ロボット２の手先は、位置Ｐ１→位置Ｐ２→位置Ｐ３→位置Ｐ４という順に移動する。なお、図７では、手先が位置Ｐ１にあるときのロボット２を実線で示し、手先がそれぞれ位置Ｐ２、Ｐ３、Ｐ４にあるときのロボット２を二点鎖線で示している。

この際、角度指令生成部２７は、ロボット２が位置Ｐ１から位置Ｐ４まで順に移動するように各モータＭを駆動させるための回転角度指令を生成周期毎に生成するとともに、その一部を出力周期毎に出力する。従って、角度指令生成部２７は、２５個（＝１×４＋７×３）の回転角度指令を生成し、そのうちの４個の回転角度指令（位置Ｐ１、Ｐ２、Ｐ３、Ｐ４をそれぞれ指令するもの）を出力する。この際、角度指令記憶部２８は、角度指令生成部２７が生成した２５個の回転角度指令を順次記憶している。制御部２６は、角度指令生成部２７から与えられる回転角度指令ｐｃに基づいて、モータＭの駆動をフィードバック制御する（通常制御）。

このような通常動作が行われている際、例えば設備などの異常によりロボット２が位置Ｐ４において停止された場合、本実施形態では、以下のような手順で遡り動作を実行することが可能となっている。すなわち、ティーチングペンダント４に設けられた遡り開始スイッチが操作されると、制御部２６は、遡り可否判断処理を実行する。この遡り可否判断処理において、制御部２６は、角度指令記憶部２８に記憶された最新の回転角度指令（この場合、位置Ｐ４を示す指令）と、現在のモータＭの回転角度との差分を各軸について求める。制御部２６は、それら差分の絶対値のうち最も大きい値が所定の誤差値以下である場合には、角度指令生成部２７に対し遡り開始指令を出力する。この誤差値は、ロボット２が正常に動作している際に生じる回転角度指令と回転角度との誤差（回転角度指令に対する回転角度の偏差）よりも十分に大きい値とすればよい。

角度指令生成部２７は、遡り開始指令が与えられると、角度指令記憶部２８に記憶されている２５個の回転角度指令のうち、位置Ｐ４よりも１つ前（位置Ｐ３寄り）の位置を示す指令から最も古いもの（位置Ｐ１＝待機位置を示す指令）へと順に読み出す。角度指令生成部２７は、読み出した２４個の回転角度指令を、読み出した順に、つまり、記憶された時期が新しいものから古いものへと順に、回転角度指令ｐｃ（遡り角度指令）として出力周期毎に出力する。

制御部２６は、角度指令生成部２７から与えられる回転角度指令ｐｃ（遡り角度指令）に基づいて、モータＭの駆動をフィードバック制御する（遡り制御）。この結果、ロボット２の手先は、位置Ｐ４→位置Ｐ３→位置Ｐ２→位置Ｐ１（待機位置）という順に移動する。しかも、この際、各軸が通常動作（進み動作）時と同じ位置を通過しながら、言い換えると、ロボット２が通常動作時と同じ姿勢を保ちつつ移動する。さらに、１ｍｓの生成周期毎に生成した回転角度指令が８ｍｓの出力周期毎に遡り角度指令として出力されるため、遡り動作時のロボット２の移動速度は、通常動作時のロボット２の移動速度の約１／８となる。

ロボット２の手先が位置Ｐ１（待機位置）に移動したことにより、制御部２６に遡り終了指令が与えられる。これに伴い、制御部２６は、角度指令生成部２７に対し遡り終了指令を出力するとともに遡り制御の実行を終了する。また、角度指令生成部２７は、制御部２６から遡り終了指令が与えられたことにより、遡り指令の出力動作を停止する。

以上説明したように、本実施形態によれば次のような効果が得られる。
ロボット２の遡り動作が行われる際、角度指令生成部２７は、角度指令記憶部２８に記憶されている回転角度指令を新しいものから古いものへと順に読み出すとともに、それら読み出した回転角度指令を遡り角度指令として出力周期毎に出力する。すなわち、進み動作時における各軸の回転角度のログを逆の順番に並べ替えたものが遡り角度指令となって出力される。制御部２６は、各軸の回転角度を遡り角度指令に一致させるように、モータＭによる各軸の駆動をフィードバック制御する遡り制御を実行する。これにより、ロボット２の全ての軸の回転角度が進み動作時と同じ値をとりつつ遡り動作が行われる。言い換えれば、ロボット２が、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。つまり、いわゆるプレイバック動作が行われるので、進み動作時と同じ姿勢を保ちつつ遡り動作が行われることになる。従って、ロボット２を停止位置から所定の待機位置まで戻す遡り動作が行われる際において、ロボット２の姿勢が進み動作と比較して変化することで生じる衝突のリスクを軽減することができる。

このような本実施形態に対し、動作プログラムを停止位置への移動を命令する行から、所定の待機位置への移動を命令する行まで単純に遡って実行することにより、ロボット２を停止位置から所定の待機位置まで戻すという手法を採用することも考えられる。しかしながら、上記手法は以下の理由から採用することはできない。すなわち、動作プログラムには、条件毎に以降の動作が変化する分岐命令（例えば、図７の０９行目）などが存在する。動作プログラムを単に逆に実行することを考えた場合、このような分岐部分において、どのように分岐させるべきかを決定することができない。従って、動作プログラムを逆に実行することができない可能性が高い。さらに、ロボット２の通常動作（進み動作９においては、所定の動作点から次の動作点に移動する際にパス動作が行われる場合がある。本実施形態によれば、進み動作時における各軸の回転角度のログを逆の順番に並べ替えたものが遡り角度指令として出力されるため、パス動作に伴う各軸の回転角度についても正確に再現される。一方、単純に動作プログラムを逆に実行するだけでは、このようなパス動作に伴う各軸の回転角度を求めることが不可能である。このようなことから、動作プログラムを単に逆に実行するだけでは、本実施形態と同様の効果は得られない。

また、遡り角度指令は、生成周期毎に生成される全ての回転角度指令を長い出力周期毎に出力したものである。つまり、本来は比較的短い生成周期毎の回転角度を指令する回転角度指令を比較的長い周期毎に出力するので、遡り動作時における各軸の回転速度、ひいてはロボット２の動作速度が通常動作時に比べて遅くなる。このように、遡り動作時には、通常動作時と比べてロボット２の動作が遅くなる（スロー動作になる）ので、仮にロボット２周辺の他の設備がユーザの予期せぬような動きを行った場合であっても、ユーザはその動きに対して余裕を持って対処することができる。このため、遡り動作が行われる際に初めて生じる衝突のリスクについても軽減することができる。

さて、ロボット２が例えば周辺設備に衝突したことで停止した場合、停止時点の各軸の回転角度が回転角度指令とは、かけ離れた値となっていることが多い。このため、正常に遡り制御を実行することは難しい。また、仮に遡り制御がうまく実行できたとしても、それは、周辺の設備に衝突したときの軌跡をロボット２が再び通ることになる。つまり、ロボット２が周辺の設備に再度衝突してしまう可能性が非常に高くなる。従って、衝突による停止時には、遡り制御を実行することなく、ロボット２を含む各設備の動作を完全に停止させた上で、衝突が生じた原因を取り除くことが望ましい。そこで、本実施形態の制御部２６は、遡り開始指令が与えられると、以下の遡り可否判断処理を実行する。

すなわち、制御部２６は、角度指令記憶部２８に記憶された最新の回転角度指令と現在のモータＭの回転角度との差分を各軸について求め、それら差分の絶対値のうち最も大きい値が所定の誤差値以下である場合には、遡り制御を実行する。一方、差分の絶対値のうち最も大きい値が所定の誤差値以下である場合には遡り制御を実行しない。ロボット２が正常に動作している場合でも、回転角度と回転角度指令とは、種々の誤差の影響によって完全に一致しない場合がある。これに対し、ロボット２が衝突により停止した場合には、回転角度と回転角度指令とは大きくかけ離れた値となる。このことから、制御部２６は、回転角度と回転角度指令との差分の絶対値が所定の誤差値を超えた場合には、ロボット２が衝突により停止した状態であると判断し、遡り制御を実行しないようにしている。これにより、遡り動作が行われることによってロボット２が再衝突するリスクを抑制することができる。

また、角度指令生成部２７は、遡り開始指令が与えられると、角度指令記憶部２８に記憶されている回転角度指令を記憶された時期が２番目に新しいものから最も古いものへと順に読み出す。角度指令記憶部２８に記憶された回転角度指令のうち最新のものは、ロボット２が停止した時点の現在位置を指令するためのものである。このため、最新のものから順に遡り角度指令とした場合には、実際にロボット２が遡り動作を開始するのは、最新のものよりも１つ前の遡り角度指令が出力された時点となる。つまり、遡り動作の開始が若干遅れることになる。これに対し、本実施形態の構成によれば、そのような若干の遅れもなく、素早く遡り動作を開始することができ、以って、遡り動作全体に要する時間を短縮することができる。

（第２の実施形態）
以下、本発明の第２の実施形態について図９〜図１４を参照しながら説明する。
図９および図１０は、それぞれ第１の実施形態における図１および図６相当図であり、第１の実施形態と同一部分には同一符号を付して説明を省略する。図９に示す本実施形態のロボットシステム６１は、図１に示した第１の実施形態のロボットシステム１に対し、ロボット２に代えてロボット６２を備えている点が異なる。

ロボット６２は、例えば６軸の垂直多関節型ロボットとして構成されている。ロボット６２は、ベース６３と、このベース６３に水平方向に回転可能に支持されたショルダ部６４と、このショルダ部６４に上下方向に回転可能に支持された下アーム６５と、この下アーム６５に上下方向に回転可能に支持された第１の上アーム６６と、この第１の上アーム６６に捻り回転可能に支持された第２の上アーム６７と、この第２の上アーム６７に上下方向に回転可能に支持された手首６８と、この手首６８に捻り回転可能に支持されたフランジ６９とから構成されている。

ベース６３、ショルダ部６４、下アーム６５、第１の上アーム６６、第２の上アーム６７、手首６８およびフランジ６９は、ロボット６２のアームとして機能し、アーム先端であるフランジ６９には、図示はしないが、エンドエフェクタ（手先）が取り付けられる。ロボット６２に設けられる複数の軸は、第１の実施形態のロボット２と同様、それぞれに対応して設けられるモータ（図２に符号Ｍを付して示す）により駆動される。また、各モータの近傍には、それぞれの回転軸の回転位置を検出するための位置検出器（図示せず）が設けられている。

図示はしないが、本実施形態の角度指令記憶部２８は、通常動作が行われる期間において、生成周期（例えば１ｍｓ）毎に電流ｉ＊の値（または電流指令ｉｃの値）を順次記憶するようになっている。すなわち、本実施形態の角度指令記憶部２８には、生成周期毎の電流値のログデータが記憶される。

図１０は、ロボットシステム６１におけるモータ制御の内容を等価的に示したブロック図である。図１０に示すように、本実施形態の制御部７１は、第１の実施形態の制御部２６が備える構成に加え、さらに、トルク値推定部７２、発生トルク値演算部７３および外乱トルク値演算部７４を備えている。

トルク値推定部７２（トルク値推定手段に相当）は、ロボット６２が所定の動作プログラムに基づいて行う動作の開始から終了までの間にモータＭに発生させる必要があるトルクの値（推定トルク値）を、回転角度指令ｐｃを用いて一括して推定する。この推定方法としては、例えば運動方程式を用いて、ロボット６２の軸を所定の回転位置からその次の回転位置まで回転させるためにモータＭに発生させる必要があるトルク値を推定するという方法が挙げられる。なお、トルク値推定部７２は、ロボット６２が現在の指令位置からその次の指令位置まで移動するためにモータＭに発生させる必要があるトルク値を逐次推定するように構成してもよい。発生トルク値演算部７３（トルク値演算手段に相当）は、モータＭに流れる現在の電流ｉ＊に対し、モータＭのトルク定数を乗算することにより、モータＭが発生する現在のトルクの値（発生トルク値）を求める。

外乱トルク値演算部７４（外乱演算手段に相当）は、推定トルク値および発生トルク値の差分から、モータＭが駆動する対象となる軸に外部から加わるトルク値（外乱負荷トルク値）を求める。すなわち、外部からロボット６２に対して負荷が加えられない限り、ロボット６２の各軸には、回転角度指令ｐｃに従いモータＭが発生するトルクがそれぞれ生じることになるので、推定トルク値と発生トルク値とは一致する。これに対し、衝突が発生したことなどにより外部からロボット６２に対して所定の負荷が加えられると、推定トルク値と発生トルク値との間には、その負荷に応じた分だけの差が生じる。そこで、外乱トルク値演算部７４は、推定トルク値Ｔｅおよび発生トルク値Ｔｇを用いて下記（１）式に基づき外乱負荷トルク値Ｔｄを演算する。
Ｔｄ＝｜Ｔｇ−Ｔｅ｜ …（１）

制御部７１（衝突判断手段に相当）は、各軸に加わる外乱負荷トルク値Ｔｄに基づいてロボット６２に対する衝突を検出する。すなわち、制御部７１は、所定の衝突検出周期毎に各軸に加わる外乱負荷トルク値Ｔｄを求め、少なくともいずれか一つの軸に加わる外乱負荷トルク値Ｔｄが予め定めたしきい値Ｔthを超えると衝突が発生したと判断する。しきい値Ｔthは、予めユーザによる設定が可能となっており、例えば１〜５００［ｋｇｆ・ｃｍ］の範囲の値（初期値は１００［ｋｇｆ・ｃｍ］）としている。なお、しきい値Ｔthの設定値を小さくすると衝突検出の感度を高められるが、種々の演算誤差などに起因する誤検出の可能性も高まる。一方、しきい値Ｔthの設定値を大きくすると衝突検出の感度は低くなるが、誤検出の可能性が低減する。しきい値Ｔthは、このようなトレードオフの関係を踏まえた上で適用するシステムとして最適な値に設定すればよい。

このように、本実施形態のロボットシステム６１では、ロボット６２が周辺機器と衝突した際、センサレスで瞬時に衝突を検出することを可能としている。すなわち、本実施形態では、制御部７１が衝突検出手段として機能する。また、制御部７１は、ロボット６２の衝突を検出すると、ロボット６２の各アームの動作を直ちに停止させる停止制御を行うようになっている。制御部７１は、その停止制御において、ロボット６２の各軸を駆動するモータＭに逆転トルクを発生させつつ、その後の移動軌跡に沿ってアームの動作を停止させる。

また、制御部７１は、ロボット６２の衝突を検出すると、衝突時の各軸の回転角度に基づいて衝突位置を特定する。図１１は、衝突前後の所定の軸における回転角度指令ｐｃ、回転角度ｐ＊および電流ｉ＊を示している。図１１における縦向きの実線は、動作指令出力ポイント（サンプリングタイミング）を示している。その動作指令出力ポイントの間隔（ロボット動作指令間隔）は、本実施形態では出力周期（例えば８ｍｓ）と同じになっている。また、図１１における縦向きの点線は、上記した衝突検出動作の実行タイミングを示している。その実行タイミングの間隔（衝突検出周期）は、出力周期よりも短くなっており、本実施形態では生成周期（例えば１ｍｓ）と同じになっている。

時刻ｔ１の時点において、ロボット６２に対する衝突が生じた場合、その後も回転角度指令ｐｃは、次の動作指令出力ポイントの値（目標値）に向けて増加し続ける。一方、検出される実際の回転角度ｐ＊（エンコーダ値）は、衝突が生じているため、その値は変化せずほぼ一定を保っている。このため、回転角度指令ｐｃに対する現在の回転角度ｐ＊の偏差（位置偏差）が急激に増加し、それに伴って電流ｉ＊も急激に増加する。その後、時刻ｔ２の時点において、前述した衝突検出動作により衝突が検出されると、回転角度指令ｐｃを回転角度ｐ＊に合わせるようにしてロボット６２が停止制御される。この際、電流ｉ＊もゼロに向けて低下し、ロボット６２が完全に停止した時点（時刻ｔ３）でゼロになる。

制御部７１は、衝突を検出してロボット６２を停止させた後、角度指令記憶部２８に記憶された電流値のログデータを、停止させた時点の値から遡り、その値が急激に増加し始めたポイントを探す。図１１の場合、時刻ｔ１の時点において電流ｉ＊が急増し始めることになる。このため、制御部７１は、その時刻ｔ１を衝突が生じたタイミング（衝突タイミング）として特定するとともに、時刻ｔ１における各軸の回転角度ｐ＊に基づいて衝突位置を特定する。この衝突位置の特定方法としては、例えば、加わった外乱負荷トルク値Ｔｄが最も大きい軸に対して衝突が発生したと判断して衝突位置を特定する方法が挙げられる。その場合、各軸の外乱負荷トルク値Ｔｄから、ヤコビ行列を用いてロボット６２の手先に加わる外乱負荷トルク値を求め、手先への衝突も特定することが可能である。

また、本実施形態では、制御部７１は、衝突検出後においても遡り制御を実行することが可能となっている。制御部７１は、衝突を検出すると、衝突後の遡り制御で用いるための進入禁止空間を設定する。進入禁止空間は球状をなしており、その中心は、特定した衝突位置（時刻ｔ１の位置）に設定される。そして、例えば、特定した衝突位置がロボット６２の手先である場合、進入禁止空間の半径は、衝突タイミングの前後における動作指令出力ポイントのうち、衝突タイミングに近い方のタイミング、つまり、衝突タイミングの直前の動作指令出力ポイント（時刻ｔａ）時点の手先の位置と、衝突時点の手先の位置（衝突位置）との間の距離に相当する長さに設定される。なお、時刻ｔａ時点の手先の位置は、時刻ｔａの時点において出力された各軸の回転角度指令に基づいて求められる。

次に、本実施形態の作用および効果について図１２〜図１４も参照して説明する。
さて、ロボット６２の衝突としては、自動運転中の衝突や、教示中または動作確認中の衝突などが考えられる。前者の場合、ロボット６２は最高速度で動作している可能性が高く、衝突速度は比較的速い。このため、ロボットの機構部品や周辺設備が多大な損傷を受ける可能性が高く、その後の自動復旧が困難な状態となる。このような場合には、衝突により停止した時点の位置偏差が過大になることが多いため、遡り制御の実行自体が困難となる。この場合には、衝突検出以前に、偏差過大、過電流、加速度限界値オーバーなどのエラーが発生してロボット６２が緊急停止されるため、本実施形態のロボットシステム６１においても遡り制御は実行されない。

一方、後者の場合、ロボット６２は低速で動作しているため、その衝突速度は比較的遅い。このため、ロボットの機構部品や周辺設備がほとんど損傷することなく、その後の自動復旧が可能な状態となる。このような場合には、衝突により停止した時点の位置偏差が比較的小さいことが多いため、遡り制御を実行することが可能となる。また、上記各衝突のうち、衝突速度の速い前者の衝突よりも衝突速度の遅い後者の衝突のほうが発生頻度は高い。なぜなら、自動運転中、ロボット６２は動作プログラムに基づいて定められた動作を繰り返し行う。そして、その繰り返される動作は、予め周辺設備などに衝突することがないように十分に検証されたものである。このため、自動運転中に衝突する可能性は極めて低い。

これに対し、教示中や動作確認中においては、周辺設備との衝突検証を兼ねて教示や動作確認を行うようになっており、その最中に動作軌跡の修正が頻繁に行われることも多い。このため、教示中や動作確認中には、ロボット６２の動作の再現性は低い。従って、新たな動作軌跡を生成した後や、動作軌跡を修正した後に行われる教示や確認動作において、周辺設備などへの衝突が生じる可能性は高くなる。このように、衝突速度が速く遡り制御を実行することができない重度の衝突よりも、衝突速度が遅く遡り制御を実行可能な軽微な衝突のほうが発生頻度は高い。このようなことから、軽微な衝突によりロボット６２が停止した場合にも、遡り制御を実行してロボットを待機位置まで移動させることが望ましい。

ただし、軽微な衝突とは言っても、むやみにその衝突回数を増やすことは、精密動作が要求されるロボット６２にとっては好ましくない。なぜなら、軽微な衝突であっても、その衝突が繰り返されることにより、ロボット６２を精密に動作させるために行われるキャリブレーションやフレームの精度に狂いが生じる危険性があるからである。そこで、本実施形態では、軽微な衝突によりロボット６２が停止した場合、以下のように再衝突の発生を防止する工夫を加えた上で、遡り制御を実行するようにしている。

図１２は、例えば教示中または動作確認中における衝突前後のロボットの動作状態を示している。まず、図１２の（１）に示すように、ロボット６２の手先は、設備８１から少し離れた位置から設備８１の方向に向けて移動している。そして、図１２の（２）に示すように、ロボット６２の手先が設備８１に衝突する。この際、制御部７１は、その衝突を検出し、ロボット６２の停止制御を開始する。その後、図１２の（３）に示すように、ロボット６２の手先は、衝突の反動で設備８１から僅かに離れた位置まで移動しながら上記停止制御により停止する。

制御部７１は、このように衝突発生に伴ってロボット６２が停止した状態において、遡り開始指令が入力されると、遡り制御を実行する。ただし、衝突によりロボット６２が停止された後に遡り制御を実行する際、第１の実施形態の遡り制御と同じように角度指令生成部２７が出力する回転角度指令ｐｃ（遡り角度指令）に基づいてロボット６２を遡り動作させると、その動作の軌跡は、必ず衝突位置を通過することになる。従って、ロボット６２が再度衝突する可能性が高くなってしまう。

そこで、本実施形態の角度指令生成部２７は、衝突が検出された状態で遡り開始指令が入力されると、角度指令記憶部２８から読み出した回転角度指令ｐｃに以下のような変更を加える。すなわち、本実施形態の角度指令生成部２７は、衝突が検出された状態で遡り指令が入力されると、角度指令記憶部２８から読み出した回転角度指令ｐｃに基づくロボット６２の遡り動作の軌跡が、制御部７１が設定した進入禁止空間の球の外郭に最初に重なる外郭位置と最後に重なる外郭位置との間では、その動作軌跡が進入禁止空間の外であり、且つ、進入禁止空間の球の表面に沿う軌跡となるように、読み出した回転角度指令ｐｃを変更し、その変更後の回転角度指令ｐｃを遡り角度指令として出力する。要するに、本実施形態では、進入禁止空間の内部を通過する動作軌跡となる回転角度指令ｐｃを遡り角度指令として使用することなく、その代わりに、進入禁止空間の球の表面を這うような動作軌跡となる回転角度指令ｐｃを新たに生成して遡り角度指令として使用する。

このように角度指令生成部２７による回転角度指令ｐｃの変更に伴って動作軌跡が変更された状態で、制御部７１による遡り制御が実行されると、ロボット６２の手先は、図１２の（３）に示した停止位置から、衝突位置を経由し、図１２の（１）に示した位置に移動することになる。ただし、遡り制御において衝突位置を経由する際には、図１３に示すように、ロボット６２の手先が衝突位置を回避するように遡り動作が行われる。以下、遡り動作時の衝突位置の回避動作について、図１４を参照して詳しく説明する。

図１４は、衝突位置付近における進み動作時の手先の動作軌跡と、衝突後の遡り動作時の手先の動作軌跡とを示している。図１４では、進み動作時の動作軌跡は実線で示し、遡り動作時の動作軌跡は一点鎖線で示している。また、衝突位置は符号ａを付した点で示し、設定した進入禁止空間は破線で表すとともに符号ｂを付している。さらに、進み動作時の動作軌跡が、最初に進入禁止空間ｂに重なる位置（脱出外郭位置）は符号ｘ１を付した点で示し、最後に進入禁止空間ｂに重なる位置（進入外郭位置）は符号ｘ２を付した点で示している。なお、図１４に示す時刻ｔ１、ｔａ、ｔｂは、いずれも図１１に同じ符号を付して示した各時刻に対応している。

図１４に示すように、衝突により停止した後、ロボット６２の手先は、その停止位置から進入外郭位置ｘ２までの区間では、進み動作時の動作軌跡を逆向きに辿るように遡り動作が行われる。進入外郭位置ｘ２から脱出外郭位置ｘ１までの区間では、進入禁止空間ｂの球の表面（外郭）に沿うようにして衝突位置ａを回避する。そして、脱出外郭位置ｘ１以降の区間では、再び進み動作の動作軌跡を逆向きに辿るように遡り動作が行われることになる。なお、図１４では、説明の都合上、停止位置から進入外郭位置ｘ２までの区間および脱出外郭位置ｘ１以降の区間において、進み動作時および遡り動作時の各動作軌跡を互いに若干ずらして描いているが、実際には各動作軌跡は一致している。

進入禁止空間ｂの半径は、前述したように設定されていることから、非常に小さいものであると言える。このため、進入禁止空間ｂの外郭を沿う遡り動作を行う際、ロボット６２の肘などの手先とは別の部分が周辺設備などに衝突するといった新たな衝突が引き起こされる可能性が高まるほど、つまり、新たな別の衝突が発生するようなレベルまでロボット６２の姿勢が変化することはない。すなわち、進入禁止空間ｂの外郭を沿う遡り動作を行う際におけるロボット６２の姿勢は、衝突が発生した進み動作時の姿勢とほとんど同じであり、新たな別の衝突を引き起こすような姿勢の変化はない。

このように衝突後の遡り動作では衝突位置ａを回避するため、完全なプレイバック動作とはならない。しかし、上記したとおり進入禁止空間ｂは小さい。また、元々、ロボット６２が通過する経路（動作軌跡）は、ロボット６２の体格を考慮した上で周辺設備などに衝突しないように所定のマージンが設けられている。そして、ロボット６２の体格は、進入禁止空間ｂよりも遥かに大きい。このため、進入禁止空間ｂの外郭を沿うように動作軌跡が変更された程度では、ロボット６２が上記マージンの範囲を越えて周辺設備に衝突する可能性は極めて低い。つまり、遡り制御における若干の動作軌跡の変更により、ロボット６２が新たな衝突を生じることはほとんどあり得ない。従って、衝突後にロボット６２が遡り動作する際、その衝突位置ａを確実に避けるとともに新たな衝突の発生を未然に防止しつつ、進み動作時と同じ姿勢を保ちながら遡り動作することが可能となる。

（その他の実施形態）
なお、本発明は上記し且つ図面に記載した各実施形態に限定されるものではなく、次のような変形または拡張が可能である。
角度指令生成部２７は、遡り開始指令が入力されたとき、角度指令記憶部２８に記憶されている回転角度指令を記憶された時期が３番目以降に新しいものから古いものへと順に読み出してもよい。また、角度指令生成部２７は、遡り開始指令が入力されたとき、角度指令記憶部２８に記憶されている回転角度指令を記憶された時期が最も新しいものから最も古いものへと順に読み出してもよい。
生成周期および出力周期は、遡り動作時のロボット２の動作速度を必要な速度に設定するように適宜変更可能である。すなわち、角度指令生成部２７が出力する回転角度指令および出力しない（間引く）回転角度指令の割合（１：ｘ）は、「ｘ＞１」という条件を満たす範囲内で適宜変更可能である。
遡り可否判断処理において、制御部２６は、角度指令記憶部２８に記憶された最新の回転角度指令と現在のモータＭの回転角度との差分を、所定の軸について求めるように変更してもよい。また、制御部２６は、ロボット２が衝突により停止した際に遡り開始指令が与えられないという条件であれば、遡り可否判断処理を行わなくてもよい。

遡り制御の実行を終了する待機位置としては、角度指令記憶部２８に記憶された最初の回転角度指令が表す位置に限らずともよく、任意の位置でよい。この任意の位置として、例えば、動作プログラムにおけるmove命令（所定の作業をするために手先を動かすなどの命令）があるところの位置（またはその前後の位置）にするとよい。move命令の位置およびその前後の位置では、手先に対しワークを掴むなどの指令が出されている可能性が高い。ロボット２がワークを掴んだまま、遡り動作が継続されると、そのワークが周辺設備（例えば狭い通路など）に衝突してしまう可能性がある。そこで、move命令の前後で遡り動作を終了させてロボット２を待機させることで、上記衝突が発生する可能性を低減することができる。

第２の実施形態において、制御部７１が設定する進入禁止空間は、衝突発生後にロボット６２を遡り動作させる際に、ロボット６２が最初に衝突した位置に再衝突することを回避するとともに、新たな別の衝突の発生を防止できるような空間であれば適宜変更可能である。例えば、特定した衝突位置がロボット６２の手先である場合、進入禁止空間の半径は、衝突タイミングの前後における動作指令出力ポイントのうち、衝突タイミングから遠い方のタイミング、つまり、衝突タイミングの直後の動作指令出力ポイント（図１１、図１４の時刻ｔｂ）時点の手先の位置と、衝突時点の手先の位置（衝突位置）との間の距離に相当する長さに設定してもよい。なお、時刻ｔｂ時点の手先の位置は、時刻ｔｂの時点において出力された各軸の回転角度指令に基づいて求められる。このように設定した進入禁止空間の半径は、第２の実施形態で設定したものと比べて若干大きくなる。このため、例えば、ロボット６２が衝突した設備の表面が、その衝突によって多少変形した場合でも、その変形部分をも避けて遡り動作を行うことが可能になり、設備の変形に伴う衝突を未然に防止できるという効果が得られる。

また、ロボット６２の精度誤差（例えば、±０．０７５ｍｍ）に基づいて進入禁止空間の半径を設定してもよい。その場合、安全サイドへのマージンをとるため、精度誤差をＮ倍（例えば、Ｎ＝２）したものを、進入禁止空間の半径に設定するとよい。または、衝突時の速度（衝突速度）に基づいて進入禁止空間の半径を設定してもよい。その場合、安全サイドへのマージンをとるため、衝突検出周期（例えば１ｍｓ）の間に衝突速度で移動する距離をＮ倍（例えば、Ｎ＝２）したものを、進入禁止空間の半径に設定するとよい。あるいは、ロボット６２の精度誤差に基づいて設定した半径と、衝突速度に基づいて設定した半径とのうち、大きい方を進入禁止空間の半径に設定するようにしてもよい。

上記衝突速度は、例えば衝突時の各軸の回転速度に基づいて求めればよい。すなわち、制御部７１は、衝突を検出した後、衝突タイミングの直前の動作指令出力ポイント（図１１の時刻ｔａ）と、その一つ前の動作指令出力ポイント（図１１の時刻ｔｃ）との間の移動量、つまり、直前の回転角度指令ｐｃと、その一つ前の回転角度指令ｐｃとの差から、衝突直前の出力周期における移動距離（移動量）を求める。そして、制御部７１は、出力周期の値（例えば８ｍｓ）と求めた移動距離とから、衝突直前の出力周期における回転速度を算出し、その値を衝突速度として取得すればよい。このようにすれば、衝突が生じた直前の時点の速度、ひいては衝突時点の速度を正確に求めることが可能となる。

衝突検出手段としては、第２の実施形態で説明した構成に限らずともよく、ロボットに対する衝突を検出可能なものであれば適用することができる。例えば、他のセンサレス方式の衝突検出手段や、加速度センサなどのセンサを用いる衝突検出手段であってもよい。
第１の実施形態では、本発明を４軸の水平多関節型のロボット２に適用した例を説明し、第２の実施形態では、本発明を６軸の垂直多関節型のロボット６２に適用した例を説明したが、本発明は、複数の軸を有する多軸型のロボット全般に適用可能である。

図面中、１、６１はロボットシステム、２、６２はロボット、２５は回転角度検出部、２６は制御部（遡り指令入力部）、２７は角度指令生成部、２８は角度指令記憶部、７１は制御部（衝突検出手段）、Ｍはモータ（駆動部）を示す。

Claims (5)

1. ロボットに設けられた複数の軸と、
   前記複数の軸をそれぞれ駆動するための駆動部と、
   前記複数の軸の回転角度を検出する回転角度検出部と、
   回転角度指令を生成周期毎に生成し、その回転角度指令の一部を前記生成周期よりも長い出力周期毎に出力する角度指令生成部と、
   前記回転角度検出部により検出される回転角度を前記角度指令生成部から出力される前記回転角度指令に一致させるように前記駆動部の動作を制御する通常制御を実行する制御部と、
   前記制御部により前記通常制御が実行されている期間に前記角度指令生成部により生成される前記回転角度指令の全てを順次記憶する角度指令記憶部と、
   前記制御部により前記通常制御が実行されていない期間にて、遡り開始指令および遡り終了指令を入力する遡り指令入力部とを備え、
   前記角度指令生成部は、
   前記遡り開始指令が入力されると、前記角度指令記憶部に記憶された前記回転角度指令を新しいものから古いものへと順に読み出すとともに、当該読み出した回転角度指令を遡り角度指令として前記出力周期毎に出力し、
   前記遡り終了指令が入力されると、前記遡り角度指令の出力を停止し、
   前記制御部は、前記遡り開始指令が入力されると、前記回転角度検出部により検出される回転角度を前記角度指令生成部から出力される遡り角度指令に一致させるように前記駆動部の動作を制御する遡り制御の実行を開始し、
   前記遡り終了指令が入力されると、前記遡り制御の実行を終了することを特徴とするロボットシステム。
2. 前記制御部は、前記遡り開始指令が入力されると、前記角度指令記憶部に記憶されている最新の前記回転角度指令と前記回転角度検出部により検出された現在の回転角度との差分を求め、その差分の絶対値が所定の誤差値以下である場合には前記遡り制御を実行し、前記差分の絶対値が前記誤差値を超える場合には前記遡り制御を実行しないことを特徴とする請求項１に記載のロボットシステム。
   
3. 前記ロボットの衝突を検出する衝突検出手段を備え、
   前記制御部は、
   前記衝突検出手段により衝突が検出されると、前記衝突が発生した時点である衝突タイミングおよび前記衝突タイミング時の位置である衝突位置を特定し、前記衝突位置が中心であり且つ前記衝突タイミングの前後における前記回転角度指令の出力タイミングのうち前記衝突タイミングに近い方の出力タイミングに出力された回転角度指令と前記衝突位置との間の長さの半径を持つ球状をなす進入禁止空間を設定し、
   前記角度指令生成部は、
   前記衝突が検出された状態で前記遡り開始指令が入力されると、前記角度指令記憶部から読み出した回転角度指令に基づく前記ロボットの動作軌跡が前記進入禁止空間の外郭に重なる２つの外郭位置の間では、前記ロボットの動作軌跡が前記進入禁止空間の外であり且つ前記進入禁止空間の球の表面に沿う軌跡となるように前記読み出した回転角度指令を変更したものを前記遡り角度指令として出力することを特徴とする請求項１にロボットシステム。
4. 前記ロボットの衝突を検出する衝突検出手段を備え、
   前記制御部は、
   前記衝突検出手段により衝突が検出されると、前記衝突が発生した時点である衝突タイミングおよび前記衝突タイミング時の位置である衝突位置を特定し、前記衝突位置が中心であり且つ前記衝突タイミングの前後における前記回転角度指令の出力タイミングのうち前記衝突タイミングから遠い方の出力タイミングに出力された回転角度指令と前記衝突位置との間の長さの半径を持つ球状をなす進入禁止空間を設定し、
   前記角度指令生成部は、
   前記衝突が検出された状態で前記遡り開始指令が入力されると、前記角度指令記憶部から読み出した回転角度指令に基づく前記ロボットの動作軌跡が前記進入禁止空間の外郭に重なる２つの外郭位置の間では、前記ロボットの動作軌跡が前記進入禁止空間の外であり且つ前記進入禁止空間の球の表面に沿う軌跡となるように前記読み出した回転角度指令を変更したものを前記遡り角度指令として出力することを特徴とする請求項１にロボットシステム。
5. 前記角度指令生成部は、前記遡り開始指令が入力されると、前記角度指令記憶部に記憶された前記回転角度指令のうち最新のものを除く回転角度指令を新しいものから古いものへと順に読み出すことを特徴とする請求項１〜４のいずれか一つに記載のロボットシステム。

Images