JP5250858B2 - 複数ロボットの衝突検知方法及びロボット装置 - Google Patents

Description

本発明は、互いの動作領域内で動作する複数のロボットマニピュレータ間の衝突を事前に検出するための複数ロボットの衝突検知方法に関し、また、複数のロボットマニピュレータを有しこれらロボットマニピュレータ間の衝突を事前に検出するようになされたロボット装置に関する。

従来、互いの動作領域内で動作する複数のロボットマニピュレータを制御するにあたっては、各ロボットマニピュレータ間の衝突や障害物への衝突を防止するため、各ロボットマニピュレータが他のロボットマニピュレータや障害物に衝突しないように、一のロボットマニピュレータの動作中には、他のロボットマニピュレータがその動作領域に入らないように教示したり、あるいは、衝突しないような教示を行っている。

また、特許文献１には、１台のロボットマニピュレータが障害物と衝突するかどうかを調べるため、ロボットマニピュレータの各リンクを直方体、円筒、または、球体で近似し、これら近似形状と障害物とが接触するかどうかの計算を行う方法が記載されている。

そして、特許文献２には、２台のロボットマニピュレータが互いに衝突するかどうかを調べるため、ロボットマニピュレータの各リンクを三次元サーフェスモデル（複数の平面で囲まれた多面体）で近似し、面同士の距離を計算・保持して接触判定を行う方法が記載されている。

さらに、特許文献３には、人間の腕の形状に構成された２台のロボットマニピュレータが互いに衝突するかどうかを調べるため、上腕部及び下腕部をそれぞれ円筒で近似し、手先を球体で近似して、１台のロボットマニピュレータをこれら３つの形状で近似し、３×３＝９通りのそれぞれについて距離計算を行い、接触の有無を判定する条件を求める方法が記載されている。

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

ところで、複数ロボットマニピュレータを互いの動作領域内に配置する場合には、これら各ロボットマニピュレータが協調して動作することが予定されている場合が多い。したがって、従来のロボットマニピュレータの制御のように、各ロボットマニピュレータを互いに干渉しない範囲で動作させるのでは、予定した動作が実行できないことになる。また、各ロボットマニピュレータが互いに衝突しないように教示をすることは困難であり、教示中の操作ミスによって衝突してしまう虞もある。

特許文献１に記載された技術は、１台のロボットマニピュレータを制御するための技術であり、また、各リンクを直方体（面）や円筒などによって近似することは、各形状の方向を考慮する必要があるため、距離計算のための演算量が多く、迅速な判断が行えない。

特許文献２に記載された技術は、２台のロボットマニピュレータを制御するための技術であるが、多数の面同士の距離計算を行わなければならず、面の方向も考慮する必要があるため、距離計算のための演算量が多く、迅速な判断が行えない。さらに、容量の大きな記憶装置が必要となり、サーフェスの定義が難しいという問題もある。

特許文献３に記載された技術は、適用対象を人型ロボットなど人間の両腕を模したロボットマニピュレータに限定した技術であるため、リンクの形状が大きな凹凸やオフセットを含むものであって人間の腕の形状からはずれた形状である場合には、不必要に大きな円筒で覆った状態に近似せざるを得ないため保守的になりすぎ、また、近傍に障害物がある場合や、ロボットマニピュレータの台数やリンク数が増えた場合への適用は困難である。さらに、円筒同士の距離計算は、円筒の方向を考慮する必要があるため、演算量が多く、迅速な判断が行えない。

そこで、本発明は、前記の実情に鑑みてなされたものであり、その目的は、ロボットマニピュレータの台数やリンク数に拘わらず、また、リンクの形状に拘わらず、容易、かつ、迅速な演算によってリンク同士及び障害物との衝突の有無を事前に検出でき、各ロボットマニピュレータの協調した動作を実現することができる複数ロボットの衝突検知方法及びロボット装置を提供することにある。

前述の課題を解決し、前記目的を達成するため、本発明に係る複数ロボットの衝突検知方法は、以下の構成のいずれか一を有するものである。

