JP3911258B2 - 制御装置による駆動装置の制御方法および制御装置 - Google Patents

Description

本発明は、駆動装置の制御方法および制御装置に関する。さらに詳しくは、ロボットアームが障害物に衝突した場合、その衝突を検知して、この衝突により発生する各部材の損傷を最小限に留めるようにロボットアームのモータを制御する制御方法および制御装置に関する。

ロボットアームと障害物との衝突を検知する技術に関して、さまざまな提案がなされている。たとえばロボットに設けられるサーボモータにトルクセンサを取付け、トルクセンサの乱れから衝突を検知する方法が開示されている（特許文献１参照）。

また他の従来技術として、ロボットアームの衝突が検知された後に、衝突前に与えられるトルクとは逆向きのトルクをロボットアームに与えるように、サーボモータに電流を与える技術が開示されている（たとえば特許文献２および特許文献３参照）。

特開平５−２０８３９４号公報 特開平７−１４３７８０号公報 特開２００１−１１７６１８号公報

上述したサーボモータの制御方法では、ロボットアームの衝突によって発生する各部材の損傷を抑制する点に関し、充分に考慮されているとは言えない。というのは、衝突してから逆向きのトルクをロボットアームに与えるまでに遅れが生じるからである。この遅れた時間の間、ロボットアームは障害物を押し続ける。

遅れ時間の経過中には、ロボットアームが障害物を押付けているので、ロボットアームおよび障害物の損傷の程度を抑制することができない。またロボットアームの衝突によって発生する衝撃を緩和することが困難である。このことは、電動モータを備えるロボット以外のモータの制御方法であっても同様である。

したがって本発明の目的は、衝突によって発生する衝撃を緩和し、各部材の損傷を低減することができる駆動装置の制御方法および制御装置を提供することである。

請求項１記載の本発明は、回転量を検出するエンコーダが設けられる電動モータを備えて構成され、電動モータによって可動体を変位駆動する駆動装置と、
時間経過毎に移動すべき可動体の位置を表わす位置指令値と、エンコーダによって検出されるエンコーダ値とに基づいて、電動モータに与える電流値を表わす指示電流指令値を決定する制御装置であって、可動体が障害物に衝突しない正常状態において、指示電流指令値が予め定められる設定電流範囲内である場合、指示電流指令値を駆動電流指令値とし、指示電流指令値が設定電流範囲以上である場合には、設定電流範囲の上限値を駆動電流指令値とし、指示電流指令値が設定電流範囲以下である場合には、設定電流範囲の下限値を駆動電流指令値とする制御装置と、
制御装置によって決定される駆動電流指令値が与えられ、駆動電流指令値の表わす電流の値に応じた駆動電流を発生し、発生した電流を電動モータに与える電流発生回路と、
可動体が障害物に衝突したか否かを検出し、可動体が障害物に衝突したことを示す衝突検知信号を出力する衝突検知手段とを含むシステムにおいて、制御装置による駆動装置の制御方法であって、
衝突検知手段から与えられる衝突検知信号与に応答して、可動体が障害物に衝突したことを検知する衝突検知工程と、
衝突検知工程で可動体の衝突を検知すると、設定電流範囲よりもさらに制限される制限範囲で制限して駆動電流指令値を生成し、制限した駆動電流指令値を電流発生回路に与える電流制限工程と、
衝突検知工程で可動体の衝突を検知すると、可動体が障害物に衝突するまでの進行移動経路を逆にたどるような時間経過毎の位置指令値を用いて、指示電流指令値を決定する電流反転工程とを含むことを特徴とする制御装置による駆動装置の制御方法である。
請求項２記載の本発明は、電流制限工程では、指示電流指令値を予め定める減少率で減少させた値を駆動電流指令値として生成することを特徴とする。
請求項３記載の本発明は、電流制限工程では、予め定める一定値を駆動電流指令値として生成することを特徴とする。

請求項４記載の本発明は、電流制限工程では、衝突直前に生成された指示電流指令値と同じ向きの指示電流指令値であって、衝突後に生成される指示電流指令値についてのみ制限して、駆動電流指令値を生成することを特徴とする。

請求項５記載の本発明は、衝突検知工程で可動体の衝突を検知してから、衝突後に生成される指示電流指令値が、衝突直前に生成された指示電流指令値と反対の向きとなると、電流制限工程における駆動電流指令値の制限を解除する電流制限解除工程をさらに含むことを特徴とする。

請求項６記載の本発明は、衝突検知工程で可動体の衝突を検知してから、衝突後に生成される指示電流指令値が、衝突直前に生成された指示電流指令値と反対の向きとなると、電流制限工程を終了して、衝突前に予め設定される設定電流範囲よりも大きい駆動電流指令値が電流発生回路に与えられることを許容する過電流許容工程をさらに含むことを特徴とする。

請求項７記載の本発明は、衝突検知工程で可動体の衝突を検知してから、エンコーダ値に基づいて、可動体が予め定める距離後退することを判断すると、過電流許容工程を終了する過電流許容解除工程をさらに含むことを特徴とする。

請求項８記載の本発明は、電流制限工程によって制限される駆動電流指令値は、可動体が重力によって変位しないトルクを発生するに必要な電流値に応じた値以上に設定されることを特徴とする。

請求項９記載の本発明は、前記駆動装置による駆動装置の制御方法をコンピュータによって実行させるためのプログラムである。

請求項１０記載の本発明は、前記プログラムを記憶するコンピュータ読取可能な記録媒体である。

請求項１１記載の本発明は、駆動量を検出する駆動量検出手段が設けられる電動モータを備えて構成され、電動モータによって可動体を変位駆動する駆動装置と、
時間経過毎に移動すべき可動体の位置を表わす位置指令値と、駆動量検出手段によって検出される検出値とに基づいて、電動モータに与える電流値を表わす指示電流指令値を決定する制御装置と、
制御装置によって決定される駆動電流指令値が与えられ、駆動電流指令値の表わす電流の値に応じた駆動電流を発生し、発生した電流を電動モータに与える電流発生回路と、
可動体が障害物に衝突したか否かを検出し、可動体が障害物に衝突したことを示す衝突検知信号を出力する衝突検知手段とを含むシステムの一部を構成する制御装置であって、
衝突検知手段から与えられる衝突検知信号に応答して、予め定められる制限範囲に制限した駆動電流指令値を生成し、制限した駆動電流指令値を電流発生回路に与える電流制限手段と、
衝突検知手段から与えられる衝突検知信号に応答して、可動体が障害物に衝突するまでの進行移動経路を逆にたどるような時間経過毎の位置指令値を用いて、指示電流指令値を決定する電流反転手段とを含むことを特徴とする制御装置である。
請求項１２記載の本発明は、電流制限手段は、指示電流指令値を予め定める減少率で減少させた値を駆動電流指令値として生成することを特徴とする。

請求項１３記載の本発明は、電流制限手段は、衝突直前に生成された指示電流指令値と同じ向きの指示電流指令値であって、衝突後に生成される指示電流値についてのみ制限して、駆動電流指令値を生成することを特徴とする。

請求項１４記載の本発明は、可動体の衝突を検知してから、衝突後に生成される指示電流指令値が、衝突直前に生成された指示電流指令値と反対の向きとなると、電流制限手段における駆動電流指令値の制限を解除する電流制限解除手段をさらに含むことを特徴とする。

請求項１５記載の本発明は、電流制限手段によって制限される駆動電流指令値は、可動体が重力によって変位しないトルクを発生するに必要な電流値に応じた値以上に設定されることを特徴とする。

請求項１６記載の本発明は、電動モータを備えて構成される駆動装置と、
駆動装置の電気モータによって変位駆動される可動体となるロボットアームと、
前記制御装置とを含むことを特徴とするロボットである。

請求項１記載の本発明によれば、衝突検知工程で可動体が障害物に衝突したことを検知すると、電流反転工程で可動体が障害物に衝突するまでの進行移動経路を逆にたどるような時間経過毎の位置指令値を用いて、指示電流指令値を決定する。これによって衝突後に、可動体の衝突前と逆向きの指示電流指令値を生成することができる。逆向きの指示電流指令値から決定した駆動電流指令値を電流発生回路に与え、電流発生回路によって発生された電流がモータに与えられることで、可動体に衝突前と逆向きのトルクが与えられる。これによって衝突前に移動していた方向と逆向きに可動体を移動させることができ、可動体が障害物を押付ける時間を短くすることができる。

