JP2020004421A - １個以上の障害物を回避して始状態から終状態集合まで移動する物体の経路を決定する方法およびシステム - Google Patents

Abstract

【課題】１個以上のノードを経由して始状態から終状態集合まで移動するロボット等の経路を決定するシステムを提供する。【解決手段】ａ）親ノードに対して特定の距離内にある複数の新しい子ノードを生成する。ｂ）親ノードから記子ノードのそれぞれへの遷移に障害物がある場合、この部分経路が除外される。ｃ）部分経路のそれぞれについてコスト値を計算する。ｄ）始ノードから親ノードまでのコスト値に、計算されたコスト値を加算する。ｅ）子ノードのそれぞれから終状態を表す終ノードまでの部分経路に対して推定または期待されるコスト値を加算する。ｆ）ステップｄ）〜ｅ）に従って決定されたコスト値から最低の全コスト値を決定し、最低コストに対応する子ノードを新しい親ノードとして選択する。ｇ）選択された子ノードが終ノードまで所与の距離内にあるという終了条件をが満たされるまでステップａ）〜ｇ）の実行を繰り返す。【選択図】図１

Description

本発明は、１個以上のノードを経由して始状態から１個以上の終状態を含む終状態集合まで移動するロボット等の物体の経路を決定する方法に関する。各ノードは１個以上の障害物を回避するように探索された無障害物状態として規定される。

また、本発明は、１個以上のノードを経由して始状態から１個以上の終状態を含む終状態集合まで移動するロボット等の物体の経路を決定するシステムに関する。各ノードは１個以上の障害物を回避するように探索された無障害物状態として規定される。

本発明は、いかなる種類の物体にも適用可能であるが、以下ではロボットに関して説明する。

ロボットおよび自律／無人運転輸送手段は、今日の日常生活に影響を及ぼす２つの広範な分野およびシステムである。ロボットは、工業自動化や生活支援に使用される。最近ではドローンもまた、軍事および民生用に導入されている。自律運転車は高度の信頼性を示しており、近い将来には公的使用のために利用可能となるであろうが、農業・鉱業用のような非公道用には既に使用されている。無人輸送手段は、無人の船舶や宇宙船も構想されている。

他の応用分野として、医療において、外科手術器具を硬軟の組織と生命に必須の器官との間で動かす場合がある。また、他の応用としては、軍事・宇宙分野において、輸送手段が障害物を回避しながら移動しなければならない場合がある。

上記の理由から、このような物体の経路や移動は、障害物の回避を含めて計画されなければならない。経路計画は、始状態から目標の終状態集合までの経路の生成を対象とするロボティクスの分野である。経路は、ロボット／輸送手段の拘束と状態発展ダイナミクスを考慮しなければならず、ロボット／輸送手段の状態の無障害物集合内に存在しなければならない。

特許文献１には、輸送手段等の物体の経路計画を行うシステムおよび方法が開示されている。このシステムは、輸送手段および障害物の移動が確率的に知られている場合に、動的に変化する不確定な環境における運動学的動力学的拘束（kinodynamic constraint）の下で動作する輸送手段に対して、実時間経路計画を扱うランダム化適応経路計画を提供する。詳細には、ツリー構築が以下のステップによって実行される。ルートノードを初期輸送手段状態に設定し、停止条件が満たされているかどうかをチェックする。満たされていない場合、決定性ツリー延長およびランダムツリー延長を実行する。これらのツリー延長に基づいて、まだ選択されていないノードを選択する。そして、該ノードが延長されているか否かをチェックする。延長されていない場合、該ノードは死んでいる（dead）と設定される。該ノードが延長されている場合、消去（kill）条件が満たされているか否かをチェックし、消去条件が満たされていない場合、すべてのノードが選択されたか否かをチェックする。すべてのノードが選択されてはいない場合、まだ選択されていないノードを選択する。それ以外の場合、停止条件が満たされているか否かを再びチェックする。

特許文献２には、自動部品計画を行うシステムおよび方法が開示されている。このシステムおよび方法は、無衝突の部品除去経路を自動的に求める。このシステムは、高次元の状態空間を粗サンプリングし、近接仮定を用いて残りの空間をマッピングする。より詳細には、特許文献２では、始状態および宛先状態を中心とする球によって規定される重なり領域内に完全に含まれる始状態と宛先状態とを結ぶ経路が障害物と衝突するかどうかをチェックする。障害物にぶつかっている場合、障害物が該領域と交わる衝突点を求め、現領域と近隣領域のサイズを、その中心から最近接衝突点までの距離が小さくなるように調整する。そして、無障害物領域が親領域として選択される。この親領域から子領域が生成されることになる。親領域の候補点を子領域の中心点として選択し、候補点が障害物にぶつかる場合、領域の半径を適当に調節し、別の候補点または親領域を繰り返し選択する。候補点が障害物にぶつからない場合、子領域が生成される。子領域は、候補点をその中心点とし、候補点のリストにおける最近接衝突位置よりも小さい半径を有する。

特許文献３には、輸送手段の経路を決定する二点間経路計画方法が開示されている。この方法では、始点および終点が規定され、障害物ディテクタが、始点と終点の間の作業領域における１個以上の障害物を検出する。各障害物に対応して、その周りに境界ゾーンが規定される。そして、始点と終点の間の候補経路が識別される。各候補経路は、各障害物に対応する１個の境界ゾーンのみと交わる。始点と終点の間で各候補経路またはその一部をたどる経済的コストが推定される。識別された候補経路のうちから、推定された経済的コストが最低であることに基づいて優先経路が選択される。

他の経路計画の方法およびアルゴリズムは、例えば、非特許文献１や非特許文献２に開示されている。

しかし、従来の方法およびシステムの欠点の１つとして、状態空間が大きくなる場合、特に未知の形状の障害物が存在する場合に、妥当な時間で経路を見つけることが問題となる。もう１つの課題は、対象となる領域内で障害物が移動する場合、すなわち時間成分を考慮する場合に、経路が手に負える（tractable）ものでなければならないことである。もう１つの課題は、従来の経路計画システムは漸近的最適性の達成や急拡大するデータ構造に注目しているが、有界な最適経路を無視していることである。

米国特許第７４４７５９３Ｂ２号明細書 米国特許第５９９９８８１号明細書 国際公開第２００６／０８０９９６Ａ２号