〔構成１〕
それぞれが複数のリンクにより構成された複数のロボットマニピュレータを制御するにあたり、各リンクについて、外形を包絡する複数の球体を設定し、各リンクの動作に応じて、各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出し、ロボットマニピュレータの手先位置に半径ゼロの球体を予め設定しておき、ロボットマニピュレータがワークを把持していないときには、手先位置の球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士が衝突するか否かの判断を行い、ロボットマニピュレータがワークを把持したときには、手先位置の球体の半径を拡大させ、または位置の変更と半径の拡大の両方を行うことによって、この球体によりワークの外形を包絡させ、この球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士及びワークが衝突するか否かの判断を行うことを特徴とするものである。

〔構成２〕
構成１を有する複数ロボットの衝突検知方法において、ロボットマニピュレータの近傍の障害物の位置情報を得て、この障害物の外形を包絡する複数の球体を設定し、各球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士、ワーク及び障害物が衝突するか否かの判断を行うことを特徴とするものである。

また、本発明に係るロボット装置は、以下の構成のいずれか一を有するものである。

〔構成３〕
それぞれが複数のリンクにより構成された複数のロボットマニピュレータと、各ロボットマニピュレータを制御する制御手段とを備え、制御手段は、各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、各リンクの動作に応じて各リンクについて設定された各球体の中心間距離の変化を計算し、ロボットマニピュレータの手先位置に半径ゼロの球体を予め設定しておき、ロボットマニピュレータがワークを把持させていないときには、手先位置の球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士が衝突するか否かの判断を行い、ロボットマニピュレータにワークを把持させたときには、手先位置の球体の半径を拡大させ、または位置の変更と半径の拡大の両方を行うことによって、この球体によりワークの外形を包絡させ、この球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士及びワークが衝突するか否かの判断を行うことを特徴とするものである。

〔構成４〕
構成３を有するロボット装置において、制御手段は、ロボットマニピュレータの近傍の障害物の位置情報を得て、この障害物の外形を包絡する複数の球体を設定し、各球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士、ワーク及び障害物が衝突するか否かの判断を行うことを特徴とするものである。

本発明に係る複数ロボットの衝突検知方法においては、構成１を有することにより、それぞれが複数のリンクにより構成された複数のロボットマニピュレータを制御するにあたり、各リンクについて、外形を包絡する複数の球体を設定し、各リンクの動作に応じて、各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出し、ロボットマニピュレータの手先位置に半径ゼロの球体を予め設定しておき、ロボットマニピュレータがワークを把持していないときには、手先位置の球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士が衝突するか否かの判断を行い、ロボットマニピュレータがワークを把持したときには、手先位置の球体の半径を拡大させ、または位置の変更と半径の拡大の両方を行うことによって、この球体によりワークの外形を包絡させ、この球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士及びワークが衝突するか否かの判断を行うので、ロボットマニピュレータの台数やリンク数に拘わらず、また、リンクの形状に拘わらず、容易、かつ、迅速な演算によってリンク同士の衝突の有無を事前に検出でき、各ロボットマニピュレータの協調した動作を実現することができ、また、容易、かつ、迅速な演算によって、リンク同士及びワークとの衝突の有無を事前に検出することができる。

本発明に係る複数ロボットの衝突検知方法においては、構成２を有することにより、ロボットマニピュレータの近傍の障害物の位置情報を得て、この障害物の外形を包絡する複数の球体を設定し、各球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士、ワーク及び障害物が衝突するか否かの判断を行うので、容易、かつ、迅速な演算によって、リンク同士、ワーク及び障害物との衝突の有無を事前に検出することができる。

本発明に係るロボット装置においては、構成３を有することにより、制御手段は、各リンクを動作させるとともに、各リンクについて外形を包絡する複数の球体を設定し、各リンクの動作に応じて各リンクについて設定された各球体の中心間距離の変化を計算し、ロボットマニピュレータの手先位置に半径ゼロの球体を予め設定しておき、ロボットマニピュレータがワークを把持させていないときには、手先位置の球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士が衝突するか否かの判断を行い、ロボットマニピュレータにワークを把持させたときには、手先位置の球体の半径を拡大させ、または位置の変更と半径の拡大の両方を行うことによって、この球体によりワークの外形を包絡させ、この球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士及びワークが衝突するか否かの判断を行うので、容易、かつ、迅速な演算によって、リンク同士及びワークとの衝突の有無を事前に検出することができる。

