LOAM

2014

Paper: LOAM: Lidar Odometry and Mapping in Real-time

Code: https://github.com/HKUST-Aerial-Robotics/A-LOAM

背景

3D 多线激光一圈扫描时间通常 >0.1 s，期间传感器会移动，导致点云存在运动畸变。再加上大场景中回环机会少，依靠闭环纠正漂移并不现实，所以算法必须单靠前后帧匹配控制累计误差

核心思想

将 SLAM 拆成两个不同频率的子任务：

  • 高频、低精度：Lidar Odometry（10Hz 左右）快速估计帧间相对位姿，用于实时性和漂移控制
  • 低频、高精度：Lidar Mapping（1Hz 左右）在更大特征集和更充分迭代下精细优化，并反向修正 Odometry 的状态

运动畸变去除

LOAM 假设一圈扫描中传感器匀速运动，但实际中可能有加减速。为满足假设，它利用 IMU 去掉非匀速部分的误差

IMU→世界系加速度：通过欧拉角去除重力，转换到世界坐标系。用匀加速度公式，积分求位移速度, 得到每个时间点的位移、速度：

\[s_1 = s_0 + vt + \frac{1}{2}at^2\]

根据激光点的相对扫描时间，将 IMU 提供的姿态、位移线性插值到该点时刻，补偿运动畸变

特征点提取

从去畸变点云中提取两类稀疏特征：

  • Edge Point（边缘点）：局部曲率大的点，表示空间结构突变（棱边）。
  • Planar Point（平面点）：局部曲率小的点，代表平面表面。

曲率计算公式（取前后各5个邻域点）:

\[\mathrm{curvature} \left( i \right) = \left \| \sum_{k=-5,k\ne =0}^{5} p_{i+k} - 10p_i \right \| ^2\]

并通过规则剔除：易被遮挡的斜面点（入射角过小）、离群点（邻近深度突变）。为了均匀覆盖视场，会把点云分成多个扇区，每个扇区按曲率排序选取固定数量的 Edge/Planar 点

Lidar Odometry（高频线程）

对比前一帧和当前帧的特征点云：

  • Edge 对齐：为当前帧每个边缘点找前一帧中最近的两条不同扫描线的边缘点，构成一条线段，误差函数是“点到直线距离”
  • Planar 对齐：为平面点找前三帧最近的三个平面点，形成平面，误差是“点到平面距离”

优化：用 LM（Levenberg–Marquardt）在 $SE(3)$ 上迭代（约25次）求相对位姿

Lidar Mapping（低频线程）

  • 局部地图维护：以当前位姿为中心，在地图中取一定范围（例如 10m 立方体）的特征点作为匹配参考。
  • 特征匹配：和高频里程计相同思路，但：特征点数量是高频里程计的 10 倍左右。
    • Edge 参考用 PCA 最大特征值方向，Planar 参考用最小特征值方向。
    • 优化频率：每秒左右运行一次（限制计算量），迭代次数较少（~10次）。
  • 输出：更精确的位姿，同时更新全局地图特征。