做机器人定位建图的朋友，大概率都遇到过这个场景：手里没有轮式里程计，底盘要么是四足要么是无人机，轮子还会打滑。这时候很多人会想到 Hector SLAM——它不需要里程计，单靠激光雷达就能在二维栅格地图上把位置估计出来。我最早接触这套代码是在一个足式底盘上，当时就好奇：为什么它不依赖里程计还能这么稳？后来把代码和论文对着啃了一遍，才发现核心就藏在两个地方：连续栅格地图的双线性插值，以及基于高斯牛顿法的扫描匹配。这篇文章就写一下我从代码里读出来的推导过程，重点讲清楚几个公式是怎么来的、对应到代码里哪个函数、参数一改会发生什么。

Hector 适合谁：手里有一套激光雷达数据，想在无里程计情况下快速建图的人；或者已经在用 Cartographer、Gmapping，但想理解“扫描匹配到底怎么迭代”的人。这篇不会泛泛讲概念，而是直接对着源码里的核心逻辑做推导，能让你在下次调参时知道自己在调什么。

1. Hector代码的整体骨架：别看它小，模块划分很清晰

1.1 代码里到底分了哪几块

Hector 的工程结构不大，但模块划分很规整。整体上可以分成三块：节点入口、地图管理、扫描匹配。很多人刚开始看代码，容易被.h和.cpp的数量搞得晕，实际上核心只有三类东西。

第一是传感器数据接入。节点订阅/scan话题，把激光数据转成内部点云格式。这一步不是简单的copy，它会把极坐标转成笛卡尔坐标，去掉无效距离，按雷达的安装外参做一次坐标变换。代码里对应的类是DataContainer，它的作用就是统一点云表示，并给扫描匹配提供“这一帧原始点在哪”的输入。

第二是地图管理层。Hector 支持多分辨率地图，也就是同一张环境地图存了好几层，每一层的分辨率逐级变粗。地图更新、查询、缩放都在这一层完成。代码里对应的是MapManager和OccGridMapUtil，它们负责把世界坐标换算成栅格坐标，并向上层提供“某个位置的地图值是多少”“这个位置的梯度是多少”的接口。

第三是扫描匹配层。这是整个算法的核心，写出estimateTransformation的地方。它接收上一帧推算出的初始位姿，以及当前帧的激光点云，在接口上构造一个最小二乘问题，再用高斯牛顿法迭代求解位姿变化量。Hector 之所以能火这么多年，靠的就是这一层的数学处理和工程化细节。

这三块各司其职，数据流是单向的：雷达原始数据进DataContainer，请求地图值时走MapManager的查询接口，最终位姿由扫描匹配器ScanMatcher输出。

1.2 为什么选栅格地图而不是ICP点云匹配

一个直觉方案是把上一帧点云和当前帧点云直接做 ICP，找刚体变换。但这个方案在真实环境里不太实用，因为激光点云是稀疏的，连续两帧之间的对应点无法直接建立，而且每帧的点云数量可能只有几百到几千，迭代过程中容易陷入局部极小。

Hector 走的是另一条路：把地图表示成占据栅格，然后用“当前点云投影到地图上后，地图值的总和”作为匹配质量的度量。这相当于把稀疏点云匹配问题转成了连续函数优化问题。地图栅格本身有连续性，即使离散分辨率下，也能通过插值得到平滑的梯度，这对高斯牛顿优化至关重要。

这个选择也解释了 Hector 为什么能实时跑。因为每帧激光做完投影后，只需要查一次地图值，求一次插值梯度，不需要额外建立点云特征描述子，也不需要维护庞大的协方差矩阵，计算量基本是线性的。实测下来，普通笔记本跑 10Hz 雷达毫无压力。

2. 栅格地图连续化：没有双线性插值，后面全白搭

2.1 占据栅格地图用数值表示什么

先明确一个基础概念：Hector 的地图里每个栅格存的是一个浮点数，范围通常在 0 到 1 之间，数值越接近 1 表示这个单元格越可能被占据，越接近 0 表示越可能是空闲。它不像某些导航地图那样直接存“占用概率”和“未知状态”，而是用一个连续标量场。

地图更新的规则很简单。激光射线经过的栅格，数值减小，表示“这里被扫过，没有障碍”；激光点末端命中的栅格，数值增大，表示“激光打到了障碍”。更新公式用对数几率形式可以写成：

更新之后栅格值衰减到 0.5 左右表示不确定。