また衝突検知工程で可動体が障害物に衝突したことを検知すると、電流制限工程で設定電流範囲よりもさらに制限される制限範囲で制限して駆動電流指令値を生成し、制限した駆動電流指令値を電流発生回路に与えて、モータに与える電流を制限する。衝突を検知してから、指示電流指令値を反転させてモータに与える電流を反転するまでにかかる時間と、駆動電流指令値を制限してモータに与える電流を制限するまでにかかる時間とでは、電流を制限する時間のほうが短い。
たとえば衝突検知時からモータへの電流の流れる向きが反転するような指示電流指令値を生成して、反転した電流をモータへ流すまでには、サーボ系の遅れによって時間遅れがある。これに対して駆動電流指令値を制限するには、サーボ系の遅れにかかわらずに行うことができる。このように衝突検知時から駆動電流指令値を制限して電流を減少させるまでにかかる時間は、駆動電流指令値を反転して、電流を逆方向に流すまでにかかる時間よりも短い。
このように本発明では、衝突を検知してから、電流反転工程でモータに与える電流の流れる向きを反転させるまでに遅れが生じる場合でも、アーム衝突後にモータに流れる電流を制限することで、可動体が障害物を押付けようとする力を抑制することができる。すなわち衝突によって発生する衝撃を緩和することができ、可動体および障害物の損傷を低減することができる。
言い換えると、衝突検知後に、電流を反転する指示電流指令値が演算されるまでに、電流を制限した駆動電流指令値を生成することができる。すなわち衝突検知後、モータに与えられる電流が反転する前に、モータに与えられる電流を低減することができ、可動体が衝突してから逆方向に移動するまでに、可動体が障害物を押付ける力をすばやく低減することができる。
また請求項２記載の本発明によれば、指示電流指令値を予め定める減少率で減少させた値を駆動電流指令値として生成する。これによって位置指令値と、エンコーダ値とに基づいて、決定される指示電流指令値よりも小さい駆動電流指令値を電流発生回路に与えることができ、衝突直後の指示電流指令値を減少させることができ、衝突後にモータに与えられる電流を減少させることができる。
また請求項３記載の本発明によれば、予め定める一定値を駆動電流指令値として生成することで、衝突直後の指示電流指令値の大小にかかわらず、衝突直後の指示電流指令値を減少させることができ、衝突後にモータに与えられる電流を減少させることができる。

また請求項４記載の本発明によれば、衝突直前に生成された指示電流指令値と同じ向きの指示電流指令値であって、衝突後に生成される指示電流指令値についてのみ制限する。これによって、電流反転工程において進行移動経路を逆にたどる位置指令値を用いてモータへの電流の流れを反転した場合に、衝突前とは逆向きにモータへ流れる電流が制限されることがない。これによって衝突による衝撃を緩和したうえで、可動体が障害物を押付ける時間を短くすることができる。また電流の流れを反転させるまでに必要な時間がばらつく場合であっても、電流の流れが反転すると直ちに逆向きに流れる電流をモータに与えることができる。

また請求項５記載の本発明によれば、電流制限解除工程で、可動体の衝突を検知してから、衝突後に生成される指示電流指令値が、衝突直前に生成された指示電流指令値と反対の向きとなると、電流の制限を解除する。これによって電流の反転前について電流を制限することができるとともに、電流の反転後について電流の制限を解除することで、可動体に与えられるトルクであって、衝突前と逆向きのトルクが制限されることを防ぐことができる。

たとえば衝突を検知してから電流が反転するまでに費やす反転時間を予め取得し、衝突検知してから反転時間が経過すると電流の制限を解除することで、電流の向きが反転するまで電流を制限することができる。またたとえば電流の向きが反転したことを検出すると、電流の制限を解除してもよい。

また請求項６記載の本発明によれば、過電流許容工程で、可動体の衝突を検知してから、衝突後に生成される指示電流指令値が、衝突直前に生成された指示電流指令値と反対の向きとなると、設定電流範囲よりも大きい駆動電流指令値が電流発生回路に与えられることを許容する。これによって可動体に与えられる逆向きのトルクを大きくすることができ、可動体が障害物を押付ける時間をさらに短くすることができる。

また請求項７記載の本発明によれば、可動体の衝突を検知してから、エンコーダ値に基づいて、可動体が予め定める距離後退することを判断すると、過電流許容工程を終了する。これによって、いつまでもモータに大きい電流が流れ続けることを防いで、モータの損傷を防ぐことができる。

また請求項８記載の本発明によれば、電流制限工程で、モータに与えられる電流が制限されたとしても、可動体が重力によって変位することを防止することができる。これによって電流制限工程において、可動体が不所望に変位することを防止することができる。たとえば可動体の自重による落下を防ぐことができる。

また請求項９記載の本発明によれば、コンピュータがプログラムを読取ることによって上述した制御方法が実行される。これによって瞬時にモータに流れる電流を制限することとができ、可動体が障害物に衝突して障害物を押付ける時間をさらに短縮することができる。すなわち衝突による衝撃をより確実に緩和することができる。

また請求項１０記載の本発明によれば、記憶媒体に記憶されるプログラムをコンピュータが読取ることによって、上述した制御方法が実現される。

また請求項１１記載の本発明によれば、衝突検知手段によって可動体が障害物に衝突したことを検知すると、電流反転手段によって可動体が障害物に衝突するまでの進行移動経路を逆にたどるような時間経過毎の位置指令値を用いて、指示電流指令値を決定する。これによって衝突後に、可動体の衝突前と逆向きの指示電流指令値を生成することができる。逆向きの指示電流指令値から決定した駆動電流指令値を電流発生回路に与え、電流発生回路によって発生された電流がモータに与えられることで、可動体に逆向きのトルクが与えられる。これによって衝突前に移動していた方向と逆向きに可動体を移動させることができ、可動体が障害物を押付ける時間を短くすることができる。

また衝突検知手段によって可動体が障害物に衝突したことを検知すると、電流制限手段によって、予め定められる制限範囲に制限した駆動電流指令値を生成し、制限した駆動電流指令値を電流発生回路に与えて、モータに与えられる電流を制限する。衝突を検知してから、指示電流指令値を反転させてモータに与える電流を反転するまでにかかる時間と、駆動電流指令値を制限してモータに与える電流を制限するまでにかかる時間とでは、電流を制限する時間のほうが短い。
したがって可動体が障害物に衝突してから、モータに与える電流の流れる向きを反転させるまでに遅れが生じる場合でも、電流制限手段によって衝突後にモータに与えられる電流を制限することで、可動体が障害物を押付けようとする力を抑制することができる。すなわち衝突によって発生する衝撃を緩和することができ、可動体および障害物の損傷を低減することができる。
また請求項１２記載の本発明によれば、指示電流指令値を予め定める減少率で減少させた値を駆動電流指令値として生成する。これによって位置指令値と、検出値とに基づいて、決定される指示電流指令値よりも小さい駆動電流指令値を電流発生回路に与えることができ、衝突直後の指示電流指令値を減少させることができ、衝突後にモータに与えられる電流を減少させることができる。

また請求項１３記載の本発明によれば、衝突直前に生成された指示電流指令値と同じ向きの指示電流指令値であって、衝突後に生成される指示電流指令値についてみを制限する。これによって、電流反転手段によって進行移動経路を逆にたどる位置指令値を用いてモータへの電流の流れを反転した場合に、衝突前とは逆向きにモータへ流れる電流が制限されることがない。これによって衝突による衝撃を緩和したうえで、可動体が障害物を押付ける時間を短くすることができる。また電流の流れを反転するまでに必要な時間がばらつく場合であっても、電流の流れが反転すると直ちに逆向きに流れる電流をモータに与えることができる。

また請求項１４記載の本発明によれば、電流制限解除手段によって可動体の衝突を検知してから、衝突後に生成される指示電流指令値が、衝突直前に生成された指示電流指令値と反対の向きとなると、電流の制限を解除する。これによって電流の反転前について電流を制限することができるとともに、電流の反転後について電流の制限を解除することで、可動体に与えられるトルクであって、衝突前と逆向きのトルクが制限されることを防ぐことができる。

たとえば衝突を検知してから電流が反転するまでに費やす反転時間を予め取得し、衝突検知してから反転時間が経過すると電流の制限を解除することで、電流の向きが反転するまで電流を制限することができる。またたとえば電流の向きが反転したことを検出すると、電流の制限を解除してもよい。

また請求項１５記載の本発明によれば、電流制限手段によって、モータに与えられる電流が制限されたとしても、可動体が重力によって不所望に変位することを防止することができる。たとえば外力として重力が可動体に与えられる場合、モータの電流を制限したとしても、可動体の自重による落下を防ぐことができる。

