ROS1机器人SLAM系列（四）：Gmapping算法详解与实战

ROS1机器人SLAM系列（四）：Gmapping算法详解与实战

本文将深入讲解Gmapping算法的原理，并通过实战演示如何使用Gmapping进行2D激光SLAM建图。

1. Gmapping算法简介

1.1 什么是Gmapping？

Gmapping是一种基于**粒子滤波（Rao-Blackwellized Particle Filter, RBPF）**的2D激光SLAM算法。它由Giorgio Grisetti等人于2007年提出，是ROS中最经典、应用最广泛的SLAM算法之一。

主要特点：

• 基于粒子滤波的概率框架
• 适用于2D激光雷达
• 需要里程计信息
• 实现成熟，稳定可靠
• 适合中小规模室内环境

1.2 算法流程概述

Gmapping算法流程

里程计数据

运动预测
Motion Model

粒子集合更新

激光雷达数据

扫描匹配
Scan Matching

观测更新
Sensor Model

粒子权重计算

重采样
Resample

地图更新

2. 核心算法原理

2.1 粒子滤波基础

粒子滤波是一种序贯蒙特卡洛方法，通过一组带权重的粒子来近似表示后验概率分布。

基本思想：

• 每个粒子代表机器人可能的位姿
• 粒子带有权重，表示该位姿的可信度
• 通过迭代更新粒子来跟踪机器人位姿

数学表示：

P(x_t | z_{1:t}, u_{1:t}) ≈ Σ w_t^{[i]} δ(x_t - x_t^{[i]}) 

其中：

• x_t^{[i]} 是第i个粒子的位姿
• w_t^{[i]} 是第i个粒子的权重
• δ 是狄拉克函数

2.2 Rao-Blackwellized粒子滤波

Gmapping采用RBPF，将SLAM问题分解为：

1. 位姿估计：使用粒子滤波
2. 地图构建：在已知位姿的条件下，使用分析方法更新地图

这种分解大大降低了计算复杂度：

P(x_{1:t}, m | z_{1:t}, u_{1:t}) = P(m | x_{1:t}, z_{1:t}) × P(x_{1:t} | z_{1:t}, u_{1:t}) 

2.3 运动模型

Gmapping使用概率运动模型预测粒子位置：

// 简化的运动模型 x'= x + Δx + noise_x y'= y + Δy + noise_y θ' = θ + Δθ + noise_θ 

其中噪声与运动量和参数（srr, srt, str, stt）相关。

2.4 扫描匹配（Scan Matching）

扫描匹配是Gmapping的核心技术，用于精确估计机器人位姿。

步骤：

1. 获取当前激光扫描数据
2. 与粒子携带的局部地图进行匹配
3. 找到使匹配度最高的位姿修正

匹配得分计算：

score = Σ map[endpoint] × beam_weight 

2.5 权重计算与重采样

权重计算：

weight[i] ∝ P(z_t | x_t^{[i]}, m^{[i]})

自适应重采样：

Gmapping采用自适应重采样策略，只在粒子退化严重时才进行重采样：

N_eff =1/ Σ(w^{[i]})² if N_eff < threshold:resample()

3. Gmapping的ROS实现

3.1 订阅的话题

必需的TF变换：

• odom → base_link：里程计变换
• base_link → laser：激光雷达位置

3.2 发布的话题

3.3 提供的服务

4. Gmapping参数详解

4.1 粒子滤波参数

# 粒子数量（核心参数）particles:30# 默认30，增大可提高精度但增加计算量# 重采样阈值resampleThreshold:0.5# 有效粒子数比例阈值# 最小得分minimumScore:0.0# 扫描匹配最小得分

参数建议：

• 小场景：30-50个粒子
• 大场景：80-100个粒子
• 实时性要求高：减少粒子数

4.2 激光雷达参数

# 最大使用距离maxUrange:80.0# 用于地图构建的最大距离maxRange:80.0# 传感器最大量程# 激光束参数lskip:0# 跳过的激光束数量（降采样）

4.3 运动模型参数

# 里程计误差参数srr:0.1# 平移引起的平移误差srt:0.2# 平移引起的旋转误差str:0.1# 旋转引起的平移误差stt:0.2# 旋转引起的旋转误差

参数说明：

• 值越大，表示里程计误差越大
• 轮式里程计较准：0.05-0.1
• 里程计不准：0.2-0.5

4.4 更新频率参数

