JP5912953B2 - 制御装置、及び制御方法 - Google Patents

Description

本発明は、マニピュレータの衝突後の制御を行う制御装置等に関する。

産業用ロボットを構成するマニピュレータが障害物等に衝突した場合には、迅速に負荷を低減させなければ、モータや減速機を破損することになる。そのため、例えば、特許文献１には、衝突時の外力ベクトルの反対方向の速度ベクトルの方向に回避動作を行う手法が記載されている。また、特許文献２には、位置データを保存しておき、衝突時には保存した位置データにおける、ある一定距離前の位置に向かって回避させることによって、負荷を抑えると共に、安全な回避を実現する手法が記載されている。また、特許文献３には、衝突時に位置ループゲインを下げることで、減速機の負荷を抑える手法が記載されている。また、特許文献４には、衝突時に速度指令値を０にし、逆転トルクを発生させると共に、全軸を停止させることによって、衝突後、迅速にロボット全体を停止させる手法が記載されている。

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

しかしながら、特許文献１の手法では、回避時の移動方向が使用者の教示した方向ではないため、回避方向に別の障害物が存在し、二次災害を引き起こす可能性があるという問題があった。
また、特許文献２の手法では、過去の軌跡上への回避を行うため、安全な回避動作を実現できるが、あらかじめ決められた距離だけの回避であるため、衝突検知が遅れた場合には、回避後でもロボットが障害物に衝突したままになり、減速機を傷める可能性があるという問題があった。
また、特許文献３の手法では、ループゲインを下げることによって負荷を低減しているが、特許文献２の場合と同様に、回避後にも衝突したままになることがあるという問題があった。
また、特許文献４の手法では、即座にロボットを停止させることができるが、特許文献２，３の場合と同様に、回避後にも衝突したままになることがあるという問題があった。

本発明は、上記問題点を解決するためになされたものであり、マニピュレータが衝突した場合に、減速機の負荷を低減させることができると共に、手先効果器を安全な位置に回避させることができるマニピュレータの制御装置等を提供することを目的とする。

上記目的を達成するため、本発明による制御装置は、複数のモータによってそれぞれ駆動される複数の関節によって連結された複数のアームにより手先効果器を移動させるマニピュレータの制御を行う制御装置であって、マニピュレータの各モータをそれぞれ制御する複数のサーボコントローラと、複数のモータの位置から手先効果器の位置を算出する位置算出部と、位置算出部が算出した手先効果器の位置を蓄積する蓄積部と、手先効果器の衝突を検知する衝突検知部と、衝突検知部が衝突を検知した時の手先効果器の位置である衝突位置を特定する衝突位置特定部と、衝突検知部が衝突を検知した後に、複数のモータのすべてが停止した状態である全軸停止状態になったことを検知する全軸停止状態検知部と、衝突検知部が衝突を検知してから、全軸停止状態検知部が全軸停止状態を検知するまで、速度指令値が０になるように複数のサーボコントローラを制御する衝突時制御部と、全軸停止状態検知部が全軸停止状態を検知した時の手先効果器の位置である全軸停止位置から、衝突位置から全軸停止位置に向かう方向に沿って移動した位置である安全基準位置を算出する安全基準位置算出部と、蓄積部が蓄積した手先効果器の過去の位置のうち、安全基準位置にあらかじめ決められた条件をみたすほど近い位置である安全位置を特定する安全位置特定部と、手先効果器の位置が安全位置となるように制御する安全位置移動制御部と、を備えたものである。
このような構成によって、衝突後、手先効果器を障害物から離れた位置である安全位置に移動させることができる。そのため、手先効果器が障害物に衝突したままになることを回避でき、減速機の負荷を低減させることができる。また、その安全位置が手先効果器の過去の軌跡上に存在することにより、安全位置への移動によって別の障害物に衝突することを防止でき、安全な回避動作を実現できる。

また、本発明による制御装置では、サーボコントローラは、モータの現在位置と位置指令値とに応じて速度指令値を生成する速度指令生成器と、そのモータの現在速度と速度指令値との差分を積分器により積分した値と、その差分に積算器により比例ゲインを積算した値とを加算することにより電流指令値を生成する電流指令生成器と、そのモータの現在電流と前記電流指令生成器が生成した電流指令値とに応じてトルク指令値を生成し、そのモータに出力するトルク指令生成器と、を備え、電流指令生成器は、積分器の積分値を蓄積する積分値蓄積部と、衝突検知部が衝突を検知した場合に、積分器の積分値を積分値蓄積部が蓄積した過去の積分値に置き換える置換部と、を備え、衝突検知部が衝突を検知してから、全軸停止状態検知部が全軸停止状態を検知するまでは、衝突時制御部によって電流指令生成器が用いる速度指令値が０にされてもよい。
このような構成により、衝突時の負荷が定常負荷として積分器の積分値に反映されることを回避でき、迅速な負荷軽減が可能となる。すなわち、衝突が検知された際には積分器の積分値が衝突時の負荷に応じて大きな値となっているため、上述した積分値の置き換えを行わなかった場合には、速度指令値を０としても迅速な負荷の軽減を行うことが難しくなる。一方、積分器の積分値を過去の値に置き換えることにより、積分値に対する衝突の影響を排除でき、それだけ迅速な負荷軽減を実現できる。

また、本発明による制御装置では、衝突検知部が衝突を検知した後に、いずれかのモータの位置があらかじめ決められた範囲以上変化した場合に、モータを停止させるモータ停止部をさらに備えてもよい。
このような構成により、例えば、衝突時の回避動作中にマニピュレータの一部が自重で落下した場合に、その落下を防止でき、マニピュレータの破損を回避できる。

また、本発明による制御装置では、安全基準位置算出部は、衝突位置から全軸停止位置までの距離と、全軸停止位置から安全基準位置までの距離とが等しくなるように安全基準位置を算出してもよい。
このような構成により、衝突の程度に応じた安全基準位置の算出を行うことができ、その結果、適切な安全位置の特定が行われることになる。

また、本発明による制御装置では、安全位置特定部は、蓄積部が蓄積した過去の位置のち、安全基準位置に距離が最も近い位置である安全位置を特定してもよい。
このような構成により、回避位置として適切であると考えられる位置に手先効果器を移動させることができる。

また、本発明による産業用ロボットは、マニピュレータと、そのマニピュレータを制御する上記制御装置と、を備えたものである。

本発明によるマニピュレータの制御装置等によれば、マニピュレータが衝突した場合に、減速機の負荷を低減させることができると共に、手先効果器を安全な位置に回避させることができる。

本発明の実施の形態１による産業用ロボットの構成を示すブロック図 同実施の形態によるサーボコントローラの構成を示すブロック図 同実施の形態による電流指令生成器の構成を示すブロック図 同実施の形態における手先効果器の回避動作について説明するための図 同実施の形態における各軸の回避状態について説明するための図 同実施の形態による制御装置の動作を示すフローチャート 同実施の形態による制御装置の動作を示すフローチャート 同実施の形態におけるコンピュータシステムの構成の一例を示す図

以下、本発明による制御装置について、実施の形態を用いて説明する。なお、以下の実施の形態において、同じ符号を付した構成要素及びステップは同一または相当するものであり、再度の説明を省略することがある。

（実施の形態１）
本発明の実施の形態１によるマニピュレータの制御装置について、図面を参照しながら説明する。本実施の形態によるマニピュレータの制御装置は、マニピュレータの衝突が検知された際に、減速機の負荷を低減させると共に、手先効果器を安全な位置に回避させるものである。

