激光雷达SLAM学习笔记

天外飞雨

已于 2025-05-05 16:26:04 修改

阅读量719

点赞数 15

文章标签：学习笔记

于 2025-05-05 16:24:48 首次发布

本文链接：https://blog.csdn.net/qq_59860384/article/details/147719177

版权

三、LOAM (Lidar Odometry and Mapping in Real-time)

一、SLAM简介

SLAM是机器人实现自主移动的关键，其问题可以理解为如下的生活问题。

当我们来到一个陌生的环境时，为了迅速熟悉周围环境，我们一般会做：

用眼睛观察周围地标如建筑、大树、花坛等，并记住他们的特征（特征提取）
在自己的脑海中，根据双目获得的信息，把特征地标在三维地图中重建出来（三维重建）
当自己在行走时，不断获取新的特征地标，并且校正自己头脑中的地图模型（bundle adjustment or EKF）
根据自己前一段时间行走获得的特征地标，确定自己的位置（trajectory）
走了很长一段路的时候，和脑海中的以往地标进行匹配，看一看是否走回了原路（loop-closure detection）

目前用在SLAM上的Sensor主要分两大类，激光雷达和摄像头。故SLAM大致可以分为激光SLAM（也分2D和3D）和视觉SLAM（也分Sparse、semiDense、Dense）两类，但其主要思路大同小异。

SLAM算法在实现的时候主要要考虑以下4大核心方面：

地图表示与建模策略：稀疏地图（如特征点、线段）适合实时定位与轻量导航，稠密地图（如点云、网格、体素）适用于精细场景重建或机器人交互。
传感器配置：根据场景约束（如室内 / 室外、光照条件）选择传感器类型（激光雷达、视觉相机（单目 / 双目 / RGBD）、IMU 等）。单传感器视野局限（如 RGBD 相机 FOV 小、激光雷达水平角分辨率高但垂直视野窄）需通过多传感器互补（如结合 IMU 预测运动补偿视角盲区）。
数据关联与多传感器融合： 跨模态匹配：处理不同传感器数据类型差异（如图像特征点、激光雷达点云、IMU 运动数据），设计统一的特征表示（如将点云投影到图像平面进行联合匹配）或概率模型（如扩展卡尔曼滤波中的状态融合）。 时间戳对齐与异步处理：解决传感器采样频率不一致问题（如 IMU 高频输出与相机低频成像），通过插值、队列管理或异步优化（如滑动窗口滤波）实现数据融合。 关联歧义性：应对动态环境中特征匹配的误关联（如相似纹理区域、移动物体干扰），需引入几何约束（如对极几何、ICP 配准）、运动模型（如里程计预测）或机器学习方法（如深度学习特征描述子）提升鲁棒性。
定位与构图的状态估计及优化
- 位姿估计方法：
  - 前端里程计：基于相邻帧的相对运动估计（如视觉里程计 VO、激光里程计 LO），需处理快速运动、低纹理或重复结构场景下的退化问题（如引入 IMU 构建视觉惯性里程计 VIO）。
  - 后端全局优化：通过图优化（Graph-based SLAM）或滤波器（如扩展卡尔曼滤波）融合历史数据，消除累计误差（如回环检测后优化全局位姿图），实现全局一致性。
- 动态环境适应：区分静态与动态物体（如通过光流、点云运动检测），避免误将移动物体纳入地图（如剔除动态点云、语义分割辅助动态建模）。
- 计算效率优化：针对实时性要求（如移动机器人低算力平台），设计稀疏化策略（如关键帧选取、子图划分）、增量式优化（如滑动窗口滤波）或硬件加速（GPU 并行计算）。

二、激光雷达

2.1激光雷达简介

激光雷达传感器由两部分组成：激光发射（顶部）和激光接收（底部）。发射系统的工作原理是利用多层激光束，层数越多，激光雷达就越精确。层数越多，传感器就越大。激光被发射到障碍物并反射，当这些激光击中障碍物时，它们会产生一组点云，传感器与飞行时间（TOF）进行工作，从本质上说，它测量的是每束激光反射回来所需的时间。如下图

激光雷达传感器可以是固态的，也可以是旋转的，固态激光雷达将把检测的重点放在一个位置上，并提供一个覆盖范围（比如FOV为90° )。在后一种情况下，它将围绕自身旋转，并提供360度旋转。在这种情况下，一般把它放在设备顶上，以提高能见度。

