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

Question

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

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

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

我很担心机器人长时间迷路。如果将粒子添加到更广泛的区域，机器人就更容易迷路。

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

Answer 1

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

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

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