激光SLAM基础(1) —— 激光SLAM框架和基本数学理论_二维激光 三维激光 slam

1、SLAM分类

1.1、基于传感器的分类

1.2、基于后端的分类

  基于图优化方法的激光SLAM有cartographer，基于滤波器的方法有GMapping。

13、基于图的SLAM

  基于图优化方法的激光SLAM，分为前端和后端。
  前端负责实现激光里程计，包括帧间匹配、回环检测等，旨在根据激光雷达的观测，构建误差函数，用于后端优化。
  后端则负责进行优化，如根据前端建立的约束，优化位姿；根据回环检测的结果，优化位姿图；发布地图等。后端的作用是修正位姿和地图等。

2、激光SLAM算法（基于优化的算法）

2.1、激光SLAM算法的流程

  基于图优化方法的激光SLAM和视觉SLAM的流程相同，只是其中用到的算法不同

2.2、激光SLAM常用算法

一、数据预处理

1. 里程计标定
     轮式里程计的机械标称值并不代表真实值，实际误差可能较大。所以需要标定后才能使用。 里程计的精度对于机器人定位和建图至关重要。
2. 激光雷达运动畸变去除
     每一帧激光数据的采集需要时间，在采集期间如果机器人运动会使测量值产生畸变。运动产生的畸变会让数据严重失真，影响匹配精度。
3. 不同系统之间的时间同步

二、帧间匹配算法（激光SLAM核心部分）
  帧间匹配算法直接影响激光SLAM的效果，后端优化只是消除该过程所积累的误差，帧间匹配估计的位姿越准确，后期建图效果越好。常用算法如下图所示：

  作者笔记：
  不同的激光slam算法使用的帧间匹配算法也不相同。
  如gmapping利用粒子滤波的方法，将当前观测的激光数据，建立Likelihood模型，利用粒子滤波，在已知地图中实现精确定位，再根据位姿将当前观测进行建图。其算法的里程计是一个scan-to-map类型的。
  cartographer是一个集成度很高的算法，其帧间匹配算法类似于Karto SLAM。其先根据最近几帧的观测，构建一个submap，然后将当前帧于构建的submap进行匹配。在匹配算法上，其先利用CSM分支定界的方法，快速实现初步定位，然后利用基于概率地图得分的优化方法，实现精确的位姿求解。
  在2D激光SLAM中，几乎不会用到 ICP 类和 NDT 类的帧间匹配算法。因为2D激光雷达的返回数据只有单线数据，测量结果中的结构信息过于简单，且重复度高，利用此类算法很容易出现误匹配，导致历程计精度低。且此类算法遇到纯旋转问题时，位姿估计会产生很大的旋转误差，几乎是无解的。所以此类算法，可以在结构信息丰富的3D激光SLAM中常见到，效果也非常好。

三、回环检测方法

  实际应用中实际多使用 Map-to-Map 来进行回环检测。

  作者笔记：
  激光SLAM的回环检测不如视觉SLAM的回环检测容易，哪怕是基于特征的方法，由于环境中的三维结构存在重复，所以激光SLAM的回环检测并不简单。目前本人所知的，有基于结构特征直方图的，基于聚类思想的，基于深度学习的，也有基于图像辅助的回环检测。
  后续我可能会列出具有回环检测的激光SLAM算法，以及其所使用的方法。

四、后端优化方法
  后端算法的实现大同小异，不同的库的使用方法都不太相同，但是核心思想都是相同的，即根据前端构建的残差函数，计算每个测量的残差，构建最小二乘问题，然后利用高斯牛顿、LM方法等，求解构建的最小二乘问题，实现位姿变换的求解。常使用的库有Ceres，G2O、gstam等。

2.3、激光SLAM在实际环境中的问题

  四大类问题：
1、退化环境（Degeneration Environment）：走廊问题、空旷环境
2、地图的动态更新（Map Update）：地图的变更到时定位出错
3、全局定位（Global Localization）：绑架问题
4、动态环境定位（Dynamic Localization）：人、车和其他物体影响
  具体问题如下：

  这些问题都是很工程话的问题，有兴趣的读者可以选择一些具有学术价值的方向进行研究，如退化环境问题、全局定位问题、动态环境问题。这些问题在SLAM落地中是非常令人头疼的问题，一般在SLAM研究中，我们会假设这些问题是很少遇到的，即外界环境是静态且相对简单的。

3、激光SLAM算法介绍

3.1、2D激光SLAM

一、2D激光SLAM的输入和输出：
输入：IMU数据、里程计数据、2D激光雷达数据
输出：覆盖栅格地图、机器人的运动轨迹或PoseGraph
二、2D激光SLAM常用帧间匹配方法

三、2D激光SLAM的回环检测方法

四、2D激光SLAM经典算法

1. 基于滤波器的方法：
   Gmapping ：基于粒子滤波，每一个粒子都带有一个地图
   Optimal RBPF：Gmapping的进一步优化
2. 基于图优化的方法：
   Cartographer：算法与Karto-SLAM原理类似，更完整，使用CSM+SBA
   Viny-SLAM：作者也没有仔细看过这篇论文，但是论文中的实验结果精度是要高于Cartographer的。算法利用IMU+2D激光雷达，实现了精度和速度都高于Cartographer的算法。

  作者笔记：
  个人人为，2D激光SLAM算法已经非常成熟了，其已经有一套完整的实现流程，读者可以查看相关算法的代码来提高自己。其外，我还人为Cartographer算法的代码集成度极高，利用了很多C++的特性，新手研读起来比较费劲，且比较难做二次开发。所以建议阅读一些简单的代码，如Gmapping、Karto，进行入门学习。然后再阅读Cartographer。

五、2D激光SLAM的发展趋势——与视觉融合

3.2、3D激光SLAM

一、3D激光SLAM的输入和输出
输入：IMU数据、里程计数据、3D激光雷达数据
输出：3D点云地图、机器人轨迹或PoseGraph
二、3D激光SLAM常用帧间匹配方法

三、3D激光SLAM与激光进行融合

4、激光SLAM的数学基础

4.1、位姿表示

  此处可以参考两个内容，在此不做展开（关键是自己重写太麻烦）：
1. 视觉SLAM中的数学基础 第一篇 3D空间的位置表示
https://www.cnblogs.com/gaoxiang12/p/5113334.html
2. 视频演示讲解：
https://www.youtube.com/watch?v=8XRvpDhTJpw

4.2、2D位姿变换

  从4.1的资料1中，我们知道了2D机器人的位姿表示和坐标变换的概念，不同坐标系之间2D位姿变换关系如上图所示。
  设机器人坐标系B中一坐标点$P_B$，其在世界坐标系O中的坐标为$P_O=T_{OB}\cdot P_B$，反过来就有$P_B={T_{OB}}^{-1}\cdot P_O=T_{BO}\cdot P_O$。
  如上图右边，由机器人A在O中的位姿$(x_A,y_A,{\theta }_A)$，得到坐标系A到坐标系O的变换$T_{OA}$，同理有$T_{OB}$。设坐标系A到坐标系B之间的变换矩阵为$T_{BA}$，则有$T_{BA}=T_{BO}\cdot T_{OA}={T_{OB}}^{-1}\cdot T_{OA}$(先将坐标系A转换到坐标系O，再由坐标系O转换到坐标系B)。
  2D坐标系中，变换矩阵无法传递角度，则最后的坐标由下式表示：