BEIKE blog

以下の論文を翻訳。

https://www.cs.cmu.edu/~reids/papers/ICRA07Melchior.pdf

本論文では、Rapidly-exploring Random Tree (RRT)経路計画アルゴリズムの新しい拡張について述べる。粒子RRTアルゴリズムは、粒子フィルタの動作と同様に領域内の不確実性を明示的に考慮している。検索木の各拡張は確率過程として扱われ、複数回のシミュレーションを行うことで、環境中の指定された不確実性に基づいてロボットの動作を特徴付けることができ、この不確実性の下での性能を保証することができます。サーチツリーへの拡張、したがってパス全体は、成功した実行の期待される確率に基づいて選択することができます。このアルゴリズムの利点は、未知の摩擦係数を持つ荒れた地形で動作するローバーのシミュレーションで実証されています。

序論

Rapidly-exploring Random Tree (RRT) アルゴリズム[1]は、kinodynamicConstraintsを用いた経路計画のための一般的な手法である。簡単に言えば，RRTは既知の到達可能な状態でランダムな動作を試みることで，到達可能な状態の探索木を構築する．その動作によってロボットが障害物に接触したり、力学的制約に違反しない限り、動作は成功したとみなされ、結果として到達可能な状態の木に追加されます。一般的にシミュレータは、ロボットの初期状態を与えられた行動の結果を決定するブラックボックスとして扱われます。これにより、複雑なシステムダイナミクスが解析的な制御を困難にしたり、不可能にしたりする領域にアルゴリズムを適用することができます。RRは、車輪付き自転車や脚付き自転車、水中ロボットや航空機などへの適用に成功しているが、アクションの成功を二進法で決定するため、このアルゴリズムの適用には2つの重要な制限がある。第一に、与えられた初期状態から成功する可能性のある複数のアクションのランク付けやスコアリングができません。アクションには多くの場合、関連するコスト（実行に必要なエネルギーや時間など）があり、計画を立てると、開始からゴールまでのパスが累積的なコストを変化させながら生成されることがあります。ほとんどのRRT実装では、少なくともユークリッド距離のパス長を考慮していますが、パスコストの他の概念も取り入れる必要があります。本論文では、未知の地形を航行するローバーを対象としたアプリケーションを紹介する。また、摩擦係数などの地形の特性は大まかな推定しかできないが、これらの特性はローバーの挙動に大きな影響を与える可能性があり、特に地形を横断する場合には、ローバーの挙動に大きな影響を与える可能性がある。関連する[2]の研究では、障害物の動きを予測することで、移動する障害物の形で環境が変化することを考慮しています。もし障害物が予期せぬ動きをした場合、パスは単純に再計画される。不確実な地形での計画のための1つのアプローチ[3]では，未知の条件の期待値の範囲内で，計画された行動が同じ結果をもたらすことを保証しています．本論文では、RRTアルゴリズムを拡張し、新しいアプローチで不確実性の下での計画の問題に直接対処する。粒子RRT(pRRT)アルゴリズムは、不確実性を計画された経路に伝播させることで、不確実性環境下でのRRTの生成を容易にする。探索木への拡張は、異なる可能性の高い条件の下で数回試行される。これらのシミュレーションの結果をクラスタリングすることで、検索ツリーのノードが作成されます。各アクションが正常に実行される確率を定量化することで、パス全体に従う確率を決定することができる。図1に検索木の例を示します。シミュレーションの実験結果を示し、このアプローチにより不確実性に強いパスが得られることを示します。

II. ALGORITHMDESCRIPTION pRRTアルゴリズムは、基本的なRRTアルゴリズムを拡張し、特徴的な不確実性を持つ環境で効率的に動作するようにしたものである。本論文では、摩擦が不明な地形上を走行するローバーを想定している。しかし、他のタイプの不確実性は、不正確なアクション実行や障害物のサイズや位置が不確実であるなど、本アルゴリズムに組み込むことができます。

基本的なRRTアルゴリズム