S. Karaman, E. Frazzoli, "Sampling-based algorithms for optimal motion planning", Int. J. of Robotics Research 30(7):846-894, 2011 Webb, D. and van den Berg, J. (2013), "Kinodynamic RRT: Asymptotically optimal motion planning for robots with linear dynamics", pp. 5054--5061

したがって、本発明の目的は、物体の経路を決定する問題に対して高速で誤差有界な解を与える、物体の経路を決定する方法およびシステムを提供することである。

本発明のもう１つの目的は、多数の相異なる応用分野に適用可能な、物体の経路を決定する方法およびシステムを提供することである。

本発明のさらにもう１つの目的は、実施が容易で費用効果の高い、物体の経路を決定する方法およびシステムを提供することである。

上記の目的は、請求項１の方法および請求項２５のシステムによって達成される。

請求項１において、１個以上のノードを経由して始状態から１個以上の終状態を含む終状態集合まで移動するロボット等の物体の経路を決定する方法が規定される。各ノードは１個以上の障害物を回避するように探索された無障害物状態として規定される。

請求項１によれば、本方法は、
ａ）親ノードに対して複数の子ノードを決定するステップであって、ステップａ）が最初に実行されるときには前記始状態を表す始ノードが前記親ノードとなり、前記子ノードは前記親ノードまである特定の距離内にある、決定するステップと、
ｂ）前記親ノードから前記子ノードのそれぞれへの遷移に障害物がないかどうかをチェックし、障害物がある場合、対応する子ノードおよび前記親ノードから該子ノードまでのこの部分経路が除外される、チェックするステップと、
ｃ）前記部分経路のそれぞれについてコスト値を計算するステップと、
ｄ）前記始ノードから前記親ノードまでのコスト値に、前記計算されたコスト値を加算するステップと、
ｅ）前記子ノードのそれぞれから終状態を表す終ノードまでの部分経路に対して推定または期待されるコスト値を加算するステップと、
ｆ）ステップｄ）〜ｆ）に従って決定されたコスト値から最低の全コスト値を決定し、該決定された最低コストに対応する子ノードを新しい親ノードとして選択するステップと、
ｇ）選択された子ノードが前記終ノードまで所与の距離内にあるという終了条件を含む少なくとも１つの終了条件が満たされるまでステップａ）〜ｇ）の実行を繰り返すステップと
を備えたことを特徴とする。

請求項２５において、１個以上のノードを経由して始状態から１個以上の終状態を含む終状態集合まで移動するロボット等の物体の経路を決定するシステムが規定される。各ノードは１個以上の障害物を回避するように探索された無障害物状態として規定される。

請求項２５によれば、本システムは、
ａ）親ノードに対して複数の子ノードを決定するステップであって、ステップａ）が最初に実行されるときには前記始状態を表す始ノードが前記親ノードとなり、前記子ノードは前記親ノードまである特定の距離内にある、決定するステップと、
ｂ）前記親ノードから前記子ノードのそれぞれへの遷移に障害物がないかどうかをチェックし、障害物がある場合、対応する子ノードおよび前記親ノードから該子ノードまでのこの部分経路が除外される、チェックするステップと、
ｃ）前記部分経路のそれぞれについてコスト値を計算するステップと、
ｄ）前記始ノードから前記親ノードまでのコスト値に、前記計算されたコスト値を加算するステップと、
ｅ）前記子ノードのそれぞれから終状態を表す終ノードまでの部分経路に対して推定または期待されるコスト値を加算するステップと、
ｆ）ステップｄ）〜ｆ）に従って決定されたコスト値から最低の全コスト値を決定し、該決定された最低コストに対応する子ノードを新しい親ノードとして選択するステップと、
ｇ）選択された子ノードが前記終ノードまで所与の距離内にあるという終了条件を含む少なくとも１つの終了条件が満たされるまでステップａ）〜ｇ）の実行を繰り返すステップと
を行うように適応されるか、または、行うために相互に協力するように適応された１個以上の計算要素を備えたことを特徴とする。

換言すれば、本発明は、好ましくは、有界近似最適コストおよび反復解による局所成長ツリー探索を使用する。

好ましくは特許請求の範囲において、特に明細書において、「コスト」という用語はその最広義に理解されるべきであり、距離、時間、エネルギー、汚染および／または経済的コスト等を含み得るが、これらに限定されない。

本発明によって認識されたこととして、ロボット、輸送手段等の物体の経路が障害物を回避するように提供され、障害物の形状は事前に未知でもよく、時間的に変動することも可能である。

また、本発明によって認識されたこととして、決定される経路は、有界時間で計算される最適コスト経路に対して有界誤差を守る。

また、本発明によって認識されたこととして、本発明は、例えば協調的自律運転輸送手段からロボットやドローンの無障害物移動や医療精密制御まで、多くの応用分野に適用可能である。

また、本発明によって認識されたこととして、本発明は、漸近的最適収束が可能となるような拡張を可能にすることに関してフレキシブルである。

さらなる特徴、利点および好ましい実施形態について、従属請求項に関して以下で説明する。

好ましい実施形態によれば、子ノードから前記終状態集合までの実コストに対する下限値が決定され、ステップｅ）における前記推定または期待されるコストとして使用される。これにより、探索時にコストが下方推定コスト以下の場合にのみ無障害物空間を探索することが可能となる。経路のコストは、中間状態までの現コストに、終状態までの実コストの下限を加えたものによって近似することが可能である。これにより、限られた時間内に費用効果の高い経路を推定することができる。下方推定コストの一例は、障害物を考慮しないか、実際の全コストの一部のみ（例えば距離のみ）を含む解（すなわち経路）に関連するコストである。

さらに好ましい実施形態によれば、子ノードを決定するため、親被覆空間が選択され、子ノードがその個別被覆空間で所定の親範囲まで前記親被覆空間を被覆するように子ノードが局所的に生成され、前記個別被覆空間は相互に重なり合わない。これにより、それぞれの探索されるノードのまわりの空間を効果的に探索することができる。障害物は、十分な精度で高速に決定することができる。

さらに好ましい実施形態によれば、子ノードに対する個別被覆空間および／または親ノードに対する親被覆空間の形が所定の半径を有する球状である。これにより、計算の有効性がさらに向上する。というのは、球は対称的であり、２個の変数、すなわち中心の位置と半径だけで記述できるからである。

