[論文レビュー] FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Aerial Robots

Luxin Han, Fei Gao+2 more|arXiv (Cornell University)|Mar 6, 2019

Robotic Path Planning Algorithms被引用数 6

ひとこと要約

FIESTAは、空中ロボットにおけるリアルタイムモーションプランニングを目的とした、高速でインクリメンタルなオイラー距離関数（ESDF）マッピングシステムを提案する。別々の挿入／削除キュー、最適化されたインデックスデータ構造、最小ノードBFS更新戦略を用いることで、先行手法（Voxbloxなど）と比較して、数個のオーダーも速く、より高い精度を達成し、マルチコアプロセッサ上で四脚ロボットのオンボード計画を効率的に行える。

ABSTRACT

Euclidean Signed Distance Field (ESDF) is useful for online motion planning of aerial robots since it can easily query the distance and gradient information against obstacles. Fast incrementally built ESDF map is the bottleneck for conducting real-time motion planning. In this paper, we investigate this problem and propose a mapping system called FIESTA to build global ESDF map incrementally. By introducing two independent updating queues for inserting and deleting obstacles separately, and using Indexing Data Structures and Doubly Linked Lists for map maintenance, our algorithm updates as few as possible nodes using a BFS framework. Our ESDF map has high computational performance and produces near-optimal results. We show our method outperforms other up-to-date methods in term of performance and accuracy by both theory and experiments. We integrate FIESTA into a completed quadrotor system and validate it by both simulation and onboard experiments. We release our method as open-source software for the community.

研究の動機と目的

• 自律的空中ロボットのオンラインモーションプランニングにおけるリアルタイムESDFマップ保守のボトルneckを解消すること。
• TSDFに依存する既存手法（例：Voxblox）が抱える準ユリッド近似誤差と高い計算コストの課題を克服すること。
• 計算オーバーヘッドを最小限に抑えつつ、インクリメンタル更新に対応できる軽量で柔軟で、グローバルに一貫性のあるESDFマッピングフレームワークを構築すること。
• リソース制約のある四脚ロボットへのオンボードデプロイメントに適した、正確でリアルタイムのESDF更新を実現すること。
• マッピングシステムを包括的な自律的四脚ロボットシステムに統合し、シミュレーションおよび実世界の実験を通じて検証すること。

提案手法

• 障害物挿入用と削除用の2つの独立したキューを導入し、更新処理を分離・最適化する。
• インデックスデータ構造（例：可変ブロックサイズを有するハッシュテーブル）とダブルリンクリストを用い、ボクセル状態と更新伝搬を効率的に管理する。
• 最小限に必要なノード数のみを拡張するBFSベースの更新アルゴリズムを設計し、冗長な計算を低減しながら高い精度を維持する。
• 24連結性（面、辺、2ステップの面）を採用することで、精度とパフォーマンスのバランスを図り、低連結性の代替手法と比較して誤差を最小限に抑える。
• ESDF計算を特定のマップ型（例：TSDFやオブザーバシオングリッド）に依存させないよう設計し、さまざまなマッピングフレームワークとの互換性を実現する。
• 空間的局所性を活用し、BFS走査における早期終了を実装することで、更新処理を最適化し、再計算の冗長性を低減する。

実験結果

リサーチクエスチョン

• RQ1 リアルタイムモーションプランニングに適した高精度なESDFマップを、計算コストを最小限に抑えつつインクリメンタルに更新する方法は何か？
• RQ2 インクリメンタルESDF更新における冗長なノード拡張を最小限に抑えるためのデータ構造と更新戦略は何か？
• RQ3 連結性の選択（例：6-, 18-, 24-, 26-, 32-連結性）が、精度と計算パフォーマンスのトレードオフに与える影響は何か？
• RQ4 特定のマップ表現（例：TSDFやオブザーバシオングリッド）に依存しないが、依然として高精度を達成できるマッピングシステムを設計可能か？
• RQ5 提案手法は、Voxbloxなどの最先端システムと比較して、実世界およびシミュレーテッド環境において、速度と精度の両面でどの程度優れているか？

主な発見

• FIESTAは、Voxbloxと比較して、更新速度と精度の両方で1桁の改善を達成し、テストデータセット上でのRMS誤差が最大10倍まで低減された。
• ブロックサイズ=8のハッシュテーブルが、時間的・空間的複雑度のバランスを最良に保ち、配列ベースや他のブロックサイズ設定を上回る性能を示した。
• 24連結性が、全テスト連結性の中で精度（最小RMS誤差）と計算コストのバランスが最良であった。
• FIESTAのグローバルESDFマップは、デュアルコア3.00GHz Intel i7プロセッサ上で20ms未満で更新され、リアルタイムオンボード計画に適した20Hzの更新周波数を達成した。
• VLP-16 LiDARを搭載した四脚ロボットを用いたオンボード実験により、FIESTAが未知で複雑な環境でも信頼性の高いリアルタイムモーションプランニングを可能にすることが確認された。
• 本システムは包括的な自律的四脚ロボットパイプラインに統合され、シミュレーションおよび実飛行実験を通じて検証され、耐障害性とスケーラビリティの両面で優れた性能を示した。