激光slam课程学习笔记－－第９课：基于图优化的激光slam方法(Grid-based)

前言：这系列笔记是学习曾书格老师的激光slam课程所得，这里分享只是个人理解，有误之处，望大佬们赐教。这节课主要介绍基于图优化的激光slam方法(Grid-based)

１． Graph-based SLAM

［front-end 是前端处理，具体为一个构图过程（scan-match帧间匹配＋loopclosure回环检测）；back-end表示后端处理］［图优化，这里的图可以从ｃ＋＋里面的图知识进行理解］

１）用一个图(graph)来表示slam问题
２）图中的节点来表示机器人的位姿

３）两个节点之间的边表示两个位姿的空间约束［也可以视为误差，因为边本身产生一个误差］

４）graph-based slam：构建图，并且找到一个最优的配置（各个节点的位姿）［除了状态x0的状态不改变，其他状态ｘ都进行改变］，让预测（里程计的预测值）与观测（激光雷达的观测值）的误差最小．

［图优化，最后也是变成非线性最小二乘求解问题］［非线性最小二乘，不仅在激光slam领域重要，而且在机器人领域非常重要的，学好其是非常有必要的］

２． 非线性最小二乘原理

2.1 解决的问题

１）给定一个系统，其状态方程由f(x)=z描述.其中：
［z为系统的可观量（传感器探测直接可以获取的值），又称为系统的观测值］［在pose-graph里面，可观量就是scan-match得到的值，也就是激光雷达观测到值，称为ｚ］

x为该系统的状态向量－－－－即需要估计的值［在机器人里面视为机器人的位姿］

f(x)是一个非线性的映射函数［重点是非线性的］
状态向量x,可以通过非线性函数f(x)映射得到z［通过状态ｘ计算获取的ｚ，ｚ又可被称为预测的观测量（预测可观值）］
z表示系统的观测值，可通过传感器进行直接观测

２）给定该系统的ｎ个混有噪声的观测值（z1,…,zn）,估计状态向量ｘ，使得其经过f(x)映射之后的预测值和观测值的误差最小

３）跟非线性最小二乘基本相同，不同之处在于f(x)是一个非线性函数

［个人理解，直接通过传感器获取的数据ｚ称为观测值，根据状态利用方程获取的ｚ称为预测值，找到一个非线性函数的f(x)使得观测值和预测值误差最小］

［求解非线性方程时，首先转换为线性方程进行思考］

2.2 非线性最小二乘示意图

以激光slam为例：
1)Ｘ为机器人的位置
2)f(x)为传感器的观测模型（似然场模块或者重投影模型）

3)Ｚ为传感器的观测值，激光数据或者图像特征点
4)找到最优的ｘ，让预测和观测的误差最小

2.3 误差函数

［＂假设误差服从高斯分布．．．．＂里面的符号Oi写错了，应该为第二个式子的右侧的类Ｅi来的］
［ei(x)表示的是观测值的可靠性大小］

［将目标函数Ｆ(x)转为非线性函数的思路是，将ei(x)线性化，从而实现非线性方程转为线性方程］

2.4 解决的问题

［思路，将非线性问题转为局部线性化处理，也就是利用泰勒张开实现的］

２.5 线性化

［泰勒展开］

［从上往下顺数第三行，右侧中间的两项都是一个值（如何分解得到？），所以可以合并成第四行第二项］



［化简补充说明］

2.6 非线性最小二乘流程

［补充说明，由于H一般是立方矩阵，直接求逆会非常耗时］

３．非线性最小二乘在(激光)slam的应用

3.1　图的构建

［里程计的测量值，又称为预测值］
［节点i,j能匹配的前提是，两者观测到环境信息，存在比较多重合的地方］
［图的构建过程，也就是slam的前端过程］

3.2　误差函数

［误差函数，预测值和观测值的差］

［jacobian矩阵（雅可比矩阵）］

3.3 误差函数的线性化

［红色部分表示非零，那么计算时候，只需要计算这四个位置即可，那么对于Ｈ矩阵而言，可以大大减少计算量］

［这里证明了H为稀疏矩阵，使得其可以利用稀疏矩阵的性质进行快速求解］

3.4 固定坐标系

１）观测到的值是两个位姿之间的相对位姿
２）满足相对位姿约束的解有无穷多组

３）为了让解唯一，必须加入一个约束条件让某一个位姿固定，一般选择第一个位姿

［让第一个位姿设置为０，属于一个额外的约束，因为接下来的位姿并没有这个约束］

3.5 构建线性系统

［向量ｂ和矩阵Ｈ在代码里面都是自己构建的（个人没有理解）］

3.6 求解

［converged表示收敛］［操作流程属于slam后端优化的过程］

４．cartographer介绍

特性：

１）基于图优化的SLAM算法
２）比较完善的匹配系统，包含建图和定位

３）比较好的开源激光SLAM系统
４）有人在专门的维护，不断增加新的特性

#####################
图片版权归原作者所有
致谢曾老师的付出

不积硅步，无以至千里
好记性不如烂笔头
感觉有点收获的话，麻烦大大们点赞收藏哈