自動運転: 経路計画の概要

グローバルパスプランニング
ローカルパスの計画

このブログでは、いくつかの一般的なグローバルパスプランニングアルゴリズムとローカルパスプランニングアルゴリズムから始めて、それらの動作原理、利点と欠点を紹介します。

グローバルパスプランニング

グローバルマップ、開始点と終了点を入力として、グローバル軌跡を出力として使用して、衝突のない、最短でより滑らかなパスが計画されます。

ダイクストラのアルゴリズム

ダイクストラアルゴリズムは、1956 年にオランダのコンピューター科学者エドガー W. ダイクストラによって提案された、グラフ内の最短経路探索のための古典的なアルゴリズムです。ネットワークルーティング、地図ナビゲーション、交通計画などの分野で広く使用されています。ダイクストラのアルゴリズムは、1 つの開始ノードから他のすべてのノードまでの最短パスを見つけることによって問題を解決します。

原理

初期化: 開始ノードを選択し、開始ノードの距離を 0 に設定し、他のすべてのノードの距離を無限大に設定します。
最も近いノードを選択: 未処理のノードの中から開始ノードに最も近いノードを選択します。最初は、このノードが開始ノードになります。
距離の更新: 選択したノードについて、開始ノードからそのノードを経由して隣接するノードまでの距離を計算します。選択したノードを介した隣接ノードまでの距離が、隣接ノードの現在の距離より小さい場合、隣接ノードの距離を新しく計算された距離に更新します。
処理済みとしてマーク: 選択したノードを処理済みとしてマークし、開始ノードからそのノードへの最短パスが見つかったことを示します。
ステップ 2 ～ 4 を繰り返します。すべてのノードが処理されるか、ターゲットノードが処理されるまで、最も近いノードを選択し、距離を更新し、処理済みとしてマークするプロセスを繰り返します。
パスのバックトラック: ターゲットノードが処理されると、ターゲットノードから開始ノードまでバックトラックすることによって最短パスを構築できます。

アドバンテージ

最適な解を保証する: ダイクストラのアルゴリズムは、すべてのエッジの重みが負でないことを条件として、見つかったパスが開始ノードからターゲットノードまでの最短パスであることを保証します。
単一ソースの最短パス問題に適しています: ダイクストラのアルゴリズムは、単一ソースの最短パス問題、つまり、1 つの開始ノードから他のすべてのノードまでの最短パスを解決するためによく使用されます。
理解と実装が簡単: アルゴリズムのロジックは比較的シンプルで、理解と実装が簡単です。

欠点がある

負の重みエッジを処理できない: ダイクストラのアルゴリズムは、その動作原理がノード間の距離を継続的に減少させることに基づいており、負の重みエッジが無限ループにつながる可能性があるため、負の重みエッジを持つグラフを処理できません。
大規模なグラフには適さない: 大規模なグラフでは、ダイクストラのアルゴリズムの計算の複雑さが高く、リアルタイムアプリケーションには適さない可能性があります。

A* アルゴリズム

アルゴリズム (A スターアルゴリズム) は、グラフまたはグラフ型の問題において、開始点から目的点までの最短経路を見つけるために使用されるヒューリスティック検索アルゴリズムです。このアルゴリズムは、ダイクストラアルゴリズムの最短経路特性とヒューリスティック探索の利点を組み合わせたもので、経路計画、ゲーム AI、ロボットナビゲーション、地図ルート計画などの分野で広く使用されています。

原理

