激光SLAM的发展和应用笔记

DW_DROME

于 2025-05-03 03:39:49 发布

阅读量1k

点赞数 18

分类专栏： ROS2 文章标签：笔记 ROS

本文链接：https://blog.csdn.net/2402_87385120/article/details/147674375

版权

ROS2 专栏收录该内容

20 篇文章

订阅专栏

SLAM（Simultaneous Localization and Mapping）

同时估计机器人位姿与环境地图

1，核心概念

Localization（定位）：在已知地图上估计机器人当前位姿
Mapping（建图）：在已知机器人位姿下构建环境地图
SLAM：同时进行定位与建图

主要任务
1. 定位问题：估计 $x_t$
2. 地图构建：生成环境地图，用于路径规划
地图分类
1. 静态地图
  - 尺度地图（Metric Map）
    - 格式化表示（如占据栅格、点云）
    - 精度高、便于导航与规划
  - 拓扑地图（Topological Map）
    - 特征点／关键帧作为节点，节点间连边表示可达性
    - 结构紧凑，适合大规模环境
  - 混合地图（Hybrid Map）
    - 在拓扑节点上附加局部尺度地图
    - 兼顾全局性与局部精度
2. 动态环境（扩展主题，不在此讨论）
  - 高动态目标：人员、车辆等运动速度较快的物体
  - 低动态目标：缓慢移动或短暂静止的物体
经典方法

滤波器方法（Filter-based SLAM）
- 如 EKF-SLAM、FastSLAM
- 在线估计当前状态，适合小规模环境
图优化方法（Graph-based SLAM）
- 基于 Pose Graph，使用非线性最小二乘
- 适合大规模场景及全局一致性优化

图优化（Graph-based SLAM）

1. Graph‐based SLAM 总览

目标概率
$p(x_{0:t}, m \mid z_{1:t}, u_{1:t})$
从控制输入 $u_{1:t}$ 与观测 $z_{1:t}$ 推估机器人全轨迹 $x_{0:t}$ 及地图 $m$ 。
两大阶段
1. 前端（Graph Construction）：构建图结构（节点、边）
2. 后端（Graph Optimization）：非线性最小二乘优化所有节点位姿

2. 图的基本要素

Graph：SLAM 问题的整体结构
Node：机器人的一时刻位姿 $x_i$
Edge：节点间的空间约束（相对变换观测）

3. 关键公式

相对位姿
$T_{ij} = T_j \;T_i^{-1}$
- $T_i$ ：从世界坐标系到时刻 $i$ 坐标系的变换
- $T_j$ ：从世界坐标系到时刻 $j$ 坐标系的变换
- $T_{ij}$ ：从 $x_i$ 到 $x_j$ 的相对位姿
误差项（单条边）
$e_{ij}(x_i,x_j) = \log\!\bigl(z_{ij}^{-1}\,(T_i^{-1}T_j)\bigr) \;\in\;\mathbb{R}^6$
- $z_{ij}$ ：传感器观测的 $i\to j$ 变换
- $T_i^{-1}T_j$ ：节点估计的 $i\to j$ 变换
- Log：SE(3)→se(3) 对数映射，6 维向量（3 平移＋3 旋转）

4. 链式变换 $T_1$ 与回环观测 $T_2$

变换	定义	表达式
$T_1$	链式累积：里程计／帧间匹配依次乘积	$T_1 = T_{1\to2}\,T_{2\to3}\,T_{3\to4}$
$T_2$	回环观测：直接从 $x_1$ 与 $x_4$ 的重叠特征／ICP 算法	$T_2 = \begin{bmatrix}R_2 & t_2\\0&1\end{bmatrix}$

同质矩阵结构
$\begin{bmatrix}R & t\\[3pt]0_{1\times3}&1\end{bmatrix}, \quad R\in\mathbb{R}^{3\times3},\; t\in\mathbb{R}^3$

5. 回环误差与优化

残差矩阵
$\Delta T = T_1^{-1}\,T_2$
若 $\Delta T=I$ 则两种变换完全一致；否则存在位姿偏差。
残差向量
$\log(\Delta T)\quad(\in\mathbb{R}^6)$
优化目标
$\min_{x_{0:t}}\;\sum_{(i,j)}\|\,e_{ij}(x_i,x_j)\|^2$
通过非线性最小二乘校正全局漂移，融合里程计与回环约束。

