PythonRobotics机器人算法库-直方图滤波器定位

说明：

• 介绍如何使用直方图滤波器定位

• 参考代码

步骤：

过滤器初始化

• 直方图过滤器不需要初始位置信息。

• 在那种情况下，我们可以将每个网格概率初始化为相同的值。

• 如果我们可以使用初始位置信息，我们可以根据它设置初始概率。

• 当初始位置信息作为高斯分布提供时，高斯网格图可能很有用。

通过运动预测概率

• 在直方图滤波器中，当机器人移动到下一个格子时，每个格子的所有概率信息都向运动方向移动。

• 这个过程代表了机器人移动时概率分布的变化。

• 机器人运动后，概率分布需要反映运动引起的估计误差。

• 例如，位置概率在观察时达到峰值：

• 但是，如果没有观察，概率会变得不确定：

• 高斯滤波器 在模拟中用于添加噪声。

通过观察更新概率

• 在这一步中，所有的概率都通过观察更新，这就是贝叶斯滤波器的更新步骤。

• 概率更新公式因使用的传感器型号而异。

• 本次模拟采用距离观测模型。

• 每个格子的概率通过这个公式更新：

• 当d为 3.0 时，h(z)分布为：

• 当观察到 RF-ID 时，观察概率分布看起来像一个圆圈：

根据概率估计位置

• 在每个时间步中，我们可以根据当前的概率分布计算出最终的机器人位置。有两种计算最终位置的方法：

• 使用最大概率网格位置。

• 使用概率加权研磨位置的平均值

测试运行

• 进入目录PythonRobotics/Localization/histogram_filter

• 执行文件

python3 histogram_filter.py

• 红十字是真实位置，黑点是 RFID 位置

• 蓝色网格显示直方图过滤器的位置概率

• 效果图