WO2012063397A1 - 移動経路探索装置および移動経路探索方法 - Google Patents

Abstract

　教示者（３０１）が可動ロボット（３０２）を直接動かして動作を教示する複数回の直接教示の各々において、可動部（５０８）が直接教示中に掃引する三次元空間を第１の空間として特定する移動空間特定部（１０２）と、複数回の直接教示のうちの一の直接教示によって特定された第１の空間のうち、一の直接教示よりも前に行われた直接教示によって特定された第１の空間と重複しない空間を第１の差分空間として算出する空間差分計算部（１０４）と、第１の差分空間に関する情報を教示者（３０１）に通知するフィードバック部（１０５）と、複数回の直接教示が終了した後に、複数回の直接教示によって特定された複数の第１の空間が合成された第１の移動可能空間内において、可動部（５０８）の移動経路を探索する経路探索部（１０６）とを備える。

Description

移動経路探索装置および移動経路探索方法

　本発明は、直接教示に基づいて可動ロボットの可動部の移動経路を探索する移動経路探索装置および移動経路探索方法に関する。

　教示者が可動ロボットを直接動かして動作を教示する直接教示（以下、「ダイレクトティーチング」という）が行われた際に、可動ロボットの可動部が移動可能な三次元空間（以下、適宜、単に「移動可能空間」という）を自動取得する技術が提案されている。そして、このように自動取得された移動可能空間を利用して、自動動作時の最適経路を自動探索するアプローチが試みられている。例えば非特許文献１に示された技術では、ダイレクトティーチング時にロボットが移動可能な空間が自動取得され、最適経路の探索空間として利用される。

　図３２は、従来の移動可能空間取得処理と最適経路探索処理との概要を示す図である。非特許文献１の中で、教示者２０１は、可動ロボット２０２の先端に実装された力センサを利用して、ダイレクトティーチングにより動作を教示する。ダイレクトティーチング中に可動ロボット２０２自体が占有した空間を足し合わせることで移動可能空間２０３が特定される。そして、特定された移動可能空間２０３内において、２点間の移動経路２０４が探索される。つまり、移動可能空間２０３は、探索空間として利用される。可動ロボット２０２は、このように探索された移動経路２０４に従って動作することにより、障害物と干渉しないように自動で作業することが可能となる。

前田他「空間掃引を用いた産業用マニピュレータの教示」、日本機械学会論文集（Ｃ編）、７４巻７３７号（２００８－１）

　しかしながら、非特許文献１の方法では、特定された移動可能空間のボリュームが、実際に可動ロボットが移動可能な空間のボリュームよりも小さくなることが多い。したがって、その移動可能空間を探索空間として利用して探索された移動経路の最適性が低いという問題がある。

　十分なボリュームの移動可能空間を得るためには、教示者は、すでに特定された移動可能空間を拡張するような動作をさらに教示しなければならない。しかしながら、教示者は、どのような動作を教示すれば移動可能空間を効率的に拡張することができるかを理解することが難しい。したがって、教示者は、試行錯誤的な教示を繰り返すことになり、教示コストの増加につながるという問題がある。

　そこで、本発明は、上記課題を考慮してなされたものであって、複数回の直接教示に基づいて移動可能空間を取得する場合に、移動可能空間を効率的に拡張することができる移動経路探索装置および移動経路探索方法を提供することを目的とする。

　前記従来の課題を解決するために、本発明の一態様に係る移動経路探索装置は、教示者が可動ロボットを直接動かして動作を教示する複数回の直接教示の各々において、可動ロボットの可動部が直接教示中に掃引する三次元空間を第１の空間として特定する第１の空間特定部と、前記複数回の直接教示のうちの一の直接教示によって特定された第１の空間のうち、前記一の直接教示よりも前に行われた直接教示によって特定された第１の空間と重複しない空間を第１の差分空間として算出する空間差分計算部と、前記第１の差分空間に関する情報を前記教示者に通知するフィードバック部と、前記複数回の直接教示が終了した後に、前記複数回の直接教示によって特定された複数の第１の空間が合成された第１の移動可能空間内において、前記可動部の移動経路を探索する経路探索部とを備える。

　本構成によれば、複数回の直接教示のうちの一の直接教示によって特定された第１の空間のうち、当該一の直接教示よりも前に行われた直接教示によって特定された第１の空間と重複しない第１の差分空間に関する情報を教示者に通知することができる。したがって、教示者は、通知された情報を参考にして、次回の直接教示を行うことが可能となる。その結果、複数回の直接教示に基づいて移動可能空間を取得する場合に、複数回の直接教示において効率的に移動可能空間を拡張することが可能となる。

　本発明によれば、複数回の直接教示に基づいて移動可能空間を取得する場合に、複数回の直接教示のうちの一の直接教示によって特定された空間のうち、当該一の直接教示よりも前に行われた直接教示によって特定された空間と重複しない空間に関する情報を教示者に通知することができるので、効率的に移動可能空間を拡張することが可能となる。

図１は、本発明の実施の形態１における移動経路探索装置の機能構成を示すブロック図である。 図２は、本発明の実施の形態１における移動経路探索装置の利用シーンを示す図である。 図３は、本発明の実施の形態１における移動可能空間を表現するボクセルを説明する図である。 図４は、本発明の実施の形態１における可動ロボットの外観図である。 図５は、本発明の実施の形態１における移動経路探索処理を示すフローチャートである。 図６は、本発明の実施の形態１における移動空間特定部による内部判定処理を説明するための図である。 図７は、本発明の実施の形態１における１回目のダイレクトティーチングによって特定されるロボット移動空間の一例を示す図である。 図８は、本発明の実施の形態１における１回目のダイレクトティーチングによって特定されるロボット移動空間を構成するボクセルの集合を示す図である。 図９は、本発明の実施の形態１における２回目のダイレクトティーチングによって特定されるロボット移動空間の一例を示す図である。 図１０は、本発明の実施の形態１における２回目までのダイレクトティーチングによって特定されたロボット移動空間を構成するボクセルの集合を示す図である。 図１１は、本発明の実施の形態１におけるフィードバック情報の通知方法の一例を示す図である。 図１２は、本発明の実施の形態１におけるフィードバック情報の通知方法の他の一例を示す図である。 図１３は、本発明の実施の形態１における３回目のダイレクトティーチングによって特定されるロボット移動空間の一例を示す図である。 図１４は、本発明の実施の形態１における３回目までのダイレクトティーチングによって特定されたロボット移動空間を構成するボクセルの集合を示す図である。 図１５は、本発明の実施の形態１における探索条件の一例を説明するための図である。 図１６は、本発明の実施の形態１における移動経路探索装置によって探索される移動経路の一例を示す図である。 図１７は、本発明の実施の形態２における移動経路探索装置の機能構成を示すブロック図である。 図１８は、本発明の実施の形態２における移動経路探索装置の利用シーンを示す図である。 図１９は、本発明の実施の形態２における移動経路探索処理を示すフローチャートである。 図２０は、本発明の実施の形態２における移動空間特定部による内部判定処理を説明するための図である。 図２１は、本発明の実施の形態２における教示者形状データベースが保持している教示者形状データの一例を示す図である。 図２２は、本発明の実施の形態２における人占有空間特定部による内部判定処理を説明するための図である。 図２３は、本発明の実施の形態２における移動空間特定部によって特定されたロボット移動空間の一例を示す図である。 図２４は、本発明の実施の形態２における人占有空間特定部によって特定された人占有空間の一例を示す図である。 図２５は、本発明の実施の形態２における１～３回目のダイレクトティーチングによって特定される第１の移動可能空間と第２の移動可能空間とを構成するボクセルの集合を示す図である。 図２６は、本発明の実施の形態２におけるフィードバック情報の通知方法の一例を示す図である。 図２７は、本発明の実施の形態４における移動経路探索装置の機能構成を示すブロック図である。 図２８は、本発明の実施の形態４における教示者の姿勢を説明するための図である。 図２９は、本発明の実施の形態４における移動経路探索処理を示すフローチャートである。 図３０は、本発明の一態様に係る移動経路探索装置の機能構成を示すブロック図である。 図３１は、本発明の一態様に係る移動可能空間出力装置の機能構成を示すブロック図である。 図３２は、従来の移動可能空間取得処理と最適経路探索処理との概要を示す図である。

　本発明の一態様に係る移動経路探索装置は、教示者が可動ロボットを直接動かして動作を教示する複数回の直接教示の各々において、可動ロボットの可動部が直接教示中に掃引する三次元空間を第１の空間として特定する第１の空間特定部と、前記複数回の直接教示のうちの一の直接教示によって特定された第１の空間のうち、前記一の直接教示よりも前に行われた直接教示によって特定された第１の空間と重複しない空間を第１の差分空間として算出する空間差分計算部と、前記第１の差分空間に関する情報を前記教示者に通知するフィードバック部と、前記複数回の直接教示が終了した後に、前記複数回の直接教示によって特定された複数の第１の空間が合成された第１の移動可能空間内において、前記可動部の移動経路を探索する経路探索部とを備える。

　この構成によれば、複数回の直接教示のうちの一の直接教示によって特定された第１の空間のうち、当該一の直接教示よりも前に行われた直接教示によって特定された第１の空間と重複しない第１の差分空間に関する情報を教示者に通知することができる。したがって、教示者は、通知された情報を参考にして、次回の直接教示を行うことが可能となる。その結果、複数回の直接教示に基づいて移動可能空間を取得する場合に、複数回の直接教示において効率的に移動可能空間を拡張することが可能となる。

　また、前記フィードバック部は、前記第１の差分空間と、前記一の直接教示よりも前に行われた直接教示によって特定された第１の空間との体積比を、前記第１の差分空間に関する情報として通知することが好ましい。

　この構成によれば、一の直接教示によって算出された第１の差分空間と当該一の直接教示よりも前に行われた直接教示によって特定された第１の空間との体積比を教示者に通知することができる。つまり、一の直接教示による移動可能空間の増加率を教示者に通知することができる。したがって、教示者は、一の直接教示が移動可能空間の拡張にどれだけ貢献したかを理解することが可能となる。その結果、複数回の直接教示において効率的に移動可能空間を拡張することが可能となる。

　また、前記第１の空間特定部は、三次元空間を分割した複数のボクセルのうち、直接教示中の各タイミングにおいて前記可動ロボットに内包されたボクセルの集合によって構成される空間を、前記第１の空間として特定し、前記フィードバック部は、前記第１の差分空間を構成するボクセルの数と、一の直接教示よりも前に行われた直接教示によって特定された第１の空間を構成するボクセルの数との比を、前記体積比として通知することが好ましい。

　この構成によれば、第１の空間を特定するときに利用されたボクセルの数を利用して体積比を通知することができ、体積比を得るための処理負荷を低減することが可能となる。