図１は、本実施の形態による産業用ロボット１００の構成を示すブロック図である。本実施の形態による産業用ロボット１００は、制御装置１と、マニピュレータ２とを備える。制御装置１は、複数のモータによってそれぞれ駆動される複数の関節によって連結された複数のアームにより手先効果器を移動させるマニピュレータ２を制御する。そのマニピュレータ２において、各モータとアームとは減速機を介して接続されている。また、直列に接続されたアームの先端に、手先効果器が設けられている。マニピュレータ２は、例えば、垂直多関節ロボットのマニピュレータであってもよく、水平多関節ロボットのマニピュレータであってもよい。また、そのマニピュレータ２の軸数は問わない。マニピュレータ２の軸数は、例えば、４軸であってもよく、５軸であってもよく、６軸であってもよく、または、その他の軸数であってもよい。本実施の形態では、マニピュレータ２が６軸である場合について主に説明する。また、制御装置１は、ティーチングプレイバック方式によりマニピュレータ２を制御する。その制御装置１は、サーボコントローラ１１−１〜１１−Ｎと、位置算出部１２と、蓄積部１３と、位置記憶部１４と、衝突検知部１５と、衝突位置特定部１６と、全軸停止状態検知部１７と、衝突時制御部１８と、安全基準位置算出部１９と、安全位置特定部２０と、安全位置移動制御部２１と、各軸位置指令算出部２２と、モータ停止部２３と、フラグ記憶部２４とを備える。

サーボコントローラ１１−１〜１１−Ｎは、マニピュレータ２の各モータ（図示せず）をそれぞれ制御する。すなわち、サーボコントローラ１１−１〜１１−Ｎは、各軸の位置指令値に応じて、各モータがその位置となるように各モータを制御する。ここで、Ｎは、２以上の整数であり、通常、マニピュレータ２の軸数と同数である。サーボコントローラ１１−１〜１１−Ｎの詳細な構成については、図２のブロック図を用いて後述する。

位置算出部１２は、マニピュレータ２の複数のモータの位置から手先効果器の位置を算出する。そのモータの位置（回転角度）は、例えば、エンコーダによって測定されてもよい。位置算出部１２は、あらかじめ設定されているアームの長さや、モータの配置等を用いて、マニピュレータ２の複数のモータの位置から手先効果器の位置を算出することができる。その手先効果器の位置は、通常、３次元空間上における位置であるが、そうでなくてもよい。なお、モータの位置と減速機の位置とは対応しているため、モータの位置は減速機の位置であってもよい。以下の説明においても同様であるとする。

蓄積部１３は、位置算出部１２が算出した手先効果器の位置を位置記憶部１４に蓄積する。位置記憶部１４での記憶は、ＲＡＭ等における一時的な記憶でもよく、あるいは、長期的な記憶でもよい。位置記憶部１４は、所定の記録媒体（例えば、半導体メモリや磁気ディスクなど）によって実現されうる。なお、後述するように、蓄積部１３が蓄積した手先効果器の過去の位置が安全位置の特定において用いられるため、位置記憶部１４では、その安全位置を含みうる手先効果器の軌跡が記憶されていることが好適である。例えば、手先効果器のすべての過去の軌跡が位置記憶部１４で記憶されていてもよく、または、最新の所定期間分の手先効果器の軌跡が位置記憶部１４で記憶されていてもよい。

衝突検知部１５は、手先効果器の衝突を検知する。衝突検知部１５が手先効果器の衝突を検知する方法は問わない。例えば、推定トルクと実トルクとの差を用いて各軸について衝突を検知してもよく、衝突検出用の検出器を用いて衝突を検知してもよく、または、その他の方法によって手先効果器の衝突を検知してもよい。なお、推定トルクと実トルクとの差を用いて各軸の衝突を検知する場合には、衝突検知部１５は、いずれかの軸において衝突を検知したときに、手先効果器の衝突を検知してもよい。また、衝突検出用の検出器を用いて衝突を検知する場合には、手先効果器の衝突を検出するように検出器を配設してもよい。

衝突位置特定部１６は、衝突検知部１５が衝突を検知した時の手先効果器の位置である衝突位置を特定する。衝突位置の特定は、例えば、衝突位置を図示しない記録媒体に蓄積することであってもよく、または、位置記憶部１４で記憶されている、衝突位置に対応する手先効果器の位置に対して、フラグ等を設定することであってもよい。このことは、他の位置が特定される場合にも同様であるとする。

全軸停止状態検知部１７は、衝突検知部１５が衝突を検知した後に、マニピュレータ２の複数のモータのすべてが停止した状態である全軸停止状態になったことを検知する。なお、後述するように、衝突が検知されると、各サーボコントローラ１１−１〜１１−Ｎにおける速度指令値が０に制御される。その結果、衝突時の負荷がなくなるまで、各モータに逆転トルクが発生し、最終的にモータが停止する。そのようにしてすべてのモータが停止した状態が、全軸停止状態である。

衝突時制御部１８は、衝突検知部１５が衝突を検知してから、全軸停止状態検知部１７が全軸停止状態を検知するまで、速度指令値が０になるように複数のサーボコントローラ１１−１〜１１−Ｎを制御する。衝突が検知された場合に、例えば、衝突時制御部１８は、後述する切替器３２を制御することによって、速度指令値を０にしてもよい。また、全軸停止状態が検知された場合に、例えば、衝突時制御部１８は、後述する切替器３２を制御することによって、速度指令値を０にする制御を解除してもよい。

安全基準位置算出部１９は、全軸停止状態検知部１７が全軸停止状態を検知した時の手先効果器の位置である全軸停止位置を特定する。また、安全基準位置算出部１９は、その全軸停止位置から、衝突位置と逆方向に移動した位置である安全基準位置を算出する。すなわち、安全基準位置算出部１９は、衝突位置から全軸停止位置に向かう方向に沿って、その全軸停止位置から移動した位置である安全基準位置を算出する。全軸停止位置から安全基準位置までの距離は、あらかじめ決められた固定値であってもよく、または、衝突位置から全軸停止位置までの距離に応じた可変値であってもよい。後者の場合には、安全基準位置算出部１９は、全軸停止位置から安全基準位置までの距離と、衝突位置から全軸停止位置までの距離にあらかじめ決められた係数を掛けた距離とが等しくなるように安全基準位置を算出してもよい。その係数は、０より大きい実数であり、あまり大きくない値であることが好適である。その係数は、例えば、３以下であってもよく、２以下であってもよく、１．５以下であってもよく、または、１であってもよい。本実施の形態では、その係数が１の場合、すなわち、安全基準位置算出部１９が、衝突位置から全軸停止位置までの距離と、全軸停止位置から安全基準位置までの距離とが等しくなるように安全基準位置を算出する場合について主に説明する。

安全位置特定部２０は、蓄積部１３が位置記憶部１４に蓄積した手先効果器の過去の位置のうち、安全基準位置にあらかじめ決められた条件をみたすほど近い位置である安全位置を特定する。安全基準位置にあらかじめ決められた条件をみたすほど近い位置とは、安全基準位置に距離が最も近い位置であってもよく、安全基準位置からの移動時間が最も短い位置であってもよく、または、それらの位置の近傍の位置、例えば、それらの位置からＭサンプル以内の位置であってもよい。そのＭは、小さい整数値、例えば、１〜５程度であることが好適である。また、そのＭサンプルは、それらの位置から過去の方向のサンプルであることが好適である。

安全位置移動制御部２１は、全軸停止状態が検知された後に、手先効果器の位置が安全位置となるように制御する。なお、結果として手先効果器の位置が安全位置となるように制御されるのであれば、その制御の内容は問わない。本実施の形態では、安全位置移動制御部２１が、割り込み処理で安全位置を各軸位置指令算出部２２に入力することにより、手先効果器の位置を安全位置に制御する場合について説明する。

各軸位置指令算出部２２は、手先効果器の位置に応じて、各軸の位置指令値を算出する。正常動作時には、各軸位置指令算出部２２は、ティーチングされた手先効果器の位置から、各軸の位置指令値を算出し、その位置指令値を各サーボコントローラ１１−１〜１１−Ｎに出力する。それにより、ティーチングに応じたマニピュレータ２の制御が行われる。一方、安全位置への回避時には、各軸位置指令算出部２２は、前述のように、割り込み処理により、安全位置移動制御部２１から安全位置の入力を受け付け、その安全位置に手先効果器が移動するように、各軸の位置指令値を算出して各サーボコントローラ１１−１〜１１−Ｎに出力する。それにより、手先効果器が安全位置に回避されることになる。

