🏄‍♂️

[Reijgwart+,2024] Waverider: 階層マップを使ったでリアクティブな動作生成

に公開
robot
地図
リアクティブ
tech

なぜこの論文を読むか

  • ワークスペースのSDF情報を使うと，障害物回避が自然に実現できる
  • しかし，その勾配情報を取り出す元となるマップをどう作ったら良いかベストプラクティスが分からない
  • ドローンのような，広範囲のマップを必要とする例題で，階層マップを用いてリアクティブな同作を実現している論文があったので読んでみる

一言でいうと

階層ボクセル地図×RMP(Riemanila motion policies) で，遠くは粗く・近くは精密に障害物を扱うことで，高速・低遅延・安全な回避を実現する仕組みを提案


  • OctoMap
    • [Hornung et al., 2013] ライダースキャンから階層的なマップが作れる
  • Potential 法

論文リンク

学会/投稿年

  • ICRA2024

著者/所属機関

  • 著者：Victor Reijgwart*, Michael Pantic*, Roland Siegwart, Lionel Ott（*は共同筆頭）
  • 所属機関：Autonomous Systems Lab, ETH Zürich（スイス）

Victor Reijgwartさん

    • ご近影出典
    • ご研究(GPT5で生成)
      • 階層マルチ解像度マップ wavemap とその応用（Waverider を含む）に取り組むロボット地図構築・経路計画系の研究者
      • 代表論文はボリューム地図のグローバル整合化 Voxgraph（RA‑L）など
    • 現在

Michael Panticさん

    • ご近影出典
    • ご研究(GPT5で生成)
      • ETH Zurich・ASL のシニアリサーチャーとして空中ロボット分野をリード
      • MBZIRC 2017 の ETH チーム論文や，T‑RO の空中接触検査（Fully Actuated Aerial Vehicle）に共著で関与
    • 現在
      • ASL で空中ロボット研究を率いるシニアリサーチャー出典をやられているご様子

Roland Siegwartさん

    • ご近影出典
    • ご研究(GPT5で生成)
      • ETH Zurich の自律システム教授・ASL 主宰
      • 教科書 Introduction to Autonomous Mobile Robots の著者の一人としても著名
    • 現在
      • ETH Zurich 正教授（2006–）・Wyss Zurich 共同ディレクター・ASL 主宰 出典

Lionel Ottさん

    • ご近影出典
    • ご研究(GPT5で生成)
      • 機械学習×地図表現（Hilbert Maps 系）で著名
      • Hilbert Maps（IJRR 2016，Ramos & Ott）や Fast Bayesian Hilbert Maps に関するICRA 2019論文に関連
    • 現在
      • ASL（ETH Zurich）のシニアリサーチャーをやられている

技術や手法のキモは？

お題「大きなマップからでも情報を集約して，リアクティブに動けるようにしたい」

  • Input
    • 階層占有地図（wavemapなど）
    • ロボット状態（位置・速度）
    • 任意の高レベル目標（例：ゴール位置）
  • Output：
    • 各時刻における合成加速度（RMP合成の結果）
    • ロボットはこれをそのまま実行
      • ドローンなので，三次元空間での加速度指令値を出力