　また、さらに、前記複数回の直接教示の各々において、前記教示者の身体の少なくとも一部が直接教示中に掃引する三次元空間を第２の空間として特定する第２の空間特定部と、複数回の直接教示によって特定された複数の第１の空間が合成された第１の移動可能空間と、前記複数回の直接教示によって特定された複数の第２の空間が合成された第２の移動可能空間とを合成する空間合成部とを備え、前記空間差分計算部は、さらに、前記一の直接教示によって特定された第２の空間のうち、前記一の直接教示よりも前に行われた直接教示によって特定された第２の空間と重複しない空間を第２の差分空間として算出し、前記フィードバック部は、さらに、前記第２の差分空間に関する情報を前記教示者に通知し、前記経路探索部は、前記第１の移動可能空間内に代えて、前記空間合成部によって合成された空間内において、前記可動部の移動経路を探索することが好ましい。

　この構成によれば、直接教示中に可動ロボットが掃引する第１の空間と教示者の身体の少なくとも一部が掃引する第２の空間とを利用して移動可能空間を得ることができる。つまり、直接教示中に可動ロボットが掃引しない空間も移動可能空間に加えることが可能となる。したがって、可動ロボットが掃引する空間のみを移動可能空間とする場合よりも拡張された移動可能空間を得ることができるので、より最適な移動経路を探索することが可能となる。また、一の直接教示によって特定された第２の空間のうち、当該一の直接教示よりも前に行われた直接教示によって特定された第２の空間と重複しない第２の差分空間に関する情報を、第１の差分空間に関する情報とは別に教示者に通知することができる。したがって、教示者は、次回の直接教示において移動可能空間を効率的に拡張するためには、例えば、可動ロボットの可動部および教示者の身体のどちらの移動経路を工夫すればよいかを理解することができる。その結果、複数回の直接教示において効率的に移動可能空間を拡張することが可能となる。

　また、前記フィードバック部は、前記第２の差分空間と、前記一の直接教示よりも前に行われた直接教示によって特定された第２の空間との体積比を、前記第２の差分空間に関する情報として通知することが好ましい。

　この構成によれば、一の直接教示によって算出された第２の差分空間と当該一の直接教示よりも前に行われた直接教示によって特定された第２の空間との体積比を教示者に通知することができる。つまり、一の直接教示による移動可能空間の増加率を教示者に通知することができる。したがって、教示者は、一の直接教示が移動可能空間の拡張にどれだけ貢献したかを理解することが可能となる。その結果、複数回の直接教示において効率的に移動可能空間を拡張することが可能となる。

　また、前記第２の空間特定部は、三次元空間を分割した複数のボクセルのうち、直接教示中の各タイミングにおいて前記教示者の身体の少なくとも一部に内包されたボクセルの集合によって構成される空間を、前記第２の空間として特定し、前記フィードバック部は、前記第２の差分空間を構成するボクセルの数と、前記一の直接教示よりも前に行われた直接教示によって特定された第２の空間を構成するボクセルの数との比を、前記体積比として通知することが好ましい。

　この構成によれば、第２の空間を特定するときに利用されたボクセルの数を利用して体積比を通知することができ、体積比を得るための処理負荷を低減することが可能となる。

　また、前記フィードバック部は、前記第１の差分空間と前記第２の差分空間との比較結果に依存させて、前記第１の差分空間に関する情報と前記第２の差分空間に関する情報との一方を通知することが好ましい。具体的には、前記フィードバック部は、前記第１の差分空間が前記第２の差分空間より小さいときに、前記第１の差分空間に関する情報を通知し、前記第２の差分空間が前記第１の差分空間より小さいときに、前記第２の差分空間に関する情報を通知することが好ましい。

　この構成によれば、可動ロボットが掃引した空間と教示者が掃引した空間とのうち、移動可能空間の拡張に対する貢献が小さい方に関する情報を教示者に通知することができる。したがって、複数回の直接教示において効率的に移動可能空間を拡張することが可能となる。

　また、さらに、前記一の直接教示の直前に行われた直接教示が開始されたときの前記教示者の姿勢を示す値と、前記一の直接教示が開始されるときの前記教示者の姿勢を示す値との差分を計算する姿勢差分計算部を備え、前記フィードバック部は、さらに、前記一の直接教示が開始されるときに、計算された差分に基づいて前記姿勢に関する情報を前記教示者に通知することが好ましい。具体的には、前記可動ロボットは、前記複数回の直接教示のために前記教示者に把持される把持部を有し、前記教示者の姿勢は、前記把持部からみた前記教示者の肘の方向であることが好ましい。

　この構成によれば、一の直接教示の直前に行われた直接教示のときの姿勢と、これから行われる一の直接教示のときの姿勢との違いを、当該一の直接教示が開始されるときに教示者に通知することができる。したがって、教示者は、一の直接教示において、一の直接教示の直前に行われた直接教示のときの姿勢から姿勢を変更することが可能となる。その結果、複数回の直接教示において効率的に移動可能空間を拡張することが可能となる。

　また、前記姿勢を示す値は、前記把持部の軸に垂直な平面上における前記肘の方向の角度を示す第１の値と、前記肘の方向と前記平面とが成す角度を示す第２の値とを含み、前記姿勢差分計算部は、前記第１の値の差分と前記第２の値の差分とを計算し、前記フィードバック部は、前記第１の値の差分と前記第２の値の差分との比較結果に基づいて、前記姿勢に関する情報を通知することが好ましい。

　この構成によれば、把持部からみた教示者の肘の方向を示す２つの角度の前回からの差分に基づいて、より詳細な情報を教示者に通知することが可能となる。

　また、前記フィードバック部は、前記一の直接教示の直前に行われた直接教示において前記第２の差分空間が前記第１の差分空間より小さい場合に、前記一の直接教示が開始されるときに前記姿勢に関する情報を通知することが好ましい。

　この構成によれば、可動ロボットが掃引した第１の空間よりも教示者が掃引した第２の空間の方が移動可能空間の拡張に対する貢献が小さい場合に、姿勢に関する情報を通知することができる。つまり、姿勢を変更する必要性が高いときに姿勢に関する情報を通知することができるので、効率的に移動可能空間を拡張することが可能となる。

　以下、本発明の実施の形態について、図面を参照しながら説明する。なお、以下で説明する実施の形態は、いずれも本発明の好ましい一具体例を示す。つまり、以下の実施の形態で示される数値、形状、材料、構成要素、構成要素の配置及び接続形態、ステップ、ステップの順序などは、本発明の一例であり、本発明を限定する主旨ではない。本発明は、請求の範囲の記載に基づいて特定される。したがって、以下の実施の形態における構成要素のうち、本発明の最上位概念を示す独立請求項に記載されていない構成要素は、本発明の課題を達成するために必ずしも必要ではないが、より好ましい形態を構成する構成要素として説明される。

　また、以下の実施の形態において、複数回のダイレクトティーチングのうちの一のダイレクトティーチングのことを、今回のダイレクトティーチングと呼ぶ。また、一のダイレクトティーチングよりも前に行われたダイレクトティーチングのことを、前回までのダイレクトティーチングと呼ぶ。また、一のダイレクトティーチングの直前に行われたダイレクトティーチングのことを、前回のダイレクトティーチングと呼ぶ。

　（実施の形態１）
　図１は、本発明の実施の形態１における移動経路探索装置１００の基本的な機能構成を示すブロック図である。図１に示すように、移動経路探索装置１００は、ロボット形状データベース１０１と、移動空間特定部１０２と、ロボット移動空間蓄積部１０３と、空間差分計算部１０４と、フィードバック部１０５と、経路探索部１０６とを備える。

　ロボット形状データベース１０１は、自動作業を行う可動ロボット３０２の形状を示すロボット形状データを保持している。

　移動空間特定部１０２は、第１の空間特定部に相当する。移動空間特定部１０２は、複数回のダイレクトティーチングの各々において、可動部がダイレクトティーチング中に掃引する三次元空間を第１の空間（以下、「ロボット移動空間」という）として特定する。

　具体的には、移動空間特定部１０２は、可動ロボット３０２の姿勢情報とロボット形状データベース１０１に保持されているロボット形状データとに基づいて、ダイレクトティーチング中の各タイミングにおいて可動ロボット３０２が占有している三次元空間を特定する。そして、移動空間特定部１０２は、ダイレクトティーチング中の各タイミングにおいて可動ロボット３０２が占有している三次元空間を合成することにより、ロボット移動空間を特定する。

　ロボット移動空間蓄積部１０３は、移動空間特定部１０２が特定するロボット移動空間を管理する。具体的には、ロボット移動空間蓄積部１０３は、例えば内蔵するメモリ等に、特定されたロボット移動空間を示す空間データを格納する。なお、ロボット移動空間蓄積部１０３は、例えば、外部の記憶装置にロボット移動空間を示す空間データを格納してもよい。

　空間差分計算部１０４は、第１の差分空間を算出する。ここで、第１の差分空間とは、今回のダイレクトティーチングによって特定されたロボット移動空間のうち、前回までのダイレクトティーチングによって特定されたロボット移動空間と重複しない空間である。つまり、第１の差分空間とは、第１の移動可能空間に新たに追加される空間である。

　フィードバック部１０５は、空間差分計算部１０４によって算出された第１の差分空間に関するフィードバック情報を教示者３０１に通知する。本実施の形態では、フィードバック部１０５は、第１の差分空間と、前回までのダイレクトティーチングによって特定された第１の空間との体積比を、第１の差分空間に関するフィードバック情報として通知する。

　経路探索部１０６は、複数回のダイレクトティーチングが行われた後に、第１の移動可能空間内において、可動ロボット３０２の可動部の移動経路を探索する。第１の移動可能空間とは、複数回のダイレクトティーチングによって特定された複数のロボット移動空間が合成された空間である。つまり、第１の移動可能空間とは、複数のロボット移動空間の和集合である。言い換えると、第１の移動可能空間とは、複数のロボット移動空間が重畳された空間である。

　具体的には、経路探索部１０６は、与えられた探索条件に基づいて、第１の移動可能空間内において取りうる移動経路のうち最適な移動経路を探索する。つまり、経路探索部１０６は、第１の移動可能空間を探索空間として利用して、探索条件を満たす最適な移動経路を探索する。ここで、探索条件とは、例えば、始点および終点の位置、最適性の評価基準（時間優先または省電力優先など）、または制約条件などである。なお、本実施の形態において、移動経路の探索方法は、従来の探索方法が利用されればよく、探索空間内の移動経路を探索する方法であれば特に限定される必要はない。

　次に、移動経路探索装置１００の利用シーンについて説明する。

　図２は、本発明の実施の形態１における移動経路探索装置１００の利用シーンを示す図である。教示者３０１は、可動ロボット３０２の部分であって外力をセンシングする部分に手で力を加えながら、可動ロボット３０２が取るべき姿勢を連続的に教示する。図２では、教示者３０１は、可動ロボット３０２の可動部を障害物３０６と接触しないように始点３０５から終点３０７まで直接動かすことにより、可動ロボット３０２に動作を教示している。

