ロボット装置の制御方法及びロボット装置
【課題】作業領域がさまざまに刻々と変化する場合においても、マニピュレータの作業領域への干渉を容易に回避できるようになされたロボット装置の制御方法及びロボット装置を提供する。
【解決手段】複数のリンクにより構成されたマニピュレータ１，２を制御するにあたり、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、マニピュレータ１，２の周囲の物体について外形を包絡する複数の球体Ｚ１，Ｚ２，Ｚ３，・・・を設定し、各リンクの動作に応じて各リンク及び周囲の物体について設定された各球体Ｚ１，Ｚ２，Ｚ３，・・・の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが周囲の物体に干渉するか否かの判断を行う。
【解決手段】複数のリンクにより構成されたマニピュレータ１，２を制御するにあたり、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、マニピュレータ１，２の周囲の物体について外形を包絡する複数の球体Ｚ１，Ｚ２，Ｚ３，・・・を設定し、各リンクの動作に応じて各リンク及び周囲の物体について設定された各球体Ｚ１，Ｚ２，Ｚ３，・・・の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが周囲の物体に干渉するか否かの判断を行う。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、マニピュレータの作業領域への干渉を回避するようにしたロボット装置の制御方法に関し、また、マニピュレータの作業領域への干渉を回避するようになされたロボット装置に関する。
【背景技術】
【０００２】
ロボット装置のマニピュレータを制御するにあたっては、制御対象となるマニピュレータと、他のマニピュレータやワーク、その他の種々の機械などが存在する作業領域への干渉を避ける必要がある。
【０００３】
従来のロボット装置の制御方法においては、特許文献１乃至特許文献３に記載されているように、マニピュレータと干渉する可能性のある作業領域をあらかじめ設定して記憶しておき、記憶しておいた領域と、マニピュレータの移動先とが重なるかどうかを計算して、干渉の有無を判定していた。
【０００４】
【特許文献１】特開平８−１６６８０９号公報
【特許文献２】特開平６−１４９３３２号公報
【特許文献３】特許第３４０２６４５号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
ところで、前述のような従来のロボット装置の制御方法においては、あらかじめ設定し記憶しておいた領域についてしか、マニピュレータの干渉の有無を判定することができない。
【０００６】
そのため、このようなロボット装置の制御方法においては、作業領域がさまざまに刻々と変化する場合については、事前準備が極めて煩雑なものとなってしまう。
【０００７】
そこで、本発明は、前記の実情に鑑みてなされたものであり、その目的は、作業領域がさまざまに刻々と変化する場合においても、マニピュレータの作業領域への干渉を容易に回避し、停止時間を最小限に抑え、安全に作業を継続できるようになされたロボット装置の制御方法及びロボット装置を提供することにある。
【課題を解決するための手段】
【０００８】
前述の課題を解決し、前記目的を達成するため、本発明に係るロボット装置の制御方法は、以下の構成のいずれか一以上を有するものである。
【０００９】
〔構成１〕
複数のリンクにより構成されたマニピュレータを制御するにあたり、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの周囲の物体について外形を包絡する複数の球体を設定し、各リンクの動作に応じて各リンク及び周囲の物体について設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが周囲の物体に干渉するか否かの判断を行うことを特徴とするものである。
【００１０】
〔構成２〕
複数のリンクにより構成されたマニピュレータを制御するにあたり、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じてこれら各リンク及び作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うことを特徴とするものである。
【００１１】
〔構成３〕
複数のリンクにより構成されたマニピュレータを制御するにあたり、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンクを包絡する球体及び作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うことを特徴とするものである。
【００１２】
〔構成４〕
複数のリンクにより構成されたマニピュレータを制御するにあたり、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンク及び進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが進入禁止領域に干渉するか否かの判断を行うことを特徴とするものである。
【００１３】
また、本発明に係るロボット装置は、以下の構成のいずれか一以上を有するものである。
【００１４】
〔構成５〕
複数のリンクにより構成されたマニピュレータと、このマニピュレータを制御する制御手段とを備え、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの周囲の物体について外形を包絡する複数の球体を設定し、各リンク及び周囲の物体について設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが周囲の物体に干渉するか否かの判断を行うことを特徴とするものである。
【００１５】
〔構成６〕
複数のリンクにより構成されたマニピュレータと、このマニピュレータを制御する制御手段とを備え、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、各リンク及び作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うことを特徴とするものである。
【００１６】
〔構成７〕
複数のリンクにより構成されたマニピュレータと、このマニピュレータを制御する制御手段とを備え、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、各リンクを包絡する球体及び作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うことを特徴とするものである。
【００１７】
〔構成８〕
複数のリンクにより構成されたマニピュレータと、このマニピュレータを制御する制御手段とを備え、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、各リンク及び進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが進入禁止領域に干渉するか否かの判断を行うことを特徴とするものである。
【発明の効果】
【００１８】
本発明に係るロボット装置の制御方法においては、構成１を有することにより、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの周囲の物体について外形を包絡する複数の球体を設定し、各リンクの動作に応じて各リンク及び周囲の物体について設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが周囲の物体に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によってマニピュレータの周囲の物体への干渉の有無を事前に検出することができる。
【００１９】
本発明に係るロボット装置の制御方法においては、構成２を有することにより、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じてこれら各リンク及び作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって他のマニピュレータが作業を行っている作業領域への干渉の有無を事前に検出することができる。
【００２０】
本発明に係るロボット装置の制御方法においては、構成３を有することにより、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンクを包絡する球体及び作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって他のマニピュレータが作業を行っている作業領域への干渉の有無を事前に検出することができる。
【００２１】
本発明に係るロボット装置の制御方法においては、構成４を有することにより、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンク及び進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが進入禁止領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって進入禁止領域への干渉の有無を事前に検出することができる。
【００２２】
また、本発明に係るロボット装置においては、構成５を有することにより、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの周囲の物体について外形を包絡する複数の球体を設定し、各リンク及び周囲の物体について設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが周囲の物体に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によってマニピュレータの周囲の物体への干渉の有無を事前に検出することができる。
【００２３】
本発明に係るロボット装置においては、構成６を有することにより、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、各リンク及び作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって他のマニピュレータが作業を行っている作業領域への干渉の有無を事前に検出することができる。
【００２４】
本発明に係るロボット装置においては、構成７を有することにより、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、各リンクを包絡する球体及び作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって他のマニピュレータが作業を行っている作業領域への干渉の有無を事前に検出することができる。
【００２５】
本発明に係るロボット装置においては、構成８を有することにより、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、各リンク及び進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが進入禁止領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって進入禁止領域への干渉の有無を事前に検出することができる。
【００２６】
すなわち、本発明は、作業領域がさまざまに刻々と変化する場合においても、マニピュレータの作業領域への干渉を容易に回避できるようになされたロボット装置の制御方法及びロボット装置を提供することができるものである。
【発明を実施するための最良の形態】
【００２７】
以下、本発明を実施するための最良の形態について図面を参照して説明する。
【００２８】
〔ロボット装置の構成〕
図１は、本発明に係るロボット装置の構成を示す模式的な斜視図である。
【００２９】
本発明に係るロボット装置は、図１に示すように、少なくとも一台のマニピュレータ１，２を有している。この実施の形態においては、二台のマニピュレータ１，２が、互いの動作領域内に干渉して配置することが可能となされている。
【００３０】
各マニピュレータ１，２は、それぞれ、複数のアクチュエータ（駆動装置）とリンク（剛体の構造物）とによって構成されている。各リンク間は、回動（屈曲）可能、または、伸縮（並進）可能な関節、あるいは、旋回可能な関節３を介して接続されており、それぞれアクチュエータによって相対駆動されるようになっている。各アクチュエータは、制御手段によって制御される。
【００３１】
各マニピュレータ１，２において、第１のリンク（基端部のリンク）１ａ，２ａは、作業台１０１上に固定されており、この第１のリンク１ａ，２ａに、第２のリンク１ｂ，２ｂが接続されている。第２のリンク１ｂ，２ｂには第３のリンク１ｃ，２ｃが接続され、第３のリンク１ｃ，２ｃには第４のリンク１ｄ，２ｄが接続され、このようにして、順次、各リンクが接続されている。先端部のリンクの先端側（以下、「手先」という。）には、ワーク１０２を把持する把持機構４が設けられている。この把持機構４も、制御手段によって制御される。
【００３２】
これらマニピュレータ１，２においては、第１のリンク１ａ，２ａに対する第２のリンク１ｂ，２ｂの位置、第２のリンク１ｂ，２ｂに対する第３のリンク１ｃ，２ｃの位置、第３のリンク１ｃ，２ｃに対する第４のリンク１ｄ，２ｄの位置と、順次先端側のリンクの位置を制御してゆくことにより、手先の位置を制御することができ、この手先において把持機構４によって把持したワークの位置を制御することができる。
【００３３】
〔ロボット装置の制御方法〕
図２は、マニピュレータを構成する各リンクについて設定された外形を包絡する複数の球体を示す側面図である。
【００３４】
そして、このロボット装置においては、本発明に係るロボット装置の制御方法が実施される。すなわち、このロボット装置においては、図２に示すように、制御手段により、マニピュレータ１，２を構成する各リンク１ａ，２ａ，１ｂ，２ｂ・・・について、外形を包絡する複数の球体５ａ，５ｂ，５ｃ・・・５ｎが設定される。このように球体５ａ，５ｂ，５ｃ・・・５ｎを設定することにより、各リンク１ａ，２ａ，１ｂ，２ｂ・・・の動作に応じて各リンクについて設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク１ａ，２ａ，１ｂ，２ｂ・・・同士が衝突するか否かの判断を行うことができる。
【００３５】
球体５ａ，５ｂ，５ｃ・・・５ｎの数、位置及び半径等は、これら球体によってどの程度正確にリンク１ａ，２ａ，１ｂ，２ｂ・・・の形状を近似するか、また、安全余裕をどの程度にするかに応じて決定する。図２中の（ｂ）に示すように、１つのリンクに対して球体の数を増やすほど、図２中の（ａ）に示す球体の数が少ない場合に比較して、正確にリンクの形状を近似することができる。ただし、球体の数を増やすほど、各球体の中心間距離を求めるための計算量は増加する。
【００３６】
図３は、６自由度のマニピュレータの構成及び各リンクについて設定された外形を包絡する複数の球体を示す側面図である。
【００３７】
例えば、このロボット装置のマニピュレータが、図３中の（ａ）に示すように、６自由度のマニピュレータである場合には、図３中の（ｂ）に示すように、このマニピュレータの各リンクａ，ｂ，ｃ，ｄ，ｅ，ｆ，ｇについて、前述のように複数の球体５を設定することによって、マニピュレータの全体について、複数の球体５によって近似することができる。
【００３８】
このロボット装置の制御方法においては、各マニピュレータ１，２についてこのような球体の設定を行うとともに、ロボット装置の近傍に存在する障害物についても、球体の設定を行う。すなわち、ロボット装置の近傍に位置する障害物について、各種センサによる検出結果や予め与えられた情報に基づいて位置情報を得て、この障害物の外形を包絡する複数の球体を設定する。
【００３９】
そして、マニピュレータの各リンクの動作に応じて各リンクについて設定した各球体及び障害物について設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士及び各リンクと障害物とが衝突するか否かの判断を行うことができる。
【００４０】
すなわち、マニピュレータの制御中に、リアルタイムで各球体の中心間の距離を計算し、この距離が２つの球体の半径の和を下回った場合には、球体同士が干渉しリンク同士、または、リンクと障害物とが接触したと判断する。各球体は、マニピュレータの各リンク及び障害物の外形を覆うとともに、ある程度の余裕量を持つように、位置及び大きさが設定されているので、球体同士が干渉しなければ、リンク同士及びリンクと障害物とは、衝突していないと判断することができる。
【００４１】
図４は、設定された球体の中心間の距離の求め方を説明する図である。
【００４２】
設定された球体の中心間の距離を求めるにあたり、例えば、図４に示すように、半径ｒ１の球体Ｓ１及び半径ｒ２の球体Ｓ２について、Ｓ１の中心位置を（ｐｘ１、ｐｙ１、ｐｚ１）、Ｓ２の中心位置を（ｐｘ２、ｐｙ２、ｐｚ２）とし、これら２つの球体の中心間の距離をＲとする。この場合、以下の（式１）が成立する。
Ｒ２＝（ｐｘ１−ｐｘ２）２＋（ｐｙ１−ｐｙ２）２＋（ｐｚ１−ｐｚ２）２・・（式１）
【００４３】
したがって、これら２つの球体が干渉しているかどうかは、以下の（式２）が満足されるかどうかによって確認することができる。
Ｒ２＜（ｒ１＋ｒ２）２ ・・・（式２）
【００４４】
この処理をすべての球体の組合せについて行うことによって、各球体間の干渉の有無を判断することができる。このようにマニピュレータの各リンクについて球体を設定することによって、球体の中心間距離の計算のみによって、球体同士の干渉の有無により、各リンクの衝突の有無の判断を行うことができる。すなわち、各リンクを円筒や平面を用いて表現した場合に比較して、方向（法線）や端点処理が不要となり、簡単な計算によって判断することができる。
【００４５】
ただし、同一のリンクについて設定された球体同士は、それぞれの半径を下回って配置されていても、接触と判断すべきではない。そこで、以下の方策に従い、衝突確認を行う必要がない球体の組を決定しておくことにより、必要な計算量を減らすことができる。
【００４６】
図５は、マニピュレータの所定のリンクの構成を示す側面図である。
【００４７】
このロボット装置においては、図５に示すように、セグメントを定義する。このセグメントは、一の屈曲（回転）または並進関節から、マニピュレータの先端側への次の屈曲（回転）または並進関節までの間の部分をいう。一の屈曲（回転）または並進関節は、次の屈曲（回転）または並進関節の位置を変化させる機能を有する。ここで、セグメントの定義においては、旋回関節（次の関節をその場で回転させるだけで次の関節の位置を変えない関節）は、除外して考える。すなわち、旋回関節で接続された複数のリンクは、同一のセグメントとなる。
【００４８】
したがって、図５に示すように、屈曲関節ｑ１から屈曲関節ｑ３までのリンクＬ１及びリンクＬ２が、同一のセグメントＳＧ１と定義される。リンクＬ１及びリンクＬ２は、旋回関節ｑ２によって接続されている。
【００４９】
このロボット装置において、各リンクについて設定される球体は、そのリンクが属するセグメント上に設定されていることになる。なお、障害物についても、１つ、または、２つ以上のセグメントとして定義し、球体を設定する。同一のセグメント上では、球体同士が干渉していても衝突が生じているわけではないので、同一のセグメント上の球体同士では、干渉が生じているかどうかの判別を行う必要がない。
【００５０】
また、このロボット装置においては、衝突が生じ得ない２つのセグメント上に設定された球体同士は、干渉が生じているかどうかの判別を行う必要がない。
【００５１】
図６は、接触が生じ得ないセグメントを示す側面図である。
【００５２】
このロボット装置において、接触が生じ得ないセグメントとは、図６に示すように、屈曲（回転）または並進関節ｑを介して隣り合ったセグメントである。すなわち、図６における「セグメント１」及び「セグメント２」、「セグメント２」及び「セグメント３」、そして、「セグメント４」及び「セグメント５」は、屈曲（回転）または並進関節ｑによって接続されているため、屈曲（回転）または並進関節ｑの可動範囲を制限したり制御することによって互いの接触を回避することができ、これら２つのセグメント上に設定された球体同士について干渉が生じているかどうかの判別を行う必要がない。
【００５３】
また、物理的に距離が離れており、互いの可動範囲内にないセグメント、すなわち、図６における「セグメント２」及び「セグメント４」は、可動範囲内でどのように駆動されても接触が生じ得ないので、これら２つのセグメント上に設定された球体同士について干渉が生じているかどうかの判別を行う必要がない。
【００５４】
さらに、作業台１０１上に固定された２つのセグメント、すなわち、図６における「セグメント１」及び「セグメント４」は、移動されることがなく接触が生じ得ないので、これら２つのセグメント上に設定された球体同士について干渉が生じているかどうかの判別を行う必要がない。
【００５５】
ところで、セグメント上に設定された複数の球体の位置を算出するには、一般的なマニピュレータの制御中に得られる順運動学の結果を用いることができる。ある関節の基準座標に対する位置及び姿勢は、一般的な順運動学により、以下の（式３）、すなわち、４×４の同次変換行列によって表現することができる。
【００５６】
【数１】
・・・（式３）
【００５７】
ここで、（ｘ、ｙ、ｚ）は、その関節における座標系を表す互いに直交した単位３次元ベクトルであり、その関節の姿勢を表す。ｐは、基準座標におけるその関節の位置を表す３次元ベクトルである。これらの値は、順運動学により、手先の位置及び姿勢を計算する過程で得られるものである。この関節から、この関節における座標系の（ｘ、ｙ、ｚ）軸方向にそれぞれ（ｌｘ、ｌｙ、ｌｚ）移動した点の座標ｐ′は、以下の（式４）で示すことができる。
ｐ′＝ｐ＋ｌｘ・ｘ＋ｌｙ・ｙ＋ｌｚ・ｚ ・・・（式４）
【００５８】
再び、図５に示した例を考えると、屈曲関節ｑ１の中心位置をｐ１、ｐ１における座標、すなわち、リンクＬ１の姿勢を（ｘ１、ｙ１、ｚ１）とし、旋回関節ｑ２の中心位置をｐ２、ｐ２における座標、すなわち、リンクＬ２の姿勢を（ｘ２、ｙ２、ｚ２）とすると、これらの値は、順運動学を計算する過程で求められる。
【００５９】
屈曲関節ｑ１からリンクＬ１の長手方向、すなわち、ｚ１方向に長さｌ１ｚ離れた位置に球体の中心Ｓ１がある場合には、Ｓ１の位置は、以下の（式５）によって示される。
Ｓ１＝ｐ１＋ｌ１ｚ・ｚ１ ・・・（式５）
【００６０】
同様に、旋回関節ｑ２からリンクＬ２の長手方向、すなわち、ｚ２方向に長さｌ２ｚ離れ、ｙ２方向に長さｌ２ｙ離れた位置に球体の中心Ｓ２がある場合には、Ｓ２の位置は、以下の（式６）によって示される。
Ｓ２＝ｐ２＋ｌ２ｚ・ｚ２＋ｌ２ｙ・ｙ２ ・・・（式６）
【００６１】
このように、このロボット装置においては、一般的なマニピュレータの制御の途中で計算されている結果を用いて球体の位置を求めることができるので、球体の位置計算のために生ずる計算量の増加は、微小なものである。
【００６２】
図７は、ワークの取り扱いを示す側面図である。
【００６３】
このロボット装置において、図７に示すように、マニピュレータの把持機構４がワーク１０２を把持するときに、図７中（ａ）に示すように、ワーク１０２を把持する前には衝突が生じなかった動作でも、図７中（ｂ）に示すように、ワーク１０２を把持した後には、衝突が生じることがある。このような場合には、ワーク１０２についても球体を設定しておくことにより、ワーク１０２も含めた接触検知を行うことができる。
【００６４】
この場合、把持機構４がワーク１０２を把持した状態に応じて、セグメントに対する球体の設定の追加及び除去を動的に行ってもよいが、把持機構４の位置に半径ゼロ、または、負の値の半径の球体ＷＳを予め設定しておき、把持機構４がワーク１０２を把持したときには、把持機構４の位置の球体ＷＳの半径を拡大させつつ位置を移動することで、この球体ＷＳによりワーク１０２の外形を包絡させるようにしてもよい。このような扱いとすることにより、処理が容易になる。
【００６５】
図７中（ａ）に示すように、把持機構４の位置の球体ＷＳの半径をゼロ、または、負の値としておくことにより、他の任意の球体との干渉の有無の判定を行っても、干渉が検知されることはない。ワーク１０２を把持したときには、図７中（ｂ）に示すように、球体ＷＳの半径をｒｗに拡大させるとともに、オフセットＬＯｗを与えて球体ＷＳを移動させることにより、ワーク１０２の外形を包絡させる。この状態において、球体ＷＳに干渉する可能性がある他の球体との干渉の有無の判別を行うことができる。把持機構４がワーク１０２を解放したときには、再び、球体ＷＳの半径をゼロ、または、負の値にし、位置を手先位置などの基準位置に戻す。
【００６６】
なお、球体ＷＳの半径ｒｗ及びオフセットＬＯｗは、ワーク１０２の形状及び大きさに応じて予め設定しておき、動作情報などに基づいて、取得するようにすることができる。さらに、カメラ、レーザ計測器やその他のセンサを使って計測することにより、人や一時的に置かれた治具など、予め情報が取得されていないものについても、検出されたときに球体を設定し、干渉有無の判別対象とすることができる。
【００６７】
そして、このロボット装置の制御方法においては、一つのマニピュレータがこれから作業を行う空間を作業領域とし、この作業領域に他のマニピュレータが進入しないように制御することができる。
【００６８】
なお、本発明に係るロボット装置においては、各セグメント上、または、ワーク１０２上に設定される各球体の半径は、各リンクの動作速度に応じて、動的に変化させるようにしてもよい。例えば、リンクの動作速度が速い場合には、このリンクが属するセグメント上に設定される各球体の半径を拡大することにより、このリンクが他のリンクに衝突することをより確実に防止することができる。また、リンクの動作速度が遅い場合には、このリンクが属するセグメント上に設定される各球体の半径を縮小することにより、このリンクが他のリンクに衝突することを防止しつつ、このリンクの可動範囲を広くすることができる。
【００６９】
図８は、マニピュレータと作業領域との位置関係を示す平面図である。
【００７０】
すなわち、図８に示すように、１つのマニピュレータ１が第１の作業点Ｐ１に対して作業を始めたときには、第１の作業点Ｐ１を包含する第１の作業領域Ｚ１を設定し、他のマニピュレータ２は、第１の作業領域Ｚ１及び作業中のマニピュレータ１に干渉しない空間内において移動する。マニピュレータ１の作業の終了後は、第１の作業領域Ｚ１への進入禁止を解除し、他のマニピュレータ２も自由に動ける空間となる。
【００７１】
このようにロボット装置を制御するには、前述したように、各ワーク１０２におけるそれぞれの作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６に対して、これらを包絡する球体状の作業領域Ｚ１，Ｚ２，Ｚ３，・・・を設定する。各マニピュレータ１，２についても、前述したように、それぞれの外形を包絡する複数の球体を設定する。そして、作業領域Ｚ１，Ｚ２，Ｚ３，・・・と各マニピュレータ１，２について設定された各球体の中心間距離を計算することにより、作業領域とマニピュレータ１，２との接触、マニピュレータ１，２同士の接触を検出することができる。
【００７２】
一方のマニピュレータ１が第１の作業点Ｐ１に対して作業を行っている間は、マニピュレータ１が第１の作業領域Ｚ１と接触し、第１の作業領域Ｚ１は、一方のマニピュレータ１の占有領域となる。このとき、他方のマニピュレータ２は、第２の作業点Ｐ２に対して作業を行うと、一方のマニピュレータ１が占有している第１の作業領域Ｚ１に接触してしまうため、第２の作業点Ｐ２に対する作業を行うことができない。他方のマニピュレータ２は、第３の作業点Ｐ３に対して作業を行っても、第１の作業領域Ｚ１には接触しないため、第３の作業点Ｐ３に対する作業を行うことが可能である。そこで、他方のマニピュレータ２は、第３の作業点Ｐ３に対する作業を行う。このようにして、各マニピュレータ１，２の全体として、作業を効率的に継続することができる。
【００７３】
また、他方のマニピュレータ２が第４の作業点Ｐ４に対する作業を行う場合、他方のマニピュレータ２が単独で作業を行うのであれば、他方のマニピュレータ２は、第４の作業点Ｐ４へまっすぐ接近して作業を行えばよい。しかし、他方のマニピュレータ２の移動経路に第１の作業領域Ｚ１が重なっており、一方のマニピュレータ１が第１の作業点Ｐ１に対する作業を行っている場合には、他方のマニピュレータ２は、一方のマニピュレータ１が占有している第１の作業領域Ｚ１に接触してしまう。
【００７４】
このような場合には、他方のマニピュレータ２は、第４の作業点Ｐ４へまっすぐ接近することができないので、マニピュレータ１が占有している第１の作業領域Ｚ１と他方のマニピュレータ２について設定された球体とが接触しないように、姿勢を調整しながら第４の作業点Ｐ４に接近して、第４の作業点Ｐ４に対する作業を行う。
【００７５】
なお、従来のロボット装置の制御方法においては、このような場合には、他のマニピュレータの動作パターンすべてに対応する回避動作をあらかじめ教示しておく必要があった。これに対し、本発明においては、基本的な作業のみを予め教示しておき、作業進行中には、リアルタイムで球体同士の中心間距離を計算することにより、互いの作業領域に接触しないように姿勢を調整することが可能になっている。
【００７６】
このようにして、本発明においては、各作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６ごとに作業領域Ｚ１，Ｚ２，Ｚ３，・・・を設定し、各作業領域Ｚ１，Ｚ２，Ｚ３，・・・と各マニピュレータ１，２との干渉を監視することにより、複数のマニピュレータ１，２が安全を保ちつつ同時に作業を継続することが可能となり、作業を効率的に実行することができる。
【００７７】
また、ロボット装置の制御においては、ワークの有無に係らず、ある空間を進入禁止領域としたいことがある。このような場合には、進入禁止領域の境界面に沿って、多数の球体を壁状に並べ、これら球体に対する干渉を監視することにより、マニピュレータが進入禁止領域に進入することを防止することができる。
【００７８】
また、進入禁止領域の境界面に近似する球面を有する大きな半径の球体を設定するようにすれば、一つの球体の設定により、進入禁止領域の境界面に近似した壁を設定することができる。この一つの球体に対する干渉を監視することにより、マニピュレータが進入禁止領域に進入することを防止することができる。この場合には、多数の球体を設定する場合に比較して、計算量を増やすことなく、進入禁止領域との干渉を監視することができる。
【００７９】
さらに、各作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６が十分に離れており、かつ、一つの作業点に対して複数のマニピュレータ１，２が交互に作業を行わなければならない場合には、ある作業点に対して作業が行われているという事実のみが判別できればよい。この場合には、各作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６について所定の大きさを有する作業領域Ｚ１，Ｚ２，Ｚ３，・・・を設定する必要はなく、各作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６のみが判別されればよい。
【００８０】
例えば、第１乃至第３の作業点Ｐ１，Ｐ２，Ｐ３が十分に離れていて、第２の作業点Ｐ２に対して、各マニピュレータ１，２が交互に作業を行う場合には、一方のマニピュレータ１が第２の作業点Ｐ２で作業をしているときには、他方のマニピュレータ２は、第３の作業点Ｐ３に対する作業を行うか、または、待機する。この場合には、各作業領域Ｚ１，Ｚ２，Ｚ３，・・・を半径０の球体として設定すればよい。干渉の有無についての計算方法は、前述したものと同様である。
【００８１】
図９は、本発明に係るロボット装置の制御方法を示すフローチャートである。
【００８２】
このロボット装置の制御方法においては、図９に示すように、事前の処理、設計として、ステップｓｔ１でマニピュレータの各リンクについて球体を設定し（リンクの近似）、ステップｓｔ２で障害物について球体を設定し（障害物の近似）、ステップｓｔ３でワークについて球体を設定し（ワークの近似）、ワークの形状及び大きさに応じた球体の半径及びオフセットも設定しておく。さらに、ステップｓｔ４で、セグメントの設定（分類）を行っておく。
【００８３】
そして、ロボット装置の動作が開始されると、オンライン制御中の処理に入り、ステップｓｔ５で任意の２つのセグメントを選択し、ステップｓｔ６で、これらセグメントが接触する可能性があるかを判別する。接触する可能性がなければ、ステップｓｔ５に戻り、他の２つのセグメントを選択して、ステップｓｔ６に進む。ステップｓｔ６で、２つのセグメントが接触する可能性があれば、ステップｓｔ７に進み、接触する可能性のある２つのセグメント上に設定された球体から、各１つの球体を選択し、ステップｓｔ８に進む。
【００８４】
ステップｓｔ８では、接触する可能性のある２つのセグメント上に設定された球体の全てについて干渉の有無が判別されたかを判断し、全ての球体について判別されていれば、ステップｓｔ５に戻り、全ての球体について判別されていなければ、ステップｓｔ９に進む。
【００８５】
ステップｓｔ９では、選択された２つの球体について、中心間の距離を計算し、ステップｓｔ１０に進み、球体の中心間距離がこれら球体の半径の合計よりも小さいかどうかを判別する。球体の中心間距離が半径の合計よりも大きければ、ステップｓｔ７に戻り、球体の中心間距離が半径の合計よりも小さければ、ステップｓｔ１１に進む。ステップｓｔ１１では、ステップｓｔ１０で選択されていた２つの球体に対応するリンク同士、リンクと障害物、または、ワークと障害物の衝突が生ずることを判別して、マニピュレータの動作を停止させるなどの所定の処理を行う。
【００８６】
図１０は、本発明に係るロボット装置の制御方法において作業領域を設定する場合のフローチャートである。
【００８７】
このロボット装置の制御方法において、作業領域を設定する場合には、図１０に示すように、ステップｓｔ２１で動作を開始すると、ステップｓｔ２２に進み、各マニピュレータ１，２が単独で各作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６に対して作業を行う場合の作業手順を教示し、ステップｓｔ２３に進む。ステップｓｔ２３では、作業を行う作業点を選択して、ステップｓｔ２４に進む。ステップｓｔ２４では、選択された作業点において他のマニピュレータによる作業が行われているか否かを判別し、他のマニピュレータによる作業が行われていればステップｓｔ２５に進み、他のマニピュレータによる作業が行われていなければステップｓｔ２６に進む。ステップｓｔ２５では、他の作業点を選択して、ステップｓｔ２４に戻る。
【００８８】
ステップｓｔ２６では、他のマニピュレータが占有している作業領域や進入禁止領域との干渉の有無を判別し、干渉がなければステップｓｔ２７に進み、干渉があればステップｓｔ２８に進む。ステップｓｔ２８では、姿勢を調整することによって干渉を回避できるか否かを判別し、干渉を回避できればステップｓｔ２９に進み、干渉を回避できなければステップｓｔ３０に進む。ステップｓｔ２９では、姿勢を調整して、ステップｓｔ２７に進む。
【００８９】
ステップｓｔ３０では、作業点を変更することによって干渉を回避できるか否かを判別し、干渉を回避できればステップｓｔ３１に進み、干渉を回避できなければステップｓｔ３２に進む。ステップｓｔ３１では、作業点を変更して、ステップｓｔ２７に進む。
【００９０】
ステップｓｔ３２では、他のマニピュレータによる作業を終了することを待ち、ステップｓｔ２６に戻る。
【００９１】
ステップｓｔ２７では、選択された作業点に対して作業領域（占有領域）を設定し、ステップｓｔ３３に進む。ステップｓｔ３３では、設定された作業領域内において、作業点に対する作業を実行し、ステップｓｔ３４に進む。ステップｓｔ３４では、全ての作業が終了したか否かを判別し、全ての作業が終了していればステップｓｔ３５に進んで動作を終了し、全ての作業が終了していなければステップｓｔ２３に戻る。
【図面の簡単な説明】
【００９２】
【図１】本発明に係るロボット装置の構成を示す模式的な斜視図である。
【図２】本発明に係るロボット装置においてマニピュレータを構成する各リンクについて設定された外形を包絡する複数の球体を示す側面図である。
【図３】本発明に係るロボット装置におけるマニピュレータの例である６自由度のマニピュレータの構成及び各リンクについて設定された外形を包絡する複数の球体を示す側面図である。
【図４】本発明に係るロボット装置における設定された各球体の中心間の距離の求め方を説明する図である。
【図５】本発明に係るロボット装置におけるマニピュレータの所定のリンクの構成を示す側面図である。
【図６】本発明に係るロボット装置において接触が生じ得ないセグメントを示す側面図である。
【図７】本発明に係るロボット装置の制御方法におけるワークの取り扱いを示す側面図である。
【図８】本発明に係るロボット装置の制御方法におけるマニピュレータと作業領域との位置関係を示す平面図である。
【図９】本発明に係るロボット装置の制御方法を示すフローチャートである。
【図１０】本発明に係るロボット装置の制御方法において作業領域を設定する場合のフローチャートである。
【符号の説明】
【００９３】
１，２ マニピュレータ
３ 関節
４ 把持部
５ 球体
１０１ 作業台
１０２ ワーク
Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６ 作業点
Ｚ１，Ｚ２，Ｚ３ 作業領域
【技術分野】
【０００１】
本発明は、マニピュレータの作業領域への干渉を回避するようにしたロボット装置の制御方法に関し、また、マニピュレータの作業領域への干渉を回避するようになされたロボット装置に関する。
【背景技術】
【０００２】
ロボット装置のマニピュレータを制御するにあたっては、制御対象となるマニピュレータと、他のマニピュレータやワーク、その他の種々の機械などが存在する作業領域への干渉を避ける必要がある。
【０００３】
従来のロボット装置の制御方法においては、特許文献１乃至特許文献３に記載されているように、マニピュレータと干渉する可能性のある作業領域をあらかじめ設定して記憶しておき、記憶しておいた領域と、マニピュレータの移動先とが重なるかどうかを計算して、干渉の有無を判定していた。
【０００４】
【特許文献１】特開平８−１６６８０９号公報
【特許文献２】特開平６−１４９３３２号公報
【特許文献３】特許第３４０２６４５号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
ところで、前述のような従来のロボット装置の制御方法においては、あらかじめ設定し記憶しておいた領域についてしか、マニピュレータの干渉の有無を判定することができない。
【０００６】
そのため、このようなロボット装置の制御方法においては、作業領域がさまざまに刻々と変化する場合については、事前準備が極めて煩雑なものとなってしまう。
【０００７】
そこで、本発明は、前記の実情に鑑みてなされたものであり、その目的は、作業領域がさまざまに刻々と変化する場合においても、マニピュレータの作業領域への干渉を容易に回避し、停止時間を最小限に抑え、安全に作業を継続できるようになされたロボット装置の制御方法及びロボット装置を提供することにある。
【課題を解決するための手段】
【０００８】
前述の課題を解決し、前記目的を達成するため、本発明に係るロボット装置の制御方法は、以下の構成のいずれか一以上を有するものである。
【０００９】
〔構成１〕
複数のリンクにより構成されたマニピュレータを制御するにあたり、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの周囲の物体について外形を包絡する複数の球体を設定し、各リンクの動作に応じて各リンク及び周囲の物体について設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが周囲の物体に干渉するか否かの判断を行うことを特徴とするものである。
【００１０】
〔構成２〕
複数のリンクにより構成されたマニピュレータを制御するにあたり、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じてこれら各リンク及び作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うことを特徴とするものである。
【００１１】
〔構成３〕
複数のリンクにより構成されたマニピュレータを制御するにあたり、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンクを包絡する球体及び作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うことを特徴とするものである。
【００１２】
〔構成４〕
複数のリンクにより構成されたマニピュレータを制御するにあたり、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンク及び進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが進入禁止領域に干渉するか否かの判断を行うことを特徴とするものである。
【００１３】
また、本発明に係るロボット装置は、以下の構成のいずれか一以上を有するものである。
【００１４】
〔構成５〕
複数のリンクにより構成されたマニピュレータと、このマニピュレータを制御する制御手段とを備え、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの周囲の物体について外形を包絡する複数の球体を設定し、各リンク及び周囲の物体について設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが周囲の物体に干渉するか否かの判断を行うことを特徴とするものである。
【００１５】
〔構成６〕
複数のリンクにより構成されたマニピュレータと、このマニピュレータを制御する制御手段とを備え、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、各リンク及び作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うことを特徴とするものである。
【００１６】
〔構成７〕
複数のリンクにより構成されたマニピュレータと、このマニピュレータを制御する制御手段とを備え、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、各リンクを包絡する球体及び作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うことを特徴とするものである。
【００１７】
〔構成８〕
複数のリンクにより構成されたマニピュレータと、このマニピュレータを制御する制御手段とを備え、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、各リンク及び進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが進入禁止領域に干渉するか否かの判断を行うことを特徴とするものである。
【発明の効果】
【００１８】
本発明に係るロボット装置の制御方法においては、構成１を有することにより、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの周囲の物体について外形を包絡する複数の球体を設定し、各リンクの動作に応じて各リンク及び周囲の物体について設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが周囲の物体に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によってマニピュレータの周囲の物体への干渉の有無を事前に検出することができる。
【００１９】
本発明に係るロボット装置の制御方法においては、構成２を有することにより、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じてこれら各リンク及び作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって他のマニピュレータが作業を行っている作業領域への干渉の有無を事前に検出することができる。
【００２０】
本発明に係るロボット装置の制御方法においては、構成３を有することにより、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンクを包絡する球体及び作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって他のマニピュレータが作業を行っている作業領域への干渉の有無を事前に検出することができる。
【００２１】
本発明に係るロボット装置の制御方法においては、構成４を有することにより、制御対象となるマニピュレータを構成する各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンク及び進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが進入禁止領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって進入禁止領域への干渉の有無を事前に検出することができる。
【００２２】
また、本発明に係るロボット装置においては、構成５を有することにより、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの周囲の物体について外形を包絡する複数の球体を設定し、各リンク及び周囲の物体について設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが周囲の物体に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によってマニピュレータの周囲の物体への干渉の有無を事前に検出することができる。
【００２３】
本発明に係るロボット装置においては、構成６を有することにより、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、各リンク及び作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって他のマニピュレータが作業を行っている作業領域への干渉の有無を事前に検出することができる。
【００２４】
本発明に係るロボット装置においては、構成７を有することにより、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、各リンクを包絡する球体及び作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが作業領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって他のマニピュレータが作業を行っている作業領域への干渉の有無を事前に検出することができる。
【００２５】
本発明に係るロボット装置においては、構成８を有することにより、制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、各リンク及び進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンクが進入禁止領域に干渉するか否かの判断を行うので、マニピュレータのリンク数やリンクの形状に拘わらず、容易、かつ、迅速な演算によって進入禁止領域への干渉の有無を事前に検出することができる。
【００２６】
すなわち、本発明は、作業領域がさまざまに刻々と変化する場合においても、マニピュレータの作業領域への干渉を容易に回避できるようになされたロボット装置の制御方法及びロボット装置を提供することができるものである。
【発明を実施するための最良の形態】
【００２７】
以下、本発明を実施するための最良の形態について図面を参照して説明する。
【００２８】
〔ロボット装置の構成〕
図１は、本発明に係るロボット装置の構成を示す模式的な斜視図である。
【００２９】
本発明に係るロボット装置は、図１に示すように、少なくとも一台のマニピュレータ１，２を有している。この実施の形態においては、二台のマニピュレータ１，２が、互いの動作領域内に干渉して配置することが可能となされている。
【００３０】
各マニピュレータ１，２は、それぞれ、複数のアクチュエータ（駆動装置）とリンク（剛体の構造物）とによって構成されている。各リンク間は、回動（屈曲）可能、または、伸縮（並進）可能な関節、あるいは、旋回可能な関節３を介して接続されており、それぞれアクチュエータによって相対駆動されるようになっている。各アクチュエータは、制御手段によって制御される。
【００３１】
各マニピュレータ１，２において、第１のリンク（基端部のリンク）１ａ，２ａは、作業台１０１上に固定されており、この第１のリンク１ａ，２ａに、第２のリンク１ｂ，２ｂが接続されている。第２のリンク１ｂ，２ｂには第３のリンク１ｃ，２ｃが接続され、第３のリンク１ｃ，２ｃには第４のリンク１ｄ，２ｄが接続され、このようにして、順次、各リンクが接続されている。先端部のリンクの先端側（以下、「手先」という。）には、ワーク１０２を把持する把持機構４が設けられている。この把持機構４も、制御手段によって制御される。
【００３２】
これらマニピュレータ１，２においては、第１のリンク１ａ，２ａに対する第２のリンク１ｂ，２ｂの位置、第２のリンク１ｂ，２ｂに対する第３のリンク１ｃ，２ｃの位置、第３のリンク１ｃ，２ｃに対する第４のリンク１ｄ，２ｄの位置と、順次先端側のリンクの位置を制御してゆくことにより、手先の位置を制御することができ、この手先において把持機構４によって把持したワークの位置を制御することができる。
【００３３】
〔ロボット装置の制御方法〕
図２は、マニピュレータを構成する各リンクについて設定された外形を包絡する複数の球体を示す側面図である。
【００３４】
そして、このロボット装置においては、本発明に係るロボット装置の制御方法が実施される。すなわち、このロボット装置においては、図２に示すように、制御手段により、マニピュレータ１，２を構成する各リンク１ａ，２ａ，１ｂ，２ｂ・・・について、外形を包絡する複数の球体５ａ，５ｂ，５ｃ・・・５ｎが設定される。このように球体５ａ，５ｂ，５ｃ・・・５ｎを設定することにより、各リンク１ａ，２ａ，１ｂ，２ｂ・・・の動作に応じて各リンクについて設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク１ａ，２ａ，１ｂ，２ｂ・・・同士が衝突するか否かの判断を行うことができる。
【００３５】
球体５ａ，５ｂ，５ｃ・・・５ｎの数、位置及び半径等は、これら球体によってどの程度正確にリンク１ａ，２ａ，１ｂ，２ｂ・・・の形状を近似するか、また、安全余裕をどの程度にするかに応じて決定する。図２中の（ｂ）に示すように、１つのリンクに対して球体の数を増やすほど、図２中の（ａ）に示す球体の数が少ない場合に比較して、正確にリンクの形状を近似することができる。ただし、球体の数を増やすほど、各球体の中心間距離を求めるための計算量は増加する。
【００３６】
図３は、６自由度のマニピュレータの構成及び各リンクについて設定された外形を包絡する複数の球体を示す側面図である。
【００３７】
例えば、このロボット装置のマニピュレータが、図３中の（ａ）に示すように、６自由度のマニピュレータである場合には、図３中の（ｂ）に示すように、このマニピュレータの各リンクａ，ｂ，ｃ，ｄ，ｅ，ｆ，ｇについて、前述のように複数の球体５を設定することによって、マニピュレータの全体について、複数の球体５によって近似することができる。
【００３８】
このロボット装置の制御方法においては、各マニピュレータ１，２についてこのような球体の設定を行うとともに、ロボット装置の近傍に存在する障害物についても、球体の設定を行う。すなわち、ロボット装置の近傍に位置する障害物について、各種センサによる検出結果や予め与えられた情報に基づいて位置情報を得て、この障害物の外形を包絡する複数の球体を設定する。
【００３９】
そして、マニピュレータの各リンクの動作に応じて各リンクについて設定した各球体及び障害物について設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士及び各リンクと障害物とが衝突するか否かの判断を行うことができる。
【００４０】
すなわち、マニピュレータの制御中に、リアルタイムで各球体の中心間の距離を計算し、この距離が２つの球体の半径の和を下回った場合には、球体同士が干渉しリンク同士、または、リンクと障害物とが接触したと判断する。各球体は、マニピュレータの各リンク及び障害物の外形を覆うとともに、ある程度の余裕量を持つように、位置及び大きさが設定されているので、球体同士が干渉しなければ、リンク同士及びリンクと障害物とは、衝突していないと判断することができる。
【００４１】
図４は、設定された球体の中心間の距離の求め方を説明する図である。
【００４２】
設定された球体の中心間の距離を求めるにあたり、例えば、図４に示すように、半径ｒ１の球体Ｓ１及び半径ｒ２の球体Ｓ２について、Ｓ１の中心位置を（ｐｘ１、ｐｙ１、ｐｚ１）、Ｓ２の中心位置を（ｐｘ２、ｐｙ２、ｐｚ２）とし、これら２つの球体の中心間の距離をＲとする。この場合、以下の（式１）が成立する。
Ｒ２＝（ｐｘ１−ｐｘ２）２＋（ｐｙ１−ｐｙ２）２＋（ｐｚ１−ｐｚ２）２・・（式１）
【００４３】
したがって、これら２つの球体が干渉しているかどうかは、以下の（式２）が満足されるかどうかによって確認することができる。
Ｒ２＜（ｒ１＋ｒ２）２ ・・・（式２）
【００４４】
この処理をすべての球体の組合せについて行うことによって、各球体間の干渉の有無を判断することができる。このようにマニピュレータの各リンクについて球体を設定することによって、球体の中心間距離の計算のみによって、球体同士の干渉の有無により、各リンクの衝突の有無の判断を行うことができる。すなわち、各リンクを円筒や平面を用いて表現した場合に比較して、方向（法線）や端点処理が不要となり、簡単な計算によって判断することができる。
【００４５】
ただし、同一のリンクについて設定された球体同士は、それぞれの半径を下回って配置されていても、接触と判断すべきではない。そこで、以下の方策に従い、衝突確認を行う必要がない球体の組を決定しておくことにより、必要な計算量を減らすことができる。
【００４６】
図５は、マニピュレータの所定のリンクの構成を示す側面図である。
【００４７】
このロボット装置においては、図５に示すように、セグメントを定義する。このセグメントは、一の屈曲（回転）または並進関節から、マニピュレータの先端側への次の屈曲（回転）または並進関節までの間の部分をいう。一の屈曲（回転）または並進関節は、次の屈曲（回転）または並進関節の位置を変化させる機能を有する。ここで、セグメントの定義においては、旋回関節（次の関節をその場で回転させるだけで次の関節の位置を変えない関節）は、除外して考える。すなわち、旋回関節で接続された複数のリンクは、同一のセグメントとなる。
【００４８】
したがって、図５に示すように、屈曲関節ｑ１から屈曲関節ｑ３までのリンクＬ１及びリンクＬ２が、同一のセグメントＳＧ１と定義される。リンクＬ１及びリンクＬ２は、旋回関節ｑ２によって接続されている。
【００４９】
このロボット装置において、各リンクについて設定される球体は、そのリンクが属するセグメント上に設定されていることになる。なお、障害物についても、１つ、または、２つ以上のセグメントとして定義し、球体を設定する。同一のセグメント上では、球体同士が干渉していても衝突が生じているわけではないので、同一のセグメント上の球体同士では、干渉が生じているかどうかの判別を行う必要がない。
【００５０】
また、このロボット装置においては、衝突が生じ得ない２つのセグメント上に設定された球体同士は、干渉が生じているかどうかの判別を行う必要がない。
【００５１】
図６は、接触が生じ得ないセグメントを示す側面図である。
【００５２】
このロボット装置において、接触が生じ得ないセグメントとは、図６に示すように、屈曲（回転）または並進関節ｑを介して隣り合ったセグメントである。すなわち、図６における「セグメント１」及び「セグメント２」、「セグメント２」及び「セグメント３」、そして、「セグメント４」及び「セグメント５」は、屈曲（回転）または並進関節ｑによって接続されているため、屈曲（回転）または並進関節ｑの可動範囲を制限したり制御することによって互いの接触を回避することができ、これら２つのセグメント上に設定された球体同士について干渉が生じているかどうかの判別を行う必要がない。
【００５３】
また、物理的に距離が離れており、互いの可動範囲内にないセグメント、すなわち、図６における「セグメント２」及び「セグメント４」は、可動範囲内でどのように駆動されても接触が生じ得ないので、これら２つのセグメント上に設定された球体同士について干渉が生じているかどうかの判別を行う必要がない。
【００５４】
さらに、作業台１０１上に固定された２つのセグメント、すなわち、図６における「セグメント１」及び「セグメント４」は、移動されることがなく接触が生じ得ないので、これら２つのセグメント上に設定された球体同士について干渉が生じているかどうかの判別を行う必要がない。
【００５５】
ところで、セグメント上に設定された複数の球体の位置を算出するには、一般的なマニピュレータの制御中に得られる順運動学の結果を用いることができる。ある関節の基準座標に対する位置及び姿勢は、一般的な順運動学により、以下の（式３）、すなわち、４×４の同次変換行列によって表現することができる。
【００５６】
【数１】
・・・（式３）
【００５７】
ここで、（ｘ、ｙ、ｚ）は、その関節における座標系を表す互いに直交した単位３次元ベクトルであり、その関節の姿勢を表す。ｐは、基準座標におけるその関節の位置を表す３次元ベクトルである。これらの値は、順運動学により、手先の位置及び姿勢を計算する過程で得られるものである。この関節から、この関節における座標系の（ｘ、ｙ、ｚ）軸方向にそれぞれ（ｌｘ、ｌｙ、ｌｚ）移動した点の座標ｐ′は、以下の（式４）で示すことができる。
ｐ′＝ｐ＋ｌｘ・ｘ＋ｌｙ・ｙ＋ｌｚ・ｚ ・・・（式４）
【００５８】
再び、図５に示した例を考えると、屈曲関節ｑ１の中心位置をｐ１、ｐ１における座標、すなわち、リンクＬ１の姿勢を（ｘ１、ｙ１、ｚ１）とし、旋回関節ｑ２の中心位置をｐ２、ｐ２における座標、すなわち、リンクＬ２の姿勢を（ｘ２、ｙ２、ｚ２）とすると、これらの値は、順運動学を計算する過程で求められる。
【００５９】
屈曲関節ｑ１からリンクＬ１の長手方向、すなわち、ｚ１方向に長さｌ１ｚ離れた位置に球体の中心Ｓ１がある場合には、Ｓ１の位置は、以下の（式５）によって示される。
Ｓ１＝ｐ１＋ｌ１ｚ・ｚ１ ・・・（式５）
【００６０】
同様に、旋回関節ｑ２からリンクＬ２の長手方向、すなわち、ｚ２方向に長さｌ２ｚ離れ、ｙ２方向に長さｌ２ｙ離れた位置に球体の中心Ｓ２がある場合には、Ｓ２の位置は、以下の（式６）によって示される。
Ｓ２＝ｐ２＋ｌ２ｚ・ｚ２＋ｌ２ｙ・ｙ２ ・・・（式６）
【００６１】
このように、このロボット装置においては、一般的なマニピュレータの制御の途中で計算されている結果を用いて球体の位置を求めることができるので、球体の位置計算のために生ずる計算量の増加は、微小なものである。
【００６２】
図７は、ワークの取り扱いを示す側面図である。
【００６３】
このロボット装置において、図７に示すように、マニピュレータの把持機構４がワーク１０２を把持するときに、図７中（ａ）に示すように、ワーク１０２を把持する前には衝突が生じなかった動作でも、図７中（ｂ）に示すように、ワーク１０２を把持した後には、衝突が生じることがある。このような場合には、ワーク１０２についても球体を設定しておくことにより、ワーク１０２も含めた接触検知を行うことができる。
【００６４】
この場合、把持機構４がワーク１０２を把持した状態に応じて、セグメントに対する球体の設定の追加及び除去を動的に行ってもよいが、把持機構４の位置に半径ゼロ、または、負の値の半径の球体ＷＳを予め設定しておき、把持機構４がワーク１０２を把持したときには、把持機構４の位置の球体ＷＳの半径を拡大させつつ位置を移動することで、この球体ＷＳによりワーク１０２の外形を包絡させるようにしてもよい。このような扱いとすることにより、処理が容易になる。
【００６５】
図７中（ａ）に示すように、把持機構４の位置の球体ＷＳの半径をゼロ、または、負の値としておくことにより、他の任意の球体との干渉の有無の判定を行っても、干渉が検知されることはない。ワーク１０２を把持したときには、図７中（ｂ）に示すように、球体ＷＳの半径をｒｗに拡大させるとともに、オフセットＬＯｗを与えて球体ＷＳを移動させることにより、ワーク１０２の外形を包絡させる。この状態において、球体ＷＳに干渉する可能性がある他の球体との干渉の有無の判別を行うことができる。把持機構４がワーク１０２を解放したときには、再び、球体ＷＳの半径をゼロ、または、負の値にし、位置を手先位置などの基準位置に戻す。
【００６６】
なお、球体ＷＳの半径ｒｗ及びオフセットＬＯｗは、ワーク１０２の形状及び大きさに応じて予め設定しておき、動作情報などに基づいて、取得するようにすることができる。さらに、カメラ、レーザ計測器やその他のセンサを使って計測することにより、人や一時的に置かれた治具など、予め情報が取得されていないものについても、検出されたときに球体を設定し、干渉有無の判別対象とすることができる。
【００６７】
そして、このロボット装置の制御方法においては、一つのマニピュレータがこれから作業を行う空間を作業領域とし、この作業領域に他のマニピュレータが進入しないように制御することができる。
【００６８】
なお、本発明に係るロボット装置においては、各セグメント上、または、ワーク１０２上に設定される各球体の半径は、各リンクの動作速度に応じて、動的に変化させるようにしてもよい。例えば、リンクの動作速度が速い場合には、このリンクが属するセグメント上に設定される各球体の半径を拡大することにより、このリンクが他のリンクに衝突することをより確実に防止することができる。また、リンクの動作速度が遅い場合には、このリンクが属するセグメント上に設定される各球体の半径を縮小することにより、このリンクが他のリンクに衝突することを防止しつつ、このリンクの可動範囲を広くすることができる。
【００６９】
図８は、マニピュレータと作業領域との位置関係を示す平面図である。
【００７０】
すなわち、図８に示すように、１つのマニピュレータ１が第１の作業点Ｐ１に対して作業を始めたときには、第１の作業点Ｐ１を包含する第１の作業領域Ｚ１を設定し、他のマニピュレータ２は、第１の作業領域Ｚ１及び作業中のマニピュレータ１に干渉しない空間内において移動する。マニピュレータ１の作業の終了後は、第１の作業領域Ｚ１への進入禁止を解除し、他のマニピュレータ２も自由に動ける空間となる。
【００７１】
このようにロボット装置を制御するには、前述したように、各ワーク１０２におけるそれぞれの作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６に対して、これらを包絡する球体状の作業領域Ｚ１，Ｚ２，Ｚ３，・・・を設定する。各マニピュレータ１，２についても、前述したように、それぞれの外形を包絡する複数の球体を設定する。そして、作業領域Ｚ１，Ｚ２，Ｚ３，・・・と各マニピュレータ１，２について設定された各球体の中心間距離を計算することにより、作業領域とマニピュレータ１，２との接触、マニピュレータ１，２同士の接触を検出することができる。
【００７２】
一方のマニピュレータ１が第１の作業点Ｐ１に対して作業を行っている間は、マニピュレータ１が第１の作業領域Ｚ１と接触し、第１の作業領域Ｚ１は、一方のマニピュレータ１の占有領域となる。このとき、他方のマニピュレータ２は、第２の作業点Ｐ２に対して作業を行うと、一方のマニピュレータ１が占有している第１の作業領域Ｚ１に接触してしまうため、第２の作業点Ｐ２に対する作業を行うことができない。他方のマニピュレータ２は、第３の作業点Ｐ３に対して作業を行っても、第１の作業領域Ｚ１には接触しないため、第３の作業点Ｐ３に対する作業を行うことが可能である。そこで、他方のマニピュレータ２は、第３の作業点Ｐ３に対する作業を行う。このようにして、各マニピュレータ１，２の全体として、作業を効率的に継続することができる。
【００７３】
また、他方のマニピュレータ２が第４の作業点Ｐ４に対する作業を行う場合、他方のマニピュレータ２が単独で作業を行うのであれば、他方のマニピュレータ２は、第４の作業点Ｐ４へまっすぐ接近して作業を行えばよい。しかし、他方のマニピュレータ２の移動経路に第１の作業領域Ｚ１が重なっており、一方のマニピュレータ１が第１の作業点Ｐ１に対する作業を行っている場合には、他方のマニピュレータ２は、一方のマニピュレータ１が占有している第１の作業領域Ｚ１に接触してしまう。
【００７４】
このような場合には、他方のマニピュレータ２は、第４の作業点Ｐ４へまっすぐ接近することができないので、マニピュレータ１が占有している第１の作業領域Ｚ１と他方のマニピュレータ２について設定された球体とが接触しないように、姿勢を調整しながら第４の作業点Ｐ４に接近して、第４の作業点Ｐ４に対する作業を行う。
【００７５】
なお、従来のロボット装置の制御方法においては、このような場合には、他のマニピュレータの動作パターンすべてに対応する回避動作をあらかじめ教示しておく必要があった。これに対し、本発明においては、基本的な作業のみを予め教示しておき、作業進行中には、リアルタイムで球体同士の中心間距離を計算することにより、互いの作業領域に接触しないように姿勢を調整することが可能になっている。
【００７６】
このようにして、本発明においては、各作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６ごとに作業領域Ｚ１，Ｚ２，Ｚ３，・・・を設定し、各作業領域Ｚ１，Ｚ２，Ｚ３，・・・と各マニピュレータ１，２との干渉を監視することにより、複数のマニピュレータ１，２が安全を保ちつつ同時に作業を継続することが可能となり、作業を効率的に実行することができる。
【００７７】
また、ロボット装置の制御においては、ワークの有無に係らず、ある空間を進入禁止領域としたいことがある。このような場合には、進入禁止領域の境界面に沿って、多数の球体を壁状に並べ、これら球体に対する干渉を監視することにより、マニピュレータが進入禁止領域に進入することを防止することができる。
【００７８】
また、進入禁止領域の境界面に近似する球面を有する大きな半径の球体を設定するようにすれば、一つの球体の設定により、進入禁止領域の境界面に近似した壁を設定することができる。この一つの球体に対する干渉を監視することにより、マニピュレータが進入禁止領域に進入することを防止することができる。この場合には、多数の球体を設定する場合に比較して、計算量を増やすことなく、進入禁止領域との干渉を監視することができる。
【００７９】
さらに、各作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６が十分に離れており、かつ、一つの作業点に対して複数のマニピュレータ１，２が交互に作業を行わなければならない場合には、ある作業点に対して作業が行われているという事実のみが判別できればよい。この場合には、各作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６について所定の大きさを有する作業領域Ｚ１，Ｚ２，Ｚ３，・・・を設定する必要はなく、各作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６のみが判別されればよい。
【００８０】
例えば、第１乃至第３の作業点Ｐ１，Ｐ２，Ｐ３が十分に離れていて、第２の作業点Ｐ２に対して、各マニピュレータ１，２が交互に作業を行う場合には、一方のマニピュレータ１が第２の作業点Ｐ２で作業をしているときには、他方のマニピュレータ２は、第３の作業点Ｐ３に対する作業を行うか、または、待機する。この場合には、各作業領域Ｚ１，Ｚ２，Ｚ３，・・・を半径０の球体として設定すればよい。干渉の有無についての計算方法は、前述したものと同様である。
【００８１】
図９は、本発明に係るロボット装置の制御方法を示すフローチャートである。
【００８２】
このロボット装置の制御方法においては、図９に示すように、事前の処理、設計として、ステップｓｔ１でマニピュレータの各リンクについて球体を設定し（リンクの近似）、ステップｓｔ２で障害物について球体を設定し（障害物の近似）、ステップｓｔ３でワークについて球体を設定し（ワークの近似）、ワークの形状及び大きさに応じた球体の半径及びオフセットも設定しておく。さらに、ステップｓｔ４で、セグメントの設定（分類）を行っておく。
【００８３】
そして、ロボット装置の動作が開始されると、オンライン制御中の処理に入り、ステップｓｔ５で任意の２つのセグメントを選択し、ステップｓｔ６で、これらセグメントが接触する可能性があるかを判別する。接触する可能性がなければ、ステップｓｔ５に戻り、他の２つのセグメントを選択して、ステップｓｔ６に進む。ステップｓｔ６で、２つのセグメントが接触する可能性があれば、ステップｓｔ７に進み、接触する可能性のある２つのセグメント上に設定された球体から、各１つの球体を選択し、ステップｓｔ８に進む。
【００８４】
ステップｓｔ８では、接触する可能性のある２つのセグメント上に設定された球体の全てについて干渉の有無が判別されたかを判断し、全ての球体について判別されていれば、ステップｓｔ５に戻り、全ての球体について判別されていなければ、ステップｓｔ９に進む。
【００８５】
ステップｓｔ９では、選択された２つの球体について、中心間の距離を計算し、ステップｓｔ１０に進み、球体の中心間距離がこれら球体の半径の合計よりも小さいかどうかを判別する。球体の中心間距離が半径の合計よりも大きければ、ステップｓｔ７に戻り、球体の中心間距離が半径の合計よりも小さければ、ステップｓｔ１１に進む。ステップｓｔ１１では、ステップｓｔ１０で選択されていた２つの球体に対応するリンク同士、リンクと障害物、または、ワークと障害物の衝突が生ずることを判別して、マニピュレータの動作を停止させるなどの所定の処理を行う。
【００８６】
図１０は、本発明に係るロボット装置の制御方法において作業領域を設定する場合のフローチャートである。
【００８７】
このロボット装置の制御方法において、作業領域を設定する場合には、図１０に示すように、ステップｓｔ２１で動作を開始すると、ステップｓｔ２２に進み、各マニピュレータ１，２が単独で各作業点Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６に対して作業を行う場合の作業手順を教示し、ステップｓｔ２３に進む。ステップｓｔ２３では、作業を行う作業点を選択して、ステップｓｔ２４に進む。ステップｓｔ２４では、選択された作業点において他のマニピュレータによる作業が行われているか否かを判別し、他のマニピュレータによる作業が行われていればステップｓｔ２５に進み、他のマニピュレータによる作業が行われていなければステップｓｔ２６に進む。ステップｓｔ２５では、他の作業点を選択して、ステップｓｔ２４に戻る。
【００８８】
ステップｓｔ２６では、他のマニピュレータが占有している作業領域や進入禁止領域との干渉の有無を判別し、干渉がなければステップｓｔ２７に進み、干渉があればステップｓｔ２８に進む。ステップｓｔ２８では、姿勢を調整することによって干渉を回避できるか否かを判別し、干渉を回避できればステップｓｔ２９に進み、干渉を回避できなければステップｓｔ３０に進む。ステップｓｔ２９では、姿勢を調整して、ステップｓｔ２７に進む。
【００８９】
ステップｓｔ３０では、作業点を変更することによって干渉を回避できるか否かを判別し、干渉を回避できればステップｓｔ３１に進み、干渉を回避できなければステップｓｔ３２に進む。ステップｓｔ３１では、作業点を変更して、ステップｓｔ２７に進む。
【００９０】
ステップｓｔ３２では、他のマニピュレータによる作業を終了することを待ち、ステップｓｔ２６に戻る。
【００９１】
ステップｓｔ２７では、選択された作業点に対して作業領域（占有領域）を設定し、ステップｓｔ３３に進む。ステップｓｔ３３では、設定された作業領域内において、作業点に対する作業を実行し、ステップｓｔ３４に進む。ステップｓｔ３４では、全ての作業が終了したか否かを判別し、全ての作業が終了していればステップｓｔ３５に進んで動作を終了し、全ての作業が終了していなければステップｓｔ２３に戻る。
【図面の簡単な説明】
【００９２】
【図１】本発明に係るロボット装置の構成を示す模式的な斜視図である。
【図２】本発明に係るロボット装置においてマニピュレータを構成する各リンクについて設定された外形を包絡する複数の球体を示す側面図である。
【図３】本発明に係るロボット装置におけるマニピュレータの例である６自由度のマニピュレータの構成及び各リンクについて設定された外形を包絡する複数の球体を示す側面図である。
【図４】本発明に係るロボット装置における設定された各球体の中心間の距離の求め方を説明する図である。
【図５】本発明に係るロボット装置におけるマニピュレータの所定のリンクの構成を示す側面図である。
【図６】本発明に係るロボット装置において接触が生じ得ないセグメントを示す側面図である。
【図７】本発明に係るロボット装置の制御方法におけるワークの取り扱いを示す側面図である。
【図８】本発明に係るロボット装置の制御方法におけるマニピュレータと作業領域との位置関係を示す平面図である。
【図９】本発明に係るロボット装置の制御方法を示すフローチャートである。
【図１０】本発明に係るロボット装置の制御方法において作業領域を設定する場合のフローチャートである。
【符号の説明】
【００９３】
１，２ マニピュレータ
３ 関節
４ 把持部
５ 球体
１０１ 作業台
１０２ ワーク
Ｐ１，Ｐ２，Ｐ３，Ｐ４，Ｐ５，Ｐ６ 作業点
Ｚ１，Ｚ２，Ｚ３ 作業領域
【特許請求の範囲】
【請求項１】
複数のリンクにより構成されたマニピュレータを制御するにあたり、
制御対象となるマニピュレータを構成する各リンクについて、外形を包絡する複数の球体を設定し、
前記マニピュレータの周囲の物体について、外形を包絡する複数の球体を設定し、
前記各リンクの動作に応じて、前記各リンク及び前記周囲の物体について設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記周囲の物体に干渉するか否かの判断を行う
ことを特徴とするロボット装置の制御方法。
【請求項２】
複数のリンクにより構成されたマニピュレータを制御するにあたり、
制御対象となるマニピュレータを構成する各リンクについて、外形を包絡する複数の球体を設定し、
他のマニピュレータが作業を行っているときには、当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、
制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンク及び前記作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記作業領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置の制御方法。
【請求項３】
複数のリンクにより構成されたマニピュレータを制御するにあたり、
制御対象となるマニピュレータを構成する各リンクについて、外形を包絡する複数の球体を設定し、
他のマニピュレータが作業を行っているときには、当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、
制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンクを包絡する球体及び前記作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記作業領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置の制御方法。
【請求項４】
複数のリンクにより構成されたマニピュレータを制御するにあたり、
制御対象となるマニピュレータを構成する各リンクについて、外形を包絡する複数の球体を設定し、
前記マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、
制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンク及び前記進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記進入禁止領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置の制御方法。
【請求項５】
複数のリンクにより構成されたマニピュレータと、
前記マニピュレータを制御する制御手段と
を備え、
前記制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、前記各リンクについて外形を包絡する複数の球体を設定し、前記マニピュレータの周囲の物体について外形を包絡する複数の球体を設定し、前記各リンク及び前記周囲の物体について設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記周囲の物体に干渉するか否かの判断を行う
ことを特徴とするロボット装置。
【請求項６】
複数のリンクにより構成されたマニピュレータと、
前記マニピュレータを制御する制御手段と
を備え、
前記制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、前記各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、前記各リンク及び前記作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記作業領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置。
【請求項７】
複数のリンクにより構成されたマニピュレータと、
前記マニピュレータを制御する制御手段と
を備え、
前記制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、前記各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、前記各リンクを包絡する球体及び前記作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記作業領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置。
【請求項８】
複数のリンクにより構成されたマニピュレータと、
前記マニピュレータを制御する制御手段と
を備え、
前記制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、前記各リンクについて外形を包絡する複数の球体を設定し、前記マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、前記各リンク及び前記進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記進入禁止領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置。
【請求項１】
複数のリンクにより構成されたマニピュレータを制御するにあたり、
制御対象となるマニピュレータを構成する各リンクについて、外形を包絡する複数の球体を設定し、
前記マニピュレータの周囲の物体について、外形を包絡する複数の球体を設定し、
前記各リンクの動作に応じて、前記各リンク及び前記周囲の物体について設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記周囲の物体に干渉するか否かの判断を行う
ことを特徴とするロボット装置の制御方法。
【請求項２】
複数のリンクにより構成されたマニピュレータを制御するにあたり、
制御対象となるマニピュレータを構成する各リンクについて、外形を包絡する複数の球体を設定し、
他のマニピュレータが作業を行っているときには、当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、
制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンク及び前記作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記作業領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置の制御方法。
【請求項３】
複数のリンクにより構成されたマニピュレータを制御するにあたり、
制御対象となるマニピュレータを構成する各リンクについて、外形を包絡する複数の球体を設定し、
他のマニピュレータが作業を行っているときには、当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、
制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンクを包絡する球体及び前記作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記作業領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置の制御方法。
【請求項４】
複数のリンクにより構成されたマニピュレータを制御するにあたり、
制御対象となるマニピュレータを構成する各リンクについて、外形を包絡する複数の球体を設定し、
前記マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、
制御対象となるマニピュレータを構成する各リンクの動作に応じて、これら各リンク及び前記進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記進入禁止領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置の制御方法。
【請求項５】
複数のリンクにより構成されたマニピュレータと、
前記マニピュレータを制御する制御手段と
を備え、
前記制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、前記各リンクについて外形を包絡する複数の球体を設定し、前記マニピュレータの周囲の物体について外形を包絡する複数の球体を設定し、前記各リンク及び前記周囲の物体について設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記周囲の物体に干渉するか否かの判断を行う
ことを特徴とするロボット装置。
【請求項６】
複数のリンクにより構成されたマニピュレータと、
前記マニピュレータを制御する制御手段と
を備え、
前記制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、前記各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域を包絡する球体を設定し、前記各リンク及び前記作業領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記作業領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置。
【請求項７】
複数のリンクにより構成されたマニピュレータと、
前記マニピュレータを制御する制御手段と
を備え、
前記制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、前記各リンクについて外形を包絡する複数の球体を設定し、他のマニピュレータが作業を行っているときには当該他のマニピュレータ及び作業対象物を含む作業領域の中心に半径が０の球体を設定し、前記各リンクを包絡する球体及び前記作業領域の中心の球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記作業領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置。
【請求項８】
複数のリンクにより構成されたマニピュレータと、
前記マニピュレータを制御する制御手段と
を備え、
前記制御手段は、制御対象となるマニピュレータを構成する各リンクを動作させるとともに、前記各リンクについて外形を包絡する複数の球体を設定し、前記マニピュレータの進入が禁止される進入禁止領域を包絡する一、または、複数の球体を設定し、前記各リンク及び前記進入禁止領域を包絡する球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンクが前記進入禁止領域に干渉するか否かの判断を行う
ことを特徴とするロボット装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２００９−２３３７５７（Ｐ２００９−２３３７５７Ａ）
【公開日】平成２１年１０月１５日（２００９．１０．１５）
【国際特許分類】
【出願番号】特願２００８−７９１１７（Ｐ２００８−７９１１７）
【出願日】平成２０年３月２５日（２００８．３．２５）
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【出願人】（５０４１５７０２４）国立大学法人東北大学 (2,297)
【Ｆターム（参考）】
【公開日】平成２１年１０月１５日（２００９．１０．１５）
【国際特許分類】
【出願日】平成２０年３月２５日（２００８．３．２５）
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【出願人】（５０４１５７０２４）国立大学法人東北大学 (2,297)
【Ｆターム（参考）】
[ Back to top ]