特許 7119556 多関節ロボットの干渉判定装置、多関節ロボットの干渉判定方法、多関節ロボットの干渉判定プログラム、及び経路計画生成装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2022-08-08

(45)【発行日】2022-08-17

(54)【発明の名称】多関節ロボットの干渉判定装置、多関節ロボットの干渉判定方法、多関節ロボットの干渉判定プログラム、及び経路計画生成装置

(51)【国際特許分類】

   B25J 19/06 20060101AFI20220809BHJP

【ＦＩ】

B25J19/06

【請求項の数】 12

(21)【出願番号】P 2018093928

(22)【出願日】2018-05-15

(65)【公開番号】P2019198914

(43)【公開日】2019-11-21

【審査請求日】2020-12-14

(73)【特許権者】

【識別番号】000002945

【氏名又は名称】オムロン株式会社

(74)【代理人】

【識別番号】110001519

【氏名又は名称】特許業務法人太陽国際特許事務所

(72)【発明者】

【氏名】藤井 春香

(72)【発明者】

【氏名】森谷 俊洋

(72)【発明者】

【氏名】小島 岳史

(72)【発明者】

【氏名】殿谷 徳和

【審査官】杉田 隼一

(56)【参考文献】

【文献】特開平０９－１９０２０７（ＪＰ，Ａ）

【文献】特開平０４－２８０３０４（ＪＰ，Ａ）

【文献】特開平０３－０９８７８６（ＪＰ，Ａ）

【文献】特開昭６２－１２０９９５（ＪＰ，Ａ）

【文献】特開平０３－１３１４９４（ＪＰ，Ａ）

【文献】特開平０９－２５１３０８（ＪＰ，Ａ）

【文献】長谷川 勉，自由空間分類表現法によるマニピュレータの衝突回避動作の計画，計測自動制御学会論文集，Vol22,N06，20-26

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｂ２５Ｊ １９／０６

(57)【特許請求の範囲】

【請求項1】

多関節ロボットの２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される領域であり、前記多関節ロボットが自己又は障害物に対して干渉する領域として、前記特定の関節の回転角度の大きさに基づいて定められた干渉領域と、前記多関節ロボットが自己又は障害物に対して干渉しない領域として、前記特定の関節の回転角度の大きさに基づいて定められた非干渉領域と、を表す領域情報を取得する取得部と、
前記特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する判定部と、
を含み、
前記領域情報は、前記干渉領域及び前記非干渉領域の何れにも属さない領域として詳細判定領域を表す情報を含む
多関節ロボットの干渉判定装置。

【請求項2】

前記干渉が、前記多関節ロボットが自己に干渉する自己干渉であり、かつ、前記座標が前記詳細判定領域に属する場合、前記判定部は、前記多関節ロボットを構成する複数のリンクの形状に基づいて前記自己干渉が発生するか否かを判定する
請求項１記載の多関節ロボットの干渉判定装置。

【請求項3】

前記多関節ロボットを構成する複数のリンクの形状及び前記複数のリンクを接続する複数の関節の可動域に基づいて前記領域情報を算出する算出部
を備えた請求項１又は請求項２記載の多関節ロボットの干渉判定装置。

【請求項4】

前記算出部は、前記干渉領域の輪郭に収まるように前記干渉領域の形状を簡易化する
請求項３記載の多関節ロボットの干渉判定装置。

【請求項5】

多関節ロボットの２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される領域であり、前記多関節ロボットが自己又は障害物に対して干渉する領域として、前記特定の関節の回転角度の大きさに基づいて定められた干渉領域と、前記多関節ロボットが自己又は障害物に対して干渉しない領域として、前記特定の関節の回転角度の大きさに基づいて定められた非干渉領域と、を表す領域情報を取得する取得部と、
前記特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する判定部と、
前記多関節ロボットを構成する複数のリンクの形状及び前記複数のリンクを接続する複数の関節の可動域に基づいて前記領域情報を算出する算出部と、
を含み、
前記算出部は、前記多関節ロボットの根元側の２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される前記干渉領域及び前記非干渉領域を表す第１の領域情報と、前記多関節ロボットの手先側の２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される前記干渉領域及び前記非干渉領域を表す第２の領域情報と、を算出し、
前記取得部は、前記算出部により算出された前記第１の領域情報及び前記第２の領域情報を取得し、
前記判定部は、前記根元側の２つ又は３つの特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記第１の領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定し、前記手先側の２つ又は３つの特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記第２の領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する
多関節ロボットの干渉判定装置。

【請求項6】

前記算出部は、前記第１の領域情報のうち前記非干渉領域を表す情報を算出する場合、前記手先側のリンクの形状及び前記手先側の関節の可動域に基づいて最大可動域を算出し、算出した最大可動域と前記根元側の複数のリンクの形状とに基づいて、前記非干渉領域を表す情報を算出する
請求項５記載の多関節ロボットの干渉判定装置。

【請求項7】

前記干渉が、前記多関節ロボットが障害物に対して干渉する障害物干渉であり、
前記算出部は、前記多関節ロボットが前記障害物に対して干渉する対障害物干渉領域と、前記多関節ロボットが前記障害物に対して干渉しない対障害物非干渉領域と、を表す領域情報を算出する
請求項３～請求項５の何れか１項に記載の多関節ロボットの干渉判定装置。

【請求項8】

コンピュータが、
多関節ロボットの２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される領域であり、前記多関節ロボットが自己又は障害物に対して干渉する領域として、前記特定の関節の回転角度の大きさに基づいて定められた干渉領域と、前記多関節ロボットが自己又は障害物に対して干渉しない領域として、前記特定の関節の回転角度の大きさに基づいて定められた非干渉領域と、を表す領域情報を取得する取得工程と、
前記特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得工程により取得された前記領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉が発生するか否かを判定する判定工程と、
を含む処理を実行し、
前記領域情報は、前記干渉領域及び前記非干渉領域の何れにも属さない領域として詳細判定領域を表す情報を含む
多関節ロボットの干渉判定方法。

【請求項9】

コンピュータが、
多関節ロボットの２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される領域であり、前記多関節ロボットが自己又は障害物に対して干渉する領域として、前記特定の関節の回転角度の大きさに基づいて定められた干渉領域と、前記多関節ロボットが自己又は障害物に対して干渉しない領域として、前記特定の関節の回転角度の大きさに基づいて定められた非干渉領域と、を表す領域情報を取得する取得工程と、
前記特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得工程により取得された前記領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する判定工程と、
前記多関節ロボットを構成する複数のリンクの形状及び前記複数のリンクを接続する複数の関節の可動域に基づいて前記領域情報を算出する算出工程と、
を含む処理を実行し、
前記算出工程は、前記多関節ロボットの根元側の２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される前記干渉領域及び前記非干渉領域を表す第１の領域情報と、前記多関節ロボットの手先側の２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される前記干渉領域及び前記非干渉領域を表す第２の領域情報と、を算出し、
前記取得工程は、前記算出工程により算出された前記第１の領域情報及び前記第２の領域情報を取得し、
前記判定工程は、前記根元側の２つ又は３つの特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得工程により取得された前記第１の領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定し、前記手先側の２つ又は３つの特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得工程により取得された前記第２の領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する
多関節ロボットの干渉判定方法。

【請求項10】

コンピュータを、
多関節ロボットの２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される領域であり、前記多関節ロボットが自己又は障害物に対して干渉する領域として、前記特定の関節の回転角度の大きさに基づいて定められた干渉領域と、前記多関節ロボットが自己又は障害物に対して干渉しない領域として、前記特定の関節の回転角度の大きさに基づいて定められた非干渉領域と、を表す領域情報を取得する取得部、及び、
前記特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する判定部
として機能させ、
前記領域情報は、前記干渉領域及び前記非干渉領域の何れにも属さない領域として詳細判定領域を表す情報を含む
多関節ロボットの干渉判定プログラム。

【請求項11】

コンピュータを、
多関節ロボットの２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される領域であり、前記多関節ロボットが自己又は障害物に対して干渉する領域として、前記特定の関節の回転角度の大きさに基づいて定められた干渉領域と、前記多関節ロボットが自己又は障害物に対して干渉しない領域として、前記特定の関節の回転角度の大きさに基づいて定められた非干渉領域と、を表す領域情報を取得する取得部、
前記特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する判定部、及び、
前記多関節ロボットを構成する複数のリンクの形状及び前記複数のリンクを接続する複数の関節の可動域に基づいて前記領域情報を算出する算出部
として機能させ、
前記算出部は、前記多関節ロボットの根元側の２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される前記干渉領域及び前記非干渉領域を表す第１の領域情報と、前記多関節ロボットの手先側の２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される前記干渉領域及び前記非干渉領域を表す第２の領域情報と、を算出し、
前記取得部は、前記算出部により算出された前記第１の領域情報及び前記第２の領域情報を取得し、
前記判定部は、前記根元側の２つ又は３つの特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記第１の領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定し、前記手先側の２つ又は３つの特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記第２の領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する
多関節ロボットの干渉判定プログラム。

【請求項12】