　次に、可動ロボット３０２の三次元モデルが仮想的に配置される三次元空間について説明する。三次元空間の閉領域は、ボクセル（Ｖｏｘｅｌ）で表現される。ボクセルは、図３に示すように一定の大きさの立方体を基本単位とする。原点からの三次元座標位置が（ｘ，ｙ，ｚ）であるボクセルをＶ（ｘ，ｙ，ｚ）と表現する。各ボクセルＶ（ｘ，ｙ，ｚ）は、可動ロボット３０２が移動可能かどうか示す論理値Ｍ（ｘ，ｙ，ｚ）を持つ。Ｖ（ｘ，ｙ，ｚ）の内部にロボットの移動を妨げる障害物があるか否がか不明な場合は、論理値Ｍ（ｘ，ｙ，ｚ）は、以下の式（１）のように表される。

　Ｍ（ｘ，ｙ，ｚ）＝ｆａｌｓｅ（偽）　　　式（１）

　また、Ｖ（ｘ，ｙ，ｚ）の内部に可動ロボット３０２の移動を妨げる障害物がなく、可動ロボット３０２がＶ（ｘ，ｙ，ｚ）に進入可能な場合は、論理値Ｍ（ｘ，ｙ，ｚ）は、以下の式（２）のように表される。

　Ｍ（ｘ，ｙ，ｚ）＝ｔｒｕｅ（真）　　　式（２）

　図３の例ではハッチングされているボクセルの論理値はｆａｌｓｅであり、ハッチングされていない白のボクセルの論理値はｔｒｕｅである。すなわち、障害物との干渉を避けるためには、可動ロボット３０２は、ハッチングされている空間への進入を避けながら、ハッチングされていない空間を移動しなければならない。以下の式（３）のように、移動可能空間は、論理値がｔｒｕｅであるボクセルの集合として表現される。

　∪｛Ｖ（ｘ，ｙ，ｚ）｜Ｍ（ｘ，ｙ，ｚ）＝ｔｒｕｅ｝　　　式（３）

　このように、障害物に接触するか否かに関わらず可動ロボット３０２が到達可能な三次元空間は、ボクセルによって表現される。なお、教示開始前において、すべてのボクセルの論理値はｆａｌｓｅに設定される。すなわち、教示開始前には、可動ロボット３０２が到達可能な空間のすべては、移動可能かどうか不明である。

　次に、本実施の形態における可動ロボット３０２の詳細を説明する。

　図４は、本発明の実施の形態１における可動ロボット３０２の外観図である。可動ロボット３０２は、例えば６軸の垂直多関節ロボットである。図４の（ａ）に示すように、本実施の形態では可動ロボット３０２の物理的機構構造は、Ｒ－Ｐ－Ｐ－Ｒ－Ｐ－Ｒ（Ｒは回転、Ｐは並進）であるとする。

　ここで、６個の回転軸５０１～５０６の角度の組み合わせを（ＪＴ１，ＪＴ２，ＪＴ３，ＪＴ４，ＪＴ５，ＪＴ６）で表現する。この６個の回転軸５０１～５０６の角度の組み合わせによって、可動ロボット３０２の姿勢は一意に決定される。

　また、ロボット形状データベース１０１は、図４に示すロボットの形状を線分あるいは曲線分で閉領域表現したロボット形状データを保持している。したがって、（ＪＴ１，ＪＴ２，ＪＴ３，ＪＴ４，ＪＴ５，ＪＴ６）が与えられれば、移動空間特定部１０２は、ロボット形状データを利用して、固定部５０７に対する可動部５０８の相対的な三次元位置を一意に算出することができる。

　例えば、図４の（ｂ）に示すように、ＪＴ１＝ＪＴ２＝ＪＴ３＝ＪＴ４＝ＪＴ５＝ＪＴ６＝０［ｄｅｇ］の場合に可動ロボット３０２が直立姿勢になるようにキャリブレーションされているとする。このとき、ＪＴ１＝ＪＴ２＝ＪＴ４＝ＪＴ６＝０［ｄｅｇ］かつＪＴ３＝ＪＴ５＝－９０［ｄｅｇ］の場合は、可動ロボット３０２は、図４の（ｃ）に示すような姿勢となる。

　教示者３０１は、ダイレクトティーチングの際に、可動ロボット３０２の把持部５０９を握り、可動ロボット３０２の内部に実装された力センサに対して力を加えることにより、可動ロボット３０２の姿勢を変形させることができる。

　なお、可動ロボット３０２は、必ずしも上記のような機構構造を有するロボットである必要はない。つまり、可動ロボット３０２は、Ｒ－Ｐ－Ｐ－Ｒ－Ｐ－Ｒ以外の機構構造を有する垂直多関節ロボットであってもよい。あるいは、可動ロボット３０２は、水平多関節ロボットであってもよい。また、可動ロボット３０２は、単軸ロボットの組み合わせであってもよい。

　次に、以上のように構成された移動経路探索装置１００における各種動作について説明する。

　図５は、本発明の実施の形態１における移動経路探索処理を示すフローチャートである。以下、教示者３０１が、可動ロボット３０２に対して、障害物３０６を避けながら始点３０５から終点３０７へ移動する動作のダイレクトティーチングを繰り返す例を用いて説明する。

　教示者３０１が可動ロボット３０２の把持部５０９を握りながらダイレクトティーチングを行うと、各回転軸の角度を示す値（ＪＴ１，ＪＴ２，ＪＴ３，ＪＴ４，ＪＴ５，ＪＴ６）が変化して可動ロボット３０２は変形する。

　このとき、移動空間特定部１０２は、可動ロボット３０２の可動部５０８が占有している空間を特定する（Ｓ６０１）。具体的には、まず、移動空間特定部１０２は、一定周期Ｔで可動ロボット３０２の各回転軸の角度を示す値を姿勢情報として可動ロボット３０２から取得する。続いて、移動空間特定部１０２は、ロボット形状データベース１０１に保持されているロボット形状データを取得する。そして、移動空間特定部１０２は、取得した姿勢情報とロボット形状データとに基づいて、三次元空間を構成する各ボクセルが可動ロボット３０２の内部に位置するか否かの内部判定処理を行う。

　内部判定処理とは、あるボクセルを構成するすべての点が、ロボット形状データが示す閉領域に含まれる（以下、「ボクセルが可動ロボット３０２に内包される」という）かどうかを、既知のｐｏｉｎｔ　ｉｎ　ｐｏｌｙｇｏｎ　ｔｅｓｔなどの手法により判定する処理をいう。

　ここで、ボクセルが可動ロボット３０２に内包される場合は、移動空間特定部１０２は、そのボクセルは可動部５０８が移動可能なボクセルであると判断する。そして、移動空間特定部１０２は、そのボクセルの論理値をｔｒｕｅに設定する。一方、ボクセルが可動ロボット３０２に内包されない場合は、移動空間特定部１０２は、そのボクセルに設定されている論理値を変更しない。移動空間特定部１０２は、このような内部判定処理によって論理値がｔｒｕｅと設定されたボクセルの集合を、可動ロボット３０２の可動部５０８が占有している空間として特定する。

　ここで図６を用いて、ステップＳ６０１の処理についてさらに詳細に説明する。

　図６は、本発明の実施の形態１における移動空間特定部１０２による内部判定処理を説明するための図である。

　図６では、説明を簡単にするために、二次元平面に射影したときの、各ボクセルと可動ロボット３０２の一部との位置関係を示している。図６に示すように、可動ロボット３０２に内包されるボクセル７０２の論理値がｔｒｕｅと設定される。つまり、一部でも可動ロボット３０２の外側に位置するボクセル７０１の論理値は変更されない。

　ここで、移動空間特定部１０２が可動ロボット３０２に内包されると判定したボクセルの集合Ｒは、以下の式（４）によって表される。

　Ｒ＝∪｛Ｖ（ｘ，ｙ，ｚ）｜Ｍ（ｘ，ｙ，ｚ）＝ｔｒｕｅ｝　　　式（４）

　ここでは、ボクセルの集合Ｒは、ダイレクトティーチング中の一タイミングにおいて可動ロボット３０２に占有されている空間に相当する。

　ここで、教示者３０１が、ｋ回目（ｋは１以上の整数）のダイレクトティーチングの終了を指示しない限り、ステップＳ６０１の処理が継続される。つまり、移動空間特定部１０２は、ｋ回目のダイレクトティーチングの終了指示を教示者３０１から受け付けるまで、一定周期Ｔで内部判定処理を繰り返す（Ｓ６０２）。

　ｋ回目のダイレクトティーチングが終了したときには、ボクセルの集合Ｒは、ｋ回目のダイレクトティーチング中の各タイミングにおいて一度でも可動ロボット３０２に内包されると判定されたボクセルの集合となる。このようにして得られたボクセルの集合Ｒが、ｋ回目のダイレクトティーチング中に可動ロボット３０２の可動部５０８が掃引するロボット移動空間に相当する。

　次に、ｋ回目のダイレクトティーチングが終了した後に、ロボット移動空間蓄積部１０３は、ｋ回目のダイレクトティーチングにおいて特定されたロボット移動空間を示す空間データをメモリなどに格納する（Ｓ６０３）。

　続いて、空間差分計算部１０４は、今回（ｋ回目）のダイレクトティーチングにおいて特定されたロボット移動空間のうち、前回（ｋ－１回目）までのダイレクトティーチングにおいて特定されたロボット移動空間と重複しない空間を、第１の差分空間として算出する（Ｓ６０４）。具体的には、空間差分計算部１０４は、例えばメモリなどに格納された空間データを利用して第１の差分空間を算出する。

　次に、フィードバック部１０５は、第１の差分空間に関する情報を教示者３０１に通知する（Ｓ６０５）。具体的には、フィードバック部１０５は、第１の差分空間と、前回までのダイレクトティーチングによって特定されたロボット移動空間との体積比を、第１の差分空間に関する情報として通知する。

　ここで、教示者３０１からダイレクトティーチングの繰り返しの指示を受け付けた場合（Ｓ６０６のＹｅｓ）、ステップＳ６０１の処理に戻り、（ｋ＋１）回目のダイレクトティーチングが行われる。一方、教示者３０１からダイレクトティーチングの繰り返しの指示を受け付けなかった場合（Ｓ６０６のＮｏ）、経路探索部１０６は、ｋ回目までのダイレクトティーチングによって特定された複数のロボット移動空間を合成して得られる第１の移動可能空間内において、可動ロボット３０２の可動部５０８の移動経路を探索する（Ｓ６０７）。

　このように、移動経路探索装置１００は、複数回のダイレクトティーチングに基づいて、可動ロボット３０２の可動部５０８の移動経路を探索する。

　次に、図５のステップＳ６０３～Ｓ６０５の処理を、図７～図１４を用いてさらに詳細に説明する。まず、１回目のダイレクトティーチングが終了したときの処理を図７および図８を用いて説明する。なお、１回目のダイレクトティーチングの前には、ボクセルの集合Ｒ＿ａｌｌは、φ（空集合）に初期化されている。

