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 订阅的话题

话题	消息类型	说明
/scan	sensor_msgs/LaserScan	激光雷达数据
/tf	tf/tfMessage	坐标变换

必需的 TF 变换：

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

3.2 发布的话题

话题	消息类型	说明
/map	nav_msgs/OccupancyGrid	栅格地图
/map_metadata	nav_msgs/MapMetaData	地图元数据
/tf	tf/tfMessage	map → odom 变换

3.3 提供的服务

服务	类型	说明
dynamic_map	nav_msgs/GetMap	获取当前地图

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.0
ymin: -100.0
xmax: 100.0
ymax: 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 准备工作

确保已安装必要的包：

# 安装 Gmapping
sudo apt install ros-noetic-gmapping
# 安装键盘控制
sudo apt install ros-noetic-teleop-twist-keyboard
# 安装地图服务
sudo apt install ros-noetic-map-server

5.2 创建 Gmapping Launch 文件

创建 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>

5.3 使用 TurtleBot3 仿真实战

步骤 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：地图配置

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.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

6.2 不同场景的参数建议

小型室内环境（< 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

7. Gmapping 的优缺点

7.1 优点

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

7.2 缺点

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

7.3 适用场景

适用	不适用
中小型室内环境	大规模室外环境
静态环境	高度动态环境
有可靠里程计	无里程计或里程计很差
实时建图	需要极高精度

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

8.1 获取地图数据

#!/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()

8.2 判断建图完成度

#!/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()

9. 总结

本文详细介绍了 Gmapping 算法：

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

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