基于滤波的激光SLAM方法（Grid-based）

本是古典何须时尚 2022-11-04 01:32 347阅读 0赞

### 这里写自定义目录标题 ###

*  前言
 *  1、贝叶斯滤波
 *   *  1.1、数学概念
     *  1.2、贝叶斯滤波推导
     *  贝叶斯滤波实例
 *  2、粒子滤波
 *   *  2.1、示意图
     *  2.2、 特性
     *  2.3 流程
     *  2.4、状态传播
     *  2.5 权重评估
     *  2.6 重采样
 *  3、FastSLAM的原理及优化
 *   *  3.1、 Fast-SLAM介绍
     *  算法流程
     *  存在的问题及优化
     *  进一步优化proposal
 *  4、Gmapping的介绍
 *   *  Gmapping特性

# 前言 #

第一个SLAM系统，基于滤波器的SLAM方法当前来说已经落后，滤波就是只记录当前时刻状态，前面的时刻一旦出现错误就无法修正，在激光里面，效果不会特别好，但是他在发展过程中，是必不可少的一环。，尤其是粒子滤波，贝叶斯估计。一般来说有两种地图，一种是feature-based 视觉常用，激光的话就是栅格地图（2d）

# 1、贝叶斯滤波 #

状态估计，大部分是贝叶斯滤波。

## 1.1、数学概念 ##

**独立：**  
 p ( x , y ) = p ( x ) p ( y ) p(x,y)=p(x)p(y) p(x,y)=p(x)p(y)  
 p ( x ∣ y ) = p ( x ) p(x|y)=p(x) p(x∣y)=p(x)  
 p ( y ∣ x ) = p ( y ) p(y|x) = p(y) p(y∣x)=p(y)  
**条件独立：**  
 p ( x , y ∣ z ) = p ( x ∣ z ) p ( y ∣ z ) p(x,y|z)=p(x|z)p(y|z) p(x,y∣z)=p(x∣z)p(y∣z)  
 p ( x ∣ y , z ) = p ( x ∣ z ) p(x|y,z)=p(x|z) p(x∣y,z)=p(x∣z)  
 p ( y ∣ x , z ) = p ( y ∣ z ) p(y|x,z)=p(y|z) p(y∣x,z)=p(y∣z)  
**全概率公式**  
 p ( x ) = ∫ p ( x ∣ y ) p ( y ) d y p(x)=\\displaystyle\\int p(x|y)p(y)dy p(x)=∫p(x∣y)p(y)dy  
条件概率公式：  
 P ( x , y ) = P ( x ∣ y ) P ( y ) = P ( y ∣ x ) P ( x ) P(x,y)=P(x|y)P(y)=P(y|x)P(x) P(x,y)=P(x∣y)P(y)=P(y∣x)P(x)  
**贝叶斯概率公式：**  
 p ( x , y ) = p ( x ∣ y ) p ( y ) = p ( y ∣ x ) p ( x ) p(x,y)=p(x|y)p(y)=p(y|x)p(x) p(x,y)=p(x∣y)p(y)=p(y∣x)p(x)  
\->  p ( x ∣ y , z ) = p ( y ∣ x , z ) p ( x ∣ z ) p ( y ∣ z ) = η p ( y ∣ x , z ) p ( x ∣ z ) p(x|y,z)=\\frac\{p(y|x,z)p(x|z)\}\{p(y|z)\}=\\eta p(y|x,z)p(x|z) p(x∣y,z)=p(y∣z)p(y∣x,z)p(x∣z)=ηp(y∣x,z)p(x∣z)

## 1.2、贝叶斯滤波推导 ##

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

