JP5962560B2 - 経路探索装置、移動体、経路探索方法及びプログラム - Google Patents

Description

本発明は、経路探索装置、移動体、経路探索方法及びプログラムに関し、特に把持対象物を把持するために把持位置まで移動し、把持後に目標位置まで移動する経路探索装置、移動体、経路探索方法及びプログラムに関する。

特許文献１には、運動学的及び動力学的に望ましく、動作移動の効率を考慮した経路を計画することが可能な運動計画方法に関する技術が開示されている。また、特許文献２には、ロボットがワークピースを保持し、目標位置まで障害物と衝突しないような運動計画を行う手法が開示されている。

特開２００６−４８３７２号公報 特表平０９−５０９５１３号公報

以下では、ロボットが初期位置から移動し、途中で把持対象物である物体を把持し、当該物体を把持した状態で目標位置まで移動する場合の運動計画を考える。一般に、ロボットが物体を把持するためには、どの場所（把持位置）から物体表面のどのあたりの領域（把持領域）をどのような姿勢（把持姿勢）で把持するか等の把持パターンを決める必要がある。また、把持パターンは、同一の物体を把持する場合に、複数取り得ることが多い。図９は、初期位置と目標位置の間で複数の把持パターンにより物体を把持した場合における運動計画の例を示す図である。図９では、ロボットが初期位置から物体を把持するまでの運動計画ＰＡと、各把持位置から目標位置までの運動計画ＰＢとを示す。

ここで、把持パターンが異なる場合には、把持後のロボットが目標位置まで移動する際の経路の探索空間が異なる。例えば、把持位置が異なる場合には、運動計画ＰＢの初期位置が異なるため、当然に探索空間は異なる。また、ロボットがアームにより全く同じ姿勢で同じ対象物体を把持したとしても、把持の仕方により、対象物体を運搬中の経路において他の物体に接触するか否かが影響を受ける。そのため、把持領域又は把持姿勢が異なる場合には、衝突空間が異なり、結果として探索空間も異なる。

図１０及び図１１は、同一の初期位置Ｐｓｔａｒｔから同一の目標位置Ｐｇｏａｌまで同一の物体を把持した状態で移動する場合における運動計画を行う際に、把持領域に応じた衝突空間の例を示す図である。図１０では、棒状の物体ＯＢＪを横向きで中央部分を把持した場合（把持パターンＧＰ１）を示す。この場合、衝突領域ＡＣ１１及びＡＣ１２を回避した空間を探索する必要がある。一方、図１１では、棒状の物体ＯＢＪを縦向きで上部を把持した場合（把持パターンＧＰ２）を示す。この場合、衝突領域ＡＣ２１及びＡＣ２２を回避した空間を探索する必要がある。尚、把持領域が異なる場合には、同一の物体を異なる方向から把持する場合、つまり、把持位置が異なる場合も含まれる。そのため、把持パターンが異なる場合には、把持後の衝突空間も異なるといえる。

そのため、図９の運動計画ＰＡである経路に決定した後に、運動計画ＰＢである経路の探索が失敗した場合、運動計画ＰＡからやり直さなければならない。このような場合、前記の経路を決定するための運動計画ＰＡの処理まで無駄になってしまい、計算時間のロスが大きいという問題点がある。

尚、特許文献１にかかる技術では、運動計画が同じ干渉条件での遷移運動に限られており、物体を把持した場合の衝突空間の変化については考慮されていない。また、特許文献２にかかる技術では、複数の把持パターンが存在する場合の運動計画を想定していない。

本発明は、このような問題を解決するためになされたものであり、複数の把持パターンが存在する場合に、運動計画の計算時間を短縮するための経路探索装置、移動体、経路探索方法及びプログラムを提供することを目的としている。

本発明の第１の態様にかかる経路探索装置は、移動体が把持対象物を把持して運搬するために、当該移動体の移動経路を探索する経路探索装置であって、所定空間内で、前記把持対象物を把持するための複数の把持位置のうち、初期位置から移動可能な経路が存在する把持位置の選出処理を実行する把持位置選出手段と、前記把持位置選出手段が前記把持位置を選出した場合に、当該把持位置において前記把持対象物を把持した状態で移動可能な探索空間内で、当該把持位置から目標位置まで運搬する際の運搬経路の探索処理を実行する運搬経路探索手段と、を備え、前記把持位置選出手段は、前記運搬経路探索手段による前記探索処理の実行と併せて、未選出の把持位置についての前記選出処理を実行する。