多関節ロボットを初期姿勢から目標姿勢まで動作させる場合に、前記多関節ロボットが自己又は障害物に対して干渉しない姿勢を探索することで経路計画を生成する経路探索装置と、
前記経路探索装置により探索された姿勢における前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する請求項１～７の何れか１項に記載の干渉判定装置と、
を含む経路計画生成装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、多関節ロボットの干渉判定装置、多関節ロボットの干渉判定方法、多関節ロボットの干渉判定プログラム、及び経路計画生成装置に関する。

【背景技術】

【0002】

従来、ロボットの自己干渉を考慮して経路計画を作成する技術が提案されている（例えば特許文献１、２参照）。

【0003】

ロボットの自己干渉を判定する場合、従来ではロボットを構成する複数のリンクの組み合わせの各々について網羅的に判定していた。例えばリンク数をｎ個とした場合、リンクの形状等にもよるが、自己干渉を判定すべきリンクの組み合わせは_ｎＣ_２－（ｎ－１）通りとなる。

【先行技術文献】

【特許文献】

【0004】

【文献】特開２０１１－１３１３０３号公報

【文献】特開２００８－３０１３６号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

例えば関節が６個の６自由度の多関節ロボットの場合、図２４に示すように、エンドエフェクタＥＥも含めると少なくともリンクの数は８個となり、自己干渉を判定すべきリンクの組み合わせは最低でも２１通りとなる。また、リンク数が増加した場合は、自己干渉を判定すべきリンクの組み合わせは大きく増加する。これにより、自己干渉の判定に要する時間が増加すると共に、ロボットの経路計画の作成に要する時間も増加する。また、エンドユーザーにおいては、ロボットが動作開始するまでの時間が増加すると共に、タクトタイムが増加する、という問題があった。

【0006】

本発明は、上記の点に鑑みてなされたものであり、多関節ロボットの干渉判定に要する時間を短縮することができる多関節ロボットの干渉判定装置、多関節ロボットの干渉判定方法、多関節ロボットの干渉判定プログラム、及び経路計画生成装置を提供することを目的とする。

【課題を解決するための手段】

【0007】

本発明に係る多関節ロボットの干渉判定装置は、多関節ロボットの２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される領域であり、前記多関節ロボットが自己又は障害物に対して干渉する領域として、前記特定の関節の回転角度の大きさに基づいて定められた干渉領域と、前記多関節ロボットが自己又は障害物に対して干渉しない領域として、前記特定の関節の回転角度の大きさに基づいて定められた非干渉領域と、を表す領域情報を取得する取得部と、前記特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する判定部と、を含む。

【0008】

また、前記領域情報は、前記干渉領域及び前記非干渉領域の何れにも属さない領域として詳細判定領域を表す情報を含むようにしてもよい。

【0009】

また、前記干渉が、前記多関節ロボットが自己に干渉する自己干渉であり、かつ、前記座標が前記詳細判定領域に属する場合、前記判定部は、前記多関節ロボットを構成する複数のリンクの形状に基づいて前記自己干渉が発生するか否かを判定するようにしてもよい。

【0010】

また、前記多関節ロボットを構成する複数のリンクの形状及び前記複数のリンクを接続する複数の関節の可動域に基づいて前記領域情報を算出する算出部を備えてもよい。

【0011】

また、前記算出部は、前記干渉領域の輪郭に収まるように前記干渉領域の形状を簡易化するようにしてもよい。

【0012】

また、前記算出部は、前記非干渉領域の輪郭に収まるように前記干渉領域の形状を簡易化するようにしてもよい。

【0013】

また、前記算出部は、前記多関節ロボットの根元側の２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される前記干渉領域及び前記非干渉領域を表す第１の領域情報と、前記多関節ロボットの手先側の２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される前記干渉領域及び前記非干渉領域を表す第２の領域情報と、を算出し、前記取得部は、前記算出部により算出された前記第１の領域情報及び前記第２の領域情報を取得し、前記判定部は、前記根元側の２つ又は３つの特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記第１の領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定し、前記手先側の２つ又は３つの特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記第２の領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定するようにしてもよい。

【0014】

また、前記算出部は、前記第１の領域情報のうち前記非干渉領域を表す情報を算出する場合、前記手先側のリンクの形状及び前記手先側の関節の可動域に基づいて最大可動域を算出し、算出した最大可動域と前記根元側の複数のリンクの形状とに基づいて、前記非干渉領域を表す情報を算出するようにしてもよい。

【0015】

また、前記干渉が、前記多関節ロボットが障害物に対して干渉する障害物干渉であり、前記算出部は、前記多関節ロボットが前記障害物に対して干渉する対障害物干渉領域と、前記多関節ロボットが前記障害物に対して干渉しない対障害物非干渉領域と、を表す領域情報を算出するようにしてもよい。

【0016】

本発明に係る多関節ロボットの干渉判定方法は、コンピュータが、多関節ロボットの２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される領域であり、前記多関節ロボットが自己又は障害物に対して干渉する領域として、前記特定の関節の回転角度の大きさに基づいて定められた干渉領域と、前記多関節ロボットが自己又は障害物に対して干渉しない領域として、前記特定の関節の回転角度の大きさに基づいて定められた非干渉領域と、を表す領域情報を取得する取得工程と、前記特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得工程により取得された前記領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉が発生するか否かを判定する判定工程と、を含む処理を実行する方法である。

【0017】

本発明に係る多関節ロボットの干渉判定プログラムは、コンピュータを、多関節ロボットの２つ又は３つの特定の関節の回転角度を座標軸とするコンフィグレーション空間内に設定される領域であり、前記多関節ロボットが自己又は障害物に対して干渉する領域として、前記特定の関節の回転角度の大きさに基づいて定められた干渉領域と、前記多関節ロボットが自己又は障害物に対して干渉しない領域として、前記特定の関節の回転角度の大きさに基づいて定められた非干渉領域と、を表す領域情報を取得する取得部、及び、前記特定の関節の回転角度によって定まる姿勢を表す座標が、前記取得部により取得された前記領域情報により表される前記干渉領域及び前記非干渉領域の何れに属するかを判定することにより、前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する判定部として機能させるためのプログラムである。

【0018】

本発明に係る経路計画生成装置は、多関節ロボットを初期姿勢から目標姿勢まで動作させる場合に、前記多関節ロボットが自己又は障害物に対して干渉しない姿勢を探索することで経路計画を生成する経路探索装置と、前記経路探索装置により探索された姿勢における前記多関節ロボットが自己又は障害物に対して干渉するか否かを判定する上記記載の干渉判定装置と、を含む。

【発明の効果】

【0019】

本発明によれば、多関節ロボットの干渉判定に要する時間を短縮することができる。

【図面の簡単な説明】

【0020】

【図1】第１実施形態に係る干渉判定装置のハードウェア構成を示すブロック図である。

【図2】垂直多関節ロボットの一例を示す斜視図である。

【図3】干渉判定装置の機能構成の例を示すブロック図である。

【図4】領域情報算出処理のメインルーチンの流れを示すフローチャートである。

【図5】領域情報算出処理のサブルーチンの流れを示すフローチャートである。

【図6】領域情報算出処理のサブルーチンの流れを示すフローチャートである。

【図7】領域情報算出処理のサブルーチンの流れを示すフローチャートである。

【図8】領域情報算出処理のサブルーチンの流れを示すフローチャートである。

【図9】コンフィグレーション空間における自己干渉領域及び自己非干渉領域の一例を示す図である。

【図10】エッジ削減処理について説明するための図である。

【図11】凸包の一例を示す図である。

【図12】非凸包の一例を示す図である。

【図13】凸包化の一例を示す図である。

【図14】根元側の自己干渉の具体例を示す図である。

【図15】手先側のリンクの最大可動域について説明するための図である。

【図16】手先側のリンクの最大可動域をリンクと見なした場合の垂直多関節ロボットの一例を示す図である。

【図17】詳細判定領域について説明するための図である。

【図18】手先側の自己干渉領域及び自己非干渉領域の具体例を示す図である。

【図19】干渉判定処理のメインルーチンの流れを示すフローチャートである。

【図20】根元側のジョイントの３次元のコンフィグレーション空間の自己干渉領域及び自己非干渉領域の一例を示す図である。

【図21】手先側のジョイントの３次元のコンフィグレーション空間の自己干渉領域及び自己非干渉領域の一例を示す図である。

【図22】障害物の３次元モデルの一例を示す図である。

【図23】障害物及び垂直多関節ロボットの３次元モデルの一例を示す図である。

【図24】自己干渉を判定すべき組み合わせについて説明するための図である。

【発明を実施するための形態】

【0021】

以下、本発明の実施形態の一例を、図面を参照しつつ説明する。なお、各図面において同一又は等価な構成要素及び部分には同一の参照符号を付与している。また、図面の寸法比率は、説明の都合上誇張されている場合があり、実際の比率とは異なる場合がある。

【0022】

（第１実施形態）

【0023】

図１は、第１実施形態に係る多関節ロボットの干渉判定装置（以下、単に干渉判定装置と称する）１０のハードウェア構成を示すブロック図である。

【0024】

本実施形態の干渉判定装置１０は、対象となる多関節ロボットの自己干渉を判定する。多関節ロボットとしては、例えば、垂直多関節ロボット、水平多関節ロボット（スカラーロボット）、及びパラレルリンクロボット等がある。本実施形態では、一例として垂直多関節ロボットの自己干渉を判定する場合について説明する。