# 线性运动更新阈值linearUpdate:1.0# 移动1米更新一次# 角度运动更新阈值angularUpdate:0.5# 旋转0.5弧度更新一次# 时间更新阈值temporalUpdate:-1.0# -1表示禁用时间更新# 处理周期throttle_scans:1# 每处理1帧扫描

4.5 地图参数

# 地图分辨率delta:0.05# 每个栅格0.05米# 地图大小（初始）xmin:-100.0ymin:-100.0xmax:100.0ymax:100.0# 占据概率参数occ_thresh:0.25# 占据阈值

4.6 扫描匹配参数

# 似然场参数sigma:0.05# 高斯平滑kernelSize:1# 核大小# 优化迭代次数iterations:5# 扫描匹配迭代次数# 搜索步长lstep:0.05# 线性搜索步长astep:0.05# 角度搜索步长# 搜索范围llsamplerange:0.01# 线性采样范围llsamplestep:0.01# 线性采样步长lasamplerange:0.005# 角度采样范围lasamplestep:0.005# 角度采样步长

5. 实战：使用Gmapping建图

5.1 准备工作

确保已安装必要的包：

# 安装Gmappingsudoaptinstall ros-noetic-gmapping # 安装键盘控制sudoaptinstall ros-noetic-teleop-twist-keyboard # 安装地图服务sudoaptinstall ros-noetic-map-server 

5.2 创建Gmapping Launch文件

创建 gmapping.launch：

<launch><!-- Gmapping节点 --><nodepkg="gmapping"type="slam_gmapping"name="slam_gmapping"output="screen"><!-- 基本参数 --><paramname="base_frame"value="base_link"/><paramname="odom_frame"value="odom"/><paramname="map_frame"value="map"/><!-- 激光雷达参数 --><paramname="maxUrange"value="10.0"/><paramname="maxRange"value="12.0"/><paramname="lskip"value="0"/><!-- 粒子滤波参数 --><paramname="particles"value="50"/><paramname="minimumScore"value="50"/><!-- 运动模型参数 --><paramname="srr"value="0.1"/><paramname="srt"value="0.2"/><paramname="str"value="0.1"/><paramname="stt"value="0.2"/><!-- 更新频率 --><paramname="linearUpdate"value="0.5"/><paramname="angularUpdate"value="0.436"/><paramname="temporalUpdate"value="-1.0"/><!-- 地图参数 --><paramname="delta"value="0.05"/><paramname="xmin"value="-50.0"/><paramname="ymin"value="-50.0"/><paramname="xmax"value="50.0"/><paramname="ymax"value="50.0"/><!-- 扫描匹配参数 --><paramname="sigma"value="0.05"/><paramname="kernelSize"value="1"/><paramname="iterations"value="5"/><paramname="lstep"value="0.05"/><paramname="astep"value="0.05"/><paramname="llsamplerange"value="0.01"/><paramname="llsamplestep"value="0.01"/><paramname="lasamplerange"value="0.005"/><paramname="lasamplestep"value="0.005"/></node></launch>

5.3 使用TurtleBot3仿真实战

步骤1：启动仿真环境

# 设置机器人型号exportTURTLEBOT3_MODEL=burger # 启动Gazebo仿真 roslaunch turtlebot3_gazebo turtlebot3_world.launch 

步骤2：启动Gmapping

# 使用TurtleBot3的Gmapping配置 roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=gmapping # 或使用自定义配置 roslaunch my_robot_slam gmapping.launch 

步骤3：启动RViz可视化

# 使用TurtleBot3的RViz配置 roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch # 在RViz中添加以下显示：# - Map: /map# - LaserScan: /scan# - TF# - RobotModel

步骤4：控制机器人建图

# 键盘遥控 rosrun teleop_twist_keyboard teleop_twist_keyboard.py # 控制键：# u i o# j k l# m , .## i: 前进 ,: 后退# j: 左转 l: 右转# k: 停止

步骤5：保存地图

# 建图完成后保存 rosrun map_server map_saver -f ~/maps/my_map # 会生成两个文件：# - my_map.pgm：地图图像# - my_map.yaml：地图配置

5.4 使用rosbag回放建图

如果有录制的数据包，可以离线建图：

# 播放数据包 rosbag play --clock recorded_data.bag # 启动Gmapping（需要使用仿真时间） roslaunch gmapping.launch use_sim_time:=true 

6. 建图效果优化

6.1 常见问题与解决方案

问题1：地图有重影/不清晰

原因：里程计误差大或参数不匹配

解决方案：

# 增加粒子数particles:80# 调整运动模型参数srr:0.2srt:0.3str:0.2stt:0.3# 降低更新阈值linearUpdate:0.3angularUpdate:0.3

