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_wx、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, sigmaMOTION_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 到网格中心的距离。当 $d3.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_RANGE1σ 距离噪声2.0 m速度输入同样叠加NOISE_SPEED1σ 速度噪声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]并以v1.0 m/s、yaw_rate0.1 rad/s的恒定控制输入驱动。该示例的核心可调参数均定义在 histogram_filter.py 文件头部汇总如下参数默认值含义SIM_TIME50.0 s模拟总时长DT0.1 s时间步长MAX_RANGE10.0 m最大观测距离超过则无观测MOTION_STD1.0运动高斯分布标准差高斯模糊 sigmaRANGE_STD3.0观测高斯分布标准差XY_RESOLUTION0.5 m网格分辨率MIN_X / MIN_Y / MAX_X / MAX_Y-15.0 / -5.0 / 15.0 / 25.0网格地图边界NOISE_RANGE2.0 m距离观测 1σ 噪声NOISE_SPEED0.5 m/s速度输入 1σ 噪声EXTEND_AREA10.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 Roboticsprobabilistic-robotics.org与Robust Vehicle Localization in Urban Environments Using Probabilistic MapsStanford ICRA 2010为算法理论背景来源。【免费下载链接】PythonRoboticsPython sample codes and textbook for robotics algorithms.项目地址: https://gitcode.com/GitHub_Trending/py/PythonRobotics创作声明：本文部分内容由AI辅助生成（AIGC），仅供参考