干渉判定装置および干渉判定方法
【課題】干渉のない立体交差を考慮しつつも、干渉判定を短時間で行う。
【解決手段】
識別番号を各部位に割り当てる識別番号割当部４２１と、干渉情報を格納する干渉情報格納部４１２と、干渉条件情報を格納する干渉条件情報格納部４１３と、ロボットの動作する空間を２次元平面上に投影した上で、２次元平面をｎ１×ｎ２のマス目に区切るマス目管理部４２４と、対応マス目を特定し、占有情報を対応マス目ごとに記録する占有情報記録部４２５と、占有情報および干渉条件情報の桁ごとの論理積を求め、論理積の全ての桁が「０」なら干渉無しと判断し、少なくとも一つの桁が「１」なら干渉有りと判断する判断部４２６と、を備える。干渉情報は、立体交差する際に干渉を伴わないなら「０」、干渉を伴うなら「１」を表す。
【解決手段】
識別番号を各部位に割り当てる識別番号割当部４２１と、干渉情報を格納する干渉情報格納部４１２と、干渉条件情報を格納する干渉条件情報格納部４１３と、ロボットの動作する空間を２次元平面上に投影した上で、２次元平面をｎ１×ｎ２のマス目に区切るマス目管理部４２４と、対応マス目を特定し、占有情報を対応マス目ごとに記録する占有情報記録部４２５と、占有情報および干渉条件情報の桁ごとの論理積を求め、論理積の全ての桁が「０」なら干渉無しと判断し、少なくとも一つの桁が「１」なら干渉有りと判断する判断部４２６と、を備える。干渉情報は、立体交差する際に干渉を伴わないなら「０」、干渉を伴うなら「１」を表す。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、干渉判定装置および干渉判定方法に関するものである。
【背景技術】
【０００２】
先端に昇降軸を持つ二台の水平多関節アームを人間の両腕のように配置した双腕ロボットは、設置に要するスペースの割に自由度が高く、また両腕の作業領域の少なくとも一部が重複していることからワークを押さえつつねじ締めを行うなどの複合作業が可能であるという利点がある。しかし、各アームは移動経路で衝突する危険があり、作業者はその動きを予測することが困難である。そこで、双腕ロボットの各アームが他方のアームの作業領域に一部でも重複するような作業領域内で移動する際、互いのアームが相手の移動軌道上に重なるかどうかという干渉判定が必要となる。また、双腕ロボットが実際に作業を行う上で、左右のアーム同士以外にも、治具や安全柵などの障害物との干渉が生じる可能性があり、水平多関節アームと障害物の干渉判定も必要になる場合がある。
【０００３】
干渉判定を行う方法としては、いくつかの方式が存在する。例えば、工作機械を制御する数値制御装置で干渉チェックを行うために、「複数の機械構造物の干渉領域を各々定義し、前記干渉領域を移動させ、前記各々の機械構造物の干渉領域がそれぞれ干渉するかをチェックする機能を有する数値制御装置について、前記干渉領域として、直方体、円筒、平面の立体図形で定義し、各々の立体図形が交差するか交差しないかにより干渉をチェックする」という方式がある（特許文献１参照）。また、干渉判定を行う他の方法として、水平多関節ロボットに対して、「３次元ロボットモデルを２次元平面上に投影した２次元ロボットモデルに変換」し、「３次元障害物モデルを２次元障害物モデルとして分割」したものとの干渉判定を行う方式も存在する（特許文献２参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００９−０５４０４３号公報
【特許文献２】特開２００４−２０２６２７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
上記の特許文献１の発明のように、３次元方向の運動を行う３次元物体に対する干渉判定は、定義が容易な３次元の単純近似モデルに変換して行われることが多い。しかしながら、双腕ロボットの２次元方向の運動のみを考慮する（ロボットアームの昇降軸の動作と治具の上昇下降を考えない）場合に、これを３次元モデルで扱うことは処理時間の短縮やメモリ確保の観点で有益ではない。
【０００６】
また、特許文献２の発明においては、障害物モデルの分割は３次元障害物モデルの高さ毎に行われ、水平多関節ロボットの設置面からアームまでの高さ（高さは一定であるが、ある厚みを持つ）と一致する障害物モデルのみが干渉判定の対象となる。そのため、２次元ロボットモデルを用いているにもかかわらず、立体交差が可能な部分を干渉判定から除外した結果を短時間で得ることができる。この方式の優れている点は、水平多関節ロボットの高さと一致する分割面のみを判定すればよいという点である。しかしながら、例えば水平多関節ロボットを複数の分割面に分けなければならないような場合には、それぞれの分割面で干渉判定を行わなければならない。双腕ロボットもこれに該当し、ロボットアームを構成する各部分（第１リンク、第２リンク、先端の昇降軸など）で異なる高さを持つために干渉判定を行うべき分割面が多数になる可能性があり、このような場合には、演算量が増加し、処理に長時間を要するという問題がある。また、特許文献２の発明においては、２次元平面上に投影した２次元ロボットモデルの占有領域の重複だけで干渉判定を行っており、干渉のない立体交差は存在しないことを前提としている。つまり、特許文献２の場合において、干渉のない立体交差は考慮の対象に入っていない。しかし、干渉のない立体交差があるか否かを更に考慮に入れることにより、干渉判定の精度が更に向上することが期待される。
【０００７】
そこで、本発明は上記に鑑みてなされたもので、干渉のない立体交差を考慮しつつも、干渉判定を短時間で行うことが可能な干渉判定装置および干渉判定方法を提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記課題を解決するために、本発明の干渉判定装置は、３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定装置であって、複数の桁数を有し且つ他の識別番号との間で桁ごとの論理積を計算すると全ての桁が「０」となるように設定された識別番号を、前記各部位に割り当てる識別番号割当手段と、前記各部位のうち移動を伴う部位である移動部位が、他の前記部位との間で立体交差する際に干渉を伴わない場合には「０」、立体交差する際に干渉を伴う場合には「１」を表す情報を干渉情報として格納する干渉情報格納手段と、前記干渉情報に基づき設定され、前記複数の桁数を有し、前記移動部位が前記他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である干渉条件情報を、前記移動部位ごとに格納する干渉条件情報格納手段と、前記ロボットの動作する空間を２次元平面上に投影した上で、前記２次元平面をｎ１×ｎ２のマス目に区切るとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域を確保するマス目管理手段と、前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目を特定し、前記各部位の前記識別番号に基づく情報であり前記複数の桁数を有する占有情報を前記対応マス目に対応する前記メモリ領域に記録する占有情報記録手段と、前記占有情報および前記干渉条件情報における桁ごとの論理積を前記対応マス目ごとに求め、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断し、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断する判断手段と、を備えることを特徴とする。
【０００９】
また、本発明の干渉判定方法は、３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定方法であって、複数の桁数を有し且つ他の識別番号との間で桁ごとの論理積を計算すると全ての桁が「０」となるように設定された識別番号が、前記各部位に割り当てられ、前記各部位のうち移動を伴う部位である移動部位が、他の前記部位との間で立体交差する際に干渉を伴わない場合には「０」、立体交差する際に干渉を伴う場合には「１」を表す情報が、干渉情報として格納され、前記干渉情報に基づき設定され、前記複数の桁数を有し、前記移動部位が前記他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である干渉条件情報が、前記移動部位ごとに格納され、前記ロボットの動作する空間が２次元平面上に投影された上で、前記２次元平面がｎ１×ｎ２のマス目に区切られるとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域が確保され、前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目が特定され、前記各部位の前記識別番号に基づく情報であり前記複数の桁数を有する占有情報が前記対応マス目に対応する前記メモリ領域に記録され、前記占有情報および前記干渉条件情報における桁ごとの論理積が前記対応マス目ごとに求められ、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断され、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断される、ことを特徴とする。
【００１０】
このような本発明の干渉判定装置および干渉判定方法では、識別番号、干渉情報、干渉条件情報、および占有情報が各部位間の干渉を判定するための必須の情報として用いられる。このうち、特に干渉情報および干渉条件情報は、移動部位が他の部位との間で立体交差する際に干渉を伴わない場合の値および干渉を伴う場合の値に基づいた情報である。したがって、これら干渉情報および干渉条件情報を用いることにより、本発明による干渉判定の結果には各部位間の立体交差時の干渉の有無が考慮されることとなる。これにより、干渉判定の精度が向上される。
【００１１】
また、本発明においては、ロボットの動作する空間を２次元平面上に投影した上で、干渉判定を行うため、例えば３次元での干渉判定に比べ、干渉判定処理を短時間で行うことが可能となる。また、本発明で用いる干渉情報は「０」または「１」を表す情報であり、そのデータ量が少ないことから、装置全体における計算量や計算時間が短縮できる。
【００１２】
また、本発明で用いる識別番号、干渉条件情報、および占有情報はロボットおよび障害物の各部位の数と同数の桁数を有する情報であるため、例えば部位の数が増えた場合には、増えた部位に関する干渉条件を設定した上で、増えた部位の数だけ識別番号、干渉条件情報、および占有情報の桁数を増やして、本発明における干渉判定を行うことができる。このように、本発明によれば、部位の数が増えた場合に、柔軟な対応が可能である。また、「０」か「１」かの少ないデータ量で計算を行うことから、部位の数が増えたことに伴って、装置全体の計算量や計算時間が極端に増えることを防止できる。
【００１３】
また、本発明において、前記干渉条件情報格納手段は、下記の数式（１）により計算される情報Ｃｉを前記干渉条件情報として格納しても良い。
【数１】
ただし、ｈは前記各部位の数であり、ｉおよびｊは１以上ｈ以下の自然数であり、Ｃｉはｉ番目の部位の前記干渉条件情報であり、ＩＤｊはｊ番目の部位の前記識別番号であり、ｔｉｊはｉ番目の部位とｊ番目の部位における前記干渉情報である。
【００１４】
この発明によれば、干渉条件情報を計算するための具体的な手法が提供される。
【００１５】
また、本発明において、前記占有情報記録手段は、前記メモリ領域が初期化されている場合には、前記識別番号を前記占有情報として前記メモリ領域に記録し、前記メモリ領域が初期化されていない場合には、既存の占有情報と前記識別番号との間の桁ごとの論理和を前記占有情報として前記メモリ領域に記録しても良い。
【００１６】
この発明によれば、占有情報を記録するための具体的な手法が提供される。
【００１７】
また、本発明においては、前記ロボットの動作における動作開始時刻と動作終了時刻との間が一定時間間隔ごとに区切られ、当該区切られた各時刻における前記各部位の位置に対して、前記占有情報記録手段は、前記各時刻における前記対応マス目を特定し、前記占有情報を前記対応マス目に対応する前記メモリ領域に記録し、前記判断手段は、前記占有情報および前記干渉条件情報における桁ごとの論理積を前記対応マス目ごとに求め、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断し、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断しても良い。
【００１８】
この発明によれば、ロボットの動作における動作開始時刻と動作終了時刻との間の各時刻に対して干渉判定が行われる。これにより、動作中の全ての時刻において干渉が生じないことを判断した上で、安全にロボットを動作させることができる。
【００１９】
また、本発明において、前記ロボットは、水平多関節アームを有する双腕ロボットであっても良い。
【００２０】
この発明は、ロボットが水平多関節アームを有する双腕ロボットである場合に特に有用である。
【００２１】
また、本発明において、前記識別番号、前記干渉条件情報、および前記占有情報は、前記各部位の数と同数の前記複数の桁数を有する情報であっても良い。
【００２２】
この発明は、識別番号、干渉条件情報、および占有情報が、ロボットおよび障害物の各部位の数と同数の桁数を有することから、各情報が必要最小限の桁数を有することとなり、演算に無駄がなく、必要最小限のメモリ量を用いて演算を行うことができる。
【００２３】
また、本発明は、３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定装置であって、前記ロボットの動作する空間を２次元平面上に投影した上で、前記２次元平面をｎ１×ｎ２のマス目に区切るとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域を確保するマス目管理手段と、前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目を部位毎に特定し、前記各部位が前記対応マス目を占有するか否かを示す占有情報を前記部位毎に順次前記対応マス目に対応する前記メモリ領域に記録する占有情報記録手段と、前記占有情報を前記メモリ領域に記録するときに、そのメモリ領域に既に別の部位の前記占有情報が記録されている場合には、それらの部位同士が干渉すると判断する判断手段と、を備えることを特徴とする。
【００２４】
このような本発明の干渉判定装置は、マス目管理手段、占有情報記録手段、および判断手段のみを必須の構成要素とする簡素な構成となり、このような構成により、簡易的な干渉判定処理が可能となる。この簡易的な干渉判定処理は、干渉判定対象部位が少なく、２次元平面上での動作（例えばアームの旋回動作）だけ考慮すれば良いロボットおよび障害物に対して特に有効である。すなわち、マス目を使って２次元平面上での干渉判定を行うため、３次元的な空間やモデルを用いて干渉判定を行う場合に比べて極めて少ないデータ量で計算ができ、干渉判定を高速化でき、ロボットの２次元方向の運動のみを考慮する（ロボットアームの昇降軸の動作や冶具の上昇下降がない）場合に特に有効である。
【発明の効果】
【００２５】
本発明によれば、干渉のない立体交差を考慮しつつも、干渉判定を短時間で行うことが可能な干渉判定装置および干渉判定方法を提供することができる。
【図面の簡単な説明】
【００２６】
【図１】双腕ロボットシステム１００の構成概要図である。
【図２】双腕ロボット３の側面概要図である。
【図３】双腕ロボット３の上面概要図である。
【図４】干渉判定部４の機能的構成要素を示したブロック図である。
【図５】識別番号割当部４２１が双腕ロボット３および障害物５の各部位に割り当てた識別番号ＩＤｉの一例を示す図である。
【図６】干渉情報生成部４２２が作成したテーブルの一例を示す図である。
【図７】干渉条件情報生成部４２３により生成された干渉条件情報の一例を示す図である。
【図８】干渉条件情報生成部４２３により生成された干渉条件情報の一例を示す図である。
【図９】マス目管理部４２４により作成された判定領域６の一例を示す図である。
【図１０】双腕ロボットシステム１００にて行われるメイン処理を示すフローチャートである。
【図１１】干渉判定部４による干渉判定処理を示すフローチャートである。
【図１２】図１１のステップＳ２０５にて諸元等格納部４１１に記憶された角度テーブルの一例を示す図である。
【図１３】図１１のループ１内のある時刻におけるステップＳ２０８からステップＳ２１０の処理の具体例を示す図である（ｉ＝１の場合）。
【図１４】図１１のループ１内のある時刻におけるステップＳ２０８からステップＳ２１０の処理の具体例を示す図である（ｉ＝２の場合）。
【図１５】図１１のループ１内のある時刻におけるステップＳ２０８からステップＳ２１０の処理の具体例を示す図である（ｉ＝６の場合）。
【図１６】占有情報記録部４２５による占有領域の特定方法を説明するための図である。
【図１７】占有情報記録部４２５による占有領域の特定方法を説明するための図である（手順１）。
【図１８】占有情報記録部４２５による占有領域の特定方法を説明するための図である（手順２）。
【図１９】占有情報記録部４２５による占有領域の特定方法を説明するための図である（手順３）。
【発明を実施するための形態】
【００２７】
以下、添付図面を参照して本発明にかかる干渉判定装置および干渉判定方法の好適な実施形態を詳細に説明する。なお、図面の説明において同一の要素には同一の符号を付し、重複する説明を省略する。
【００２８】
［双腕ロボットシステム１００の全体構成］
まず、本発明の実施形態に係る双腕ロボットシステム１００の全体構成について、図１〜３を参照しながら説明する。図１は、双腕ロボットシステム１００の構成概要図である。図１に示すように、双腕ロボットシステム１００は、軌道生成部１、サーボドライバ２ａ〜２ｄ、双腕ロボット３、および干渉判定部４を含んで構成され、各構成要素は例えば図１にて矢印で表現される通信手段により互いに通信できるように接続されている。図２は双腕ロボット３の側面概要図であり、図３は双腕ロボット３の上面概要図である。
【００２９】
双腕ロボット３は、例えば図２および図３に示されるように、先端に昇降軸３１３および３２３を持つ二台の水平多関節アーム３１および３２を有する。水平多関節アーム３１および３２は、人間の両腕のように作業領域の少なくとも一部が重複する間隔で例えばワーク５を挟んで配置される。各アームの第１軸３１１および３２１の付け根（双腕ロボット３の肩部分）と、第２軸３１２および３２２の付け根（双腕ロボット３のひじ部分）には、それぞれ、サーボモータ３ａ〜３ｄが設けられている。この構成により各軸は水平方向に旋回移動できるようになっている。
【００３０】
図１にて示される軌道生成部１は、双腕ロボット３の各部位が現在位置から目標位置まで移動できるように、各関節の移動量および移動速度を演算し、各サーボドライバに指令を送る部分である。軌道生成部１は、記憶部１１および演算部１２を含んで構成される。記憶部１１は、軌道生成に必要な双腕ロボット３の概形寸法や移動に関する補間情報ならびに速度情報などの諸元と、教示もしくはロボット言語によるプログラミングによって記述されたロボットの動作順序を制御する動作プログラム、および干渉判定に必要な干渉条件を格納する部分である。演算部１２は、双腕ロボット３の各部位が現在位置から目標位置まで移動できるように、各関節の移動量および移動速度を演算する部分である。双腕ロボット３の移動前に、軌道生成部１の演算部１２は干渉判定部４へ、移動前の位置から移動後の位置に移動する場合の移動中および移動後の干渉の有無を問い合わせる。
【００３１】
サーボドライバ２ａ〜２ｄは、軌道生成部１からの指令を受けて、双腕ロボット３の各関節に設けられたサーボモータ３ａ〜３ｄをそれぞれ駆動し、駆動後の現在位置を軌道生成部１にフィードバックする部分である。
【００３２】
［干渉判定部４の構成］
以下、双腕ロボットシステム１００の一構成要素である干渉判定部４（特許請求の範囲における「干渉判定装置」に相当）の構成について、図４〜図９を更に参照しながら詳細に説明する。干渉判定部４は、軌道生成部１から問い合わせを受けて、３次元で動作する双腕ロボット３の各部位（移動部位）と、障害物としてワークまたは治具の各部位（固定部位）に対して干渉判定を行う部分である。図４は、干渉判定部４の機能的構成要素を示したブロック図である。図４に示されるように、干渉判定部４は、格納部４１および干渉判定演算部４２を含んで構成される。なお、以下の説明においては、「移動部位」とは、双腕ロボット３および障害物５の各部位のうち移動を伴う部位をいい、「固定部位」とは、双腕ロボット３および障害物５の各部位のうち移動を伴わない部位をいい、「部位」は、移動部位と固定部位の総称である。
【００３３】
干渉判定部４の格納部４１は、諸元等格納部４１１、干渉情報格納部４１２（特許請求の範囲における「干渉情報格納手段」に相当）、および干渉条件情報格納部４１３（特許請求の範囲における「干渉条件情報格納手段」に相当）を含んで構成される。諸元等格納部４１１は、電源投入後に受け取った双腕ロボット３の諸元、および軌道生成部１からの問い合わせ毎に受け取った移動前後の移動部位毎の角度を格納する部分である。干渉情報格納部４１２は、双腕ロボット３および障害物５の移動部位における干渉情報を格納する部分である。干渉情報は、後に詳細に説明されるように、移動部位が他の部位との間で立体交差する際に、干渉を伴わない場合には「０」を、干渉を伴う場合には「１」を表す情報である。干渉条件情報格納部４１３は、干渉条件情報を移動部位ごとに格納する部分である。干渉条件情報は、後に詳細に説明されるように、干渉情報に基づき設定され、双腕ロボット３および障害物５の各部位の数と同数の複数の桁数を有し、移動部位が他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である。
【００３４】
干渉判定部４の干渉判定演算部４２は、格納部４１に格納された各情報に基づき干渉判定を行う部分である。干渉判定演算部４２は、双腕ロボット３の移動前後の各関節の角度に対して、更に一定時間毎に分割された移動過程の各関節の角度に対して、それぞれ干渉判定を行う。干渉判定演算部４２は、図４に示されるように、識別番号割当部４２１（特許請求の範囲における「識別番号割当手段」に相当）、干渉情報生成部４２２（特許請求の範囲における「干渉情報格納手段」に相当）、干渉条件情報生成部４２３（特許請求の範囲における「干渉条件情報格納手段」に相当）、マス目管理部４２４（特許請求の範囲における「マス目管理手段」に相当）、占有情報記録部４２５（特許請求の範囲における「占有情報記録手段」に相当）、および判断部４２６（特許請求の範囲における「判断手段」に相当）を含んで構成される。以下、干渉判定演算部４２を構成する各構成要素について詳細に説明する。
【００３５】
［識別番号割当部４２１］
識別番号割当部４２１は、双腕ロボット３および障害物５の各部位に識別番号を割り当てる部分である。各識別番号は、双腕ロボット３および障害物５の各部位の数と同数の複数の桁数を有し且つ他の全ての識別番号との間で桁ごとの論理積を計算すると全ての桁が「０」となるように設定される情報である。具体的に、識別番号割当部４２１が図２および図３にて示した双腕ロボット３および障害物５の各部位に割り当てた識別番号ＩＤｉの一例を図５に示す。図５の一例において、識別番号割当部４２１は、アーム３１の第１軸（第１リンク）３１１に対して、識別番号ＩＤ１として「１００００００」を割り当てている。また、識別番号割当部４２１は、アーム３２の昇降軸３２３に対して、識別番号ＩＤ６として「０００００１０」を割り当てている。更に、識別番号割当部４２１は、冶具５に対して、識別番号ＩＤ７として「００００００１」を割り当てている。ここで挙げた一例においては、双腕ロボット３および障害物５の各部位が「７つ」あるため、各識別番号ＩＤｉの桁数は「７」となっている。また、ＩＤ１からＩＤ７までの桁ごとの論理積を計算すると、全ての桁において「０」となる。
【００３６】
［干渉情報生成部４２２］
干渉情報生成部４２２は、双腕ロボット３の各移動部位と他の部位との間で、干渉のない立体交差が存在しない場合（つまり立体交差する際に干渉を伴う場合）は「１」とし、干渉のない立体交差が可能である場合（つまり立体交差する際に干渉を伴わない場合）もしくは自分自身との関係は「０」としたテーブルを作成する部分である。
【００３７】
具体的に、干渉情報生成部４２２が作成したテーブルの一例を図６に示す。図６の一例において、例えばｉ＝１およびｊ＝２の場合における干渉の有無を示す干渉情報ｔ１２の値は「０」であり、これはｉ＝１に相当するアーム３１の第１軸３１１とｊ＝２に相当するアーム３１の第２軸３１２との間では、干渉のない立体交差が存在すること、つまり立体交差する際に干渉を伴わないことを示す。また、例えばｉ＝４およびｊ＝３の場合における干渉の有無を示す干渉情報ｔ４３の値は「１」であり、これはｉ＝４に相当するアーム３２の第１軸３２１とｊ＝３に相当するアーム３１の昇降軸３１３との間では、干渉のない立体交差が存在しないこと、つまり立体交差する際に干渉を伴うことを示す。また、例えばｉ＝５およびｊ＝５の場合における干渉の有無を示す干渉情報ｔ５５の値は「０」であり、これはｉ＝５に相当するアーム３２の第２軸３２２が自分自身との間で干渉することは有り得ないことを示す。
【００３８】
干渉情報生成部４２２は、このようにして作成したテーブルを格納部４１の干渉情報格納部４１２に格納させる。なお、干渉情報生成部４２２は、移動を伴わない部位である固定部位つまり識別番号ＩＤ７の冶具５に対しては、干渉情報ｔ７１、ｔ７２などを求めなくても良い。他の移動部位の干渉情報ｔ１７、ｔ２７などにおいて冶具５との干渉の有無が既に考慮されているからである。また、この意味では、干渉情報ｔｉｊの値と干渉情報ｔｊｉの値は常に同じであるため、図６の一例をｔｉｊかｔｊｉのうち何れか一方のみを明らかにしたテーブルにしても良い。
【００３９】
［干渉条件情報生成部４２３］
干渉条件情報生成部４２３は、干渉条件情報を生成する部分である。干渉条件情報は、干渉情報に基づき設定され、双腕ロボット３および障害物５の各部位の数と同数の複数の桁数を有し、移動部位が他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である。干渉条件情報生成部４２３は、下記の数式（１）により計算される情報Ｃｉを干渉条件情報として生成する。
【数２】
ただし、ｈは双腕ロボット３および障害物５の各部位の数であり、ｉおよびｊは１以上ｈ以下の自然数であり、Ｃｉはｉ番目の移動部位の干渉条件情報であり、ＩＤｊはｊ番目の部位の識別番号であり、ｔｉｊはｉ番目の移動部位とｊ番目の部位における干渉情報である。
【００４０】
以下、干渉条件情報生成部４２３による干渉条件情報の生成方法を詳細に説明する。まず、簡単な一例として、干渉判定部位が２個のみのケースを考える。すなわち、右腕（ＩＤ１＝０１）と左腕（ＩＤ２＝１０）のみの干渉判定を行う場合を考える。この場合に干渉条件情報生成部４２３により生成される干渉条件情報を図７に示す。なお、図７の一例においては、右腕と左腕とが立体交差する際に干渉を伴うとして、干渉情報が設定されている。つまり、ｔ１２＝１であり、ｔ２１＝１である。
【００４１】
図７の一例において、ｉ＝１の識別番号ＩＤ１の部位である右腕に対して干渉条件情報Ｃ１を求めると、上記式（１）に従い、
Ｃ１＝ＩＤ１・ｔ１１＋ＩＤ２・ｔ１２…（２）
である。そして、図７の一例において、ＩＤ１＝０１、ｔ１１＝０、ＩＤ２＝１０、ｔ１２＝１であるため、それぞれの値を上記式（２）に代入すると、
Ｃ１＝０１・０＋１０・１＝１０…（３）
となり、識別番号ＩＤ１の右腕の干渉条件情報Ｃ１の値は１０として求められる。
【００４２】
続いて、より複雑な例として、図２および図３にて示した干渉判定部位が７つであるケースを考える。説明の便宜上、図５および図６の場合と同様の識別番号および干渉情報について考える。この場合に干渉条件情報生成部４２３により生成される干渉条件情報を図８に示す。この場合において、ｉ＝１の識別番号ＩＤ１の部位であるアーム３１の第１軸３１１に対して干渉条件情報Ｃ１を求めると、上記式（１）に従い、
Ｃ１＝ＩＤ１・ｔ１１＋ＩＤ２・ｔ１２＋ＩＤ３・ｔ１３＋ＩＤ４・ｔ１４＋ＩＤ５・ｔ１５＋ＩＤ６・ｔ１６＋ＩＤ７・ｔ１７…（４）
である。そして、ＩＤ１＝１００００００、ｔ１１＝０、ＩＤ２＝０１０００００、ｔ１２＝０、ＩＤ３＝００１００００、ｔ１３＝１、ＩＤ４＝０００１０００、ｔ１４＝１、ＩＤ５＝００００１００、ｔ１５＝０、ＩＤ６＝０００００１０、ｔ１６＝１、ＩＤ７＝００００００１、ｔ１７＝０であるため、それぞれの値を上記式（４）に代入すると、
Ｃ１＝１００００００・０
＋０１０００００・０
＋００１００００・１
＋０００１０００・１
＋００００１００・０
＋０００００１０・１
＋００００００１・０
＝００１１０１０…（５）
となり、識別番号ＩＤ１のアーム３１の第１軸３１１の干渉条件情報Ｃ１の値は００１１０１０として求められる。
【００４３】
同様に、他の部位に対して干渉条件情報を求めることができ、その結果が図８にＣ２〜Ｃ６の値として示されている。干渉条件情報生成部４２３は、このようにして生成した干渉条件情報を格納部４１の干渉条件情報格納部４１３に格納させる。なお、干渉条件情報生成部４２３は、固定部位である識別番号ＩＤ７の冶具５に対しては干渉条件情報を求めなくても良い。移動部位に対する干渉条件情報であるＣ１〜Ｃ６の値に、固定部位の冶具５との間の干渉の有無が既に反映されているからである。
【００４４】
［マス目管理部４２４］
マス目管理部４２４は、双腕ロボット３の動作する空間を２次元平面上に投影した上で、その２次元平面をｎ１×ｎ２のマス目に区切った判定領域を作成するとともに、そのｎ１×ｎ２の個々のマス目に対応するメモリ領域を確保する部分である。図９は、マス目管理部４２４により作成された判定領域６の一例を示す。判定領域６は、双腕ロボット３の姿勢および障害物５の位置を演算するために要する諸元を格納した諸元等格納部４１１の値を元に、双腕ロボット３の動作する範囲を包含する空間、もしくは双腕ロボット３の動作する範囲の一部であって干渉判定の対象となる空間を２次元平面上に投影したｎ１×ｎ２の２次元平面のマス目で表現されるものである。
【００４５】
［占有情報記録部４２５］
占有情報記録部４２５は、マス目管理部４２４による上記ｎ１×ｎ２のマス目のうち、双腕ロボット３および障害物５の各部位に対応するマス目である対応マス目を特定し、占有情報を上記対応マス目に対応するメモリ領域に記録する部分である。図９は、占有情報記録部４２５により特定された対応マス目の一例を示す。図９の一例では、同じ平面上に投影した判定対象となる姿勢をとった双腕ロボット３や障害物５の各部位が存在する領域（以下、「占有領域」ともいう。）に対応する対応マス目が符号３１１ｍ、３１２ｍ、３１３ｍ、３２１ｍ、３２２ｍ、３２３ｍ、および５ｍで示されるように特定されている。なお、対応マス目を特定する方法については、後述の［占有領域の特定方法］の欄にて詳細に説明する。
【００４６】
占有情報記録部４２５は、対応マス目の特定後に、占有情報を当該対応マス目に対応するメモリ領域に記録する。占有情報は、双腕ロボット３および障害物５の各部位の識別番号に基づく情報であり、双腕ロボット３および障害物５の各部位の数と同数の複数の桁数を有する情報である。占有情報記録部４２５は、占有情報を記録する対象となるメモリ領域が初期化されている場合には、対応マス目を占有する部位の識別番号を占有情報としてメモリ領域に記録する。一方で、占有情報記録部４２５は、占有情報を記録する対象となるメモリ領域が初期化されていない場合（既に占有情報が記録されている場合）には、既存の占有情報と対応マス目を占有する部位の識別番号との間の桁ごとの論理和を占有情報としてメモリ領域に記録する。占有情報記録部４２５による占有情報の記録により、個々のマス目を占有する双腕ロボット３および障害物５の各部位に対応するビットが立った状態となる。なお、占有情報記録部４２５による動作の詳細については後述する。
【００４７】
［判断部４２６］
判断部４２６は、占有情報および干渉条件情報における桁ごとの論理積を対応マス目ごとに求め、当該論理積の結果に基づき干渉判断を行うものである。判断部４２６は、当該論理積の結果が全ての桁において「０」であれば、双腕ロボット３および障害物５の各部位同士は干渉しないと判断する。一方で、判断部４２６は、当該論理積の結果が少なくとも一つの桁において「１」であれば、双腕ロボット３および障害物５の各部位同士は干渉すると判断する。判断部４２６は、干渉判断の結果を示す情報を軌道生成部１に出力する。判断部４２６は、干渉判断の結果を出力する際に、干渉が生じている部位を特定する情報をともに出力するようにしても良い。
【００４８】
［双腕ロボットシステム１００の動作］
続いて、双腕ロボットシステム１００により行われる動作について、図１０〜図１５を参照しながら説明する。下記の説明は、双腕ロボットシステム１００におけるメイン処理と、干渉判定部４による干渉判定処理とに大別される。
【００４９】
［メイン処理］
まず、双腕ロボットシステム１００におけるメイン処理について図１０を参照しながら説明する。図１０は、双腕ロボットシステム１００にて行われるメイン処理を示すフローチャートである。
【００５０】
最初に、軌道生成部１の演算部１２が、記憶部１１に格納されているパラメータを干渉判定部４の干渉判定演算部４２に送信する。このパラメータは、干渉判定の対象部位毎に設定されるものであり、上述した干渉情報ｔｉｊを含む（ステップＳ１０１）。
【００５１】
次に、軌道生成部１の演算部１２が、各サーボドライバ２ａ〜２ｄから双腕ロボット３の各部位の現在角度を読み込む（ステップＳ１０２）。
【００５２】
次に、軌道生成部１の演算部１２が、記憶部１１に格納された動作プログラムを解析して、双腕ロボット３の各部位の移動先の角度（目標角度）を求める（ステップＳ１０３）。
【００５３】
次に、軌道生成部１の演算部１２が、双腕ロボット３の各部位の現在角度と目標角度を干渉判定部４の干渉判定演算部４２に送信する（ステップＳ１０４）。
【００５４】
次に、軌道生成部１の演算部１２は干渉判定部４の干渉判定演算部４２からの判定結果信号を待つ（ステップＳ１０５）。この間、干渉判定部４では詳細を後述する干渉判定処理が行われ、双腕ロボット３のアーム３１，３２の移動の全体で部位同士の干渉が生じないかどうかが判定される。そして、干渉判定部４の干渉判定演算部４２からの判定結果信号が「干渉なし」の場合（ステップＳ１０６：ＹＥＳ）、軌道生成部１の演算部１２は、表示部（図示せず）に「干渉なし」の表示をさせ（ステップＳ１０７）、記憶部１１に格納されている動作プログラムを実行させ、双腕ロボット３の動作制御を行う（ステップＳ１０８）。
【００５５】
一方で、干渉判定部４の干渉判定演算部４２からの判定結果信号が「干渉あり」の場合には（ステップＳ１０６：ＮＯ）、軌道生成部１の演算部１２は、表示部（図示せず）に「干渉あり」の表示をさせ（ステップＳ１０９）、そのまま処理を終了させる。以上のように軌道生成部１は、双腕ロボット３の移動の全体で部位同士の干渉が生じない判定結果が得られるまでサーボドライバ２ａ〜２ｄに指令を送らずに待機し、干渉が生じる場合には双腕ロボット３の各移動部位の移動を開始させない。
【００５６】
［干渉判定処理］
引き続き、干渉判定部４による干渉判定処理について図１１および図１２を参照しながら説明する。図１１は、干渉判定部４による干渉判定処理を示すフローチャートであり、図１２はステップＳ２０５の説明で参照する図である。
【００５７】
最初に、識別番号割当部４２１が干渉判定の対象部位毎に識別番号ＩＤｉ（ｉ＝１〜ｈ）を割り当てる（ステップＳ２０１、図５参照）。
【００５８】
次に、干渉条件情報生成部４２３が、ステップＳ２０１にて割り当てられた識別番号ＩＤｉと、干渉情報生成部４２２により作成され干渉情報格納部４１２に格納されたテーブル（図６参照）に記載された干渉情報ｔｉｊに基づいて、上述した式（１）に従い干渉条件情報Ｃｉを生成する（ステップＳ２０２、図７および図８参照）。
【００５９】
次に、マス目管理部４２４が、判定領域（ｎ１×ｎ２のマス目領域、図９参照）に対応するメモリ領域を確保する（ステップＳ２０３）。
【００６０】
次に、干渉判定演算部４２はステップＳ１０４（図１０参照）にて軌道生成部１から双腕ロボット３の各部位の現在角度と目標角度が入力されることを待つ（ステップＳ２０４）。
【００６１】
軌道生成部１から双腕ロボット３の各部位の現在角度と目標角度の入力があった場合に（ステップＳ２０４：ＹＥＳ）、干渉判定演算部４２は、双腕ロボット３の各部位の目標角度と現在角度の差Δθおよび各部位の速度パラメータから一定時間間隔の各部位の角度を求め、これを諸元等格納部４１１の角度テーブルに記憶する。図示はしないが、この処理のために干渉判定演算部４２が別途の処理部を備えていても良い。図１２は諸元等格納部４１１に記憶された角度テーブルの一例を示す。図１２に示されるように、角度テーブルは、一定時間間隔毎のレコード（レコード数：ｍ）に部位毎のフィールドを備えて構成されるものである（ステップＳ２０５）。
【００６２】
以下、ステップＳ２０６からステップＳ２１３までのループ１が開始される。ステップＳ２０５の次に、マス目管理部４２４が、判定領域（ｎ１×ｎ２のマス目領域、図９参照）に対応するメモリ領域に占有情報の初期値として０を設定することにより、メモリ領域を初期化する（ステップＳ２０６）。
【００６３】
次に、占有情報記録部４２５が、図１２にて例示した角度テーブルからｋ番目（ｋ＝１〜ｍ）のレコードの角度データを読み込む（ステップＳ２０７）。
【００６４】
以下、ステップＳ２０８からステップＳ２１０までのループ２が開始される。ステップＳ２０７の次に、占有情報記録部４２５が、干渉判定の対象部位ｉの角度データと寸法パラメータから判定領域（二次元平面）において干渉判定の対象部位ｉが占有するマス目、すなわち対応マス目を特定する。なお、対応マス目を特定する処理については、後述の［占有領域の特定方法］の欄で詳細に説明する。また、占有情報記録部４２５は、特定した当該対応マス目に対応するメモリ領域に記録された占有情報を取得する（ステップＳ２０８）。
【００６５】
次に、占有情報記録部４２５が、干渉判定の対象部位ｉが占有するメモリ領域の内、既存の占有情報が０のメモリ領域には占有情報として識別番号ＩＤｉを書き込み、既存の占有情報が０以外のメモリ領域には既存の占有情報と識別番号ＩＤｉの論理和を占有情報として書き込む（ステップＳ２０９）。
【００６６】
次に、判断部４２６が、最前のステップＳ２０９の処理で占有情報を書き込んだ全てのメモリ領域について、干渉条件Ｃｉとの論理積を求める（ステップＳ２１０）。
【００６７】
ステップＳ２０８からステップＳ２１０までの処理をｉ＝ｈとなるまで、すなわち全ての干渉判定の対象部位について繰り返す。これにより、ループ２が終了し、ある時点ｋにおける双腕ロボット３の姿勢に対する干渉判定が終了する。なお、ループ２の処理の具体例については、下記の［ループ２の処理の具体例］の欄で後述する。
【００６８】
ループ２が終了した時点で、判断部４２６はステップＳ２１０にて求めたそれぞれの論理積が全ての桁において０か否かを判断する（ステップＳ２１１）。ステップＳ２１１の判断にて、全ての論理積が全ての桁において０ならば（ステップＳ２１１：ＹＥＳ）、判断部４２６は「干渉なし」を表す情報を格納部４１に記憶させる（ステップＳ２１２）。一方で、ステップＳ２１１の判断にて、何れかの論理積に全ての桁において０でないものがある場合には（ステップＳ２１１：ＮＯ）、判断部４２６は「干渉あり」を表す情報を格納部４１に記憶させる（ステップＳ２１３）。格納部４１はこの記憶処理のために別途の記憶手段を更に備えていても良く、諸元等格納部４１１がこの記憶処理を行っても良い。
【００６９】
ステップＳ２０６からステップＳ２１３までの処理をｋ＝ｍとなるまで、すなわち各部位の目標角度まで繰り返す。これにより、ループ１が終了し、双腕ロボット３の各アームが目標とする姿勢に移行するまでの全ての姿勢における干渉判定処理が終了する。
【００７０】
ループ１が終了した時点で、判断部４２６は、ステップＳ２１２またはステップＳ２１３にて格納部４１に記憶された一定時間間隔毎の判定結果情報から最終的な判定結果を求め、これを判定結果信号として軌道生成部１に送る。判定結果は、一定時間間隔毎の判定結果情報の全てが干渉なしなら「干渉なし」、一つでも干渉ありがあれば「干渉あり」となる（ステップＳ２１４）。このような干渉判定処理により、双腕ロボット３のアーム３１，３２が初期の姿勢から目標とする姿勢まで動作する間の一定時間間隔毎の各姿勢について、部位同士の干渉の有無を確認することができる。
【００７１】
［ループ２の処理の具体例］
続いて、図１３〜図１５を参照しながら、ループ２の処理の具体例について説明する。図１３〜図１５は、ループ１内のある時刻（例えばｋ番目の時刻）におけるステップＳ２０８からステップＳ２１０までの処理の具体例を示す。図１３はｉ＝１の場合の処理の具体例であり、図１４はｉ＝２の場合の処理の具体例であり、図１５はｉ＝６の場合の処理の具体例である。
【００７２】
図１３が示すｉ＝１の場合には、アーム３１の第１軸３１１に対してステップＳ２０８からステップＳ２１０までの処理が行われる。すなわち、最初に、占有情報記録部４２５が判定領域６において第１軸３１１の占有領域（図１３にてドットを付した領域Ｈ１に対応するマス目）を特定する（ステップＳ２０８）。次に、占有情報記録部４２５がステップＳ２０８にて特定した対応マス目に対応するメモリ領域に占有情報を書き込む。この時、全てのメモリ領域は、ステップＳ２０６で初期化された状態（占有情報の初期値として「０」が書き込まれた状態）にあることから、領域Ｈ１のマス目に対応するメモリ領域には第１軸３１１の識別番号ＩＤ１＝「１００００００」が書き込まれる（ステップＳ２０９）。次に、占有情報記録部４２５が、最前のステップＳ２０９で書き込まれた占有情報それぞれと干渉条件Ｃ１との論理積を求める（ステップＳ２１０）。つまり、判断部４２６は、下記の式（６）にて示される論理積を求める。
領域Ｈ１のマス目に係る論理積＝
１００００００
×００１１０１０
＝０００００００・・・（６）
【００７３】
図１４が示すｉ＝２の場合には、アーム３１の第２軸３１２に対してステップＳ２０８からステップＳ２１０までの処理が行われる。すなわち、最初に、占有情報記録部４２５が判定領域６において第２軸３１２の占有領域（図１４にてドットを付した領域Ｈ２およびハッチを付した領域Ｈ３に対応するマス目）を特定する（ステップＳ２０８）。領域Ｈ２は第２軸３１２のみが存在する領域であり、領域Ｈ３は第２軸３１２と第１軸３１１が重なる領域である。次に、占有情報記録部４２５がステップＳ２０８にて特定した対応マス目に対応するメモリ領域に占有情報を書き込む。メモリ領域が初期化された状態のままの領域Ｈ２のマス目に対応するメモリ領域には第２軸３１２の識別番号ＩＤ２＝「０１０００００」が書き込まれる。一方、ｉ＝１での書き込みによりメモリ領域が初期化された状態になくなっている領域Ｈ３のマス目に対応するメモリ領域には既存の占有情報「１００００００」と第２軸３１２の識別番号ＩＤ２＝「０１０００００」の論理和が新たな占有情報として書き込まれる。つまり、占有情報記録部４２５は、下記の式（７）によって示される占有情報を領域Ｈ３のマス目に対応するメモリ領域に記録する。
領域Ｈ３のマス目に対応するメモリ領域に書き込まれる占有情報＝
１００００００
＋０１０００００
＝１１０００００・・・（７）
【００７４】
次に、判断部４２６が、領域Ｈ２と領域Ｈ３のそれぞれに対応するマス目について、次の式（８）および式（９）によって示される占有情報と干渉条件Ｃ２との論理積をマス目ごとに求める。
領域Ｈ２のマス目に係る論理積＝
０１０００００
×００００１１０
＝０００００００・・・（８）
領域Ｈ３のマス目に係る論理積＝
１１０００００
×００００１１０
＝０００００００・・・（９）
【００７５】
前述のようにして、i＝３で昇降軸３１３、i＝４で第１軸３２１、i＝５で第２軸３２２と、それぞれ判定対象となる部位毎に処理が進み、図１５が示すｉ＝６の場合には、アーム３２の昇降軸３２３に対してステップＳ２０８からステップＳ２１０までの処理が行われる。すなわち、最初に、占有情報記録部４２５が判定領域６において昇降軸３２３の占有領域（図１５にてドットを付した領域Ｈ４、ハッチを付した領域Ｈ５、領域Ｈ６、および領域Ｈ７に対応するマス目）を特定する（ステップＳ２０８）。領域Ｈ４は昇降軸３２３のみが存在する領域であり、領域Ｈ５は昇降軸３２３とアーム３２の第２軸３２２が重なる領域であり、領域Ｈ６は昇降軸３２３とアーム３１の第２軸３１２が重なる領域であり、領域Ｈ７は昇降軸３２３とアーム３１の第１軸３１１および第２軸３１２が重なる領域である。
【００７６】
次に、占有情報記録部４２５がステップＳ２０８にて特定した対応マス目に対応するメモリ領域に占有情報を書き込む。メモリが初期化された状態のままの領域Ｈ４のマス目に対応するメモリ領域には、昇降軸３２３の識別番号ＩＤ６＝「０００００１０」が書き込まれる。一方、ｉ＝５での書き込みによりメモリが初期化された状態になくなっている領域Ｈ５の対応マス目に対応するメモリ領域には、既存の占有情報としてのアーム３２の第２軸３２２の識別番号ＩＤ５＝「００００１００」と昇降軸３２３の識別番号ＩＤ６＝「０００００１０」の論理和が新たな占有情報として書き込まれる。つまり、占有情報記録部４２５は、下記の式（１０）によって示される占有情報を領域Ｈ５のマス目に対応するメモリ領域に記録する。
領域Ｈ５のマス目に対応するメモリ領域に書き込まれる占有情報＝
００００１００
＋０００００１０
＝００００１１０・・・（１０）
【００７７】
また、ｉ＝２での書き込みによりメモリが初期化された状態になくなっている領域Ｈ６のマス目に対応するメモリ領域には、既存の占有情報としてのアーム１の第２軸３２１の識別番号ＩＤ２＝「０１０００００」と昇降軸３２３の識別番号ＩＤ６＝「０００００１０」の論理和が新たな占有情報として書き込まれる。つまり、占有情報記録部４２５は、下記の式（１１）によって示される占有情報を領域Ｈ６のマス目に対応するメモリ領域に記録する。
０１０００００
＋０００００１０
＝０１０００１０・・・（１１）
【００７８】
更に、ｉ＝１およびｉ＝２での書き込みによりメモリが初期化された状態になくなっている領域Ｈ７のマス目に対応するメモリ領域には、既存の占有情報としてのアーム１の第１軸３１１の識別番号ＩＤ１＝「１００００００」と第２軸３１２の識別番号ＩＤ２＝「０１０００００」と昇降軸３２３の識別番号ＩＤ６＝「０００００１０」の論理和が新たな占有情報として書き込まれる。つまり、占有情報記録部４２５は、下記の式（１２）によって示される占有情報を領域Ｈ６のマス目に対応するメモリ領域に記録する。
１００００００
＋０１０００００
＋０００００１０
＝１１０００１０・・・（１２）
【００７９】
次に、判断部４２６が、領域Ｈ４ないし領域Ｈ７のそれぞれに対応するマス目について、次の式（１３）ないし式（１６）によって示される占有情報と干渉条件Ｃ６との論理積をマス目ごとに求める。
領域Ｈ４のマス目に係る論理積＝
０００００１０
×１１１１００１
＝０００００００・・・（１３）
領域Ｈ５のマス目に係る論理積＝
００００１１０
×１１１１００１
＝０００００００・・・（１４）
領域Ｈ６のマス目に係る論理積＝
０１０００１０
×１１１１００１
＝０１０００００・・・（１５）
領域Ｈ７のマス目に係る論理積＝
１１０００１０
×１１１１００１
＝１１０００００・・・（１６）
【００８０】
以上、ループ２の処理の具体例について説明したが、このようにしてループ２が終了する（全ての判定対象部位について占有情報と干渉条件との論理積とが求まる）と、判断部４２６は、それぞれの論理積が０か否かを判断する(ステップＳ２１１)。この場合、判断部４２６は、上述の具体例におけるｉ＝１およびｉ＝２の時のように論理積の結果が全ての桁で０であれば、干渉は生じないと判断する。実際、具体例のｉ＝１の場合、第１軸３１１が自分自身と干渉することはあり得ない。また、ｉ＝２の場合は、第１軸３１１と第２軸３１２とは関節でつながって立体交差しており、干渉することはあり得ない。一方、上述の具体例におけるｉ＝６の時のように論理積の結果が０でない（少なくとも一つの桁が１である）場合は、干渉が生じると判断する。なお、干渉が生じる場所は、上記の式（１５）および式（１６）において値が「１」である桁と同一桁で値を「１」とする識別番号の部位である。つまり、上記の式（１５）において値が「１」である桁は左から二番目の桁であり、この桁で値を「１」とする識別番号の部位はアーム３１の第２軸３１２（識別番号「０１０００００」）である。したがって、昇降軸３２３はアーム３１の第２軸３１２と干渉することがわかる。また、式（１６）において値が「１」である桁は左から一番目と二番目の各桁であり、これらの桁で値を「１」とする識別番号の部位はアーム３１の第１軸３１１（識別番号「１００００００」）と第２軸３１２（識別番号「０１０００００」）である。したがって、昇降軸３２３はアーム３１の第１軸３１１と第２軸３１２とが重なる位置、つまり右ひじ部においても干渉を生じることがわかる。
【００８１】
なお、図１３〜図１５では説明の便宜上マス目（メモリ領域）を大きく設定して説明した。しかし、実際のマス目は図１３〜図１５の例よりもっと小さい設定となる。
【００８２】
［占有領域の特定方法］
前述したように、占有情報記録部４２５は、マス目管理部４２４が作成したｎ１×ｎ２のマス目（判定領域、図９参照）のうち、ある時点における双腕ロボット３および障害物５の各部位が存在する領域（占有領域）に対応する対応マス目を特定する。以下、占有情報記録部４２５による占有領域の特定方法について、図１６〜図１９を参照し、具体例を挙げながら詳細に説明する。
【００８３】
図１６は、占有情報記録部４２５による占有領域の特定方法を説明するための図である。この図１６に示す例は、前後に関節ＡＡおよび関節ＢＢを有するアームＣＣがＸＹ平面上において存在する領域（アームＣＣの占有領域、図１６ではグレーで表示された領域Ｋ）を特定する例である。また、図１６に示す例は、例えば前述したアーム３１の第１軸３１１に対して、その対応マス目を特定するための例として理解されることができ、この場合の第１軸３１１は図１６に示すアームＣＣに該当する。なお、この場合の第１軸３１１は、例えばオーバル形状である。また、実際のＸＹ平面は、図９に示した判定領域に係るｎ１×ｎ２のマス目が全て存在可能な平面領域が確保されるが、図１６では説明の便宜上、アームＣＣの部分だけを拡大して示している。その特定方法としては、図１６のＸＹ平面上に存在する点（マス目を構成する４つの頂点）に対して、下記の手順１〜手順３を行い、各点がアームＣＣの占有領域に含まれるか否かを判定することで、ＸＹ平面上におけるアームＣＣの占有領域を特定することができる。
【００８４】
この例において、関節ＡＡの中心座標、関節ＢＢの中心座標、およびアームＣＣの幅は、既知の情報である。これらの既知の情報は、軌道生成部１の記憶部１１に格納されており、干渉判定演算部４２に送信される。図１６において、関節ＡＡの中心座標は点Ａ（ｘ１、ｙ１）であり、関節ＢＢの中心座標は点Ｂ（ｘ２、ｙ２）であり、アームＣＣの幅はｗである。また、関節ＡＡおよび関節ＢＢの半径Ｒ＝ｗ／２である。つまり、
点Ａ（ｘ１、ｙ１）：関節ＡＡの中心座標
点Ｂ（ｘ２、ｙ２）：関節ＢＢの中心座標
ｗ：アームの幅（Ｒ＝ｗ／２）
である。また、この例において、点ＤはアームＣＣの占有領域に入っているか否かの判定対象となる点である。つまり、
点Ｄ（ｘ０、ｙ０）：任意のマス目の任意の頂点
である。また、各補助線および補助点の定義は以下のとおりである。
ｌ１：点Ａ、点Ｂを通る直線
ｌ１１：関節ＡＡおよび関節ＢＢを近似した円（点Ａ、Ｂを中心とした半径Ｒの円）の接線（下側）
ｌ１２：関節ＡＡおよび関節ＢＢを近似した円（点Ａ、Ｂを中心とした半径Ｒの円）の接線（上側）
ｌ２：点Ｄを通り、ｌ１と垂直に交わる直線（つまりｌ１とｌ２は垂直関係）
点Ｃ（ｘ３、ｙ３）：ｌ１とｌ２の交点
点Ａｉ、Ｂｉ：点Ａ、Ｂを中心とした半径Ｒの円の接線ｌ１ｉとの接点
点Ｅ’：点ＡからＸ軸に垂線を引いたときにＹ座標がＡ１と同じになる点
点Ｂ’：点ＤからＸ軸に垂線を引いたときにｌ１と交わる点
なお、関節ＡＡおよび関節ＢＢの半径は同じとする。ゆえに、ｌ１、ｌ１１、およびｌ１２は互いに並行である。また、関節ＡＡおよび関節ＢＢの近似円及び線分ｌ１１及びｌ１２に囲まれた範囲（領域Ｋ）がアームＣＣに相当する。
【００８５】
（手順１）
最初に、図１７に示すルールに従い、判定対象部位（アームＣＣ）が位置する座標系を特定し、占有される可能性のあるマス目領域を限定する。点Ｄの座標（ｘ０、ｙ０）が、
（ｘｍｉｎ< ｘ０< ｘｍａｘ）…（１７）
および
（ｙｍｉｎ <ｙ０ < ｙｍａｘ）…（１８）
を満たすならば、点Ｄは判定対象部位の占有領域に含まれる可能性がある。この範囲外のマス目は、下記の手順３の項で後述する円形部分に含まれるか、もしくは判定対象部位の占有領域に含まれないことになる。
【００８６】
ここで、ｘ１２ｗおよびｙ１２ｗは、三角形ＡＢＥと三角形ＡＡ１Ｅ’との相似関係より、
Ｌａｂ：ａ１２：ｂ１２＝ Ｒ：ｙ１２ｗ：ｘ１２ｗ
（但し、ａ１２＝ｙ１−ｙ２、ｂ１２＝ｘ１−ｘ２）
が成り立つから、方向性を加味して、
ｘ１２ｗ＝ −Ｒ ｂ１２／Ｌａｂ
ｙ１２ｗ＝ −Ｒ ａ１２／Ｌａｂ
として与えられる。Ｌａｂは、三平方の定理から、
【数３】
となる。
【００８７】
（手順２）
次に、上記の式（１７）および（１８）、つまり（ｘｍｉｎ <ｘ０ < ｘｍａｘ）および（ｙｍｉｎ< ｙ０< ｙｍａｘ）を満たす各マス目の点Ｄが、図１８にてグレーで表示された領域Ｋ１に入るか否かを判定する。これには、点Ｄから線分ｌ１までの垂線の長さＬｃｄがＲ以下となるかどうか、すなわち、
Ｌｃｄ≦ Ｒ…（１９）
となるかどうかを判定すればよい。これが成立すれば、点Ｄを有するマス目は占有領域に含まれることになる。
【００８８】
ここで、Ｌｃｄは、以下の通り、軌道生成部１の記憶部１１に格納されていたパラメータ等から与えられる既知の条件値から求めることができる。まず、線分ｌ１の方程式は、傾きをａ１、切片をｂ１とすると、
ａ１ ｘ＋ ｂ１＝ ｙ
である。これにより、図１８における点Ｂ’の座標は、Ｘ座標が点Ｄと同じｘ０であるから、（ｘ０、ａ１ｘ０ ＋ ｂ１） である。したがって、２点ＤＢ’間の距離Ｌｄｂ’は、
Ｌｄｂ’ ＝ｙ０ − （ａ１ｘ０ ＋ ｂ１）
である。ここで、三角形ＡＢＥと三角形ＤＢ’Ｃとが相似関係となることから、
Ｌｄｂ’ ：Ｌｃｄ ＝ Ｌａｂ ： （ｘ２− ｘ１）
が成り立つ。これを、２点ＣＤ間の距離Ｌｃｄについて解けば、
Ｌｃｄ＝ （ｘ２ −ｘ１） Ｌｄｂ’ ／Ｌａｂ
= （ｘ２ −ｘ１） ｛ｙ０ − （ａ１ｘ０ ＋ ｂ１）｝ ／ Ｌａｂ
= −（ｘ１− ｘ２） ［ｙ０ − ｛（ｙ１ - ｙ２）／（ｘ１ - ｘ２）｝ ｘ０− （ｙ２ｘ１- ｘ２ｙ１）／（ｘ１ - ｘ２） ］／ Ｌａｂ
= ｛（ｙ１ - ｙ２）ｘ０−（ｘ１− ｘ２）ｙ０＋ （ｙ２ｘ１- ｘ２ｙ１）｝ ／Ｌａｂ
但し、上記の通り、
【数４】
である。
【００８９】
（手順３）
最後に、全てのマス目を対象に、点Ｄが図１９にてグレーで表示された円形領域Ｋ２に入るか否かを判定する。座標（ｘｉ、ｙｉ）を中心とする半径Ｒの円の方程式は、
（ｘ −ｘｉ）２＋ （ｙ −ｙｉ）２ ＝Ｒ２
であるから、円の境界を含む内側は、
（ｘ −ｘｉ）２＋ （ｙ −ｙｉ）２ ≦Ｒ２
と表現できる。ここで、点Ｄ（ｘ０、ｙ０）に対して関節ＡＡおよび関節ＢＢの２つの円を考えれば、
（ｘ０−ｘ１）２＋ （ｙ０ −ｙ１）２ ≦Ｒ２…（２０）
（ｘ０−ｘ２）２＋ （ｙ０ −ｙ２）２ ≦Ｒ２…（２１）
のいずれかが成立すれば、点Ｄ（ｘ０、ｙ０）を有するマス目は占有領域に含まれることになる。
【００９０】
（まとめ）
以上の説明のように、上記の手順１で特定した領域に存在する各マス目の４つの頂点座標が手順２の条件（式（１９））に該当するか否かを判定し、さらに、全てのマス目の４つの頂点座標が手順３（式（２０）または式（２１））の条件に該当するか否かを判定する。そして、何れかの頂点が何れかの条件に該当する場合には、そのマス目はアームＣＣ、すなわち判定対象部位ｉの占有領域に含まれると判定する。
【００９１】
［本実施形態の作用及び効果］
続いて、本実施形態にかかる双腕ロボットシステム１００の作用及び効果について説明する。本実施形態の双腕ロボットシステム１００では、識別番号、干渉情報、干渉条件情報、および占有情報が各部位間の干渉を判定するための必須の情報として用いられる。このうち、特に干渉情報および干渉条件情報は、移動部位が他の部位との間で立体交差する際に干渉を伴わない場合の値および干渉を伴う場合の値に基づいた情報である。したがって、これら干渉情報および干渉条件情報を用いることにより、本実施形態による干渉判定の結果には各部位間の立体交差時の干渉の有無が考慮されることとなる。これにより、干渉判定の精度が向上される。
【００９２】
また、本実施形態においては、双腕ロボット３の動作する空間を２次元平面上に投影した上で、干渉判定を行うため、例えば３次元での干渉判定に比べ、干渉判定処理を短時間で行うことが可能となる。また、本実施形態で用いる干渉情報は「０」または「１」を表す情報であり、そのデータ量が少ないことから、装置全体における計算量や計算時間が短縮できる。
【００９３】
また、本実施形態で用いる識別番号、干渉条件情報、および占有情報は双腕ロボット３および障害物５の各部位の数と同数の桁数を有する情報であるため、例えば部位の数が増えた場合には、増えた部位に関する干渉条件を設定した上で、増えた部位の数だけ識別番号、干渉条件情報、および占有情報の桁数を増やして、本実施形態における干渉判定を行うことができる。このように、本実施形態によれば、部位の数が増えた場合に、柔軟な対応が可能である。また、「０」か「１」かの少ないデータ量で計算を行うことから、本実施形態によれば、部位の数が増えたことに伴って、装置全体の計算量や計算時間が極端に増えることを防止できる。
【００９４】
また、本実施形態において、識別番号、干渉条件情報、および占有情報が、双腕ロボット３および障害物５の各部位の数と同数の桁数を有することは、言い換えれば各情報が必要最小限の桁数を有することであり、この場合には、演算に無駄がなく、必要最小限のメモリ量を用いて演算を行うことができる。
【００９５】
また、本実施形態によれば、双腕ロボット３の動作における動作開始時刻と動作終了時刻との間の各時刻に対して干渉判定が行われる。これにより、動作中の全ての時刻において干渉が生じないことを判断した上で、安全に双腕ロボット３を動作させることができる。
【００９６】
以上で説明した軌道生成部１および干渉判定部４は実際に双腕ロボット３を動作させる前に事前に干渉判定を行うシミュレーション装置としても利用することができる。つまり、図１に示した双腕ロボット３やサーボドライバ２ａ〜２ｄが実際には存在しない場合でも、これらの機械部分をパソコン上で擬似的に構成し、双腕ロボット３および障害物の各部位間の干渉判定をシミュレーションすることができる。
【００９７】
［他の実施形態］
以上、本発明の好適な実施形態について説明したが、本発明が上記実施形態に限定されないことは言うまでもない。例えば、図示はしないが、干渉判定演算部４２をマス目管理部４２４、占有情報記録部４２５、および判断部４２６のみを必須の構成要素とする簡素な構成としてもよい。
【００９８】
この場合のマス目管理部４２４は、前述の通り、双腕ロボット３の動作する空間を２次元平面上に投影した上で、その２次元平面をｎ１×ｎ２のマス目に区切った判定領域を作成するとともに、そのｎ１×ｎ２の個々のマス目に対応するメモリ領域を確保する。また、占有情報記録部４２５は、マス目管理部４２４による上記ｎ１×ｎ２のマス目のうち、双腕ロボット３および障害物５の各部位に対応するマス目である対応マス目を特定し、占有情報を部位毎に順次対応マス目に対応するメモリ領域に記録する。ここで、この場合の占有情報は、双腕ロボット３および障害物５の各部位が対応マス目を占有するか否かを示す情報である。また、判断部４２６は、占有情報をメモリ領域に記録するときに、そのメモリ領域に既に別の部位の占有情報が記録されている場合には、それらの部位同士が干渉すると判断する。
【００９９】
以上のような構成により、以下に示すような簡易的な干渉判定処理が可能となる。説明の便宜上、干渉判定部位が２個のみのケースを考える。例えば、右腕（識別番号ＩＤ＝０１）と左腕（識別番号ＩＤ＝１０）のみの干渉判定を行う場合が例として挙げられる。この場合に、最初に、マス目管理部４２４が、双腕ロボット３の動作空間を２次元平面上に投影した上で、その２次元平面をｎ１×ｎ２のマス目に区切った判定領域を作成するとともに、そのｎ１×ｎ２の個々のマス目に対応するメモリ領域を確保する。次に、占有情報記録部４２５が、ＩＤ＝０１の部位が占有するマス目を特定し、そのメモリ領域に占有情報として「ＩＤ＝０１を示す旨の情報」を書き込む。次に、占有情報記録部４２５が、ＩＤ＝１０の部位が占有するマス目を特定し、そのメモリ領域に占有情報として「ＩＤ＝１０を示す旨の情報」を書き込む。この「ＩＤ＝１０を示す旨の情報」の書き込み処理時に、占有情報記録部４２５が既に「ＩＤ＝０１を示す旨の情報」が書き込まれたメモリ領域に「ＩＤ＝１０を示す旨の情報」を書き込まなければならなくなった場合には、判断部４２６は、ＩＤ＝０１の部位とＩＤ＝１０の部位との間で部位同士が「干渉する」と判定する。一方で、「ＩＤ＝１０を示す旨の情報」を書き込むメモリ領域の何れにも「ＩＤ＝０１を示す旨の情報」が書き込まれていない場合には、部位同士が「干渉しない」と判定する。
【０１００】
以上のような簡易的な干渉判定処理は、干渉判定対象部位が少なく、２次元平面上での動作（例えばアームの旋回動作）だけ考慮すれば良いロボットおよび障害物に対して特に有効である。すなわち、マス目を使って２次元平面上での干渉判定を行うため、３次元的な空間やモデルを用いて干渉判定を行う場合に比べて極めて少ないデータ量で計算ができ、干渉判定を高速化でき、双腕ロボット３の２次元方向の運動のみを考慮する（ロボットアームの昇降軸の動作や冶具の上昇下降がない）場合に特に有効である。
【符号の説明】
【０１０１】
１００…双腕ロボットシステム、１…軌道生成部、１１…記憶部、１２…演算部、２ａ〜２ｄ…サーボドライバ、３ａ〜３ｄ…サーボモータ、３…双腕ロボット、３１，３２…水平多関節アーム、３１１…アーム３１の第１軸、３１２…アーム３１の第２軸、３１３…アーム３１の昇降軸、３２１…アーム３２の第１軸、３２２…アーム３２の第２軸、３２３…アーム３２の昇降軸、４…干渉判定部、４１…格納部、４１１…諸元等格納部、４１２…干渉情報格納部、４１３…干渉条件情報格納部、４２…干渉判定演算部、４２１…識別番号割当部、４２２…干渉情報生成部、４２３…干渉条件情報生成部、４２４…マス目管理部、４２５…占有情報記録部、４２６…判断部、５…障害物、ワーク、冶具、６…判定領域。
【技術分野】
【０００１】
本発明は、干渉判定装置および干渉判定方法に関するものである。
【背景技術】
【０００２】
先端に昇降軸を持つ二台の水平多関節アームを人間の両腕のように配置した双腕ロボットは、設置に要するスペースの割に自由度が高く、また両腕の作業領域の少なくとも一部が重複していることからワークを押さえつつねじ締めを行うなどの複合作業が可能であるという利点がある。しかし、各アームは移動経路で衝突する危険があり、作業者はその動きを予測することが困難である。そこで、双腕ロボットの各アームが他方のアームの作業領域に一部でも重複するような作業領域内で移動する際、互いのアームが相手の移動軌道上に重なるかどうかという干渉判定が必要となる。また、双腕ロボットが実際に作業を行う上で、左右のアーム同士以外にも、治具や安全柵などの障害物との干渉が生じる可能性があり、水平多関節アームと障害物の干渉判定も必要になる場合がある。
【０００３】
干渉判定を行う方法としては、いくつかの方式が存在する。例えば、工作機械を制御する数値制御装置で干渉チェックを行うために、「複数の機械構造物の干渉領域を各々定義し、前記干渉領域を移動させ、前記各々の機械構造物の干渉領域がそれぞれ干渉するかをチェックする機能を有する数値制御装置について、前記干渉領域として、直方体、円筒、平面の立体図形で定義し、各々の立体図形が交差するか交差しないかにより干渉をチェックする」という方式がある（特許文献１参照）。また、干渉判定を行う他の方法として、水平多関節ロボットに対して、「３次元ロボットモデルを２次元平面上に投影した２次元ロボットモデルに変換」し、「３次元障害物モデルを２次元障害物モデルとして分割」したものとの干渉判定を行う方式も存在する（特許文献２参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００９−０５４０４３号公報
【特許文献２】特開２００４−２０２６２７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
上記の特許文献１の発明のように、３次元方向の運動を行う３次元物体に対する干渉判定は、定義が容易な３次元の単純近似モデルに変換して行われることが多い。しかしながら、双腕ロボットの２次元方向の運動のみを考慮する（ロボットアームの昇降軸の動作と治具の上昇下降を考えない）場合に、これを３次元モデルで扱うことは処理時間の短縮やメモリ確保の観点で有益ではない。
【０００６】
また、特許文献２の発明においては、障害物モデルの分割は３次元障害物モデルの高さ毎に行われ、水平多関節ロボットの設置面からアームまでの高さ（高さは一定であるが、ある厚みを持つ）と一致する障害物モデルのみが干渉判定の対象となる。そのため、２次元ロボットモデルを用いているにもかかわらず、立体交差が可能な部分を干渉判定から除外した結果を短時間で得ることができる。この方式の優れている点は、水平多関節ロボットの高さと一致する分割面のみを判定すればよいという点である。しかしながら、例えば水平多関節ロボットを複数の分割面に分けなければならないような場合には、それぞれの分割面で干渉判定を行わなければならない。双腕ロボットもこれに該当し、ロボットアームを構成する各部分（第１リンク、第２リンク、先端の昇降軸など）で異なる高さを持つために干渉判定を行うべき分割面が多数になる可能性があり、このような場合には、演算量が増加し、処理に長時間を要するという問題がある。また、特許文献２の発明においては、２次元平面上に投影した２次元ロボットモデルの占有領域の重複だけで干渉判定を行っており、干渉のない立体交差は存在しないことを前提としている。つまり、特許文献２の場合において、干渉のない立体交差は考慮の対象に入っていない。しかし、干渉のない立体交差があるか否かを更に考慮に入れることにより、干渉判定の精度が更に向上することが期待される。
【０００７】
そこで、本発明は上記に鑑みてなされたもので、干渉のない立体交差を考慮しつつも、干渉判定を短時間で行うことが可能な干渉判定装置および干渉判定方法を提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記課題を解決するために、本発明の干渉判定装置は、３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定装置であって、複数の桁数を有し且つ他の識別番号との間で桁ごとの論理積を計算すると全ての桁が「０」となるように設定された識別番号を、前記各部位に割り当てる識別番号割当手段と、前記各部位のうち移動を伴う部位である移動部位が、他の前記部位との間で立体交差する際に干渉を伴わない場合には「０」、立体交差する際に干渉を伴う場合には「１」を表す情報を干渉情報として格納する干渉情報格納手段と、前記干渉情報に基づき設定され、前記複数の桁数を有し、前記移動部位が前記他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である干渉条件情報を、前記移動部位ごとに格納する干渉条件情報格納手段と、前記ロボットの動作する空間を２次元平面上に投影した上で、前記２次元平面をｎ１×ｎ２のマス目に区切るとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域を確保するマス目管理手段と、前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目を特定し、前記各部位の前記識別番号に基づく情報であり前記複数の桁数を有する占有情報を前記対応マス目に対応する前記メモリ領域に記録する占有情報記録手段と、前記占有情報および前記干渉条件情報における桁ごとの論理積を前記対応マス目ごとに求め、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断し、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断する判断手段と、を備えることを特徴とする。
【０００９】
また、本発明の干渉判定方法は、３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定方法であって、複数の桁数を有し且つ他の識別番号との間で桁ごとの論理積を計算すると全ての桁が「０」となるように設定された識別番号が、前記各部位に割り当てられ、前記各部位のうち移動を伴う部位である移動部位が、他の前記部位との間で立体交差する際に干渉を伴わない場合には「０」、立体交差する際に干渉を伴う場合には「１」を表す情報が、干渉情報として格納され、前記干渉情報に基づき設定され、前記複数の桁数を有し、前記移動部位が前記他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である干渉条件情報が、前記移動部位ごとに格納され、前記ロボットの動作する空間が２次元平面上に投影された上で、前記２次元平面がｎ１×ｎ２のマス目に区切られるとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域が確保され、前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目が特定され、前記各部位の前記識別番号に基づく情報であり前記複数の桁数を有する占有情報が前記対応マス目に対応する前記メモリ領域に記録され、前記占有情報および前記干渉条件情報における桁ごとの論理積が前記対応マス目ごとに求められ、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断され、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断される、ことを特徴とする。
【００１０】
このような本発明の干渉判定装置および干渉判定方法では、識別番号、干渉情報、干渉条件情報、および占有情報が各部位間の干渉を判定するための必須の情報として用いられる。このうち、特に干渉情報および干渉条件情報は、移動部位が他の部位との間で立体交差する際に干渉を伴わない場合の値および干渉を伴う場合の値に基づいた情報である。したがって、これら干渉情報および干渉条件情報を用いることにより、本発明による干渉判定の結果には各部位間の立体交差時の干渉の有無が考慮されることとなる。これにより、干渉判定の精度が向上される。
【００１１】
また、本発明においては、ロボットの動作する空間を２次元平面上に投影した上で、干渉判定を行うため、例えば３次元での干渉判定に比べ、干渉判定処理を短時間で行うことが可能となる。また、本発明で用いる干渉情報は「０」または「１」を表す情報であり、そのデータ量が少ないことから、装置全体における計算量や計算時間が短縮できる。
【００１２】
また、本発明で用いる識別番号、干渉条件情報、および占有情報はロボットおよび障害物の各部位の数と同数の桁数を有する情報であるため、例えば部位の数が増えた場合には、増えた部位に関する干渉条件を設定した上で、増えた部位の数だけ識別番号、干渉条件情報、および占有情報の桁数を増やして、本発明における干渉判定を行うことができる。このように、本発明によれば、部位の数が増えた場合に、柔軟な対応が可能である。また、「０」か「１」かの少ないデータ量で計算を行うことから、部位の数が増えたことに伴って、装置全体の計算量や計算時間が極端に増えることを防止できる。
【００１３】
また、本発明において、前記干渉条件情報格納手段は、下記の数式（１）により計算される情報Ｃｉを前記干渉条件情報として格納しても良い。
【数１】
ただし、ｈは前記各部位の数であり、ｉおよびｊは１以上ｈ以下の自然数であり、Ｃｉはｉ番目の部位の前記干渉条件情報であり、ＩＤｊはｊ番目の部位の前記識別番号であり、ｔｉｊはｉ番目の部位とｊ番目の部位における前記干渉情報である。
【００１４】
この発明によれば、干渉条件情報を計算するための具体的な手法が提供される。
【００１５】
また、本発明において、前記占有情報記録手段は、前記メモリ領域が初期化されている場合には、前記識別番号を前記占有情報として前記メモリ領域に記録し、前記メモリ領域が初期化されていない場合には、既存の占有情報と前記識別番号との間の桁ごとの論理和を前記占有情報として前記メモリ領域に記録しても良い。
【００１６】
この発明によれば、占有情報を記録するための具体的な手法が提供される。
【００１７】
また、本発明においては、前記ロボットの動作における動作開始時刻と動作終了時刻との間が一定時間間隔ごとに区切られ、当該区切られた各時刻における前記各部位の位置に対して、前記占有情報記録手段は、前記各時刻における前記対応マス目を特定し、前記占有情報を前記対応マス目に対応する前記メモリ領域に記録し、前記判断手段は、前記占有情報および前記干渉条件情報における桁ごとの論理積を前記対応マス目ごとに求め、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断し、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断しても良い。
【００１８】
この発明によれば、ロボットの動作における動作開始時刻と動作終了時刻との間の各時刻に対して干渉判定が行われる。これにより、動作中の全ての時刻において干渉が生じないことを判断した上で、安全にロボットを動作させることができる。
【００１９】
また、本発明において、前記ロボットは、水平多関節アームを有する双腕ロボットであっても良い。
【００２０】
この発明は、ロボットが水平多関節アームを有する双腕ロボットである場合に特に有用である。
【００２１】
また、本発明において、前記識別番号、前記干渉条件情報、および前記占有情報は、前記各部位の数と同数の前記複数の桁数を有する情報であっても良い。
【００２２】
この発明は、識別番号、干渉条件情報、および占有情報が、ロボットおよび障害物の各部位の数と同数の桁数を有することから、各情報が必要最小限の桁数を有することとなり、演算に無駄がなく、必要最小限のメモリ量を用いて演算を行うことができる。
【００２３】
また、本発明は、３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定装置であって、前記ロボットの動作する空間を２次元平面上に投影した上で、前記２次元平面をｎ１×ｎ２のマス目に区切るとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域を確保するマス目管理手段と、前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目を部位毎に特定し、前記各部位が前記対応マス目を占有するか否かを示す占有情報を前記部位毎に順次前記対応マス目に対応する前記メモリ領域に記録する占有情報記録手段と、前記占有情報を前記メモリ領域に記録するときに、そのメモリ領域に既に別の部位の前記占有情報が記録されている場合には、それらの部位同士が干渉すると判断する判断手段と、を備えることを特徴とする。
【００２４】
このような本発明の干渉判定装置は、マス目管理手段、占有情報記録手段、および判断手段のみを必須の構成要素とする簡素な構成となり、このような構成により、簡易的な干渉判定処理が可能となる。この簡易的な干渉判定処理は、干渉判定対象部位が少なく、２次元平面上での動作（例えばアームの旋回動作）だけ考慮すれば良いロボットおよび障害物に対して特に有効である。すなわち、マス目を使って２次元平面上での干渉判定を行うため、３次元的な空間やモデルを用いて干渉判定を行う場合に比べて極めて少ないデータ量で計算ができ、干渉判定を高速化でき、ロボットの２次元方向の運動のみを考慮する（ロボットアームの昇降軸の動作や冶具の上昇下降がない）場合に特に有効である。
【発明の効果】
【００２５】
本発明によれば、干渉のない立体交差を考慮しつつも、干渉判定を短時間で行うことが可能な干渉判定装置および干渉判定方法を提供することができる。
【図面の簡単な説明】
【００２６】
【図１】双腕ロボットシステム１００の構成概要図である。
【図２】双腕ロボット３の側面概要図である。
【図３】双腕ロボット３の上面概要図である。
【図４】干渉判定部４の機能的構成要素を示したブロック図である。
【図５】識別番号割当部４２１が双腕ロボット３および障害物５の各部位に割り当てた識別番号ＩＤｉの一例を示す図である。
【図６】干渉情報生成部４２２が作成したテーブルの一例を示す図である。
【図７】干渉条件情報生成部４２３により生成された干渉条件情報の一例を示す図である。
【図８】干渉条件情報生成部４２３により生成された干渉条件情報の一例を示す図である。
【図９】マス目管理部４２４により作成された判定領域６の一例を示す図である。
【図１０】双腕ロボットシステム１００にて行われるメイン処理を示すフローチャートである。
【図１１】干渉判定部４による干渉判定処理を示すフローチャートである。
【図１２】図１１のステップＳ２０５にて諸元等格納部４１１に記憶された角度テーブルの一例を示す図である。
【図１３】図１１のループ１内のある時刻におけるステップＳ２０８からステップＳ２１０の処理の具体例を示す図である（ｉ＝１の場合）。
【図１４】図１１のループ１内のある時刻におけるステップＳ２０８からステップＳ２１０の処理の具体例を示す図である（ｉ＝２の場合）。
【図１５】図１１のループ１内のある時刻におけるステップＳ２０８からステップＳ２１０の処理の具体例を示す図である（ｉ＝６の場合）。
【図１６】占有情報記録部４２５による占有領域の特定方法を説明するための図である。
【図１７】占有情報記録部４２５による占有領域の特定方法を説明するための図である（手順１）。
【図１８】占有情報記録部４２５による占有領域の特定方法を説明するための図である（手順２）。
【図１９】占有情報記録部４２５による占有領域の特定方法を説明するための図である（手順３）。
【発明を実施するための形態】
【００２７】
以下、添付図面を参照して本発明にかかる干渉判定装置および干渉判定方法の好適な実施形態を詳細に説明する。なお、図面の説明において同一の要素には同一の符号を付し、重複する説明を省略する。
【００２８】
［双腕ロボットシステム１００の全体構成］
まず、本発明の実施形態に係る双腕ロボットシステム１００の全体構成について、図１〜３を参照しながら説明する。図１は、双腕ロボットシステム１００の構成概要図である。図１に示すように、双腕ロボットシステム１００は、軌道生成部１、サーボドライバ２ａ〜２ｄ、双腕ロボット３、および干渉判定部４を含んで構成され、各構成要素は例えば図１にて矢印で表現される通信手段により互いに通信できるように接続されている。図２は双腕ロボット３の側面概要図であり、図３は双腕ロボット３の上面概要図である。
【００２９】
双腕ロボット３は、例えば図２および図３に示されるように、先端に昇降軸３１３および３２３を持つ二台の水平多関節アーム３１および３２を有する。水平多関節アーム３１および３２は、人間の両腕のように作業領域の少なくとも一部が重複する間隔で例えばワーク５を挟んで配置される。各アームの第１軸３１１および３２１の付け根（双腕ロボット３の肩部分）と、第２軸３１２および３２２の付け根（双腕ロボット３のひじ部分）には、それぞれ、サーボモータ３ａ〜３ｄが設けられている。この構成により各軸は水平方向に旋回移動できるようになっている。
【００３０】
図１にて示される軌道生成部１は、双腕ロボット３の各部位が現在位置から目標位置まで移動できるように、各関節の移動量および移動速度を演算し、各サーボドライバに指令を送る部分である。軌道生成部１は、記憶部１１および演算部１２を含んで構成される。記憶部１１は、軌道生成に必要な双腕ロボット３の概形寸法や移動に関する補間情報ならびに速度情報などの諸元と、教示もしくはロボット言語によるプログラミングによって記述されたロボットの動作順序を制御する動作プログラム、および干渉判定に必要な干渉条件を格納する部分である。演算部１２は、双腕ロボット３の各部位が現在位置から目標位置まで移動できるように、各関節の移動量および移動速度を演算する部分である。双腕ロボット３の移動前に、軌道生成部１の演算部１２は干渉判定部４へ、移動前の位置から移動後の位置に移動する場合の移動中および移動後の干渉の有無を問い合わせる。
【００３１】
サーボドライバ２ａ〜２ｄは、軌道生成部１からの指令を受けて、双腕ロボット３の各関節に設けられたサーボモータ３ａ〜３ｄをそれぞれ駆動し、駆動後の現在位置を軌道生成部１にフィードバックする部分である。
【００３２】
［干渉判定部４の構成］
以下、双腕ロボットシステム１００の一構成要素である干渉判定部４（特許請求の範囲における「干渉判定装置」に相当）の構成について、図４〜図９を更に参照しながら詳細に説明する。干渉判定部４は、軌道生成部１から問い合わせを受けて、３次元で動作する双腕ロボット３の各部位（移動部位）と、障害物としてワークまたは治具の各部位（固定部位）に対して干渉判定を行う部分である。図４は、干渉判定部４の機能的構成要素を示したブロック図である。図４に示されるように、干渉判定部４は、格納部４１および干渉判定演算部４２を含んで構成される。なお、以下の説明においては、「移動部位」とは、双腕ロボット３および障害物５の各部位のうち移動を伴う部位をいい、「固定部位」とは、双腕ロボット３および障害物５の各部位のうち移動を伴わない部位をいい、「部位」は、移動部位と固定部位の総称である。
【００３３】
干渉判定部４の格納部４１は、諸元等格納部４１１、干渉情報格納部４１２（特許請求の範囲における「干渉情報格納手段」に相当）、および干渉条件情報格納部４１３（特許請求の範囲における「干渉条件情報格納手段」に相当）を含んで構成される。諸元等格納部４１１は、電源投入後に受け取った双腕ロボット３の諸元、および軌道生成部１からの問い合わせ毎に受け取った移動前後の移動部位毎の角度を格納する部分である。干渉情報格納部４１２は、双腕ロボット３および障害物５の移動部位における干渉情報を格納する部分である。干渉情報は、後に詳細に説明されるように、移動部位が他の部位との間で立体交差する際に、干渉を伴わない場合には「０」を、干渉を伴う場合には「１」を表す情報である。干渉条件情報格納部４１３は、干渉条件情報を移動部位ごとに格納する部分である。干渉条件情報は、後に詳細に説明されるように、干渉情報に基づき設定され、双腕ロボット３および障害物５の各部位の数と同数の複数の桁数を有し、移動部位が他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である。
【００３４】
干渉判定部４の干渉判定演算部４２は、格納部４１に格納された各情報に基づき干渉判定を行う部分である。干渉判定演算部４２は、双腕ロボット３の移動前後の各関節の角度に対して、更に一定時間毎に分割された移動過程の各関節の角度に対して、それぞれ干渉判定を行う。干渉判定演算部４２は、図４に示されるように、識別番号割当部４２１（特許請求の範囲における「識別番号割当手段」に相当）、干渉情報生成部４２２（特許請求の範囲における「干渉情報格納手段」に相当）、干渉条件情報生成部４２３（特許請求の範囲における「干渉条件情報格納手段」に相当）、マス目管理部４２４（特許請求の範囲における「マス目管理手段」に相当）、占有情報記録部４２５（特許請求の範囲における「占有情報記録手段」に相当）、および判断部４２６（特許請求の範囲における「判断手段」に相当）を含んで構成される。以下、干渉判定演算部４２を構成する各構成要素について詳細に説明する。
【００３５】
［識別番号割当部４２１］
識別番号割当部４２１は、双腕ロボット３および障害物５の各部位に識別番号を割り当てる部分である。各識別番号は、双腕ロボット３および障害物５の各部位の数と同数の複数の桁数を有し且つ他の全ての識別番号との間で桁ごとの論理積を計算すると全ての桁が「０」となるように設定される情報である。具体的に、識別番号割当部４２１が図２および図３にて示した双腕ロボット３および障害物５の各部位に割り当てた識別番号ＩＤｉの一例を図５に示す。図５の一例において、識別番号割当部４２１は、アーム３１の第１軸（第１リンク）３１１に対して、識別番号ＩＤ１として「１００００００」を割り当てている。また、識別番号割当部４２１は、アーム３２の昇降軸３２３に対して、識別番号ＩＤ６として「０００００１０」を割り当てている。更に、識別番号割当部４２１は、冶具５に対して、識別番号ＩＤ７として「００００００１」を割り当てている。ここで挙げた一例においては、双腕ロボット３および障害物５の各部位が「７つ」あるため、各識別番号ＩＤｉの桁数は「７」となっている。また、ＩＤ１からＩＤ７までの桁ごとの論理積を計算すると、全ての桁において「０」となる。
【００３６】
［干渉情報生成部４２２］
干渉情報生成部４２２は、双腕ロボット３の各移動部位と他の部位との間で、干渉のない立体交差が存在しない場合（つまり立体交差する際に干渉を伴う場合）は「１」とし、干渉のない立体交差が可能である場合（つまり立体交差する際に干渉を伴わない場合）もしくは自分自身との関係は「０」としたテーブルを作成する部分である。
【００３７】
具体的に、干渉情報生成部４２２が作成したテーブルの一例を図６に示す。図６の一例において、例えばｉ＝１およびｊ＝２の場合における干渉の有無を示す干渉情報ｔ１２の値は「０」であり、これはｉ＝１に相当するアーム３１の第１軸３１１とｊ＝２に相当するアーム３１の第２軸３１２との間では、干渉のない立体交差が存在すること、つまり立体交差する際に干渉を伴わないことを示す。また、例えばｉ＝４およびｊ＝３の場合における干渉の有無を示す干渉情報ｔ４３の値は「１」であり、これはｉ＝４に相当するアーム３２の第１軸３２１とｊ＝３に相当するアーム３１の昇降軸３１３との間では、干渉のない立体交差が存在しないこと、つまり立体交差する際に干渉を伴うことを示す。また、例えばｉ＝５およびｊ＝５の場合における干渉の有無を示す干渉情報ｔ５５の値は「０」であり、これはｉ＝５に相当するアーム３２の第２軸３２２が自分自身との間で干渉することは有り得ないことを示す。
【００３８】
干渉情報生成部４２２は、このようにして作成したテーブルを格納部４１の干渉情報格納部４１２に格納させる。なお、干渉情報生成部４２２は、移動を伴わない部位である固定部位つまり識別番号ＩＤ７の冶具５に対しては、干渉情報ｔ７１、ｔ７２などを求めなくても良い。他の移動部位の干渉情報ｔ１７、ｔ２７などにおいて冶具５との干渉の有無が既に考慮されているからである。また、この意味では、干渉情報ｔｉｊの値と干渉情報ｔｊｉの値は常に同じであるため、図６の一例をｔｉｊかｔｊｉのうち何れか一方のみを明らかにしたテーブルにしても良い。
【００３９】
［干渉条件情報生成部４２３］
干渉条件情報生成部４２３は、干渉条件情報を生成する部分である。干渉条件情報は、干渉情報に基づき設定され、双腕ロボット３および障害物５の各部位の数と同数の複数の桁数を有し、移動部位が他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である。干渉条件情報生成部４２３は、下記の数式（１）により計算される情報Ｃｉを干渉条件情報として生成する。
【数２】
ただし、ｈは双腕ロボット３および障害物５の各部位の数であり、ｉおよびｊは１以上ｈ以下の自然数であり、Ｃｉはｉ番目の移動部位の干渉条件情報であり、ＩＤｊはｊ番目の部位の識別番号であり、ｔｉｊはｉ番目の移動部位とｊ番目の部位における干渉情報である。
【００４０】
以下、干渉条件情報生成部４２３による干渉条件情報の生成方法を詳細に説明する。まず、簡単な一例として、干渉判定部位が２個のみのケースを考える。すなわち、右腕（ＩＤ１＝０１）と左腕（ＩＤ２＝１０）のみの干渉判定を行う場合を考える。この場合に干渉条件情報生成部４２３により生成される干渉条件情報を図７に示す。なお、図７の一例においては、右腕と左腕とが立体交差する際に干渉を伴うとして、干渉情報が設定されている。つまり、ｔ１２＝１であり、ｔ２１＝１である。
【００４１】
図７の一例において、ｉ＝１の識別番号ＩＤ１の部位である右腕に対して干渉条件情報Ｃ１を求めると、上記式（１）に従い、
Ｃ１＝ＩＤ１・ｔ１１＋ＩＤ２・ｔ１２…（２）
である。そして、図７の一例において、ＩＤ１＝０１、ｔ１１＝０、ＩＤ２＝１０、ｔ１２＝１であるため、それぞれの値を上記式（２）に代入すると、
Ｃ１＝０１・０＋１０・１＝１０…（３）
となり、識別番号ＩＤ１の右腕の干渉条件情報Ｃ１の値は１０として求められる。
【００４２】
続いて、より複雑な例として、図２および図３にて示した干渉判定部位が７つであるケースを考える。説明の便宜上、図５および図６の場合と同様の識別番号および干渉情報について考える。この場合に干渉条件情報生成部４２３により生成される干渉条件情報を図８に示す。この場合において、ｉ＝１の識別番号ＩＤ１の部位であるアーム３１の第１軸３１１に対して干渉条件情報Ｃ１を求めると、上記式（１）に従い、
Ｃ１＝ＩＤ１・ｔ１１＋ＩＤ２・ｔ１２＋ＩＤ３・ｔ１３＋ＩＤ４・ｔ１４＋ＩＤ５・ｔ１５＋ＩＤ６・ｔ１６＋ＩＤ７・ｔ１７…（４）
である。そして、ＩＤ１＝１００００００、ｔ１１＝０、ＩＤ２＝０１０００００、ｔ１２＝０、ＩＤ３＝００１００００、ｔ１３＝１、ＩＤ４＝０００１０００、ｔ１４＝１、ＩＤ５＝００００１００、ｔ１５＝０、ＩＤ６＝０００００１０、ｔ１６＝１、ＩＤ７＝００００００１、ｔ１７＝０であるため、それぞれの値を上記式（４）に代入すると、
Ｃ１＝１００００００・０
＋０１０００００・０
＋００１００００・１
＋０００１０００・１
＋００００１００・０
＋０００００１０・１
＋００００００１・０
＝００１１０１０…（５）
となり、識別番号ＩＤ１のアーム３１の第１軸３１１の干渉条件情報Ｃ１の値は００１１０１０として求められる。
【００４３】
同様に、他の部位に対して干渉条件情報を求めることができ、その結果が図８にＣ２〜Ｃ６の値として示されている。干渉条件情報生成部４２３は、このようにして生成した干渉条件情報を格納部４１の干渉条件情報格納部４１３に格納させる。なお、干渉条件情報生成部４２３は、固定部位である識別番号ＩＤ７の冶具５に対しては干渉条件情報を求めなくても良い。移動部位に対する干渉条件情報であるＣ１〜Ｃ６の値に、固定部位の冶具５との間の干渉の有無が既に反映されているからである。
【００４４】
［マス目管理部４２４］
マス目管理部４２４は、双腕ロボット３の動作する空間を２次元平面上に投影した上で、その２次元平面をｎ１×ｎ２のマス目に区切った判定領域を作成するとともに、そのｎ１×ｎ２の個々のマス目に対応するメモリ領域を確保する部分である。図９は、マス目管理部４２４により作成された判定領域６の一例を示す。判定領域６は、双腕ロボット３の姿勢および障害物５の位置を演算するために要する諸元を格納した諸元等格納部４１１の値を元に、双腕ロボット３の動作する範囲を包含する空間、もしくは双腕ロボット３の動作する範囲の一部であって干渉判定の対象となる空間を２次元平面上に投影したｎ１×ｎ２の２次元平面のマス目で表現されるものである。
【００４５】
［占有情報記録部４２５］
占有情報記録部４２５は、マス目管理部４２４による上記ｎ１×ｎ２のマス目のうち、双腕ロボット３および障害物５の各部位に対応するマス目である対応マス目を特定し、占有情報を上記対応マス目に対応するメモリ領域に記録する部分である。図９は、占有情報記録部４２５により特定された対応マス目の一例を示す。図９の一例では、同じ平面上に投影した判定対象となる姿勢をとった双腕ロボット３や障害物５の各部位が存在する領域（以下、「占有領域」ともいう。）に対応する対応マス目が符号３１１ｍ、３１２ｍ、３１３ｍ、３２１ｍ、３２２ｍ、３２３ｍ、および５ｍで示されるように特定されている。なお、対応マス目を特定する方法については、後述の［占有領域の特定方法］の欄にて詳細に説明する。
【００４６】
占有情報記録部４２５は、対応マス目の特定後に、占有情報を当該対応マス目に対応するメモリ領域に記録する。占有情報は、双腕ロボット３および障害物５の各部位の識別番号に基づく情報であり、双腕ロボット３および障害物５の各部位の数と同数の複数の桁数を有する情報である。占有情報記録部４２５は、占有情報を記録する対象となるメモリ領域が初期化されている場合には、対応マス目を占有する部位の識別番号を占有情報としてメモリ領域に記録する。一方で、占有情報記録部４２５は、占有情報を記録する対象となるメモリ領域が初期化されていない場合（既に占有情報が記録されている場合）には、既存の占有情報と対応マス目を占有する部位の識別番号との間の桁ごとの論理和を占有情報としてメモリ領域に記録する。占有情報記録部４２５による占有情報の記録により、個々のマス目を占有する双腕ロボット３および障害物５の各部位に対応するビットが立った状態となる。なお、占有情報記録部４２５による動作の詳細については後述する。
【００４７】
［判断部４２６］
判断部４２６は、占有情報および干渉条件情報における桁ごとの論理積を対応マス目ごとに求め、当該論理積の結果に基づき干渉判断を行うものである。判断部４２６は、当該論理積の結果が全ての桁において「０」であれば、双腕ロボット３および障害物５の各部位同士は干渉しないと判断する。一方で、判断部４２６は、当該論理積の結果が少なくとも一つの桁において「１」であれば、双腕ロボット３および障害物５の各部位同士は干渉すると判断する。判断部４２６は、干渉判断の結果を示す情報を軌道生成部１に出力する。判断部４２６は、干渉判断の結果を出力する際に、干渉が生じている部位を特定する情報をともに出力するようにしても良い。
【００４８】
［双腕ロボットシステム１００の動作］
続いて、双腕ロボットシステム１００により行われる動作について、図１０〜図１５を参照しながら説明する。下記の説明は、双腕ロボットシステム１００におけるメイン処理と、干渉判定部４による干渉判定処理とに大別される。
【００４９】
［メイン処理］
まず、双腕ロボットシステム１００におけるメイン処理について図１０を参照しながら説明する。図１０は、双腕ロボットシステム１００にて行われるメイン処理を示すフローチャートである。
【００５０】
最初に、軌道生成部１の演算部１２が、記憶部１１に格納されているパラメータを干渉判定部４の干渉判定演算部４２に送信する。このパラメータは、干渉判定の対象部位毎に設定されるものであり、上述した干渉情報ｔｉｊを含む（ステップＳ１０１）。
【００５１】
次に、軌道生成部１の演算部１２が、各サーボドライバ２ａ〜２ｄから双腕ロボット３の各部位の現在角度を読み込む（ステップＳ１０２）。
【００５２】
次に、軌道生成部１の演算部１２が、記憶部１１に格納された動作プログラムを解析して、双腕ロボット３の各部位の移動先の角度（目標角度）を求める（ステップＳ１０３）。
【００５３】
次に、軌道生成部１の演算部１２が、双腕ロボット３の各部位の現在角度と目標角度を干渉判定部４の干渉判定演算部４２に送信する（ステップＳ１０４）。
【００５４】
次に、軌道生成部１の演算部１２は干渉判定部４の干渉判定演算部４２からの判定結果信号を待つ（ステップＳ１０５）。この間、干渉判定部４では詳細を後述する干渉判定処理が行われ、双腕ロボット３のアーム３１，３２の移動の全体で部位同士の干渉が生じないかどうかが判定される。そして、干渉判定部４の干渉判定演算部４２からの判定結果信号が「干渉なし」の場合（ステップＳ１０６：ＹＥＳ）、軌道生成部１の演算部１２は、表示部（図示せず）に「干渉なし」の表示をさせ（ステップＳ１０７）、記憶部１１に格納されている動作プログラムを実行させ、双腕ロボット３の動作制御を行う（ステップＳ１０８）。
【００５５】
一方で、干渉判定部４の干渉判定演算部４２からの判定結果信号が「干渉あり」の場合には（ステップＳ１０６：ＮＯ）、軌道生成部１の演算部１２は、表示部（図示せず）に「干渉あり」の表示をさせ（ステップＳ１０９）、そのまま処理を終了させる。以上のように軌道生成部１は、双腕ロボット３の移動の全体で部位同士の干渉が生じない判定結果が得られるまでサーボドライバ２ａ〜２ｄに指令を送らずに待機し、干渉が生じる場合には双腕ロボット３の各移動部位の移動を開始させない。
【００５６】
［干渉判定処理］
引き続き、干渉判定部４による干渉判定処理について図１１および図１２を参照しながら説明する。図１１は、干渉判定部４による干渉判定処理を示すフローチャートであり、図１２はステップＳ２０５の説明で参照する図である。
【００５７】
最初に、識別番号割当部４２１が干渉判定の対象部位毎に識別番号ＩＤｉ（ｉ＝１〜ｈ）を割り当てる（ステップＳ２０１、図５参照）。
【００５８】
次に、干渉条件情報生成部４２３が、ステップＳ２０１にて割り当てられた識別番号ＩＤｉと、干渉情報生成部４２２により作成され干渉情報格納部４１２に格納されたテーブル（図６参照）に記載された干渉情報ｔｉｊに基づいて、上述した式（１）に従い干渉条件情報Ｃｉを生成する（ステップＳ２０２、図７および図８参照）。
【００５９】
次に、マス目管理部４２４が、判定領域（ｎ１×ｎ２のマス目領域、図９参照）に対応するメモリ領域を確保する（ステップＳ２０３）。
【００６０】
次に、干渉判定演算部４２はステップＳ１０４（図１０参照）にて軌道生成部１から双腕ロボット３の各部位の現在角度と目標角度が入力されることを待つ（ステップＳ２０４）。
【００６１】
軌道生成部１から双腕ロボット３の各部位の現在角度と目標角度の入力があった場合に（ステップＳ２０４：ＹＥＳ）、干渉判定演算部４２は、双腕ロボット３の各部位の目標角度と現在角度の差Δθおよび各部位の速度パラメータから一定時間間隔の各部位の角度を求め、これを諸元等格納部４１１の角度テーブルに記憶する。図示はしないが、この処理のために干渉判定演算部４２が別途の処理部を備えていても良い。図１２は諸元等格納部４１１に記憶された角度テーブルの一例を示す。図１２に示されるように、角度テーブルは、一定時間間隔毎のレコード（レコード数：ｍ）に部位毎のフィールドを備えて構成されるものである（ステップＳ２０５）。
【００６２】
以下、ステップＳ２０６からステップＳ２１３までのループ１が開始される。ステップＳ２０５の次に、マス目管理部４２４が、判定領域（ｎ１×ｎ２のマス目領域、図９参照）に対応するメモリ領域に占有情報の初期値として０を設定することにより、メモリ領域を初期化する（ステップＳ２０６）。
【００６３】
次に、占有情報記録部４２５が、図１２にて例示した角度テーブルからｋ番目（ｋ＝１〜ｍ）のレコードの角度データを読み込む（ステップＳ２０７）。
【００６４】
以下、ステップＳ２０８からステップＳ２１０までのループ２が開始される。ステップＳ２０７の次に、占有情報記録部４２５が、干渉判定の対象部位ｉの角度データと寸法パラメータから判定領域（二次元平面）において干渉判定の対象部位ｉが占有するマス目、すなわち対応マス目を特定する。なお、対応マス目を特定する処理については、後述の［占有領域の特定方法］の欄で詳細に説明する。また、占有情報記録部４２５は、特定した当該対応マス目に対応するメモリ領域に記録された占有情報を取得する（ステップＳ２０８）。
【００６５】
次に、占有情報記録部４２５が、干渉判定の対象部位ｉが占有するメモリ領域の内、既存の占有情報が０のメモリ領域には占有情報として識別番号ＩＤｉを書き込み、既存の占有情報が０以外のメモリ領域には既存の占有情報と識別番号ＩＤｉの論理和を占有情報として書き込む（ステップＳ２０９）。
【００６６】
次に、判断部４２６が、最前のステップＳ２０９の処理で占有情報を書き込んだ全てのメモリ領域について、干渉条件Ｃｉとの論理積を求める（ステップＳ２１０）。
【００６７】
ステップＳ２０８からステップＳ２１０までの処理をｉ＝ｈとなるまで、すなわち全ての干渉判定の対象部位について繰り返す。これにより、ループ２が終了し、ある時点ｋにおける双腕ロボット３の姿勢に対する干渉判定が終了する。なお、ループ２の処理の具体例については、下記の［ループ２の処理の具体例］の欄で後述する。
【００６８】
ループ２が終了した時点で、判断部４２６はステップＳ２１０にて求めたそれぞれの論理積が全ての桁において０か否かを判断する（ステップＳ２１１）。ステップＳ２１１の判断にて、全ての論理積が全ての桁において０ならば（ステップＳ２１１：ＹＥＳ）、判断部４２６は「干渉なし」を表す情報を格納部４１に記憶させる（ステップＳ２１２）。一方で、ステップＳ２１１の判断にて、何れかの論理積に全ての桁において０でないものがある場合には（ステップＳ２１１：ＮＯ）、判断部４２６は「干渉あり」を表す情報を格納部４１に記憶させる（ステップＳ２１３）。格納部４１はこの記憶処理のために別途の記憶手段を更に備えていても良く、諸元等格納部４１１がこの記憶処理を行っても良い。
【００６９】
ステップＳ２０６からステップＳ２１３までの処理をｋ＝ｍとなるまで、すなわち各部位の目標角度まで繰り返す。これにより、ループ１が終了し、双腕ロボット３の各アームが目標とする姿勢に移行するまでの全ての姿勢における干渉判定処理が終了する。
【００７０】
ループ１が終了した時点で、判断部４２６は、ステップＳ２１２またはステップＳ２１３にて格納部４１に記憶された一定時間間隔毎の判定結果情報から最終的な判定結果を求め、これを判定結果信号として軌道生成部１に送る。判定結果は、一定時間間隔毎の判定結果情報の全てが干渉なしなら「干渉なし」、一つでも干渉ありがあれば「干渉あり」となる（ステップＳ２１４）。このような干渉判定処理により、双腕ロボット３のアーム３１，３２が初期の姿勢から目標とする姿勢まで動作する間の一定時間間隔毎の各姿勢について、部位同士の干渉の有無を確認することができる。
【００７１】
［ループ２の処理の具体例］
続いて、図１３〜図１５を参照しながら、ループ２の処理の具体例について説明する。図１３〜図１５は、ループ１内のある時刻（例えばｋ番目の時刻）におけるステップＳ２０８からステップＳ２１０までの処理の具体例を示す。図１３はｉ＝１の場合の処理の具体例であり、図１４はｉ＝２の場合の処理の具体例であり、図１５はｉ＝６の場合の処理の具体例である。
【００７２】
図１３が示すｉ＝１の場合には、アーム３１の第１軸３１１に対してステップＳ２０８からステップＳ２１０までの処理が行われる。すなわち、最初に、占有情報記録部４２５が判定領域６において第１軸３１１の占有領域（図１３にてドットを付した領域Ｈ１に対応するマス目）を特定する（ステップＳ２０８）。次に、占有情報記録部４２５がステップＳ２０８にて特定した対応マス目に対応するメモリ領域に占有情報を書き込む。この時、全てのメモリ領域は、ステップＳ２０６で初期化された状態（占有情報の初期値として「０」が書き込まれた状態）にあることから、領域Ｈ１のマス目に対応するメモリ領域には第１軸３１１の識別番号ＩＤ１＝「１００００００」が書き込まれる（ステップＳ２０９）。次に、占有情報記録部４２５が、最前のステップＳ２０９で書き込まれた占有情報それぞれと干渉条件Ｃ１との論理積を求める（ステップＳ２１０）。つまり、判断部４２６は、下記の式（６）にて示される論理積を求める。
領域Ｈ１のマス目に係る論理積＝
１００００００
×００１１０１０
＝０００００００・・・（６）
【００７３】
図１４が示すｉ＝２の場合には、アーム３１の第２軸３１２に対してステップＳ２０８からステップＳ２１０までの処理が行われる。すなわち、最初に、占有情報記録部４２５が判定領域６において第２軸３１２の占有領域（図１４にてドットを付した領域Ｈ２およびハッチを付した領域Ｈ３に対応するマス目）を特定する（ステップＳ２０８）。領域Ｈ２は第２軸３１２のみが存在する領域であり、領域Ｈ３は第２軸３１２と第１軸３１１が重なる領域である。次に、占有情報記録部４２５がステップＳ２０８にて特定した対応マス目に対応するメモリ領域に占有情報を書き込む。メモリ領域が初期化された状態のままの領域Ｈ２のマス目に対応するメモリ領域には第２軸３１２の識別番号ＩＤ２＝「０１０００００」が書き込まれる。一方、ｉ＝１での書き込みによりメモリ領域が初期化された状態になくなっている領域Ｈ３のマス目に対応するメモリ領域には既存の占有情報「１００００００」と第２軸３１２の識別番号ＩＤ２＝「０１０００００」の論理和が新たな占有情報として書き込まれる。つまり、占有情報記録部４２５は、下記の式（７）によって示される占有情報を領域Ｈ３のマス目に対応するメモリ領域に記録する。
領域Ｈ３のマス目に対応するメモリ領域に書き込まれる占有情報＝
１００００００
＋０１０００００
＝１１０００００・・・（７）
【００７４】
次に、判断部４２６が、領域Ｈ２と領域Ｈ３のそれぞれに対応するマス目について、次の式（８）および式（９）によって示される占有情報と干渉条件Ｃ２との論理積をマス目ごとに求める。
領域Ｈ２のマス目に係る論理積＝
０１０００００
×００００１１０
＝０００００００・・・（８）
領域Ｈ３のマス目に係る論理積＝
１１０００００
×００００１１０
＝０００００００・・・（９）
【００７５】
前述のようにして、i＝３で昇降軸３１３、i＝４で第１軸３２１、i＝５で第２軸３２２と、それぞれ判定対象となる部位毎に処理が進み、図１５が示すｉ＝６の場合には、アーム３２の昇降軸３２３に対してステップＳ２０８からステップＳ２１０までの処理が行われる。すなわち、最初に、占有情報記録部４２５が判定領域６において昇降軸３２３の占有領域（図１５にてドットを付した領域Ｈ４、ハッチを付した領域Ｈ５、領域Ｈ６、および領域Ｈ７に対応するマス目）を特定する（ステップＳ２０８）。領域Ｈ４は昇降軸３２３のみが存在する領域であり、領域Ｈ５は昇降軸３２３とアーム３２の第２軸３２２が重なる領域であり、領域Ｈ６は昇降軸３２３とアーム３１の第２軸３１２が重なる領域であり、領域Ｈ７は昇降軸３２３とアーム３１の第１軸３１１および第２軸３１２が重なる領域である。
【００７６】
次に、占有情報記録部４２５がステップＳ２０８にて特定した対応マス目に対応するメモリ領域に占有情報を書き込む。メモリが初期化された状態のままの領域Ｈ４のマス目に対応するメモリ領域には、昇降軸３２３の識別番号ＩＤ６＝「０００００１０」が書き込まれる。一方、ｉ＝５での書き込みによりメモリが初期化された状態になくなっている領域Ｈ５の対応マス目に対応するメモリ領域には、既存の占有情報としてのアーム３２の第２軸３２２の識別番号ＩＤ５＝「００００１００」と昇降軸３２３の識別番号ＩＤ６＝「０００００１０」の論理和が新たな占有情報として書き込まれる。つまり、占有情報記録部４２５は、下記の式（１０）によって示される占有情報を領域Ｈ５のマス目に対応するメモリ領域に記録する。
領域Ｈ５のマス目に対応するメモリ領域に書き込まれる占有情報＝
００００１００
＋０００００１０
＝００００１１０・・・（１０）
【００７７】
また、ｉ＝２での書き込みによりメモリが初期化された状態になくなっている領域Ｈ６のマス目に対応するメモリ領域には、既存の占有情報としてのアーム１の第２軸３２１の識別番号ＩＤ２＝「０１０００００」と昇降軸３２３の識別番号ＩＤ６＝「０００００１０」の論理和が新たな占有情報として書き込まれる。つまり、占有情報記録部４２５は、下記の式（１１）によって示される占有情報を領域Ｈ６のマス目に対応するメモリ領域に記録する。
０１０００００
＋０００００１０
＝０１０００１０・・・（１１）
【００７８】
更に、ｉ＝１およびｉ＝２での書き込みによりメモリが初期化された状態になくなっている領域Ｈ７のマス目に対応するメモリ領域には、既存の占有情報としてのアーム１の第１軸３１１の識別番号ＩＤ１＝「１００００００」と第２軸３１２の識別番号ＩＤ２＝「０１０００００」と昇降軸３２３の識別番号ＩＤ６＝「０００００１０」の論理和が新たな占有情報として書き込まれる。つまり、占有情報記録部４２５は、下記の式（１２）によって示される占有情報を領域Ｈ６のマス目に対応するメモリ領域に記録する。
１００００００
＋０１０００００
＋０００００１０
＝１１０００１０・・・（１２）
【００７９】
次に、判断部４２６が、領域Ｈ４ないし領域Ｈ７のそれぞれに対応するマス目について、次の式（１３）ないし式（１６）によって示される占有情報と干渉条件Ｃ６との論理積をマス目ごとに求める。
領域Ｈ４のマス目に係る論理積＝
０００００１０
×１１１１００１
＝０００００００・・・（１３）
領域Ｈ５のマス目に係る論理積＝
００００１１０
×１１１１００１
＝０００００００・・・（１４）
領域Ｈ６のマス目に係る論理積＝
０１０００１０
×１１１１００１
＝０１０００００・・・（１５）
領域Ｈ７のマス目に係る論理積＝
１１０００１０
×１１１１００１
＝１１０００００・・・（１６）
【００８０】
以上、ループ２の処理の具体例について説明したが、このようにしてループ２が終了する（全ての判定対象部位について占有情報と干渉条件との論理積とが求まる）と、判断部４２６は、それぞれの論理積が０か否かを判断する(ステップＳ２１１)。この場合、判断部４２６は、上述の具体例におけるｉ＝１およびｉ＝２の時のように論理積の結果が全ての桁で０であれば、干渉は生じないと判断する。実際、具体例のｉ＝１の場合、第１軸３１１が自分自身と干渉することはあり得ない。また、ｉ＝２の場合は、第１軸３１１と第２軸３１２とは関節でつながって立体交差しており、干渉することはあり得ない。一方、上述の具体例におけるｉ＝６の時のように論理積の結果が０でない（少なくとも一つの桁が１である）場合は、干渉が生じると判断する。なお、干渉が生じる場所は、上記の式（１５）および式（１６）において値が「１」である桁と同一桁で値を「１」とする識別番号の部位である。つまり、上記の式（１５）において値が「１」である桁は左から二番目の桁であり、この桁で値を「１」とする識別番号の部位はアーム３１の第２軸３１２（識別番号「０１０００００」）である。したがって、昇降軸３２３はアーム３１の第２軸３１２と干渉することがわかる。また、式（１６）において値が「１」である桁は左から一番目と二番目の各桁であり、これらの桁で値を「１」とする識別番号の部位はアーム３１の第１軸３１１（識別番号「１００００００」）と第２軸３１２（識別番号「０１０００００」）である。したがって、昇降軸３２３はアーム３１の第１軸３１１と第２軸３１２とが重なる位置、つまり右ひじ部においても干渉を生じることがわかる。
【００８１】
なお、図１３〜図１５では説明の便宜上マス目（メモリ領域）を大きく設定して説明した。しかし、実際のマス目は図１３〜図１５の例よりもっと小さい設定となる。
【００８２】
［占有領域の特定方法］
前述したように、占有情報記録部４２５は、マス目管理部４２４が作成したｎ１×ｎ２のマス目（判定領域、図９参照）のうち、ある時点における双腕ロボット３および障害物５の各部位が存在する領域（占有領域）に対応する対応マス目を特定する。以下、占有情報記録部４２５による占有領域の特定方法について、図１６〜図１９を参照し、具体例を挙げながら詳細に説明する。
【００８３】
図１６は、占有情報記録部４２５による占有領域の特定方法を説明するための図である。この図１６に示す例は、前後に関節ＡＡおよび関節ＢＢを有するアームＣＣがＸＹ平面上において存在する領域（アームＣＣの占有領域、図１６ではグレーで表示された領域Ｋ）を特定する例である。また、図１６に示す例は、例えば前述したアーム３１の第１軸３１１に対して、その対応マス目を特定するための例として理解されることができ、この場合の第１軸３１１は図１６に示すアームＣＣに該当する。なお、この場合の第１軸３１１は、例えばオーバル形状である。また、実際のＸＹ平面は、図９に示した判定領域に係るｎ１×ｎ２のマス目が全て存在可能な平面領域が確保されるが、図１６では説明の便宜上、アームＣＣの部分だけを拡大して示している。その特定方法としては、図１６のＸＹ平面上に存在する点（マス目を構成する４つの頂点）に対して、下記の手順１〜手順３を行い、各点がアームＣＣの占有領域に含まれるか否かを判定することで、ＸＹ平面上におけるアームＣＣの占有領域を特定することができる。
【００８４】
この例において、関節ＡＡの中心座標、関節ＢＢの中心座標、およびアームＣＣの幅は、既知の情報である。これらの既知の情報は、軌道生成部１の記憶部１１に格納されており、干渉判定演算部４２に送信される。図１６において、関節ＡＡの中心座標は点Ａ（ｘ１、ｙ１）であり、関節ＢＢの中心座標は点Ｂ（ｘ２、ｙ２）であり、アームＣＣの幅はｗである。また、関節ＡＡおよび関節ＢＢの半径Ｒ＝ｗ／２である。つまり、
点Ａ（ｘ１、ｙ１）：関節ＡＡの中心座標
点Ｂ（ｘ２、ｙ２）：関節ＢＢの中心座標
ｗ：アームの幅（Ｒ＝ｗ／２）
である。また、この例において、点ＤはアームＣＣの占有領域に入っているか否かの判定対象となる点である。つまり、
点Ｄ（ｘ０、ｙ０）：任意のマス目の任意の頂点
である。また、各補助線および補助点の定義は以下のとおりである。
ｌ１：点Ａ、点Ｂを通る直線
ｌ１１：関節ＡＡおよび関節ＢＢを近似した円（点Ａ、Ｂを中心とした半径Ｒの円）の接線（下側）
ｌ１２：関節ＡＡおよび関節ＢＢを近似した円（点Ａ、Ｂを中心とした半径Ｒの円）の接線（上側）
ｌ２：点Ｄを通り、ｌ１と垂直に交わる直線（つまりｌ１とｌ２は垂直関係）
点Ｃ（ｘ３、ｙ３）：ｌ１とｌ２の交点
点Ａｉ、Ｂｉ：点Ａ、Ｂを中心とした半径Ｒの円の接線ｌ１ｉとの接点
点Ｅ’：点ＡからＸ軸に垂線を引いたときにＹ座標がＡ１と同じになる点
点Ｂ’：点ＤからＸ軸に垂線を引いたときにｌ１と交わる点
なお、関節ＡＡおよび関節ＢＢの半径は同じとする。ゆえに、ｌ１、ｌ１１、およびｌ１２は互いに並行である。また、関節ＡＡおよび関節ＢＢの近似円及び線分ｌ１１及びｌ１２に囲まれた範囲（領域Ｋ）がアームＣＣに相当する。
【００８５】
（手順１）
最初に、図１７に示すルールに従い、判定対象部位（アームＣＣ）が位置する座標系を特定し、占有される可能性のあるマス目領域を限定する。点Ｄの座標（ｘ０、ｙ０）が、
（ｘｍｉｎ< ｘ０< ｘｍａｘ）…（１７）
および
（ｙｍｉｎ <ｙ０ < ｙｍａｘ）…（１８）
を満たすならば、点Ｄは判定対象部位の占有領域に含まれる可能性がある。この範囲外のマス目は、下記の手順３の項で後述する円形部分に含まれるか、もしくは判定対象部位の占有領域に含まれないことになる。
【００８６】
ここで、ｘ１２ｗおよびｙ１２ｗは、三角形ＡＢＥと三角形ＡＡ１Ｅ’との相似関係より、
Ｌａｂ：ａ１２：ｂ１２＝ Ｒ：ｙ１２ｗ：ｘ１２ｗ
（但し、ａ１２＝ｙ１−ｙ２、ｂ１２＝ｘ１−ｘ２）
が成り立つから、方向性を加味して、
ｘ１２ｗ＝ −Ｒ ｂ１２／Ｌａｂ
ｙ１２ｗ＝ −Ｒ ａ１２／Ｌａｂ
として与えられる。Ｌａｂは、三平方の定理から、
【数３】
となる。
【００８７】
（手順２）
次に、上記の式（１７）および（１８）、つまり（ｘｍｉｎ <ｘ０ < ｘｍａｘ）および（ｙｍｉｎ< ｙ０< ｙｍａｘ）を満たす各マス目の点Ｄが、図１８にてグレーで表示された領域Ｋ１に入るか否かを判定する。これには、点Ｄから線分ｌ１までの垂線の長さＬｃｄがＲ以下となるかどうか、すなわち、
Ｌｃｄ≦ Ｒ…（１９）
となるかどうかを判定すればよい。これが成立すれば、点Ｄを有するマス目は占有領域に含まれることになる。
【００８８】
ここで、Ｌｃｄは、以下の通り、軌道生成部１の記憶部１１に格納されていたパラメータ等から与えられる既知の条件値から求めることができる。まず、線分ｌ１の方程式は、傾きをａ１、切片をｂ１とすると、
ａ１ ｘ＋ ｂ１＝ ｙ
である。これにより、図１８における点Ｂ’の座標は、Ｘ座標が点Ｄと同じｘ０であるから、（ｘ０、ａ１ｘ０ ＋ ｂ１） である。したがって、２点ＤＢ’間の距離Ｌｄｂ’は、
Ｌｄｂ’ ＝ｙ０ − （ａ１ｘ０ ＋ ｂ１）
である。ここで、三角形ＡＢＥと三角形ＤＢ’Ｃとが相似関係となることから、
Ｌｄｂ’ ：Ｌｃｄ ＝ Ｌａｂ ： （ｘ２− ｘ１）
が成り立つ。これを、２点ＣＤ間の距離Ｌｃｄについて解けば、
Ｌｃｄ＝ （ｘ２ −ｘ１） Ｌｄｂ’ ／Ｌａｂ
= （ｘ２ −ｘ１） ｛ｙ０ − （ａ１ｘ０ ＋ ｂ１）｝ ／ Ｌａｂ
= −（ｘ１− ｘ２） ［ｙ０ − ｛（ｙ１ - ｙ２）／（ｘ１ - ｘ２）｝ ｘ０− （ｙ２ｘ１- ｘ２ｙ１）／（ｘ１ - ｘ２） ］／ Ｌａｂ
= ｛（ｙ１ - ｙ２）ｘ０−（ｘ１− ｘ２）ｙ０＋ （ｙ２ｘ１- ｘ２ｙ１）｝ ／Ｌａｂ
但し、上記の通り、
【数４】
である。
【００８９】
（手順３）
最後に、全てのマス目を対象に、点Ｄが図１９にてグレーで表示された円形領域Ｋ２に入るか否かを判定する。座標（ｘｉ、ｙｉ）を中心とする半径Ｒの円の方程式は、
（ｘ −ｘｉ）２＋ （ｙ −ｙｉ）２ ＝Ｒ２
であるから、円の境界を含む内側は、
（ｘ −ｘｉ）２＋ （ｙ −ｙｉ）２ ≦Ｒ２
と表現できる。ここで、点Ｄ（ｘ０、ｙ０）に対して関節ＡＡおよび関節ＢＢの２つの円を考えれば、
（ｘ０−ｘ１）２＋ （ｙ０ −ｙ１）２ ≦Ｒ２…（２０）
（ｘ０−ｘ２）２＋ （ｙ０ −ｙ２）２ ≦Ｒ２…（２１）
のいずれかが成立すれば、点Ｄ（ｘ０、ｙ０）を有するマス目は占有領域に含まれることになる。
【００９０】
（まとめ）
以上の説明のように、上記の手順１で特定した領域に存在する各マス目の４つの頂点座標が手順２の条件（式（１９））に該当するか否かを判定し、さらに、全てのマス目の４つの頂点座標が手順３（式（２０）または式（２１））の条件に該当するか否かを判定する。そして、何れかの頂点が何れかの条件に該当する場合には、そのマス目はアームＣＣ、すなわち判定対象部位ｉの占有領域に含まれると判定する。
【００９１】
［本実施形態の作用及び効果］
続いて、本実施形態にかかる双腕ロボットシステム１００の作用及び効果について説明する。本実施形態の双腕ロボットシステム１００では、識別番号、干渉情報、干渉条件情報、および占有情報が各部位間の干渉を判定するための必須の情報として用いられる。このうち、特に干渉情報および干渉条件情報は、移動部位が他の部位との間で立体交差する際に干渉を伴わない場合の値および干渉を伴う場合の値に基づいた情報である。したがって、これら干渉情報および干渉条件情報を用いることにより、本実施形態による干渉判定の結果には各部位間の立体交差時の干渉の有無が考慮されることとなる。これにより、干渉判定の精度が向上される。
【００９２】
また、本実施形態においては、双腕ロボット３の動作する空間を２次元平面上に投影した上で、干渉判定を行うため、例えば３次元での干渉判定に比べ、干渉判定処理を短時間で行うことが可能となる。また、本実施形態で用いる干渉情報は「０」または「１」を表す情報であり、そのデータ量が少ないことから、装置全体における計算量や計算時間が短縮できる。
【００９３】
また、本実施形態で用いる識別番号、干渉条件情報、および占有情報は双腕ロボット３および障害物５の各部位の数と同数の桁数を有する情報であるため、例えば部位の数が増えた場合には、増えた部位に関する干渉条件を設定した上で、増えた部位の数だけ識別番号、干渉条件情報、および占有情報の桁数を増やして、本実施形態における干渉判定を行うことができる。このように、本実施形態によれば、部位の数が増えた場合に、柔軟な対応が可能である。また、「０」か「１」かの少ないデータ量で計算を行うことから、本実施形態によれば、部位の数が増えたことに伴って、装置全体の計算量や計算時間が極端に増えることを防止できる。
【００９４】
また、本実施形態において、識別番号、干渉条件情報、および占有情報が、双腕ロボット３および障害物５の各部位の数と同数の桁数を有することは、言い換えれば各情報が必要最小限の桁数を有することであり、この場合には、演算に無駄がなく、必要最小限のメモリ量を用いて演算を行うことができる。
【００９５】
また、本実施形態によれば、双腕ロボット３の動作における動作開始時刻と動作終了時刻との間の各時刻に対して干渉判定が行われる。これにより、動作中の全ての時刻において干渉が生じないことを判断した上で、安全に双腕ロボット３を動作させることができる。
【００９６】
以上で説明した軌道生成部１および干渉判定部４は実際に双腕ロボット３を動作させる前に事前に干渉判定を行うシミュレーション装置としても利用することができる。つまり、図１に示した双腕ロボット３やサーボドライバ２ａ〜２ｄが実際には存在しない場合でも、これらの機械部分をパソコン上で擬似的に構成し、双腕ロボット３および障害物の各部位間の干渉判定をシミュレーションすることができる。
【００９７】
［他の実施形態］
以上、本発明の好適な実施形態について説明したが、本発明が上記実施形態に限定されないことは言うまでもない。例えば、図示はしないが、干渉判定演算部４２をマス目管理部４２４、占有情報記録部４２５、および判断部４２６のみを必須の構成要素とする簡素な構成としてもよい。
【００９８】
この場合のマス目管理部４２４は、前述の通り、双腕ロボット３の動作する空間を２次元平面上に投影した上で、その２次元平面をｎ１×ｎ２のマス目に区切った判定領域を作成するとともに、そのｎ１×ｎ２の個々のマス目に対応するメモリ領域を確保する。また、占有情報記録部４２５は、マス目管理部４２４による上記ｎ１×ｎ２のマス目のうち、双腕ロボット３および障害物５の各部位に対応するマス目である対応マス目を特定し、占有情報を部位毎に順次対応マス目に対応するメモリ領域に記録する。ここで、この場合の占有情報は、双腕ロボット３および障害物５の各部位が対応マス目を占有するか否かを示す情報である。また、判断部４２６は、占有情報をメモリ領域に記録するときに、そのメモリ領域に既に別の部位の占有情報が記録されている場合には、それらの部位同士が干渉すると判断する。
【００９９】
以上のような構成により、以下に示すような簡易的な干渉判定処理が可能となる。説明の便宜上、干渉判定部位が２個のみのケースを考える。例えば、右腕（識別番号ＩＤ＝０１）と左腕（識別番号ＩＤ＝１０）のみの干渉判定を行う場合が例として挙げられる。この場合に、最初に、マス目管理部４２４が、双腕ロボット３の動作空間を２次元平面上に投影した上で、その２次元平面をｎ１×ｎ２のマス目に区切った判定領域を作成するとともに、そのｎ１×ｎ２の個々のマス目に対応するメモリ領域を確保する。次に、占有情報記録部４２５が、ＩＤ＝０１の部位が占有するマス目を特定し、そのメモリ領域に占有情報として「ＩＤ＝０１を示す旨の情報」を書き込む。次に、占有情報記録部４２５が、ＩＤ＝１０の部位が占有するマス目を特定し、そのメモリ領域に占有情報として「ＩＤ＝１０を示す旨の情報」を書き込む。この「ＩＤ＝１０を示す旨の情報」の書き込み処理時に、占有情報記録部４２５が既に「ＩＤ＝０１を示す旨の情報」が書き込まれたメモリ領域に「ＩＤ＝１０を示す旨の情報」を書き込まなければならなくなった場合には、判断部４２６は、ＩＤ＝０１の部位とＩＤ＝１０の部位との間で部位同士が「干渉する」と判定する。一方で、「ＩＤ＝１０を示す旨の情報」を書き込むメモリ領域の何れにも「ＩＤ＝０１を示す旨の情報」が書き込まれていない場合には、部位同士が「干渉しない」と判定する。
【０１００】
以上のような簡易的な干渉判定処理は、干渉判定対象部位が少なく、２次元平面上での動作（例えばアームの旋回動作）だけ考慮すれば良いロボットおよび障害物に対して特に有効である。すなわち、マス目を使って２次元平面上での干渉判定を行うため、３次元的な空間やモデルを用いて干渉判定を行う場合に比べて極めて少ないデータ量で計算ができ、干渉判定を高速化でき、双腕ロボット３の２次元方向の運動のみを考慮する（ロボットアームの昇降軸の動作や冶具の上昇下降がない）場合に特に有効である。
【符号の説明】
【０１０１】
１００…双腕ロボットシステム、１…軌道生成部、１１…記憶部、１２…演算部、２ａ〜２ｄ…サーボドライバ、３ａ〜３ｄ…サーボモータ、３…双腕ロボット、３１，３２…水平多関節アーム、３１１…アーム３１の第１軸、３１２…アーム３１の第２軸、３１３…アーム３１の昇降軸、３２１…アーム３２の第１軸、３２２…アーム３２の第２軸、３２３…アーム３２の昇降軸、４…干渉判定部、４１…格納部、４１１…諸元等格納部、４１２…干渉情報格納部、４１３…干渉条件情報格納部、４２…干渉判定演算部、４２１…識別番号割当部、４２２…干渉情報生成部、４２３…干渉条件情報生成部、４２４…マス目管理部、４２５…占有情報記録部、４２６…判断部、５…障害物、ワーク、冶具、６…判定領域。
【特許請求の範囲】
【請求項１】
３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定装置であって、
複数の桁数を有し且つ他の識別番号との間で桁ごとの論理積を計算すると全ての桁が「０」となるように設定された識別番号を、前記各部位に割り当てる識別番号割当手段と、
前記各部位のうち移動を伴う部位である移動部位が、他の前記部位との間で立体交差する際に干渉を伴わない場合には「０」、立体交差する際に干渉を伴う場合には「１」を表す情報を干渉情報として格納する干渉情報格納手段と、
前記干渉情報に基づき設定され、前記複数の桁数を有し、前記移動部位が前記他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である干渉条件情報を、前記移動部位ごとに格納する干渉条件情報格納手段と、
前記ロボットの動作する空間を２次元平面上に投影した上で、前記２次元平面をｎ１×ｎ２のマス目に区切るとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域を確保するマス目管理手段と、
前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目を特定し、前記各部位の前記識別番号に基づく情報であり前記複数の桁数を有する占有情報を前記対応マス目に対応する前記メモリ領域に記録する占有情報記録手段と、
前記占有情報および前記干渉条件情報における桁ごとの論理積を前記対応マス目ごとに求め、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断し、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断する判断手段と、
を備えることを特徴とする干渉判定装置。
【請求項２】
前記干渉条件情報格納手段は、下記の数式（１）により計算される情報Ｃｉを前記干渉条件情報として格納することを特徴とする請求項１に記載の干渉判定装置。
【数１】
ただし、ｈは前記各部位の数であり、ｉおよびｊは１以上ｈ以下の自然数であり、Ｃｉはｉ番目の部位の前記干渉条件情報であり、ＩＤｊはｊ番目の部位の前記識別番号であり、ｔｉｊはｉ番目の部位とｊ番目の部位における前記干渉情報である。
【請求項３】
前記占有情報記録手段は、
前記メモリ領域が初期化されている場合には、前記識別番号を前記占有情報として前記メモリ領域に記録し、
前記メモリ領域が初期化されていない場合には、既存の占有情報と前記識別番号との間の桁ごとの論理和を前記占有情報として前記メモリ領域に記録する、
ことを特徴とする請求項１または２に記載の干渉判定装置。
【請求項４】
前記ロボットの動作における動作開始時刻と動作終了時刻との間が一定時間間隔ごとに区切られ、当該区切られた各時刻における前記各部位の位置に対して、
前記占有情報記録手段は、前記各時刻における前記対応マス目を特定し、前記占有情報を前記対応マス目に対応する前記メモリ領域に記録し、
前記判断手段は、前記占有情報および前記干渉条件情報における桁ごとの論理積を前記対応マス目ごとに求め、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断し、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断する、
ことを特徴とする請求項１〜３の何れか１項に記載の干渉判定装置。
【請求項５】
前記ロボットは、水平多関節アームを有する双腕ロボットである、
ことを特徴とする請求項１〜４の何れか１項に記載の干渉判定装置。
【請求項６】
前記識別番号、前記干渉条件情報、および前記占有情報は、前記各部位の数と同数の前記複数の桁数を有する情報である、
ことを特徴とする請求項１〜５の何れか１項に記載の干渉判定装置。
【請求項７】
３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定装置であって、
前記ロボットの動作する空間を２次元平面上に投影した上で、前記２次元平面をｎ１×ｎ２のマス目に区切るとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域を確保するマス目管理手段と、
前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目を部位毎に特定し、前記各部位が前記対応マス目を占有するか否かを示す占有情報を前記部位毎に順次前記対応マス目に対応する前記メモリ領域に記録する占有情報記録手段と、
前記占有情報を前記メモリ領域に記録するときに、そのメモリ領域に既に別の部位の前記占有情報が記録されている場合には、それらの部位同士が干渉すると判断する判断手段と、
を備えることを特徴とする干渉判定装置。
【請求項８】
３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定方法であって、
複数の桁数を有し且つ他の識別番号との間で桁ごとの論理積を計算すると全ての桁が「０」となるように設定された識別番号が、前記各部位に割り当てられ、
前記各部位のうち移動を伴う部位である移動部位が、他の前記部位との間で立体交差する際に干渉を伴わない場合には「０」、立体交差する際に干渉を伴う場合には「１」を表す情報が、干渉情報として格納され、
前記干渉情報に基づき設定され、前記複数の桁数を有し、前記移動部位が前記他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である干渉条件情報が、前記移動部位ごとに格納され、
前記ロボットの動作する空間が２次元平面上に投影された上で、前記２次元平面がｎ１×ｎ２のマス目に区切られるとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域が確保され、
前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目が特定され、前記各部位の前記識別番号に基づく情報であり前記複数の桁数を有する占有情報が前記対応マス目に対応する前記メモリ領域に記録され、
前記占有情報および前記干渉条件情報における桁ごとの論理積が前記対応マス目ごとに求められ、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断され、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断される、
ことを特徴とする干渉判定方法。
【請求項１】
３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定装置であって、
複数の桁数を有し且つ他の識別番号との間で桁ごとの論理積を計算すると全ての桁が「０」となるように設定された識別番号を、前記各部位に割り当てる識別番号割当手段と、
前記各部位のうち移動を伴う部位である移動部位が、他の前記部位との間で立体交差する際に干渉を伴わない場合には「０」、立体交差する際に干渉を伴う場合には「１」を表す情報を干渉情報として格納する干渉情報格納手段と、
前記干渉情報に基づき設定され、前記複数の桁数を有し、前記移動部位が前記他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である干渉条件情報を、前記移動部位ごとに格納する干渉条件情報格納手段と、
前記ロボットの動作する空間を２次元平面上に投影した上で、前記２次元平面をｎ１×ｎ２のマス目に区切るとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域を確保するマス目管理手段と、
前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目を特定し、前記各部位の前記識別番号に基づく情報であり前記複数の桁数を有する占有情報を前記対応マス目に対応する前記メモリ領域に記録する占有情報記録手段と、
前記占有情報および前記干渉条件情報における桁ごとの論理積を前記対応マス目ごとに求め、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断し、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断する判断手段と、
を備えることを特徴とする干渉判定装置。
【請求項２】
前記干渉条件情報格納手段は、下記の数式（１）により計算される情報Ｃｉを前記干渉条件情報として格納することを特徴とする請求項１に記載の干渉判定装置。
【数１】
ただし、ｈは前記各部位の数であり、ｉおよびｊは１以上ｈ以下の自然数であり、Ｃｉはｉ番目の部位の前記干渉条件情報であり、ＩＤｊはｊ番目の部位の前記識別番号であり、ｔｉｊはｉ番目の部位とｊ番目の部位における前記干渉情報である。
【請求項３】
前記占有情報記録手段は、
前記メモリ領域が初期化されている場合には、前記識別番号を前記占有情報として前記メモリ領域に記録し、
前記メモリ領域が初期化されていない場合には、既存の占有情報と前記識別番号との間の桁ごとの論理和を前記占有情報として前記メモリ領域に記録する、
ことを特徴とする請求項１または２に記載の干渉判定装置。
【請求項４】
前記ロボットの動作における動作開始時刻と動作終了時刻との間が一定時間間隔ごとに区切られ、当該区切られた各時刻における前記各部位の位置に対して、
前記占有情報記録手段は、前記各時刻における前記対応マス目を特定し、前記占有情報を前記対応マス目に対応する前記メモリ領域に記録し、
前記判断手段は、前記占有情報および前記干渉条件情報における桁ごとの論理積を前記対応マス目ごとに求め、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断し、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断する、
ことを特徴とする請求項１〜３の何れか１項に記載の干渉判定装置。
【請求項５】
前記ロボットは、水平多関節アームを有する双腕ロボットである、
ことを特徴とする請求項１〜４の何れか１項に記載の干渉判定装置。
【請求項６】
前記識別番号、前記干渉条件情報、および前記占有情報は、前記各部位の数と同数の前記複数の桁数を有する情報である、
ことを特徴とする請求項１〜５の何れか１項に記載の干渉判定装置。
【請求項７】
３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定装置であって、
前記ロボットの動作する空間を２次元平面上に投影した上で、前記２次元平面をｎ１×ｎ２のマス目に区切るとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域を確保するマス目管理手段と、
前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目を部位毎に特定し、前記各部位が前記対応マス目を占有するか否かを示す占有情報を前記部位毎に順次前記対応マス目に対応する前記メモリ領域に記録する占有情報記録手段と、
前記占有情報を前記メモリ領域に記録するときに、そのメモリ領域に既に別の部位の前記占有情報が記録されている場合には、それらの部位同士が干渉すると判断する判断手段と、
を備えることを特徴とする干渉判定装置。
【請求項８】
３次元で動作するロボットおよび障害物の各部位に対して干渉判定を行う干渉判定方法であって、
複数の桁数を有し且つ他の識別番号との間で桁ごとの論理積を計算すると全ての桁が「０」となるように設定された識別番号が、前記各部位に割り当てられ、
前記各部位のうち移動を伴う部位である移動部位が、他の前記部位との間で立体交差する際に干渉を伴わない場合には「０」、立体交差する際に干渉を伴う場合には「１」を表す情報が、干渉情報として格納され、
前記干渉情報に基づき設定され、前記複数の桁数を有し、前記移動部位が前記他の部位との間で立体交差する際に干渉を伴うか否かを表す情報である干渉条件情報が、前記移動部位ごとに格納され、
前記ロボットの動作する空間が２次元平面上に投影された上で、前記２次元平面がｎ１×ｎ２のマス目に区切られるとともに、前記ｎ１×ｎ２のマス目それぞれに対応するメモリ領域が確保され、
前記ｎ１×ｎ２のマス目のうち前記各部位に対応するマス目である対応マス目が特定され、前記各部位の前記識別番号に基づく情報であり前記複数の桁数を有する占有情報が前記対応マス目に対応する前記メモリ領域に記録され、
前記占有情報および前記干渉条件情報における桁ごとの論理積が前記対応マス目ごとに求められ、前記論理積の結果が全ての桁において「０」であれば前記各部位同士は干渉しないと判断され、前記論理積の結果が少なくとも一つの桁において「１」であれば前記各部位同士は干渉すると判断される、
ことを特徴とする干渉判定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【公開番号】特開２０１２−６１００（Ｐ２０１２−６１００Ａ）
【公開日】平成２４年１月１２日（２０１２．１．１２）
【国際特許分類】
【出願番号】特願２０１０−１４２９９１（Ｐ２０１０−１４２９９１）
【出願日】平成２２年６月２３日（２０１０．６．２３）
【出願人】（０００２２７４６７）日東精工株式会社 (263)
【Ｆターム（参考）】
【公開日】平成２４年１月１２日（２０１２．１．１２）
【国際特許分類】
【出願日】平成２２年６月２３日（２０１０．６．２３）
【出願人】（０００２２７４６７）日東精工株式会社 (263)
【Ｆターム（参考）】
[ Back to top ]