モータ停止部２３は、衝突検知部１５が衝突を検知した後に、いずれかのモータの位置があらかじめ決められた範囲以上変化した場合に、そのモータを停止させる。モータの位置があらかじめ決められた範囲以上変化した場合には、マニピュレータ２の一部が自重で落下していると考えられる。したがって、その落下を止め、マニピュレータ２の破損を回避するために、モータ停止部２３は、そのモータをロックさせる。具体的には、モータ停止部２３は、その停止対象のモータを制御しているサーボコントローラをオフにすることにより、そのモータをロックさせてもよい。なお、例えば、衝突時のモータの現在位置が蓄積され、その衝突時のモータの現在位置と、衝突後のモータの現在位置とがあらかじめ決められたしきい値以上になった場合に、モータの位置があらかじめ決められた範囲以上に変化したと判断されてもよい。また、例えば、衝突後にモータの現在速度があらかじめ決められたしきい値以上になった場合に、モータの位置があらかじめ決められた範囲以上に変化したと判断されてもよい。そのしきい値を適切に設定することにより、モータの現在速度がしきい値以上である場合には、モータの位置がしきい値以上変化していると考えることができるからである。なお、モータ停止部２３がそれらの判断を行ってもよく、または、他の構成要素がそれらの判断を行い、モータ停止部２３がその判断結果を用いてモータの停止制御を行ってもよい。本実施の形態では、衝突時のモータの現在位置と、衝突後のモータの現在位置とがあらかじめ決められたしきい値以上になったときに、モータの位置があらかじめ決められた範囲以上に変化したと判断される場合について主に説明する。また、モータ停止部２３は、その位置があらかじめ決められた範囲以上変化したと判断したモータのみを停止させてもよく、または、その他のモータも停止させてもよい。後者の場合には、モータ停止部２３は、すべてのモータを停止させてもよい。本実施の形態では、モータ停止部２３がすべてのモータを停止させる場合について主に説明する。

フラグ記憶部２４では、衝突に関するフラグや状態が記憶される。本実施の形態では、フラグ記憶部２４で、衝突フラグと、全軸停止フラグと、各軸の回避状態とが記憶される場合について説明する。それらのフラグや回避状態については後述する。フラグ記憶部２４での記憶は、ＲＡＭ等における一時的な記憶でもよく、あるいは、長期的な記憶でもよい。フラグ記憶部２４は、所定の記録媒体（例えば、半導体メモリや磁気ディスクなど）によって実現されうる。

なお、位置記憶部１４と、フラグ記憶部２４とは、同一の記録媒体によって実現されてもよく、あるいは、別々の記録媒体によって実現されてもよい。前者の場合には、手先効果器の位置を記憶している領域が位置記憶部１４となり、衝突フラグ等を記憶している領域がフラグ記憶部２４となる。

図２は、本実施の形態によるサーボコントローラ１１−１の構成を示すブロック図である。本実施の形態によるサーボコントローラ１１−１は、速度指令生成器３１と、切替器３２と、電流指令生成器３３と、トルク指令生成器３４と、現在位置蓄積部３５と、判断部３６とを備える。

速度指令生成器３１は、モータの現在位置と位置指令値とに応じて速度指令値を生成する。速度指令生成器３１は、モータの現在位置が位置指令値に近づくようにフィードバック制御をするための速度指令値を生成する。

切替器３２は、後段の電流指令生成器３３に渡す速度指令値を、速度指令生成器３１の生成した速度指令値と、０とのいずれかに切り替える。その切り替えは、前述のように、例えば、衝突時制御部１８の制御によって行われてもよい。すなわち、衝突検知部１５が衝突を検知してから、全軸停止状態検知部１７が全軸停止状態を検知するまでは、衝突時制御部１８によって、電流指令生成器３３が用いる速度指令値が０になるように切替器３２が設定されてもよい。また、全軸停止状態検知部１７が全軸停止状態を検知してからは、衝突時制御部１８によって、電流指令生成器３３が用いる速度指令値が速度指令生成器３１によって生成された速度指令値になるように切替器３２が設定されてもよい。

電流指令生成器３３は、モータの現在速度と速度指令値とに応じて電流指令値を生成する。その速度指令値は、切替器３２から受け取るものであり、衝突が検知されるまで、及び、全軸停止状態が検知されてからは速度指令生成器３１が生成した速度指令値であり、衝突が検知されてから全軸停止状態が検知されるまでは０である。なお、電流指令生成器３３の詳細な構成については、図３のブロック図を用いて後述する。

トルク指令生成器３４は、モータの現在電流と電流指令生成器３３が生成した電流指令値とに応じてトルク指令値を生成し、モータに出力する。トルク指令生成器３４は、モータの現在電流が電流指令値に近づくようにフィードバック制御をするためのトルク指令値を生成する。モータは、このトルク指令値によって動作する。

現在位置蓄積部３５は、衝突検知部１５が衝突を検知した時の現在位置を、所定の記録媒体に蓄積する。この記録媒体は、例えば、半導体メモリや、磁気ディスク等であり、現在位置蓄積部３５が有していてもよく、あるいは現在位置蓄積部３５の外部に存在してもよい。また、この記録媒体は、現在位置を一時的に記憶するものであってもよく、そうでなくてもよい。

判断部３６は、衝突が検知された後に、モータの現在速度が０になったかどうか判断する。モータの現在速度が０になると、そのモータに対応する軸は停止したことになる。
また、判断部３６は、衝突が検知された後に、モータの現在位置と、現在位置蓄積部３５が蓄積した現在位置との差が、あらかじめ決められたしきい値以上であるかどうか判断してもよい。その差がしきい値を超えている場合には、モータの位置があらかじめ決められた範囲以上変化したことになる。

なお、モータの現在電流は、トルク指令生成器３４に入力される。また、モータの現在速度は、電流指令生成器３３と、判断部３６とに入力される。また、モータの現在位置は、速度指令生成器３１と、現在位置蓄積部３５と、判断部３６とに入力される。なお、モータの現在速度は、モータの現在位置が時間微分されたものであってもよい。
また、サーボコントローラ１１−２〜１１−Ｎは、制御対象のモータが異なる以外は、サーボコントローラ１１−１と同様のものであり、その説明を省略する。

図３は、本実施の形態による電流指令生成器３３の構成を示すブロック図である。本実施の形態による電流指令生成器３３は、減算器４１と、積算器４２と、積分器４３と、加算器４４と、積分値蓄積部４５と、置換部４６とを備え、速度ＰＩ制御を行う。

減算器４１は、モータの現在速度と、切替器３２からの速度指令値との差分を算出する。具体的には、減算器４１は、切替器３２からの速度指令値から、モータの現在速度を減算してもよい。その減算器４１の出力は、積算器４２と積分器４３とに入力される。

積算器４２は、入力値である差分に比例ゲインを積算する。その積算値は、加算器４４に入力される。その比例ゲインは、比例制御で用いられる定数であり、設計者や使用者により設定される値である。減算器４１と、積算器４２とにより、ＰＩ制御における比例動作が行われることになる。

積分器４３は、モータの現在速度と速度指令値と差分である入力値を積分する。その積分した値である積分値は、加算器４４と積分値蓄積部４５とに入力される。この積分器４３による積分の期間は、設計者や使用者により設定される。減算器４１と積分器４３とにより、ＰＩ制御における積分動作が行われることになる。

加算器４４は、積分器４３からの積分値と、積算器４２からの積算値とを加算する。この加算器４４によって加算された加算値が、電流指令値である。

積分値蓄積部４５は、積分器４３が積分した積分値を所定の記録媒体に蓄積する。この記録媒体は、例えば、半導体メモリや、磁気ディスク等であり、積分値蓄積部４５が有していてもよく、あるいは積分値蓄積部４５の外部に存在してもよい。また、この記録媒体は、積分値を一時的に記憶するものであってもよく、そうでなくてもよい。なお、その記録媒体では、最新の所定期間分の積分値が記憶されていることが好適である。その所定期間は、実際に衝突が起こってからその衝突が検知されるまでの期間より長いことが好適である。衝突検知時に、衝突前の積分値が蓄積されているようにするためである。