　図７は、本発明の実施の形態１における１回目のダイレクトティーチングによって特定されるロボット移動空間８０１の一例を示す図である。また、図８は、本発明の実施の形態１における１回目のダイレクトティーチングによって特定されるロボット移動空間８０１を構成するボクセルの集合Ｒ［１］を示す図である。

　まず、ロボット移動空間蓄積部１０３は、１回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［１］を示す空間データをメモリなどに格納する。

　次に、空間差分計算部１０４は、図８に示すような１回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［１］とＲ＿ａｌｌ（ここでは空集合）との差分ＤＲを、以下の式（５）のように計算する。

　ＤＲ＝｜Ｒ［１］－Ｒ＿ａｌｌ｜＝１０６　　　式（５）

　ここで、Ａ－ＢはＡ∩（Ｂの補集合）の略記であり、｜Ａ｜はＡの要素数である。また、Ｒ＿ａｌｌは、前回までのダイレクトティーチングによって特定されたロボット移動空間が合成された空間を構成するボクセルの集合である。つまり、Ｒ＿ａｌｌは、前回までのダイレクトティーチングによって得られる第１の移動可能空間に相当する。

　このように、空間差分計算部１０４は、今回のダイレクトティーチングによって特定されたロボット移動空間のうち、前回までのダイレクトティーチングによって特定されたロボット移動空間と重複しない空間を第１の差分空間として算出する。そして、空間差分計算部１０４は、算出した第１の差分空間を構成するボクセルの数を差分ＤＲとして計算する。

　なお、ここでは１回目のダイレクトティーチングであるので、Ｒ＿ａｌｌは空集合である。したがって、空間差分計算部１０４は、差分ＤＲを計算しなくてもよい。また、差分ＤＲが計算されないときは、フィードバック部１０５は、教示者３０１に対してフィードバック情報を通知しない。

　次に、２回目のダイレクトティーチングが終了したときの処理を図９～図１２を用いて説明する。

　図９は、本発明の実施の形態１における２回目のダイレクトティーチングによって特定されるロボット移動空間１００１の一例を示す図である。また、図１０は、本発明の実施の形態１における２回目までのダイレクトティーチングによって特定されたロボット移動空間を構成するボクセルの集合を示す図である。

　なお、以下の処理が行われる前に、ロボット移動空間蓄積部１０３は、ボクセルの集合Ｒ＿ａｌｌを、１回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［１］を用いて、以下のように更新している。

　Ｒ＿ａｌｌ←Ｒ＿ａｌｌ∪Ｒ［１］

　まず、ロボット移動空間蓄積部１０３は、２回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［２］を示す空間データをメモリなどに格納する。

　次に、空間差分計算部１０４は、２回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［２］と、前回までのダイレクトティーチングによって得られたボクセルの集合Ｒ＿ａｌｌとの差分ＤＲを計算する。つまり、空間差分計算部１０４は、２回目のダイレクトティーチングによって特定されたロボット移動空間のうち、１回目のダイレクトティーチングによって特定されたロボット移動空間と重複しない空間を第１の差分空間として算出する。そして、空間差分計算部１０４は、算出した第１の差分空間を構成するボクセルの数を差分ＤＲとして計算する。

　説明を簡単にするために、図６と同様に、三次元空間におけるボクセルを二次元平面に射影したときのボクセルを示す図１０を用いて以下の処理を説明する。図１０では、差分ＤＲと｜Ｒ＿ａｌｌ｜とは、以下の値となる。

　ＤＲ＝｜Ｒ［２］－Ｒ＿ａｌｌ｜＝３１
　｜Ｒ＿ａｌｌ｜＝１０６

　空間差分計算部１０４は、このようにして得られた差分ＤＲと｜Ｒ＿ａｌｌ｜とを差分情報としてフィードバック部１０５に出力する。

　フィードバック部１０５は、Ｒ＿ａｌｌの増加率が３１／１０６＝２９．２［％］であることを教示者３０１に通知する。つまり、フィードバック部１０５は、第１の差分空間を構成するボクセルの数と、１回目までのダイレクトティーチングによって特定されたロボット移動空間を構成するボクセルの数との比を、教示者３０１に通知する。これにより、教示者３０１は、今回のダイレクトティーチングが移動可能空間の拡張にどれだけ貢献したかを理解することが可能となる。

　なお、本実施の形態では、各ボクセルの体積は同一であるので、ボクセル数の比は体積比に相当する。つまり、フィードバック部１０５は、ボクセルの数を利用して体積比を通知することができ、体積比を得るための処理負荷を低減することができる。

　また、通知方法としては、フィードバック部１０５は、例えば図１１に示すように、ディスプレイに、計算された比を示す数値を表示すればよい。あるいは、フィードバック部１０５は、例えば図１２に示すように、音声合成を利用して、計算された比を示す数値を読み上げてもよい。

　次に、３回目のダイレクトティーチングが終了したときの処理を図１３および図１４を用いて説明する。

　図１３は、本発明の実施の形態１における３回目のダイレクトティーチングによって特定されるロボット移動空間１２０１の一例を示す図である。また、図１４は、本発明の実施の形態１における３回目までのダイレクトティーチングによって特定されたロボット移動空間を構成するボクセルの集合を示す図である。

　なお、以下の処理が行われる前に、ロボット移動空間蓄積部１０３は、ボクセルの集合Ｒ＿ａｌｌを、２回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［２］を用いて、以下のように更新している。

　Ｒ＿ａｌｌ←Ｒ＿ａｌｌ∪Ｒ［２］

　まず、ロボット移動空間蓄積部１０３は、３回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［３］を示す空間データをメモリなどに格納する。

　次に、空間差分計算部１０４は、３回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［３］と、前回までのダイレクトティーチングによって得られたボクセルの集合Ｒ＿ａｌｌとの差分ＤＲを計算する。つまり、空間差分計算部１０４は、３回目のダイレクトティーチングによって特定されたロボット移動空間のうち、２回目までのダイレクトティーチングによって特定されたロボット移動空間と重複しない空間を第１の差分空間として算出する。そして、空間差分計算部１０４は、算出した第１の差分空間を構成するボクセルの数を差分ＤＲとして計算する。

　説明を簡単にするために、図６および図１０と同様に、三次元空間におけるボクセルを二次元平面に射影したときのボクセルを示す図１２を用いて、以下の処理を説明する。図１２では、差分ＤＲと｜Ｒ＿ａｌｌ｜とは、以下の値となる。

　ＤＲ＝｜Ｒ［３］－Ｒ＿ａｌｌ｜＝１３
　｜Ｒ＿ａｌｌ｜＝１３７

　空間差分計算部１０４は、このように得られた差分ＤＲと｜Ｒ＿ａｌｌ｜とを差分情報としてフィードバック部１０５に出力する。

　フィードバック部１０５は、Ｒ＿ａｌｌの増加率が１３／１３７＝９．４９［％］であることを教示者３０１に通知する。つまり、フィードバック部１０５は、第１の差分空間を構成するボクセルの数と、２回目までのダイレクトティーチングによって特定されたロボット移動空間を構成するボクセルの数との比を教示者３０１に通知する。

　これにより、教示者３０１は、第１の移動可能空間の増加率が２回目のダイレクトティーチングのときよりも減少していることを認識することができる。つまり、教示者３０１は、移動可能空間を拡張するためには、次回のダイレクトティーチングにおいて、今回までの移動経路から少し変更した移動経路を教示することが必要であることを理解することができる。そこで、次回のダイレクトティーチングにおいて移動経路を変更することが可能であれば、教示者３０１は、変更された移動経路を教示することにより、移動可能空間を効率的に拡張することができる。あるいは、次回のダイレクトティーチングにおいて移動経路を変更することが困難であれば、教示者３０１は、ダイレクトティーチングを速やかに終了することを指示できる。

　次に、図５のステップＳ６０７の処理を、図１５および図１６を用いてさらに詳細に説明する。

　図１５は、本発明の実施の形態１における探索条件の一例を説明するための図である。また、図１６は、本発明の実施の形態１における移動経路探索装置１００によって探索される移動経路の一例を示す図である。なお、図１５および図１６は、説明を簡単にするため、三次元空間を二次元平面に射影したときの図を示す。

　図１５に示すように、経路探索部１０６は、可動ロボット３０２の可動部５０８の先端を、始点１５０１から終点１５０２まで最短時間で移動させることを探索条件として、移動経路を探索する。本実施の形態における移動経路探索装置１００による経路探索では、図１６に示す通り、教示者３０１がダイレクトティーチングを複数回行うことにより特定された移動可能空間を探索空間として利用することができる。したがって、図１６に示すような、垂直多関節ロボットが最速で移動することができる円弧状の経路を移動経路１６０１として探索することが可能になる。

　以上のように、本実施の形態に係る移動経路探索装置１００によれば、今回のダイレクトティーチングによって特定されたロボット移動空間のうち、前回までのダイレクトティーチングによって特定されたロボット移動空間と重複しない第１の差分空間に関する情報を教示者３０１に通知することができる。したがって、教示者は、通知された情報を参考にして、次回のダイレクトティーチングを行うことが可能となる。その結果、複数回のダイレクトティーチングに基づいて移動可能空間を取得する場合に、複数回のダイレクトティーチングにおいて効率的に移動可能空間を拡張することが可能となる。例えば、従来よりも少ないダイレクトティーチングの回数で従来と同等のボリュームの移動可能空間を獲得することができる。

　なお、本実施の形態において、フィードバック部１０５は、ボクセルの数の比を教示者３０１に通知していたが、必ずしもボクセル数の比を通知する必要はない。例えば、フィードバック部１０５は、ボクセルの数そのものを通知してもよい。また例えば、フィードバック部１０５は、第１の移動可能空間が拡張された様子を、直接的かつ視覚的に教示者３０１に通知してもよい。例えば、フィードバック部１０５は、各回のダイレクトティーチングが終了したときに、図８、図１０または図１４に示す内容をディスプレイに表示させてもよい。これにより、教示者３０１は、可動ロボット３０２に三次元空間のどの部分を通過させれば移動可能空間が最も拡張されるかを直接的に知覚することが可能になる。

　（実施の形態２）
　次に、本発明の実施の形態２について図面を参照しながら説明する。なお、以下において、実施の形態１と共通する部分については、適宜説明を省略する。

　なお、本実施の形態における移動経路探索装置１７００は、ダイレクトティーチング中に教示者３０１の身体が掃引した空間も、移動経路の探索空間として利用する点が、実施の形態１における移動経路探索装置１００と主として異なる。

　図１７は、本発明の実施の形態２における移動経路探索装置１７００の基本的な機能構成を示すブロック図である。なお、図１７において、図１と同様の構成要素については、同一の符号を付し、説明を省略する。