また、前記運搬経路探索手段は、前記把持位置選出手段により選出された把持位置において、所定の把持姿勢により前記把持対象物を把持した状態で移動可能な探索空間を算出し、当該算出した探索空間内で前記探索処理を実行することが望ましい。

さらに、前記運搬経路探索手段は、前記把持位置選出手段が複数の前記把持位置を選出した場合には、各把持位置において前記把持対象物を把持した状態で移動可能な探索空間をそれぞれ算出し、各把持位置について当該算出した探索空間内で前記探索処理を並列実行することが望ましい。

さらに、前記運搬経路探索手段は、前記把持位置選出手段が複数の前記把持位置を選出した場合には、各把持位置について異なる優先度を設定し、前記設定された優先度に基づいて所定数の把持位置を選択し、当該選択された所定数の把持位置のそれぞれに基づく前記探索処理を並列実行することが望ましい。

さらに、前記運搬経路探索手段は、前記並列実行される複数の探索処理について、各把持位置に設定された優先度に応じたリソースの割り当てを行うとよい。

また、前記把持位置選出手段は、前記初期位置における初期姿勢から前記複数の把持位置における各把持姿勢までの第１の運動計画を実行することで、前記選出処理を実行し、前記運搬経路探索手段は、前記把持位置選出手段が前記把持位置を選出した場合に、当該把持位置における把持姿勢から前記目標位置における姿勢までの第２の運動計画を１ステップ単位で実行することで、前記探索処理を実行し、前記把持位置選出手段は、前記運搬経路探索手段が前記第２の運動計画を１ステップ実行しても終了しなかった場合に、前記第１の運動計画を１ステップ実行するとよい。

本発明の第２の態様にかかる移動体は、前記経路探索装置を備え、前記初期位置から、前記経路探索装置により探索された移動経路にかかる把持位置まで移動して前記把持対象物を把持し、当該把持対象物を把持した状態で、当該探索された運搬経路に基づいて当該把持位置から前記目標位置まで移動する。

本発明の第３の態様にかかる経路探索方法は、移動体が把持対象物を把持して運搬するために、当該移動体の移動経路を探索する経路探索方法であって、所定空間内で、把持対象物を把持するための複数の把持位置のうち、初期位置から移動可能な経路が存在する第１の把持位置の選出処理を実行し、前記第１の把持位置が選出された場合に、当該第１の把持位置において前記把持対象物を把持した状態で移動可能な第１の空間内で、当該第１の把持位置から目標位置までの第１の運搬経路の探索処理を開始し、前記第１の運搬経路の探索処理の実行と併せて、第２の把持位置の選出処理を実行する。

また、前記第１の運搬経路の探索処理中に、前記第２の把持位置が選出された場合に、当該第２の把持位置において前記把持対象物を把持した状態で移動可能な第２の空間内で、当該第２の把持位置から前記目標位置までの第２の運搬経路の探索処理を開始することが望ましい。

本発明の第４の態様にかかるプログラムは、所定空間内の把持対象物を把持するための把持部を有する移動体を制御するためのプログラムであって、前記移動体に、所定空間内の把持対象物を把持するための複数の把持位置のうち、初期位置から移動可能な経路が存在する把持位置を選出する把持位置選出処理と、前記把持位置選出処理により前記把持位置が選出された場合に、当該把持位置において前記把持対象物を把持した状態で移動可能な探索空間内で、当該把持位置から目標位置まで運搬する際の運搬経路を探索する運搬経路探索処理と、を実行させ、前記運搬経路探索処理による前記探索と併せて、未選出の把持位置についての前記把持位置選出処理を実行させる。

本発明により、複数の把持パターンが存在する場合に、運動計画の計算時間を短縮するための経路探索装置、移動体、経路探索方法及びプログラムを提供することができる。

