概率机器人-贝叶斯滤波

贝叶斯状态迭代是概率机器人的基础，理解它对平面slam技术，如gmapping算法原理有很大的帮助。先掌握基础原来，以后再考虑如何进行创新。

文章目录

概率机器人-贝叶斯滤波
前言
一、什么是贝叶斯滤波？
- 1、置信度
- 2、贝叶斯滤波算法
二、贝叶斯滤波的推导
- 1.基本概念
- 2.公式推导
总结

前言

书中介绍的概率机器人是基于隐马尔可夫模型，或者叫动态贝叶斯网络，即时刻ttt的状态随机地依赖t-1时刻的状态和控制输入utu_tut，测量ztz_tzt随机地依赖时刻t的状态。这就是贝叶斯滤波的基本假设了。看到这个假设，其实我们会想时刻ttt的状态是如何与前面的东西关联起来的，即那状态是如何更新的？大家可以带着问题进行思考。

一、什么是贝叶斯滤波？

1、置信度

首先介绍置信度的概念，贝叶斯滤波就是置信度的递归计算。

置信度的概念：
概率机器人通过条件概率分布表示置信度，是对给定所有过去的传感器测量和所有过去控制的环境状态的一个后验分布。比如bel(xt)=p(xt∣z1:t,u1:t)bel(x_t)=p(x_t|z_{1:t},u_{1:t})bel(xt)=p(xt∣z1:t,u1:t)，表示状态变量xtx_txt的置信度。

如何理解置信度？
机器人的状态是不能测量的，你说xtx_txt发生的可能性是多少，给个概率吧！那不就是以过去测量z1:tz_{1:t}z1:t和所有过去控制u1:tu_{1:t}u1:t为条件，时刻t下状态xtx_txt的概率分布。

有了bel(xt)bel(x_t)bel(xt)，据说刚执行完控制utu_tut，用zt−1z_{t-1}zt−1计算后验也是有用的。bel‾(xt)=p(xt∣z1:t−1,u1:t)\overline{bel}(x_t)=p(x_t|z_{1:t-1},u_{1:t})bel(xt)=p(xt∣z1:t−1,u1:t)。该概率被称为预测，是基于以前状态的后验。由bel‾(xt)\overline{bel}(x_t)bel(xt)计算bel(xt)bel(x_t)bel(xt)称为测量更新。理解了置信度的概念，那到底怎么进行更新？

2、贝叶斯滤波算法

贝叶斯滤波算法讲的就是测量更新的过程。

伪代码如下：

Algorithm Bayes filter
for all xtx_txt do
bel‾(xt−1)=∫p(xt∣ut,xt−1)bel(xt−1)dxt−1\quad \overline{bel}(x_{t-1}) =\int p(x_t \mid u_t,x_{t-1})bel(x_{t-1})dx_{t-1}bel(xt−1)=∫p(xt∣ut,xt−1)bel(xt−1)dxt−1
bel(xt)=ηp(zt∣xt)bel‾(xt)\quad bel(x_t)=\eta p(z_t\mid x_t)\overline{bel}(x_t)bel(xt)=ηp(zt∣xt)bel(xt)
end for
return bel(xt)bel(x_t)bel(xt)

贝叶斯算法就是一个递归的过程，它分两个部分：第一部分，控制更新。通过基于xt−1x_{t-1}xt−1的置信度和控制输入utu_tut来计算状态xtx_txt的置信度。第二部分，测量更新。用已经观测到的测量ztz_tzt的概率乘以置信度bel‾(xt)\overline{bel}(x_t)bel(xt)。但这样有个问题，对每个后验状态xtx_txt都这样做，乘积结果常不是一个概率，它的和可能不为1，所以公式中乘了一个归一化系数。

那这公式写得到底可信度有多少？下面来推导一下。

二、贝叶斯滤波的推导

1.基本概念

条件概率：
p(x∣y)=p(x,y)p(y)=p(x)p(y)p(y)p(x\mid y)=\frac{p(x,y)}{p(y)}=\frac{p(x)p(y)}{p(y)}p(x∣y)=p(y)p(x,y)=p(y)p(x)p(y)

全概率公式：
p(x)=∑yp(x∣y)p(y)p(x)=\sum_y p(x\mid y)p(y)p(x)=y∑p(x∣y)p(y)

