激光slam-粒子滤波

       从逻辑上讲，机器人SLAM的过程是：先控制机器人到达一个位姿，然后再进行观测。但是问题出现在了我们是由里程计数据和激光雷达观测数据来求机器人位姿，这就出现了一个逆向的过程即：我们是先有位姿再得到观测数据却变成了先得到观测数据再来求位姿的过程。这就用到了贝叶斯估计。

粒子滤波是基于贝叶斯滤波器实现的，也是最典型的应用便是定位问题，主要特性为：

• 贝叶斯估计器的一种实现方式
• 能处理非线性情况
• 能处理多峰分布的情况
• 用一系列的粒子(particle)近似概率分布
• 非参滤波器

 

算法流程：

1：算法的输入为 时刻的粒子也就是 时刻的状态，时刻的控制与观测

2：定义两个空的粒子集

3：粒子集中数目都为M,，针对时刻的 M 个粒子逐一处理

4：状态转移，对 时刻的某一可能状态施加控制作用，得到该粒子经过  控制后的状态分布    并从中采样获得一个新的粒子。所有粒子这样处理后得到的粒子集便是先验  的粒子表示。

5：为每一个粒子计算一个权重，称为重要性因子，该权重的计算是基于观测模型，也就是反应了粒子的可信程度。

6：粒子的状态及其对应的重要性因子组成完整的粒子集

7：循环结束

8：处理新得到的粒子集    ，将先验粒子集转换为后验粒子集

9：从粒子集    中采样M次以生成新的粒子集，因为每个粒子的可信度并不相同，所以并不能等概率采样，抽取到每个粒子的概率便由重要性因子决定。因此重要性因子高，也就是可信度高的例子被采样到的次数相对较多。因此采样结束后，粒子向可信度高的状态处聚集。所得到的便是后验粒子集。

10：采样到的粒子添加到新的粒子集  中

11：循环结束

12：得到后验分布的抽样表示

如下图所示， 小竖线即表示粒子，越密集也就是粒子越多的地方表示概率越大。

 

 

我们有一个小车🚗，可以在2D平面上移动我们离散化整个过程，意思是，我们在每个k*T（T是周期）的时候用粒子滤波定位小车的位置。比如，出发后第一秒1s，第二秒2s，第三秒3s。

粒子滤波定位小车的过程分为三步，需要三个信息（就是我们知道三件事）。

哪三个信息呢？

1.车上一秒在哪

2.车的运动是什么

3.测量结果，比如GPS

哪三步呢？

1.Prediction Step
 我们用“粒子”点来表示车的位置，上一步的位置，我们用x0来表示，他们都是车的可能的位置。比如(1,5),(3,5),(3,7),(5,5)

那你说这些位置听谁的呢？车在哪呢？我们可以给每个位置定个权重，然后加权平均一下。在预测这步权重都是一样的，我们有四个粒子，那么每个权重就是0.25。也就是我们有四个粒子x1，x2，x3，x4，x[(横坐标x，纵坐标y)，权重w]

                                                  x1[(1,5),0.25]， x2[(3,5),0.25]，x3[(3,7),0.25]，x4[(5,5),0.25]
然后我们有个运动是(5,4)，预测这步就是把所有权重不变，在状态（运动）加上这个运动的结果。

                                                   x1[(6,9),0.25]，x2[(8,9),0.25]，x3[(8,11),0.25]，x4[(10,9),0.25]

如果你知道噪声的大小，可以在结果上加上一个随机数。结果如下：

                                                   x1[(6.1,9.2),0.25]，x2[(7.9,9),0.25]，x3[(8.3,10.8),0.25]，x4[(10.1,8.9),0.25]

预测这步就完啦。
2.Innovation Step
按照激光测量的概率分布来重新分配权重。高斯分布的话当然是离中心点越近权重越大啦。

                                                  x1[(6.1,9.2),0.4]，x2[(7.9,9),0.5]，x3[(8.3,10.8),0.35]，x4[(10.1,8.9),0.25]

现在权重更新了，但是还有个问题！我们稍后希望用概率的方式来使用权重，但是权重加在一起大于1了怎么办？归一化一下就好了。

                                                             每个新权重=旧权重/求和（旧权重）

                                                     x1[(6.1,9.2),0.26]，x2[(7.9,9),0.33]，x3[(8.3,10.8),0.23]，x4[(10.1,8.9),0.16]

然后这步就完了，简单吧
3. Re-sampling Step
你上一步得到的那些权重没用上对不对，这步用上了。

我们之前那些粒子不用了，重新摇色子摇几个粒子出来。不过一般来讲，粒子数量是不变的，都是能足够多就足够多。

比如之前的

                                                       x1[(6.1,9.2),0.26]，x2[(7.9,9),0.33]，x3[(8.3,10.8),0.23]，x4[(10.1,8.9),0.16]

比如x2的概率是0.33，那么新摇号摇出来的粒子可能有两个x2，因为它概率最大，x4的概率比较小可能一个没有。

然后我们认为摇出来的新的粒子权重相等。

                                                         x1[(6.1,9.2),0.25]，x2[(7.9,9),0.25]，x3[(8.3,10.8),0.25]，x4[(7.9,9),0.25]

最终的估计值可以是四个粒子的平均。