做激光雷达 SLAM 的工程师基本绕不开 LOAM。LeGO-LOAM 是 LOAM 家族里一个非常能打的改进版论文全称Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain核心就一句话轻量化、面向地面优化、适配变地形。我第一次完整读它的时候最强的感受是作者没有在数学上搞花活而是非常务实地利用了一个前提——我的机器人是贴着地面跑的。整条算法链路的每一步都在回答同一个问题哪些点可以信哪些点是噪声哪些约束值得花算力去优化。这篇文章把我读论文时收获的设计思路、关键细节、复现经验和踩坑心得完整写出来适合刚接触 LiDAR SLAM 的入门者也适合准备把算法往小车、巡检机器人上部署的工程师。1. 论文在回答什么问题1.1 原版 LOAM 的痛点在哪里原版 LOAM 是激光雷达里程计与建图领域的一个里程碑。它把点云按曲率分成边缘点和平面点先做相邻帧的运动估计再做当前帧到局部地图的精配准。这个框架到今天仍然在不少产品里服役但它有几个在实际工程里非常别扭的地方。第一它一视同仁地处理所有点。对一台地面车辆来说一帧 16 线雷达点云里真正有几何价值的点大部分来自地面而地面点之间变化平缓、特征高度重复把它们全部拿去做曲率计算和特征匹配算力基本上白费了。低线束雷达的垂直视场角只有 30 度左右分配给地面的线束占比非常高这个问题会被进一步放大。第二它对植被几乎无能为力。16 线雷达打到草丛、树冠、灌木上的时候会得到大量离散、无规则的点。这些点不仅没有稳定的几何结构反而会作为高曲率点混进特征集合把后续的点到直线、点到平面匹配带偏。做过户外实验的人应该都有体会草地一多轨迹就容易飘。第三6 自由度优化一锅端。相邻两帧之间明明有些方向的约束很强、有些方向约束很弱原版 LOAM 在每次迭代里对所有六个参数一起调整。当某个方向缺乏约束时整个法方程就会出现病态既拖慢收敛速度也容易陷入局部极小值。第四全局一致性偏弱。原版 LOAM 虽然有建图模块但缺少显式的回环检测和位姿图优化跑大场景时会缓慢累积漂移最后地图出现明显的左右分层。LeGO-LOAM 的出发点就是把这四个痛点逐个解决掉。1.2 三个关键词拆开看论文标题里藏着三个关键词值得一个一个拆开看。**Lightweight轻量化**不是单纯的跑得快而是一种系统性的减法。它通过地面分割和聚类把一帧三万个点左右的数据砍到只剩几千个候选特征再把 6 自由度优化拆成两步每一步只解 3 个自由度。这样做的意义在于算法可以跑在嵌入式设备上而不是非得配一台高性能工控机。**Ground-Optimized地面优化**是全文最有价值的设计哲学。在 LeGO-LOAM 里地面不再是被动处理的对象而是主动利用的约束来源。地面平面被当作一把天然的标尺用来限制车辆的 roll、pitch 和 z 方向位移。这个思路对我后来的很多项目都有启发环境里最稳定、最不起眼的结构往往就是最好的参考系。**Variable Terrain变地形**是它的目标场景。论文面向的不是平整的停车场而是草地、土坡、林间小路这种满地是噪声点的环境。植被点被聚类过滤掉坡道通过地面分割的延伸逻辑被适配这让它在真实户外场景里比原版 LOAM 更可靠。2. 系统设计与整体流程2.1 四模块架构总览LeGO-LOAM 的完整链路可以分成四个模块我习惯用一张表把它们的关系记下来模块输入输出核心作用分割模块原始 3D 点云地面点/物体点/噪声标记投影成范围图判地面聚类去噪特征提取分割后的点云边缘特征点、平面特征点大幅压缩特征数量保证特征类型纯净里程计模块相邻帧特征6DOF 位姿两步 L-M 优化高频粗配准建图模块特征与位姿全局地图与优化轨迹低频精配准因子图与回环优化整个数据流可以概括为原始点云先进分割得到带标签的点云特征提取根据标签分别挑选边缘点和平面点里程计利用前后两帧特征做运动估计输出位姿建图模块把位姿作为约束加入因子图并持续构建和更新全局地图。一句话总结就是先回答哪些点是可信的再回答可信点里哪些是特征最后才谈怎么配准。2.2 为什么先分割再做特征很多人会问原版 LOAM 不做分割也能跑为什么非要加这一层答案是在低线束雷达上不做分割的特征提取质量会明显下降。16 线雷达每线之间的俯仰角只有约 2 度同一扫描环上的点分布本来就不够密如果地面点、墙面点、树叶点混在一起算曲率特征类型的判断会经常出错。一个本来很平的草地表面因为夹杂着杂草点曲率会忽高忽低结果被错误地选成边缘特征。分割模块相当于给后续所有处理加了一个语义过滤层。有了地面和物体的标签之后特征提取就不再是纯几何运算而是带有语义的几何运算地面点的曲率再高也永远不会成为边缘特征物体点的曲率再低也不会被拿去当平面特征。这个设计不仅减少了计算量更重要的是减少了错误约束。在我看来这是 LeGO-LOAM 对 LOAM 家族最重要的结构性改进。3. 关键环节逐层拆解3.1 地面分割一张范围图加一个角度阈值LeGO-LOAM 的第一步是把 3D 点云投影成一张二维范围图。行是雷达线束编号列是水平扫描角像素值就是该点的距离。投影本身不复杂但它把不规则的 3D 点云变成了规则网格为后面的邻域搜索和聚类提供了极大的便利。地面分割的核心逻辑是逐列扫描从图像底部向上比较相邻两行点之间的垂直夹角verticalAngle atan2(Δz, sqrt(Δx² Δy²))如果夹角小于某个阈值论文和代码里常用约 10 度说明相邻两条扫描线之间的点在垂直方向上几乎没升高这段表面基本贴着地面于是把当前点标记为地面点。为什么这个判据成立因为 16 线雷达相邻线的俯仰角是固定的在平坦地面上越靠下的线束打到的距离越近相邻两点的 Δz 很小一旦遇到台阶、路沿、低矮植被Δz 会突然变大夹角一下子就超过阈值。处理坡道时LeGO-LOAM 还有一个向上延伸的技巧。它并不是只判断最底下那几行而是从已判定为地面的行继续向上扫描只要当前点的下邻点是地面点并且当前点与下邻点之间的垂直夹角依然小于阈值就把当前点也标记为地面。这样斜坡上那些看起来比平地更高的点也能被连续地划进地面集合不会出现坡道中段被拦腰截断的情况。注意10 度这个阈值是经验值对应的是雷达基本水平安装的工况。如果你的雷达有较大的仰角安装或者车辆经常行驶在陡坡上必须先做外参补偿再重新标定这个阈值。调得太大墙根和低矮灌木会被划成地面调得太小坡道上的地面点会漏检后面平面特征就会不足。3.2 点云聚类怎么把植被噪声一次滤掉地面分割之后剩下的非地面点还要做一次聚类。LeGO-LOAM 用的是在范围图上的广度优先搜索通过相邻像素在三维空间中的距离和夹角判断它们是否属于同一个物体。这里的思想也很直观同一个物体表面上的点在空间上是连续且平滑的而两个物体之间的边界处从雷达原点看过去方向和距离都会产生明显跳变。利用这个跳变信息就能把点云聚成一个个连通区域。聚类完成之后只保留点数超过阈值的簇论文常用 30 个点作为下限。那些只有十几二十个点的零散点云往往来自树叶、细枝、电线杆的稀疏回波直接被丢掉。这一步就是 LeGO-LOAM 在变地形环境里表现好的关键。草地和灌木产生的点云在范围图上呈现为一个个细小碎片聚类后大多不满足最小簇大小丢掉它们之后后续的特征提取和匹配面对的是一个干净得多的点云。我实际跑实验的时候对比过开与不开聚类过滤的效果开了之后轨迹的抖动明显减小地图上的毛刺也少了很多。当然这个阈值不是越大越好。如果环境里存在小型障碍物比如路肩、小石块、单个行人它们的点云簇也可能很小阈值太大会把有用的物体直接滤掉。这一点在做避障类项目时要格外小心。3.3 特征选取地面点出平面特征物体点出角点特征特征提取沿用 LOAM 的曲率平滑度定义取同一条扫描线上当前点前后若干个点计算它们在雷达坐标系下的距离差平方和作为曲面变化量 c。c 越大说明当前点越像边缘c 越小说明它附近越平坦。LeGO-LOAM 的关键变化在于加上了分割标签的约束。只有地面点才有资格被选为平面特征只有分割后的物体点才有资格被选为边缘特征。同时它把每条扫描线划分成若干子区域每个子区域只取曲率最大或最小的若干点。这样做的好处有两个一是特征在空间中分布更均匀不会全部挤在某个方向二是从根源上限制了特征总数。实际实现里还有一些剔除逻辑比如丢弃被遮挡区域边缘的点、丢弃与激光束方向近似平行的点。这些点和真实角点类似但匹配时极不稳定一点小噪声就会引起很大的位姿误差。剔除它们之后特征匹配的鲁棒性会明显提升。这样选出来的特征集合数量会从原版 LOAM 的几千个下降到几百个的量级。别小看这个数字变化它直接决定了后续 L-M 优化里雅可比矩阵的规模也决定了单帧计算耗时。3.4 两步优化把 6 自由度拆成 33LeGO-LOAM 里程计最核心的设计是两步优化这也是它和原版 LOAM 拉开差距的地方。先说平面特征的约束能力。平面特征对应的残差是点到平面距离一个平面点与它匹配到的局部平面之间的距离应该为零。这个距离对沿平面法线方向的平移极其敏感但对平面内的平移几乎不敏感。地面平面的法线方向基本是垂直的所以地面平面特征能够强有力地约束 roll、pitch 和 z 方向的平移。再说边缘特征的约束能力。边缘特征对应的残差是点到直线距离一个边缘点与它匹配到的空间直线之间的距离应该为零。这条残差对沿直线方向的平移不敏感但能很好地约束垂直直线方向的两个平移自由度以及绕直线方向以外的旋转。地面车辆在水平面上运动时边缘特征主要来自路边的立杆、树干、建筑墙角它们天然能约束 x、y 和 yaw。于是优化被拆成两步第一步只使用地面平面特征求解 roll、pitch、z 三个参数第二步固定这三个参数只使用边缘特征求解 x、y、yaw 三个参数 最后合并成一个完整的 6DOF 位姿。为什么这样拆是对的从优化理论角度看如果同时优化 6 个参数地面平面特征在水平方向上的梯度接近于零整个法方程处于病态会让收敛速度变得极慢还容易掉进局部极小值。拆开之后每一步的残差都是同构的、约束方向一致的优化问题从病态变成良态。更重要的是地面平面特征对 x、y、yaw 几乎没有约束能力如果把它们混在一起优化地面的微小噪声反而会污染水平方向和偏航角的估计。分开处理实现了每种特征只干自己最擅长的事。代码层面这一步通过两次独立的 L-M 求解完成。第一次优化只构建平面特征对应的残差项第二次优化在第一次结果的基础上构建边缘特征对应的残差项。两次优化的计算量都很小这也是 LeGO-LOAM 能实时运行的关键原因之一。3.5 回环检测与图优化让地图保持一致里程计负责高频粗配准建图模块负责低频精配准。LeGO-LOAM 的建图模块做两件事第一用当前帧特征与全局地图配准得到更精确的位姿并更新地图第二把位姿作为节点、位姿间的约束作为因子构建因子图用增量式平滑与建图方法做全局优化并在检测到回环时加入回环因子。回环检测的主要思路并不复杂。当机器人回到曾经到过的位置附近时系统会在当前位姿周围一定半径内搜索历史关键帧取出这些关键帧附近的局部地图与当前帧做配准再用配准分值判断是不是真的回到了同一位置。如果通过了判断就把当前位姿应约等于历史位姿作为一个新因子加进图优化。这一步让 LeGO-LOAM 在大范围场景里能显著拉回累积漂移比原版 LOAM 只有前向递推要完整得多。值得一提的还有地图的数据结构。LeGO-LOAM 会对加入全局地图的特征点做体素滤波下采样避免地图点云无限膨胀。这个细节在实际工程里非常重要因为 16 线雷达跑上十几分钟如果不对地图做降采样内存和匹配耗时都会失控。4. 实验设计与效果复盘4.1 测试平台与场景论文使用的传感器是 16 线激光雷达频率 10Hz搭载在一台小型地面无人车上。测试场景不是干净的停车场而是校园里那种植被茂密、有坡道、有建筑物和围栏的混合地形。这类场景对低线束雷达相当不友好草丛贡献大量离散点树冠制造阴影和遮挡坡道又让雷达水平安装、地面平整的默认假设失效。选择这样的场景是很刻意的。如果只在平整路面上测试地面优化带来的收益很难体现出来只有在满地是噪声点、地形起伏的环境里分割、聚类、两步优化的价值才会被充分放大。这也提醒我们评价一个 SLAM 算法测试场景的选择往往和算法本身一样重要。4.2 关键结论更轻、不更差、甚至更稳论文的定量对比是在相同数据上把 LeGO-LOAM 和原版 LOAM 放在一起比较的。核心结论可以概括成三点。第一特征点大幅减少。由于分割和聚类先滤掉了大量无效点参与配准的特征点数量下降非常明显论文报告大约在 40% 的量级。这个数字在不同场景会有波动但数量显著下降这个方向是确定的。第二单帧处理时间显著下降。更少的特征点加上两步优化让每帧点云从接收到输出位姿的耗时大幅缩短。作者在论文中给出的运行时间是远优于原版 LOAM 的这也是它能跑在嵌入式平台上的底气。第三精度不降反升。在带植被的户外测试数据上LeGO-LOAM 的轨迹漂移与原版 LOAM 相当甚至更小。原因在前面的设计里已经埋下地面约束提供了稳定的垂直方向约束聚类又过滤掉了会误导匹配的植被点少了错误约束配准自然更干净。需要提醒的是40%这类数字是在特定数据、特定参数下得到的换到你的传感器和场景里不一定能复现。但分割 聚类 两类特征分治带来的收益方向是明确的这个思路本身比具体数字更有价值。4.3 对实际工程的启示LeGO-LOAM 给我最大的启发是永远不要把你的传感器数据当成一个没有结构的点包。点云里天然带有哪些点在哪面哪些点属于哪棵树的信息只是需要花一点成本去提取。对地面车辆来说地面不是要绕开的障碍而是最稳定的标定物对无人机来说地面加上顶面可能是类似的组合。把语义信息注入几何配准是让算法在复杂地形上依然可靠的关键思路。这个思想后来影响了我的很多项目。哪怕不直接用 LeGO-LOAM我也会在点云预处理阶段先做一次快速的地面分割和聚类去噪再来做配准。它带来的稳定性提升往往比换一个更复杂的优化器来得更直接。5. 复现与调参实录5.1 环境与数据准备复现 LeGO-LOAM 比想象中容易因为它是一个标准的 ROS 工程主流程用 PCL 做点云处理用 Ceres 做优化用因子图库做图优化。基础步骤大概是准备一台装好 ROS 的电脑可以是实体机也可以是虚拟机安装 PCL、Ceres、GTSAM 等依赖库编译工程准备一份包含点云话题的 rosbag或者直接接上真实雷达启动雷达驱动和 LeGO-LOAM 节点在 rviz 里观察全局地图和里程计轨迹。如果你是第一次跑建议先用默认参数跑通再逐步调整。默认情况下点云话题通常是/velodyne_points类型是sensor_msgs/PointCloud2如果你的雷达是其他品牌需要先改话题名有时还要改点云类型。如果你用的不是 16 线雷达那么有几个宏定义必须改线束数量、水平扫描分辨率、初始地面扫描行数。这些参数直接决定范围图的行列数改错会导致地面分割和特征提取全部错乱。我见过不少同学在这上面卡了很久最后发现只是线束数量没改。5.2 关键参数的调整方法这里整理一份我实际调参时最常用的参数速查表参数项作用调整建议地面分割角度阈值约 10 度判断相邻扫描线是否属于地面雷达有仰角时先做外参补偿坡道多可以从 10 度往 12 度试最小聚类点数约 30过滤小物体和植被噪声植被密可以调大需要保留小障碍物就调小回环搜索半径与历史关键帧数量决定回环触发的窗口漏报就加大半径和数量误报就调严配准分数地图下采样体素大小控制地图内存和匹配精度16 线雷达常用 0.2 米量级可据场景上下浮动建图频率决定后台地图更新节奏嵌入式平台可降低建图频率保证里程计实时性调参的顺序也很重要。我一般先调地面分割确认范围图上的地面标签稳定再调聚类和特征提取确认特征数量不过多也不过少最后才动回环和建图的参数。前两步没调好后面所有参数都没有意义。经验之谈如果你发现轨迹在 z 方向持续震荡十有八九是地面分割漏检或者阈值太松导致非地面点混进了平面特征。如果你发现水平方向缓慢漂移先检查边缘特征数量是否太少再考虑回环参数。5.3 常见问题与排查下面是我和身边同事实际踩过的高频问题整理成一张排查表现象可能原因排查思路有里程计输出但全局地图一直是空建图频率低关键帧条件未满足等待几秒检查关键帧计数看 rviz 里节点是否在发布轨迹发散地图明显撕裂外参不对、点云时间戳不同步用标定工具重新标定检查各话题时间戳是否一致回环从来不被触发搜索半径太小、关键帧太少、配准分阈值太严调大历史关键帧搜索数量放宽配准分数阈值长走廊、空旷停车场漂移明显退化场景边缘特征不足加轮式里程计或 IMU 因子兜底不要指望单体雷达硬扛地面分割把墙面底部划成地面角度阈值过大或雷达安装角度不对重新检查外参适当调小角度阈值排查这类问题最忌讳的就是盲调参数。我每次都会先把可视化打开把地面点、物体点、边缘特征、平面特征用不同颜色显示出来一眼就能看出问题出在哪个环节。可视化永远比盲调高效。6. 我个人的实践体会6.1 先跑通再回来看代码读这类论文不要把注意力只放在公式上。我的习惯是先拿到开源实现把默认参数跑通再回头看代码里哪一行实现了论文里的哪句话。LeGO-LOAM 的代码结构非常清晰模块划分和论文的章节几乎是一一对应的非常适合这种边看代码边读论文的学习方式。我把地面分割那段逐行读完之后才真正理解了 10 度阈值到底在保护什么也才知道坡道延伸逻辑为什么写得那么啰嗦——那每一行都是在为真实场景兜底。6.2 认清适用边界LeGO-LOAM 不是万金油。它默认传感器下方一定存在一个可用的地面平面并且这个平面在雷达视场里是连续的。如果你的机器人是足式的或者雷达装在无人机上或者作业场景是密集的室内环境这个前提就不成立效果会明显打折。这类场景下更适合借鉴它的思路——分割、聚类、分治优化——而不是直接套用它的参数和结构。说到底算法是服务于假设的先搞清楚假设再选算法是我在这个项目里收获最大的一点。6.3 调参之前先看数据这是我想强调很多次的一条经验。LeGO-LOAM 的输出对参数其实比较敏感尤其是地面分割阈值和聚类点数。但绝大多数问题在 rviz 里都能一眼看出来地面点是不是把墙根包进去了、物体点聚类是不是碎成渣、边缘特征是不是全部挤在同一个方向。先看数据再决定调哪个参数一般十分钟内能定位到问题。盲调参数的话可能调一下午都找不到方向。6.4 部署时的融合兜底如果你准备把它部署到量产平台上我强烈建议加上退化检测和多传感器融合。LeGO-LOAM 在普通场景表现稳定但在长走廊、空旷停车场、高速运动这类工况下单靠 16 线雷达总有极限。工程上宁可多一个轮速计因子也不要让系统在退化场景里裸奔。我之后的几个项目里都是把 LeGO-LOAM 输出的里程计作为主因子再叠加轮速计或 IMU 的辅助因子整体鲁棒性立刻上了一个台阶。最后说一个小技巧复现 LeGO-LOAM 的时候最值得反复看的是地面分割的可视化结果。把分割输出打开逐帧观察它把哪些点判成了地面哪些点被聚类丢掉会让你对地面优化这四个字有非常直观的理解。看完之后你对这篇论文的理解会比单纯读十遍公式都深。 LeGO-LOAM详解：面向地面优化的轻量化激光雷达SLAM算法
做激光雷达 SLAM 的工程师基本绕不开 LOAM。LeGO-LOAM 是 LOAM 家族里一个非常能打的改进版论文全称Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain核心就一句话轻量化、面向地面优化、适配变地形。我第一次完整读它的时候最强的感受是作者没有在数学上搞花活而是非常务实地利用了一个前提——我的机器人是贴着地面跑的。整条算法链路的每一步都在回答同一个问题哪些点可以信哪些点是噪声哪些约束值得花算力去优化。这篇文章把我读论文时收获的设计思路、关键细节、复现经验和踩坑心得完整写出来适合刚接触 LiDAR SLAM 的入门者也适合准备把算法往小车、巡检机器人上部署的工程师。1. 论文在回答什么问题1.1 原版 LOAM 的痛点在哪里原版 LOAM 是激光雷达里程计与建图领域的一个里程碑。它把点云按曲率分成边缘点和平面点先做相邻帧的运动估计再做当前帧到局部地图的精配准。这个框架到今天仍然在不少产品里服役但它有几个在实际工程里非常别扭的地方。第一它一视同仁地处理所有点。对一台地面车辆来说一帧 16 线雷达点云里真正有几何价值的点大部分来自地面而地面点之间变化平缓、特征高度重复把它们全部拿去做曲率计算和特征匹配算力基本上白费了。低线束雷达的垂直视场角只有 30 度左右分配给地面的线束占比非常高这个问题会被进一步放大。第二它对植被几乎无能为力。16 线雷达打到草丛、树冠、灌木上的时候会得到大量离散、无规则的点。这些点不仅没有稳定的几何结构反而会作为高曲率点混进特征集合把后续的点到直线、点到平面匹配带偏。做过户外实验的人应该都有体会草地一多轨迹就容易飘。第三6 自由度优化一锅端。相邻两帧之间明明有些方向的约束很强、有些方向约束很弱原版 LOAM 在每次迭代里对所有六个参数一起调整。当某个方向缺乏约束时整个法方程就会出现病态既拖慢收敛速度也容易陷入局部极小值。第四全局一致性偏弱。原版 LOAM 虽然有建图模块但缺少显式的回环检测和位姿图优化跑大场景时会缓慢累积漂移最后地图出现明显的左右分层。LeGO-LOAM 的出发点就是把这四个痛点逐个解决掉。1.2 三个关键词拆开看论文标题里藏着三个关键词值得一个一个拆开看。**Lightweight轻量化**不是单纯的跑得快而是一种系统性的减法。它通过地面分割和聚类把一帧三万个点左右的数据砍到只剩几千个候选特征再把 6 自由度优化拆成两步每一步只解 3 个自由度。这样做的意义在于算法可以跑在嵌入式设备上而不是非得配一台高性能工控机。**Ground-Optimized地面优化**是全文最有价值的设计哲学。在 LeGO-LOAM 里地面不再是被动处理的对象而是主动利用的约束来源。地面平面被当作一把天然的标尺用来限制车辆的 roll、pitch 和 z 方向位移。这个思路对我后来的很多项目都有启发环境里最稳定、最不起眼的结构往往就是最好的参考系。**Variable Terrain变地形**是它的目标场景。论文面向的不是平整的停车场而是草地、土坡、林间小路这种满地是噪声点的环境。植被点被聚类过滤掉坡道通过地面分割的延伸逻辑被适配这让它在真实户外场景里比原版 LOAM 更可靠。2. 系统设计与整体流程2.1 四模块架构总览LeGO-LOAM 的完整链路可以分成四个模块我习惯用一张表把它们的关系记下来模块输入输出核心作用分割模块原始 3D 点云地面点/物体点/噪声标记投影成范围图判地面聚类去噪特征提取分割后的点云边缘特征点、平面特征点大幅压缩特征数量保证特征类型纯净里程计模块相邻帧特征6DOF 位姿两步 L-M 优化高频粗配准建图模块特征与位姿全局地图与优化轨迹低频精配准因子图与回环优化整个数据流可以概括为原始点云先进分割得到带标签的点云特征提取根据标签分别挑选边缘点和平面点里程计利用前后两帧特征做运动估计输出位姿建图模块把位姿作为约束加入因子图并持续构建和更新全局地图。一句话总结就是先回答哪些点是可信的再回答可信点里哪些是特征最后才谈怎么配准。2.2 为什么先分割再做特征很多人会问原版 LOAM 不做分割也能跑为什么非要加这一层答案是在低线束雷达上不做分割的特征提取质量会明显下降。16 线雷达每线之间的俯仰角只有约 2 度同一扫描环上的点分布本来就不够密如果地面点、墙面点、树叶点混在一起算曲率特征类型的判断会经常出错。一个本来很平的草地表面因为夹杂着杂草点曲率会忽高忽低结果被错误地选成边缘特征。分割模块相当于给后续所有处理加了一个语义过滤层。有了地面和物体的标签之后特征提取就不再是纯几何运算而是带有语义的几何运算地面点的曲率再高也永远不会成为边缘特征物体点的曲率再低也不会被拿去当平面特征。这个设计不仅减少了计算量更重要的是减少了错误约束。在我看来这是 LeGO-LOAM 对 LOAM 家族最重要的结构性改进。3. 关键环节逐层拆解3.1 地面分割一张范围图加一个角度阈值LeGO-LOAM 的第一步是把 3D 点云投影成一张二维范围图。行是雷达线束编号列是水平扫描角像素值就是该点的距离。投影本身不复杂但它把不规则的 3D 点云变成了规则网格为后面的邻域搜索和聚类提供了极大的便利。地面分割的核心逻辑是逐列扫描从图像底部向上比较相邻两行点之间的垂直夹角verticalAngle atan2(Δz, sqrt(Δx² Δy²))如果夹角小于某个阈值论文和代码里常用约 10 度说明相邻两条扫描线之间的点在垂直方向上几乎没升高这段表面基本贴着地面于是把当前点标记为地面点。为什么这个判据成立因为 16 线雷达相邻线的俯仰角是固定的在平坦地面上越靠下的线束打到的距离越近相邻两点的 Δz 很小一旦遇到台阶、路沿、低矮植被Δz 会突然变大夹角一下子就超过阈值。处理坡道时LeGO-LOAM 还有一个向上延伸的技巧。它并不是只判断最底下那几行而是从已判定为地面的行继续向上扫描只要当前点的下邻点是地面点并且当前点与下邻点之间的垂直夹角依然小于阈值就把当前点也标记为地面。这样斜坡上那些看起来比平地更高的点也能被连续地划进地面集合不会出现坡道中段被拦腰截断的情况。注意10 度这个阈值是经验值对应的是雷达基本水平安装的工况。如果你的雷达有较大的仰角安装或者车辆经常行驶在陡坡上必须先做外参补偿再重新标定这个阈值。调得太大墙根和低矮灌木会被划成地面调得太小坡道上的地面点会漏检后面平面特征就会不足。3.2 点云聚类怎么把植被噪声一次滤掉地面分割之后剩下的非地面点还要做一次聚类。LeGO-LOAM 用的是在范围图上的广度优先搜索通过相邻像素在三维空间中的距离和夹角判断它们是否属于同一个物体。这里的思想也很直观同一个物体表面上的点在空间上是连续且平滑的而两个物体之间的边界处从雷达原点看过去方向和距离都会产生明显跳变。利用这个跳变信息就能把点云聚成一个个连通区域。聚类完成之后只保留点数超过阈值的簇论文常用 30 个点作为下限。那些只有十几二十个点的零散点云往往来自树叶、细枝、电线杆的稀疏回波直接被丢掉。这一步就是 LeGO-LOAM 在变地形环境里表现好的关键。草地和灌木产生的点云在范围图上呈现为一个个细小碎片聚类后大多不满足最小簇大小丢掉它们之后后续的特征提取和匹配面对的是一个干净得多的点云。我实际跑实验的时候对比过开与不开聚类过滤的效果开了之后轨迹的抖动明显减小地图上的毛刺也少了很多。当然这个阈值不是越大越好。如果环境里存在小型障碍物比如路肩、小石块、单个行人它们的点云簇也可能很小阈值太大会把有用的物体直接滤掉。这一点在做避障类项目时要格外小心。3.3 特征选取地面点出平面特征物体点出角点特征特征提取沿用 LOAM 的曲率平滑度定义取同一条扫描线上当前点前后若干个点计算它们在雷达坐标系下的距离差平方和作为曲面变化量 c。c 越大说明当前点越像边缘c 越小说明它附近越平坦。LeGO-LOAM 的关键变化在于加上了分割标签的约束。只有地面点才有资格被选为平面特征只有分割后的物体点才有资格被选为边缘特征。同时它把每条扫描线划分成若干子区域每个子区域只取曲率最大或最小的若干点。这样做的好处有两个一是特征在空间中分布更均匀不会全部挤在某个方向二是从根源上限制了特征总数。实际实现里还有一些剔除逻辑比如丢弃被遮挡区域边缘的点、丢弃与激光束方向近似平行的点。这些点和真实角点类似但匹配时极不稳定一点小噪声就会引起很大的位姿误差。剔除它们之后特征匹配的鲁棒性会明显提升。这样选出来的特征集合数量会从原版 LOAM 的几千个下降到几百个的量级。别小看这个数字变化它直接决定了后续 L-M 优化里雅可比矩阵的规模也决定了单帧计算耗时。3.4 两步优化把 6 自由度拆成 33LeGO-LOAM 里程计最核心的设计是两步优化这也是它和原版 LOAM 拉开差距的地方。先说平面特征的约束能力。平面特征对应的残差是点到平面距离一个平面点与它匹配到的局部平面之间的距离应该为零。这个距离对沿平面法线方向的平移极其敏感但对平面内的平移几乎不敏感。地面平面的法线方向基本是垂直的所以地面平面特征能够强有力地约束 roll、pitch 和 z 方向的平移。再说边缘特征的约束能力。边缘特征对应的残差是点到直线距离一个边缘点与它匹配到的空间直线之间的距离应该为零。这条残差对沿直线方向的平移不敏感但能很好地约束垂直直线方向的两个平移自由度以及绕直线方向以外的旋转。地面车辆在水平面上运动时边缘特征主要来自路边的立杆、树干、建筑墙角它们天然能约束 x、y 和 yaw。于是优化被拆成两步第一步只使用地面平面特征求解 roll、pitch、z 三个参数第二步固定这三个参数只使用边缘特征求解 x、y、yaw 三个参数 最后合并成一个完整的 6DOF 位姿。为什么这样拆是对的从优化理论角度看如果同时优化 6 个参数地面平面特征在水平方向上的梯度接近于零整个法方程处于病态会让收敛速度变得极慢还容易掉进局部极小值。拆开之后每一步的残差都是同构的、约束方向一致的优化问题从病态变成良态。更重要的是地面平面特征对 x、y、yaw 几乎没有约束能力如果把它们混在一起优化地面的微小噪声反而会污染水平方向和偏航角的估计。分开处理实现了每种特征只干自己最擅长的事。代码层面这一步通过两次独立的 L-M 求解完成。第一次优化只构建平面特征对应的残差项第二次优化在第一次结果的基础上构建边缘特征对应的残差项。两次优化的计算量都很小这也是 LeGO-LOAM 能实时运行的关键原因之一。3.5 回环检测与图优化让地图保持一致里程计负责高频粗配准建图模块负责低频精配准。LeGO-LOAM 的建图模块做两件事第一用当前帧特征与全局地图配准得到更精确的位姿并更新地图第二把位姿作为节点、位姿间的约束作为因子构建因子图用增量式平滑与建图方法做全局优化并在检测到回环时加入回环因子。回环检测的主要思路并不复杂。当机器人回到曾经到过的位置附近时系统会在当前位姿周围一定半径内搜索历史关键帧取出这些关键帧附近的局部地图与当前帧做配准再用配准分值判断是不是真的回到了同一位置。如果通过了判断就把当前位姿应约等于历史位姿作为一个新因子加进图优化。这一步让 LeGO-LOAM 在大范围场景里能显著拉回累积漂移比原版 LOAM 只有前向递推要完整得多。值得一提的还有地图的数据结构。LeGO-LOAM 会对加入全局地图的特征点做体素滤波下采样避免地图点云无限膨胀。这个细节在实际工程里非常重要因为 16 线雷达跑上十几分钟如果不对地图做降采样内存和匹配耗时都会失控。4. 实验设计与效果复盘4.1 测试平台与场景论文使用的传感器是 16 线激光雷达频率 10Hz搭载在一台小型地面无人车上。测试场景不是干净的停车场而是校园里那种植被茂密、有坡道、有建筑物和围栏的混合地形。这类场景对低线束雷达相当不友好草丛贡献大量离散点树冠制造阴影和遮挡坡道又让雷达水平安装、地面平整的默认假设失效。选择这样的场景是很刻意的。如果只在平整路面上测试地面优化带来的收益很难体现出来只有在满地是噪声点、地形起伏的环境里分割、聚类、两步优化的价值才会被充分放大。这也提醒我们评价一个 SLAM 算法测试场景的选择往往和算法本身一样重要。4.2 关键结论更轻、不更差、甚至更稳论文的定量对比是在相同数据上把 LeGO-LOAM 和原版 LOAM 放在一起比较的。核心结论可以概括成三点。第一特征点大幅减少。由于分割和聚类先滤掉了大量无效点参与配准的特征点数量下降非常明显论文报告大约在 40% 的量级。这个数字在不同场景会有波动但数量显著下降这个方向是确定的。第二单帧处理时间显著下降。更少的特征点加上两步优化让每帧点云从接收到输出位姿的耗时大幅缩短。作者在论文中给出的运行时间是远优于原版 LOAM 的这也是它能跑在嵌入式平台上的底气。第三精度不降反升。在带植被的户外测试数据上LeGO-LOAM 的轨迹漂移与原版 LOAM 相当甚至更小。原因在前面的设计里已经埋下地面约束提供了稳定的垂直方向约束聚类又过滤掉了会误导匹配的植被点少了错误约束配准自然更干净。需要提醒的是40%这类数字是在特定数据、特定参数下得到的换到你的传感器和场景里不一定能复现。但分割 聚类 两类特征分治带来的收益方向是明确的这个思路本身比具体数字更有价值。4.3 对实际工程的启示LeGO-LOAM 给我最大的启发是永远不要把你的传感器数据当成一个没有结构的点包。点云里天然带有哪些点在哪面哪些点属于哪棵树的信息只是需要花一点成本去提取。对地面车辆来说地面不是要绕开的障碍而是最稳定的标定物对无人机来说地面加上顶面可能是类似的组合。把语义信息注入几何配准是让算法在复杂地形上依然可靠的关键思路。这个思想后来影响了我的很多项目。哪怕不直接用 LeGO-LOAM我也会在点云预处理阶段先做一次快速的地面分割和聚类去噪再来做配准。它带来的稳定性提升往往比换一个更复杂的优化器来得更直接。5. 复现与调参实录5.1 环境与数据准备复现 LeGO-LOAM 比想象中容易因为它是一个标准的 ROS 工程主流程用 PCL 做点云处理用 Ceres 做优化用因子图库做图优化。基础步骤大概是准备一台装好 ROS 的电脑可以是实体机也可以是虚拟机安装 PCL、Ceres、GTSAM 等依赖库编译工程准备一份包含点云话题的 rosbag或者直接接上真实雷达启动雷达驱动和 LeGO-LOAM 节点在 rviz 里观察全局地图和里程计轨迹。如果你是第一次跑建议先用默认参数跑通再逐步调整。默认情况下点云话题通常是/velodyne_points类型是sensor_msgs/PointCloud2如果你的雷达是其他品牌需要先改话题名有时还要改点云类型。如果你用的不是 16 线雷达那么有几个宏定义必须改线束数量、水平扫描分辨率、初始地面扫描行数。这些参数直接决定范围图的行列数改错会导致地面分割和特征提取全部错乱。我见过不少同学在这上面卡了很久最后发现只是线束数量没改。5.2 关键参数的调整方法这里整理一份我实际调参时最常用的参数速查表参数项作用调整建议地面分割角度阈值约 10 度判断相邻扫描线是否属于地面雷达有仰角时先做外参补偿坡道多可以从 10 度往 12 度试最小聚类点数约 30过滤小物体和植被噪声植被密可以调大需要保留小障碍物就调小回环搜索半径与历史关键帧数量决定回环触发的窗口漏报就加大半径和数量误报就调严配准分数地图下采样体素大小控制地图内存和匹配精度16 线雷达常用 0.2 米量级可据场景上下浮动建图频率决定后台地图更新节奏嵌入式平台可降低建图频率保证里程计实时性调参的顺序也很重要。我一般先调地面分割确认范围图上的地面标签稳定再调聚类和特征提取确认特征数量不过多也不过少最后才动回环和建图的参数。前两步没调好后面所有参数都没有意义。经验之谈如果你发现轨迹在 z 方向持续震荡十有八九是地面分割漏检或者阈值太松导致非地面点混进了平面特征。如果你发现水平方向缓慢漂移先检查边缘特征数量是否太少再考虑回环参数。5.3 常见问题与排查下面是我和身边同事实际踩过的高频问题整理成一张排查表现象可能原因排查思路有里程计输出但全局地图一直是空建图频率低关键帧条件未满足等待几秒检查关键帧计数看 rviz 里节点是否在发布轨迹发散地图明显撕裂外参不对、点云时间戳不同步用标定工具重新标定检查各话题时间戳是否一致回环从来不被触发搜索半径太小、关键帧太少、配准分阈值太严调大历史关键帧搜索数量放宽配准分数阈值长走廊、空旷停车场漂移明显退化场景边缘特征不足加轮式里程计或 IMU 因子兜底不要指望单体雷达硬扛地面分割把墙面底部划成地面角度阈值过大或雷达安装角度不对重新检查外参适当调小角度阈值排查这类问题最忌讳的就是盲调参数。我每次都会先把可视化打开把地面点、物体点、边缘特征、平面特征用不同颜色显示出来一眼就能看出问题出在哪个环节。可视化永远比盲调高效。6. 我个人的实践体会6.1 先跑通再回来看代码读这类论文不要把注意力只放在公式上。我的习惯是先拿到开源实现把默认参数跑通再回头看代码里哪一行实现了论文里的哪句话。LeGO-LOAM 的代码结构非常清晰模块划分和论文的章节几乎是一一对应的非常适合这种边看代码边读论文的学习方式。我把地面分割那段逐行读完之后才真正理解了 10 度阈值到底在保护什么也才知道坡道延伸逻辑为什么写得那么啰嗦——那每一行都是在为真实场景兜底。6.2 认清适用边界LeGO-LOAM 不是万金油。它默认传感器下方一定存在一个可用的地面平面并且这个平面在雷达视场里是连续的。如果你的机器人是足式的或者雷达装在无人机上或者作业场景是密集的室内环境这个前提就不成立效果会明显打折。这类场景下更适合借鉴它的思路——分割、聚类、分治优化——而不是直接套用它的参数和结构。说到底算法是服务于假设的先搞清楚假设再选算法是我在这个项目里收获最大的一点。6.3 调参之前先看数据这是我想强调很多次的一条经验。LeGO-LOAM 的输出对参数其实比较敏感尤其是地面分割阈值和聚类点数。但绝大多数问题在 rviz 里都能一眼看出来地面点是不是把墙根包进去了、物体点聚类是不是碎成渣、边缘特征是不是全部挤在同一个方向。先看数据再决定调哪个参数一般十分钟内能定位到问题。盲调参数的话可能调一下午都找不到方向。6.4 部署时的融合兜底如果你准备把它部署到量产平台上我强烈建议加上退化检测和多传感器融合。LeGO-LOAM 在普通场景表现稳定但在长走廊、空旷停车场、高速运动这类工况下单靠 16 线雷达总有极限。工程上宁可多一个轮速计因子也不要让系统在退化场景里裸奔。我之后的几个项目里都是把 LeGO-LOAM 输出的里程计作为主因子再叠加轮速计或 IMU 的辅助因子整体鲁棒性立刻上了一个台阶。最后说一个小技巧复现 LeGO-LOAM 的时候最值得反复看的是地面分割的可视化结果。把分割输出打开逐帧观察它把哪些点判成了地面哪些点被聚类丢掉会让你对地面优化这四个字有非常直观的理解。看完之后你对这篇论文的理解会比单纯读十遍公式都深。