本発明の実施の形態１にかかる経路探索装置１の構成を示すブロック図である。 本発明の実施の形態１にかかる把持フレームの概念の例を示す図である。 本発明の実施の形態１にかかる把持フレームの概念の例を示す図である。 本発明の実施の形態１にかかる把持姿勢の例を示す図である。 本発明の実施の形態１にかかる把持姿勢の例を示す図である。 本発明の実施の形態１にかかる把持姿勢の例を示す図である。 本発明の実施の形態１にかかる経路探索処理の流れを示すフローチャートである。 本発明の実施の形態１にかかる経路探索処理の概念を説明するための図である。 初期位置と目標位置の間で複数の把持パターンにより物体を把持した場合における運動計画の例を示す図である。 把持領域に応じた衝突空間の例を示す図である。 把持領域に応じた衝突空間の例を示す図である。

以下では、上述した各態様を含む本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略する。

＜発明の実施の形態１＞
図１は、本発明の実施の形態１にかかる経路探索装置１の構成を示すブロック図である。経路探索装置１は、アームを備えたロボットなどの移動体が初期位置から移動して把持対象物を把持するための把持位置までの把持経路と、当該物体を把持した状態で、把持位置から目標位置まで運搬する際の運搬経路とを探索するものである。経路探索装置１は、例えば移動体などに搭載されている。そして、ロボットなどの移動体は、経路探索装置１により探索された把持経路及び運搬経路に従って把持及び移動（把持対象物の運搬）を行うことができる。

経路探索装置１は、把持経路探索部１１と、運搬経路探索部１２と、記憶部１３とを備える。なお、経路探索装置１は、例えば、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバスによって相互に接続されている。

記憶部１３は、例えば、上記ＲＯＭやＲＡＭなどにより構成されている。記憶部１３は、少なくとも把持フレーム情報１３１、把持姿勢情報１３２、把持経路情報１３３及び運搬経路情報１３４を記憶する。把持フレーム情報１３１は、把持対象となる物体（把持対象物）における複数の把持フレームを示す情報である。ここで、把持フレームとは、把持対象となる物体上に定義された座標である。そして、ロボット等はアームのハンドを把持フレームまで移動させることで、物体を安定して把持することができる。また、把持フレームは、一般に一つの物体に対して複数存在する。把持フレームは、例えば以下の式（１）に示すように、ハンドから物体までの同時変換行列で表すことができる。

また、図２及び図３は、本発明の実施の形態１にかかる把持フレームの概念の例を示す図である。図２では、ハンドＨが上側から物体ＯＢＪを把持できることを示す。つまり、把持フレームＦ１は物体ＯＢＪの上側の領域である。図３では、ハンドＨが右側面の下部から物体ＯＢＪを把持できることを示す。つまり、把持フレームＦ２は物体ＯＢＪの右側面の下部の領域である。

把持姿勢情報１３２は、物体ＯＢＪを把持する際のロボットの全身姿勢である。図４〜図６は、本発明の実施の形態１にかかる把持姿勢の例を示す図である。同じ物体ＯＢＪにおける把持フレームＦ１に対する把持姿勢として、冗長なロボットでは、例えば、図４に示す姿勢ＰＳ１、図５に示す姿勢ＰＳ２及び図６に示す姿勢ＰＳ３の３種類を取り得ることを示す。尚、把持姿勢の例はこれに限定されない。尚、把持姿勢が異なることにより、把持位置や把持フレームが異なることがある。

把持経路情報１３３は、経路探索装置１により探索される把持経路を示す情報である。把持経路は、移動体が初期位置から所定の把持位置まで移動するために通過する経路である。尚、把持経路情報１３３には、探索途中の経路、つまり、把持位置に到達していない経路も含むものとする。

また、運搬経路情報１３４は、経路探索装置１により探索される運搬経路を示す情報である。運搬経路は、所定の把持位置において物体を把持した状態で、把持位置から目標位置まで運搬するために通過する経路である。尚、運搬経路情報１３４には、探索途中の経路、つまり、目標位置に到達していない経路も含むものとする。