RRT(Rapidly-exploring Random Tree)アルゴリズムは、探索が困難な大規模な状態空間を探索する際に有用なランダム化アルゴリズムです。図2と図3に示すアルゴリズムは、状態空間内のランダムな点を反復的に選択し、その点に向かって現在の探索木を拡張しようとします。ＲＡＮＤＯＭＳＴＡＴＥ関数は、状態空間上の一様分布から選択してもよいが、一般的には、目標に向かって多少のバイアスを含む。この拡張は、木T内のRANDOMPOINTpとその最も近い近傍qを考慮することで実行されます。このアルゴリズムの迅速な探索は、この手順に暗黙のバイアスがあることに起因しています。ノードqは、それがランダムポイントpの最近傍である場合にのみ拡張されます。 NEWSTATE関数は、ロボットの動的制約を観察するアクションunnewthatを決定し、状態qでロボットに適用された場合、いくつかの新しい状態exnewをもたらします。ここで重要なことは、xnewはfromqの方向にありますが、xnewとpareは一致することは期待されていないということです。RRTアルゴリズムはロボットの逆運動学を知らなければならないわけではありません。前方運動学だけが、与えられたqと新しい状態を決定するために必要とされています。 NEWSTATEが動的制約に違反したり障害物と衝突したりすることなく新しい状態と動作を見つけた場合、その新しい状態と動作がツリーに追加されます。このように、NEWSTATEは、環境に不確実性があり、シミュレーションの結果に不確実性がある場合でも、通常、拡張の成功について二項決定を行います。このように、拡張するノードを選択してシミュレーションを実行するというプロセスは、目標の許容範囲内の状態xgoalがツリーに追加されるまで繰り返されます。RRTはランダム化アルゴリズムであるため、たとえ実現可能なパスが存在していても、ゴールが見つかる保証はありません。また、探索木の生成に用いられる探索・拡張の性質上、最終的な経路はギザギザした蛇行したものになる可能性があり、より自然な動きを生成するために、生成された経路を滑らかにするための工夫がなされることが多い。これらの問題は、ランダム再起動を利用することで解決されることが多い。

パーティクル拡張

RRTフレームワークには、粒子フィルタの予測ステップに似た粒子ベースの拡張技術を導入する。簡単に言えば、pRRTアルゴリズムは、様々な状況下で、各拡張を複数回シミュレーションすることで動作する。結果として得られた状態は類似のクラスタにグループ化され、各クラスタは木の中の1つのノードとして扱われる。拡張の結果として特定のノードに到達する可能性は、その分布の基礎となる粒子を生成した条件の尤度に基づいて計算することができる。EXTENDステップでは、既知の到達可能な状態qに適用するアクションを選択する。一般的に、不確実なパラメータ（パラメータ）は単一の変数Fとして表現することができ、Fをサンプリングするための確率密度関数（PDF）が利用可能であると仮定します。実際には、Fは状態空間の部分によって異なる分布を持っているかもしれません。表記を簡単にするために、ここではFが単一のPDFで表現されていると仮定します。5]で紹介したフレームワークの中では、この不確実性は環境センシングの領域である。初期設定センシングの不確かさも同様の方法で簡単に取り込むことができる。拡張ステップには2つの要素が必要です：シミュレーションの開始状態（図3のノードq）と目的地（図3のポイント）、または等価的に、qtをpに向かって拡張するために使用された公称アクションです。粒子ノード（複数の粒子を含むノード）から拡張する場合、開始状態には主に2つのオプションがあります。単純に各パーティクルの状態変数の加重平均を使用してノードを表現し、この状態からすべての拡張を実行することもできます。この平均状態qは、粒子ノードqにおける真のPDFの平均の推定値です。つまり、このパーティクルノードが無限の数のパーティクルを含んでいた場合の平均値の推定値です。 qの計算に使用される各粒子の重みは、その拡張子を計算するために使用された条件が真の条件である可能性を表しています。各粒子qはパラメータFの値と関連しており、これをFqiと呼びます。この表記法を用いて、粒子ノードの加重平均状態を計算することができます。いくつかの拡張を試みることになるので、ノードqのPDFから拡張の開始状態をサンプリングするオプションもあります。ガウス関数のような関数を粒子qに当てはめて真の連続的なPDFを近似するならば、この分布から開始状態をサンプリングすることができます。しかし、各ノードには少数の粒子が存在することが予想されます。実際、いくつかのノードでは粒子が1つしかないこともあるので、連続関数をフィットさせるための統計的根拠はやや不安定です。その代わりに、パーティクルの離散集合から比例してサンプルすることを選択することができます。これは、我々が拡張する状態が本当に実現可能な状態であることを保証するという付加的な利点があります。これは、拡張する状態が、通過粒子間の補間ではなくシミュレーションの結果として計算されているためです。平均状態を計算するか、離散サンプリングを行うか、始点状態を選択するための戦略の選択については、第III節で説明します。この選択が行われると、通常通りに状態を拡張するために使用されるアクションを計算することができます。アルゴリズムのEXTENDステップでは、新しい粒子を生成するために、フォワードシミュレーションを何度も繰り返して、このアクションを開始状態または状態に適用します。各シミュレーションでは、そのPDFからサンプリングしたFの値を適用します。シミュレーションに成功した状態は、下記のようにクラスタ化してからツリーに追加します。

