このブログは、株式会社フィックスターズのエンジニアが、あらゆるテーマについて自由に書いているブログです。
teb_local_planner は ROS Navigation Stack や Nav2 で利用可能な local planner (controller) の1つです。 DWA (Dynamic Window Approach) に比べて複雑な回避軌道を生成できますが、計算コストが比較的高いという特徴があります。 計算コストを抑えつつ意図した挙動を実現するためには、アルゴリズムを理解して、パラメータ調整を行う必要があります。 そこで本記事では、teb_local_planner のアルゴリズムの概要を理解し、パラメータ調整ができるようになることを目的として解説を行います。
ナビゲーションモジュールにゴールが与えられると、
のような流れを繰り返してゴールまでの移動が実現されます。
teb_local_planner は軌道計画と速度・角速度制御を扱います。
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_local_planner は以下の制約・目的関数を満たすように最適な軌道を計算します。
隣接するPoseの間の関係によって定義される制約・目的関数は、疎行列(成分のほとんどが零である行列)で表現することができるので、疎行列最適化ライブラリを使用して、効率良く計算できます。(※ teb_local_planner の実装には他にも制約が導入されているがここでは省略)
上記の制約・目的関数を Hyper-Graph(エッジが複数のノードをつなぐグラフ)で表現して、 g2o で最適化します。
(ref. Efficient trajectory optimization using a sparse model. のFig. 2 (b))
上の図で、s は Pose、p はウェイポイント、o は障害物、ΔT が Pose 間の移動にかかる時間のノードを表しています。これらのノードを制約・目的関数のエッジで接続することによって最適化の対象をグラフで表現します。
Timed Elastic Band を最適化した後、サンプリング間隔が不適切になる場合があります。例としては以下の通りです。
サンプリングが粗いと軌跡の表現能力が落ち、サンプリングが過剰に細かいと計算コストがかかります。auto resizing は、Pose および Pose 間の移動にかかる時間を挿入したり、削除したりすることによって、この問題を解消しています。以下にサンプリングが粗くなる場合の処理の流れを図示します。
最適化前はほぼ等間隔にサンプリングされた状態です。
最適化後は、サンプリングの間隔が粗くなることがあります。
auto resizingによって、Pose および Pose 間の移動にかかる時間を挿入して、サンプリング間隔を調整します。
ROS Navigation Stack や Nav2 から速度・角速度の計算要求があるたびに、デフォルトでは、
という流れを4回反復します。
(ref. Efficient trajectory optimization using a sparse model. のFig. 3)
TEB では軌道を連続的に変形するので、障害物をまたぐような変形が行われません。 そこで、複数の軌道を扱えるようにするために、HomotopyClassPlanner が使われています。
複数の軌道を扱う際、同じ Homotopy class に属する軌道は1つあれば充分なので、 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)
計算コストに影響するパラメータや注意が必要なパラメータについて解説します。 (特に記載がない限り、ROS2版のversion 0.9.1 (コミットIDとしては630a22e) に基づいて記載します。)
点、円、線、2つの円、ポリゴン、を指定できます。とくにポリゴンを使用する場合は計算コストが大きくなります。
true にすると、速度制約や加速度制約の計算に直線距離ではなく円弧の長さを使うようになります。
コストマップをポリゴンなどに変換するためのプラグインを指定します。デフォルトではコストマップをそのまま使いますが、ポリゴンに変換することによって計算コストを下げられる可能性があります。
コストマップを costmap_converter でポリゴンに変換する周期を指定しています。
時間解像度を指定できます。auto resizing の際に Pose 間の移動にかかる時間がこの時間に近づくように調整されます。
Pose 間の移動にかかる時間が dt_ref +/- dt_hysteresis の範囲外になると、 auto resizing による Pose 挿入・削除が行われます。
auto resizing による Pose 挿入・削除はこの個数の範囲内で行われます。
TEB を初期化する際にスタートからゴールまで初期 Pose 列を生成する処理を行いますが、このパラメータが true だと、ゴールがロボットの後ろにある場合にロボットが後退することを許容します。
最適解が障害物と衝突する軌道になっている場合があるため、衝突判定を行いますが、その際に何個目の Pose まで判定するかを指定します。負の値にすると全ての Pose を判定します。
feasibility_check_no_poses だと、ローカルコストマップの範囲内に収まらない可能性があるなどの理由で、 noetic/galactic 以降で導入されました。非負の値を指定した場合、feasibility_check_no_poses よりも優先して扱われ、指定した距離まで衝突判定を行います。
グローバルプランを指定した間隔で分割した地点を経由点として扱います。負の値にするとこの方法では経由点を設定しません。
true の場合、全ての候補軌道で経由点を考慮します。false の場合、グローバルプランと同じ Homotopy class に属する軌道のみで経由点を考慮します。
経由点を通るようにするための制約の重みを指定します。
true の場合、軌道ごとに1つのスレッドを割り当てて TEB の最適化処理を行います。
true の場合、Probablistic Roadmap のようなサンプリングではなく、障害物の左右のみでサンプリングを行います。
サンプリング数を指定します。(simple_exploration が false の場合のみ)
サンプリング範囲を指定します。
扱う軌道の上限を指定します。
グローバルプランをこの長さまで切り出して、初期の軌跡として扱います。ただし、ローカルコストマップのサイズの 85% の範囲外に軌跡が出る場合は、そこまでを初期の軌跡として扱います。終端がローカルゴールになります。
ローカルゴールの終端の orientation を軌跡に沿った角度で上書きします。グローバルプランナーが orientaion をゴールのみに設定している場合には true にする必要があります。
auto resizing をかける回数を指定します。
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” を参考にしました。
keisuke.kimura in Livox Mid-360をROS1/ROS2で動かしてみた
Sorry for the delay in replying. I have done SLAM (FAST_LIO) with Livox MID360, but for various reasons I have not be...
Miya in ウエハースケールエンジン向けSimulated Annealingを複数タイルによる並列化で実装しました
作成されたプロファイラがとても良さそうです :) ぜひ詳細を書いていただきたいです!...
Deivaprakash in Livox Mid-360をROS1/ROS2で動かしてみた
Hey guys myself deiva from India currently i am working in this Livox MID360 and eager to knwo whether you have done the...
岩崎システム設計 岩崎 満 in Alveo U50で10G Ethernetを試してみる
仕事の都合で、検索を行い、御社サイトにたどりつきました。 内容は大変参考になりま...
Prabuddhi Wariyapperuma in Livox Mid-360をROS1/ROS2で動かしてみた
This issue was sorted....