概率机器人学习笔记：递归状态估计

不再可能情况的单一“最好推测”，而用概率算法表示整个推测空间的概率分布信息。其核心为模糊性和置信度，通过在整个空间上的概率密度函数来表示机器人的瞬时置信度<机器人瞬时位置各可能点的各自概率>，机器人感知问题用概率来描述，就是一个状态估计问题。概率算法预支未来的不确定性，在本质上确实比非概率算法效率要低，因为它考虑的是整个概率密度而不是单一的推测。

Kantsang

1584人浏览 · 2018-10-25 13:56:44

Kantsang · 2018-10-25 13:56:44 发布

由归纳法推导说明。在将时刻t=0的置信度正确初始化的假设下，环境特征以状态来表征，认为状态是所有会对未来产生影响的机器人以及环境的所有方面的因素。假设一个状态x可以最好的预测未来，则称其为完整的，未来可以是随机的，但没有先于x的状态变量可以影响未来状态的随机变化。满足这些条件的暂态过程称为马尔科夫链。状态完整性主要是理论上的，在实际的机器人系统当中，不可能指定一个完整的系统。

滤波过程一般分为两个步骤：预测和更新。贝叶斯滤波器当然也不例外。状态转移概率预测，测量概率修正。

概率生成法则

在整个状态估计当中，有两个很重要的概念即状态和测量的演变，由概率法则表示为状态转移概率和测量概率。在设定好初始化概率的情况下，系统产生控制作用 $u_{t}$ ,经过计算的到一个控制作用产生后一个当前的系统状态即为状态转移 $P(x_{t}|x_{t-1},u_{t})$ ，

即t-1时刻的系统对其施加一个 $u_{t}$ 的控制作用，经过计算推导得到t时刻系统状态，这是一个先验状态值，它指出环境状态作为机器人控制 $u_{t}$ 的函数是如何变化的。

而后再经过传感器数据推导得到真实的状态，就可将之前计算得到的系统状态做一个修正，这个中间量就是测量概率 $P(z_{t}|x_{t})$ ，即经过 $u_{t}$ 作用在t-1时刻的系统上得到t时刻的系统状态值，这是一个后验值。

以次，状态转移概率和测量概率一起，描述了机器人及其环境组成的动态随机系统。

置信分布

状态不可测量，而机器人必须从数据中推导出当前位姿。对于真实的状态，置信度分布为每一个可能的假设分配一个概率或者说一个概率密度值。置信度分布是以可获得数据为条件的后验概率，用 $bel(x_{t})$ 表示置信度。

$bel(x_{t})=P(x_{t}|z_{1:t},u_{1:t})$ 经过测量值修正的后验概率

$\bar{bel}(x_{t})=P(x_{t}|z_{1:t-1},u_{1:t})$ 先验值，即预测值

贝叶斯滤波数学推导

我们来回顾一下一些基本的概率统计当中的一些概念。首先是贝叶斯公式的推导：

$P(A|B)=P(A\cap B)/P(B)$ $P(B|A)=P(B\cap A)/P(A)$

则 $P(A|B)\cdot P(B)=P(B|A)\cdot P(A)$ 即 $P(A|B)=(P(B|A)\cdot P(A))/P(B)$

以任意随机变量为条件，如以条件 $Z=z$ 为条件由贝叶斯公式：

$P(x|y,z) = \frac{P(y|x,z)P(x|z)}{P(y|z)}$

由上一步后验值 $p(x_{t-1}|z_{1:t-1},u_{1:t})$ 计算后验分布 $p(x_{t}|z_{1:t},u_{1:t})$ 。在将t=0时刻的置信度 $bel(x0)$ 正确初始化的假设下

由贝叶斯公式得到目标后验：

$\mathit{p(x_{t}|z_{1:t},u_{1:t})=\frac{p(z_{t}|x_{t},z_{1:t-1},u_{1:t})p(x_{t}|z_{1:t-1},u_{1:t})}{p(z_{t}|z_{1:t-1},u_{1:t})}}$

$=\eta p(z_{t}|x_{t},z_{1:t-1},u_{1:t})p(x_{t}|z_{1:t-1},u_{1:t})$

而在个条件独立的情况下 $p(z_{t}|x_{t},z_{1:t-1},u_{1:t})=p(z_{t}|x_{t})$ , $p(x_{t}|z_{1:t-1},u_{1:t})=\bar{bel}(x_{t})$ 则：

$p(x_{t}|z_{1:t},u_{1:t})=\eta p(z_{t}|x_{t})\bar{bel}(x_{t})$ 即 $bel(x_{t})=\eta p(z_{t}|x_{t})\bar{bel}(x_{t})$

由全概率公式 $p(x)=\int p(x|y)p(y)dy$ 可得：

$\bar{bel}(x_{t})=p(x_{t}|z_{1:t-1},u_{1:t})$

$=\int p(x_{t}|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t},u_{1:t})d_{x-1}$

马尔科夫假设是全书的基本假设，所有的公式都是在这个假设基础行推导而来的，马尔科夫假设核心思想是：已知当前时刻的状态,不会影响过去和将来的状态，即条件相互独立。

$\bar{bel}(x_{t})=\int p(x_{t}|x_{t-1},u_{t})p(x_{t-1}|z_{1:t-1},u_{1:t-1})d_{x-1}$ 即

$\bar{bel}(x_{t})=\int p(x_{t}|x_{t-1},u_{t})bel(x_{t-1})d_{x-1}$

$bel(x_{t})=\eta p(z_{t}|x_{t})\bar{bel}(x_{t})$

而基本的贝叶斯滤波算法输入量为前一时刻后验值、此刻的控制量和此刻的测量值三个值。伪代码如下：

1.机器人使用一个可以测量0~3m距离的传感器。为了简化，假定真实的距离在这个范围中均匀分布。很不幸的是，传感器会坏掉。当传感器故障时，不管传感器的锥形测量范围内实际测距结果应该是多少，其输出测距值均小于1m，已知对于传感器故障的先验概率是p=0.01。
设想机器人查询了N次传感器，每次测量值都小于1m。对于N=1,2,...,10 的传感器故障的后验概率是多少？用公式表示相关的概率模型。
答：设Xn为随机变量，表示距离传感器在日期n时的状态。

$x_{n}=\left\{\begin{matrix} 0 & broken & \\ 1& intact& \end{matrix}\right.$ 则 $\begin{matrix} P(X_{n} = 0)= P = 0.01& & \\ P(X_{n} = 1)= 1 - P = 0.99& & \end{matrix}$