Gmapping 原理之目标分布与提议分布

1. 概述

简单讲一下Gmapping的原理中的提议分布与目标分布,并指出论文中的公式错误,如果阅读此文章,需要先熟悉原始论文,如果本文有错误,欢迎指出讨论,同时此文章供自己以后参考(已经写了很久了,一直没有发出来)。

2. 目标分布与提议分布

2.0 概率复习

可能用到的概率知识
条件概率公式
在这里插入图片描述
贝叶斯公式
在这里插入图片描述
全概率公式
在这里插入图片描述
组合拳
在这里插入图片描述
以上图片均来自百度图片搜索结果,感谢!

2.1 目标分布

目标分布也称为真实分布,如果考虑机器人的运动,那么机器人在t时刻位置的真实分布就是目标分布,但是这是不能得到的,只是理论上的分布,我们需要只能通过其他分布来得到该分布,评价的标准就是权重f/gf表示目标分布,g表示提议分布

2.2 提议分布

因为目标分布是得不到的,因此需要使用提议分布来得到目标分布的真实情况。一般的,使用里程计模型来作为提议分布。但是改进的提议分布发现激光具有单峰的特性,方差更小,更适合作为提议分布,因此考虑激光和里程计一起作为改进的提议分布。
简单列出一下权重计算不同,后面会推导权重计算公式。
只使用里程计模型,那么得到的权重计算公式为

### GMapping 算法原理 GMapping 是一种基于激光雷达数据的二维 SLAM(Simultaneous Localization and Mapping,同步定位地图构建)算法,广泛应用于机器人领域。其核心原理基于 **Rao-Blackwellized 粒子滤波(RBPF)**,将定位建图问题解耦,分别处理。在 RBPF 框架中,每个粒子代表机器人的一条可能路径,同时每个粒子都携带一个地图估计。通过激光雷达数据里程计数据的融合,GMapping 能够实时构建环境的地图并估计机器人的位姿 [^3]。 #### 核心思想 GMapping 的核心思想是:在已知激光雷达数据的前提下,同时估计机器人的位姿和构建地图。该算法通过粒子滤波的方法对机器人路径进行估计,并基于贝叶斯推理更新地图模型。RBPF 的引入使得每个粒子独立地维护一个地图,从而减少了计算复杂度,提高了算法的实时性 [^3]。 ### GMapping 的工作流程 GMapping 的工作流程主要包括以下几个步骤: 1. **数据输入** GMapping 依赖于激光雷达数据和里程计数据。激光雷达数据以 `sensor_msgs/LaserScan` 的形式提供,而里程计数据则以 `nav_msgs/Odometry` 的形式提供 [^2]。 2. **初始化粒子集** 系统初始化时,生成一组粒子,每个粒子表示机器人的一种可能路径。每个粒子携带一个初始的地图估计。 3. **粒子预测** 基于里程计数据,预测每个粒子的下一时刻位姿。这一步骤利用了机器人的运动模型,将里程计信息转化为粒子的位姿更新 。 4. **激光雷达数据更新** 使用激光雷达数据对每个粒子进行权重更新。具体来说,根据激光雷达的观测数据,计算每个粒子对应的地图观测数据的匹配程度,从而调整粒子的权重 [^3]。 5. **重采样** 根据粒子的权重进行重采样,保留权重较高的粒子,淘汰权重较低的粒子。这一步骤确保了粒子集能够更好地反映机器人的真实路径 [^3]。 6. **地图更新** 每个粒子维护的地图随着粒子的更新而更新。最终的地图是所有粒子地图的加权平均,反映了环境的二维格栅地图 [^2]。 7. **发布地图** GMapping 会发布两种地图数据:地图的元数据(如分辨率、地图大小等)和栅格数据(表示每个栅格的占用概率) [^2]。 ### 代码示例 以下是一个简化的 GMapping 算法的伪代码示例: ```cpp // 初始化粒子集 void initializeParticles() { // 生成初始粒子集 for (int i = 0; i < NUM_PARTICLES; ++i) { Particle particle; particle.pose = initialPose; // 初始位姿 particle.map = initialMap; // 初始地图 particles.push_back(particle); } } // 预测粒子位姿 void predictParticles(const Odometry& odometry) { for (auto& particle : particles) { particle.pose = updatePose(particle.pose, odometry); // 根据里程计更新位姿 } } // 更新粒子权重 void updateWeights(const LaserScan& scan) { for (auto& particle : particles) { double weight = calculateWeight(particle.map, scan, particle.pose); // 计算权重 particle.weight = weight; } } // 重采样 void resampleParticles() { std::vector<Particle> newParticles; std::discrete_distribution<> distribution(weights.begin(), weights.end()); for (int i = 0; i < NUM_PARTICLES; ++i) { int index = distribution(generator); // 根据权重选择粒子 newParticles.push_back(particles[index]); } particles = newParticles; } // 更新地图 void updateMaps(const LaserScan& scan) { for (auto& particle : particles) { particle.map = updateMap(particle.map, scan, particle.pose); // 根据激光雷达数据更新地图 } } ``` ###
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值