企业官网建设流程全解析

前阵子有人拿着一张建到一半就飘掉的栅格地图来问我：同样是激光SLAM，为什么Hector这套方案在没有里程计的情况下也能跑起来？这个问题其实问到了点子上。Hector作为一套经典的栅格地图匹配定位方案，核心不在传感器多不多，而在每一帧激光都跟已有地图做了一次高斯牛顿优化。这篇文章不打算空谈概念，直接从代码推导的角度，把目标函数、梯度、双线性插值、多分辨率匹配这几个关键步骤拆开。看完你至少能回答：源码里那个Hessian到底是怎么来的，为什么一定要做插值，多分辨率地图到底在解决什么问题。

如果你是想快速复现一套2D激光定位模块的开发者，或者准备把手里的轮式机器人搬到没有可靠里程计的平台，这篇文章都适用。我默认你具备基础线性代数和一点多元微分知识，剩下的部分我会尽量用大白话补全。

1. 在动源代码之前，先想明白Hector的定位

1.1 它到底解决什么问题

Hector本质上是一个“扫描匹配 + 地图构建”的轻量级方案。输入是每帧2D激光点云和当前估计位姿，输出是优化后的位姿，以及一张不断更新的占据栅格地图。它不依赖轮式里程计，也不需要视觉特征，这是它最吸引人的地方。

但代价也很明显：它没有显式的回环检测，也没有复杂的图优化后端。也就是说，每一帧匹配都是“局部最优”的，匹配误差会一点点累积。所以Hector适合短时、小范围、结构化环境下的建图定位，比如室内清扫机器人、手持扫描设备这类平台。你要是拿它跑一个几百米的长走廊闭环，大概率会看到地图首尾对不上。

这个定位决定了后面所有代码组织方式。Hector核心只有三件事：把激光点变换到地图坐标系，用双线性插值读取地图占据概率，然后用高斯牛顿法最小化残差。地图更新反而是相对简单的部分。

1.2 代码整体的模块划分

如果你打开这套方案源码，会发现代码量不大，但模块边界很清晰。我一般按三个模块去读：

匹配器负责纯数学计算：给定当前位姿、激光点、地图，返回最优位姿。栅格地图只负责一件事——你给我一个连续坐标，我还你占据概率和梯度。调度处理器则决定先在哪一层分辨率匹配，再在哪一层精修，同时负责把匹配后的激光点写入地图。

我建议初学者也按这个顺序读代码，而不是从头到尾扫一遍。先把匹配器里的数学吃透，再看地图插值，最后看调度流程，基本上就通了。

1.3 为什么没有里程计也能定位

不少人对“无里程计”有误解，觉得没有轮式编码器，机器人就无法知道自己在哪。其实Hector的做法是：地图本身就是最强的空间先验。每一帧激光通过与地图匹配，相当于把“当前局部观测”安放到“全局坐标系”中。

打个比方：你在一个陌生小区里迷路了，但你手里有一张很详细的小区地图。你只要环顾四周，把看到的几栋楼、路口和地图上的轮廓对上，就能确定自己站在哪里。你不需要记得自己刚才走了多少米、转了多少度。Hector的每一帧匹配就是在做这件事。

理解了这一点，后面的数学目标函数就顺理成章了——本质上就是让激光端点落到地图里“看起来像被占据”的位置上。

2. 把模糊的“匹配”写成一个可微的数学问题

2.1 用占据概率给匹配打分

栅格地图里每个栅格都会保存一个占据概率，我记作 M(P)，P 是地图坐标系下的一个点。M = 1 表示该点肯定被占据，M = 0 表示肯定空闲。

理想的匹配效果是：每一个激光端点映射到地图后，都应该落在占据概率高的地方，最好是 M ≈ 1。所以很自然地，单个点的误差可以定义为：

e_i(ξ) = 1 - M(P_i(ξ))

其中 ξ = (x, y, θ) 是机器人当前位姿，P_i(ξ) 是第 i 个激光点经过位姿变换后的世界坐标。所有点的误差平方和就是我们要最小化的目标函数：

