激光雷达SLAM定位算法
激光雷达SLAM定位算法
雷达定位算法全解析：从A-LOAM到LeGO-LOAM
前言
搞激光SLAM的同学应该对LOAM不陌生。2014年，CMU的Ji Zhang（张绩）在RSS上发表了一篇论文，用一个高频低精度的里程计加一个低频高精度的建图模块，把同时定位与建图这个复杂问题拆成了两个子问题，直接奠定了3D激光SLAM的基础框架。后来港科大的秦通博士把LOAM的代码做了一个工程化的高级实现，也就是A-LOAM（Advanced implementation of LOAM），让这个算法真正变得可复现、可部署。再后来，Tixiao Shan等人在LOAM的基础上加了地面优化和闭环检测，搞出了LeGO-LOAM（Lightweight and Ground-Optimized Lidar Odometry and Mapping），在非结构化地面环境中表现突出。
本文梳理一下这两个经典框架的核心思路和源码结构，最后再做个对比分析，希望能帮到正在啃这两个算法的同学。
一、A-LOAM：LOAM的工程化实现
1.1 整体架构
A-LOAM基本沿袭了LOAM的思路，核心思想就是把同时优化大量变量的SLAM问题拆成两个算法：一个高频低精度的里程计（Odometry），估计帧间运动；一个低频高精度的建图（Mapping），把姿态校准好，把地图拼出来。
源码组织上，A-LOAM分成了四个主要节点，对应四个.cpp文件：
- scanRegistration.cpp：前端点云处理，负责特征提取
- laserOdometry.cpp：前端里程计，做帧间位姿粗估计
- laserMapping.cpp：后端建图，做位姿精估计和局部地图维护
- kittiHelper.cpp：KITTI数据集适配工具
依赖方面，A-LOAM用到了ROS的基础包、PCL点云库、Ceres非线性优化库和OpenCV。编译的时候需要注意Ceres的版本兼容问题，很多同学在环境搭建这一步就卡住了。
1.2 前端：scanRegistration——点云特征提取
这部分是整个流程的起点。主函数main()做了三件事：读参数、订阅话题、发布话题。
几个关键参数：scan_line（雷达线数，默认16）、minimum_range（最小扫描距离，小于这个值的点会被滤掉，默认0.1m）。有意思的是，作者只支持16、32、64线这三种雷达，其他线数直接报错退出。
订阅的话题只有/velodyne_points，也就是原始雷达数据。一旦有数据进来，回调函数laserCloudHandler就开始工作：先把点云按线束有序化，计算每个点的曲率，然后根据曲率把点分成四类——
- 极大边线点（sharp corners）：曲率特别大的点，大概率是棱角或边缘
- 次极大边线点（less sharp corners）：曲率较大的点
- 极小平面点（flat surfaces）：曲率特别小的点，大概率是平面
- 次极小平面点（less flat surfaces）：曲率较小的点
这四类点分别发到不同的topic上，供后面的里程计模块使用。
点云有序化这一步其实是在模拟机械式激光雷达的扫描结构。通过计算每个点的俯仰角和水平角，把一帧无序的点云整理成一个按线束排列的二维表，后续的特征提取和运动补偿都依赖于这个有序结构。
1.3 前端：laserOdometry——帧间位姿粗估计
laserOdometry节点订阅了来自scanRegistration的五个话题：有序点云、极大边线点、次极大边线点、极小平面点、次极小平面点。向外发布上一帧的边线点、平面点，以及当前帧的位姿粗估计。
核心思路很直接：找到当前帧和上一帧之间的位姿变换T，让当前帧的特征点通过T变换后，与上一帧的对应特征匹配得最好。具体做法是用Ceres求解一个非线性最小二乘问题：
- 对边线点，构建点到线距离的残差
- 对平面点，构建点到面距离的残差
这里有一个重要的细节——运动畸变补偿。激光雷达在扫描过程中，载体是在运动的，导致一帧内不同时刻采集的点其实不在同一个坐标系下。A-LOAM假设帧内匀速运动，利用上一帧估计的位姿对每个点做补偿，把它们都对齐到帧起始时刻的坐标系。如果有IMU的话可以直接用IMU数据补偿，没有IMU就用匀速模型近似。
另外，mapping_skip_frame这个参数控制传给后端的频率。设为2的话，后端以5Hz运行，前端以10Hz运行，这也是LOAM经典的高低频配合策略。
1.4 后端：laserMapping——位姿精估计与建图
laserMapping节点订阅来自laserOdometry的四个话题：当前帧全部点云、上一帧边线点、上一帧平面点、当前帧位姿粗估计。发布的内容包括附近帧组成的子地图（submap）、全局点云地图、以及经过后端优化后的精确位姿。
后端的优化思路和前端类似，但有两个关键区别：
第一，匹配的对象不同。前端是当前帧和上一帧做匹配（scan-to-scan），后端是当前帧和一个局部子地图做匹配（scan-to-map）。这个局部子地图由当前帧周围的若干关键帧构成。
第二，线面约束的构建方式不同。前端特征点直接取自相邻扫描线，后端需要在局部地图中搜索最近邻点来构建约束。对于边线点，需要在局部地图中找到最近的5个点做协方差分解，判断是否在一条直线上；对于平面点，同样找5个点判断是否在一个平面上。
为了控制计算量，后端对点云做了体素滤波降采样（边线点分辨率默认0.4m，平面点默认0.8m），并且在更新地图时采用增量式策略——只更新当前帧附近的地图区域，而不是每次都重建整个地图。
1.5 A-LOAM的局限
A-LOAM虽然工程化做得好，代码清晰、注释完整，但也有几个明显的不足：
- 没有闭环检测。里程计漂移会一直累积，跑大场景时地图会逐渐扭曲。不过后来有工作把ScanContext回环检测模块集成进了A-LOAM（即SC-A-LOAM），定位精度提升超过50%。
- 计算效率问题。随着地图规模增大，局部地图的维护和特征搜索会消耗大量内存和时间。
- 对非结构化环境不够鲁棒。在几何特征不明显、噪声多的场景下，边线点和平面点的提取质量会下降。
二、LeGO-LOAM：地面优化的轻量级框架
2.1 整体架构
LeGO-LOAM在LOAM的基础上做了两个重要改进：一是充分利用地面点来约束位姿估计，二是加入了轻量级的闭环检测。
系统分为四个模块：
- Image Projection：点云投影与分割
- Feature Association：特征提取与帧间匹配
- Map Optimization：后端建图与位姿图优化
- Transform Fusion：多源位姿融合
2.2 Image Projection——点云分割
这是LeGO-LOAM区别于A-LOAM最核心的部分。它先把点云投影成一个距离图像（range image），然后在这个图像上做地面分割和聚类。
地面分割的思路很巧妙：计算相邻扫描线对应点的俯仰角，如果俯仰角小于阈值，说明这两个点大概率都在地面上。这样就能把地面点和非地面点分离开来。
聚类用的是广度优先搜索（BFS），在距离图像上把相邻且距离相近的非地面点归为一簇。簇太小（比如少于30个点）的直接判定为噪声丢掉。这一步能有效过滤掉树叶、草丛等不可靠的特征，大幅提升后续特征提取的质量。
2.3 Feature Association——两步优化
和A-LOAM一步到位优化六自由度位姿不同，LeGO-LOAM的前端里程计分成了两步：
- 利用地面点估计竖直方向的位移和两个旋转角（3自由度）
- 利用边缘点估计水平方向的位移和偏航角（3自由度）
这样分解的好处是计算更快、收敛更稳定。地面点在约束竖直方向位姿时特别有效，能有效抑制里程计的漂移，尤其是竖直方向的漂移。
特征提取部分和A-LOAM类似，也是基于曲率分边线点和平面点。不过LeGO-LOAM的平面点专门用了地面点，边线点从分割后的非地面簇中提取，有效避免了噪声干扰。提取出的特征点通过点到线、点到面的ICP方式做帧间匹配。
此外，LeGO-LOAM直接使用了IMU数据来做运动补偿，比A-LOAM的匀速模型假设更精确。如果IMU安装不正，反而会适得其反，这个问题在实际部署时需要留意。
2.4 Map Optimization——后端与闭环
Map Optimization模块接收前端里程计的位姿估计，在全局地图中做scan-to-map的精细匹配。和A-LOAM的后端思路类似，但多了闭环检测的功能
LeGO-LOAM的闭环检测采用的是基于轨迹位姿的简单方法：当前位姿和所有历史位姿做比较，找到距离最近且时间间隔足够远的历史帧作为候选回环帧，然后做ICP验证。检测到闭环后，通过图优化（gtsam库）对全局位姿进行修正。
这种方法的优点是简单轻量，候选帧距离较近时也能有不错的效果。缺点是当里程计漂移累积较大后，基于轨迹距离的检测就会失效——比如一个环形场景跑完回来，轨迹上显示当前位置和历史位置差了好几十米。所以有工作把ScanContext这种基于点云描述子的回环检测方法集成到了LeGO-LOAM中（SC-LeGO-LOAM），进一步提升闭环检测的鲁棒性。
三、对比分析
把这两个框架放在一起比较，差异还是比较明显的：
| 维度 | A-LOAM | LeGO-LOAM |
| 基础架构 | LOAM的工程化实现 | LOAM的基础上加地面优化 |
| 特征提取 | 基于曲率，无地面分割 | 先分割地面，再提取特征 |
| 前端优化 | 一步到位优化6自由度 | 两步优化（地面3+非地面3） |
| 闭环检测 | 无（需外部集成） | 内置基于轨迹的简易回环检测 |
| 传感器 | 纯激光 | 激光+IMU紧耦合 |
| 计算效率 | 较高 | 更高（地面点减少+两步优化） |
| 适用场景 | 结构化环境、几何特征丰富的场景 | 非结构化地面、户外粗糙地形 |
| 内存管理 | 后期可能内存爆炸 | 较轻量 |
有论文做了系统的对比评估，在不同的地下环境（SubT）场景中，BLAM、A-LOAM、LeGO-LOAM、Cartographer和Fast-LIO都表现出了相对可信的结果，而LeGO-LOAM在地面点丰富的场景中优势明显。需要注意的是，不同算法的落地表现高度依赖于场景特征——几何特征丰富时A-LOAM更稳定，地面点丰富时LeGO-LOAM更稳健。
四、总结
A-LOAM和LeGO-LOAM代表了3D激光SLAM的两个重要方向：
- A-LOAM追求的是简洁纯粹，代码少、依赖少，适合入门学习和理论研究。把LOAM的核心思想——特征提取、运动补偿、帧间匹配、局部建图——串成了一个干净清晰的流程。如果你是刚入门激光SLAM，从A-LOAM开始啃代码是性价比最高的选择。
- LeGO-LOAM追求的是实用鲁棒。地面分割、两步优化、闭环检测这些改进让它在实际场景中更有战斗力，尤其是非结构化的户外地形。计算效率也更高，更适合资源受限的平台部署。
这两个框架的源码都不算太长，把论文和代码对照着读一遍，基本上就能掌握基于特征的激光SLAM的核心套路了。之后再去看LIO-SAM、FAST-LIO这些更复杂的框架，理解起来也会顺畅很多。
参考阅读
北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台
更多推荐
16
0- 0
已为社区贡献10条内容
所有评论(0)