激光SLAM从理论到实践学习——第六节（基于图优化的激光SLAM方法）

最新推荐文章于 2024-02-26 22:10:41 发布

执念丶

最新推荐文章于 2024-02-26 22:10:41 发布

阅读量5k

点赞数 12

分类专栏： SLAM 机器人

本文链接：https://blog.csdn.net/qq_42263553/article/details/103263321

版权

SLAM 同时被 2 个专栏收录

13 篇文章 34 订阅

订阅专栏

机器人

11 篇文章 1 订阅

订阅专栏

基于图优化的激光SLAM方法（Grid-based栅格地图）

之前说的都是基于滤波器的，典型的就是gmapping算法

在估计的时候只是估计当前的位置xt，那么一旦x0:t-1中间出现错误的话，那么估计xt出错的可能性就会越来越大。
基于滤波器的算法每次估计的是xt，当前时刻的位姿。而图优化估计的是x0:t，估计的是一个轨迹，轨迹上的每一个位姿都会进行估计，假设当你在某点出现一个很大的误差的时候，那么当他回到误差点的时候他可以对这个误差进行消除。
图优化最终的求解是要用到非线性最小二乘法。

Graph-base SLAM

Graph-base SLAM把SLAM分为两部分，前端和后端。前端构图（帧间匹配，回环检测），后端优化。用图来表示SLAM的问题，图中节点表示机器人的位姿，两个节点之间的边表示两个位姿的空间约束。

Graph-base SLAM：构建图，并找到一个最优的配置（机器人在各节点中的位姿），让里程计预测值与激光雷达的观测值的误差最小（最小二乘）。注意他是各节点中的位姿，也就是说x1到t时刻的位姿都需要找。

在这里插入图片描述

非线性最小二乘原理(二乘就是误差的平方和)

需要解决的问题
- 给定一个系统，其状态方程由f(x)=z描述。其中：x为该系统的状态向量（即需要估计的值，在激光SLAM中就是机器人的位姿）、f(x)是一个非线性的映射函数、状态向量x，可以通过非线性函数f(x)映射得到z1预测值，z表示系统的观测值，可以通过传感器进行直接观测（激光雷达的值）。
- 给定该系统的n个混有噪声的观测值(z1,⋯,zn)，估计状态向量x，使得其经过f(x)映射之后的预测值和观测值的误差最小。
  
  我们已知zi激光雷达的观测值了，现在假设预测值为z1，我们要做的就是估计状态向量x，使得||z1-zi||最小（预测值与观测值误差最小）
- 跟线性最小二乘基本相同，不同之处在于f(x)是一个非线性函数
示意图

看图可知，x是状态量，是机器人的位姿。f(x)是传感器的观测模型，或者说观测函数，求解这个观测函数就可以得到预测值z1。z是传感器的观测值，一般就是激光数据。||z1-z||表示的是预测值与观测值的误差，目的就是找到最优的x，让这个误差最小。
误差函数
- 目标我们已经知道了，就是最小化预测值和观测值的差，因此误差即为预测值与雷达数据观测值的差为：
  
  ei(x) = fi(x) - zi
- 假设误差服从高斯分布，因此其对应的信息矩阵为∑i，因此该预测值与观测值的误差的平方定义为：
  
  Ei(x) = ei(x)T∑iei(x)
  
  注：向量的平方是其转置矩阵。
  
  每个观测值都有不同的可靠性，因此用的是一个加权最小二乘，所有存在一个信息矩阵∑i（权重矩阵）来进行加权。
- 因此，非线性最小二乘的目标函数就是
  
  误差的平方和，也就是我们要找到一个x，使得这个目标函数最小。当然，目标函数F(x)是非线性的，是因为ei(x)是非线性的（实际是因为ei(x)计算中的fi(x)是非线性的）。
解决的问题：
- 我们的直接想法就是跟线性一样，求目标函数关于变量x的导数，令其等于0，求解方程即可。但是这样对于非线性问题是行不通的。
- F(x)为关于x的非线性方程，能否把其化为关于x的线性方程？是可以的，线性化只需要对其进行泰勒展开（所有线性化的工程问题基本都是用的泰勒展开）。
线性化
- F(x)是关于x的非线性函数的原因是，误差函数ei(x)是一个非线性函数（ei(x)中的f(x)），他是引起系统非线性的原因。因此直接对误差看书ei(x)进行线性化即可。泰勒展开有：
  
  其中J为映射函数对状态向量x的导数，称之为Jacobian矩阵（雅克比矩阵）。
  
  因此函数F(x)可化简为：
- F(x)的化解
  
  首先进行分别相乘，得到第三步，然后化简结合得到第四步和第五步
- 得到解
- 求解
  
  2b+2H△x=0是一个适定方程组（未知数个数和方程个数相等），所以不能用最小二乘来求。
- 流程
  - 1.对误差函数进行线性化
  - 2.构建一个线性系统
  - 3.求解线性系统
  - 4.更新解，并不断迭代直至收敛

非线性最小二乘原理总结

x是状态量，是机器人的位姿。f(x)是传感器的观测模型或者说观测函数，求解这个观测函数就可以得到预测值z1。z是传感器的观测值，一般就是激光数据。 ||z1-z||表示的是预测值与观测值的误差，目的就是找到最优的x，使得f(x)预测值与雷达观测值的误差最小。

非线性最小二乘在图优化SLAM中的应用

SLAM前端：图的构建
- 构建地图的边方式分为两种：
  - 第一种是里程计测量构建边
    
    机器人从节点xi运动到节点xi+1，里程计测量得到此运动信息。并在对应的节点中连上一条边，边为里程计测量值。
  - 第二种是回环检测构建边
    
    节点xi和节点xj观测到同样的环境信息（比较多的重合），那么两者进行匹配得到相对位姿。并在对应的节点中连一条边，边为匹配的相对位姿。用信息矩阵来描述本次匹配的可靠性。
SLAM后端：进行优化
- 误差函数
  - 观测值zij=(dx,dy,dθ)表示经过匹配计算得到节点xi和节点xj的相对位姿。节点xj在节点xi坐标系下的坐标。
  - 预测值为里程计的测量值，图中xi和xj即为里程计测量得到的坐标，因此预测值z1ij = Xi-1Xj,其中Xi表示xi对应的转换矩阵。
  - 因此误差函数eij(x) = t2v(Zij-1Z1ij)。t2v表示把转换矩阵转换到对应的位姿。
- 误差函数的形式
  
  写出误差函数的矩阵形式后，就要写出对应的贾科比矩阵，知道贾科比矩阵后就可以进行线性化了。
- 误差函数的线性化
- 固定坐标系
  
  观测值观测到的值是两个位姿之间的相对位姿。
- 构建线性系统
- 求解
  
  已知矩阵H和向量b，求解线性方程组，不断进行迭代，直至收敛。