初期化: 開始ノードを選択し、開始ノードのコストを 0 に設定し、探索するノードのリストに入れます。同時に、ターゲットノードへの推定コストがノードごとに記録され、通常はヒューリスティック関数を使用して推定されます。
ノードの選択: 調査するノードのリストからノードを選択します。通常は、総コスト (実際のコストと推定コストの合計) が最小のノードです。最初は、開始ノードが唯一の候補です。
ノードの展開: 選択したノードについて、その隣接ノードを移動します。開始ノードから現在のノードを経由して隣接ノードまでの実際のコストを計算し、隣接ノードからターゲットノードまでの推定コストを加算します。この合計コストが隣接ノードの現在の合計コストより小さい場合、隣接ノードの合計コストを更新し、現在のノードを隣接ノードの親ノードとして設定します。
ノードをマークする: 選択したノードを探索済みとしてマークし、探索するノードのリストから削除します。
ステップ 2 ～ 4 を繰り返します。ターゲットノードが見つかるか、探索するノードのリストが空になるまで、ノードの選択、展開、およびマーク付けのプロセスを繰り返します。
パスのバックトラック: ターゲットノードが見つかったら、ターゲットノードから開始ノードまでバックトラックすることによって最短パスを構築できます。

アドバンテージ

(条件付き) 最適解の保証: A* アルゴリズムは、ヒューリスティック関数が特定の条件 (「一貫性」または「単調性」と呼ばれる) を満たしている限り、見つかったパスが開始ノードからターゲットノードまでの最短パスであることを保証します。
効率: A* アルゴリズムは、ヒューリスティック関数を通じて検索をガイドし、不必要なノードの拡張を削減できるため、通常、純粋なダイクストラアルゴリズムよりも高速です。
幅広い適用性: A* アルゴリズムは、経路計画、ゲーム AI、ロボットナビゲーション、地図ルート計画などを含むさまざまな問題に適しています。

欠点がある

ヒューリスティック関数の選択: A* アルゴリズムのパフォーマンスは、選択したヒューリスティック関数に大きく依存します。不適切なヒューリスティック関数を選択すると、アルゴリズムが不正確または非効率になる可能性があります。
ストレージ要件: A* アルゴリズムは、探索するノードのリストを保存および管理する必要があり、大規模な問題の場合はより多くのメモリが必要になる場合があります。

RRT (ランダム高速探索ツリー) アルゴリズム

RRT (Randomized Rapidly-Exploring Tree) は、1998 年に Steven M. LaValle によって最初に提案された、経路計画のためのランダムサンプリングアルゴリズムです。RRT は、ロボットやその他の自律エージェントの動作計画の問題を解決し、未知の環境や複雑な環境で効果的な経路を見つけられるようにすることを目的としています。

原理

初期化: 開始点から開始して、開始点のみを含むツリーを作成します。
ランダムサンプリング: マップ空間内に点をランダムに生成します。この点は通常はランダムですが、環境に関する事前の知識も考慮されることがあります。
ツリーに接続: サンプリングポイントに最も近いツリー上のノードを見つけて、このノードからサンプリングポイントまでのエッジを生成します。このエッジの生成は通常、ツリーが急速に拡張しないようにするために、最大距離制限などのいくつかのルールに従います。
繰り返し: スパニングツリーの一部がターゲットエリアに近づくまで、手順 2 と 3 を繰り返します。各ステップで、RRT は新しいノードと新しいエッジをツリーに追加して、検索空間を徐々に拡大します。
ターゲットに接続: ツリーの一部がターゲットポイントに近づくと、開始点からターゲットポイントに接続するパスがツリー内で見つかります。
パスの最適化 (オプション): 生成されたパスを最適化して、パスの滑らかさと効率を向上させることができます。

アドバンテージ

高次元環境への適応: RRT は高次元状態空間に適しているため、ロボットや自由度の高い問題に非常に役立ちます。
多様なパス: ランダムな性質により、RRT は多様なパスを生成でき、これはローカルミニマム問題の克服に役立ちます。
リアルタイム計画: RRT は増分的な性質を備えているため、リアルタイムの経路計画に適しており、ロボットの移動中に経路を再計画できます。

欠点がある

