SLAM算法的简明原理

Simultaneous Localization and Mapping (SLAM)的处理方法主要分为滤波和图优化两类。

基于滤波器的SLAM

其中，黑色的为预测（经验），红色的为观测

图例	观测	预测（经验）	概率
图一	未观测到特征	初始经验无	各个位置等概率
图二	观测到特征	经验更新	三个有门位置的概率增加
图三	未观测到新的特征	经验保持	随着运动模型，概率平移，并平滑
图四	观测到新的特征	经验更新	当前位置为第二个门的概率增高

其中，Z为观测，X为状态，U为运动模型。

常用算法

算法	优点	缺点
扩展卡尔曼滤波器（EKF）	简单易用	特征变多时，计算量增大
单点RANSAC算法应用于EKF	提高了外点的剔除速度，提高了SLAM系统的鲁棒性
粒子滤波器（PF）	表达非高斯噪声	计算量较大，不适用于建图
Rao-Blackwellized粒子滤波器（RBPF）	综合了PF和EKF的优点
Fast SLAM	简化了计算复杂度	复杂环境计算量剧增，存在例子退化问题

图优化SLAM

图优化是什么？

在graph-based SLAM中，机器人的位姿是一个节点或顶点（vetex），位姿之间的关系构成边（edge），比如t+1时刻和t时刻之间的odometry关系构成边，或者由视觉计算出来的位姿转换矩阵也可以构成边。一旦图构成了，就要调整机器人的位姿去尽量满足这些边构成约束。

图优化SLAM能分解成俩个任务：
1.构建图，机器人位姿当做顶点，位姿关系当做边，称为前端。
2.优化图，调整机器人位姿顶点尽量满足边的约束，称为后端。

为了表述得更加形象，我们举两个例子来说明这个问题，其中对第二个例子的后面还引入了置信度信息，从而得到更为准确的结果。

示例

示例1 基于闭环检测

如下图所示，假设一个机器人初始起点在0处，然后机器人向前移动，通过编码器测得它向前移动了1m，到达第二个地点。接着，又向后返回，编码器测得它向后移动了0.8米。但是，通过闭环检测，发现它回到了原始起点。

首先构建位姿之间的关系，即图的边：

线性方程组中变量小于方程的个数，要计算出最优的结果，使出杀手锏最小二乘法。先构建残差平方和函数：

为了使残差平方和最小，我们对上面的函数每个变量求偏导，并使得偏导数等于0.

整理得到：

接着矩阵求解线性方程组：

所以调整以后为满足这些边的条件，机器人的位姿为：

示例2 基于LandMark

如下图所示，假设一个机器人初始起点在0处，并观测到其正前方2m处有一个路标。然后机器人向前移动，通过编码器测得它向前移动了1m，这时观测到路标在其前方0.8m。

在这个图中，我们把路标也当作了一个顶点。构建边的关系如下：

转换

残差平方和：

求偏导：

最后整理并计算得：

得到路标和机器人位姿：

我们知道传感器的精度是有差别的，也就是说我们对传感器的相信程度，即置信度，应该不同。比如假设这里编码器信息很精确，测得的路标距离不准，我们应该赋予编码器信息更高的权重，假设是10。重新得到残差平方和如下：

求偏导得：

转换为矩阵：

最后计算得到:

将这个结果和之前对比，可以看到这里的机器人位姿x1更靠近编码器测量的结果。

基于滤波器和基于图优化算法的简单比较

基于滤波的方法是以显性的递推误差估计为基础的串行运算，难以消除误差累积；而图优化方法基于不同时间测量值的集合估计，寻找参与计算的各方的均衡全局最优值，误差累积增长较为缓慢。

详细比较请参考：
Strasdat. 《Visual SLAM: Why Filter?》

参考链接：
https://heyijia.blog.csdn.net/article/details/47686523