[论文解读] Incremental Sparse GP Regression for Continuous-time Trajectory Estimation & Mapping

Xinyan Yan, Vadim Indelman+1 more|arXiv (Cornell University)|Apr 10, 2015

Gaussian Processes and Bayesian Inference参考文献 4被引用 9

一句话总结

本文提出了一种用于连续时间轨迹估计与建图（STEAM）的增量稀疏高斯过程回归算法，结合了高斯过程在处理连续、异步数据方面的优势与iSAM 2.0的贝叶斯树在增量变量重排序和即时重线性化方面的高效性。该方法在真实数据集上相比iSAM 2.0计算速度提升高达68%，仅增加8厘米的RMSE，实现了实时、可扩展的机器人定位与建图。

ABSTRACT

Recent work on simultaneous trajectory estimation and mapping (STEAM) for mobile robots has found success by representing the trajectory as a Gaussian process. Gaussian processes can represent a continuous-time trajectory, elegantly handle asynchronous and sparse measurements, and allow the robot to query the trajectory to recover its estimated position at any time of interest. A major drawback of this approach is that STEAM is formulated as a batch estimation problem. In this paper we provide the critical extensions necessary to transform the existing batch algorithm into an extremely efficient incremental algorithm. In particular, we are able to vastly speed up the solution time through efficient variable reordering and incremental sparse updates, which we believe will greatly increase the practicality of Gaussian process methods for robot mapping and localization. Finally, we demonstrate the approach and its advantages on both synthetic and real datasets.

研究动机与目标

• 解决基于批量高斯过程的STEAM方法在每次新增测量后需全量重新计算的局限性。
• 实现实时、增量的轨迹估计与建图，采用连续时间表示。
• 将iSAM 2.0的贝叶斯树计算效率与高斯过程在异步和非规则采样数据上的建模灵活性相结合。
• 在保持大规模机器人建图场景中高精度的同时降低计算成本。

提出的方法

• 将STEAM问题建模为连续时间高斯过程回归，将轨迹表示为时间的函数，包含均值函数与协方差函数。
• 采用稀疏近似方法，通过选择少量的诱导点来表示完整轨迹，从而降低计算复杂度。
• 使用贝叶斯树数据结构，实现增量变量重排序与即时重线性化，避免昂贵的批量重新优化。
• 应用高斯过程插值技术，在测量时间点之间估计状态，减少需显式维护的状态数量。
• 通过带加性高斯噪声的非线性测量模型，将里程计和传感器测量（如测距-测向）集成到GP框架中。
• 通过贝叶斯树执行增量更新，实现实时推理并保持整个估计过程中稀疏性。

实验结果

研究问题

• RQ1 基于高斯过程的连续时间轨迹估计能否实现增量化且足够高效，以适用于实时机器人应用？
• RQ2 将贝叶斯树与稀疏GP回归结合，如何提升大规模STEAM问题中的计算效率？
• RQ3 在不降低估计精度的前提下，高斯过程插值能在多大程度上减少显式建模的状态数量？
• RQ4 所提出的增量方法在真实与合成数据集上与批量方法及iSAM 2.0相比，在速度与精度方面表现如何？

主要发现

• 在自主割草机数据集上，所提出的BTGP算法相比iSAM 2.0将计算时间减少高达68%，同时仅增加8厘米的RMSE。
• 在包含6,969个时间步与151个特征点的维多利亚公园数据集中，BTGP在大规模场景下仍保持较低计算时间，展现出对大问题规模的鲁棒性。
• 通过BTGP对90%的状态进行插值（即仅显式建模10%的状态），计算时间相比iSAM 2.0减少33%，且未牺牲精度。
• 贝叶斯树中的变量重排序显著提升了性能，尤其在特征点数量远超轨迹状态数的大规模问题中。
• 结合插值的增量方法在大幅降低计算负载的同时，实现了接近最优的精度（与iSAM 2.0在完整数据上的表现相当）。
• 该方法可高效扩展至近10,000个轨迹状态，计算时间几乎无增长，证实其在大规模建图中的实用性。