クラスタリング

クラスタリングの基本的な目的は、実質的に類似したパーティクルをグループ化することである。クラスター内のパーティクルは多少異なりますが、我々はFの値の変化によるツリー内の定性的な分岐を検出したいと考えています。そうしないと、ノードを拡張するために使用される平均値qは、そのノード内のパーティクルの分布の悪い近似値になってしまいます。図４の例の軌跡は、ローバーの軌跡に大きな分岐があることを示しています。この例では、ローバーは円柱で表されるゴールに向かって稜線に沿って移動しています。摩擦係数が十分に大きければ、ローバーは斜面を登ってゴールに到達することができます。摩擦係数が徐々に小さくなると、ローバーは移動に伴って坂道を下っていきます。滑り具合によっては、前方の岩に衝突したり、左や右に通過したりすることもあります。これらの意味的に異なるケースは、計画ツリーの異なるノードとして表現されるべきである。クラスタリングは、重み付きユークリッド距離メトリックを用いた階層的クラスタリングツリー[6]を使用して達成される。我々はロボットの位置(x, y)とヨー(θ)に関心があるので、以下の距離メトリックを使用することを選択しました：ここでαとβは粒子間の差を決定する際に測定単位を正規化するために使用されるスケーリング係数です。階層的クラスタリングツリーアルゴリズムは、このメトリックを使用して、最短距離で分離されたパーティクルとクラスタを反復的に凝集させます。パーティクル間の距離は簡単に計算できますが、クラスター間の距離は様々な方法で計算することができます。以下の結果セクションでは、シングルリンケージと完全リンケージ[7]と呼ばれる2つのストラテジー間の性能の違いを調べます。図5のデンドログラムは、図4のパーティクルに対するアルゴリズムの動作を示しています。粒子には横軸に沿って番号が付けられており、距離は縦軸に再表示されています。反復アルゴリズムは、下から上に向かってデンドログラムを構築していきます。このケースでは、最も近い粒子は8と9なので、図の左下隅付近の水平線セグメントで表されるように、これらの粒子を最初に結合します。次に最も近い粒子を結合して、すべての粒子が1つのクラスターになるまで結合します。この場合、最終的な凝集はパーティクル1と他のすべてのパーティクルを含むクラスターを組み合わせたものになります。成功した凝集の間の距離に最大の差があるかどうかを調べることで、最も異質なサブツリーを結合したツリー内のリンクを見つけます。このリンクとそれに続くすべてのリンクは無視され、以前に作られたリンクは粒子のクラスタリングを決定するために使用されます。このカットオフは、図5の破線の水平線で表されます。この場合、粒子は5つのクラスタに分割されています。テストでは、このアプローチはk-meansアルゴリズム[8]のガウシアンクラスタリングよりも優れた性能を発揮することがわかりました。階層的クラスタリングツリーは、各クラスタのテーマと共分散行列を直接推定する必要がないため、より少ないパラメータを計算することができます。このことは、我々が一握りのポイントだけをクラスタリングしているので、特に重要である。