问题2：建图速度慢

原因：粒子数过多或更新过于频繁

解决方案：

# 减少粒子数particles:30# 增加更新阈值linearUpdate:1.0angularUpdate:0.5# 降采样激光数据lskip:1

问题3：地图有漂移

原因：里程计累积误差

解决方案：

# 提高扫描匹配精度minimumScore:100iterations:10# 减小搜索步长lstep:0.02astep:0.02

问题4：无法建图/地图为空

检查项：

# 检查TF树 rosrun tf tf_monitor # 检查必需的TF变换是否存在 rosrun tf tf_echo odom base_link rosrun tf tf_echo base_link laser # 检查激光数据 rostopic echo /scan 

6.2 不同场景的参数建议

小型室内环境（< 100㎡）：

particles:30maxUrange:10.0delta:0.05linearUpdate:0.5angularUpdate:0.3

中型室内环境（100-500㎡）：

particles:50maxUrange:15.0delta:0.05linearUpdate:0.8angularUpdate:0.4

大型环境（> 500㎡）：

particles:100maxUrange:20.0delta:0.1linearUpdate:1.0angularUpdate:0.5

7. Gmapping的优缺点

7.1 优点

• ✅ 算法成熟，经过大量验证
• ✅ 实现简单，参数调整直观
• ✅ 对于中小场景效果良好
• ✅ 计算资源需求适中
• ✅ ROS社区支持完善

7.2 缺点

• ❌ 不支持回环检测
• ❌ 大场景下粒子数需求高
• ❌ 依赖较准确的里程计
• ❌ 只支持2D激光SLAM
• ❌ 无法处理动态环境

7.3 适用场景

8. 代码示例：程序中调用Gmapping

8.1 获取地图数据

#!/usr/bin/env python3import rospy from nav_msgs.msg import OccupancyGrid from nav_msgs.srv import GetMap defmap_callback(msg):"""处理地图回调""" width = msg.info.width height = msg.info.height resolution = msg.info.resolution rospy.loginfo(f"地图大小: {width}x{height}, 分辨率: {resolution}m")# 统计占据、空闲、未知栅格 occupied =sum(1for cell in msg.data if cell >50) free =sum(1for cell in msg.data if0<= cell <=50) unknown =sum(1for cell in msg.data if cell ==-1) rospy.loginfo(f"占据: {occupied}, 空闲: {free}, 未知: {unknown}")defget_map_service():"""通过服务获取地图""" rospy.wait_for_service('dynamic_map')try: get_map = rospy.ServiceProxy('dynamic_map', GetMap) response = get_map()return response.mapexcept rospy.ServiceException as e: rospy.logerr(f"服务调用失败: {e}")returnNoneif __name__ =='__main__': rospy.init_node('map_listener')# 方法1：订阅话题 rospy.Subscriber('/map', OccupancyGrid, map_callback)# 方法2：调用服务 current_map = get_map_service() rospy.spin()

8.2 判断建图完成度

#!/usr/bin/env python3import rospy from nav_msgs.msg import OccupancyGrid classMappingMonitor:def__init__(self): self.coverage_history =[] rospy.Subscriber('/map', OccupancyGrid, self.map_callback)defmap_callback(self, msg): total_cells =len(msg.data) known_cells =sum(1for cell in msg.data if cell !=-1) coverage = known_cells / total_cells *100 self.coverage_history.append(coverage)# 判断是否稳定（建图基本完成）iflen(self.coverage_history)>10: recent = self.coverage_history[-10:]ifmax(recent)-min(recent)<0.5: rospy.loginfo(f"建图已稳定，覆盖率: {coverage:.1f}%")if __name__ =='__main__': rospy.init_node('mapping_monitor') monitor = MappingMonitor() rospy.spin()

9. 总结

本文详细介绍了Gmapping算法：

1. 算法原理：基于RBPF的粒子滤波SLAM
2. 核心技术：运动模型、扫描匹配、权重计算、重采样
3. 参数配置：详细解释了各类参数的含义和调整方法
4. 实战演练：完整的建图流程和优化技巧
5. 适用场景：明确了Gmapping的优缺点和适用范围

Gmapping作为经典的2D SLAM算法，非常适合学习SLAM的入门者。掌握Gmapping后，下一篇文章将介绍更先进的Cartographer算法。

系列导航：

• 上一篇：激光雷达与传感器配置
• 下一篇：Cartographer算法详解

版权声明：本文为原创文章，转载请注明出处