【0025】

図１に示すように、干渉判定装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１１、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１３、ストレージ１４、入力部１５、モニタ１６、光ディスク駆動装置１７、及び通信インタフェース１８を有する。各構成は、バス１９を介して相互に通信可能に接続されている。

【0026】

本実施形態では、ＲＯＭ１２又はストレージ１４には、自己干渉判定を実行する自己干渉判定プログラムが格納されている。ＣＰＵ１１は、中央演算処理ユニットであり、各種プログラムを実行したり、各構成を制御したりする。すなわち、ＣＰＵ１１は、ＲＯＭ１２又はストレージ１４からプログラムを読み出し、ＲＡＭ１３を作業領域としてプログラムを実行する。ＣＰＵ１１は、ＲＯＭ１２又はストレージ１４に記録されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。

【0027】

ＲＯＭ１２は、各種プログラム及び各種データを格納する。ＲＡＭ１３は、作業領域として一時的にプログラム又はデータを記憶する。ストレージ１４は、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）又はＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

【0028】

入力部１５は、キーボード１５１、及びマウス１５２等のポインティングデバイスを含み、各種の入力を行うために使用される。モニタ１６は、例えば、液晶ディスプレイであり、各種の情報を表示する。モニタ１６は、タッチパネル方式を採用して、入力部１５として機能してもよい。光ディスク駆動装置１７は、各種の記録媒体(ＣＤ－ＲＯＭ又はブルーレイディスクなど)に記憶されたデータの読み込みや、記録媒体に対するデータの書き込み等を行う。

【0029】

通信インタフェース１８は、他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ又はＷｉ－Ｆｉ（登録商標）等の規格が用いられる。

【0030】

次に、垂直多関節ロボットについて説明する。

【0031】

図２は、垂直多関節ロボットＬＢの構成を示す図である。図２に示すように、垂直多関節ロボットＬＢは、ベースリンクＢＬ、リンクＬ１～Ｌ６、ジョイントＪ１～Ｊ６を備えた６自由度の６軸ロボットである。なお、ジョイントとは、リンク同士を接続する関節である。

【0032】

ベースリンクＢＬとリンクＬ１とは、図２において鉛直軸Ｓ１を中心として矢印Ｃ１方向に回転するジョイントＪ１を介して接続されている。従って、リンクＬ１は、ベースリンクＢＬを支点として矢印Ｃ１方向に回転する。

【0033】

リンクＬ１とリンクＬ２とは、図２において水平軸Ｓ２を中心として矢印Ｃ２方向に回転するジョイントＪ２を介して接続されている。従って、リンクＬ２は、リンクＬ１を支点として矢印Ｃ２方向に回転する。

【0034】

リンクＬ２とリンクＬ３とは、図２において軸Ｓ３を中心として矢印Ｃ３方向に回転するジョイントＪ３を介して接続されている。従って、リンクＬ３は、リンクＬ２を支点として矢印Ｃ３方向に回転する。

【0035】

リンクＬ３とリンクＬ４とは、図２において軸Ｓ４を中心として矢印Ｃ４方向に回転するジョイントＪ４を介して接続されている。従って、リンクＬ４は、リンクＬ３を支点として矢印Ｃ４方向に回転する。

【0036】

リンクＬ４とリンクＬ５とは、図２において軸Ｓ５を中心として矢印Ｃ５方向に回転するジョイントＪ５を介して接続されている。従って、リンクＬ５は、リンクＬ４を支点として矢印Ｃ５方向に回転する。

【0037】

リンクＬ５とリンクＬ６とは、図２において軸Ｓ６を中心として矢印Ｃ６方向に回転するジョイントＪ６を介して接続されている。従って、リンクＬ６は、リンクＬ５を支点として矢印Ｃ６方向に回転する。

【0038】

ジョイントＪ１～Ｊ６は、予め定めた回転角度の範囲が可動域として各々設定されている。

【0039】

多関節ロボットＬＢの姿勢は、ジョイントＪ１～Ｊ６の各々の回転角度によって定まる。すなわち、垂直多関節ロボットＬＢのリンクの自由度を表すパラメータをジョイントＪ１～Ｊ６の回転角度θ１～θ６とし、回転角度θ１～θ６を座標軸の値とした場合、垂直多関節ロボットＬＢの姿勢は、コンフィグレーション空間上の座標、すなわち点として表すことができる。ここで、コンフィグレーション空間とは、回転角度θ１～θ６のうち、少なくとも２つを座標軸とした座標系を表す空間である。

【0040】

次に、干渉判定装置１０の機能構成について説明する。

【0041】

図３は、干渉判定装置１０の機能構成の例を示すブロック図である。

【0042】

図３に示すように、干渉判定装置１０は、機能構成として、算出部１０１、取得部１０２、判定部１０３、及び記憶部１０４を有する。各機能構成は、ＣＰＵ１１がＲＯＭ１２又はストレージ１４に記憶された自己干渉判定プログラムを読み出し、ＲＡＭ１３に展開して実行することにより実現される。

【0043】

算出部１０１は、垂直多関節ロボットＬＢを構成するリンクＬ１～Ｌ６の形状及びリンクＬ１～Ｌ６を接続するジョイントＪ１～Ｊ６の可動域に基づいて領域情報を算出する。

【0044】

領域情報は、垂直多関節ロボットＬＢの２つ又は３つの特定のジョイントの回転角度を座標軸とするコンフィグレーション空間内に設定される領域であり、垂直多関節ロボットＬＢが自己干渉する領域として、特定のジョイントの回転角度の大きさに基づいて定められた干渉領域と、垂直多関節ロボットＬＢが自己干渉しない非干渉領域として、特定の関節の回転角度の大きさに基づいて定められた非干渉領域と、を表す情報である。

【0045】

ここで、自己干渉領域とは、垂直多関節ロボットＬＢが必ず自己干渉する領域として定めた領域であり、特定のジョイント以外のジョイントの回転角度に関係なく必ず自己干渉する領域として定めた領域である。また、干渉とは、リンク同士が物理的に接触すること又はリンクとエンドエフェクタとが物理的に接触することをいう。

【0046】

また、自己非干渉領域とは、垂直多関節ロボットＬＢが必ず自己干渉しない領域として定めた領域であり、特定のジョイント以外のジョイントの回転角度に関係なく必ず非干渉となる領域として定めた領域である。また、領域情報は、自己干渉領域及び自己非干渉領域の何れにも属さない領域として詳細判定領域を表す情報を含む。ここで、詳細判定領域とは、自己干渉するか否かを３次元の実空間に配置されたリンクの形状に基づいて具体的に判定する必要がある領域である。

【0047】

なお、本実施形態では、自己干渉領域が、垂直多関節ロボットＬＢが必ず自己干渉する領域である場合について説明するが、自己干渉領域は、必ず自己干渉する領域でなくてもよい。例えば、自己干渉領域の予め定めた閾値以上の領域、例えば９０％以上の領域で垂直多関節ロボットＬＢが自己干渉する領域を自己干渉領域としてもよい。これに対し自己非干渉領域は、垂直多関節ロボットＬＢが必ず自己干渉しない領域でなければならない。これは、実際は自己干渉する姿勢が自己非干渉領域に含まれていた場合において、自己非干渉と判定されたとしても、実際は自己干渉する虞があるためである。従って、自己非干渉領域は、垂直多関節ロボットＬＢが必ず自己干渉しない領域でなければならない。

【0048】

一方、実際は自己非干渉となる姿勢が自己干渉領域に含まれていた場合において、自己干渉と判定されたとしても、単に垂直多関節ロボットＬＢがその姿勢を取れないだけである。このため、垂直多関節ロボットＬＢの動作に制限はでるものの、自己干渉することは無い。従って、実際は自己非干渉となる姿勢が自己干渉領域に含まれていても特に問題は無い。

【0049】

垂直多関節ロボットＬＢの根元側の相互に隣接するＪ１～Ｊ３が垂直多関節ロボットＬＢの手先の位置を定めるのに寄与し、垂直多関節ロボットＬＢの手先側の相互に隣接するジョイントＪ４～Ｊ６が手先の姿勢を定めるのに寄与するため、根元側のジョイントＪ１～Ｊ３と、手先側のジョイントＪ４～Ｊ６とに分けて自己干渉を判定する。

【0050】

特定のジョイントは、自己干渉領域及び自己非干渉領域を算出するのに適したジョイントが予め選択される。どのジョイントが自己干渉領域及び自己非干渉領域を算出するのに適しているかは、各リンクの形状及び各ジョイントの可動域に基づいて決定される。

【0051】