本発明に係るロボット装置においては、構成４を有することにより、制御手段は、ロボットマニピュレータの近傍の障害物の位置情報を得て、この障害物の外形を包絡する複数の球体を設定し、各球体及び各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士、ワーク及び障害物が衝突するか否かの判断を行うので、容易、かつ、迅速な演算によって、リンク同士、ワーク及び障害物との衝突の有無を事前に検出することができる。

すなわち、本発明は、ロボットマニピュレータの台数やリンク数に拘わらず、また、リンクの形状に拘わらず、容易、かつ、迅速な演算によってリンク同士及び障害物との衝突の有無を事前に検出でき、各ロボットマニピュレータの協調した動作を実現することができる複数ロボットの衝突検知方法及びロボット装置を提供することができるものである。

以下、本発明を実施するための最良の形態について図面を参照して説明する。

〔ロボット装置の構成〕
図１は、本発明に係るロボット装置の構成を示す模式的な斜視図である。

本発明に係るロボット装置は、図１に示すように、複数のロボットマニピュレータ１，２を有し、これらロボットマニピュレータ１，２が互いの動作領域内に干渉して配置することが可能となされているものである。

各ロボットマニピュレータ１，２は、それぞれ、複数のアクチュエータ（駆動装置）とリンク（剛体の構造物）とによって構成されている。各リンク間は、回動（屈曲）可能、または、伸縮（並進）可能な関節、あるいは、旋回可能な関節３を介して接続されており、それぞれアクチュエータによって相対駆動されるようになっている。各アクチュエータは、制御手段によって制御される。

各ロボットマニピュレータ１，２において、第１のリンク（基端部のリンク）１ａ，２ａは、作業台１０１上に固定されており、この第１のリンク１ａ，２ａに、第２のリンク１ｂ，２ｂが接続されている。第２のリンク１ｂ，２ｂには第３のリンク１ｃ，２ｃが接続され、第３のリンク１ｃ，２ｃには第４のリンク１ｄ，２ｄが接続され、このようにして、順次、各リンクが接続されている。先端部のリンクの先端側（以下、「手先」という。）には、ワーク１０２を把持する把持機構４が設けられている。この把持機構４も、制御手段によって制御される。

これらロボットマニピュレータ１，２においては、第１のリンク１ａ，２ａに対する第２のリンク１ｂ，２ｂの位置、第２のリンク１ｂ，２ｂに対する第３のリンク１ｃ，２ｃの位置、第３のリンク１ｃ，２ｃに対する第４のリンク１ｄ，２ｄの位置と、順次先端側のリンクの位置を制御してゆくことにより、手先の位置を制御することができ、この手先において把持機構４によって把持したワークの位置を制御することができる。

図２は、ロボットマニピュレータを構成する各リンクについて設定された外形を包絡する複数の球体を示す側面図である。

そして、このロボット装置においては、本発明に係る複数ロボットの衝突検知方法が実施される。すなわち、このロボット装置においては、図２に示すように、制御手段により、ロボットマニピュレータ１，２を構成する各リンク１ａ，２ａ，１ｂ，２ｂ・・・について、外形を包絡する複数の球体５ａ，５ｂ，５ｃ・・・５ｎが設定される。このように球体５ａ，５ｂ，５ｃ・・・５ｎを設定することにより、各リンク１ａ，２ａ，１ｂ，２ｂ・・・の動作に応じて各リンクについて設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク１ａ，２ａ，１ｂ，２ｂ・・・同士が衝突するか否かの判断を行うことができる。

球体５ａ，５ｂ，５ｃ・・・５ｎの数、位置及び半径等は、これら球体によってどの程度正確にリンク１ａ，２ａ，１ｂ，２ｂ・・・の形状を近似するか、また、安全余裕をどの程度にするかに応じて決定する。図２中の（ｂ）に示すように、１つのリンクに対して球体の数を増やすほど、図２中の（ａ）に示す球体の数が少ない場合に比較して、正確にリンクの形状を近似することができる。ただし、球体の数を増やすほど、各球体の中心間距離を求めるための計算量は増加する。

