base_local_planner¶

3.1 パッケージの目的¶

base_local_plannerパッケージは、モバイルベースを平面上で運転するコントローラーを提供します。 このコントローラーは、パスプランナーをロボットに接続します。 プランナーはマップを使い、ロボットがスタートからゴール位置に到達するまでの運動の軌道を作成します。 その過程で、プランナーはロボットの周囲にグリッドマップとして表される価値関数を作成します。 この価値関数は、グリッドセルを通過するコストを表現します。 コントローラーの仕事は、この価値関数を使用して、ロボットに送信する速度 (縦方向速度, 横方向速度, 回転速度) を決定することです。 (移動方向の定義については 座標系 参照。)

出典: http://wiki.ros.org/base_local_planner

3.2 ローカルプランニングの処理概要¶

Trajectory Rollout と Dynamic Window Approach (DWA) アルゴリズムの基本的な考え方は次のとおりです。

1. ロボットの速度空間 (縦方向速度, 横方向速度, 回転速度) を離散的にサンプリングします。
2. サンプリング速度ごとに、ロボットの現在の状態から フォワードシミュレーション を実行して、サンプリング速度を一定（短い）時間適用した場合にどう動くかを予測します。(軌道の予測)
3. フォワードシミュレーションから得られた各軌道を評価 (スコア) します。評価には、障害物への近さ、ゴール地点への近さ、グローバルパスへの近さ、速度などの特性をとりこんだ尺度を使用します。 不正な軌道（障害物と衝突する軌道）は破棄します。
4. 最高得点の軌道を見つけだし、その速度をロボットに送信します。
5. 以上の手順を繰り返します。

DWAと Trajectory Rollout とでは、ロボットの速度空間のサンプリング方法が異なります。 Trajectory Rollout では、フォワードシミュレーションの全期間でロボットの加速度制限から到達可能な速度セットをサンプリングするのに対し、 DWAでは、コントローラー呼び出し周期の期間のみでロボットの加速度制限から到達可能な速度セットをサンプリングします。 つまり、DWAはより小さな速度空間をサンプリングするため、より効率的なアルゴリズムですが、加速度制限が低いロボットでは Trajectory Rollout の方が性能がよくなるかもしれません。なぜならDWAは一定加速度をフォワードシミュレートしないためです。 ただし、我々の実際のすべてのテストでは、DWAと Trajectory Rollout は同等の性能を発揮しており、性能効率からみてDWAの使用を推奨します。

便利なリファレンス：

• Brian P. Gerkey and Kurt Konolige. "Planning and Control in Unstructured Terrain". LAGRロボットで使用される Trajectory Rollout アルゴリズムの説明。
• D. Fox, W. Burgard, and S. Thrun. "The dynamic window approach to collision avoidance". ローカルコントロールへの Dynamic Window Approach。
• Alonzo Kelly. "An Intelligent Predictive Controller for Autonomous Vehicles". 過去の同様のアプローチで制御するシステム。

3.3　速度空間のサンプリング¶

ロボットの速度空間 (縦方向速度, 横方向速度, 回転速度) のサンプリングを次の各アルゴリズムで行います。

3.4 軌道の計算¶

サンプリング速度ごとに、ロボットの現在の状態からフォワードシミュレーションを実行して、サンプリング速度を一定（短い）時間適用した場合にどう動くかを予測します。

フォワードシミュレーションでは、次のサイクルを繰り返します。

1. 現在ステップ(時刻)でのロボットの位置・向きを軌道に追加します

2. ロボットの次ステップの速度を計算します。

   サンプリング速度を目標として、それに近づくように現在速度を毎ステップ加速・減速します。(ただしDWAの場合はすぐに目標速度に達します。)

3. ロボットの次ステップの位置・向きを、次ステップの速度から計算します。

4. 現在ステップを1つ進めます。

フォワードシミュレーション時間は、sim_time パラメータで設定されます。

フォワードシミュレーションの1ステップの時間は、速度サンプリング で出てくるコントローラー呼び出し周期とは少し異なり、「フォワードシミュレーション時間 / ステップ数」です。 ステップ数は、

「 縦横方向サンプリング速度の合成値 * フォワードシミュレーション時間 / 距離ステップサイズ(sim_granularity) 」

または

「サンプリング回転速度の絶対値 / 角度ステップサイズ(angular_sim_granularity) 」 (注：フォワードシミュレーション時間は掛けません)

のどちらか大きい方で決まりますが、 ロボットの経路への向きに基づくスコアリングを行う場合(heading_scoring が trueの場合)は