例えば本実施形態では、根元側のジョイントＪ１～Ｊ３のうち、ジョイントＪ１に接続されたリンクＬ１はＳ１軸を中心として旋回運動する。この旋回運動を考えると、自己干渉するか否かはジョイントＪ１以外のジョイントＪ２～Ｊ６の回転角度に大きく依存する。従って、ジョイントＪ１を特定のジョイントとした場合、ジョイントＪ２、Ｊ３を特定のジョイントとする場合と比較して、自己干渉領域及び自己非干渉領域の何れにも属さない詳細判定領域が広くなる。詳細判定領域は、後述の通り、３次元モデルに基づいて自己干渉を判定しなくてはならない領域であるため、詳細判定領域が広い程、自己干渉の判定に時間がかかることとなる。従って、ジョイントＪ１を特定のジョイントとすることは、ジョイントＪ２、Ｊ３を特定のジョイントとする場合と比較して、自己干渉領域及び自己非干渉領域を算出するのに適していない。このように、根元側のジョイントＪ１～Ｊ３について、任意の一つ又は任意の組み合わせを特定のジョイントと仮定し、仮定したそれぞれの場合における詳細判定領域の大きさを比較して、最も詳細判定領域が小さくなる場合のジョイントが特定のジョイントとして決定される。なお、３次元モデルとは、３次元空間に配置された垂直多関節ロボットＬＢの各リンクの形状を表すモデルである。

【0052】

また、手先側のジョイントＪ４～Ｊ６からも、根元側のジョイントＪ１～Ｊ３と同様に、特定のジョイントが決定される。例えば、ジョイントＪ６に接続されるエンドエフェクタはＳ６軸を中心として旋回運動する。この旋回運動を考えると、自己干渉するか否かはジョイントＪ６以外のジョイントＪ１～Ｊ５の回転角度に大きく依存する。従って、ジョイントＪ６を特定のジョイントとした場合、ジョイントＪ４、Ｊ５を特定のジョイントとする場合と比較して、自己干渉領域及び自己非干渉領域の何れにも属さない詳細判定領域が広くなる。このため、自己干渉の判定に時間がかかることとなる。従って、ジョイントＪ６を特定のジョイントとすることは、ジョイントＪ４、Ｊ５を特定のジョイントとする場合と比較して、自己干渉領域及び自己非干渉領域を算出するのに適していない。

【0053】

本実施形態では、一例として、垂直多関節ロボットＬＢの根元側のジョイントＪ１～Ｊ３のうちジョイントＪ２、Ｊ３を特定のジョイントとすると共に、垂直多関節ロボットＬＢの手先側のジョイントＪ４～Ｊ６のうちジョイントＪ４、Ｊ５を特定のジョイントとした場合について説明する。

【0054】

取得部１０２は、算出部１０１が算出した領域情報を取得する。なお、算出部１０１が算出した領域情報ではなく、外部装置によって算出された領域情報を取得するようにしてもよい。

【0055】

判定部１０３は、特定のジョイントの回転角度によって定まる姿勢を表す座標が、取得部１０２により取得された領域情報により表される自己干渉領域及び自己非干渉領域の何れに属するかを判定することにより、垂直多関節ロボットＬＢが自己に干渉するか否かを判定する。

【0056】

記憶部１０４は、後述する領域情報算出プログラム、自己干渉判定プログラム、算出部１０１により算出された領域情報、垂直多関節ロボットＬＢのベースリンクＢＬ及びリンクＬ１～Ｌ６の形状データ、ジョイントＪ１～Ｊ６の可動域に関する情報、ジョイントＪ６に接続されるエンドエフェクタの形状データ、エンドエフェクタの処理対象であるワークの形状データ等を記憶する。

【0057】

干渉判定装置１０は、経路探索装置２０と接続されている。垂直多関節ロボットＬＢの経路計画は、垂直多関節ロボットＬＢを初期姿勢から目標姿勢まで動作させる場合の姿勢のリストである。経路計画を生成する際には、経路探索装置２０により探索された姿勢が自己干渉するか否かを干渉判定装置１０で判定する。

【0058】

経路探索装置２０は、現在の姿勢から垂直多関節ロボットＬＢを動かす量及び方向を決定し、次の姿勢を探索する。干渉判定装置１０は、経路探索装置２０により探索された姿勢が自己干渉するか否かを判定し、判定結果を経路探索装置２０に通知する。経路探索装置２０では、探索した姿勢が自己干渉しないと判定された場合は、その姿勢を姿勢のリストに加える。一方、自己干渉すると判定された場合は、別の姿勢を探索し、干渉判定装置１０に自己干渉判定を依頼する。このように経路探索装置２０による経路探索処理と干渉判定装置１０による干渉判定処理が繰り返されることにより経路計画が生成される。

【0059】

次に、干渉判定装置１０の作用について説明する。

【0060】

図４は、干渉判定装置１０のＣＰＵ１１により実行される領域情報算出処理の流れを示すフローチャートである。ＣＰＵ１１がＲＯＭ１２又はストレージ１４から領域情報算出プログラムを読み出して、ＲＡＭ１３に展開し実行することにより、領域情報算出処理が実行される。領域情報算出処理は、垂直多関節ロボットＬＢの経路計画を生成する際に実行される干渉判定処理に先立って実行される処理である。

【0061】

ＣＰＵ１１は、算出部１０１として、ジョイントＪ２、Ｊ３のコンフィグレーション空間における自己干渉領域を算出する（ステップＳ１００）。具体的には、図５に示す自己干渉領域算出処理を実行してジョイントＪ２、Ｊ３の自己干渉領域を算出する。図５の自己干渉領域算出処理については後述する。

【0062】

ＣＰＵ１１は、算出部１０１として、ジョイントＪ２、Ｊ３のコンフィグレーション空間における自己非干渉領域を算出する（ステップＳ１０２）。具体的には、図６に示す自己非干渉領域算出処理を実行してジョイントＪ２、Ｊ３の自己非干渉領域を算出する。図６の自己非干渉領域算出処理については後述する。なお、ステップＳ１００で算出される自己干渉領域の情報及びステップＳ１０２で算出される自己非干渉領域の情報は、本発明における第１の領域情報の一例である。

【0063】

ＣＰＵ１１は、算出部１０１として、ジョイントＪ４、Ｊ５のコンフィグレーション空間における自己干渉領域を算出する（ステップＳ１０４）。具体的には、図７に示す自己干渉領域算出処理を実行してジョイントＪ４、Ｊ５の自己干渉領域を算出する。図７の自己干渉領域算出処理については後述する。

【0064】

ＣＰＵ１１は、算出部１０１として、ジョイントＪ４、Ｊ５のコンフィグレーション空間における自己非干渉領域を算出する（ステップＳ１０６）。具体的には、図８に示す自己非干渉領域算出処理を実行してジョイントＪ４、Ｊ５の自己非干渉領域を算出する。図８の自己非干渉領域算出処理については後述する。なお、ステップＳ１０４で算出される自己干渉領域の情報及びステップＳ１０６で算出される自己非干渉領域の情報は、本発明における第２の領域情報の一例である。

【0065】

ＣＰＵ１１は、ステップＳ１００で算出したジョイントＪ２、Ｊ３のコンフィグレーション空間における自己干渉領域を表す情報と、ステップＳ１０２で算出したジョイントＪ２、Ｊ３のコンフィグレーション空間における自己非干渉領域を表す情報と、ステップＳ１０４で算出したジョイントＪ４、Ｊ５のコンフィグレーション空間における自己干渉領域を表す情報と、ステップＳ１０６で算出したジョイントＪ４、Ｊ５のコンフィグレーション空間における自己非干渉領域を表す情報と、を領域情報としてストレージ１４に記憶する（ステップＳ１０８）。

【0066】

次に、図５を参照して、ジョイントＪ２、Ｊ３の自己干渉領域を算出する処理について説明する。

【0067】

ＣＰＵ１１は、ジョイントＪ１～Ｊ６の各々について、可動域を等間隔で予め定めた数（例えば１００個）に分割してジョイント値を生成する（ステップＳ２００）。なお、予め定めた角度（例えば１度）で可動域を分割してもよい。なお、ジョイント値とは、ジョイントの回転角度を表す値である。

【0068】

ＣＰＵ１１は、ステップＳ２００で生成されたジョイントＪ２、Ｊ３のジョイント値の組み合わせのうち、自己干渉するか否かの判定が未処理のジョイント値の組み合わせを選択する（ステップＳ２０２）。

【0069】

ＣＰＵ１１は、ステップＳ２００で生成されたジョイントＪ１、Ｊ４～Ｊ６のジョイント値の組み合わせのうち、自己干渉するか否かの判定が未処理のジョイント値の組み合わせを選択する（ステップＳ２０４）。

【0070】

ＣＰＵ１１は、ステップＳ２０２、Ｓ２０４で選択されたジョイントＪ１～Ｊ６のジョイント値で表される垂直多関節ロボットＬＢの姿勢が自己干渉するか否かを判定する（ステップＳ２０６）。具体的には、記憶部１０４からベースリンクＢＬ、リンクＬ１～Ｌ６の形状データを取得し、垂直多関節ロボットＬＢの姿勢における３次元モデルを生成する。そして、生成した３次元モデルに基づいて、自己干渉、すなわちリンク同士の干渉が発生するリンクの組み合わせが存在するか否かを判定する。

【0071】