置換部４６は、衝突検知部１５が衝突を検知した場合に、積分器４３の積分値を積分値蓄積部４５が蓄積した過去の積分値に置き換える。その過去の積分値は、衝突前の積分値である。例えば、置換部４６は、衝突が検知された場合に、積分値蓄積部４５が蓄積した積分値から、あらかじめ決められた期間だけ過去の積分値を読み出し、積分器４３の積分値を、その読み出した積分値に置き換えてもよい。そのあらかじめ決められた期間は、設計者や使用者によって設定される期間であり、マニピュレータ２が障害物に衝突してから、衝突検知部１５が衝突を検知するまでの期間と同程度か、それよりも少し長く設定されることが好適である。衝突が起こった場合には、減算器４１の出力が大きくなり、積分値も大きくなる。そのため、前述のように、衝突が検知された後に、速度指令値が０にされても、その大きな積分値が原因となって、モータに逆転トルクが働くのに時間がかかり、迅速な負荷低減を実現することが難しくなる。一方、衝突が検知された場合に、置換部４６によって積分器４３の積分値を衝突前の積分値に置換することによって、迅速な負荷低減を実現でき、減速機を傷めないようにすることができる。

ここで、衝突後に手先効果器が安全位置に回避される動作について、図４を用いて説明する。なお、図４では、説明の便宜上、手先効果器がｘｙ平面上を移動する場合について示しているが、実際には、手先効果器は３次元空間上を移動する。

手先効果器が障害物に衝突したことが検知されると、その時点の衝突位置が特定される。そして、衝突時制御部１８の制御によって、各モータに対する速度指令値が０に制御され、衝突による負荷がなくなるまで各モータに逆転トルクが発生する。その後、各モータの速度が０になると、その時点の全軸停止位置が特定され、衝突位置から全軸停止位置までの差分を全軸停止位置に加算した安全基準位置が算出される。また、手先効果器の軌跡から安全基準位置に近い位置である安全位置が特定され、手先効果器が、全軸停止位置から安全位置に移動される。それによって、衝突後の回避動作が終了する。なお、本実施の形態では、全軸停止位置から安全位置に移動する動作を終了動作と呼ぶこともある。全軸停止位置までの移動であれば、手先効果器が障害物にあたったままになっており、減速機を傷める可能性もあるが、終了動作によって、障害物から完全に離れることができ、減速機の負荷を適切に低減することができる。また、衝突位置から全軸停止位置までの距離と、全軸停止位置から安全基準位置までの距離とが等しくなるように安全基準位置が算出される場合には、衝突の程度に応じた安全基準位置の算出を行うことができる。例えば、衝突の検知が遅かった場合には、衝突位置から全軸停止位置までの距離が長くなることによって、全軸停止位置から安全基準位置までの距離も長くなり、より確実な回避が実現される。一方、衝突の検知が早かった場合には、衝突位置から全軸停止位置までの距離が短くなることによって、全軸停止位置から安全基準位置までの距離も短くなるが、早期に衝突を検知できているため減速機への負荷も小さく、それで問題ないと考えられる。また、安全位置は手先効果器の軌跡上の一点であるため、手先効果器が他の障害物に衝突する二次災害を防止することができる。なお、衝突位置や全軸停止位置は、各モータのエンコーダによって測定された位置である。

次に、フラグ記憶部２４で記憶されている衝突フラグ、全停止フラグ、各軸の回避状態について説明する。
衝突フラグは、衝突が発生したかどうかを示すフラグであり、衝突が検知されるとＴＲＵＥに設定され、衝突が検知されるまではＦＡＬＳＥに設定される。本実施の形態では、手先効果器の衝突を検知した場合に、衝突検知部１５が、フラグ記憶部２４で記憶されている衝突フラグをＴＲＵＥにするものとする。なお、フラグ記憶部２４で記憶されている衝突フラグの初期値はＦＡＬＳＥである。

全軸停止フラグは、全軸停止状態となっているかどうかを示すフラグであり、全軸停止状態が検知されるとＴＲＵＥに設定され、全軸停止状態が検知されるまではＦＡＬＳＥに設定される。本実施の形態では、全軸停止状態を検知した場合に、全軸停止状態検知部１７が、フラグ記憶部２４で記憶されている全軸停止フラグをＴＲＵＥにするものとする。なお、フラグ記憶部２４で記憶されている全軸停止フラグの初期値はＦＡＬＳＥである。

回避状態は、軸ごとに回避がどの状態であるのかを示す情報であり、「未回避」、「回避中」、「終了待ち」、「終了動作」の４種類がある。その状態の変化について、図５を用いて説明する。回避前、すなわち、衝突の検知前には、回避状態は未回避に設定される。また、衝突が検知されると、回避状態は回避中に設定される。また、衝突が検知された後に、現在速度が０になると、回避状態は終了待ちに設定される。また、回避状態が回避中である場合にモータの位置があらかじめ決められた範囲以上変化したとき、または、回避状態が終了待ちである場合に全軸停止状態が検知されたときには、回避状態は終了動作に設定される。なお、フラグ記憶部２４で記憶されている各軸に対応する回避状態の初期値は未回避である。

次に、制御装置１の動作について図６のフローチャートを用いて説明する。
（ステップＳ１０１）各軸位置指令算出部２２は、手先効果器の位置を受け付けたかどうか判断する。そして、位置を受け付けた場合には、ステップＳ１０２に進み、そうでない場合には、ステップＳ１０４に進む。

（ステップＳ１０２）各軸位置指令算出部２２は、受け付けた手先効果器の位置から、各軸の位置指令値を算出し、それぞれ対応するサーボコントローラ１１−１〜１１−Ｎに出力する。

（ステップＳ１０３）サーボコントローラ１１−１〜１１−Ｎは、受け取った位置指令値に応じて、マニピュレータ２の各モータを制御するサーボコントロールを行う。そして、ステップＳ１０１に戻る。なお、このサーボコントロールの詳細な処理については、図７のフローチャートを用いて後述する。

（ステップＳ１０４）位置算出部１２は、手先効果器の位置を算出するかどうか判断する。そして、手先効果器の位置を算出する場合には、ステップＳ１０５に進み、そうでない場合には、ステップＳ１０７に進む。なお、位置算出部１２は、例えば、定期的に手先効果器の位置を算出すると判断してもよく、または、所定のイベントの発生に応じて手先効果器の位置を算出すると判断してもよい。その所定のイベントは、例えば、各モータのエンコーダの値を受け取ったことであってもよい。

（ステップＳ１０５）位置算出部１２は、各モータの位置を用いて、手先効果器の位置を算出する。そのモータの位置は、例えば、エンコーダの値であってもよい。

（ステップＳ１０６）蓄積部１３は、位置算出部１２が算出した手先効果器の位置を位置記憶部１４に蓄積する。そして、ステップＳ１０１に戻る。

（ステップＳ１０７）衝突検知部１５は、手先効果器の衝突を検知したかどうか判断する。そして、衝突を検知した場合には、ステップＳ１０８に進み、そうでない場合には、ステップＳ１１２に進む。なお、衝突検知部１５は、衝突を検知すると、フラグ記憶部２４で記憶されている衝突フラグをＴＲＵＥに変更し、各回避状態を回避中に変更する。

（ステップＳ１０８）衝突時制御部１８は、各サーボコントローラ１１−１〜１１−Ｎにおいて速度指令値が０になるように制御する。すなわち、衝突時制御部１８は、各サーボコントローラ１１−１〜１１−Ｎの切替器３２が、速度指令値「０」を出力するように切り替える。

（ステップＳ１０９）衝突位置特定部１６は、その時点の手先効果器の位置である衝突位置を特定する。

（ステップＳ１１０）各サーボコントローラ１１−１〜１１−Ｎの現在位置蓄積部３５は、その時点の現在位置を記録媒体に蓄積する。

（ステップＳ１１１）各サーボコントローラ１１−１〜１１−Ｎの電流指令生成器３３が有する置換部４６は、積分器４３の積分値を積分値蓄積部４５が蓄積した過去の積分値に置換する。そして、ステップＳ１０１に戻る。

