JP2013136109A - 干渉判定装置及び干渉判定方法 - Google Patents
干渉判定装置及び干渉判定方法 Download PDFInfo
- Publication number
- JP2013136109A JP2013136109A JP2011287407A JP2011287407A JP2013136109A JP 2013136109 A JP2013136109 A JP 2013136109A JP 2011287407 A JP2011287407 A JP 2011287407A JP 2011287407 A JP2011287407 A JP 2011287407A JP 2013136109 A JP2013136109 A JP 2013136109A
- Authority
- JP
- Japan
- Prior art keywords
- interference
- interference determination
- determination target
- information
- area
- 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.)
- Pending
Links
Images
Landscapes
- Manipulator (AREA)
Abstract
【課題】干渉判定時間を短縮可能な干渉判定装置及び干渉判定方法を提供する。
【解決手段】 一実施形態に係る干渉判定装置4は、各対象部位PiのZ軸における存在領域の情報に基づいて、各対象部位について、その対象部位の存在領域と他の対象部位の存在領域との重複状態を示す存在領域重複情報を生成する存在領域重複情報生成部441と、各対象部位のXY平面内での占有領域の情報に基づいて、各対象部位について、その対象部位の占有領域における他の対象部位の占有領域の重複状態を示す占有領域重複情報を取得する占有領域重複情報生成部442と、複数の対象部位のうち存在領域重複情報と占有領域重複情報とがいずれも重複していることを示す少なくとも2つの対象部位が存在する場合、複数の対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の対象部位間に干渉が存在しないと判定する判定部443と、を備える。
【選択図】図1
【解決手段】 一実施形態に係る干渉判定装置4は、各対象部位PiのZ軸における存在領域の情報に基づいて、各対象部位について、その対象部位の存在領域と他の対象部位の存在領域との重複状態を示す存在領域重複情報を生成する存在領域重複情報生成部441と、各対象部位のXY平面内での占有領域の情報に基づいて、各対象部位について、その対象部位の占有領域における他の対象部位の占有領域の重複状態を示す占有領域重複情報を取得する占有領域重複情報生成部442と、複数の対象部位のうち存在領域重複情報と占有領域重複情報とがいずれも重複していることを示す少なくとも2つの対象部位が存在する場合、複数の対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の対象部位間に干渉が存在しないと判定する判定部443と、を備える。
【選択図】図1
Description
本発明は、干渉判定装置及び干渉判定方法に関する。
先端に昇降部を持つ二台の水平多関節アームを人間の両腕のように配置した双腕ロボットは、設置に要するスペースの割に自由度が高く、また両腕の作業領域の少なくとも一部が重複していることからワークを押さえつつねじ締めを行うなどの複合作業が可能であるという利点がある。しかし、各アームは移動経路で衝突する危険があり、作業者はその動きを予測することが困難である。そこで、双腕ロボットの各アームが他方のアームの作業領域に一部でも重複するような作業領域内で移動する際、互いのアームが相手の移動軌道上に重なるかどうかという干渉判定が必要となる。また、双腕ロボットが実際に作業を行う上で、左右のアーム同士以外にも、治具や安全柵などの障害物との干渉が生じる可能性があり、水平多関節アームと障害物の干渉判定も必要になる場合がある。また、このようなロボット分野に限らず、例えばゲーム分野や自動車分野などにおいて、複数の物体の干渉(又は衝突)を判定する必要が生じる場合もある。
干渉判定を行う方法としては、いくつかの方式が存在する。例えば、工作機械を制御する数値制御装置で干渉チェックを行うために、「複数の機械構造物の干渉領域を各々定義し、前記干渉領域を移動させ、前記各々の機械構造物の干渉領域がそれぞれ干渉するかをチェックする機能を有する数値制御装置について、前記干渉領域として、直方体、円筒、平面の立体図形で定義し、各々の立体図形が交差するか交差しないかにより干渉をチェックする」という方式がある(特許文献1参照)。また、干渉判定を行う他の方法として、水平多関節ロボットに対して、「3次元ロボットモデルを2次元平面上に投影した2次元ロボットモデルに変換」し、「3次元障害物モデルを2次元障害物モデルとして分割」したものとの干渉判定を行う方式も存在する(特許文献2参照)。
上記の特許文献1の発明のように、3次元方向の運動を行う3次元物体に対する干渉判定は、定義が容易な3次元の単純近似モデルに変換して行われることが多い。しかしながら、立体図形が交差するかしないかの判断を行う回数は、判定対象の組み合わせの数だけ必要となる。そのため、各部位の干渉判定を行うような場合、演算処理に時間を要する。また、それぞれの立体図形が交差するかしないかの判断を、判定空間を小さく区切った直方体の占有とその占有領域の重複から判断する場合、判定空間の各辺をq分割したとして、判定空間の演算量はq3オーダーになる。
また、特許文献2の発明においては、障害物モデルの分割は3次元障害物モデルの高さ毎に行われ、水平多関節ロボットの設置面からアームまでの高さ(高さは一定であるが、ある厚みを持つ)と一致する障害物モデルのみが干渉判定の対象となる。そのため、2次元ロボットモデルを用いているにもかかわらず、立体交差が可能な部分を干渉判定から除外した結果を短時間で得ることができる。この方式の優れている点は、水平多関節ロボットの高さと一致する分割面のみを判定すればよいという点である。しかしながら、例えば水平多関節ロボットを複数の分割面に分けなければならないような場合には、それぞれの分割面で干渉判定を行わなければならない。双腕ロボットもこれに該当し、ロボットアームを構成する各部分(第1リンク、第2リンク、先端の昇降部など)で異なる高さを持つために干渉判定を行うべき分割面が多数になって演算量は最大q3オーダーにまで増加し、処理に時間を要する。
そこで、本発明は、干渉判定時間を短縮可能な干渉判定装置及び干渉判定方法を提供することを目的とする。
本発明に係る干渉判定装置は、互いに交差する第1及び第2の軸と、第1及び第2の軸を含む2次元平面と交差する第3の軸と、で構成される3次元空間における複数の干渉判定対象部位の干渉を判定する干渉判定装置であって、各干渉判定対象部位を第3の軸に投影した各干渉判定対象部位の第3の軸における存在領域の情報に基づいて、各干渉判定対象部位について、その干渉判定対象部位の存在領域と他の少なくとも一つの干渉判定対象部位の存在領域との重複状態を示す存在領域重複情報を生成する存在領域重複情報生成部と、各干渉判定対象部位を2次元平面へ投影した2次元平面内での各干渉判定対象部位の占有領域の情報に基づいて、各干渉判定対象部位について、その干渉判定対象部位の占有領域における他の少なくとも一つの干渉判定対象部位の占有領域の重複状態を示す占有領域重複情報を取得する占有領域重複情報生成部と、複数の干渉判定対象部位のうち存在領域重複情報と占有領域重複情報とがいずれも重複していることを示す少なくとも2つの干渉判定対象部位が存在する場合、複数の判定対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の干渉判定対象部位間に干渉が存在しないと判定する判定部と、を備える。
本発明に係る干渉判定方法は、互いに交差する第1及び第2の軸と、第1及び第2の軸を含む2次元平面と交差する第3の軸と、で構成される3次元空間における複数の干渉判定対象部位の干渉を干渉判定装置が判定する干渉判定方法であって、各干渉判定対象部位を第3の軸に投影した各干渉判定対象部位の第3の軸における存在領域の情報に基づいて、各干渉判定対象部位について、その干渉判定対象部位の存在領域と他の少なくとも一つの干渉判定対象部位の存在領域との重複状態を示す存在領域重複情報を干渉判定装置が生成するステップと、各干渉判定対象部位を2次元平面へ投影した2次元平面内での各干渉判定対象部位の占有領域の情報に基づいて、各干渉判定対象部位について、その干渉判定対象部位の占有領域における他の少なくとも一つの干渉判定対象部位の占有領域の重複状態を示す占有領域重複情報を干渉判定装置が生成するステップと、複数の干渉判定対象部位の干渉のうち存在領域重複情報と占有領域重複情報とがいずれも重複することを示す少なくとも2つの干渉判定対象部位が存在する場合、複数の干渉判定対象部位間に干渉が存在すると干渉判定装置が判定し、それ以外の場合は、複数の干渉判定対象部位間に干渉が存在しないと干渉判定装置が判定するステップと、を備える。
本発明に係る干渉判定装置及び干渉判定方法では、存在領域重複情報生成部が生成する存在領域重複情報が少なくとも2つの干渉判定対象部位の第3の軸上の存在領域が重複していることを示している場合、その存在領域が重複した干渉判定対象部位同士は、干渉(衝突)の可能性があることになる。同様に、占有領域重複情報生成部が生成する占有領域重複情報が少なくとも2つの干渉判定対象部位の上記2次元平面における占有領域が重複していることを示している場合、その占有領域が重複した干渉判定対象部位同士は、干渉(衝突)の可能性があることになる。従って、判定部が、存在領域重複情報及び占有領域重複情報を利用して上記のように干渉を判定することによって、3次元的に干渉判定対象部位同士の干渉(衝突)を判定し得る。そして、第3の軸方向における複数の干渉判定対象部位の配置状態における干渉の可能性をそれらの干渉判定対象部位を第3の軸に投影した存在領域に基づいて1次元で評価しているため、干渉判定に要する演算量を低減できる。その結果、干渉判定の時間を短縮可能である。
一実施形態に係る干渉判定装置は、各干渉判定対象部位に識別番号を割り当てる識別番号割当部であって、各干渉判定対象部位の識別番号は、複数の桁数を有し且つ他の識別番号との間での論理積の結果において全ての桁が「0」となるように設定された識別番号である、識別番号割当部と、複数の干渉判定対象部位の存在領域を含む1次元判定領域を第3の軸に設定し、1次元判定領域を複数の線分に区切ると共に、各線分に対応しており存在領域の情報を格納する線分対応メモリ要素を確保する1次元判定領域管理部と、複数の干渉判定対象部位の占有領域を含む2次元判定領域を2次元平面に設定すると共に、2次元判定領域を、2次元的に配列されたマス目に区切ると共に、各マス目に対応しており占有領域の情報を格納するマス目対応メモリ要素を確保する2次元判定領域管理部と、を備え、存在領域重複情報生成部は、複数の線分のうち各干渉判定対象部位の存在領域の少なくとも一部を含む線分を特定し、特定された線分に対応する線分対応メモリ要素に、特定された線分に存在領域が位置する干渉判定対象部位が一つの場合は、その干渉判定対象部位の識別番号を存在領域重複情報として格納し、特定された線分に存在領域が位置する干渉判定対象部位が2つ以上の場合は、それらの干渉判定対象部位の識別番号の論理和を存在領域重複情報として格納し、各干渉判定対象部位について、その干渉判定対象部位の存在領域の少なくとも一部を含む全ての線分の線分対応メモリ要素に格納された存在領域重複情報の論理和を干渉コードとして生成し、占有領域重複情報生成部は、複数のマス目のうち各干渉判定対象部位の占有領域の少なくとも一部を含むマス目を特定し、特定されたマス目に対応するマス目対応メモリ要素に、特定されたマス目に占有領域が位置する干渉判定対象部位が一つの場合は、その干渉判定対象部位の識別番号を占有領域重複情報として格納し、特定されたマス目に占有領域が位置する干渉判定対象部位が2つ以上の場合は、それらの干渉判定対象部位の識別番号の論理和を占有領域重複情報として格納し、判定部は、各干渉判定対象部位について、その干渉判定対象部位の占有領域の少なくとも一部を含むマス目に対応するマス目対応メモリ要素に格納された占有領域重複情報と、その干渉判定対象部位の干渉コードとの論理積を演算し、少なくとも一つの干渉判定対象部位に対する占有領域重複情報と干渉コードとの論理積の演算結果の何れかの桁に「1」が含まれる場合は、複数の干渉判定対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の干渉判定対象部位間に干渉が存在しないと判定してもよい。
一実施形態に係る干渉判定方法は、各干渉判定対象部位に識別番号を割り当てるステップであって、各干渉判定対象部位の識別番号は、複数の桁数を有し且つ他の識別番号との間での論理積の結果において全ての桁が「0」となるように設定された識別番号である、ステップと、複数の干渉判定対象部位の存在領域を含む1次元判定領域を干渉判定装置が第3の軸に設定し、1次元判定領域を複数の線分に区切って、各線分に対応しており存在領域の情報を格納する線分対応メモリ要素を干渉判定装置が確保するステップと、複数の干渉判定対象部位の占有領域を含む2次元判定領域を2次元平面に干渉判定装置が設定し、2次元判定領域を、2次元的に配列されたマス目に区切り、各マス目に対応しており占有領域の情報を格納するマス目対応メモリ要素を干渉判定装置が確保するステップと、を更に備え、存在領域重複情報を生成するステップは、複数の線分のうち各干渉判定対象部位の存在領域の少なくとも一部を含む線分を特定し、特定された線分に対応する線分対応メモリ要素に、特定された線分に存在領域が位置する干渉判定対象部位が一つの場合は、その干渉判定対象部位の識別番号を存在領域重複情報として格納し、特定された線分に存在領域が位置する干渉判定対象部位が2つ以上の場合は、それらの干渉判定対象部位の識別番号の論理和を存在領域重複情報として格納し、各干渉判定対象部位について、その干渉判定対象部位の存在領域の少なくとも一部を含む全ての線分の線分対応メモリ要素に格納された存在領域重複情報の論理和を干渉コードとして生成し、占有領域重複情報を生成するステップは、複数のマス目のうち各干渉判定対象部位の占有領域の少なくとも一部を含むマス目を特定し、特定されたマス目に対応するマス目対応メモリ要素に、特定されたマス目に占有領域が位置する干渉判定対象部位が一つの場合は、その干渉判定対象部位の識別番号を占有領域重複情報として格納し、特定されたマス目に占有領域が位置する干渉判定対象部位が2つ以上の場合は、それらの干渉判定対象部位の識別番号の論理和を占有領域重複情報として格納し、判定するステップは、各干渉判定対象部位について、その干渉判定対象部位の占有領域の少なくとも一部を含むマス目に対応するマス目対応メモリ要素に格納された占有領域重複情報と、その干渉判定対象部位の干渉コードとの論理積を演算し、少なくとも一つの干渉判定対象部位に対する占有領域重複情報と干渉コードとの論理積の演算結果の何れかの桁に「1」が含まれる場合は、複数の干渉判定対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の干渉判定対象部位間に干渉が存在しないと判定してもよい。
上記のように、干渉判定対象部位に識別番号を割り当てていることによって、存在領域の情報及び占有領域の情報を1変数で表すことが可能である。その結果、演算量を低減でき、結果として、干渉判定の時間を低減できる。更に、第3の軸に1次元判定領域を設定し、その1次元判定領域を複数の線分に区切って各線分に対して線分対応メモリ要素を確保すると共に、2次元平面に2次元判定領域を設定し、その2次元判定領域を複数のマス目に区切って各マス目に対応するマス目対応メモリ要素を確保していることから、複数の干渉判定対象部位の動作を離散的に扱うことが可能である。更に、1次元判定領域に格納された存在領域重複情報を利用して干渉コードを生成しているので、一つの干渉判定対象部位の存在領域に渡る他の干渉判定対象部との存在領域の重複状態を一つの干渉コードとして扱い得る。
一実施形態において、存在領域重複情報生成部は、複数の干渉判定対象部位について所定の順番に沿って干渉判定対象部位の存在領域の少なくとも一部を含む線分を特定し、存在領域重複情報生成部は、干渉判定対象部位に対応する線分を特定する毎に、その干渉判定対象部位に対する干渉コードを生成し、占有領域重複情報生成部は、所定の順番に沿って順に干渉判定対象部位の占有領域に位置するマス目を特定し、干渉判定対象部位に対応するマス目を特定する毎に、特定されたマス目に格納されている占有領域重複情報を更新し、判定部は、干渉コードが生成される毎に、生成された干渉コードと、干渉コードの生成に対応して更新された占有領域重複情報との論理積とを演算し、何れかの論理積の演算結果の何れかの桁に「1」が含まれる場合は、複数の干渉判定対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の干渉判定対象部位間に干渉が存在しないと判定する。
この場合、所定の順番に沿って干渉判定対象部位の干渉コードを生成すると共に、占有領域重複情報を更新する。そして、ある干渉判定対象部位の存在領域の情報に基づいて干渉コードを生成すると共に、その干渉判定対象部位の占有領域の情報に基づいて占有領域重複情報を更新する毎に、その干渉判定対象部位に対する干渉の判定を行う。そのため、例えば、一度全ての干渉判定対象部位を考慮して一つの干渉判定対象部位に対する干渉コードを算出する場合に比べて、干渉コードを算出するための演算回数を低減できる。その結果、干渉判定に要する時間を短縮可能である。
一実施形態において、各干渉判定対象部位についての存在領域重複情報は、その干渉判定対象部位との干渉は生じない少なくとも一つの干渉判定対象部位を予め示す干渉判定除外情報と、各干渉判定対象部位の存在領域とから生成される。
複数の干渉判定対象部位の配置状態においては、明らかに干渉の生じない干渉判定対象部位が存在する場合がある。この場合、それら干渉が生じ得ない干渉判定対象部位を示す判定除外情報を考慮して存在領域重複情報を生成し、その判定除外情報を利用することによって、確実に、それら干渉が生じ得ない干渉判定対象部位同士の判定結果を干渉なしとし得る。
一実施形態において、複数の干渉判定対象部位は、3次元空間内で複数の干渉判定対象部位の配置状態が第1の配置状態から第2の配置状態に変形可能であって、第1の配置状態から第2の配置状態への変形過程における一定の時間間隔毎の複数の干渉判定対象部位の配置状態に対して、存在領域重複情報生成部は、時間間隔毎の複数の干渉判定対象部位の配置状態における各干渉判定対象部位の存在領域重複情報を生成し、占有領域重複情報生成部は、時間間隔毎の複数の干渉判定対象部位の配置状態における各干渉判定対象部位について占有領域重複情報を取得し、判定部は、時間間隔毎に生成された存在領域重複情報と占有領域量副情報とに基づいて複数の干渉判定対象部位の干渉の有無を判断し、全ての時間間隔の何れかにおいて干渉が存在する場合は、複数の干渉判定対象部位の配置状態の第1の配置状態から第2の配置状態への変形過程において干渉が存在すると判定し、全ての時間間隔の何れかにおいても干渉が存在しない場合は、変形過程において干渉が存在しないと判定する。
この場合、複数の干渉判定対象部位が第1の配置状態から第2の配置状態に変形する過程における干渉の有無を判定し得る。
また、本発明は、互いに交差する第1及び第2の軸と、第1及び第2の軸を含む2次元平面と交差する第3の軸と、で構成される3次元空間における複数の干渉判定対象部位の干渉を干渉判定装置が判定する干渉判定方法であって、各干渉判定対象部位に識別番号を干渉判定装置が割り当てるステップであって、各干渉判定対象部位の識別番号は、複数の桁数を有し且つ他の識別番号との間での論理積の結果において全ての桁が「0」となるように設定された識別番号である、ステップと、複数の干渉判定対象部位の存在領域を含む1次元判定領域を第3の軸に干渉判定装置が設定し、1次元判定領域を複数の線分に区切って、各線分に対応しており存在領域の情報を格納する線分対応メモリ要素を干渉判定装置が確保するステップと、全ての線分対応メモリ要素に初期値として「0」を格納して線分対応メモリ要素を干渉判定装置が初期化するステップと、複数の干渉判定対象部位の占有領域を含む2次元判定領域を2次元平面に干渉判定装置が設定し、2次元判定領域を、2次元的に配列されたマス目に区切り、各マス目に対応しており占有領域の情報を格納するマス目対応メモリ要素を干渉判定装置が確保するステップと、全てのマス目対応メモリ要素に初期値として「0」を格納してマス目対応メモリ要素を干渉判定装置が初期化するステップと、複数の線分のうち干渉判定対象部位の存在領域の少なくとも一部を含む線分に対応する線分対応メモリ要素に、初期値が格納されている場合は、その干渉判定対象部位の識別番号を干渉判定装置が存在領域重複情報として格納し、その線分対応メモリ要素に初期値以外の値が格納されている場合は、干渉判定対象部位の識別番号と、その線分対応メモリ要素に格納されている値との論理和を干渉判定装置が存在領域重複情報として格納するステップと、干渉判定対象部位の存在領域の少なくとも一部を含む全ての線分の線分対応メモリ要素に格納された存在領域重複情報の論理和を干渉判定装置が干渉コードとして生成するステップと、複数のマス目のうち干渉判定対象部位の占有領域の少なくとも一部を含むマス目に対応するマス目対応メモリ要素に初期値が格納されている場合は、その干渉判定対象部位の識別番号を干渉判定装置が占有領域重複情報として格納し、そのマス目対応メモリ要素に初期値以外の値が格納されている場合は、干渉判定対象部位の識別番号と、線分対応メモリ要素に格納されている値との論理和を干渉判定装置が占有領域重複情報として格納するステップと、干渉判定対象部位の占有領域の少なくとも一部を含むマス目に対応するマス目対応メモリ要素に格納された占有領域重複情報と、その干渉判定対象部位の干渉コードとの論理積を干渉判定装置が演算し、その論理積の演算結果の何れかの桁に「1」が含まれる場合は、その干渉判定対象部位と他の少なくとも一つの干渉判定対象部位との間に干渉が存在すると干渉判定装置が判定し、それ以外の場合は、その干渉判定対象部位と他の少なくとも一つの干渉判定対象部位との間に干渉が存在しないと干渉判定装置が判定するステップと、を備え、所定の順番で複数の干渉判定対象部位毎に、線分対応メモリ要素を初期化するステップと、マス目対応メモリ要素を初期化するステップと、存在領域重複情報として格納するステップと、干渉コードとして生成するステップと、占有領域重複情報として格納するステップと、判定するステップとを、干渉判定装置が実行する。
存在領域重複情報生成部が生成する存在領域重複情報が少なくとも2つの干渉判定対象部位の第3の軸上の存在領域が重複していることを示している場合、その存在領域が重複した干渉判定対象部位同士は、干渉(衝突)の可能性があることになる。同様に、占有領域重複情報生成部が生成する占有領域重複情報が少なくとも2つの干渉判定対象部位の上記2次元平面における占有領域が重複していることを示している場合、その占有領域が重複した干渉判定対象部位同士は、干渉(衝突)の可能性があることになる。従って、判定部が、存在領域重複情報及び占有領域重複情報を利用して上記のように干渉を判定することによって、3次元的に干渉判定対象部位同士の干渉(衝突)を判定し得る。そして、第3の軸方向における複数の干渉判定対象部位の配置状態における干渉の可能性をそれらの干渉判定対象部位を第3の軸に投影した存在領域に基づいて1次元で評価しているため、干渉判定に要する演算量を低減できる。その結果、干渉判定の時間を短縮可能である。なお、干渉コードは、存在領域重複情報を利用して生成されているので、干渉コードに基づいて干渉を判定することによって、存在領域重複情報を利用して干渉を判定していることになる。
また、干渉判定対象部位に識別番号を割り当てていることによって、存在領域の情報及び占有領域の情報を1変数で表すことが可能である。その結果、演算量を低減でき、結果として、干渉判定の時間を低減できる。更に、第3の軸に1次元判定領域を設定し、その1次元判定領域を複数の線分に区切って各線分に対して線分対応メモリ要素を確保すると共に、2次元平面に2次元判定領域を設定し、その2次元判定領域を複数のマス目に区切って各マス目に対応するマス目対応メモリ要素を確保していることから、複数の干渉判定対象部位の動作を離散的に扱うことが可能である。更に、1次元判定領域に格納された存在領域重複情報を利用して干渉コードを生成しているので、一つの干渉判定対象部位の存在領域に渡る他の干渉判定対象部との存在領域の重複状態を一つの干渉コードとして扱い得る。
更に、上記方法では、ある干渉判定対象部位の存在領域の情報に基づいて干渉コードを生成すると共に、その干渉判定対象部位の占有領域の情報に基づいて占有領域重複情報を更新する毎に、その干渉判定対象部位に対する干渉の判定を行う。そのため、例えば、一度全ての干渉判定対象部位を考慮して一つの干渉判定対象部位に対する干渉コードを算出する場合に比べて、干渉コードを算出するための演算回数を低減できる。その結果、干渉判定に要する時間を短縮可能である。
本発明によれば、干渉判定時間を短縮できる。
以下、添付図面を参照して本発明に係る干渉判定装置及び干渉判定方法の実施形態を詳細に説明する。図面の説明において同一の要素には同一の符号を付し、重複する説明を省略する。
[双腕ロボットシステム100の全体構成]
一実施形態に係る干渉判定装置を含む双腕ロボットシステムの全体構成について、図1〜3を参照しながら説明する。図1は、一実施形態に係る干渉判定装置を含む双腕ロボットシステムの構成概要図である。図1に示すように、双腕ロボットシステム100は、軌道生成部1、サーボドライバ2a〜2d、昇降制御ドライバ2e,2f、双腕ロボット3、及び、干渉判定部4を含んで構成され、各構成要素は例えば図1にて矢印で表現される通信手段により互いに通信できるように接続されている。図2は双腕ロボット3の側面概要図であり、図3は双腕ロボット3の上面概要図である。
一実施形態に係る干渉判定装置を含む双腕ロボットシステムの全体構成について、図1〜3を参照しながら説明する。図1は、一実施形態に係る干渉判定装置を含む双腕ロボットシステムの構成概要図である。図1に示すように、双腕ロボットシステム100は、軌道生成部1、サーボドライバ2a〜2d、昇降制御ドライバ2e,2f、双腕ロボット3、及び、干渉判定部4を含んで構成され、各構成要素は例えば図1にて矢印で表現される通信手段により互いに通信できるように接続されている。図2は双腕ロボット3の側面概要図であり、図3は双腕ロボット3の上面概要図である。
双腕ロボット3は、例えば図2及び図3に示されるように、支持台30に取り付けられた水平多関節アーム31,32を有する。水平多関節アーム31,32は、支持台30に一端部が取り付けられた第1リンク部311,321と、第1リンク部311,321の他端部に一端部が取り付けられた第2リンク部312,322と、第2リンク部312,322の他端部に取り付けられた昇降部313,323とを含む。
各アーム31,32の第1リンク部311,321の付け根(双腕ロボット3の肩部分)、すなわち、第1リンク部311,321と支持台30との接合部には、サーボモータ3a,3cが設けられている。また、第2リンク部312,322の付け根(双腕ロボット3のひじ部分)、すなわち、第1リンク部311,321と第2リンク部312,322の接合部には、それぞれサーボモータ3b,3dが設けられている。この構成により、各第1アーム部31及び第2アーム部32は水平方向に旋回移動できるようになっている。また、昇降部313,323は、鉛直方向に延びる昇降軸に沿って昇降部313,323の下端部が鉛直方向に昇降可能となっている。昇降部313,323は、昇降制御部3e、3fによって、その昇降状態が制御される。図2及び図3では、昇降制御部3e、3fは、昇降部313,323に内蔵されているとして模式的に示しているが、昇降制御部3e,3fは、昇降部313,323の昇降状態を制御可能なように昇降部313,323に取り付けられていればよい。
支持台30は、双腕ロボット3により作業される例えばワークが載置される載置台と対向して配置されており、水平多関節アーム31,32は、ワーク及びそのワークの載置台を挟んで、水平多関節アーム31,32の作業範囲が少なくとも一部重複するように支持台30に取り付けられている。なお、ワーク及びそのワークの載置台は、水平多関節アーム31,32の動作の軌跡上においては、水平多関節アーム31,32の動作の障害物となる。よって、ワーク及びそのワークの載置台をまとめて障害物5とも称す。
以下では、昇降部313,323の昇降方向(延長方向)をZ軸(第3の軸)方向とし、Z軸方向に直交する2つの軸方向をX軸(第1の軸)方向及びY軸(第2の軸)方向とする。X軸及びY軸は互いに直交する。X軸、Y軸及びZ軸は、後述する軌道生成部1による軌道の算出及び干渉判定処理のための仮想的な軸である。この場合、双腕ロボット3は、X軸、Y軸及びZ軸から構成される3次元空間内で動作可能なロボットである。
図1にて示される軌道生成部1は、双腕ロボット3の各部位が現在位置から目標位置まで移動できるように、各関節の移動量及び移動速度並びに昇降部313,323の昇降状態及び昇降速度を演算し、各サーボドライバ2a〜2d及び昇降制御ドライバ2e,2fに指令を送る部分である。双腕ロボット3の各部位の現在位置により双腕ロボット3の現在の姿勢(以下、現在姿勢とも称す)が規定され、双腕ロボット3の各部位の目標位置により双腕ロボット3の目標の姿勢(以下、目標姿勢とも称す)が規定される。
軌道生成部1は、記憶部11及び演算部12を含んで構成される。記憶部11は、軌道生成に必要な双腕ロボット3の概形寸法や移動に関する補間情報ならびに速度情報などの諸元と、教示もしくはロボット言語によるプログラミングによって記述されたロボットの動作順序を制御する動作プログラムとを格納する部分である。
演算部12は、双腕ロボット3の各部位が現在位置から目標位置まで移動できるように、各関節の移動量及び移動速度並びに昇降部313,323の昇降量及び昇降速度を演算する部分である。双腕ロボット3の移動前に、軌道生成部1の演算部12は干渉判定部4へ、移動前の姿勢から移動後の姿勢に移動する場合の移動中および移動後の干渉の有無を問い合わせる。
サーボドライバ2a〜2dは、軌道生成部1からの指令を受けて、双腕ロボット3の各関節に設けられたサーボモータ3a〜3dをそれぞれ駆動する。サーボドライバ2a〜2dは、現在の角度情報などを軌道生成部1にフィードバックする機能も有する。
昇降制御ドライバ2e,2fは、軌道生成部1からの指令を受けて、双腕ロボット3の昇降部313,323に設けられた昇降制御部3e,3fを駆動して昇降部313,323の昇降状態を制御する。昇降制御ドライバ2e,2fは、現在の昇降状態の情報などを軌道生成部1にフィードバックする機能も有する。
[干渉判定部の構成]
以下、双腕ロボットシステム100の一構成要素である干渉判定部(干渉判定装置)4の構成について、図4〜図8を更に参照しながら詳細に説明する。
以下、双腕ロボットシステム100の一構成要素である干渉判定部(干渉判定装置)4の構成について、図4〜図8を更に参照しながら詳細に説明する。
干渉判定部4は、軌道生成部1から問い合わせを受けて、双腕ロボット3が3次元で動作する際に、双腕ロボット3の各部位が互いに干渉するか否か、又は、双腕ロボット3の各部位と障害物5とが干渉するか否かの干渉判定を行う部分である。双腕ロボット3と障害物5とにおいて、このように、相互の干渉判定を行うh個(hは2以上の整数)の部位を干渉判定対象部位(以下、単に「対象部位」と称す)P1〜Phと称す。また、対象部位P1〜Phを対象部位Pi(iは、1以上h以下の整数)とも称す場合もある。
本実施形態では、双腕ロボット3の各部位は、支持台30、第1リンク部311,321、第2リンク部312,322及び昇降部313,323である。第1リンク部311,321及び第2リンク部312,322は水平移動可能であり、昇降部313,323は鉛直方向に昇降可能である。そのため、双腕ロボット3が所定の姿勢から他の姿勢に移動する際に、支持台30、第1リンク部311,321、第2リンク部312,322及び昇降部313,323並びに障害物5の間に衝突を伴う立体交差、すなわち、干渉が生じる可能性はある。そのため、支持台30、第1リンク部311,321、第2リンク部312,322及び昇降部313,323並びに障害物5は、干渉判定の対象部位である。よって、本実施形態では、8個の対象部位P1〜P8が存在する。
支持台30、第1リンク部311,321、第2リンク部312,322、昇降部313,323及び障害物5と対象部位P1〜P8との対応関係は次の通りである。
支持台30:対象部位P1
第1リンク部311:対象部位P2
第2リンク部312:対象部位P3
昇降部313:対象部位P4
第1リンク部321:対象部位P5
第2リンク部322:対象部位P6
昇降部323:対象部位P7
障害物5:対象部位P8
支持台30:対象部位P1
第1リンク部311:対象部位P2
第2リンク部312:対象部位P3
昇降部313:対象部位P4
第1リンク部321:対象部位P5
第2リンク部322:対象部位P6
昇降部323:対象部位P7
障害物5:対象部位P8
図4は、干渉判定部の機能的構成要素を示したブロック図である。図4に示されるように、干渉判定部4は、識別番号割当部41、判定領域管理部42、格納部43及び干渉判定処理部44を含んで構成される。
[識別番号割当部]
識別番号割当部41は、対象部位Piに識別番号IDiを割り当てる部分である。一つの識別番号IDiは、他の全ての識別番号との間で桁ごとの論理積を計算すると全ての桁が「0」となるように設定される情報である。識別番号IDiは、例えば、対象部位Piの数と同数の複数の桁数から構成され得る。
識別番号割当部41は、対象部位Piに識別番号IDiを割り当てる部分である。一つの識別番号IDiは、他の全ての識別番号との間で桁ごとの論理積を計算すると全ての桁が「0」となるように設定される情報である。識別番号IDiは、例えば、対象部位Piの数と同数の複数の桁数から構成され得る。
具体的に、識別番号割当部41が図2及び図3にて示した双腕ロボット3及び障害物5の各対象部位に割り当てた識別番号IDiの一例を図5に示す。図5の一例において、識別番号割当部41は、対象部位P1である支持台30に対して識別番号ID1として「10000000」を割り当てている。識別番号割当部41は、対象部位P2である第1リンク部311に対して、識別番号ID2として「01000000」を割り当てている。識別番号割当部41は、対象部位P7である昇降部323に対して、識別番号ID7として「00000010」を割り当てている。更に、識別番号割当部41は、対象部位P7である障害物5に対して、識別番号ID8として「00000001」を割り当てている。ここで挙げた一例においては、双腕ロボット3の各部位および障害物5である対象部位P1〜P8の数が「8」であるため、各識別番号IDiの桁数は「8」となっている。また、ID1からID8までの桁ごとの論理積を計算すると、全ての桁において「0」となる。
他の全ての識別番号との間で桁ごとの論理積を計算すると全ての桁が「0」となるように設定されていれば、識別番号の構成は特に限定されない。例えば、識別番号内に、対象部位に対応しない桁が含まれていてもよい。また、対象部位P1〜Phの数と同数の複数の桁数を上位ビットとし、更に下位ビットを付加した番号を識別番号としてもよい。或いは、逆に、対象部位P1〜Phの数と同数の複数の桁数を下位ビットとし、上位ビットを付加した番号を識別番号としてもよい。更に、複数の桁を一つの対象部位に対応させてもよい。
[判定領域管理部]
判定領域管理部42は、第1の判定領域管理部421と、第2の判定領域管理部421とを有する。
判定領域管理部42は、第1の判定領域管理部421と、第2の判定領域管理部421とを有する。
第1の判定領域管理部421は、干渉判定対象P1〜PhのそれぞれのZ軸上の存在領域を含む領域である第1の判定領域6(図6参照)を設定する。干渉判定対象P1〜PhのZ軸上の存在領域は、干渉判定対象P1〜PhをそれぞれZ軸へ投影した領域である。第1の判定領域管理部421は、第1の判定領域6をu個の複数の線分61m(mは1以上u以下の整数)に区切ると共に、各線分61mに対応する線分対応メモリ要素(以下、単に「メモリ要素」と称す)HOcc[m]から構成される第1のメモリ領域部を格納部43に確保する。線分61mは、第1の判定領域6を所定の領域で分割した場合の第1の判定領域6を構成する小領域である。
図6は、複数の対象部位と第1の判定領域との関係を示す図面である。図6に示す各対象部位P1〜Phとしての支持台30、第1リンク部311、第2リンク部312、昇降部313、第1リンク部321、第2リンク部322、昇降部323及び障害物5は、諸元等格納部431に格納された各対象部位P1〜Phの大きさ(寸法)などによって仮想空間に仮想的に構築されたものに対応するが、説明の便宜上、図2及び図3等と同じ符号を付している。第1の判定領域6は、双腕ロボット3の動作する範囲を包含する空間、若しくは双腕ロボット3の動作する範囲の一部であって干渉判定の対象となる空間をZ軸上に投影した領域である。図6では、一例として、第1の判定領域6を13個の線分611〜6113に区切った場合を示している。図示の都合上、図6では、線分61mに対応するZ軸上の領域を矩形領域で模式的に示している。
第1の判定領域6を区切る線分61mの大きさ(又は線分61mの個数u)は、演算量などに基づいて決定すればよいが、より確実に干渉を判定する観点から、対象部位P1〜PhのZ軸方向の厚さのうち最小の厚さより小さいことが好ましい。
図6における状態において、例えば、対象部位P2である第1リンク部311の存在領域は、線分615,616から構成される領域に対応する。対象部位P3である第2リンク部312の存在領域は、線分617〜線分6111から構成される領域に対応する。更に、対象部位P4である昇降部313の存在領域は、線分614〜線分6111から構成される領域に対応する。
第2の判定領域管理部421は、干渉判定対象P1〜PhのXY平面上の占有領域を含む領域である第2の判定領域7(図7参照)を設定する。干渉判定対象P1〜PhのXY平面の占有領域は、干渉判定対象P1〜PhをそれぞれのXY平面への投影した領域である。第2の判定領域管理部421は、第2の判定領域7を設定し、第2の判定領域7を2次元的に配列されたv個の複数のマス目71n(nは2以上v以下の整数)に区切ると共に、その複数のマス目71nに対応するマス目対応メモリ要素(以下、単に「メモリ要素」と称す)Occ[n]から構成される第2のメモリ領域部を格納部43に確保する。
図7は、複数の対象部位と第2の判定領域との関係を示す図面である。図7に示す各対象部位P1〜Phとしての支持台30、第1リンク部311、第2リンク部312、昇降部313、第1リンク部321、第2リンク部322、昇降部323及び障害物5は、諸元等格納部431に格納された各対象部位P1〜Phの大きさ(寸法)などによって仮想空間に仮想的に構築されたものに対応することは、図6の場合と同様である。第2の判定領域7は、双腕ロボット3の動作する範囲を包含する空間、もしくは双腕ロボット3の動作する範囲の一部であって干渉判定の対象となる空間をXY平面に投影した領域である。図7では、v個のマス目71nがX軸方向にn1列、Y軸方向にn2行で2次元配列されている。なお、v=n1×n2である。第2の判定領域7を区切るマス目71nの大きさ(又は個数v)は、演算量などに基づいて決定すればよい。
図7における状態において、例えば、対象部位P2である第1リンク部311、対象部位P3である第2リンク部312及び昇降部313のそれぞれの占有領域は、第1リンク部311、第2リンク部312及び対象部位P4である昇降部313にそれぞれ付されたハッチングで示される領域である。
格納部43は、諸元等格納部431、判定除外情報格納部432、1次元干渉情報格納部433、占有領域重複情報格納部434及び判定結果格納部435を含んで構成される。
諸元等格納部431は、電源投入後に受け取った双腕ロボット3の諸元、および軌道生成部1からの問い合わせ毎に受け取った移動前後の双腕ロボット3の姿勢情報を格納する部分である。
判定除外情報格納部432は、h個の対象部位P1〜Phの間で、干渉のない立体交差が可能である場合(つまり立体交差する際に干渉を伴わない場合)若しくは自分自身との関係では、干渉判定を行わない(又は干渉が存在しない)ことを予め示す判定除外情報を格納する。判定除外情報CExiは、対象部位P1〜Phの配置関係に基づいて、双腕ロボットシステムの操作者によって予め入力されていてもよいし、諸元等格納部431に格納されている諸元等に基づいて自動生成されてもよい。
具体的に、判定除外情報格納部432に格納した判定除外情報の一例を図8に示す。図8は、図2に示した各対象部位P1〜P8のZ軸方向の長さ(高さ)に基づいたものである。図8の一例において、例えば対象部位P2についての判定除外情報CEx2の値は「00011010」である。これは、対象部位P2(第1リンク部311)は、対象部位P1(支持台30)と、対象部位P2自身と、対象部位P3(第2リンク部312)と、対象部位P6(第2リンク部322)と対象部位P8(障害物5)との間では干渉は生じる可能性はなく、対象部位P4(昇降部313)と対象部位P5(第1リンク部321)と対象部位P7(昇降部323)との間に干渉が生じる可能性があることを示している。
1次元干渉情報格納部433は、第1の判定領域管理部421で設定された第1の判定領域6に対応する第1のメモリ領域部433Aと、対象部位P1〜Ph間の干渉可能性を示す情報である干渉コードを格納する干渉コード格納部433Bとを含む部分である。第1のメモリ領域部433Aは、複数の線分61mに対応するメモリ要素HOcc[m]から構成される。
占有領域重複情報格納部434は、第2の判定領域管理部422で設定された第2の判定領域7に対応する第2のメモリ領域部434Aを含む部分である。第2のメモリ領域部434Aは、複数のマス目71nに対応するメモリ要素Occ[n]から構成される。
判定結果格納部435は、干渉判定処理部44による干渉判定の結果を格納する部分である。
干渉判定処理部44は、格納部43に格納された各情報に基づき干渉判定を行うと共に、干渉判定部4を構成する識別番号割当部41、判定領域管理部42及び格納部43の動作を制御する部分である。干渉判定処理部44は、双腕ロボット3が現在姿勢から目標姿勢へ移動する過程において、一定時間間隔Δt毎の対象部位P1〜Phの配置状態に対して、それぞれ干渉判定を行う。現在姿勢の時刻を基準時刻tsとし、目標姿勢の基準時刻に対する時刻を到達時刻tLとした場合、一定時間間隔Δt毎の時刻tは、t=ts+rΔt(rは、0以上の整数)で表される。Δtの係数rの上限は、時刻tsと時刻tLとの間の時間差を時間間隔Δtで除した数であり、演算量などによって適宜設定すればよい。
双腕ロボット3の第1リンク部311,321、第2リンク部312,322及び昇降部313,323のある姿勢が規定されると、それらと障害物5との一つの配置状態が規定される。従って、双腕ロボット3の現在姿勢が決まると、対象部位P1〜Phの第1の配置状態が決定され、双腕ロボット3の目標姿勢が決まると、対象部位P1〜Phの目標とする第2の配置状態が決定される。そして、時刻tでの双腕ロボット3の姿勢は、その時刻tでの対象部位P1〜Phの配置状態を決定している。
干渉判定処理部44は、図4に示されるように、干渉可能性情報生成部(存在領域重複情報生成部)441、占有領域重複情報生成部442及び判定部443を含んで構成される。以下、干渉判定処理部44を構成する各構成要素について説明する。
[干渉可能性情報生成部]
干渉可能性情報生成部441は、ある時刻tにおける対象部位P1〜Phの配置状態に基づいて、対象部位P1〜PhのZ軸上の存在領域を算出し、その存在領域を含む線分61mを特定する。特定された線分61mに対応するメモリ要素HOcc[m]に、線分61mに存在領域がある対象部位P1〜Phの識別番号ID1〜IDhを対象部位P1〜Phの存在領域の重複状態を示す存在領域重複情報として格納する。この際、一つの線分61mに2つ以上の対象部位P1〜Phの存在領域が重なっている場合には、対象部位P1〜Phのうち存在領域が重複する対象部位の論理和を存在領域重複情報としてメモリ要素HOcc[m]に格納する。なお、「存在領域を含む線分61m」とは、存在領域の少なくとも一部を含む又は存在領域の少なくとも一部がかかっている線分61mである。
干渉可能性情報生成部441は、ある時刻tにおける対象部位P1〜Phの配置状態に基づいて、対象部位P1〜PhのZ軸上の存在領域を算出し、その存在領域を含む線分61mを特定する。特定された線分61mに対応するメモリ要素HOcc[m]に、線分61mに存在領域がある対象部位P1〜Phの識別番号ID1〜IDhを対象部位P1〜Phの存在領域の重複状態を示す存在領域重複情報として格納する。この際、一つの線分61mに2つ以上の対象部位P1〜Phの存在領域が重なっている場合には、対象部位P1〜Phのうち存在領域が重複する対象部位の論理和を存在領域重複情報としてメモリ要素HOcc[m]に格納する。なお、「存在領域を含む線分61m」とは、存在領域の少なくとも一部を含む又は存在領域の少なくとも一部がかかっている線分61mである。
メモリ要素HOcc[m]に格納される存在領域重複情報が2つ以上の識別番号ID1〜IDhの論理和である場合、その論理和を構成する識別番号ID1〜IDhに対応する対象部位P1〜Ph同士は干渉する可能性があることになる。すなわち、存在領域重複情報は対象部位P1〜Phの間の干渉の可能性を示す情報である。
干渉可能性情報生成部441は、対象部位Piの存在領域を含む全ての線分61mに対応するメモリ要素HOcc[m]に格納された存在領域重複情報と、対象部位Piの判定除外情報CExiとの論理積を演算することによって、対象部位Piの干渉コードCIDiを生成する。
すなわち、干渉可能性情報生成部441で生成される対象部位Piに対する干渉コードCIDiは次式で表される。
式(1)中のmiは、m=1〜uのうち対象部位Piの存在領域を含む線分61m及びそれに対応するメモリ要素HOcc[m]を区別するためのmである。すなわち、miは対象部位Piに対してHOcc[m]・IDi≠0が成り立つmである。また、式(1)中のjは、1以上h以下の整数である。なお、式(1)に示されるようなΣ(加算記号)で表される加算演算及び「・」で示される積算演算は、それぞれ論理和及び論理積を表す。
式(1)中のmiは、m=1〜uのうち対象部位Piの存在領域を含む線分61m及びそれに対応するメモリ要素HOcc[m]を区別するためのmである。すなわち、miは対象部位Piに対してHOcc[m]・IDi≠0が成り立つmである。また、式(1)中のjは、1以上h以下の整数である。なお、式(1)に示されるようなΣ(加算記号)で表される加算演算及び「・」で示される積算演算は、それぞれ論理和及び論理積を表す。
干渉可能性情報生成部441は、各対象部位P1〜Phに対して式(1)で示される干渉コードCID1〜CIDhをそれぞれ算出し、干渉コードCID1〜CIDhを干渉コード格納部433Bに格納する。
干渉コードCIDiは、前述したように、対象部位Piの存在領域を含む線分61mに対応するメモリ要素HOcc[m]に格納された存在領域重複情報の論理和に基づいて作成される。メモリ要素HOcc[m]に格納された存在領域重複情報が、2つ以上の識別番号IDiの論理和である場合、前述したように、存在領域重複情報は、その2つ以上の識別番号IDiに対応する対象部位Piが干渉する可能性を示している。従って、干渉コードCIDiも、対象部位Pi間の干渉可能性の情報を含んだ干渉可能性情報でもある。
[占有領域重複情報処理部]
占有領域重複情報生成部442は、ある時刻tにおける対象部位P1〜Phの配置状態に基づいて、対象部位P1〜Phの占有領域を含むマス目71nを特定する。占有領域重複情報生成部442は特定されたマス目71nに対応するメモリ要素Occ[n]に、その特定されたマス目71nに占有領域がある対象部位P1〜Phの識別番号ID1〜IDhを占有領域重複情報として格納する。この際、一つのマス目71nに2つ以上の対象部位P1〜Phの占有領域が重なっている場合には、対象部位P1〜Phのうち占有領域が重複する対象部位の論理和を占有領域重複情報としてメモリ要素Occ[n]に格納する。メモリ要素Occ[n]に、識別番号IDiの論理和が格納される場合、論理和を構成する各識別番号に対応する対象部位同士に干渉の可能性があることを示す。なお、「占有領域を含むマス目71n」とは、占有領域の少なくとも一部を含む又は占有領域の少なくとも一部がかかっているマス目71nである。
占有領域重複情報生成部442は、ある時刻tにおける対象部位P1〜Phの配置状態に基づいて、対象部位P1〜Phの占有領域を含むマス目71nを特定する。占有領域重複情報生成部442は特定されたマス目71nに対応するメモリ要素Occ[n]に、その特定されたマス目71nに占有領域がある対象部位P1〜Phの識別番号ID1〜IDhを占有領域重複情報として格納する。この際、一つのマス目71nに2つ以上の対象部位P1〜Phの占有領域が重なっている場合には、対象部位P1〜Phのうち占有領域が重複する対象部位の論理和を占有領域重複情報としてメモリ要素Occ[n]に格納する。メモリ要素Occ[n]に、識別番号IDiの論理和が格納される場合、論理和を構成する各識別番号に対応する対象部位同士に干渉の可能性があることを示す。なお、「占有領域を含むマス目71n」とは、占有領域の少なくとも一部を含む又は占有領域の少なくとも一部がかかっているマス目71nである。
[判定部]
判定部443は、対象部位P1〜Phのうち、占有領域重複情報生成部442で生成された占有領域重複情報が重複することを示しており且つ干渉コードが干渉の可能性がある、すなわち、重複することを示す対象部位同士が存在する場合は、対象部位P1〜Ph間に「干渉あり」と判定し、それ以外の場合は、対象部位P1〜Ph間に「干渉なし」と判定する。
判定部443は、対象部位P1〜Phのうち、占有領域重複情報生成部442で生成された占有領域重複情報が重複することを示しており且つ干渉コードが干渉の可能性がある、すなわち、重複することを示す対象部位同士が存在する場合は、対象部位P1〜Ph間に「干渉あり」と判定し、それ以外の場合は、対象部位P1〜Ph間に「干渉なし」と判定する。
具体的には、判定部443は、各対象部位Piに対して、その対象部位Piが占有する全てのマス目71nに対応するメモリ要素Occ[n]内の占有領域重複情報各々と干渉コードCIDiとの論理積を演算する。判定部443は、算出された全ての論理積のいずれの桁も「0」の場合は、「干渉なし」と判断し、何れかの論理積において、何れかの桁に「1」が含まれている場合は、「干渉あり」と判断する。
以下、双腕ロボットシステム100の動作の一例について説明すると共に、干渉判定部4の動作の一例について詳細に説明する。
[双腕ロボットシステムの動作]
双腕ロボットシステム100により行われる動作について、図9〜図14を参照しながら説明する。下記の説明は、双腕ロボットシステム100におけるメイン処理と、干渉判定部4による干渉判定処理とに大別される。
双腕ロボットシステム100により行われる動作について、図9〜図14を参照しながら説明する。下記の説明は、双腕ロボットシステム100におけるメイン処理と、干渉判定部4による干渉判定処理とに大別される。
[メイン処理]
まず、双腕ロボットシステム100におけるメイン処理について図9を参照しながら説明する。図9は、双腕ロボットシステム100にて行われるメイン処理を示すフローチャートである。
まず、双腕ロボットシステム100におけるメイン処理について図9を参照しながら説明する。図9は、双腕ロボットシステム100にて行われるメイン処理を示すフローチャートである。
最初に、軌道生成部1の演算部12が、記憶部11に格納されているパラメータを干渉判定部4に送信する。このパラメータは、干渉判定の対象部位Pi毎に設定されるものである(ステップS101)。
次に、軌道生成部1の演算部12が、双腕ロボット3の現在姿勢の情報を読み込む(ステップS102)。双腕ロボット3の現在姿勢の情報は例えばサーボドライバ2a〜2dの現在角度などである。
続いて、軌道生成部1の演算部12が、記憶部11に格納された動作プログラムを解析して、双腕ロボット3の目標姿勢を求める(ステップS103)。
その後、軌道生成部1の演算部12が、双腕ロボット3の現在姿勢と目標姿勢を干渉判定部4に送信する(ステップS104)。
次に、軌道生成部1の演算部12は干渉判定部4からの判定結果信号を待つ(ステップS105)。この間、干渉判定部4では詳細を後述する干渉判定処理が行われ、双腕ロボット3のアーム31,32の移動の全体で部位同士の干渉が生じないかどうかが判定される。そして、干渉判定部4からの判定結果信号が「干渉なし」の場合(ステップS106:YES)、軌道生成部1の演算部12は、表示部(図示せず)に「干渉なし」の表示をさせ(ステップS107)、記憶部11に格納されている動作プログラムを実行させ、双腕ロボット3の動作制御を行う(ステップS108)。
一方で、干渉判定部4からの判定結果信号が「干渉あり」の場合には(ステップS106:NO)、軌道生成部1の演算部12は、表示部(図示せず)に「干渉あり」の表示をさせ(ステップS109)、そのまま処理を終了させる。以上のように軌道生成部1は、双腕ロボット3の移動の全体で部位同士の干渉が生じない判定結果が得られるまでサーボドライバ2a〜2dに指令を送らずに待機し、干渉が生じる場合には双腕ロボット3の各移動部位の移動を開始させない。
[干渉判定処理]
引き続き、干渉判定部4による干渉判定処理について図10を利用して説明する。
引き続き、干渉判定部4による干渉判定処理について図10を利用して説明する。
干渉判定処理では、識別番号割当部41が干渉判定の対象部位P1〜Ph毎に識別番号ID1〜IDhを割り当てる(ステップS201、図5参照)。
次に、判定領域管理部42が、第1及び第2のメモリ領域部433A,434Aを格納部43に確保する(ステップS202)。すなわち、1次元干渉情報格納部433内に第1のメモリ領域部433Aを確保すると共に、占有領域重複情報格納部434に第2のメモリ領域部434Aを確保する。
その後、干渉判定処理部44はステップS104(図9参照)にて軌道生成部1から双腕ロボット3の現在姿勢と目標姿勢とが入力されることを待つ(ステップS203)。現在姿勢の情報は、第1リンク部311,321及び第2リンク部321,322の現在の角度の情報並びに昇降部313,323の現在の昇降状態の情報を含む。同様に、目標姿勢の情報は、第1リンク部311,321及び第2リンク部321,322の目標の角度の情報並びに昇降部313,323の目標の昇降状態の情報を含む。
軌道生成部1から双腕ロボット3の現在姿勢と目標姿勢の入力があった場合に(ステップS203:YES)、ステップS204からステップS214までのループL10を実行する。
ループL10では、干渉可能性情報生成部441が、第1の判定領域6に対応する第1のメモリ領域部433Aを初期化する(ステップS204)。具体的には、メモリ要素HOcc[1]〜HOcc[u]に初期値として0を格納する。
その後、ステップS205とステップS206とのループL11を開始する。ループL11では、まず、干渉可能性情報生成部441が、時刻tの対象部位Piの姿勢情報に基づいて、対象部位Piの存在領域を算出する(ステップS205)。時刻tにおける対象部位Piの存在領域は、諸元等格納部431に格納されている対象部位Piの諸元の他、対象部位Piが昇降部313,323である場合については、例えば、現在の昇降状態と目標の昇降状態との差及び昇降速度に基づいて算出され得る。なお、対象部位Piが移動を伴わない部位である支持台30及び障害物5である場合は、時刻tsから時刻tLのあらゆる時刻tにおいて存在領域は一定であるので、上述した諸元に基づいて存在領域が算出され得る。また、対象部位Piが第1リンク部311,321及び第2リンク部312,322である場合、本実施形態では、Z軸方向の移動は伴わないので、上述した諸元に基づいて存在領域が算出され得る。しかしながら、第1リンク部311,321及び第2リンク部312,322がZ軸方向の移動を伴うような形態では、それらの現在角度と目標角度との角度差と速度パラメータとに基づいて、第1リンク部311,321及び第2リンク部312,322の存在領域が算出され得る。
次に、干渉可能性情報生成部441は、存在領域重複情報を生成する(ステップS206)。
図11を参照して、存在領域重複情報の生成方法を説明する。図11は、干渉コード生成部における存在領域重複情報の生成方法を示すフローチャートである。
存在領域重複情報を生成する場合、ループL20を開始する。ループL20では、干渉可能性情報生成部441は、ステップS205で算出した対象部位Piの姿勢情報に基づいて、メモリ要素HOcc[m]に対応する線分61mが対象部位Piの存在領域を含むか否かを判定する(ステップS206a)。換言すれば、干渉可能性情報生成部441は、対象部位Piの存在領域を含む線分61mを特定する。
メモリ要素HOcc[m]に対応する線分61mが対象部位Piの存在領域を含む場合(ステップS206a:YES)、干渉可能性情報生成部441は、メモリ要素HOcc[m]が初期値か否かを判定する(ステップS206b)。メモリ要素HOcc[m]の値が初期値でない場合(ステップS206b:NO)、すなわち、既に存在領域重複情報として値が格納されておりメモリ要素HOcc[m]の値が0ではない場合、その格納されている値(既存のメモリ要素HOcc[m]の値)と対象部位Piの識別番号IDiとの論理和を存在領域重複情報としてメモリ要素HOcc[m]に格納する(ステップS206c)。メモリ要素HOcc[m]の値が初期値である場合(ステップS206b:YES)、すなわち、メモリ要素HOcc[m]の値が0である場合、対象部位Piの識別番号IDiを存在領域重複情報としてメモリ要素HOcc[m]に格納する(ステップS206d)。
上記ステップ206bと、ステップS206c又はステップS206dとで行われる処理は、メモリ要素HOcc[m]の更新に対応する。
ステップ206a〜ステップ206dをm=uになるまで、すなわち、全てのメモリ要素HOcc[1]〜HOcc[u]に対して繰り返す。これにより、ループL20が終了する。ステップ206aにおいて、メモリ素HOcc[m]に対応する線分61mが対象部位Piの存在領域を含まない場合(ステップS206a:NO)、次の対象部位Pi+1に対してステップS206aを実施することによって、ステップS206a〜ステップS206dを繰り返す。なお、対象部位Pi+1は、対象部位Piの次にステップS206aを実行する対象部位を示す便宜的な記載である。以下、同様に対象部位Pi+1という表記は、便宜的に、対象部位Piのループ処理の次にループ処理を示す対象部位Pi+1を示すものとする。
図6に示したある時刻tにおける対象部位P1〜Phの配置状態及び第1の判定領域6の分割例において、対象部位P4に対してループL20を実行した場合について存在領域重複情報の生成について具体的に説明する。
図6に示した配置状態では、線分614〜6110が対象部位P4の存在領域を含む。従って、ステップS206aに対してメモリ要素HOcc[1]〜HOcc[3]に対応する線分611〜613は対象部位P4の存在領域外と判定される。すなわち、ステップS206aにおいてNOであるため、メモリ要素HOcc[1]〜HOcc[3]は更新されずに、単にメモリ要素HOcc[1]〜HOcc[3]に対してステップS206aが繰り返される。メモリ要素HOcc[4]に対してループL20が実行されると、ステップS206aでYESと判定される。そして、メモリ要素HOcc[4]に対応する線分614は対象部位P1の存在領域を含み、対象部位P2及び対象部位P3の存在領域を含まないため、メモリ要素HOcc[4]には、対象部位P1の識別番号ID1が存在領域重複情報として格納されている。従って、ステップS206bでNOと判定され、ステップS206cにおいて、識別番号ID1と対象部位P4の識別番号ID4との論理和、すなわち、ID1+ID4がメモリ要素HOcc[4]に格納される。
同様に、メモリ要素HOcc[5]〜HOcc[13]までループL20がそれぞれ実行される。その結果、メモリ要素HOcc[5]及びHOcc[6]に格納されている存在領域重複情報は識別番号ID2と識別番号ID4との論理和であり、メモリ要素HOcc[7]〜HOcc[10]に格納される存在領域重複情報は識別番号ID3と識別番号ID4との論理和であり、メモリ要素HOcc[11]〜HOcc[13]には初期値としての0が格納される。
図6に示した場合を例にした上記ループL20を含むループL11が終了した時点において、メモリ要素HOcc[1]〜HOcc[13]に格納されている情報は次の通りである。
HOcc[1]:ID1+ID8
HOcc[2]:ID1+ID8
HOcc[3]:ID1+ID7
HOcc[4]:ID1+ID4+ID7
HOcc[5]:ID2+ID4+ID5+ID7
HOcc[6]:ID2+ID4+ID5+ID7
HOcc[7]:ID3+ID4+ID6+ID7
HOcc[8]:ID3+ID4+ID6+ID7
HOcc[9]:ID3+ID4+ID6+ID7
HOcc[10]:ID3+ID4+ID6
HOcc[11]:0(初期値)
HOcc[12]:0(初期値)
HOcc[13]:0(初期値)
HOcc[1]:ID1+ID8
HOcc[2]:ID1+ID8
HOcc[3]:ID1+ID7
HOcc[4]:ID1+ID4+ID7
HOcc[5]:ID2+ID4+ID5+ID7
HOcc[6]:ID2+ID4+ID5+ID7
HOcc[7]:ID3+ID4+ID6+ID7
HOcc[8]:ID3+ID4+ID6+ID7
HOcc[9]:ID3+ID4+ID6+ID7
HOcc[10]:ID3+ID4+ID6
HOcc[11]:0(初期値)
HOcc[12]:0(初期値)
HOcc[13]:0(初期値)
図10に戻って、干渉判定処理について説明する。
ループL11が終了すると、干渉可能性情報生成部441は、干渉コードCIDiを生成する(ステップS207)。
図12を利用して干渉コードの生成方法について説明する。干渉コードCIDiは、ステップS207aからステップS207dまでのループL30を実行する。ループL30では、干渉コードCIDiを初期化する(ステップ207a)。すなわち、干渉コード格納部433Bに格納されている干渉コードCIDiとして「0」を格納する。その後、ステップS207bとステップS207cとのループL31を実行する。
ループL31では、メモリ要素HOcc[m]に対応する線分61mが対象部位Piの存在領域を含むか否かを判定する(ステップS207b)。メモリ要素HOcc[m] に対応する線分61mが対象部位Piの存在領域を含む場合(ステップS207b:YES)、干渉コードCIDiを更新する(ステップS207c)。すなわち、既存のCIDiとメモリ要素HOcc[m]との論理和を新たなCIDiとする。ステップS207bとステップS207cとをm=uになるまで繰り返す。すなわち、全てのメモリ要素HOcc[1]〜HOcc[u]に対して繰り返す。これにより、ループL31が終了する。メモリ要素HOcc[m] に対応する線分61mが対象部位Piの存在領域を含まない場合(ステップS207b:NO)、次のメモリ要素HOcc[m+1]のステップS207bの処理に移り、ループL31をm=uになるまで繰り返す。なお、メモリ要素HOcc[m+1]は、メモリ要素HOcc[n]の次にステップS207bを実行するメモリ要素を示す便宜的な記載である。
次に、干渉可能性情報生成部441は、ループL31において生成された干渉コードCIDiに判定除外情報CExiを考慮して対象部位Piに対する干渉コードCIDiを得る(ステップS207d)。具体的には、干渉可能性情報生成部441は、ループL31において生成された干渉コードCIDiと判定除外情報CExiとの論理積を対象部位Piに対する干渉コードCIDiとする。これにより、式(1)で示される干渉コードCIDiが生成される。
存在領域重複情報の生成の場合と同様に、図6に示した対象部位P1〜Phの配置状態及び第1の判定領域6の分割例において、対象部位P4に対してループL30を実行した場合における存在領域重複情報の生成について具体的に説明する。
図6に示した配置状態では、線分614〜6110が対象部位P4の存在領域を含む。従って、ステップS207aにおいてメモリ要素HOcc[1]〜HOcc[3]に対応する線分611〜613は対象部位P4の存在領域を含まないと判定される。すなわち、ステップS207bにおいてNOであるため、干渉コードCID4は更新されずに、単にメモリ要素HOcc[1]〜HOcc[3]に対してステップS207bが繰り返される。メモリ要素HOcc[4]に対してループL31が実行されると、ステップS207bでYESと判定される。そして、メモリ要素HOcc[1]〜HOcc[3]に対するループL31の実行では干渉コードCID4は更新されずに初期値のままであるため、ステップS207cにおいて、初期値(すなわち、0)+HOcc[4]を演算し、その結果を干渉コード格納部433Bに格納する。これにより、干渉コード格納部433B内の干渉コードCID4が更新される。すなわち、CID4=HOcc[4]である。
次に、メモリ要素HOcc[5]に対してループL31が実行されると、ステップS207bでYESと判定される。そして、ステップS207cにおいて、既存のCID4とHOcc[5]の論理和を演算し、その結果を干渉コード格納部433Bに格納する。これにより、干渉コード格納部433B内の干渉コードCID4が更新される。すなわち、メモリ要素HOcc[5]に対してループL31が実行された後の干渉コードCID4は、HOcc[4]+HOcc[5]である。
同様にして、残りのメモリ要素HOcc[6]〜[13]に対してループL31が実行され、干渉コードCID4の値は、HOcc[4]〜HOcc[10]の論理和として取得される。
その後、ステップS207dにおいて、HOcc[4]〜HOcc[10]の論理和と判定除外情報CEx4との論理積が演算されて、その結果が、最終的な対象部位P4に対する干渉コードCID4として、干渉コード格納部433B内に格納される。
ステップS207aからステップS207dまでの処理をi=hとなるまで、すなわち全ての干渉判定の対象部位P1〜Phについて繰り返す。これにより、ループL30が終了する。
図10に戻って、干渉判定処理について説明する。
干渉コードCID1〜CIDhを生成すると、占有領域重複情報生成部442が第2の判定領域7に対する第2のメモリ領域部434Aを初期化する(ステップ208)。すなわち、第2の判定領域7を区切るマス目71nに対応するメモリ要素Occ[n]に初期値として0を格納する。
次に、ステップ209からステップS211のループL12を開始する。ループL12では、ステップS209で、占有領域重複情報生成部442が、時刻tの対象部位Piの姿勢情報に基づいて、対象部位Piの占有領域を算出する。この算出は、ステップS205における存在領域の算出と同様の情報に基づいて行い得る。すなわち、占有領域の算出では対象部位PiのXY平面への投影領域を算出するので、存在領域の算出において対象部位PiのZ軸への投影領域の代わりにXY平面への投影領域を算出すればよい。なお、XY平面へ対象部位Piを投影する場合には、昇降部313,323だけではなく、第1リンク部311,321及び第2リンク部312,322も現在姿勢から目標姿勢に向けて移動が伴うので、それらの現在角度と目標角度との角度差と速度パラメータとに基づいて、第1リンク部311,321及び第2リンク部312,322の占有領域(投影領域)が算出され得る。次に、占有領域重複情報生成部442が、占有領域重複情報を生成する(ステップS210)。図13を参照して、占有領域重複情報の生成方法について説明する。図13は、占有領域重複情報の生成処理を示すフローチャートである。
占有領域重複情報を生成する場合、占有領域重複情報生成部442はループL40を実行する。ループL40では、ステップS209で取得した対象部位Piの姿勢情報に基づいて、メモリ要素Occ[n]に対応するメモリ71nが対象部位Piの占有領域を含むか否かを判定する(ステップS210a)。メモリ要素Occ[n]に対応するマス目71nが対象部位Piの占有領域を含む場合(ステップS210a:YES)、占有領域重複情報生成部442は、メモリ要素Occ[n]が初期値か否かを判定する(ステップS210b)。
メモリ要素Occ[n]の値が初期値でない場合(ステップS210b:NO)、すなわち、既に占有領域重複情報として値が格納されている場合、占有領域重複情報生成部442は、その格納されている値(既存のメモリ要素Occ[n]の値)と対象部位Piの識別番号IDiとの論理和を占有領域重複情報としてメモリ要素Occ[n]に格納する(ステップS210c)。メモリ要素Occ[n]の値が初期値である場合(ステップS210b:YES)、占有領域重複情報生成部442は、対象部位Piの識別番号IDiを占有領域重複情報としてメモリ要素Occ[n]に格納する(ステップS210d)。
上記ステップ210bと、ステップS210c又はステップS210dとで行われる処理は、メモリ要素Occ[n]の更新に対応する。
ステップS210a〜S210dをn=vになるまで、すなわち、全てのメモリ要素Occ[1]〜Occ[v]に対して繰り返す。これにより、ループL40が終了する。ステップS210aにおいて、メモリ要素Occ[n]に対応するマス目71nが対象部位Piの占有領域を含まない場合(ステップS210a:NO)、次のメモリ要素Occ[n+1]に対してステップS210aを実施することによって、ステップS210a〜S210dを繰り返す。なお、メモリ要素Occ[n+1]は、メモリ要素Occ[n]の次にステップS210aを実行するメモリ要素を示す便宜的な記載である。
図13を利用して説明した占有領域重複情報の生成処理は、存在領域重複情報の生成における線分61m及びメモリ要素HOcc[m]の代わりに、マス目71n及びメモリ要素Occ[n]を使用する点以外は、同様の処理である。
従って、図14に示したようなある時刻tにおける対象部位P1〜Phの配置状態において、対象部位P7、すなわち、昇降部323の占有領域重複情報を生成すると、対象部位P7の占有領域(図14におけるハッチング領域H1〜H5)のうち、領域H1内のマス目71nに対応するメモリ要素Occ[n]には、識別番号ID7が占有領域重複情報として格納される。領域H2内のマス目71nに対応するメモリ要素Occ[n]には、識別番号ID6と識別番号ID7の論理和が占有領域重複情報として格納される。領域H3内のマス目71nに対応するメモリ要素Occ[n]には、識別番号ID3と識別番号ID7の論理和が占有領域重複情報として格納される。更に、領域H4のマス目71nには、識別番号ID3と識別番号ID2と識別番号ID7の論理和が占有領域重複情報として格納される。更に、領域H5のマス目71nには、識別番号ID3と、識別番号ID4と識別番号ID7との論理和が占有領域重複情報として格納される。
図10に戻って、干渉判定処理について説明する。
占有領域重複情報生成部442が、対象部位Piに対する占有領域重複情報を作成すると、判定部443が、対象部位Piの占有領域を含む全てのマス目71nに対応するメモリ要素Occ[n]、すなわち、ステップS210bと、ステップS210c又はステップS210dにおいて占有領域重複情報が更新された全てのメモリ要素Occ[n]の各々と、干渉コードCIDiとの論理積を演算する(ステップS211)。判定部443は、算出した論理積を判定結果格納部435に格納する。
ステップS209からステップS211までの処理をi=hとなるまで繰り返す。すなわち、全ての対象部位Piについて繰り返す。
ループL12が終了した時点で、判定部443は、ステップS211にて求めたそれぞれの論理積が全ての桁において0か否かを判断する(ステップS212)。具体的には、判定部443は、判定結果格納部435からステップS211にて求めたそれぞれの論理積を読み出し、その論理積が全ての桁において0か否かを判定する。ステップS212の判定にて、全ての論理積が全ての桁において0ならば(ステップS212:YES)、判定部443は「干渉なし」を表す情報を判定結果格納部435に記憶させる(ステップS213)。一方で、ステップS212の判定にて、何れかの論理積に全ての桁において0でないものがある場合には(ステップS212:NO)、判定部443は「干渉あり」を表す情報を判定結果格納部435に記憶させる(ステップS214)。ここでは、判定部443は、干渉結果を判定結果格納部435に記憶させるとしたが、判定結果格納部435を設けずに、諸元等格納部431に格納してもよい。これは、ステップS211における論理演算結果についても同様である。
ステップS212と、ステップS213又はステップS214の処理を終了することによって、ある時刻tにおける対象部位P1〜Phの配置状態に対する干渉判定が終了する。
ステップS204からステップS214までの処理を時間間隔Δt毎の姿勢について、時刻tが時刻tLとなるまで、すなわち目標姿勢まで繰り返す。これにより、ループL10が終了し、双腕ロボット3の各アーム31,32及び昇降部313,323が目標とする姿勢に移行するまでの全ての姿勢における干渉判定処理が終了する。
ループL10が終了した時点で、判定部443は、ステップS213又はステップS214にて判定結果格納部435に記憶された一定時間間隔Δt毎の判定結果情報から最終的な判定結果を求め、これを判定結果信号として軌道生成部1に送る。判定結果は、一定時間間隔Δt毎の判定結果情報の全てが干渉なしなら「干渉なし」、一つでも干渉ありがあれば「干渉あり」となる(ステップS215)。このような干渉判定処理により、双腕ロボット3のアーム31,32が初期の姿勢から目標とする姿勢まで動作する間の一定時間間隔毎の各姿勢について、部位同士の干渉の有無を確認することができる。
なお、ここでは、ステップS212と、ステップS213又はステップS214の処理をループL12の終了後に実行しているが、例えば、ループL12においてステップS211の後に、ステップS212として、ステップS211で演算した論理積が0か否かを判定して、ステップS213又はステップS214を実行してもよい。
以上説明した干渉判定部4及びそれを利用した干渉判定方法では、各対象部位PiをZ軸に投影した各対象部位Piの存在領域の情報に基づいて対象部位間の干渉可能性の情報を含む干渉コードを生成し、その干渉コードと、XY平面内の占有領域の情報とを利用して干渉判定を行っている。その結果、3次元的な対象部位P1〜Phの干渉(衝突を伴う立体交差)の有無を判定し得る。そして、上記のように、鉛直方向(Z軸方向)における対象部位Pi同士の干渉(衝突)の可能性を1次元で評価するので、干渉判定に要する演算量を低減できる。
例えば、双腕ロボットの動作範囲及び障害物を含む仮想的な立方体を干渉判定用の3次元判定領域として設定し、その3次元判定領域の一辺をq個に分割した各セル領域を利用して干渉判定を行う場合、q3の計算量になる。
これに対して、干渉判定部4及びその干渉判定部4による干渉判定方法では、比較のために、k=n=m=qと仮定すると、(q+q2)オーダーの計算量ですむ。
そのため、前述したように、干渉判定に要する演算量が低減され、結果として、干渉判定のための処理時間が短縮される。
更に、対象部位Piに識別番号IDiを割り当てていることによって、存在領域情報及び占有領域重複情報を1変数で表すことが可能である。その結果、各対象部位の全ての組み合わせ、すなわち、h×(h−1)のオーダーで判定処理を行う必要はなく、hのオーダーで判定処理を行うことが可能である。また、第1の判定領域6及び第2の判定領域7をそれぞれ線分61m及びマス目71nで量子化しているので、対象部位P1〜Phの姿勢等を離散的に表すことが可能である。更に、対象部位Piの存在領域の少なくとも一部を含む線分61mに対応するメモリ要素HOcc[m]に格納された存在領域重複情報を利用して干渉コードCIDiを生成しているので、対象部位Piの存在領域にわたる他の対象部位との干渉の可能性(存在領域重複情報)を一つの干渉コードCIDiとして扱い得る。
(第2の実施形態)
図15は、本発明に係る他の実施形態に係る干渉判定部の概略構成図である。干渉判定部(干渉判定装置)8は、干渉判定処理部44の代わりに干渉判定処理部81を備える点で、図4に示した干渉判定部4と相違する。干渉判定部8は、図1に示した双腕ロボットシステム100において、干渉判定部4の代わりに双腕ロボットシステム100に適用され得る部分である。よって、以下では、干渉判定処理部81の構成及びその処理方法を中心にして説明する。
図15は、本発明に係る他の実施形態に係る干渉判定部の概略構成図である。干渉判定部(干渉判定装置)8は、干渉判定処理部44の代わりに干渉判定処理部81を備える点で、図4に示した干渉判定部4と相違する。干渉判定部8は、図1に示した双腕ロボットシステム100において、干渉判定部4の代わりに双腕ロボットシステム100に適用され得る部分である。よって、以下では、干渉判定処理部81の構成及びその処理方法を中心にして説明する。
干渉判定処理部81は、干渉可能性情報生成部811、占有領域重複情報生成部812及び判定部813を備える。
干渉可能性情報生成部811は、対象部位P1〜Phについて順に対象部位Piの存在領域を含む線分61mを特定する。干渉可能性情報生成部811は、対象部位Piに対応する線分61mを特定する毎に、その対象部位Piに対する干渉コードCIDiを生成する。干渉可能性情報生成部811は、対象部位Piの干渉コードCIDiを生成する場合、対象部位Piに対する判定除外情報CExiを考慮して対象部位Piの干渉コードCIDiを生成する。
対象部位Piに存在領域を含む線分61mを特定した後の干渉コードCIDiの生成方法は、第1の実施形態の場合と同じである。ただし、本実施形態では、対象部位P1〜Phの順に対象部位Piの干渉コードCIDiが生成されるので、ある対象部位Piに対する線分61mを特定し、その線分61mに対応するメモリ要素HOcc[m]に格納されている情報は、全ての対象部位P1〜Phの存在領域の情報を反映した情報ではなくて、その対象部位Piまでに干渉コードが生成された対象部位P1〜Pi−1の存在領域の情報のみが反映された情報である。なお、対象部位Pi−1は、対象部位Piの干渉コードを生成する一つ前に干渉コードが生成された対象部位を示す表記である。
占有領域重複情報生成部812は、対象部位Piに対して干渉コードCIDiが生成される毎に、その干渉コードCIDiが生成された対象部位Piの占有領域に対応するマス目71nを特定し、そのマス目71nに対応する占有領域重複情報を生成する。
判定部813は、干渉可能性情報生成部811が、ある対象部位Piに対して干渉コードCIDiを生成し、占有領域重複情報生成部812が、その干渉コードCIDiが生成された対象部位Piの占有領域を含むマス目71nに対応する占有領域重複情報を更新すると、その干渉コードCIDiが生成された対象部位Piの占有領域を含むマス目71nに対応するメモリ要素Occ[n]と干渉コードCIDiとの論理積を演算する。判定部813は、その論理積の結果の何れかの桁に「1」が含まれている場合は、対象部位Piと、それまでに干渉コードを生成した他の対象部位との間に「干渉あり」と判定し、それ以外の場合は、対象部位Piとそれまでに干渉コードを生成した他の対象部位との間に「干渉なし」と判定する。
そして、何れかの対象部位Piにおいて、上記論理積の結果に「1」が含まれている場合は、判定部813は、対象部位P1〜Ph間に「干渉あり」と判定し、それ以外の場合、すなわち、全ての対象部位P1〜Phに対して上記論理積の結果のいずれの桁も「0」である場合は、対象部位P1〜Ph間に「干渉なし」と判定する。
次に、干渉判定部8による干渉判定処理について図16を利用して説明する。図16は、干渉判定部8による干渉判定処理を示すフローチャートである。
最初に、ステップS301〜ステップS303を実行する。ステップS301からステップS303の各ステップの処理は、第1の実施形態の場合のステップS201からS203(図10参照)の各ステップの処理と同様であるため、説明を省略する。
ステップS303において、現在姿勢と目標姿勢の入力があれば、ステップS304からステップS313までのループL50が開始される。ループL50では、判定領域管理部42が、第1のメモリ領域部433Aを初期化する(ステップS304)。具体的には、メモリ要素HOcc[1]〜メモリ要素HOcc[u]に初期値として0(ゼロ)を格納する。次いで、判定領域管理部42が、第2のメモリ領域部434Aを初期化する(ステップS305)。具体的には、メモリ要素Occ[1]〜メモリ要素Occ[n]に初期値として0(ゼロ)を格納する。なお、ステップS304とステップS305と2つのステップに分けて説明しているが、これらのステップS304,305の処理は同じ一つのステップとして実行されてもよく、ステップS304とステップS305の順番は反対でもよい。
次に、干渉判定処理部81は、ループL51を開始する。干渉判定処理部81は、対象部位Piの姿勢を算出する(ステップS306)。ステップS306における、ある時刻tの対象部位Piの姿勢の算出は、対象部位Piが昇降部313,323である場合については、例えば、現在の昇降状態と目標の昇降状態との差及び昇降速度に基づいて算出され得る。なお、対象部位Piが移動を伴わない部位である支持台30及び障害物5である場合は、時刻tsから時刻tLのあらゆる時刻tにおいて姿勢は一定であるので、諸元等格納部431に格納された諸元に基づいて姿勢が算出され得る。また、対象部位Piが第1リンク部311,321及び第2リンク部312,322である場合、本実施形態でも、Z軸方向の移動は伴わないので、上述した諸元に基づいて姿勢が算出され得る。しかしながら、第1リンク部311,321及び第2リンク部312,322がZ軸方向の移動を伴うような形態では、それらの現在角度と目標角度との角度差と速度パラメータとに基づいて、第1リンク部311,321及び第2リンク部312,322の姿勢が算出され得る。
次に、干渉可能性情報生成部811は、干渉コードCIDiを生成する(ステップS307)。干渉コードCIDiの生成方法を、図17を参照して説明する。図17は、干渉コードの生成処理のフローチャートである。
まず、干渉可能性情報生成部811は、干渉コードCIDiを初期化する(ステップS307a)。すなわち、干渉コード格納部433Bの干渉コードCIDiの値を0(ゼロ)に設定する。次に、ステップS307bからステップS307fのループL60を開始する。
ループL60が開始されると、ループL60では、ステップS306で取得した対象部位Piの姿勢情報に基づいて、メモリ要素HOcc[m]に対応する線分61mが対象部位Piの存在領域を含むか否かを判定する(ステップS307b)。
メモリ要素HOcc[m]に対応する線分61mが対象部位Piの存在領域を含む場合、干渉可能性情報生成部811は、メモリ要素HOcc[m]が初期値か否かを判定する(ステップS307c)。メモリ要素HOcc[m]の値が初期値でない場合(ステップS307c:NO)、すなわち、既に存在領域重複情報として値がメモリ要素HOcc[m]に格納されている場合、その格納されている値(既存のメモリ要素HOcc[m]の値)と対象部位Piの識別番号IDiとの論理和を存在領域重複情報としてメモリ要素HOcc[m]に格納する(ステップS307d)。メモリ要素HOcc[m]の値が初期値である場合(ステップS307c:YES)、対象部位Piの識別番号IDiを存在領域重複情報としてメモリ要素HOcc[m]に格納する(ステップS307e)。ステップS307cと、ステップS307d又はステップS307eの処理は、メモリ要素HOcc[m]の更新処理に対応する。
ステップ307d又はステップS307eを実行した後、干渉可能性情報生成部811は、干渉コードCIDiを更新する(ステップS307f)。すなわち、既存の干渉コードCIDiとメモリ要素HOcc[m]との論理和を新たな干渉コードCIDiとする。ステップS307b〜ステップS307fとステップとをm=uになるまで繰り返す。すなわち、全てのメモリ要素HOcc[1]〜HOcc[u]に対して繰り返す。これにより、ループL60が終了する。ステップS307bにおいて、メモリ要素HOcc[m]に対応する線分61mが対象部位Piの存在領域を含まない場合(ステップS307b:NO)、次のメモリ要素HOcc[m+1]に対してステップS307bを実施することによって、ステップS307b〜S307fを繰り返す。なお、メモリ要素HOcc[m+1]は、メモリ要素HOcc[m]の次にステップS307bを実行するメモリ要素を示す便宜的な記載である。
次に、ループL60において生成された干渉コードCIDiに判定除外情報CExiを考慮して対象部位Piに対する干渉コードCIDiを得る(ステップS307g)。具体的には、ループL60において生成された干渉コードCIDiと判定除外情報CExiとの論理積を対象部位Piに対する干渉コードCIDiとする。干渉可能性情報生成部811は、ステップS307gにおいて算出した干渉コードCIDiを干渉コード格納部433Bに格納する。
図16に戻って、干渉判定処理について説明する。
対象部位Piの干渉コードCIDiを生成すると、占有領域重複情報生成部812が占有領域重複情報を生成する。占有領域重複情報の生成方法は、第1の実施形態の場合と同様であるため、説明を省略する。
そして、判定部443は、干渉コード格納部433Bから干渉コードCIDiを読み出し、占有情報を書き込んだ全てのメモリ要素Occ[n]、すなわち、対象部位Piの占有領域を含む全てのマス目71nに対応するメモリ要素Occ[n]の値と、干渉コードCIDiとの論理積を求める(ステップS309)。
これによって、対象部位Piと、対象部位P1〜対象部位Pi−1までの干渉判定が実質的に終了したことになる。
ステップS304からステップS309までの処理をi=hとなるまで繰り返す。すなわち、全ての対象部位Piについて繰り返す。
その後、ステップS310からステップS312を実行する。これにより、ある時刻tにおける双腕ロボット3の姿勢に対する干渉判定が終了する。ステップS310〜ステップS312までの処理は、図10におけるステップS212〜S214までの処理と同様であるため説明を省略する。ステップS304〜ステップS312までの処理を時刻tS〜時刻tLまで、一定時間間隔Δt毎の時刻tに対して繰り返すことによって、ループL50が終了し、双腕ロボット3の各アームが目標とする姿勢に移行するまでの全ての姿勢における干渉判定処理が終了する。
ループL50が終了した時点で、判定部443は、ステップS311又はステップS312にて判定結果格納部435に記憶された一定時間間隔Δt毎の判定結果情報から最終的な判定結果を求め、これを判定結果信号として軌道生成部1に送る(ステップS313)。判定結果は、一定時間間隔Δt毎の判定結果情報の全てが干渉なしなら「干渉なし」となり、一つでも干渉ありがあれば「干渉あり」となる。このような干渉判定処理により、双腕ロボット3のアーム31,32が初期の姿勢から目標とする姿勢まで動作する間の一定時間間隔毎の各姿勢について、部位同士の干渉の有無を確認することができる。
以上説明した第2の実施形態に係る干渉判定装置及び干渉判定方法では、第1の実施形態の場合と同様に、各対象部位PiをZ軸に投影した各対象部位Piの存在領域の情報に基づいて対象部位間の干渉可能性の情報を含む干渉コードを生成し、その干渉コードと、XY平面内の占有情報とを利用して干渉判定を行っている。従って、このように、鉛直方向(Z軸方向)における対象部位Pi同士の干渉(衝突)の可能性を1次元で評価するので、干渉判定に要する演算量を低減できる。
更に、第2の実施形態の干渉可能性情報生成部811では、所定の順番で対象部位Piの干渉コードCIDiを生成している。そして、干渉コードCIDiを生成する場合には、全ての対象部位Piの存在領域を考慮して生成するのではなく、干渉コードCIDiを生成する対象部位Piより前に既に干渉コードが生成された他の対象部位P1〜対象部位pi−1の存在領域のみを考慮して干渉コードCIDiを生成している。そして、対象部位Piの干渉コードを生成すると、判定部813は、その干渉コードCIDiを使用して対象部位Piに対するXY平面の占有情報を考慮した干渉判定を行っている。従って、干渉コードCIDiを、全ての対象部位Piの存在領域を考慮して生成する場合に比べて、効率よく生成できる。その結果、干渉判定の処理時間を短縮できる。
以上、本発明の種々の実施形態について説明したが、本発明は上記実施形態に限定されない。例えば、判定除外情報CExiを設定して、干渉が生じ得ない干渉判定対象部位同士を特定している場合には、占有領域重複情報を生成する際、干渉判定対象部位同士の占有領域が重複していても、干渉が生じ得ない干渉判定対象部位として特定された干渉判定対象部位以外の干渉判定対象部位の識別番号のみを考慮して占有領域重複情報を生成すればよい。例えば、干渉判定対象部位P1と干渉判定対象部位P2と干渉判定対象部位P5との占有領域が重複する場合において干渉判定対象部位P5について占有領域重複情報を生成する場合、干渉判定対象部位P1と干渉判定対象部位P2とは干渉が生じないので、干渉判定対象部位P1,P2,P5の占有領域が重複している領域を含むマス目71nに対応するメモリ要素Occ[n]には、干渉判定対象部位P2の識別番号ID2と干渉判定対象部位P5の識別番号ID5の論理和を占有領域重複情報として格納すればよい。
更に、占有領域重複情報は、着目したある干渉判定対象部位Piについて、その干渉判定対象部位Piの占有領域を含むマス目71nに対応するメモリ要素Occ[n]が初期値の場合はその干渉判定対象部位Piの識別番号IDiとし、メモリ要素Occ[n]が初期値以外の場合はメモリ要素Occ[n]の値とその干渉判定対象部位Piの識別番号IDiの論理和とした。しかしながら、判定除外情報CExiを設定している場合は、占有領域重複情報は、その干渉判定対象部位Piの占有領域を含むマス目71nに対応するメモリ要素Occ[n]に格納されている値、すなわち、初期値又はその干渉判定対象部位Piの識別番号IDiを加算する前の値(情報)でもよい。この場合、上記のように生成した占有領域重複情報と、判定除外情報CExiを考慮した干渉コードCIDiとの論理積を演算することによって、干渉判定を行えば、着目したある干渉判定対象部位Piとその干渉判定対象部位Pi自身との干渉は生じ得ないことが判定除外情報CExiに既に含まれているので、適切に干渉判定を行い得る。
また、判定除外情報CExiを利用せずに、干渉判定を行っても良い。この場合、第1の実施形態における対象部位Piに対する干渉コードCIDiは、式(3)で表される。第2の実施形態における対象部位Piに対する干渉コードCIDiは式(4)で表される。
更に、第1及び第2の実施形態の存在領域重複情報及び占有領域重複情報の生成において、メモリ要素HOcc[m]及びメモリ要素Occ[n]に初期値が含まれているかどうかの判定を行ったが、例えば、初期値として0を格納している場合には、判定せずに、単に、メモリ要素HOcc[m]及びメモリ要素Occ[n]に格納されている値と、そのメモリ要素HOcc[m]及びメモリ要素Occ[n]に対応する線分61m及びマス目71nに存在領域及び占有領域がある干渉判定対象部位Piの識別番号IDiとの論理和を演算し、メモリ要素HOcc[m]及びメモリ要素Occ[n]に格納してもよい。
更に、干渉可能性情報生成部441,811は、存在領域重複情報に基づいて干渉コードCIDiを生成し、判定部413,813は干渉コードCIDiを利用して干渉判定を行ったが、判定部413,813は、干渉コードCIDiの代わりに存在領域重複情報と、占有領域重複情報とを利用して干渉判定を行えばよい。この場合、判定部413,813は、複数の干渉判定対象部位Pのうち存在領域重複情報と占有領域重複情報とがいずれも重複していることを示す少なくとも2つの干渉判定対象部位が存在する場合、それらの干渉判定対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の干渉判定対象部位間に干渉が存在しないと判定すればよい。特に、第2の実施形態に示したように、所定の順番で干渉判定対象部位毎に存在領域重複情報及び占有領域情報を更新して、干渉判定対象部位の存在領域及び占有領域を考慮して存在領域重複情報及び占有領域情報を更新する毎に、その干渉判定対象部位に関する干渉判定を行う場合、その干渉判定対象部位に対する存在領域重複情報においてその干渉判定対象部位と存在領域が重複する干渉判定対象部位に対して2次元平面(XY平面)の占有領域を利用した干渉判定を行えばよい。このように干渉コードCIDiを利用しない場合、干渉可能性情報生成部441,811は、干渉コードCIDiを生成しなくてもよい。
更にまた、第2の実施形態では、判定部813は、全ての干渉判定対象部位Piに対して干渉判定を実施しているが、例えば、所定の順番である干渉判定対象部位Piに対して「干渉あり」という判定をした場合に、軌道生成部1に「干渉あり」の判定結果を送信して干渉判定処理を終了してもよい。これは、時刻t毎の干渉判定においても同様である。すなわち、何れかの時刻tの配置状態で「干渉あり」の判定をした場合には、その時点で干渉判定を終了してもよい。
第1及び第2の実施形態では、双腕ロボット3と障害物5との配置空間としての3次元空間を互いに直交するX軸(第1の軸)、Y軸(第2の軸)及びZ軸(第3の軸)を用いて表したが、これに限定されない。例えば、双腕ロボット3の動作方向に応じて、互いに交差する第1〜第3の軸を設定すればよい。また、X軸、Y軸及びZ軸を設定する場合においても、干渉判定対象部位P1〜Phのうち姿勢の変化する部位の動作に応じて、干渉判定部位P1〜Phを1次元の軸に投影するための第3の軸をZ軸の代わりにX軸又はY軸に設定するようにしてもよい。そして、干渉判定部位P1〜Phの、ある第1の配置状態から第2の配置状態への変形過程と、他の第1の配置状態から第2の配置状態への変形過程とにおいて、干渉判定部位P1〜Phを1次元の軸に投影するための第3の軸を設定する軸は異なっていてもよい。例えば、干渉判定部位P1〜Phのある第1の配置状態から第2の配置状態への変形過程では、第3の軸を第1及び第2の実施形態で説明したように、Z軸として干渉判定を行い、他の第1の配置状態から第2の配置状態への変形過程とでは、第3の軸をX軸又はY軸に設定して行ってもよい。
更に、干渉判定対象部位P1〜Phの存在領域を、Z軸(第3の軸)に設定した第1の判定領域6を複数の線分61mに対応して量子化したが、例えば、干渉判定対象部位P1〜Phの存在領域を各干渉判定対象部位P1〜Phの存在領域をZ軸における上限値及び下限値によって表してもよい。すなわち、例えば干渉判定対象部位P1のZ軸上の存在領域の上限値と下限値とをデータとして取得し、その上限値と下限値との間のZ軸上の領域を存在領域と見なしてもよい。この場合、他の干渉判定対象部位P2〜Phに対しても存在領域を上限値及び下限値で表現し、その存在領域の重複状態を存在領域重複情報として利用してもよい。
更に、第1及び第2の実施形態の干渉判定処理方法では、Z軸上の存在領域を考慮した干渉コードを生成した後に、XY平面の占有領域を考慮して占有領域重複情報を生成しているが、この順番は逆でもよい。
また、第1及び第2の実施形態では、双腕ロボット3の現在姿勢から目標姿勢に移動する移動前に干渉判定を実施するとして説明したが、双腕ロボット3の現在姿勢から目標姿勢の移動過程におけるある時間間隔毎に更に行っても良い。すなわち、双腕ロボット3の現在姿勢から目標姿勢の移動過程におけるある時間間隔毎の姿勢を、新たな現在姿勢として干渉判定を行ってもよい。これにより、例えば、サーボモータ2a〜2d等の駆動部の位相遅れなどによる干渉を回避することが可能である。
第1及び第2の実施形態では、実空間において実際に存在する(実際の装置としての)双腕ロボット3の姿勢の変化を想定して干渉判定を行ったが、干渉判定装置(干渉判定部4,8)は、例えば、実際の装置として作製するためのシミュレーションとして干渉判定を行ってもよい。この場合、干渉判定装置は、シミュレーション条件として干渉判定対象部位P1〜Phの諸元及び移動条件などを入力するキーボード等の入力部と、シミュレーション結果又は干渉判定対象部位P1〜Phの配置状態の時刻毎の変化状態を表示する表示部などを更に備えてよい。第1及び第2の実施形態では、双腕ロボット3の各部位及び障害物5を干渉判定対象部位として設定したが、干渉判定装置は、例えば、ゲームにおける仮想生物及び仮想非生物との衝突(干渉)の判定や、自動車の衝突シミュレーションに適用され得る。
更に、XY平面を分割するマス目71nの形状は正方形に限らず、多角形であればよい。例えば、マス目71nの形状は、矩形(長方形)や平行四辺形といった四角形、更には、三角形や六角形でもよい。
第1及び第2の実施形態では、対象部位Piに「0」及び「1」で構成される識別番号IDiを割り当てたが、対象部位Piの識別番号IDiの構成は、対象部位P1〜Phを識別できれば、第1及び第2の実施形態で例示したものに限定されない。また、これまでの説明では、干渉判定において、干渉判定の対象となる情報(例えば、図10のステップS211の演算結果等)の何れかの桁に「1」が含まれている場合は、「干渉あり」と判定し、それ以外の場合を「干渉なし」と判定するように、識別番号及び初期値を設定した形態を説明した。しかしながら、干渉判定を行う情報において、「干渉あり」(又は「干渉なし」)の状態が区別できれば、それらの表し方は「1」(又は「0」)に限定されない。例えば、「干渉あり」に対応する情報を「0」で表し、「干渉なし」に対応する情報を「1」であらわしてもよいし、複数の桁を利用して「干渉あり」(又は「干渉なし」)を区別してもよい。そして、対象部位に付与する識別番号やメモリ要素HOcc[m],Occ[n]の初期値もそれに応じて設定すればよい。
また、種々変形例を例示したが、各変形例を適宜互いに組み合わせた形態としてもよい。
P1〜P8…対象部位(干渉判定対象部位)、611〜6113…線分、4,8…干渉判定部(干渉判定装置)、5…障害物(干渉判定対象部位)、6…第1の判定領域(1次元判定領域)、7…第2の判定領域(2次元判定領域)、30…支持台(干渉判定部位)、41…識別番号割当部、42…判定領域管理部、71n…マス目、312,322…第2のリンク部(干渉判定部位)、313,323…昇降部(干渉判定部位)、321,322…第2のリンク部(干渉判定部位)、421…第1の判定領域管理部、433A…第1のメモリ領域部、441…干渉可能性情報生成部(存在領域重複情報生成部)、442…占有領域重複情報生成部、443…判定部、811…干渉可能性情報生成部、812…占有領域重複情報生成部、813…判定部。
Claims (8)
- 互いに交差する第1及び第2の軸と、前記第1及び第2の軸を含む2次元平面と交差する第3の軸と、で構成される3次元空間における複数の干渉判定対象部位の干渉を判定する干渉判定装置であって、
各前記干渉判定対象部位を前記第3の軸に投影した各前記干渉判定対象部位の前記第3の軸における存在領域の情報に基づいて、各前記干渉判定対象部位について、当該干渉判定対象部位の存在領域と他の少なくとも一つの前記干渉判定対象部位の存在領域との重複状態を示す存在領域重複情報を生成する存在領域重複情報生成部と、
各前記干渉判定対象部位を前記2次元平面へ投影した前記2次元平面内での各前記干渉判定対象部位の占有領域の情報に基づいて、各前記干渉判定対象部位について、当該干渉判定対象部位の占有領域における他の少なくとも一つの前記干渉判定対象部位の占有領域の重複状態を示す占有領域重複情報を取得する占有領域重複情報生成部と、
複数の前記干渉判定対象部位のうち前記存在領域重複情報と前記占有領域重複情報とがいずれも重複していることを示す少なくとも2つの前記干渉判定対象部位が存在する場合、複数の干渉判定対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の干渉判定対象部位間に干渉が存在しないと判定する、前記判定部と、
を備える干渉判定装置。 - 各前記干渉判定対象部位に識別番号を割り当てる識別番号割当部であって、各前記干渉判定対象部位の前記識別番号は、複数の桁数を有し且つ他の前記識別番号との間での論理積の結果において全ての桁が「0」となるように設定された識別番号である、前記識別番号割当部と、
複数の前記干渉判定対象部位の前記存在領域を含む1次元判定領域を前記第3の軸に設定し、前記1次元判定領域を複数の線分に区切ると共に、各前記線分に対応しており前記存在領域の情報を格納する線分対応メモリ要素を確保する1次元判定領域管理部と、
複数の前記干渉判定対象部位の前記占有領域を含む2次元判定領域を前記2次元平面に設定すると共に、前記2次元判定領域を、2次元的に配列されたマス目に区切ると共に、各前記マス目に対応しており前記占有領域の情報を格納するマス目対応メモリ要素を確保する2次元判定領域管理部と、
を備え、
前記存在領域重複情報生成部は、
複数の前記線分のうち各前記干渉判定対象部位の前記存在領域の少なくとも一部を含む前記線分を特定し、
特定された前記線分に対応する前記線分対応メモリ要素に、
特定された前記線分に前記存在領域が位置する前記干渉判定対象部位が一つの場合は、その干渉判定対象部位の前記識別番号を前記存在領域重複情報として格納し、
特定された前記線分に前記存在領域が位置する前記干渉判定対象部位が2つ以上の場合は、それらの干渉判定対象部位の前記識別番号の論理和を前記存在領域重複情報として格納し、
各前記干渉判定対象部位について、当該干渉判定対象部位の前記存在領域の少なくとも一部を含む全ての前記線分の前記線分対応メモリ要素に格納された前記存在領域重複情報の論理和を干渉コードとして生成し、
前記占有領域重複情報生成部は、
複数の前記マス目のうち各前記干渉判定対象部位の前記占有領域の少なくとも一部を含む前記マス目を特定し、
特定された前記マス目に対応する前記マス目対応メモリ要素に、
特定された前記マス目に前記占有領域が位置する前記干渉判定対象部位が一つの場合は、その干渉判定対象部位の前記識別番号を前記占有領域重複情報として格納し、
特定された前記マス目に前記占有領域が位置する前記干渉判定対象部位が2つ以上の場合は、それらの干渉判定対象部位の前記識別番号の論理和を前記占有領域重複情報として格納し、
前記判定部は、各前記干渉判定対象部位について、
その干渉判定対象部位の前記占有領域の少なくとも一部を含む前記マス目に対応する前記マス目対応メモリ要素に格納された前記占有領域重複情報と、その干渉判定対象部位の前記干渉コードとの論理積を演算し、
少なくとも一つの前記干渉判定対象部位に対する前記占有領域重複情報と前記干渉コードとの論理積の演算結果の何れかの桁に「1」が含まれる場合は、複数の前記干渉判定対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の前記干渉判定対象部位間に干渉が存在しないと判定する、
請求項1記載の干渉判定装置。 - 前記存在領域重複情報生成部は、複数の前記干渉判定対象部位について所定の順番に沿って前記干渉判定対象部位の前記存在領域の少なくとも一部を含む前記線分を特定し、
前記存在領域重複情報生成部は、前記干渉判定対象部位に対応する前記線分を特定する毎に、その干渉判定対象部位に対する前記干渉コードを生成し、
前記占有領域重複情報生成部は、前記所定の順番に沿って順に前記干渉判定対象部位の占有領域に位置する前記マス目を特定し、前記干渉判定対象部位に対応する前記マス目を特定する毎に、特定された前記マス目に格納されている前記占有領域重複情報を更新し、
前記判定部は、前記干渉コードが生成される毎に、生成された前記干渉コードと、前記干渉コードの生成に対応して更新された前記占有領域重複情報との論理積とを演算し、何れかの論理積の演算結果の何れかの桁に「1」が含まれる場合は、数の前記干渉判定対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の前記干渉判定対象部位間に干渉が存在しないと判定する、
請求項2記載の干渉判定装置。 - 各前記干渉判定対象部位についての前記存在領域重複情報は、その干渉判定対象部位との干渉は生じない少なくとも一つの干渉判定対象部位を予め示す干渉判定除外情報と、各前記干渉判定対象部位の存在領域とから生成される、
請求項1記載の干渉判定装置。 - 複数の前記干渉判定対象部位は、前記3次元空間内で複数の前記干渉判定対象部位の配置状態が第1の配置状態から第2の配置状態に変形可能であって、前記第1の配置状態から前記第2の配置状態への変形過程における一定の時間間隔毎の複数の前記干渉判定対象部位の配置状態に対して、
前記存在領域重複情報生成部は、前記時間間隔毎の複数の前記干渉判定対象部位の配置状態における各前記干渉判定対象部位の前記存在領域重複情報を生成し、
前記占有領域重複情報生成部は、前記時間間隔毎の複数の前記干渉判定対象部位の配置状態における各前記干渉判定対象部位について前記占有領域重複情報を取得し、
前記判定部は、前記時間間隔毎に生成された前記存在領域重複情報と前記占有領域量副情報とに基づいて複数の前記干渉判定対象部位の干渉の有無を判断し、全ての前記時間間隔の何れかにおいて干渉が存在する場合は、複数の前記干渉判定対象部位の配置状態の前記第1の配置状態から前記第2の配置状態への変形過程において干渉が存在すると判定し、全ての前記時間間隔の何れかにおいても干渉が存在しない場合は、前記変形過程において干渉が存在しないと判定する、
請求項1〜3の何れか一項記載の干渉判定装置。 - 互いに交差する第1及び第2の軸と、前記第1及び第2の軸を含む2次元平面と交差する第3の軸と、で構成される3次元空間における複数の干渉判定対象部位の干渉を干渉判定装置が判定する干渉判定方法であって、
各前記干渉判定対象部位を前記第3の軸に投影した各前記干渉判定対象部位の前記第3の軸における存在領域の情報に基づいて、各前記干渉判定対象部位について、その前記干渉判定対象部位の存在領域と他の少なくとも一つの前記干渉判定対象部位の存在領域との重複状態を示す存在領域重複情報を前記干渉判定装置が生成するステップと、
各前記干渉判定対象部位を前記2次元平面へ投影した前記2次元平面内での各前記干渉判定対象部位の占有領域の情報に基づいて、各前記干渉判定対象部位について、その前記干渉判定対象部位の占有領域における他の少なくとも一つの前記干渉判定対象部位の占有領域の重複状態を示す占有領域重複情報を前記干渉判定装置が生成するステップと、
複数の干渉判定対象部位の干渉のうち前記存在領域重複情報と前記占有領域重複情報とがいずれも重複することを示す少なくとも2つの前記干渉判定対象部位が存在する場合、複数の干渉判定対象部位間に干渉が存在すると前記干渉判定装置が判定し、それ以外の場合は、複数の干渉判定対象部位間に干渉が存在しないと前記干渉判定装置が判定するステップと、
を備える干渉判定方法。 - 各前記干渉判定対象部位に識別番号を割り当てるステップであって、各前記干渉判定対象部位の前記識別番号は、複数の桁数を有し且つ他の前記識別番号との間での論理積の結果において全ての桁が「0」となるように設定された識別番号である、前記ステップと、
複数の前記干渉判定対象部位の前記存在領域を含む1次元判定領域を前記干渉判定装置が前記第3の軸に設定し、前記1次元判定領域を複数の線分に区切って、各前記線分に対応しており前記存在領域の情報を格納する線分対応メモリ要素を前記干渉判定装置が確保するステップと、
複数の前記干渉判定対象部位の前記占有領域を含む2次元判定領域を前記2次元平面に前記干渉判定装置が設定し、前記2次元判定領域を、2次元的に配列されたマス目に区切り、各前記マス目に対応しており前記占有領域の情報を格納するマス目対応メモリ要素を前記干渉判定装置が確保するステップと、
を更に備え、
前記存在領域重複情報を生成するステップは、
複数の前記線分のうち各前記干渉判定対象部位の前記存在領域の少なくとも一部を含む前記線分を特定し、
特定された前記線分に対応する前記線分対応メモリ要素に、
特定された前記線分に前記存在領域が位置する前記干渉判定対象部位が一つの場合は、その干渉判定対象部位の前記識別番号を前記存在領域重複情報として格納し、
特定された前記線分に前記存在領域が位置する前記干渉判定対象部位が2つ以上の場合は、それらの干渉判定対象部位の前記識別番号の論理和を前記存在領域重複情報として格納し、
各前記干渉判定対象部位について、その干渉判定対象部位の前記存在領域の少なくとも一部を含む全ての前記線分の前記線分対応メモリ要素に格納された前記存在領域重複情報の論理和を干渉コードとして生成し、
前記占有領域重複情報を生成するステップは、
複数の前記マス目のうち各前記干渉判定対象部位の前記占有領域の少なくとも一部を含む前記マス目を特定し、
特定された前記マス目に対応する前記マス目対応メモリ要素に、
特定された前記マス目に前記占有領域が位置する前記干渉判定対象部位が一つの場合は、その干渉判定対象部位の前記識別番号を前記占有領域重複情報として格納し、
特定された前記マス目に前記占有領域が位置する前記干渉判定対象部位が2つ以上の場合は、それらの干渉判定対象部位の前記識別番号の論理和を前記占有領域重複情報として格納し、
前記判定するステップは、各前記干渉判定対象部位について、
その干渉判定対象部位の前記占有領域の少なくとも一部を含む前記マス目に対応する前記マス目対応メモリ要素に格納された前記占有領域重複情報と、その干渉判定対象部位の前記干渉コードとの論理積を演算し、
少なくとも一つの前記干渉判定対象部位に対する前記占有領域重複情報と前記干渉コードとの論理積の演算結果の何れかの桁に「1」が含まれる場合は、複数の前記干渉判定対象部位間に干渉が存在すると判定し、それ以外の場合は、複数の前記干渉判定対象部位間に干渉が存在しないと判定する、
請求項1記載の干渉判定方法。 - 互いに交差する第1及び第2の軸と、前記第1及び第2の軸を含む2次元平面と交差する第3の軸と、で構成される3次元空間における複数の干渉判定対象部位の干渉を干渉判定装置が判定する干渉判定方法であって、
各前記干渉判定対象部位に識別番号を前記干渉判定装置が割り当てるステップであって、各前記干渉判定対象部位の前記識別番号は、複数の桁数を有し且つ他の前記識別番号との間での論理積の結果において全ての桁が「0」となるように設定された識別番号である、前記ステップと、
複数の前記干渉判定対象部位の前記存在領域を含む1次元判定領域を前記第3の軸に前記干渉判定装置が設定し、前記1次元判定領域を複数の線分に区切って、各前記線分に対応しており前記存在領域の情報を格納する線分対応メモリ要素を前記干渉判定装置が確保するステップと、
全ての前記線分対応メモリ要素に初期値として「0」を格納して前記線分対応メモリ要素を前記干渉判定装置が初期化するステップと、
複数の前記干渉判定対象部位の前記占有領域を含む2次元判定領域を前記2次元平面に前記干渉判定装置が設定し、前記2次元判定領域を、2次元的に配列されたマス目に区切り、各前記マス目に対応しており前記占有領域の情報を格納するマス目対応メモリ要素を前記干渉判定装置が確保するステップと、
全ての前記マス目対応メモリ要素に初期値として「0」を格納して前記マス目対応メモリ要素を前記干渉判定装置が初期化するステップと、
複数の前記線分のうち前記干渉判定対象部位の前記存在領域の少なくとも一部を含む前記線分に対応する前記線分対応メモリ要素に、初期値が格納されている場合は、その干渉判定対象部位の前記識別番号を前記干渉判定装置が前記存在領域重複情報として格納し、その線分対応メモリ要素に初期値以外の値が格納されている場合は、前記干渉判定対象部位の前記識別番号と、その線分対応メモリ要素に格納されている値との論理和を前記干渉判定装置が前記存在領域重複情報として格納するステップと、
前記干渉判定対象部位の前記存在領域の少なくとも一部を含む全ての前記線分の前記線分対応メモリ要素に格納された前記存在領域重複情報の論理和を前記干渉判定装置が干渉コードとして生成するステップと、
複数の前記マス目のうち前記干渉判定対象部位の前記占有領域の少なくとも一部を含む前記マス目に対応する前記マス目対応メモリ要素に初期値が格納されている場合は、その干渉判定対象部位の前記識別番号を前記干渉判定装置が前記占有領域重複情報として格納し、そのマス目対応メモリ要素に初期値以外の値が格納されている場合は、前記干渉判定対象部位の前記識別番号と、前記線分対応メモリ要素に格納されている値との論理和を前記干渉判定装置が前記占有領域重複情報として格納するステップと、
前記干渉判定対象部位の前記占有領域の少なくとも一部を含む前記マス目に対応する前記マス目対応メモリ要素に格納された前記占有領域重複情報と、その干渉判定対象部位の前記干渉コードとの論理積を前記干渉判定装置が演算し、その論理積の演算結果の何れかの桁に「1」が含まれる場合は、その干渉判定対象部位と他の少なくとも一つの前記干渉判定対象部位との間に干渉が存在すると前記干渉判定装置が判定し、それ以外の場合は、その干渉判定対象部位と他の少なくとも一つの前記干渉判定対象部位との間に干渉がしなしと前記干渉判定装置が判定するステップと、
を備え、
所定の順番で複数の前記干渉判定対象部位毎に、前記線分対応メモリ要素を初期化するステップと、前記マス目対応メモリ要素を初期化するステップと、前記存在領域重複情報として格納するステップと、前記干渉コードとして生成するステップと、前記占有領域重複情報として格納するステップと、前記判定するステップとを、前記干渉判定装置が実行する、
干渉判定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011287407A JP2013136109A (ja) | 2011-12-28 | 2011-12-28 | 干渉判定装置及び干渉判定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011287407A JP2013136109A (ja) | 2011-12-28 | 2011-12-28 | 干渉判定装置及び干渉判定方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2013136109A true JP2013136109A (ja) | 2013-07-11 |
Family
ID=48912308
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2011287407A Pending JP2013136109A (ja) | 2011-12-28 | 2011-12-28 | 干渉判定装置及び干渉判定方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2013136109A (ja) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2014035722A (ja) * | 2012-08-10 | 2014-02-24 | Omron Corp | データ表示装置、方法、プログラム |
JP2017087302A (ja) * | 2015-11-02 | 2017-05-25 | セイコーエプソン株式会社 | 制御装置、ロボットおよびロボットシステム |
WO2022153994A1 (ja) | 2021-01-14 | 2022-07-21 | 川崎重工業株式会社 | 双腕ロボットシステム |
WO2023089817A1 (ja) * | 2021-11-22 | 2023-05-25 | 三菱電機株式会社 | 情報処理装置、シミュレーションシステム、シミュレーション方法、及びシミュレーションプログラム |
-
2011
- 2011-12-28 JP JP2011287407A patent/JP2013136109A/ja active Pending
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2014035722A (ja) * | 2012-08-10 | 2014-02-24 | Omron Corp | データ表示装置、方法、プログラム |
JP2017087302A (ja) * | 2015-11-02 | 2017-05-25 | セイコーエプソン株式会社 | 制御装置、ロボットおよびロボットシステム |
WO2022153994A1 (ja) | 2021-01-14 | 2022-07-21 | 川崎重工業株式会社 | 双腕ロボットシステム |
KR20230127332A (ko) | 2021-01-14 | 2023-08-31 | 가와사끼 쥬고교 가부시끼 가이샤 | 쌍완 로봇 시스템 |
WO2023089817A1 (ja) * | 2021-11-22 | 2023-05-25 | 三菱電機株式会社 | 情報処理装置、シミュレーションシステム、シミュレーション方法、及びシミュレーションプログラム |
JPWO2023089817A1 (ja) * | 2021-11-22 | 2023-05-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11256240B2 (en) | Planning and adapting projects based on a buildability analysis | |
JP7009051B2 (ja) | レイアウト設定方法、制御プログラム、記録媒体、制御装置、部品の製造方法、ロボットシステム、ロボット制御装置、情報処理方法、情報処理装置 | |
CN102814813B (zh) | 在多机器人系统中自动防止死锁的方法和系统 | |
JP6378783B2 (ja) | アーム型のロボットの障害物自動回避方法及び制御装置 | |
JP3604005B2 (ja) | アセンブリワークセルをレイアウトするための最適化ツール | |
JP7439206B2 (ja) | 情報処理方法、情報処理装置、プログラム、記録媒体、生産システム、ロボットシステム、物品の製造方法 | |
US20140025203A1 (en) | Collision detection system, collision detection data generator, and robot | |
CN109760040B (zh) | 干扰判定方法、干扰判定系统以及存储介质 | |
JP2013136109A (ja) | 干渉判定装置及び干渉判定方法 | |
US20220019939A1 (en) | Method and system for predicting motion-outcome data of a robot moving between a given pair of robotic locations | |
CN114611362B (zh) | 一种大型器械工作面的安装调试方法、电子装置及介质 | |
Ragel et al. | Comparison of motion planning techniques for a multi-rotor UAS equipped with a multi-joint manipulator arm | |
US11886174B2 (en) | Virtualized cable modeling for manufacturing resource simulation | |
JP2021169149A (ja) | 分解ベースのアセンブリ計画 | |
JP7475841B2 (ja) | 情報処理方法、ロボットシステム、物品の製造方法、および情報処理装置 | |
JP2023084115A (ja) | 点集合の干渉チェック | |
CN110948489B (zh) | 一种带电作业机器人安全工作空间限定方法及系统 | |
CN110625612A (zh) | 基于ros的可视化带电作业机器人碰撞检测方法 | |
JP2021068108A (ja) | 3次元仮想画像データ処理システム | |
CN117519156B (zh) | 带电作业机器人地面定位优化方法、系统、终端及介质 | |
JP2004127190A (ja) | ロボットの設計支援装置 | |
US20230226693A1 (en) | Projecting safety-related monitoring for a multi-axis kinematic system with multiple movable segments | |
JP2012006100A (ja) | 干渉判定装置および干渉判定方法 | |
Baizid et al. | Virtual reality based tool for optimal robot placement in robotized site based on CAD’s application programming interface | |
JP6932618B2 (ja) | ロボット教示方法、ロボット教示装置、ロボットシステム、プログラム及び記録媒体 |