また請求項１６記載の本発明によれば、ロボットアームが障害物に衝突した場合に、衝突によって発生する衝撃を緩和することができ、各部材の損傷を低減することができる。これによってロボットアームの移動経路が用途によって異なる場合であって、衝突位置が予想できない場合であっても、障害物とロボットアームとの衝突時の衝撃を有効に緩和することができる。また電流指令値を変更して、モータに与える電流を反転および制限することによって、ソフトウェアによって本発明の制御装置を実現することができ、ハードウェアに比べて容易に実現することができる。

図１は、本発明の実施の一形態であるロボット１の主要な構成を示すブロック図である。ロボット１は、ロボット本体８と、ロボット本体８を制御する制御装置２０とを含む。たとえばロボット１は、汎用ロボットであって６自由度を有する垂直多関節型ロボットである。ロボット本体８は、ロボットアーム４０と、サーボ機構４１とを含んで構成される。ロボットアーム４０は、変位可能に設けられる。サーボ機構４１は、制御装置２０の指令に従ってロボットアーム４０を変位駆動する。

制御装置２０は、ロボットアーム４０が障害物Ｇに衝突したことを検知すると、サーボ機構４１を制御し、衝突によって発生する各部材の損傷を可及的に抑制するようにロボットアーム４０を動作させる。

サーボ機構４１は、アーム４０を変位駆動するための駆動装置である駆動手段と、駆動手段の動力をアーム４０に伝達するための伝達機構とを含む。なお、駆動手段は、駆動量を検出する駆動量検出手段が設けられる。本実施の形態では、駆動手段は、電動モータ、たとえばサーボモータによって実現される。サーボモータには、駆動量検出手段としてモータの回転量を検出するエンコーダ２８が設けられる。

制御装置２０は、電流発生回路６１と、移動量指令手段５４と、衝突検知手段５１と、後退指令手段５３と、電流制限手段５２とを含む。移動量指令手段５４は、アーム４０を予め定める位置に移動させるために必要な電流指令値を演算する。アーム４０が障害物Ｇに衝突しない正常状態では、移動量指令手段５４が演算した電流指令値が、電流発生回路６１に与えられる。電流発生回路６１は、与えられる電流指令値に基づいて電流を発生し、発生した電流をサーボモータに流す。電流発生回路６１は、電流指令値に応じてモータの駆動電流を発生する増幅器、いわゆるサーボアンプである。

衝突検知手段５１は、ロボットアーム４０が障害物Ｇに衝突したことを検知し、衝突したことを示す衝突検知信号を出力する。後退指令手段５３は、衝突検知手段５１から衝突検知信号が与えられると、衝突するまでの進行移動径路を逆にたどるようなロボットアーム４０の後退移動経路を演算し、演算した後退移動径路を示す情報を移動量指令手段５４に与える。移動量指令手段５４は、後退指令手段５３から後退移動径路を示す情報が与えられると、その後退移動径路に基づいた電流指令値を演算して電流発生回路６１に与える。

電流制限手段５２は、衝突検知手段５１から衝突検知信号が与えられると、移動量指令手段５４が演算した電流指令値を制限し、制限した電流指令値を電流発生回路６１に与える。これによってアーム衝突検知時には、電流発生回路６１から出力される電流は、電流制限前に比べて減少されて、モータに与えられる。

本発明において電流を制限することは、移動量指令手段５４の電流指令値を予め定める減少率で減少させる場合、移動量指令手段５４の電流指令値にかかわらず予め定める一定値に制限する場合のいずれであってもよい。また予め定める減少率は、０％である場合も含む。この場合、電流制限時には、電流発生回路６１からサーボモータに与えられる電流はゼロであり、モータに電流が流れることが阻止される。

制御装置２０は、いわゆるロボットコントローラによって実現される。この場合、本発明のロボットコントローラは、通常のロボットコントローラに比べて、衝突検知手段５１、電流制限手段５２および後退指令手段５３をさらに備える構成である。したがって衝突検知手段５１、電流制限手段５２および後退指令手段５３以外の他の構成については、通常のロボットコントローラと同様の構成であってもよい。

図２は、本発明のロボットアーム４０の関節構造を模式的に示す図である。ロボットアーム４０は、第１〜第６の可動体４４ａ〜４４ｆを有する。各可動体４４ａ〜４４ｆは、アーム軸１１〜１６と、関節部２〜７とによってそれぞれ構成される。

各可動体４４ａ〜４４ｆは、直列に連結され、相互に変位可能に設けられる。各可動体４４ａ〜４４ｆのうち、直列方向一方側の第１可動体４４ａは、ベース４３に固定される。また各可動体４４ａ〜４４ｆのうち、直列方向他方側の第６可動体４４ｆは、エンドエフェクタ４２が連結される。エンドエフェクタ４２は、ロボットハンドや溶接ツールなどであり、ロボットアームの変形動作によって予め定める姿勢で予め定める位置に変位駆動される。

第１〜第６可動体４４ａ〜４４ｆは、第１可動体４４ａから第６可動体４４ｆまで、順に連結され、それぞれ対応する角変位軸線を有する。各可動体４４ａ〜４４ｆは、対応する角変位軸線まわりに角変位可能に連結される。

第１可動体４４ａに対応する第１角変位軸線は鉛直に延びる。第１可動体４４ａは、第１関節部２によって、ベース４３に対して第１角変位軸線まわりに角変位可能に設けられる。第２可動体４４ｂに対応する第２角変位軸線は、第１角変位軸線に対して垂直に延びる。第２可動体４４ｂは、第２関節部３によって、第１可動体４４ａに対して第２角変位軸線まわりに角変位可能に設けられる。

第３可動体４４ｃに対応する第３角変位軸線は、第２角変位軸線に対して垂直に延びる。第３可動体４４ｃは、第３関節部４によって、第２可動体４４ｂに対して第３角変位軸線まわりに角変位可能に設けられる。第４可動体４４ｄに対応する第４角変位軸線は、第３角変位軸線と同軸に延びる。第４可動体４４ｄは、第４関節部５によって、第３可動体４４ｃに対して第４角変位軸線まわりに角変位可能に設けられる。

第５可動体４４ｅに対応する第５角変位軸線は、第４角変位軸線に対して垂直に延びる。第５可動体４４ｅは、第５関節部６によって、第４可動体４４ｄに対して第５角変位軸線まわりに角変位可能に設けられる。第６可動体４４ｆに対応する第６角変位軸線は、第５角変位軸線と同軸に延びる。第６可動体４４ｆは、第６関節部７によって、第５可動体４４ｅに対して第６角変位軸線まわりに角変位可能に設けられる。

第１可動体４４ａ、第４可動体４４ｄ、第６可動体４４ｆは、いわゆる旋回軸を構成する。第２可動体４４ｂ、第３可動体４４ｃ、第５可動体４４ｅは、いわゆる回転軸を構成する。また各可動体４４ａ〜４４ｆは、それぞれ対応する第１〜第６角変位軸線まわりに角変位駆動される各サーボ機構４１がそれぞれ設けられる。

このように本発明の実施の一形態であるロボット１は、垂直多関節型の６自由度ロボットである。なお、上述したロボット１は、本発明の一例示であって、この構成に限定されない。たとえば本発明の制御装置２０は、半導体ウェハを搬送する円筒座標型のロボットに搭載されてもよい。また図２において参照符号Ｇは、障害物を示す。

図３は、制御装置２０の物理的構成を示すブロック図である。制御装置２０は、コンピュータによって実現され、予め定められるプログラムを実行することによって、上述する移動量指令手段５４、衝突検知手段５１、後退指令手段５３、電流制限手段５２を実現することができる。またコンピュータによって制御装置２０が実現されることで、モータを瞬時に制御することができる。

制御装置２０は、演算部７０と、記憶部７１と、インターフェース部７２と、電流発生回路６１とを含んで構成される。記憶部７１は、後述する各演算器の全てまたは一部の機能を実現するためのプログラムを記憶する。演算部７０は、記憶部７１に記憶されるプログラムを読取り、プログラムを実行することによって各演算器の全てまたは一部の機能を実現することができる。これによって本発明の各手段を物理的に有していない制御装置であっても、記憶媒体に記憶されるプログラムを読取ることによって構成を大きく変更することなく、本発明の制御装置２０として機能させることができる。すなわち従来の制御装置６１に比べて、ソフトウェアを変更するだけでよく、新たにハードウェアを加える必要がないので、容易に本発明の制御装置２０を実現することができる。