図３は、６自由度のロボットマニピュレータの構成及び各リンクについて設定された外形を包絡する複数の球体を示す側面図である。

例えば、このロボット装置のロボットマニピュレータが、図３中の（ａ）に示すように、６自由度のロボットマニピュレータである場合には、図３中の（ｂ）に示すように、このロボットマニピュレータの各リンクａ，ｂ，ｃ，ｄ，ｅ，ｆ，ｇについて、前述のように複数の球体５を設定することによって、ロボットマニピュレータの全体について、複数の球体５によって近似することができる。

このロボット装置においては、各ロボットマニピュレータ１，２についてこのような球体の設定を行うとともに、ロボット装置の近傍に存在する障害物についても、球体の設定を行うようにしてもよい。すなわち、ロボット装置の近傍に位置する障害物について、各種センサによる検出結果や予め与えられた情報に基づいて位置情報を得て、この障害物の外形を包絡する複数の球体を設定する。

そして、ロボットマニピュレータの各リンクの動作に応じて各リンクについて設定した各球体及び障害物について設定した各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、各リンク同士及び各リンクと障害物とが衝突するか否かの判断を行うことができる。

すなわち、ロボットマニピュレータの制御中に、リアルタイムで各球体の中心間の距離を計算し、この距離が２つの球体の半径の和を下回った場合には、球体同士が干渉しリンク同士、または、リンクと障害物とが接触したと判断する。各球体は、ロボットマニピュレータの各リンク及び障害物の外形を覆うとともに、ある程度の余裕量を持つように、位置及び大きさが設定されているので、球体同士が干渉しなければ、リンク同士及びリンクと障害物とは、衝突していないと判断することができる。

図４は、設定された球体の中心間の距離の求め方を説明する図である。

設定された球体の中心間の距離を求めるにあたり、例えば、図４に示すように、半径ｒ１の球体Ｓ１及び半径ｒ２の球体Ｓ２について、Ｓ１の中心位置を（ｐｘ１、ｐｙ１、ｐｚ１）、Ｓ２の中心位置を（ｐｘ２、ｐｙ２、ｐｚ２）とし、これら２つの球体の中心間の距離をＲとする。この場合、以下の（式１）が成立する。
Ｒ^２＝（ｐｘ１−ｐｘ２）^２＋（ｐｙ１−ｐｙ２）^２＋（ｐｚ１−ｐｚ２）^２・・（式１）

したがって、これら２つの球体が干渉しているかどうかは、以下の（式２）が満足されるかどうかによって確認することができる。
Ｒ^２＜（ｒ１＋ｒ２）^２ ・・・（式２）

この処理をすべての球体の組合せについて行うことによって、各球体間の干渉の有無を判断することができる。このようにロボットマニピュレータの各リンクについて球体を設定することによって、球体の中心間距離の計算のみによって、球体同士の干渉の有無により、各リンクの衝突の有無の判断を行うことができる。すなわち、各リンクを円筒や平面を用いて表現した場合に比較して、方向（法線）や端点処理が不要となり、簡単な計算によって判断することができる。

ただし、同一のリンクについて設定された球体同士は、それぞれの半径を下回って配置されていても、接触と判断すべきではない。そこで、以下の方策に従い、衝突確認を行う必要がない球体の組を決定しておくことにより、必要な計算量を減らすことができる。

図５は、ロボットマニピュレータの所定のリンクの構成を示す側面図である。

このロボット装置においては、図５に示すように、セグメントを定義する。このセグメントは、一の屈曲（回転）または並進関節から、ロボットマニピュレータの先端側への次の屈曲（回転）または並進関節までの間の部分をいう。一の屈曲（回転）または並進関節は、次の屈曲（回転）または並進関節の位置を変化させる機能を有する。ここで、セグメントの定義においては、旋回関節（次の関節をその場で回転させるだけで次の関節の位置を変えない関節）は、除外して考える。すなわち、旋回関節で接続された複数のリンクは、同一のセグメントとなる。

したがって、図５に示すように、屈曲関節ｑ１から屈曲関節ｑ３までのリンクＬ１及びリンクＬ２が、同一のセグメントＳＧ１と定義される。リンクＬ１及びリンクＬ２は、旋回関節ｑ２によって接続されている。