（ステップＳ１１２）全軸停止状態検知部１７は、全軸停止状態を検知したかどうか判断する。そして、全軸停止状態を検知した場合には、ステップＳ１１３に進み、そうでない場合には、ステップＳ１１７に進む。なお、全軸停止状態検知部１７は、フラグ記憶部２４で記憶されている衝突フラグがＴＲＵＥであり、かつ、すべての回避状態が終了待ちである場合に、全軸停止状態を検知してもよい。また、全軸停止状態検知部１７は、全軸停止状態を検知した場合に、フラグ記憶部２４で記憶されている全軸停止フラグをＴＲＵＥに変更し、各回避状態を終了動作に変更する。

（ステップＳ１１３）衝突時制御部１８は、各サーボコントローラ１１−１〜１１−Ｎにおける速度指令値を０にする制御を解除する。すなわち、衝突時制御部１８は、各サーボコントローラ１１−１〜１１−Ｎの切替器３２が、速度指令生成器３１の生成した速度指令値を出力するように切り替える。

（ステップＳ１１４）安全基準位置算出部１９は、その時点の手先効果器の位置である全軸停止位置と衝突位置とを用いて、安全基準位置を算出する。

（ステップＳ１１５）安全位置特定部２０は、位置記憶部１４で記憶されている手先効果器の過去の位置のうち、安全基準位置算出部１９が算出した安全基準位置に近い位置である安全位置を特定する。

（ステップＳ１１６）安全位置移動制御部２１は、手先効果器が安全位置に移動するように制御する。すなわち、安全位置移動制御部２１は、手先効果器が安全位置に移動するように、割り込み処理により、その安全位置を各軸位置指令算出部２２に渡す。その結果、各軸位置指令算出部２２は、手先効果器が安全位置に移動するように各軸の位置指令値を算出し、各サーボコントローラ１１−１〜１１−Ｎは、その各軸の位置指令値に応じて、マニピュレータ２の各モータを制御する。この各軸位置指令算出部２２と、各サーボコントローラ１１−１〜１１−Ｎとによる動作は、ステップＳ１０２、Ｓ１０３と同様の処理によって実現される。そして、手先効果器の位置が安全位置となると、一連の回避処理は終了となる。

（ステップＳ１１７）モータ停止部２３は、いずれかのモータの位置があらかじめ決められた範囲以上変化したかどうか判断する。そして、いずれかのモータの位置があらかじめ決められた範囲以上変化した場合には、ステップＳ１１８に進み、そうでない場合には、ステップＳ１０１に戻る。なお、モータ停止部２３は、フラグ記憶部２４で記憶されている全軸停止フラグがＦＡＬＳＥであり、いずれかの回避状態が終了動作である場合に、いずれかのモータの位置があらかじめ決められた範囲以上変化したと判断してもよい。

（ステップＳ１１８）モータ停止部２３は、すべてのモータを停止させる。そして、一連の回避処理は終了となる。

図７は、図６のフローチャートにおけるサーボコントロールの処理（ステップＳ１０３）の詳細を示すフローチャートである。図７のフローチャートでは、サーボコントローラ１１−１によるサーボコントロールの処理について説明するが、他のサーボコントローラ１１−２〜１１−Ｎによるサーボコントロールの処理も同様である。

（ステップＳ２０１）速度指令生成器３１は、位置指令値とモータの現在位置とを用いて速度指令値を生成する。

（ステップＳ２０２）電流指令生成器３３は、速度指令値とモータの現在速度とを用いて電流指令値を生成する。なお、その電流指令値の生成で用いる速度指令値は、衝突が検知され、全軸停止状態が検知されるまでの期間は０であり、それ以外の期間は速度指令生成器３１の生成した速度指令値である。その切り替えは、切替器３２によって行われる。また、積分値蓄積部４５は、積分器４３が積分した積分値を蓄積する。

（ステップＳ２０３）トルク指令生成器３４は、電流指令生成器３３が生成した電流指令値とモータの現在電流とを用いて、トルク指令値を生成し、モータに出力する。その結果、サーボコントローラ１１−１の制御対象であるモータが、そのトルク指令値に応じて動作する。

（ステップＳ２０４）判断部３６は、サーボコントローラ１１−１が制御するモータに対応する回避状態が「回避中」であるかどうか判断する。そして、回避中である場合には、ステップＳ２０５に進み、そうでない場合には、図６のフローチャートに戻る。

（ステップＳ２０５）判断部３６は、その時点の現在位置が、現在位置蓄積部３５が蓄積した現在位置としきい値以上変化しているかどうか判断する。そして、しきい値以上変化している場合、すなわち、制御対象のモータの位置があらかじめ決められた範囲以上変化している場合には、ステップＳ２０６に進み、そうでない場合には、ステップＳ２０７に進む。

（ステップＳ２０６）判断部３６は、サーボコントローラ１１−１が制御するモータに対応する回避状態を「終了動作」に更新する。そして、図６のフローチャートに戻る。

（ステップＳ２０７）判断部３６は、その時点の現在速度が０であるかどうか判断する。そして、０である場合には、ステップＳ２０８に進み、そうでない場合には、図６のフローチャートに戻る。

（ステップＳ２０８）判断部３６は、サーボコントローラ１１−１が制御するモータに対応する回避状態を「終了待ち」に更新する。そして、図６のフローチャートに戻る。

なお、図７のフローチャートでは、サーボコントローラ１１−１のみのサーボコントロールの処理について説明したが、他のサーボコントローラ１１−２〜１１−Ｎのサーボコントロールの処理も並列して行われるものとする。また、図７のフローチャートで示されるサーボコントロールの処理（図６のフローチャートにおけるステップＳ１０３）と、図６のフローチャートにおけるステップＳ１０３以外の処理とは、並列して実行されてもよい。その場合には、ステップＳ１０３は、各サーボコントローラ１１−１〜１１−Ｎに位置指令値を指示する処理であると考えてもよい。また、図７のフローチャートで示されるサーボコントロールの処理と、図６のフローチャートにおけるステップＳ１０３以外の処理とが並列して実行される場合には、そのサーボコントロール中の手先効果器の衝突も検知されることになる。

次に、本実施の形態による制御装置１の動作について、具体例を用いて説明する。この具体例では、マニピュレータ２が第１軸から第６軸までの６軸を有しており、サーボコントローラ１１−１〜１１−６によって、それぞれの軸を制御する場合について説明する。したがって、フラグ記憶部２４で記憶されているフラグ等の初期値は次の通りである。
衝突フラグ：ＦＡＬＳＥ
全軸停止フラグ：ＦＡＬＳＥ
第１軸の回避状態：未回避
第２軸の回避状態：未回避
第３軸の回避状態：未回避
第４軸の回避状態：未回避
第５軸の回避状態：未回避
第６軸の回避状態：未回避

まず、正常動作時の動作について簡単に説明する。正常動作時には、各軸位置指令算出部２２は、ティーチングされた手先効果器の位置を受け付け（ステップＳ１０１）、その手先効果器の位置から各軸の位置指令値を算出し、それぞれ対応するサーボコントローラ１１−１〜１１−６に出力する（ステップＳ１０２）。そして、各サーボコントローラ１１−１〜１１−６では、各軸位置指令算出部２２から受け取った位置指令値を用いて、制御対象のモータに対するサーボコントロールが行われる（ステップＳ１０３）。具体的には、速度指令値の生成、電流指令値の生成、及びトルク指令値の生成が行われ、そのトルク指令値に応じたモータの動作が行われる（ステップＳ２０１〜２０３）。なお、各電流指令生成器３３では、新たな積分値が算出されるごとに、積分値蓄積部４５がその積分値を蓄積する。また、この時点ではまだ衝突が検知されていないため、各サーボコントローラ１１−１〜１１−６において、切替器３２は、速度指令生成器３１の生成した速度指令値を電流指令生成器３３に入力することになる。

また、位置算出部１２は、定期的に位置を算出すると判断し（ステップＳ１０４）、各軸のエンコーダ値から手先効果器の位置を算出する（ステップＳ１０５）。その手先効果器の位置は、蓄積部１３によって位置記憶部１４に蓄積される（ステップＳ１０６）。この位置の算出と蓄積とは定期的に繰り返して行われる。

