4、3D SLAM算法原理与实现：LOAM系列算法解析、LIO-SAM紧耦合框架、基于图优化的SLAM
各位同学，欢迎来到第4章。今天咱们聊聊3D SLAM，这玩意儿在机器人学里，说白了就是让机器人在未知环境里一边走一边给自己画地图。我做了这么多年机器人，可以负责任地告诉你：没有SLAM，你的机械臂就是个瞎子。
3D SLAM的算法流派不少，但今天我只讲三个核心：LOAM系列、LIO-SAM、还有图优化。这三样东西，你搞懂了，基本就能应付大多数实战场景了。
核心观点：3D SLAM的本质，就是解决“我在哪”和“周围长啥样”这两个问题。LOAM系列是开山鼻祖，LIO-SAM是工程化典范，图优化是底层数学工具。
4.1 LOAM系列算法解析
LOAM，全称Lidar Odometry and Mapping。我第一次接触这个算法是在2018年，当时被它的设计思路惊艳到了。它把SLAM拆成了两个线程：一个跑高频低精度的里程计，一个跑低频高精度的建图。你想想看，这不就是典型的“快慢结合”思路吗？
LOAM的核心思想其实很简单：特征提取 + 扫描匹配。它从点云里提取两类特征——角点和平面点。角点就是那些尖锐的边缘，平面点就是那些平坦的表面。我个人习惯用曲率来区分这两类特征：曲率大的就是角点，曲率小的就是平面点。
代码实现上，特征提取的伪代码大概是这样的：
// 计算每个点的曲率
for each point in scan:
curvature = 0
for each neighbor in 5 points:
curvature += distance(neighbor, point)
if curvature > threshold:
label as corner point
else:
label as planar point嗯，这里要注意：曲率阈值的选择很关键。我刚开始做的时候，阈值设得太高，角点太少，匹配经常失败；设得太低，又全是角点，计算量爆炸。后来我总结了一个经验：先看你的激光雷达线数。16线的雷达，阈值可以设低一点；64线的，阈值可以高一些。
LOAM之后，又出了A-LOAM、LeGO-LOAM这些变体。A-LOAM是港科大做的，把代码写得特别干净，适合学习。LeGO-LOAM则加入了地面分割，在越野场景下表现更好。我在项目中遇到过一个问题：用A-LOAM在室内跑得好好的，一到室外草地就飘。后来换成LeGO-LOAM，加了地面约束，问题就解决了。
避坑指南：我曾经在隧道里跑LOAM，结果因为特征太少，直接炸了。后来我加了IMU预积分，才稳住。所以，纯激光SLAM在退化场景下（长走廊、隧道、空旷广场）很容易挂，建议配合IMU使用。
4.2 LIO-SAM紧耦合框架
说到LIO-SAM，我得先吐槽一下它的名字。LIO-SAM = LiDAR + IMU + Optimization + SAM。说白了，就是把激光雷达和IMU的数据“紧耦合”在一起，然后用图优化来求解。
为什么叫“紧耦合”？因为激光和IMU的数据不是分开处理的，而是在同一个优化框架里一起优化。我打个比方：激光是眼睛，IMU是内耳前庭。你闭着眼睛走路，靠内耳也能感知运动，但会飘；你睁着眼睛，但内耳坏了，也会晕。两者结合，才是王道。
LIO-SAM的框架图如下：
从图上你能看到，LIO-SAM的核心是因子图优化。它把每一帧激光数据、IMU预积分、回环检测都当作因子，一起塞进图里优化。我特别喜欢它的设计哲学：不抛弃任何有用的信息。
LIO-SAM的代码实现里，有几个关键点：
- IMU预积分：把两帧激光之间的IMU数据积分起来，得到一个相对运动估计。这玩意儿能帮你做点云去畸变，还能给优化提供一个好的初值。
- 关键帧策略：不是每一帧都加入优化，而是每隔一定距离或角度才插入一个关键帧。我一般设平移阈值1米，旋转阈值10度。
- 回环检测：用Scan Context这个全局描述子来检测回环。一旦检测到，就加入一个回环因子，把累积误差拉回来。
警告：LIO-SAM虽然强，但也不是万能的。我曾经在高速运动场景下测试，IMU预积分如果频率不够高（低于200Hz），状态预测会严重偏离，导致优化发散。建议IMU频率至少200Hz，最好400Hz以上。
4.3 基于图优化的SLAM
图优化，说白了就是把SLAM问题建模成一个图。图的节点是机器人的位姿，图的边是位姿之间的约束（比如激光匹配、IMU积分、回环检测）。然后，你调整所有节点的位置，让所有边的误差之和最小。
我刚开始学图优化的时候，觉得这东西好抽象。后来我把它想象成一个弹簧系统：每个约束都是一根弹簧，弹簧的松弛长度就是约束的期望值。你调整节点的位置，让所有弹簧的拉伸量之和最小。嗯，这样就好理解了。
图优化的数学形式是这样的：
minimize: sum( e_ij^T * Omega_ij * e_ij )
其中：
e_ij = 位姿i和位姿j之间的误差
Omega_ij = 信息矩阵（约束的置信度）这里的信息矩阵Omega_ij很关键。它表示你对这个约束有多信任。IMU的约束，信息矩阵可以设大一点；激光匹配的约束，如果场景特征少，信息矩阵就设小一点。我在项目中吃过亏：把所有约束的信息矩阵都设成单位矩阵，结果回环检测的约束被激光匹配的约束“淹没”了，回环根本闭合不上。
常用的图优化库有g2o、Ceres、GTSAM。我个人推荐GTSAM，因为它支持iSAM2增量式求解，不用每次都从头优化。LIO-SAM用的就是GTSAM。
| 优化库 | 特点 | 适用场景 | 我的评价 |
|---|---|---|---|
| g2o | 通用图优化，支持多种求解器 | 小规模SLAM、BA | 经典但有点老了 |
| Ceres | Google出品，自动求导 | 非线性最小二乘 | 灵活但需要自己搭图 |
| GTSAM | 因子图，支持增量式求解 | 大规模SLAM、实时系统 | 我的首选，强烈推荐 |
图优化的流程，我总结为三步：
- 建图：把所有的位姿和约束加进去，形成一个图。
- 求解：用Levenberg-Marquardt或Gauss-Newton迭代求解。
- 更新：把优化后的位姿更新到地图上。
这里有个细节：图优化不是一次性的。随着机器人运动，新的位姿和约束不断加入，图会越来越大。如果每次都从头优化，计算量会爆炸。所以，增量式优化就很重要了。iSAM2的做法是：只优化受新约束影响的那部分节点，其他节点保持不变。这样，即使跑了几个小时，计算量也不会太大。
实战技巧：我曾经在一个大型仓库里做SLAM，跑了两个小时，图里有上万个节点。如果用g2o全量优化，一次就要几十秒。换成GTSAM的iSAM2，每次优化只需要几十毫秒。所以，大规模场景下，一定要用增量式求解。
最后，我想说一句：3D SLAM这东西，理论是一回事，工程是另一回事。你可能会遇到各种奇怪的问题——点云畸变、IMU漂移、回环误检、地图错位。别慌，这些都是正常的。我做了这么多年，每次遇到新问题，第一反应不是“完了”，而是“有意思，又学到新东西了”。
好了，这一章的内容就到这里。记住：LOAM是基础，LIO-SAM是进阶，图优化是灵魂。把这三点吃透了，你的机械臂就能在未知环境里自由穿梭了。