b e l ( x t − 1 ) 是 t 1 时 刻 的 概 率 分 布 ， u t 是 里 程 计 测 量 ， z t 是 激 光 雷 达 的 观 测 bel(x\_\{t-1\})是t\_1时刻的概率分布，u\_t是里程计测量，z\_t是激光雷达的观测 bel(xt−1)是t1时刻的概率分布，ut是里程计测量，zt是激光雷达的观测  
代码第三行是预测机器人的位姿，根据 b e l ( x t − 1 ) 就 可 以 预 测 t 1 时 刻 机 器 人 的 位 姿 bel(x\_\{t-1\})就可以预测t\_1时刻机器人的位姿 bel(xt−1)就可以预测t1时刻机器人的位姿  
第四行，在预测的基础上，用到了观测模型对机器人位姿进行矫正，得到 x t x\_t xt时刻的后验分布。  
卡尔曼滤波是贝叶斯滤波在线性高斯下的特例。  
![目标：][20210301142830515.png]![在这里插入图片描述][20210301142849541.png]其中， p ( x t ∣ z 1 : t , u 1 : t ) 可 以 分 成 p ( x t ∣ z t , u 1 : t , z 1 : t − 1 ) p(x\_t|z\_\{1:t\},u\_\{1:t\})可以分成p(x\_t|z\_t,u\_\{1:t\},z\_\{1:t-1\}) p(xt∣z1:t,u1:t)可以分成p(xt∣zt,u1:t,z1:t−1)  
而我们先只看 x t 和 z t x\_t和z\_t xt和zt  
就可以把公式写成这样呢  
 p ( x t ∣ z t ) = η p ( z t ∣ x t ) p ( z t ) p(x\_t|z\_t)=\\eta p(z\_t|x\_t)p(z\_t) p(xt∣zt)=ηp(zt∣xt)p(zt)，然后把 z 1 : t − 1 , u 1 : t z\_\{1:t-1\},u\_\{1:t\} z1:t−1,u1:t原封不动的带入到公式中就会得到下面的结果  
 p ( x t ∣ z t ) = η p ( z t ∣ x t , p(x\_t|z\_t)=\\eta p(z\_t|x\_t, p(xt∣zt)=ηp(zt∣xt, z 1 : t − 1 , u 1 : t z\_\{1:t-1\},u\_\{1:t\} z1:t−1,u1:t ) p ( z t ∣ )p(z\_t| )p(zt∣ z 1 : t − 1 , u 1 : t z\_\{1:t-1\},u\_\{1:t\} z1:t−1,u1:t)  
一个地图，你观测到什么东西，是由你的位置决定的，与其他的东西无关。  
所以观测值 z t z\_t zt是，与位姿状态 x t x\_t xt的是相关的。所以公式可以写为  
 p ( z t ∣ x t , z 1 : t − 1 , u 1 : t ) = p ( z t ∣ x t ) p(z\_t|x\_t,z\_\{1:t-1\},u\_\{1:t\})=p(z\_t|x\_t) p(zt∣xt,z1:t−1,u1:t)=p(zt∣xt)  
这样的话，化成迭代的方式， x t x\_t xt必须分解，为了实现这个过程，我们实现下面的全概率公式。  
此时y就是 x t − 1 ∣ z 1 : t − 1 , u 1 : t x\_\{t-1\}|z\_\{1:t-1\},u\_\{1:t\} xt−1∣z1:t−1,u1:t  
 p ( x t ∣ z 1 : t − 1 , u 1 : t ) = ∫ p ( x t ∣ x t − 1 , z 1 : t − 1 , u 1 : t ) p ( x t − 1 ∣ z 1 : t − 1 , u 1 : t ) d x t − 1 p(x\_t|z\_\{1:t-1\},u\_\{1:t\})=\\displaystyle\\int p(x\_t|x\_\{t-1\},z\_\{1:t-1\},u\_\{1:t\})p(x\_\{t-1\}|z\_\{1:t-1\},u\_\{1:t\})dx\_\{t-1\} p(xt∣z1:t−1,u1:t)=∫p(xt∣xt−1,z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1  
 p ( x t ∣ x t − 1 , z 1 : t − 1 , u 1 : t ) = p ( x t ∣ x t − 1 , u t ) p(x\_t|x\_\{t-1\},z\_\{1:t-1\},u\_\{1:t\})=p(x\_t|x\_\{t-1\},u\_t) p(xt∣xt−1,z1:t−1,u1:t)=p(xt∣xt−1,ut) 知 x t − 1 x\_\{t-1\} xt−1之后， x t x\_t xt就跟 z 1 : t − 1 z\_\{1:t-1\} z1:t−1没有关系了  
 x t 是 与 x t − 1 相 关 ， 跟 z t 是 没 有 关 系 的 ， 那 么 跟 前 前 面 的 里 程 计 数 据 z 1 : t − 1 是 没 有 关 系 的 ， 跟 u t 相 关 x\_t是与x\_\{t-1\}相关，跟z\_t是没有关系的，那么跟前前面的里程计数据z\_\{1:t-1\}是没有关系的，跟u\_t相关 xt是与xt−1相关，跟zt是没有关系的，那么跟前前面的里程计数据z1:t−1是没有关系的，跟ut相关这就是运动模型。  
 p ( x t − 1 ∣ z 1 : t − 1 , u 1 : t ) = p ( x t − 1 ∣ z 1 : t − 1 , u 1 : t − 1 ) p(x\_\{t-1\}|z\_\{1:t-1\},u\_\{1:t\})=p(x\_\{t-1\}|z\_\{1:t-1\},u\_\{1:t-1\}) p(xt−1∣z1:t−1,u1:t)=p(xt−1∣z1:t−1,u1:t−1) 后验概率分布。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 1]后验概率就是观测数据与预测数据的概率相乘。  
然后反复的进行迭代。

## 贝叶斯滤波实例 ##

完全不知道自己在什么地方，叫均匀分布，任何地方都是等可能的。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 2]

# 2、粒子滤波 #

粒子滤波是贝叶斯滤波的特例，是贝叶斯滤波的实现方式，是贝叶斯滤波的特例。可以做物体跟踪，机器人定位，做SLAM，视觉做SLAM产生问题，因为要同时估计机器人的位姿以及特征地图，特征地图会越来越大。更新的过程中涉及到求逆，求逆的复杂度是 n 3 n^3 n3，状态向量越来越大，会越来越慢，在小环境适用。在激光里面用了一种方式，把位姿和地图估计分开，可以在大地图中适用，但是也有例子耗散问题，大部分问题都是这个问题，无法解决，只能缓解。

## 2.1、示意图 ##

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

## 2.2、 特性 ##

*  贝叶斯估计器的一种实现方式
 *  能处理非线性情况
 *  能处理多峰分布的情况  
    全局定位
 *  用一系列的粒子近似概率分布
 *  非参滤波器  
    高斯分布可以由u 和 σ \\sigma σ确定

## 2.3 流程 ##

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 4]  
重采样，去除权重小的粒子的过程。 粒子的重新分布