さらに好ましい実施形態によれば、子ノードを局所的に生成するため、前記親被覆空間の中心から限られた数の方向が生成され、該方向に子ノードを生成することによって前記親被覆空間を被覆するために該方向が使用される。これにより、例えば、ある特定の方向を選択または優先することが可能となる。このためフレキシビリティが向上する。また、いくつかの方向の選択により、これらの方向に沿った局所探索を実行して、個別被覆空間の例として新しい球を最適に配置することが可能となる。

さらに好ましい実施形態によれば、ステップｂ）で障害物があると判定された場合、親被覆空間のうちで該障害物のない部分がある特定の範囲まで被覆されるように子ノードが前記親被覆空間内に生成される。これにより、物体が存在する場合に無障害物空間の被覆が向上する。

さらに好ましい実施形態によれば、ステップｂ）で２個以上の障害物があると判定された場合、２個の障害物の間の最小障害物距離が決定され、個別被覆空間の最小距離の方向における最大範囲が前記最小障害物距離よりも小さく設定される。この場合、例えば、最小障害物距離の半分の半径を有する球でも依然として両方の障害物を通る可能性がある。これにより、近接した障害物の間の経路を検出することが可能となる。

さらに好ましい実施形態によれば、現在の親ノードの最近接ノードが決定され、該最近接ノードが現在の親ノードを中心とする子ノードの個別被覆空間内にある場合、現在の親ノードから前記最近接ノードまでの部分経路に対するコストを含む全コスト値が前記最近接ノードに対して決定され、該コストが現在の親ノードの全コストよりも低い場合、前記最近接ノードが新しい親ノードとして選択される。これにより、例えば個別被覆空間が球のとき、ノードが最小半径よりも近い場合、さらにノードを探索することなく、経路のさらなる最適化が可能となることが保証される。

さらに好ましい実施形態によれば、好ましくは球状の、親被覆空間および／または個別被覆空間の形状を規定するパラメータがランダムに決定され、好ましくは、球の半径が１個以上の確率分布に従ってランダムに決定される。これにより、十分な精度を保つことが可能でありながら、精度を低減することによって、より短い時間で適当な経路を見つけることが可能となる。その場合、有効な半径は、最小値と最大値の平均、または一般に、例えば球の半径の分布の平均である。最小半径は、分布の最小半径である。ランダム化を用いて通過する障害物間の最小距離は、確率分布から決定される大きいほうの半径の半分である。

さらに好ましい実施形態によれば、個別被覆空間および／または親被覆空間が球の場合、対応する半径が、前記２個以上の障害物の間の最小距離の半分よりも小さく決定される。これにより、このような半径を有する球が障害物間の最小距離を通ることができるように最小半径を選択することが可能となる。

さらに好ましい実施形態によれば、障害物の時間発展、好ましくは障害物の移動を実現するため、障害物および前記経路に沿ったノードがさらに時間の次元を有するように実現される。例えば、始点と終点の間の状態空間はこの場合には時間次元も含む。例えば２次元空間内の矩形状の障害物は、その矩形を第３次元に拡張した直平行六面体の形を有する。これにより、障害物の時間発展を実現することが容易となる。

さらに好ましい実施形態によれば、障害物の時間発展、好ましくは障害物の移動を実現するため、ノードの状態に、障害物のない時間を示す時間間隔が割り当てられる。例えば、この間隔は、１から番号付けされてもよい。ノードを探索しながら、到達した各ノードにおける時間間隔が保存される。次の間隔が始状態間隔に状態移動時間を加えたものと両立し、移動において障害物がない場合にのみ、次のノードに到達することができる。ノードを探索しながら、コストを更新する。コストは、以前の到達時刻を使用する。移動を効率的に保存するため、移動は、所定の時間ステップで、移動する物体を含む最小体積の多次元矩形と結びつけることができる。

さらに好ましい実施形態によれば、前記始状態から前記終状態までの発展が運動学的動力学的である場合、親ノードと子ノードの２状態間のコストが局所最適化タスクを実行することによって計算される。これにより、２個の状態またはノードの間のコストを非常に効率的に計算することが可能となる。

さらに好ましい実施形態によれば、ステップａ）〜ｇ）が、初期親ノードとしての前記終ノードから逆向きに前記始ノードに戻るようにも実行され、前記始ノードから出発する経路と前記終ノードから出発する経路の両方の経路のノードが相互にある特定の距離内にある場合に終了条件が満たされ、前記終状態集合がただ１つの終状態のみを含む。これは、始点と終点の間の経路を決定する別法を提供し、始ノードと終ノードの間の経路のさらに高速な決定を可能にする。

さらに好ましい実施形態によれば、状態、ノードおよび障害物、好ましくは移動する障害物の時間発展を実現するため、逆向きの時間方向を考慮するように時間が減算される。これにより、状態、ノードおよび障害物、好ましくは移動する障害物の時間発展の容易な実現が可能となり、逆向きの時間方向を考慮するように時間が減算されることで、始ノードと終ノードの間の経路を決定するための二重伝搬ツリーに対する時間発展の容易な実現が可能となる。

さらに好ましい実施形態によれば、新しい親および／または子ノードごとに、宛先までの直接経路に沿ってノードが追加され、判定条件は、ステップａ）〜ｇ）が実行され前記新しい親ノードが前記終状態集合において全コストがより低い終状態を表すノードであるような部分経路によって終ノード集合に到達する場合にのみ適用される。これにより、それぞれの新しい親ノードにおいて直接に宛先に到達する試行を実現することが可能となる。宛先までの直接経路に沿ったノードは単純に追加される。直接に接続する延長を含む経路によって終状態／終ノードに到達する場合には、終了条件は満たされないのが好ましい。その理由は、より低い全コストを加える経路が存在する可能性があるからである。終了条件が満たされるのは、終状態／終ノードに到達するときに経路が延長ノードを含まない場合、または、キュー（後述）が空であるか、もしくは所定の反復数に到達した場合だけである。

