[论文解读] A White-Noise-On-Jerk Motion Prior for Continuous-Time Trajectory Estimation on SE(3)

Tim Y. Tang, David J. Yoon+1 more|arXiv (Cornell University)|Sep 17, 2018

Time Series Analysis and Forecasting被引用 4

一句话总结

本文提出了一种用于SE(3)连续时间轨迹估计的白噪声在加加速度上（WNOJ）运动先验，相较于高斯过程-based同时轨迹估计与建图（STEAM）中的标准白噪声在加速度上（WNOA）先验，实现了改进。通过建模先验均值以鼓励恒定加速度，新公式减少了偏差并提高了准确性，尤其在具有非零加速度的轨迹中，与WNOA相比，在真实世界激光雷达数据集上实现了高达9%的误差降低。

ABSTRACT

Simultaneous trajectory estimation and mapping (STEAM) offers an efficient approach to continuous-time trajectory estimation, by representing the trajectory as a Gaussian process (GP). Previous formulations of the STEAM framework use a GP prior that assumes white-noise-on-acceleration, with the prior mean encouraging constant body-centric velocity. We show that such a prior cannot sufficiently represent trajectory sections with non-zero acceleration, resulting in a bias to the posterior estimates. This paper derives a novel motion prior that assumes white-noise-on-jerk, where the prior mean encourages constant body-centric acceleration. With the new prior, we formulate a variation of STEAM that estimates the pose, body-centric velocity, and body-centric acceleration. By evaluating across several datasets, we show that the new prior greatly outperforms the white-noise-on-acceleration prior in terms of solution accuracy.

研究动机与目标

• 解决当运动先验无法表示非零加速度时，连续时间轨迹估计中的偏差问题。
• 在具有动态运动（如城市驾驶）的场景中，提高基于高斯过程的轨迹估计准确性。
• 推导并验证一种假设白噪声作用于加加速度的新运动先验，以更好地表示加速度变化。
• 证明新先验在具有运动失真的真实世界激光雷达数据集中，优于标准的白噪声在加速度上先验。
• 通过在状态向量中包含加速度，实现更精确的连续时间位姿与速度估计。

提出的方法

• 推导了用于运动建模的新随机微分方程（SDE），假设加加速度上存在白噪声，从而导出WNOJ先验。
• 扩展了STEAM框架，将机体中心加速度纳入状态向量，实现位姿、速度与加速度的联合估计。
• 使用基于WNOJ SDE推导出的核函数的连续时间高斯过程先验，确保轨迹先验平滑且符合物理规律。
• 通过批量优化求解估计问题，保持逆核矩阵的稀疏性以提升计算效率。
• 使用KITTI训练集调参过程噪声协方差矩阵 $\mathbf{Q}_c$，并在所有数据集中统一应用以保证公平比较。
• 利用从连续时间GP后验插值得到的位姿进行点云配准，补偿扫描激光雷达数据中的运动失真。

实验结果

研究问题

• RQ1 与标准的白噪声在加速度上先验相比，白噪声在加加速度上的运动先验是否能减少连续时间轨迹估计中的偏差？
• RQ2 在具有非均匀运动的真实世界激光雷达数据集中，通过在先验中建模恒定加速度，是否能提升估计准确性？
• RQ3 在状态向量中包含加速度对估计性能和计算成本有何影响？
• RQ4 与WNOA相比，WNOJ先验在运动失真的激光雷达数据中能多大程度上改善轨迹估计？
• RQ5 该新先验是否能在包括城市、郊区和高速公路环境在内的多样化驾驶场景中实现良好泛化？

主要发现

• 在包含城市驾驶中频繁加速度变化的多伦多大学数据集中，WNOJ先验将整体轨迹误差降低了9%。
• 在里士满山数据集中，新估计器相比基线WNOA估计器，整体误差降低了8.4%。
• 在具有高动态运动（如在路口转弯和减速）的序列中，WNOJ先延显著提升了轨迹估计的准确性。
• 新估计器在运动失真的点云中表现出更小的航位推算偏差，如三维轨迹图所示。
• 尽管由于在状态向量中增加了加速度，优化成本增加了50%，但由于其他组件的计算成本保持不变，整体运行时间增加有限。
• WNOJ先延对过程噪声协方差 $\mathbf{Q}_c$ 更为敏感，但仅在KITTI数据集上调参即可在所有测试数据集中实现一致的性能提升。