激光雷达进行障碍物识别的步骤通常分为4个步骤：

点云预处理：提升数据质量 目标：去除噪声、降低数据冗余、对齐多传感器数据。
- 统计滤波：通过计算邻域点的统计特性（如均值、标准差）剔除离群点（如激光雷达回波异常的孤立点）。
- 体素网格滤波：将点云划分为三维网格（体素），每个体素仅保留一个代表性点（如质心），减少数据量并平滑密度差异
- 运动补偿：对于高速运动平台（如自动驾驶车辆），需通过 IMU 或里程计数据补偿激光雷达旋转或平移带来的点云畸变。
- 多传感器校准：完成激光雷达与其他传感器（如摄像头、IMU）的时间同步和空间外参标定（如手眼校准），确保数据时空一致性。
地面分割：分离静态背景与障碍物 目标：从点云中提取地面点，剩余点作为潜在障碍物。
- RANSAC（随机抽样一致性）：通过迭代随机采样点拟合平面（如地面），计算内点比例，保留最优平面。例如，在自动驾驶场景中，RANSAC 可拟合道路平面，剔除地面点后剩余点即为障碍物候选。
- 高斯过程（GP-INSAC）：结合概率模型提升平面拟合鲁棒性，适用于复杂地形（如非平整路面）。
- 高度阈值法：利用激光雷达安装高度和地面高度范围，通过 z 轴阈值筛选障碍物点。例如，农业机器人中，设定 z 值高于地面一定范围的点为障碍物
障碍物聚类与特征提取：识别独立障碍物 目标：将障碍物点云分割为独立团簇，并提取几何特征。
- 聚类算法：
  - 欧氏聚类：基于点间欧氏距离分组，适用于离散障碍物（如车辆、行人）。例如，PCL 库中的欧氏聚类算法可设置距离阈值（如 0.5 米）和最小点数（如 10 个点），将点云划分为多个障碍物簇。
  - DBSCAN聚类：基于密度的聚类，能处理任意形状障碍物，抗噪声能力强。
- 特征提取：
  - 几何特征：计算聚类质心、包围盒尺寸（长宽高）、点云密度等。
  - 运动特征：结合多帧点云计算障碍物速度、加速度（如通过卡尔曼滤波预测轨迹）
障碍物识别与跟踪：动态环境下的持续感知

目标：为障碍物分配唯一标识，预测运动状态，输出实时信息。
- 分类与匹配：
  - 机器学习分类器：通过 SVM、随机森林等模型对障碍物进行分类（如车辆、行人、路障）。
  - 数据关联：基于位置、速度等信息匹配前后帧障碍物，避免 ID 跳变。例如，联合概率数据关联（JPDA）算法可处理多目标跟踪中的误匹配问题3。
- 状态估计与滤波：
  - 卡尔曼滤波（KF）/ 扩展卡尔曼滤波（EKF）：融合传感器测量值与运动模型，平滑障碍物轨迹，预测未来位置。
  - 多模型交互（IMM）：针对动态障碍物（如突然加速的车辆），结合多个运动模型（如匀速、匀加速）提升跟踪鲁棒性3。
- 动态障碍物处理：
  - 光流法：通过相邻帧点云位移检测运动区域。
  - 深度学习分割：利用 3D-MiniNet 等网络直接识别动态物体，滤除干扰点

2.2Cartographer 算法详解

2.2.1背景

Cartographer 是Google 于 2016 年开源的轻量级、实时 SLAM（同步定位与地图构建）算法，支持2D 和 3D 场景，核心优势是高精度、低计算资源消耗和大规模环境适配。其设计目标是为移动机器人（如仓储机器人、自动驾驶车）提供可靠的定位与建图能力，尤其适合计算资源受限的嵌入式设备（如手机、无人机）。

2.2.2核心架构与技术模块

Cartographer 采用模块化设计，核心分为前端（里程计与子图构建）和后端（闭环检测与全局优化），辅以传感器融合与高效数据结构。

在这里插入图片描述

1. 前端：局部定位与子图构建

