前阵子有人拿着一张建到一半就飘掉的栅格地图来问我同样是激光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_yw_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_ig Σ 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₀ 1a 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, scale0.05, max_iter25): # 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 米的点在世界坐标里移动非常快会给雅可比矩阵注入很大的噪声梯度一般直接剔除。第二类是超过量程的无效点。这些点根本没有打到障碍物上不能当作占据点约束通常给一个很大的距离阈值或者直接丢弃。第三类是动态障碍物上的点比如行人、开关的门。它们会在地图上留下短暂而错误的占据痕迹匹配时把估计位姿往错误方向拉。异常点类型对匹配的影响建议处理方式过近点 0.05m梯度突变容易跳飞直接剔除超量程无效点当作占据点会拖偏按无效值过滤动态障碍物点局部错误约束降低权重或鲁棒退出地图外越界点梯度不存在跳过该点约束动态障碍物问题在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方案会轻松得多。