このロボット装置において、各リンクについて設定される球体は、そのリンクが属するセグメント上に設定されていることになる。なお、障害物についても、１つ、または、２つ以上のセグメントとして定義し、球体を設定する。同一のセグメント上では、球体同士が干渉していても衝突が生じているわけではないので、同一のセグメント上の球体同士では、干渉が生じているかどうかの判別を行う必要がない。

また、このロボット装置においては、衝突が生じ得ない２つのセグメント上に設定された球体同士は、干渉が生じているかどうかの判別を行う必要がない。

図６は、接触が生じ得ないセグメントを示す側面図である。

このロボット装置において、接触が生じ得ないセグメントとは、図６に示すように、屈曲（回転）または並進関節ｑを介して隣り合ったセグメントである。すなわち、図６における「セグメント１」及び「セグメント２」、「セグメント２」及び「セグメント３」、そして、「セグメント４」及び「セグメント５」は、屈曲（回転）または並進関節ｑによって接続されているため、屈曲（回転）または並進関節ｑの可動範囲を制限したり制御することによって互いの接触を回避することができ、これら２つのセグメント上に設定された球体同士について干渉が生じているかどうかの判別を行う必要がない。

また、物理的に距離が離れており、互いの可動範囲内にないセグメント、すなわち、図６における「セグメント２」及び「セグメント４」は、可動範囲内でどのように駆動されても接触が生じ得ないので、これら２つのセグメント上に設定された球体同士について干渉が生じているかどうかの判別を行う必要がない。

さらに、作業台１０１上に固定された２つのセグメント、すなわち、図６における「セグメント１」及び「セグメント４」は、移動されることがなく接触が生じ得ないので、これら２つのセグメント上に設定された球体同士について干渉が生じているかどうかの判別を行う必要がない。

ところで、セグメント上に設定された複数の球体の位置を算出するには、一般的なロボットマニピュレータの制御中に得られる順運動学の結果を用いることができる。ある関節の基準座標に対する位置及び姿勢は、一般的な順運動学により、以下の（式３）、すなわち、４×４の同次変換行列によって表現することができる。

・・・（式３）

ここで、（ｘ、ｙ、ｚ）は、その関節における座標系を表す互いに直交した単位３次元ベクトルであり、その関節の姿勢を表す。ｐは、基準座標におけるその関節の位置を表す３次元ベクトルである。これらの値は、順運動学により、手先の位置及び姿勢を計算する過程で得られるものである。この関節から、この関節における座標系の（ｘ、ｙ、ｚ）軸方向にそれぞれ（ｌｘ、ｌｙ、ｌｚ）移動した点の座標ｐ′は、以下の（式４）で示すことができる。
ｐ′＝ｐ＋ｌｘ・ｘ＋ｌｙ・ｙ＋ｌｚ・ｚ ・・・（式４）

再び、図５に示した例を考えると、屈曲関節ｑ１の中心位置をｐ１、ｐ１における座標、すなわち、リンクＬ１の姿勢を（ｘ１、ｙ１、ｚ１）とし、旋回関節ｑ２の中心位置をｐ２、ｐ２における座標、すなわち、リンクＬ２の姿勢を（ｘ２、ｙ２、ｚ２）とすると、これらの値は、順運動学を計算する過程で求められる。

屈曲関節ｑ１からリンクＬ１の長手方向、すなわち、ｚ１方向に長さｌ１ｚ離れた位置に球体の中心Ｓ１がある場合には、Ｓ１の位置は、以下の（式５）によって示される。
Ｓ１＝ｐ１＋ｌ１ｚ・ｚ１ ・・・（式５）

同様に、旋回関節ｑ２からリンクＬ２の長手方向、すなわち、ｚ２方向に長さｌ２ｚ離れ、ｙ２方向に長さｌ２ｙ離れた位置に球体の中心Ｓ２がある場合には、Ｓ２の位置は、以下の（式６）によって示される。
Ｓ２＝ｐ２＋ｌ２ｚ・ｚ２＋ｌ２ｙ・ｙ２ ・・・（式６）

