移动机器人的蒙特卡罗定位

Question

我正在为我的机器人实施蒙特卡洛定位，给出了环境地图及其起始位置和方向。我的方法如下：

1. 在给定位置周围均匀地创建500个粒子
2. 然后在每一步：
   • 运动用odometry更新所有粒子（我目前的方法是newX = oldX + odometryX（1 + standardGaussianRandom）等）。
   • 使用声纳数据为每个粒子分配权重（公式是针对每个传感器概率* = gaussianPDF（realReading），其中高斯具有均值predictReading）
   • 以最大概率返回粒子作为此步骤的位置
   • 然后根据重量从旧的粒子重新采样9/10新粒子，并在预测位置周围均匀采样1/10

现在，我为机器人的环境编写了一个模拟器，以下是本地化的行为：http://www.youtube.com/watch?v=q7q3cqktwZI

我非常害怕机器人可能会在较长一段时间内迷路。如果将粒子添加到更广泛的区域，机器人会更容易丢失。

我期待更好的表现。有什么建议吗？

Answer 1

最大的错误是你假设重量最高的粒子是你的后状态。这不同意粒子滤波器的主要思想。

您使用里程表读数更新的粒子集是您的投标分配。通过考虑具有最高权重的粒子，您完全忽略了这种分布。如果你只是在整个状态空间中随机散布粒子，然后取出一个最能解释声纳数据的粒子，那就相同了。你只依靠声纳读数，因为声纳数据非常嘈杂，你估计非常糟糕。更好的方法是为每个粒子分配一个权重，对权重进行归一化，将每个粒子状态乘以其权重，然后将它们相加以获得后验状态。

对于重新采样步骤，我建议删除预测状态周围的随机样本，因为它们会破坏您的提案分发。生成随机样本以便从故障中恢复是合法的，但是这些应该分布在整个状态空间中，并且明确地不在您当前的预测范围内。