演算部７０は、たとえばＣＰＵ（Central Processing Unit）によって実現される。また記憶部７１は、たとえばＲＡＭ（Random Access Memory）およびＲＯＭ（Read Only
memory）などによって実現される。

各演算器の機能を実現するためのプログラムは、別途コンピュータ読取り可能な記憶媒体に記憶されてもよい。この場合、演算部７０が記憶媒体に記憶されるプログラムを読取り、そのプログラムを実行することによって、各演算器全てまたは一部の機能を実現することができる。またインターフェース部７２は、演算部７０と記憶部７１との情報伝達を行う。またインターフェース部７２は、入力手段となるティーチングペンダント７３、位置検知手段となるエンコーダ２８および電流発生回路６１と、演算部７０との情報伝達を行う。

演算部７０は、ティーチングペンダント７３から与えられる情報または記憶部７１に記憶される情報に基づいて外部位置指令値を生成する。また演算部７０は、エンコーダ２８から検出位置情報が与えられる。これらの情報に基づいて、電流指令値を演算し、演算した電流指令値を電流発生回路６１に与える。電流発生回路６１は、電流指令値に従って電流を発生し、その電流をモータＭに与える。

またアーム４０が障害物Ｇに衝突すると、演算部７０は、位置指令値とエンコーダ値とに基づいて、アーム４０が障害物Ｇに衝突したか否かを検知する。そして演算部７０は、アーム４０の衝突を検知すると、電流指令値を制限してモータＭに与えられる電流を制限するとともに、可動体４４が後退方向に移動するような電流指令値を生成して電流発生回路６１に与える。

図４は、制御装置２０の機能的構成を示すブロック図である。図３に示す演算部７０が記憶部７１に記憶されるプログラムを実行することによって、図４に示す移動量指令手段５４、衝突検知手段５１、後退指令手段５３、電流制限手段５２の機能を実現することができる。

移動量指令手段５４は、予め定められる位置指令値とエンコーダ２８によるエンコーダ値との偏差に基づいて、各サーボ機構のモータＭを駆動するための移動指令値を決定する。具体的には、移動量指令手段５４は、外部位置指令値生成部６０と、位置偏差算出器３２と、第１比例器３３と、速度偏差算出器３４と、第２比例器３５と、第３比例器３６と、積分器３７と、加算器３８と、電流発生回路６１と、フィードバック速度算出器２９とを含む。

外部位置指令値生成部６０は、予め教示されるロボットアーム４０の移動径路に従って位置指令値を生成する。位置指令値は、時間経過毎に移動すべき各可動体４４ａ〜４４ｆの位置を表わす情報である。したがって各可動体４４ａ〜４４ｆを移動させる場合には、位置指令値も時間毎に変化する。

各可動体４４ａ〜４４ｆの移動径路は、単にエンドエフェクタ４２の移動径路が制御装置２０に設けられる記憶手段に記憶されていてもよい。この場合、外部位置指令値生成部６０は、記憶手段からエンドエフェクタ４２の移動経路を読み出して、各可動体４４ａ〜４４ｆの移動経路を算出して、各可動体４４ａ〜４４ｆに対応する外部位置指令値をそれぞれ生成する。

以後、複数のうち１つの可動体４４に対応するサーボ機構４１のモータＭに関する制御装置２０の構成を説明する。残余の可動体４４に対応するサーボ機構のモータＭに関する制御装置２０の構成は、１つの可動体４４に関する構成と同様の構成であるので説明を省略する。

位置偏差算出器３２は、ロボットアーム４０が障害物Ｇに衝突しない正常状態では、外部位置指令値生成部６０から位置指令値が与えられ、ロボットアーム４０が障害物Ｇに衝突したアーム衝突時には、後述する内部指令値生成部３０から位置指令値が与えられる。また位置偏差算出器３２は、エンコーダ２８から検出位置情報が与えられる。検出位置情報は、エンコーダ２８のエンコーダ値であって、可動体４４の位置を表わす情報である。

位置偏差算出器３２は、位置指令値の表わす位置から検出位置情報の表わす位置を減算して、位置偏差を表わす位置偏差情報を演算する。位置偏差算出器３２は、第１比例器３３に演算した位置偏差情報を与える。

第１比例器３３は、位置偏差算出器３２から与えられる位置偏差情報の表わす位置偏差に、予め定める第１係数Ｋｐを乗算して、可動体４４の速度を表わす速度情報を演算する。第１比例器３３は、演算した速度情報を速度偏差算出器３４に与える。

またフィードバック速度算出器２９は、エンコーダ２８から時間経過ごとに検出位置情報が随時与えられ、その検出位置情報の表わす位置を時間で微分して速度を表わすフィードバック速度情報を演算する。フィードバック速度算出器２９は、演算したフィードバック速度情報を、速度偏差算出器３４および後述する推定速度偏差算出器２３に与える。

速度偏差算出器３４は、第１比例器３３から与えられる速度情報の表わす速度から、フィードバック速度情報の表わす速度を減算して、速度偏差を表わす速度偏差情報を演算する。速度偏差算出器３４は、速度偏差情報を第２比例器３５に与える。第２比例器３５は、速度偏差算出器３４から与えられる速度偏差情報の表わす速度偏差に、予め定める第２係数Ｋｖｐを乗算して、モータＭに与える第１の電流の変化量を表わす１次電流指令値を演算する。

第３比例器３６は、１次電流指令値の表わす電流の変化量に予め定める第３係数Ｋｖｉを乗算して、第２の電流の変化量を表わす２次電流指令値を演算し、２次電流指令値を積分器３７に与える。積分器３７は、２次電流指令値の表わす第２の電流の変化量を時間で積分し、第２の電流の変化量の積分値を表わす積分電流指令値を演算する。

加算器３８は、積分電流指令値と１次電流指令値とが与えられる。加算器３８は、与えられるそれぞれの指令値の表わす電流値を互いに加算し、指示電流指令値を演算する。指示電流指令値は、モータＭに与える電流の値を表わす情報である。加算器３８は、指示電流指令値を電流制限手段５２に与える。

電流制限手段５２は、正常状態すなわちアーム４０が障害物Ｇに衝突していない状態では、指示電流指令値を制限せずに、指示電流指令値を駆動電流指令値として電流発生回路６１に与える。電流発生回路６１は、駆動電流指令値の表わす電流の値に応じた駆動電流を発生し、発生した電流をモータＭに流す。

電流制限手段５２は、アーム４０が障害物Ｇに衝突したアーム衝突状態以降では、指示電流指令値を予め定める減少率で減少して制限し、それを駆動電流指令値として電流発生回路６１に与える。電流発生回路６１は、駆動電流指令値の表わす電流の値に応じた駆動電流を発生し、発生した電流をモータＭに流す。モータＭは、電流発生回路６１から電流が流れることによって可動体４４を変位駆動する。

なお、加算器３８で、１次電流指令値が示す電流値と、積分電流指令値が示す電流値とを加算して指示電流指令値として演算するのは、可動体４４の移動動作を維持させるためである。たとえば可動体４４を一定速度で移動させる場合、目標速度に到達して速度偏差がゼロとなり、１次電流指令値がゼロとなった場合であっても、積分電流指令値が加算器３８に与えられることで、可動体４４の速度を維持して動作させることができる。すなわち加算器３８は、可動体４４の現在動作を維持するための現在動作維持指令値生成器として機能する。

第１比較器３３は、通常のロボットコントローラに用いられる位置指令値を速度に変換する比例器と同様であってもよい。第２比例器は、通常のロボットコントローラに用いられる速度を電流指令値に変換する比例器と同様であってもよい。

衝突検知手段５１は、可動体４４の推定移動偏差および推定加速度偏差のうちのいずれか一方がしきい値を超えた場合に、ロボットアーム４０が障害物Ｇに衝突したとして判断する。具体的には、衝突検知手段５１は、推定位置算出器２１と、推定速度算出器２２と、推定速度偏差算出器２３と、第１判定器２４と、推定加速度偏差算出器２５と、第２判定器２５と、ＯＲ回路２７とを含む。

推定位置算出器２１は、可動体４４の理論上の位置を推定する。推定位置算出器２１は、その時定数がロボット１の時定数とほぼ同等に設定される。推定位置算出器２１は、外部位置指令値生成部６０から外部位置指令値が与えられ、推定位置算出器２１に設定される時定数に基づいて、サーボ系などの遅れ要素を考慮して、可動体４４の理論上の位置である推定位置を演算する。推定位置算出器２１は、演算した推定位置を表わす推定位置情報を推定速度算出器２２に与える。

