gmapping 实现过程和数据走向

2024-09-06 04:12

本文主要是介绍gmapping 实现过程和数据走向,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

全局地图坐标系gmap_pose数据处理过程

初始化Map位姿

初始化地图坐标map:map是不会变化的,是gmapping 程序启动的位置姿态就是地图的原地

  // 地图原点设置为激光中心位置 启动程序时,设置原点GMapping::OrientedPoint gmap_pose(0, 0, 0);

激光初始化里程计的位姿

  // 获取初始姿态GMapping::OrientedPoint initialPose;if(!getOdomPose(initialPose, scan.header.stamp)){ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);}

获取当前激光雷达时间下的odom->lase的tf来得到里程计的位姿
通过tf变换来计算里程计的位姿

/*** @brief 尝试获取指定时间下的机器人里程计位置* * @param gmap_pose 输出参数,用于存储计算出的里程计位置* @param t 指定的时间点* * @return 返回是否成功获取里程计位置*/
bool SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
{// 获取中心激光雷达在指定时间的位置centered_laser_pose_.stamp_ = t;// 使用TF变换获取激光雷达中心位置对应的里程计坐标系下的位置tf::Stamped<tf::Transform> odom_pose;try{tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);}catch(tf::TransformException e){ROS_WARN("计算里程计位置失败,跳过扫描 (%s)", e.what());return false;}// 提取里程计坐标系下位置的姿态角double yaw = tf::getYaw(odom_pose.getRotation());// 构造并返回里程计位置gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),odom_pose.getOrigin().y(),yaw);return true;
}

传递位姿数据setPose—>getPose()

后面通过getPose来读取gmap_pose的位姿

  // 设置激光雷达扫描的姿态reading.setPose(gmap_pose);

gmap_pose数据保存和传递

// 防止多次定义
#ifndef ODOMETRYREADING_H
#define ODOMETRYREADING_H// 引入基本的传感器读数类和点类
#include <string.h>
#include <sensor/sensor_base/sensorreading.h>
#include <utils/point.h>
// 引入里程计传感器类
#include "odometrysensor.h"// 命名空间GMapping,用于地图构建相关功能
namespace GMapping{// 里程计读数类,继承自SensorReading
class OdometryReading: public SensorReading{public:// 构造函数,初始化里程计读数// 参数odo: 里程计传感器对象的指针// 参数time: 读数的时间戳,默认为0OdometryReading(const OdometrySensor* odo, double time=0);// 获取当前姿态inline const OrientedPoint& getPose() const {return m_pose;}// 获取当前速度inline const OrientedPoint& getSpeed() const {return m_speed;}// 获取当前加速度inline const OrientedPoint& getAcceleration() const {return m_acceleration;}// 设置当前姿态inline void setPose(const OrientedPoint& pose) {m_pose=pose;}// 设置当前速度inline void setSpeed(const OrientedPoint& speed) {m_speed=speed;}// 设置当前加速度inline void setAcceleration(const OrientedPoint& acceleration) {m_acceleration=acceleration;}protected:// 当前姿态OrientedPoint m_pose;// 当前速度OrientedPoint m_speed;// 当前加速度OrientedPoint m_acceleration;
};};
// 结束防止多次定义
#endif

激光雷达数据处理过程

laserCallback----->initMapper----->addScan---->updataMap

1. 数据预处理

  • 激光雷达数据获取:从激光雷达传感器获取原始的扫描数据。
  • 数据滤波:对原始数据进行滤波处理,去除噪声和异常点。

2. 粒子滤波初始化

  • 粒子初始化:在初始时刻,生成一组粒子(即机器人的可能位置),每个粒子代表机器人可能的一个状态。
  • 权重分配:为每个粒子分配初始权重,通常所有粒子的权重相等。

3. 运动模型更新

  • 预测步骤:根据机器人的运动模型(如里程计数据),预测每个粒子在下一个时刻的位置。
  • 粒子扩散:由于运动模型存在误差,粒子会在预测位置周围进行一定程度的扩散,以模拟不确定性。

4. 观测模型更新

  • 激光数据匹配:将当前时刻的激光雷达数据与地图(由粒子表示)进行匹配,计算每个粒子的似然度。
  • 权重更新:根据激光数据的匹配结果,更新每个粒子的权重。匹配效果好的粒子权重增加,匹配效果差的粒子权重减少。

5. 重采样

  • 粒子重采样:根据更新后的粒子权重,进行重采样。权重高的粒子被保留或复制,权重低的粒子被淘汰,以确保粒子集能够更好地代表机器人的真实状态。

