1. 从零理解激光雷达SLAM里的扫描匹配到底在做什么

很多人第一次接触激光雷达SLAM，脑子里冒出来的第一个问题往往是"建图到底是怎么建出来的"。我刚开始做这块的时候也一样，看着rviz里那些点云慢慢拼成一张地图，觉得挺神奇，但拆开来看，核心其实就一件事： 把当前这一帧激光扫描数据，准确地塞到已经建好的地图坐标系里去 。这个"塞"的过程，就是扫描匹配（Scan Matching）。

激光雷达每一帧扫出来的是一圈或者多圈距离数据，比如常见的16线雷达，一帧下来就是几万个点。这些点是在雷达自身坐标系下的，雷达装在机器人上，机器人又在动，所以每一帧的点云坐标系都不一样。如果我们不做任何处理，直接把所有帧的点云堆在一起，得到的会是一团糊掉的乱麻，因为每帧的参考系都在漂。扫描匹配要解决的就是：给定上一帧到这一帧之间机器人的大致运动（通常由轮式里程计或者IMU给出一个初值），然后通过优化算法，找到一个最优的位姿变换（平移+旋转），使得当前帧点云和已有地图（或者上一帧点云）的重合度最高。

为什么这件事这么关键？因为SLAM的精度几乎完全取决于位姿估计的精度。位姿估计差一点点，几十帧累积下来地图就会歪得不成样子，回环检测再厉害也救不回来。而扫描匹配就是位姿估计里最核心的那一环。你可以把它理解成拼图：每帧点云是一块拼图，扫描匹配就是找到这块拼图应该放在桌面的哪个位置、转多少角度，才能和已经拼好的部分严丝合缝。

我这次要聊的，是用Matlab实现一个 基本的扫描匹配算法 ，并且用 贪心算法 来做位姿优化。为什么选贪心？因为扫描匹配本质上是一个非线性优化问题，标准做法是高斯-牛顿或者LM（Levenberg-Marquardt），但这些方法需要计算雅可比矩阵，推导和实现都有一定门槛。贪心算法的思路非常朴素：在当前位姿附近撒一批候选位姿，逐个评估匹配得分，挑最好的那个，然后缩小搜索范围再来一轮，直到收敛。它不需要求导，实现简单，对于理解扫描匹配的本质特别友好，适合作为入门的第一版实现。

这篇文章适合谁看？如果你已经学过SLAM的基础理论，知道什么是位姿、什么是点云配准，但还没自己动手写过完整的扫描匹配代码，那这篇就是给你准备的。如果你是完全的小白，也没关系，我会把每个环节的原理和为什么这么做都讲清楚。代码用Matlab写，不需要额外的工具箱，基础版本就能跑。

提示：本文的贪心搜索策略是一种"暴力+迭代细化"的思路，工业级SLAM系统里很少直接用，但它是理解更复杂优化算法的绝佳跳板。先把这版跑通，再去看CSM（Correlative Scan Matching）或者NDT，会顺畅很多。

2. 扫描匹配的数学本质与贪心优化的切入点

2.1 位姿变换的数学表达：为什么是三个自由度

在二维平面SLAM里，机器人的位姿用一个三维向量表示： [x, y, θ] 。x和y是平移，θ是旋转角。为什么是三个自由度而不是六个？因为假设机器人在地面上跑，不会飞也不会翻，z轴、俯仰角、横滚角都固定。这个假设在室内轮式机器人上基本成立，所以二维SLAM至今仍是主流。

给定一个位姿 T = [x, y, θ] ，把雷达坐标系下的一个点 p = [px, py] 变换到世界坐标系，公式是：

p_world_x = x + px * cos(θ) - py * sin(θ)
p_world_y = y + px * sin(θ) + py * cos(θ)

这就是标准的二维刚体变换。写成矩阵形式就是齐次变换矩阵，但Matlab里直接按上面两行算反而更快，因为避免了矩阵乘法的开销。我实测过，对于几万个点的点云，逐点用三角函数算比构造变换矩阵再乘要快不少，尤其是当你要在贪心搜索里反复评估成千上万个候选位姿的时候，这个优化很值得。