このように、このロボット装置においては、一般的なロボットマニピュレータの制御の途中で計算されている結果を用いて球体の位置を求めることができるので、球体の位置計算のために生ずる計算量の増加は、微小なものである。

図７は、ワークの取り扱いを示す側面図である。

このロボット装置において、図７に示すように、ロボットマニピュレータの把持機構４がワーク１０２を把持するときに、図７中（ａ）に示すように、ワーク１０２を把持する前には衝突が生じなかった動作でも、図７中（ｂ）に示すように、ワーク１０２を把持した後には、衝突が生じることがある。このような場合には、ワーク１０２についても球体を設定しておくことにより、ワーク１０２も含めた接触検知を行うことができる。

この場合、把持機構４がワーク１０２を把持した状態に応じて、セグメントに対する球体の設定の追加及び除去を動的に行ってもよいが、把持機構４の位置に半径ゼロ、または、負の値の半径の球体ＷＳを予め設定しておき、把持機構４がワーク１０２を把持したときには、把持機構４の位置の球体ＷＳの半径を拡大させつつ位置を移動することで、この球体ＷＳによりワーク１０２の外形を包絡させるようにしてもよい。このような扱いとすることにより、処理が容易になる。

図７中（ａ）に示すように、把持機構４の位置の球体ＷＳの半径をゼロ、または、負の値としておくことにより、他の任意の球体との干渉の有無の判定を行っても、干渉が検知されることはない。ワーク１０２を把持したときには、図７中（ｂ）に示すように、球体ＷＳの半径をｒｗに拡大させるとともに、オフセットＬＯｗを与えて球体ＷＳを移動させることにより、ワーク１０２の外形を包絡させる。この状態において、球体ＷＳに干渉する可能性がある他の球体との干渉の有無の判別を行うことができる。把持機構４がワーク１０２を解放したときには、再び、球体ＷＳの半径をゼロ、または、負の値にし、位置を手先位置などの基準位置に戻す。

なお、球体ＷＳの半径ｒｗ及びオフセットＬＯｗは、ワーク１０２の形状及び大きさに応じて予め設定しておき、動作情報などに基づいて、取得するようにすることができる。

本発明に係るロボット装置においては、各セグメント上、または、ワーク１０２上に設定される各球体の半径は、各リンクの動作速度に応じて、動的に変化させるようにしてもよい。例えば、リンクの動作速度が速い場合には、このリンクが属するセグメント上に設定される各球体の半径を拡大することにより、このリンクが他のリンクに衝突することをより確実に防止することができる。また、リンクの動作速度が遅い場合には、このリンクが属するセグメント上に設定される各球体の半径を縮小することにより、このリンクが他のリンクに衝突することを防止しつつ、このリンクの可動範囲を広くすることができる。

図８は、本発明に係るロボット装置の制御フローチャートである。

このロボット装置においては、図８に示すように、事前の処理、設計として、ステップｓｔ１でロボットマニピュレータの各リンクについて球体を設定し（リンクの近似）、ステップｓｔ２で障害物について球体を設定し（障害物の近似）、ステップｓｔ３でワークについて球体を設定し（ワークの近似）、ワークの形状及び大きさに応じた球体の半径及びオフセットも設定しておく。さらに、ステップｓｔ４で、セグメントの設定（分類）を行っておく。

そして、ロボット装置の動作が開始されると、オンライン制御中の処理に入り、ステップｓｔ５で任意の２つのセグメントを選択し、ステップｓｔ６で、これらセグメントが接触する可能性があるかを判別する。接触する可能性がなければ、ステップｓｔ５に戻り、他の２つのセグメントを選択して、ステップｓｔ６に進む。ステップｓｔ６で、２つのセグメントが接触する可能性があれば、ステップｓｔ７に進み、接触する可能性のある２つのセグメント上に設定された球体から、各１つの球体を選択し、ステップｓｔ８に進む。

ステップｓｔ８では、接触する可能性のある２つのセグメント上に設定された球体の全てについて干渉の有無が判別されたかを判断し、全ての球体について判別されていれば、ステップｓｔ５に戻り、全ての球体について判別されていなければ、ステップｓｔ９に進む。