自己干渉しないと判定された場合（ステップＳ２０８：ＮＯ）、ステップＳ２１４へ移行する。一方、自己干渉すると判定された場合（ステップＳ２０８：ＹＥＳ）、ＣＰＵ１１は、ジョイントＪ１、Ｊ４～Ｊ６の全てのジョイント値の組み合わせについてステップＳ２０６の判定を実行したか否かを判定する（ステップＳ２１０）。ジョイントＪ１、Ｊ４～Ｊ６の全てのジョイント値の組み合わせについてステップＳ２０６の判定を実行していない場合（ステップＳ２１０：ＮＯ）、ステップＳ２０４へ移行する。一方、ジョイントＪ１、Ｊ４～Ｊ６の全てのジョイント値の組み合わせについてステップＳ２０６の判定を実行した場合（ステップＳ２１０：ＹＥＳ）、ＣＰＵ１１は、ステップＳ２０２で選択されたジョイントＪ２、Ｊ３のジョイント値の組み合わせを自己干渉するジョイント値の組み合わせとして記憶部１０４に記憶する（ステップＳ２１２）。

【0072】

ＣＰＵ１１は、ジョイントＪ２、Ｊ３の全てのジョイント値の組み合わせについて自己干渉を判定したか否かを判定する（ステップＳ２１４）。ジョイントＪ２、Ｊ３の全てのジョイント値の組み合わせについて自己干渉を判定していない場合（ステップＳ２１４：ＮＯ）、ステップＳ２０２へ移行する。

【0073】

ジョイントＪ２、Ｊ３の全てのジョイント値の組み合わせについて自己干渉を判定した場合（ステップＳ２１４：ＹＥＳ）、ＣＰＵ１１は、自己干渉すると判定されたジョイントＪ２、Ｊ３のジョイント値の集合を領域化する（ステップＳ２１６）。一例として、図９には、自己干渉すると判定されたジョイントＪ２、Ｊ３のジョイント値の集合である点群２１Ａ～２１Ｃを示した。ステップＳ２１６では、各点群２１Ａ～２１Ｃ毎に、点群を全て含む領域、例えばバウンディングボックス化した領域を自己干渉領域とする。具体的には、点群２１Ａ～２１Ｃの各々について、外側の点を直線（以下、エッジと称する）で接続することにより得られた領域を自己干渉領域とする。これにより、例えば図９に示すような自己干渉領域２２Ａ～２２Ｃが得られる。

【0074】

ＣＰＵ１１は、ステップＳ２１６で領域化された自己干渉領域の輪郭に収まるように自己干渉領域の形状を簡易化する（ステップＳ２１８）。簡易化の方法としては、例えばエッジ削減処理及び凸包化処理が挙げられる。

【0075】

以下、エッジ削減処理について説明する。例えば図１０に示すように、自己干渉領域２２Ａ～２２Ｃの各々について、複数のエッジを一つのエッジにまとめることにより、自己干渉領域を簡易化する。具体的には、例えば図１０に示すように、自己干渉領域２２Ａの複数のエッジ２５Ａ～２５Ｅを、一つのエッジ２７Ａ又はエッジ２７Ｂに置き換えることにより簡易化する。なお、エッジ２７Ａは自己干渉領域２２Ａの内側に配置され、エッジ２７Ｂは自己干渉領域２２Ａの外側に配置されている。エッジ２７Ａのように、自己干渉領域２２Ａの内側に配置した場合には、簡易化後の自己干渉領域が狭くなる。このため、実際には自己非干渉となる姿勢が簡易化後の自己干渉領域に含まれる可能性が低くなるため、判定の精度が向上する。一方、エッジ２７Ｂのように、自己干渉領域２２の外側に配置した場合には、簡易化後の自己干渉領域が広くなる。このため、元々詳細判定領域に存在する姿勢も自己干渉領域に含まれることになるが、自己干渉領域が広くなるため、判定に要する時間は短縮される。

【0076】

このように自己干渉領域２２Ａ～２２Ｃの各々についてエッジ削減処理を実行することにより、図９に示すような自己干渉領域３０Ａ～３０Ｃが生成される。なお、以下では、自己干渉領域３０Ａ～３０Ｃを特に区別しない場合は単に自己干渉領域３０と称する。

【0077】

次に、凸包化処理について説明する。凸包（Ｃｏｎｖｅｘ）とは、凹みが無い形状をいう。凸包は、２次元の場合は凸多角形、３次元の場合は凸多面体となる。例えば図１１に示すような形状３１Ａ～３１Ｃは凸包である。また、非凸包（Ｃｏｎｃａｖｅ）とは、凹みが有る形状をいう。例えば図１２に示すような形状３３Ａ～３３Ｃは非凸包である。そして、凸包化とは、非凸包を凸包の集合で表すことである。例えば図１２に示した非凸包である形状３３Ａは、図１３に示すように、複数の凸包３５Ａ、３５Ｂで表される。形状３３Ｂ、３３Ｃについても同様に、複数の凸包で表される。

【0078】

従って、図９に示す自己干渉領域２２Ａを凸包化した場合、複数の凸包３７Ａ、３７Ｂで表され、自己干渉領域２２Ｂを凸包化した場合、複数の凸包３９Ａ～３９Ｃで表される。なお、自己干渉領域２２Ｃは元々凸包なので凸包化されない。このように自己干渉領域を凸包化することにより、凸包化しない場合と比較して、後述する干渉判定処理における自己干渉の判定に要する時間を短縮することができる。なお、ステップＳ２１８の簡易化処理を省略してもよい。すなわち、後述する干渉判定処理において、自己干渉領域２２Ａ～２２Ｃを用いて自己干渉を判定してもよい。

【0079】

図１４には、自己干渉領域における垂直多関節ロボットＬＢの姿勢の一例を示した。図１４に示すように、ジョイントＪ２、Ｊ３のジョイント値を座標軸とするコンフィグレーション空間の自己干渉領域３０にプロットされた点Ｐ１で表される垂直多関節ロボットＬＢの姿勢Ａ１は、リンクＬ３とリンクＬ１とが干渉する姿勢である。また、自己干渉領域３０にプロットされた点Ｐ２で表される垂直多関節ロボットＬＢの姿勢Ａ２は、リンクＬ６とベースリンクＢＬとが干渉する姿勢である。また、自己干渉領域３０にプロットされた点Ｐ３で表される垂直多関節ロボットＬＢの姿勢Ａ３は、リンクＬ３とベースリンクＢＬとが干渉する姿勢である。また、自己干渉領域３０にプロットされた点Ｐ４で表される垂直多関節ロボットＬＢの姿勢Ａ４は、リンクＬ４とベースリンクＢＬとが干渉する姿勢である。

【0080】

次に、図６を参照して、ジョイントＪ２、Ｊ３の自己非干渉領域を算出する処理について説明する。

【0081】

ＣＰＵ１１は、ジョイントＪ４～Ｊ６に接続されるリンクＬ４～Ｌ６及びエンドエフェクタの最大可動域を算出する（ステップＳ３００）。具体的には、まずジョイントＪ４～Ｊ６の各々に接続されるリンクＬ４～Ｌ６の形状データ及びジョイントＪ６に接続されるエンドエフェクタの形状データを記憶部１０４から読み出す。そして、ジョイントＪ４～Ｊ６の各々を可動域全体で可動させた場合におけるリンクＬ４～Ｌ６及びエンドエフェクタの最大可動域を算出する。例えば図１５に示すように、エンドエフェクタＥＥが接続されたジョイントＪ６を可動域全体で可動させた場合、エンドエフェクタＥＥが可動する最大の領域は最大可動域３２のようになる。また、ジョイントＪ５を可動域全体で可動させた場合、エンドエフェクタＥＥ及びリンクＬ６が可動する最大の領域は最大可動域３４のようになる。また、ジョイントＪ４を可動域全体で可動させた場合、エンドエフェクタＥＥ、リンクＬ６、及びリンクＬ５が可動する最大の領域は最大可動域３６のようになる。この場合、垂直多関節ロボットＬＢは、図１６に示すように、リンクＬ３に、図１５に示す最大可動域３６で表される形状のリンクＬ７が接続された構成と考えられる。このように、自己非干渉領域を算出する際にリンクＬ４～Ｌ６及びエンドエフェクタＥＥの最大可動域を算出するのは、最大可動域で表されるリンクが接続された状態で自己非干渉であれば、リンク４～Ｌ６及びエンドエフェクタＥＥがどのような姿勢でも必ず自己非干渉であると判定できるからである。

【0082】

ＣＰＵ１１は、図５のステップＳ２００で生成されたジョイントＪ２、Ｊ３のジョイント値の組み合わせのうち、自己非干渉の判定が未処理のジョイント値の組み合わせを選択する（ステップＳ３０２）。

【0083】

ＣＰＵ１１は、図５のステップＳ２００で生成されたジョイントＪ１のジョイント値のうち、自己非干渉の判定が未処理のジョイント値を選択する（ステップＳ３０４）。

【0084】

ＣＰＵ１１は、ステップＳ３０２、Ｓ３０４で選択されたＪ１～Ｊ３のジョイント値で表される垂直多関節ロボットＬＢの姿勢が自己非干渉であるか否かを判定する（ステップＳ３０６）。具体的には、記憶部１０４からベースリンクＢＬ及びリンクＬ１～Ｌ３の形状データを取得する。そして、取得したベースリンクＢＬ及びリンクＬ１～Ｌ３の形状データと、ステップＳ３００で算出したリンクＬ７の形状データと、に基づいて、ベースリンクＢＬ、リンクＬ１～Ｌ３、及びリンクＬ７で構成される垂直多関節ロボットＬＢの３次元モデルを生成する。そして、生成した３次元モデルに基づいて、自己非干渉であるか否かを判定する。