「フォワードシミュレーション時間/ 距離ステップサイズ(sim_granularity) 」 (注：式はソースのママ)

となります。

3.5　軌道のコスト¶

ロボットの辿る軌道をスコアリングするため、次のようなコストを使います。

3.5.1　障害物コスト¶

ローカルコストマップは、2D平面上の障害物のコスト分布を表したマップです。これを用いて、次のように軌道をスコアリングします。

ローカルコストマップにロボットを当てはめて、ロボットの footprint を形成する線が含まれているセルの中で、コストが最大のものを採用し、これを footprint cost と呼びます。 footprint形成線 が障害物セル(値254のセル)にかかる軌道は footprint costを-1.0とします。

occ_cost は、footprint cost とロボットの中心点が含まれるセルのコストで、大きい方を採用します。

図の例では、occ_cost は128となります。

3.5.2　マップグリッドコスト¶

軌道を効率的にスコアリングするために、マップグリッドを使用します。 マップグリッドは、ロボットの周りのグリッドの各セルに path_dist と goal_distの2種類の評価値(コスト)を付与したものです。

マップグリッドは制御サイクルごとに次の手順で構築します。

1. ロボットの周りにグリッド（ローカルコストマップと同サイズ）を作成します。
2. グローバルパスをグリッドの領域にマッピングします。
3. path_distの指標については、グローバルパスの通るセルを経路点までの距離0でマークし、また goal_dist の指標についてはローカルゴールのセルをゴール地点までの距離0でマークします。
4. 伝播アルゴリズムによって、他のすべてのセルを、ゼロでマークされた最も近い点までのマンハッタン距離で効率的にマークします。

このマップグリッドを使って、軌道をスコアリングします。

グローバルパスのゴール地点は、多くの場合、マップグリッドでカバーされる小さな領域の外側にあります。ゴール地点への近さで軌道をスコアリングする場合、考えるのは「ローカルゴール」となります。それは、領域外へ続いている経路上の領域外縁の点です。領域のサイズはmove_baseによって決定されます。

path_dist 経路上のセルをコスト0として、経路からどれだけ離れているかをコストとして表したもの	goal_dist ローカルゴールをコスト0として、ゴール地点からどれだけ離れているかをコストとして表したもの

3.6　軌道の評価¶

コスト分布を表すマップ(path_distマップ , goal_distマップ , ローカルコストマップ ) より、軌道のコストを算出し、どの軌道がよいか評価します。

出典: http://wiki.ros.org/base_local_planner の画像を加工

軌道のコストには下表のものがあります。

3つ（または4つ）のコストを、所定の重み付け（カスタマイズ可能） を掛け合わせて合算し、与えられた軌道のコストとします。 各軌道ごとにコストを算出し、最も低コストの軌道を結果の軌道とします。

軌道上のいずれかの点で、occ_costが負値になる(衝突する)場合や、impossible_costのセルに入る場合は、その軌道を破棄します。

3.7　その場回転の軌道の追加評価¶

その場での回転は、縦方向速度を0固定とし、回転速度のバリエーションで評価します。 その際、その場回転ではロボットの位置は変わらないため、左右どちらのどのような速度の回転でも、path_distおよびgoal_distには差異が生じません。 occ_costについては、ロボットが障害物に近い位置にいる場合は、footprintの変化により差が出る可能性がありますが、開けた場所であれば差が生じません。

そのため、その場回転同士の比較には、追加の評価軸が用いられます。 回転後に、ロボットがその方向に少し前進（heading_lookahead パラメータ ）したと仮定して、前進後の位置のgoal_distがより小さい方を採用します。下図の例では、右回転より左回転の方が有利となります。

ただし、ロボットが既にその場回転を始めていた場合は、現在の回転方向が優先されます。（ロボットが首を左右に振り続けてスタックするような挙動を回避するため。振動抑制 参照。）

3.8　振動抑制¶

振動は、(縦, 横, 回転) のいずれかの次元で正と負の値が連続して選択されたときに発生します。 振動を防ぐため、ロボットがある方向に移動すると、次のサイクルでは反対方向への移動を不正と設定し、それをフラグが設定された位置からの移動距離が特定の値 (oscillation_reset_dist) を超えるまで続けます。

3.9　ゴール地点到達時の処理¶

ローカルプランナーがロボットを停止すべき場所に正確に停止させるのが理想です。 しかし実際には、センサーのノイズとアクチュエータの不確実性によりロボットが目標地点を行き過ぎて、延々と切り返しを続けてしまうことがあります。 それを防ぐため、ロボットがゴール地点に十分近づくと走行を停止し、指示されたゴール方向へその場回転のみ行うようにします。 次に処理手順を示します。