前瞻：在 ROS2 中，可借助 nav2_amcl／slam_toolbox 等包前端生成回环边，后端调用 g2o 或 Ceres 完成全局优化。

滤波器（Filter-based SLAM）

1. 目标

仅估计当前位姿 $x_t$ 并构建地图 $m$
$p(x_{t}, m \mid z_{1:t}, u_{1:t})$
– 适合小范围场景，易受里程计漂移影响

2. 基本要素

状态向量 $x_t$ ：当前位姿 $\theta]$
地图 $m$ ：特征点集合或栅格地图
控制输入 $u_t$ ：里程计／速度指令
观测 $z_t$ ：激光／视觉测量
过程噪声 $w_t\sim\mathcal{N}(0,Q_t)$
测量噪声 $v_t\sim\mathcal{N}(0,R_t)$

3. 关键流程与公式

预测（Prediction）
$\begin{cases} \bar x_t = f(x_{t-1},u_t) \\[2pt] \bar P_t = F_t\,P_{t-1}\,F_t^\top + Q_t \end{cases}$
- $f$ ：运动模型（差动／自行车）
- $F_t = \frac{\partial f}{\partial x}$ ：雅可比
更新（Correction）
$\begin{cases} K_t = \bar P_t\,H_t^\top\,(H_t\,\bar P_t\,H_t^\top+R_t)^{-1} \\[2pt] x_t = \bar x_t + K_t\,(z_t - h(\bar x_t)) \\[2pt] P_t = (I - K_t\,H_t)\,\bar P_t \end{cases}$
- $h$ ：观测模型
- $H_t = \frac{\partial h}{\partial x}$ ：雅可比
- $K_t$ ：卡尔曼增益

4. 常见算法

EKF‐SLAM
- 状态扩展为 $[x_t, m^1, m^2, \dots]$ ，复杂度 $O(n^2)$
FastSLAM
- 粒子滤波＋每粒子独立 landmark‐EKF，复杂度 $O(M\log n)$
Gmapping

优缺点对比

特性	Filter‐based	Graph‐based
估计对象	当前位姿 + 地图	全轨迹 + 地图
计算复杂度	EKF $O(n^2)$	优化边多时开销大
适用场景	小范围、低速	中大型环境、需全局一致性
回环处理	依赖线性化更新／重采样	直接添加回环边，全局优化

激光slam

基本要素

传感器
- 惯性测量单位 (IMU)：角度、角速度
- 轮式里程计 (Wheel Odometry)：线速度、累计里程
- 激光雷达 (Lidar)：高频点云扫描
地图类型
- 覆盖栅格地图（Occupancy Grid Map）：结构化、易识别
- 点云地图（Point Cloud Map）：高精度三维表示

核心技术

帧间匹配算法
- ICP（点到点）：简单但对点云密度与噪声敏感
  
  PI-ICP（点到线）：利用线特征，抗噪性更强
  
  NDT（高斯分布）：将扫描/地图划分为高斯单元，多用于 3D
  
  CSM（相关匹配）：“暴力”搜索，鲁棒性高，不会陷入到局部特征值
  
  深度学习配准：端到端网络或特征学习（如 LOAM 中的边角特征）
回环检测
- Scan-to-Scan：已弱化，信息量少，易误匹配
  
  Scan-to-Map：主流（Cartographer），实时与历史子图匹配
  
  Map-to-Map：多帧聚合匹配，需高效数据结构
  
  特征法：Bag-of-Words、Scan Context、Segmatch
  
  分层检测：粗匹配 + 精细配准；Branch & Bound + Lazy Decision
后端优化(补充)
- Pose Graph 优化
  - 节点：机器人位姿；边：相对位姿约束
  - 工具：g2o、Ceres
  - 稀疏化：剔除冗余边，加速收敛
- 因子图与滑动窗口
  - 因子图框架：灵活添加多传感器因子
  - 滑动窗口：限定优化范围，保证实时性

2D激光slam

2D 激光 SLAM 框架对比

