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