一、SLAM的问题起源与发展

​ 起初，机器人的定位与建图问题被看成是两个独立的问题来研究。

​ 所谓机器人的定位问题，是在已知全局地图的条件下，通过机器人传感器测量环境，利用环境测量信息与地图之间存在的关系求解机器人在地图中的位姿。定位问题的关键是必须事先给定环境地图。例如分拣仓库中的二维码路标，就是人为给定的环境地图路标信息，机器人只需识别二维码就能得到机器人当前的位姿。

​ 机器人的建图问题，是在已知机器人全局位姿的条件下，通过机器人传感器测量环境，利用测量地图路标时的机器人位姿和测量距离及方位信息，求解出观测到的地图路标点坐标值。建图问题的关键是必须事先给定机器人观测时刻的全局位姿。例如装载了GPS定位的测绘飞机，飞机由GPS提供定位信息，基于GPS定位信息完成地形测绘。显然，这种建立在环境先验基础上的定位和建图具有很大的局限性。倘若把机器人放置在未知环境中，机器人将无法实现定位与建图，陷入进退两难的局面。

​ 2006年，Durrant-Whyte和Bailey在其研究课题中首次使用SLAM这一名词进行表述。SLAM按照时间可以简单划分为古典SLAM和现代SLAM。

(一)古典SLAM

​ 传统的定位和建图问题，采用构建机器人运动的数学模型的方法来解决，显然这种方法没有考虑到实际机器人问题中存在的众多不确定性因素，如传感器测量噪声、电机控制偏差、软件近似精度等等。因此实际上需要特别关注机器人中的不确定性问题。

​ 1986年，Smith等人，将机器人定位与建图问题转换为状态估计问题，在概率框架之中展开研究，利用扩展卡尔曼滤波(EKF)、粒子滤波（PF）等滤波方法来求解。其中有两个开创性的点，算得上是SLAM问题的起源：

1. 第一采用了基于概率论理论框架对机器人的**不确定性**进行讨论；
1. 第二将将**定位与建图**中的机器人位姿量与地图路标点作为同一的估计量**进行整体状态估计**；

​ 2006年，Whyte等人，逐渐建立起来SLAM理论体系，该理论框架的收敛性得到了验证。古典SLAM时期，滤波方法是解决SLAM问题的主要方法，例如EKF-SLAM算法，基于Rao-Blackwellized粒子滤波SLAM算法(即FAST-slam). 而基于Rao-Blackwellized粒子滤波来研究栅格地图的SLAM算法，就是著名的Gmapping算法。可以说，基于粒子滤波的SLAM算法大大提高了求解效率，使得SLAM在工程实践中具有可行性。

(二)现代SLAM

​ 贝叶斯网络中采用滤波法求解SLAM的方法，需要实时获取每一时刻的信息，并把信息分解到贝叶斯网络的概率分布中去，因此滤波方法是一种在线SLAM系统，计算代价非常大，因此只构建小规模的地图。

​ 为了进行大规模建图，在因子图中采用优化方法求解SLAM的方法被提出，优化方法的思路与滤波方法相反，它只是简单地累计获取到的信息，然后利用之前所有时刻累计到的全局信息离线计算机器人的轨迹和路标点，即优化方法是一种完全SLAM系统。随着优化方法在稀疏性和增量求解方面的突破，以及闭环检测方面的额研究，体现出了巨大的价值，例如基于视觉传感器的优化方法成为现代SLAM研究的主流方向。特别是2016年ORB_SLAM2开源算法的问世。

1.1 数据关联、收敛、一致性

数据关联

​ 将传感器观测数据与已构建出来的地图进行匹配，判断传感器观测数据中哪些路标特征是新的、哪些是旧的，并将新路标特征数据有效地融入到地图中。

严格的路标特征剔除机制是地图信息自我纠错的有效手段，闭环检测则能大大降低定位累计误差。

收敛

​ 收敛用于衡量SLAM系统在理论上的可行性。由于机器人观测模型和运动模型都具有不确定性，所以将SLAM放在概率框架下讨论，并利用状态估计去求解，即估计量都是带有不确定性的估计值。

​ 由路标特征组成的地图是实际环境特征的估计，估计出来的路标特征与实际环境特征存在偏差的原因是观测存在不充分性。换句话说，只要给机器人足够的机会对环境进行彻底的观测，所得到地图的不确定性将最终消除，即构建出来的地图路标特征会收敛于实际环境特征。

一致性

收敛一致性讨论的是估计量收敛于实际数值的问题。

1. 弱一致性收敛：观测值规模趋于无穷大时，依概率收敛。
2. 强一致性收敛：观测值规模趋于无穷大时，严格收敛。

二、SLAM的基本理论

​ Gmapping算法：滤波方法+激光+占据栅格地图

​ ORG_SLAM算法：优化方法+视觉+路标特征地图

​ 学习SLAM，需要在全局上把握理论本质，再将具体的SLAM实现算法在机器人本体上用起来。

​ SLAM是一个状态估计问题，按照求解方法的不同，已经形成了两大类别，即滤波方法和优化方法。

(一) 滤波方法

​ 滤波方法可以看成一种增量算法，机器人需要实时获取每一时刻的信息，并把信息分解到贝叶斯网络的概率分布中去，状态估计只针对当前时刻。计算信息都存储在平均状态矢量以及对应的协方差矩阵中，而协方差矩阵的规模随着地图路标数量的二次方而增长，也就是说其具有O(n²)计算复杂度。滤波方法在每一次观测后，都要对该协方差矩阵执行更新计算，当地图规模很大时，计算将无法执行下去。