框架	配准算法	后端优化	适用场景
Gmapping	粒子滤波+ICP	无	资源受限平台、简单室内
Hector SLAM	CSM	无	无里程计依赖、高动态环境
Cartographer	CSM+梯度优化	Pose Graph	2D/3D混合、大范围室外
LeGO-LOAM	特征+ICP	滑动窗口	室外地形变化、兼容多传感器

基本要素

传感器
- 惯性测量单位(IMU): 角度
- 轮式里程计(Wheel Odometry)：距离
- 2D激光雷达数据(Lidar)
2D激光雷达Slam的输出
- 覆盖栅格地图（Occupany Grid Map）：更好识别，更多
- 机器人的轨迹 or PoseGraph

核心技术

帧间匹配算法

PI-ICP (Point-to-Line ICP)
- 使用点到线距离准则，收敛速度快、抗噪声能力强
CSM (Correlation Scan Matching)
- 基于相关性搜索，精度高但受激光分辨率限制
梯度优化方法
- 将配准问题建模为非线性最小二乘（如 Karto-SLAM），能够实现全局最优但对初值敏感
State-of-the-Art
- 将 CSM 与梯度优化结合，兼顾实时性与配准精度

回环检测策略

Scan-to-Map
- 当前激光帧与历史局部子图匹配，主流于 Cartographer
Map-to-Map
- 多帧子图整体匹配，覆盖更大范围的闭环识别
分支定界 & 延迟决策 (Branch & Bound + Lazy Decision)
- 金字塔剪枝：多级地图加速相关性搜索
- 延迟闭环验证：在粗匹配后再提取最大一致子集，剔除误检
- 有效缓解回环误匹配，但在几何对称环境下仍存在挑战

备注：Cartographer 同时使用了 Scan-to-Map、Branch & Bound 和 Lazy Decision，在大多数室内外应用中表现优异，但在强几何对称区域仍可能出现误检。

2D 激光 SLAM 的发展历程

一、基于滤波（Filter-based）

EKF-SLAM（1990s）
- 最早的特征点图（feature-based）滤波方法；导航多结合网格地图（grid-based）使用
FastSLAM（2002–2003）
- 引入粒子滤波与独立 landmark-EKF，真正开启高效 SLAM 时代
GMapping（2007）
- 基于粒子滤波的栅格 SLAM；加入重要性采样改善 proposal 分布
- 局限：对所有粒子一视同仁的里程计传播易受延迟与噪声影响，缺乏后端全局优化
Optimal RBPF（2010）
- MRPT 库实现；每步多次传播选最优，提高精度
后续少有重大进展

二、基于图优化（Graph-based）

Globally Consistent Range Scan（1997）
- 首个全局一致性范围扫描建图；采用非线性最小二乘但未利用稀疏性优势
Incremental Mapping of Large Cyclic Environments（1999）
- 开创跨大规模闭环环境的增量式图优化；具备 map-to-map 约束但忽略稀疏矩阵优化
Karto SLAM（2010）
- 基于 SPA（Sparse Pose Adjustment），改进稀疏优化框架，仍有性能瓶颈
Cartographer（2016）
- 实时构建局部子图，结合 Scan-to-Map、回环检测与全局稀疏优化，性能与精度显著提升

小结：Filter-based 方法奠定了 SLAM 基础，Graph-based 技术依托稀疏优化与回环约束，实现了大规模、实时且高精度的 2D 激光 SLAM。

2D 激光SLAM的应用

数据预处理（至关重要）
- 轮式里程计标定
  - 离线标定：标定轮径、轮距等几何参数，为算法提供准确的初始运动模型
  - 在线标定：在实际运行中实时校正轮径偏差，补偿地面材质、载重变化带来的误差
- 激光雷达运动畸变校正
  - 考虑激光扫描的时间戳与机器人运动的叠加效应
  - 基于里程计/IMU 数据对白点云做插值补偿，消除因平台旋转或平移导致的畸变
- 多传感器时间同步
  - 统一里程计（ODOM）、MCU 与主机（PC／NPC）采集的时钟
  - 消除从电机驱动到控制单元，再到上位机打时戳过程中产生的延迟
  - 尤其在快速旋转时，同步误差会显著影响地图构建与位姿估计