　図１７に示すように、移動経路探索装置１７００は、図１に示す実施の形態１における移動経路探索装置１００と同様に、ロボット形状データベース１０１と、移動空間特定部１０２と、ロボット移動空間蓄積部１０３とを備える。また、移動経路探索装置１７００は、教示者位置検出部１７０３と、教示者形状データベース１７０４と、人占有空間特定部１７０５と、人占有空間蓄積部１７０７と、空間差分計算部１７０８と、フィードバック部１７０９と、空間合成部１７１０と、経路探索部１７１１とを備える。

　教示者位置検出部１７０３は、教示者３０１の身体の少なくとも一部の三次元位置を検出する。本実施の形態では、教示者位置検出部１７０３は、可動ロボット３０２に動作を教示する教示者３０１の肘の三次元位置を検出する。なお、教示者位置検出部１７０３は、必ずしも肘の三次元位置を検出する必要はなく、例えば肩あるいは手首などの関節部位の三次元位置を検出してもよい。また、教示者位置検出部１７０３は、肘の三次元位置に加えて、例えば肩あるいは手首などの三次元位置を検出してもよい。

　教示者形状データベース１７０４は、教示者３０１の身体の少なくとも一部の形状を示す教示者形状データを保持している。本実施の形態では、教示者形状データベース１７０４は、教示者３０１の下腕の形状を示す教示者形状データを保持している。なお、教示者形状データベース１７０４は、さらに、教示者３０１の上腕の形状を示す教示者形状データを保持してもよい。またさらに、教示者形状データベース１７０４は、教示者３０１の全身の形状を示す教示者形状データを保持してもよい。

　人占有空間特定部１７０５は、第２の空間特定部に相当する。人占有空間特定部１７０５は、複数回のダイレクトティーチングの各々において、教示者３０１の身体の少なくとも一部がダイレクトティーチング中に掃引する三次元空間を第２の空間（以下、「人占有空間」という）として特定する。本実施の形態では、人占有空間特定部１７０５は、ダイレクトティーチング中に教示者３０１の下腕が掃引する三次元空間を人占有空間として特定する。

　人占有空間蓄積部１７０７は、人占有空間特定部１７０５が特定する人占有空間を管理する。具体的には、人占有空間蓄積部１７０７は、例えば内蔵するメモリ等に、特定された人占有空間を示す空間データを格納する。なお、人占有空間蓄積部１７０７は、例えば、外部の記憶装置に人占有空間を示す空間データを格納してもよい。

　空間差分計算部１７０８は、実施の形態１と同様に第１の差分空間を算出することに加えて、第２の差分空間を算出する。ここで、第２の差分空間とは、今回のダイレクトティーチングによって特定された人占有空間のうち、前回までのダイレクトティーチングによって特定された人占有空間と重複しない空間である。

　フィードバック部１７０９は、第１の差分空間に関するフィードバック情報に加えて、第２の差分空間に関するフィードバック情報を教示者３０１に通知する。

　空間合成部１７１０は、第１の移動可能空間と第２の移動可能空間とを合成する。つまり、空間合成部１７１０は、第１の移動可能空間と第２の移動可能空間との和集合を算出する。ここで、第１の移動可能空間とは、複数回のダイレクトティーチングによって特定された複数のロボット移動空間が合成された空間である。また、第２の移動可能空間とは、複数回のダイレクトティーチングによって特定された複数の人占有空間が合成された空間である。

　経路探索部１７１１は、第１の移動可能空間内ではなく、空間合成部１７１０によって合成された空間内において、可動ロボット３０２の可動部５０８の移動経路を探索する。

　次に、移動経路探索装置１７００の利用シーンについて説明する。

　図１８は、本発明の実施の形態２における移動経路探索装置１７００の利用シーンを示す図である。図１８では、教示者３０１は、可動ロボット３０２の一部である把持部を手で把持している。そして、教示者３０１は、可動ロボット３０２の可動部を障害物３０６と接触しないように始点３０５から終点３０７まで直接動かすことにより、可動ロボット３０２に動作を教示している。

　教示者３０１の肘には、三次元位置計測デバイス１８０３が取り付けられている。また、作業環境内（例えば可動ロボット３０２が設置された部屋の天井あるいは壁など）には、位置センシングデバイス１８０４が設置される。三次元位置計測デバイス１８０３と位置センシングデバイス１８０４とは、例えばＵＷＢ（Ｕｌｔｒａ　Ｗｉｄｅ　Ｂａｎｄ）を利用した公知の屋内ＧＰＳ（Ｇｌｏｂａｌ　Ｐｏｓｉｔｉｏｎｉｎｇ　Ｓｙｓｔｅｍ）などを実現する。つまり、教示者位置検出部１７０３は、三次元位置計測デバイス１８０３と位置センシングデバイス１８０４とを備える。そして、位置センシングデバイス１８０４は、三次元位置計測デバイス１８０３が取り付けられた、教示者３０１の身体の一部の三次元位置を検出する。

　次に、以上のように構成された移動経路探索装置１７００における各種動作について説明する。

　図１９は、本発明の実施の形態２における移動経路探索処理を示すフローチャートである。以下、教示者３０１が、可動ロボット３０２に対して、障害物３０６を避けながら始点３０５から終点３０７へ移動する動作のダイレクトティーチングを繰り返す例を用いて説明する。

　教示者３０１が可動ロボット３０２の把持部５０９を握りながらダイレクトティーチングを行うと、各回転軸の角度を示す値（ＪＴ１，ＪＴ２，ＪＴ３，ＪＴ４，ＪＴ５，ＪＴ６）が変化して可動ロボット３０２は変形する。　このとき、移動空間特定部１０２は、実施の形態１のステップＳ６０１と同様に、可動ロボット３０２の可動部５０８が占有している空間を特定する（Ｓ１９０１）。具体的には、移動空間特定部１０２は、図２０に示すように、可動ロボット３０２に内包されるボクセル（論理値がｔｒｕｅと設定されたボクセル）の集合Ｒを特定する。

　次に、教示者位置検出部１７０３は、三次元位置計測デバイス１８０３と位置センシングデバイス１８０４とを利用して、教示者３０１の肘の三次元位置Ｇ＿ｈを検出する（Ｓ１９０２）。

　続いて、人占有空間特定部１７０５は、教示者３０１の下腕が占有している空間を特定する（Ｓ１９０３）。ここで、ステップＳ１９０３の処理の詳細を説明する前に、図２１を用いて、教示者形状データベースに保持されている教示者形状データについて説明する。

　図２１は、本発明の実施の形態２における教示者形状データベース１７０４が保持している教示者形状データの一例を示す図である。教示者形状データベース１７０４は、教示者３０１の下腕の近似形状を示すデータを保持している。教示者３０１の下腕は、例えば図２１に示すように円柱形状の三次元モデル２２０１で表される。円柱形状の三次元モデル２２０１は、２つの数値パラメータ（半径ｒと高さｈ）によって規定される。

　本実施の形態では、半径ｒおよび高さｈは、例えば、図示しない入力部を介して教示者３０１によってあらかじめ入力された値である。なお、三次元モデル２２０１は、必ずしも半径ｒおよび高さｈによって規定される必要はない。例えば、三次元モデルは、円周長と高さとによって規定されてもよい。

　なお、半径ｒは、教示者３０１の下腕のうち最も外周が短い部分の半径であることが好ましい。これにより、教示者３０１の下腕を過大にモデル化することによって、障害物が位置する空間が移動可能空間と算出されることを抑制することができる。

　また、教示者３０１の下腕の三次元モデルは、必ずしも円柱形状で表される必要はなく、より実際の下腕の形状に近い形状で表されてもよい。ただし、三次元モデルの形状が複雑になるほど内部判定処理の負荷が増大する。したがって、三次元モデルの形状は、本実施の形態のように、比較的単純であることが好ましい。

　人占有空間特定部１７０５は、このように教示者形状データベース１７０４に保持されている教示者形状データを用いて、教示者３０１の下腕が占有している空間を特定する。具体的には、人占有空間特定部１７０５は、教示者３０１の肘の三次元位置Ｇ＿ｈと可動ロボット３０２の把持部５０９の三次元位置Ｇ＿ｒとを利用して、教示者形状データが示す三次元モデル２２０１を三次元空間に仮想的に配置する。そして、人占有空間特定部１７０５は、三次元空間に配置された三次元モデル２２０１に対して内部判定処理を行う。

　図２２は、本発明の実施の形態２における人占有空間特定部１７０５による内部判定処理を説明するための図である。図２２に示すように、人占有空間特定部１７０５は、三次元空間を構成するボクセルのうち三次元モデル２２０１に内包されるボクセルの論理値をｔｒｕｅと設定する。このように、人占有空間特定部１７０５が教示者３０１の三次元モデル２２０１に内包されると判定したボクセルの集合Ｈは、以下の式（６）によって表される。

　Ｈ＝∪｛Ｖ（ｘ，ｙ，ｚ）｜Ｍ（ｘ，ｙ，ｚ）＝ｔｒｕｅ｝　　　式（６）

　このボクセルの集合Ｈは、ダイレクトティーチング中の一タイミングにおいて教示者３０１の身体の少なくとも一部に占有されている空間に相当する。

　ここで、教示者３０１が、ｋ回目のダイレクトティーチングの終了を指示しない限り、ステップＳ１９０１～Ｓ１９０３の処理が継続される。つまり、人占有空間特定部１７０５は、ｋ回目のダイレクトティーチングの終了指示を教示者３０１から受け付けるまで、一定周期Ｔで内部判定処理を繰り返す（Ｓ１９０４）。

　その結果、ｋ回目のダイレクトティーチングが終了したときに、ボクセルの集合ＲおよびＨは、ｋ回目のダイレクトティーチング中の各タイミングにおいて一度でも可動ロボット３０２または教示者３０１に内包されると判定されたボクセルの集合となる。

　このように得られたボクセルの集合Ｒは、ｋ回目のダイレクトティーチング中に可動ロボット３０２の可動部５０８が掃引するロボット移動空間に相当する。また、このように得られるボクセルの集合Ｈは、ｋ回目のダイレクトティーチング中に教示者３０１の身体の少なくとも一部が掃引する人占有空間に相当する。

　図２３は、本発明の実施の形態２における移動空間特定部１０２によって特定されたロボット移動空間２１０１の一例を示す図である。また、図２４は、本発明の実施の形態２における人占有空間特定部１７０５によって特定された人占有空間２４０１の一例を示す図である。

　図２３および図２４に示すように、ｋ回目のダイレクトティーチングが終了した後に、移動空間特定部１０２および人占有空間特定部１７０５によってロボット移動空間２１０１と人占有空間２４０１とが特定される。

　ロボット移動空間蓄積部１０３と人占有空間蓄積部１７０７とは、このようにｋ回目のダイレクトティーチングにおいて特定されたロボット移動空間２１０１を示す空間データまたは人占有空間２４０１を示す空間データをメモリなどに格納する（Ｓ１９０５）。

