gmapping 实现过程和数据走向

本文主要是介绍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 实现过程和数据走向的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---