さらに好ましい実施形態によれば、前記始状態から前記終状態集合までの経路がある時間限界内に見つかった場合、より低いコストの代替経路をチェックするために残りの時間を使用してＲＲＴ^＊手順を適用する。これにより、有界な理想または最適の経路が既に見つかったが時間が残っている場合、すなわち時間拘束を破らずに、見つかった経路をさらに最適化することが可能となる。さらなる利点として、有界誤差経路が見つかった後に漸近的最適化が達成される。ＲＲＴ^＊手順は、ＲＲＴ手順の漸近的に最適なバージョンであり、KaramanとFrazzoliにより開発され、上記の非特許文献１に開示されている。

さらに好ましい実施形態によれば、複数の物体がある場合、第１の物体に対してステップａ）〜ｇ）が実行された後、障害物として固定された該第１の物体に対して決定された経路を考慮して１個以上の第２の物体に対してステップａ）〜ｇ）が実行され、すべての経路に対する共通の全コストが決定される。これにより、それぞれの始ノードから対応する終ノードまでの経路を見つけるべき複数の物体を容易に実現することが可能となる。この場合の一例は、クワッドローターが、障害物のある環境で同時に移動する必要がある場合である。その場合、共通コスト関数が規定され、本発明の一実施形態による方法は、各ロボット／輸送手段に対して、他のロボット輸送手段の経路を固定して考慮することによって、別個に反復される。コストは、右から近づく輸送手段が左から到着する輸送手段よりも小さい罰金コストを有するように、輸送手段の優先度値に関連する因子を含んでもよい。その場合、判定基準は、例えば、見つかった経路においてもはや有意な変化がない場合や、所定の反復数に到達した場合のような、何らかの収束基準としてもよい。

さらに好ましい実施形態によれば、共通の全コストが第１の物体と第２の物体の間の相互作用コストを含み、好ましくは、該相互作用コストは、前記第１の物体と第２の物体の間の距離が大きいほど減少する。これは、２個の物体がどのように接近してもコストが高くなるため衝突が回避されることを考慮している。

さらに好ましい実施形態によれば、個別被覆空間および／または親被覆空間の中心がグリッド構造に割り当てられる。これにより、例えば、球の中心を規則的および／または固定的なグリッド上に置くことが可能となる。その利点の１つは、衝突をチェックする必要がないことである。すなわち、ノードのすべての位置は相互に固定されるため、球状の個別被覆領域はいかにしても重なり合わないように選択可能である。フレキシビリティを提供するため、グリッド上のノードの一部に到達しないようにして、どのノードを探索すべきかにおいて少なくともある程度の適応可能性が実現される。グリッドを生成するためには、任意の格子またはグリッド生成手順を使用可能である。例えば、格子は、空間の次元数だけの線型独立なベクトルの基底によって生成可能である。

さらに好ましい実施形態によれば、各ノードに対して、始状態から終状態集合までの経路を取得するのに何個のノードが接続される必要があるかを指定するために閾値が使用される。これにより、始ノードから終ノードまでのノード数を低減あるいは制限することが可能となる。この臨界閾値は、各ノードに対して、何個の球およびノードが接続される必要があるかを決定するために使用可能であり、接続性を取得するため、すなわち、少なくとも１つの経路を見つけるための平均値である。例えば、すべてのノードが同じ接続性を有する（すなわち、接続されたノードの数がｋに等しい）規則的ツリーグラフでは、臨界確率閾値は１／ｋである。接続されたノードの数が低減されると、ステップａ）〜ｇ）を実行した場合に実際には経路が見つからないことがあり、あるいは、見つかった経路の誤差が所与の限界内に入ることが保証されない。これらの２つの事態を軽減するため、ノードあたりいくつかの到達ノードを保存し、より多くの反復が可能な場合や、宛先に到達せずさらなるノードに行けない場合に、過去に未発見のノードをさらに使用してもよい。

さらに好ましい実施形態によれば、各ノードに対して、コスト値を示すためにキューが使用され、期待コスト値が最低のノードが次のノードとして使用され、より高いコストの他のノードは前記キュー内に期待コスト値に従って配列される。これにより、探索中にノードが挿入される優先度キューを提供することが可能となる。この優先度キューは、より低コストの要素を返して除去し、その要素が親ノードとして使用される。

さらに好ましい実施形態によれば、ステップｂ）〜ｆ）を実行するため、各子ノードを経由する経路に対する全コスト値が後で決定され、現在の子ノードを経由する全コスト値が前の子ノードのものよりも小さい場合、現在の子ノードが新しい親ノードとして選択され、前の子ノードが前記キューに再挿入される。これにより、容易かつ効率的に新しい親ノードを決定することが可能となる。

さらに好ましい実施形態によれば、ステップａ）を実行するため、前記始ノードが前記キューに挿入される。これにより、経路を決定するための反復が非常に容易に初期化される。

本発明を好ましい態様で実施するにはいくつもの可能性がある。このためには、一方で請求項１に従属する諸請求項を参照しつつ、他方で図面により例示された本発明の好ましい実施形態についての以下の説明を参照されたい。図面を用いて本発明の好ましい実施形態を説明する際には、本発明の教示による好ましい実施形態一般およびその変形例について説明する。

経路を決定する基礎問題を模式的に示す図である。 本発明の第１の実施形態によるシステムを示す図である。 球状の親被覆空間および個別被覆空間を示す図である。 障害物によって部分的に遮蔽された親被覆空間を示す図である。 ２個の物体によって部分的に遮蔽された親被覆空間を示す図である。 本発明の第２の実施形態による方法の結果として決定される経路の結果を示す図である。 最終ノードに到達する際の最大可能コスト誤差を模式的に示す図である。 本発明の第３の実施形態による方法の使用例を示す図である。 従来の急速探索ランダムツリー法の結果を示す図である。 従来の連結急速探索ランダムツリー法を示す図である。 本発明の第４の実施形態による方法のステップを示す図である。 本発明の第５の実施形態による方法のステップを示す図である。

図１は、経路を決定する基礎問題を模式的に示している。

図１に、例示的な問題を示す。始状態Ｎ０と、終ノード（ＮＥ）を含む終状態集合ＥＳＳとの間の無衝突経路Ｐを見つけたい。ただし、障害物Ｏ（ここでは矩形状）が一般には始状態Ｎ０と終状態集合ＥＳＳの間にある。

図２は、本発明の第１の実施形態によるシステムを示している。