ノードとパスの確率

各ノードの粒子は、ロボットがその状態に到達することを可能にするFの値の分布の推定値を提供します。このようにして、木構造の成長をロボットが辿る可能性の高いパスを生成する方向にバイアスをかけることができます。このヒューリスティックはRRTアルゴリズムのSELECTEXTENSION関数を変更したもので、状態空間のランダムな点pと木の中の最も近い隣人qを単純に受け入れるのではなく、hRRT技術はノードqの品質に比例して拡張することを選択する。我々の場合、qの品質は次のように定義されます。ここで、qは木の根元からqに到達する確率、qは探索木のすべてのリーフノードの最小確率である。0から1の間の一様分布からランダムな値が選ばれ、qquality> rの場合、点pとqのペアが受け入れられ、拡張が試みられます。また、木の根元からノードに到達する確率だけでなく、このノードからゴールに到達する確率も計算できれば、品質ヒューリスティックの有効性が実証されるかもしれません。そうすれば、A∗[10] のようなヒューリスティックな方法で、スタートから特定のノードを通過してゴールに到達する確率を推定することができる。しかし、このような確率の推定は楽観的なものでなければならない。この状態からすべての選択肢を探索しなければ、唯一許容されるヒューリスティックは1.0であり、これは追加情報を提供しない。今後の研究では、許容されないヒューリスティックがパスの品質に大きな影響を与えずにアルゴリズムの実行時間を改善するかどうかを調査するかもしれない。このため、アルゴリズムは、ゴールに近いノードが合理的に存在する可能性が高い場合でも、ルートに近いノードからの拡張を有利に進めることになる。根元から離れたノードからの拡張を促進するために、経路長を用いて経路確率を正規化します。ここでは，木の中でのノードの深さをノードの深さとする品質計算を，√qを代入している．これにより，根元からの距離が長くなってもノードの品質が急激に低下しないため，実行時間が改善される．しかし，経路の長さにわたって経路確率を平均化することで，そうでなければ可能性の高いノードの長い経路の中で，単一の可能性の低いノードの影響が気づかれないようになる可能性がある．

III. テストと結果

pRRTアルゴリズムは、いくつかのローバーのナビゲーションシナリオを使用して実装され、インシミュレーションテストされています。テストは、図6の傾きのようなかなり単純な領域から始めました。このような状況では、pRRTアルゴリズムによる決定が、摩擦に関係なく一貫した実行への懸念を反映していることを確認することができます。この画像の球の列で表されるロバストパスは、平地を進み、大きく曲がってから斜面を真正面から登っていくものである。さらに、図7の等高線図のような複雑な環境でのテストも行いました。図7の等高線図に示すように、ローバーの出発位置を星印、ゴールを○印で示しています。これらの地形は、実際のローバーが遭遇するであろう状況の代表として選んだものですが、最も直接的な道がベストであるとは限らないことを示しています。台地とクレーターの両方のシナリオでは、ローバーがゴールにたどり着くまでの直接の道のりは、物理シミュレーションが示すように、ローバーが安全に横断できるような標高の変化に対応しなければなりません。残念ながら、実際の地形の特性を実物のローバーが確実に判断することはできないため、ローバーがこれらの経路をうまく追跡できる可能性は低いと思われます。これらのテストの目的は、単にRRTプランナーが高コストの地形を回避できることを示すためだけではありません。しかし、シミュレーションでは、摩擦係数が異なると、これらの領域で異なる結果が得られる可能性が高い。pRRTと標準RRTを用いたパスのロバスト性を定量化するために、各シナリオでパスを計画し、様々な均一な摩擦条件の下でオープンループで実行した。従来のRRTを用いて構築された木は常に単一の摩擦値を仮定しており，pRRTは可能な範囲の摩擦値の一様分布を考慮している．前節で紹介したアルゴリズムとパラメータオプションの最適な選択を決定するために、pRRTアルゴリズムのいくつかのバリエーションを使用しました。

拡張戦略