这样，才能确保后续配准、回环检测和全局优化都建立在可靠的输入数据之上。
实际环境中的问题
- 动态物体
- 环境变化
- 几何结构相似环境
- 建图操作复杂 （最复杂的问题）
- 全局定位（对 2D 来说难，需 MHT 多假设跟踪）
- 地面材质的变化（里程计标定于硬地面，换路面误差增大）
- 地面凹凸不平（里程计漂移）
- 机器人载重的改变

2D 激光SLAM的趋势 —— 与视觉融合

视觉信息优势

高精度视觉里程计 (VO/VIO/VWO)
- 补偿轮式里程计在地面材质、路面凹凸及载重变化下的漂移
丰富的视觉特征
- 构建稠密、语义化地图，提升回环检测和全局定位的鲁棒性

视觉–激光融合带来的关键改进
1. 动态场景与环境变化
  - 视觉特征可区分移动/静态对象，减少伪回环
2. 几何相似环境下的误检
  - 纹理信息补充几何数据，降低误匹配
3. 全局定位与闭环检测
  - 视觉回环与激光回环互为验证，增强全局一致性
4. 里程计误差补偿
  - 视觉增量位姿修正轮式里程计误差，提高地图精度
注意事项

视觉在无纹理（如白墙）环境中性能下降，可结合深度或语义信息缓解
融合设计需平衡计算量与精度，否则易出现“1+1<2”的效果

3D激光SLAM

基本要素

3D 激光SLAM的输入
- imu数据
- 里程计数据
- 3D激光雷达数据（稠密图）
3D 激光SLAM的输出
- 3D点云地图（如高分用点云地图来做路径规划）
- 机器人轨迹 or PoseGraph

核心技术

3D激光SLAM的帧间匹配方法
- Point-to-Plane ICP
- NDT
- Feature-based Method（在点云上提取特侦点，3d特征的匹配方法和视觉有点相似了）(LOAM(Line Point和Planar Point))
3D激光SLAM的回环检测方法（和2d没有什么区别）
- Scan-to-Scan
- Scan-to-Map
- Branch and Bound & Lazy Decision

3D激光Slam的发展

LOAM (Lidar Odometry and Mapping)
- 纯激光方案
- 假设设备在一定的 Sweep 区间内以匀速运动，且没有回环检测
- Sweep 假设：在其论文中，Sweep 时间为 1 秒，但在实际中可能存在不合理之处
- 该方法在实验中表现不错，广泛应用于激光 SLAM 任务中
V-LOAM (Visual-Lidar Odometry and Mapping)
- 视觉与激光融合
- 仍然假设匀速运动，但与 LOAM 的区别在于：利用视觉（VO）作为里程计提供初始解
- Sweep 假设：误差偏移被假设为线性（与 LOAM 的不同之处在于对偏移的处理方式）
- 视觉提供更高频的运动估计（60 Hz），高于激光雷达的频率，有助于提供更精确的初始估计
VELO (Visual-Lidar Odometry)
- 视觉与激光融合
- 不再假设匀速运动，去除了运动畸变假设
- 引入了回环检测，进一步提高系统的精度和鲁棒性，适用于更复杂的环境

这些方法逐渐演变，从最初的激光里程计到视觉-激光融合，再到去除运动畸变和加入回环检测，使得 3D 激光 SLAM 能够在更复杂的动态环境下稳定运行，并且能够处理大范围的场景。

3D激光Slam的应用

数据预处理（至关重要）
- 轮式里程计标定
- 激光雷达运动畸变校正
- 多传感器时间同步
与视觉融合
- 3D激光雷达为视觉提供深度信息(因为3d信息量很大（在V-LOAM论文中有提及）)
- 视觉辅助激光雷达进行运动畸变去除（因为3d相比于2d来说，其运动畸变是极其严重的（因为运用于室外无人车等高速场景）），对位姿估计非常有用。
- 视觉辅助回环检测
- 视觉提供精确里程信息

激光SLAM中的问题（更多在2D上）

