ROS1 机器人 SLAM 系列（四）：Gmapping 算法详解与实战 > 将深入讲解 Gmapping 算法的原理，并通过实战演示如何使用 Gmapping 进行 2D 激光 SLAM 建图。 Gmapping 算法简介 1.1 什么是 Gmapping？ Gmapping 是一种基于**粒子滤波（Rao-Blackwellized Particle Filter, RBPF）**的 2D…
本文将深入讲解 Gmapping 算法的原理，并通过实战演示如何使用 Gmapping 进行 2D 激光 SLAM 建图。
Gmapping 是一种基于**粒子滤波（Rao-Blackwellized Particle Filter, RBPF）**的 2D 激光 SLAM 算法。它由 Giorgio Grisetti 等人于 2007 年提出，是 ROS 中最经典、应用最广泛的 SLAM 算法之一。
主要特点：
Gmapping 算法流程：
粒子滤波是一种序贯蒙特卡洛方法，通过一组带权重的粒子来近似表示后验概率分布。
基本思想：
数学表示：
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 个粒子的权重Gmapping 采用 RBPF，将 SLAM 问题分解为：
这种分解大大降低了计算复杂度：
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})
Gmapping 使用概率运动模型预测粒子位置：
// 简化的运动模型
x' = x + Δx + noise_x
y' = y + Δy + noise_y
θ' = θ + Δθ + noise_θ
其中噪声与运动量和参数（srr, srt, str, stt）相关。
扫描匹配是 Gmapping 的核心技术，用于精确估计机器人位姿。
步骤：
匹配得分计算：
score = Σ map[endpoint] × beam_weight
权重计算：
weight[i] ∝ P(z_t | x_t^{[i]}, m^{[i]})
自适应重采样：
Gmapping 采用自适应重采样策略，只在粒子退化严重时才进行重采样：
N_eff = 1 / Σ(w^{[i]})²
if N_eff < threshold: resample()
| 话题 | 消息类型 | 说明 |
|---|---|---|
/scan | sensor_msgs/LaserScan | 激光雷达数据 |
/tf | tf/tfMessage | 坐标变换 |
必需的 TF 变换：
odom → base_link：里程计变换base_link → laser：激光雷达位置| 话题 | 消息类型 | 说明 |
|---|---|---|
/map | nav_msgs/OccupancyGrid | 栅格地图 |
/map_metadata | nav_msgs/MapMetaData | 地图元数据 |
/tf | tf/tfMessage | map → odom 变换 |
| 服务 | 类型 | 说明 |
|---|---|---|
dynamic_map | nav_msgs/GetMap | 获取当前地图 |
# 粒子数量（核心参数）
particles: 30
# 默认 30，增大可提高精度但增加计算量
# 重采样阈值
resampleThreshold: 0.5
# 有效粒子数比例阈值
# 最小得分
minimumScore: 0.0
# 扫描匹配最小得分
参数建议：
# 最大使用距离
maxUrange: 80.0
# 用于地图构建的最大距离
maxRange: 80.0
# 传感器最大量程
# 激光束参数
lskip: 0
# 跳过的激光束数量（降采样）
# 里程计误差参数
srr: 0.1
# 平移引起的平移误差
srt: 0.2
# 平移引起的旋转误差
str: 0.1
# 旋转引起的平移误差
stt: 0.2
# 旋转引起的旋转误差
参数说明：
# 线性运动更新阈值
linearUpdate: 1.0
# 移动 1 米更新一次
# 角度运动更新阈值
angularUpdate: 0.5
# 旋转 0.5 弧度更新一次
# 时间更新阈值
temporalUpdate: -1.0
# -1 表示禁用时间更新
# 处理周期
throttle_scans: 1
# 每处理 1 帧扫描
# 地图分辨率
delta: 0.05
# 每个栅格 0.05 米
# 地图大小（初始）
xmin: -100.0
ymin: -100.0
xmax: 100.0
ymax: 100.0
# 占据概率参数
occ_thresh: 0.25
# 占据阈值
# 似然场参数
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
# 角度采样步长
确保已安装必要的包：
# 安装 Gmapping
sudo apt install ros-noetic-gmapping
# 安装键盘控制
sudo apt install ros-noetic-teleop-twist-keyboard
# 安装地图服务
sudo apt install ros-noetic-map-server
创建 gmapping.launch：
<launch>
<!-- Gmapping 节点 -->
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<!-- 基本参数 -->
<param name="base_frame" value="base_link"/>
<param name="odom_frame" value="odom"/>
<param name="map_frame" value="map"/>
<!-- 激光雷达参数 -->
<param name="maxUrange" value="10.0"/>
<param name="maxRange" value="12.0"/>
<param name="lskip" value="0"/>
<!-- 粒子滤波参数 -->
<param name="particles" value="50"/>
<param name="minimumScore" value="50"/>
<!-- 运动模型参数 -->
<param name="srr" value="0.1"/>
<param name="srt" value="0.2"/>
<param name="str" value="0.1"/>
<param name="stt" value="0.2"/>
<!-- 更新频率 -->
<param name="linearUpdate" value="0.5"/>
<param name="angularUpdate" value="0.436"/>
<param name="temporalUpdate" value="-1.0"/>
<!-- 地图参数 -->
<param name="delta" value="0.05"/>
<param name="xmin" value="-50.0"/>
<param name="ymin" value="-50.0"/>
<param name="xmax" value="50.0"/>
<param name="ymax" value="50.0"/>
<!-- 扫描匹配参数 -->
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="iterations" value="5"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
</node>
</launch>
步骤 1：启动仿真环境
# 设置机器人型号
export TURTLEBOT3_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：地图配置
如果有录制的数据包，可以离线建图：
# 播放数据包
rosbag play --clock recorded_data.bag
# 启动 Gmapping（需要使用仿真时间）
roslaunch gmapping.launch use_sim_time:=true
问题 1：地图有重影/不清晰
原因：里程计误差大或参数不匹配
解决方案：
# 增加粒子数
particles: 80
# 调整运动模型参数
srr: 0.2
srt: 0.3
str: 0.2
stt: 0.3
# 降低更新阈值
linearUpdate: 0.3
angularUpdate: 0.3
问题 2：建图速度慢
原因：粒子数过多或更新过于频繁
解决方案：
# 减少粒子数
particles: 30
# 增加更新阈值
linearUpdate: 1.0
angularUpdate: 0.5
# 降采样激光数据
lskip: 1
问题 3：地图有漂移
原因：里程计累积误差
解决方案：
# 提高扫描匹配精度
minimumScore: 100
iterations: 10
# 减小搜索步长
lstep: 0.02
astep: 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
小型室内环境（< 100㎡）：
particles: 30
maxUrange: 10.0
delta: 0.05
linearUpdate: 0.5
angularUpdate: 0.3
中型室内环境（100-500㎡）：
particles: 50
maxUrange: 15.0
delta: 0.05
linearUpdate: 0.8
angularUpdate: 0.4
大型环境（> 500㎡）：
particles: 100
maxUrange: 20.0
delta: 0.1
linearUpdate: 1.0
angularUpdate: 0.5
| 适用 | 不适用 |
|---|---|
| 中小型室内环境 | 大规模室外环境 |
| 静态环境 | 高度动态环境 |
| 有可靠里程计 | 无里程计或里程计很差 |
| 实时建图 | 需要极高精度 |
#!/usr/bin/env python3
import rospy
from nav_msgs.msg import OccupancyGrid
from nav_msgs.srv import GetMap
def map_callback(msg):
"""处理地图回调"""
width = msg.info.width
height = msg.info.height
resolution = msg.info.resolution
rospy.loginfo(f"地图大小：{width}x{height}, 分辨率：{resolution}m")
# 统计占据、空闲、未知栅格
occupied = sum(1 for cell in msg.data if cell > 50)
free = sum(1 for cell in msg.data if 0 <= cell <= 50)
unknown = sum(1 for cell in msg.data if cell == -1)
rospy.loginfo(f"占据：{occupied}, 空闲：{free}, 未知：{unknown}")
def get_map_service():
"""通过服务获取地图"""
rospy.wait_for_service('dynamic_map')
try:
get_map = rospy.ServiceProxy('dynamic_map', GetMap)
response = get_map()
return response.map
except rospy.ServiceException as e:
rospy.logerr(f"服务调用失败：{e}")
return None
if __name__ == '__main__':
rospy.init_node('map_listener')
# 方法 1：订阅话题
rospy.Subscriber('/map', OccupancyGrid, map_callback)
# 方法 2：调用服务
current_map = get_map_service()
rospy.spin()
#!/usr/bin/env python3
import rospy
from nav_msgs.msg import OccupancyGrid
class MappingMonitor:
def __init__(self):
self.coverage_history = []
rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
def map_callback(self, msg):
total_cells = len(msg.data)
known_cells = sum(1 for cell in msg.data if cell != -1)
coverage = known_cells / total_cells * 100
self.coverage_history.append(coverage)
# 判断是否稳定（建图基本完成）
if len(self.coverage_history) > 10:
recent = self.coverage_history[-10:]
if max(recent) - min(recent) < 0.5:
rospy.loginfo(f"建图已稳定，覆盖率：{coverage:.1f}%")
if __name__ == '__main__':
rospy.init_node('mapping_monitor')
monitor = MappingMonitor()
rospy.spin()
本文详细介绍了 Gmapping 算法：
Gmapping 作为经典的 2D SLAM 算法，非常适合学习 SLAM 的入门者。掌握 Gmapping 后，下一篇文章将介绍更先进的 Cartographer 算法。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
解析常见 curl 参数并生成 fetch、axios、PHP curl 或 Python requests 示例代码。 在线工具，curl 转代码在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online