最初のテストでは、4つの異なるバリエーションのpRRTアルゴリズムを使用した。セクションII-Bで議論したように、我々は各拡張のための初期状態を決定するための戦略を選択しなければならない。我々は、ノードの平均状態qを使用して、各ノードのパーティクルからサンプリングすることによって、アルゴリズムをテストしました。このオプションは、短いパスへの偏りを減らす方法としてII-Dで議論されました。誤差測定は、実際の摩擦係数がRRTによって計画されたものよりも小さい場合に、計画されたパスの終点と計画のオープンループ実行によって到達した終点を比較するために行われました。図8のプロットは、台地シナリオ(図7の上)の500回の実行で平均した誤差(および誤差の標準偏差)を示しています。クレーターシナリオでも同様の結果が得られています。これらのプロットの横軸は、オープンループ実行のための一様な摩擦係数を、RRTで使用される値の何分の一かとして示しています。pRRTアルゴリズムは、計画中にプロットの縦線より右側のすべての摩擦係数を考慮しました。縦軸に示されたエラーは、指定された開始状態とゴール状態の間の距離の分数で表されます。これらのプロットは、pRRT アルゴリズムによる摩擦の不確実性を考慮した結果、pRRT アルゴリズムのすべてのバリエーションで有意に正確なパスが得られることを示しています。通常のRRTは完全決定論的な世界（真の摩擦が計画に使用される摩擦に等しい）では誤差を生じませんが、不確実性が導入された場合、pRRTはRRTよりも誤差が少なくなります。このことは、pRRTによって計画されたパスは、Fchangesの真の値として安全かつ一貫して実行できることを意味している。実際，pRRTは，計画時に考慮した摩擦値の範囲外では，垂直線の左側ほど精度の高いパスを生成し続けている．このアプローチは、不確実性の影響を木の各延長線上でより正確に考慮しているため、よりロバストなものとなっています。しかし、この精度と精度の向上の代償は、表Iに示された統計量で見ることができます。この表は、pRRTのために議論されてきたバリエーションによって引き起こされた違いのいくつかを示しています。このアルゴリズムは、検索木が250ノードを含む時点までにゴールに到達しなかった場合に停止され、成功の列には、500回の試行でゴールが見つかった割合が記載されています。パス確率ヒューリスティックの正規化を行わないと、短いパスへの自然なバイアスにより、検索木のどの枝もゴールに到達するのに十分な長さに成長することができません。この効果は、開始ノードがサンプリングされている場合に最も顕著である。各開始ノードの精度が低いため、1つの拡張子のパーティクルがより広い領域に広がることになり、結果としてより多くのクラスターが発生します。これらのクラスターはそれぞれ確率が低くなるので、正規化の効果がないと長いパスの確率が低下します。表の2行目に記載されているpRRTのバージョンは、拡張を開始するために平均状態qを使用し、経路確率を正規化することにより、不確実性の影響が最小化されているため、RRTと最も類似した性能を発揮します。

クラスタリング戦略

このアルゴリズムのもう一つの重要なパラメータは、各拡張で作成されるパーティクルの数です。このアルゴリズムでは、ローバーの動作のシミュレーションが最も計算量の多いステップであるため、pRRTの実行時間は、RRTの実行時間に拡張子ごとのパーティクルの数を乗じたものにほぼ等しくなります。このパラメータの選択は、パーティクルのクラスタリングのためのリンケージ戦略の選択と併せて検討された。図9のエラープロットは、これらのテストの結果をまとめたものである。驚くべきことに、単一連結は粒子数が増加するにつれて結果が悪くなった。これは、クラスターのサイズに自然な限界がないためだと考えられます。この問題を説明するために、線に沿ってほぼ等間隔に配置された点の集合を考えてみましょう。完全なリンケージ戦略では、点はほぼ等しいサイズのクラスタに細分化されます。このようにして配置された粒子の確率は、クラスタ間で任意に分割されます。図9(b)は、完全なリンケージで最高の性能を発揮するには、非常に少ない粒子数で達成できることを示しているので、心強いです。

実行監視