【0085】

自己非干渉でないと判定された場合（ステップＳ３０８：ＮＯ）、ステップＳ３１４へ移行する。一方、自己非干渉であると判定された場合（ステップＳ３０８：ＹＥＳ）、ＣＰＵ１１は、ジョイントＪ１の全てのジョイント値について自己非干渉の判定を実行したか否かを判定する（ステップＳ３１０）。ジョイントＪ１の全てのジョイント値について自己非干渉の判定を実行していない場合（ステップＳ３１０：ＮＯ）、ステップＳ３０４へ移行する。一方、ジョイントＪ１の全てのジョイント値について自己非干渉の判定を実行した場合（ステップＳ３１０：ＹＥＳ）、ＣＰＵ１１は、ステップＳ３０２で選択されたジョイントＪ２、Ｊ３のジョイント値の組み合わせを自己非干渉のジョイント値の組み合わせとして記憶部１０４に記憶する（ステップＳ３１２）。

【0086】

ＣＰＵ１１は、ジョイントＪ２、Ｊ３の全てのジョイント値の組み合わせについて自己非干渉の判定を実行したか判定する（ステップ３１４）。ジョイントＪ２、Ｊ３の全てのジョイント値の組み合わせについて自己非干渉を判定していない場合（ステップＳ３１４：ＮＯ）、ステップＳ３０２へ移行する。ジョイントＪ２、Ｊ３の全てのジョイント値の組み合わせについて自己非干渉を判定した場合（ステップＳ３１４：ＹＥＳ）、ＣＰＵ１１は、自己非干渉であると判定されたジョイントＪ２、Ｊ３のジョイント値の集合を領域化する（ステップＳ３１６）。一例として、図９には、自己非干渉すると判定されたジョイントＪ２、Ｊ３のジョイント値の集合である点群２４を示した。ステップＳ３１６では、点群２４を全て含む領域、例えばバウンディングボックス化した領域を自己非干渉領域とする。具体的には、点群２４について、外側の点を直線で接続することにより得られた領域を自己非干渉領域とする。これにより、例えば図９に示すような自己非干渉領域２６が得られる。

【0087】

ＣＰＵ１１は、ステップＳ３１６で領域化された自己非干渉領域の輪郭に収まるように自己非干渉領域の形状を簡易化する（ステップＳ３１８）。簡易化の方法としては、前述した自己干渉領域の形状を簡易化する場合と同様に、例えばエッジ削減処理及び凸包化処理が挙げられる。

【0088】

前述したように、自己非干渉領域は、垂直多関節ロボットＬＢが必ず自己干渉しない領域でなければならない。このため、自己非干渉領域２６に対してエッジ削減処理を実行する場合において、自己非干渉領域２６の複数のエッジを一つのエッジに置き換える際には、当該一つのエッジを自己非干渉領域２６の内側に配置する。すなわちエッジ削減後の自己非干渉領域が自己非干渉領域２６よりも小さくなるようにエッジ削減処理を実行する。このように自己非干渉領域２６に対してエッジ削減処理を実行した場合は、例えば図９に示すような自己非干渉領域２８が得られる。また、自己非干渉領域２６を凸包化した場合、複数の凸包２６Ａ～２６Ｃで表される。このように自己非干渉領域を凸包化することにより、凸包化しない場合と比較して、後述する干渉判定処理における自己非干渉の判定に要する時間を短縮することができる。

【0089】

図１７に示すように、自己干渉領域３０及び自己非干渉領域２８の何れにも属さない領域は、詳細判定領域４０として定められる。

【0090】

なお、自己干渉領域でも自己非干渉領域でもない詳細判定領域ができるのは、３次元空間に配置されたリンクＬ４、Ｌ５、Ｌ６、及びエンドエフェクタＥＥの形状によっては自己干渉する場合もあれば自己干渉しない場合もあるのが主な理由である。

【0091】

次に、図７を参照して、ジョイントＪ４、Ｊ５の自己干渉領域を算出する処理について説明する。

【0092】

ＣＰＵ１１は、図５のステップＳ２００で生成されたジョイントＪ４、Ｊ５のジョイント値の組み合わせのうち、自己干渉するか否かの判定が未処理のジョイント値の組み合わせを選択する（ステップＳ４００）。

【0093】

ＣＰＵ１１は、図５のステップＳ２００で生成されたジョイントＪ６のうち、自己干渉するか否かの判定が未処理のジョイント値を選択する（ステップＳ４０２）。

【0094】

ＣＰＵ１１は、ステップＳ４００、Ｓ４０２で選択されたＪ４～Ｊ６のジョイント値で表される垂直多関節ロボットＬＢの手先側の姿勢が自己干渉するか否かを判定する（ステップＳ４０４）。具体的には、記憶部１０４からリンクＬ４～Ｌ６の形状データを取得し、垂直多関節ロボットＬＢの手先の姿勢における３次元モデルを生成する。そして、生成した３次元モデルに基づいて、自己干渉、すなわちリンク同士の干渉が発生するリンクの組み合わせが存在するか否かを判定する。このように、根元側のリンクＬ１～Ｌ３は考慮せずに、手先側のリンクＬ４～Ｌ６及びエンドエフェクタＥＥのみを考慮して自己干渉を判定する。

【0095】

自己干渉しないと判定された場合（ステップＳ４０６：ＮＯ）、ステップ４１２へ移行する。一方、自己干渉すると判定された場合（ステップＳ４０６：ＹＥＳ）、ＣＰＵ１１は、ＣＰＵ１１は、ジョイントＪ６の全てのジョイント値についてステップＳ４０６の判定を実行したか否かを判定する（ステップＳ４０８）。ジョイントＪ６の全てのジョイント値についてステップＳ４０６の判定を実行した場合（ステップＳ４０８：ＹＥＳ）、ＣＰＵ１１は、ステップＳ４００で選択されたジョイントＪ４、Ｊ５のジョイント値の組み合わせを自己干渉する組み合わせとして記憶部１０４に記憶する（ステップＳ４１０）。一方、ジョイントＪ６の全てのジョイント値についてステップＳ４０６の判定を実行していない場合（ステップＳ４０８：ＮＯ）、ステップＳ４０２へ移行する。

【0096】

ＣＰＵ１１は、ジョイントＪ４、Ｊ５の全てのジョイント値の組み合わせについてステップＳ４０２～Ｓ４１０の処理を実行したか否かを判定する（ステップＳ４１２）。ジョイントＪ４、Ｊ５の全てのジョイント値の組み合わせについてステップＳ４０２～Ｓ４１０の処理を実行していない場合（ステップＳ４１２：ＮＯ）、ステップＳ４００へ移行する。一方、ジョイントＪ４、Ｊ５の全てのジョイント値の組み合わせについてステップＳ４０２～Ｓ４１０の処理を実行した場合（ステップＳ４１２：ＹＥＳ）、ＣＰＵ１１は、自己干渉すると判定されたジョイントＪ４、Ｊ５のジョイント値の集合を自己干渉領域として領域化する（ステップ４１４）。この処理は、図５のステップＳ２１６の処理と同様であるため説明は省略する。

【0097】

ＣＰＵ１１は、ステップＳ４１４で領域化された自己干渉領域の輪郭に収まるように自己干渉領域の形状を簡易化する（ステップＳ４１６）。この処理は、図５のステップＳ２１８の処理と同様であるため説明は省略する。これにより、例えば図１８に示すような自己干渉領域４２が得られる。

【0098】

次に、図８を参照して、ジョイントＪ４、Ｊ５の自己非干渉領域を算出する処理について説明する。

【0099】

ステップＳ５００～Ｓ５０４は、図７のステップＳ４００～Ｓ４０４と同様の処理であるため説明は省略する。

【0100】

自己干渉すると判定された場合（ステップＳ５０６：ＮＯ）、ステップ５１２へ移行する。一方、自己干渉しないと判定された場合（ステップＳ５０６：ＹＥＳ）、ＣＰＵ１１は、ジョイントＪ６の全てのジョイント値についてステップＳ５０６の判定を実行したか否かを判定する（ステップＳ５０８）。ジョイントＪ６の全てのジョイント値についてステップＳ５０６の判定を実行した場合（ステップＳ５０８：ＹＥＳ）、ＣＰＵ１１は、ステップＳ５００で選択されたジョイントＪ４、Ｊ５のジョイント値の組み合わせを自己非干渉の組み合わせとして記憶部１０４に記憶する（ステップＳ５１０）。一方、ジョイントＪ６の全てのジョイント値についてステップＳ５０６の判定を実行していない場合（ステップＳ５０８：ＮＯ）、ステップＳ５０２へ移行する。

【0101】

ステップＳ５１２、Ｓ５１４は、図７のステップＳ４１２、Ｓ４１４と同様の処理であり、ステップＳ５１６は図６のステップ３１８と同様の処理であるため説明は省略する。これにより、例えば図１８に示すような自己非干渉領域４４が得られる。

【0102】