2.2 匹配得分函数：怎么判断"匹配得好不好"

扫描匹配需要一个量化指标来评价某个位姿下当前帧和地图的重合程度。最常用的有两种思路：

第一种是 点到点距离 。把当前帧每个点变换到世界坐标系，然后找地图里离它最近的点，累加所有最近邻距离的平方。距离越小，匹配越好。这个思路直观，但需要做最近邻搜索，计算量大。

第二种是 占据栅格得分 。把地图表示成栅格，每个格子有一个占据概率。当前帧的点变换后落在哪个格子，就把那个格子的得分加起来。落在高占据概率格子上的点越多，总分越高。这个方法不需要最近邻搜索，查表就行，速度快得多。

我这里用的是第二种思路的简化版： 双线性插值得分 。地图是一个二维数组，值在0到1之间，1表示确定有障碍物，0表示空闲。对于变换后的点，不直接取整查表，而是在四个相邻格子上做双线性插值。为什么要插值？因为如果直接取整，位姿稍微变一点点，点可能落到同一个格子里，得分不变，梯度就消失了，贪心搜索会卡住。双线性插值让得分随位姿连续变化，搜索才能有方向感。

得分函数的具体形式：

score = Σ bilinear_interp(map, p_world_x, p_world_y)

把所有点的插值得分加起来，再除以点数做归一化。得分越高越好，理论上完美匹配时每个点都落在占据格子上，得分接近1。

2.3 贪心搜索为什么能替代梯度下降

标准优化方法需要计算得分对 [x, y, θ] 的偏导数，然后沿着梯度方向走。但得分函数里有双线性插值和查表操作，解析求导很麻烦，数值求导又要额外评估好几次函数，计算量翻倍。

贪心算法的逻辑完全不同：我不求导，我在当前位姿周围直接撒一批候选点，比如x方向±0.1米、y方向±0.1米、θ方向±1度，每个维度取几个离散值，组合起来就是一批候选位姿。逐个算得分，挑最高的那个，把当前位置挪过去，然后缩小搜索步长，再来一轮。重复几轮之后，步长小到一定程度就停。

这个思路的本质是 坐标下降+多分辨率搜索 。每一轮只在离散的候选里找最优，虽然不能保证全局最优，但只要初始位姿离真值不太远（里程计通常能做到），就能收敛到局部最优，而且这个局部最优通常就是我们要的。

为什么叫贪心？因为每一轮都选当前看起来最好的那个，不考虑长远。这确实有陷入局部最优的风险，但在扫描匹配这个场景下，得分函数的局部极值通常比较"浅"，只要初始值靠谱，贪心就能稳定收敛。我试过在初始误差0.3米、5度以内的情况下，基本都能收敛到正确位姿。

注意：贪心搜索的收敛性高度依赖初始位姿的质量。如果你的里程计漂移很大，或者机器人运动很剧烈，建议先用IMU或者视觉做粗配准，再交给扫描匹配做精配准。

3. 用Matlab把算法一步步搭起来

3.1 数据准备：没有真实雷达怎么办

如果你手头有速腾16线或者思岚S2这类雷达，可以直接录rosbag然后导出成mat文件。但大多数人刚开始学的时候没有硬件，所以我们需要一个仿真数据生成器。思路很简单：造一个虚拟的二维环境，里面放一些墙壁和障碍物，然后模拟一个机器人在里面走，每个位置生成一帧激光扫描。

具体做法：定义一个 occupancyMap ，用 binaryOccupancyMap 或者自己用矩阵画一个地图，1表示墙，0表示空地。然后模拟激光射线，从机器人位置向360个方向发射射线，用射线与地图格子的交点计算距离。Matlab的 raycast 函数可以直接做这件事，但为了理解原理，我建议自己写一个简单的射线投射，用Bresenham直线算法或者步进法都行。