把持経路探索部１１は、把持位置選出手段の一具体例であり、所定空間内の物体を把持するための複数の把持位置のうち、初期位置から移動可能な経路が存在する把持位置の選出処理を実行するものである。また、運搬経路探索部１２は、運搬経路探索手段の一具体例であり、把持経路探索部１１が把持位置を選出した場合に、当該把持位置において物体を把持した状態で移動可能な探索空間内で、当該把持位置から目標位置まで運搬する際の運搬経路の探索処理を実行するものである。ここで、把持経路探索部１１は、運搬経路探索部１２による探索処理の実行と併せて、未選出の把持位置についての選出処理を実行する。すなわち、選出処理と探索処理を並列的に実行する。つまり、第１の把持位置に基づく第１の運搬経路探索処理の結果が判明する前に、第２の把持位置の第２の選出処理が開始されている。そのため、先に実行中の第１の運搬経路探索処理が失敗したとしても、その時点から第２の選出処理を開始する場合に比べて、第２の把持位置がより早く選出され得る。よって、失敗時のロスを減らすことができる。

また、運搬経路探索部１２は、把持経路探索部１１により選出された把持位置において、所定の把持姿勢により物体を把持した状態で移動可能な探索空間を算出し、当該算出した探索空間内で探索処理を実行する。探索空間は、把持位置だけでなく把持姿勢によっても異なるため、探索処理の精度が高まる。

さらに、運搬経路探索部１２は、把持経路探索部１１が複数の把持位置を選出した場合には、各把持位置において物体を把持した状態で移動可能な探索空間をそれぞれ算出し、各把持位置について当該算出した探索空間内で探索処理を並列実行する。このように、複数の運搬経路の探索処理を並列することで、いずれかの探索処理に失敗したとしても、他の探索処理も途中まで実行されているため、失敗のロスを軽減することができる。

また、運搬経路探索部１２は、把持経路探索部１１が複数の把持位置を選出した場合には、各把持位置について異なる優先度を設定し、設定された優先度に基づいて所定数の把持位置を選択し、当該選択された所定数の把持位置のそれぞれに基づく探索処理を並列実行する。これにより、把持位置の候補のうち一部の探索処理にリソースを集中でき、探索される確率を向上させることができる。

さらに、運搬経路探索部１２は、並列実行される複数の探索処理について、各把持位置に設定された優先度に応じたリソースの割り当てを行う。これにより、確率の高い探索処理にリソースをより多く割り当てることで、リソースを有効活用し、早期に探索処理を終える確率が高まる。

また、把持経路探索部１１は、初期位置における初期姿勢から複数の把持位置における各把持姿勢までの第１の運動計画を実行することで、選出処理を実行する。そして、運搬経路探索部１２は、把持経路探索部１１が把持位置を選出した場合に、当該把持位置における把持姿勢から目標位置における姿勢までの第２の運動計画を１ステップ単位で実行することで、探索処理を実行する。その後、把持経路探索部１１は、運搬経路探索部１２が第２の運動計画を１ステップ実行しても終了しなかった場合に、第１の運動計画を１ステップ実行する。このように、一つ選出された以後は、見つかるまで交互に１ステップずつ運動計画を実行することで、把持前後の運動計画を並列実行できる。尚、第１の運動計画及び第２の運動計画で実行するステップの単位は、１ステップ以外でも構わない。

図７は、本発明の実施の形態１にかかる経路探索処理の流れを示すフローチャートである。まず、経路探索装置１は、把持フレーム群Ｔｉ（ｉ＝０〜ｎ。ｎは１以上の整数。）を取得する（Ｓ１１）。例えば、把持経路探索部１１は、記憶部１３から把持フレーム情報１３１を読み出す。

次に、経路探索装置１は、把持フレーム群Ｔｉに対して、複数ゴール運動計画を１ステップ進める（Ｓ１２）。尚、初期姿勢から把持姿勢までは同じ探索空間で探索できるので、例えば、ＩＫＢｉＲＲＴ等を適用することができる。

そして、経路探索装置１は、初期姿勢から把持フレームＴｉまでつながるパスがあるか否かを判定する（Ｓ１３）。ここで、つながるパスがあると判定されればステップＳ１４へ進む。一方、つながるパスがないと判定された場合、ステップＳ１８へ進む。一般には、１ステップでは、パスがないと判定される。その場合、ステップ数が規定回数以上か否かを判定する（Ｓ１８）。同様に、一般には、１ステップでは予め規定した回数に満たないため、ステップＳ１２へ戻る。つまり、ステップＳ１３で初期姿勢から把持フレームまでつながる第１のパスが見つかるまで、ステップＳ１２を繰り返すこととなる。尚、ここでは、把持フレームにより把持位置や把持姿勢も一意に定まるものとするが、これに限定されない。例えば、把持位置、把持領域及び把持姿勢を含めた把持パターンが定まるようにすればよい。