1. コントローラーの各サイクルで、ロボットがグローバルゴールの近傍に到達したか判定します。
   • ロボットからゴール地点までの距離が、xy_goal_tolerance 以下の場合、ゴール地点に到達済みと判定します。
   • ロボットがゴール地点に到達していれば以下の最終補正処理に移行します。
2. ロボットを停止させます。
   • ロボットを許される加速度の範囲内で減速し、停止させます。
   • ロボットが停止したかの判定には、オドメトリの縦方向速度、横方向速度、回転速度がしきい値(それぞれ 0.01m/s, 0.01m/s, 0.01rad/s)以下であるかを調べます。
3. ロボットが停止した後、ゴール方向にその場回転を行います。
   • ロボットの向きがゴール方向と合っているかを判定します。 ロボットの現在の向きと指示されたゴール時の方向とのなす角が、yaw_goal_tolerance 以下か判定します。
   • ロボットの向きが合っていない場合、ゴール方向へその場回転を行います。
   • ロボットの向きが合っていれば、ゴール地点に最終到達したとして、速度0を指令します。

最終補正処理の途中でゴール地点の許容範囲外に出た場合は、最終補正処理をやめて通常の DWA/Trajectory Rollout に戻ります。 ただし、ゴール許容誤差ラッチフラグ が trueの場合は、ゴール地点から外れても DWA/Trajectory Rollout に戻らず最終補正処理を続行します。

4.1 Subscribe トピック¶

4.2　Publish トピック¶

4.3　パラメーター¶

base_local_planner::TrajectoryPlannerROS ラッパーの動作をカスタマイズするために設定可能なROS パラメーター が多数あります。 これらのパラメーターは、ロボット構成、目標許容誤差、フォワードシミュレーション、軌道スコアリング、振動防止、グローバルプランなど、いくつかのカテゴリに分類されます。

cost =
pdist_scale * (軌道終端からパスへの距離。単位は マップセル か m のどちらか。meter_scoring パラメーターによる)
+ gdist_scale * (軌道終端からローカルゴールへの距離。単位は マップセル か m のどちらか。meter_scoring パラメーターによる)
+ occdist_scale * (軌道中の最大障害物コスト。単位は障害物コスト (0-254))
+ 0.3 * (ロボットの経路への向きのスコア)

4.4　下位クラス¶

5.1　メソッドコールシーケンスの概要¶

5.2　各メソッドの処理概要¶

• TrajectoryPlannerROS::computeVelocityCommands() … 速度指令計算
  • グローバルパスをローカルコストマップの範囲で切り取ります。
  • ゴール地点に到達済みかを判定し、到達済みなら最終補正の速度指令を返します。
    • ロボットが未停止であれば減速/停止の速度指令を返却します。
    • ロボットが停止済みであれば角度を合わせるための回転速度指令を返却します。
    • 角度も合っていればゼロ速度指令を返却します。
  • ゴール地点に到達していなければ、TrajectoryPlanner::findBestPath()をコールします。

• TrajectoryPlanner::findBestPath() … 最良経路検索
  • 経路評価用マップを更新します。
  • TrajectoryPlanner::createTrajectories()をコールします。

• TrajectoryPlanner::createTrajectories() … 軌道作成
  • 縦方向速度、回転速度のとり得る組み合わせを求め、それぞれの組み合わせに対してTrajectoryPlanner::generateTrajectory()をコールしてコストを計算し、コストが最小となるものを求めます。
    • ①現在速度、②加速／減速の最大値、③速度の最大／最小値、④速度変化の単位から、とりうる全ての組み合わせを評価します。
    • ホロノミックロボットの場合、向きを保ったまま左斜め前方 or 右斜め前方への移動を評価します。速度は 縦方向0.1, 横方向±0.1(m/s)の固定値です。
    • 縦方向速度を0固定にして、回転速度のバリエーション（その場での回転）を評価します。
    • 上記3点で有効な組み合わせがない＆ホロノミックロボットの場合、横移動を評価します。横方向速度のバリエーションは、y_vels パラメータのリストです。縦方向速度と回転速度のサンプリング値は0とします。
    • それでも有効な組み合わせがなかった場合は、脱出(少しの後退)を評価します。