function ranges = simulateLidar(map, pose, num_beams, max_range)
    % map: 二值占据栅格
    % pose: [x, y, theta]
    % 返回每个beam的距离
    angles = linspace(-pi, pi, num_beams);
    ranges = zeros(1, num_beams);
    for i = 1:num_beams
        a = pose(3) + angles(i);
        % 步进法找第一个占据格子
        step = 0.02;
        r = 0;
        while r < max_range
            r = r + step;
            px = pose(1) + r * cos(a);
            py = pose(2) + r * sin(a);
            if ~inBounds(map, px, py) || map(round(py), round(px)) == 1
                break;
            end
        end
        ranges(i) = r;
    end
end

这段代码虽然效率不高，但逻辑清晰，适合理解。实际跑的时候可以把步长调大一点，或者用向量化操作加速。生成数据的时候，让机器人沿着一条预设轨迹走，每个位置存一帧扫描和对应的真实位姿，这样后面评估算法精度的时候就有基准了。

3.2 地图的栅格化表示与得分查表

地图用二维矩阵存，每个元素是0到1之间的浮点数。初始时全0，每来一帧扫描，把点变换到世界坐标系后，在对应格子上累加一个值，最后归一化。但更常见的做法是维护一个对数占据概率，用 log-odds 更新，避免数值不稳定。

对于得分查表，我们需要一个函数，输入世界坐标 (x, y) ，输出插值后的占据值。Matlab里矩阵索引是从1开始的，所以要注意坐标转换。假设地图分辨率是 res 米/格，地图原点在世界坐标 (origin_x, origin_y) ，那么：

function val = getOccupancy(map, x, y, res, origin)
    col = (x - origin(1)) / res + 1;
    row = (y - origin(2)) / res + 1;
    % 双线性插值
    r0 = floor(row); c0 = floor(col);
    dr = row - r0; dc = col - c0;
    if r0 < 1 || c0 < 1 || r0+1 > size(map,1) || c0+1 > size(map,2)
        val = 0;
        return;
    end
    val = (1-dr)*(1-dc)*map(r0,c0) + (1-dr)*dc*map(r0,c0+1) + ...
          dr*(1-dc)*map(r0+1,c0) + dr*dc*map(r0+1,c0+1);
end

这个函数会被调用几万次甚至几十万次，所以一定要写成向量化版本。Matlab的循环很慢，但矩阵操作很快。我的做法是把一帧所有点先变换到世界坐标，得到一个N×2的矩阵，然后一次性算所有点的插值值，用 sub2ind 或者直接构造索引。实测下来，向量化版本比循环版本快50倍以上。

3.3 贪心搜索的完整实现框架

贪心搜索的主循环结构如下：

function [best_pose, best_score] = greedyScanMatch(map, points, init_pose, res, origin)
    pose = init_pose;
    step = [0.2, 0.2, deg2rad(5)];  % 初始步长
    min_step = [0.01, 0.01, deg2rad(0.2)];  % 最小步长
    shrink = 0.5;  % 每轮缩小比例
    
    while any(step > min_step)
        best_local_score = -inf;
        best_local_pose = pose;
        % 在三个维度上撒候选
        for dx = -1:1
            for dy = -1:1
                for dth = -1:1
                    if dx==0 && dy==0 && dth==0
                        continue;
                    end
                    candidate = pose + [dx*step(1), dy*step(2), dth*step(3)];
                    s = evaluateScore(map, points, candidate, res, origin);
                    if s > best_local_score
                        best_local_score = s;
                        best_local_pose = candidate;
                    end
                end
            end
        end
        if best_local_score > evaluateScore(map, points, pose, res, origin)
            pose = best_local_pose;
        end
        step = step * shrink;
    end
    best_pose = pose;
    best_score = evaluateScore(map, points, pose, res, origin);
end

这个框架里，每一轮在26个方向（3×3×3减1）上试探，选最好的，然后步长减半。为什么用3×3×3而不是更多？因为候选太多计算量爆炸，太少又容易错过方向。3×3×3是一个经验上的平衡点，每轮26次评估，通常10到15轮收敛，总共也就几百次评估，对于一帧几千个点的数据，Matlab里跑一次大概几十毫秒，勉强能接受。

evaluateScore 函数就是前面说的双线性插值得分，把所有点变换后查表累加。这里有个细节：点云里有些点可能是无效的（超出量程或者噪声），这些点要提前过滤掉，否则会拉低得分，干扰搜索方向。

