(57)【要約】  教示者（３０１）が可動ロボット（３０２）を直接動かして動作を教示する直接教示が可能な可動ロボット（３０２）の可動部（５０８）の移動経路を探索する移動経路探索装置（１００）であって、直接教示中に可動ロボット（３０２）の可動部（５０８）が掃引する空間を特定する第１の空間特定部（１３１）と、直接教示中に教示者（３０１）の身体の少なくとも一部と可動ロボット（３０２）に取り付けられた空間教示用治具（３０８）とのうちの少なくとも一方が掃引する空間を特定する第２の空間特定部（１３２）と、第１の空間特定部（１３１）によって特定された空間と第２の空間特定部（１３２）によって特定された空間との和集合を移動可能空間として算出する空間合成部（１０６）と、空間合成部（１０６）によって算出された移動可能空間内において、可動部（５０８）の移動経路を探索する経路探索部（１０７）とを備える。
2012014409.tif 000002
【特許請求の範囲】
【請求項１】
教示者が可動ロボットを直接動かして動作を教示する直接教示が可能な可動ロボットの可動部の移動経路を探索する移動経路探索装置であって、
直接教示中に可動ロボットの可動部が掃引する空間を特定する第１の空間特定部と、
直接教示中に教示者の身体の少なくとも一部と前記可動ロボットに取り付けられた空間教示用治具とのうちの少なくとも一方が掃引する空間を特定する第２の空間特定部と、
前記第１の空間特定部によって特定された空間と前記第２の空間特定部によって特定された空間との和集合を移動可能空間として算出する空間合成部と、
前記空間合成部によって算出された移動可能空間内において、前記可動部の移動経路を探索する経路探索部とを備える
移動経路探索装置。
【請求項２】
前記第２の空間特定部は、前記教示者の身体の少なくとも一部の三次元位置に基づいて前記教示者の身体の少なくとも一部の形状を有する三次元モデルを三次元空間に仮想的に配置し、前記三次元空間において前記三次元モデルが占める領域を特定することにより、前記教示者の身体の少なくとも一部が掃引する空間を特定する
請求項１に記載の移動経路探索装置。
【請求項３】
前記可動ロボットは、直接教示中に前記教示者が把持する把持部を有し、
前記第２の空間特定部は、前記教示者の肘の三次元位置と前記把持部の三次元位置とに基づいて、前記教示者の下腕の形状を有する前記三次元モデルを前記三次元空間に仮想的に配置し、前記三次元空間において前記三次元モデルが占める領域を特定することにより、前記教示者の身体の少なくとも一部が掃引する空間を特定する
請求項２に記載の移動経路探索装置。
【請求項４】
前記三次元モデルは円柱形状を有する
請求項３に記載の移動経路探索装置。
【請求項５】
前記移動経路探索装置は、さらに、
前記把持部の三次元位置と前記教示者の肘の三次元位置との距離に基づいて、前記三次元モデルが有する円柱形状の高さを取得する教示者形状取得部を備え、
前記第２の空間特定部は、前記教示者形状取得部によって取得された高さの円柱形状を有する前記三次元モデルを前記三次元空間に仮想的に配置する
請求項４に記載の移動経路探索装置。
【請求項６】
前記移動経路探索装置は、さらに、
前記教示者の左肘の三次元位置と前記把持部の三次元位置との距離、および前記教示者の右肘の三次元位置と前記把持部の三次元位置との距離のそれぞれを、予め定められた閾値と比較することにより、前記教示者が直接教示のために使用している下腕を判定する教示者形状判定部を備え、
前記第２の空間特定部は、前記教示者形状判定部によって使用していると判定された下腕が掃引する空間を特定する
請求項３に記載の移動経路探索装置。
【請求項７】
前記移動経路探索装置は、さらに、
前記教示者の身体の少なくとも一部の形状を示す形状データを保持している教示者形状データベースを備え、
前記第２の空間特定部は、前記教示者形状データベースに保持されている形状データが示す形状を有する前記三次元モデルを前記三次元空間に仮想的に配置する
請求項２に記載の移動経路探索装置。
【請求項８】
前記移動経路探索装置は、さらに、
前記教示者の身体の少なくとも一部の三次元位置を検出する教示者位置検出部を備え、
前記第２の空間特定部は、前記教示者位置検出部によって検出された三次元位置に基づいて、前記三次元モデルを三次元空間に仮想的に配置する
請求項２に記載の移動経路探索装置。
【請求項９】
前記第２の空間特定部は、前記空間教示用治具が取り付けられた、前記可動ロボットの部位に基づいて、前記空間教示用治具の形状を有する三次元モデルを三次元空間に仮想的に配置し、前記三次元空間において前記三次元モデルが占める領域を特定することにより、前記空間教示用治具が掃引する空間を特定する
請求項１〜８のいずれか１項に記載の移動経路探索装置。
【請求項１０】
前記移動経路探索装置は、さらに、
前記空間教示用治具が取り付けられた、前記可動ロボットの部位を検出する治具検出部を備え、
前記第２の空間特定部は、前記治具検出部によって検出された部位に基づいて前記三次元モデルを三次元空間に仮想的に配置する
請求項９に記載の移動経路探索装置。
【請求項１１】
前記移動経路探索装置は、さらに、
複数の空間教示用治具の形状をそれぞれ示す複数の形状データの各々を識別子に対応づけて保持している治具形状データベースを備え、
前記治具検出部は、さらに、前記空間教示用治具の識別子を検出し、
前記第２の空間特定部は、前記治具検出部によって検出された識別子に対応する形状データを前記治具形状データベースから読み出し、読み出した前記形状データが示す形状を有する前記三次元モデルを三次元空間に仮想的に配置する
請求項１０に記載の移動経路探索装置。
【請求項１２】
前記移動経路探索装置は、さらに、
直接教示が終了した後に前記空間教示用治具が前記可動ロボットに取り付けられている場合に、前記空間教示用治具を前記可動ロボットから取り外す必要があることを通知する通知部を備える
請求項９〜１１のいずれか１項に記載の移動経路探索装置。
【請求項１３】
教示者が可動ロボットを直接動かして動作を教示する直接教示が可能な可動ロボットの可動部の移動経路を探索する移動経路探索方法であって、
直接教示中に可動ロボットの可動部が掃引する空間を特定する第１の空間特定ステップと、
直接教示中に教示者の身体の少なくとも一部と前記可動ロボットに取り付けられた空間教示用治具とのうちの少なくとも一方が掃引する空間を特定する第２の空間特定ステップと、
前記第１の空間特定ステップにおいて特定された空間と前記第２の空間特定ステップにおいて特定された空間との和集合を移動可能空間として算出する空間合成ステップと、
前記空間合成ステップにおいて算出された移動可能空間内において、前記可動部の移動経路を探索する経路探索ステップとを含む
移動経路探索方法。
【請求項１４】
請求項１３に記載の移動経路探索方法をコンピュータに実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、直接教示が可能な可動ロボットの可動部の移動経路を探索する移動経路探索装置および移動経路探索方法に関する。
【背景技術】
【０００２】
従来の可動ロボットに組立て加工作業などの所望の作業を実行させるためには、まず教示者が、ティーチングペンダントあるいは力センサなどの操作デバイスを用いて動作を教示する必要がある。【０００３】
可動ロボットは、自由移動を阻む多くの複雑な形状の障害物（例えば、組立て加工の対象物を固定する治具、あるいは可動ロボットを固定するベース台など）に囲まれて動作するのが一般的である。そこで、教示者は、可動ロボットがそれらの障害物に干渉しないように、可動ロボットの姿勢を変形させながら、通過すべき点、または、とるべき姿勢を教示していく。【０００４】
このように教示された通過すべき点を可動ロボットが正確に追従するだけでは、可動ロボットの動作に無駄が多く、自動作業のサイクルタイムが長くなる場合がある。このような場合、教示者は、低速で可動ロボットを実際に動作させることによって、可動ロボットが通過すべき点あるいは軌道を試行錯誤的に調整して最適化せざるを得ない。しかし、試行錯誤的な調整は、時間コストあるいはケアレスミスによる物損コストが高く、特に教示者の経験が浅い場合はこれらのコストが著しく増大する。【０００５】
そこで、直接教示（以下、「ダイレクトティーチング」という）中に可動ロボットが移動可能な空間を自動取得し、自動動作時に最適な経路を自動探索する方法が試みられている（例えば、非特許文献１参照）。非特許文献１に開示された方法では、ダイレクトティーチング時に可動ロボットが掃引する空間（以下、「掃引空間」ともいう）を移動可能空間として自動取得し、自動取得した空間を最適経路の探索空間として利用する。【０００６】
なお、ダイレクトティーチングとは、教示者が身体（例えば手など）を用いて可動ロボットを直接動かすことにより、可動ロボットに動作を教示することをいう。ダイレクトティーチングが可能な可動ロボットでは、例えば、力センサによって検知された外力に応じて可動部が動くように制御される。【０００７】
また、掃引空間とは、物体が移動することによって形成される空間を示す。つまり、掃引空間とは、物体によって一時的に占有された空間の集合である。したがって、ダイレクトティーチング中における可動ロボットの掃引空間とは、ダイレクトティーチング中に可動ロボットが移動した空間および静止していた空間の集合である。【０００８】
図３４は、従来の移動可能空間取得処理と最適経路探索処理との概要を示す図である。非特許文献１の中で、教示者２０１は、可動ロボット２０２の先端に実装された力センサを利用して、ダイレクトティーチングにより動作を教示する。ダイレクトティーチング中に可動ロボット２０２自体が占有した空間を足し合わせることで移動可能空間２０３が算出される。そして、算出された移動可能空間２０３内において、２点間の移動経路２０４が探索される。つまり、移動可能空間２０３は、探索空間として利用される。可動ロボット２０２は、このように探索された移動経路２０４に従って動作することにより、障害物と干渉しないように自動で作業することが可能となる。
【先行技術文献】
【非特許文献】
【０００９】
【非特許文献１】前田他「空間掃引を用いた産業用マニピュレータの教示」、日本機械学会論文集（Ｃ編）、７４巻７３７号（２００８−１）
【発明の概要】
【発明が解決しようとする課題】
【００１０】
しかしながら、非特許文献１の方法では、算出された移動可能空間のボリュームが、実際に可動ロボットが移動可能な空間のボリュームよりも小さくなることが多い。したがって、探索された移動経路の最適性が低いという問題がある。【００１１】
十分なボリュームの移動可能空間を得るためには、例えば、教示者は、移動可能空間を拡張するような動作を可動ロボットにさらに教示することが考えられる。しかしながら、教示者は、どのような動作を教示すれば移動可能空間を拡張することができるかを理解することが難しい。したがって、教示者は、移動可能空間を拡張するために試行錯誤的な教示を繰返すことになり、教示コストの増加につながるという問題がある。【００１２】
そこで、本発明は、上記課題を考慮してなされたものであって、可動ロボットの移動経路を探索するための移動可能空間を自動的に拡張することにより、教示者の負荷の増加を抑制しつつ、より最適な移動経路を探索することができる移動経路探索装置および移動経路探索方法を提供することを目的とする。
【課題を解決するための手段】
【００１３】
上記従来の課題を解決するために、本発明の一態様に係る移動経路探索装置は、教示者が可動ロボットを直接動かして動作を教示する直接教示が可能な可動ロボットの可動部の移動経路を探索する移動経路探索装置であって、直接教示中に可動ロボットの可動部が掃引する空間を特定する第１の空間特定部と、直接教示中に教示者の身体の少なくとも一部と前記可動ロボットに取り付けられた空間教示用治具とのうちの少なくとも一方が掃引する空間を特定する第２の空間特定部と、前記第１の空間特定部によって特定された空間と前記第２の空間特定部によって特定された空間との和集合を移動可能空間として算出する空間合成部と、前記空間合成部によって算出された移動可能空間内において、前記可動部の移動経路を探索する経路探索部とを備える。【００１４】
本構成によれば、直接教示中に可動ロボットが掃引する空間と、教示者の身体の少なくとも一部および空間教示用治具とのうちの少なくとも一方が掃引する空間との和集合を、移動経路を探索するときの移動可能空間として算出することができる。つまり、直接教示中に可動ロボットが掃引しない空間も移動可能空間に加えることが可能となる。したがって、可動ロボットが掃引する空間のみを移動可能空間とする場合よりも拡張された移動可能空間を得ることができるので、より最適な移動経路を探索することが可能となる。また、直接教示中に、教示者の身体と空間教示用治具とのうちの少なくとも一方が掃引する空間を用いて移動可能空間を算出することができるので、教示者は、移動可能空間を拡張するためだけに試行錯誤的に教示を繰り返す必要がない。したがって、教示者の負荷の増加も抑制することが可能となる。
【発明の効果】
【００１５】
本発明によれば、可動ロボットの移動経路を探索するための移動可能空間を自動的に拡張することができる。その結果、教示者の負荷の増加を抑制しつつ、より最適な移動経路を探索することができる。
【図面の簡単な説明】
【００１６】
【図１】図１は、本発明の実施の形態１における移動経路探索装置の基本的な機能構成を示すブロック図である。
【図２】図２は、本発明の実施の形態１における移動経路探索装置の利用シーンを示す図である。
【図３】図３は、本発明の実施の形態１における三次元空間を表現するボクセルを説明するための図である。
【図４】図４は、本発明の実施の形態１における可動ロボットの外観図である。
【図５】図５は、本発明の実施の形態１における移動経路探索処理の流れを示すフローチャートである。
【図６】図６は、本発明の実施の形態１における移動空間特定部による内部判定処理を説明するための図である。
【図７】図７は、本発明の実施の形態１における移動空間特定部によって特定された空間の一例を示す図である。
【図８】図８は、本発明の実施の形態１における教示者形状データベースが保持している教示者形状データの一例を示す図である。
【図９】図９は、本発明の実施の形態１における人占有空間特定部による内部判定処理を説明するための図である。
【図１０】図１０は、本発明の実施の形態１における人占有空間特定部によって特定された空間の一例を示す図である。
【図１１】図１１は、本発明の実施の形態１における空間合成部によって算出される移動可能空間を説明するための図である。
【図１２】図１２は、本発明の実施の形態１における空間合成部によって算出される移動可能空間を説明するための図である。
【図１３】図１３は、探索条件の一例を説明するための図である。
【図１４】図１４は、従来の方法によって探索される移動経路の一例を示す図である。
【図１５】図１５は、本発明の実施の形態１における移動経路探索装置によって探索される移動経路の一例を示す図である。
【図１６】図１６は、本発明の実施の形態２における移動経路探索装置の基本的な機能構成を示すブロック図である。
【図１７】図１７は、本発明の実施の形態２における教示者形状データベースが保持している教示者形状データの一例を示す図である。
【図１８】図１８は、本発明の実施の形態３における移動経路探索装置の基本的な機能構成を示すブロック図である。
【図１９】図１９は、本発明の実施の形態３における移動経路探索装置の利用シーンを示す図である。
【図２０】図２０は、本発明の実施の形態３における教示者形状データベースが保持している教示者形状データの一例を示す図である。
【図２１】図２１は、本発明の実施の形態３における教示者の教示姿勢を説明するための図である。
【図２２】図２２は、本発明の実施の形態４における移動経路探索装置の基本的な機能構成を示すブロック図である。
【図２３】図２３は、本発明の実施の形態４における可動ロボットおよび空間教示用治具の外観図である。
【図２４】図２４は、本発明の実施の形態４における移動経路探索処理の流れを示すフローチャートである。
【図２５】図２５は、本発明の実施の形態４における移動空間特定部による内部判定処理を説明するための図である。
【図２６】図２６は、本発明の実施の形態４における治具占有空間特定部による内部判定処理を説明するための図である。
【図２７】図２７は、本発明の実施の形態４における空間合成部によって算出される移動可能空間を説明するための図である。
【図２８】図２８は、本発明の実施の形態４における空間合成部によって算出される移動可能空間を説明するための図である。
【図２９】図２９は、従来の方法によって探索される移動経路の一例を示す図である。
【図３０】図３０は、本発明の実施の形態４における移動経路探索装置によって探索される移動経路の一例を示す図である。
【図３１】図３１は、本発明の実施の形態４の変形例における移動経路探索装置の基本的な機能構成を示すブロック図である。
【図３２】図３２は、本発明の一態様に係る移動経路探索装置の機能構成を示すブロック図である。
【図３３】図３３は、本発明の一態様に係る移動可能空間出力装置の機能構成を示すブロック図である。
【図３４】図３４は、従来の移動可能空間取得処理と最適経路探索処理との概要を示す図である。
【発明を実施するための形態】
【００１７】
本発明の一態様に係る移動経路探索装置は、教示者が可動ロボットを直接動かして動作を教示する直接教示が可能な可動ロボットの可動部の移動経路を探索する移動経路探索装置であって、直接教示中に可動ロボットの可動部が掃引する空間を特定する第１の空間特定部と、直接教示中に教示者の身体の少なくとも一部と前記可動ロボットに取り付けられた空間教示用治具とのうちの少なくとも一方が掃引する空間を特定する第２の空間特定部と、前記第１の空間特定部によって特定された空間と前記第２の空間特定部によって特定された空間との和集合を移動可能空間として算出する空間合成部と、前記空間合成部によって算出された移動可能空間内において、前記可動部の移動経路を探索する経路探索部とを備える。
【００１８】
この構成によれば、直接教示中に可動ロボットが掃引する空間と、教示者の身体の少なくとも一部および空間教示用治具のうちの少なくとも一方が掃引する空間との和集合を、移動経路を探索するときの移動可能空間として算出することができる。つまり、直接教示中に可動ロボットが掃引しない空間も移動可能空間に加えることが可能となる。したがって、可動ロボットが掃引する空間のみを移動可能空間とする場合よりも拡張された移動可能空間を得ることができるので、より最適な移動経路を探索することが可能となる。また、直接教示中に、教示者の身体の少なくとも一部と空間教示用治具とのうちの少なくとも一方が掃引する空間を用いて移動可能空間を算出することができるので、教示者は、移動可能空間を拡張するために試行錯誤的に教示を繰り返す必要がない。したがって、教示者の負荷の増加も抑制することが可能となる。
【００１９】
また、前記第２の空間特定部は、前記教示者の身体の少なくとも一部の三次元位置に基づいて前記教示者の身体の少なくとも一部の形状を有する三次元モデルを三次元空間に仮想的に配置し、前記三次元空間において前記三次元モデルが占める領域を特定することにより、前記教示者の身体の少なくとも一部が掃引する空間を特定することが好ましい。
【００２０】
この構成によれば、教示者の身体の少なくとも一部の三次元位置に基づいて、教示者の身体の少なくとも一部が掃引する空間を特定することができる。したがって、比較的簡易な構成で、教示者の身体の少なくとも一部が掃引する空間を特定することが可能となる。
【００２１】
また、前記可動ロボットは、直接教示中に前記教示者が把持する把持部を有し、前記第２の空間特定部は、前記教示者の肘の三次元位置と前記把持部の三次元位置とに基づいて、前記教示者の下腕の形状を有する前記三次元モデルを前記三次元空間に仮想的に配置し、前記三次元空間において前記三次元モデルが占める領域を特定することにより、前記教示者の身体の少なくとも一部が掃引する空間を特定することが好ましい。
【００２２】
この構成によれば、可動ロボットの把持部の三次元位置を利用することにより、教示者の下腕が掃引する空間をより高精度に特定することが可能となる。
【００２３】
また、前記三次元モデルは円柱形状を有することが好ましい。
【００２４】
この構成によれば、教示者の下腕の形状を円柱形状で近似することができ、教示者の下腕が掃引する空間を特定する際の処理負荷の軽減を図ることが可能となる。
【００２５】
また、前記移動経路探索装置は、さらに、前記把持部の三次元位置と前記教示者の肘の三次元位置との距離に基づいて、前記三次元モデルが有する円柱形状の高さを取得する教示者形状取得部を備え、前記第２の空間特定部は、前記教示者形状取得部によって取得された高さの円柱形状を有する前記三次元モデルを前記三次元空間に仮想的に配置することが好ましい。
【００２６】
この構成によれば、把持部の三次元位置と教示者の肘の三次元位置との距離に基づいて円柱形状の高さを取得することができるので、円柱形状と教示者の腕の形状との差異を減少させることができる。したがって、教示者の下腕が掃引する空間をより高精度に特定することが可能となる。また、教示者が下腕の長さを直接計測する必要がないので、教示者の負荷の増加を抑制することができる。
【００２７】
また、前記移動経路探索装置は、さらに、前記教示者の左肘の三次元位置と前記把持部の三次元位置との距離、および前記教示者の右肘の三次元位置と前記把持部の三次元位置との距離のそれぞれを、予め定められた閾値と比較することにより、前記教示者が直接教示のために使用している下腕を判定する教示者形状判定部を備え、前記第２の空間特定部は、前記教示者形状判定部によって使用していると判定された下腕が掃引する空間を特定することが好ましい。
【００２８】
この構成によれば、左右の肘の三次元位置に基づいて、直接教示中に利用されている下腕を判定することができる。したがって、教示者が教示中に使用していない側の肘の三次元位置に基づいて教示者の下腕が掃引する空間を特定することを回避することができる。その結果、教示者の下腕が掃引する空間をより高精度に特定することが可能となる。
【００２９】
また、前記移動経路探索装置は、さらに、前記教示者の身体の少なくとも一部の形状を示す形状データを保持している教示者形状データベースを備え、前記第２の空間特定部は、前記教示者形状データベースに保持されている形状データが示す形状を有する前記三次元モデルを前記三次元空間に配置することが好ましい。
【００３０】
この構成によれば、教示者形状データベースに保持されている形状データが示す形状に基づいて三次元モデルを形成することができる。したがって、より高精度に教示者の身体の少なくとも一部が掃引する空間を特定することが可能となる。
【００３１】
また、前記移動経路探索装置は、さらに、前記教示者の身体の少なくとも一部の三次元位置を検出する教示者位置検出部を備え、前記第２の空間特定部は、前記教示者位置検出部によって検出された三次元位置に基づいて、前記三次元モデルを三次元空間に仮想的に配置することが好ましい。
【００３２】
この構成によれば、検出された三次元位置に基づいて三次元モデルを三次元空間に仮想的に配置することができる。したがって、より高精度に教示者の身体の少なくとも一部が掃引する空間を特定することが可能となる。
【００３３】
また、前記第２の空間特定部は、前記空間教示用治具が取り付けられた、前記可動ロボットの部位に基づいて、前記空間教示用治具の形状を有する三次元モデルを三次元空間に仮想的に配置し、前記三次元空間において前記三次元モデルが占める領域を特定することにより、前記空間教示用治具が掃引する空間を特定することが好ましい。
【００３４】
この構成によれば、空間教示用治具が取り付けられた部位に基づいて、空間教示用治具が掃引する空間を特定することができる。したがって、比較的簡易な構成で、空間教示用治具が掃引する空間を特定することが可能となる。
【００３５】
また、前記移動経路探索装置は、さらに、前記空間教示用治具が取り付けられた、前記可動ロボットの部位を検出する治具検出部を備え、前記第２の空間特定部は、前記治具検出部によって検出された部位に基づいて前記三次元モデルを三次元空間に仮想的に配置することが好ましい。
【００３６】
この構成によれば、検出された可動ロボットの部位に基づいて、三次元モデルを三次元空間に仮想的に配置することができる。したがって、教示者が手動で可動ロボットの部位を入力する必要がないので、教示者の負荷を軽減することが可能となる。
【００３７】
また、前記移動経路探索装置は、さらに、複数の空間教示用治具の形状をそれぞれ示す複数の形状データの各々を識別子に対応づけて保持している治具形状データベースを備え、前記治具検出部は、さらに、前記空間教示用治具の識別子を検出し、前記第２の空間特定部は、前記治具検出部によって検出された識別子に対応する形状データを前記治具形状データベースから読み出し、読み出した前記形状データが示す形状を有する前記三次元モデルを三次元空間に仮想的に配置することが好ましい。
【００３８】
この構成によれば、複数の空間教示用治具を用いて直接教示を行う場合であっても、教示者が手動で空間教示用治具の識別子を入力する必要がないので、教示者の負荷を軽減することが可能となる。
【００３９】
また、前記移動経路探索装置は、さらに、直接教示が終了した後に前記空間教示用治具が前記可動ロボットに取り付けられている場合に、前記空間教示用治具を前記可動ロボットから取り外す必要があることを通知する通知部を備えることが好ましい。
【００４０】
この構成によれば、直接教示が終了した後に、空間教示用治具を可動ロボットから取り外す必要があることを教示者あるいは作業者などに通知することができる。したがって、空間教示用治具が可動ロボットに取り付けられた状態で、作業が開始されることを抑制することができ、作業の安全性を向上させることが可能となる。
【００４１】
以下、本発明の実施の形態について、図面を参照しながら説明する。
【００４２】
（実施の形態１）
図１は、本発明の実施の形態１における移動経路探索装置１００の基本的な機能構成を示すブロック図である。図１に示すように、移動経路探索装置１００は、ロボット形状データベース１０１と、移動空間特定部１０２と、教示者位置検出部１０３と、教示者形状データベース１０４と、人占有空間特定部１０５と、空間合成部１０６と、経路探索部１０７とを備える。
【００４３】
ロボット形状データベース１０１は、自動作業を行う可動ロボット３０２の形状を示すロボット形状データを保持している。
【００４４】
移動空間特定部１０２は、第１の空間特定部に相当する。移動空間特定部１０２は、ダイレクトティーチング中に可動ロボット３０２の可動部が掃引する空間を特定する。
【００４５】
具体的には、移動空間特定部１０２は、可動ロボット３０２の姿勢情報とロボット形状データベース１０１に保持されているロボット形状データとに基づいて、ダイレクトティーチング中の各タイミングにおいて可動ロボット３０２が占有している空間を特定することにより、ダイレクトティーチング中に可動ロボット３０２の可動部が掃引する空間を特定する。
【００４６】
教示者位置検出部１０３は、教示者３０１の身体の少なくとも一部の三次元位置を検出する。本実施の形態では、教示者位置検出部１０３は、可動ロボット３０２に動作を教示する教示者３０１の肘の三次元位置を検出する。なお、教示者位置検出部１０３は、必ずしも肘の三次元位置を検出する必要はなく、例えば肩あるいは手首などの関節部位の三次元位置を検出してもよい。また、教示者位置検出部１０３は、肘の三次元位置に加えて、例えば肩あるいは手首などの三次元位置を検出してもよい。
【００４７】
教示者形状データベース１０４は、教示者３０１の身体の少なくとも一部の形状を示す教示者形状データを保持している。本実施の形態では、教示者形状データベース１０４は、教示者３０１の下腕の形状を示す教示者形状データを保持している。なお、教示者形状データベース１０４は、さらに、教示者３０１の上腕の形状を示す教示者形状データを保持してもよい。またさらに、教示者形状データベース１０４は、教示者３０１の全身の形状を示す教示者形状データを保持してもよい。
【００４８】
人占有空間特定部１０５は、第２の空間特定部に相当する。人占有空間特定部１０５は、ダイレクトティーチング中に教示者３０１の身体の少なくとも一部が掃引する空間を特定する。本実施の形態では、人占有空間特定部１０５は、ダイレクトティーチング中に教示者３０１の下腕が掃引する空間を特定する。
【００４９】
具体的には、人占有空間特定部１０５は、教示者位置検出部１０３によって検出される教示者３０１の肘の三次元位置と、教示者形状データベース１０４に保持されている教示者形状データとに基づいて、ダイレクトティーチング中の各タイミングにおいて教示者３０１の下腕が占有している空間を特定する。そして、人占有空間特定部１０５は、特定した空間の和集合を、教示者３０１の下腕が掃引する空間として特定する。
【００５０】
なお、人占有空間特定部１０５は、必ずしも教示者３０１の下腕が掃引する空間を特定する必要はない。例えば、人占有空間特定部１０５は、教示者３０１の腕全体が掃引する空間を特定してもよい。
【００５１】
空間合成部１０６は、移動空間特定部１０２と人占有空間特定部１０５とが出力する空間の和集合を可動ロボット３０２の移動可能空間として算出する。
【００５２】
経路探索部１０７は、空間合成部１０６によって算出された移動可能空間内において、可動ロボット３０２の可動部の移動経路を探索する。具体的には、経路探索部１０７は、与えられた探索条件に基づいて、移動可能空間内において取りうる移動経路のうち最適な移動経路を探索する。ここで、探索条件とは、例えば、始点および終点の位置、最適性の評価基準（時間優先または省電力優先など）、または制約条件などである。
【００５３】
次に、移動経路探索装置１００の利用シーンについて説明する。
【００５４】
図２は、本発明の実施の形態１における移動経路探索装置１００の利用シーンを示す図である。図２では、教示者３０１は、可動ロボット３０２の一部である把持部を手で把持している。そして、教示者３０１は、可動ロボット３０２の可動部を障害物３０６と接触しないように始点３０５から終点３０７まで直接動かすことにより、可動ロボット３０２に動作を教示している。
【００５５】
教示者３０１の肘には、三次元位置計測デバイス３０３が取り付けられている。また、作業環境内（例えば可動ロボット３０２が設置された部屋の天井あるいは壁など）には、位置センシングデバイス３０４が設置される。三次元位置計測デバイス３０３と位置センシングデバイス３０４とは、例えばＵＷＢ（Ｕｌｔｒａ  Ｗｉｄｅ  Ｂａｎｄ）を利用した公知の屋内ＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）などを実現する。つまり、教示者位置検出部１０３は、三次元位置計測デバイス３０３と位置センシングデバイス３０４とを備え、三次元位置計測デバイス３０３が取り付けられた、教示者３０１の身体の一部の三次元位置を検出する。
【００５６】
ここで、三次元モデルが仮想的に配置される三次元空間について説明する。三次元空間の閉領域は、ボクセル（Ｖｏｘｅｌ）で表現される。ボクセルは、図３に示すように一定の大きさの立方体を基本単位とする。原点からの三次元座標位置が（ｘ，ｙ，ｚ）であるボクセルをＶ（ｘ，ｙ，ｚ）と表現する。各ボクセルＶ（ｘ，ｙ，ｚ）は可動ロボット３０２が移動可能かどうか示す論理値Ｍ（ｘ，ｙ，ｚ）を持つ。
【００５７】
Ｖ（ｘ，ｙ，ｚ）の内部に可動ロボット３０２の移動を妨げる障害物があるか否かが不明な場合は、論理値Ｍ（ｘ，ｙ，ｚ）は、以下の式（１）のように表される。
【００５８】
Ｍ（ｘ，ｙ，ｚ）＝ｆａｌｓｅ（偽）      式（１）
【００５９】
また、Ｖ（ｘ，ｙ，ｚ）の内部に可動ロボット３０２の移動を妨げる障害物が無く、可動ロボット３０２がＶ（ｘ，ｙ，ｚ）に進入可能な場合は、論理値Ｍ（ｘ，ｙ，ｚ）は、以下の式（２）のように表される。
【００６０】
Ｍ（ｘ，ｙ，ｚ）＝ｔｒｕｅ（真）        式（２）
【００６１】
図３の例ではハッチングされているボクセルの論理値はｆａｌｓｅであり、ハッチングされていない白のボクセルの論理値はｔｒｕｅである。すなわち、障害物との干渉を避けるためには、可動ロボット３０２は、ハッチングされている空間への進入を避けながら、ハッチングされていない空間を移動しなければならない。以下の式（３）のように、移動可能空間は、論理値がｔｒｕｅであるボクセルの集合として表現される。
【００６２】
∪｛Ｖ（ｘ，ｙ，ｚ）｜Ｍ（ｘ，ｙ，ｚ）＝ｔｒｕｅ｝      式（３）
【００６３】
このように、障害物に接触するか否かに関わらず可動ロボット３０２が到達可能な三次元空間は、ボクセルによって表現される。なお、教示開始前において、すべてのボクセルの論理値はｆａｌｓｅに設定される。すなわち、教示開始前には、可動ロボット３０２が到達可能な空間のすべては、移動可能かどうか不明である。
【００６４】
次に、本実施の形態における可動ロボット３０２の詳細を説明する。
【００６５】
図４は、本発明の実施の形態１における可動ロボット３０２の外観図である。可動ロボット３０２は、例えば６軸の垂直多関節ロボットである。図４の（ａ）に示すように、本実施の形態では、可動ロボット３０２の物理的機構構造は、Ｒ−Ｐ−Ｐ−Ｒ−Ｐ−Ｒ（Ｒは回転、Ｐは並進）であるとする。
【００６６】
ここで、６個の回転軸５０１〜５０６の角度の組み合わせを（ＪＴ１，ＪＴ２，ＪＴ３，ＪＴ４，ＪＴ５，ＪＴ６）で表現する。この６個の回転軸５０１〜５０６の角度の組み合わせによって、可動ロボット３０２の姿勢は一意に決定される。
【００６７】
また、ロボット形状データベース１０１は、図４に示すロボットの形状を線分あるいは曲線分で閉領域表現したロボット形状データを保持している。したがって、（ＪＴ１，ＪＴ２，ＪＴ３，ＪＴ４，ＪＴ５，ＪＴ６）が与えられれば、移動空間特定部１０２は、ロボット形状データを利用して、固定部５０７に対する可動部５０８の相対的な三次元位置を一意に算出することができる。
【００６８】
例えば、図４の（ｂ）に示すように、ＪＴ１＝ＪＴ２＝ＪＴ３＝ＪＴ４＝ＪＴ５＝ＪＴ６＝０［ｄｅｇ］の場合に可動ロボット３０２が直立姿勢になるようにキャリブレーションされているとする。このとき、ＪＴ１＝ＪＴ２＝ＪＴ４＝ＪＴ６＝０［ｄｅｇ］かつＪＴ３＝ＪＴ５＝−９０［ｄｅｇ］の場合は、可動ロボット３０２は、図４の（ｃ）に示すような姿勢となる。
【００６９】
教示者３０１は、ダイレクトティーチングの際に、可動ロボット３０２の把持部５０９を握り、可動ロボット３０２の内部に実装された力センサに対して力を加えることにより、可動ロボット３０２の姿勢を変形させることができる。
【００７０】
なお、可動ロボット３０２は、Ｒ−Ｐ−Ｐ−Ｒ−Ｐ−Ｒ以外の機構構造を有する垂直多関節ロボットであってもよい。あるいは、可動ロボット３０２は、水平多関節ロボットであってもよい。また、可動ロボット３０２は、単軸ロボットの組合せであってもよい。
【００７１】
次に、以上のように構成された移動経路探索装置１００における各種動作について説明する。
【００７２】
図５は、本発明の実施の形態１における移動経路探索処理の流れを示すフローチャートである。以下、教示者３０１が、可動ロボット３０２に対して、障害物３０６を避けながら始点３０５から終点３０７へ移動する動作をダイレクトティーチングする例について説明する。
【００７３】
教示者３０１が可動ロボット３０２の把持部５０９を握りながらダイレクトティーチングを行うと、各回転軸の角度を示す値（ＪＴ１，ＪＴ２，ＪＴ３，ＪＴ４，ＪＴ５，ＪＴ６）が変化して可動ロボット３０２は変形する。
【００７４】
このとき、移動空間特定部１０２は、可動ロボット３０２の可動部５０８が占有している空間を特定する（Ｓ６０１）。具体的には、まず、移動空間特定部１０２は、一定周期Ｔで可動ロボット３０２の各回転軸の角度を示す値を姿勢情報として可動ロボット３０２から取得する。続いて、移動空間特定部１０２は、ロボット形状データベース１０１に保持されているロボット形状データを取得する。そして、移動空間特定部１０２は、取得した姿勢情報とロボット形状データとに基づいて、空間を構成する各ボクセルが可動ロボット３０２の内部に位置するか否かの内部判定処理を行う。ここでの内部判定処理とは、あるボクセルを構成するすべての点が、ロボット形状データが示す閉領域に含まれる（以下、「ボクセルが可動ロボット３０２に内包される」という）かどうかを、既知のｐｏｉｎｔ  ｉｎ  ｐｏｌｙｇｏｎ  ｔｅｓｔなどの手法により判定する処理をいう。
【００７５】
ここで、ボクセルが可動ロボット３０２に内包される場合は、移動空間特定部１０２は、そのボクセルは移動可能なボクセルであると判断し、そのボクセルの論理値をｔｒｕｅと設定する。一方、ボクセルが可動ロボット３０２に内包されない場合、移動空間特定部１０２は、そのボクセルに設定されている論理値を変更しない。移動空間特定部１０２は、このような内部判定処理によって論理値がｔｒｕｅと設定されたボクセルの集合によって構成される三次元空間を、可動ロボット３０２の可動部５０８が占有している空間として特定する。
【００７６】
ここで図６および図７を用いて、移動空間特定部１０２による処理についてさらに詳細に説明する。
【００７７】
図６は、本発明の実施の形態１における移動空間特定部１０２による内部判定処理を説明するための図である。また、図７は、本発明の実施の形態１における移動空間特定部１０２によって特定された空間の一例を示す図である。
【００７８】
図６では、説明を簡単にするために、二次元平面に射影したときの、各ボクセルと可動ロボット３０２の一部との位置関係を示している。図６に示すように、可動ロボット３０２に内包されるボクセル７０２の論理値がｔｒｕｅと設定される。つまり、一部でも可動ロボット３０２の外側に位置するボクセル７０１の論理値は変更されない。
【００７９】
ここで、移動空間特定部１０２が移動可能と判定したボクセルの集合Ｒは、以下の式（４）によって算出される。
【００８０】
Ｒ＝∪｛Ｖ（ｘ，ｙ，ｚ）｜Ｍ（ｘ，ｙ，ｚ）＝ｔｒｕｅ｝  式（４）
【００８１】
移動空間特定部１０２は、このボクセルの集合Ｒを可動ロボット３０２に占有されている空間として特定する。ダイレクトティーチング中の各タイミングに対して算出されるボクセルの集合Ｒの和が、図７に示すような、可動ロボット３０２の掃引空間８０１に相当する。以下、ボクセルの集合Ｒを、適宜、ロボット掃引空間Ｒと記載する。
【００８２】
図５のフローチャートの説明に戻る。
【００８３】
教示者位置検出部１０３は、三次元位置計測デバイス３０３と位置センシングデバイス３０４とを利用して、教示者３０１の肘の三次元位置Ｇ＿ｈを一定周期Ｔごとに検出する（Ｓ６０２）。
【００８４】
続いて、人占有空間特定部１０５は、教示者３０１の下腕が占有している空間を特定する（Ｓ６０３）。具体的には、人占有空間特定部１０５は、教示者３０１の肘の三次元位置に基づいて教示者３０１の下腕の形状を有する三次元モデルを三次元空間に仮想的に配置する。そして、人占有空間特定部１０５は、三次元空間に配置された三次元モデルが占める領域を特定する。
【００８５】
より具体的には、人占有空間特定部１０５は、検出された教示者３０１の肘の三次元位置Ｇ＿ｈと、教示者形状データベースに保持されている教示者形状データとに基づいて、教示者３０１の下腕の形状を有する三次元モデルを三次元空間に仮想的に配置する。
【００８６】
図８を用いて、人占有空間特定部１０５は、教示者形状データベースに保持されている教示者形状データについて説明する。
【００８７】
図８は、本発明の実施の形態における教示者形状データベース１０４が保持している教示者形状データの一例を示す図である。教示者形状データベース１０４は、教示者３０１の下腕の近似形状を示すデータを保持している。下腕の近似形状は、例えば図８に示すように円柱形状９０１で表される。円柱形状９０１は、２つの数値パラメータ（半径ｒおよび高さｈ）によって規定される。本実施の形態では、半径ｒおよび高さｈは、例えば、図示しない入力部を介して教示者３０１によって予め入力された値である。なお、円柱形状９０１は、必ずしも半径ｒおよび高さｈによって規定される必要はない。例えば、円柱形状は、円周長と高さとによって規定されてもよい。
【００８８】
ここで、半径ｒは、教示者３０１の下腕のうち最も外周が短い部分の半径であることが好ましい。これにより、教示者３０１の下腕を過大にモデル化することによって、障害物が位置する空間が移動可能空間と算出されることを抑制することができる。
【００８９】
なお、教示者３０１の下腕の形状は、必ずしも円柱形状で表される必要はなく、より実際の下腕の形状に近い形状で表されてもよい。ただし、三次元モデルの形状が複雑になるほど内部判定処理の負荷が増大する。したがって、三次元モデルの形状は、本実施の形態のように、比較的単純であることが好ましい。
【００９０】
人占有空間特定部１０５は、このような教示者形状データが示す円柱形状を有する三次元モデルを三次元空間に仮想的に配置する。より具体的には、人占有空間特定部１０５は、教示者３０１の肘の三次元位置Ｇ＿ｈと、移動空間特定部１０２から得られる可動ロボット３０２の把持部５０９の三次元位置Ｇ＿ｒとを利用して、教示者３０１の下腕の形状を有する三次元モデルを三次元空間に仮想的に配置する。
【００９１】
例えば、人占有空間特定部１０５は、肘の三次元位置Ｇ＿ｈと把持部５０９の三次元位置Ｇ＿ｒとを結ぶ線分の方向と三次元モデルの高さ方向とが一致するように、三次元モデルを三次元空間に仮想的に配置する。このように、可動ロボット３０２の把持部５０９の三次元位置を利用することにより、人占有空間特定部１０５は、教示者３０１の下腕の形状を有する三次元モデルを、より正確に配置することが可能となる。
【００９２】
そして、人占有空間特定部１０５は、三次元空間を構成する各ボクセルが三次元空間に仮想的に配置された教示者３０１の下腕の形状を有する三次元モデルの内部に位置するか否かの内部判定処理を行う。ここでの内部判定処理とは、あるボクセルを構成するすべての点が、教示者３０１の形状を示す三次元モデルによって形成される閉領域に含まれる（以下、「ボクセルが教示者３０１に内包される」という）かどうかを判定する処理をいう。
【００９３】
ここで、ボクセルが教示者３０１に内包される場合には、人占有空間特定部１０５は、そのボクセルは可動ロボット３０２が移動可能なボクセルであると判断し、そのボクセルの論理値をｔｒｕｅと設定する。一方、ボクセルが教示者３０１に内包されない場合、人占有空間特定部１０５は、そのボクセルに設定された論理値を変更しない。人占有空間特定部１０５は、このような内部判定処理によって論理値がｔｒｕｅと設定されたボクセルの集合によって構成される三次元空間を、教示者３０１の下腕が占有している空間として特定する。
【００９４】
図９および図１０を用いて、人占有空間特定部による処理についてさらに詳細に説明する。
【００９５】
図９は、本発明の実施の形態１における人占有空間特定部１０５による内部判定処理を説明するための図である。また、図１０は、本発明の実施の形態１における人占有空間特定部１０５によって特定された空間の一例を示す図である。
【００９６】
図９では、説明を簡単にするために、二次元平面に射影したときの、各ボクセルと教示者３０１の下腕の形状を有する三次元モデル１００１との位置関係を示している。図９に示すように、三次元モデル１００１に内包されるボクセル１００３の論理値がｔｒｕｅと設定される。つまり、一部でも三次元モデル１００１の外側に位置するボクセル１００２の論理値は変更されない。
【００９７】
ここで、人占有空間特定部１０５が移動可能と判定したボクセルの集合Ｈは、以下の式（５）によって算出される。
【００９８】
Ｈ＝∪｛Ｖ（ｘ，ｙ，ｚ）｜Ｍ（ｘ，ｙ，ｚ）＝ｔｒｕｅ｝  式（５）
【００９９】
人占有空間特定部１０５は、このボクセルの集合Ｈを教示者３０１に占有されている空間として特定する。ダイレクトティーチング中の各タイミングに対して算出されるボクセルの集合Ｈの和が、図１０に示すような、教示者３０１の掃引空間１１０１に相当する。以下、ボクセルの集合Ｈを、適宜、教示者掃引空間Ｈと記載する。
【０１００】
図５のフローチャートの説明に戻る。
【０１０１】
教示者３０１が教示終了を指示したか否かが判定される（Ｓ６０４）。ここで、教示終了が指示されない限り（Ｓ６０４のＮｏ）、ステップＳ６０１〜Ｓ６０３の処理が繰り返し実行される。つまり、移動空間特定部１０２は、可動ロボット３０２の可動部５０８がダイレクトティーチング中の各タイミングにおいて占有している空間の特定を繰り返すことにより、図７に示すような、可動ロボット３０２の可動部５０８が掃引した掃引空間８０１を特定する。また、人占有空間特定部１０５は、ダイレクトティーチング中の各タイミングにおいて教示者３０１の身体の少なくとも一部（ここでは下腕）が占有している空間の特定を繰り返すことにより、図１０に示すような、教示者３０１の身体の少なくとも一部が掃引した掃引空間１１０１を特定する。
【０１０２】
教示者３０１が教示終了を指示した場合（Ｓ６０４のＹｅｓ）、空間合成部１０６は、移動空間特定部１０２が出力する移動可能なボクセルの集合Ｒと、人占有空間特定部１０５が出力する移動可能なボクセルの集合Ｈとの和集合Ｒ∪Ｈを、可動ロボット３０２の移動可能空間として算出する（Ｓ６０５）。つまり、空間合成部１０６は、移動空間特定部１０２によって特定された空間と人占有空間特定部１０５によって特定された空間との和集合を移動可能空間として算出する。
【０１０３】
図１１および図１２は、本発明の実施の形態１における空間合成部１０６によって算出される移動可能空間を説明するための図である。図１１に示す移動可能空間１２０１が、可動ロボット３０２の掃引空間８０１と教示者３０１の掃引空間１１０１との和集合Ｒ∪Ｈに相当する。また、図１２では、説明を簡単にするために、二次元平面に射影したときの和集合Ｒ∪Ｈを示している。和集合Ｒ∪Ｈは、白のボクセル１２０３とドットテクスチャのボクセル１２０４とによって形成される領域に相当する。つまり、和集合Ｒ∪Ｈは、ハッチングされたボクセル１２０２以外のボクセルによって形成される領域に相当する。
【０１０４】
図５のフローチャートの説明に戻る。
【０１０５】
最後に、経路探索部１０７は、空間合成部１０６によって算出された移動可能空間内において、可動部５０８の移動経路を探索する（Ｓ６０６）。具体的には、経路探索部１０７は、空間合成部１０６によって算出された移動可能空間を探索空間として移動経路を探索する。その結果、経路探索部１０７は、与えられる探索条件（三次元位置を示す始点および終点、並びに制約条件）に対してより最適な経路を出力することが可能になる。
【０１０６】
ここで、図１３〜図１５を用いて、本実施の形態における経路探索部１０７によって探索される移動経路の一例を、従来の方法によって探索される移動経路と比較しながら説明する。
【０１０７】
図１３は、探索条件の一例を説明するための図である。また、図１４は、従来の方法によって探索される移動経路の一例を示す図である。また、図１５は、本発明の実施の形態１における移動経路探索装置１００によって探索される移動経路の一例を示す図である。なお、図１３〜図１５は、説明を簡単にするため、三次元空間を二次元平面に射影したときの図を示す。
【０１０８】
図１３に示すように、経路探索部１０７は、可動ロボット３０２の可動部５０８の先端を、始点１４０１から終点１４０２まで最短時間で移動させることを探索条件として、移動経路を探索する。
【０１０９】
従来の方法では、ロボット掃引空間Ｒの内部だけで移動経路の探索を行う。したがって、探索結果として得られる移動経路１５０１は、図１４に示すような複雑な曲線経路となる。このような移動経路１５０１に従って可動部５０８の先端を移動させるためには、可動ロボット３０２全体を方向１５０２に移動させてから回転させる必要がある。つまり、従来の方法によって探索される移動経路１５０１は、可動ロボット３０２の動作に無駄が多く最適性が低い。
【０１１０】
しかしながら、本実施の形態における移動経路探索装置１００による経路探索では、図１５に示すとおり、ロボット掃引空間Ｒに加えて教示者掃引空間Ｈを利用して、移動経路を探索することができる。つまり、経路探索部１０７は、ロボット掃引空間Ｒに、空間Ｈ−Ｒ（マイナス記号はＨからＨ∩Ｒである要素を除く意味）を加えた空間を探索に利用することができる。したがって、図１５に示すような、垂直多関節ロボットが最速で移動することができる円弧状の経路が移動経路１６０１として探索される。つまり、本実施の形態における移動経路探索装置１００によって探索される移動経路１６０１に従って動作することにより、可動ロボット３０２は、従来よりも動作の無駄を抑制することができる。すなわち、可動ロボット３０２は、従来よりも短時間で始点と終点との間を移動することが可能になる。つまり、移動経路探索装置１００は、より最適な移動経路を探索することが可能となる。
【０１１１】
以上のように、本実施の形態における移動経路探索装置１００によれば、ダイレクトティーチング中に可動ロボット３０２が掃引する空間と教示者３０１の身体の少なくとも一部が掃引する空間との和集合を、移動経路を探索するときの移動可能空間として算出することができる。つまり、ダイレクトティーチング中に可動ロボット３０２が掃引しない空間も移動可能空間に加えることが可能となる。したがって、可動ロボット３０２が掃引する空間のみを移動可能空間とする場合よりも拡張された移動可能空間を得ることができるので、より最適な移動経路を探索することが可能となる。また、ダイレクトティーチング中に教示者３０１の身体の少なくとも一部が掃引する空間を用いて移動可能空間を算出することができるので、教示者３０１は、移動可能空間を拡張するために試行錯誤的に教示を繰り返す必要がない。したがって、教示者３０１の負荷の増加も抑制することが可能となる。
【０１１２】
（実施の形態２）
次に、本発明の実施の形態２について図面を参照しながら説明する。なお、実施の形態１と同じ部分については説明を省略する。
【０１１３】
図１６は、本発明の実施の形態２における移動経路探索装置１００の基本的な機能構成を示すブロック図である。図１６に示すように、本実施の形態における移動経路探索装置は、実施の形態１における移動経路探索装置が備える構成要素に加えて、教示者形状取得部１７０１を備える。
【０１１４】
教示者形状取得部１７０１は、教示者位置検出部１０３によって検出される教示者の位置を入力として、教示者形状データベース１０４に保持された教示者形状データを変更する。
【０１１５】
教示者形状データベース１０４は、教示者３０１の下腕の形状を示す教示者形状データを保持している。教示者形状データは、例えば図１７に示すように円柱形状１８０１を示す。円柱形状１８０１の大きさは、２つの数値パラメータ（半径ｒ、高さｈ）によって規定される。
【０１１６】
半径ｒは、例えば、図示しない入力部を介して教示者３０１によって予め入力された値である。なお、半径ｒは、例えば、教示者３０１の下腕の外周を測定し、測定された値のうち最も小さい値に決定されることが好ましい。
【０１１７】
高さｈは、教示者３０１が可動ロボット３０２の把持部５０９を握ったときの、把持部５０９の三次元位置Ｇ＿ｒと三次元位置計測デバイス３０３の計測位置Ｇ＿ｈとの三次元距離｜Ｇ＿ｈ−Ｇ＿ｒ｜を計測することにより決定される。
【０１１８】
つまり、教示者形状取得部１７０１は、把持部５０９の三次元位置と教示者３０１の肘の三次元位置との距離に基づいて、三次元モデルが有する円柱形状の高さを取得する。具体的には、教示者形状取得部１７０１は、例えば、ダイレクトティーチング前、あるいはダイレクトティーチング中に、複数回計測された三次元距離｜Ｇ＿ｈ−Ｇ＿ｒ｜の最小値（ｈ＝ｍｉｎ  ｜Ｇ＿ｈ−Ｇ＿ｒ｜）を高さｈとして決定する。そして、教示者形状取得部１７０１は、決定した高さｈを教示者形状データに反映する。
【０１１９】
以上のように、本実施の形態における移動経路探索装置１００によれば、教示者３０１の下腕の形状を有する三次元モデルの高さｈを教示者３０１が巻き尺などを利用して手動で計測する手間が削減され、速やかに可動ロボット３０２の教示を開始することが可能になる。また、予め定められた高さｈが利用される場合に比べて、教示者形状データが示す円柱形状と教示者の腕の形状との差異を減少させることができる。したがって、本実施の形態における移動経路探索装置１００によれば、教示者の下腕が掃引する空間をより高精度に特定することが可能となる。
【０１２０】
（実施の形態３）
次に、本発明の実施の形態３について図面を参照しながら説明する。なお、実施の形態１と同じ部分については説明を省略する。
【０１２１】
図１８は、本発明の実施の形態３における移動経路探索装置１００の基本的な機能構成を示すブロック図である。図１８に示すように、本実施の形態における移動経路探索装置は、実施の形態１における移動経路探索装置が備える構成要素に加えて、教示者形状判定部１９０１を備える。
【０１２２】
教示者形状判定部１９０１は、教示者位置検出部１０３によって検出される教示者３０１の位置と、移動空間特定部１０２が出力する可動ロボット３０２の姿勢と、教示者形状データベース１０４に保持された教示者形状データとを入力として、教示者３０１が可動ロボット３０２に動作を教示するために使用している腕が、左腕、右腕および両腕のいずれであるかを判定する。そして、教示者形状判定部１９０１は、判定結果を人占有空間特定部１０５に出力する。
【０１２３】
人占有空間特定部１０５は、教示者形状判定部１９０１による判定結果に基づいて、教示者３０１が占有する空間を特定する。
【０１２４】
図１９に示すように、教示者３０１は、左肘に三次元位置計測デバイス２００１を装着し、右肘に三次元位置計測デバイス２００２を装着する。
【０１２５】
図２０に示すように、教示者形状データベース１０４は、教示者３０１の左腕の下腕の教示者形状データ（円柱形状２１０１）と、右腕の下腕の教示者形状データ（円柱形状２１０２）とをそれぞれ保持している。教示者形状データが示す円柱形状２１０１の大きさは、２つの数値パラメータ（半径ｒ＿ｌｅｆｔ、高さｈ＿ｌｅｆｔ）によって規定される。同様に、教示者形状データが示す円柱形状２１０２の大きさは、２つの数値パラメータ（半径ｒ＿ｒｉｇｈｔ、高さｈ＿ｒｉｇｈｔ）によって規定される。
【０１２６】
半径ｒ＿ｌｅｆｔと半径ｒ＿ｒｉｇｈｔとは、例えば、図示しない入力部を介して教示者３０１によって入力された値である。具体的には、半径ｒ＿ｌｅｆｔと半径ｒ＿ｒｉｇｈｔとは、例えば、教示者３０１の左右それぞれの下腕のうち最も外周が小さい部分に基づいてそれぞれ決定される。
【０１２７】
高さｈ＿ｌｅｆｔと高さｈ＿ｒｉｇｈｔは、教示者３０１が可動ロボット３０２の把持部５０９を握ったときの、位置センシングデバイス２２０１、２２０２から把持部５０９までの距離を計測することにより決定される。
【０１２８】
教示者３０１が可動ロボット３０２の把持部５０９を、左手、右手または両手で握りながらダイレクトティーチングを行っているときに、教示者位置検出部１０３は、位置センシングデバイス２２０１と位置センシングデバイス２２０２とを利用して、教示者３０１の左右の肘の三次元位置Ｇ＿ｈ＿ｌｅｆｔ、Ｇ＿ｈ＿ｒｉｇｈｔを一定周期Ｔごとに検出する。また、移動空間特定部１０２は、可動ロボット３０２の把持部５０９の三次元位置Ｇ＿ｒを左右の肘の三次元位置の検出周期と同期させて出力する。
【０１２９】
教示者形状判定部１９０１は、教示者３０１の左右の肘の三次元位置Ｇ＿ｈ＿ｌｅｆｔ、Ｇ＿ｈ＿ｒｉｇｈｔのそれぞれと把持部５０９の三次元位置Ｇ＿ｒとの距離と、教示者形状データベース１０４が保持している左腕および右腕の教示者形状データが示す高さｈ＿ｌｅｆｔ、ｈ＿ｒｉｇｈｔとを比較することにより、教示者３０１が教示に使用している腕を判定する。
【０１３０】
具体的には、教示者形状判定部１９０１は、｜Ｇ＿ｈ＿ｌｅｆｔ  −  Ｇ＿ｒ｜  ＜＝  ｈ＿ｌｅｆｔが満たされる場合、教示者３０１が左手を使用して教示している（図２１の（ａ））と判定する。また、教示者形状判定部１９０１は、｜Ｇ＿ｈ＿ｒｉｇｈｔ  −  Ｇ＿ｒ｜  ＜＝  ｈ＿ｒｉｇｈｔが満たされる場合、教示者３０１が右手を使用して教示している（図２１の（ｂ））と判定する。また、教示者形状判定部１９０１は、｜Ｇ＿ｈ＿ｌｅｆｔ  −  Ｇ＿ｒ｜  ＜＝  ｈ＿ｌｅｆｔ、かつ、｜Ｇ＿ｈ＿ｒｉｇｈｔ  −  Ｇ＿ｒ｜  ＜＝  ｈ＿ｒｉｇｈｔが満たされる場合、教示者３０１が両手を使用して教示している（図２１の（ｃ））と判定する。
【０１３１】
人占有空間特定部１０５は、教示者形状判定部１９０１によって判定された下腕の形状を示す三次元モデルを三次元空間に仮想的に配置する。そして、三次元空間を構成する各ボクセルが教示者３０１の下腕の形状を示す三次元モデルの内部に位置するか否かの内部判定処理を行う。このように、人占有空間特定部１０５は、教示者形状判定部１９０１によって使用していると判定された。
【０１３２】
以上のように本実施の形態における移動経路探索装置１００によれば、教示者３０１がいずれの手を用いてダイレクトティーチングを行なった場合であっても、人占有空間特定部１０５はダイレクトティーチングに使用している下腕が掃引する空間を特定することができる。また、本実施の形態における移動経路探索装置１００によれば、教示者３０１は、いずれの手を使用してダイレクトティーチングを行なうかを能動的に通知する必要が無いため、教示者３０１の負荷の増加を抑制することができる。
【０１３３】
（実施の形態４）
次に、本発明の実施の形態４について図面を参照しながら説明する。なお、実施の形態１と同じ部分については説明を省略する。
【０１３４】
図２２は、本発明の実施の形態４における移動経路探索装置１００の基本的な機能構成を示すブロック図である。図２２に示すように、本実施の形態における移動経路探索装置１００は、教示者位置検出部１０３と教示者形状データベース１０４と人占有空間特定部１０５との代わりに、治具検出部１２１と治具形状データベース１２２と治具占有空間特定部１２３とを備える。
【０１３５】
治具検出部１２１は、空間教示用治具３０８が取り付けられた、可動ロボット３０２の部位を検出する。さらに、治具検出部１２１は、空間教示用治具３０８の識別子を検出する。
【０１３６】
例えば、可動ロボット３０２の部位に空間教示用治具３０８を固定するロック機構が設けられている場合、治具検出部１２１は、ロック機構がロック状態であるか否かを検出することにより、ロック機構が設けられた部位に空間教示用治具３０８が取り付けられているか否かを検出する。
【０１３７】
また例えば、治具検出部１２１は、例えばフォトインタラプタを用いて、空間教示用治具３０８の識別子を検出する。また、治具検出部１２１は、例えばバーコードリーダを用いて、空間教示用治具３０８に設けられたバーコードから識別子を読み出してもよい。
【０１３８】
治具形状データベース１２２は、複数の空間教示用治具の形状をそれぞれ示す複数の形状データの各々を識別子に対応づけて保持している。この識別子は、空間教示用治具を識別するための識別子である。空間教示用治具の形状は、区分線形近似形状であってもよい。
【０１３９】
なお、移動経路探索装置１００は、必ずしも治具検出部１２１を備える必要はない。この場合、教示者３０１は、例えば、空間教示用治具３０８が取り付けられた部位と、空間教示用治具３０８の識別子とを手動で移動経路探索装置１００に入力すればよい。
【０１４０】
治具占有空間特定部１２３は、第２の空間特定部に相当する。治具占有空間特定部１２３は、ダイレクトティーチング中に空間教示用治具３０８が掃引する空間を特定する。
【０１４１】
具体的には、治具占有空間特定部１２３は、空間教示用治具３０８が取り付けられた可動ロボット３０２の部位と、治具形状データベース１２２に保持されている治具形状データとに基づいて、ダイレクトティーチング中の各タイミングにおいて空間教示用治具３０８が占有している空間を特定する。そして、治具占有空間特定部１２３は、特定した空間の和集合を、空間教示用治具３０８が掃引する空間として特定する。
【０１４２】
空間合成部１０６は、移動空間特定部１０２と治具占有空間特定部１２３とが出力する空間の和集合を可動ロボット３０２の移動可能空間として算出する。
【０１４３】
経路探索部１０７は、空間合成部１０６によって算出された移動可能空間内において、空間教示用治具３０８が取り外されたときの可動ロボット３０２の可動部の移動経路を探索する。
【０１４４】
次に、本実施の形態における可動ロボット３０２および空間教示用治具３０８の詳細を説明する。
【０１４５】
図２３は、本発明の実施の形態４における可動ロボット３０２および空間教示用治具３０８の外観図である。具体的には、図２３の（ａ）は、可動ロボット３０２に空間教示用治具３０８が取り付けられる前の状態を示す。一方、図２３の（ｂ）は、可動ロボット３０２に空間教示用治具３０８が取り付けられた後の状態を示す。
【０１４６】
空間教示用治具３０８は、例えばアルミニウムまたはアクリルなどによって構成される剛体である。空間教示用治具３０８は、可動ロボット３０２の予め定められた部位に取り付けられる。
【０１４７】
図２３の例では、可動ロボット３０２の複数の部位の各々にロック機構３０９が設けられている（図２３の（ａ））。そして、ダイレクトティーチングが行われる前に、空間教示用治具３０８は、これらの複数の部位のうちの少なくとも１つの部位にロック機構を介して取り付けられる（図２３の（ｂ））。その後、ダイレクトティーチングが行われた後に、空間教示用治具３０８は、可動ロボット３０２から取り外される。
【０１４８】
つまり、空間教示用治具３０８は、ダイレクトティーチング中に移動可能空間を教示するために、可動ロボット３０２に取り付けられる治具である。したがって、ダイレクトティーチング後に、空間教示用治具３０８は可動ロボット３０２から取り外される。
【０１４９】
なお、空間教示用治具３０８は、必ずしもロック機構を介して可動ロボット３０２の部位に取り付けられる必要はない。例えば、空間教示用治具３０８は、可動ロボット３０２の部位に磁力を利用して取り付けられてもよい。
【０１５０】
次に、以上のように構成された移動経路探索装置１００における各種動作について説明する。
【０１５１】
図２４は、本発明の実施の形態４における移動経路探索処理の流れを示すフローチャートである。なお、図２４において、図５と同様のステップについては、同一の符号を付し、適宜説明を省略する。以下、教示者３０１が、空間教示用治具３０８が取り付けられた可動ロボット３０２に対して、障害物３０６を避けながら始点３０５から終点３０７へ移動する動作をダイレクトティーチングする例について説明する。
【０１５２】
教示者３０１が可動ロボット３０２の把持部５０９を握りながらダイレクトティーチングを行うと、各回転軸の角度を示す値（ＪＴ１，ＪＴ２，ＪＴ３，ＪＴ４，ＪＴ５，ＪＴ６）が変化して可動ロボット３０２は変形する。
【０１５３】
このとき、移動空間特定部１０２は、可動ロボット３０２の可動部５０８が占有している空間を特定する（Ｓ６０１）。その結果、図２５に示すように、可動ロボット３０２に内包されるボクセル２５０２の論理値がｔｒｕｅと設定される。なお、一部でも可動ロボット３０２の外側に位置するボクセル２５０１の論理値は変更されない。
【０１５４】
続いて、治具検出部１２１は、可動ロボット３０２に取り付けられた空間教示用治具３０８を検出する（Ｓ２４０２）。具体的には、治具検出部１２１は、空間教示用治具３０８が取り付けられた部位と、空間教示用治具３０８の識別子とを検出する。
【０１５５】
治具占有空間特定部１２３は、空間教示用治具３０８が占有している空間を特定する（Ｓ２４０３）。具体的には、治具占有空間特定部１２３は、空間教示用治具３０８が取り付けられた部位に基づいて、空間教示用治具３０８の形状を有する三次元モデルを三次元空間に仮想的に配置する。そして、治具占有空間特定部１２３は、三次元空間に配置された三次元モデルが占める領域を特定する。
【０１５６】
より具体的には、治具占有空間特定部１２３は、治具検出部１２１によって検出された識別子に対応する治具形状データを治具形状データベース１２２から読み出す。そして、治具占有空間特定部１２３は、読み出した治具形状データが示す形状を有する三次元モデルを三次元空間に仮想的に配置する。すなわち、治具占有空間特定部１２３は、可動ロボット３０２の姿勢情報とロボット形状データと空間教示用治具３０８が取り付けられた部位の情報とから算出される位置に、治具形状データが示す形状を有する三次元モデルを三次元空間に仮想的に配置する。
【０１５７】
そして、治具占有空間特定部１２３は、三次元空間を構成する各ボクセルが三次元空間に仮想的に配置された空間教示用治具３０８の形状を有する三次元モデルの内部に位置するか否かの内部判定処理を行う。ここでの内部判定処理とは、あるボクセルを構成するすべての点が、空間教示用治具３０８の形状を示す三次元モデルによって形成される閉領域に含まれる（以下、「ボクセルが空間教示用治具３０８に内包される」という）かどうかを判定する処理をいう。
【０１５８】
ここで、ボクセルが空間教示用治具３０８に内包される場合には、治具占有空間特定部１２３は、そのボクセルは可動ロボット３０２が移動可能なボクセルであると判断し、そのボクセルの論理値をｔｒｕｅと設定する。一方、ボクセルが空間教示用治具３０８に内包されない場合、治具占有空間特定部１２３は、そのボクセルに設定された論理値を変更しない。
【０１５９】
その結果、図２６に示すように、空間教示用治具３０８に内包されるボクセル２６０２の論理値がｔｒｕｅと設定される。なお、一部でも空間教示用治具３０８の外側に位置するボクセル２６０１の論理値は変更されない。
【０１６０】
ここで、治具占有空間特定部１２３が移動可能と判定したボクセルの集合Ｓは、以下の式（６）によって算出される。
【０１６１】
Ｓ＝∪｛Ｖ（ｘ，ｙ，ｚ）｜Ｍ（ｘ，ｙ，ｚ）＝ｔｒｕｅ｝  式（６）
【０１６２】
治具占有空間特定部１２３は、このボクセルの集合Ｓを空間教示用治具３０８に占有されている空間として特定する。
【０１６３】
次に、教示者３０１が教示終了を指示したか否かが判定される（Ｓ６０４）。ここで、教示終了が指示されない限り（Ｓ６０４のＮｏ）、ステップＳ６０１、Ｓ２４０２、およびＳ２４０３の処理が繰り返し実行される。
【０１６４】
つまり、移動空間特定部１０２は、可動ロボット３０２の可動部５０８がダイレクトティーチング中の各タイミングにおいて占有している空間の特定を繰り返すことにより、可動ロボット３０２の可動部５０８が掃引した掃引空間を特定する。また、治具占有空間特定部１２３は、ダイレクトティーチング中の各タイミングにおいて空間教示用治具３０８が占有している空間の特定を繰り返すことにより、空間教示用治具３０８が掃引した掃引空間を特定する。
【０１６５】
教示者３０１が教示終了を指示した場合（Ｓ６０４のＹｅｓ）、空間合成部１０６は、図２８に示すように、移動空間特定部１０２が出力する移動可能なボクセルの集合Ｒと、治具占有空間特定部１２３が出力する移動可能なボクセルの集合Ｓとの和集合Ｒ∪Ｓを、可動ロボット３０２の移動可能空間として算出する（Ｓ２４０５）。つまり、空間合成部１０６は、移動空間特定部１０２によって特定された空間と治具占有空間特定部１２３によって特定された空間との和集合を移動可能空間として算出する。
【０１６６】
図２７および図２８は、本発明の実施の形態４における空間合成部１０６によって算出される移動可能空間を説明するための図である。図２７に示す移動可能空間２７０３が、可動ロボット３０２の掃引空間２７０１と空間教示用治具３０８の掃引空間２７０２との和集合Ｒ∪Ｓに相当する。また、図２８では、説明を簡単にするために、二次元平面に射影したときの和集合Ｒ∪Ｓを示している。和集合Ｒ∪Ｓは、白のボクセル２８０２とドットテクスチャのボクセル２８０３とによって形成される領域に相当する。つまり、和集合Ｒ∪Ｓは、ハッチングされたボクセル２８０１以外のボクセルによって形成される領域に相当する。
【０１６７】
最後に、経路探索部１０７は、空間合成部１０６によって算出された移動可能空間内において、可動部５０８の移動経路を探索する（Ｓ６０６）。具体的には、経路探索部１０７は、空間合成部１０６によって算出された移動可能空間を探索空間として移動経路を探索する。その結果、経路探索部１０７は、与えられる探索条件（三次元位置を示す始点および終点、並びに制約条件）に対してより最適な経路を出力することが可能になる。
【０１６８】
ここで、図２９および図３０を用いて、本実施の形態における経路探索部１０７によって探索される移動経路の一例を、従来の方法によって探索される移動経路と比較しながら説明する。
【０１６９】
図２９は、従来の方法によって探索される移動経路の一例を示す図である。また、図３０は、本発明の実施の形態４における移動経路探索装置１００によって探索される移動経路の一例を示す図である。
【０１７０】
従来の方法では、ロボット掃引空間Ｒの内部だけで、始点２９０１から終点２９０２までの移動経路の探索を行う。したがって、探索結果として得られる移動経路２９０３は、図２９に示すような複雑な曲線経路となる。このような移動経路２９０３に従って可動部５０８の先端を移動させるためには、可動ロボット３０２全体を方向２９０４に移動させてから回転させる必要がある。つまり、従来の方法によって探索される移動経路２９０３は、可動ロボット３０２の動作に無駄が多く最適性が低い。
【０１７１】
しかしながら、本実施の形態における移動経路探索装置１００による経路探索では、図３０に示すとおり、ロボット掃引空間Ｒに加えて治具掃引空間Ｓを利用して、移動経路を探索することができる。つまり、経路探索部１０７は、ロボット掃引空間Ｒに、空間Ｓ−Ｒ（マイナス記号はＳからＳ∩Ｒである要素を除く意味）を加えた空間を探索に利用することができる。
【０１７２】
したがって、図３０に示すような、垂直多関節ロボットが最速で移動することができる円弧状の経路が移動経路３００１として探索される。つまり、本実施の形態における移動経路探索装置１００によって探索される移動経路３００１に従って動作することにより、可動ロボット３０２は、従来よりも動作の無駄を抑制することができる。すなわち、可動ロボット３０２は、従来よりも短時間で始点と終点との間を移動することが可能になる。つまり、移動経路探索装置１００は、より最適な移動経路を探索することが可能となる。
【０１７３】
以上のように、本実施の形態における移動経路探索装置１００によれば、ダイレクトティーチング中に可動ロボット３０２が掃引する空間と空間教示用治具３０８が掃引する空間との和集合を、移動経路を探索するときの移動可能空間として算出することができる。つまり、ダイレクトティーチング中に可動ロボット３０２が掃引しない空間も移動可能空間に加えることが可能となる。したがって、可動ロボット３０２が掃引する空間のみを移動可能空間とする場合よりも拡張された移動可能空間を得ることができるので、より最適な移動経路を探索することが可能となる。また、ダイレクトティーチング中に空間教示用治具３０８が掃引する空間を用いて移動可能空間を算出することができるので、教示者３０１は、移動可能空間を拡張するために試行錯誤的に教示を繰り返す必要がない。したがって、教示者３０１の負荷の増加も抑制することが可能となる。
【０１７４】
（実施の形態４の変形例）
次に、実施の形態４の変形例について説明する。
【０１７５】
本変形例における移動経路探索装置は、実施の形態４における移動経路探索装置が備える構成要素に加えて、空間教示用治具を前記可動ロボットから取り外す必要があることを通知する通知部を備える。
【０１７６】
図３１は、本発明の実施の形態４の変形例における移動経路探索装置１００の基本的な機能構成を示すブロック図である。なお、図３１において、図２２と同様の構成要素については、同一の符号を付し、説明を省略する。
【０１７７】
図３１に示すように、移動経路探索装置１００は、さらに、通知部１２４を備える。
【０１７８】
通知部１２４は、直接教示が終了した後に空間教示用治具３０８が可動ロボット３０２に取り付けられている場合に、空間教示用治具３０８を可動ロボット３０２から取り外す必要があることを教示者あるいは作業者などに通知する。
【０１７９】
具体的には、通知部１２４は、例えば、可動ロボット３０２に設けられたランプを点灯あるいは点滅させることにより、空間教示用治具３０８を可動ロボット３０２から取り外す必要があることを教示者あるいは作業者などに通知する。また例えば、通知部１２４は、警告音を出力することにより、空間教示用治具３０８を可動ロボット３０２から取り外す必要があることを教示者あるいは作業者などに通知してもよい。
【０１８０】
このように、本変形例における移動経路探索装置１００によれば、直接教示が終了した後に、空間教示用治具３０８を可動ロボット３０２から取り外す必要があることを教示者あるいは作業者などに通知することができる。したがって、空間教示用治具３０８が可動ロボット３０２に取り付けられた状態で、作業が開始されることを抑制することができ、作業の安全性を向上させることが可能となる。
【０１８１】
以上、本発明の一態様に係る移動経路探索装置１００について、実施の形態に基づいて説明したが、本発明は、これらの実施の形態に限定されるものではない。本発明の趣旨を逸脱しない限り、当業者が思いつく各種変形を本実施の形態に施したもの、あるいは異なる実施の形態における構成要素を組み合わせて構築される形態も、本発明の範囲内に含まれる。
【０１８２】
例えば、移動経路探索装置１００は、実施の形態２と実施の形態３とを組み合わせて、構築されてもよい。つまり、移動経路探索装置１００は、教示者形状取得部１７０１と教示者形状判定部１９０１との両方を備えてもよい。これにより、移動経路探索装置１００は、左下腕と右下腕の両方の三次元モデルの高さｈを自動取得することが可能になる。
【０１８３】
また、上記実施の形態１〜３において、教示者３０１は腕を、利用してダイレクトティーチングを行っていたが、例えば、足を利用してダイレクトティーチングする場合であっても、本発明を適用することができる。この場合、人占有空間特定部１０５は、ダイレクトティーチング中に教示者３０１の足が掃引する空間を特定すればよい。
【０１８４】
また、移動経路探索装置１００は、実施の形態１と実施の形態４とを組み合わせて、構築されてもよい。つまり、移動経路探索装置１００は、人占有空間特定部１０５と治具占有空間特定部１２３との両方を備えてもよい。この場合、空間合成部１０６は、移動空間特定部１０２によって特定された空間と、人占有空間特定部１０５によって特定された空間と、治具占有空間特定部１２３によって特定された空間との和集合を移動可能空間として算出する。
【０１８５】
つまり、空間合成部１０６は、可動ロボット３０２の可動部が掃引する空間と、教示者３０１の身体の少なくとも一部が掃引する空間と、空間教示用治具３０８が掃引する空間との和集合を移動可能空間として算出する。したがって、移動経路探索装置１００は、さらに拡張された移動可能空間を得ることができるので、より最適な移動経路を探索することが可能となる。
【０１８６】
また、本発明の一態様に係る移動経路探索装置は、上記実施の形態１〜４における移動経路探索装置が備える構成要素のすべてを備える必要はない。例えば、移動経路探索装置は、図３２に示すように構成されてもよい。
【０１８７】
図３２は、本発明の一態様に係る移動経路探索装置１００の機能構成を示すブロック図である。図３２に示す移動経路探索装置１００は、第１の空間特定部１３１と、第２の空間特定部１３２と、空間合成部１０６と、経路探索部１０７とを備える。
【０１８８】
ここで、第１の空間特定部１３１は、移動空間特定部１０２に相当する。また、第２の空間特定部１３２は、人占有空間特定部１０５および治具占有空間特定部１２３の少なくとも一方に相当する。
【０１８９】
このように、移動経路探索装置１００は、必ずしも、ロボット形状データベース１０１、教示者位置検出部１０３、教示者形状データベース１０４、治具検出部１２１、および治具形状データベース１２２等を備える必要はない。図３２のような場合、移動経路探索装置１００は、例えば、ダイレクトティーチング中の教示者３０１の身体の少なくとも一部の三次元位置とロボット形状データと教示者形状データとを、図示していない通信部あるいは入力部などを介して取得すればよい。また例えば、移動経路探索装置１００は、ダイレクトティーチング中の空間教示用治具３０８が取り付けられた部位と治具形状データとを、通信部あるいは入力部などを介して取得してもよい。
【０１９０】
また、本発明は、移動可能空間出力装置１１０として実現されてもよい。図３３は、本発明の一態様に係る移動可能空間出力装置１１０の機能構成を示すブロック図である。図３３に示すように、移動可能空間出力装置１１０は、第１の空間特定部１３１と、第２の空間特定部１３２と、空間合成部１０６とを備える。この場合、空間合成部１０６は、算出した移動可能空間を、例えば、表示装置に出力する。その結果、移動可能空間出力装置１１０は、教示者３０１に移動可能空間を提示することができる。また、空間合成部１０６は、算出した移動可能空間を移動経路探索装置に出力してもよい。その結果、教示者の身体が掃引する空間を利用して移動可能空間を算出することができない移動経路探索装置であっても、最適な移動経路を探索することが可能となる。
【０１９１】
また、上記実施の形態１、２、３または４における移動経路探索装置１００が備える構成要素の一部または全部は、１個のシステムＬＳＩ（Ｌａｒｇｅ  Ｓｃａｌｅ  Ｉｎｔｅｇｒａｔｉｏｎ：大規模集積回路）から構成されているとしてもよい。例えば、移動経路探索装置１００は、第１の空間特定部１３１と、第２の空間特定部１３２と、空間合成部１０６と、経路探索部１０７とを有するシステムＬＳＩから構成されてもよい。
【０１９２】
システムＬＳＩは、複数の構成部を１個のチップ上に集積して製造された超多機能ＬＳＩであり、具体的には、マイクロプロセッサ、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｍｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）などを含んで構成されるコンピュータシステムである。前記ＲＡＭには、コンピュータプログラムが記憶されている。前記マイクロプロセッサが、前記コンピュータプログラムに従って動作することにより、システムＬＳＩは、その機能を達成する。
【０１９３】
なお、ここでは、システムＬＳＩとしたが、集積度の違いにより、ＩＣ、ＬＳＩ、スーパーＬＳＩ、ウルトラＬＳＩと呼称されることもある。また、集積回路化の手法はＬＳＩに限るものではなく、専用回路または汎用プロセッサで実現してもよい。ＬＳＩ製造後に、プログラムすることが可能なＦＰＧＡ（Ｆｉｅｌｄ  Ｐｒｏｇｒａｍｍａｂｌｅ  Ｇａｔｅ  Ａｒｒａｙ）、あるいはＬＳＩ内部の回路セルの接続や設定を再構成可能なリコンフィギュラブル・プロセッサを利用してもよい。
【０１９４】
さらには、半導体技術の進歩または派生する別技術によりＬＳＩに置き換わる集積回路化の技術が登場すれば、当然、その技術を用いて機能ブロックの集積化を行ってもよい。バイオ技術の適用等が可能性としてありえる。
【０１９５】
また、本発明は、このような特徴的な処理部を備える移動経路探索装置として実現することができるだけでなく、移動経路探索装置に含まれる特徴的な処理部をステップとする移動経路探索方法として実現することもできる。また、移動経路探索方法に含まれる特徴的な各ステップをコンピュータに実行させるコンピュータプログラムとして実現することもできる。そして、そのようなコンピュータプログラムを、ＣＤ−ＲＯＭ（Ｃｏｍｐａｃｔ  Ｄｉｓｃ  Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）等のコンピュータ読取可能な非一時的な記録媒体あるいはインターネット等の通信ネットワークを介して流通させることができるのは、言うまでもない。
【産業上の利用可能性】
【０１９６】
本発明の一態様に係る移動経路探索装置は、教示後に可動ロボットが自律的に移動経路を探索して動作するロボットアプリケーションとして有用である。
【符号の説明】
【０１９７】
１００  移動経路探索装置
１０１  ロボット形状データベース
１０２  移動空間特定部
１０３  教示者位置検出部
１０４  教示者形状データベース
１０５  人占有空間特定部
１０６  空間合成部
１０７  経路探索部
１１０  移動可能空間出力装置
１２１  治具検出部
１２２  治具形状データベース
１２３  治具占有空間特定部
１２４  通知部
１３１  第１の空間特定部
１３２  第２の空間特定部
３０１  教示者
３０２  可動ロボット
３０３、２００１、２００２  三次元位置計測デバイス
３０４、２２０１、２２０２  位置センシングデバイス
３０５  始点
３０６  障害物
３０７  終点
３０８  空間教示用治具
３０９  ロック機構
５０１、５０２、５０３、５０４、５０５、５０６  回転軸
５０７  固定部
５０８  可動部
５０９  把持部
１７０１  教示者形状取得部
１９０１  教示者形状判定部
【図１】
2012014409.tif 000003
【図２】
2012014409.tif 000004
【図３】
2012014409.tif 000005
【図４】
2012014409.tif 000006
【図５】
2012014409.tif 000007
【図６】
2012014409.tif 000008
【図７】
2012014409.tif 000009
【図８】
2012014409.tif 000010
【図９】
2012014409.tif 000011
【図１０】
2012014409.tif 000012
【図１１】
2012014409.tif 000013
【図１２】
2012014409.tif 000014
【図１３】
2012014409.tif 000015
【図１４】
2012014409.tif 000016
【図１５】
2012014409.tif 000017
【図１６】
2012014409.tif 000018
【図１７】
2012014409.tif 000019
【図１８】
2012014409.tif 000020
【図１９】
2012014409.tif 000021
【図２０】
2012014409.tif 000022
【図２１】
2012014409.tif 000023
【図２２】
2012014409.tif 000024
【図２３】
2012014409.tif 000025
【図２４】
2012014409.tif 000026
【図２５】
2012014409.tif 000027
【図２６】
2012014409.tif 000028
【図２７】
2012014409.tif 000029
【図２８】
2012014409.tif 000030
【図２９】
2012014409.tif 000031
【図３０】
2012014409.tif 000032
【図３１】
2012014409.tif 000033
【図３２】
2012014409.tif 000034
【図３３】
2012014409.tif 000035
【図３４】
2012014409.tif 000036
【手続補正書】
【提出日】平成23年12月22日(2011.12.22)
【手続補正１】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
教示者が可動ロボットを直接動かして動作を教示する直接教示が可能な可動ロボットの可動部の移動経路を探索する移動経路探索装置であって、
直接教示中に可動ロボットの可動部が掃引する空間を特定する第１の空間特定部と、
直接教示中に教示者の身体の少なくとも一部と前記可動ロボットに取り付けられた空間教示用治具とのうちの少なくとも一方が掃引する空間を特定する第２の空間特定部と、
前記第１の空間特定部によって特定された空間と前記第２の空間特定部によって特定された空間との和集合を移動可能空間として算出する空間合成部と、
前記空間合成部によって算出された移動可能空間内において、前記可動部の移動経路を探索する経路探索部とを備え、
前記第２の空間特定部は、前記教示者の身体の少なくとも一部の三次元位置に基づいて前記教示者の身体の少なくとも一部の形状を有する三次元モデルを三次元空間に仮想的に配置し、前記三次元空間において前記三次元モデルが占める領域を特定することにより、前記教示者の身体の少なくとも一部が掃引する空間を特定する
移動経路探索装置。
【請求項２】
前記可動ロボットは、直接教示中に前記教示者が把持する把持部を有し、
前記第２の空間特定部は、前記教示者の肘の三次元位置と前記把持部の三次元位置とに基づいて、前記教示者の下腕の形状を有する前記三次元モデルを前記三次元空間に仮想的に配置し、前記三次元空間において前記三次元モデルが占める領域を特定することにより、前記教示者の身体の少なくとも一部が掃引する空間を特定する
請求項１に記載の移動経路探索装置。
【請求項３】
前記三次元モデルは円柱形状を有する
請求項２に記載の移動経路探索装置。
【請求項４】
前記移動経路探索装置は、さらに、
前記把持部の三次元位置と前記教示者の肘の三次元位置との距離に基づいて、前記三次元モデルが有する円柱形状の高さを取得する教示者形状取得部を備え、
前記第２の空間特定部は、前記教示者形状取得部によって取得された高さの円柱形状を有する前記三次元モデルを前記三次元空間に仮想的に配置する
請求項３に記載の移動経路探索装置。
【請求項５】
前記移動経路探索装置は、さらに、
前記教示者の左肘の三次元位置と前記把持部の三次元位置との距離、および前記教示者の右肘の三次元位置と前記把持部の三次元位置との距離のそれぞれを、予め定められた閾値と比較することにより、前記教示者が直接教示のために使用している下腕を判定する教示者形状判定部を備え、
前記第２の空間特定部は、前記教示者形状判定部によって使用していると判定された下腕が掃引する空間を特定する
請求項２に記載の移動経路探索装置。
【請求項６】
前記移動経路探索装置は、さらに、
前記教示者の身体の少なくとも一部の形状を示す形状データを保持している教示者形状データベースを備え、
前記第２の空間特定部は、前記教示者形状データベースに保持されている形状データが示す形状を有する前記三次元モデルを前記三次元空間に仮想的に配置する
請求項１に記載の移動経路探索装置。
【請求項７】
前記移動経路探索装置は、さらに、
前記教示者の身体の少なくとも一部の三次元位置を検出する教示者位置検出部を備え、
前記第２の空間特定部は、前記教示者位置検出部によって検出された三次元位置に基づいて、前記三次元モデルを三次元空間に仮想的に配置する
請求項１に記載の移動経路探索装置。
【請求項８】
教示者が可動ロボットを直接動かして動作を教示する直接教示が可能な可動ロボットの可動部の移動経路を探索する移動経路探索方法であって、
直接教示中に可動ロボットの可動部が掃引する空間を特定する第１の空間特定ステップと、
直接教示中に教示者の身体の少なくとも一部と前記可動ロボットに取り付けられた空間教示用治具とのうちの少なくとも一方が掃引する空間を特定する第２の空間特定ステップと、
前記第１の空間特定ステップにおいて特定された空間と前記第２の空間特定ステップにおいて特定された空間との和集合を移動可能空間として算出する空間合成ステップと、
前記空間合成ステップにおいて算出された移動可能空間内において、前記可動部の移動経路を探索する経路探索ステップとを含み、
前記第２の空間特定ステップでは、前記教示者の身体の少なくとも一部の三次元位置に基づいて前記教示者の身体の少なくとも一部の形状を有する三次元モデルを三次元空間に仮想的に配置し、前記三次元空間において前記三次元モデルが占める領域を特定することにより、前記教示者の身体の少なくとも一部が掃引する空間を特定する
移動経路探索方法。
【請求項９】
請求項８に記載の移動経路探索方法をコンピュータに実行させるためのプログラム。
【国際調査報告】
2012014409.tif 000037
2012014409.tif 000038
2012014409.tif 000039
2012014409.tif 000040
2012014409.tif 000041
2012014409.tif 000042
2012014409.tif 000043
2012014409.tif 000044