図２に、有界近似最適コストの局所成長ツリーに対するシステムアーキテクチャの例を示す。入力パラメータとして、障害物関数、始状態、終状態集合、コスト関数および近似未来コスト関数、状態の動的記述、ならびに球ＩＣＳの密度等の他の方法パラメータが使用される。その場合、経路計画システムは、近似された最適経路、見つかった経路のコスト、探索されたツリーおよびノードコストを出力する。

経路を計算するため、すなわち、始状態ｘ_ｓと終状態集合Ｘ_ｆの間の１個以上の経路を見つけるため、本発明の一実施形態による方法を用いることにより、いわゆる状態発展関数によって限定された時間変動可能な障害物を回避することが可能となる。返される経路は、コスト関数ｆ（ｘ）に関して測定可能である。ただしｆ（ｘ）は、始状態ｘ_ｓから現在の状態ｘまで移動するコストである。経路のコストは、中間状態までの現コストに、終状態または終状態集合までの実コストの下限を加えたものによって近似される。

本発明によれば、探索時に経路のコストが下方推定コスト以下の場合にのみ無障害物空間が探索される。

より詳細には、以下で次式を仮定する。
∃ｈ（ｘ，ｙ）：ｈ（ｘ，ｙ）≦ｆ（ｘ，ｙ）
ｆ（ｘ，ｙ｜ｚ）＝ｆ（ｘ，ｚ）＋ｆ（ｚ，ｙ）
ただし、ｆ（ｘ，ｙ｜ｚ）は、ｚを通ってｘからｙまで行くコストであり、ｈ（ｘ，ｙ）は実コスト関数ｆ（ｘ，ｙ）の下限である。近似下方コストはｇ（ｘ_ｓ，ｘ_ｅ｜ｘ）＝ｆ（ｘ_ｓ，ｘ）＋ｈ（ｘ，ｘ_ｅ）である。ｘ_ｓ、ｘ_ｅは始状態および終状態であり、ｘは経路に沿った状態である。

本発明により提案される方法は、探索中にノードが挿入される優先度キューの使用を含む。優先度キューは、より低コストの要素を返して除去する。ただし、そのコストは期待コストｇ（ｘ_ｓ，ｘ）である。

始状態／始ノードから開始して、それぞれの探索されたノード（最初は始ノードのみ）に対して、半径Ｒ_ｍｉｎの球ＩＣＳとなる近隣空間をおよそ被覆する新ノードＣＮ０，ＣＮ１，ＣＮ２が生成されるように、追加のノードが探索される。新ノードＣＮ０，ＣＮ１，ＣＮ２は、半径Ｒ_ｍａｘの球ＰＣＳが、半径Ｒ_ｍｉｎの重なり合わない球ＩＣＳで被覆されるまで局所的に生成される。これは、好ましくは、十分だが限られた数の方向を生成して近傍ノードＣＮ０，ＣＮ１，ＣＮ２についてチェックすることによって得られる。近傍ノードＣＮ１，ＣＮ２は、探索関数を用いて見つけてもよい。半径Ｒおよび対応する入力ノードｘの入力に対して、現在のノードＣＮ０を中心とし所与の半径の球ＰＣＳ内にすべてのノードＣＮ１，ＣＮ２が見つかる。返されるノードＣＮ１，ＣＮ２は、好ましくは距離で順序づけされる。また、Ｎ_ｎ個の最近接ノードＣＮ０，ＣＮ１，ＣＮ２のみをチェックすることができる。ただし、Ｎ_ｎは選択されるべきパラメータである。

図３は、球状の親被覆空間および個別被覆空間を示している。

図３に、個別被覆空間ＩＣＳを有する新ノードＣＮ０，ＣＮ１，ＣＮ２の生成を示す。球ＩＣＳの最大個数は次式で規定される。
Ｎ_ｍａｘ＝Ｖ_ｍａｘ／Ｖ_ｍｉｎ
ただし、Ｖ_ｍａｘ、Ｖ_ｍｉｎはｄ次元球ＩＣＳの体積である。最大反復数Ｎ_{ｉｔｅｒ，ｍａｘ}は、始状態および１個以上の終状態を焦点とする超楕円体内の半径Ｒ_ｍｉｎの球の数によって与えられ、距離の和は最適解のコストによって与えられる。
Ｎ_{ｉｔｅｒ，ｍａｘ}＝Ｖ_{ｅｌｌｉｐｓｅ}／Ｖ_ｍｉｎ

図４は、障害物によって部分的に遮蔽された親被覆空間を示している。

図４において、障害物Ｏが存在するとき、半径Ｒ_ｍａｘの球ＰＣＳが半径Ｒ_ｍｉｎの子ノードＣＮ０，ＣＮ１，ＣＮ２で被覆されている。障害物Ｏの近くで探索が実行される場合、ノードＣＮ０，ＣＮ１，ＣＮ２は、残りの空間を考慮し、子ノードＣＮ０，ＣＮ１，ＣＮ２の半径Ｒ_ｍｉｎによって与えられる密度で残りの空間を被覆するように生成される。

図５は、２個の物体によって部分的に遮蔽された親被覆空間を示している。

図５において、狭い通路を被覆する球が示されている。より詳細には、図５は、２個の障害物Ｏ１，Ｏ２が存在する場合における親ノード空間ＰＣＳの被覆を示している。障害物Ｏ１，Ｏ２が互いに非常に近い場合、球ＩＣＳの半径Ｒ_ｍｉｎが２個の障害物Ｏ１，Ｏ２の間の最小距離Ｄ_ｍｉｎの大きさの約半分であれば、個別被覆空間ＩＣＳ（すなわち球）はうまく通過できる。

図６は、本発明の第２の実施形態による方法によって決定される経路の結果を示している。

図６に、障害物Ｏが存在する場合の、始ノードＮ０から終ノードＮＥまでの局所成長ツリーあるいは経路Ｐの一例が示されている。図６からわかるように、ツリーのない領域に部分的に外挿されたすべてのツリーの周囲は楕円体の形である。また、図６からわかるように、経路Ｐは、始ノードＮ０から終ノードＮＥまでの途中のすべての障害物を回避している。

図７は、最終ノードに到達する際の最大可能コスト誤差を模式的に示している。