退化环境 (Degeneration Environment)
- 问题描述：例如狭窄走廊或封闭空间，通常在 SLAM 中表现为局部地图不稳定，缺乏足够的特征来进行有效的定位和建图
- 影响：由于环境的几何结构单一，激光雷达无法获取多样的参考信息，导致定位精度下降
- 常见场景：长走廊、简单房间、直线结构明显的环境
地图的动态更新 (Map Update)
- 问题描述：环境变化导致地图需要实时更新，如移动物体的加入或场景结构的改变
- 解决方案：如 DPG（Dynamic Programming Graph）等方法，尽管能实现动态更新，但在实际应用中，处理起来较为复杂且不够高效
- 挑战：需要平衡地图更新速度与精度，确保系统在动态环境中不会因数据延迟或误差积累而失效
全局定位 (Global Localization)
- 问题描述：在没有已知地图或仅部分地图信息时，激光 SLAM 在进行全局定位时容易出现失效或误差积累
- 挑战：尤其在 2D 场景下，激光 SLAM 对初始位姿的依赖较强，容易导致定位漂移
- 解决方案：例如 MHT（Multiple Hypothesis Tracking）方法，通过生成多个假设来优化全局定位，或者结合视觉方案提高定位准确性
动态环境定位 (Dynamic Localization)
- 问题描述：在包含动态物体的环境中，SLAM 系统往往难以区分静态与动态物体，进而影响地图精度和定位稳定性
- 应用场景：如商场、繁忙街道等
- 解决方案：可以通过 DTAM（Dynamic Texture and Motion）进行动态物体的跟踪，结合路径规划算法来优化机器人的运动轨迹，提升动态环境下的稳定性和精度

这些问题在不同的 SLAM 实施中会有所体现，尤其是在复杂环境或动态场景中，如何进行效的动态更新与全局定位，是当前 SLAM 系统研究中的重点。

Q&A

老师说：

激光-惯导融合没有什么必要，没有里程计那么好用（假定是便宜的IMU）
视觉-激光的体系比较深，不好讲清楚
IMU+雷达挺靠谱的。但是加个里程计更加靠谱
3Dslam在自动驾驶上是很重要的（因为要高精地图（但不仅仅是3Dslam）），而且包含所有的室外场景。
摄像头数据和3D雷达数据的区别：3D雷达数据可以给摄像头数据提供深度信息。（视觉-激光融合方案）
里程计是有姿态信息的
VEO？+轮式里程计是一个非常有前途的对象。VIO（激光slam的普遍方案）。因为轮式里程计的精度比IMU高太多了，所以VEO的效果应该比VIO高很多（但只限于平面环境下）
2Dslam在无人机上的应用是有的，但是效果一般。（视觉好一点，因为3d激光太贵了）
图优化当回环错误会导致图变形
在室外用GPS辅助激光slam更好。虽然GPS有10cm的误差，但是不飘移。
激光雷达一帧就是其旋转一圈。
里程计得到位姿（基础性学习，老师说这块比较容易知道）
GPS+IMU+ODOM不会是一般的方案，基本上需要加上slam。一起做。
cartographer还是非常完善的开源算法的。
基于地图的AMCL定位当然是可以的，但是AMCL的问题在于它有非常多的缺陷。就是当地图有大的变动的时候，AMCL非常容易发生错误，而且它的全局定位基本不成功。拉毛。
cartographer3d用的是点云，2d用的是栅格
里程计一般不是单一传感器，而是两个轮子共同组成的系统。
3D导航一般是在室外高精地图上
融合的传感器越多，意味着定位地越鲁棒而不是越好
激光的运动畸变很难一下说清楚
滤波和非线性优化的融合还是很好实现的
退化环境：走廊等（特别对二维来说），就是传感器无法进行足够的约束。
3D雷达的标定方法和图像标定非常相似。
在提供足够约束的情况下，2Dslam和3Dslam没什么区别。
特征点的判断方法很多（如曲率判断（大的就是角点）等方法，以及各种数据集）
最好自己写一个，而不是用ROS的包。效果缺失不是很好（ROS的导航包不是很完善，ROS本身也有缺陷（不确定是否包含ROS2））。自己写的话，包括代码的复现以及通信系统的完善。ROS的导航包非常一般（做demo还行）
卡尔曼滤波中的误差系统模型？基本上是根据feature-based的特征地图，提取特征，然后通过特征匹配的不确定度，来确定误差的协方差。
课程中暂时不涉及3Dslam