基于图优化的激光SLAM

ゞ浴缸里的玫瑰 2022-11-05 03:04 218阅读 0赞

### 基于图优化的激光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 ##

### 数学概念 ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70]前端是构图部分，包括一个帧间匹配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)是一个非线性函数  
    *处理非线性问题，只能是先转化成线性问题，再求解*

### 示意图 ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 1]

### 误差函数 ###

*  目标为最小化预测和观测的差，因此误差为预测和观测的差：  
     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)的化解  
    ![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 2]此时， 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来表示  
    ![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 3]![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 4]这里用到了一些矩阵的求导方法  
     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的前端  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 5]

### 误差函数 ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 6]误差函数，说白了就是预测值和观测值的差。 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坐标系下的转换  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 7] 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)

*雅克比矩阵知道后，就可以对误差函数进行线性化了*  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 8]![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 9]![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 10]

### 固定坐标系 ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 11]  
 H . Δ = − b H.\\Delta = -b H.Δ=−b  
 说 明 H 是 不 可 逆 的 说明H是不可逆的 说明H是不可逆的  
 d e t ∣ H ∣ = 0 det|H| = 0 det∣H∣=0  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 12]

### 构建线性系统 ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 13]

### 求解 ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 14]

## 4、cartographer介绍 ##

### 特性 ###

*  基于图优化的ＳＬＡＭ算法
 *  比较完善的匹配系统，包含建图和定位
 *  目前效果最好的开源激光ＳＬＡＭ系统
 *  有人在专门的维护，不断增加新的特性

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70]: /images/20221023/02df6010a3e24ef0b29fc7114382bcbe.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 1]: /images/20221023/f4a84e31f494458e9a5e3138824685e4.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 2]: /images/20221023/2738374771734b89905a441d2a23afcb.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 3]: https://img-blog.csdnimg.cn/20210304145551729.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 4]: /images/20221023/c37f530e905e422da7bb8663d6d4bd42.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 5]: /images/20221023/9930ad003c774c93babe07ea0c962d65.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 6]: /images/20221023/983caec8d1fe48bfa0e63180cc4723af.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 7]: /images/20221023/1327b51df0b74ffc96be604d5570ec9a.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 8]: https://img-blog.csdnimg.cn/20210304192308770.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 9]: https://img-blog.csdnimg.cn/20210304193216552.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 10]: /images/20221023/66cfdcc1abb44951953036699025bd7a.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 11]: /images/20221023/9edbf624030d4aa4b9344ecc0f045ef8.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 12]: /images/20221023/c9381f59d09342e8b7e00c74f38f66a0.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 13]: /images/20221023/0ad72ea71cda4acaa11a6beb22c2d1f8.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 14]: /images/20221023/52a278aee5094faa9bb28c77df610104.png