図７には、最終ノードに到達する際のコストに生じ得る最大誤差が示されている。この誤差は、中心が最適経路に沿って配置されていない可能性のある球の配置に由来する。これは、最大で１／ｓｉｎ（４５°）＊Ｌの誤差に相当し、約２Ｌに等しい。ただし、Ｌはノードに沿った経路の長さである。コスト誤差は、実際のコスト関数およびそれと経路の長さとの関係に依存する。決定される経路の実際の誤差はこれより低い。

図８は、本発明の第３の実施形態による方法の使用例を示している。

図８に、領域被覆のための局所成長ツリーの使用例が示されている。基礎となる領域被覆問題は、一般的な経路計画問題の拡張である。ここで、初期状態から出発する無障害物空間がほとんど被覆されなければならない。ここでは相異なる経路間のコストは重要でないため、コストは各経路に対して同一とすることができ、探索経路の接続性のみが重要である。各反復において、すべての近傍ノードが好ましくは保存され、新ノードは、最小半径Ｒ_ｍｉｎよりも近い距離にある近傍である場合には追加が回避される。新ノードがもはや追加できなくなった自由空間の探索の終了時に、ハミルトン閉路問題あるいは巡回セールスマン問題を定式化し、生成されたグラフについて始点から解くことができる。最小半径Ｒ_ｍｉｎは好ましくはそれらの被覆経路間の距離の半分に設定される。球状の空間を探索するためにキューを使用することにより、探索が始点からある特定の距離で停止するようにすることができる。領域被覆を高速化するため、キューをリストとしての先入れ先出しＦＩＦＯまたは後入れ先出しＬＩＦＯのデータ構造で置き換えることが可能である。

図９は従来の急速探索ランダムツリー法の結果を示し、図１０は従来の連結急速探索ランダムツリー法を示している。

図１によるケースに対して、図９に従来の急速探索ランダムツリー（rapid-exploring random tree, ＲＲＴ）法を示し、図１０に連結急速探索ランダムツリー（connected rapid-exploring random tree, Ｃ−ＲＲＴ）法を示している。始ノードＮ０から出発し、始ノードＮ０から終ノードＮＥまでの経路Ｐの間に障害物があり、障害物Ｏを回避する経路Ｐは発見できない。

図１１は、本発明の第４の実施形態による方法のステップを示している。

図１１に、本発明の一実施形態による方法を示す。

第１のステップＳ１で、親ノードに対して複数の子ノードが決定される。ただし、ステップＳ１が最初に実行されるとき、始状態を表す始ノードが親ノードとなり、子ノードは親ノードまである特定の距離内にある。

第２のステップＳ２で、親ノードから子ノードのそれぞれへの遷移に障害物がないかどうかがチェックされ、障害物がある場合、対応する子ノードおよび親ノードからこの子ノードまでのこの部分経路が除外される。

第３のステップＳ３で、前記部分経路のそれぞれについてコスト値が計算される。

第４のステップＳ４で、始ノードから親ノードまでのコスト値に、前記計算されたコスト値が加算される。

第５のステップＳ５で、子ノードのそれぞれから終状態を表す終ノードまでの部分経路に対して推定または期待されるコスト値が加算される。

第６のステップＳ６で、ステップｄ）〜ｆ）に従って決定されたコスト値から最低の全コスト値が決定され、決定された最低コストに対応する子ノードが新しい親ノードとして選択される。

第７のステップＳ７で、選択された子ノードが終ノードまで所与の距離内にあるという終了条件を含む少なくとも１つの終了条件が満たされるまでステップＳ１〜Ｓ６が繰り返し実行される。

図１２は、本発明の第５の実施形態による方法のステップを示している。

図１２に、本発明の別の実施形態による方法のステップを示す。

第１のステップＴ１で、始ノードが優先度キューに挿入される。

第２のステップＴ２で、最低のコストが期待されるノードが優先度キューから取り出される。

第３のステップＴ３で、半径Ｒ_ｍａｘの球内に半径Ｒ_ｍｉｎの子ノードを被覆する局所的球が生成される。

第４のステップＴ４で、現在の親ノードから子ノードのうちの１つへの遷移に障害物がないかどうかがチェックされる。

第５のステップＴ５で、親のコストと、現在の親ノードから子ノードまでの接続コストを加算することによって、始ノードから現在の親ノードまでのコストが計算される。また、宛先／終ノードまでのコストｈ（ｘ，ｘ_ｆ）と現ノードのコストを加算することによって、期待コストが計算される。

第６のステップＴ６で、現ノードに対する最近接ノードまたはノード群が決定される。

第７のステップＴ７で、最近接ノードが現ノードまでＲ_ｍｉｎよりも近いかどうかがチェックされ、そうである場合、最近接ノードからのコストと現ノードからのコストがチェックされ、現ノードからのコストが最近接ノードからのコストよりも高い場合、現ノードが優先度キューに再挿入され、その場合、親ノードが、決定された最近接ノードとなる。

第８のステップＴ８で、半径Ｒ_ｍｉｎ内に近傍ノードが見つからない場合、現ノードのコストが計算されて優先度キューに追加され、現ノードが経路に追加され、経路が更新される。

第９のステップＴ９で、半径Ｒ_ｍａｘ内のすべての近傍ノードが決定される。

第１０のステップＴ１０で、決定された近傍ノードのそれぞれに対するコストが、新しい子ノードの決定されたコストに、新ノードから近傍ノードまでの接続に対するコストを加えたものと比較してチェックされる。

第１１のステップＴ１１で、新ノードのコストが親ノードのコストよりも低い場合、親ノードが優先度キューに再挿入され、経路の全コストが更新され、ツリー内の経路が幅優先探索でたどられて、接続されたノードのコストが更新される。

第１２のステップＴ１２で、優先度キューが空になるか、最大反復数または宛先集合／終ノードに到達するまで、ステップＴ２〜Ｔ１１が反復される。

要約すれば、本発明により、現在到達したノードまでのコストを維持し、下限コスト関数を用いるとともに確率的局所探索を実行して最も有望なノードを選択することにより、誤差有界の最適コストで無障害物状態空間経路を発見することが可能となる。

また、本発明により、所定の確率分布に従った半径の球で自由空間を探索することが可能となる。

また、本発明により、期待コストを用いて、有界な最適解を達成するための実コストの下限を計算することが可能となる。