这个值域对后续优化很有用，因为优化目标里会用到“1 - 地图值”这种残差，地图值接近 1 意味着残差接近 0，这刚好表达了“如果激光点落在已建图的障碍物上，这一帧的位姿就应该是对的”。

2.2 离散地图如何变成可求导的连续函数

离散栅格最大的问题是不可导。想象一下，一个点在栅格坐标 (10.3, 8.6) 处，它的“地图值”如果直接用最近栅格(10, 8)的值来代表，那么位姿稍微变一点点，取整结果跳变一下，优化函数就会产生一个台阶，梯度没法用，高斯牛顿直接崩。

Hector 的解决办法是双线性插值。对任意连续坐标 (x, y)，取它周围的四个栅格点，按距离权重组合成一个连续的地图值。公式如下：

其中 是 x 方向的小数部分，是 y 方向的小数部分，四个邻居分别是(x0, y0),(x0+1, y0),(x0, y0+1),(x0+1, y0+1)。

这里的关键不只是求值，而是求梯度。优化过程需要知道“地图值对坐标的偏导”，这个偏导直接从插值公式里推出来：

需要注意的是，梯度公式里乘以的除了相邻栅格的差值，还有分辨率换算。因为代码里坐标转换时会把世界坐标除以map_resolution得到栅格坐标，求导链式法则里多一个1/resolution因子。很多人在看代码时发现最后的雅可比矩阵里除了解析度还乘了个系数，就是这一步没对上。

2.3 代码中地图查询接口的对应关系

在OccGridMapUtil接口里，getMapValue负责返回插值后的地图值，getMapGradient负责返回梯度。具体实现就是把上一小节的两个公式翻译成代码，注意处理栅格坐标越界的情况。越界时一般直接返回 0，因为地图外的点没有占据信息，残差会鼓励它们“往地图里靠”。

这也是为什么 Hector 在开放式大场地里容易飘逸的原因之一：当大片激光点落在已建图区域之外，地图值为 0，梯度也为 0，这些点对优化没有任何约束力。

提示：如果你自己改代码，千万不要在图幅边界处做最近邻取整，而应该沿用插值逻辑，否则位姿优化会出现非常突兀的跳变。

3. 高斯牛顿推导：扫描匹配的数学内核

3.1 位姿参数化与残差定义

2D 位姿用一个三维向量表示：

其中 是平移，是旋转角。某帧激光扫描点在世界坐标系下的坐标 经过刚体变换得到：

写成矩阵形式更清楚： 是旋转矩阵。

接下来定义残差。地图插值函数 已经把地图变成了连续函数，对于当前帧第 个激光点，我们希望它投影到地图上之后，对应的地图值越接近 1 越好。因此残差可以定义成：

目标函数就是所有残差的平方和：

这个目标函数的含义很好理解：如果激光点恰好落在障碍物栅格上，地图值为 1，残差为 0；如果落在空闲区域，地图值为 0，残差为 1，优化会把这个质量推高，反映在优化中就表现为位姿向更匹配的方向移动。

3.2 雅可比矩阵的链式推导

高斯牛顿需要目标函数对位姿 的一阶导数和二阶近似。关键是求出残差对位姿的雅可比。

先对中间变量求导。设投影后的点为 ，则：

用链式法则展开：

这里的三项分别处理。前两项是地图梯度，由双线性插值直接得到。后两项是对位姿的导数，从坐标变换公式推出：

把旋转矩阵展开后计算偏导数，可以得到：

注意这里还没换到栅格坐标，如果代码是在栅格坐标下计算的，每个分量还要再除以map_resolution。实际代码里，为了统一计算，Hector 先把世界坐标转成栅格坐标，再查插值和梯度，最后构建的雅可比向量是：

其中 为地图分辨率。

3.3 Hessian 近似与线性方程求解

高斯牛顿的第二步是把残差的平方和做二阶泰勒展开，用一阶雅可比近似 Hessian。展开后得到：

这里的二阶信息完全由一阶雅可比外积近似，省去了求二阶导的麻烦。这是优化领域里非常常见的手段，SIFT 匹配、BA、ICP 都用了类似思路。

代码里迭代求解的形式是：

其中：

这个是 3x3 矩阵和 3x1 向量，求解一次线性方程就能得到位姿增量。

3.4 迭代更新与具体实现逻辑

