SLAM实战指南（五）：基于纯激光雷达的GMapping建图与laser_scan_matcher定位实战

1. 纯激光雷达SLAM的挑战与机遇

当你手头只有一个激光雷达，却想实现机器人自主建图时，这个场景就像只用一把尺子测量整个房间的尺寸。传统SLAM通常依赖编码器、IMU等多传感器融合，但在教育机器人、低成本AGV等场景中，我们常常遇到只有单线激光雷达的硬件配置。

我去年帮学生团队改造一台旧扫地机器人时就遇到过这种情况。原厂里程计已经损坏，但激光雷达完好，最终我们通过GMapping+laser_scan_matcher的方案，用不到200元的硬件成本实现了基础建图功能。这种方案最大的优势是硬件门槛极低——市面上常见的RPLIDAR A1或思岚A1M8等雷达，二手价格通常在300元以内。

不过要注意，纯激光方案有两个明显短板：一是运动方向上的位置估计容易漂移，就像蒙眼走路时会逐渐偏离直线；二是快速转向时容易丢失定位，建议控制机器人以低于0.3m/s的速度运动。实测发现，在10m×10m的室内环境中，建图误差可以控制在5%以内，足够应对大多数教学演示或简单巡检需求。

2. GMapping算法深度解析

2.1 粒子滤波的网格魔法

GMapping的核心是Rao-Blackwellized粒子滤波（RBPF），这种算法巧妙地将SLAM问题分解为定位和建图两个子任务。就像考古学家通过碎片复原文物，每个粒子都携带一张可能的地图假设，通过激光扫描数据不断修正这些假设。

具体实现时，算法会维护约30个粒子（可通过particles参数调整）。每个粒子包含：

• 位姿假设(x,y,θ)
• 地图概率网格（通常分辨率0.05m）
• 权重值（表示该粒子的可信度）

当新一帧激光数据到达时，算法会执行三个关键步骤：

1. 运动更新：根据预估位移分散粒子（无里程计时使用匀速模型）
2. 观测更新：计算每个粒子与当前扫描的匹配度
3. 重采样：淘汰低权重粒子，复制高权重粒子

python复制# 简化的粒子更新伪代码
for particle in particles:
    # 运动模型预测
    particle.pose += motion_model(last_pose, current_pose)
    
    # 计算扫描匹配得分
    particle.weight = scan_match(particle.map, laser_scan)
    
    # 地图更新
    update_map(particle.map, laser_scan, particle.pose)

2.2 参数调优实战经验

在gmapping.launch文件中，这几个参数对纯激光方案尤为关键：

xml复制<param name="maxUrange" value="8.0"/>  <!-- 最大有效测距距离 -->
<param name="sigma" value="0.05"/>     <!-- 高斯噪声参数 -->
<param name="kernelSize" value="1"/>   <!-- 搜索窗口大小 -->
<param name="lstep" value="0.05"/>     <!-- 平移优化步长 -->
<param name="astep" value="0.05"/>     <!-- 旋转优化步长 -->
<param name="iterations" value="5"/>   <!-- 优化迭代次数 -->

根据我的实测经验，在无里程计情况下建议：

• 将lstep和astep减小到0.02-0.03，提高匹配精度
• iterations增加到10，但会提升CPU占用
• maxUrange设为雷达实际有效距离的80%（避免噪声干扰）

3. laser_scan_matcher的替代方案

3.1 PLICP算法原理剖析

laser_scan_matcher采用的PLICP（Point-to-Line ICP）算法，比传统ICP更适合结构化环境。它的核心思想是将激光点匹配到特征线上，就像用乐高凸点对准基板凹槽。

算法流程分为三个阶段：

1. 特征提取：从扫描数据中识别线段、角点等特征
2. 对应点搜索：为当前扫描点寻找上一帧的最佳匹配线
3. 位姿优化：最小化点到线的距离函数

