4. 2D激光SLAM原理：粒子滤波与Gmapping、扫描匹配、栅格地图构建与参数调优
好，咱们今天聊点硬核的。2D激光SLAM，说白了就是让机器人一边走路一边画地图。你想想看，一个扫地机器人，它怎么知道哪里扫过了、哪里还没扫？靠的就是这套技术。
我个人习惯把2D激光SLAM拆成三个核心模块来看：状态估计（我在哪？）、扫描匹配（我看到了什么？）、地图更新（怎么把看到的东西记下来？）。Gmapping就是这三者的集大成者。
核心逻辑：粒子滤波负责猜位置，扫描匹配负责修正位置，栅格地图负责记录环境。三者循环迭代，就是SLAM。
4.1 粒子滤波：一群"猜位置"的小人
粒子滤波，你可以把它想象成一群小人在房间里猜自己的位置。每个小人（粒子）都带着一个"我觉得我在哪"的猜测，以及一个"我猜得有多准"的权重。
我在项目中遇到过一个问题：粒子数设得太少，机器人拐个弯就把所有粒子都丢了。设得太多，又跑不动。后来我总结了一个经验——粒子数不是越多越好，够用就行。
我的经验：对于普通室内环境（50-100平米），30个粒子就够用了。如果是长廊或者对称环境，建议加到80个。再多？说实话，边际效益递减得很厉害。
粒子滤波的核心流程就三步：
- 预测——根据里程计数据，让每个粒子往前挪一步，加点噪声
- 更新——用激光扫描数据，给每个粒子打分，权重高的留下
- 重采样——扔掉权重低的粒子，复制权重高的粒子
嗯，这里要注意：重采样这一步很容易出问题。我曾经见过一个团队，重采样做得太频繁，结果所有粒子都挤到同一个位置去了，这叫"粒子耗尽"。解决办法是加一个有效粒子数阈值，只有当粒子多样性下降到一定程度时才触发重采样。
4.2 扫描匹配：让激光数据"对齐"
扫描匹配，说白了就是回答一个问题：我当前看到的这帧激光数据，和之前看到的那帧，到底差了多少距离和角度？
常用的方法有两种：ICP和CSM。我分别说说。
4.2.1 ICP（迭代最近点）
ICP的思路很直接：找两帧激光点之间的对应关系，然后算一个旋转平移矩阵，让它们尽量重合。迭代几次，直到收敛。
// 伪代码：ICP核心逻辑
while (未收敛) {
1. 对每个源点，找最近的目标点（最近邻搜索）
2. 计算最优旋转R和平移t（最小二乘）
3. 应用变换，计算误差
4. 如果误差变化小于阈值，跳出
}ICP的问题在于——它太依赖初始值了。如果两帧数据差得远，它很容易掉进局部最优。我踩过这个坑：有一次在长廊里跑，ICP直接把地图匹配歪了，因为长廊两边都是墙，特征太相似。
避坑指南：ICP不适合大角度旋转的场景。如果你的机器人经常原地掉头，建议用CSM或者加一个IMU辅助。
4.2.2 CSM（相关扫描匹配）
CSM是Gmapping里用的方法。它不直接匹配点云，而是把激光数据投影到一个似然域上，然后暴力搜索最优位姿。
你想想看，CSM的做法其实很"笨"——它把可能的位姿都试一遍，看哪个得分最高。但正因为暴力，它不容易陷入局部最优。代价就是计算量大，所以Gmapping里用了多分辨率搜索来加速。
我记得有一次调试，发现CSM的搜索步长设得太粗，导致定位精度不够。后来把角度步长从1度改成0.5度，效果立竿见影。当然，计算量也翻倍了——这就是个trade-off。
4.3 栅格地图构建：把世界画成小格子
栅格地图，就是把环境划分成一个个小方格，每个格子存一个概率值，表示"这个格子有障碍物的可能性"。0表示肯定空，1表示肯定占，0.5表示未知。
更新栅格地图用的是贝叶斯更新。每次来一帧激光数据，就沿着激光射线走一遍：
- 射线穿过的格子 → 降低占用概率（表示空）
- 射线终点附近的格子 → 提高占用概率（表示有障碍物）
这里有个细节：画射线用的是Bresenham算法，就是计算机图形学里画直线那个。我刚开始做的时候没注意，直接用浮点数算射线路径，结果地图上全是锯齿。后来换成Bresenham，问题就解决了。
地图分辨率怎么选？ 5cm/格是室内场景的黄金选择。太细（1cm）的话，地图文件大、更新慢；太粗（20cm）的话，门缝都识别不出来。
4.4 Gmapping参数调优实战
Gmapping是ROS里最经典的2D激光SLAM算法。它把粒子滤波和扫描匹配结合得很好。但说实话，默认参数往往不是最优的。我调过几十次Gmapping的参数，下面是我的心得。
| 参数名 | 默认值 | 我的推荐 | 说明 |
|---|---|---|---|
| particles | 30 | 30~80 | 环境对称或长廊用80，普通房间30足够 |
| delta | 0.5 | 0.3~0.6 | 采样步长（米），越小越精细但计算量大 |
| lsigma | 0.05 | 0.05~0.1 | 激光似然域的高斯标准差，越大对噪声越鲁棒 |
| ogain | 3.0 | 3.0~5.0 | 似然增益，影响重采样频率 |
| lskip | 0 | 0~2 | 跳过的激光束数，用于降采样加速 |
| srr/srt | 0.01/0.02 | 视里程计噪声而定 | 里程计噪声模型参数，需要标定 |
调参的时候，我建议按这个顺序来：
- 先调粒子数——太少会丢位姿，太多跑不动
- 再调采样步长——delta设太大，地图会飘；设太小，计算量爆炸
- 最后调噪声参数——srr/srt需要根据你的机器人底盘来标定
一个小技巧：如果你发现地图有重影（同一个墙出现两条线），大概率是里程计噪声参数设得太小了。把srr和srt调大一点，让粒子滤波更"信任"激光数据而不是里程计。
我曾经在一个工厂项目里调Gmapping，环境是那种特别空旷的大车间，激光打到远处墙上反射很弱。一开始地图建出来全是洞。后来我把lskip设为1（跳掉一半激光束），同时把lsigma从0.05调到0.08，地图质量明显提升。为什么？因为跳掉一些激光束减少了噪声干扰，而增大lsigma让似然域更平滑，不容易被异常点带偏。
嗯，最后说一句：参数调优没有银弹。同样的参数，换个环境可能就不好使了。我的习惯是——先跑一遍默认参数看看效果，然后根据地图上的具体问题（重影、漂移、空洞）去针对性调整。每次只改一个参数，改完跑一遍bag，对比效果。这样虽然慢，但心里有底。
好了，这一章的内容就到这。粒子滤波、扫描匹配、栅格地图、Gmapping调参——这四个东西串起来，就是2D激光SLAM的完整链路。你动手试试，遇到问题再回来翻翻这一章。