最適なパスを確保できない: RRT は、グローバルな最適なパスを常に見つけるとは限りません。RRT は、検索プロセスの迅速な探索と多様性に重点を置いています。
収束速度が不確実: RRT の収束速度はランダムサンプリングとツリーの成長規則に依存するため、場合によっては適切なパスを見つけるまでに時間がかかることがあります。
限定環境モデル: RRT は、ロボットが動的制約を考慮せずに自由に移動できることを前提としているため、場合によっては適用できない場合があります。

PRM (確率論的ロードマップ) アルゴリズム

PRM (Probabilistic Roadmap) は、パス計画のための確率的アルゴリズムであり、特に高次元で複雑な環境に適しています。PRM アルゴリズムは、マップ上にサンプルポイントのセットをランダムに生成し、それらのポイントを接続してグラフを構築することによってパスを生成します。PRM アルゴリズムは、ロボットナビゲーション、動作計画、自律ロボットの分野で広く使用されています。PRM アルゴリズムの基本的な動作原理は次のとおりです。

原理

サンプリング: マップ上に一連のサンプリングポイントをランダムに生成します。これらのポイントは通常、ロボットが合法的に到達できる場所です。サンプリングポイントの数と分布は、問題の複雑さとニーズに応じて調整できます。
サンプリングポイントを接続する: 各サンプリングポイントについて、近くに他のサンプリングポイントがあるかどうかを確認し、存在する場合はそれらを接続してエッジを形成します。2 つのサンプリングポイントを接続するエッジは、通常、ロボットが 2 つのポイント間を移動できるパスを表します。
通過可能性のチェック: 接続されたエッジについては、パスが障害物と交差していないことを確認するために通過可能性チェックが実行されます。エッジがアクセシビリティ要件を満たしていない場合は、削除します。
パス検索: ダイクストラや A* などの標準パス検索アルゴリズムを使用して、生成されたグラフ上で最短パスを見つけます。通常、開始点と目標点は最も近いサンプル点に接続されるため、パスがグラフ内に存在することが保証されます。
パスの最適化 (オプション): 生成されたパスを最適化して、パスの滑らかさと効率を向上させることができます。

アドバンテージ

高次元環境への適応: PRM アルゴリズムは高次元状態空間に適しており、ロボットや自由度の高い問題に非常に役立ちます。
複数の動作制約のサポート: PRM アルゴリズムは、最小回転半径、最大速度、最大加速度などの複数の動作制約を簡単に統合して、さまざまなロボットの動作要件を満たすことができます。
グローバルパスプランニング: PRM アルゴリズムをグローバルパスプランニングに使用して、開始点とターゲットポイントを接続するパスが確実に見つかるようにすることができます。
複数のパスの事前計画: 一連のサンプリングポイントを生成することにより、PRM アルゴリズムは複数のパスを事前計画して緊急事態に備えることができます。

欠点がある

ロードマップの構築には時間がかかる: PRM のロードマップの構築には、特に高次元環境では、かなりの計算時間が必要となる場合があります。
パス検索の複雑さ: パス検索フェーズの計算の複雑さは、グラフのサイズと形状に依存し、場合によってはより高くなる場合があります。
必ずしも最適ではない: PRM アルゴリズムは常に全体的な最適なパスを見つけるとは限らず、そのパフォーマンスはサンプリングポイントの分布と数に依存します。

ローカルパスの計画

ローカルマップと周囲の環境情報を入力として、ローカル軌道を出力として取得し、運動学的または動的制約を満たす衝突のない軌道が計画されます。これは、衝突を回避し、快適性やその他の要件を満たし、軌道を調整するためによく使用されます。周囲の環境に応じてリアルタイムに制御します。

DWA (ダイナミックウィンドウメソッド) アルゴリズム

DWA (Dynamic Window Approach) アルゴリズムは、移動ロボットの経路計画と障害物回避のためのリアルタイム制御手法です。これは、動的環境で衝突を回避し、リアルタイムで経路を計画するためにロボットによって一般的に使用されます。DWA アルゴリズムは、ロボットの状態空間で一連の可能な動作軌道を生成し、各軌道のパフォーマンスを評価することで最適な動作を選択するモデル予測制御手法です。