ステップＳ１３において、つながるパスがあると判定された場合、つまり、把持位置が選出された場合、経路探索装置１は、つながった把持フレームＴｊ（ｊ＝０〜ｍ。ｍは、１以上の整数。）についてコスト付けを行う（Ｓ１４）。ここで、コスト付けとは、各把持フレームに対して、探索処理における優先度を設定することを指す。また、優先度は、把持フレームや把持姿勢ごとに予め設定しておいても構わない。

続いて、経路探索装置１は、把持フレームＴｊのうち、コストのより高いｋ（ｋはｍ以下の整数。）個の把持フレームＴｊを選択する（Ｓ１５）。つまり、運搬経路探索部１２は、選出された複数の把持フレームのうち少なくとも一部を選択する。そして、経路探索装置１は、選択した各把持フレームＴｊのそれぞれについて並列に、終端姿勢までの１対１の運動計画を１ステップ進める（Ｓ１６）。すなわち、把持姿勢から最終姿勢までは把持した際の持ち方により空間が異なるので、並列に単ゴール探索を行う。このとき、経路探索装置１は、各把持フレームにおける運動計画の処理について、優先度に応じてリソースを割り当てる。尚、ステップＳ１４によるコスト付けは必須ではなく、単に、複数の把持フレームを全て選択し、リソースを均等に割り当てて実行しても構わない。

その後、運搬経路探索部１２は、把持フレームＴｊから終端姿勢までつながるパスがあるか否かを判定する（Ｓ１７）。ここで、つながるパスがあると判定されれば、探索が成功したとして当該処理を終了する。一方、つながるパスがないと判定された場合、ステップＳ１８へ進む。

尚、ステップＳ１２及びＳ１６において運動計画を進めるのは、１ステップ単位である必要はない。例えば、２以上のステップであってもよい。そして、ステップＳ１２とＳ１６とで異なるステップ単位であってもよい。また、ステップＳ１２では最初のパスが見つかるまで複数ステップをまとめて実行し、見つかった後に１ステップ単位で実行してもよい。さらに、ステップＳ１２とＳ１６とのステップ数を所定のルールにより調整しても構わない。

図８は、本発明の実施の形態１にかかる経路探索処理の概念を説明するための図である。まず、物体を掴むまでの運動計画ＰＡにおいては、初期位置Ｐｓｔａｒｔから物体を把持することができる把持位置ｇｒａｓｐ１〜５をゴールとした複数ゴール運動計画が実行される。ここで、運動計画ＰＡでは、いずれのゴールにおいても衝突領域ＡＣ３１及びＡＣ３２を除いた探索空間ＳＳＡ内で移動可能な経路を探索することとなる。例えば、初期位置Ｐｓｔａｒｔでは初期姿勢ＰＳ１０であり、ｇｒａｓｐ１、２では把持姿勢ＰＳ２１、２２となるものとする。そのため、運動計画ＰＡは、初期姿勢ＰＳ１０から所定の把持姿勢までの探索処理と捉えることもできる。また、物体を掴んだ後の運動計画ＰＢにおいては、運動計画ＰＡで把持位置が選出され次第、適宜、当該把持位置から目標位置Ｐｇｏａｌをゴールとした単ゴール運動計画が並列に実行される。ここで、運動計画ＰＢでは、例えば、衝突領域ＡＣ４１を除いた探索空間ＳＳＢ１内で移動可能な経路を探索する探索処理と、衝突領域ＡＣ４２及びＡＣ４３を除いた探索空間ＳＳＢ２内で移動可能な経路を探索する探索処理と、衝突領域ＡＣ４４及びＡＣ４５を除いた探索空間ＳＳＢ３内で移動可能な経路を探索する探索処理と、を並列に実行することとなる。また、運動計画ＰＢは、各把持姿勢から最終姿勢ＰＳ３０までの探索処理と捉えることもできる。

上述したように従来では、初期姿勢から複数の把持姿勢までの運動計画ＰＡを行い、把持姿勢が一つ決まった場合、当該把持姿勢から最終姿勢までの運動計画ＰＢを行うだけであった。そのため、ＰＢが失敗した場合に、再度、運動計画ＰＡからやり直すといった計算時間のロスが発生していた。