cpp复制// 简化的PLICP核心计算
for (int iter = 0; iter < max_iterations; ++iter) {
    // 建立点-线对应关系
    findCorrespondences(current_scan, reference_scan);
    
    // 计算变换矩阵
    Eigen::Matrix3f H = computeHessian(correspondences);
    Eigen::Vector3f dx = H.ldlt().solve(-computeGradient(correspondences));
    
    // 应用位姿更新
    current_pose = current_pose * transformFromDelta(dx);
}

3.2 安装避坑指南

原生的laser_scan_matcher在Noetic上编译可能会遇到这些问题：

1. Eigen3依赖问题：

bash复制sudo apt install libeigen3-dev

2. CSM库链接错误：
   需要手动修改CMakeLists.txt，添加：

cmake复制find_package(Eigen3 REQUIRED)
include_directories(${Eigen3_INCLUDE_DIRS})

我推荐使用改进版的ira_laser_tools，它对现代ROS支持更好：

bash复制git clone https://github.com/iralabdisco/ira_laser_tools.git

4. 完整建图实战流程

4.1 硬件连接检查

以思岚A1雷达为例，先确认设备权限：

bash复制ls -l /dev/ttyUSB*  # 查看设备节点
sudo chmod 666 /dev/ttyUSB0  # 设置权限

启动雷达测试：

bash复制roslaunch rplidar_ros view_rplidar.launch

正常应该看到类似雷达的扇形扫描可视化。

4.2 融合启动文件配置

创建gmapping_no_odom.launch文件：

xml复制<launch>
    <!-- 雷达驱动 -->
    <node pkg="rplidar_ros" type="rplidarNode" name="rplidar">
        <param name="serial_port" value="/dev/ttyUSB0"/>
        <param name="frame_id" value="laser"/>
    </node>

    <!-- laser_scan_matcher -->
    <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" 
          name="laser_scan_matcher" output="screen">
        <param name="use_odom" value="false"/>
        <param name="publy_pose" value="true"/>
        <param name="base_frame" value="base_link"/>
        <param name="max_iterations" value="10"/>
    </node>

    <!-- Gmapping -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping">
        <param name="base_frame" value="base_link"/>
        <param name="odom_frame" value="odom"/>
        <remap from="scan" to="/scan"/>
    </node>
</launch>

4.3 建图过程技巧

1. 运动控制建议：

   • 保持线性速度<0.5m/s
   • 角速度<0.3rad/s
   • 避免急停急转

2. 地图评估命令：

bash复制# 查看地图话题
rostopic echo /map

# 保存地图
rosrun map_server map_saver -f ~/mymap

3. 常见问题处理：

• 地图出现重影：尝试减小xmax参数
• 定位突然丢失：检查雷达数据是否中断
• 建图偏移严重：降低运动速度，增加particles数量

5. 性能优化与扩展思考

在树莓派4B上实测，这个方案的内存占用约500MB，CPU使用率在70%左右。如果资源紧张，可以尝试以下优化：

1. 降采样处理：

xml复制<node pkg="topic_tools" type="throttle" name="scan_throttle"
      args="messages /scan 5.0 /scan_throttled"/>

2. 使用更轻量的ICP实现：

bash复制git clone https://github.com/ethz-asl/libpointmatcher.git

3. 多雷达融合方案：
   对于复杂环境，可以配置多个雷达：

xml复制<node pkg="ira_laser_tools" type="laserscan_multi_merger" name="laser_merge">
    <param name="destination_frame" value="base_link"/>
    <param name="scan_destination_topic" value="/merged_scan"/>
    <param name="laserscan_topics" value="/front_scan /rear_scan"/>
</node>

这套方案虽然简单，但在2023年RoboMaster高校赛中，有队伍就用类似配置完成了室内对抗赛的自动导航。当硬件条件受限时，充分挖掘单一传感器的潜力，往往能收获意想不到的效果。