推定位置算出器２１の時定数は、外部位置指令値が与えられてから可動体４４が、その外部位置指令値の表わす位置に移動するまでの時間遅れに基づいて設定される。推定位置算出器２１は、与えられる外部位置指令値をフィルタに通過させて、フィルタ処理することで時間遅れを考慮した可動体４４の理論上の位置を演算する。

推定速度算出器２２は、推定位置情報の表わす推定位置を時間で微分し、推定速度を表わす推定速度情報を演算する。推定速度算出器２２は、推定速度情報を推定速度偏差算出器２３に与える。また前述したフィードバック速度算出器２９は、フィードバック速度情報を推定速度偏差算出器２３に与える。推定速度偏差算出器２３は、推定速度情報の表わす推定速度からフィードバック速度情報の表わす速度を減算し、推定速度偏差を表わす推定速度偏差情報を演算する。

第１判定器２４は、推定速度偏差算出器２３から推定速度偏差情報が与えられる。第１判定器２４は、推定速度偏差情報の表わす推定速度偏差が予め定められる第１しきい値を超えているか否かを判定する。第１判定器２４は、第１しきい値を超えている場合には、ロボットアーム４０が障害物Ｇに衝突したことを表わす衝突検知信号を作成し、作成した衝突検知信号をＯＲ回路２７に与える。

推定加速度偏差算出器２５は、推定速度偏差算出器２３から推定速度偏差情報が与えられる。推定加速度偏差算出器２５は、推定速度偏差情報の表わす推定速度偏差を時間で微分し、推定加速度偏差を表わす推定加速度偏差情報を演算して、その推定加速度偏差情報を第２判定器２６に与える。

第２判定器２６は、推定加速度偏差算出器２５から推定加速度偏差が与えられる。第２判定器２６は、推定加速度偏差情報の表わす推定加速度偏差が予め定める第２しきい値を超えているか否かを判定する。第２判定器２６は、第２しきい値を超えている場合には、ロボットアーム４０が障害物Ｇに衝突したことを表わす衝突検知信号を作成し、作成した衝突検知信号をＯＲ回路２７に与える。

ＯＲ回路２７は、第１判定器２４および第２判定器２６の少なくともいずれか一方から衝突検知信号が与えられると、ロボットアーム４０が障害物Ｇに衝突したことを検知し、衝突検知信号を出力する。ＯＲ回路２７は、後述する内部位置指令値生成部３０、切換スイッチ３１、電流制限手段５２に衝突検知信号を与える。

ＯＲ回路２７は、推定速度偏差および推定加速度偏のいずれかのうち少なくとも一方に基づいて衝突検知信号を作成する。これによって制御装置２０は、ロボットアームと障害物Ｇとの衝突を精度よく検知することができる。さらに加速度変化は、速度変化よりも速く衝突の影響が現れるので、推定加速度偏差を用いて衝突を検知することで、衝突後迅速に衝突検知信号を出力することができる。

比較例として、モータＭに流れる電流に基づいてアーム衝突を検知する場合には、衝突検知にあたって、アーム４０の関節に充填されている潤滑剤の粘度の影響を受けやすい。たとえば冬季には、潤滑剤の粘度が著しく上昇してしまう。この場合、正常状態であってもモータＭの理論電流値が大きくなり、衝突と誤検知される場合がある。これに対して本実施形態では、エンコーダ値と位置指令値とに基づくことによって、モータＭに流れる電流がばらついたとしても、精度よくアーム４０の衝突を検知することができる。

このように制御装置２０は、外部位置指令値から可動体４４の推定位置を算出する推定位置演算手順と、推定位置から可動体４４の推定速度を算出する推定速度演算手順と、推定速度から可動体４４の推定速度偏差および推定加速度偏差を算出する偏差演算手順と、推定速度偏差および推定加速度偏差のいずれかが対応する各しきい値を超えているか否か判定する判定手順と、推定速度偏差または推定加速度偏差が対応する各しきい値を超えている場合に衝突検知信号を出力する衝突検知信号出力手順とを順に行うことによって、アーム衝突を検知することができる。

後退指令手段５３は、ロボットアーム４０が障害物Ｇに衝突した場合に、可動体４４が衝突前に移動した径路を逆にたどるように、ロボットアームを移動させるために設けられる。後退指令手段５３から移動量指令手段５４に後退指令が与えられることによって、衝突が検知されたときに可動体４４が移動していた移動方向と、逆向きに移動する後退方向に移動するような力が可動体４４に与えられる。すなわち制御装置２０は、アーム４０が障害物Ｇに衝突すると、衝突前にモータＭが可動体４４に与えるトルクとは逆向きのトルクを与えるように、モータＭに与える電流の向きを逆、すなわち反転する。

後退指令手段５３は、内部位置指令値生成部３０と、切換スイッチ３１とを含む。内部位置指令値生成部３０は、エンコーダ値であって可動体４４の位置を表わす検出位置情報を随時記録する。内部位置指令値生成部３０は、ＯＲ回路２７から衝突検知信号が与えられると、記録している検出位置情報から内部位置指令値を生成し、生成した内部位置指令値を切換スイッチ３１に与える。

内部位置指令値は、衝突前に可動体４４が移動方向に移動した移動経路を逆にたどるような可動体４４の位置の時間変化を表わす。内部位置指令値の表わす位置は、時間経過とともに可動体４４の位置が衝突時の移動方向と逆向きとなる後退方向に移り変わる。これによって内部位置指令値に従うと、可動体４４は後退方向に移動する。

内部位置指令値生成部３０は、衝突検知信号が与えられると、記憶する検出位置情報のうち、最も新しい検出位置情報から古くなる順に検出位置情報を出力して内部位置指令値を演算する。内部位置指令値生成部３０は、その記憶容量に制限がある場合には、時間順に記憶される検出位置情報のうち、衝突前であって、衝突時に最も近い時間に記憶される検出位置情報から内部位置指令値を生成してもよい。

切換スイッチ３１は、外部位置指令値と内部位置指令値とを切換える。切換スイッチ３１は、外部位置指令値生成部６０から外部位置指令値が与えられるとともに内部位置指令値生成部３０から内部位置指令値が与えられる。切換スイッチ３１は、通常状態では、外部位置指令値を位置偏差算出器３２に与える。また切換スイッチ３１は、ＯＲ回路２７から衝突検知信号が与えられると、内部位置指令値を位置偏差算出器３２に与える。

位置偏差算出器３２は、アーム４０が障害物Ｇに衝突すると、切換スイッチ３１によって内部位置指令値が与えられる。そして第２比例器３５は、後退方向に可動体４４が移動するような１次電流指令値を演算する。そして電流発生回路６１は、後退方向に移動するような電流をモータＭに流す。

このように本実施の形態では、後退指令手段５３と移動量指令手段５４とを含んで電流反転手段を構成する。電流反転手段は、アーム衝突後に、アーム衝突直前にモータＭに流れる電流に対して向きを反転させた電流をモータＭに流す。このようにしてアーム４０が障害物Ｇに衝突すると、可動体４４が障害物Ｇに対して後退方向に移動する。

なお、積分器３７は、ＯＲ回路２７から衝突検知信号が与えられると、現在の積分結果を一度ゼロにして、衝突検知信号が与えられてから新たに１次電流指令値の表わす電流の変化量を積分する。言い換えるとアーム衝突時には、積分電流指令値をリセットし、アーム衝突前における可動体４４の移動動作の維持を解除する。

これによって衝突前の速度にかかわらず、アーム衝突後、後退方向に移動するような指示電流指令値を速く電流発生回路６１に与えることができる。また電流指令値をリセットすることによって、アーム衝突前に比べて指示電流指令値を低減することができ、モータＭに流れる電流を低減して、ロボットアーム４０による障害物Ｇの押付け力を緩和することができる。

比較例としてアーム４０が衝突しても積分電流指令値をリセットしない場合には、衝突後に後退方向に移動するような１次電流指令値を演算したとしても、加算器３８で衝突前の速度を表わす積分電流指令値と１次電流指令値とが加算されるので、衝突後に後退方向に移動するような指示電流指令値を速く電流発生回路６１に与えることができない。したがって、可動体４４による障害物Ｇの押付け力を緩和するまでに時間がかかる。

これに対して上述したように、衝突後に積分電流指令値をリセットすることによって、可動体４４の後退方向の動作を迅速に行うことができるとともに、可動体４４による障害物Ｇの押し付け力を緩和することができる。そのうえ、衝突後に可動体４４を後退方向に移動させることで、衝突後の再起動作を迅速に行うことができる。