次に、衝突が検知されたときの動作について説明する。衝突検知部１５が手先効果器の衝突を検知すると、フラグ記憶部２４で記憶されているフラグ等を次のように更新し、衝突位置特定部１６、衝突時制御部１８、各現在位置蓄積部３５、及び各置換部４６に衝突の発生を伝える（ステップＳ１０７）。
衝突フラグ：ＴＲＵＥ
全軸停止フラグ：ＦＡＬＳＥ
第１軸の回避状態：回避中
第２軸の回避状態：回避中
第３軸の回避状態：回避中
第４軸の回避状態：回避中
第５軸の回避状態：回避中
第６軸の回避状態：回避中

衝突の発生が伝えられると、衝突時制御部１８は、各サーボコントローラ１１−１〜１１−６において、電流指令生成器３３への入力が０になるように切替器３２を切り替える（ステップＳ１０８）。また、衝突位置特定部１６は、その時点に位置記憶部１４で記憶されている最新の位置である衝突位置を読み出して、衝突位置特定部１６が有する図示しない記録媒体に蓄積する（ステップＳ１０９）。また、各サーボコントローラ１１−１〜１１−６の現在位置蓄積部３５は、その時点のモータの現在位置を図示しない記録媒体に蓄積する（ステップＳ１１０）。また、各サーボコントローラ１１−１〜１１−６の電流指令生成器３３が有する置換部４６は、積分器４３の積分値を、積分値蓄積部４５が蓄積した、あらかじめ決められている期間だけ以前の積分値に置換する（ステップＳ１１１）。この後、各軸の速度が０になるようにサーボコントロールが行われ（ステップＳ１０１〜Ｓ１０３）、その動作に応じた位置が順次、蓄積される（ステップＳ１０４〜Ｓ１０６）。なお、衝突が検知されてから全軸停止状態が検知されるまでは、各軸の位置指令値に応じて生成された速度指令値は用いられないため、各軸位置指令算出部２２による各軸の位置指令値の算出処理（ステップＳ１０２）や、速度指令値の生成処理（ステップＳ２０１）を行わなくてもよい。

各軸の回避状態が回避中である際に、第５軸のサーボコントローラ１１−５の判断部３６が、第５軸のモータの現在速度が０になったと判断したとする（ステップＳ２０７）。すると、その判断部３６は、フラグ記憶部２４で記憶されている第５軸の回避状態を次のように更新する（ステップＳ２０８）。
衝突フラグ：ＴＲＵＥ
全軸停止フラグ：ＦＡＬＳＥ
第１軸の回避状態：回避中
第２軸の回避状態：回避中
第３軸の回避状態：回避中
第４軸の回避状態：回避中
第５軸の回避状態：終了待ち
第６軸の回避状態：回避中

その後、他の軸においても現在速度が０になったと判断され、全軸の回避状態が「終了待ち」になると、全軸停止状態検知部１７は、全軸停止状態を検知し、フラグ記憶部２４で記憶されているフラグ等を次のように更新し、衝突時制御部１８、及び安全基準位置算出部１９に全軸の停止を伝える（ステップＳ１１２）。
衝突フラグ：ＴＲＵＥ
全軸停止フラグ：ＴＲＵＥ
第１軸の回避状態：終了動作
第２軸の回避状態：終了動作
第３軸の回避状態：終了動作
第４軸の回避状態：終了動作
第５軸の回避状態：終了動作
第６軸の回避状態：終了動作

全軸の停止が伝えられると、衝突時制御部１８は、各サーボコントローラ１１−１〜１１−６において、電流指令生成器３３への入力が速度指令生成器３１の生成した速度指令値になるように切替器３２を切り替える（ステップＳ１１３）。また、安全基準位置算出部１９は、その時点に位置記憶部１４で記憶されている最新の位置である全軸停止位置を読み出すと共に、衝突位置特定部１６から衝突位置を受け取る。そして、安全基準位置算出部１９は、衝突位置を始点として、全軸停止位置を終点とするベクトルを算出し、全軸停止位置から、そのベクトルだけ離れた点である安全基準位置を算出して、安全位置特定部２０に渡す（ステップＳ１１４）。安全位置特定部２０は、安全基準位置を受け取ると、位置記憶部１４で記憶されている手先効果器の軌跡のうち、その安全基準位置に最も近い位置である安全位置を特定し、その安全位置を安全位置移動制御部２１に渡す（ステップＳ１１５）。その安全位置を受け取ると、安全位置移動制御部２１は、手先効果器が安全位置に移動するように制御する（ステップＳ１１６）。具体的には、安全位置移動制御部２１は、割り込み処理により、安全位置を各軸位置指令算出部２２に入力する。そして、手先効果器が安全位置に移動するための各軸の位置指令値を各軸位置指令算出部２２が生成して各サーボコントローラ１１−１〜１１−６に出力することによって、手先効果器が安全位置に移動するようにサーボコントロールが行われ、手先効果器が安全位置に移動する。このようにして、手先効果器の安全位置への回避動作が終了する。

次に、モータが停止される場合の動作について説明する。第３軸の回避状態が「回避中」である場合に、サーボコントローラ１１−３の判断部３６によって、現在位置蓄積部３５が蓄積した現在位置と、その時点の現在位置との差があらかじめ決められたしきい値以上になったと判断されたとする（ステップＳ２０４，Ｓ２０５）。すると、その判断部３６は、フラグ記憶部２４で記憶されているフラグ等を次のように更新する（ステップＳ２０６）。
衝突フラグ：ＴＲＵＥ
全軸停止フラグ：ＦＡＬＳＥ
第１軸の回避状態：回避中
第２軸の回避状態：回避中
第３軸の回避状態：終了動作
第４軸の回避状態：回避中
第５軸の回避状態：回避中
第６軸の回避状態：回避中

すると、全軸停止フラグが「ＦＡＬＳＥ」であり、回避状態が「終了動作」である軸が存在するため、モータ停止部２３は、モータの位置が所定の範囲以上変化したしたと判断し（ステップＳ１１７）、回避状態が「終了動作」である第３軸のモータと、その他のモータとを停止させる（ステップＳ１１８）。したがって、マニピュレータ２がその時点で停止させられ、回避動作が終了する。このようにして、回避動作中に一部のアームが自重で落下した場合に、それによってマニピュレータ２が破損することを回避できる。

以上のように、本実施の形態による制御装置１によれば、手先効果器が衝突した場合に、手先効果器の位置を全軸停止位置よりもさらに障害物から離れる方向に移動させることができ、減速機に負荷がかかったままになることを回避することができる。また、その手先効果器の移動先を安全位置とすることによって、過去の軌跡上に手先効果器を回避することができる。その結果、回避動作によって手先効果器が他の障害物に衝突する二次災害を防止することができる。また、衝突が検知された場合に、各サーボコントローラ１１−１〜１１−Ｎの電流指令生成器３３が有する積分器４３の積分値を衝突による負荷の影響を受ける前の値に置換することにより、迅速な回避が可能となり、衝突による負荷によって減速機を傷める可能性をより低減することができる。また、衝突が検知された後に、モータの位置があらかじめ決められた範囲以上変化した場合に、そのモータを停止させることによって、アームが回避中に自重などの影響で落下することを防止できる。

なお、本実施の形態では、衝突が検知された後に、モータの位置があらかじめ決められた範囲以上変化すると、すべてのモータを停止させ、回避動作を終了する場合について説明したが、そうでなくてもよい。例えば、モータの位置があらかじめ決められた範囲以上変化した場合に、そのモータを停止させるが、それ以外のモータについて、全軸停止状態となるまで移動させ、その後、停止されたモータ以外を用いて、手先効果器が安全位置に移動するように制御してもよい。すなわち、あるモータが停止された場合には、停止されていないモータについて、全軸停止状態を検知し、その検知に応じて安全基準位置の算出や、安全位置の特定を行い、手先効果器をその安全位置に移動させるようにしてもよい。

