global_planner¶

1. 概要¶

このパッケージは、ナビゲーション用の高速で補間されたグローバルプランナーの実装を提供します。nav_core :: BaseGlobalPlannerインタフェースに付随するクラスは、 nav_core パッケージに準拠しています。 これは、 NF1 に基づいた navfn パッケージの、より柔軟な代替パッケージとして構築されました。

• 管理状態: 管理済み
• 管理者: David V. Lu!! <davidvlu AT gmail DOT com>, Michael Ferguson <mfergs7 AT gmail DOT com>, Aaron Hoy <ahoy AT fetchrobotics DOT com>
• 著者: David Lu!!
• ライセンス: BSD
• ソース: git https://github.com/ros-planning/navigation.git (branch: melodic-devel)

2. アルゴリズム¶

global_plannerは navfn を元に構成されているため、基本的な処理の流れは同じです。

global_plannerは、グローバルコストマップおよびグローバルコストマップ上の開始位置、目標位置を入力値として、経路を出力します。経路を出力するまでの過程は、以下のようになっています。

1. ポテンシャル計算を行うセルの探索を行います。探索方式は、 use_dijkstraパラメータ で、 ダイクストラ か A* のどちらかを選択できます。

2. 探索されたセルに対して、ポテンシャル計算を行います。ポテンシャル計算法は、 use_quadraticパラメータ で、 2次方程式 か 単純加算 のどちらかを選択できます。

3. ポテンシャル計算時に、 ニュートラルコスト値 で底上げし、ポテンシャルマップを作成します。ニュートラルコスト値の扱いは、ダイクストラとA*で異なります。それぞれの計算方法は下記の通りです。

   1. ダイクストラ

      下記の条件のいずれかの時に、コスト値の底上げを行う。

      • コスト値が 致命的コスト値 - 1 より小さい
      • allow_unknownパラメータ がtrue、且つ、コスト値が255

      底上げされたコスト値 = コスト値 × cost_factorパラメータ + ニュートラルコスト値

      底上げされたコスト値が、 致命的コスト値 以上となった場合は、 致命的コスト値 - 1 を底上げされたコスト値とします。

   2. A*

      底上げされたコスト値 = コスト値 + ニュートラルコスト値

4. 目標位置が到達可能である場合は、ポテンシャルマップ上の開始位置から目標位置まで、経路を生成します。経路生成は、use_grid_pathパラメータで、 グリッド経路 か 傾斜経路 を選択できます。

5. ポテンシャルマップから生成された経路の最後に、入力値をもとに目標位置を補正します。

6. 後処理として orientation_modeパラメータ で、 オリエンテーション を追加することができます。

3. パラメータの違いによる動作例¶

パラメータの設定値の違いによる、ポテンシャル計算及び経路の違いを以下に示します。 後出の図は、ポテンシャルマップであり、緑点が目標位置、赤点が開始位置で、それを結ぶ緑線は探索結果の経路です。 青色セルから赤色セルへのグラデーションは、経路探索のためにポテンシャル計算を行ったセルで、青色から赤色になるほどポテンシャル値が高くなります。灰色セルは、障害物又は経路探索の対象とならなかったセルです。経路は、ポテンシャル値の高いところ(開始位置)から低いところ(目標位置)に流れるようなイメージで、ポテンシャルマップ上に生成されます。

3.1. 標準動作¶

すべてのパラメータがデフォルトです。

use_grid_path=False

use_quadratic=True

use_dijkstra=True

old_navfn_behavior=False

出典：http://wiki.ros.org/global_planner

3.2. グリッド経路¶

use_grid_path=True

グリッド経路は、現在位置の周囲のセル単位でポテンシャル値を評価し、その中でポテンシャル値が最小のものを経路として選択する動作を、目標位置が出現するまで繰り返します。セル単位での評価であるため、計算量は少なくなりますが、経路の軌跡の滑らかさに欠ける場合があります。

出典：http://wiki.ros.org/global_planner

経路はグリッド境界（セルの中心）をたどります。

3.3. 単純加算¶

use_quadratic=False

単純加算は、上下左右のセルのうちポテンシャル値が低いものを選択し、コスト値を加算します。2次方程式より計算量は少なくなりますが、斜め方向の評価が行われません。

