使用 GPS 测量校正 SLAM 漂移误差

Posted 2023-02-21

【中文标题】使用 GPS 测量校正 SLAM 漂移误差

【英文标题】：Correcting SLAM drift error using GPS measurements

【发布时间】：2017-08-25 00:14:21

【问题描述】：

我试图弄清楚如何使用 GPS 测量来纠正由 SLAM 方法引入的漂移误差，我在固定时刻在欧几里得 3d 空间中有两个点集：

红色数据集由 GPS 引入，没有漂移误差，蓝色数据集基于 SLAM 算法，随时间漂移。

我们的想法是 SLAM 在短距离上准确但最终会漂移，而 GPS 在长距离上准确而在短距离上不准确。因此，我想弄清楚如何将 SLAM 数据与 GPS 融合，以使两种测量都具有最佳精度。至少如何解决这个问题？

【参考方案1】：

由于您的 GPS 看起来非常偏向本地，我假设它是低成本的并且不使用任何校正技术，例如它不是差分的。您可能知道，GPS 误差不是高斯误差。 this paper 中的人表明，模拟 GPS 噪声的一种好方法是 v+eps，其中 v 是一个局部恒定的“偏置”向量（它通常保持几米不变，然后或多或少平滑地变化或突然）和eps 是高斯噪声。

鉴于此信息，一种选择是使用基于卡尔曼的融合，例如您将 GPS 噪声和偏差添加到状态向量，并适当地定义您的转移方程并按照您的方式进行一个普通的EKF。注意，如果我们忽略卡尔曼的预测步骤，这大致相当于最小化形式的误差函数

   measurement_constraints + some_weight * GPS_constraints 

这为您提供了更直接的第二选择。例如，如果您的 SLAM 是可视的，您可以只使用平方重投影误差之和（即束调整误差）作为测量约束，并将 GPS 约束定义为||x- x_gps||，其中@ 987654329@ 是2d 或3d GPS 位置（您可能希望忽略使用低成本 GPS 的高度）。

如果您的 SLAM 是基于视觉和特征点的（您没有真正说明您使用的是哪种类型的 SLAM，所以我假设是最广泛使用的类型），那么与上述任何方法的融合都可能导致“内部损失” ”。你做了一个突然的、猛烈的修正，并增加了重投影误差。这意味着您在 SLAM 的跟踪中丢失了内点。所以你必须重新对点进行三角测量，等等。另外，请注意，尽管我在上面链接到的论文提供了 GPS 误差模型，但它不是一个非常准确的模型，并且假设 GPS 误差的分布是单峰的（EKF 所必需的）对我来说似乎有点冒险.

所以，我认为一个不错的选择是使用障碍项优化。基本上，这个想法是这样的：因为你真的不知道如何对 GPS 误差建模，假设你对本地的 SLAM 更有信心，并最小化一个函数S(x)，它可以捕获你的 SLAM 重建的质量。注意x_opt 是S 的最小化器。然后，与 GPS 数据融合，只要它不会恶化S(x_opt) 超过给定阈值。从数学上讲，您希望最小化

some_coef/(thresh - S(X))  + ||x-x_gps||

然后您将使用x_opt 初始化最小化。 S 的一个不错的选择是捆绑调整错误，因为通过不降低它，您可以防止内部损失。文献中还有其他S 的选择，但它们通常是为了减少计算时间并在准确性方面增加一点。

这与 EKF 不同，它没有很好的概率解释，但在实践中产生了非常好的结果（我也将它用于与 GPS 以外的其他事物融合，并且效果很好）。例如，您可以查看this excellent paper，它解释了如何彻底实现这一点，如何设置阈值等。

希望这会有所帮助。如果您在我的回答中发现不准确/错误，请随时告诉我。

【讨论】：

你用了几个月的时间来理解那篇“优秀的论文”？ 0_o'

@johnktejik 好吧...我认为阅读起来比阅读更乏味。如果您熟悉捆绑调整中的基本矩阵分析（或者只是矩阵的泰勒展开式）和基本协方差反向传播（参见 Zisserman 关于多视图计算机视觉的书），那么它就像是“休，一个完整的定理”的继承这个？”

@johnktejik 基本上，对于这个问题，我认为唯一有趣的部分是第 3.2 部分和第 4 部分和第 5 部分。但我明白你的意思，这位作者很少提供直觉并深入研究符号马上嘎吱作响。我认为这篇旧版本的论文给人一种更好、更自然的感觉maxime.lhuillier.free.fr/pCvpr11.pdf，例如用更自然的术语解释了这个定理（第 4.3 节）。