電流制限手段５２は、アーム衝突時にＯＲ回路２７から衝突検知信号が与えられて、与えられる指示電流指令値を制限して、駆動電流指令値として電流発生回路６１に与える。電流発生回路６１の直前に電流制限手段が設けられることによって、アーム衝突検知後、直ちにモータＭに流れる電流を低減することができる。

後退指令手段５３は、衝突検知信号が与えられてから、内部位置指令値を生成して移動量指令手段５４に与えるまでに時間遅れが生じる。また移動量指令手段５４は、フィードバック機構を有するので内部位置指令値が与えられてから、指示電流指令値を電流制限手段５２に与えるまでに時間遅れが生じる。このようにアーム４０が衝突してから、モータＭに流れる電流を反転するまでには時間遅れが生じる。

後退方向に移動するような電流が流れるまでは、モータＭには、可動体４４が障害物Ｇを押付けるような電流が流れる。このような電流が流れつづけている期間中、可動体４４は障害物Ｇを押付け続ける。また流れる電流が大きいほど、可動体４４が障害物Ｇを押付ける力が大きくなる。

本実施形態では、アーム衝突時に電流制限手段５２によってモータＭに流れる電流を制限する。これによって衝突検知信号を出力してから後退方向に移動するような電流を流すまでに遅れがある場合であっても、可動体４４が障害物Ｇを押付けようとする力を抑制することができ、障害物Ｇおよびロボットアーム４０が損傷することを防止することができる。

また本実施の形態では、電流制限手段５２は、モータＭの許容電流を越える電流が長時間与えられることを防ぐために、予め設定電流範囲が設定される。アーム４０が衝突しない正常状態において、電流制限手段５２は、指示電流指令値が設定電流範囲以上の電流である場合には、設定電流範囲の上限値となる駆動電流指令値を電流発生回路６１に与える。また電流制限手段５２は、指示電流指令値が設定電流範囲以下の電流である場合には、設定電流範囲の下限値となる駆動電流指令値を電流発生回路６１に与える。

また内部位置指令値生成部３０は、アーム衝突検知後に可動体４４が予め定める移動距離後退して、可動体４４が障害物Ｇから離反したことを判断すると、可動体４４の移動を停止させる位置指令値を出力するとともに、電流制限手段５２に衝突解除信号を与える。

図５は、電流制限手段５２の具体的構成を示すブロック図である。本発明の実施の一形態である電流制限手段５２は、衝突直前に電流モータＭに与えられる電流と同じ向きに流れる電流に対応する指示電流指令値を予め定める減少率で減少する。

電流制限手段５２は、制限部８０と、電流方向検出部８１とを有する。アーム衝突検知前では、制限部８０は、指示電流指令値が設定電流範囲にある場合、制限せずに駆動電流指令値として電流発生回路６１に与える。

電流方向検出部８１は、衝突検知信号が与えられると、そのときに流れる電流の方向を検出し、電流の方向を含んだ電流制限指令を制限部８０に与える。制限部８０は、電流制限指令が与えられると、電流方向検出部８１によって検出される方向のみに対応する指示電流指令値を予め定める減少率に減少して駆動電流指令値として電流発生回路６１に与える。

制御部８０は、電流方向検出部８１によって与えられる方向と反対に流れる電流を表わす指示電流指令値が与えられると、指示電流指令値が設定電流範囲以上および設定電流範囲以下であっても制限せず、指示電流指令値を駆動電流指令値として電流発生回路６１に与える。すなわち制御部８０は、モータに与えられる電流の向きが反転すると、設定電流範囲よりも大きい電流がモータに流れる事を許容する過電流許容手段を兼用する。

また制限部８０は、可動体４４が障害物Ｇから離反して内部位置指令値生成部３０から衝突解除信号が与えられると、指示電流指令値を無制限に駆動電流指令値として電流発生回路に与える状態を解除し、設定電流範囲内の駆動電流指令値を電流発生回路６１に与える通常制限状態に移行する。したがって内部位置指令値生成部３０は、可動体４４が障害物Ｇから離反すると、大きい電流の許容を解除する過電流許容解除手段となる。

また他に、電流方向検出手段８１が指示電流指令値が反転したことを検出して、電流制限解除指令を制御部８０に与えることで、電流の制限を解除してもよい。

図６は、アーム衝突時における制御装置２０の動作手順を示すフローチャートである。また図７は、現在位置と電流制限状態と駆動電流との時間変化を示すタイムチャートである。図７（１）は、現在位置の時間変化を示し、図７（２）は、電流制限手段５２の電流制限状態の時間変化を示し、図７（３）は、理想的な駆動電流の時間変化を示す。

ステップＳ０で、制御装置２０は、可動体４４を移動させるために位置指令値を生成し、予め定める移動経路に従って可動体４４が移動するように、モータＭに与える駆動電流を調整する。一定速度で可動体４４を移動させる場合、図７（１）に示すように、移動位置は、一定の時間変化率で変化する。このとき理想的には、図７（３）に示すように駆動電流は一定値に調整される。そしてロボットアーム４０が衝突時刻Ｔ４で障害物Ｇに衝突すると、ステップＳ１に進み、アーム衝突時における動作を開始する。

ステップＳ１では、制御装置２０は、ロボットアーム４０が障害物Ｇに衝突して可動体４４が停止すると、エンコーダ値と移動指令値との位置偏差が大きくなり、衝突前に比べて指示電流指令値を大きくする。このとき電流制限手段５２によって、指示電流指令値が設定電流範囲以上となっても、駆動電流指令値が大きくなることが阻止され、設定電流範囲の上限値または下限値に応じた駆動電流がモータＭに与えられる。

またアーム衝突後、衝突検知手段５１によって衝突検知工程が行われ、図７に示す衝突検知時刻Ｔ１で、アーム４０が衝突したことを検知すると、ステップＳ２に進む。

ステップＳ２では、電流制限手段５２によって電流制限工程が行われ、モータＭに与える駆動電流をさらに制限する。また電流反転手段によって電流反転工程が行われ、衝突前とは逆向きに駆動電流をモータＭに流す。

衝突検知時刻Ｔ１から電流が制限されるまでには、第１遅れ時間Ｗ１がかかる。また衝突検知時刻Ｔ１から逆向きの電流がモータＭに流れるまでには、第２遅れ時間Ｗ２がかかる。たとえばこの第１遅れ時間Ｗ１は、約数ミリ秒である。また第２遅れ時間Ｗ２は、約数十ミリ秒である。このように第１遅れ時間Ｗ１は、第２遅れ時間Ｗ２に比べて十分短い。衝突検知時刻Ｔ１から逆向きの駆動電流がモータＭに流れる反転開始時刻Ｔ２に達すると、ステップＳ３に進む。

ステップＳ３では、反転開始時刻Ｔ２では、電流制限手段５２は、電流制限を行わず、指示電流指令値を駆動電流指令値として電流発生回路６１に与える。電流発生回路６１は、衝突前とは逆方向の駆動電流をモータＭに流す。これによって可動体４４は、後退方向に移動するトルクが与えられる。

この場合、上述したように電流制限手段５２は、過電流許容工程を行い、指示電流指令値を駆動電流指令値として無制限に電流発生回路６１に与える。これによって大きな駆動電流を発生することが可能となり、可動体４４を速やかに後退方向に移動させることができる。また衝突するまでの進行移動径路を逆にたどるような後退移動経路に従って可動体４４が移動するので、後退時に他の障害物に衝突することがない。

内部位置指令値生成部３０が、エンコーダ値に基づいて、可動体４４が障害物から予め定める移動距離後退したことを判断し、可動体４４が障害物Ｇから離れた状態となると、ステップＳ４に進む。

ステップＳ４では、内部位置指令値生成部３０は、可動体４４の変位駆動を停止するような内部指令値を生成する。また衝突解除信号を電流制限手段５２に与え、電流制限手段５２による電流の制限状態を衝突前の通常制限状態に移行して過電流許容解除工程を行い、ステップＳ５に進む。ステップＳ５では、制御装置２０は、衝突時のモータ駆動動作を終了する。

作業者は、ロボットアーム４０の移動径路プログラムを書き換え、障害物Ｇの除去などを行い、可動体４４と障害物Ｇとが衝突しないことを確認した後で、ティーチングペンダント７３などの入力手段によって、再起動信号を制御装置２０に与える。これによって切換スイッチ３１が切換り、外部位置指令値生成部６０からの位置指令値に従って、ロボットアーム４０を移動可能な状態となる。

