[論文レビュー] Towards Online Observability-Aware Trajectory Optimization for Landmark-based Estimators

Kristoffer M. Frey, Ted J. Steiner+1 more|arXiv (Cornell University)|Aug 10, 2019

Robotics and Sensor-Based Localization参考文献 24被引用数 6

ひとこと要約

本論文は、視覚SLAMなどのランドマークベース推定器向けに、計算効率が良く、観測可能性に配慮したトラジェクトリ最適化フレームワークを提案する。Lie微分測定束縛と区間ベースフィルタリングを活用することで、多数のランドマークに対してほぼ定常時間の事後共分散予測が可能となり、オンライン計画における推定精度と安全性が著しく向上する。ランドマーク線形化点が事前に分かっている必要がない。

ABSTRACT

As autonomous systems increasingly rely on onboard sensing for localization and perception, the parallel tasks of motion planning and state estimation become more strongly coupled. This coupling is well-captured by augmenting the planning objective with a posterior-covariance penalty -- however, prediction of the estimator covariance is challenging when the observation model depends on unknown landmarks, as is the case in Simultaneous Localization and Mapping (SLAM). This paper addresses these challenges in the case of landmark- and SLAM-based estimators, enabling efficient prediction (and ultimately minimization) of this performance metric. First, we provide an interval-based filtering approximation of the SLAM inference process which allows for recursive propagation of the ego-covariance while avoiding the quadratic complexity of explicitly tracking landmark uncertainty. Secondly, we introduce a Lie-derivative measurement bundling scheme that simplifies the recursive "bundled" update, representing significant computational savings for high-rate sensors such as cameras. Finally, we identify a large class of measurement models (which includes orthographic camera projection) for which the contributions from each landmark can be directly combined, making evaluation of the information gained at each timestep (nearly) independent of the number of landmarks. This also enables the generalization from finite sets of landmarks $\{\ell^{(n)} \}$ to distributions, foregoing the need for fully-specified linearization points at planning time and allowing for new landmarks to be anticipated. Taken together, these contributions allow SLAM performance to be accurately and efficiently predicted, paving the way for online, observability-aware trajectory optimization in unknown space.

研究の動機と目的

• ランドマーク位置が初期状態で未知の環境において、リアルタイムで観測可能性に配慮したトラジェクトリ最適化を可能にすること。
• オンライン計画中にSLAMにおけるランドマーク不確実性を明示的に追跡する計算の非現実的さを克服すること。
• ランドマーク数にほぼ依存しないスケーラブルな方法で、事後共分散を予測すること。
• 有限なランドマーク集合を超えて、連続的な潜在ランドマーク分布へのトラジェクトリ計画を一般化すること。
• 最適化目的関数に事後共分散を直接最小化することで、自律走行における状態不確実性を有界に保つこと。

提案手法

• ランドマーク不確実性の明示的追跡を避けることで、自己状態共分散を再帰的に伝搬する区間ベースフィルタリング近似を導入する。
• 高レートセンサー（例：カメラ）の再帰的更新を簡素化・高速化するために、Lie微分測定束縛方式を採用する。
• ランドマークが情報量に与える寄与を直接組み合わせられる測定モデルのクラス（例：垂直投影）を特定し、ランドマーク数に応じてほぼ定常的な複雑度スケーリングを実現する。
• さらなる計算コスト低減を図るために、アフィンセンサーモデル近似を活用するが、推定精度を維持する。
• 情報行列更新にLieテイラー近似を適用することで、高速かつ高精度な事後共分散予測を可能にする。
• 近似された事後共分散を信念空間トラジェクトリ最適化フレームワークに統合し、観測可能性に配慮した良好なトラジェクトリを指向する運動計画を支援する。

実験結果

リサーチクエスチョン

• RQ1 ランドマークベースSLAMにおける事後共分散は、オンライントラジェクトリ最適化に十分なほど効率的に予測可能か？
• RQ2 ランドマーク数に応じて、共分散予測の計算複雑度を二次的からほぼ定常的スケーリングに低減できるか？
• RQ3 観測可能性に配慮した計画は、有限な離散的ランドマーク集合を超えて、連続的分布への拡張が可能か？
• RQ4 事後共分散を最小化することは、可視性や局所観測可能性グラミアンといったヒューリスティックな観測可能性指標よりも優れた推定性能をもたらすか？
• RQ5 提案手法は、実世界の自律走行シナリオにおいて、状態不確実性を有界に保てるか？

主な発見

• 提案されたLieテイラー近似により、100個のランドマークと垂直カメラを用いた場合、共分散伝搬時間が明示的計算の1.86秒から151ミリ秒に短縮され、12倍の高速化が達成された。
• アフィンセンサーモデルでは、Lieテイラー法がすべてのランドマーク数においてほぼ定常的な計算時間（約18ms）を維持し、スケーラビリティを示した。
• 事後共分散目的関数（pc-lie）を用いて最適化された軌道は、最大可視性（max-viz）や最大グラミアン（max-gramian）といったヒューリスティック指標を用いたものよりも顕著に低いRMSEを達成した。
• pc-lie近似は、正確な事後共分散法（pc-exact）の性能に非常に近い結果を示し、推定改善の損失は最小限に抑えられた。
• 精製なしでは不確実性が安全な閾値を超えて増大する可能性があるが、pc-lieに基づく最適化により、多様なシナリオにおいて不確実性の増大が効果的に抑制された。
• 本手法により、未知環境における推定器性能を効率的に予測できるため、VIO搭載ドロノプターにおけるオンライン・リアルタイム運動計画が可能になった。