提示：初始步长的选择很关键。如果里程计比较准，初始步长可以小一点，比如0.1米、2度，收敛更快。如果里程计漂移大，步长要放大，但太大又容易跳过真值。我的经验是初始步长设为预期误差的1.5倍左右比较稳妥。

4. 实测中那些文档不会告诉你的坑

4.1 得分函数的"平台效应"与步长选择

我第一次跑通代码的时候，发现算法经常卡在某个位姿不动，得分也不涨。排查了半天，发现是得分函数存在"平台效应"：当所有点都落在同一个格子里时，位姿在小范围内变化，插值得分几乎不变，贪心搜索就失去了方向。

解决办法有两个。一是提高地图分辨率，让格子更小，平台区域缩小。但分辨率太高会导致地图稀疏，很多格子没有值，得分反而下降。二是改用 多分辨率地图 ，先用粗地图搜一个大范围，再用细地图精调。这个思路和图像金字塔很像，效果很好，但实现复杂度上去了。

我最后的做法是折中：地图分辨率用0.05米，同时在得分函数里加一个 距离变换 的惩罚项。具体来说，预先计算地图的距离变换，每个格子存到最近障碍物的距离。得分函数变成占据得分减去距离惩罚。这样即使点落在空地上，离障碍物越近得分越高，梯度就出来了。

4.2 角度维度的周期性陷阱

角度θ是周期性的，-π和π是同一个方向。贪心搜索的时候，如果当前角度接近π，候选角度可能跳到-π附近，数值上差了很多，但实际上只差一点点。如果不处理，搜索会乱跳。

处理办法是在计算角度差的时候做归一化：

function d = angleDiff(a, b)
    d = a - b;
    while d > pi
        d = d - 2*pi;
    end
    while d < -pi
        d = d + 2*pi;
    end
end

或者在更新位姿后直接把θ归一化到[-π, π]。这个坑很隐蔽，因为大部分时候角度不会接近边界，但一旦接近，算法就会莫名其妙地发散。我当初调了整整一个下午才发现这个问题，血泪教训。

4.3 点云稀疏区域的匹配退化

激光雷达在长走廊或者空旷区域，扫描到的点很少，而且分布很集中。这种情况下，扫描匹配会出现 退化 ：沿着走廊方向平移，得分几乎不变，因为所有点都在墙上，平移不改变重合度。算法会在这个方向上随机漂移。

这是扫描匹配的固有问题，不是贪心算法独有的。解决办法是引入其他传感器约束，比如轮式里程计在平移方向上的观测，或者用IMU约束旋转。如果只有激光，那就只能靠回环检测来纠正累积误差。我在代码里加了一个简单的处理：如果某个方向上的得分变化小于阈值，就认为该方向不可观测，保持里程计给出的值不变。

4.4 Matlab性能优化的几个实用技巧

Matlab跑SLAM最大的问题就是慢。我总结了几条实测有效的优化：

第一， 预分配所有数组 。循环里不要动态增长数组，提前用 zeros 分配好。这一条能省30%以上的时间。

第二， 向量化点云变换 。不要用for循环逐点算，把点云存成N×2矩阵，用矩阵运算一次性变换。三角函数也只算一次，因为同一帧所有点的θ是一样的。

第三， 用 single 类型代替 double 。地图和点云用单精度浮点，内存减半，速度提升明显，精度损失可以忽略。

第四， 避免在循环里调用函数 。Matlab的函数调用开销不小，把得分计算内联到主循环里，或者用 inline 函数。我实测内联后快了将近一倍。

第五， 用 mex 编译热点代码 。如果实在嫌慢，把得分计算写成C mex文件，速度能提升10倍以上。但这需要配置编译器，入门阶段可以先不做。

优化手段	预期加速比	实现难度
预分配数组	1.3x	低
向量化变换	5-10x	中
single类型	1.5x	低
内联函数	1.8x	中
mex编译	10x+	高

5. 从贪心到更实用的扫描匹配：下一步怎么走

5.1 贪心搜索的局限与改进方向