なお上述の記載では、１つの可動体４４の制御に関する構成について、説明したが他の可動体４４についても同様である。複数の可動体４４を同時に移動させる場合には、それぞれの可動体４４に対応するモータＭが、それぞれ制御される。

以上のように本発明の実施の形態に従えば、アーム４０の衝突を検知してから電流の向きを反転してモータＭに電流を流すまでに費やす第２遅れ時間Ｗ２に比べて、アーム４０の衝突を検知してから電流を制限するまでに費やす第１遅れ時間Ｗ１のほうが短いので、電流制限手段５２によって駆動電流を制限することで、反転開始時刻Ｔ２に達する前に可動体４４が障害物Ｇを押付けようとする力を抑制することができ、衝突による衝撃を緩和することができる。

比較例として電流を制限しない場合には、ステップＳ２において、図７（３）に二点差線１００で示すように、可動体４４が障害物Ｇに衝突した後であっても、反転開始時刻Ｔ２に達するまで、アーム衝突前以上に大きな駆動電流がモータＭに与えられつづけ、可動体４４が障害物Ｇを大きな力で押付け続ける。したがって衝突による衝撃を緩和することができない。

これに対して本発明では、モータＭに与える電流を制限することで、衝突による衝撃を緩和することができ、モータＭ、減速器、アーム本体および障害物Ｇなどの衝突による損傷を低減することができる。

また本発明の実施の形態における電流制限手段５２の構成は、制御装置を構成する記憶部のプログラムを更新することによって実現することができ、非常に容易に実現することができる。また電流発生回路６１からの電流を制限して、衝突による衝撃を緩和する方法は、アーム４０の衝突検知方法に無関係であるので、アーム４０の衝突検知が遅れることがない。

また本発明の実施の形態では、反転開始時刻Ｔ２に達すると、モータＭに後退方向に移動するような電流が流れ、可動体４４に後退方向のトルクを与えることができる。これによって可動体４４を速やかに後退させて、可動体４４が障害物Ｇを押付ける状態を短時間で解消することができる。

また電流制限手段５２が、衝突前にモータＭに流れる方向の電流についてのみ制限し、衝突前にモータＭに流れる方向とは反対方向に流れる電流については、制限せずに通過させる。これによって可動体４４の速度などによって、アーム４０の衝突を検知してから電流の流れを反転するまでに費やす第２遅れ時間Ｗ２がばらつく場合であっても、電流の流れが反転すると直ちに逆向きの電流をモータＭに与えることができる。言い換えると、電流の制限を解除する時間を正確に決定する必要が無く、さらに利便性を向上することができる。

また指示電流指令値が反転した後に、設定電流範囲よりも大きい電流指令値が与えられた場合には、電流制限手段５２が制限せずにその指示電流指令値を駆動電流指令値として電流発生回路６１に与えることによって、電動モータＭに与える逆向きのトルクを大きくすることができ、可動体４４が障害物Ｇを押付ける時間をさらに短くすることができる。また正常状態では、設定電流範囲を超える電流および設定電流範囲未満の電流が流れることが防止され、モータＭおよび電流発生回路６１が損傷することを防止することができる。

また可動体４４が障害物Ｇから離反したことを判断すると、通常制限状態に移行することによって、モータＭおよび電流発生回路６１に長時間、大きい電流が流れることを防止することができ、モータＭおよび電流発生回路６１の損傷をさらに防止することができる。

また電流制限手段５２が、移動量指令手段５４によって演算された指示電流指令値を制限する。サーボ系の遅れによって衝突検知時から電流の流れる向きを反転する指示電流指令値を生成するまでには時間遅れがある。これに対して駆動電流指令値を制限することは、サーボ系の遅れにかかわらずに行うことができる。したがって衝突検知時から指示電流指令値を制限して駆動電流指令値として生成するまでにかかる時間は、電流の流れを反転する指示電流指令値を生成するまでにかかる時間よりも短い。したがって衝突検知後に、電流を反転する指示電流指令値が演算されるまでに、電流を制限した駆動電流指令値を演算することができる。これによって衝突検知後、モータＭに与えられる電流が反転する前に、モータＭに与えられる電流を速く低減することができる。

電流制限手段５２によって制限されてモータＭに流れる電流値は、ゼロまたは可及的に最小値にすることが好ましい。これによって可動体４４および障害物Ｇの衝突による衝撃をより緩和することができる。これに対し、モータＭに流れる電流が大きいと電流の制限が開始される制限開始時刻Ｔ３以降でも、可動体４４が障害物Ｇを押付ける力を十分に低減することができない。

またロボットアーム４０に重力などの外力が与えられる場合、電流制限手段５２によって制限されてモータＭに流れる電流は、可動体４４が重力などの外力によって変位しないトルクを発生するに必要な電流値以上に設定される。これによって可動体４４が、不所望に変位することを防止することができ、ロボットアーム４０が自重によって落下することを防ぐことができる。

アーム４０が外力を受ける場合には、アーム４０の重量、モータＭの発生可能トルクなどから、ロボットの姿勢に応じてその姿勢を維持し得る最低限の電流を演算し、その電流値を表わす駆動電流指令値を電流発生手段６１に与えてもよい。また予め演算される電流指令値の減少率を設定していてもよい。また姿勢毎に演算される減少率と、予め定められる減少率とを、アーム４０の状態に応じて切換えて用いてもよい。

さらに本実施の制御装置２０は、外部位置指令値とエンコーダ値とに基づいて、アーム４０の衝突を検知する。したがって実際にアーム４０が障害物Ｇに衝突したことを検知するセンサを必要とせず、コンピュータの演算回路を用いて衝突検知手段を実現することができる。したがって近接センサ、リミットスイッチおよび加速度センサなどの衝突検知センサを設ける必要がない。

またロボットコントローラの制御プログラムを更新することによって、衝突検知および衝突検知後のアーム制御を実現することができるので、衝突検知手段５４および反転指令手段のロボットへの実装に要する期間を短縮できる。また従来と同様の物理的構成で実現することができる。

また推定加速度偏差を用いてアーム衝突の検知を行うことによって、検知精度の向上が図られる。さらに推定速度偏差および推定加速度偏差を用いて衝突を判定することによって、衝突を検知するために複雑な計算、たとえばアーム４０に関する運動方程式の解を演算する必要がなく、衝突検知に要する時間を短縮できる。

またアーム衝突後のモータＭの制御方法において、アーム４０の衝突を検知すると、衝突前にモータＭの動作を継続させるために与えていた電流指令値を消去した後、アーム４０が後退方向に移動する電流指令値を与える。すなわち、駆動手段の被駆動部材に対する現在動作の維持を停止させた後、駆動手段の被駆動部材に衝突前の経路を逆にたどらせる。これによって衝突後におけるロボットアーム４０の後退動作を迅速になし得ることができる。さらに、衝突後に後退動作をさせているので、ロボットアーム４０の再起動を迅速になし得る。

図８は、本発明の他の実施の形態である電流制限手段１５２を示すブロック図である。他の実施の形態である電流制限手段１５２は、電流方向検出部８１に代えて反転開始時刻Ｔ２に達すると電流制限手段５２による電流の制限を解除するタイマ部１８１を有していてもよい。タイマ部１８１は、衝突検知信号が与えられると、制限部１８０に電流制限指令を与える。制御部１８０は、電流の向きにかかわらず指示電流指令値を制限する。

タイマ部１８１は、電流の向きを反転するために必要な第２遅れ時間Ｗ２が経過したと判断すると、制限部１８０に電流制限解除指令を与える。制御部１８０は、電流制限解除指令が与えられることによって、指示電流指令値の制限を解除する。このとき制御部１８０は、設定電流範囲以上の電流が流れることを許容する。これによって電流が反転した場合には、モータに与えられる電流が制限されることを防いで、指示電流指令値を駆動電流指令値として与えることができる。そして制御部１８０に衝突解除信号が与えられると、過電流許容状態を解除し、衝突前の通常制限状態に移行する。

このような他の実施の形態の電流制限手段１５２を有する場合であっても、上述と同様の効果を得ることができる。またこの場合、指示電流指令値から衝突前に流れる電流の方向を検出する必要がない。

またさらに他の形態の電流制限手段として、電流制限手段は、演算部７０またはティーチングペンダント７３などの入力手段からの解除指令に基づいて、電流の制限を解除してもよい。