また、本実施の形態では、フラグ記憶部２４において、衝突フラグ、全軸停止フラグ、各軸の回避状態がそれぞれ記憶されている場合について説明したが、そうでなくてもよい。例えば、衝突フラグ、及び全軸停止フラグは、フラグ記憶部２４で記憶されており、各軸の回避状態は、各軸に対応するサーボコントローラ１１−１〜１１−Ｎにおいてそれぞれ記憶されていてもよい。また、その場合に、各軸の回避状態は、各サーボコントローラ１１−１〜１１−Ｎが有する、その回避状態を更新する図示しない回避状態更新部によって更新されてもよい。

また、本実施の形態では、フラグや回避状態を用いて処理を行う場合について主に説明したが、そうでなくてもよい。フラグ等を用いないで処理を行ってもよい。その場合には、例えば、制御装置１は、フラグ記憶部２４を備えていなくてもよい。

また、本実施の形態では、衝突が検知された後に、モータの位置があらかじめ決められた範囲以上変化すると、モータを停止させる場合について説明したが、そうでなくてもよい。回避中にモータの位置があらかじめ決められた範囲以上変化したとしても、モータを停止させなくてもよい。その場合には、制御装置１は、モータ停止部２３を備えていなくてもよい。また、そのモータの停止を行わない場合には、各サーボコントローラ１１−１〜１１−Ｎは、現在位置蓄積部３５を備えていなくてもよく、また各判断部３６は、モータの現在位置に関する判断を行わなくてもよい。

また、本実施の形態では、衝突が検知されると、各サーボコントローラ１１−１〜１１−Ｎの電流指令生成器３３が有する積分器４３の積分値を衝突による負荷の影響を受ける前の値に置換する場合について説明したが、そうでなくてもよい。衝突が検知されたとしても、そのような置換を行わなくてもよい。その場合には、各サーボコントローラ１１−１〜１１−Ｎの電流指令生成器３３は、積分値蓄積部４５や置換部４６を備えていなくてもよい。

また、本実施の形態において、位置算出部１２は、複数のモータの位置から手先効果器の位置を算出すると共に、その算出した位置の間の位置を補間によって算出することによって、さらに細かい手先効果器の位置をも算出してもよい。その場合には、補間された手先効果器の位置も、蓄積部１３によって位置記憶部１４に蓄積されてもよい。

また、本実施の形態では、電流指令生成器３３がＰＩ制御を行う場合について説明したが、それに限定されないことは言うまでもない。電流指令生成器３３は、例えば、ＰＩＤ制御を行ってもよく、または、その他のフィードバック制御を行ってもよい。

また、本実施の形態において、制御装置１が、複数のサーボコントローラ１１−１〜１１−Ｎ以外の構成要素の部分を、モーションコントローラと呼んでもよい。すなわち、本実施の形態による制御装置１は、複数のサーボコントローラ１１−１〜１１−Ｎと、その複数のサーボコントローラ１１−１〜１１−Ｎを制御するモーションコントローラとを備えていると考えてもよい。そのモーションコントローラは、前述のように、例えば、手先効果器の軌道や目標位置から各軸の位置指令値を計算し、各サーボコントローラ１１−１〜１１−Ｎを制御する。

また、上記実施の形態において、各処理または各機能は、単一の装置または単一のシステムによって集中処理されることによって実現されてもよく、あるいは、複数の装置または複数のシステムによって分散処理されることによって実現されてもよい。

また、上記実施の形態において、各構成要素間で行われる情報の受け渡しは、例えば、その情報の受け渡しを行う２個の構成要素が物理的に異なるものである場合には、一方の構成要素による情報の出力と、他方の構成要素による情報の受け付けとによって行われてもよく、あるいは、その情報の受け渡しを行う２個の構成要素が物理的に同じものである場合には、一方の構成要素に対応する処理のフェーズから、他方の構成要素に対応する処理のフェーズに移ることによって行われてもよい。

また、上記実施の形態において、各構成要素が実行する処理に関係する情報、例えば、各構成要素が受け付けたり、取得したり、選択したり、生成したり、送信したり、受信したりした情報や、各構成要素が処理で用いるしきい値や数式、アドレス、設置値等の情報等は、上記説明で明記していない場合であっても、図示しない記録媒体において、一時的に、あるいは長期にわたって保持されていてもよい。また、その図示しない記録媒体への情報の蓄積を、各構成要素、あるいは、図示しない蓄積部が行ってもよい。また、その図示しない記録媒体からの情報の読み出しを、各構成要素、あるいは、図示しない読み出し部が行ってもよい。

また、上記実施の形態において、各構成要素等で用いられる情報、例えば、各構成要素が処理で用いるしきい値やアドレス、各種の設定値等の情報がユーザによって変更されてもよい場合には、上記説明で明記していない場合であっても、ユーザが適宜、それらの情報を変更できるようにしてもよく、あるいは、そうでなくてもよい。それらの情報をユーザが変更可能な場合には、その変更は、例えば、ユーザからの変更指示を受け付ける図示しない受付部と、その変更指示に応じて情報を変更する図示しない変更部とによって実現されてもよい。その図示しない受付部による変更指示の受け付けは、例えば、入力デバイスからの受け付けでもよく、通信回線を介して送信された情報の受信でもよく、所定の記録媒体から読み出された情報の受け付けでもよい。

また、上記実施の形態において、制御装置１に含まれる２以上の構成要素が通信デバイスや入力デバイス等を有する場合に、２以上の構成要素が物理的に単一のデバイスを有してもよく、あるいは、別々のデバイスを有してもよい。

また、上記実施の形態において、各構成要素は専用のハードウェアにより構成されてもよく、あるいは、ソフトウェアにより実現可能な構成要素については、プログラムを実行することによって実現されてもよい。例えば、ハードディスクや半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。その実行時に、プログラム実行部は、記憶部や記録媒体にアクセスしながらプログラムを実行してもよい。なお、上記実施の形態における制御装置１を実現するソフトウェアは、以下のようなプログラムである。つまり、このプログラムは、コンピュータを、複数のモータによってそれぞれ駆動される複数の関節によって連結された複数のアームにより手先効果器を移動させるマニピュレータの制御を行う制御装置として機能させるためのプログラムであって、複数のモータの位置から手先効果器の位置を算出する位置算出部、位置算出部が算出した手先効果器の位置を蓄積する蓄積部、手先効果器の衝突を検知する衝突検知部、衝突検知部が衝突を検知した時の手先効果器の位置である衝突位置を特定する衝突位置特定部、衝突検知部が衝突を検知した後に、複数のモータのすべてが停止した状態である全軸停止状態になったことを検知する全軸停止状態検知部、衝突検知部が衝突を検知してから、全軸停止状態検知部が全軸停止状態を検知するまで、速度指令値が０になるように、マニピュレータの各モータをそれぞれ制御する複数のサーボコントローラを制御する衝突時制御部、全軸停止状態検知部が全軸停止状態を検知した時の手先効果器の位置である全軸停止位置から、衝突位置から全軸停止位置に向かう方向に沿って移動した位置である安全基準位置を算出する安全基準位置算出部、蓄積部が蓄積した手先効果器の過去の位置のうち、安全基準位置にあらかじめ決められた条件をみたすほど近い位置である安全位置を特定する安全位置特定部、手先効果器の位置が安全位置となるように制御する安全位置移動制御部として機能させるためのプログラムである。

なお、上記プログラムにおいて、上記プログラムが実現する機能には、ハードウェアでしか実現できない機能は含まれない。例えば、情報を蓄積する蓄積部や、制御を行う制御部などにおけるモデムやインターフェースカードなどのハードウェアでしか実現できない機能は、上記プログラムが実現する機能には少なくとも含まれない。

また、このプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体（例えば、ＣＤ−ＲＯＭなどの光ディスクや磁気ディスク、半導体メモリなど）に記録されたプログラムが読み出されることによって実行されてもよい。また、このプログラムは、プログラムプロダクトを構成するプログラムとして用いられてもよい。
また、このプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、あるいは分散処理を行ってもよい。