　続いて、空間差分計算部１７０８は、第１の差分空間に加えて、第２の差分空間を算出する（Ｓ１９０６）。ここで、第２の差分空間とは、今回のダイレクトティーチングにおいて特定された人占有空間のうち、前回までのダイレクトティーチングにおいて特定された人占有空間と重複しない空間である。具体的には、空間差分計算部１７０８は、例えばメモリなどに格納された空間データを利用して、第１の差分空間と第２の差分空間とを算出する。

　次に、フィードバック部１７０９は、第１の差分空間に関する情報に加えて、第２の差分空間に関する情報を教示者３０１に通知する（Ｓ１９０７）。具体的には、フィードバック部１７０９は、第２の差分空間と、前回までのダイレクトティーチングによって特定された人占有空間との体積比を、第２の差分空間に関する情報として通知する。

　ここで、教示者３０１からダイレクトティーチングの繰り返しの指示を受け付けた場合（Ｓ１９０８のＹｅｓ）、ステップＳ１９０１の処理に戻り、（ｋ＋１）回目のダイレクトティーチングが行われる。一方、教示者３０１からダイレクトティーチングの繰り返しの指示を受け付けなかった場合（Ｓ１９０８のＮｏ）、空間合成部１７１０は、第１の移動可能空間と第２の移動可能空間とを合成する（Ｓ１９０９）。第２の移動可能空間とは、ｋ回目までのダイレクトティーチングによって特定された複数の人占有空間が合成された空間である。

　そして、経路探索部１７１１は、合成された移動可能空間内において、可動ロボット３０２の可動部５０８の移動経路を探索する（Ｓ１９１０）。

　このように、移動経路探索装置１７００は、複数回のダイレクトティーチングに基づいて、可動ロボット３０２の可動部５０８の移動経路を探索する。

　次に、図１９のステップＳ１９０５～Ｓ１９０７の処理を、図２５を用いてさらに詳細に説明する。

　図２５は、本発明の実施の形態２における１～３回目のダイレクトティーチングによって特定される第１の移動可能空間と第２の移動可能空間とを構成するボクセルの集合を示す図である。具体的には、図２５の（ａ）および（ｂ）は、１回目のダイレクトティーチングによって特定される第１の移動可能空間および第２の移動可能空間を構成するボクセルの集合を示す図である。また、図２５の（ｃ）および（ｄ）は、２回目までのダイレクトティーチングによって特定される第１の移動可能空間および第２の移動可能空間を構成するボクセルの集合を示す図である。また、図２５の（ｅ）および（ｆ）は、３回目までのダイレクトティーチングによって特定される第１の移動可能空間および第２の移動可能空間を構成するボクセルの集合を示す図である。

　なお、１回目のダイレクトティーチングの前には、第１の移動可能空間を構成するボクセルの集合Ｒ＿ａｌｌと、第２の移動可能空間を構成するボクセルの集合Ｈ＿ａｌｌとは、φ（空集合）に初期化されている。

　まず、ロボット移動空間蓄積部１０３は、１回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［１］を示す空間データをメモリなどに格納する。また、人占有空間蓄積部１７０７は、１回目のダイレクトティーチングによって得られたボクセルの集合Ｈ［１］を示す空間データをメモリなどに格納する。なお、１回目のダイレクトティーチングであるので、第１の差分空間および第２の差分空間の算出とフィードバック情報の通知とは行われない。

　次に、２回目のダイレクトティーチングが終了したときの処理を図２５の（ｃ）および（ｄ）を用いて説明する。なお、以下の処理が行われる前に、ロボット移動空間蓄積部１０３と人占有空間蓄積部１７０７とは、ボクセルの集合Ｒ＿ａｌｌまたはＨ＿ａｌｌを、１回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［１］またはＨ［１］を用いて、以下のように更新している。

　Ｒ＿ａｌｌ←Ｒ＿ａｌｌ∪Ｒ［１］
　Ｈ＿ａｌｌ←Ｈ＿ａｌｌ∪Ｒ［１］

　そして、ロボット移動空間蓄積部１０３は、２回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［２］を示す空間データをメモリなどに格納する。また、人占有空間蓄積部１７０７は、２回目のダイレクトティーチングによって得られたボクセルの集合Ｈ［２］を示す空間データをメモリなどに格納する。

　次に、空間差分計算部１７０８は、２回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［２］と、前回までのダイレクトティーチングによって得られたボクセルの集合Ｒ＿ａｌｌとの差分ＤＲを計算する。図２５の（ｃ）では、差分ＤＲと｜Ｒ＿ａｌｌ｜とは、以下の値となる。

　ＤＲ＝｜Ｒ［２］－Ｒ＿ａｌｌ｜＝１５
　｜Ｒ＿ａｌｌ｜＝１８２

　さらに、空間差分計算部１７０８は、２回目のダイレクトティーチングによって得られたボクセルの集合Ｈ［２］と、前回までのダイレクトティーチングによって得られたボクセルの集合Ｈ＿ａｌｌとの差分ＤＨを計算する。図２５の（ｄ）では、差分ＤＨと｜Ｈ＿ａｌｌ｜とは、以下の値となる。

　ＤＨ＝｜Ｈ［２］－Ｈ＿ａｌｌ｜＝１１
　｜Ｈ＿ａｌｌ｜＝３３

　空間差分計算部１７０８は、このようにして得られた差分ＤＲと｜Ｒ＿ａｌｌ｜と差分ＤＨと｜Ｈ＿ａｌｌ｜とを差分情報としてフィードバック部１７０９に出力する。

　フィードバック部１７０９は、Ｒ＿ａｌｌの増加率が１５／１８２＝８．２４［％］であり、Ｈ＿ａｌｌの増加率が１１／３３＝３３．３３［％］であることを教示者３０１に通知する。つまり、フィードバック部１７０９は、第１の差分空間に関する情報に加えて、第２の差分空間に関する情報を通知する。ここでは、第２の差分空間に関する情報は、第２の差分空間を構成するボクセルの数と、１回目のダイレクトティーチングによって特定された人占有空間を構成するボクセルの数との比である。

　なお、通知方法としては、フィードバック部１７０９は、例えば図２６に示すように、ディスプレイに、計算された比を示す数値を表示すればよい。あるいは、フィードバック部１７０９は、音声合成を利用して、計算された比を示す数値を読み上げてもよい。

　次に、３回目のダイレクトティーチングが終了したときの処理を図２５の（ｅ）および（ｆ）を用いて説明する。なお、以下の処理が行われる前に、ロボット移動空間蓄積部１０３と人占有空間蓄積部１７０７とは、ボクセルの集合Ｒ＿ａｌｌおよびＨ＿ａｌｌを、２回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［２］およびＨ［２］を用いて、以下のように更新している。

　Ｒ＿ａｌｌ←Ｒ＿ａｌｌ∪Ｒ［２］
　Ｈ＿ａｌｌ←Ｈ＿ａｌｌ∪Ｒ［２］

　そして、ロボット移動空間蓄積部１０３は、３回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［３］を示す空間データをメモリなどに格納する。また、人占有空間蓄積部１７０７は、３回目のダイレクトティーチングによって得られたボクセルの集合Ｈ［３］を示す空間データをメモリなどに格納する。

　次に、空間差分計算部１７０８は、３回目のダイレクトティーチングによって得られたボクセルの集合Ｒ［３］と、前回までのダイレクトティーチングによって得られたボクセルの集合Ｒ＿ａｌｌとの差分ＤＲを計算する。図２５の（ｅ）では、差分ＤＲと｜Ｒ＿ａｌｌ｜とは、以下の値となる。

　ＤＲ＝｜Ｒ［３］－Ｒ＿ａｌｌ｜＝９
　｜Ｒ＿ａｌｌ｜＝１９７

　さらに、空間差分計算部１７０８は、３回目のダイレクトティーチングによって得られたボクセルの集合Ｈ［３］と、前回までのダイレクトティーチングによって得られたボクセルの集合Ｈ＿ａｌｌとの差分ＤＨを計算する。図２５の（ｆ）では、差分ＤＨと｜Ｈ＿ａｌｌ｜とは、以下の値となる。

　ＤＨ＝｜Ｈ［３］－Ｈ＿ａｌｌ｜＝１１
　｜Ｈ＿ａｌｌ｜＝４４

　空間差分計算部１７０８は、このようにして得られた差分ＤＲと｜Ｒ＿ａｌｌ｜と差分ＤＨと｜Ｈ＿ａｌｌ｜とを差分情報としてフィードバック部１７０９に出力する。

　フィードバック部１７０９は、Ｒ＿ａｌｌの増加率が９／１９７＝４．５７［％］であり、Ｈ＿ａｌｌの増加率が１１／４４＝２５．００［％］であることを教示者３０１に通知する。つまり、フィードバック部１７０９は、第１の差分空間に関する情報に加えて、第２の差分空間に関する情報を通知する。ここでは、第２の差分空間に関する情報は、第２の差分空間を構成するボクセルの数と、２回目までのダイレクトティーチングによって特定された人占有空間を構成するボクセルの数との比である。

　その結果、教示者３０１は、ロボット移動空間による移動可能空間の増加率が減少していることに気付く。そこで、ダイレクトティーチングにおける可動部５０８の移動経路を少し変更することが可能であれば、教示者３０１は、次回のダイレクトティーチングにおいて、移動経路を変更することにより移動可能空間を効率的に拡張することができる。

　あるいは、可動部５０８の移動経路の変更が困難であれば、教示者３０１は、次回のダイレクトティーチングにおいて、可動ロボット３０２の把持部５０９の握り方を変えることにより、移動可能空間を効率的に拡張することもできる。

　また、教示者３０１は、第１の移動可能空間も第２の移動可能空間もこれ以上拡張できないと判断して、速やかにダイレクトティーチングの終了を指示することもできる。

　以上のように、本実施の形態に係る移動経路探索装置１７００によれば、ダイレクトティーチング中に可動ロボット３０２が掃引するロボット移動空間と教示者３０１の身体の少なくとも一部が掃引する人占有空間とを利用して移動可能空間を得ることができる。つまり、ダイレクトティーチング中に可動ロボット３０２が掃引しない空間も移動可能空間に加えることが可能となる。したがって、可動ロボット３０２が掃引する空間のみを移動可能空間とする場合よりも拡張された移動可能空間を得ることができるので、より最適な移動経路を探索することが可能となる。

　また、今回のダイレクトティーチングによって特定された人占有空間のうち、前回までのダイレクトティーチングによって特定された人占有空間と重複しない第２の差分空間に関する情報を、第１の差分空間に関する情報に加えて、教示者３０１に通知することができる。したがって、教示者３０１は、次回のダイレクトティーチングにおいて移動可能空間を効率的に拡張するためには、例えば、可動ロボット３０２の可動部５０８および教示者３０１の身体のどちらの移動経路を工夫すればよいかを理解することができる。その結果、複数回のダイレクトティーチングにおいて効率的に移動可能空間を拡張することが可能となる。