提案手法

  1. 階層マップ（wavemap）から，ロボットからの距離に応じて適切な解像度の“障害物セル”だけを抽出（遠方＝粗く，近傍＝細かく）．抽出は粗→細の再帰探索で実装（後ほど紹介）
  2. 各セルに対し，RMPの回避ポリシー（反発＋速度依存ダンパ＋距離ゲーティング）を構築
    • 回避すべきセルの数(5000個とか）のポリシーが作られる
  3. すべての回避ポリシーをRMP和で合成し，さらにゴール吸引ポリシーと合成して加速度を出力(これも後ほど紹介)
    • 1~3まで，平均36 ms くらい

先行研究と比べてなにがすごい？

  • 固定解像度（最高分解能のみ）での回避や，Signed Distance Field (SDF)前処理＋最適化（CHOMP等）に比べ，ほぼ前処理ゼロで超高速，かつ広視野
  • RMPにより，加速度場として滑らかで指向性を持つ回避が得られ，追加目的（視覚サーボ，操作，補助操縦など）との素直な合成が可能

従来研究へのツッコミ

  • CHOMP：SDF生成が高コスト（本論文の設定で≈30 s），収束を待つ必要がありリアルタイム性に乏しい．非収束時に安全でない点が混ざり得る
    • CHOMPの雑な説明
  • 固定解像度RMP：視野を広げると見るべきボクセル数が爆発し，実時間性が犠牲に

本手法

  • 距離に応じた解像度選択で，視野の広さと近傍の精細度を両立

どうやって有効だと検証した？

  • 統計評価：Newer College LiDARデータから作った2地図（math/mine）で各500試行のランダム出発‐到達テスト
    • 成功率は4手法とも≃75%
    • 本手法は高速かつ障害物からの距離分布が安全側
  • 計算効率：Jetson Orin AGX（CPUのみ）＋Livox Mid-360でマッピング29 ms＋ポリシー実行6.9 ms／スキャン，RAM 355 MB，平均2.4スレッド
    • ある移動の全計算時間
  • 実機（MAV）：屋内障害物コースで目標追従＋回避．最近接距離平均1.16 ± 0.32 m，最小0.75 m

議論はある？

  • ローカルミニマ
    • リアクティブ手法のため局所解に陥ることはあり得る（ただし安全側で停止）
    • 軽量な大局的ヒューリスティックやゴール再設定と組み合わせる余地あり
  • 動的障害物
    • 本論文は主に静的環境評価
    • RMP自体は速度項を持つので拡張余地は大
  • パラメータ
    • スケール依存の半径・スケーリングの与え方（後述）に設計自由度
    • 環境ごとに保守的/攻めのチューニングが必要（実機実験はかなり保守的にしている）

次に読むべき論文は？

  • RMP本体：Ratliff et al., Riemannian Motion Policies（理論の土台）[6]
  • CHOMP：Zucker et al., Covariant Hamiltonian Optimization for Motion Planning（比較対象）[8]
  • 占有/距離地図：OctoMap [2]，UFOMap [3]，supereight [4]，Voxblox（ESDF）[9]，FIESTA [10]
  • 多解像度計画：Orientation-aware hierarchical A* [20]，AMRA* [19]，Multi-resolution mapping for planning [18]
  • RMP×高速回避：Pantic et al., raycasting + RMP at kHz（関連）[15]

感想

  • SDFを作らずに，絞った考慮すべき全グリッドから反発を考えて足しわせるアプローチは，自分では思いつかなかったので面白かった
  • ポテンシャル法に比べてRMPがどれくらい，複数の方策を足し合わせるのに向いているのかが気になるところ
  • 大規模空間でSDF計算するのは重いだろうなという感想
    • 比較用のSDFの計算方法が書いてないから手法にもよりそう

技術的詳細

要素技術１：階層ボクセル地図とマルチ解像度障害物セル抽出

  • 地図：wavemap(同著者の過去作)は各オクトリーノードにHaarウェーブレット係数を格納し，全解像度が常に同期．粗→細の高速統合で低レイテンシの更新が可能
    • ご発表スライドから転載
    • 余談： wavlet変換
      • 引用元
      • 大体フーリエ変換みたいなもん！
        • 波の重ね合わせで元の信号を表現
      • 恐らく，以下のマルチ解像度の関係をうまく取り入れたと思われるが...
        • こういう感じと思われる(私による作図)
        • (興味ある人は読んでみて教えてくださると助かります)
        • 今回紹介している論文自体は階層的な占有格子地図だという知識だけで理解はできる
  • 考慮すべき障害物マスを抽出するアルゴリズム（Algorithm 1）
    • 根から再帰し，子ノードの展開は「距離が閾値を超えるまで」
    • 閾値はスケールλの関数
      • d_{\max}(\lambda) = 3^{\lambda/3} - 0.25
      • λ：解像度の分割深さ
      • 遠方ほど粗いノードで打切り，近傍は子へ分解
    • 展開を止めたノードが自身または子に占有を含めば，1つの障害物セルとして登録
  • 計算量の利点
    • 固定解像度で1–3 mの視野を取るよりも，階層抽出は訪問ボクセル数が桁違いに少ない
  • 定性説明
    ロボット（赤）近傍では濃い緑＝高解像度，遠方は薄色＝低解像度のセルのみ処理．これで広い視野と近傍の精密回避を両立

アルゴリズムの動作を少し見てみる

  • (私の作図)

要素技術２：RMPによる衝突回避ポリシー生成と合成

  • Riemanian Motion Policiesの論文

    • ポテンシャル法より出来ること by GPT5
      RMP/RMPflow は「勾配ベースのスカラポテンシャル」ではなく，加速度ポリシー＋リーマン計量を単位として，複数タスクを安定に合成できる枠組みです
      1. 複数目標の順序に依らない合成：目標到達・回避・関節限界など，各タスクの局所ポリシーを計量付きで合成し，順序非依存（可換・結合的）な演算で一つの加速度ポリシーにできる
      2. タスク空間⇄関節空間の幾何一貫変換：自然なタスク空間（距離，方向，EE空間等）で設計して自動にプルバックして制御できる（座標不変）
      3. 安定性の保証：GDS/CLF に基づく条件下で，合成後も安定（Lyapunov的）になる解析がある
      4. 方向重み付け・速度依存が素直に書ける：計量で“どの方向をどれだけ重視するか”を表現し，ポリシーは x,\dot x\mapsto \ddot x（速度依存・減衰込み）として設計できる
      5. 計算グラフとして再利用・拡張しやすい（RMPflow）：メッセージパッシングで効率よく合成でき，実装が楽

  • Notation:

  • RMPの合成

    • 各ポリシー P_i=(f_i, A_i) をリーマン計量A_iで重み付けし，以下で擬似逆行列を用いて合成．加速度の向きと重要度が計量に反映される
P_c=(f_c, A_c) = \Big( \big(\textstyle\sum_i A_i\big)^{+}\,\sum_i A_i f_i,\ \sum_i A_i \Big) \tag{1}
  • 回避ポリシー（障害物セルごと）：反発＋速度ダンパ＋距離ゲーティング
    • 反発（d：障害物からの距離，r：障害物→ロボットの単位ベクトル）
f_{\text{rep}}(x,r,d)=\eta_{\text{rep}}\exp\!\Big(-\frac{d}{\upsilon_{\text{rep}}}\Big)\,r \tag{2}
  • 速度依存ダンパ：
    • P_{\text{obs}}(\dot{x},r) は速度を障害物方向へ射影（障害物へ向かう速度だけを減衰）
f_{\text{damp}}(\dot{x},r,d)=\eta_{\text{damp}}\Big(\frac{d}{\upsilon_{\text{damp}}}+\varepsilon\Big)\,P_{\text{obs}}(\dot{x},r) \tag{3}
  • 合成：
    • s(\cdot)：ソフト正規化，w_r(d)=\big(\tfrac{d}{r}-1\big)^2 = \tfrac{1}{r^2}d^2-\tfrac{2}{r}d+1（距離ゲート）
f_{\text{obs}}=f_{\text{rep}}-f_{\text{damp}},\qquad A_{\text{obs}} = w_r(d)\, s(f_{\text{obs}})\,s(f_{\text{obs}})^{\!\top} \tag{4–5}
  • 意味：ロボット速度が障害物方向を向かないときは計量が弱まり，不要な反発を抑制
  • スケール依存チューニング
    • セルのスケールλに応じて以下とし，粗いセルほど影響半径を大きく設定（遠方での“早めの回避”を誘発）
\upsilon_{\text{damp}}=0.45\,\lambda,\quad \upsilon_{\text{rep}}=0.75\,\lambda,\quad r=1.5\,\lambda
  • ゴール吸引RMP：
    • 回避RMPとEq.(1)で合成して単一の加速度を毎ステップ出力
f_a(x,\dot{x})=\alpha_a\,s(x_a-x)-\beta_a\dot{x},\quad A_a=I_{3\times3} \tag{6}
  • 実時間性のポイント
    • SDF不要
    • 各ステップは“地図→セル抽出→RMP合成”のみ
    • コントローラ周期で逐次1回の評価を回すだけ
    • 実測でLiDAR統合29 ms＋RMP 6.9 ms ≈ 36 ms
    • 30 mの広視野を維持しながら数百Hz級の反応が可能

手法の数値的解析

  • 4x4x4のグリッドを，全部個別に見るか，くっつけて１つと見做す(近似する)かでの影響を解析

出典：本文・図表は記述がない限り Reijgwart et al., Waverider: Leveraging Hierarchical, Multi-Resolution Maps for Efficient and Reactive Obstacle Avoidance に基づく．ブログ用に説明を書きこむ修正を加えている．図番号・式番号は論文に準拠．

Discussion