[论文解读] 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）。通过利用李导数测量捆绑与基于区间的滤波方法，该方法在大规模路标点下实现了近乎恒定时间的后验协方差预测，显著提升了在线规划中的估计精度与安全性，且无需已知路标点的线性化点。

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中路标点不确定性的计算不可行性。
• 开发一种可扩展的后验协方差预测方法，其计算复杂度几乎与路标点数量无关。
• 将轨迹规划从有限路标点集合推广至潜在路标的连续分布。
• 通过在优化目标中直接最小化后验协方差，确保自主导航过程中状态不确定性的有界性。

提出的方法

• 提出一种基于区间的滤波近似方法，递归传播自身状态协方差，同时避免显式追踪路标点的不确定性。
• 采用李导数测量捆绑方案，简化并加速高采样率传感器（如相机）的递归更新。
• 识别出一类测量模型（包括正交投影），其路标点对信息增益的贡献可直接合并，从而实现与路标点数量近乎恒定的复杂度增长。
• 利用仿射传感器模型近似进一步降低计算成本，同时保持估计精度。
• 对信息矩阵更新应用李-泰勒近似，实现快速且精确的后验协方差预测。
• 将近似后的后验协方差集成到信念空间轨迹优化框架中，以引导运动规划朝向可观测性良好的轨迹进行。

实验结果

研究问题

• RQ1 在基于路标点的SLAM中，后验协方差能否被高效预测以支持在线轨迹优化？
• RQ2 如何将协方差预测的计算复杂度从二次方级降低至与路标点数量近乎恒定的复杂度？
• RQ3 可观测性感知规划能否推广至路标点的分布而非离散有限集合？
• RQ4 与可见性或局部可观测性格拉姆矩阵等启发式可观测性度量相比，最小化后验协方差是否能带来更好的估计性能？
• RQ5 所提方法能否在真实世界自主导航场景中确保状态不确定性的有界性？

主要发现

• 所提出的李-泰勒近似将100个路标点、正交相机条件下的协方差传播时间从显式方法的1.86秒降低至151毫秒，实现12倍加速。
• 对于仿射传感器模型，李-泰勒方法在所有路标点数量下均保持近似恒定的计算时间（约18ms），展现出良好的可扩展性。
• 采用后验协方差目标（pc-lie）优化的轨迹，其RMSE显著低于使用启发式度量（如max-viz或max-gramian）的轨迹。
• pc-lie近似与精确后验协方差方法（pc-exact）的性能极为接近，估计性能提升损失极小。
• 未经优化时，不确定性可能超出安全阈值；然而，基于pc-lie的优化能有效在多种场景下限制不确定性增长。
• 该方法通过高效预测未知环境中估计器性能，实现了VIO增强型无人旋翼机的在线实时运动规划。