なお、上述した本発明の構成は、発明の一例示であって発明の範囲内で構成を変更することができる。本発明の実施の形態では、推定速度偏差と推定加速度偏差とに基づいて、アーム４０の衝突を検知することが好ましいが、アーム４０の衝突検知にあたっては、特に限定せず従来技術を用いてもよい。

またロボットの制御方法および制御装置について説明したが、電動モータＭを有する駆動手段を備える装置であれば、同様の制御方法および制御装置を用いることができる。ロボット以外の産業機械、たとえばＮＣ（Numerical Control、数値制御）機械、搬送装置
などであっても、上述した制御方法および制御装置を用いることで、上述した効果と同様の効果を達成することができる。

また移動量指令手段５４の具体的な構成も、他の構成であってもよい。また制御装置２０は、プログラムを読取ることによって各演算器の構成を実現したが、電気回路などの物理的な構成によって各演算器の構成を実現してもよい。また本発明の制御装置２０は、ロボットコントローラと別体に設けられていてもよい。また指示電流指令値が反転した場合、与えられる指示電流指令値に対して予め定める増幅率で増大して駆動電流指令値を生成して、電流発生回路に与えてもよい。これによって可動体４４が障害物Ｇに押付けられる時間をさらに短縮することができる。

本発明の実施の一形態であるロボット１の主要な構成を示すブロック図である。 本発明のロボットアーム４０の関節構造を模式的に示す図である。 制御装置２０の物理的構成を示すブロック図である。 制御装置２０の機能的構成を示すブロック図である。 移動方向に流れる電流のみ制御する電流制限手段５２の構成の一例を示すブロック図である。 アーム衝突時における制御装置２０の動作手順を示すフローチャートである。 現在位置と電流制限状態と駆動電流との時間変化を示すタイムチャートである。 本発明の他の実施の形態である電流制限手段１５２を示すブロック図である。

符号の説明

１ ロボット
８ ロボット本体
２０ 制御装置
２８ エンコーダ
４０ ロボットアーム
４１ サーボ機構
４２ エンドエフェクタ
４４ 可動体
５１ 衝突検知手段
５２，１５２ 電流制限手段
５３ 後退指令手段
５４ 移動量指令手段
６１ 電流発生回路
Ｍ モータ

Claims (16)

1. 回転量を検出するエンコーダが設けられる電動モータを備えて構成され、電動モータによって可動体を変位駆動する駆動装置と、
   時間経過毎に移動すべき可動体の位置を表わす位置指令値と、エンコーダによって検出されるエンコーダ値とに基づいて、電動モータに与える電流値を表わす指示電流指令値を決定する制御装置であって、可動体が障害物に衝突しない正常状態において、指示電流指令値が予め定められる設定電流範囲内である場合、指示電流指令値を駆動電流指令値とし、指示電流指令値が設定電流範囲以上である場合には、設定電流範囲の上限値を駆動電流指令値とし、指示電流指令値が設定電流範囲以下である場合には、設定電流範囲の下限値を駆動電流指令値とする制御装置と、
   制御装置によって決定される駆動電流指令値が与えられ、駆動電流指令値の表わす電流の値に応じた駆動電流を発生し、発生した電流を電動モータに与える電流発生回路と、
   可動体が障害物に衝突したか否かを検出し、可動体が障害物に衝突したことを示す衝突検知信号を出力する衝突検知手段とを含むシステムにおいて、制御装置による駆動装置の制御方法であって、
   衝突検知手段から与えられる衝突検知信号与に応答して、可動体が障害物に衝突したことを検知する衝突検知工程と、
   衝突検知工程で可動体の衝突を検知すると、設定電流範囲よりもさらに制限される制限範囲で制限して駆動電流指令値を生成し、制限した駆動電流指令値を電流発生回路に与える電流制限工程と、
   衝突検知工程で可動体の衝突を検知すると、可動体が障害物に衝突するまでの進行移動経路を逆にたどるような時間経過毎の位置指令値を用いて、指示電流指令値を決定する電流反転工程とを含むことを特徴とする制御装置による駆動装置の制御方法。
2. 電流制限工程では、指示電流指令値を予め定める減少率で減少させた値を駆動電流指令値として生成することを特徴とする請求項１記載の制御装置による駆動装置の制御方法。
3. 電流制限工程では、予め定める一定値を駆動電流指令値として生成することを特徴とする請求項１記載の制御装置による駆動装置の制御方法。
4. 電流制限工程では、衝突直前に生成された指示電流指令値と同じ向きの指示電流指令値であって、衝突後に生成される指示電流指令値についてのみ制限して、駆動電流指令値を生成することを特徴とする請求項１〜３のいずれか１つに記載の制御装置による駆動装置の制御方法。
5. 衝突検知工程で可動体の衝突を検知してから、衝突後に生成される指示電流指令値が、衝突直前に生成された指示電流指令値と反対の向きとなると、電流制限工程における駆動電流指令値の制限を解除する電流制限解除工程をさらに含むことを特徴とする請求項１〜４のいずれか１つに記載の制御装置による駆動装置の制御方法。
6. 衝突検知工程で可動体の衝突を検知してから、衝突後に生成される指示電流指令値が、衝突直前に生成された指示電流指令値と反対の向きとなると、電流制限工程を終了して、衝突前に予め設定される設定電流範囲よりも大きい駆動電流指令値が電流発生回路に与えられることを許容する過電流許容工程をさらに含むことを特徴とする請求項１〜４のいずれか１つに記載の制御装置による駆動装置の制御方法。
7. 衝突検知工程で可動体の衝突を検知してから、エンコーダ値に基づいて、可動体が予め定める距離後退することを判断すると、過電流許容工程を終了する過電流許容解除工程をさらに含むことを特徴とする請求項６記載の制御装置による駆動装置の制御方法。
8. 電流制限工程によって制限される駆動電流指令値は、可動体が重力によって変位しないトルクを発生するに必要な電流値に応じた値以上に設定されることを特徴とする請求項１〜７のいずれか１つに記載の制御装置による駆動装置の制御方法。
9. 請求項１〜８のいずれか１つに記載の駆動装置による駆動装置の制御方法をコンピュータによって実行させるためのプログラム。
10. 請求項９記載のプログラムを記憶するコンピュータ読取可能な記録媒体。
11. 駆動量を検出する駆動量検出手段が設けられる電動モータを備えて構成され、電動モータによって可動体を変位駆動する駆動装置と、
    時間経過毎に移動すべき可動体の位置を表わす位置指令値と、駆動量検出手段によって検出される検出値とに基づいて、電動モータに与える電流値を表わす指示電流指令値を決定する制御装置と、
    制御装置によって決定される駆動電流指令値が与えられ、駆動電流指令値の表わす電流の値に応じた駆動電流を発生し、発生した電流を電動モータに与える電流発生回路と、
    可動体が障害物に衝突したか否かを検出し、可動体が障害物に衝突したことを示す衝突検知信号を出力する衝突検知手段とを含むシステムの一部を構成する制御装置であって、
    衝突検知手段から与えられる衝突検知信号に応答して、予め定められる制限範囲に制限した駆動電流指令値を生成し、制限した駆動電流指令値を電流発生回路に与える電流制限手段と、
    衝突検知手段から与えられる衝突検知信号に応答して、可動体が障害物に衝突するまでの進行移動経路を逆にたどるような時間経過毎の位置指令値を用いて、指示電流指令値を決定する電流反転手段とを含むことを特徴とする制御装置。
12. 電流制限手段は、指示電流指令値を予め定める減少率で減少させた値を駆動電流指令値として生成することを特徴とする請求項１１記載の制御装置。
13. 電流制限手段は、衝突直前に生成された指示電流指令値と同じ向きの指示電流指令値であって、衝突後に生成される指示電流値についてのみ制限して、駆動電流指令値を生成することを特徴とする請求項１１または１２記載の制御装置。
14. 可動体の衝突を検知してから、衝突後に生成される指示電流指令値が、衝突直前に生成された指示電流指令値と反対の向きとなると、電流制限手段における駆動電流指令値の制限を解除する電流制限解除手段をさらに含むことを特徴とする請求項１１〜１３のいずれか１つに記載の制御装置。
15. 電流制限手段によって制限される駆動電流指令値は、可動体が重力によって変位しないトルクを発生するに必要な電流値に応じた値以上に設定されることを特徴とする請求項１１〜１４のいずれか１つに記載の制御装置。
16. 電動モータを備えて構成される駆動装置と、
    駆動装置の電気モータによって変位駆動される可動体となるロボットアームと、
    請求項１１〜１５のいずれか１つに記載の制御装置とを含むことを特徴とするロボット。

Images