前言:这系列笔记是学习曾书格老师的激光slam课程所得,这里分享只是个人理解,有误之处,望大佬们赐教。这节课主要介绍基于图优化的激光slam方法(Grid-based)
1. Graph-based SLAM
[front-end 是前端处理,具体为一个构图过程(scan-match帧间匹配+loopclosure回环检测);back-end表示后端处理][图优化,这里的图可以从c++里面的图知识进行理解]
1)用一个图(graph)来表示slam问题
2)图中的节点来表示机器人的位姿
3)两个节点之间的边表示两个位姿的空间约束[也可以视为误差,因为边本身产生一个误差]
4)graph-based slam:构建图,并且找到一个最优的配置(各个节点的位姿)[除了状态x0的状态不改变,其他状态x都进行改变],让预测(里程计的预测值)与观测(激光雷达的观测值)的误差最小.
[图优化,最后也是变成非线性最小二乘求解问题][非线性最小二乘,不仅在激光slam领域重要,而且在机器人领域非常重要的,学好其是非常有必要的]
2. 非线性最小二乘原理
2.1 解决的问题
1)给定一个系统,其状态方程由f(x)=z描述.其中:
[z为系统的可观量(传感器探测直接可以获取的值),又称为系统的观测值][在pose-graph里面,可观量就是scan-match得到的值,也就是激光雷达观测到值,称为z]
x为该系统的状态向量----即需要估计的值[在机器人里面视为机器人的位姿]
f(x)是一个非线性的映射函数[重点是非线性的]
状态向量x,可以通过非线性函数f(x)映射得到z[通过状态x计算获取的z,z又可被称为预测的观测量(预测可观值)]
z表示系统的观测值,可通过传感器进行直接观测
2)给定该系统的n个混有噪声的观测值(z1,…,zn),估计状态向量x,使得其经过f(x)映射之后的预测值和观测值的误差最小
3)跟非线性最小二乘基本相同,不同之处在于f(x)是一个非线性函数
[个人理解,直接通过传感器获取的数据z称为观测值,根据状态利用方程获取的z称为预测值,找到一个非线性函数的f(x)使得观测值和预测值误差最小]
[求解非线性方程时,首先转换为线性方程进行思考]
2.2 非线性最小二乘示意图
以激光slam为例:
1)X为机器人的位置
2)f(x)为传感器的观测模型(似然场模块或者重投影模型)
3)Z为传感器的观测值,激光数据或者图像特征点
4)找到最优的x,让预测和观测的误差最小
2.3 误差函数
["假设误差服从高斯分布...."里面的符号Oi写错了,应该为第二个式子的右侧的类Ei来的]
[ei(x)表示的是观测值的可靠性大小]
[将目标函数F(x)转为非线性函数的思路是,将ei(x)线性化,从而实现非线性方程转为线性方程]
2.4 解决的问题
[思路,将非线性问题转为局部线性化处理,也就是利用泰勒张开实现的]
2.5 线性化
[泰勒展开]
[从上往下顺数第三行,右侧中间的两项都是一个值(如何分解得到?),所以可以合并成第四行第二项]
[化简补充说明]
2.6 非线性最小二乘流程
[补充说明,由于H一般是立方矩阵,直接求逆会非常耗时]
3.非线性最小二乘在(激光)slam的应用
3.1 图的构建
[里程计的测量值,又称为预测值]
[节点i,j能匹配的前提是,两者观测到环境信息,存在比较多重合的地方]
[图的构建过程,也就是slam的前端过程]
3.2 误差函数
[误差函数,预测值和观测值的差]
[jacobian矩阵(雅可比矩阵)]
3.3 误差函数的线性化
[红色部分表示非零,那么计算时候,只需要计算这四个位置即可,那么对于H矩阵而言,可以大大减少计算量]
[这里证明了H为稀疏矩阵,使得其可以利用稀疏矩阵的性质进行快速求解]
3.4 固定坐标系
1)观测到的值是两个位姿之间的相对位姿
2)满足相对位姿约束的解有无穷多组
3)为了让解唯一,必须加入一个约束条件让某一个位姿固定,一般选择第一个位姿
[让第一个位姿设置为0,属于一个额外的约束,因为接下来的位姿并没有这个约束]
3.5 构建线性系统
[向量b和矩阵H在代码里面都是自己构建的(个人没有理解)]
3.6 求解
[converged表示收敛][操作流程属于slam后端优化的过程]
4.cartographer介绍
特性:
1)基于图优化的SLAM算法
2)比较完善的匹配系统,包含建图和定位
3)比较好的开源激光SLAM系统
4)有人在专门的维护,不断增加新的特性
#####################
图片版权归原作者所有
致谢曾老师的付出
不积硅步,无以至千里
好记性不如烂笔头
感觉有点收获的话,麻烦大大们点赞收藏哈