図８は、上記プログラムを実行して、上記実施の形態による制御装置１を実現するコンピュータシステム９００の構成を示す図である。図８において、コンピュータシステム９００は、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）ドライブ９０５を含むコンピュータ９０１と、キーボード９０２と、マウス９０３と、モニタ９０４とを備える。コンピュータ９０１は、ＣＤ−ＲＯＭドライブ９０５に加えて、ＭＰＵ（Ｍｉｃｒｏ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）９１１と、ブートアッププログラム等のプログラムを記憶するためのＲＯＭ９１２と、ＭＰＵ９１１に接続され、アプリケーションプログラムの命令を一時的に記憶すると共に、一時記憶空間を提供するＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）９１３と、アプリケーションプログラム、システムプログラム、及びデータを記憶するハードディスク９１４と、ＭＰＵ９１１、ＲＯＭ９１２等を相互に接続するバス９１５とを備える。なお、コンピュータ９０１は、ＬＡＮやＷＡＮ等への接続を提供する図示しないネットワークカードを含んでいてもよい。また、コンピュータシステム９００は、例えば、キーボード９０２、マウス９０３、及びモニタ９０４に代えてタッチパネルを有してもよい。

コンピュータシステム９００に、上記実施の形態による制御装置１の機能を実行させるプログラムは、例えば、ＣＤ−ＲＯＭ９２１に記憶されて、ＣＤ−ＲＯＭドライブ９０５に挿入され、ハードディスク９１４に転送されてもよい。これに代えて、そのプログラムは、図示しないネットワークを介してコンピュータ９０１に送信され、ハードディスク９１４に記憶されてもよい。プログラムは実行の際にＲＡＭ９１３にロードされる。なお、プログラムは、ＣＤ−ＲＯＭ９２１、またはネットワークから直接、ロードされてもよい。

プログラムは、コンピュータ９０１に、上記実施の形態による制御装置１の機能を実行させるオペレーティングシステム（ＯＳ）、またはサードパーティプログラム等を必ずしも含んでいなくてもよい。プログラムは、制御された態様で適切な機能（モジュール）を呼び出し、所望の結果が得られるようにする命令の部分のみを含んでいてもよい。コンピュータシステム９００がどのように動作するのかについては周知であり、詳細な説明は省略する。

また、本発明は、以上の実施の形態に限定されることなく、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。

以上より、本発明による制御装置等によれば、手先効果器が衝突した場合に、減速機の負荷を低減させることができ、手先効果器を安全な位置に回避させることができるという効果が得られ、マニピュレータの制御装置等として有用である。

１ 制御装置
２ マニピュレータ
１１−１〜１１−Ｎ サーボコントローラ
１２ 位置算出部
１３ 蓄積部
１４ 位置記憶部
１５ 衝突検知部
１６ 衝突位置特定部
１７ 全軸停止状態検知部
１８ 衝突時制御部
１９ 安全基準位置算出部
２０ 安全位置特定部
２１ 安全位置移動制御部
２２ 各軸位置指令算出部
２３ モータ停止部
２４ フラグ記憶部
３１ 速度指令生成器
３２ 切替器
３３ 電流指令生成器
３４ トルク指令生成器
３５ 現在位置蓄積部
３６ 判断部
４１ 減算器
４２ 積算器
４３ 積分器
４４ 加算器
４５ 積分値蓄積部
４６ 置換部
１００ 産業用ロボット

Claims (6)

1. 複数のモータによってそれぞれ駆動される複数の関節によって連結された複数のアームにより手先効果器を移動させるマニピュレータの制御を行う制御装置であって、
   前記マニピュレータの各モータをそれぞれ制御する複数のサーボコントローラと、
   前記複数のモータの位置から手先効果器の位置を算出する位置算出部と、
   前記位置算出部が算出した手先効果器の位置を蓄積する蓄積部と、
   前記手先効果器の衝突を検知する衝突検知部と、
   前記衝突検知部が衝突を検知した時の前記手先効果器の位置である衝突位置を特定する衝突位置特定部と、
   前記衝突検知部が衝突を検知した後に、前記複数のモータのすべてが停止した状態である全軸停止状態になったことを検知する全軸停止状態検知部と、
   前記衝突検知部が衝突を検知してから、前記全軸停止状態検知部が全軸停止状態を検知するまで、速度指令値が０になるように前記複数のサーボコントローラを制御する衝突時制御部と、
   前記全軸停止状態検知部が全軸停止状態を検知した時の前記手先効果器の位置である全軸停止位置から、前記衝突位置から前記全軸停止位置に向かう方向に沿って移動した位置である安全基準位置を算出する安全基準位置算出部と、
   前記蓄積部が蓄積した手先効果器の過去の位置のうち、前記安全基準位置にあらかじめ決められた条件をみたすほど近い位置である安全位置を特定する安全位置特定部と、
   前記手先効果器の位置が前記安全位置となるように制御する安全位置移動制御部と、を備えた制御装置。
2. 前記サーボコントローラは、
   モータの現在位置と位置指令値とに応じて速度指令値を生成する速度指令生成器と、
   前記モータの現在速度と速度指令値との差分を積分器により積分した値と、当該差分に積算器により比例ゲインを積算した値とを加算することにより電流指令値を生成する電流指令生成器と、
   前記モータの現在電流と前記電流指令生成器が生成した電流指令値とに応じてトルク指令値を生成し、前記モータに出力するトルク指令生成器と、を備え、
   前記電流指令生成器は、
   前記積分器の積分値を蓄積する積分値蓄積部と、
   前記衝突検知部が衝突を検知した場合に、前記積分器の積分値を前記積分値蓄積部が蓄積した過去の積分値に置き換える置換部と、を備え、
   前記衝突検知部が衝突を検知してから、前記全軸停止状態検知部が全軸停止状態を検知するまでは、前記衝突時制御部によって前記電流指令生成器が用いる速度指令値が０にされる、請求項１記載の制御装置。
3. 前記衝突検知部が衝突を検知した後に、いずれかのモータの位置があらかじめ決められた範囲以上変化した場合に、当該モータを停止させるモータ停止部をさらに備えた、請求項１または請求項２記載の制御装置。
4. 前記安全基準位置算出部は、前記衝突位置から前記全軸停止位置までの距離と、前記全軸停止位置から前記安全基準位置までの距離とが等しくなるように前記安全基準位置を算出する、請求項１から請求項３のいずれか記載の制御装置。
5. 前記安全位置特定部は、前記蓄積部が蓄積した過去の位置のうち、前記安全基準位置に距離が最も近い位置である安全位置を特定する、請求項１から請求項４のいずれか記載の制御装置。
6. 複数のモータによってそれぞれ駆動される複数の関節によって連結された複数のアームにより手先効果器を移動させるマニピュレータの制御方法であって、
   前記複数のモータの位置から手先効果器の位置を算出する位置算出ステップと、
   前記位置算出ステップで算出した手先効果器の位置を蓄積する蓄積ステップと、
   前記手先効果器の衝突を検知する衝突検知ステップと、
   前記衝突検知ステップで衝突を検知した時の前記手先効果器の位置である衝突位置を特定する衝突位置特定ステップと、
   前記衝突検知ステップで衝突を検知した後に、前記複数のモータのすべてが停止した状態である全軸停止状態になったことを検知する全軸停止状態検知ステップと、
   前記衝突検知ステップで衝突を検知してから、前記全軸停止状態検知ステップで全軸停止状態を検知するまで、速度指令値が０になるように、前記マニピュレータの各モータをそれぞれ制御する複数のサーボコントローラを制御する衝突時制御ステップと、
   前記全軸停止状態検知ステップで全軸停止状態を検知した時の前記手先効果器の位置である全軸停止位置から、前記衝突位置から前記全軸停止位置に向かう方向に沿って移動した位置である安全基準位置を算出する安全基準位置算出ステップと、
   前記蓄積ステップで蓄積した手先効果器の過去の位置のうち、前記安全基準位置にあらかじめ決められた条件をみたすほど近い位置である安全位置を特定する安全位置特定ステップと、
   前記手先効果器の位置が前記安全位置となるように制御する安全位置移動制御ステップと、を備えた制御方法。

Images