F(ξ) = Σ e_i(ξ)²

这个式子看着简单，但它把“匹配”这个直觉性概念变成了一个可以求导、可以迭代优化的数学问题。接下来唯一要解决的问题就是：怎么求最优的 ξ。

2.2 高斯牛顿法推导：H 和 g 到底从哪来

高斯牛顿法的思路是对残差做一阶泰勒展开。假设当前位姿是 ξ，我们想找一个增量 Δ，让 F(ξ + Δ) 变小。残差在 ξ 附近近似为：

e(ξ + Δ) ≈ e(ξ) + J Δ

其中 J = ∂e / ∂ξ 是残差对位姿的雅可比矩阵。把近似后的残差代入目标函数，展开后对 Δ 求导并令其为零，可以得到经典的增量方程：

(J^T J) Δ = -J^T e

这个方程里 J^T J 在SLAM代码里通常直接叫 H，-J^T e 叫 g。解出 Δ 后更新位姿：

ξ ← ξ + Δ

关键是 J 怎么算。激光点从机器人坐标系映射到世界坐标系的公式是：

w_x = x + cosθ · s_x - sinθ · s_y
w_y = y + sinθ · s_x + cosθ · s_y

其中 s = (s_x, s_y) 是激光点在机器人坐标系下的坐标。残差对位姿的雅可比是：

J = ∂(1 - M(P)) / ∂ξ = -[∂M/∂x, ∂M/∂y] · [∂P/∂ξ]

这里 ∂P/∂ξ 是一个 2×3 矩阵。逐项算一下：

∂P_x/∂x = 1, ∂P_x/∂y = 0, ∂P_x/∂θ = -sinθ·s_x - cosθ·s_y
∂P_y/∂x = 0, ∂P_y/∂y = 1, ∂P_y/∂θ = cosθ·s_x - sinθ·s_y

如果记 w_x 和 w_y 是地图坐标系下的坐标，上面第三列还可以简化成：

∂P_x/∂θ = -(w_y - y)
∂P_y/∂θ = w_x - x

这个简化在代码里经常用到，因为它直接基于已经算出来的世界坐标，省去再算一遍三角函数的开销。

再把 ∂M/∂x 和 ∂M/∂y 定义为地图梯度，组合起来：

A = [∂M/∂x, ∂M/∂y, ∂M/∂x · (-sinθ·s_x - cosθ·s_y) + ∂M/∂y · (cosθ·s_x - sinθ·s_y)]

那么 H 和 g 可以统一写成：

H = Σ A_i^T A_i
g = Σ A_i^T · (1 - M(P_i))

增量方程就是：

H Δ = g

注意这里符号不用纠结太久。对比标准高斯牛顿公式，因为 e = 1 - M，∂e/∂ξ = -A，所以 -J^T e = +Σ A^T (1-M)。最终代码里看到的就是“梯度矩阵乘残差”，没有额外负号。

2.3 为什么用一阶项近似Hessian

严格来说 H = J^T J 只是对海森矩阵的一阶近似，真正的海森矩阵应该对目标函数求二阶导。高斯牛顿法之所以敢这么干，是因为在小残差情况下，二阶项的影响远小于一阶项贡献，而且这样做有两个实打实的好处。

第一是计算量小。每一帧激光可能有几百到几千个点，如果每个点都去计算二阶导数矩阵，开销会成倍增长，实时性立刻崩掉。第二是正定性好。J^T J 天然是对称半正定矩阵，加一个小的正则项就能稳定求解，而真实海森矩阵无法保证正定，迭代反而容易发散。

我早期手推公式的时候总觉得 Hessian 应该算二阶导，后来看代码才发现，这里的 H 就是 J^T J，本质是梯度外积的累加。这个认知转变对读懂代码很重要。

3. 双线性插值：让栅格地图变成光滑山坡的关键

3.1 没有插值之前，栅格地图是不可导的