これに対し、発明の実施の形態１により、複数の把持位置等が存在する場合の把持経路と運搬経路とを探索する運動計画を実行するための計算時間を短縮することができる。特に、物体を把持する際の複数の把持位置の候補について、それらの探索空間を優先順位をつけ並列的に探索することで、計算時間を短く自然な結果が出易くなるという効果を奏する。言い換えると、発明の実施の形態１では、平均的に解を求めることができ。また、把持前と把持後の運動計画を同時並列に進めることにより、失敗した場合でも処理時間のロスを抑えることもできる。

また、本発明の実施の形態１には、次のような側面を有する。すなわち、物体を把持した後、該物体を移動させるロボットアームの姿勢制御方法であって、前記アームの初期姿勢から、所定空間に存在する物体を把持する第１の把持姿勢までの把持経路を探索する把持経路探索ステップと、前記第１の把持姿勢で把握した後の運搬経路を探索する運搬経路探索ステップと、を有し、前記把持経路探索ステップで第１の把持姿勢までの把持経路を探索した後、前記運搬経路探索ステップを実行するとともに、再度、把持経路探索ステップを実行し、第２の把持姿勢までの把持経路の探索を繰り返すことを特徴とする。これにより、複数の把持姿勢が想定されるロボットアームの運動計画をスピーディに行うことが可能となる。

または、経路探索装置１は、次のような構成を有してもよい。例えば、経路探索装置１は、障害物を避けて、軌道を算出する複数の運動計画機であり、複数の把持パターンを算出する把持計画部と、ロボットと障害物の干渉をチェックする干渉検出部とを備えるものとしてもよい。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上記一実施の形態において、物体を把持する場合について説明したがこれに限らず、複数の別空間をまたぐ運動計画にも適用が可能である。

上記実施形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、例えば、上記把持経路探索部１１、運搬経路探索部１２が実行する処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

上述の例において、経路探索処理をコンピュータに行わせるための命令群を含むプログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ 経路探索装置
１１ 把持経路探索部
１２ 運搬経路探索部
１３ 記憶部
１３１ 把持フレーム情報
１３２ 把持姿勢情報
１３３ 把持経路情報
１３４ 運搬経路情報
Ｆ１ 把持フレーム
Ｆ２ 把持フレーム
Ｈ ハンド
ＯＢＪ 物体
ＰＳ１ 姿勢
ＰＳ２ 姿勢
ＰＳ３ 姿勢
ＰＳ１０ 初期姿勢
ＰＳ２１ 把持姿勢
ＰＳ２２ 把持姿勢
ＰＳ３０ 最終姿勢
ｇｒａｓｐ１ 把持位置
ｇｒａｓｐ２ 把持位置
ｇｒａｓｐ３ 把持位置
ｇｒａｓｐ４ 把持位置
ｇｒａｓｐ５ 把持位置
ＰＡ 運動計画
ＰＢ 運動計画
ＳＳＡ 探索空間
ＳＳＢ１ 探索空間
ＳＳＢ２ 探索空間
ＳＳＢ３ 探索空間
Ｐｓｔａｒｔ 初期位置
Ｐｇｏａｌ 目標位置
ＧＰ１ 把持パターン
ＧＰ２ 把持パターン
ＡＣ１１ 衝突領域
ＡＣ１２ 衝突領域
ＡＣ２１ 衝突領域
ＡＣ２２ 衝突領域
ＡＣ３１ 衝突領域
ＡＣ３２ 衝突領域
ＡＣ４１ 衝突領域
ＡＣ４２ 衝突領域
ＡＣ４３ 衝突領域
ＡＣ４４ 衝突領域
ＡＣ４５ 衝突領域

Claims (10)

1. 移動体が把持対象物を把持して運搬するために、当該移動体の移動経路を探索する経路探索装置であって、
   所定空間内で、前記把持対象物を把持するための複数の把持位置のうち、初期位置から移動可能な経路が存在する把持位置の選出処理を実行する把持位置選出手段と、
   前記把持位置選出手段が前記把持位置を選出した場合に、当該把持位置において前記把持対象物を把持した状態で移動可能な探索空間内で、当該把持位置から目標位置まで運搬する際の運搬経路の探索処理を実行する運搬経路探索手段と、を備え、
   前記把持位置選出手段は、
   前記運搬経路探索手段による前記探索処理の実行と併せて、未選出の把持位置についての前記選出処理を実行する
   経路探索装置。
