SLAM后端基于扩展卡尔曼、粒子滤波与非线性优化的思路整理

0 说点儿什么

半个月前利用TUM的测试数据集实现了一个简单的SLAM前段视觉里程计，对前端算法的实现过程理解得比较清楚，于是想尝试一下SLAM后端的算法。经过一周的学习和编程，感觉对后端的算法可以理解，但没有一个比较清晰的思路逻辑，因此在这儿做一个思路的整理，内容会随着理解的深入逐渐填充。

1 SLAM后端

SLAM前端完成了一个短时间内机器人移动和位姿估计的任务。但是这种估计是基于短时间内甚至是仅仅两帧之间的估计，对于累积的误差和较长时间尺度下的较优位置缺少处理能力。因此，缺少后端的SLAM会在精度上出现比较大的问题。

如下图所示，每一次的位姿估计和路标点的估计都会存在些许误差，应该也需要存在一些算法，能够通过估计和预测来消减这种误差，使对运动状态和路标点的估计在即使是有噪声的情况下可以更加准确。这便是后端在SLAM算法中的作用，即如何使用获得的数据去更好的估计运动和状态。

机器人移动的轨迹与位姿估计

下面通过不同的算法的演进过程来整理针对复杂运动和状态估计的思路：

2 系统的状态估计

2.1 运动方程与观测方程

在SLAM的过程中，系统可以由运动方程和观测方程来进行描述。对于

到

的时间段内，有位姿

到

并且有路标

到

，则运动方程和观测方程可以写为：

运动方程和观测方程

上式的意义可以简单解释一下，对于运动方程来说：这一时刻的位姿可由前一时刻的位姿与输入量按运动模型fx来估计，当然还需要加上运动的输入噪声的影响；对于观测方程来说，观测数据由路标和机器人的位姿按观测模型hx来决定，此时也需要加入观测噪声的影响。

2.2 优化问题的提出

当然，对于上式来说，针对于具体的系统和传感器，上式是可以给出不同的具体形式的。而且对于SLAM系统尤其是vSLAM来说，因为特征点众多，在实际中观测方程会明显多于运动方程的数量。当理解了上式后，我们的目标就很明确了：设计一种方式，从已知的运动输入数据u和观测数据z来确定状态量x和y的分布。其也可以说，当我们存在一些运动数据和观测数据的时候，我们如何去估计状态量的分布。

针对于具体的问题，解决方案就有了不同的分支：根据fx与hx的形式可分为线性与非线性问题；根据噪声w和v的分布可分为高斯和非高斯问题。为了解决不同的问题，就有了题目中所说的不同方法：卡尔曼滤波、扩展卡尔曼滤波、粒子滤波、非线性优化等等。为了理清解决问题的思路，下面从最简单的线性高斯问题开始一步一步来解决这个问题。

3 卡尔曼滤波