如果不做任何平滑处理，栅格地图就是一块一块的像素块。你查询任意一个坐标点，得到的就是它所在那个格子的整数索引对应的值。这种情况下，地图占据概率关于坐标的梯度要么是 0（因为你平移了一点还是落在同一个格子里），要么是无穷大（因为恰好跨过格子边界，值跳变）。

梯度为 0，高斯牛顿法的增量方程里 H 就变成零矩阵，迭代完全失灵。梯度无穷大，数值上直接不可用。所以必须对离散栅格做连续化处理。

双线性插值把每个栅格的值当成一个采样点，然后在相邻四格之间做线性过渡。这样整个地图就变成了一张分段双线性曲面，在每个格子内部是可导的。这也是Hector能做亚栅格定位的原因——优化器可以让位姿“滑”到格子之间的最优位置，而不是只能停在整数栅格上。

3.2 双线性插值的公式与梯度

假设查询点 P 落在栅格 (x₀, y₀) 内，令：

x₁ = x₀ + 1, y₁ = y₀ + 1
a = x - x₀, b = y - y₀

四个角点的占据概率分别是 M₀₀、M₁₀、M₀₁、M₁₁，那么插值后的占据概率为：

M(P) = (1-a)(1-b)·M₀₀ + a(1-b)·M₁₀ + (1-a)b·M₀₁ + a·b·M₁₁

对 x 和 y 分别求偏导，得到梯度：

∂M/∂x = (1-b)·(M₁₀ - M₀₀) + b·(M₁₁ - M₀₁)
∂M/∂y = (1-a)·(M₀₁ - M₀₀) + a·(M₁₁ - M₁₀)

这个梯度在格子内部其实是常数，因为插值函数是关于 x、y 的线性函数。到了格子边界，梯度会跳变，但迭代步长一般很小，实际影响可以忽略。

3.3 代码里如何组装H和g

理解了插值公式，代码里那一长串变量名就清楚了。我见过很多二次实现，核心循环基本长这样：

def match_scan(pose, scan_points, occ_grid, scale=0.05, max_iter=25): # pose: 机器人位姿(x, y, theta)，单位米 # scale: 地图分辨率，米/格 for _ in range(max_iter): H = np.zeros((3, 3)) g = np.zeros(3) for sx, sy in scan_points: # 1. 激光点变换到世界坐标（米） c, s = np.cos(pose[2]), np.sin(pose[2]) wx = pose[0] + c * sx - s * sy wy = pose[1] + s * sx + c * sy # 2. 转成栅格坐标 gx, gy = wx / scale, wy / scale # 3. 读取地图值、梯度（需要处理越界） M, dM_dgx, dM_dgy = bilinear_value_and_gradient(occ_grid, gx, gy) # 4. 残差 residual = 1.0 - M # 5. 地图值对世界坐标的梯度要除以scale dM_dwx = dM_dgx / scale dM_dwy = dM_dgy / scale # 6. 地图值对位姿的梯度A A = np.array([ dM_dwx, dM_dwy, dM_dwx * (-(wy - pose[1])) + dM_dwy * (wx - pose[0]) ]) # 7. 累加Hessian和梯度向量 H += np.outer(A, A) g += A * residual # 8. 求解增量，加小正则防奇异 try: delta = np.linalg.solve(H + np.eye(3) * 1e-6, g) except np.linalg.LinAlgError: break # 9. 更新位姿 pose += delta return pose

这段代码基本还原了源码里匹配器的核心逻辑。我刻意把每一个步骤都写出来了，因为很多人看不懂源码，就是卡在“栅格梯度为什么要除以scale”以及“A的第三项为什么长这样”这两个地方。

看代码时还有一个容易忽略的点：微分运算是线性叠加的，所以地图梯度先对栅格坐标求导，再除scale得到对米制世界坐标的导数；位姿梯度第三项用-(wy - pose[1])和wx - pose[0]代替了原来的三角函数项。只要你在纸上推导一遍，就会发现这两组表达式完全等价。

4. 多分辨率地图：先粗后细的匹配艺术