また、本発明により、優先度キューまたは整列データ構造を用いて自由空間を探索することが可能となる。

特に、本発明により、近隣のノードをチェックし前方にコストを更新することによって、探索経路ツリーの再最適化が可能となる。

また、本発明により、無障害物状態空間における経路の発見において、始ノードと終ノードを二重に発展させることが可能となる。

また、本発明により、無障害物時間間隔を規定し、ある間隔から、新しい状態における別の無衝突時間間隔まで移動する実現可能性を発展させながらチェックすることによって、状態空間に完全に連続的な別の次元を追加することなく、時間すなわち状態空間における時間軸を考慮することが可能となる。

本発明により、以下のものが提供可能となる。
１．優先度付きの高速局所成長ツリー構造を使用することにより、障害物を回避する経路計画問題に対する有界な近似最適解を計算するシステム。
２．局所成長ツリー構造を用いた協調的経路計画を計算するシステム。
３．まず所定の密度半径の球で自由空間を探索してから、結果として得られたグラフに対してハミルトン閉路を計算することによって、領域被覆経路を計算するシステム。
４．輸送手段等の各移動物体ごとに、他は固定して反復的に解を求め、輸送手段やロボット等の移動物体間の距離の増大とともに減少する相互作用のコストを含む共通の目的関数を規定することによって、協調的かつ分散的な経路計画を計算する方法。
５．まず所定の分布半径の球で局所優先度データ構造を用いて探索をしてから、結果として得られたグラフに対してハミルトン閉路／巡回セールスマン問題を計算することによって、移動物体／輸送手段／ロボットに対する領域被覆経路を計算する方法。

要約すれば、本発明は、協調的経路の計算を含む移動物体／輸送手段／ロボットに対する計画経路を計算し、領域被覆計算の問題を解決する方法を提供する。この方法は、好ましくは以下のステップを含む。
１．最適コスト計算により、所定の確率分布に従う等半径の球を用いた、局所成長ツリー／グラフ。
２．有望でない解領域を回避するため、有界最適コストの自由空間を探索することを可能にする優先度データ構造。
３．次善でない解に対してのみ自由空間を探索することを可能にする下限コスト関数。

本発明は、とりわけ、以下の利点を有する。本発明により、自由空間の興味のない領域の探索を回避した有界な誤差コストが可能となる。また、本発明は、優先度キューを用いて解の有界最適性を保証し、誤差有界な最適解までの反復数を限定することが可能となる。

本発明は、有界なステップ数の後に経路が見つかることを保証し、探索半径が障害物間の最小通路の半分である場合に最適解からの誤差が有界な経路が得られる。本発明は、経路計画問題および拡張に対する高速かつ誤差有界な解を提供する。

また、本発明は、医療、ロボット動作、ドローン、自律運転輸送手段、協調的輸送手段、精密制御、コンピュータアニメーション、ナビゲーション、コンピュータシミュレーション、ゲーム制作、軍事・宇宙、海洋・海峡航行、農業・鉱業用無人輸送手段等の多数のさまざまな応用分野に適用可能である。

上記の説明および添付図面の記載に基づいて、当業者は本発明の多くの変形例および他の実施形態に想到し得るであろう。したがって、本発明は、開示した具体的実施形態に限定されるものではなく、変形例および他の実施形態も、添付の特許請求の範囲内に含まれるものと解すべきである。本明細書では特定の用語を用いているが、それらは総称的・説明的意味でのみ用いられており、限定を目的としたものではない。

Claims (25)

1. １個以上のノードを経由して始状態から１個以上の終状態を含む終状態集合まで移動するロボット等の物体の経路を動的に決定する方法において、各ノードは１個以上の障害物を回避するように探索された無障害物状態として規定され、該方法は、
   ａ）親ノードに対して複数の新しい子ノードを生成するステップであって、ステップａ）が最初に実行されるときには前記始状態を表す始ノードが前記親ノードとなり、前記子ノードは前記親ノードまである特定の距離内にある、生成するステップと、
   ｂ）前記親ノードから前記子ノードのそれぞれへの遷移に障害物がないかどうかをチェックし、障害物がある場合、対応する子ノードおよび前記親ノードから該子ノードまでのこの部分経路が除外される、チェックするステップと、
   ｃ）前記部分経路のそれぞれについてコスト値を計算するステップと、
   ｄ）前記始ノードから前記親ノードまでのコスト値に、前記計算されたコスト値を加算するステップと、
   ｅ）前記子ノードのそれぞれから終状態を表す終ノードまでの部分経路に対して推定または期待されるコスト値を加算するステップと、
   ｆ）ステップｄ）〜ｅ）に従って決定されたコスト値から最低の全コスト値を決定し、該決定された最低コストに対応する子ノードを新しい親ノードとして選択するステップと、
   ｇ）選択された子ノードが前記終ノードまで所与の距離内にあるという終了条件を含む少なくとも１つの終了条件が満たされるまでステップａ）〜ｇ）の実行を繰り返すステップと
   を備えたことを特徴とする、物体の経路を動的に決定する方法。
