做机器人定位建图的朋友大概率都遇到过这个场景手里没有轮式里程计底盘要么是四足要么是无人机轮子还会打滑。这时候很多人会想到 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),(x01, y0),(x0, y01),(x01, y01)。这里的关键不只是求值而是求梯度。优化过程需要知道“地图值对坐标的偏导”这个偏导直接从插值公式里推出来需要注意的是梯度公式里乘以的除了相邻栅格的差值还有分辨率换算。因为代码里坐标转换时会把世界坐标除以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.head2(); 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_resolution0.025 - 0.1越高越精细计算量越大map_size1024 - 8192太小地图容易出界iterations1 - 10太少收敛不了太多浪费算力laser_min_dist0.1 - 1.0滤掉近处噪声laser_z_min0.1 - 0.5滤掉地面杂点laser_z_max1.0 - 3.0滤掉头顶天花板干扰scan_subscriber_queue_size5 - 30高频率雷达下防丢帧这几个参数不是孤立的。地图分辨率调低后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观察位姿轨迹的变化。这种方式调试局部极值问题非常直观能帮你把参数和公式之间的关系完全串起来。希望能给正在啃同一段代码的你省下几个晚上。