また、図１８には、自己干渉領域及び自己非干渉領域における垂直多関節ロボットＬＢの手先側の姿勢の一例を示した。図１８に示すように、ジョイントＪ４、Ｊ５のジョイント値を座標軸とするコンフィグレーション空間の自己干渉領域４２にプロットされた点Ｐ５で表される垂直多関節ロボットＬＢの姿勢Ａ５は、リンクＬ４とエンドエフェクタＥＥとが干渉する姿勢である。また、自己非干渉領域４４にプロットされた点Ｐ６で表される垂直多関節ロボットＬＢの姿勢Ａ６は、自己干渉しない姿勢である。

【0103】

なお、図１８に示す例では、詳細判定領域はほとんど生じていないが、手先側のリンクＬ４～Ｌ６及びエンドエフェクタＥＥの形状によって詳細判定領域の大きさは異なる。

【0104】

次に、垂直多関節ロボットＬＢの経路計画を生成する際に実行される自己干渉判定処理について説明する。図１９は、干渉判定装置１０のＣＰＵ１１により実行される自己干渉判定処理の流れを示すフローチャートである。ＣＰＵ１１がＲＯＭ１２又はストレージ１４から自己干渉判定プログラムを読み出して、ＲＡＭ１３に展開し実行することにより、自己干渉判定処理が実行される。

【0105】

ＣＰＵ１１は、取得部１０２として、ジョイントＪ２、Ｊ３のコンフィグレーション空間における自己干渉領域及び自己非干渉領域を表す領域情報と、ジョイントＪ４、Ｊ５のコンフィグレーション空間における自己干渉領域及び自己非干渉領域を表す領域情報と、を記憶部１０４から取得する（ステップＳ６００）。

【0106】

ＣＰＵ１１は、経路探索装置２０によって探索された姿勢を表すジョイントＪ１～Ｊ６のジョイント値を経路探索装置２０から取得する（ステップＳ６０２）。

【0107】

ＣＰＵ１１は、ステップＳ５０２で取得したジョイントＪ１～Ｊ６のジョイント値のうち、ジョイントＪ２、Ｊ３のジョイント値が、ステップＳ６００で取得した領域情報で表されるジョイントＪ２、Ｊ３のコンフィグレーション空間における自己干渉領域内にあるか否かを判定する（ステップＳ６０４）。

【0108】

ジョイントＪ２、Ｊ３のジョイント値が自己干渉領域内にある場合（ステップＳ６０４：ＹＥＳ）、ＣＰＵ１１は、ステップＳ６０２で取得したジョイントＪ１～Ｊ６のジョイント値で表される姿勢が自己干渉することを経路探索装置２０に通知する（ステップＳ６１２）。

【0109】

ジョイントＪ２、Ｊ３のジョイント値が自己干渉領域内にない場合（ステップＳ６０４：ＮＯ）、ＣＰＵ１１は、ジョイントＪ２、Ｊ３のジョイント値が、ステップＳ６００

【0110】

で取得した領域情報で表されるジョイントＪ２、Ｊ３のコンフィグレーション空間における自己非干渉領域内にあるか否かを判定する（ステップＳ６０６）。

【0111】

ジョイントＪ２、Ｊ３のジョイント値が自己非干渉領域内にある場合（ステップＳ６０６：ＹＥＳ）、ステップＳ６１４へ移行する。

【0112】

ジョイントＪ２、Ｊ３のジョイント値が自己非干渉領域内にない場合（ステップＳ６０６：ＮＯ）、ＣＰＵ１１は、垂直多関節ロボットＬＢが自己干渉するか否かを詳細に判定する（ステップＳ６０８）。具体的には、記憶部１０４からベースリンクＢＬ、リンクＬ１～Ｌ３の形状データを取得する。そして、図２４に示すベースリンクＢＬ、Ｌ１～Ｌ３のリンク同士の組み合わせのうち、判定が「○」となっている１８通りのリンクの組み合わせについて、３次元モデルを生成し、生成した３次元モデルに基づいて、自己干渉が発生するか否かを判定する処理を順次実行する。

【0113】

ＣＰＵ１１は、自己干渉するリンクの組み合わせが存在すると判定した場合（ステップＳ６１０：ＹＥＳ）、自己干渉することを経路探索装置２０へ通知する（ステップＳ６１２）。

【0114】

一方、自己干渉するリンクの組み合わせが存在しないと判定した場合（ステップＳ６１０：ＮＯ）、ステップＳ６００で取得したジョイントＪ１～Ｊ６のジョイント値のうち、ジョイントＪ４、Ｊ５のジョイント値が、ステップＳ６００で取得した領域情報で表されるジョイントＪ４、Ｊ５のコンフィグレーション空間における自己干渉領域内にあるか否かを判定する（ステップＳ６１４）。

【0115】

ジョイントＪ４、Ｊ５のジョイント値が自己干渉領域内にある場合（ステップＳ６１４：ＹＥＳ）、ＣＰＵ１１は、自己干渉することを経路探索装置２０へ通知する（ステップＳ６１２）。

【0116】

一方、ジョイントＪ４、Ｊ５のジョイント値が自己干渉領域内にない場合（ステップＳ６１４：ＮＯ）、ＣＰＵ１１は、ジョイントＪ４、Ｊ５のジョイント値が、ステップＳ５００で取得した領域情報で表されるジョイントＪ４、Ｊ５のコンフィグレーション空間における自己非干渉領域内にあるか否かを判定する（ステップＳ６１６）。

【0117】

ジョイントＪ２、Ｊ３のジョイント値が自己非干渉領域内にある場合（ステップＳ５１６：ＹＥＳ）、ＣＰＵ１１は、自己非干渉であることを経路探索装置２０へ通知する（ステップＳ６２２）。

【0118】

ジョイントＪ２、Ｊ３のジョイント値が自己非干渉領域内にない場合（ステップＳ６１６：ＮＯ）、ＣＰＵ１１は、ステップＳ６０２で取得したジョイントＪ１～Ｊ６のジョイント値で表される姿勢が自己干渉するか否かを詳細に判定する（ステップＳ６１８）。具体的には、記憶部１０４からリンクＬ４、Ｌ５の形状データを取得する。そして、図２４に示すＬ４、Ｌ５のリンク同士の組み合わせのうち、判定が「○」となっている３通りのリンクの組み合わせについて、３次元モデルを生成し、生成した３次元モデルに基づいて、自己干渉が発生するか否かを判定する処理を順次実行する。

【0119】

ＣＰＵ１１は、自己干渉するリンクの組み合わせが存在すると判定した場合（ステップＳ６２０：ＹＥＳ）、自己干渉することを経路探索装置２０へ通知する（ステップＳ６１２）。一方、自己干渉するリンクの組み合わせが存在しないと判定した場合（ステップＳ６２０：ＮＯ）、ＣＰＵ１１は、自己非干渉であることを経路探索装置２０へ通知する（ステップＳ６２２）。

【0120】

なお、領域情報を取得する処理であるステップＳ６００の処理は、図１９の処理を最初に実行したときだけ実行すればよいため、２回目以降はステップＳ６００の処理を省略してもよい。

【0121】

以上のように、第１実施形態の干渉判定装置１０によれば、以下の効果が得られる。干渉判定装置１０は、特定のジョイントの回転角度を座標軸とするコンフィグレーション空間内に自己干渉領域及び自己非干渉領域を設定し、自己干渉領域及び自己非干渉領域の何れにも属さない領域は詳細判定領域とする。そして、特定のジョイントのジョイント値の組み合わせが自己干渉領域内にあるか否かを判定することによって自己干渉するか否かを判定する。このため、従来のようにリンクの形状に基づく３次元モデルを生成して自己干渉するか否かを判定する場合と比較して、自己干渉の判定に要する時間を短縮することができる。

【0122】

また、自己干渉領域及び自己非干渉領域を簡易化するので、自己干渉の判定及び自己非干渉の判定に要する時間を更に短縮することができる。

【0123】

干渉判定装置１０は、上記の実施形態に限定されず、種々の改変が可能である。例えば、図５のフローチャートにおいて、ジョイントＪ５、Ｊ６に接続されるリンクを考慮せずに自己干渉を判定してもよい。この場合、図５のフローチャートの各ステップにおける「ジョイントＪ１、Ｊ４～Ｊ６」は「ジョイントＪ１、Ｊ４」に読み替えられる。この場合、ジョイントＪ５、Ｊ６に接続されるリンクを考慮しないことによって自己干渉領域が狭くなり詳細判定領域が増加するが、領域情報の算出に要する時間を短縮することができる。

【0124】

また、本実施形態では、２つの特定のジョイントの回転角度を座標軸とする２次元のコンフィグレーション空間に自己干渉領域及び自己非干渉領域を設定したが、３つの特定のジョイントの回転角度を座標軸とする３次元のコンフィグレーション空間に自己干渉領域及び自己非干渉領域を設定してもよい。この場合、図４に示したフローチャートの各ステップにおける「ジョイントＪ２、Ｊ３」は「ジョイントＪ１～Ｊ３」に、「ジョイントＪ４、Ｊ５」は「ジョイントＪ４～Ｊ６」にそれぞれ読み替えられる。また、図５に示したフローチャートの各ステップにおける「ジョイントＪ２、Ｊ３」は「ジョイントＪ１～Ｊ３」に、「ジョイントＪ１、Ｊ４～Ｊ６」は「ジョイントＪ４～Ｊ６」にそれぞれ読み替えられる。また、図６に示したフローチャートの各ステップにおける「ジョイントＪ２、Ｊ３」は「ジョイントＪ１～Ｊ３」に読み替えられる。この場合、ステップＳ３０４、Ｓ３１０の処理は省略される。また、図７、８に示したフローチャートの各ステップにおける「ジョイントＪ４、Ｊ５」は「ジョイントＪ４～Ｊ６」に読み替えられる。この場合、図７のステップＳ４０２、Ｓ４０８、図８のステップ５０２、５０８の処理は省略される。また、図１９に示したフローチャートの各ステップにおける「ジョイントＪ２、Ｊ３」は「ジョイントＪ１～Ｊ３」に、「ジョイントＪ４、Ｊ５」は「ジョイントＪ４～Ｊ６」に読み替えられる。

