基于图优化的激光SLAM

基于图优化的激光SLAM方法（Grid-based）
- 1、Graph-base SLAM
- - 数学概念
- 2、非线性最小二乘
- - 解决的问题
  - 示意图
  - 误差函数
  - 解决的问题
  - 线性化
  - 流程
- 3、非线性最小二乘在SLAM中的应用
- - 图的构建
  - 误差函数
  - 固定坐标系
  - 构建线性系统
  - 求解
- 4、cartographer介绍
- - 特性

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

基于滤波的激光SLAM方法只计算当前时刻t的位姿，不太能处理大的环境
而基于图优化的激光SLAM方法是计算机从0到t时刻的位姿 x 0 : t x_{0:t} x0:t，估计整个地图上机器人的轨迹，通过优化的方式可以将优化消除掉，这也是能处理大场景的主要原因。
基于栅格地图来说，基于滤波的基本上是没什么优势的。

1、Graph-base SLAM

数学概念

在这里插入图片描述前端是构图部分，包括一个帧间匹配Scan-Match

用一个图（Graph）来表示SLAM的问题
图中的节点来表示机器人的位姿
两个节点之间的边表示两个位姿的空间约束
Graph-based SLAM ：构件图，并且找到一个最优的配置（各个节点的位姿），让预测与观测的误差最小

2、非线性最小二乘

解决的问题

给定一个系统，其状态方程由 f ( x ) = z 描述 f(x)=z描述 f(x)=z描述。其中：
x为该系统的状态向量—即需要估计的值
f(x)为一个非线性映射函数
状态向量x，可以通过非线性函数f(x)映射得到z
z表示系统的观测值，可以通过传感器进行直接观测
给定该系统的n个混有噪声的观测值 ( z 1 , . . . , z n ) (z_1,…,z_n) (z1,…,zn),估计状态向量x,使得其经过f(x)映射之后的预测值和观测值的误差最小
跟非线性最小二乘基本相同，不同之处在于f(x)是一个非线性函数
处理非线性问题，只能是先转化成线性问题，再求解

示意图

在这里插入图片描述

误差函数

目标为最小化预测和观测的差，因此误差为预测和观测的差：
e i ( x ) = f i ( x ) − z i e_i(x)=f_i(x)-z_i ei(x)=fi(x)−zi
假设误差服从高斯分布，因此其对应的信息矩阵为 σ i ，因此该观测值误差的平方定义为： \sigma_i，因此该观测值误差的平方定义为： σi，因此该观测值误差的平方定义为：
E i ( x ) = e i ( x ) T Σ i e i ( x ) E_i(x)=e_i(x)^T\Sigma_ie_i(x) Ei(x)=ei(x)TΣiei(x) 信息权重测量值你越信任他，也即是测量值精确度越高，那么权重越大，就会优先优化他，很多观测值
因此，非线性最小二乘的目标函数为：
F ( x ) = ∑ E i ( x ) = ∑ e i ( x ) T Σ i e i ( x ) F(x)=\sum E_i(x)=\sum e_i(x)^T\Sigma _i e_i(x) F(x)=∑Ei(x)=∑ei(x)TΣiei(x) 平方和
m i n x F ( x ) min_{x}F(x) minxF(x) 目标函数是非线性的，即 e i ( x ) e_i(x) ei(x)是非线性化的，导致了 F ( x ) F(x) F(x)也是非线性化的

解决的问题

目标函数：
m i n F ( x ) min F(x) minF(x)
直接想法:求 F ( x ) F(x) F(x)关于变量x的导数，令其等于0，求解方程即可。
对于线性问题，该方法可以正确，但是对于非线性问题不正确
F(x)为关于x的非线性方程，能否把其化为关于x的线性方程
线性化：泰勒展开

线性化

产生非线性的根源就是误差函数的非线性化。线性化只在进行线性化那一点的附近区域比较准确，远了就不准了。而我们最关心的区域一般都是平衡点附近，于是都是在平衡点附近做线性化。