拿到增量后，位姿更新不是简单加减，因为旋转是非线性的。代码里常见的做法是把增量拆成平移增量和角度增量：

角度更新后，构造新的旋转矩阵，再更新平移。对应到伪代码，整个单次迭代逻辑大概是下面这样：

// 伪代码：扫描匹配单次高斯牛顿迭代 for (int iter = 0; iter < max_iterations; ++iter) { Eigen::Matrix3f H = Eigen::Matrix3f::Zero(); Eigen::Vector3f g = Eigen::Vector3f::Zero(); for (auto& point : current_scan) { // 1. 当前位姿投影激光点 Eigen::Vector2f pw = transform(pose, point); // 2. 查地图值和插值梯度 float map_val = map_util.getMapValue(pw); Eigen::Vector2f grad = map_util.getMapGradient(pw); // 3. 构建雅可比：地图梯度 + 旋转部分的链式导数 Eigen::Vector3f J; J << grad.x(), grad.y(), grad.x() * (-sin(pose.theta) * point.x - cos(pose.theta) * point.y) + grad.y() * ( cos(pose.theta) * point.x - sin(pose.theta) * point.y); // 4. 累加 H 和 g H += J * J.transpose(); g += J * (1.0f - map_val); } // 5. 求解 H * delta = g Eigen::Vector3f delta = H.ldlt().solve(g); // 6. 更新位姿 pose.translation() += delta.head<2>(); pose.theta() += normalize_angle(delta.z()); }

这段伪代码就是我当年对着源码一步一步画出来的，每一步和论文里的公式都能对上。如果只看论文，很容易忽略雅可比向量里第三行那一长串，那是旋转角对地图值的复合偏导，少了这一项，角度估计会明显抖。

4. 多分辨率地图：防止迭代掉进局部极值

4.1 单层地图为什么容易卡死

高斯牛顿是局部优化方法，对初始值敏感。如果初始位姿和真实位姿偏差太大，尤其角度偏差超过一定范围，单层栅格地图上的目标函数经常有多个局部极小值。典型场景是：在一条走廊里，激光扫描局部看起来“在墙边和离开墙边”两个位置都能得到不错的地图值，算法可能就被吸到错误的那个局部极值里。

解决思路是“由粗到细”的多分辨率策略。先用低分辨率地图做粗略对齐，低分辨率栅格更大，目标函数更平滑，局部极小值也更少，迭代不容易卡错。得到初步位姿后，再换到更高分辨率的地图精修，因为此时初始位姿已经比较准了，只需要小范围精细对齐。

4.2 分辨率金字塔是怎么构建的

Hector 的地图管理类里维护一组地图层，每层地图是上一层的缩放版本，常用于 2 倍缩放。也就是说，最细的一层保持原始分辨率，第二层是它的 2 倍降采样，第三层是 4 倍降采样，以此类推。

匹配代码里循环的顺序很关键：先取最粗的地图层开启迭代，把所有迭代次数跑完，然后把得到的位姿作为下一层的初始值。对每一层来说，迭代次数通常可以设成一样，但底层粗地图的迭代会让位姿发生大步长的修正，上层细地图只做局部微调。

我做验证时打印过各层迭代后的位姿变化，发现最粗那一层的角度修正往往有半度到几度，而最细一层每次角度变化不到 0.1 度。这就是多分辨率发挥作用的地方。

4.3 分辨率参数与地图尺寸的取舍

多分辨率地图的代价是内存和计算量成倍增加。地图层数太多或分辨率太低都会拖慢系统。实际调参中，地图分辨率不一定要很高，0.05 米每栅格在多数室内场景足够。地图边长如果太大，比如 4096 以上，多分辨率层的内存占用会明显上涨，但好处是机器人长时间跑动不容易出图。

经验值是：室内小场景，边长 1024 到 2048，分辨率 0.05，地图层数自动生成 5 层左右就够。做室外大场景时，要么提高边长，要么接受一定程度的漂移。不要试图用 0.025 分辨率把全场地图都建到厘米级，那样计算量会翻好几倍，收益却很小。

5. 地图更新机制：Hector如何维持长期可用性

5.1 网格更新的对数几率形式

扫描匹配得到位姿后，就要把这一帧激光“写入”地图。写入过程分两类：一类是命中的栅格，增加占据值；一类是射线穿过的栅格，减小占据值。Hector 的更新用对数几率叠加实现，避免数值越界：