6. 地图更新

  • 地图构建:根据重采样后的粒子集,更新地图。每个粒子对应一个局部地图,通过融合所有粒子的局部地图,得到全局地图。
  • 地图优化:对地图进行优化,去除噪声点,平滑地图边界。

7. 循环迭代

  • 时间更新:重复上述步骤,随着时间的推移,不断更新粒子集和地图,直到完成整个环境的建图。

8. 结果输出

  • 定位结果:输出机器人的最终位置估计。
  • 地图输出:输出最终构建的地图。

这篇关于gmapping 实现过程和数据走向的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/1140969

相关文章

pandas中位数填充空值的实现示例

《pandas中位数填充空值的实现示例》中位数填充是一种简单而有效的方法,用于填充数据集中缺失的值,本文就来介绍一下pandas中位数填充空值的实现,具有一定的参考价值,感兴趣的可以了解一下... 目录什么是中位数填充?为什么选择中位数填充?示例数据结果分析完整代码总结在数据分析和机器学习过程中,处理缺失数

Golang HashMap实现原理解析

《GolangHashMap实现原理解析》HashMap是一种基于哈希表实现的键值对存储结构,它通过哈希函数将键映射到数组的索引位置,支持高效的插入、查找和删除操作,:本文主要介绍GolangH... 目录HashMap是一种基于哈希表实现的键值对存储结构,它通过哈希函数将键映射到数组的索引位置,支持

Pandas使用AdaBoost进行分类的实现

《Pandas使用AdaBoost进行分类的实现》Pandas和AdaBoost分类算法,可以高效地进行数据预处理和分类任务,本文主要介绍了Pandas使用AdaBoost进行分类的实现,具有一定的参... 目录什么是 AdaBoost?使用 AdaBoost 的步骤安装必要的库步骤一:数据准备步骤二:模型

Pandas统计每行数据中的空值的方法示例

《Pandas统计每行数据中的空值的方法示例》处理缺失数据(NaN值)是一个非常常见的问题,本文主要介绍了Pandas统计每行数据中的空值的方法示例,具有一定的参考价值,感兴趣的可以了解一下... 目录什么是空值?为什么要统计空值?准备工作创建示例数据统计每行空值数量进一步分析www.chinasem.cn处

使用Pandas进行均值填充的实现

《使用Pandas进行均值填充的实现》缺失数据(NaN值)是一个常见的问题,我们可以通过多种方法来处理缺失数据,其中一种常用的方法是均值填充,本文主要介绍了使用Pandas进行均值填充的实现,感兴趣的... 目录什么是均值填充?为什么选择均值填充?均值填充的步骤实际代码示例总结在数据分析和处理过程中,缺失数

Java对象转换的实现方式汇总

《Java对象转换的实现方式汇总》:本文主要介绍Java对象转换的多种实现方式,本文通过实例代码给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要的朋友参考下吧... 目录Java对象转换的多种实现方式1. 手动映射(Manual Mapping)2. Builder模式3. 工具类辅助映

如何使用 Python 读取 Excel 数据

《如何使用Python读取Excel数据》:本文主要介绍使用Python读取Excel数据的详细教程,通过pandas和openpyxl,你可以轻松读取Excel文件,并进行各种数据处理操... 目录使用 python 读取 Excel 数据的详细教程1. 安装必要的依赖2. 读取 Excel 文件3. 读

Go语言开发实现查询IP信息的MCP服务器

《Go语言开发实现查询IP信息的MCP服务器》随着MCP的快速普及和广泛应用,MCP服务器也层出不穷,本文将详细介绍如何在Go语言中使用go-mcp库来开发一个查询IP信息的MCP... 目录前言mcp-ip-geo 服务器目录结构说明查询 IP 信息功能实现工具实现工具管理查询单个 IP 信息工具的实现服

SpringBoot基于配置实现短信服务策略的动态切换

《SpringBoot基于配置实现短信服务策略的动态切换》这篇文章主要为大家详细介绍了SpringBoot在接入多个短信服务商(如阿里云、腾讯云、华为云)后,如何根据配置或环境切换使用不同的服务商,需... 目录目标功能示例配置(application.yml)配置类绑定短信发送策略接口示例:阿里云 & 腾

Spring 请求之传递 JSON 数据的操作方法

《Spring请求之传递JSON数据的操作方法》JSON就是一种数据格式,有自己的格式和语法,使用文本表示一个对象或数组的信息,因此JSON本质是字符串,主要负责在不同的语言中数据传递和交换,这... 目录jsON 概念JSON 语法JSON 的语法JSON 的两种结构JSON 字符串和 Java 对象互转