3、2D SLAM算法原理与实现：Gmapping算法详解、Cartographer算法入门、SLAM精度评估方法
各位同学，今天我们来啃一块硬骨头——2D SLAM。说实话，SLAM（同步定位与地图构建）是移动机器人最核心的能力之一。没有它，机器人就是个瞎子。我最早接触SLAM是在一个仓储机器人项目上，当时机器人走着走着就撞墙了，排查了半天，发现是激光雷达数据没对齐。嗯，从那以后，我对SLAM的敬畏心就上来了。
这一章，我们重点讲三个东西：Gmapping、Cartographer，以及怎么评估SLAM做得好不好。说白了，就是教你选算法、用算法、评算法。
3.1 Gmapping算法详解
Gmapping，全称是“Grid-based FastSLAM”。它基于粒子滤波，是目前ROS里最常用的2D SLAM方案之一。我个人习惯把它叫做“轻量级选手”——适合小场景、计算资源有限的情况。
3.1.1 核心思想
Gmapping的核心是粒子滤波。每个粒子代表一个可能的机器人轨迹和地图。随着机器人移动，粒子会不断更新权重，权重高的粒子存活，权重低的被淘汰。这就是“重采样”。
你想想看，如果粒子数太少，机器人容易“迷路”；粒子数太多，计算量又扛不住。Gmapping的巧妙之处在于，它引入了提议分布，利用激光雷达的观测数据来指导粒子的生成方向。这样，同样的粒子数，定位精度能提升不少。
关键参数：
particles：粒子数，默认30。场景越大，粒子数要适当增加。我一般设50~80。linearUpdate：机器人平移多少米触发一次更新。默认0.5米。angularUpdate：旋转多少弧度触发更新。默认0.5弧度。resampleThreshold：重采样阈值，默认0.5。低于这个值才重采样，避免频繁重采样导致粒子退化。
3.1.2 代码实战：启动Gmapping
在ROS里启动Gmapping非常简单。你只需要一个激光雷达话题（比如/scan）和里程计话题（比如/odom）。
rosrun gmapping slam_gmapping scan:=/scan _particles:=50 _linearUpdate:=0.3 _angularUpdate:=0.2这里我调整了参数：粒子数50，更新频率更高。为什么？因为我在一个走廊场景测试，转弯多，更新慢了容易丢位置。你想想看，机器人拐个弯，如果更新间隔太大，地图就歪了。
避坑指南：我曾经在瓷砖地面上跑Gmapping，结果地图一直漂移。后来发现是轮子打滑导致里程计不准。解决办法：要么加IMU数据，要么降低linearUpdate，让激光雷达主导定位。
3.2 Cartographer算法入门
Cartographer是Google开源的SLAM库。它比Gmapping重，但精度更高，尤其适合大场景。我最早接触Cartographer是在一个工厂巡检项目上，厂房有几千平米，Gmapping跑着跑着就崩了，换成Cartographer才稳住。
3.2.1 核心思想
Cartographer的核心是图优化。它把机器人的位姿和激光扫描数据当作图的节点，节点之间的约束当作边。然后通过最小化误差来优化整个图。
说白了，Gmapping是“边走边猜”，Cartographer是“先记下来，回头再算”。Cartographer会维护一个子图（submap），每插入几帧激光数据就生成一个子图。当检测到闭环时，它会全局优化，把累积误差一次性消除。
关键参数：
num_range_data：每个子图包含多少帧激光数据。默认70。场景大可以设到100。global_sampling_ratio：全局优化时的采样比例。默认0.003。调高会提高精度，但计算量暴增。use_odometry：是否使用里程计。如果轮子打滑严重，建议关掉，只用IMU+激光。
3.2.2 代码实战：启动Cartographer
Cartographer的启动比Gmapping复杂一些，需要配置.lua文件。下面是一个简化版的配置：
-- 文件：my_robot.lua
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = true,
use_odometry = true,
num_laser_scans = 1,
num_subdivisions_per_laser_scan = 1,
}
TRAJECTORY_BUILDER_2D.num_range_data = 70
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 30.0启动命令：
roslaunch cartographer_ros my_robot.launch注意：Cartographer对CPU要求较高。我建议至少4核以上，否则实时性会出问题。另外，如果激光雷达帧率低于10Hz，记得调低num_range_data，否则子图更新太慢。
3.3 SLAM精度评估方法
算法选好了，跑起来了，怎么知道它准不准？我见过不少同学，地图看着挺漂亮，一测误差，直接傻眼。所以，评估这一步不能省。
3.3.1 常用指标
| 指标 | 全称 | 说明 |
|---|---|---|
| ATE | Absolute Trajectory Error | 估计轨迹与真值轨迹的偏差。单位：米。 |
| RPE | Relative Pose Error | 相邻时间步之间的相对位姿误差。单位：米/秒。 |
| RMSE | Root Mean Square Error | 均方根误差，综合反映整体精度。 |
我个人习惯先看ATE，因为它最直观。如果ATE小于0.1米，说明SLAM精度不错。如果大于0.5米，那就要排查问题了。
3.3.2 评估工具：evo
evo是SLAM领域最常用的评估工具。它支持TUM、KITTI、EuRoC等多种数据格式。用法很简单：
# 安装
pip install evo
# 评估ATE
evo_ape tum groundtruth.txt estimated.txt -va --plot
# 评估RPE
evo_rpe tum groundtruth.txt estimated.txt -va --plot这里groundtruth.txt是真值轨迹，estimated.txt是SLAM估计的轨迹。evo会自动对齐时间戳，计算误差并画图。
避坑指南：我曾经用evo评估时，发现ATE很大，但RPE很小。这说明轨迹整体漂移了，但局部相对关系是对的。这种情况通常是里程计标定不准，或者闭环没检测到。解决办法：重新标定里程计，或者调整Cartographer的闭环检测参数。
3.3.3 实战建议
- 先跑仿真：在Gazebo里用已知地图测试，这样真值轨迹是确定的。
- 多场景测试：走廊、开阔地、有回环的场景，各跑一遍。
- 记录日志：每次实验都保存轨迹和参数，方便复盘。
知识体系总览
下面这张图，是我自己画的本章知识结构。你可以把它当作一张“地图”，随时回来对照。
好了，这一章的内容就到这里。Gmapping和Cartographer各有千秋，选哪个取决于你的场景和硬件。精度评估是检验真理的唯一标准，别偷懒。下一章，我们会聊点更刺激的——3D SLAM。嗯，到时候见。