PythonRobotics 直方图滤波定位（Histogram Filter Localization）原理与源码实战解析
【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics
本文围绕开源机器人算法库 PythonRobotics 中的 直方图滤波 2D 定位示例，系统讲解直方图滤波器（Histogram Filter，即连续空间中的离散贝叶斯滤波器）的四大核心步骤——初始化、运动预测、观测更新、位姿估计，并结合仓库源码与测试用例给出可直接运行的参数配置与实现细节。读完本文，你将掌握直方图滤波器的概率网格建模方法、高斯噪声注入技巧，以及如何用 RFID 距离观测在无需初始位置信息的条件下完成机器人 x-y 定位。
直方图滤波定位（Histogram filter localization）是 PythonRobotics 中 Localization 模块的一个 2D 定位示例。模拟场景为：红色叉号表示机器人真实位置，黑色点为 RFID 标签位置，蓝色网格热力图表示直方图滤波器输出的位置概率分布。该模拟假设机器人航向角（yaw）与 RFID 位置已知，而 x、y 坐标未知，滤波器仅依靠速度输入与 RFID 距离观测完成定位，且不需要任何初始位置信息。
直方图滤波器是什么：连续空间中的离散贝叶斯滤波
直方图滤波器本质上是将连续空间离散化后的贝叶斯滤波器。它用规则网格（regular grids）管理机器人存在的概率：某个网格的概率越高，表示机器人位于该网格的可能性越大。本示例需要估计 x-y 位置，因此使用 2D 网格数据（二维数组）承载概率分布。
从源码看，网格由 GridMap 类 与 init_grid_map() 构造，关键字段包括：
xy_resolution：网格分辨率（每格边长，米）；min_x / min_y / max_x / max_y：网格地图边界；x_w / y_w：x、y 方向的网格数量（由边界与分辨率计算得出）；data：二维概率数组，初始全部为 1.0，随后归一化；dx / dy：累积的亚网格位移残差，用于运动更新时判断是否发生网格平移。
直方图滤波器估计概率分布共分为 4 步，下面逐一结合源码展开。
Step 1：滤波器初始化——均匀分布，免初始位姿
直方图滤波器不需要初始位置信息。在没有先验时，可将每个网格的初始概率设为相同值（均匀分布），对应源码中init_grid_map()将data全部初始化为1.0后再调用 normalize_probability() 归一化，使总概率和为 1。
如果能够获得初始位置信息，则可以基于该信息设置初始概率分布。文档特别提示：当初始位置以高斯分布形式给出时，gaussian_grid_map 示例 可能很有用，它演示了如何构建高斯概率网格地图。
Step 2：运动预测——概率平移 + 高斯模糊
当机器人从一个网格移动到下一个网格时，所有网格的概率信息都沿着运动方向整体平移，以此反映"机器人移动导致概率分布迁移"这一过程。运动之后，还需要在概率分布中体现运动带来的估计误差。
源码中的运动更新由 motion_update() 完成，具体分为两阶段：
- 网格平移：按
dx += DT·cos(yaw)·u[0]、dy += DT·sin(yaw)·u[0]累积位移，当累积位移超过一个网格（abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0）时，调用 map_shift() 将整个概率数组平移，并扣减已消耗的位移残差； - 噪声注入：对概率数组施加
gaussian_filter(grid_map.data, sigma=MOTION_STD)，用高斯滤波模拟运动不确定性。
文档用两张热力图直观展示了这一特性：有观测时概率分布是尖锐的（peaky）；而长时间无观测时，概率逐渐变得不确定、分布发散（如 1.png 与 2.png 所示）。
Step 3：观测更新——按传感器模型修正概率
观测更新是贝叶斯滤波的"更新"环节：用观测数据修正所有网格的概率。更新公式随传感器模型不同而不同，本模拟采用距离观测模型，每个网格的概率按如下公式更新：
$$p_t = p_{t-1} \cdot h(z)$$
$$h(z) = \frac{\exp\left(-(d - z)^{2} / 2\right)}{\sqrt{2 \pi}}$$
其中：
- $p_t$ 为时刻 t 的概率；
- $h(z)$ 为观测值为 $z$ 时的观测概率（似然）；
- $d$ 为已知的 RFID 到网格中心的距离。
当 $d=3.0$ 时，$h(z)$ 的分布即第 4 张图片展示的高斯钟形曲线；而当一个 RFID 被观测到时，观测概率分布在平面上呈一个圆形（如 3.png 所示）。
源码实现中，observation_update() 遍历所有观测与所有网格，逐格调用 calc_gaussian_observation_pdf()：先计算网格中心与 RFID 的欧氏距离d = math.hypot(x - z[iz,1], y - z[iz,2])，再用norm.pdf(d - z[iz,0], 0.0, std)计算似然并累乘到该网格概率上，最后归一化。这里RANGE_STD即观测高斯分布的标准差（源码中为 3.0）。
观测数据的产生由 observation() 模拟：仅当机器人到 RFID 的距离不超过MAX_RANGE时才会产生观测，且观测距离叠加NOISE_RANGE（1σ 距离噪声，2.0 m）；速度输入同样叠加NOISE_SPEED（1σ 速度噪声，0.5 m/s）。
Step 4：位姿估计——从概率分布计算最终位置
在每个时间步，都可以从当前概率分布推算机器人的最终位置，常用两种方式：
- 取概率最大的网格位置（最大后验估计）；
- 取概率加权平均的网格位置（期望估计）。
文档与源码将这两种估计方式作为可选策略列出，实现上直接对grid_map.data处理即可，无需额外维护状态向量。
主流程与可运行参数速查
直方图滤波定位的完整入口是 histogram_filter_localization()，它依次执行motion_update与observation_update。模拟主循环位于 main()，设置了 4 个 RFID 标签位置（如[10.0, 0.0]、[10.0, 10.0]、[0.0, 15.0]、[-5.0, 20.0]），并以v=1.0 m/s、yaw_rate=0.1 rad/s的恒定控制输入驱动。
该示例的核心可调参数（均定义在 histogram_filter.py 文件头部）汇总如下：
| 参数 | 默认值 | 含义 |
|---|---|---|
SIM_TIME | 50.0 s | 模拟总时长 |
DT | 0.1 s | 时间步长 |
MAX_RANGE | 10.0 m | 最大观测距离（超过则无观测） |
MOTION_STD | 1.0 | 运动高斯分布标准差（高斯模糊 sigma） |
RANGE_STD | 3.0 | 观测高斯分布标准差 |
XY_RESOLUTION | 0.5 m | 网格分辨率 |
MIN_X / MIN_Y / MAX_X / MAX_Y | -15.0 / -5.0 / 15.0 / 25.0 | 网格地图边界 |
NOISE_RANGE | 2.0 m | 距离观测 1σ 噪声 |
NOISE_SPEED | 0.5 m/s | 速度输入 1σ 噪声 |
EXTEND_AREA | 10.0 m | 网格地图扩展长度（文档保留字段） |
依赖环境以仓库根目录下 requirements/requirements.txt 为准，核心依赖为 numpy、scipy（提供gaussian_filter与norm.pdf）与 matplotlib（热力图绘制）。
运行与验证方式
直接运行示例脚本即可观察蓝色概率热力图随时间的演化：
python Localization/histogram_filter/histogram_filter.py运行过程中按Esc键可随时终止模拟（见 main() 中的键盘事件绑定）。关闭动画可设置show_animation = False。
仓库提供了对应的单元测试 test_histogram_filter.py，其做法是关闭动画（show_animation = False）并把SIM_TIME缩短为 1.0 s 后调用main()，验证主流程可正常跑通；测试通过 conftest.py 将仓库根目录加入sys.path以正确导入Localization包。可在仓库根目录执行：
pytest tests/test_histogram_filter.py小结
直方图滤波器以"网格概率数组"这一简洁数据结构，完整实现了贝叶斯滤波的预测—更新闭环：初始化阶段用均匀分布免除初始位姿需求，运动预测阶段通过数组平移加高斯模糊刻画不确定性增长，观测更新阶段以高斯似然逐网格修正概率，最后从概率分布中提取位姿估计。PythonRobotics 的这一示例代码结构清晰、参数集中、可视化直观，非常适合作为理解离散贝叶斯滤波与网格概率定位的入门范本；相关的粒子滤波（Particle Filter）、扩展卡尔曼滤波（EKF）等定位示例可进一步对比不同概率定位方法的建模差异。
参考
- 直方图滤波定位源码
- 直方图滤波定位单元测试
- gaussian_grid_map 高斯网格地图示例
- 文档引用：Probabilistic Robotics（probabilistic-robotics.org）与Robust Vehicle Localization in Urban Environments Using Probabilistic Maps（Stanford ICRA 2010）为算法理论背景来源。
【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics
创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考