これまでの議論では、計画パスのオープンループ実行のロバスト性を評価してきた。しかし、もしローバーが実行中に自分の位置を確実に推定する方法を持っているならば、ローバーの位置と計画パスの次のノードの位置に基づいて実行されるアクションを調整することで、計画パスの追跡性を向上させることができるかもしれない。ここでも、この更新を実行するための2つの戦略を議論します。 1つ目は、purepursuit[12]と呼ばれるよく知られたアルゴリズムである。この戦略では、拡張のためのアクションを推定するためにRRTで使用されているのと同じ単純な運動方程式を使用して新しいアクションを計算します。これらの方程式は平坦な地形を仮定しており，力学や雨後の相互作用の影響は無視している．この戦略では、スリップのために、車両がある節点から別の節点へ進む際に使用する操縦角が、必ずしも節点間の角度と等しくないという事実を利用しています。その結果を図10に示す．いずれの場合も，計算された新しい動作は，計画された動作を完全に置き換えるものではなかった．その代わりに，2 つのアクションの線形組み合わせをゲインパラメータで使用して，新しいアクションに割り当てられた割合を決定した．両方の戦略について、最高のパフォーマンスは30％近くのゲインで達成されました（これはプロットに示されているゲインです）。より単純なピュアスート法はプロポーショナルアプローチよりも低い誤差を生み出しましたが、pRRTは実行監視なしでも、実行監視付きのオリジナルRRTよりも良い結果を出すことができたことに注意してください。

IV. 結論と今後の展望

パーティクルRRTアルゴリズムは、環境の不確実性に対して、より正確で正確なロバストパスを生成するための有効な手法です。パーティクルRRTを用いた経路計画は、環境の不確実性の変化にもかかわらず、より綿密に経路を辿ることができるため、本質的に安全性が高い。RRTはシングルカーパスプランナーと考えられていますが、ロボットが追加情報を入手できるようになると、既存の木の一部を再利用して再計画を行うことができるようになると考えています。計画されたパスが実行されると、環境の不確実性は減少しますが、計画木の一部は残ります。過去のシミュレーションをキャッシュしておけば、追加の問い合わせに対して新たなRRTを構築したり、同じ問い合わせが再度発生した場合にリファインしたりすることができます。地形の探索によりFのPDFが変更され、計画木のノードの確率が変化した場合に、リアクティブでリアルタイムな再計画[13]を行うための方法を調査する。 CLARAtyプログラミングフレームワーク[14]を用いたiRobot ATRV-Jrroboticプラットフォームへの実装が計画されている。このテストベッドは、ステレオビジョンを使用して構築された地形モデルの真の不確実性に直面して、pRRTの有効性を実証する。このアルゴリズムの進歩は、現在使用されている火星探査ローバーと比較して、特に荒れた地形での将来のローバーの効率を向上させるだろう。最も興味深い科学のターゲットのいくつかは、クレーターや急な丘陵地などのdifficultterrainのエリアに位置する岩の露頭です。現在、theroversは、少なくとも1日のadelayを必要とするこれらの領域で遠隔操作する必要があります。人間のチームがステレオビジョンデータを分析し、ローバーにアップロードするための運転コマンドのprecisesetを構築しながら、ローバーはアイドルに座っています。ローバーのささやかなハードウェアの計算需要が増加しても、pRRTアルゴリズムは、ローバーが次の運転コマンドを待つのに費やす時間を短縮するのに役立つはずです。また、このアルゴリズムは、ローバーにアップロードされる計画を検証するための地上ベースのツールとしても有用である。

BEIKE blog

備忘録です

詳解確率ロボティクス勉強会(木曜日)

2021/03/04

grid_mapの調査

何の調査？

状況説明

調査結果

考え中

grid_mapを使用しない方法

またまたProtocol Buffersでこけた話

1.原因を探る

2.問題がわかった

3.パスの削除

4.出来た

5.直したはずなんだけどな

submoduleの使い方

Gazeboの終了時間の調整をする(メモ)

経路生成に関する論文やら（メモ）

rosでpython3を使う方法（デフォは2.7）