SLAM框架讲解

系列文章目录

SLAM是ROS机器人中一个重要的研究方向，当然也是有难度的，涉及到的知识非常广，数学知识要足够深厚，这一节主要介绍相关的基本概念，不介绍数学知识。

---

---

前言

SLAM是Simultaneous Localiza and Mapping的缩写，意为：同时定位与建图。简单的来说，机器人在一个未知的环境中运动，在移动过程中根据传感器的信息，一边计算身在何处，一边构建周围的环境，从而实现机器人的自主定位和导航 SLAM自1998年被提出来，已经过去三十几年了。最开始SLAM研究侧重于使用滤波器理论，最小化运动物体的位姿和地图的路标点的噪声。之后，研究者们借鉴了SFM(structure from motion)中的方式，以优化理论为基础求解SLAM问题，这一方式取得了重大的成就，并在视觉SLAM中取得了主导地位。 按照传感器来分类，SLAM主要分为激光和视觉两大类。其中激光SLAM技术相对于视觉SLAM技术成熟得多，这一节中也是着重介绍激光SLAM。

---

一、激光SLAM的基本框架

1.1、传感器信息处理
激光SLAM可以获取得传感器得信息一般有里程计、激光雷达数据和IMU数据，对于里程计数据，常见得有两轮差速运动模型和三轮全向地盘运动模型；而激光雷达数据则会由于传感器得移动而导致得采集到的数据发生畸变，需要通过估计或者里程计辅组等方法对畸变进行矫正。

1.2 前端配准方法（Visual Odometry简称OV）
在实现OV时需要考虑特征点，根据输入激光数据提取特征，之后再对两帧数据得特征点位置进行匹配。值得注意得是仅仅凭借两帧数据得估计往往不够，需要吧特征点缓存成一个小地图，计算当前帧与地图之间得位置关系。这样才能算得上是一个完成得前端配准方法。
简单来说就是，将两个或者以上的坐标系中的点云数据转化到统一的坐标系中的数学计算过程，本质上就是空间坐标的变换。空间的坐标变换可以由三类参数确定：尺度、旋转和平移。

• ICP匹配方法（point-point）：ICP算法能够使不同坐标下的点云数据合并到同一个坐标系中，首先是找到一个可用的变换，配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换。其实就是找到一个旋转矩阵R和平移矩阵T，使得两个点的对齐匹配转换。
• PL-ICP（point-Line）：与普通的ICP相比，PL-ICP是连续的。由于激光雷达的扫描出的数据是离散的，我们不能保证两次在不同位置上扫描同地点的扫描点全部落在同一个物理点上，这就是点对点的距离产生的误差，而PL-ICP中 则使用点到线的距离作为误差。由于PL-ICP是 点到线的误差，所以PL-ICP为二阶收敛，比PP ICP -阶收 敛的速度更快，精度也比P-ICP更加精准。在PL-ICP中通过点与线进行匹配，将当前帧的数招根据初始价位置投影到参考帧的坐标下，对当前帧的点i，在参考帧中寻找到最近的两点（P1，P2），再对误差进行计算，去除快差过大的点；最后的除最小化误差的函数。
• 基于优化的匹配方法：如果把要优化的目标通数看成山容的话，可以把里优化为多、宿席下仙鹤有束，数随机化为个随机数可以看做在山谷的某个位置以0进度开始往下浪。 目标据数的规度可以看做给石头施加的力。在给定一个目标函数时，可以把激光的帧间匹配问题转 换为求解目标函数的极值问题。

1.3 后端优化（Optimization）
在后端优化上，通常需要考虑更长一段时间或者所有时间上得状态估计问题，不仅仅使用过去得信息来更新自己得状态，也会用未来得信息更新自己得状态。
前端配准只能给出短时间内的轨迹和地图，会产生误差累积，导致地图在长时间内并不是非常准确。所以在前端配准的基础上，我们再构建出一个尺度、规模更大的优化问题，用以考虑长时间的最优轨迹和地图这就是后端优化。
基于滤波器的激光SLAM方法：机器人SLAM的过程是:先控制机器人到达一个位姿，然后再进行观测。但是问题出现在了我们是由控制数据利观测数据来求机器人位姿，这就出现了一个逆向的过程即: 我们是先有位姿再得到观测数据却变成了先得到观测数据再来求位姿的过程。通俗来讲就是我们需要由结果(观测数据)来反推原因(位姿) ;因此这里就引入了贝叶斯公式，贝叶斯公式的存在就是为了求解这种由结果反推原因的问题。

• 贝叶斯滤波估计的是概率分布，不是具体的数值，是一大 类方法的统称，它是一个抽象的表达形式一对于不同问题有不同的实现方式(卡尔曼家族、粒子滤波)，迭代估计形式。贝叶斯滤波主要分为预测和修正(更正)两个过程。
• 粒子滤波：粒子滤波通过非参数化的蒙特卡洛(Monte Carlo)模拟方 法来实现递推贝叶斯滤波，适用于任何能用状态空间模型描述的非线性系统，精度可以逼近最优估计。粒子滤波器E具有简单、易于实现等特点，它为分析非线性动态系统提供了一种有 效的解决方法，从而引起目标跟踪、信号处理以及自动控制等领域的广泛关注。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运管，从而获得状态最小方差分布的过程。
• 基于图优化的激光SLAM方法：
  