贝斯准则：
p(x∣y)=p(y∣x)p(x)p(y)=p(y∣x)p(x)∑x′p(y∣x′)p(x′)p(x\mid y)=\frac{p(y\mid x)p(x)}{p(y)}=\frac{p(y\mid x)p(x)}{\sum_{x'}p(y\mid x')p(x')}p(x∣y)=p(y)p(y∣x)p(x)=∑x′p(y∣x′)p(x′)p(y∣x)p(x)

2.公式推导

bel‾(xt−1)=p(xt∣z1:t−1,u1:t)=∫p(xt∣z1:t−1,xt−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1=∫p(xt∣xt−1,u1:t)p(xt−1∣z1:t−1,u1:t−1)dxt−1\begin{aligned} \overline{bel}(x_{t-1})&=p(x_t\mid z_{1:t-1},u_{1:t})\\ &=\int p(x_t\mid z_{1:t-1},x_{t-1},u_{1:t})p(x_{t-1}\mid z_{1:t-1},u_{1:t})dx_{t-1}\\ &=\int p(x_t\mid x_{t-1},u_{1:t})p(x_{t-1}\mid z_{1:t-1},u_{1:t-1})dx_{t-1} \end{aligned}bel(xt−1)=p(xt∣z1:t−1,u1:t)=∫p(xt∣z1:t−1,xt−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1=∫p(xt∣xt−1,u1:t)p(xt−1∣z1:t−1,u1:t−1)dxt−1

因为状态是完整的，当条件中有xt−1x_{t-1}xt−1时可以将z1:t−1z_{1:t-1}z1:t−1去掉。又因为控制量utu_tut不能发生在状态xt−1x_{t-1}xt−1前，所以可以近似写成ut−1u_{t-1}ut−1发生时，xt−1x_{t-1}xt−1发生的概率。

用条件概率和贝叶斯准则对bel(xt)bel(x_t)bel(xt)进行展开:
bel(xt)=p(xt∣z1:t,u1:t)=p(z1:t∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)p(z1:t∣z1:t−1,u1:t)=p(z1:t∣xt,u1:t)p(xt∣z1:t−1,u1:t)p(z1:t∣z1:t−1,u1:t)=ηp(z1:t∣xt)p(xt∣z1:t−1,u1:t)\begin{aligned} bel(x_t)&=p(x_t\mid z_{1:t}, u_{1:t})\\ &=\frac{p(z_{1:t}\mid x_t, z_{1:t-1}, u_{1:t}) p(x_t\mid z_{1:t-1}, u_{1:t})}{p(z_{1:t}\mid z_{1:t-1}, u_{1:t})}\\ &=\frac{p(z_{1:t}\mid x_t, u_{1:t}) p(x_t\mid z_{1:t-1}, u_{1:t})}{p(z_{1:t}\mid z_{1:t-1}, u_{1:t})}\\ &=\eta p(z_{1:t}\mid x_t) p(x_t\mid z_{1:t-1}, u_{1:t}) \end{aligned} bel(xt)=p(xt∣z1:t,u1:t)=p(z1:t∣z1:t−1,u1:t)p(z1:t∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)=p(z1:t∣z1:t−1,u1:t)p(z1:t∣xt,u1:t)p(xt∣z1:t−1,u1:t)=ηp(z1:t∣xt)p(xt∣z1:t−1,u1:t)
还是基于状态完整的假设，条件中状态xtx_txt存在时可以将测量状态和控制输入去掉。

至此，公式就推完了。该算法的实现需要三个概率分布：初始置信度p(x0)p(x_0)p(x0)、测量概率p(zt∣xt)p(z_t\mid x_t)p(zt∣xt)和状态转移概率p(xt∣ut,xt−1)p(x_t\mid u_t, x_{t-1})p(xt∣ut,xt−1)。但是这些概率怎么求，或者如何设定，不在此讨论，本文仅对贝叶斯滤波进行介绍和推导。

总结

机器人及其环境的动态以两种概率法则，即状态转移分布和测量分布的形式为特点。（需要初始置信度、测量概率和状态转移概率）状态转移分布描述状态如何随时间变化的特征，可能作为机器人控制的效果。测量分布描述测量如何由状态控制的特征，两个分布都是概率性的，导致状态演变和检测固有的不确定性。

贝叶斯滤波是基于隐马尔可夫假设，即过去和未来数据都是独立的。但是未建模因素，概率的不精确性会影响位姿的估计。所以以此原理的位姿估计，在同一位置，估计结果应该会不一样。