【0125】

図２０には、ジョイントＪ１～Ｊ３の回転角度を座標軸とした３次元のコンフィグレーション空間に設定された自己干渉領域５０及び自己非干渉領域５２を示した。また、図２１には、ジョイントＪ４～Ｊ６の回転角度を座標軸とした３次元のコンフィグレーション空間に設定された自己干渉領域５４及び自己非干渉領域５６を示した。

【0126】

このように、３次元のコンフィグレーション空間に自己干渉領域及び自己非干渉領域を設定した場合、２次元のコンフィグレーション空間に自己干渉領域及び自己非干渉領域を設定する場合と比較して、干渉判定に要する時間は長くなるが、精度良く自己干渉を判定することができる。

【0127】

また、本実施形態では、垂直多関節ロボットＬＢが６個の関節を有する６軸ロボットである場合について説明したが、垂直多関節ロボットＬＢが５個以下又は７個以上の関節を有する多関節ロボットに適用してもよい。

【0128】

（第２実施形態）

【0129】

第１実施形態においては、垂直多関節ロボットＬＢの自己干渉を判定する場合について説明したが、第２実施形態においては、垂直多関節ロボットＬＢが障害物に対して干渉する障害物干渉が発生するか否かを判定する場合について説明する。なお、第１実施形態と同一部分には同一符号を付し、詳細な説明は省略する。

【0130】

第２実施形態に係る干渉判定装置のハードウェア構成及び機能構成は、第１実施形態で説明した図１、３に示す干渉判定装置１０と基本的に同一であるが、干渉を判定する対象が障害物である点が異なる。

【0131】

第２実施形態における算出部１０１は、垂直多関節ロボットＬＢを構成するリンクＬ１～Ｌ６の形状、リンクＬ１～Ｌ６を接続するジョイントＪ１～Ｊ６の可動域、及び障害物の形状に基づいて、垂直多関節ロボットＬＢが障害物に対して干渉する対障害物干渉領域と、垂直多関節ロボットＬＢが障害物に対して干渉しない対障害物非干渉領域と、を表す領域情報を算出する。図２２にて一例として障害物６０を示した。

【0132】

第２実施形態における取得部１０２は、算出部１０１が算出した領域情報を取得する。

【0133】

第２実施形態における判定部１０３は、特定のジョイントの回転角度によって定まる姿勢を表す座標が、取得部１０２により取得された領域情報により表される対障害物干渉領域及び対障害物非干渉領域の何れに属するかを判定することにより、垂直多関節ロボットＬＢが障害物に対して干渉するか否かを判定する。

【0134】

第２実施形態における記憶部１０４は、後述する領域情報算出プログラム、対障害物干渉判定プログラム、算出部１０１により算出された領域情報、垂直多関節ロボットＬＢのベースリンクＢＬ及びリンクＬ１～Ｌ６の形状データ、ジョイントＪ１～Ｊ６の可動域に関する情報、ジョイントＪ６に接続されるエンドエフェクタの形状データ、エンドエフェクタの処理対象であるワークの形状データ、障害物の形状データ等が記憶される。

【0135】

次に、第２実施形態における干渉判定装置１０の作用について説明する。

【0136】

領域情報については、第１実施形態において３つの特定のジョイントの回転角度を座標軸とする３次元のコンフィグレーション空間に自己干渉領域及び自己非干渉領域を設定する場合と同様に、図４～８の処理を実行することにより算出することができる。この場合、図４～８のフローチャートの各ステップにおける「自己干渉領域」は「対障害物干渉領域」に、「自己非干渉領域」は「対障害物非干渉領域」にそれぞれ読み替えられる。また、図５～８のフローチャートの各ステップにおける「自己干渉」は「対障害物干渉」に、「自己非干渉」は「対障害物非干渉」にそれぞれ読み替えられる。

【0137】

また、第２実施形態における図５のステップＳ２０６では、記憶部１０４からベースリンクＢＬ、リンクＬ１～Ｌ６の形状データ、及び障害物の形状データを取得し、垂直多関節ロボットＬＢ及び障害物の３次元モデルを生成する。そして、生成した３次元モデルに基づいて、垂直多関節ロボットＬＢが障害物に対して干渉するか否かを判定する。その他の処理は第１実施形態と同様であるため説明を省略する。

【0138】

また、第２実施形態における図６のステップＳ３０６では、記憶部１０４からベースリンクＢＬ及びリンクＬ１～Ｌ３の形状データ、及び障害物の形状データを取得する。そして、取得したベースリンクＢＬ及びリンクＬ１～Ｌ３の形状データと、ステップＳ３００で算出したリンクＬ７の形状データと、に基づいて、ベースリンクＢＬ、リンクＬ１～Ｌ３、及びリンクＬ７で構成される垂直多関節ロボットＬＢの３次元モデルを生成すると共に、障害物の形状データに基づいて障害物の３次元モデルを生成する。図２３には、一例としてステップＳ３０６の処理で生成された３次元モデルの垂直多関節ロボットＬＢ及び障害物６０を示した。そして、生成した３次元モデルに基づいて、垂直多関節ロボットＬＢが障害物に対して非干渉であるか否かを判定する。その他の処理は第１実施形態と同様であるため説明を省略する。

【0139】

また、第２実施形態における図７のステップＳ４０４、図８のステップＳ５０４では、記憶部１０４からリンクＬ４～Ｌ６の形状データ及び障害物の形状データを取得し、垂直多関節ロボットＬＢの手先の姿勢における３次元モデルを生成すると共に、障害物の３次元モデルを生成する。そして、生成した３次元モデルに基づいて、垂直多関節ロボットＬＢが障害物に対して干渉するか否かを判定する。その他の処理は第１実施形態と同様であるため説明を省略する。

【0140】

また、第２実施形態における図１８のステップＳ６０２では、ジョイントＪ２、Ｊ３のコンフィグレーション空間における対障害物干渉領域及び対障害物非干渉領域を表す領域情報と、ジョイントＪ４、Ｊ５のコンフィグレーション空間における対障害物干渉領域及び対障害物非干渉領域を表す領域情報と、を記憶部１０４から取得する。

【0141】

また、第２実施形態におけるステップＳ６０８では、記憶部１０４からベースリンクＢＬ、リンクＬ１～Ｌ３の形状データを取得する。そして、ベースリンクＢＬ、Ｌ１～Ｌ３の各リンクと障害物との組み合わせについて３次元モデルを生成し、生成した３次元モデルに基づいて、垂直多関節ロボットＬＢが障害物に対して干渉が発生するか否かを判定する処理を順次実行する。

【0142】

また、第２実施形態におけるステップＳ６１８では、記憶部１０４からリンクＬ４、Ｌ５の形状データを取得する。そして、Ｌ４、Ｌ５の各リンクと障害物との組み合わせについて３次元モデルを生成し、生成した３次元モデルに基づいて、垂直多関節ロボットＬＢが障害物に対して干渉が発生するか否かを判定する処理を順次実行する。その他の処理は第１実施形態と同様であるため説明を省略する。

【0143】

このように、第２実施形態では、干渉判定装置１０は、特定のジョイントの回転角度を座標軸とするコンフィグレーション空間内に対障害物干渉領域及び対障害物非干渉領域を設定し、対障害物干渉領域及び対障害物非干渉領域の何れにも属さない領域は詳細判定領域とする。そして、特定のジョイントのジョイント値の組み合わせが対障害物干渉領域内にあるか否かを判定することによって、障害物に対して干渉するか否かを判定する。このため、リンクの形状に基づく３次元モデルを生成して障害物に干渉するか否かを判定する場合と比較して、障害物に対して干渉するか否かの判定に要する時間を短縮することができる。

【0144】

なお、上記各実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した干渉判定処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、干渉判定処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

【0145】

また、上記各実施形態では、干渉判定プログラムがストレージ１４又はＲＯＭ１２に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ－ＲＯＭ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

【符号の説明】

【0146】

１０ 干渉判定装置
１４ ストレージ
１５ 入力部
１６ モニタ
１７ 光ディスク駆動装置
１８ 通信インタフェース
１９ バス
２０ 経路探索装置
２２Ａ～２２Ｃ、３０Ａ～３０Ｃ、４２、５０、５４ 自己干渉領域
２６、２８、４４、５２、５６ 自己非干渉領域
３２、３４、３６ 最大可動域
４０ 詳細判定領域
６０ 障害物
１０１ 算出部
１０２ 取得部
１０３ 判定部
１０４ 記憶部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】

【図22】

【図23】

【図24】