2. 子ノードから前記終状態集合までの実コストに対する下限値が決定され、ステップｅ）における前記推定または期待されるコストとして使用されることを特徴とする請求項１に記載の方法。
3. 子ノードを決定するため、親被覆空間が選択され、子ノードがその個別被覆空間で所定の親範囲まで前記親被覆空間を被覆するように子ノードが局所的に生成され、前記個別被覆空間は相互に重なり合わないことを特徴とする請求項１または２に記載の方法。
4. すべての子ノードに対する個別被覆空間および／またはすべての親ノードに対する親被覆空間の形が所定の半径を有する球状であることを特徴とする請求項３に記載の方法。
5. 子ノードを局所的に生成するため、前記親被覆空間の中心から限られた数の方向が生成され、該方向に子ノードを生成することによって前記親被覆空間を被覆するために該方向が使用されることを特徴とする請求項３または４に記載の方法。
6. ステップｂ）で障害物があると判定された場合、親被覆空間のうちで該障害物のない部分がある特定の範囲まで被覆されるように子ノードが前記親被覆空間内に生成されることを特徴とする請求項３ないし５のいずれか１項に記載の方法。
7. ステップｂ）で２個以上の障害物があると判定された場合、２個の障害物の間の最小障害物距離が決定され、個別被覆空間の最小距離の方向における最大範囲が前記最小障害物距離よりも小さく設定されることを特徴とする請求項３ないし６のいずれか１項に記載の方法。
8. 現在の親ノードの最近接ノードが決定され、該最近接ノードが現在の親ノードを中心とする子ノードの個別被覆空間内にある場合、現在の親ノードから前記最近接ノードまでの部分経路に対するコストを含む全コスト値が前記最近接ノードに対して決定され、該全コストが現在の親ノードの全コストよりも低い場合、前記最近接ノードが新しい親ノードとして選択されることを特徴とする請求項３ないし７のいずれか１項に記載の方法。
9. 親被覆空間および／または個別被覆空間の形状を規定するパラメータがランダムに決定されることを特徴とする請求項３ないし８のいずれか１項に記載の方法。
10. 個別被覆空間および／または親被覆空間が球の場合、対応する半径が、前記２個以上の障害物の間の最小距離の半分よりも小さく決定されることを特徴とする請求項７ないし９のいずれか１項に記載の方法。
11. 障害物の時間発展を実現するため、障害物および前記経路に沿ったノードがさらに時間の次元を有するように実現されることを特徴とする請求項１ないし１０のいずれか１項に記載の方法。
12. 障害物の時間発展を実現するため、ノードの状態に、障害物のない時間を示す時間間隔が割り当てられることを特徴とする請求項１ないし１０のいずれか１項に記載の方法。
13. 前記始状態から前記終状態集合までの発展がキノダイナミックである場合、親ノードと子ノードの２状態間のコストが局所最適化タスクを実行することによって計算されることを特徴とする請求項１ないし１２のいずれか１項に記載の方法。
14. ステップａ）〜ｇ）が、初期親ノードとしての前記終ノードから逆向きに前記始ノードに戻るようにも実行され、前記始ノードから出発する経路と前記終ノードから出発する経路の両方の経路のノードが相互にある特定の距離内にある場合に終了条件が満たされ、前記終状態集合がただ１つの終状態のみを含むことを特徴とする請求項１ないし１３のいずれか１項に記載の方法。
15. 新しい親および／または子ノードごとに、前記終ノードまでの直接経路に沿ってノードが追加され、終了条件は、ステップａ）〜ｇ）が実行され前記新しい親ノードが前記終状態集合において終状態を表すノードであるような部分経路によって終ノード集合に到達する場合にのみ適用されることを特徴とする請求項１ないし１４のいずれか１項に記載の方法。
16. 前記始状態から前記終状態集合までの経路がある時間限界内に見つかった場合、より低いコストの代替経路をチェックするために残りの時間を使用してＲＲＴ^＊手順を適用することを特徴とする請求項１ないし１５のいずれか１項に記載の方法。
17. 複数の物体がある場合、第１の物体に対してステップａ）〜ｇ）が実行された後、障害物として固定された該第１の物体に対して決定された経路を考慮して１個以上の第２の物体に対してステップａ）〜ｇ）が実行され、共通の全コストが決定されることを特徴とする請求項１ないし１６のいずれか１項に記載の方法。
18. 共通の全コストが第１の物体と第２の物体の間の相互作用コストを含むことを特徴とする請求項１ないし１７のいずれか１項に記載の方法。
19. 前記相互作用コストは、前記第１の物体と第２の物体の間の距離が大きいほど減少することを特徴とする請求項１８に記載の方法。
20. 個別被覆空間および／または親被覆空間の中心がグリッド構造に割り当てられることを特徴とする請求項３ないし１９のいずれか１項に記載の方法。
21. 各ノードに対して、始状態から終状態集合までの経路を取得するのに何個のノードが接続される必要があるかを指定するために閾値が使用されることを特徴とする請求項１ないし２０のいずれか１項に記載の方法。
22. 各ノードに対して、期待コスト値を示すためにキューが使用され、期待コスト値が最低のノードが次のノードとして使用され、より高いコストの他のノードは前記キュー内に期待コスト値に従って配列されることを特徴とする請求項１ないし２１のいずれか１項に記載の方法。
23. ステップｂ）〜ｆ）を実行するため、各子ノードを経由する経路に対する全コスト値が決定され、現在の子ノードを経由する全コスト値が前の子ノードのものよりも小さい場合、現在の子ノードが新しい親ノードとして選択され、前の子ノードが前記キューに再挿入されることを特徴とする請求項２２に記載の方法。
24. ステップａ）を実行するため、前記始ノードが前記キューに挿入されることを特徴とする請求項２２または２３に記載の方法。
25. １個以上のノードを経由して始状態から１個以上の終状態を含む終状態集合まで移動するロボット等の物体の経路を動的に決定するシステムにおいて、各ノードは１個以上の障害物を回避するように探索された無障害物状態として規定され、該システムは、
    ａ）親ノードに対して複数の新しい子ノードを生成するステップであって、ステップａ）が最初に実行されるときには前記始状態を表す始ノードが前記親ノードとなり、前記子ノードは前記親ノードまである特定の距離内にある、生成するステップと、
    ｂ）前記親ノードから前記子ノードのそれぞれへの遷移に障害物がないかどうかをチェックし、障害物がある場合、対応する子ノードおよび前記親ノードから該子ノードまでのこの部分経路が除外される、チェックするステップと、
    ｃ）前記部分経路のそれぞれについてコスト値を計算するステップと、
    ｄ）前記始ノードから前記親ノードまでのコスト値に、前記計算されたコスト値を加算するステップと、
    ｅ）前記子ノードのそれぞれから終状態を表す終ノードまでの経路に対して推定または期待されるコスト値を加算するステップと、
    ｆ）ステップｄ）〜ｅ）に従って決定されたコスト値から最低の全コスト値を決定し、該決定された最低コストに対応する子ノードを新しい親ノードとして選択するステップと、
    ｇ）選択された子ノードが前記終ノードまで所与の距離内にあるという終了条件を含む少なくとも１つの終了条件が満たされるまでステップａ）〜ｇ）の実行を繰り返すステップと
    を行うように適応されるか、または、行うために相互に協力するように適応された１個以上の計算要素を備えたことを特徴とする、物体の経路を動的に決定するシステム。
    

Images