原理

状態空間モデリング: ロボットの状態空間を姿勢 (位置と方向) と速度 (線速度と角速度) の組み合わせとして表します。この状態空間は、環境内でのロボットの位置と動作を記述します。
運動軌道の生成: 通常は速度空間 (線速度と角速度) でのサンプリングによって、状態空間で一連の可能な運動軌道を生成します。これらの軌道は、ロボットの可能な動作オプションを表します。
軌道を評価する: 生成された軌道ごとに、そのパフォーマンスを評価します。通常、パフォーマンス評価には、軌道が目標位置に到達できるかどうか (目標指向性)、および軌道が障害物を回避できるかどうか (障害物回避性) の 2 つの側面が含まれます。これらの評価指標は、多くの場合、予測モデルと環境意識に基づいています。
最適な動作を選択: ロボットの次の動作として、最高のパフォーマンスを発揮する軌道を選択します。通常、最適な軌道は、目標指向性と障害物回避のバランスがとれた軌道として定義されます。
ロボットの制御: 選択したアクションを実行し、ロボットの移動を制御します。ロボットは現在の軌道上でモーションコマンドを実行し、次の動作に向けてパスを再計画します。
リアルタイム更新: 上記の手順を繰り返して、リアルタイムの経路計画と障害物回避を実現し、ロボットの動きを継続的に更新します。

アドバンテージ

リアルタイム: DWA アルゴリズムはリアルタイムの動作計画に適しており、ロボットが移動するときの環境変化に適応する軌道を迅速に生成できます。
適応性: DWA アルゴリズムのリアルタイム性により、動的環境における障害物の動きや変化に適応できます。
障害物回避機能: DWA アルゴリズムは、障害物回避パフォーマンスの評価を回避することで衝突を回避し、ロボットが複雑な環境をナビゲートできるようにします。

欠点がある

ローカル検索: DWA アルゴリズムは通常、ローカル検索方式であるため、常にグローバルな最適パスが見つかるとは限りません。
パフォーマンスはパラメータに依存します: DWA アルゴリズムのパフォーマンスは、パラメータ、特にパフォーマンス評価に関連するパラメータの選択に大きく依存します。これにはデバッグと最適化が必要です。

TEB (タイムエラスティックバンド) アルゴリズム

TEB (Time-Elastic Band) アルゴリズムは、ロボットの動作計画と軌道の最適化に使用される手法で、動的環境における移動ロボットの経路計画に特に適しています。TEB アルゴリズムは、複雑で動的な環境におけるロボットの経路計画問題を解決し、ロボットが安全かつ効率的に移動できるように設計されています。

原理

初期化:
位置や速度などのロボットの初期状態を決定します。
時間分解能（タイムステップ）や速度分解能（スピードステップ）などのパラメータを設定します。
初期状態を含む時空間軌跡を構築します。
目標設定：
ロボットの目標位置を決定します。
パス生成:
時空間軌跡において、可能な位置と速度のペア (姿勢と速度) が時間ごとに生成されます。
ロボットの動的モデルを考慮して、生成された軌道が物理的に適切であることを確認します。
速度分解能を考慮して、速度の異なる複数の軌道分岐を生成します。
障害物回避の検出:
生成された各軌道について、軌道上の各タイムステップで障害物と交差するかどうかを確認します。
タイムステップが障害物と交差する場合、その軌跡を不正なものとしてマークします。
パフォーマンス評価:
法的軌道については、そのパフォーマンスを評価します。
パフォーマンス評価には、通常、目標指向性 (軌道が目標位置に近いかどうか)、障害物回避性 (軌道が障害物を回避するかどうか)、および滑らかさが含まれます。
軌道選択：
性能評価に基づいて、ロボットの次の動作として最適な軌道または軌道分岐を選択します。
通常、最適な軌道は、目標への方向性と障害物回避のバランスがとれた軌道です。
ロボットの制御:
選択した軌道でモーションコマンドを実行して、ロボットを次の状態に移動します。
リアルタイム更新:
ロボットの移動中、軌道は常に更新され、環境の動的な変化に適応するために再計画されます。
目標に到達:
ロボットが目標位置に近づき、終了条件を満たした場合、アルゴリズムは終了します。