F(x)是关于x的非线性函数的原因是，误差函数 e i ( x ) e_i(x) ei(x)是一个非线性函数。因此直接对误差函数 e i ( x ) e_i(x) ei(x)进行线性化即可。
e i ( x + Δ x ) = e i ( x ) + J i ( x ) Δ x e_i(x+\Delta x ) = e_i(x) + J_i(x)\Delta x ei(x+Δx)=ei(x)+Ji(x)Δx
其中J为映射函数对状态向量x的导数，称之为Jacobian矩阵。
J i ( x ) = ( ∂ f i ( x ) ∂ x 1 , ∂ f i ( x ) ∂ x 2 , . . . , , ∂ f i ( x ) ∂ x n ) J_i(x) = (\frac{\partial f_i(x)}{\partial x_1},\frac{\partial f_i(x)}{\partial x_2},…,,\frac{\partial f_i(x)}{\partial x_n}) Ji(x)=(∂x1∂fi(x),∂x2∂fi(x),…,,∂xn∂fi(x))
因此函数F(x)的可化解为：
F ( x + Δ x ) = ∑ e i T ( x + Δ x ) Σ i e i ( x + Δ x ) F(x+\Delta x) = \sum e^T_i(x+\Delta x)\Sigma_i e_i(x+\Delta x) F(x+Δx)=∑eiT(x+Δx)Σiei(x+Δx)
F(x)的化解
此时， e i T Σ i e i 用 c i 表示， e i T Σ i J i 用 b i T 来表示 J i T Σ i J i 用 H i 来表示 e^T_i\Sigma_i e_i用c_i表示，e_i^T \Sigma_iJ_i用b_i^T来表示J^T_i\Sigma_iJ_i用H_i来表示 eiTΣiei用ci表示，eiTΣiJi用biT来表示JiTΣiJi用Hi来表示
这里用到了一些矩阵的求导方法
A T Δ x 求导之后是 A A^T \Delta x 求导之后是A ATΔx求导之后是A
∂ X T A X ∂ X = ( A T + A ) . Δ X \frac{\partial X^TAX}{\partial X} = (A^{T}+A).\Delta X ∂X∂XTAX=(AT+A).ΔX
因为H是对称矩阵，所以
( H T + H ) . Δ X = 2 H Δ X (H^T + H).\Delta X = 2H\Delta X (HT+H).ΔX=2HΔX
X T A X X^TAX XTAX的导数是（ A T + A ） X （A^T+A）X （AT+A）X

适定方程组，不是超定方程组，未知数的个数与方程数量是一样的，一般情况下H是可逆的

找到能被线性化的段里面的局部最小值，在线性化里面的最小值，直接到收敛。这就是最小二乘的全部流程。

总结一下：

流程

1. 线性误差函数
  e i ( x + Δ x ) = e i ( x ) + J i Δ x e_i(x+\Delta x) = e_i(x) + J_i \Delta x ei(x+Δx)=ei(x)+JiΔx
  首先，对误差函数进行线性化，就可以对目标函数进行化解，最终目标构建线性系统
1. 构建线性系统
  b T = ∑ e i T Σ i J i b^T =\sum e^T_i \Sigma_iJ_i bT=∑eiTΣiJi
  H = ∑ J i T Σ i J i H = \sum J^T_i\Sigma_iJ_i H=∑JiTΣiJi
  构建好了方程组
1. 求解线性系统
  Δ x ∗ = − H − 1 b \Delta x^* = -H^{-1}b Δx∗=−H−1b
1. 更新解，并不断迭代至收敛：
  x = x + Δ x x = x + \Delta x x=x+Δx

3、非线性最小二乘在SLAM中的应用

图优化SLAM

图的构建

SLAM的前端
在这里插入图片描述

误差函数

在这里插入图片描述误差函数，说白了就是预测值和观测值的差。 x i , x j x_i,x_j xi,xj
其中 z i j ′ = X i − 1 X j z_{ij}^{‘}=X_i^{-1}X_j zij′=Xi−1Xj,其中，这个公式预测值，表示从 X j 到 X i 坐标系下的转换 X_j到X_i坐标系下的转换 Xj到Xi坐标系下的转换
在这里插入图片描述 x i − 1 ∗ x j = R i T ( t j − t i ) θ j − θ i x_i^{-1}*x_j=\frac{R_i^T(t_j-t_i)}{\theta_j-\theta_i} xi−1∗xj=θj−θiRiT(tj−ti)