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

Question

我正在为我的机器人实施 Monte-Carlo 定位，该机器人获得了环境地图及其起始位置和方向。我的做法如下：

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

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

我很害怕机器人在更长的时间内可能会迷路。如果将粒子添加到更大的区域，机器人更容易迷路。

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

Answer 1

最大的错误是你假设权重最高的粒子是你的后验状态。这不符合粒子滤波器的主要思想。

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

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