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

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

说明:

步骤:

过滤器初始化

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

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

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

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

通过运动预测概率

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

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

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

  • 例如,位置概率在观察时达到峰值:

请输入图片描述

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

请输入图片描述

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

通过观察更新概率

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

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

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

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

请输入图片描述

请输入图片描述

  • 当d为 3.0 时,h(z)分布为:

请输入图片描述

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

请输入图片描述

根据概率估计位置

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

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

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

测试运行

  • 进入目录PythonRobotics/Localization/histogram_filter

  • 执行文件

python3 histogram_filter.py
  • 红十字是真实位置,黑点是 RFID 位置

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

  • 效果图

请输入图片描述

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: none