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

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

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

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

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