1.4 回环检测（Loop CLosing）
什么是回环检测呢？在SLAM问题中，位姿的估计往往是一个递推的过程，即由上一帧位姿解算当前帧位姿，因此其中的误差便这样一帧一帧的传递下去，也就是我们所说的累计误差。我们的位姿约束都是与上一帧建立的，第五帧的位姿误差中便已经积累了前面四个约束中的误差。但如果我们发现第五帧位姿不一定要由第四帧推出来，还可以由第二帧推算出来，显然这样计算误差会小很多，因为只存在两个约束的误差了。像这样与之前的某一帧建立位姿约束关系就叫做回环。回环通过减少约束数，起到了减小累计误差的作用。

那么我们怎么知道可以由第二帧推算第五帧位姿呢？也许第一帧、第三帧也可以呢。确实，我们之所以用前一帧递推下一帧位姿，因为这两帧足够近，肯定可以建立两帧的约束，但是距离较远的两帧就不一定可以建立这样的约束关系了。找出可以建立这种位姿约束的历史帧，就是回环检测。

1.5 建图（Mapping）
在SLAM中，构建的地图就是所有路标点的集合，一旦确定了各个路标点的位置，那么就可以说我们完成了建图。但即使是地图，也会有许多的不同需求，大致分为：定位、导航、、避障、重现、交互这五个方面。

二、激光雷达数据的采集和处理

LiDAR(Light Detection and Ranging)，是激光探测及测距系统的简称，另外也叫做Laser Radar。采用激光器作为发射光源，采用光电探测技术手段的主激光雷达遥感设备。激光雷达是激光技术与现代光电探测技术结合的先进探测方式。由发射系统、接收系统、信息处理等部分组成。发射系统是各种形式的激光器，如二氧化碳激光器、半导体激光器及波长可调谐的固体激光器及光学扩束单元组成；接收系统采用望远镜和各种形式的光电探测器，如光电倍增管、半导体光电二级管、雪崩光电二极管、红外和可见光多元探测器件等组合。

2.1 激光雷达测距原理

方法一：三角测距
三角法的原理如下图所示，激光器发射激光，在照射到物体后，反射光由线性CCD 接收，由于激光器和探测器间隔了一段距离，所以依照光学路径，不同距离的物体将会成像在CCD 上不同的位置。按照三角公式进行计算，就能推导出被测物体的距离。

方法二：飞行时间（TOF）
激光器发射一个激光脉冲，并由计时器记录下出射的时间，回返光经接收器接收，并由计时器记录下回返的时间。两个时间相减即得到了光的“飞行时间”，而光速是一定的，因此在已知速度和时间后很容易就可以计算出距离。

两种方法的对比
测距距离。从测量的原理来说，TOF到的测距相对比较远，三角测距雷达测距相对较近。通过三角测距的图解可以知道，如果三角测距雷达测量的物体越远的话，那么返回到CCD上的位置差别也会越来越小，如果超过一定的限度，那么CCD就无法分辨了。而且三角测距雷达的信噪没有比TOF雷达的高，由于TOF雷达采用脉冲激光的方式，可以严格控制视野中外界环境的影响。
采样率。在转速一定下，采样率决定了每一帧图像的点云数目以及角分辨了。如果角分辨率越高，点云数量就会越多，那么对周围环境的精度也就越来越高。在市场上的三角测距雷达的采样率一般在20KHz以下，而TOF雷达会更高，这是由于三角测距的运算量更大所导致的。

精度。由于原理的不同，三角测距在距离下精度会非常的高，但是距离越远，测量精度就会越来越低。而TOF测距依赖的是飞行时间，时间精度并不会随距离的变化而变化。

2.2 运动畸变的去除
由于激光雷达的频率有限，机器人移动而激光点的数据不能实时获得，就会发生畸变。如果激光雷达以90°/s的速度旋转，如果5Hz的雷达，那么每200ms可以获取一帧数据，每帧数据相差18°，这样就导致了每一个激光点都有不同的基准位置，这样就会产生运动畸变。
方法一：纯估计方法----VICP
在ICP中我们没有考虑到激光的运动畸变和当前激光数据是有错误的，而作为ICP的变种，VICP在考虑到机器人运动的同时也匹配了估计机器人的速度。
方法二：里程计辅助
如果激光雷达的帧率过低，那么匀速运动的假设就不成立，不能真实的反应机器人的情况，数据处理和状态过程耦合。所以我们需要传感器进行辅组（odom/IMU），这样我们就可以有极高的更新频率（200Hz），也可以比较准确的反应运动情况，有比较高精度的局部位置枯寂，这样我们就可以实现估计与预处理。

• 惯性测量单元（IMU）：惯性测量单元可以直接测量角速度和线速度，可以得到较高的角速度和测量精度。测量频率高，可以达到1KHz以上，但线加速度比较差，在二次积分的局部精度也很差。
• 轮式里程计（odom）：可以直接测量机器人的位移和角度，具有较高的局部角度测量精度和局部位置测量精度，更新速度虽然没有IMU高，但是也能达到200Hz。

总结

本节中对SLAM中的常用概念进行了简单的解释。