Fixstars Tech Blog /proc/cpuinfo
🗺️

teb_local_planner のアルゴリズムとパラメータについての解説

tyamada01
に公開
ROS2
ROS1
tech

概要

teb_local_planner は ROS Navigation Stack や Nav2 で利用可能な local planner (controller) の1つです。 DWA (Dynamic Window Approach) に比べて複雑な回避軌道を生成できますが、計算コストが比較的高いという特徴があります。 計算コストを抑えつつ意図した挙動を実現するためには、アルゴリズムを理解して、パラメータ調整を行う必要があります。 そこで本記事では、teb_local_planner のアルゴリズムの概要を理解し、パラメータ調整ができるようになることを目的として解説を行います。

ナビゲーションの概要と teb_local_planner の役割

ナビゲーションモジュールにゴールが与えられると、

  1. 自己位置および障害物の観測・推定
  2. 経路計画（ゴールまでの間の最適な Pose 列を計算する。単純な場合、ゴールが与えられるたびに1度だけ）
  3. 軌道計画（ゴールまでの間の最適な各時刻の Pose を計算する）
  4. 速度・角速度制御（目標速度・角速度を計算する）

のような流れを繰り返してゴールまでの移動が実現されます。

teb_local_planner は軌道計画と速度・角速度制御を扱います。

Timed Elastic Band (TEB) とは

Timed Elastic Band は軌道を「 Pose （位置と角度）の列と Pose 間の移動にかかる時間の列」で表現するモデルです。 i 番目の Pose s_i (x_i, y_i, \beta_i) から、 i+1 番目の Pose s_{i+1} (x_{i+1}, y_{i+1}, \beta_{i+1}) までの移動にかかる時間が \Delta T_i のとき、以下のようなイメージです。

(ref. Efficient trajectory optimization using a sparse model. のFig. 1 (a))

TEB の最適化

制約・目的関数

teb_local_planner は以下の制約・目的関数を満たすように最適な軌道を計算します。

  • 速度制約（2つの連続したPoseとPose間の移動にかかる時間）
  • 加速度制約（3つの連続したPoseと2つのPose間の移動にかかる時間）
  • 障害物/waypoint（障害物/waypoint の近くの(k=3~5個の)Pose）
  • 非ホロノミック制約（2つの連続したPose）
  • 最短時間
  • 最短距離

隣接するPoseの間の関係によって定義される制約・目的関数は、疎行列（成分のほとんどが零である行列）で表現することができるので、疎行列最適化ライブラリを使用して、効率良く計算できます。（※ teb_local_planner の実装には他にも制約が導入されているがここでは省略）

疎行列最適化

上記の制約・目的関数を Hyper-Graph（エッジが複数のノードをつなぐグラフ）で表現して、 g2o で最適化します。

(ref. Efficient trajectory optimization using a sparse model. のFig. 2 (b))

上の図で、s は Pose、p はウェイポイント、o は障害物、ΔT が Pose 間の移動にかかる時間のノードを表しています。これらのノードを制約・目的関数のエッジで接続することによって最適化の対象をグラフで表現します。

auto resizing

Timed Elastic Band を最適化した後、サンプリング間隔が不適切になる場合があります。例としては以下の通りです。

  • 障害物を回避するために経路が最適化され、サンプリングが粗くなる
  • ゴールが近づいて、サンプリングが過剰に細かくなる

サンプリングが粗いと軌跡の表現能力が落ち、サンプリングが過剰に細かいと計算コストがかかります。auto resizing は、Pose および Pose 間の移動にかかる時間を挿入したり、削除したりすることによって、この問題を解消しています。以下にサンプリングが粗くなる場合の処理の流れを図示します。

  • 最適化前

最適化前はほぼ等間隔にサンプリングされた状態です。

  • 最適化後

最適化後は、サンプリングの間隔が粗くなることがあります。

  • auto resizing後

auto resizingによって、Pose および Pose 間の移動にかかる時間を挿入して、サンプリング間隔を調整します。

TEB の最適化処理の流れ

ROS Navigation Stack や Nav2 から速度・角速度の計算要求があるたびに、デフォルトでは、

  • auto resizing (Insert/delete TEB states)
  • Hyper-graph の最適化（5回のLevenberg-Marquardtの反復）