　（実施の形態３）
　次に、本発明の実施の形態３について図面を参照しながら説明する。なお、以下において、実施の形態２と共通する部分については、適宜説明を省略する。

　なお、本実施の形態における経路探索装置は、第１の差分空間と第２の差分空間との比較結果に応じて、教示者３０１に通知する内容を変更する点が、実施の形態２における経路探索装置と異なる。なお、本実施の形態における経路探索装置の機能構成は、実施の形態２における経路探索装置と同一であるので、図示を省略する。

　本実施の形態では、フィードバック部１７０９は、第１の差分空間と第２の差分空間との比較結果に依存させて、第１の差分空間に関する情報と前記第２の差分空間に関する情報との一方を通知する。具体的には、フィードバック部１７０９は、第１の差分空間が第２の差分空間より小さいときに、第１の差分空間に関する情報を通知する。一方、第２の差分空間が第１の差分空間より小さいときに、フィードバック部１７０９は、第２の差分空間に関する情報を通知する。より具体的には、フィードバック部１７０９は、ボクセルの集合Ｒ＿ａｌｌとボクセルの集合Ｈ＿ａｌｌとのボクセル数の増分を比較する。そして、フィードバック部１７０９は、その比較結果に依存させて、ダイレクトティーチング中の移動経路を教示者３０１に工夫させるための情報を教示者３０１に通知する。

　すなわち、第１の移動可能空間の増分の方が少ない場合は、フィードバック部１７０９は、「ダイレクトティーチング時のロボットの移動経路を工夫する」ように教示者３０１にフィードバックする。一方、第２の移動可能空間の増分の方が少ない場合は、フィードバック部１７０９は、「ダイレクトティーチング時の持ち方を工夫する」ように教示者３０１にフィードバックする。

　例えば、図２５の（ｃ）および（ｄ）に示すように、ＤＲ＝１５、ＤＨ＝１１である場合、ＤＨがＤＲより小さいので、フィードバック部１７０９は、「ダイレクトティーチング時の持ち方を工夫する」ことを指示するメッセージをディスプレイに表示する。

　また例えば、図２５の（ｅ）および（ｆ）に示すように、ＤＲ＝９、ＤＨ＝１１である場合、ＤＲがＤＨより小さいので、フィードバック部１７０９は、「ダイレクトティーチング時のロボットの移動経路を工夫する」ことを指示するメッセージをディスプレイに表示する。

　以上のように本実施の形態に係る移動経路探索装置１７００によれば、教示者３０１は、次回のダイレクトティーチングにおいて、可動部５０８の移動経路を少し変更すべきか、あるいは、可動ロボット３０２の持ち方を変更すべきかを速やかに判断することが可能になり、移動可能空間を効率的に拡張することができる。

　（実施の形態４）
　次に、本発明の実施の形態４について図面を参照しながら説明する。なお、以下において、実施の形態２と同じ部分については、適宜説明を省略する。

　なお、本実施の形態における移動経路探索装置２８００は、ダイレクトティーチングが開始されるときの教示者３０１の姿勢に関する情報を通知する点が、実施の形態３と異なる。

　図２７は、本発明の実施の形態４における移動経路探索装置の基本的な機能構成を示すブロック図である。なお、図２７において、図１７と同様の構成要素については、同一の符号を付し、説明を省略する。

　教示者位置検出部２８０３は、さらに、教示者３０１の姿勢を検出する。具体的には、教示者位置検出部２８０３は、教示開始時に、把持部５０９からみた三次元位置計測デバイス１８０３の方向を検出する。本実施の形態では、三次元位置計測デバイス１８０３は、教示者３０１の肘に装着されている。したがって、教示者位置検出部２８０３は、把持部５０９からみた教示者３０１の肘の方向を教示者３０１の姿勢として検出する。

　三次元位置計測デバイス１８０３の方向は、図２８に示すように、２つの回転角θとφとによって表される。このθの値が第１の値に相当し、φの値が第２の値に相当する。

　より具体的には、把持部５０９の中心位置２７０３と、把持部５０９が置かれる仮想的な平面２７０５の法線ベクトル２７０１とは、可動ロボット３０２の姿勢から既知である。なお、この法線ベクトル２７０１の方向は、把持部５０９の軸の方向に相当する。

　そこで、θの基準位置（θ＝０）をあらかじめ平面２７０５の一定位置２７０２とし、φの基準位置（φ＝０）を平面２７０５とすれば、教示者位置検出部２８０３は、三次元位置計測デバイス１８０３の位置から、θ（０≦θ＜２π）とφ（－π／２≦φ≦π／２）とを求めることができる。

　なお、本実施の形態では、θは、可動部５０８の先端部からみて反時計回りに計測されている。また、φは、可動部５０８の先端側を正の角度、それとは反対側を負の角度として計測されている。

　ここで、ｋ回目の教示を開始するときの把持部５０９からみた三次元位置計測デバイス１８０３の方向を、それぞれθ（ｋ）、φ（ｋ）とする。

　姿勢差分計算部２８１２は、前回のダイレクトティーチングが開始されたときの教示者３０１の姿勢を示す値と、今回のダイレクトティーチングが開始されるときの教示者３０１の姿勢を示す値との差分を計算する。なお、本実施の形態では、教示者３０１の姿勢は、把持部５０９からみた教示者３０１の肘の方向である。具体的には、姿勢差分計算部２８１２は、θの差分とφの差分とを計算する。

　フィードバック部２８０９は、ダイレクトティーチングが開始されるときに、計算された差分に基づいて姿勢に関する情報を教示者３０１に通知する。具体的には、フィードバック部２８０９は、θの差分とφの差分との比較結果に基づいて、姿勢に関する情報を通知する。

　なお、フィードバック部２８０９は、前回のダイレクトティーチングにおいて第２の差分空間が第１の差分空間より小さい場合に、今回のダイレクトティーチングが開始されるときに姿勢に関する情報を通知することが好ましい。

　次に、以上のように構成された移動経路探索装置２８００における各種動作について説明する。

　図２９は、本発明の実施の形態４における移動経路探索処理を示すフローチャートである。なお、図２９において、図１９と同一のステップについては、同一の符号を付し、適宜説明を省略する。

　フィードバック部２８０９は、Ｒ＿ａｌｌのボクセル増分に対して、Ｈ＿ａｌｌのボクセル増分が少ない場合に、「ダイレクトティーチング時の持ち方を工夫する」ことを指示するメッセージをディスプレイに表示する（Ｓ１９０７）。

　ここで、ダイレクトティーチングが繰り返されるとき（Ｓ１９０８のＹｅｓ）、教示者位置検出部２８０３は、教示者３０１の姿勢を検出する（Ｓ２９１１）。具体的には、教示者位置検出部２８０３は、例えば、教示者３０１が（ｋ＋１）回目のダイレクトティーチングを開始することを指示するためのボタンを押下したときに、θとφとを検出する。

　さらに、教示者３０１が（ｋ＋１）回目のダイレクトティーチングを開始する直前に、フィードバック部２８０９は、教示者位置検出部２８０３から、θ（ｋ＋１）、φ（ｋ＋１）、θ（ｋ）、およびφ（ｋ）を取得する。

　そして、以下のように、フィードバック部２８０９は、姿勢に関する情報をディスプレイに表示する（Ｓ２９１２）。

　つまり、｜θ（ｋ＋１）－θ（ｋ）｜＜｜φ（ｋ＋１）－φ（ｋ）｜の場合、フィードバック部２８０９は、「肘の位置はそのままで手首を回す」ことを指示するためのメッセージをディスプレイに表示する。逆に、｜φ（ｋ＋１）－φ（ｋ）｜≦｜θ（ｋ＋１）－θ（ｋ）｜の場合、フィードバック部２８０９は、「手首はそのままで肘の位置を変える」ことを指示するためのメッセージをディスプレイに表示する。

　以上のように、本実施の形態に係る移動経路探索装置２８００によれば、教示者３０１は、ダイレクトティーチングロボットの持ち方を工夫するように指示された際に、腕と手とをどのように変更すればよいかを容易に知ることが可能になり、移動可能空間を効率的に拡張することができる。

　なお、本実施の形態において、フィードバック部２８０９は、「肘の位置はそのままで手首を回す」ことを指示するためのメッセージを表示していたが、必ずしもこのようなメッセージを表示する必要はない。例えば、フィードバック部２８０９は、あらかじめ保持されている把持方法を示す画像を、姿勢に関する情報として表示してもよい。

　また、本実施の形態では、姿勢差分計算部２８１２は、角度θの差分と角度φの差分との両方を計算していたが、角度θの差分と角度φの差分との一方のみを計算してもよい。例えば、姿勢差分計算部２８１２は、角度θの差分のみを計算してもよい。この場合、例えば、フィードバック部２８０９は、差分が閾値より小さい場合に、教示者３０１に、把持部５０９を把持する手（例えば右手）を反対の手（例えば左手）に変更することを指示するための情報を通知してもよい。

　以上、本発明の一態様に係る移動経路探索装置について、実施の形態に基づいて説明したが、本発明は、これらの実施の形態に限定されるものではない。本発明の趣旨を逸脱しない限り、当業者が思いつく各種変形を本実施の形態に施したもの、あるいは異なる実施の形態における構成要素を組み合わせて構築される形態も、本発明の範囲内に含まれる。

　例えば、上記実施の形態１～４において、教示者３０１は腕を利用してダイレクトティーチングを行っていたが、例えば、足を利用してダイレクトティーチングを行う場合であっても、本発明を適用することができる。この場合、人占有空間特定部１７０５は、ダイレクトティーチング中に教示者３０１の足が掃引する空間を特定すればよい。また、姿勢差分計算部２８１２は、足の姿勢を示す値の差分を計算すればよい。

　また、本発明の一態様に係る移動経路探索装置は、上記実施の形態１～４における移動経路探索装置が備える構成要素のすべてを備える必要はない。例えば、移動経路探索装置は、図３０に示すように構成されてもよい。

　図３０は、本発明の一態様に係る移動経路探索装置１０の機能構成を示すブロック図である。図３０に示す移動経路探索装置１０は、移動空間特定部１０２と、空間差分計算部１０４と、フィードバック部１０５と、経路探索部１０６とを備える。つまり、移動経路探索装置１０は、必ずしも、ロボット形状データベース１０１と、ロボット移動空間蓄積部１０３とを備える必要はない。このような場合、移動経路探索装置１０は、ロボット形状データを、図示していない通信部あるいは入力部などを介して取得すればよい。また、移動空間特定部１０２が、過去に特定されたロボット移動空間を示す空間データを保持すればよい。