## 2.4、状态传播 ##

![在这里插入图片描述][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]

## 2.5 权重评估 ##

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

## 2.6 重采样 ##

# 3、FastSLAM的原理及优化 #

基于粒子滤波（ＲＢＰＦ）SLAM，RB-Particle-filter，把问题分解了。  
RB能报地图分解出来。

## 3.1、 Fast-SLAM介绍 ##

*  SLAM:在给定传感器数据的情况下，同时估计机器人位姿和环境地图：  
     p ( x 1 : t , m ∣ u 1 : t , z 1 : t ) p(x\_\{1:t\},m|u\_\{1:t\},z\_\{1:t\}) p(x1:t,m∣u1:t,z1:t)  
    *除了要知道1到t时刻的位姿x还要估计地图，光知道t时刻的位姿没办法知道地图*
 *  SLAM可以分解成两个问题：RB分解
    
    1.  机器人定位
    2.  基于已知机器人位姿的构图 简单问题  
        全概率公式 p ( a , b ) = p ( a ) p ( b ∣ a ) p(a,b)=p(a)p(b|a) p(a,b)=p(a)p(b∣a)  
         p ( x 1 , m ∣ u 1 : t , u 1 : t , z 1 : t ) p(x\_1,m|u\_\{1:t\},u\_\{1:t\},z\_\{1:t\}) p(x1,m∣u1:t,u1:t,z1:t)  
        = p ( x 1 : t ∣ u 1 : t , z 1 : t ) p ( m ∣ x 1 : t , u 1 : t , z 1 : t ) p(x\_\{1:t\}|u\_\{1:t\},z\_\{1:t\})p(m|x\_\{1:t\},u\_\{1:t\},z\_\{1:t\}) p(x1:t∣u1:t,z1:t)p(m∣x1:t,u1:t,z1:t)  
        = p ( x 1 : t ∣ u 1 : t , z 1 : t ) p ( m ∣ x 1 : t , z 1 : t ) p(x\_\{1:t\}|u\_\{1:t\},z\_\{1:t\})p(m|x\_\{1:t\},z\_\{1:t\}) p(x1:t∣u1:t,z1:t)p(m∣x1:t,z1:t)  
         p ( x 1 : t ∣ u 1 : t , z 1 : t ) p(x\_\{1:t\}|u\_\{1:t\},z\_\{1:t\}) p(x1:t∣u1:t,z1:t)为估计机器人路径轨迹  
         p ( m ∣ x 1 : t , z 1 : t ) p(m|x\_\{1:t\},z\_\{1:t\}) p(m∣x1:t,z1:t)为给定机器人的位姿和传感器观测数据的情况，进行地图构建。本问题可以实现close-form的求解。
 *  用粒子滤波来估算机器人的位姿，然后分别为每一个粒子计算地图即可。因此一个粒子包含以下数据。
    
    1.  机器人的轨迹 x 1 : t x\_\{1:t\} x1:t
    2.  对应的环境地图

