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

Question

我试图找出如何纠正SLAM方法引入的漂移误差，利用GPS测量，我在欧几里得三维空间中有两个点集，它们是在固定时刻拍摄的：

​

​

​

​

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

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

Answer 1

因为你的全球定位系统看起来非常局部偏倚，我假设它是低成本的，不使用任何校正技术，例如，它是没有差别的。您可能知道，GPS误差不是高斯的。本论文的研究表明，一种很好的建模方法是v+eps，其中v是一个局部常量的“偏差”向量(通常是一些metters的常数，然后或多或少地平滑或突然地变化)，而eps是高斯噪声。

考虑到这些信息，一种选择是使用基于卡尔曼的融合，例如，将GPS噪声和偏差添加到状态向量中，并适当地定义您的转换方程，并像处理普通的EKF那样继续进行。请注意，如果忽略Kalman的预测步骤，这大致相当于将形式的误差函数最小化。

   measurement_constraints + some_weight * GPS_constraints 

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

如果你的SLAM是基于视觉和特征点的(你并没有说你使用的是哪种类型的SLAM，所以我假设是最广泛的类型)，那么与上面任何一种方法的融合都会导致"inlier损失“。你做了一个突然的，猛烈的纠正，并增加了重新投影的错误。这意味着你在SLAM的跟踪中失去了轻量级。所以你必须重新定位点，等等。另外，请注意，尽管我所链接的这篇论文提供了一个GPS误差模型，但它并不是一个非常精确的模型，并且假设GPS误差的分布是单峰的(EKF所必需的)，对我来说似乎有点冒险。

因此，我认为一个很好的选择是使用障碍项优化。基本上，我们的想法是:由于您并不真正知道如何建模GPS错误，假设您在本地对SLAM有更多的信任，并且最小化一个捕捉SLAM重建质量的函数S(x)。注意，x_opt是S的最小化器。然后，与全球定位系统数据融合，只要它不使S(x_opt)恶化超过一个给定的阈值。从数学上讲，你会想要最小化

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

然后用x_opt初始化最小化。对于S来说，一个很好的选择是包调整错误，因为通过不降低它，就可以防止inlier损失。在文言文中还有其他S的选择，但它们通常是为了减少计算时间，并且在准确性方面几乎不增加什么。

这与EKF不同，没有很好的概率解释，但在实践中产生了非常好的结果(我也将它用于与GPS以外的其他事物的融合，而且效果很好)。例如，您可以看到这篇优秀的论文，它解释了如何彻底实现这一点，以及如何设置阈值，等等。

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