出典：http://wiki.ros.org/global_planner

navfnの元のポテンシャル計算は2次方程式であることに注意してください。 2次方程式の計算については、 navfn を参照してください。

3.4. A*経路¶

use_dijkstra=False

出典：http://wiki.ros.org/global_planner

ポテンシャル計算されたセルがはるかに少ないことに注目してください（色付きの領域で示されています）。これは、ダイクストラを使用するよりも確かに高速ですが、必ずしも同じ経路を生成するわけではありません。もう1つ注意すべき点は、A*の実装は、経路探索時は上下左右に隣接する4つのグリッドに対して計算し展開していきますが、目標位置発見時の開始位置までのトレース時には、上下左右と斜めを含む隣接する8つのグリッドを評価し、経路を生成します。 したがって、A*で探索された経路は、必ずしも最短距離とならない場合があります。 （A*の一般的な実装は、経路探索時にポテンシャル計算を行わなかったグリッドは不要なグリッドと判断し、目標位置発見時のトレースの対象になりません。） ダイクストラの動作とA*の動作の違いを確認するには、後述の例を参考にしてください。

3.4.1. ダイクストラ¶

目標位置のセルから上下左右のセルに対してポテンシャル計算を行い対象となるセルを広げていき、各セルで同様にポテンシャル計算を順次行います。開始位置のセルが出現したら、探索を終了します。

出典：http://wiki.ros.org/global_planner

3.4.2. A*¶

開始位置のセルから上下左右のセルに対してヒューリスティック関数による推定値を加味したポテンシャル計算を行い探索対象となるセルを展開し、その中でコスト値の小さいものからポテンシャル計算を行います。目標位置が出現したら、探索を終了します。グローバルプランナーに実装されているヒューリスティック関数は、下記のとおりです。

　　ヒューリスティック関数 h(n) = 現在位置X座標と目標位置X座標の距離 + 現在位置Y座標と目標位置Y座標の距離

出典：http://wiki.ros.org/global_planner

3.5. 古いNavfnの動作¶

old_navfn_behavior=True　 NavFn と同じように経路を再現します。

old_navfn_behavior=Trueが設定された際の動作の差異は以下の通りです。

• 経路の開始点の計算を正確に行いません。
• 座標計算時にオフセットを使用しません。
• 目標位置周辺の未計算セルのポテンシャル計算を行いません。
• ポテンシャルマップからの経路生成の最後に、入力値の目標位置情報を追加します。

出典：http://wiki.ros.org/global_planner

注意：

• 経路の開始点が実際の開始位置と一致しません。
• 経路の最後がグリッド線に沿って移動します。
• すべての座標がグリッドセルの半分だけわずかにずれます。

4. オリエンテーションフィルター¶

後処理ステップとして、方向を経路上の通過点に追加できます。 ~orientation_modeパラメータ（動的再構成）を使用すると、次の方向モードを設定できます。

• None=0 （目標位置の向き以外の向きは追加されません）
• Forward=1 （目標位置の向きを除き、通過点での向きは経路に沿って正のx軸方向）
• Interpolate=2 （方向は開始姿勢と目標姿勢の線形混合です）
• ForwardThenInterpolate=3 （最後の直進まで順方向、その後目標姿勢まで線形混合）
• Backward=4 （目標位置の向きを除き、通過点での向きは経路に沿って負のx軸方向）
• Leftward=5 （目標位置の向きを除き、通過点での向きは経路に沿って正のy軸方向）
• Rightward=6 （目標位置の向きを除き、通過点での向きは経路に沿って負のy軸方向）

経路に沿った、進行方向を前向きは正のx軸方向、進行方向に向かって左向きは正のy軸方向とし、それぞれ後ろ向きは負のx軸方向、右向きは負のy軸方向とします。

経路のi番目の通過点の方向は、`i - orientation_window_size`番目の通過点の座標と `i + orientation_window_size` 番目の通過点の座標を使用して計算されます。 ウィンドウのサイズは方向の計算で、なめらかになるように変更できます。

Global planner ROS - Orientation filter

5. ROS API¶