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

Question

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

红色数据集由GPS引入，不包含漂移误差，而蓝色数据集基于SLAM算法，随着时间的推移漂移。

这个想法是SLAM在短距离内准确但最终漂移，而GPS在长距离上是准确的而在短距离上是不准确的。因此，我想弄清楚如何将SLAM数据与GPS融合，以便获得两种测量的最佳精度。至少如何处理这个问题？

Answer 1

由于您的GPS看起来非常局部偏向，我认为它是低成本的并且不使用任何校正技术，例如它不是差分的。您可能已经意识到，GPS错误不是高斯错误。 this paper中的人表示，对GPS噪声进行建模的好方法是v+eps，其中v是局部常数“偏置”向量（对于少数metters来说通常是常数，然后或多或少地平滑或突然地改变）eps是高斯噪声。

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

的误差函数

   measurement_constraints + some_weight * GPS_constraints 

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

如果你的SLAM是基于视觉和特征点的（你没有真正说出你使用的是什么类型的SLAM，所以我认为是最普遍的类型），那么融合上述任何一种方法都会导致“内部损失” ”。你做了一个突然的，暴力的纠正，并增加了重投的错误。这意味着您在SLAM的跟踪中丢失了内部因素。所以你必须重新三角测量点，等等。另外，请注意，即使我上面链接的论文提供了GPS错误的模型，它也不是一个非常准确的模型，并且假设GPS错误的分布是单峰的（对于EKF来说是必要的）对我来说似乎有点冒险。

所以，我认为一个好的选择是使用障碍期优化。基本上，这个想法是这样的：因为你真的不知道如何建模GPS误差，假设你在本地SLAM中有更多的指导，并最小化捕获SLAM重建质量的函数S(x)。请注意x_opt S的最小化程序。然后，只要不超过给定阈值S(x_opt)，就会融合GPS数据。在数学上，你想要最小化

some_coef/(thresh - S(X))  + ||x-x_{gps}||

并使用x_opt初始化最小化。 S的一个很好的选择是捆绑调整错误，因为通过不降级它，可以防止内部丢失。文学中还有S的其他选择，但它们通常意味着减少计算时间并且在准确性方面增加很少。

这与EKF不同，它没有很好的概率解释，但在实践中产生了非常好的结果（我已经将它用于与GPS以外的其他东西融合，并且效果很好）。例如，您可以参阅this excellent paper，了解如何彻底实施此操作，如何设置阈值等。

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