(57)【要約】【課題】高速に、かつ質の高い経路を探索することができる。【解決手段】可動部１１の初期姿勢と目標姿勢とを含む探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行し、ＲＲＴによって得られた経路を含む範囲内でロードマップ法を実行して、初期姿勢から目標姿勢へ移動する経路を探索する。【選択図】図４
2014073550.tif 000002
【特許請求の範囲】
【請求項１】
ロボットの可動部が初期姿勢から目標姿勢へ移動する経路を探索する経路探索方法であって、
前記初期姿勢と前記目標姿勢とを含む探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行することにより前記ロボットの第１の移動経路を生成する第１のステップと、
前記第１の移動経路を含む第１の範囲内でロードマップ法を実行することにより、前記第１の移動経路とは異なる第２の移動経路を生成する第２のステップと、
前記第２の移動経路を探索された経路として出力する第３のステップと、
を有することを特徴とする経路探索方法。
【請求項２】
請求項１に記載の経路探索方法において、
前記第１のステップでは、複数の第１の点を連結した前記第１の経路を生成し、
前記第２のステップでは、前記第１の点及び前記第１の点の間の点を第２の点として設定し、前記第２の点のなかから複数の点を選択し、当該選択された点を連結して前記第２の移動経路を生成する
ことを特徴とする経路探索方法。
【請求項３】
請求項１又は２に記載の経路探索方法において、
前記第２のステップと前記第３のステップとの間に行われる第４のステップであって、前記第１の範囲より狭く、かつ前記第２の移動経路の一部を含む第２の範囲内でロードマップ法を実行することにより、前記第２の移動経路とは異なる第３の移動経路を生成する第４のステップを有し、
前記第３のステップでは、前記第２の移動経路の代わりに、前記第３の移動経路を出力する
ことを特徴とする経路探索方法。
【請求項４】
請求項３に記載の経路探索方法において、
前記第４のステップでは、前記第１の点を少なくとも１つ含むように前記第２の範囲を設定する
ことを特徴とする経路探索方法。
【請求項５】
請求項３又は４に記載の経路探索方法において、
前記第４のステップでは、前記第２の範囲を異なる位置に複数設定し、当該複数設定された第２の範囲のそれぞれの内部でロードマップ法を実行することにより前記第３の移動経路を生成する
ことを特徴とする経路探索方法。
【請求項６】
請求項３から５のいずれか一項に記載の経路探索方法において、
前記第４のステップと前記第３のステップとの間に行われる第５のステップであって、前記第１の範囲より狭く、前記第２の移動経路の一部を含み、かつ前記第２の範囲とは異なる第３の範囲内でロードマップ法を実行することにより、前記第３の移動経路とは異なる第４の移動経路を生成する第５のステップと、
前記第５のステップと前記第３のステップとの間に行われる第６のステップであって、前記第３の経路と前記第４の経路とのどちらがより優れた経路であるかを評価する第６のステップと、を有し、
前記第３のステップでは、前記第２の移動経路の代わりに、前記第６のステップでより優れた経路であると評価された経路を出力する
ことを特徴とする経路探索方法。
【請求項７】
ロボットの可動部が初期姿勢から目標姿勢へ移動する経路を探索する経路探索装置であって、
前記初期姿勢と前記目標姿勢とを含むと探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行することにより前記ロボットの第１の移動経路を生成する第１移動経路生成手段と、
前記第１の移動経路を含む第１の範囲内でロードマップ法を実行することにより、前記第１の移動経路とは異なる第２の移動経路を生成する第２の移動経路生成手段と、
前記第２の移動経路を探索された経路として出力する出力手段と、
を有することを特徴とする経路探索装置。
【請求項８】
可動部を有するロボットを制御するロボット制御装置であって、
前記可動部の初期姿勢と目標姿勢とを含む探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行することにより前記ロボットの第１の移動経路を生成する第１移動経路生成手段と、
前記第１の移動経路を含む第１の範囲内でロードマップ法を実行することにより、前記第１の移動経路とは異なる第２の移動経路を生成する第２の移動経路生成手段と、
前記第２の移動経路を探索された経路として前記ロボットへ出力する出力手段と、
を有することを特徴とするロボット制御装置。
【請求項９】
初期姿勢から目標姿勢へ移動可能に形成された可動部と、
前記初期姿勢と前記目標姿勢とを含む探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行することにより前記ロボットの第１の移動経路を生成する第１移動経路生成手段と、
前記第１の移動経路を含む第１の範囲内でロードマップ法を実行することにより、前記第１の移動経路とは異なる第２の移動経路を生成する第２の移動経路生成手段と、
前記第２の移動経路を探索された経路として前記移動手段を制御する制御手段と、
を有することを特徴とするロボット。
【請求項１０】
ロボットの可動部が初期姿勢から目標姿勢へ移動する経路を探索するプログラムであって、
前記初期姿勢と前記目標姿勢とを含む探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行することにより前記ロボットの第１の移動経路を生成する第１のステップと、
前記第１の移動経路を含む第１の範囲内でロードマップ法を実行することにより、前記第１の移動経路とは異なる第２の移動経路を生成する第２のステップと、
前記第２の移動経路を探索された経路として出力する第３のステップと、
を演算装置に実行させることを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、経路探索方法、経路探索装置、ロボット制御装置、ロボット及びプログラムに関する。
【背景技術】
【０００２】
特許文献１には、現在位置から目標位置までの軌道をＲＲＴ（Rapidly-exploring Random Tree）により獲得し、ＲＲＴで発見した軌道上の位置を初期条件とした動力学シミュレーションを実行することで、すべての要求を満たす軌道の生成を行なう軌道計画装置が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００８−５５６８１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ＲＲＴは、ロードマップの作成を必要とせず高速に経路を計算することが出来るが、ロードマップ法等のその他手法と比較して、発見される経路が雑、すなわち無駄な動きの多い経路となってしまう。
【０００５】
特許文献１に記載の発明は、このＲＲＴの問題点を解決することはできるが、動力学シミュレーションを用いるため、計算時間の増加は避けられない。具体的には、処理の度にポテンシャル場を生成しなければならないため、時間がかかる。また、極値に陥ると処理が進まないという問題がある。
【０００６】
そこで、本発明は、高速に、かつ質の高い経路を探索することができる経路探索方法、経路探索装置、ロボット制御装置、ロボット及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記課題を解決するための第一の態様は、ロボットの可動部が初期姿勢から目標姿勢へ移動する経路を探索する経路探索方法であって、前記初期姿勢と前記目標姿勢とを含む探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行することにより前記ロボットの第１の移動経路を生成する第１のステップと、前記第１の移動経路を含む第１の範囲内でロードマップ法を実行することにより、前記第１の移動経路とは異なる第２の移動経路を生成する第２のステップと、前記第２の移動経路を探索された経路として出力する第３のステップと、を有することを特徴とする。
【０００８】
第一の態様によれば、ＲＲＴにより経路を探索し、その結果を用いてロードマップ法（確率的ロードマップ法）により経路を探索する。これにより、高速に、かつ質の高い経路を探索することができる。
【０００９】
ここで、前記第１のステップでは、複数の第１の点を連結した前記第１の経路を生成し、前記第２のステップでは、前記第１の点及び前記第１の点の間の点を第２の点として設定し、前記第２の点のなかから複数の点を選択し、当該選択された点を連結して前記第２の移動経路を生成してもよい。これにより、ＲＲＴにより探索された経路より優れた経路を確実に探索することができる。
【００１０】
ここで、前記第２のステップと前記第３のステップとの間に行われる第４のステップであって、前記第１の範囲より狭く、かつ前記第２の移動経路の一部を含む第２の範囲内でロードマップ法を実行することにより、前記第２の移動経路とは異なる第３の移動経路を生成する第４のステップを有し、前記第３のステップでは、前記第２の移動経路の代わりに、前記第３の移動経路を出力してもよい。これにより、計算時間をなるべく増やすことなく、より優れた経路を探索することができる。
【００１１】
ここで、前記第４のステップでは、前記第１の点を少なくとも１つ含むように前記第２の範囲を設定してもよい。これにより、ロードマップ法により確実によい経路を見つけることができる。
【００１２】
ここで、前記第４のステップでは、前記第２の範囲を異なる位置に複数設定し、当該複数設定された第２の範囲のそれぞれの内部でロードマップ法を実行することにより前記第３の移動経路を生成してもよい。これにより、計算時間をなるべく増やすことなく、より優れた経路を探索することができる。
【００１３】
ここで、前記第４のステップと前記第３のステップとの間に行われる第５のステップであって、前記第１の範囲より狭く、前記第２の移動経路の一部を含み、かつ前記第２の範囲とは異なる第３の範囲内でロードマップ法を実行することにより、前記第３の移動経路とは異なる第４の移動経路を生成する第５のステップと、前記第５のステップと前記第３のステップとの間に行われる第６のステップであって、前記第３の経路と前記第４の経路とのどちらがより優れた経路であるかを評価する第６のステップと、を有し、前記第３のステップでは、前記第２の移動経路の代わりに、前記第６のステップでより優れた経路であると評価された経路を出力してもよい。これにより、計算時間をなるべく増やすことなく、より優れた経路を探索することができる。
【００１４】
本発明の第二の態様は、ロボットの可動部が初期姿勢から目標姿勢へ移動する経路を探索する経路探索装置であって、前記初期姿勢と前記目標姿勢とを含むと探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行することにより前記ロボットの第１の移動経路を生成する第１移動経路生成手段と、前記第１の移動経路を含む第１の範囲内でロードマップ法を実行することにより、前記第１の移動経路とは異なる第２の移動経路を生成する第２の移動経路生成手段と、前記第２の移動経路を探索された経路として出力する出力手段と、を有することを特徴とする。これにより、高速に、かつ質の高い経路を探索することができる。
【００１５】
本発明の第三の態様は、可動部を有するロボットを制御するロボット制御装置であって、前記可動部の初期姿勢と目標姿勢とを含む探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行することにより前記ロボットの第１の移動経路を生成する第１移動経路生成手段と、前記第１の移動経路を含む第１の範囲内でロードマップ法を実行することにより、前記第１の移動経路とは異なる第２の移動経路を生成する第２の移動経路生成手段と、前記第２の移動経路を探索された経路として前記ロボットへ出力する出力手段と、を有することを特徴とする。これにより、高速に、かつ質の高い経路を探索することができる。
【００１６】
本発明の第四の態様は、ロボットであって、初期姿勢から目標姿勢へ移動可能に形成された可動部と、前記初期姿勢と前記目標姿勢とを含む探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行することにより前記ロボットの第１の移動経路を生成する第１移動経路生成手段と、前記第１の移動経路を含む第１の範囲内でロードマップ法を実行することにより、前記第１の移動経路とは異なる第２の移動経路を生成する第２の移動経路生成手段と、前記第２の移動経路を探索された経路として前記移動手段を制御する制御手段と、を有することを特徴とする。これにより、高速に、かつ質の高い経路を探索することができる。
【００１７】
本発明の第五の態様は、ロボットの可動部が初期姿勢から目標姿勢へ移動する経路を探索するプログラムであって、前記初期姿勢と前記目標姿勢とを含む探索範囲内でＲＲＴ（Ｒａｐｉｄｌｙ−Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ）を実行することにより前記ロボットの第１の移動経路を生成する第１のステップと、前記第１の移動経路を含む第１の範囲内でロードマップ法を実行することにより、前記第１の移動経路とは異なる第２の移動経路を生成する第２のステップと、前記第２の移動経路を探索された経路として出力する第３のステップと、を演算装置に実行させることを特徴とする。これにより、高速に、かつ質の高い経路を探索することができる。
【図面の簡単な説明】
【００１８】
【図１】本発明の一実施形態におけるロボットシステム１の構成の一例を示す図である。
【図２】ロボットシステム１の機能構成の一例を示すブロック図である。
【図３】制御部３０のハードウェア構成を示す図である。
【図４】経路探索処理（第１形態）の流れを示すフローチャートである。
【図５】ステップＳ１００の処理を説明する図である。
【図６】ステップＳ１０２の処理を説明する図である。
【図７】ステップＳ１０４の処理を説明する図である。
【図８】ステップＳ１０４の処理を説明する図である。
【図９】経路探索処理（第２形態）の流れを示すフローチャートである。
【図１０】ステップＳ１１０の処理を説明する図である。
【図１１】ステップＳ１１２の処理を説明する図である。
【図１２】経路探索処理に追加可能な処理を説明する図である。
【図１３】ステップＳ１１０の変形例を説明する図である。
【図１４】経路探索処理（第３形態）の流れを示すフローチャートである。
【図１５】ステップＳ１１４の処理を説明する図である。
【発明を実施するための形態】
【００１９】
本発明の一実施形態について、図面を参照しながら説明する。
【００２０】
図１は、本発明の一実施形態におけるロボットシステム１の構成の一例を示すシステム構成図である。本実施形態におけるロボットシステム１は、主として、ロボット１０と、撮影部２０と、制御部３０とを備える。
【００２１】
ロボット１０は、複数のジョイント（関節）１２と、複数のリンク１３とを含むアーム部を有するアーム型のロボットである。アーム部の先端には、対象物（ワーク）を把持したり、道具を把持して対象物に対して所定の作業を行ったりすることが可能なハンド１４（いわゆるエンドエフェクター）が設けられる。以下、ロボット１０のアーム部及びハンド１４を可動部１１という。
【００２２】
ジョイント１２及びハンド１４には、それらを動作させるためのアクチュエーター（図示せず）が設けられる。アクチュエーターは、例えば、サーボモーターやエンコーダーなどを備える。エンコーダーが出力するエンコーダー値は、制御部３０によるロボット１０のフィードバック制御に使用される。
【００２３】
なお、ロボット１０の構成は、本実施形態の特徴を説明するにあたって主要構成を説明したのであって、上記の構成に限られない。一般的な把持ロボットが備える構成を排除するものではない。例えば、図１では６軸のアームが示されているが、軸数（ジョイント数）をさらに増加させてもよいし、減らしてもよい。リンクの数を増減させてもよい。また、アーム、ハンド、リンク、ジョイント等の各種部材の形状、大きさ、配置、構造等も適宜変更してよい。また、エンドエフェクターはハンド１４に限られない。また、ロボットは、アームを２本有するいわゆる双腕ロボットとすることもできる。さらに、配置空間を作ることの出来るロボットであれば、どのような態様のロボット、例えば自走式のロボットにも適用することもできる。
【００２４】
撮影部２０は、可動部１１の作業領域付近を撮影して、画像データを生成するユニットである。撮影部２０は、例えば、カメラを含み、作業台、天井、壁などに設けられる。図１では、撮影部２０は、ロボット１０の上端部に設けられている。撮影部２０としては、可視光カメラ、赤外線カメラ等を採用することができる。
【００２５】
制御部３０は、ロボット１０の全体を制御する処理を行う。制御部３０は、ロボット１０の本体とは離れた場所に設置してもよいし、ロボット１０に内蔵してもよい。制御部３０がロボット１０の本体と離れた場所に設置されている場合には、制御部３０は、有線又は無線でロボット１０と接続されている。また、撮影部２０は、制御部３０ではなくロボット１０に接続されるようにしてもよいし、ロボット１０に内蔵されるようにしてもよい。
【００２６】
次に、ロボットシステム１の機能構成例について説明する。図２は、ロボットシステム１の機能ブロック図である。
【００２７】
ロボット１０は、アクチュエーターのエンコーダー値、及びセンサーのセンサー値等に基づいて可動部１１を制御する動作制御部１０１を備える。
【００２８】
制御部３０は、主として、ＲＲＴ経路発見部３０１と、衝突検出部３０２と、ロードマップ法実行部３０３とを備える。また、制御部３０は、有線又は無線によりロボット１０と通信可能に接続されている。
【００２９】
ＲＲＴ経路発見部３０１は、ＲＲＴを用いて経路を探索する。具体的には、ＲＲＴ経路発見部３０１は、探索空間上に初期位置を設定した後、探索空間内にランダムなサンプル位置を配置すると共に、初期位置からサンプル位置に向かう最近傍位置を探索し、初期位置と最近傍位置とを結んで木に追加する。ＲＲＴ経路発見部３０１は、当該処理を繰り返すことで、探索空間を網羅する木を生成する。ＲＲＴ経路発見部３０１は、木に基づいて、初期姿勢と目標姿勢との間の経路を探索する。ＲＲＴはすでに公知の手法であるため、詳細な説明を省略する。
【００３０】
衝突検出部３０２は、ＲＲＴ経路発見部３０１及びロードマップ法実行部３０３が経路を探索するときに、可動部１１が障害物に衝突するか否かを検出する。検出結果は、ＲＲＴ経路発見部及びロードマップ法実行部に出力される。可動部１１が障害物に衝突するか否かを検出する方法は、すでに公知であるため、説明を省略する。
【００３１】
ロードマップ法実行部３０３は、任意の範囲を設定し、設定した範囲内でロードマップ法（確率的ロードマップ法）を実行して経路を探索する。ロードマップ法実行部３０３は、ロードマップ作成部３０３Ａと、最短経路検出部３０３Ｂとを有する。
【００３２】
ロードマップ作成部３０３Ａは、任意の範囲を設定し、設定した範囲内でいくつかの姿勢をランダムに選び、近接する姿勢に移動可能であれば枝でつないでいき、ロードマップが連結できなければ障害物の多い領域にある姿勢からランダムウォークを行い、ロードマップを拡張する。ロードマップを生成する方法はすでに公知の手法であるため、詳細な説明を省略する。任意の範囲を設定する方法については、後に詳述する。
【００３３】
最短経路検出部３０３Ｂは、Ａ＊（Ａスター）アルゴリズム、ダイクストラ法等の公知な手法を用いて、ロードマップ作成部３０３Ａが生成したロードマップにおける最短経路を求める。Ａ＊アルゴリズム、ダイクストラ法等は、すでに公知の手法であるため、詳細な説明を省略する。
【００３４】
図３は、制御部３０の概略構成の一例を示すブロック図である。図示するように、例えばコンピューターなどで構成される制御部３０は、演算装置であるＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）３１と、揮発性の記憶装置であるＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）や不揮発性の記憶装置であるＲＯＭ（Ｒｅａｄ  ｏｎｌｙ  Ｍｅｍｏｒｙ）からなるメモリー３２と、外部記憶装置３３と、ロボット１０等の外部の装置と通信を行う通信装置３４と、マウスやキーボード等の入力装置３５と、ディスプレイ等の出力装置３６と、ＣＤドライブ、ＤＶＤドライブ等の読み書き装置３７と、制御部３０と他のユニットを接続するインターフェイス（Ｉ／Ｆ）３８とを備える。
【００３５】
上記の各機能部は、例えば、ＣＰＵ３１がメモリー３２に格納された所定のプログラムをメモリー３２に読み出して実行することにより実現される。なお、所定のプログラムは、例えば、予めメモリー３２にインストールされてもよいし、通信装置３４を介してネットワークからダウンロードされてインストール又は更新されてもよい。
【００３６】
以上のロボットシステム１の構成は、本実施形態の特徴を説明するにあたって主要構成を説明したのであって、上記の構成に限られない。例えば、ロボット１０が撮影部２０や制御部３０を備えていてもよい。また、一般的なロボットシステムが備える構成を排除するものではない。
【００３７】
次に、本実施形態における、上記構成からなるロボットシステム１の特徴的な処理について説明する。
【００３８】
＜経路探索処理（第１形態）＞
図４は、経路探索処理（第１形態）の処理の流れを示すフローチャートである。図５〜８は、経路探索処理（第１形態）を説明する図である。この処理は、例えば、図示しないボタン等を介して探索開始指示が入力されることにより開始される。
【００３９】
なお、図５〜８においては、説明を簡略化するため、ノードを平面上の位置として示しているが、実際には、この点は可動部１１の姿勢を示すものである。本実施の形態では、可動部１１は６軸（６自由度）であるため、各ノードは６次元のパラメーターで表される。なお、パラメーターは、ジョイント１２やハンド１４の位置のみでなく、角度、姿勢等を含むものである。
【００４０】
ＲＲＴ経路発見部３０１は、可動部１１の移動開始位置及び目的位置を含む領域の画像を撮影部２０から取得し、取得した画像上に動作可能範囲を設定する（ステップＳ１００）。具体的には、図５に示すように、ＲＲＴ経路発見部３０１は、可動部１１の移動開始位置Ｓと、目的位置Ｇとを含む最も小さな矩形形状の領域を動作可能範囲として設定する。図５において、動作不可能範囲は、例えば障害物等が存在する領域であり、可動部１１が干渉してはならない領域を示す。
【００４１】
なお、動作可能範囲は、矩形に限定されず、移動開始位置Ｓと目的位置Ｇとを含んでいればどのような形態でもよい。また、動作可能範囲の大きさも、移動開始位置Ｓと目的位置Ｇとを含む最も小さな範囲に限定されない。しかし、効率よく経路を発見するためには、動作可能範囲を移動開始位置Ｓと目的位置Ｇとを含む最も小さな範囲で設定することが望ましい。
【００４２】
ＲＲＴ経路発見部３０１は、動作可能範囲内でＲＲＴを実行し、経路を探索する（ステップＳ１０２）。ステップＳ１０２は、本発明の第１のステップに相当する。ステップＳ１０２において、衝突検出部３０２は、ＲＲＴを実行する各段階において、可動部１１が動作不可能範囲に干渉しないか否かを判定し、ＲＲＴ経路発見部３０１に出力する。ＲＲＴ経路発見部３０１は、衝突検出部３０２から出力された結果に基づいて、可動部１１が動作不可能範囲に干渉しない経路を探索する。
【００４３】
図６は、図５に示す動作可能範囲内でＲＲＴを実行した結果を示す。図６の白丸は、ＲＲＴにより設定されたノードＮ１であり、各ノードＮ１を連結した線がＲＲＴにより探索された経路である。なお、図６においては、経路に関係のないノードを省略している。ＲＲＴにより探索された経路は、本発明の第１の移動経路に相当し、ＲＲＴにより設定されたノードＮ１は、本発明の第１の点に相当する。
【００４４】
ＲＲＴ経路発見部３０１は、ステップＳ１０２で探索された経路の情報をロードマップ法実行部３０３へ出力する。ロードマップ作成部３０３Ａは、ＲＲＴ経路発見部３０１から出力された経路に基づいて、ステップＳ１００で取得した画像上に、ロードマップ法を実行する範囲である範囲Ａを設定する（ステップＳ１０４）。範囲Ａは、本発明の第１の範囲に相当する。
【００４５】
範囲Ａは、動作可動範囲を、ステップＳ１０２で発見された経路におけるパラメーターの最大値と最小値の範囲で限定した範囲である。言い換えると、範囲Ａは、移動開始位置Ｓと、目的位置Ｇと、ステップＳ１０２で探索された経路のすべてのノードとを含む最も小さな矩形形状の領域である。これにより、経路に使わないと思われる領域を除くことができる。図６に示す例においては、範囲Ａは、動作可能範囲と同様の範囲となる。なお、範囲Ａは、矩形に限定されず、移動開始位置Ｓと、目的位置Ｇと、ステップＳ１０２で探索された経路のすべてのノードとを含んでいればよい。
【００４６】
ここで、範囲Ａは、可能な限り小さな領域とすることが望ましい。ロードマップ法は、ノード数の二乗に比例して時間がかかるため、ノード数を減らす、すなわちロードマップ法を行う範囲を狭くすることが望まれるからである。
【００４７】
ロードマップ法実行部３０３は、ステップＳ１０４で設定した範囲Ａ内でロードマップ法を実行する（ステップＳ１０６）。ステップＳ１０６は、本発明の第２のステップに相当する。以下、ステップＳ１０６の処理について説明する。
【００４８】
ロードマップ作成部３０３Ａは、範囲Ａ内にノードを設定する。図７は、範囲Ａ内ノードを設定した様子を示す図である。図７の黒丸は、ステップＳ１０２で探索された経路のノードＮ１である。
【００４９】
図７に示すように、ロードマップ作成部３０３Ａは、ロードマップ作成に用いるノードの一部として、ステップＳ１０２で探索された経路のノードＮ１（図７黒丸参照）と、そのノードの中間点Ｎ２（図７網掛けの丸参照）をノードとして利用する。ノードの中間点Ｎ２は、本発明の第２の点に相当する。ノードＮ１の中間点Ｎ２を利用することにより、経路をショートカットできる可能性が高くなるからである。なお、ノードの中間点は、ノードとノードの真ん中の点に限らず、３等分した点でもよいし、１０等分した点でもよい。さらに、ノードの中間点の数も１つに限らず、２個でもよいし、１０個でもよい。さらに、ロードマップ作成部３０３Ａは、ノードＮ１及びノードＮ２とは関係のないノードＮ３も設定する。
【００５０】
本実施の形態では、ＲＲＴで探索された経路のノードＮ１を再利用しているため、最低でもＲＲＴで探索された経路より質の悪い経路を見つけることは無くなる。なお、このノードの設定は、ユーザーが入力できるようにしてもよい。これは、ロードマップ法実行部３０３が図７に示すような画像を出力装置３６に出力し、ユーザーが入力装置３５を介して入力することにより可能となる。
【００５１】
最短経路検出部３０３Ｂは、範囲Ａ内で設定された多数のノードのなかから最短距離を探して、それをロードマップ法により探索された経路として決定する。このロードマップ法により探索された経路は、本発明の第２の移動経路に相当する。図８は、図７に示すノードを用いて決定された経路を示す。図８に示す経路は、図６に示すＲＲＴで探索された経路より改善されていることが分かる。
【００５２】
ロードマップ法実行部３０３は、ステップＳ１０６で探索された経路をロボット１０に出力する（ステップＳ１０８）。ステップＳ１０８は、本発明の第３のステップに相当する。本実施の形態では、探索された経路をロボットに出力したが、出力装置３６等に出力してユーザーが確認できるようにしてもよい。
【００５３】
本実施の形態によれば、ＲＲＴによって探索された経路を、ロードマップ法を用いて最適化することで、短時間で、実用に耐えうる経路を発見することができる。また、ロードマップ法は軸数が増えても計算量がほとんど増えない手法であるため、経路の最適化にロードマップ法を用いることで、多数の軸を有するロボットの経路についても効率よく探索することができる。
【００５４】
なお、本実施の形態では、ロードマップ法を行う処理（ステップＳ１０６）を一度行ったが、ステップＳ１０６を繰り返し行うようにしてもよい。例えば、任意の回数をメモリー３２に記憶させておき、ロードマップ法実行部３０３がその回数だけステップＳ１０６の処理を繰り返し行うようにしてもよい。なお、任意の回数は、予め設定しておいてもよいし、ユーザーが入力装置３５を介して設定してもよい。
【００５５】
＜経路探索処理（第２形態）＞
図９は、経路探索処理（第２形態）の処理の流れを示すフローチャートである。図１０、１１は、経路探索処理（第２形態）を説明する図である。この処理は、例えば、図示しないボタン等を介して探索開始指示が入力されることにより開始される。なお、経路探索処理（第１形態）と同一の部分については、同一の符号を付し、説明を省略する。また、図１０、１１においては、経路探索処理（第１形態）と同様に、説明を簡略化するため、ノードを平面上の位置として示している。
【００５６】
ＲＲＴ経路発見部３０１は、可動部１１の移動開始位置及び目的位置を含む領域の画像を撮影部２０から取得し、取得した画像上に動作可能範囲を設定する（ステップＳ１００）。
【００５７】
ＲＲＴ経路発見部３０１は、動作可能範囲内でＲＲＴを実行し、経路を探索する（ステップＳ１０２）。ＲＲＴ経路発見部３０１は、ステップＳ１０２で探索された経路の情報をロードマップ法実行部３０３へ出力する。
【００５８】
ロードマップ作成部３０３Ａは、ＲＲＴ経路発見部３０１から出力された経路に基づいて、ステップＳ１００で取得した画像上に、ロードマップ法を実行する範囲である範囲Ａを設定する（ステップＳ１０４）。そして、ロードマップ法実行部３０３は、ステップＳ１０４で設定した範囲Ａ内でロードマップ法を実行する（ステップＳ１０６）。
【００５９】
更に、ロードマップ法実行部３０３は、ロードマップ法を用いて経路の再探索を行う（ステップＳ１１０〜１１２）。ロードマップ作成部３０３Ａは、範囲Ａより小さい範囲であって、かつステップＳ１０６で探索された経路を含む範囲Ｂを、動作可能範囲内に設定する（ステップＳ１１０）。範囲Ｂは、本発明の第２の範囲に相当する。
【００６０】
図１０は、動作可能範囲内に範囲Ｂを設定した様子を示す図である。図１０では、３個の範囲Ｂが設定されている。範囲Ｂは、全て範囲Ａより小さい。また、範囲Ｂは、図８に示すステップＳ１０６で探索された経路を含んでいる。そして、全ての範囲Ｂを設定することにより、ステップＳ１０６で探索された経路は、全て範囲Ｂに含まれている。
【００６１】
特に、図１０においては、範囲Ｂが、ステップＳ１０６で探索された経路を構成するノードを少なくとも１つ含んでいる点に特徴がある。このようにノードを少なくとも１つ含むことで、経路をショートカットできる可能性が高くなるからである。また、範囲を狭くすることで、ノードを減らし、処理を早くすることができる。
【００６２】
例えば、ロードマップ作成部３０３Ａは、移動開始位置Ｓ又は移動開始位置Ｓに最も近いノードを頂点（始点）とし、頂点（始点）に対向する頂点にもノードが来るように、かつノードを少なくとも１つ含むように、範囲Ｂを設定する。なお、図１０においては、範囲Ｂの角にノードが配置されているが、範囲Ｂの角にノードが配置される必要はない。例えば、ノードとノードの中間点を頂点としてもよい。また、例えば、ロードマップ作成部３０３Ａは、移動開始位置Ｓと目的位置Ｇとを等分して範囲Ｂを設定してもよい。
【００６３】
ロードマップ法実行部３０３は、ステップＳ１１０で設定した範囲Ｂ内でロードマップ法を実行する（ステップＳ１１２）。ステップＳ１１２は、本発明の第４のステップに相当する。具体的には、全ての範囲Ｂに対して、ロードマップ作成部３０３Ａはノードを設定し、最短経路検出部３０３Ｂは設定された多数のノードのなかから最短距離を探して、それをロードマップ法により探索された経路として決定する。ステップＳ１１２でロードマップ法により探索された経路は、本発明の第３の移動経路に相当する。図１１は、図１０に示すノードを用いて決定された経路を示す。図１１に示す経路は、図１０に示す１回目のロードマップ法により探索された経路より改善されていることが分かる。
【００６４】
ステップＳ１０４、Ｓ１０６では、ステップＳ１０２で探索された経路全体を範囲（範囲Ａ）としてロードマップ法を行うため、まだ明らかに経路の最適化と無駄な範囲にノードを選ぶ可能性がある。そこで、ステップＳ１１０、Ｓ１１２では、発見された経路をいくつかに分割して再探索を行うことにしている。これにより、無駄な経路を作る可能性がある部分を除外して探索できるようになる。また、１回のロードマップ法の対象となる範囲が狭いため、設定するノード数を減らすことができ、各処理が非常に短時間で行われる。これにより、全体の計算時間を短くすることができる。
【００６５】
ロードマップ法実行部３０３は、ステップＳ１０６で探索された経路をロボット１０に出力する（ステップＳ１１３）。ステップＳ１１３とステップＳ１０８（図４参照）とは、出力される経路のみが異なり、他は同じである。
【００６６】
本実施の形態によれば、小規模なロードマップ法を複数回繰り返すことで、より良い経路を発見することができる。また、ロードマップ法を用いるため、一度経路として出力した後でも再探索を容易に行うことができる。その結果、よりよい経路を探索することができる。
【００６７】
なお、本実施の形態では、再探索を１回のみ行ったが、再探索は複数回行うようにしてもよい。この場合には、探索結果をメモリー３２に記憶しておき、ロードマップ法実行部３０３がそれを読みだして再探索を行う。図１２は、２回目の再探索を行う例を示す。図１２に示す再探索を行うことで、図１１に示す経路より経路が短くなっている。このように、経路の更新を繰り返し行うことで、システムを利用しながらより質の高い経路へと更新を続けることができる。
【００６８】
なお、再探索を行う場合には、再探索の回数が少ないうちは、広い範囲でロードマップ法を行い、再探索の回数が増えるにつれて狭い範囲に限定してロードマップ法を行うことが望ましい。これにより、効率よく探索を行うことができる。
【００６９】
また、再検索時（ステップＳ１１０、Ｓ１１２）には、最初に撮影部２０が撮影した画像を取得する（ステップＳ１００）とは別に、撮影部２０が撮影した画像を取得してもよいし、取得しなくてもよい。ロボット１０の周囲の環境が変わらないことが想定されるのであれば、撮影部２０が撮影した画像を取得する必要はない。ただし、再探索を行う場合に、最初の探索からしばらく時間が経過した後に再探索を行う場合も考えられる。この場合には、ロードマップ法実行部３０３は、処理を行う前に撮影部２０が撮影した画像を再度取得することが望ましい。そして、初回の検索時とロボット１０の周囲の環境が変わらない場合には再検索を行い、初回の検索時とロボット１０の周囲の環境が変わった場合には最初から検索をやり直すようにしてもよい。
【００７０】
また、本実施の形態では、ステップＳ１０６で探索された経路が全て範囲Ｂに含まれるように範囲Ｂを設定したが、範囲Ｂは少なくとも１つ設定されていればよい。これにより、より処理を短時間に行うことができる。
【００７１】
更に、ステップＳ１０６で探索された経路が全て範囲Ｂに含まれるように、かつ範囲Ｂの一部が重なるように、範囲Ｂを設定することもできる。図１３は、範囲Ｂの一部が重なるように、範囲Ｂ（ここでは範囲Ｂ１〜Ｂ４）を設定した様子を示す。このように範囲Ｂを設定した場合は、ロードマップ法実行部３０３は、移動開始位置Ｓに最も近い（又は移動開始位置Ｓを含む）範囲からロードマップ法を実行し、移動開始位置Ｓから徐々に遠ざかる範囲に対してロードマップ法を実行すればよい。図１３においては、範囲Ｂ１、範囲Ｂ２、範囲Ｂ３、範囲Ｂ４の順番でロードマップ法を実行すればよい。
【００７２】
また、範囲Ｂ（１つでも複数でもよい）をユーザー設定ができるようにしてもよい。例えば、ロードマップ法実行部３０３が、出力装置３６に探索された経路を出力し、ユーザーが入力装置３５を介して範囲Ｂを入力するようにすればよい。ロードマップ法実行部３０３が範囲Ｂを設定した結果を含めて出力装置３６に出力し、ユーザーが入力装置３５を介して範囲Ｂの修正を入力できるようにしてもよい。これは、ロードマップ法実行部３０３が探索結果及び範囲Ｂ等が含まれる画像（例えば図１０）を出力装置３６に出力し、ユーザーが入力装置３５を介して入力することにより可能となる。
【００７３】
＜経路探索処理（第３形態）＞
図１４は、経路探索処理（第３形態）の処理の流れを示すフローチャートである。図１５は、経路探索処理（第３形態）を説明する図である。この処理は、例えば、図示しないボタン等を介して探索開始指示が入力されることにより開始される。なお、経路探索処理（第１形態）及び経路探索処理（第２形態）と同一の部分については、同一の符号を付し、説明を省略する。また、図１４においては、経路探索処理（第１形態）と同様に、説明を簡略化するため、ノードを平面上の位置として示している。
【００７４】
ＲＲＴ経路発見部３０１は、可動部１１の移動開始位置及び目的位置を含む領域の画像を撮影部２０から取得し、取得した画像上に動作可能範囲を設定する（ステップＳ１００）。
【００７５】
ＲＲＴ経路発見部３０１は、動作可能範囲内でＲＲＴを実行し、経路を探索する（ステップＳ１０２）。ＲＲＴ経路発見部３０１は、ステップＳ１０２で探索された経路の情報をロードマップ法実行部３０３へ出力する。
【００７６】
ロードマップ作成部３０３Ａは、ＲＲＴ経路発見部３０１から出力された経路に基づいて、ステップＳ１００で取得した画像上に、ロードマップ法を実行する範囲である範囲Ａを設定する（ステップＳ１０４）。そして、ロードマップ法実行部３０３は、ステップＳ１０４で設定した範囲Ａ内でロードマップ法を実行する（ステップＳ１０６）。
【００７７】
更に、ロードマップ法実行部３０３は、ロードマップ法を用いて経路の再探索を行う（ステップＳ１１０〜１１２）。ロードマップ法実行部３０３は、ステップＳ１１２で探索された経路を経路Ｂとして生成する。ステップＳ１１２は、本発明の第４のステップに相当し、経路Ｂは、本発明の第３の移動経路に相当する。
【００７８】
また、ロードマップ法実行部３０３は、ロードマップ法を用いて、経路Ｂとは異なる経路の再探索を行う（ステップＳ１１４〜１１６）。ロードマップ作成部３０３Ａは、範囲Ａより小さい範囲であり、範囲Ｂとは異なる範囲である、かつステップＳ１０６で探索された経路を含む範囲である範囲Ｃを、動作可能範囲内に設定する（ステップＳ１１４）。範囲Ｃは、本発明の第３の範囲に相当する。
【００７９】
図１５は、動作可能範囲内に範囲Ｃを設定した様子を示す図である。図１５では、３個の範囲Ｃが設定されているが、全て範囲Ａより小さい。また、範囲Ｃは、範囲Ｂと同様、図８に示すステップＳ１０６で探索された経路を含んでいる。さらに、範囲Ｃは、範囲Ｂと同様、ステップＳ１０６で探索された経路を構成するノードを少なくとも１つ含んでいる。さらに、範囲Ｃは、図１０に示す範囲Ｂとは異なる。
【００８０】
図１５においては、全ての範囲Ｃを設定することにより、ステップＳ１０６で探索された経路は、全て範囲Ｃに含まれている。なお、範囲Ｃは、必ずしもステップＳ１０６で探索された経路をすべて含むように設定される必要はない。範囲Ｃは１つでもよい。
【００８１】
ロードマップ法実行部３０３は、ステップＳ１１４で設定した範囲Ｃ内でロードマップ法を実行する（ステップＳ１１６）。ステップＳ１１６は、本発明の第５のステップに相当する。具体的には、全ての範囲Ｃに対して、ロードマップ作成部３０３Ａはノードを設定し、最短経路検出部３０３Ｂは設定された多数のノードのなかから最短距離を探して、それを経路Ｃとして決定する。経路Ｃは、本発明の第４の移動経路に相当する。図１５点線は、図１０に示す１回目のロードマップ法により探索された経路であり、図１５実線は、ステップＳ１１６で探索された経路を示す。図１５実線で示す経路（経路Ｃ）は、１回目のロードマップ法により探索された経路より改善されていることが分かる。
【００８２】
ロードマップ法実行部３０３は、ステップＳ１１２で生成された経路Ｂと、ステップＳ１１６で生成された経路Ｃのコストをそれぞれ評価し、コストが低い経路はどちらの経路であるか否かを判定する（ステップＳ１１８）。ステップＳ１１８は、本発明の第６のステップに相当する。コストは、消費電力（エネルギー）、移動距離、移動時間、ジョイント１２の角度の総変化量等がある。本実施の形態では、コストは移動距離であるとして説明する。なお、コストの評価は、すでに公知の方法を用いることができるため、説明を省略する。
【００８３】
具体例を用いて説明する。ロードマップ法実行部３０３は、図１１に示す経路Ｂの移動距離を求め、また図１５の実線で示す経路Ｃの移動距離を求める。そして、ロードマップ法実行部３０３は、経路Ｂの移動距離と経路Ｃの移動距離とを比較する。その結果、経路Ｃの方が移動距離が短いため、ロードマップ法実行部３０３は、経路Ｃをコストが低い経路と判定する。
【００８４】
ロードマップ法実行部３０３は、ステップＳ１１８でコストが低いと判定された経路をロボット１０に出力する（ステップＳ１２０）。ステップＳ１２０は、本発明の第３のステップに相当する。
【００８５】
本実施の形態によれば、計算時間を大幅に増やすことなく、より良い経路を探索することができる。
【００８６】
なお、本実施の形態では、経路Ｂ、経路Ｃの２つの経路を生成し、２つの経路のコストを比較し、コストが低い経路を出力したが、コストを比較する経路の数はこれに限られない。例えば、３個以上の経路を生成し、これらの経路のコストを比較するようにしてもよい。
【００８７】
以上、本発明を実施の形態を用いて説明したが、本発明の技術的範囲は上記実施の形態に記載の範囲には限定されない。上記実施の形態に多様な変更または改良を加えることが可能であることが当業者には明らかである。また、そのような変更または改良を加えた形態も本発明の技術的範囲に含まれ得ることが、特許請求の範囲の記載から明らかである。特に、本発明は、ロボットと、制御部及び撮影部とが別に設けられたロボットシステムに限らず、ロボットに制御部及び撮影部が含まれたロボットとして提供することもできるし、制御部のみ、又は制御部及び撮影部からなる探索装置として提供することもできる。また、本発明は、ロボット等を制御するプログラムやプログラムを記憶した記憶媒体として提供することもできる。
【符号の説明】
【００８８】
１：ロボットシステム、１０：ロボット、１１：可動部、１２：ジョイント、１３：リンク、１４：ハンド、２０：撮影部、３０：制御部、３１：ＣＰＵ、３２：メモリー、３３：外部記憶装置、３４：通信装置、３５：入力装置、３６：出力装置、３７：読み書き装置、４０：作業台、１０１：動作制御部、３０１：ＲＲＴ経路発見部、３０２：衝突検出部、３０３：ロードマップ法実行部、３０３Ａ：ロードマップ作成部、３０３Ｂ：最短経路検出部
【図１】
2014073550.tif 000003
【図２】
2014073550.tif 000004
【図３】
2014073550.tif 000005
【図４】
2014073550.tif 000006
【図５】
2014073550.tif 000007
【図６】
2014073550.tif 000008
【図７】
2014073550.tif 000009
【図８】
2014073550.tif 000010
【図９】
2014073550.tif 000011
【図１０】
2014073550.tif 000012
【図１１】
2014073550.tif 000013
【図１２】
2014073550.tif 000014
【図１３】
2014073550.tif 000015
【図１４】
2014073550.tif 000016
【図１５】
2014073550.tif 000017