(二) 优化方法

​ 优化方法简单地累积获取到的信息，然后利用之前所有时刻累积到的全局性信息离线计算机器人的轨迹和路标点，这样就可以处理大规模地图了。

​ 滤波方法和优化方法其实就是最大似然和最小二乘的区别。滤波方法是增量式的算法，能实时在线更新机器人位姿和地图路标点。二优化方法是非增量式的算法，要计算机器人位姿和地图路标点，每次都要在历史信息中推算一遍，因此不能做到实时。滤波方法的问题在于计算复杂度，而优化方法的问题在于历史信息的存储。

2.1 SLAM中的概率理论

​ 考虑到实际机器人问题中存在的众多不确定性因素，例如传感器测量噪声、电机控制偏差、计算机软件计算精度近似等，然后利用概率描述机器人中的不确定性，这样机器人中的不确定性就可以被计算和推演，这就是概率机器人学。

​ 在概率机器人位置例子中，描述机器人位置x的概率分布P(x)也称为置信度，在实际工程中可以直接取置信度最大的区间作为随机变量的估计值。

​ 虽然机器人的运动过程和观测过程都存在不确定性，但是通过不断引入观测信息，这种不确定性会越来越小。

2.2 状态估计问题

​ SLAM问题中的不确定性，即状态估计问题。

​ 在机器人的实际运动过程中，运动位移量一般由轮式里程计或者IMU反馈得到，观测通常由机器人上的激光雷达或相机来完成，运动过程和观测过程都存在误差。随时间推移，机器人的位姿将受误差影响偏离真实位姿；因机器人位姿偏差，加上观测误差，观测到的路标特征也自然会偏离真实路标特征。因为误差渗透进了各个地方，所以机器人的位姿和路标特征坐标的真实值是无法直接通过观测信息得到的。那么**SLAM问题其实就是一个状态估计问题，待估计量是机器人位姿和路标点。通过机器人运动过程和观测过程**所提供的信息，利用统计手段逐步减小状态估计量与真实值的偏差，从而完成对机器人位姿和路标特征点的估计。

​ 机器人通过观测和运动两个过程与环境进行交互，同时运行在机器人上的SLAM算法不断地对路标点$m_i$和机器人位姿$x_k$进行估计。用条件概率描述观测过程和运动过程的不确定性，分布如图中的概率运动模型和概率观测模型所示。而SLAM问题就是对路标点$m_i$和机器人位姿$x_k$的状态估计问题。

​ 从状态估计问题的概率表述中可以发现，单独的定位问题是在路标点$m$已知的前提下对机器人位姿$x_k$进行估计，单独的建图问题是在机器人各个位姿$x_{0:k}$已知的前提下对路标点$m$进行估计，而SLAM问题是同时对机器人位姿和路标点进行估计。但是，单独定位问题的概率分布乘以单独建图问题的概率分布并不等于SLAM问题的概率分布，这也说明SLAM问题并不等同于定位和建图问题。

2.3 概率运动模型

​ 机器人感知自身在环境中运动的距离，工程中常用的是前馈与反馈两种表示方法，如图7*6所示。

​ 一般假设底盘在$\Delta t$时间内以前馈速度$u_k$做匀速运动，那么底盘的运动就可以用$u_k*\Delta t$来表示，然而实际上底盘的运动速度会与控制命令有很多出入，所以底盘基本上都配备了轮式里程计来获取实际运动反馈量，轮式里程计并不需要底盘匀速运动的假设就能有效预测底盘的运动情况。这两种方式就是所谓的速度运动模型与里程计运动模型。

1.速度运动模型

​ 速度运动模型的推导基于两大假设，一是假设运动时间$\Delta t$很小，二是假设$u_k、x_k-1和x_k$都服从高斯分布。但是这两个假设性太强了，不符合实际情况。

2.里程计运动模型

​ 移动机器人一般都会配备轮式里程计用于反馈运动情况，因此里程计运动模型是主流的模型。

​ (1) 不考虑里程计的误差

​ (2) 考虑里程计的误差

​ 里程计的误差既可以用底盘系数$a_1{\sim}a_4$描述，也可以用6x6的协方差矩阵来描述。例如，ROS中的Gmapping算法使用的就是系数$a_1{\sim}a_4$；

​ 机器人利用里程计运动模型推算位姿，其不确定性的演变过程如图7-9所示。图中轨迹上的黑点表示机器人可能出现的位姿，黑点周围的密度表示概率大小，黑点越集中的地方表示出现的概率越大。随着机器人的移动，由于里程计存在的误差，直接利用里程计推导出来的机器人位姿的不确定性越来愈大，最终这些可能的位姿将分散到整个空间，即机器人位姿将变得完全随机。为了不让机器人位姿发散，必须在里程计运动模型的基础上，引入传感器对环境的观测数据。即概率观测模型。

2.4 概率观测模型

​ 工程中常用的环境观测传感器是激光雷达和相机。

传感器环境观测分为两步：

• 第一步是提取环境路标特征；
  • 例如单线激光雷达，路标特征可以使用观测角度和测量距离来表示；
  • 多线激光雷达，路标特征除了观测角度和测量距离，还能包含高层级点云轮廓信息；
  • 而相机中，路标特征可以用像素坐标、像素颜色、高层级特征等；
• 第二步是数据关联；
  • 路标特征提取出后，需要判断该路标特征是否为新路标，并将新出现的路标特征融入到已有地图中，这就是数据关联。

以激光雷达观测模型的不确定性问题为例，主要通过波束模型和似然域模型对观测不确定性进行建模，即所谓的概率观测模型。