4.1 单层高分辨率为什么容易匹配失败

高分辨率地图细节丰富，目标函数非常“尖锐”。稍微偏离真实位姿一点，得分就断崖式下降，很容易陷入局部极值。比如初始角度误差超过十几度，或者平移误差超过几十厘米，单层匹配经常直接失效。

这和我们在高处眺望城市是一个道理。地图放得很大时，你只能看见自己眼前的几栋楼，很难判断整体方位。这时候就需要先把视野缩小，看到整个街区的轮廓，知道自己大概在哪，再放大精确到门牌号。

Hector的多分辨率匹配做的就是这件事。

4.2 多层地图怎么构建

多分辨率地图的构建方式是逐层降采样。最高分辨率层就是当前需要建图的地图层，下一层地图通过合并上一层相邻 2×2 栅格得到。

合并策略常见两种：取四个子格的平均值，或者取最大值。取平均保留的连续梯度信息更多，匹配时更平滑；取最大则偏向保守，只要四个子格中有一个被占据，父格就会被判为占据。

源码里的多分辨率地图管理模块会把每一层地图单独存一份，匹配时从最粗糙的层开始，逐层往最精细层推进。这里有一个单位细节：每层地图都有自己的分辨率，所以同一份激光点在不同层的栅格坐标并不一样。低分辨率层每个栅格对应的物理尺寸更大，所以相同的位姿偏差在低分辨率层的“栅格距离”更小，等价于把搜索步长变大了。

4.3 完整的从粗到细匹配流程

多分辨率匹配的流程可以概括成这样一个循环：

pose = 初始估计位姿 for level from max_level down to 0: current_map = maps[level] pose = matchDataInMap(current_map, pose, scan_points)

每一层内部做的仍然是高斯牛顿迭代，只是图层不同。低分辨率层能容忍更大的初始误差，先把位姿拉到正确位姿附近的“盆地”里；高分辨率层再负责精修到亚栅格精度。

我实测下来的直观感受是：如果没有多分辨率匹配，初始角度误差超过10度就很容易卡错方向；有了多分辨率，即使初始误差到20度左右，也有不小概率被拉回来。对应到代码里，就是因为低分辨率地图的梯度变化平缓，局部极小值少，梯度下降更容易走向全局最优的方向。

匹配完成后还有一个容易被忽略的步骤：把当前帧激光写入地图。Hector用的是典型的光线模型——从机器人位置到激光命中点画一条线段，线段经过的栅格空闲概率增大，命中点所在的栅格占据概率增大。这个操作发生在匹配成功之后，因为只有位姿准了，激光点写入的位置才可信。

5. 工程化必须补上的几个细节

5.1 米制坐标与栅格坐标的换算

这是我自己调代码时最容易踩的坑。激光雷达读出来的点云单位是米，栅格地图的索引单位是格，中间差着一个分辨率 scale。如果忘了统一单位，Hessian矩阵里的数值会差好几个数量级，迭代一步就能飞出去几十米。

统一的办法是：匹配之前把位姿和激光点都除以 scale，变成栅格坐标下的数值，匹配完之后再把位姿增量乘回 scale，转回米制。上面的伪代码里已经演示了这个过程，我再强调一次：不只是激光点要除，位姿本身也要除，因为位姿里的 x、y 同样是以米为单位的。

角度不需要缩放，因为它和长度单位无关，这也是一些代码看起来“x、y做了变换，θ却原封不动”的原因。

5.2 激光异常点与动态障碍物

激光数据不是干干净净的点。我在实际项目中常遇到三类问题，都会直接影响匹配结果。

第一类是过近的点。机器人自旋时，距离小于 0.05 米的点在世界坐标里移动非常快，会给雅可比矩阵注入很大的噪声梯度，一般直接剔除。第二类是超过量程的无效点。这些点根本没有打到障碍物上，不能当作占据点约束，通常给一个很大的距离阈值或者直接丢弃。第三类是动态障碍物上的点，比如行人、开关的门。它们会在地图上留下短暂而错误的占据痕迹，匹配时把估计位姿往错误方向拉。

