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