アドバンテージ

動的適応性: TEB アルゴリズムには、軌道を動的に調整する機能があり、リアルタイムで認識された環境情報に基づいて経路を再計画できます。これにより、動的な障害物や環境の変化に対処するのに適しています。
軌道の滑らかさ: TEB アルゴリズムは滑らかな軌道を生成する傾向があり、ロボットの動きの快適さと安定性の向上に役立ちます。
速度制御: TEB アルゴリズムにより、環境内の障害物の密度とロボットの移動能力に適応してロボットの速度を動的に調整できます。
複数の動作制約のサポート: TEB アルゴリズムは、最小回転半径、最大速度、最大加速度などの複数の動作制約を簡単に統合して、さまざまなロボットの動作要件を満たすことができます。

欠点がある

計算の複雑さ: TEB アルゴリズムの計算の複雑さは、特に高次元の状態空間では比較的高く、リアルタイムの課題につながる可能性があります。
パラメータに敏感: TEB アルゴリズムは、時間分解能、空間分解能などの多くのパラメータに依存します。適切なパラメーターを選択することはアルゴリズムのパフォーマンスにとって重要ですが、多少の調整が必要になる場合があります。

ラティスプランナーアルゴリズム

Lattice Planner は、複雑な動作制約を持つ高次元状態空間やロボットに適したサンプリングベースの経路計画アルゴリズムです。ロボットの状態空間を高次元グリッド図としてモデル化し、サンプリングを通じて候補軌道を生成し、さまざまな指標評価を使用して最適なパスを見つけます。

原理

状態空間モデリング: ロボットの状態空間を高次元のグリッド図として表現します。
候補軌道の生成: 状態空間内に一連の候補軌道を生成します。各軌道はロボットの可能な動作パスを表します。
衝突検出: 各候補軌道に対して衝突検出を実行し、軌道が障害物と交差しないことを確認します。
性能評価: 目標の向き、障害物回避、滑らかさ、快適さなどの指標を考慮して、法定軌道に関する性能評価を実行します。
最適な軌道を選択する: 開始状態から目標状態までの最適な経路を決定します。
軌道の最適化: 必要に応じて、生成されたパスを最適化します。

アドバンテージ

高次元の状態空間と複雑な運動制約に適応します。
複数のモーションコンストレイントをサポートします。

欠点がある

計算の複雑さは高いです。
パラメータの選択はパフォーマンスに大きな影響を与えます。

EMプランナーアルゴリズム

EM Planner は、未知の環境におけるロボットの経路計画問題を解決するために使用される、Expectation-Maximization (EM) 法を使用した経路計画アルゴリズムです。未知の環境の確率分布を推定することで経路を計画します。

環境モデリング: センサーデータを使用して、障害物の位置や不確実性を含む環境の確率的マップを構築します。
EM アルゴリズム: EM アルゴリズムを使用して、障害物の分布と不確実性を含む、未知の環境の確率分布を推定します。
経路計画: 経路計画アルゴリズムを使用して、推定された環境確率分布に基づいて経路を計画します。
軌道の実行: 新しいセンサーデータに適応するために環境モデルとパスを継続的に更新しながら、計画されたパスを実行します。

アドバンテージ

未知の環境や不確実な環境に適しており、環境の確率分布を推定することができます。

欠点がある

計算の複雑さが高くなります。