という流れを4回反復します。

(ref. Efficient trajectory optimization using a sparse model. のFig. 3)

初期軌道生成

HomotopyClassPlanner

TEB では軌道を連続的に変形するので、障害物をまたぐような変形が行われません。 そこで、複数の軌道を扱えるようにするために、HomotopyClassPlanner が使われています。

Homotopy class とは

  • 定義1-1(Homotopy) 同じ始点と終点を結ぶ二つの軌道は、一方が障害物と交差することなく他方に連続的に変形できる場合にのみ、ホモトピーである
  • 定義1-2(Homotopy classes) 互いに Homotopy であるすべての軌道の集合を Homotopy class と呼ぶ

複数の軌道を扱う際、同じ Homotopy class に属する軌道は1つあれば充分なので、 Homotopy class の判定を行って、フィルタリングを行います。

H-signature による Homotopy class 判定

Homology class に属する軌跡 \tau に対して不変な量を以下のように定義します。

\begin{align} \mathcal{H}(\tau) &= \int_{\tau}\mathcal{F}(z)dz \\ \mathcal{F}(z) &= \frac{f_{0}(z)}{(z-\xi_{1})(z-\xi_{2})\dots(z-\xi_{R})} \end{align}

ここで、\xi_{k} は障害物の代表点です。

この不変量 H-signature を数値的に（離散的な軌跡に対して）計算すると、2つの軌跡の間の H-signature を比較することによって、同じ Homology class に属するか否かの判定を行うことができます。（実部と虚部でそれぞれ差の絶対値を計算して、しきい値より小さければ同じ Homotopy class に属します）

初期軌道生成から最適化までの処理の概要

(ref. Planning of Multiple Robot Trajectories in Distinctive Topologies の Fig. 2)

  1. Probabilistic Roadmap のように、頂点のサンプリングを行う
  2. スタートの頂点から、ゴールに近づき、かつ、線分が障害物と交差しないようにエッジを作成する。さらに、スタートの頂点とエッジで接続された頂点から、ゴールに近づき、かつ、線分が障害物と交差しないようにエッジを作成するという処理を繰り返す (図(a))
  3. スタートからゴールまでの各パスに対して、 H-signature を計算して、新しい Homotopy class であれば、軌跡と H-signature を保存する (図(b))
  4. 各軌跡を TEB で最適化する (図(c))

主要なパラメータについて

計算コストに影響するパラメータや注意が必要なパラメータについて解説します。 （特に記載がない限り、ROS2版のversion 0.9.1 (コミットIDとしては630a22e) に基づいて記載します。）

コストマップ関連

footprint_model/type (string, default: “point”)

点、円、線、2つの円、ポリゴン、を指定できます。とくにポリゴンを使用する場合は計算コストが大きくなります。

exact_arc_length (bool, default: false)

true にすると、速度制約や加速度制約の計算に直線距離ではなく円弧の長さを使うようになります。

costmap_converter_plugin (string, default: ““)

コストマップをポリゴンなどに変換するためのプラグインを指定します。デフォルトではコストマップをそのまま使いますが、ポリゴンに変換することによって計算コストを下げられる可能性があります。

costmap_converter_rate (double, default: 5.0)

コストマップを costmap_converter でポリゴンに変換する周期を指定しています。

TEB、auto resizing 関連

dt_ref (double, default: 0.3)

時間解像度を指定できます。auto resizing の際に Pose 間の移動にかかる時間がこの時間に近づくように調整されます。

dt_hysteresis (double, default: 0.1)

Pose 間の移動にかかる時間が dt_ref +/- dt_hysteresis の範囲外になると、 auto resizing による Pose 挿入・削除が行われます。

min_samples (int, default: 3)

max_samples (int, default: 500)

auto resizing による Pose 挿入・削除はこの個数の範囲内で行われます。

allow_init_with_backwards_motion (bool, default: true)

TEB を初期化する際にスタートからゴールまで初期 Pose 列を生成する処理を行いますが、このパラメータが true だと、ゴールがロボットの後ろにある場合にロボットが後退することを許容します。

feasibility_check_no_poses (int, default: 5)

最適解が障害物と衝突する軌道になっている場合があるため、衝突判定を行いますが、その際に何個目の Pose まで判定するかを指定します。負の値にすると全ての Pose を判定します。

