激光SLAM理论与实践 笔记 - 激光的后端图优化理论 第四期

– 第六章 后端图优化理论

6.1 图优化理论

课程介绍：

• 这期开始不讲滤波器的内容了（gmapping），只讲图优化（karto、cartographer）。
• 但是滤波器在视觉slam之中的应用还是非常广泛的。
• 三种slam方向：

滤波 估计当前位姿，**平滑（图优化）**估计过去位姿，**预测（暂未接触）**估计未来位姿；

– 6.1.1 Graph-based SLAM

• 2Dslam问题会比较简单，因为只是欧式空间的问题，不用考虑李代数等问题。
• 激光和视觉的差异：
  • 激光一般只进行pose位姿优化（精度较高）；节点表示机器人位姿；
  • 视觉精度比较差，除了pose还有其他的；
• 回环检测：走了一圈之后，不同的两个节点1、5到了同样的位置，产生一个回环，可以根据边（约束）计算相对空间变换；即 节点1和5相互满足两条约束。
• 求解目标：由于里程计的误差，导致误差不为零，上述两条边误差不为零，所以需要 使误差尽可能趋向零 ；这个 是图优化 的 目标 ；
• 抽象问题：求解两个节点的位姿变换过程，由于旋转矩阵涉及sin和cos计算，实际上是非线性计算，即 图优化slam 依然是可以抽象成，求解 非线性最小二乘 问题；
• 前端和后端的差别：
  • 前端：节点和节点之间构建边（约束）、节点连线过程，以及之前的过程都是前端问题；
  • 后端：节点连成边后，构建了图后，让误差最小化的就是后端优化；优化是预测和观测问题；

– 6.1.2 非线性最小二乘

• 状态方程f(x)可以认为是一个预测过程；
• x是 预测值 ；
• 状态向量是机器人位姿robot pose；
• z表示的是 观测值 ，即传感器值；由于噪声，f(x)和z不相等；
• 由于数学算法的局限，需要把非线性问题，转化成一个线性的问题求解；

• 勘误 ：f2(x) = z’2 z2
• 目标：找到最优的x，让 观测 和 预测 的误差最小化；

• 人造的 传感器 通常服从 高斯分布 （因为人造的模块化、相互独立等因素）**。**自然界造物一般服从贝塔分布或其他；

• 直接想法的解释：极值点的导数必定零；
• 非线性问题：需要线性化才能求解，使用 泰勒展开 方法；

• 误差函数有个一阶的近似，高阶的余项可忽略
• 化简过程：

• 非线性最小二乘求解过程：

• Ax = b，还是一个适定方程，刚好有一个解。
• 不断迭代求解到最后就是一个机器人位姿，graph-based位姿图优化问题；

– 6.1.3 非线性最小二乘在slam中的应用

• 图的构建

• 两者特点
  • 里程计：时间上是 连续 的，连续的 机器人位姿 才可以构建为 图 ；
  • 回环检测：时间上是 不连续 的；不同时间的节点可以通过边（约束）匹配构建成图；
• 误差函数

• xi和xj之间的相对位姿（节点j在节点i的坐标，即 xi逆*xj ）通过帧间匹配算法求得（ICP、NDT等）；
• 观测值Zij 和**预测值Zij’**的相对坐标计算， 观测值从传感器得 ， 预测值从里程计推算 。
• 误差函数 定义：即两个位姿变换关系，**Zij的逆*Zij’**矩阵。
• 实际上上述问题都是第一节课提到的 坐标位姿变换关系 问题，这就是所谓的误差函数。

• 误差函数的矩阵形式，实际就是上一段 观测值和预测值 推导的内容；
• 这就是为什么说2Dslam问题是 机器人位姿图 问题，抽象。
• 接下来的问题，实际上就是非线性函数的线性化问题；
• 误差函数的线性化

• 上图的是一个jacobian矩阵，属于稀疏的；

• 因为jacobian是稀疏的向量，所以H矩阵（黑森矩阵）也是稀疏的；

• 最终也是一个稀疏矩阵；
• H（黑森）矩阵是一个稀疏矩阵，就可以进行快速的求解；
• 已知H、b，则由（H▽X = b），可求解▽X ；
• 但是还需要解决坐标系的转化问题 ，需要增加固定的坐标系给xi、xj，如下图：

• 设第一帧位姿为 世界坐标系 的 原点 ，即增加固定坐标系的约束，后续求解都在世界坐标系。△X1=0；如下图

• 构建线性系统
• 以上所有过程已经推导了后端图优化必备信息，可以开始 构建线性系统 ：

• 上图构建了线性的系统，另外这是我们作业的内容。

• 上图就是 图优化的求解根本流程 ；
• 即：
  • 1、构建线性系统；
  • 2、求解线性系统；
  • 3、迭代求解，直到收敛；
• 所以调用库的时候，只要提供 误差函数 就好了，因为流程都是一致的。

6.2 cartographer代码讲解

在整体的原有框架中代码主要分为两个部分： cartographer 和 cartographer_ros 。

• cartographer 主要负责处理来自雷达、IMU和里程计的数据并基于这些数据进行地图的构建，是cartographer理论的底层实现。建图过程主要分为前端Local累加式建图与后端Global位姿图构建与回环匹配。
• cartographer_ros 则基于ros的通信机制获取传感器的数据并将它们转换成cartographer中定义的格式传递给cartographer处理，与此同时也将cartographer的处理结果发布用于显示或保存，在低层算法和机器人操作系统ROS之间提供了交互的接口。

其中输入项中 Range Data的激光数据为必须 ， 其余均为非必须 。

整体的建图可分为Local前端部分和Global后端回环部分。

特点：

• 基于图优化的slam算法
• 比较完善的匹配系统，包含建图和定位；
• 目前效果最好的开源激光slam系统；
• 有人专门维护，不断更新增加新特性，从没停止；

2Dslam代码。（源码用了大量C++高级特性）

– 6.2.1 代码查看流程：

流程分析：

1、从入口global_trajectory_builder.cc（mapping->internal）进入。

– 6.2.2 具体代码分析

• 前端匹配

1、global_trajectory_builder.cc

子图匹配，帧间匹配，前端局部子图匹配

CSM暴力求解：

打分：似然场和TSDF评价体系，本质相似。

• 外插器实现：保存一段时间的位姿用来计算线速度和角速度
• CSM实现：
• 后端优化

1、入口为 pose_graph_2d 文件，AddNode函数；

• 增加 节点 和计算 约束（边） ；
• 增加队列即可。

2、AddWorkItem函数：

• 判断work队列；

3、DrainWorkQueue函数才是调用队列

work_item实际上就是约束。

回环检测及回环优化

• 多线程：AddWorkItem，增加工作队列
• 回环成功：说明需要进行优化
  • 回环–优化–回环–优化……
• FCSM：fast correlative scan match 快速关联扫描匹配
• 地图的层数depth？
• 分支定界：求最优解