激光SLAM之图优化理论

1.常用的两种优化方法介绍

      SLAM问题的处理方法主要分为滤波和图优化两类。滤波的方法中常见的是扩展卡尔曼滤波、粒子滤波、信息滤波等，熟悉滤波思想的同学应该容易知道这类SLAM问题是递增的、实时的处理数据并矫正机器人位姿。比如基于粒子滤波的SLAM的处理思路是：假设机器人知道当前时刻的位姿，利用编码器或者IMU之类的惯性导航又能够计算下一时刻的位姿，然而这类传感器有累计误差，所以再将每个粒子的激光传感器数据或者图像特征对比当前建立好的地图中的特征，挑选和地图特征匹配最好的粒子的位姿当做当前位姿，如此往复。当然在gmapping、hector_slam这类算法中，不会如此轻易的使用激光数据，激光测距这么准，当然不能只用来计算粒子权重，而是将激光数据与地图环境进行匹配（scan match）估计机器人位姿，比用编码器之流精度高出很多。

        目前SLAM主流研究热点几乎都是基于图优化的，为啥都用图优化了。处理激光SLAM如果用EKF，随着时间推移地图扩大，内存消耗，计算量都很大；而使用图优化计算在高建图精度的前提下效率还快。 在图优化的方法中（graph-based slam），处理数据的方式就和滤波的方法不同了，它不是在线的纠正位姿，而是把所有数据记下来，最后一次性算账。

2.图优化理论

        图优化又被称为Graph-based SLAM，它的基本思想是将机器人不同时刻的位姿抽象为点（pose），机器人在不同位置上的观测所产生的约束被抽象为点之间的边，或者叫约束（constraint）。

        图是由节点和边构成,SLAM问题是怎么构成图呢？在graph-base SLAM中，图中的节点来表示机器人的位姿（x,y,yaw）,两个节点之间的边表示两个位姿的空间约束（相对位姿关系以及对应方差）。机器人的位姿表示一个节点（node）或顶点(vertex)，位姿之间的关系构成边(edge)。相邻节点之间的相对位姿关系，可以由里程计、IMU、帧间匹配计算得到，如下图所示。

        上一步已经完成了图的构建，机器人位姿当做顶点，位姿间关系当做边，这一步常常被成为前端（front-end），这一步往往是传感器信息的堆积。

        接下来可以开始优化图，调整机器人位姿顶点尽量满足边的约束，这一步称为后端（back-end）。后端的首要任务是通过回环检测来构建回环边。

        如下图所示，在机器人在位姿xi观测到的数据和机器人在位姿xj观测到的数据存在相同（回环检测），所有可以通过帧间匹配计算xi和xj的相对位姿，即完成了回环边的构建，如C所示。

 2.1 回环检测

        现在，回环边有了，节点和边也有了，就可以构建观测值（匹配计算得到的节点𝑖和节点𝑗的相对位姿）与预测值（里程积分得到的当前节点𝑖和节点𝑗的相对位姿）的误差函数来优化各个节点的位姿了，误差函数如下图所示。

        误差函数去预测值都知道了，现在可以正式开始对图中的位姿进行优化了。由于器人位姿之间的变化函数是不连续性，也就是说误差函数是非线性的，图优化问题是一个非线性最小二乘问题。非线性最小二乘求解流程如下：

 使用上述方法对误差函数求解，有：

         对于每一个位姿，都需要求解一次Jacobian矩阵，非常耗时、耗资源，这是不可取的。后来大牛通过分析，发现Jacobian矩阵具有稀疏性，这一发现，大大简化了Jacobian矩阵的求解，具体如下：

 注意：

       由于观测值观测到的是两个位姿之间的相对位姿，所以在满足相对位姿约束的情况下，在任意坐标系，都能找到一个解满足约束，这也意味着解有无穷多的。为了让解唯一，必须加入一个约束条件让某一个位姿固定，一般选择第一个位姿，即：

等价于：