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

制御装置、及び制御方法 Download PDF

Info

Publication number
JP5912953B2
JP5912953B2 JP2012158672A JP2012158672A JP5912953B2 JP 5912953 B2 JP5912953 B2 JP 5912953B2 JP 2012158672 A JP2012158672 A JP 2012158672A JP 2012158672 A JP2012158672 A JP 2012158672A JP 5912953 B2 JP5912953 B2 JP 5912953B2
Authority
JP
Japan
Prior art keywords
collision
unit
motor
axis
hand effector
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2012158672A
Other languages
English (en)
Other versions
JP2014018901A (ja
Inventor
洋人 坂原
洋人 坂原
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Daihen Corp
Original Assignee
Daihen Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Daihen Corp filed Critical Daihen Corp
Priority to JP2012158672A priority Critical patent/JP5912953B2/ja
Publication of JP2014018901A publication Critical patent/JP2014018901A/ja
Application granted granted Critical
Publication of JP5912953B2 publication Critical patent/JP5912953B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Manipulator (AREA)

Description

本発明は、マニピュレータの衝突後の制御を行う制御装置等に関する。
産業用ロボットを構成するマニピュレータが障害物等に衝突した場合には、迅速に負荷を低減させなければ、モータや減速機を破損することになる。そのため、例えば、特許文献1には、衝突時の外力ベクトルの反対方向の速度ベクトルの方向に回避動作を行う手法が記載されている。また、特許文献2には、位置データを保存しておき、衝突時には保存した位置データにおける、ある一定距離前の位置に向かって回避させることによって、負荷を抑えると共に、安全な回避を実現する手法が記載されている。また、特許文献3には、衝突時に位置ループゲインを下げることで、減速機の負荷を抑える手法が記載されている。また、特許文献4には、衝突時に速度指令値を0にし、逆転トルクを発生させると共に、全軸を停止させることによって、衝突後、迅速にロボット全体を停止させる手法が記載されている。
特開2005−100143号公報 特開平11−245191号公報 特開平11−10580号公報 特開平3−281194号公報
しかしながら、特許文献1の手法では、回避時の移動方向が使用者の教示した方向ではないため、回避方向に別の障害物が存在し、二次災害を引き起こす可能性があるという問題があった。
また、特許文献2の手法では、過去の軌跡上への回避を行うため、安全な回避動作を実現できるが、あらかじめ決められた距離だけの回避であるため、衝突検知が遅れた場合には、回避後でもロボットが障害物に衝突したままになり、減速機を傷める可能性があるという問題があった。
また、特許文献3の手法では、ループゲインを下げることによって負荷を低減しているが、特許文献2の場合と同様に、回避後にも衝突したままになることがあるという問題があった。
また、特許文献4の手法では、即座にロボットを停止させることができるが、特許文献2,3の場合と同様に、回避後にも衝突したままになることがあるという問題があった。
本発明は、上記問題点を解決するためになされたものであり、マニピュレータが衝突した場合に、減速機の負荷を低減させることができると共に、手先効果器を安全な位置に回避させることができるマニピュレータの制御装置等を提供することを目的とする。
上記目的を達成するため、本発明による制御装置は、複数のモータによってそれぞれ駆動される複数の関節によって連結された複数のアームにより手先効果器を移動させるマニピュレータの制御を行う制御装置であって、マニピュレータの各モータをそれぞれ制御する複数のサーボコントローラと、複数のモータの位置から手先効果器の位置を算出する位置算出部と、位置算出部が算出した手先効果器の位置を蓄積する蓄積部と、手先効果器の衝突を検知する衝突検知部と、衝突検知部が衝突を検知した時の手先効果器の位置である衝突位置を特定する衝突位置特定部と、衝突検知部が衝突を検知した後に、複数のモータのすべてが停止した状態である全軸停止状態になったことを検知する全軸停止状態検知部と、衝突検知部が衝突を検知してから、全軸停止状態検知部が全軸停止状態を検知するまで、速度指令値が0になるように複数のサーボコントローラを制御する衝突時制御部と、全軸停止状態検知部が全軸停止状態を検知した時の手先効果器の位置である全軸停止位置から、衝突位置から全軸停止位置に向かう方向に沿って移動した位置である安全基準位置を算出する安全基準位置算出部と、蓄積部が蓄積した手先効果器の過去の位置のうち、安全基準位置にあらかじめ決められた条件をみたすほど近い位置である安全位置を特定する安全位置特定部と、手先効果器の位置が安全位置となるように制御する安全位置移動制御部と、を備えたものである。
このような構成によって、衝突後、手先効果器を障害物から離れた位置である安全位置に移動させることができる。そのため、手先効果器が障害物に衝突したままになることを回避でき、減速機の負荷を低減させることができる。また、その安全位置が手先効果器の過去の軌跡上に存在することにより、安全位置への移動によって別の障害物に衝突することを防止でき、安全な回避動作を実現できる。
また、本発明による制御装置では、サーボコントローラは、モータの現在位置と位置指令値とに応じて速度指令値を生成する速度指令生成器と、そのモータの現在速度と速度指令値との差分を積分器により積分した値と、その差分に積算器により比例ゲインを積算した値とを加算することにより電流指令値を生成する電流指令生成器と、そのモータの現在電流と前記電流指令生成器が生成した電流指令値とに応じてトルク指令値を生成し、そのモータに出力するトルク指令生成器と、を備え、電流指令生成器は、積分器の積分値を蓄積する積分値蓄積部と、衝突検知部が衝突を検知した場合に、積分器の積分値を積分値蓄積部が蓄積した過去の積分値に置き換える置換部と、を備え、衝突検知部が衝突を検知してから、全軸停止状態検知部が全軸停止状態を検知するまでは、衝突時制御部によって電流指令生成器が用いる速度指令値が0にされてもよい。
このような構成により、衝突時の負荷が定常負荷として積分器の積分値に反映されることを回避でき、迅速な負荷軽減が可能となる。すなわち、衝突が検知された際には積分器の積分値が衝突時の負荷に応じて大きな値となっているため、上述した積分値の置き換えを行わなかった場合には、速度指令値を0としても迅速な負荷の軽減を行うことが難しくなる。一方、積分器の積分値を過去の値に置き換えることにより、積分値に対する衝突の影響を排除でき、それだけ迅速な負荷軽減を実現できる。
また、本発明による制御装置では、衝突検知部が衝突を検知した後に、いずれかのモータの位置があらかじめ決められた範囲以上変化した場合に、モータを停止させるモータ停止部をさらに備えてもよい。
このような構成により、例えば、衝突時の回避動作中にマニピュレータの一部が自重で落下した場合に、その落下を防止でき、マニピュレータの破損を回避できる。
また、本発明による制御装置では、安全基準位置算出部は、衝突位置から全軸停止位置までの距離と、全軸停止位置から安全基準位置までの距離とが等しくなるように安全基準位置を算出してもよい。
このような構成により、衝突の程度に応じた安全基準位置の算出を行うことができ、その結果、適切な安全位置の特定が行われることになる。
また、本発明による制御装置では、安全位置特定部は、蓄積部が蓄積した過去の位置のち、安全基準位置に距離が最も近い位置である安全位置を特定してもよい。
このような構成により、回避位置として適切であると考えられる位置に手先効果器を移動させることができる。
また、本発明による産業用ロボットは、マニピュレータと、そのマニピュレータを制御する上記制御装置と、を備えたものである。
本発明によるマニピュレータの制御装置等によれば、マニピュレータが衝突した場合に、減速機の負荷を低減させることができると共に、手先効果器を安全な位置に回避させることができる。
本発明の実施の形態1による産業用ロボットの構成を示すブロック図 同実施の形態によるサーボコントローラの構成を示すブロック図 同実施の形態による電流指令生成器の構成を示すブロック図 同実施の形態における手先効果器の回避動作について説明するための図 同実施の形態における各軸の回避状態について説明するための図 同実施の形態による制御装置の動作を示すフローチャート 同実施の形態による制御装置の動作を示すフローチャート 同実施の形態におけるコンピュータシステムの構成の一例を示す図
以下、本発明による制御装置について、実施の形態を用いて説明する。なお、以下の実施の形態において、同じ符号を付した構成要素及びステップは同一または相当するものであり、再度の説明を省略することがある。
(実施の形態1)
本発明の実施の形態1によるマニピュレータの制御装置について、図面を参照しながら説明する。本実施の形態によるマニピュレータの制御装置は、マニピュレータの衝突が検知された際に、減速機の負荷を低減させると共に、手先効果器を安全な位置に回避させるものである。
図1は、本実施の形態による産業用ロボット100の構成を示すブロック図である。本実施の形態による産業用ロボット100は、制御装置1と、マニピュレータ2とを備える。制御装置1は、複数のモータによってそれぞれ駆動される複数の関節によって連結された複数のアームにより手先効果器を移動させるマニピュレータ2を制御する。そのマニピュレータ2において、各モータとアームとは減速機を介して接続されている。また、直列に接続されたアームの先端に、手先効果器が設けられている。マニピュレータ2は、例えば、垂直多関節ロボットのマニピュレータであってもよく、水平多関節ロボットのマニピュレータであってもよい。また、そのマニピュレータ2の軸数は問わない。マニピュレータ2の軸数は、例えば、4軸であってもよく、5軸であってもよく、6軸であってもよく、または、その他の軸数であってもよい。本実施の形態では、マニピュレータ2が6軸である場合について主に説明する。また、制御装置1は、ティーチングプレイバック方式によりマニピュレータ2を制御する。その制御装置1は、サーボコントローラ11−1〜11−Nと、位置算出部12と、蓄積部13と、位置記憶部14と、衝突検知部15と、衝突位置特定部16と、全軸停止状態検知部17と、衝突時制御部18と、安全基準位置算出部19と、安全位置特定部20と、安全位置移動制御部21と、各軸位置指令算出部22と、モータ停止部23と、フラグ記憶部24とを備える。
サーボコントローラ11−1〜11−Nは、マニピュレータ2の各モータ(図示せず)をそれぞれ制御する。すなわち、サーボコントローラ11−1〜11−Nは、各軸の位置指令値に応じて、各モータがその位置となるように各モータを制御する。ここで、Nは、2以上の整数であり、通常、マニピュレータ2の軸数と同数である。サーボコントローラ11−1〜11−Nの詳細な構成については、図2のブロック図を用いて後述する。
位置算出部12は、マニピュレータ2の複数のモータの位置から手先効果器の位置を算出する。そのモータの位置(回転角度)は、例えば、エンコーダによって測定されてもよい。位置算出部12は、あらかじめ設定されているアームの長さや、モータの配置等を用いて、マニピュレータ2の複数のモータの位置から手先効果器の位置を算出することができる。その手先効果器の位置は、通常、3次元空間上における位置であるが、そうでなくてもよい。なお、モータの位置と減速機の位置とは対応しているため、モータの位置は減速機の位置であってもよい。以下の説明においても同様であるとする。
蓄積部13は、位置算出部12が算出した手先効果器の位置を位置記憶部14に蓄積する。位置記憶部14での記憶は、RAM等における一時的な記憶でもよく、あるいは、長期的な記憶でもよい。位置記憶部14は、所定の記録媒体(例えば、半導体メモリや磁気ディスクなど)によって実現されうる。なお、後述するように、蓄積部13が蓄積した手先効果器の過去の位置が安全位置の特定において用いられるため、位置記憶部14では、その安全位置を含みうる手先効果器の軌跡が記憶されていることが好適である。例えば、手先効果器のすべての過去の軌跡が位置記憶部14で記憶されていてもよく、または、最新の所定期間分の手先効果器の軌跡が位置記憶部14で記憶されていてもよい。
衝突検知部15は、手先効果器の衝突を検知する。衝突検知部15が手先効果器の衝突を検知する方法は問わない。例えば、推定トルクと実トルクとの差を用いて各軸について衝突を検知してもよく、衝突検出用の検出器を用いて衝突を検知してもよく、または、その他の方法によって手先効果器の衝突を検知してもよい。なお、推定トルクと実トルクとの差を用いて各軸の衝突を検知する場合には、衝突検知部15は、いずれかの軸において衝突を検知したときに、手先効果器の衝突を検知してもよい。また、衝突検出用の検出器を用いて衝突を検知する場合には、手先効果器の衝突を検出するように検出器を配設してもよい。
衝突位置特定部16は、衝突検知部15が衝突を検知した時の手先効果器の位置である衝突位置を特定する。衝突位置の特定は、例えば、衝突位置を図示しない記録媒体に蓄積することであってもよく、または、位置記憶部14で記憶されている、衝突位置に対応する手先効果器の位置に対して、フラグ等を設定することであってもよい。このことは、他の位置が特定される場合にも同様であるとする。
全軸停止状態検知部17は、衝突検知部15が衝突を検知した後に、マニピュレータ2の複数のモータのすべてが停止した状態である全軸停止状態になったことを検知する。なお、後述するように、衝突が検知されると、各サーボコントローラ11−1〜11−Nにおける速度指令値が0に制御される。その結果、衝突時の負荷がなくなるまで、各モータに逆転トルクが発生し、最終的にモータが停止する。そのようにしてすべてのモータが停止した状態が、全軸停止状態である。
衝突時制御部18は、衝突検知部15が衝突を検知してから、全軸停止状態検知部17が全軸停止状態を検知するまで、速度指令値が0になるように複数のサーボコントローラ11−1〜11−Nを制御する。衝突が検知された場合に、例えば、衝突時制御部18は、後述する切替器32を制御することによって、速度指令値を0にしてもよい。また、全軸停止状態が検知された場合に、例えば、衝突時制御部18は、後述する切替器32を制御することによって、速度指令値を0にする制御を解除してもよい。
安全基準位置算出部19は、全軸停止状態検知部17が全軸停止状態を検知した時の手先効果器の位置である全軸停止位置を特定する。また、安全基準位置算出部19は、その全軸停止位置から、衝突位置と逆方向に移動した位置である安全基準位置を算出する。すなわち、安全基準位置算出部19は、衝突位置から全軸停止位置に向かう方向に沿って、その全軸停止位置から移動した位置である安全基準位置を算出する。全軸停止位置から安全基準位置までの距離は、あらかじめ決められた固定値であってもよく、または、衝突位置から全軸停止位置までの距離に応じた可変値であってもよい。後者の場合には、安全基準位置算出部19は、全軸停止位置から安全基準位置までの距離と、衝突位置から全軸停止位置までの距離にあらかじめ決められた係数を掛けた距離とが等しくなるように安全基準位置を算出してもよい。その係数は、0より大きい実数であり、あまり大きくない値であることが好適である。その係数は、例えば、3以下であってもよく、2以下であってもよく、1.5以下であってもよく、または、1であってもよい。本実施の形態では、その係数が1の場合、すなわち、安全基準位置算出部19が、衝突位置から全軸停止位置までの距離と、全軸停止位置から安全基準位置までの距離とが等しくなるように安全基準位置を算出する場合について主に説明する。
安全位置特定部20は、蓄積部13が位置記憶部14に蓄積した手先効果器の過去の位置のうち、安全基準位置にあらかじめ決められた条件をみたすほど近い位置である安全位置を特定する。安全基準位置にあらかじめ決められた条件をみたすほど近い位置とは、安全基準位置に距離が最も近い位置であってもよく、安全基準位置からの移動時間が最も短い位置であってもよく、または、それらの位置の近傍の位置、例えば、それらの位置からMサンプル以内の位置であってもよい。そのMは、小さい整数値、例えば、1〜5程度であることが好適である。また、そのMサンプルは、それらの位置から過去の方向のサンプルであることが好適である。
安全位置移動制御部21は、全軸停止状態が検知された後に、手先効果器の位置が安全位置となるように制御する。なお、結果として手先効果器の位置が安全位置となるように制御されるのであれば、その制御の内容は問わない。本実施の形態では、安全位置移動制御部21が、割り込み処理で安全位置を各軸位置指令算出部22に入力することにより、手先効果器の位置を安全位置に制御する場合について説明する。
各軸位置指令算出部22は、手先効果器の位置に応じて、各軸の位置指令値を算出する。正常動作時には、各軸位置指令算出部22は、ティーチングされた手先効果器の位置から、各軸の位置指令値を算出し、その位置指令値を各サーボコントローラ11−1〜11−Nに出力する。それにより、ティーチングに応じたマニピュレータ2の制御が行われる。一方、安全位置への回避時には、各軸位置指令算出部22は、前述のように、割り込み処理により、安全位置移動制御部21から安全位置の入力を受け付け、その安全位置に手先効果器が移動するように、各軸の位置指令値を算出して各サーボコントローラ11−1〜11−Nに出力する。それにより、手先効果器が安全位置に回避されることになる。
モータ停止部23は、衝突検知部15が衝突を検知した後に、いずれかのモータの位置があらかじめ決められた範囲以上変化した場合に、そのモータを停止させる。モータの位置があらかじめ決められた範囲以上変化した場合には、マニピュレータ2の一部が自重で落下していると考えられる。したがって、その落下を止め、マニピュレータ2の破損を回避するために、モータ停止部23は、そのモータをロックさせる。具体的には、モータ停止部23は、その停止対象のモータを制御しているサーボコントローラをオフにすることにより、そのモータをロックさせてもよい。なお、例えば、衝突時のモータの現在位置が蓄積され、その衝突時のモータの現在位置と、衝突後のモータの現在位置とがあらかじめ決められたしきい値以上になった場合に、モータの位置があらかじめ決められた範囲以上に変化したと判断されてもよい。また、例えば、衝突後にモータの現在速度があらかじめ決められたしきい値以上になった場合に、モータの位置があらかじめ決められた範囲以上に変化したと判断されてもよい。そのしきい値を適切に設定することにより、モータの現在速度がしきい値以上である場合には、モータの位置がしきい値以上変化していると考えることができるからである。なお、モータ停止部23がそれらの判断を行ってもよく、または、他の構成要素がそれらの判断を行い、モータ停止部23がその判断結果を用いてモータの停止制御を行ってもよい。本実施の形態では、衝突時のモータの現在位置と、衝突後のモータの現在位置とがあらかじめ決められたしきい値以上になったときに、モータの位置があらかじめ決められた範囲以上に変化したと判断される場合について主に説明する。また、モータ停止部23は、その位置があらかじめ決められた範囲以上変化したと判断したモータのみを停止させてもよく、または、その他のモータも停止させてもよい。後者の場合には、モータ停止部23は、すべてのモータを停止させてもよい。本実施の形態では、モータ停止部23がすべてのモータを停止させる場合について主に説明する。
フラグ記憶部24では、衝突に関するフラグや状態が記憶される。本実施の形態では、フラグ記憶部24で、衝突フラグと、全軸停止フラグと、各軸の回避状態とが記憶される場合について説明する。それらのフラグや回避状態については後述する。フラグ記憶部24での記憶は、RAM等における一時的な記憶でもよく、あるいは、長期的な記憶でもよい。フラグ記憶部24は、所定の記録媒体(例えば、半導体メモリや磁気ディスクなど)によって実現されうる。
なお、位置記憶部14と、フラグ記憶部24とは、同一の記録媒体によって実現されてもよく、あるいは、別々の記録媒体によって実現されてもよい。前者の場合には、手先効果器の位置を記憶している領域が位置記憶部14となり、衝突フラグ等を記憶している領域がフラグ記憶部24となる。
図2は、本実施の形態によるサーボコントローラ11−1の構成を示すブロック図である。本実施の形態によるサーボコントローラ11−1は、速度指令生成器31と、切替器32と、電流指令生成器33と、トルク指令生成器34と、現在位置蓄積部35と、判断部36とを備える。
速度指令生成器31は、モータの現在位置と位置指令値とに応じて速度指令値を生成する。速度指令生成器31は、モータの現在位置が位置指令値に近づくようにフィードバック制御をするための速度指令値を生成する。
切替器32は、後段の電流指令生成器33に渡す速度指令値を、速度指令生成器31の生成した速度指令値と、0とのいずれかに切り替える。その切り替えは、前述のように、例えば、衝突時制御部18の制御によって行われてもよい。すなわち、衝突検知部15が衝突を検知してから、全軸停止状態検知部17が全軸停止状態を検知するまでは、衝突時制御部18によって、電流指令生成器33が用いる速度指令値が0になるように切替器32が設定されてもよい。また、全軸停止状態検知部17が全軸停止状態を検知してからは、衝突時制御部18によって、電流指令生成器33が用いる速度指令値が速度指令生成器31によって生成された速度指令値になるように切替器32が設定されてもよい。
電流指令生成器33は、モータの現在速度と速度指令値とに応じて電流指令値を生成する。その速度指令値は、切替器32から受け取るものであり、衝突が検知されるまで、及び、全軸停止状態が検知されてからは速度指令生成器31が生成した速度指令値であり、衝突が検知されてから全軸停止状態が検知されるまでは0である。なお、電流指令生成器33の詳細な構成については、図3のブロック図を用いて後述する。
トルク指令生成器34は、モータの現在電流と電流指令生成器33が生成した電流指令値とに応じてトルク指令値を生成し、モータに出力する。トルク指令生成器34は、モータの現在電流が電流指令値に近づくようにフィードバック制御をするためのトルク指令値を生成する。モータは、このトルク指令値によって動作する。
現在位置蓄積部35は、衝突検知部15が衝突を検知した時の現在位置を、所定の記録媒体に蓄積する。この記録媒体は、例えば、半導体メモリや、磁気ディスク等であり、現在位置蓄積部35が有していてもよく、あるいは現在位置蓄積部35の外部に存在してもよい。また、この記録媒体は、現在位置を一時的に記憶するものであってもよく、そうでなくてもよい。
判断部36は、衝突が検知された後に、モータの現在速度が0になったかどうか判断する。モータの現在速度が0になると、そのモータに対応する軸は停止したことになる。
また、判断部36は、衝突が検知された後に、モータの現在位置と、現在位置蓄積部35が蓄積した現在位置との差が、あらかじめ決められたしきい値以上であるかどうか判断してもよい。その差がしきい値を超えている場合には、モータの位置があらかじめ決められた範囲以上変化したことになる。
なお、モータの現在電流は、トルク指令生成器34に入力される。また、モータの現在速度は、電流指令生成器33と、判断部36とに入力される。また、モータの現在位置は、速度指令生成器31と、現在位置蓄積部35と、判断部36とに入力される。なお、モータの現在速度は、モータの現在位置が時間微分されたものであってもよい。
また、サーボコントローラ11−2〜11−Nは、制御対象のモータが異なる以外は、サーボコントローラ11−1と同様のものであり、その説明を省略する。
図3は、本実施の形態による電流指令生成器33の構成を示すブロック図である。本実施の形態による電流指令生成器33は、減算器41と、積算器42と、積分器43と、加算器44と、積分値蓄積部45と、置換部46とを備え、速度PI制御を行う。
減算器41は、モータの現在速度と、切替器32からの速度指令値との差分を算出する。具体的には、減算器41は、切替器32からの速度指令値から、モータの現在速度を減算してもよい。その減算器41の出力は、積算器42と積分器43とに入力される。
積算器42は、入力値である差分に比例ゲインを積算する。その積算値は、加算器44に入力される。その比例ゲインは、比例制御で用いられる定数であり、設計者や使用者により設定される値である。減算器41と、積算器42とにより、PI制御における比例動作が行われることになる。
積分器43は、モータの現在速度と速度指令値と差分である入力値を積分する。その積分した値である積分値は、加算器44と積分値蓄積部45とに入力される。この積分器43による積分の期間は、設計者や使用者により設定される。減算器41と積分器43とにより、PI制御における積分動作が行われることになる。
加算器44は、積分器43からの積分値と、積算器42からの積算値とを加算する。この加算器44によって加算された加算値が、電流指令値である。
積分値蓄積部45は、積分器43が積分した積分値を所定の記録媒体に蓄積する。この記録媒体は、例えば、半導体メモリや、磁気ディスク等であり、積分値蓄積部45が有していてもよく、あるいは積分値蓄積部45の外部に存在してもよい。また、この記録媒体は、積分値を一時的に記憶するものであってもよく、そうでなくてもよい。なお、その記録媒体では、最新の所定期間分の積分値が記憶されていることが好適である。その所定期間は、実際に衝突が起こってからその衝突が検知されるまでの期間より長いことが好適である。衝突検知時に、衝突前の積分値が蓄積されているようにするためである。
置換部46は、衝突検知部15が衝突を検知した場合に、積分器43の積分値を積分値蓄積部45が蓄積した過去の積分値に置き換える。その過去の積分値は、衝突前の積分値である。例えば、置換部46は、衝突が検知された場合に、積分値蓄積部45が蓄積した積分値から、あらかじめ決められた期間だけ過去の積分値を読み出し、積分器43の積分値を、その読み出した積分値に置き換えてもよい。そのあらかじめ決められた期間は、設計者や使用者によって設定される期間であり、マニピュレータ2が障害物に衝突してから、衝突検知部15が衝突を検知するまでの期間と同程度か、それよりも少し長く設定されることが好適である。衝突が起こった場合には、減算器41の出力が大きくなり、積分値も大きくなる。そのため、前述のように、衝突が検知された後に、速度指令値が0にされても、その大きな積分値が原因となって、モータに逆転トルクが働くのに時間がかかり、迅速な負荷低減を実現することが難しくなる。一方、衝突が検知された場合に、置換部46によって積分器43の積分値を衝突前の積分値に置換することによって、迅速な負荷低減を実現でき、減速機を傷めないようにすることができる。
ここで、衝突後に手先効果器が安全位置に回避される動作について、図4を用いて説明する。なお、図4では、説明の便宜上、手先効果器がxy平面上を移動する場合について示しているが、実際には、手先効果器は3次元空間上を移動する。
手先効果器が障害物に衝突したことが検知されると、その時点の衝突位置が特定される。そして、衝突時制御部18の制御によって、各モータに対する速度指令値が0に制御され、衝突による負荷がなくなるまで各モータに逆転トルクが発生する。その後、各モータの速度が0になると、その時点の全軸停止位置が特定され、衝突位置から全軸停止位置までの差分を全軸停止位置に加算した安全基準位置が算出される。また、手先効果器の軌跡から安全基準位置に近い位置である安全位置が特定され、手先効果器が、全軸停止位置から安全位置に移動される。それによって、衝突後の回避動作が終了する。なお、本実施の形態では、全軸停止位置から安全位置に移動する動作を終了動作と呼ぶこともある。全軸停止位置までの移動であれば、手先効果器が障害物にあたったままになっており、減速機を傷める可能性もあるが、終了動作によって、障害物から完全に離れることができ、減速機の負荷を適切に低減することができる。また、衝突位置から全軸停止位置までの距離と、全軸停止位置から安全基準位置までの距離とが等しくなるように安全基準位置が算出される場合には、衝突の程度に応じた安全基準位置の算出を行うことができる。例えば、衝突の検知が遅かった場合には、衝突位置から全軸停止位置までの距離が長くなることによって、全軸停止位置から安全基準位置までの距離も長くなり、より確実な回避が実現される。一方、衝突の検知が早かった場合には、衝突位置から全軸停止位置までの距離が短くなることによって、全軸停止位置から安全基準位置までの距離も短くなるが、早期に衝突を検知できているため減速機への負荷も小さく、それで問題ないと考えられる。また、安全位置は手先効果器の軌跡上の一点であるため、手先効果器が他の障害物に衝突する二次災害を防止することができる。なお、衝突位置や全軸停止位置は、各モータのエンコーダによって測定された位置である。
次に、フラグ記憶部24で記憶されている衝突フラグ、全停止フラグ、各軸の回避状態について説明する。
衝突フラグは、衝突が発生したかどうかを示すフラグであり、衝突が検知されるとTRUEに設定され、衝突が検知されるまではFALSEに設定される。本実施の形態では、手先効果器の衝突を検知した場合に、衝突検知部15が、フラグ記憶部24で記憶されている衝突フラグをTRUEにするものとする。なお、フラグ記憶部24で記憶されている衝突フラグの初期値はFALSEである。
全軸停止フラグは、全軸停止状態となっているかどうかを示すフラグであり、全軸停止状態が検知されるとTRUEに設定され、全軸停止状態が検知されるまではFALSEに設定される。本実施の形態では、全軸停止状態を検知した場合に、全軸停止状態検知部17が、フラグ記憶部24で記憶されている全軸停止フラグをTRUEにするものとする。なお、フラグ記憶部24で記憶されている全軸停止フラグの初期値はFALSEである。
回避状態は、軸ごとに回避がどの状態であるのかを示す情報であり、「未回避」、「回避中」、「終了待ち」、「終了動作」の4種類がある。その状態の変化について、図5を用いて説明する。回避前、すなわち、衝突の検知前には、回避状態は未回避に設定される。また、衝突が検知されると、回避状態は回避中に設定される。また、衝突が検知された後に、現在速度が0になると、回避状態は終了待ちに設定される。また、回避状態が回避中である場合にモータの位置があらかじめ決められた範囲以上変化したとき、または、回避状態が終了待ちである場合に全軸停止状態が検知されたときには、回避状態は終了動作に設定される。なお、フラグ記憶部24で記憶されている各軸に対応する回避状態の初期値は未回避である。
次に、制御装置1の動作について図6のフローチャートを用いて説明する。
(ステップS101)各軸位置指令算出部22は、手先効果器の位置を受け付けたかどうか判断する。そして、位置を受け付けた場合には、ステップS102に進み、そうでない場合には、ステップS104に進む。
(ステップS102)各軸位置指令算出部22は、受け付けた手先効果器の位置から、各軸の位置指令値を算出し、それぞれ対応するサーボコントローラ11−1〜11−Nに出力する。
(ステップS103)サーボコントローラ11−1〜11−Nは、受け取った位置指令値に応じて、マニピュレータ2の各モータを制御するサーボコントロールを行う。そして、ステップS101に戻る。なお、このサーボコントロールの詳細な処理については、図7のフローチャートを用いて後述する。
(ステップS104)位置算出部12は、手先効果器の位置を算出するかどうか判断する。そして、手先効果器の位置を算出する場合には、ステップS105に進み、そうでない場合には、ステップS107に進む。なお、位置算出部12は、例えば、定期的に手先効果器の位置を算出すると判断してもよく、または、所定のイベントの発生に応じて手先効果器の位置を算出すると判断してもよい。その所定のイベントは、例えば、各モータのエンコーダの値を受け取ったことであってもよい。
(ステップS105)位置算出部12は、各モータの位置を用いて、手先効果器の位置を算出する。そのモータの位置は、例えば、エンコーダの値であってもよい。
(ステップS106)蓄積部13は、位置算出部12が算出した手先効果器の位置を位置記憶部14に蓄積する。そして、ステップS101に戻る。
(ステップS107)衝突検知部15は、手先効果器の衝突を検知したかどうか判断する。そして、衝突を検知した場合には、ステップS108に進み、そうでない場合には、ステップS112に進む。なお、衝突検知部15は、衝突を検知すると、フラグ記憶部24で記憶されている衝突フラグをTRUEに変更し、各回避状態を回避中に変更する。
(ステップS108)衝突時制御部18は、各サーボコントローラ11−1〜11−Nにおいて速度指令値が0になるように制御する。すなわち、衝突時制御部18は、各サーボコントローラ11−1〜11−Nの切替器32が、速度指令値「0」を出力するように切り替える。
(ステップS109)衝突位置特定部16は、その時点の手先効果器の位置である衝突位置を特定する。
(ステップS110)各サーボコントローラ11−1〜11−Nの現在位置蓄積部35は、その時点の現在位置を記録媒体に蓄積する。
(ステップS111)各サーボコントローラ11−1〜11−Nの電流指令生成器33が有する置換部46は、積分器43の積分値を積分値蓄積部45が蓄積した過去の積分値に置換する。そして、ステップS101に戻る。
(ステップS112)全軸停止状態検知部17は、全軸停止状態を検知したかどうか判断する。そして、全軸停止状態を検知した場合には、ステップS113に進み、そうでない場合には、ステップS117に進む。なお、全軸停止状態検知部17は、フラグ記憶部24で記憶されている衝突フラグがTRUEであり、かつ、すべての回避状態が終了待ちである場合に、全軸停止状態を検知してもよい。また、全軸停止状態検知部17は、全軸停止状態を検知した場合に、フラグ記憶部24で記憶されている全軸停止フラグをTRUEに変更し、各回避状態を終了動作に変更する。
(ステップS113)衝突時制御部18は、各サーボコントローラ11−1〜11−Nにおける速度指令値を0にする制御を解除する。すなわち、衝突時制御部18は、各サーボコントローラ11−1〜11−Nの切替器32が、速度指令生成器31の生成した速度指令値を出力するように切り替える。
(ステップS114)安全基準位置算出部19は、その時点の手先効果器の位置である全軸停止位置と衝突位置とを用いて、安全基準位置を算出する。
(ステップS115)安全位置特定部20は、位置記憶部14で記憶されている手先効果器の過去の位置のうち、安全基準位置算出部19が算出した安全基準位置に近い位置である安全位置を特定する。
(ステップS116)安全位置移動制御部21は、手先効果器が安全位置に移動するように制御する。すなわち、安全位置移動制御部21は、手先効果器が安全位置に移動するように、割り込み処理により、その安全位置を各軸位置指令算出部22に渡す。その結果、各軸位置指令算出部22は、手先効果器が安全位置に移動するように各軸の位置指令値を算出し、各サーボコントローラ11−1〜11−Nは、その各軸の位置指令値に応じて、マニピュレータ2の各モータを制御する。この各軸位置指令算出部22と、各サーボコントローラ11−1〜11−Nとによる動作は、ステップS102、S103と同様の処理によって実現される。そして、手先効果器の位置が安全位置となると、一連の回避処理は終了となる。
(ステップS117)モータ停止部23は、いずれかのモータの位置があらかじめ決められた範囲以上変化したかどうか判断する。そして、いずれかのモータの位置があらかじめ決められた範囲以上変化した場合には、ステップS118に進み、そうでない場合には、ステップS101に戻る。なお、モータ停止部23は、フラグ記憶部24で記憶されている全軸停止フラグがFALSEであり、いずれかの回避状態が終了動作である場合に、いずれかのモータの位置があらかじめ決められた範囲以上変化したと判断してもよい。
(ステップS118)モータ停止部23は、すべてのモータを停止させる。そして、一連の回避処理は終了となる。
図7は、図6のフローチャートにおけるサーボコントロールの処理(ステップS103)の詳細を示すフローチャートである。図7のフローチャートでは、サーボコントローラ11−1によるサーボコントロールの処理について説明するが、他のサーボコントローラ11−2〜11−Nによるサーボコントロールの処理も同様である。
(ステップS201)速度指令生成器31は、位置指令値とモータの現在位置とを用いて速度指令値を生成する。
(ステップS202)電流指令生成器33は、速度指令値とモータの現在速度とを用いて電流指令値を生成する。なお、その電流指令値の生成で用いる速度指令値は、衝突が検知され、全軸停止状態が検知されるまでの期間は0であり、それ以外の期間は速度指令生成器31の生成した速度指令値である。その切り替えは、切替器32によって行われる。また、積分値蓄積部45は、積分器43が積分した積分値を蓄積する。
(ステップS203)トルク指令生成器34は、電流指令生成器33が生成した電流指令値とモータの現在電流とを用いて、トルク指令値を生成し、モータに出力する。その結果、サーボコントローラ11−1の制御対象であるモータが、そのトルク指令値に応じて動作する。
(ステップS204)判断部36は、サーボコントローラ11−1が制御するモータに対応する回避状態が「回避中」であるかどうか判断する。そして、回避中である場合には、ステップS205に進み、そうでない場合には、図6のフローチャートに戻る。
(ステップS205)判断部36は、その時点の現在位置が、現在位置蓄積部35が蓄積した現在位置としきい値以上変化しているかどうか判断する。そして、しきい値以上変化している場合、すなわち、制御対象のモータの位置があらかじめ決められた範囲以上変化している場合には、ステップS206に進み、そうでない場合には、ステップS207に進む。
(ステップS206)判断部36は、サーボコントローラ11−1が制御するモータに対応する回避状態を「終了動作」に更新する。そして、図6のフローチャートに戻る。
(ステップS207)判断部36は、その時点の現在速度が0であるかどうか判断する。そして、0である場合には、ステップS208に進み、そうでない場合には、図6のフローチャートに戻る。
(ステップS208)判断部36は、サーボコントローラ11−1が制御するモータに対応する回避状態を「終了待ち」に更新する。そして、図6のフローチャートに戻る。
なお、図7のフローチャートでは、サーボコントローラ11−1のみのサーボコントロールの処理について説明したが、他のサーボコントローラ11−2〜11−Nのサーボコントロールの処理も並列して行われるものとする。また、図7のフローチャートで示されるサーボコントロールの処理(図6のフローチャートにおけるステップS103)と、図6のフローチャートにおけるステップS103以外の処理とは、並列して実行されてもよい。その場合には、ステップS103は、各サーボコントローラ11−1〜11−Nに位置指令値を指示する処理であると考えてもよい。また、図7のフローチャートで示されるサーボコントロールの処理と、図6のフローチャートにおけるステップS103以外の処理とが並列して実行される場合には、そのサーボコントロール中の手先効果器の衝突も検知されることになる。
次に、本実施の形態による制御装置1の動作について、具体例を用いて説明する。この具体例では、マニピュレータ2が第1軸から第6軸までの6軸を有しており、サーボコントローラ11−1〜11−6によって、それぞれの軸を制御する場合について説明する。したがって、フラグ記憶部24で記憶されているフラグ等の初期値は次の通りである。
衝突フラグ:FALSE
全軸停止フラグ:FALSE
第1軸の回避状態:未回避
第2軸の回避状態:未回避
第3軸の回避状態:未回避
第4軸の回避状態:未回避
第5軸の回避状態:未回避
第6軸の回避状態:未回避
まず、正常動作時の動作について簡単に説明する。正常動作時には、各軸位置指令算出部22は、ティーチングされた手先効果器の位置を受け付け(ステップS101)、その手先効果器の位置から各軸の位置指令値を算出し、それぞれ対応するサーボコントローラ11−1〜11−6に出力する(ステップS102)。そして、各サーボコントローラ11−1〜11−6では、各軸位置指令算出部22から受け取った位置指令値を用いて、制御対象のモータに対するサーボコントロールが行われる(ステップS103)。具体的には、速度指令値の生成、電流指令値の生成、及びトルク指令値の生成が行われ、そのトルク指令値に応じたモータの動作が行われる(ステップS201〜203)。なお、各電流指令生成器33では、新たな積分値が算出されるごとに、積分値蓄積部45がその積分値を蓄積する。また、この時点ではまだ衝突が検知されていないため、各サーボコントローラ11−1〜11−6において、切替器32は、速度指令生成器31の生成した速度指令値を電流指令生成器33に入力することになる。
また、位置算出部12は、定期的に位置を算出すると判断し(ステップS104)、各軸のエンコーダ値から手先効果器の位置を算出する(ステップS105)。その手先効果器の位置は、蓄積部13によって位置記憶部14に蓄積される(ステップS106)。この位置の算出と蓄積とは定期的に繰り返して行われる。
次に、衝突が検知されたときの動作について説明する。衝突検知部15が手先効果器の衝突を検知すると、フラグ記憶部24で記憶されているフラグ等を次のように更新し、衝突位置特定部16、衝突時制御部18、各現在位置蓄積部35、及び各置換部46に衝突の発生を伝える(ステップS107)。
衝突フラグ:TRUE
全軸停止フラグ:FALSE
第1軸の回避状態:回避中
第2軸の回避状態:回避中
第3軸の回避状態:回避中
第4軸の回避状態:回避中
第5軸の回避状態:回避中
第6軸の回避状態:回避中
衝突の発生が伝えられると、衝突時制御部18は、各サーボコントローラ11−1〜11−6において、電流指令生成器33への入力が0になるように切替器32を切り替える(ステップS108)。また、衝突位置特定部16は、その時点に位置記憶部14で記憶されている最新の位置である衝突位置を読み出して、衝突位置特定部16が有する図示しない記録媒体に蓄積する(ステップS109)。また、各サーボコントローラ11−1〜11−6の現在位置蓄積部35は、その時点のモータの現在位置を図示しない記録媒体に蓄積する(ステップS110)。また、各サーボコントローラ11−1〜11−6の電流指令生成器33が有する置換部46は、積分器43の積分値を、積分値蓄積部45が蓄積した、あらかじめ決められている期間だけ以前の積分値に置換する(ステップS111)。この後、各軸の速度が0になるようにサーボコントロールが行われ(ステップS101〜S103)、その動作に応じた位置が順次、蓄積される(ステップS104〜S106)。なお、衝突が検知されてから全軸停止状態が検知されるまでは、各軸の位置指令値に応じて生成された速度指令値は用いられないため、各軸位置指令算出部22による各軸の位置指令値の算出処理(ステップS102)や、速度指令値の生成処理(ステップS201)を行わなくてもよい。
各軸の回避状態が回避中である際に、第5軸のサーボコントローラ11−5の判断部36が、第5軸のモータの現在速度が0になったと判断したとする(ステップS207)。すると、その判断部36は、フラグ記憶部24で記憶されている第5軸の回避状態を次のように更新する(ステップS208)。
衝突フラグ:TRUE
全軸停止フラグ:FALSE
第1軸の回避状態:回避中
第2軸の回避状態:回避中
第3軸の回避状態:回避中
第4軸の回避状態:回避中
第5軸の回避状態:終了待ち
第6軸の回避状態:回避中
その後、他の軸においても現在速度が0になったと判断され、全軸の回避状態が「終了待ち」になると、全軸停止状態検知部17は、全軸停止状態を検知し、フラグ記憶部24で記憶されているフラグ等を次のように更新し、衝突時制御部18、及び安全基準位置算出部19に全軸の停止を伝える(ステップS112)。
衝突フラグ:TRUE
全軸停止フラグ:TRUE
第1軸の回避状態:終了動作
第2軸の回避状態:終了動作
第3軸の回避状態:終了動作
第4軸の回避状態:終了動作
第5軸の回避状態:終了動作
第6軸の回避状態:終了動作
全軸の停止が伝えられると、衝突時制御部18は、各サーボコントローラ11−1〜11−6において、電流指令生成器33への入力が速度指令生成器31の生成した速度指令値になるように切替器32を切り替える(ステップS113)。また、安全基準位置算出部19は、その時点に位置記憶部14で記憶されている最新の位置である全軸停止位置を読み出すと共に、衝突位置特定部16から衝突位置を受け取る。そして、安全基準位置算出部19は、衝突位置を始点として、全軸停止位置を終点とするベクトルを算出し、全軸停止位置から、そのベクトルだけ離れた点である安全基準位置を算出して、安全位置特定部20に渡す(ステップS114)。安全位置特定部20は、安全基準位置を受け取ると、位置記憶部14で記憶されている手先効果器の軌跡のうち、その安全基準位置に最も近い位置である安全位置を特定し、その安全位置を安全位置移動制御部21に渡す(ステップS115)。その安全位置を受け取ると、安全位置移動制御部21は、手先効果器が安全位置に移動するように制御する(ステップS116)。具体的には、安全位置移動制御部21は、割り込み処理により、安全位置を各軸位置指令算出部22に入力する。そして、手先効果器が安全位置に移動するための各軸の位置指令値を各軸位置指令算出部22が生成して各サーボコントローラ11−1〜11−6に出力することによって、手先効果器が安全位置に移動するようにサーボコントロールが行われ、手先効果器が安全位置に移動する。このようにして、手先効果器の安全位置への回避動作が終了する。
次に、モータが停止される場合の動作について説明する。第3軸の回避状態が「回避中」である場合に、サーボコントローラ11−3の判断部36によって、現在位置蓄積部35が蓄積した現在位置と、その時点の現在位置との差があらかじめ決められたしきい値以上になったと判断されたとする(ステップS204,S205)。すると、その判断部36は、フラグ記憶部24で記憶されているフラグ等を次のように更新する(ステップS206)。
衝突フラグ:TRUE
全軸停止フラグ:FALSE
第1軸の回避状態:回避中
第2軸の回避状態:回避中
第3軸の回避状態:終了動作
第4軸の回避状態:回避中
第5軸の回避状態:回避中
第6軸の回避状態:回避中
すると、全軸停止フラグが「FALSE」であり、回避状態が「終了動作」である軸が存在するため、モータ停止部23は、モータの位置が所定の範囲以上変化したしたと判断し(ステップS117)、回避状態が「終了動作」である第3軸のモータと、その他のモータとを停止させる(ステップS118)。したがって、マニピュレータ2がその時点で停止させられ、回避動作が終了する。このようにして、回避動作中に一部のアームが自重で落下した場合に、それによってマニピュレータ2が破損することを回避できる。
以上のように、本実施の形態による制御装置1によれば、手先効果器が衝突した場合に、手先効果器の位置を全軸停止位置よりもさらに障害物から離れる方向に移動させることができ、減速機に負荷がかかったままになることを回避することができる。また、その手先効果器の移動先を安全位置とすることによって、過去の軌跡上に手先効果器を回避することができる。その結果、回避動作によって手先効果器が他の障害物に衝突する二次災害を防止することができる。また、衝突が検知された場合に、各サーボコントローラ11−1〜11−Nの電流指令生成器33が有する積分器43の積分値を衝突による負荷の影響を受ける前の値に置換することにより、迅速な回避が可能となり、衝突による負荷によって減速機を傷める可能性をより低減することができる。また、衝突が検知された後に、モータの位置があらかじめ決められた範囲以上変化した場合に、そのモータを停止させることによって、アームが回避中に自重などの影響で落下することを防止できる。
なお、本実施の形態では、衝突が検知された後に、モータの位置があらかじめ決められた範囲以上変化すると、すべてのモータを停止させ、回避動作を終了する場合について説明したが、そうでなくてもよい。例えば、モータの位置があらかじめ決められた範囲以上変化した場合に、そのモータを停止させるが、それ以外のモータについて、全軸停止状態となるまで移動させ、その後、停止されたモータ以外を用いて、手先効果器が安全位置に移動するように制御してもよい。すなわち、あるモータが停止された場合には、停止されていないモータについて、全軸停止状態を検知し、その検知に応じて安全基準位置の算出や、安全位置の特定を行い、手先効果器をその安全位置に移動させるようにしてもよい。
また、本実施の形態では、フラグ記憶部24において、衝突フラグ、全軸停止フラグ、各軸の回避状態がそれぞれ記憶されている場合について説明したが、そうでなくてもよい。例えば、衝突フラグ、及び全軸停止フラグは、フラグ記憶部24で記憶されており、各軸の回避状態は、各軸に対応するサーボコントローラ11−1〜11−Nにおいてそれぞれ記憶されていてもよい。また、その場合に、各軸の回避状態は、各サーボコントローラ11−1〜11−Nが有する、その回避状態を更新する図示しない回避状態更新部によって更新されてもよい。
また、本実施の形態では、フラグや回避状態を用いて処理を行う場合について主に説明したが、そうでなくてもよい。フラグ等を用いないで処理を行ってもよい。その場合には、例えば、制御装置1は、フラグ記憶部24を備えていなくてもよい。
また、本実施の形態では、衝突が検知された後に、モータの位置があらかじめ決められた範囲以上変化すると、モータを停止させる場合について説明したが、そうでなくてもよい。回避中にモータの位置があらかじめ決められた範囲以上変化したとしても、モータを停止させなくてもよい。その場合には、制御装置1は、モータ停止部23を備えていなくてもよい。また、そのモータの停止を行わない場合には、各サーボコントローラ11−1〜11−Nは、現在位置蓄積部35を備えていなくてもよく、また各判断部36は、モータの現在位置に関する判断を行わなくてもよい。
また、本実施の形態では、衝突が検知されると、各サーボコントローラ11−1〜11−Nの電流指令生成器33が有する積分器43の積分値を衝突による負荷の影響を受ける前の値に置換する場合について説明したが、そうでなくてもよい。衝突が検知されたとしても、そのような置換を行わなくてもよい。その場合には、各サーボコントローラ11−1〜11−Nの電流指令生成器33は、積分値蓄積部45や置換部46を備えていなくてもよい。
また、本実施の形態において、位置算出部12は、複数のモータの位置から手先効果器の位置を算出すると共に、その算出した位置の間の位置を補間によって算出することによって、さらに細かい手先効果器の位置をも算出してもよい。その場合には、補間された手先効果器の位置も、蓄積部13によって位置記憶部14に蓄積されてもよい。
また、本実施の形態では、電流指令生成器33がPI制御を行う場合について説明したが、それに限定されないことは言うまでもない。電流指令生成器33は、例えば、PID制御を行ってもよく、または、その他のフィードバック制御を行ってもよい。
また、本実施の形態において、制御装置1が、複数のサーボコントローラ11−1〜11−N以外の構成要素の部分を、モーションコントローラと呼んでもよい。すなわち、本実施の形態による制御装置1は、複数のサーボコントローラ11−1〜11−Nと、その複数のサーボコントローラ11−1〜11−Nを制御するモーションコントローラとを備えていると考えてもよい。そのモーションコントローラは、前述のように、例えば、手先効果器の軌道や目標位置から各軸の位置指令値を計算し、各サーボコントローラ11−1〜11−Nを制御する。
また、上記実施の形態において、各処理または各機能は、単一の装置または単一のシステムによって集中処理されることによって実現されてもよく、あるいは、複数の装置または複数のシステムによって分散処理されることによって実現されてもよい。
また、上記実施の形態において、各構成要素間で行われる情報の受け渡しは、例えば、その情報の受け渡しを行う2個の構成要素が物理的に異なるものである場合には、一方の構成要素による情報の出力と、他方の構成要素による情報の受け付けとによって行われてもよく、あるいは、その情報の受け渡しを行う2個の構成要素が物理的に同じものである場合には、一方の構成要素に対応する処理のフェーズから、他方の構成要素に対応する処理のフェーズに移ることによって行われてもよい。
また、上記実施の形態において、各構成要素が実行する処理に関係する情報、例えば、各構成要素が受け付けたり、取得したり、選択したり、生成したり、送信したり、受信したりした情報や、各構成要素が処理で用いるしきい値や数式、アドレス、設置値等の情報等は、上記説明で明記していない場合であっても、図示しない記録媒体において、一時的に、あるいは長期にわたって保持されていてもよい。また、その図示しない記録媒体への情報の蓄積を、各構成要素、あるいは、図示しない蓄積部が行ってもよい。また、その図示しない記録媒体からの情報の読み出しを、各構成要素、あるいは、図示しない読み出し部が行ってもよい。
また、上記実施の形態において、各構成要素等で用いられる情報、例えば、各構成要素が処理で用いるしきい値やアドレス、各種の設定値等の情報がユーザによって変更されてもよい場合には、上記説明で明記していない場合であっても、ユーザが適宜、それらの情報を変更できるようにしてもよく、あるいは、そうでなくてもよい。それらの情報をユーザが変更可能な場合には、その変更は、例えば、ユーザからの変更指示を受け付ける図示しない受付部と、その変更指示に応じて情報を変更する図示しない変更部とによって実現されてもよい。その図示しない受付部による変更指示の受け付けは、例えば、入力デバイスからの受け付けでもよく、通信回線を介して送信された情報の受信でもよく、所定の記録媒体から読み出された情報の受け付けでもよい。
また、上記実施の形態において、制御装置1に含まれる2以上の構成要素が通信デバイスや入力デバイス等を有する場合に、2以上の構成要素が物理的に単一のデバイスを有してもよく、あるいは、別々のデバイスを有してもよい。
また、上記実施の形態において、各構成要素は専用のハードウェアにより構成されてもよく、あるいは、ソフトウェアにより実現可能な構成要素については、プログラムを実行することによって実現されてもよい。例えば、ハードディスクや半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをCPU等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。その実行時に、プログラム実行部は、記憶部や記録媒体にアクセスしながらプログラムを実行してもよい。なお、上記実施の形態における制御装置1を実現するソフトウェアは、以下のようなプログラムである。つまり、このプログラムは、コンピュータを、複数のモータによってそれぞれ駆動される複数の関節によって連結された複数のアームにより手先効果器を移動させるマニピュレータの制御を行う制御装置として機能させるためのプログラムであって、複数のモータの位置から手先効果器の位置を算出する位置算出部、位置算出部が算出した手先効果器の位置を蓄積する蓄積部、手先効果器の衝突を検知する衝突検知部、衝突検知部が衝突を検知した時の手先効果器の位置である衝突位置を特定する衝突位置特定部、衝突検知部が衝突を検知した後に、複数のモータのすべてが停止した状態である全軸停止状態になったことを検知する全軸停止状態検知部、衝突検知部が衝突を検知してから、全軸停止状態検知部が全軸停止状態を検知するまで、速度指令値が0になるように、マニピュレータの各モータをそれぞれ制御する複数のサーボコントローラを制御する衝突時制御部、全軸停止状態検知部が全軸停止状態を検知した時の手先効果器の位置である全軸停止位置から、衝突位置から全軸停止位置に向かう方向に沿って移動した位置である安全基準位置を算出する安全基準位置算出部、蓄積部が蓄積した手先効果器の過去の位置のうち、安全基準位置にあらかじめ決められた条件をみたすほど近い位置である安全位置を特定する安全位置特定部、手先効果器の位置が安全位置となるように制御する安全位置移動制御部として機能させるためのプログラムである。
なお、上記プログラムにおいて、上記プログラムが実現する機能には、ハードウェアでしか実現できない機能は含まれない。例えば、情報を蓄積する蓄積部や、制御を行う制御部などにおけるモデムやインターフェースカードなどのハードウェアでしか実現できない機能は、上記プログラムが実現する機能には少なくとも含まれない。
また、このプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体(例えば、CD−ROMなどの光ディスクや磁気ディスク、半導体メモリなど)に記録されたプログラムが読み出されることによって実行されてもよい。また、このプログラムは、プログラムプロダクトを構成するプログラムとして用いられてもよい。
また、このプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、あるいは分散処理を行ってもよい。
図8は、上記プログラムを実行して、上記実施の形態による制御装置1を実現するコンピュータシステム900の構成を示す図である。図8において、コンピュータシステム900は、CD−ROM(Compact Disk Read Only Memory)ドライブ905を含むコンピュータ901と、キーボード902と、マウス903と、モニタ904とを備える。コンピュータ901は、CD−ROMドライブ905に加えて、MPU(Micro Processing Unit)911と、ブートアッププログラム等のプログラムを記憶するためのROM912と、MPU911に接続され、アプリケーションプログラムの命令を一時的に記憶すると共に、一時記憶空間を提供するRAM(Random Access Memory)913と、アプリケーションプログラム、システムプログラム、及びデータを記憶するハードディスク914と、MPU911、ROM912等を相互に接続するバス915とを備える。なお、コンピュータ901は、LANやWAN等への接続を提供する図示しないネットワークカードを含んでいてもよい。また、コンピュータシステム900は、例えば、キーボード902、マウス903、及びモニタ904に代えてタッチパネルを有してもよい。
コンピュータシステム900に、上記実施の形態による制御装置1の機能を実行させるプログラムは、例えば、CD−ROM921に記憶されて、CD−ROMドライブ905に挿入され、ハードディスク914に転送されてもよい。これに代えて、そのプログラムは、図示しないネットワークを介してコンピュータ901に送信され、ハードディスク914に記憶されてもよい。プログラムは実行の際にRAM913にロードされる。なお、プログラムは、CD−ROM921、またはネットワークから直接、ロードされてもよい。
プログラムは、コンピュータ901に、上記実施の形態による制御装置1の機能を実行させるオペレーティングシステム(OS)、またはサードパーティプログラム等を必ずしも含んでいなくてもよい。プログラムは、制御された態様で適切な機能(モジュール)を呼び出し、所望の結果が得られるようにする命令の部分のみを含んでいてもよい。コンピュータシステム900がどのように動作するのかについては周知であり、詳細な説明は省略する。
また、本発明は、以上の実施の形態に限定されることなく、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。
以上より、本発明による制御装置等によれば、手先効果器が衝突した場合に、減速機の負荷を低減させることができ、手先効果器を安全な位置に回避させることができるという効果が得られ、マニピュレータの制御装置等として有用である。
1 制御装置
2 マニピュレータ
11−1〜11−N サーボコントローラ
12 位置算出部
13 蓄積部
14 位置記憶部
15 衝突検知部
16 衝突位置特定部
17 全軸停止状態検知部
18 衝突時制御部
19 安全基準位置算出部
20 安全位置特定部
21 安全位置移動制御部
22 各軸位置指令算出部
23 モータ停止部
24 フラグ記憶部
31 速度指令生成器
32 切替器
33 電流指令生成器
34 トルク指令生成器
35 現在位置蓄積部
36 判断部
41 減算器
42 積算器
43 積分器
44 加算器
45 積分値蓄積部
46 置換部
100 産業用ロボット

Claims (6)

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

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2012158672A JP5912953B2 (ja) 2012-07-17 2012-07-17 制御装置、及び制御方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2012158672A JP5912953B2 (ja) 2012-07-17 2012-07-17 制御装置、及び制御方法

Publications (2)

Publication Number Publication Date
JP2014018901A JP2014018901A (ja) 2014-02-03
JP5912953B2 true JP5912953B2 (ja) 2016-04-27

Family

ID=50194374

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2012158672A Active JP5912953B2 (ja) 2012-07-17 2012-07-17 制御装置、及び制御方法

Country Status (1)

Country Link
JP (1) JP5912953B2 (ja)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5937706B1 (ja) 2015-01-21 2016-06-22 ファナック株式会社 ロボットに加わる外力に基づいてロボットを制御するロボット制御装置、およびロボットシステム
JP6055002B2 (ja) 2015-02-20 2016-12-27 ファナック株式会社 ロボットを退避動作させる人間協調ロボットシステム
JP6034895B2 (ja) * 2015-02-20 2016-11-30 ファナック株式会社 外力に応じてロボットを退避動作させる人間協調ロボットシステム
JP6055014B2 (ja) 2015-03-23 2016-12-27 ファナック株式会社 物または人との接触を検知する機能を有するロボット制御装置
JP6737827B2 (ja) * 2018-03-29 2020-08-12 ファナック株式会社 協働ロボットの制御装置及び制御方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0276691A (ja) * 1988-09-09 1990-03-16 Daikin Ind Ltd 産業用ロボットにおける動作停止方法およびその装置
JP2608161B2 (ja) * 1990-03-29 1997-05-07 ファナック株式会社 産業用ロボットの停止制御方法
JP2788149B2 (ja) * 1992-03-31 1998-08-20 ファナック株式会社 数値制御装置におけるスキップ制御方法
JPH06170754A (ja) * 1992-10-30 1994-06-21 Yaskawa Electric Corp マスタ・スレーブロボットの制御装置
JP3367641B2 (ja) * 1998-07-31 2003-01-14 株式会社安川電機 ロボットの制御装置
JP4098748B2 (ja) * 2004-05-07 2008-06-11 ファナック株式会社 緊急停止指令を出力する機能を有する数値制御装置

Also Published As

Publication number Publication date
JP2014018901A (ja) 2014-02-03

Similar Documents

Publication Publication Date Title
JP5912953B2 (ja) 制御装置、及び制御方法
US11279033B2 (en) Method and apparatus for collision-free motion planning of a manipulator
JP5218524B2 (ja) ロボットシステムおよびロボット動作規制方法
US11040449B2 (en) Robot control system and method of controlling a robot
KR102026490B1 (ko) 로봇을 제어하기 위한 방법 및 제어수단
CN110000815B (zh) 一种碰撞检测方法、装置、电子设备及存储介质
KR102037565B1 (ko) 다축 기계, 특히 로봇을 작동하기 위한 방법 및 시스템
JP6272599B1 (ja) 制御装置およびモータ制御システム
CN109153117B (zh) 对机器人复合体的控制
CN113799123B (zh) 机械手防撞控制方法、系统、智能终端及存储介质
JP2017127964A (ja) 学習機能を備えたロボット装置
CN112828890A (zh) 机械臂轨迹规划方法、装置、电子设备及存储介质
JP2018062026A (ja) ロボットの速度や加速度を制限する機能を備えたロボット制御装置
KR101820580B1 (ko) 경로 전진 변수들을 갖는 안전한 로봇
WO2022166330A1 (zh) 位置调整方法、装置、终端设备及可读存储介质
JP2014018941A (ja) 制御装置、及び制御方法
CN114536342A (zh) 一种多臂系统及其臂间防撞控制方法
JP2012216151A (ja) 干渉回避制御装置
JP5220241B1 (ja) 指令生成装置および指令生成方法
JP5918097B2 (ja) 情報処理装置、情報処理方法及びプログラム
TW202415506A (zh) 穩健運動規劃及/或用於多機器人環境之控制
JP2014117787A (ja) 制御装置
CN117297791A (zh) 机械臂的控制方法、医疗系统、计算机设备
CN116700231A (zh) 磁浮平台安全停车方法、装置、控制设备及安全停车系统
WO2023138757A1 (en) Method and apparatus for implementing a safety configuration

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20150206

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20151225

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20160105

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20160303

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20160401

R150 Certificate of patent or registration of utility model

Ref document number: 5912953

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250