## 算法流程 ##

*  回顾贝叶斯公式和贝叶斯估计，可得  
    ![在这里插入图片描述][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]  
scan-matching 优化来对分布进行采用，然后用scan-match优化位姿。得到一个更优的位姿  
![在这里插入图片描述][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]  
目的还是为了减少粒子数量

## 进一步优化proposal ##

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 14]  
跟 z t z\_t zt相关，有m  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 15]

# 4、Gmapping的介绍 #

对Gmapping进行优化。

## Gmapping特性 ##

*  目前使用的最广泛的2D激光SLAM算法
 *  在较小的环境中能实现较好的建图效果
 *  以FastSLAM为基本原理  
    （粒子滤波）
 *  在FastSLAM的基础上进行了优化1和优化2
 *  Gmapping没有使用优化3

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70]: /images/20221023/0866796621074cc9b30403fd2633e2a2.png
[20210301142830515.png]: https://img-blog.csdnimg.cn/20210301142830515.png
[20210301142849541.png]: /images/20221023/b8e1996ab4d8498db669e0a57e978d4d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 1]: /images/20221023/614416273d0e4ea7a2d7e1c57b67ebcf.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 2]: /images/20221023/c8cd34221f6e4163821e15d2238ab461.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 3]: /images/20221023/8e57dd0820064e929e5debca5633798f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 4]: /images/20221023/0ac0f14af0c94664b0c9b9bd4b16fe56.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 5]: https://img-blog.csdnimg.cn/20210301162155424.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 6]: /images/20221023/ad4465a0f5c04abca14bd15cd3d4487f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 7]: /images/20221023/a80c865e0af64a90b7e676a5e7c037af.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 8]: /images/20221023/7894f27c72474cf98d86c2c0db85b39f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 9]: https://img-blog.csdnimg.cn/20210302164223242.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/e8aeac6a91704622beddc06133817891.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 11]: /images/20221023/c714d250e1d741828ce71a8ea5c066f5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 12]: /images/20221023/a8e3047e360540d488ca4c021f62b4cc.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 13]: /images/20221023/034261cb3da242fa85555b61a4dfbf4e.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 14]: /images/20221023/3153366e6f784783abe98f9b679a805b.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 15]: /images/20221023/a5ca1429ec3940eb87ca19b9620130ed.png