(57)【要約】 【課題】  衝突によって発生する衝撃を緩和し、各部材の損傷を低減することができる駆動手段の制御方法および制御装置を提供する。【解決手段】  ロボットアーム４０の衝突を検知すると、電流制限手段５２によって、電流発生回路６１からモータＭに流れる電流を制限する。衝突を検知してから、電流を反転するまでにかかる時間と電流を制限するまでにかかる時間とでは、電流を制限する時間のほうが短い。これによってロボットアーム４０が障害物Ｇに衝突してから、ロボットアーム４０を後退方向に変位させるまでに、遅れが生じる場合でも、ロボットアーム４０が障害物Ｇを押付けようとする力を抑制することができる。また衝突によって発生する衝撃を緩和することができ、ロボットアーム４０および障害物の損傷を低減することができる。【選択図】      図１
2005059171.tif 000002
【特許請求の範囲】
【請求項１】
電動モータを備えて構成される駆動手段の制御方法であって、
駆動手段によって変位駆動される可動体が障害物に衝突したことを検知する衝突検知工程と、
可動体の衝突を検知すると、電動モータに与えられる電流を制限する電流制限工程と、
可動体の衝突を検知すると、電動モータに与えられる電流の流れる向きを反転する電流反転工程とを含むことを特徴とする駆動手段の制御方法。
【請求項２】
電流制限工程では、衝突直前に電動モータに与えられる電流と同じ向きに流れる電流のみを制限することを特徴とする請求項１記載の駆動手段の制御方法。
【請求項３】
可動体の衝突が検知されてから、電動モータに与えられる電流の向きが反転すると、電流制限工程における電流制限を解除する電流制限解除工程をさらに含むことを特徴とする請求項１または２記載の駆動手段の制御方法。
【請求項４】
可動体の衝突が検知されてから、電動モータに与えられる電流の向きが反転すると、衝突前に予め設定される設定電流範囲よりも大きい電流が電動モータに流れることを許容する過電流許容工程をさらに含むことを特徴とする請求項１〜３のいずれかに記載の駆動手段の制御方法。
【請求項５】
可動体の衝突が検知されてから、可動体が障害物から離反すると過電流許容工程における大きい電流の許容を解除する過電流許容解除工程をさらに含むことを特徴とする請求項４記載の駆動手段の制御方法。
【請求項６】
電流制限工程によって制限される電流は、可動体が外力によって変位しないトルクを発生するに必要な電流以上に設定されることを特徴とする請求項１〜５のいずれかに記載の駆動手段の制御方法。
【請求項７】
電流制限工程では、可動体の移動量に関して演算される指示電流指令値を制限して駆動電流指令値を生成し、駆動電流指令値に応じた駆動電流が電動モータに与えられることで、電動モータに与えられる電流を制限することを特徴とする請求項１〜６のいずれかに記載の駆動手段の制御方法。
【請求項８】
衝突検知工程では、予め指令される位置指令値と検知される電動モータの角変位位置とに基づいて推定速度偏差および推定加速度偏差を演算する演算手順と、
推定速度偏差および推定加速度偏差の少なくともいずれかが、それぞれ予め定めるしきい値を超えた場合に、可動体が衝突したことを検知する検知手順とを有することを特徴とする請求項１〜７のいずれかに記載の駆動手段の制御方法。
【請求項９】
請求項１〜８のいずれかに記載の駆動手段の制御方法をコンピュータによって実現させるためのプログラム。
【請求項１０】
請求項９記載のプログラムを記憶するコンピュータ読取可能な記録媒体。
【請求項１１】
電動モータを備えて構成される駆動手段の制御装置であって、
駆動手段によって駆動される可動体が障害物に衝突したことを検知する衝突検知手段と、
衝突検知手段の検知結果に基づいて、電動モータに与えられる電流を制限する電流制限手段と、
衝突検知手段の検知結果に基づいて、電動モータに与えられる電流の流れる向きを反転する電流反転手段と、
電流制限手段および電流反転手段によって与えられる指令に基づいて、電動モータに与える電流を発生する電流発生回路とを含むことを特徴とする駆動手段の制御装置。
【請求項１２】
電流制限手段は、衝突前に電動モータに与えられる電流と同じ向きに流れる電流のみを制限することを特徴とする請求項１１記載の駆動手段の制御装置。
【請求項１３】
可動体の衝突が検知されてから、電動モータに与えられる電流の向きが反転すると、電流制限手段における電流制限を解除する電流制限解除手段をさらに含むことを特徴とする請求項１１または１２記載の駆動手段の制御装置。
【請求項１４】
電流制限手段によって制限される電流は、可動体が外力によって変位しないトルクを発生するに必要な電流以上に設定されることを特徴とする請求項１１〜１３のいずれかに記載の駆動手段の制御装置。
【請求項１５】
電流制限手段は、可動体の移動量に関して演算される指示電流指令値を制限して駆動電流指令値として電流発生回路に与えることを特徴とする請求項１１〜１４のいずれかに記載の駆動手段の制御装置。
【請求項１６】
電動モータを備えて構成される駆動手段と、
駆動手段によって変位駆動されるロボットアームと、
請求項１１〜１５のいずれかに記載の制御装置とを含むことを特徴とするロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、駆動手段の制御方法および制御装置に関する。さらに詳しくは、ロボットアームが障害物に衝突した場合、その衝突を検知して、この衝突により発生する各部材の損傷を最小限に留めるようにロボットアームのモータを制御する制御方法および制御装置に関する。
【背景技術】
【０００２】
ロボットアームと障害物との衝突を検知する技術に関して、さまざまな提案がなされている。たとえばロボットに設けられるサーボモータにトルクセンサを取付け、トルクセンサの乱れから衝突を検知する方法が開示されている（特許文献１参照）。
【０００３】
また他の従来技術として、ロボットアームの衝突が検知された後に、衝突前に与えられるトルクとは逆向きのトルクをロボットアームに与えるように、サーボモータに電流を与える技術が開示されている（たとえば特許文献２および特許文献３参照）。
【０００４】
【特許文献１】特開平０８３９４号公報
【特許文献２】特開平７−１４３７８０号公報
【特許文献３】特開２００１−１１７６１８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
上述したサーボモータの制御方法では、ロボットアームの衝突によって発生する各部材の損傷を抑制する点に関し、充分に考慮されているとは言えない。というのは、衝突してから逆向きのトルクをロボットアームに与えるまでに遅れが生じるからである。この遅れた時間の間、ロボットアームは障害物を押し続ける。
【０００６】
遅れ時間の経過中には、ロボットアームが障害物を押付けているので、ロボットアームおよび障害物の損傷の程度を抑制することができない。またロボットアームの衝突によって発生する衝撃を緩和することが困難である。このことは、電動モータを備えるロボット以外のモータの制御方法であっても同様である。
【０００７】
したがって本発明の目的は、衝突によって発生する衝撃を緩和し、各部材の損傷を低減することができる駆動手段の制御方法および制御装置を提供することである。
【課題を解決するための手段】
【０００８】
請求項１記載の本発明は、電動モータを備えて構成される駆動手段の制御方法であって、
駆動手段によって変位駆動される可動体が障害物に衝突したことを検知する衝突検知工程と、
可動体の衝突を検知すると、電動モータに与えられる電流を制限する電流制限工程と、
可動体の衝突を検知すると、電動モータに与えられる電流の流れる向きを反転する電流反転工程とを含むことを特徴とする駆動手段の制御方法である。
【０００９】
請求項２記載の本発明は、電流制限工程では、衝突直前に電動モータに与えられる電流と同じ向きに流れる電流のみを制限することを特徴とする。
【００１０】
請求項３記載の本発明は、可動体の衝突が検知されてから、電動モータに与えられる電流の向きが反転すると、電流制限工程における電流制限を解除する電流制限解除工程をさらに含むことを特徴とする。
【００１１】
請求項４記載の本発明は、可動体の衝突が検知されてから、電動モータに与えられる電流の向きが反転すると、衝突前に予め設定される設定電流範囲よりも大きい電流が電動モータに流れることを許容する過電流許容工程をさらに含むことを特徴とする。
【００１２】
請求項５記載の本発明は、可動体の衝突が検知されてから、可動体が障害物から離反すると過電流許容工程における大きい電流の許容を解除する過電流許容解除工程をさらに含むことを特徴とする。
【００１３】
請求項６記載の本発明は、電流制限工程によって制限される電流は、可動体が外力によって変位しないトルクを発生するに必要な電流以上に設定されることを特徴とする。
【００１４】
請求項７記載の本発明は、電流制限工程では、可動体の移動量に関して演算される指示電流指令値を制限して駆動電流指令値を生成し、駆動電流指令値に応じた駆動電流が電動モータに与えられることで、電動モータに与えられる電流を制限することを特徴とする。
【００１５】
請求項８記載の本発明は、衝突検知工程では、予め指令される位置指令値と検知される電動モータの角変位位置とに基づいて推定速度偏差および推定加速度偏差を演算する演算手順と、
推定速度偏差および推定加速度偏差の少なくともいずれかが、それぞれ予め定めるしきい値を超えた場合に、可動体が衝突したことを検知する検知手順とを有することを特徴とする。
【００１６】
請求項９記載の本発明は、前記駆動手段の制御方法をコンピュータによって実現させるためのプログラムである。
【００１７】
請求項１０記載の本発明は、前記プログラムを記憶するコンピュータ読取可能な記録媒体である。
【００１８】
請求項１１記載の本発明は、電動モータを備えて構成される駆動手段の制御装置であって、
駆動手段によって駆動される可動体が障害物に衝突したことを検知する衝突検知手段と、
衝突検知手段の検知結果に基づいて、電動モータに与えられる電流を制限する電流制限手段と、
衝突検知手段の検知結果に基づいて、電動モータに与えられる電流の流れる向きを反転する電流反転手段と、
電流制限手段および電流反転手段によって与えられる指令に基づいて、電動モータに与える電流を発生する電流発生回路とを含むことを特徴とする駆動手段の制御装置である。
【００１９】
請求項１２記載の本発明は、電流制限手段は、衝突前に電動モータに与えられる電流と同じ向きに流れる電流のみを制限することを特徴とする。
【００２０】
請求項１３記載の本発明は、可動体の衝突が検知されてから、電動モータに与えられる電流の向きが反転すると、電流制限手段における電流制限を解除する電流制限解除手段をさらに含むことを特徴とする。
【００２１】
請求項１４記載の本発明は、電流制限手段によって制限される電流は、可動体が外力によって変位しないトルクを発生するに必要な電流以上に設定されることを特徴とする。
【００２２】
請求項１５記載の本発明は、電流制限手段は、可動体の移動量に関して演算される指示電流指令値を制限して駆動電流指令値として電流発生回路に与えることを特徴とする。
【００２３】
請求項１６記載の本発明は、電動モータを備えて構成される駆動手段と、
駆動手段によって変位駆動されるロボットアームと、
前記制御装置とを含むことを特徴とするロボットである。
【発明の効果】
【００２４】
請求項１記載の本発明によれば、衝突検知工程で可動体が障害物に衝突したことを検知すると、電流反転工程でモータに与えられる電流の流れを反転する。これによって衝突後に、可動体に衝突前と逆向きのトルクが与えられる。これによって衝突前に移動していた方向と逆向きに可動体を移動させることができ、可動体が障害物を押付ける時間を短くすることができる。
【００２５】
また衝突検知工程で可動体が障害物に衝突したことを検知すると、電流制限工程でモータに与えられる電流を制限する。衝突を検知してから、電流を反転するまでにかかる時間と電流を制限するまでにかかる時間とでは、電流を制限する時間のほうが短い。電流反転工程でモータに与える電流の流れる向きを反転させるまでに遅れが生じる場合でも、アーム衝突後にモータに流れる電流を制限することで、可動体が障害物を押付けようとする力を抑制することができる。すなわち衝突によって発生する衝撃を緩和することができ、可動体および障害物の損傷を低減することができる。
【００２６】
また請求項２記載の本発明によれば、衝突前にモータに与えられる電流と同じ向きに流れる電流のみを制限することによって、電流反転工程において電流の流れを反転した場合に、逆向きに流れる電流が制限されることがない。これによって衝突による衝撃を緩和したうえで、可動体が障害物を押付ける時間を短くすることができる。また電流の流れを反転させるまでに必要な時間がばらつく場合であっても、電流の流れが反転すると直ちに逆向きに流れる電流をモータに与えることができる。
【００２７】
また請求項３記載の本発明によれば、電流制限工程で、電流の流れる方向にかかわらずモータに与えられる電流を制限し、電流の向きが反転すると電流の制限を解除する。これによって電流の反転前について電流を制限することができる。
【００２８】
たとえば衝突を検知してから電流が反転するまでに費やす反転時間を予め取得し、衝突検知してから反転時間が経過すると電流の制限を解除することで、電流の向きが反転するまで電流を制限することができる。またたとえば電流の向きが反転したことを検出すると、電流の制限を解除してもよい。
【００２９】
また請求項４記載の本発明によれば、過電流許容工程で、設定電流範囲よりも大きい電流が電動モータに与えられることを許容することで、可動体に与えられる逆向きのトルクを大きくすることができ、可動体が障害物を押付ける時間をさらに短くすることができる。
【００３０】
また請求項５記載の本発明によれば、可動体が障害物から離反すると電動モータへ大きい電流が流れることを解除することによって、いつまでもモータに大きい電流が流れ続けることを防いで、モータの損傷を防ぐことができる。
【００３１】
また請求項６記載の本発明によれば、電流制限工程で、モータに与えられる電流が制限されたとしても、可動体が外力によって変位することを防止することができる。これによって電流制限工程において、可動体が不所望に変位することを防止することができる。たとえば外力として重力が可動体に与えられる場合であっても、可動体の自重による落下を防ぐことができる。
【００３２】
また請求項７記載の本発明によれば、指示電流指令値を制限して駆動電流指令値を演算する。衝突検知時から電流の流れる向きを反転する指示電流指令値を生成するまでには、サーボ系の遅れによって時間遅れがある。これに対して指示電流指令値を制限するには、サーボ系の遅れにかかわらずに行うことができる。これによって衝突検知時から指示電流指令値を制限して駆動電流指令値として生成するまでにかかる時間は、電流の流れを反転する指示電流指令値を生成するまでにかかる時間よりも短い。
【００３３】
したがって衝突検知後に、電流を反転する指示電流指令値が演算されるまでに、電流を制限した駆動電流指令値を生成することができる。すなわち衝突検知後、モータに与えられる電流が反転する前に、モータに与えられる電流を低減することができ、可動体が衝突してから逆方向に移動するまでに、可動体が障害物を押付ける力をすばやく低減することができる。
【００３４】
たとえば駆動電流指令値を制限する場合には、指示電流指令値を予め定める減少率で減少させて駆動電流指令値としてもよい。また指示電流指令値を、重力などの外力に対して姿勢を維持し得る最低限の電流を表わす駆動電流指令値に制限してもよい。また指示電流指令値にかかわらず、予め定める一定の駆動電流指令値としてもよい。
【００３５】
また請求項８記載の本発明によれば、推定速度偏差および推定加速度偏差の少なくともいずれかがしきい値を超えた場合に、可動体が衝突したことを検知する。推定速度偏差および推定加速度偏差は、位置指令値と角変位位置とに基づいて、可動体が衝突したことを検知することができるので、別途衝突検知手段を設ける必要がない。これによって簡単な構成によって、可動体と障害物との衝突を検知できる。
【００３６】
また請求項９記載の本発明によれば、コンピュータがプログラムを読取ることによって上述した制御方法が実現される。これによって瞬時にモータに流れる電流を制限することとができ、可動体が障害物に衝突して障害物を押付ける時間をさらに短縮することができる。すなわち衝突による衝撃をより確実に緩和することができる。
【００３７】
また請求項１０記載の本発明によれば、記憶媒体に記憶されるプログラムをコンピュータが読取ることによって、上述した制御方法が実現される。
【００３８】
また請求項１１記載の本発明によれば、衝突検知手段によって可動体が障害物に衝突したことを検知すると、電流反転手段によってモータに与えられる電流の流れを反転する。これによって可動体に逆向きのトルクを与えることができ、可動体が障害物を押付ける時間を短くすることができる。
【００３９】
また衝突検知手段によって可動体が障害物に衝突したことを検知すると、電流制限手段によって、モータに与えられる電流を制限する。衝突を検知してから、電流を反転するまでにかかる時間と電流を制限するまでにかかる時間とでは、電流を制限する時間のほうが短い。可動体が障害物に衝突してから、モータに与える電流の流れる向きを反転させるまでに遅れが生じる場合でも、電流制限手段によって衝突後にモータに与えられる電流を制限することで、可動体が障害物を押付けようとする力を抑制することができる。すなわち衝突によって発生する衝撃を緩和することができ、可動体および障害物の損傷を低減することができる。
【００４０】
また請求項１２記載の本発明によれば、衝突前にモータに与えられる電流と同じ向きに流れる電流のみを制限することによって、電流反転手段によって電流の流れを反転した場合に、逆向きの電流の流れが制限されることがない。これによって衝突による衝撃を緩和したうえで、可動体が障害物を押付ける時間を短くすることができる。また電流の流れを反転するまでに必要な時間がばらつく場合であっても、電流の流れが反転すると直ちに逆向きに流れる電流をモータに与えることができる。
【００４１】
また請求項１３記載の本発明によれば、電流制限解除手段によって電動モータに与えられる電流の向きにかかわらず、モータに与える電流を制限し、電流の向きが反転すると電流の制限を解除する。これによって電流の反転前について電流を制限することができる。
【００４２】
たとえば衝突を検知してから電流が反転するまでに費やす反転時間を予め取得し、衝突検知してから反転時間が経過すると電流の制限を解除することで、電流の向きが反転するまで電流を制限することができる。またたとえば電流の向きが反転したことを検出すると、電流の制限を解除してもよい。
【００４３】
また請求項１４記載の本発明によれば、電流制限手段によって、モータに与えられる電流が制限されたとしても、可動体が外力によって不所望に変位することを防止することができる。たとえば外力として重力が可動体に与えられる場合、モータの電流を制限したとしても、可動体の自重による落下を防ぐことができる。
【００４４】
また請求項１５記載の本発明によれば、指示電流指令値を制限して駆動電流指令値を演算する。衝突検知時から電流の流れる向きを反転する指示電流指令値を生成するまでには、サーボ系の遅れによって時間遅れがある。これに対して指示電流指令値を制限するには、サーボ系の遅れにかかわらずに行うことができる。これによって衝突検知時から指示電流指令値を制限して駆動電流指令値として生成するまでにかかる時間は、電流の流れを反転する指示電流指令値を生成するまでにかかる時間よりも短い。
【００４５】
したがって衝突検知後に、電流を反転する指示電流指令値が演算されるまでに、電流を制限した駆動電流指令値を電流発生回路に与えることができる。これによって衝突検知後、モータに与えられる電流が反転する前に、モータに与えられる電流を低減することができ、可動体が衝突してから逆方向に移動するまでに、可動体が障害物を押付ける力を低減することができる。また電流指令値を変更して、モータに与える電流を反転および制限することによって、ソフトウェアによって本発明の制御装置を実現することができ、ハードウェアに比べて容易に実現することができる。
【００４６】
たとえば駆動電流指令値を制限する場合には、指示電流指令値を予め定める減少率で減少させて駆動電流指令値としてもよい。また指示電流指令値を、重力などの外力に対して姿勢を維持し得る最低限の電流を表わす駆動電流指令値に制限してもよい。また指示電流指令値にかかわらず、予め定める一定の駆動電流指令値としてもよい。
【００４７】
また請求項１６記載の本発明によれば、ロボットアームが障害物に衝突した場合に、衝突によって発生する衝撃を緩和することができ、各部材の損傷を低減することができる。これによってロボットアームの移動経路が用途によって異なる場合であって、衝突位置が予想できない場合であっても、障害物とロボットアームとの衝突時の衝撃を有効に緩和することができる。
【発明を実施するための最良の形態】
【００４８】
図１は、本発明の実施の一形態であるロボット１の主要な構成を示すブロック図である。ロボット１は、ロボット本体８と、ロボット本体８を制御する制御装置２０とを含む。たとえばロボット１は、汎用ロボットであって６自由度を有する垂直多関節型ロボットである。ロボット本体８は、ロボットアーム４０と、サーボ機構４１とを含んで構成される。ロボットアーム４０は、変位可能に設けられる。サーボ機構４１は、制御装置２０の指令に従ってロボットアーム４０を変位駆動する。
【００４９】
制御装置２０は、ロボットアーム４０が障害物Ｇに衝突したことを検知すると、サーボ機構４１を制御し、衝突によって発生する各部材の損傷を可及的に抑制するようにロボットアーム４０を動作させる。
【００５０】
サーボ機構４１は、アーム４０を変位駆動するための駆動手段と、駆動手段の動力をアーム４０に伝達するための伝達機構とを含む。なお、駆動手段は、駆動量を検出する駆動量検出手段が設けられる。本実施の形態では、駆動手段は、電動モータ、たとえばサーボモータによって実現される。サーボモータには、駆動量検出手段としてモータの回転量を検出するエンコーダ２８が設けられる。
【００５１】
制御装置２０は、電流発生回路６１と、移動量指令手段５４と、衝突検知手段５１と、後退指令手段５３と、電流制限手段５２とを含む。移動量指令手段５４は、アーム４０を予め定める位置に移動させるために必要な電流指令値を演算する。アーム４０が障害物Ｇに衝突しない正常状態では、移動量指令手段５４が演算した電流指令値が、電流発生回路６１に与えられる。電流発生回路６１は、与えられる電流指令値に基づいて電流を発生し、発生した電流をサーボモータに流す。電流発生回路６１は、電流指令値に応じてモータの駆動電流を発生する増幅器、いわゆるサーボアンプである。
【００５２】
衝突検知手段５１は、ロボットアーム４０が障害物Ｇに衝突したことを検知し、衝突したことを示す衝突検知信号を出力する。後退指令手段５３は、衝突検知手段５１から衝突検知信号が与えられると、衝突するまでの進行移動径路を逆にたどるようなロボットアーム４０の後退移動経路を演算し、演算した後退移動径路を示す情報を移動量指令手段５４に与える。移動量指令手段５４は、後退指令手段５３から後退移動径路を示す情報が与えられると、その後退移動径路に基づいた電流指令値を演算して電流発生回路６１に与える。
【００５３】
電流制限手段５２は、衝突検知手段５１から衝突検知信号が与えられると、移動量指令手段５４が演算した電流指令値を制限し、制限した電流指令値を電流発生回路６１に与える。これによってアーム衝突検知時には、電流発生回路６１から出力される電流は、電流制限前に比べて減少されて、モータに与えられる。
【００５４】
本発明において電流を制限することは、移動量指令手段５４の電流指令値を予め定める減少率で減少させる場合、移動量指令手段５４の電流指令値にかかわらず予め定める一定値に制限する場合のいずれであってもよい。また予め定める減少率は、０％である場合も含む。この場合、電流制限時には、電流発生回路６１からサーボモータに与えられる電流はゼロであり、モータに電流が流れることが阻止される。
【００５５】
制御装置２０は、いわゆるロボットコントローラによって実現される。この場合、本発明のロボットコントローラは、通常のロボットコントローラに比べて、衝突検知手段５１、電流制限手段５２および後退指令手段５３をさらに備える構成である。したがって衝突検知手段５１、電流制限手段５２および後退指令手段５３以外の他の構成については、通常のロボットコントローラと同様の構成であってもよい。
【００５６】
図２は、本発明のロボットアーム４０の関節構造を模式的に示す図である。ロボットアーム４０は、第１〜第６の可動体４４ａ〜４４ｆを有する。各可動体４４ａ〜４４ｆは、アーム軸１１〜１６と、関節部２〜７とによってそれぞれ構成される。
【００５７】
各可動体４４ａ〜４４ｆは、直列に連結され、相互に変位可能に設けられる。各可動体４４ａ〜４４ｆのうち、直列方向一方側の第１可動体４４ａは、ベース４３に固定される。また各可動体４４ａ〜４４ｆのうち、直列方向他方側の第６可動体４４ｆは、エンドエフェクタ４２が連結される。エンドエフェクタ４２は、ロボットハンドや溶接ツールなどであり、ロボットアームの変形動作によって予め定める姿勢で予め定める位置に変位駆動される。
【００５８】
第１〜第６可動体４４ａ〜４４ｆは、第１可動体４４ａから第６可動体４４ｆまで、順に連結され、それぞれ対応する角変位軸線を有する。各可動体４４ａ〜４４ｆは、対応する角変位軸線まわりに角変位可能に連結される。
【００５９】
第１可動体４４ａに対応する第１角変位軸線は鉛直に延びる。第１可動体４４ａは、第１関節部２によって、ベース４３に対して第１角変位軸線まわりに角変位可能に設けられる。第２可動体４４ｂに対応する第２角変位軸線は、第１角変位軸線に対して垂直に延びる。第２可動体４４ｂは、第２関節部３によって、第１可動体４４ａに対して第２角変位軸線まわりに角変位可能に設けられる。
【００６０】
第３可動体４４ｃに対応する第３角変位軸線は、第２角変位軸線に対して垂直に延びる。第３可動体４４ｃは、第３関節部４によって、第２可動体４４ｂに対して第３角変位軸線まわりに角変位可能に設けられる。第４可動体４４ｄに対応する第４角変位軸線は、第３角変位軸線と同軸に延びる。第４可動体４４ｄは、第４関節部５によって、第３可動体４４ｃに対して第４角変位軸線まわりに角変位可能に設けられる。
【００６１】
第５可動体４４ｅに対応する第５角変位軸線は、第４角変位軸線に対して垂直に延びる。第５可動体４４ｅは、第５関節部６によって、第４可動体４４ｄに対して第５角変位軸線まわりに角変位可能に設けられる。第６可動体４４ｆに対応する第６角変位軸線は、第５角変位軸線と同軸に延びる。第６可動体４４ｆは、第６関節部７によって、第５可動体４４ｅに対して第６角変位軸線まわりに角変位可能に設けられる。
【００６２】
第１可動体４４ａ、第４可動体４４ｄ、第６可動体４４ｆは、いわゆる旋回軸を構成する。第２可動体４４ｂ、第３可動体４４ｃ、第５可動体４４ｅは、いわゆる回転軸を構成する。また各可動体４４ａ〜４４ｆは、それぞれ対応する第１〜第６角変位軸線まわりに角変位駆動される各サーボ機構４１がそれぞれ設けられる。
【００６３】
このように本発明の実施の一形態であるロボット１は、垂直多関節型の６自由度ロボットである。なお、上述したロボット１は、本発明の一例示であって、この構成に限定されない。たとえば本発明の制御装置２０は、半導体ウェハを搬送する円筒座標型のロボットに搭載されてもよい。また図２において参照符号Ｇは、障害物を示す。
【００６４】
図３は、制御装置２０の物理的構成を示すブロック図である。制御装置２０は、コンピュータによって実現され、予め定められるプログラムを実行することによって、上述する移動量指令手段５４、衝突検知手段５１、後退指令手段５３、電流制限手段５２を実現することができる。またコンピュータによって制御装置２０が実現されることで、モータを瞬時に制御することができる。
【００６５】
制御装置２０は、演算部７０と、記憶部７１と、インターフェース部７２と、電流発生回路６１とを含んで構成される。記憶部７１は、後述する各演算器の全てまたは一部の機能を実現するためのプログラムを記憶する。演算部７０は、記憶部７１に記憶されるプログラムを読取り、プログラムを実行することによって各演算器の全てまたは一部の機能を実現することができる。これによって本発明の各手段を物理的に有していない制御装置であっても、記憶媒体に記憶されるプログラムを読取ることによって構成を大きく変更することなく、本発明の制御装置２０として機能させることができる。すなわち従来の制御装置６１に比べて、ソフトウェアを変更するだけでよく、新たにハードウェアを加える必要がないので、容易に本発明の制御装置２０を実現することができる。
【００６６】
演算部７０は、たとえばＣＰＵ（Central Processing Unit）によって実現される。また記憶部７１は、たとえばＲＡＭ（Random Access Memory）およびＲＯＭ（Read Only
memory）などによって実現される。
【００６７】
各演算器の機能を実現するためのプログラムは、別途コンピュータ読取り可能な記憶媒体に記憶されてもよい。この場合、演算部７０が記憶媒体に記憶されるプログラムを読取り、そのプログラムを実行することによって、各演算器全てまたは一部の機能を実現することができる。またインターフェース部７２は、演算部７０と記憶部７１との情報伝達を行う。またインターフェース部７２は、入力手段となるティーチングペンダント７３、位置検知手段となるエンコーダ２８および電流発生回路６１と、演算部７０との情報伝達を行う。
【００６８】
演算部７０は、ティーチングペンダント７３から与えられる情報または記憶部７１に記憶される情報に基づいて外部位置指令値を生成する。また演算部７０は、エンコーダ２８から検出位置情報が与えられる。これらの情報に基づいて、電流指令値を演算し、演算した電流指令値を電流発生回路６１に与える。電流発生回路６１は、電流指令値に従って電流を発生し、その電流をモータＭに与える。
【００６９】
またアーム４０が障害物Ｇに衝突すると、演算部７０は、位置指令値とエンコーダ値とに基づいて、アーム４０が障害物Ｇに衝突したか否かを検知する。そして演算部７０は、アーム４０の衝突を検知すると、電流指令値を制限してモータＭに与えられる電流を制限するとともに、可動体４４が後退方向に移動するような電流指令値を生成して電流発生回路６１に与える。
【００７０】
図４は、制御装置２０の機能的構成を示すブロック図である。図３に示す演算部７０が記憶部７１に記憶されるプログラムを実行することによって、図４に示す移動量指令手段５４、衝突検知手段５１、後退指令手段５３、電流制限手段５２の機能を実現することができる。
【００７１】
移動量指令手段５４は、予め定められる位置指令値とエンコーダ２８によるエンコーダ値との偏差に基づいて、各サーボ機構のモータＭを駆動するための移動指令値を決定する。具体的には、移動量指令手段５４は、外部位置指令値生成部６０と、位置偏差算出器３２と、第１比例器３３と、速度偏差算出器３４と、第２比例器３５と、第３比例器３６と、積分器３７と、加算器３８と、電流発生回路６１と、フィードバック速度算出器２９とを含む。
【００７２】
外部位置指令値生成部６０は、予め教示されるロボットアーム４０の移動径路に従って位置指令値を生成する。位置指令値は、時間経過毎に移動すべき各可動体４４ａ〜４４ｆの位置を表わす情報である。したがって各可動体４４ａ〜４４ｆを移動させる場合には、位置指令値も時間毎に変化する。
【００７３】
各移動体４４ａ〜４４ｆの移動径路は、単にエンドエフェクタ４２の移動径路が制御装置２０に設けられる記憶手段に記憶されていてもよい。この場合、外部位置指令値生成部６０は、記憶手段からエンドエフェクタ４２の移動経路を読み出して、各移動体４４ａ〜４４ｆの移動経路を算出して、各移動体４４ａ〜４４ｆに対応する外部位置指令値をそれぞれ生成する。
【００７４】
以後、複数のうち１つの可動体４４に対応するサーボ機構４１のモータＭに関する制御装置２０の構成を説明する。残余の可動体４４に対応するサーボ機構のモータＭに関する制御装置２０の構成は、１つの可動体４４に関する構成と同様の構成であるので説明を省略する。
【００７５】
位置偏差算出器３２は、ロボットアーム４０が障害物Ｇに衝突しない正常状態では、外部位置指令値生成部６０から位置指令値が与えられ、ロボットアーム４０が障害物Ｇに衝突したアーム衝突時には、後述する内部指令値生成部３０から位置指令値が与えられる。また位置偏差算出手段３２は、エンコーダ２８から検出位置情報が与えられる。検出位置情報は、エンコーダ２８のエンコーダ値であって、可動体４４の位置を表わす情報である。
【００７６】
位置偏差算出器３２は、位置指令値の表わす位置から検出位置情報の表わす位置を減算して、位置偏差を表わす位置偏差情報を演算する。位置偏差算出器３２は、第１比例器３３に演算した位置偏差情報を与える。
【００７７】
第１比例器３３は、位置偏差算出器３２から与えられる位置偏差情報の表わす位置偏差に、予め定める第１係数Ｋｐを乗算して、可動体４４の速度を表わす速度情報を演算する。第１比例器３３は、演算した速度情報を速度偏差算出器３４に与える。
【００７８】
またフィードバック速度算出器２９は、エンコーダ２８から時間経過ごとに検出位置情報が随時与えられ、その検出位置情報の表わす位置を時間で微分して速度を表わすフィードバック速度情報を演算する。フィードバック速度算出器２９は、演算したフィードバック速度情報を、速度偏差算出器３４および後述する推定速度偏差算出器２３に与える。
【００７９】
速度偏差算出器３４は、第１比例器３３から与えられる速度情報の表わす速度から、フィードバック速度情報の表わす速度を減算して、速度偏差を表わす速度偏差情報を演算する。速度偏差算出器３４は、速度偏差情報を第２比例器３５に与える。第２比例器３５は、速度偏差算出器３４から与えられる速度偏差情報の表わす速度偏差に、予め定める第２係数Ｋｖｐを乗算して、モータＭに与える第１の電流の変化量を表わす１次電流指令値を演算する。
【００８０】
第３比例器３６は、１次電流指令値の表わす電流の変化量に予め定める第３係数Ｋｖｉを乗算して、第２の電流の変化量を表わす２次電流指令値を演算し、２次電流指令値を積分器３７に与える。積分器３７は、２次電流指令値の表わす第２の電流の変化量を時間で積分し、第２の電流の変化量の積分値を表わす積分電流指令値を演算する。
【００８１】
加算器３８は、積分電流指令値と１次電流指令値とが与えられる。加算器３８は、与えられるそれぞれの指令値の表わす電流値を互いに加算し、指示電流指令値を演算する。指示電流指令値は、モータＭに与える電流の値を表わす情報である。加算器３８は、指示電流指令値を電流制限手段５２に与える。
【００８２】
電流制限手段５２は、正常状態すなわちアーム４０が障害物Ｇに衝突していない状態では、指示電流指令値を制限せずに、指示電流指令値を駆動電流指令値として電流発生回路６１に与える。電流発生回路６１は、駆動電流指令値の表わす電流の値に応じた駆動電流を発生し、発生した電流をモータＭに流す。
【００８３】
電流制限手段５２は、アーム４０が障害物Ｇに衝突したアーム衝突状態以降では、指示電流指令値を予め定める減少率で減少して制限し、それを駆動電流指令値として電流発生回路６１に与える。電流発生回路６１は、駆動電流指令値の表わす電流の値に応じた駆動電流を発生し、発生した電流をモータＭに流す。モータＭは、電流発生回路６１から電流が流れることによって可動体４４を変位駆動する。
【００８４】
なお、加算器３８で、１次電流指令値が示す電流値と、積分電流指令値が示す電流値とを加算して指示電流指令値として演算するのは、可動体４４の移動動作を維持させるためである。たとえば可動体４４を一定速度で移動させる場合、目標速度に到達して速度偏差がゼロとなり、１次電流指令値がゼロとなった場合であっても、積分電流指令値が加算器３８に与えられることで、可動体４４の速度を維持して動作させることができる。すなわち加算器３８は、可動体４４の現在動作を維持するための現在動作維持指令値生成器として機能する。
【００８５】
第１比較器３３は、通常のロボットコントローラに用いられる位置指令値を速度に変換する比例器と同様であってもよい。第２比例器は、通常のロボットコントローラに用いられる速度を電流指令値に変換する比例器と同様であってもよい。
【００８６】
衝突検知手段５１は、可動体４４の推定移動偏差および推定加速度偏差のうちのいずれか一方がしきい値を超えた場合に、ロボットアーム４０が障害物Ｇに衝突したとして判断する。具体的には、衝突検知手段５１は、推定位置算出器２１と、推定速度算出器２２と、推定速度偏差算出器２３と、第１判定器２４と、推定加速度偏差算出器２５と、第２判定器２５と、ＯＲ回路２７とを含む。
【００８７】
推定位置算出器２１は、可動体４４の理論上の位置を推定する。推定位置算出器２１は、その時定数がロボット１の時定数とほぼ同等に設定される。推定位置算出器２１は、外部位置指令値生成部６０から外部位置指令値が与えられ、推定位置算出器２１に設定される時定数に基づいて、サーボ系などの遅れ要素を考慮して、可動体４４の理論上の位置である推定位置を演算する。推定位置算出器２１は、演算した推定位置を表わす推定位置情報を推定速度算出器２２に与える。
【００８８】
推定位置算出器２１の時定数は、外部位置指令値が与えられてから可動体４４が、その外部位置指令値の表わす位置に移動するまでの時間遅れに基づいて設定される。推定位置算出器２１は、与えられる外部位置指令値をフィルタに通過させて、フィルタ処理することで時間遅れを考慮した可動体４４の理論上の位置を演算する。
【００８９】
推定速度算出器２２は、推定位置情報の表わす推定位置を時間で微分し、推定速度を表わす推定速度情報を演算する。推定速度算出器２２は、推定速度情報を推定速度偏差算出器２３に与える。また前述したフィードバック速度算出器２９は、フィードバック速度情報を推定速度偏差算出器２３に与える。推定速度偏差算出器２３は、推定速度情報の表わす推定速度からフィードバック速度情報の表わす速度を減算し、推定速度偏差を表わす推定速度偏差情報を演算する。
【００９０】
第１判定器２４は、推定速度偏差算出器２３から推定速度偏差情報が与えられる。第１判定器２４は、推定速度偏差情報の表わす推定速度偏差が予め定められる第１しきい値を超えているか否かを判定する。第１判定器２４は、第１しきい値を超えている場合には、ロボットアーム４０が障害物Ｇに衝突したことを表わす衝突検知信号を作成し、作成した衝突検知信号をＯＲ回路２７に与える。
【００９１】
推定加速度偏差算出器２５は、推定速度偏差算出器２３から推定速度偏差情報が与えられる。推定加速度偏差算出器２５は、推定速度偏差情報の表わす推定速度偏差を時間で微分し、推定加速度偏差を表わす推定加速度偏差情報を演算して、その推定加速度偏差情報を第２判定器２６に与える。
【００９２】
第２判定器２６は、推定加速度偏差算出器２５から推定加速度偏差が与えられる。第２判定器２６は、推定加速度偏差情報の表わす推定加速度偏差が予め定める第２しきい値を超えているか否かを判定する。第２判定器２６は、第２しきい値を超えている場合には、ロボットアーム４０が障害物Ｇに衝突したことを表わす衝突検知信号を作成し、作成した衝突検知信号をＯＲ回路２７に与える。
【００９３】
ＯＲ回路２７は、第１判定器２４および第２判定器２６の少なくともいずれか一方から衝突検知信号が与えられると、ロボットアーム４０が障害物Ｇに衝突したことを検知し、衝突検知信号を出力する。ＯＲ回路２７は、後述する内部位置指令値生成部３０、切換スイッチ３１、電流制限手段５２に衝突検知信号を与える。
【００９４】
ＯＲ回路２７は、推定速度偏差および推定加速度偏のいずれかのうち少なくとも一方に基づいて衝突検知信号を作成する。これによって制御装置２０は、ロボットアームと障害物Ｇとの衝突を精度よく検知することができる。さらに加速度変化は、速度変化よりも速く衝突の影響が現れるので、推定加速度偏差を用いて衝突を検知することで、衝突後迅速に衝突検知信号を出力することができる。
【００９５】
比較例として、モータＭに流れる電流に基づいてアーム衝突を検知する場合には、衝突検知にあたって、アーム４０の関節に充填されている潤滑剤の粘度の影響を受けやすい。たとえば冬季には、潤滑剤の粘度が著しく上昇してしまう。この場合、正常状態であってもモータＭの理論電流値が大きくなり、衝突と誤検知される場合がある。これに対して本発明では、エンコーダ値と位置指令値とに基づくことによって、モータＭに流れる電流がばらついたとしても、精度よくアーム４０の衝突を検知することができる。
【００９６】
このように制御装置２０は、外部位置指令値から可動体４４の推定位置を算出する推定位置演算手順と、推定位置から可動体４４の推定速度を算出する推定速度演算手順と、推定速度から可動体４４の推定速度偏差および推定加速度偏差を算出する偏差演算手順と、推定速度偏差および推定加速度偏差のいずれかが対応する各しきい値を超えているか否か判定する判定手順と、推定速度偏差または推定加速度偏差が対応する各しきい値を超えている場合に衝突検知信号を出力する衝突検知信号出力手順とを順に行うことによって、アーム衝突を検知することができる。
【００９７】
後退指令手段５３は、ロボットアーム４０が障害物Ｇに衝突した場合に、可動体４４が衝突前に移動した径路を逆にたどるように、ロボットアームを移動させるために設けられる。後退指令手段５３から移動量指令手段５４に後退指令が与えられることによって、衝突が検知されたときに可動体４４が移動していた移動方向と、逆向きに移動する後退方向に移動するような力が可動体４４に与えられる。すなわち制御装置２０は、アーム４０が障害物Ｇに衝突すると、衝突前にモータＭが可動体４４に与えるトルクとは逆向きのトルクを与えるように、モータＭに与える電流の向きを逆、すなわち反転する。
【００９８】
後退指令手段５３は、内部位置指令値生成部３０と、切換スイッチ３１とを含む。内部位置指令値生成部３０は、エンコーダ値であって可動体４４の位置を表わす検出位置情報を随時記録する。内部位置指令値生成部３０は、ＯＲ回路２７から衝突検知信号が与えられると、記録している検出位置情報から内部位置指令値を生成し、生成した内部位置指令値を切換スイッチ３１に与える。
【００９９】
内部位置指令値は、衝突前に可動体４４が移動方向に移動した移動経路を逆にたどるような可動体４４の位置の時間変化を表わす。内部位置指令値の表わす位置は、時間経過とともに可動体４４の位置が衝突時の移動方向と逆向きとなる後退方向に移り変わる。これによって内部位置指令値に従うと、可動体４４は後退方向に移動する。
【０１００】
内部位置指令値生成部３０は、衝突検知信号が与えられると、記憶する検出位置情報のうち、最も新しい検出位置情報から古くなる順に検出位置情報を出力して内部位置指令値を演算する。内部位置指令値生成部３０は、その記憶容量に制限がある場合には、時間順に記憶される検出位置情報のうち、衝突前であって、衝突時に最も近い時間に記憶される検出位置情報から内部位置指令値を生成してもよい。
【０１０１】
切換スイッチ３１は、外部位置指令値と内部位置指令値とを切換える。切換スイッチ３１は、外部位置指令値生成部６０から外部位置指令値が与えられるとともに内部位置指令値生成部３０から内部位置指令値が与えられる。切換スイッチ３１は、通常状態では、外部位置指令値を位置偏差算出器３２に与える。また切換スイッチ３１は、ＯＲ回路２７から衝突検知信号が与えられると、内部位置指令値を位置偏差算出器３２に与える。
【０１０２】
位置偏差算出器３２は、アーム４０が障害物Ｇに衝突すると、切換スイッチ３１によって内部位置指令値が与えられる。そして第２比例器３５は、後退方向に可動体４４が移動するような１次電流指令値を演算する。そして電流発生回路６１は、後退方向に移動するような電流をモータＭに流す。
【０１０３】
このように本実施の形態では、後退指令手段５３と移動量指令手段５４とを含んで電流反転手段を構成する。電流反転手段は、アーム衝突後に、アーム衝突直前にモータＭに流れる電流に対して向きを反転させた電流をモータＭに流す。このようにしてアーム４０が障害物Ｇに衝突すると、可動体４４が障害物Ｇに対して後退方向に移動する。
【０１０４】
なお、積分器３７は、ＯＲ回路２７から衝突検知信号が与えられると、現在の積分結果を一度ゼロにして、衝突検知信号が与えられてから新たに１次電流指令値の表わす電流の変化量を積分する。言い換えるとアーム衝突時には、積分電流指令値をリセットし、アーム衝突前における可動体４４の移動動作の維持を解除する。
【０１０５】
これによって衝突前の速度にかかわらず、アーム衝突後、後退方向に移動するような指示電流指令値を速く電流発生回路６１に与えることができる。また電流指令値をリセットすることによって、アーム衝突前に比べて指示電流指令値を低減することができ、モータＭに流れる電流を低減して、ロボットアーム４０による障害物Ｇの押付け力を緩和することができる。
【０１０６】
比較例としてアーム４０が衝突しても積分電流指令値をリセットしない場合には、衝突後に後退方向に移動するような１次電流指令値を演算したとしても、加算器３８で衝突前の速度を表わす積分電流指令値と１次電流指令値とが加算されるので、衝突後に後退方向に移動するような指示電流指令値を速く電流発生回路６１に与えることができない。したがって、可動体４４による障害物Ｇの押付け力を緩和するまでに時間がかかる。
【０１０７】
これに対して上述したように、衝突後に積分電流指令値をリセットすることによって、可動体４４の後退方向の動作を迅速に行うことができるとともに、可動体４４による障害物Ｇの押し付け力を緩和することができる。そのうえ、衝突後に可動体４４を後退方向に移動させることで、衝突後の再起動作を迅速に行うことができる。
【０１０８】
電流制限手段５２は、アーム衝突時にＯＲ回路２７から衝突検知信号が与えられて、与えられる指示電流指令値を制限して、駆動電流指令値として電流発生回路６１に与える。電流発生回路６１の直前に電流制限手段が設けられることによって、アーム衝突検知後、直ちにモータＭに流れる電流を低減することができる。
【０１０９】
後退指令手段５３は、衝突検知信号が与えられてから、内部位置指令値を生成して移動量指令手段５４に与えるまでに時間遅れが生じる。また移動量指令手段５４は、フィードバック機構を有するので内部位置指令値が与えられてから、指示電流指令値を電流制限手段５２に与えるまでに時間遅れが生じる。このようにアーム４０が衝突してから、モータＭに流れる電流を反転するまでには時間遅れが生じる。
【０１１０】
後退方向に移動するような電流が流れるまでは、モータＭには、可動体４４が障害物Ｇを押付けるような電流が流れる。このような電流が流れつづけている期間中、可動体４４は障害物Ｇを押付け続ける。また流れる電流が大きいほど、可動体４４が障害物Ｇを押付ける力が大きくなる。
【０１１１】
本発明では、アーム衝突時に電流制限手段５２によってモータＭに流れる電流を制限する。これによって衝突検知信号を出力してから後退方向に移動するような電流を流すまでに遅れがある場合であっても、可動体４４が障害物Ｇを押付けようとする力を抑制することができ、障害物Ｇおよびロボットアーム４０が損傷することを防止することができる。
【０１１２】
また本実施の形態では、電流制限手段５２は、モータＭの許容電流を越える電流が長時間与えられることを防ぐために、予め設定電流範囲が設定される。アーム４０が衝突しない正常状態において、電流制限手段５２は、指示電流指令値が設定電流範囲以上の電流である場合には、設定電流範囲の上限値となる駆動電流指令値を電流発生回路６１に与える。また電流制限手段５２は、指示電流指令値が設定電流範囲以下の電流である場合には、設定電流範囲の下限値となる駆動電流指令値を電流発生回路６１に与える。
【０１１３】
また内部位置指令値生成部３０は、アーム衝突検知後に可動体４４が予め定める移動距離後退して、可動体４４が障害物Ｇから離反したことを判断すると、可動体４４の移動を停止させる位置指令値を出力するとともに、電流制限手段５２に衝突解除信号を与える。
【０１１４】
図５は、電流制限手段５２の具体的構成を示すブロック図である。本発明の実施の一形態である電流制限手段５２は、衝突直前に電流モータＭに与えられる電流と同じ向きに流れる電流に対応する指示電流指令値を予め定める減少率で減少する。
【０１１５】
電流制限手段５２は、制限部８０と、電流方向検出部８１とを有する。アーム衝突検知前では、制限部８０は、指示電流指令値が設定電流範囲にある場合、制限せずに駆動電流指令値として電流発生回路６１に与える。
【０１１６】
電流方向検出部８１は、衝突検知信号が与えられると、そのときに流れる電流の方向を検出し、電流の方向を含んだ電流制限指令を制限部８０に与える。制限部８０は、電流制限指令が与えられると、電流方向検出部８１によって検出される方向のみに対応する指示電流指令値を予め定める減少率に減少して駆動電流指令値として電流発生回路６１に与える。
【０１１７】
制御部８０は、電流方向検出部８１によって与えられる方向と反対に流れる電流を表わす指示電流指令値が与えられると、指示電流指令値が設定電流範囲以上および設定電流範囲以下であっても制限せず、指示電流指令値を駆動電流指令値として電流発生回路６１に与える。すなわち制御部８０は、モータに与えられる電流の向きが反転すると、設定電流範囲よりも大きい電流がモータに流れる事を許容する過電流許容手段を兼用する。
【０１１８】
また制限部８０は、可動体４４が障害物Ｇから離反して内部位置指令値生成部３０から衝突解除信号が与えられると、指示電流指令値を無制限に駆動電流指令値として電流発生回路に与える状態を解除し、設定電流範囲内の駆動電流指令値を電流発生回路６１に与える通常制限状態に移行する。したがって内部位置指令値生成部３０は、可動体４４が障害物Ｇから離反すると、大きい電流の許容を解除する過電流許容解除手段となる。
【０１１９】
また他に、電流方向検出手段８１が指示電流指令値が反転したことを検出して、電流制限解除指令を制御部８０に与えることで、電流の制限を解除してもよい。
【０１２０】
図６は、アーム衝突時における制御装置２０の動作手順を示すフローチャートである。また図７は、現在位置と電流制限状態と駆動電流との時間変化を示すタイムチャートである。図７（１）は、現在位置の時間変化を示し、図７（２）は、電流制限手段５２の電流制限状態の時間変化を示し、図７（３）は、理想的な駆動電流の時間変化を示す。
【０１２１】
ステップＳ０で、制御装置２０は、可動体４４を移動させるために位置指令値を生成し、予め定める移動経路に従って可動体４４が移動するように、モータＭに与える駆動電流を調整する。一定速度で可動体４４を移動させる場合、図７（１）に示すように、移動位置は、一定の時間変化率で変化する。このとき理想的には、図７（３）に示すように駆動電流は一定値に調整される。そしてロボットアーム４０が衝突時刻Ｔ４で障害物Ｇに衝突すると、ステップＳ１に進み、アーム衝突時における動作を開始する。
【０１２２】
ステップＳ１では、制御装置２０は、ロボットアーム４０が障害物Ｇに衝突して可動体４４が停止すると、エンコーダ値と移動指令値との位置偏差が大きくなり、衝突前に比べて指示電流指令値を大きくする。このとき電流制限手段５２によって、指示電流指令値が設定電流範囲以上となっても、駆動電流指令値が大きくなることが阻止され、設定電流範囲の上限値または下限値に応じた駆動電流がモータＭに与えられる。
【０１２３】
またアーム衝突後、衝突検知手段５１によって衝突検知工程が行われ、図７に示す衝突検知時刻Ｔ１で、アーム４０が衝突したことを検知すると、ステップＳ２に進む。
【０１２４】
ステップＳ２では、電流制限手段５２によって電流制限工程が行われ、モータＭに与える駆動電流をさらに制限する。また電流反転手段によって電流反転工程が行われ、衝突前とは逆向きに駆動電流をモータＭに流す。
【０１２５】
衝突検知時刻Ｔ１から電流が制限されるまでには、第１遅れ時間Ｗ１がかかる。また衝突検知時刻Ｔ１から逆向きの電流がモータＭに流れるまでには、第２遅れ時間Ｗ２がかかる。たとえばこの第１遅れ時間Ｗ１は、約数ミリ秒である。また第２遅れ時間Ｗ２は、約数十ミリ秒である。このように第１遅れ時間Ｗ１は、第２遅れ時間Ｗ２に比べて十分短い。衝突検知時刻Ｔ１から逆向きの駆動電流がモータＭに流れる反転開始時刻Ｔ２に達すると、ステップＳ３に進む。
【０１２６】
ステップＳ３では、反転開始時刻Ｔ２では、電流制限手段５２は、電流制限を行わず、指示電流指令値を駆動電流指令値として電流発生回路６１に与える。電流発生回路６１は、衝突前とは逆方向の駆動電流をモータＭに流す。これによって可動体４４は、後退方向に移動するトルクが与えられる。
【０１２７】
この場合、上述したように電流制限手段５２は、過電流許容工程を行い、指示電流指令値を駆動電流指令値として無制限に電流発生回路６１に与える。これによって大きな駆動電流を発生することが可能となり、可動体４４を速やかに後退方向に移動させることができる。また衝突するまでの進行移動径路を逆にたどるような後退移動経路に従って可動体４４が移動するので、後退時に他の障害物に衝突することがない。
【０１２８】
内部位置指令値生成部３０が、エンコーダ値に基づいて、可動体４４が障害物から予め定める移動距離後退したことを判断し、可動体４４が障害物Ｇから離れた状態となると、ステップＳ４に進む。
【０１２９】
ステップＳ４では、内部位置指令値生成部３０は、可動体４４の変位駆動を停止するような内部指令値を生成する。また衝突解除信号を電流制限手段５２に与え、電流制限手段５２による電流の制限状態を衝突前の通常制限状態に移行して過電流許容解除工程を行い、ステップＳ５に進む。ステップＳ５では、制御装置２０は、衝突時のモータ駆動動作を終了する。
【０１３０】
作業者は、ロボットアーム４０の移動径路プログラムを書き換え、障害物Ｇの除去などを行い、可動体４４と障害物Ｇとが衝突しないことを確認した後で、ティーチングペンダント７３などの入力手段によって、再起動信号を制御装置２０に与える。これによって切換スイッチ３１が切換り、外部位置指令値生成部６０からの位置指令値に従って、ロボットアーム４０を移動可能な状態となる。
【０１３１】
なお上述の記載では、１つの可動体４４の制御に関する構成について、説明したが他の可動体４４についても同様である。複数の可動体４４を同時に移動させる場合には、それぞれの可動体４４に対応するモータＭが、それぞれ制御される。
【０１３２】
以上のように本発明の実施の形態に従えば、アーム４０の衝突を検知してから電流の向きを反転してモータＭに電流を流すまでに費やす第２遅れ時間Ｗ２に比べて、アーム４０の衝突を検知してから電流を制限するまでに費やす第１遅れ時間Ｗ１のほうが短いので、電流制限手段５２によって駆動電流を制限することで、反転開始時刻Ｔ２に達する前に可動体４４が障害物Ｇを押付けようとする力を抑制することができ、衝突による衝撃を緩和することができる。
【０１３３】
比較例として電流を制限しない場合には、ステップＳ２において、図７（３）に二点差線１００で示すように、可動体４４が障害物Ｇに衝突した後であっても、反転開始時刻Ｔ２に達するまで、アーム衝突前以上に大きな駆動電流がモータＭに与えられつづけ、可動体４４が障害物Ｇを大きな力で押付け続ける。したがって衝突による衝撃を緩和することができない。
【０１３４】
これに対して本発明では、モータＭに与える電流を制限することで、衝突による衝撃を緩和することができ、モータＭ、減速器、アーム本体および障害物Ｇなどの衝突による損傷を低減することができる。
【０１３５】
また本発明の実施の形態における電流制限手段５２の構成は、制御装置を構成する記憶部のプログラムを更新することによって実現することができ、非常に容易に実現することができる。また電流発生回路６１からの電流を制限して、衝突による衝撃を緩和する方法は、アーム４０の衝突検知方法に無関係であるので、アーム４０の衝突検知が遅れることがない。
【０１３６】
また本発明の実施の形態では、反転開始時刻Ｔ２に達すると、モータＭに後退方向に移動するような電流が流れ、可動体４４に後退方向のトルクを与えることができる。これによって可動体４４を速やかに後退させて、可動体４４が障害物Ｇを押付ける状態を短時間で解消することができる。
【０１３７】
また電流制限手段５２が、衝突前にモータＭに流れる方向の電流についてのみ制限し、衝突前にモータＭに流れる方向とは反対方向に流れる電流については、制限せずに通過させる。これによって可動体４４の速度などによって、アーム４０の衝突を検知してから電流の流れを反転するまでに費やす第２遅れ時間Ｗ２がばらつく場合であっても、電流の流れが反転すると直ちに逆向きの電流をモータＭに与えることができる。言い換えると、電流の制限を解除する時間を正確に決定する必要が無く、さらに利便性を向上することができる。
【０１３８】
また指示電流指令値が反転した後に、設定電流範囲よりも大きい電流指令値が与えられた場合には、電流制限手段５２が制限せずにその指示電流指令値を駆動電流指令値として電流発生回路６１に与えることによって、電動モータＭに与える逆向きのトルクを大きくすることができ、可動体４４が障害物Ｇを押付ける時間をさらに短くすることができる。また正常状態では、設定電流範囲を超える電流および設定電流範囲未満の電流が流れることが防止され、モータＭおよび電流発生回路６１が損傷することを防止することができる。
【０１３９】
また可動体４４が障害物Ｇから離反したことを判断すると、通常制限状態に移行することによって、モータＭおよび電流発生回路６１に長時間、大きい電流が流れることを防止することができ、モータＭおよび電流発生回路６１の損傷をさらに防止することができる。
【０１４０】
また電流制限手段５２が、移動量指令手段５４によって演算された指示電流指令値を制限する。サーボ系の遅れによって衝突検知時から電流の流れる向きを反転する指示電流指令値を生成するまでには時間遅れがある。これに対して駆動電流指令値を制限することは、サーボ系の遅れにかかわらずに行うことができる。したがって衝突検知時から指示電流指令値を制限して駆動電流指令値として生成するまでにかかる時間は、電流の流れを反転する指示電流指令値を生成するまでにかかる時間よりも短い。したがって衝突検知後に、電流を反転する指示電流指令値が演算されるまでに、電流を制限した駆動電流指令値を演算することができる。これによって衝突検知後、モータＭに与えられる電流が反転する前に、モータＭに与えられる電流を速く低減することができる。
【０１４１】
電流制限手段５２によって制限されてモータＭに流れる電流値は、ゼロまたは可及的に最小値にすることが好ましい。これによって可動体４４および障害物Ｇの衝突による衝撃をより緩和することができる。これに対し、モータＭに流れる電流が大きいと電流の制限が開始される制限開始時刻Ｔ３以降でも、可動体４４が障害物Ｇを押付ける力を十分に低減することができない。
【０１４２】
またロボットアーム４０に重力などの外力が与えられる場合、電流制限手段５２によって制限されてモータＭに流れる電流は、可動体４４が重力などの外力によって変位しないトルクを発生するに必要な電流値以上に設定される。これによって可動体４４が、不所望に変位することを防止することができ、ロボットアーム４０が自重によって落下することを防ぐことができる。
【０１４３】
アーム４０が外力を受ける場合には、アーム４０の重量、モータＭの発生可能トルクなどから、ロボットの姿勢に応じてその姿勢を維持し得る最低限の電流を演算し、その電流値を表わす駆動電流指令値を電流発生手段６１に与えてもよい。また予め演算される電流指令値の減少率を設定していてもよい。また姿勢毎に演算される減少率と、予め定められる減少率とを、アーム４０の状態に応じて切換えて用いてもよい。
【０１４４】
さらに本実施の制御装置２０は、外部位置指令値とエンコーダ値とに基づいて、アーム４０の衝突を検知する。したがって実際にアーム４０が障害物Ｇに衝突したことを検知するセンサを必要とせず、コンピュータの演算回路を用いて衝突検知手段を実現することができる。したがって近接センサ、リミットスイッチおよび加速度センサなどの衝突検知センサを設ける必要がない。
【０１４５】
またロボットコントローラの制御プログラムを更新することによって、衝突検知および衝突検知後のアーム制御を実現することができるので、衝突検知手段５４および反転指令手段のロボットへの実装に要する期間を短縮できる。また従来と同様の物理的構成で実現することができる。
【０１４６】
また推定加速度偏差を用いてアーム衝突の検知を行うことによって、検知精度の向上が図られる。さらに推定速度偏差および推定加速度偏差を用いて衝突を判定することによって、衝突を検知するために複雑な計算、たとえばアーム４０に関する運動方程式の解を演算する必要がなく、衝突検知に要する時間を短縮できる。
【０１４７】
またアーム衝突後のモータＭの制御方法において、アーム４０の衝突を検知すると、衝突前にモータＭの動作を継続させるために与えていた電流指令値を消去した後、アーム４０が後退方向に移動する電流指令値を与える。すなわち、駆動手段の被駆動部材に対する現在動作の維持を停止させた後、駆動手段の被駆動部材に衝突前の経路を逆にたどらせる。これによって衝突後におけるロボットアーム４０の後退動作を迅速になし得ることができる。さらに、衝突後に後退動作をさせているので、ロボットアーム４０の再起動を迅速になし得る。
【０１４８】
図８は、本発明の他の実施の形態である電流制限手段１５２を示すブロック図である。他の実施の形態である電流制限手段１５２は、電流方向検出部８１に代えて反転開始時刻Ｔ２に達すると電流制限手段５２による電流の制限を解除するタイマ部１８１を有していてもよい。タイマ部１８１は、衝突検知信号が与えられると、制限部１８０に電流制限指令を与える。制御部１８０は、電流の向きにかかわらず指示電流指令値を制限する。
【０１４９】
タイマ部１８１は、電流の向きを反転するために必要な第２遅れ時間Ｗ２が経過したと判断すると、制限部１８０に電流制限解除指令を与える。制御部１８０は、電流制限解除指令が与えられることによって、指示電流指令値の制限を解除する。このとき制御部１８０は、設定電流範囲以上の電流が流れることを許容する。これによって電流が反転した場合には、モータに与えられる電流が制限されることを防いで、指示電流指令値を駆動電流指令値として与えることができる。そして制御部１８０に衝突解除信号が与えられると、過電流許容状態を解除し、衝突前の通常制限状態に移行する。
【０１５０】
このような他の実施の形態の電流制限手段１５２を有する場合であっても、上述と同様の効果を得ることができる。またこの場合、指示電流指令値から衝突前に流れる電流の方向を検出する必要がない。
【０１５１】
またさらに他の形態の電流制限手段として、電流制限手段は、演算部７０またはディーチングペンダント７３などの入力手段からの解除指令に基づいて、電流の制限を解除してもよい。
【０１５２】
なお、上述した本発明の構成は、発明の一例示であって発明の範囲内で構成を変更することができる。本発明の実施の形態では、推定速度偏差と推定加速度偏差とに基づいて、アーム４０の衝突を検知することが好ましいが、アーム４０の衝突検知にあたっては、特に限定せず従来技術を用いてもよい。
【０１５３】
またロボットの制御方法および制御装置について説明したが、電動モータＭを有する駆動手段を備える装置であれば、同様の制御方法および制御装置を用いることができる。ロボット以外の産業機械、たとえばＮＣ（Numerical Control、数値制御）機械、搬送装置などであっても、上述した制御方法および制御装置を用いることで、上述した効果と同様の効果を達成することができる。
【０１５４】
また移動量指令手段５４の具体的な構成も、他の構成であってもよい。また制御装置２０は、プログラムを読取ることによって各演算器の構成を実現したが、電気回路などの物理的な構成によって各演算器の構成を実現してもよい。また本発明の制御装置２０は、ロボットコントローラと別体に設けられていてもよい。また指示電流指令値が反転した場合、与えられる指示電流指令値に対して予め定める増幅率で増大して駆動電流指令値を生成して、電流発生回路に与えてもよい。これによって可動体４４が障害物Ｇに押付けられる時間をさらに短縮することができる。
【図面の簡単な説明】
【０１５５】
【図１】本発明の実施の一形態であるロボット１の主要な構成を示すブロック図である。
【図２】本発明のロボットアーム４０の関節構造を模式的に示す図である。
【図３】制御装置２０の物理的構成を示すブロック図である。
【図４】制御装置２０の機能的構成を示すブロック図である。
【図５】移動方向に流れる電流のみ制御する電流制限手段５２の構成の一例を示すブロック図である。
【図６】アーム衝突時における制御装置２０の動作手順を示すフローチャートである。
【図７】現在位置と電流制限状態と駆動電流との時間変化を示すタイムチャートである。
【図８】本発明の他の実施の形態である電流制限手段１５２を示すブロック図である。
【符号の説明】
【０１５６】
１  ロボット
８  ロボット本体
２０  制御装置
２８  エンコーダ
４０  ロボットアーム
４１  サーボ機構
４２  エンドエフェクタ
４４  可動体
５１  衝突検知手段
５２，１５２  電流制限手段
５３  後退指令手段
５４  移動量指令手段
６１  電流発生回路
Ｍ  モータ
【図１】
2005059171.tif 000003
【図２】
2005059171.tif 000004
【図３】
2005059171.tif 000005
【図４】
2005059171.tif 000006
【図５】
2005059171.tif 000007
【図６】
2005059171.tif 000008
【図７】
2005059171.tif 000009
【図８】
2005059171.tif 000010