ROS1 机器人 SLAM：Gmapping 算法详解与实战

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.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 算法。