nav_fn¶
目次
2. アルゴリズム¶
navfnは、グローバルコストマップおよびグローバルコストマップ上のスタート座標、ゴール座標を入力値として、経路を出力します。経路を出力するまでの過程は、以下のようになっています。
ポテンシャル計算のために、グローバルコストマップのコストニュートラル値（何もないセルのコスト値）を底上げします。具体的には、-1～254のコスト値を、50～254に変換します。（底上げ後の254が進入不可のセル）
- 「致命的(Lethal)」コスト → COST_OBS(254)
- 「内接半径(Inscribed)」コスト → COST_OBS(254)
- 「未知(Unknown)」コスト → COST_OBS-1(253) ※allow_unknown パラメーターがfalseの場合はCOST_OBS(254)
- その他のコスト → (コスト値 * 0.8) + 50
コスト値の底上げを行ったコストマップから、ポテンシャルマップを生成します。
ポテンシャルマップ上で、default_tolerance パラメーターに従ったゴール座標の補正を行います。ゴール座標が進入不可のセル内にある場合は、default_tolerance パラメーターの範囲内で、進入可能なセル内の最も近い座標に補正されます。
ゴール座標が到達可能である場合は、ポテンシャルマップ上のスタート座標からゴール座標まで、経路を生成します。
2.1. ポテンシャルマップの生成¶
ポテンシャルマップとは、ゴール位置のセルが最も低く、スタート位置のセルが最も高い（進入不可のセルを除く）地形図のようなものを表現したグリッド地図で、スタート位置にボールを置いて、地形の傾斜に沿ってボールが転がり、最後にゴール位置に到達するというようなイメージで、ポテンシャルマップ上に経路が生成されます。
下の図で、赤いセルがポテンシャル値が大きい（高度が高い）セル、青に近づくほどポテンシャル値が小さく（高度が低く）なり、黒いセルが、ポテンシャル値ゼロ（ゴール地点）のセルになります。
出典：http://wiki.ros.org/global_planner
ポテンシャルマップの生成は、以下のように行われます。
グローバルコストマップと同じサイズのグリッド地図を生成し、最初に、全てのセルをPOT_HIGH（ポテンシャル値未設定）で埋めます。
ゴール位置のセルにポテンシャル値0を設定し、その上下左右の4つのセルを、ポテンシャル値の計算の対象セルに加えます。
対象セルの位置のコスト値がCOST_OBS(254)である場合は、ポテンシャル値の計算を行わずPOT_HIGHのままとし、次の対象セルの計算に進みます。
対象セルのポテンシャル値（P(x)とします）の計算は、対象セルに対して、上下のセルの小さいほうのポテンシャル値（P(v)とします）、左右のセルの小さいほうのポテンシャル値（P(h)とします）および自セルのコスト値(cとします）を使って計算します。
P(v)がPOT_HIGHである場合は、P(h)にcを足し合わせて、P(x)とします。P(h)がPOT_HIGHである場合は、その逆です。
P(v)、P(h)ともに有効なポテンシャル値である場合は、下記の数式を満足するようなP(x)を求めます。ただし、P(v)とP(h)の差がc以上の場合は、下記を満足する解がないため、P(v)とP(h)の小さいほうにcを足し合わせてP(x)とします。
対象セルのポテンシャル値を計算したら、その上下左右のセルのうち、まだPOT_HIGHとなっているセルを、次の計算対象に加えます。この時、追加元のセルのポテンシャル値がより低いものを、次に優先的に計算します。
このように、ゴール位置のセルからポテンシャル値計算を広げていき、スタート位置のセルに到達したら、ポテンシャルマップ生成完了とします。
なお、上述の2次方程式は、実際には下記のような近似計算を行っています。:
1 float d = fabs(P(v) - P(h)) / c;
2 float v = -0.2301*d*d + 0.5307*d + 0.7040;
3 P(x) = min(P(v), P(h)) + (c * v);
このような2次方程式を使わずに、単純に、P(v)とP(h)の小さい方をcに足していけばよいようにも見えますが、単純な足し算だと、あるセルに対して、直線で2つ向こう側にあるセルと斜めにあるセルとで、ポテンシャル値が同等になってしまいます。（実際には、斜めにあるセルの方が近いにも関わらず。）
斜めにあるセルのポテンシャル値を、より適切な値にするために、上述の2次方程式が使用されています。
2.2. ポテンシャルマップ上での経路生成¶
ポテンシャルマップ上で、スタート位置にボールを置いて、地形の傾斜に沿ってボールが転がり、最後にゴール位置に到達するというようなイメージで、ポテンシャルマップ上に経路を生成します。
経路の生成は、以下のように行われます。
スタート座標を最初の現在位置として、経路に追加します。
ポテンシャルマップ上で、スタート座標が含まれるセルと、その右隣り、下、右下の4枚のセルを取り出します。
それぞれのセルについて、x軸方向の傾きとy軸方向の傾きを求めます。セルの傾きは、対象セルの左右のセルとのポテンシャル値の差からx軸方向の傾きを求め、対象セルの上下のセルとのポテンシャル値の差からy軸方向の傾きを求めます。（ポテンシャル値がより小さいほうに傾けます。）
4枚のセルの傾きから、x軸方向、y軸方向それぞれの加重平均を求めます。加重平均の重みは、現在座標がセル上のどの位置にいるかで決まります。
セル上で、現在位置がより右側にある場合は、右隣りおよび右下のセルの重みが大きくなります。また、現在位置がより下側にある場合は、下および右下のセルの重みが大きくなります
求めた傾きの加重平均の分、現在位置をポテンシャルマップ上で移動させ、この座標を経路に追加します。
移動後の現在位置に対して、同じ傾き計算および座標移動、経路への追加を繰り返します。現在座標がゴール位置のセルに入るかまたは近接したら、最後にゴール座標を経路に追加します。