feasibility_check_lookahead_distance (double, -1.0(disabled))

feasibility_check_no_poses だと、ローカルコストマップの範囲内に収まらない可能性があるなどの理由で、 noetic/galactic 以降で導入されました。非負の値を指定した場合、feasibility_check_no_poses よりも優先して扱われ、指定した距離まで衝突判定を行います。

経由点(viapoint)

global_plan_viapoint_sep (double, default: -0.1(disabled))

グローバルプランを指定した間隔で分割した地点を経由点として扱います。負の値にするとこの方法では経由点を設定しません。

viapoints_all_candidates (bool, default: true)

true の場合、全ての候補軌道で経由点を考慮します。false の場合、グローバルプランと同じ Homotopy class に属する軌道のみで経由点を考慮します。

weight_viapoint (double, default: 1.0)

経由点を通るようにするための制約の重みを指定します。

HomotopyClassPlanner 関連

enable_multithreading (bool, default: true)

true の場合、軌道ごとに1つのスレッドを割り当てて TEB の最適化処理を行います。

simple_exploration (bool, default: false)

true の場合、Probablistic Roadmap のようなサンプリングではなく、障害物の左右のみでサンプリングを行います。

roadmap_graph_no_samples (int, default: 15)

サンプリング数を指定します。（simple_exploration が false の場合のみ）

roadmap_graph_area_width (double, default: 5.0)

サンプリング範囲を指定します。

max_number_classes (int, default: 4)

扱う軌道の上限を指定します。

その他

max_global_plan_lookahead_dist (double, default: 3.0)

グローバルプランをこの長さまで切り出して、初期の軌跡として扱います。ただし、ローカルコストマップのサイズの 85% の範囲外に軌跡が出る場合は、そこまでを初期の軌跡として扱います。終端がローカルゴールになります。

global_plan_overwrite_orientation (bool, default: true)

ローカルゴールの終端の orientation を軌跡に沿った角度で上書きします。グローバルプランナーが orientaion をゴールのみに設定している場合には true にする必要があります。

no_outer_iterations (int, default: 4)

auto resizing をかける回数を指定します。

no_inner_iterations (int, default: 5)

auto resizing 1回あたりの g2o の OptimizationAlgorithmLevenberg での反復計算の回数を指定します。

制御周期について

速度・角速度の計算要求を出す頻度は、ナビゲーションモジュールの controller_frequency によって変更できます。

後退を禁止するには？

teb_local_planner の設定では禁止はできませんが、 weight_kinematics_forward_drive を大きな値にするとほとんど後退しなくなります。（参考：https://github.com/rst-tu-dortmund/teb_local_planner/issues/85）
（さらに、後処理で後退を停止に変更するような処理を入れれば確実です。）

参考資料

teb_local_planner の README.md では以下の5つが示されていますが、とくに TEB については “Efficient trajectory optimization using a sparse model.” 、 Homotopy class planning については “Planning of Multiple Robot Trajectories in Distinctive Topologies” を参考にしました。

  • C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153.
  • C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Trajectory modification considering dynamic constraints of autonomous robots. Proc. 7th German Conference on Robotics, Germany, Munich, May 2012, pp 74–79.
  • C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Efficient trajectory optimization using a sparse model. Proc. IEEE European Conference on Mobile Robots, Spain, Barcelona, Sept. 2013, pp. 138–143.
  • C. Rösmann, F. Hoffmann and T. Bertram: Planning of Multiple Robot Trajectories in Distinctive Topologies, Proc. IEEE European Conference on Mobile Robots, UK, Lincoln, Sept. 2015.
  • C. Rösmann, F. Hoffmann and T. Bertram: Kinodynamic Trajectory Optimization and Control for Car-Like Robots, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, Sept. 2017.
Fixstars Tech Blog /proc/cpuinfo
Fixstars Tech Blog /proc/cpuinfo

このブログは、高速化のエキスパートであるフィックスターズのエンジニアたちが、技術的な知見や検証結果を自由に発信しているテックブログです。「ソフトウェアの高速化・最適化」と「最新ハードウェア/AI技術の活用」に焦点を当てた、専門性の高い記事が中心です。

Discussion