输入数据：
- 激光雷达扫描数据（如 2D 的 2D LiDAR 或 3D 的 Velodyne）；
- 惯性测量单元（IMU）数据（用于运动补偿）；
- 轮式里程计数据（可选，提升初始位姿估计）。
核心功能：
- 运动畸变校正（Motion Compensation）：利用 IMU 的角速度和加速度数据，将激光扫描过程中平台的运动（如旋转、平移）补偿到每一帧点云，避免因运动导致的点云失真（例如快速旋转时的 “拖尾” 现象）。例：若激光雷达扫描时间为 100ms，IMU 数据可将每个激光束的时间戳对齐到扫描瞬间的位姿。
- 扫描匹配（Scan Matching）：将当前激光扫描（称为 “节点”，Node）与已构建的局部子图（Submap）进行匹配，估计当前位姿（平移、旋转）。Cartographer 支持两种扫描匹配算法：
  - 正态分布变换（NDT, Normal Distributions Transform）：将子图点云转换为多维正态分布的概率密度场，通过最大化当前扫描点与分布场的匹配概率求解位姿。优点是计算效率高（O (n log n)），适合实时性要求高的场景。
  - 迭代最近点（ICP, Iterative Closest Point）：通过迭代寻找最近点对并最小化距离误差求解位姿，精度高但计算复杂度较高（O (n²)），常用于初始位姿不确定的场景。
- 子图构建（Submap Construction）：将连续的激光扫描（节点）通过扫描匹配累积到子图中，子图是局部点云的融合结果（去除冗余点，保留表面特征）。每个子图包含一系列节点的位姿估计，且一旦优化完成即被 “冻结”，不再参与后续局部匹配，仅用于闭环检测和全局优化。 关键数据结构：子图以稀疏点云形式存储，每个子图对应一个局部地图块，用于快速局部定位。

2. 后端：闭环检测与全局优化

闭环检测（Loop Closure）：检测机器人是否回到历史位置（回环），若检测到回环，则通过全局扫描匹配计算当前位姿与历史位姿的约束，消除累积误差。
- 回环候选检测：通过空间位置近邻法（如基于位姿图的空间邻近节点筛选）或外观匹配法（如 3D 点云特征匹配，2D 中可用激光雷达强度信息）快速筛选可能的回环候选。
- 全局扫描匹配：对候选回环的当前扫描与历史子图进行高精度匹配（如使用更耗时的 ICP 算法），生成可靠的回环约束（位姿变换矩阵）。
位姿图优化（Pose Graph Optimization）：将整个 SLAM 过程建模为图结构：
- 节点：代表机器人在不同时刻的位姿（前端生成的局部位姿估计）；
- 边：代表节点间的约束（如相邻节点的里程计约束、回环检测的全局约束）。通过优化算法（如 Ceres Solver 或 g2o 库）最小化所有约束的误差平方和，调整全局位姿，消除累积误差。

3. 传感器融合与实时性优化

IMU 融合： IMU 数据用于提供高频位姿预测，辅助激光扫描的运动补偿，避免因快速运动导致的扫描匹配失败（如无人机高速飞行场景）。
高效内存管理：子图在优化后仅保留关键帧和融合点云，删除冗余数据，支持长时间运行的大规模建图（如仓库、城市街区）。
多分辨率地图： 2D 版本中，地图以不同分辨率（如 0.05m、0.5m）存储，兼顾细节精度与全局概览；3D 版本采用体素网格（Voxel Grid）压缩点云。

2.2.3算法流程（以 2D 为例）

数据输入与预处理：激光雷达扫描生成点云，IMU 提供实时运动数据，矫正点云畸变。
前端里程计：当前扫描与最新子图进行 NDT 匹配，得到初始位姿估计，添加到当前子图。若当前子图的扫描次数超过阈值（如 10 次），则冻结该子图，新建子图。
闭环检测：定期筛选与当前位姿邻近的历史子图，进行全局扫描匹配（如 ICP），检测回环。
全局优化：若检测到回环，构建位姿图，优化所有节点位姿，更新全局地图。
地图输出：融合所有子图的点云，生成全局一致的 2D 占据栅格地图（Occupancy Grid）或 3D 点云地图。

三、LOAM (Lidar Odometry and Mapping in Real-time)

LOAM是Ji Zhang早期开源的多线LiDAR SLAM算法。上文介绍的cartographer主要解决室内问题，LOAM室内外都可以，但是没有回环检测。Cartographer的3D部分，更像是2D的扩展：即用2D的思路去做3D的事情。而LOAM则主要解决3D问题，其核心思路难以解决2D问题。