• TrajectoryPlanner::generateTrajectory() … 軌道生成
  • 与えられた、ターゲットとなる縦方向速度、回転速度について、path_dist, goal_dist, occ_costの3つの評価軸（オプションでheading_diffを追加可能）で評価を行い、コストを返却します。

6.1　TrajectorySampleGenerator クラス¶

このインターフェースは、軌道のジェネレーターを表します。有限個または無限個の軌道を生成し、 nextTrajectory() の呼び出しごとに新しい軌道を返します。

6.2　TrajectoryCostFunction クラス¶

このインターフェースに含まれる最も重要なメソッドは scoreTrajectory(Trajectory &traj) です。これは、軌道を取得してスコアを返します。 負のスコアは、軌道が無効であることを意味します。 正の値の場合、意味としては、コスト関数に従うとスコアの低い軌道がスコアの高い軌道よりも好ましいということです。

各コスト関数は、他のコスト関数との相対的な影響度を変えられるスケールを持ちます。

base_local_planner パッケージには PR2 で使用されるいくつかのコスト関数が付属しています。(後述)

6.3　SimpleScoredSamplingPlanner クラス¶

このクラスは軌道探索の簡単な実装です。 TrajectorySampleGeneratorクラス と、 TrajectoryCostFunctionクラス のリストとをメンバで持ちます。 軌道ジェネレーターが軌道の生成を停止するまで nextTrajectory() を呼び出します。 軌道ごとに、コスト関数のリストをループして正の値を足し上げて評価値としますが、負の値を返すコスト関数がある軌道は評価しません。

コスト関数を足し上げる際は、コスト関数のスケールの重みをつけます。そして最良の評価値をもつ軌道を結果の軌道とします。

本クラスは、dwa_local_planner のページで説明しているアルゴリズムのうち、軌道の評価 を実装しています。 本クラスは、軌道探索のインターフェースクラス base_local_planner::TrajectorySearch を継承して実装したものです。

6.4　ヘルパークラス¶

6.5　コスト関数クラス¶

7.1　dwa_local_planner パッケージとの比較¶

base_local_planner と dwa_local_planner パッケージは両方とも DWAを使えますが、次のような違いがあります。( 参考 )

• dwa_local_plannerは (縦, 横, 回転) 方向の速度制約をサポートしますが、base_local_plannerは (縦, 回転) 方向の速度制約のみをサポートします。 横方向速度についてユーザーが指令できるのは、事前に指定する有効なy速度リストのみです。 そのため、ホロノミックまたは疑似ホロノミックなロボットには、 dwa_local_plannerの方が速度空間をよりよくサンプリングできるため適しています。
• dwa_local_planner の方が、ソースコードが整理されています。
• 他にも細かな違いがあります。個別のロボットでは base_local_plannerの方が性能を発揮することもあるかもしれませんが、作者の一人はコードが整理されていることなどからまずdwa_local_plannerを使うことを推奨しています。

7.2　座標系¶

このパッケージでは、ロボットに固定した下図のような座標系を使用します。

移動方向については次のように定義します。

• 縦移動

  縦方向(車両前後方向)への移動。前進/後退

• 横移動, または ストライフ(strafe)

  ロボットが向きを保ったまま横方向(左or右方向)に移動すること。特殊な車輪駆動などを使って行います。

• 斜め移動

  ロボットが向きを保ったまま右斜め前方 or 左斜め前方へ移動すること。縦移動と横移動を同時に行うと斜め移動になります。

• 回転移動

  前進/後退しながら進行方向を変えること

• その場回転 (超信地旋回)

  ロボットが前進せずにその場で旋回すること。 とくに左右の車輪を逆方向に回転させる旋回を超信地旋回といいます。

7.3　用語¶

• モバイルベース mobile base

  ロボットを移動可能にする駆動装置の集合

• ホロノミック/非ホロノミック

  ホロノミック/非ホロノミックは、系の拘束条件の性質を述べた言葉です。 このパッケージに限って言うと、前進と回転のみ可能なロボットは非ホロノミック、前進と回転に加えて横方向にも移動できるロボットはホロノミックです。(正確な定義ではありません。)

• フットプリント, footprint

  ロボットの接触範囲. 障害物を避ける軌道の算出に使用します. 実際のロボットの形状よりも大きめにとります。

• フォワードシミュレーション

  ここでは、時間が順方向に経過したときにロボットがどのように移動するか、運動学的に計算すること

• ドメイン

  ロボットの活動する環境。周囲の地形など

• PR2

  Willow Garage の開発した全方位移動双腕パーソナルロボット。 垂直方向にチルトするレーザースキャナーを搭載しています。