　また、本発明は、移動可能空間出力装置２０として実現されてもよい。図３１は、本発明の一態様に係る移動可能空間出力装置２０の機能構成を示すブロック図である。図３１に示すように、移動可能空間出力装置２０は、移動空間特定部１０２と、空間差分計算部１０４と、フィードバック部１０５とを備える。この場合、移動空間特定部１０２は、特定したロボット移動空間を、例えば、ディスプレイに表示すればよい。その結果、移動可能空間出力装置２０は、教示者３０１にロボット移動空間を提示することができる。また、移動空間特定部１０２は、特定したロボット移動空間を移動経路探索装置に出力してもよい。その結果、可動ロボットが掃引する空間を特定することができない移動経路探索装置であっても、ロボット移動空間を利用して最適な移動経路を探索することが可能となる。

　また、本発明は、探索された移動経路に従って可動ロボット３０２を制御する制御部を備えるロボット制御装置として実現されてもよい。

　また、上記実施の形態１～４における移動経路探索装置が備える構成要素の一部または全部は、１個のシステムＬＳＩ（Ｌａｒｇｅ　Ｓｃａｌｅ　Ｉｎｔｅｇｒａｔｉｏｎ：大規模集積回路）から構成されているとしてもよい。例えば、移動経路探索装置は、移動空間特定部１０２と、空間差分計算部１０４と、フィードバック部１０５と、経路探索部１０６とを有するシステムＬＳＩから構成されてもよい。

　システムＬＳＩは、複数の構成部を１個のチップ上に集積して製造された超多機能ＬＳＩであり、具体的には、マイクロプロセッサ、ＲＯＭ（Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）などを含んで構成されるコンピュータシステムである。前記ＲＯＭには、コンピュータプログラムが記憶されている。前記マイクロプロセッサが、前記コンピュータプログラムに従って動作することにより、システムＬＳＩは、その機能を達成する。

　なお、ここでは、システムＬＳＩとしたが、集積度の違いにより、ＩＣ、ＬＳＩ、スーパーＬＳＩ、ウルトラＬＳＩと呼称されることもある。また、集積回路化の手法はＬＳＩに限るものではなく、専用回路または汎用プロセッサで実現してもよい。ＬＳＩ製造後に、プログラムすることが可能なＦＰＧＡ（Ｆｉｅｌｄ　Ｐｒｏｇｒａｍｍａｂｌｅ　Ｇａｔｅ　Ａｒｒａｙ）、あるいはＬＳＩ内部の回路セルの接続や設定を再構成可能なリコンフィギュラブル・プロセッサを利用してもよい。

　さらには、半導体技術の進歩または派生する別技術によりＬＳＩに置き換わる集積回路化の技術が登場すれば、当然、その技術を用いて機能ブロックの集積化を行ってもよい。バイオ技術の適用等が可能性としてありえる。

　また、本発明は、このような特徴的な処理部を備える移動経路探索装置として実現することができるだけでなく、移動経路探索装置に含まれる特徴的な処理部をステップとする移動経路探索方法として実現することもできる。また、移動経路探索方法に含まれる特徴的な各ステップをコンピュータに実行させるコンピュータプログラムとして実現することもできる。そして、そのようなコンピュータプログラムを、ＣＤ－ＲＯＭ（Ｃｏｍｐａｃｔ　Ｄｉｓｃ　Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）等のコンピュータ読取可能な記録媒体あるいはインターネット等の通信ネットワークを介して流通させることができるのは、言うまでもない。

　本発明の一態様に係る移動経路探索装置は、教示後に可動ロボットが自律的に移動経路を探索して動作するロボットアプリケーションとして有用である。

　　　１０、１００、１７００、２８００　移動経路探索装置
　　　２０　移動可能空間出力装置
　　１０１　ロボット形状データベース
　　１０２　移動空間特定部
　　１０３　ロボット移動空間蓄積部
　　１０４、１７０８　空間差分計算部
　　１０５、１７０９、２８０９　フィードバック部
　　１０６、１７１１　経路探索部
　　３０１　教示者
　　３０２　可動ロボット
　　３０５、１５０１　始点
　　３０６　障害物
　　３０７、１５０２　終点
　　５０１　回転軸
　　５０７　固定部
　　５０８　可動部
　　５０９　把持部
　　７０１、７０２　ボクセル
　　８０１、１００１、１２０１、２１０１　ロボット移動空間
　１６０１　移動経路
　１７０３、２８０３　教示者位置検出部
　１７０４　教示者形状データベース
　１７０５　人占有空間特定部
　１７０７　人占有空間蓄積部
　１７１０　空間合成部
　１８０３　三次元位置計測デバイス
　１８０４　位置センシングデバイス
　２２０１　三次元モデル
　２４０１　人占有空間
　２７０１　法線ベクトル
　２７０２　一定位置
　２７０３　中心位置
　２７０５　平面
　２８１２　姿勢差分計算部

Claims (14)

1. 　教示者が可動ロボットを直接動かして動作を教示する複数回の直接教示の各々において、可動ロボットの可動部が直接教示中に掃引する三次元空間を第１の空間として特定する第１の空間特定部と、
   　前記複数回の直接教示のうちの一の直接教示によって特定された第１の空間のうち、前記一の直接教示よりも前に行われた直接教示によって特定された第１の空間と重複しない空間を第１の差分空間として算出する空間差分計算部と、
   　前記第１の差分空間に関する情報を前記教示者に通知するフィードバック部と、
   　前記複数回の直接教示が終了した後に、前記複数回の直接教示によって特定された複数の第１の空間が合成された第１の移動可能空間内において、前記可動部の移動経路を探索する経路探索部とを備える
   　移動経路探索装置。
2. 　前記フィードバック部は、前記第１の差分空間と、前記一の直接教示よりも前に行われた直接教示によって特定された第１の空間との体積比を、前記第１の差分空間に関する情報として通知する
   　請求項１に記載の移動経路探索装置。
3. 　前記第１の空間特定部は、三次元空間を分割した複数のボクセルのうち、直接教示中の各タイミングにおいて前記可動ロボットに内包されたボクセルの集合によって構成される空間を、前記第１の空間として特定し、
   　前記フィードバック部は、前記第１の差分空間を構成するボクセルの数と、前記一の直接教示よりも前に行われた直接教示によって特定された第１の空間を構成するボクセルの数との比を、前記体積比として通知する
   　請求項２に記載の移動経路探索装置。
4. 　さらに、
   　前記複数回の直接教示の各々において、前記教示者の身体の少なくとも一部が直接教示中に掃引する三次元空間を第２の空間として特定する第２の空間特定部と、
   　複数回の直接教示によって特定された複数の第１の空間が合成された第１の移動可能空間と、前記複数回の直接教示によって特定された複数の第２の空間が合成された第２の移動可能空間とを合成する空間合成部とを備え、
   　前記空間差分計算部は、さらに、前記一の直接教示によって特定された第２の空間のうち、前記一の直接教示よりも前に行われた直接教示によって特定された第２の空間と重複しない空間を第２の差分空間として算出し、
   　前記フィードバック部は、さらに、前記第２の差分空間に関する情報を前記教示者に通知し、
   　前記経路探索部は、前記第１の移動可能空間内に代えて、前記空間合成部によって合成された空間内において、前記可動部の移動経路を探索する
   　請求項１～３のいずれか１項に記載の移動経路探索装置。
5. 　前記フィードバック部は、前記第２の差分空間と、前記一の直接教示よりも前に行われた直接教示によって特定された第２の空間との体積比を、前記第２の差分空間に関する情報として通知する
   　請求項４に記載の移動経路探索装置。
6. 　前記第２の空間特定部は、三次元空間を分割した複数のボクセルのうち、直接教示中の各タイミングにおいて前記教示者の身体の少なくとも一部に内包されたボクセルの集合によって構成される空間を、前記第２の空間として特定し、
   　前記フィードバック部は、前記第２の差分空間を構成するボクセルの数と、前記一の直接教示よりも前に行われた直接教示によって特定された第２の空間を構成するボクセルの数との比を、前記体積比として通知する
   　請求項５に記載の移動経路探索装置。
7. 　前記フィードバック部は、前記第１の差分空間と前記第２の差分空間との比較結果に依存させて、前記第１の差分空間に関する情報と前記第２の差分空間に関する情報との一方を通知する
   　請求項４～６のいずれか１項に記載の移動経路探索装置。
8. 　前記フィードバック部は、前記第１の差分空間が前記第２の差分空間より小さいときに、前記第１の差分空間に関する情報を通知し、前記第２の差分空間が前記第１の差分空間より小さいときに、前記第２の差分空間に関する情報を通知する
   　請求項７に記載の移動経路探索装置。
9. 　さらに、
   　前記一の直接教示の直前に行われた直接教示が開始されたときの前記教示者の姿勢を示す値と、前記一の直接教示が開始されるときの前記教示者の姿勢を示す値との差分を計算する姿勢差分計算部を備え、
   　前記フィードバック部は、さらに、前記一の直接教示が開始されるときに、計算された差分に基づいて前記姿勢に関する情報を前記教示者に通知する
   　請求項４～８のいずれか１項に記載の移動経路探索装置。
10. 　前記可動ロボットは、前記複数回の直接教示のために前記教示者に把持される把持部を有し、
    　前記教示者の姿勢は、前記把持部からみた前記教示者の肘の方向である
    　請求項９に記載の移動経路探索装置。
11. 　前記姿勢を示す値は、前記把持部の軸に垂直な平面上における前記肘の方向の角度を示す第１の値と、前記肘の方向と前記平面とが成す角度を示す第２の値とを含み、
    　前記姿勢差分計算部は、前記第１の値の差分と前記第２の値の差分とを計算し、
    　前記フィードバック部は、前記第１の値の差分と前記第２の値の差分との比較結果に基づいて、前記姿勢に関する情報を通知する
    　請求項１０に記載の移動経路探索装置。
12. 　前記フィードバック部は、前記一の直接教示の直前に行われた直接教示において前記第２の差分空間が前記第１の差分空間より小さい場合に、前記一の直接教示が開始されるときに前記姿勢に関する情報を通知する
    　請求項９～１１のいずれか１項に記載の移動経路探索装置。
13. 　教示者が可動ロボットを直接動かして動作を教示する複数回の直接教示の各々において、可動ロボットの可動部が直接教示中に掃引する三次元空間を第１の空間として特定する第１の空間特定ステップと、
    　前記複数回の直接教示のうちの一の直接教示によって特定された第１の空間のうち、前記一の直接教示よりも前に行われた直接教示によって特定された第１の空間と重複しない空間を第１の差分空間として算出する空間差分計算ステップと、
    　前記第１の差分空間に関する情報を前記教示者に通知するフィードバックステップと、
    　前記複数回の直接教示が終了した後に、前記複数回の直接教示によって特定された複数の第１の空間が合成された第１の移動可能空間内において、前記可動部の移動経路を探索する経路探索ステップとを含む
    　移動経路探索方法。
14. 　請求項１３に記載の移動経路探索方法をコンピュータに実行させるためのプログラム。

Images