命中栅格的更新量给正数，穿过的给负数。这样设计的好处是地图值不会走到 0 或 1 的极端上，长时间运行后数值依然稳定。

实际工程中要注意更新顺序：必须先更新“穿过”的栅格，再更新“命中”的栅格。因为激光束末端命中的栅格同时也被射线“穿过”，如果先加后减，末端就可能被磨掉。

5.2 栅格坐标对齐与边界处理

地图更新的另一大坑是坐标对齐。激光点变换到世界坐标后，转成栅格坐标时会涉及浮点取整。如果更新命中点时只标记了最靠近的那一个栅格，在地图分辨率较低时就会出现栅格边缘锯齿形地图。

解决方法是：命中点更新时，把它覆盖范围内的若干栅格都按距离权重分摊更新量，而不是只更新一个格子。这个细节很多二次开发的代码都忽略了，但直接影响最终地图质量。

边界处理也一样。激光很容易打到地图范围外，这时代码应当忽略，不能给地图外的栅格分配更新量。如果地图边界太小，机器人还没走几步，地图就已经被推没了，很容易造成后续匹配失败。

5.3 增量地图更新时的时间戳问题

Hector 地图里每个栅格还维护一个时间戳索引，防止同一帧数据重复写入。如果同一帧激光数据因为回调重发、算法循环等原因被处理两次，地图会被重复更新，栅格值会出现积累偏差。

这个问题在真实机器人上很少出现，但在录包重放调试时会遇到：同一帧扫描被/scan话题重复发送，地图快速“糊掉”。排查时一旦发现建出来的图有异常重叠，先看数据里有没有时间戳完全一样的帧。

6. 调参与排障：把公式变回工程经验

6.1 关键参数速查表

代码原理弄懂了，调参才有方向。整理一份我实测常用的参数表，可以当作 debug 时的参照。

这几个参数不是孤立的。地图分辨率调低后，map_size最好同步调高，保证同样的物理范围。迭代次数调高后，如果机器人运动很快，反而可能把上一帧的匹配结果带偏，因为数据已经过期了。

6.2 常见的四类失败模式

第一类：转弯时定位漂移。最常见的诱因是旋转初值不准。Hector 的角速度退化很严重，如果上一帧角度误差已经有几度，单层迭代很难拉回来。尝试把迭代次数从 3 提到 5 到 6，同时用 IMU 的航向角做初值，漂移会明显改善。

第二类：直线走廊退化。在长走廊里沿着行走方向，激光只能看到两堵平行的墙，横向约束缺失，Hector 会慢慢往侧面飘。这类问题算法层面无解，只能引入额外的传感器。

第三类：地图出现重影或撕裂。通常是雷达外参标定不准或时间戳不同步。检查 TF 树里雷达到基座的变换，以及数据包时间戳是否和/scan的帧头对齐。

第四类：CPU 占用异常高。先看地图尺寸和分辨率，再看雷达频率。如果雷达是 40Hz，而代码里每一帧都要全量更新大面积地图，算力自然居高不下。可以把雷达降频或限制更新区域。

6.3 数据预处理与坐标系检查

Hector 对输入的点云质量要求比 Cartographer 高。你没看错，它反而更怕噪声，因为它没有回环检测，一点误差不及时修正就会积累。

我拿到新机器人时，第一步永远是检查雷达安装外参。把激光点云投影到初版地图上，如果看到墙面有规律的弧形偏移，说明外参旋转有偏差。第二步是检查雷达是否被遮挡，很多 Hector 的漂移并不是算法问题，而是雷达本身看到了一片动态物体。

注意：Hector 没有回环检测。长时间运行后地图必然存在累积漂移，这是算法特性，不是调参能根治的。需要回环时，换用带图优化的 SLAM 方案更合适。

结尾

我自己刚开始啃这段代码时，最难的地方不是看明白“高斯牛顿迭代”几个字，而是把雅可比矩阵那一串推导和源码里H += J * J.transpose()对上号。花了一个下午把公式一步步展开、在纸上画地图插值的梯度方向，之后再看任何扫描匹配代码都有一种豁然开朗的感觉。

最后分享一个小技巧：如果你手边没有机器人，可以先录一段激光数据包，在回放时把 Hector 的地图实时可视化，然后把iterations从 3 改到 7，观察位姿轨迹的变化。这种方式调试局部极值问题非常直观，能帮你把参数和公式之间的关系完全串起来。希望能给正在啃同一段代码的你省下几个晚上。