2.1框架

Point Cloud Registration：点云不是同一时刻获取的，每一个帧点云，其中的每一个点，都是不同时刻获取的，因此把它进行运动补偿：获取每个点的时间戳，位姿插值，把点云先投影到同一时刻；提取特征点。
Lidar Odometry：估计两帧点云之间的位姿变换，获得两个时刻之间的相对位姿，频率较高 10Hz
Lidar Mapping：建图模块，把连续10帧的点云数据和整个地图匹配，获得世界坐标系下的位姿，频率较低 1Hz。
Transform Intergration：实时利用世界坐标系下的位姿和两个时刻之间的相对位姿，更新各个时刻世界坐标系下的位姿。

Cartographer使用栅格地图，地图中存储着占据概率，通过把点云投影到栅格地图，计算匹配得分，找到最合适的投影，作为位姿变换。但是，LOAM使用的是点云地图，那么点云投影后，进行匹配的就还是点云地图。两堆点云，如何匹配？

传统方法，使用ICP方法，即默认两堆点云中最近的点是匹配点，构建矩阵进行奇异值分解，得到变换矩阵后投影点云，然后再次寻找匹配点，重新计算投影……直至收敛。然而，对SLAM算法而言，要求同步定位和建图。这样随便根据距离选匹配点，计算太复杂，可能算来算去都不收敛，压根就不能实时，计算量实在是太大了。LOAM作者决定对点云提取特征，然后根据稀疏的特征来计算位姿变换。作者决定提取两种特征点：平面点和边缘点。对于平面点与边缘点，作者引入曲率计算方法，直接利用激光的每条扫瞄线中，一个点前后各五个点，计算平均值到该点的距离。

直观理解如下图示

平面点：在三维空间中处于平滑平面上的点，其和周围点的大小差距不大，曲率较低，平滑度较低。
边缘点：在三维空间中处于尖锐边缘上的点，其和周围点的大小差距较大，曲率较高，平滑度较高

边缘点匹配方法：

红、绿、蓝代表多线激光雷达的扫描线；在第k+1帧中，边缘点i处在红色的扫瞄线上；在第k帧中，红线上的边缘点j和i更近，在相邻绿线上再找到一个最近的边缘点l，那么l和j构成一个边缘。因此，对边缘特征点来说，优化的目标就是，使i到直线lj的距离最近。

向量叉乘的模长，代表平行四边形的面积；除以底，代表平行四边形的高；也就是点到直线的距离。

平面点匹配方法

红、绿、蓝代表多线激光雷达的扫描线；在第k+1帧中，平面点i处在红色的扫瞄线上；在第k帧中，红线上的平面点j和i更近，在同线上再找到一个附近的平面点l，在相邻绿线上再找到一个最近的平面点m，那么lmj构成一个平面。因此，对平面特征点来说，优化的目标就是，i到平面mlj的距离最近。

分子：三维物体的体积；分母：地面构成的平行四边形的面积；高=体积/面积也就是点到平面的距离。

运动估计

在确定了两个优化目标：边缘点到边缘线的距离、平面点到平面的距离以后，就可以对特征点进行旋转，构建目标函数，通过非线性优化的方式进行梯度下降求解了。
作者论文中使用的求解方法是列文伯格-马尔夸特算法（LM），又叫阻尼牛顿法，可以避免求解中遇到的非奇异和病态问题。

scan-scan流程图

LOAMScanToScan1.drawio

该算法通过迭代优化激光雷达的位姿变换Tk+1L，结合特征点的几何关系与稳健拟合，实现对激光雷达运动的实时估计，同时校正点云失真，为后续建图提供准确的位姿信息。

地图构建和全局位姿

建图部分采用scan-map的匹配，仍然是基于边缘点和平面点，投影点云，计算点到直线和点到平面的距离。
里程计的匹配，是用两帧点云数据；
建图的匹配，是用10帧点云数据，和10立方米范围内的整个地图匹配。
特征点增加了10倍！

scan-map流程图

LOAM流程图-第 2 页.drawio

建图流程通过 位姿扩展→点云投影→特征匹配→全局优化→地图更新 的闭环，将当前扫描的点云精确注册到全局地图中。通过分层处理（特征点提取、几何匹配、非线性优化）和数据优化（下采样），在保证精度的同时控制计算复杂度，实现实时高效的 3D 建图。