2. 前記運搬経路探索手段は、
   前記把持位置選出手段により選出された把持位置において、所定の把持姿勢により前記把持対象物を把持した状態で移動可能な探索空間を算出し、当該算出した探索空間内で前記探索処理を実行する
   請求項１に記載の経路探索装置。
3. 前記運搬経路探索手段は、
   前記把持位置選出手段が複数の前記把持位置を選出した場合には、各把持位置において前記把持対象物を把持した状態で移動可能な探索空間をそれぞれ算出し、各把持位置について当該算出した探索空間内で前記探索処理を並列実行する
   請求項１又は２に記載の経路探索装置。
4. 前記運搬経路探索手段は、
   前記把持位置選出手段が複数の前記把持位置を選出した場合には、各把持位置について異なる優先度を設定し、
   前記設定された優先度に基づいて所定数の把持位置を選択し、
   当該選択された所定数の把持位置のそれぞれに基づく前記探索処理を並列実行する
   請求項３に記載の経路探索装置。
5. 前記運搬経路探索手段は、
   前記並列実行される複数の探索処理について、各把持位置に設定された優先度に応じたリソースの割り当てを行う
   請求項４に記載の経路探索装置。
6. 前記把持位置選出手段は、
   前記初期位置における初期姿勢から前記複数の把持位置における各把持姿勢までの第１の運動計画を実行することで、前記選出処理を実行し、
   前記運搬経路探索手段は、
   前記把持位置選出手段が前記把持位置を選出した場合に、当該把持位置における把持姿勢から前記目標位置における姿勢までの第２の運動計画を１ステップ単位で実行することで、前記探索処理を実行し、
   前記把持位置選出手段は、
   前記運搬経路探索手段が前記第２の運動計画を１ステップ実行しても終了しなかった場合に、前記第１の運動計画を１ステップ実行する
   請求項１乃至５のいずれか１項に記載の経路探索装置。
7. 請求項１乃至６のいずれか１項に記載の経路探索装置を備え、
   前記初期位置から、前記経路探索装置により探索された移動経路にかかる把持位置まで移動して前記把持対象物を把持し、当該把持対象物を把持した状態で、当該探索された運搬経路に基づいて当該把持位置から前記目標位置まで移動する
   ことを特徴とする移動体。
8. 移動体が把持対象物を把持して運搬するために、当該移動体の移動経路を探索する経路探索方法であって、
   所定空間内で、把持対象物を把持するための複数の把持位置のうち、初期位置から移動可能な経路が存在する第１の把持位置の選出処理を実行し、
   前記第１の把持位置が選出された場合に、当該第１の把持位置において前記把持対象物を把持した状態で移動可能な第１の空間内で、当該第１の把持位置から目標位置までの第１の運搬経路の探索処理を開始し、
   前記第１の運搬経路の探索処理の実行と併せて、第２の把持位置の選出処理を実行する
   経路探索方法。
9. 前記第１の運搬経路の探索処理中に、前記第２の把持位置が選出された場合に、当該第２の把持位置において前記把持対象物を把持した状態で移動可能な第２の空間内で、当該第２の把持位置から前記目標位置までの第２の運搬経路の探索処理を開始する
   請求項８に記載の経路探索方法。
10. 所定空間内の把持対象物を把持するための把持部を有する移動体を制御するためのプログラムであって、
    前記移動体に、
    所定空間内の把持対象物を把持するための複数の把持位置のうち、初期位置から移動可能な経路が存在する把持位置を選出する把持位置選出処理と、
    前記把持位置選出処理により前記把持位置が選出された場合に、当該把持位置において前記把持対象物を把持した状態で移動可能な探索空間内で、当該把持位置から目標位置まで移動する際の運搬経路を探索する運搬経路探索処理と、を実行させ、
    前記運搬経路探索処理による前記探索と併せて、未選出の把持位置についての前記把持位置選出処理を実行させる
    ことを特徴とするプログラム。