贪心搜索最大的问题是 计算量随精度要求指数增长 。步长减半，轮数增加，每轮评估次数不变，但总轮数是对数关系，所以还好。真正的问题是三维搜索空间，如果扩展到三维SLAM（x, y, z, roll, pitch, yaw），候选数量爆炸，贪心就不可行了。

改进方向有几个。一是 分支定界 ，把搜索空间组织成树，用得分上界剪枝，避免遍历所有候选。这个思路在CSM（Correlative Scan Matching）里用得很多，效率比纯贪心高一个数量级。二是 多分辨率 ，粗分辨率快速定位大致区域，细分辨率精调，减少细搜索的范围。三是 梯度近似 ，用有限差分代替解析梯度，然后做梯度上升，收敛更快但需要调学习率。

我个人的建议是：先把贪心版本跑通，理解每个环节，然后直接上CSM。CSM的核心思想就是分支定界+多分辨率，代码量比贪心大不了多少，但性能好很多，而且有开源实现可以参考。

5.2 和主流SLAM框架的衔接

如果你用的是ROS， slam_toolbox 或者 cartographer 都内置了扫描匹配，不需要自己写。但理解了自己写的版本，再看这些框架的配置文件，就知道每个参数在调什么。比如 slam_toolbox 里的 correlation_search_space_dimension 就是搜索窗口大小， correlation_search_space_resolution 就是搜索步长， correlation_search_space_smear_deviation 就是高斯平滑的方差。这些参数调起来心里就有底了。

如果你用Matlab的Navigation Toolbox，里面有 matchScans 函数，可以直接做扫描匹配。但它是黑盒，你不知道里面怎么实现的。自己写一遍之后，再用 matchScans ，就能对比两者的结果，理解工程实现和教学实现的差异。

5.3 实测效果与精度评估

我在仿真环境里跑了100帧数据，机器人走了一个矩形轨迹，总长度约20米。用贪心扫描匹配，最终轨迹的终点误差在0.15米左右，角度误差2度以内。这个精度对于入门实现来说已经不错了，但和工业级SLAM比还有差距，主要差距在累积误差上。

评估方法很简单：把估计轨迹和真实轨迹画在一起，看偏差。Matlab里用 plot 就行。另外可以算ATE（绝对轨迹误差），就是每个位姿和真值的欧氏距离的均方根。我的实现ATE大概是0.08米，对于20米的轨迹，相对误差0.4%，算是及格。

如果要进一步提升精度，可以加 回环检测 。当机器人回到之前走过的地方，检测到相似场景，就触发一次全局优化，把累积误差摊掉。回环检测可以用简单的描述子，比如扫描的傅里叶变换或者直方图，匹配上了就加约束。这部分内容展开又是一大篇，这里就不细说了。

提示：评估SLAM精度的时候，一定要用独立的测试数据，不要用调参用的数据。我见过有人在同一组数据上反复调参，最后精度很高，但换一组数据就崩了。这是典型的过拟合。

5.4 代码组织与复用建议

最后聊一下代码组织。我建议把整个项目分成几个模块：数据生成、地图管理、得分计算、贪心搜索、可视化、评估。每个模块一个函数文件，主脚本只负责调用。这样调试的时候可以单独测试每个模块，出问题了好定位。

Matlab的OOP也可以用来组织，定义一个 ScanMatcher 类，把地图、参数、方法都封装进去。但OOP在Matlab里性能有损耗，方法调用比函数调用慢。如果追求速度，还是用函数式。如果追求代码清晰，OOP更好。看你的取舍。

我自己的做法是混合：核心计算用函数，外层用结构体传参。这样既清晰又不损失性能。代码放在Git上，每次改完跑一遍回归测试，确保没有引入新bug。这个习惯在调SLAM的时候特别重要，因为参数多，改一个可能影响另一个，没有回归测试很容易越调越乱。

整个项目从零到跑通，我大概花了三天时间，其中两天在调bug和优化性能。如果你跟着这个思路走，应该能更快。关键是要理解每一步为什么这么做，而不是照抄代码。理解了原理，遇到问题才能自己排查。