ステップｓｔ９では、選択された２つの球体について、中心間の距離を計算し、ステップｓｔ１０に進み、球体の中心間距離がこれら球体の半径の合計よりも小さいかどうかを判別する。球体の中心間距離が半径の合計よりも大きければ、ステップｓｔ７に戻り、球体の中心間距離が半径の合計よりも小さければ、ステップｓｔ１１に進む。ステップｓｔ１１では、ステップｓｔ１０で選択されていた２つの球体に対応するリンク同士、リンクと障害物、または、ワークと障害物の衝突が生ずることを判別して、ロボットマニピュレータの動作を停止させるなどの所定の処理を行う。

本発明に係るロボット装置の構成を示す模式的な斜視図である。 本発明に係るロボット装置においてロボットマニピュレータを構成する各リンクについて設定された外形を包絡する複数の球体を示す側面図である。 本発明に係るロボット装置におけるロボットマニピュレータの例である６自由度のロボットマニピュレータの構成及び各リンクについて設定された外形を包絡する複数の球体を示す側面図である。 本発明に係るロボット装置における設定された各球体の中心間の距離の求め方を説明する図である。 本発明に係るロボット装置におけるロボットマニピュレータの所定のリンクの構成を示す側面図である。 本発明に係るロボット装置において接触が生じ得ないセグメントを示す側面図である。 本発明に係るロボット装置におけるワークの取り扱いを示す側面図である。 本発明に係るロボット装置の制御フローチャートである。

符号の説明

１，２ ロボットマニピュレータ
３ 関節
４ 把持部
５ 球体
１０１ 作業台
１０２ ワーク

Claims (4)

1. それぞれが複数のリンクにより構成された複数のロボットマニピュレータを制御するにあたり、
   前記各リンクについて、外形を包絡する複数の球体を設定し、
   前記各リンクの動作に応じて、前記各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出し、
   前記ロボットマニピュレータの手先位置に半径ゼロの球体を予め設定しておき、前記ロボットマニピュレータがワークを把持していないときには、前記手先位置の球体及び前記各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンク同士が衝突するか否かの判断を行い、前記ロボットマニピュレータがワークを把持したときには、前記手先位置の球体の半径を拡大させ、または位置の変更と半径の拡大の両方を行うことによって、この球体により前記ワークの外形を包絡させ、この球体及び前記各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンク同士及び前記ワークが衝突するか否かの判断を行う
   ことを特徴とする複数ロボットの衝突検知方法。
2. 前記ロボットマニピュレータの近傍の障害物の位置情報を得て、この障害物の外形を包絡する複数の球体を設定し、
   前記各球体及び前記各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンク同士、前記ワーク及び前記障害物が衝突するか否かの判断を行う
   ことを特徴とする請求項１記載の複数ロボットの衝突検知方法。
3. それぞれが複数のリンクにより構成された複数のロボットマニピュレータと、
   前記各ロボットマニピュレータを制御する制御手段と
   を備え、
   前記制御手段は、前記各リンクを動作させるとともに、前記各リンクについて外形を包絡する複数の球体を設定し、前記各リンクの動作に応じて各リンクについて設定された各球体の中心間距離の変化を計算し、前記ロボットマニピュレータの手先位置に半径ゼロの球体を予め設定しておき、前記ロボットマニピュレータがワークを把持させていないときには、前記手先位置の球体及び前記各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンク同士が衝突するか否かの判断を行い、前記ロボットマニピュレータにワークを把持させたときには、前記手先位置の球体の半径を拡大させ、または位置の変更と半径の拡大の両方を行うことによって、この球体により前記ワークの外形を包絡させ、この球体及び前記各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンク同士及び前記ワークが衝突するか否かの判断を行う
   ことを特徴とするロボット装置。
4. 前記ロボットマニピュレータの近傍の障害物の位置情報を得て、この障害物の外形を包絡する複数の球体を設定し、
   前記各球体及び前記各リンクについて設定された各球体の中心間距離の変化を計算し、各球体同士の干渉を検出することによって、前記各リンク同士、前記ワーク及び前記障害物が衝突するか否かの判断を行う
   ことを特徴とする請求項３記載のロボット装置。

Images