动态障碍物问题在Hector这种方案里尤其明显，因为它没有后端优化，每一帧的错误都可能直接写进地图。安全点的做法是：匹配之前用上一帧结果做一个简单的动态点剔除，匹配之后如果打分突然变差，宁可不更新地图，也不要硬写进去。

5.3 迭代退化与匹配失败兜底

想象一个很空旷的长方形房间，中间没有家具，四面都是平行墙。这时激光点能提供很强的横向约束，但沿墙长度方向的约束很弱。Hessian矩阵会有接近零的特征值，直接求解增量方程会得到一个数值巨大、方向随机的增量。

工程上的处理方案有三种，可以叠加使用：

• 给 H 的对角线加一个小量，比如 λ = 10⁻⁶，保证矩阵可逆；
• 限制单次迭代的最大位移和角度增量，超出就截断；
• 迭代完成后对比更新前后得分，如果得分变差，说明收敛异常，回退到迭代前的位姿。

匹配失败后还有一个重要原则：不要让人看到明显错乱的位姿持续累积。此时可以保留上一帧位姿，把当前帧激光丢弃在更新地图之外，等待下一帧重新匹配。这个小策略在真实场地里救过我很多次，比任何花哨的权重算法都管用。

6. 跑通简化实现之后的实测心得

6.1 一个最小实现的调试案例

我自己写简化版Hector时，第一步不是接真雷达，而是拿一张小地图和固定的一帧点云做单帧匹配测试。让同一个点云反复匹配 100 次，看它是不是稳定收敛到同一个位姿。

第一次跑出来的结果很糟糕：位姿在小数值上来回跳，残差也没有明显下降。排查后发现问题是插值越界点没有做掩码处理。激光点一旦扫到地图范围之外，get函数返回了一个很大的异常值，残差瞬间变成几百，梯度向量被一个坏点彻底带崩。

把越界点跳过之后，匹配立刻稳定了。这件事告诉我：Hector这种基于梯度的方案里，坏点的影响是“平方级”的。一个离群点的残差若比其他点大一个数量级，它的梯度贡献在平方和里会大两个数量级，完全掩盖正常点的信息。所以数据预处理比优化算法本身更重要。

6.2 Hector适合什么，不适合什么

跑了各种场景以后，我逐渐形成了自己的选型判断。Hector最适合的是室内结构化环境、计算资源有限、没有可靠轮式里程计的平台。它启动快、依赖少、对 CPU 要求不高，非常适合做轻量级定位前端。

不太适合的情况包括：大场景长走廊闭环、动态物体太多的开放环境、以及初始位姿完全未知的场合。Hector说到底是一个局部匹配器，它靠地图插值提供连续梯度，但它没有全局重定位能力。位姿一旦跑丢，后面每一帧匹配都会在错误的方向上越陷越深。

如果你需要长时间稳定建图，我建议把Hector当纯前端来用，加一个回环检测和后端图优化。或者干脆在现代方案里直接用基于因子图的SLAM框架，只有在强受限设备上再用这套轻量方案。

6.3 往后还能怎么扩展

最后聊一点扩展思路。给Hector加IMU是很常见的做法，IMU的俯仰和横滚信息可以修正激光点在3D空间里的投影，避免机器人颠簸时2D扫描畸变；IMU的偏航角也可以作为匹配迭代的初始先验，减少大角度搜索的迭代次数。

另一个我觉得很实用的扩展是：定期对地图做一次“刷新”。长时间运行后，动态物体留下的杂点会累积成错误地图，影响后续匹配。可以周期性清除低置信度的占据栅格，或者用一份干净的静态地图叠加做匹配，只更新机器人位姿，不更新地图。

这套算法虽然不是什么新东西，但它的数学结构非常干净，很适合作为学习扫描匹配的入门教材。把Hessian、插值、多分辨率这三块啃下来，再看其它激光SLAM方案会轻松得多。