《概率机器人》-递归状态估计

状态估计主要目的在于从机器人传感器测量数据中找回状态变量。概率状态估计算法在可能的状态空间上计算置信度分布。

本章旨在引入基于传感器数据进行状态估计的基本术语和数学工具。

本文综述：

（一）基本概率知识；

（二）描述了机器人环境相互作用的形式化模型；

（三）贝叶斯滤波及其引起的描述及计算问题。

（一）涉及到的概率知识

1.基本概率分布

P（X=x）表示随机变量X取值为x时的概率，可简写为P(x);

（1）基本概率形式

由于本书中大部分依赖于连续空间，故贴近连续分布进行阐述：

假定所有连续随机变量都拥有概率密度函数（PDF）。例如:本书常用的特殊分布----->正态分布，其概率密度函数为高斯函数：

x为标量值：
$$p(x)=(2\pi \sigma {)}^{-\frac{1}{2}}{e}^{-\frac{1}{2}\frac{(x-\mu {)}^2}{{\sigma }^2}}(1)$$
x为多维矢量：
$$p(x)=det(2\pi \Sigma {)}^{-\frac{1}{2}}{e}^{-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )}(2)$$
注：公式（2）其实是公式（1）的严格泛化；如果$$\Sigma ={\sigma }^2$$，那么两公式等价。

（2）常用概率公式

联合分布:两随机变量X、Y的联合分布为：

​ p（x,y）=p(X=x,Y=y)

​ 若X,Y相互独立，则进一步p（x,y）=p(x）·p(y)

条件概率：已知Y的值为y，则基于已知条件X为x的概率为：

​ p（x|y）=p(X=x|Y=y)

​ 若p(y)>0，则p（x|y）=p(x,y)/p(y)

​ 特殊的，如果两者独立，则p（x|y）=p(x)

全概率公式：

​
$$p(x)=\sum _{y}p(x|y)p(y)(离散)$$

$$p(x)=\int p(x|y)p(y)dy（连续）$$

贝叶斯准则：
$$p(x|y)=\frac{p(y|x)p(x)}{P(y)}=\frac{p(y|x)p(x)}{{\sum }_{x^{\prime }}p(y|x^{\prime })p(x^{\prime })}(离散)$$

$$p(x|y)=\frac{p(y|x)p(x)}{P(y)}=\frac{p(y|x)p(x)}{\int p(y|x^{\prime })p(x^{\prime })d{x}^{\prime }}(连续)$$

贝叶斯准则在概率机器人中起到主导的作用！！！！如果说x是一个希望由y推导出来的结果，则概率p（x）可以成为先验概率分布，y称为数据，也就是传感器测量值。分布p（x）总结了数据y关于x的所有信息，p(x|y)成为在X上的后验概率分布。贝叶斯准则的思想利用逆向思维，依靠p（y|x）以及先验概率p（x）计算后验概率p(x|y)的分布情况。通俗讲，如果想从传感器数据y推到位置x，贝叶斯准则通过假定位置为x时，传感器为y的概率。机器人学中将概率p（y|x）成为生成模型。

同时应该主要到的重点是分母p（y）不依赖x，因此分母可归一化表示：

$$p(x|y)=\eta p(y|x)p(x)$$
条件概率的贝叶斯准则(条件：Z=z)：
$$p(x|y,z)=\frac{p(y|x,z)p(x|z)}{P(y)}$$
​ 进一步，假设变量x，y相互独立：

​
$$p(x|y,z)=p(x|z)p(y|z)$$
​ 需要注意，这种独立不意味绝对独立，即：

​
$$p(x|y,z)=p(x|z)p(y|z)\ne p(x,y)=p(x)p(y)$$

（3）期望

$$E[X]=\sum _{x}xp(x)(离散)$$

$$E[X]=\int xp(x)dx(连续)$$

$$E[aX+b]=aE[X]+b$$

（4）协方差

$$Cov[X]=E[X-E[X]{]}^2=E[X^2]-E[X{]}^2$$

（5）熵！表达机器人在执行具体行动时可能接收到的信息

$$H_p(x)=E[-lo{g}_{2}p(x)]$$

​ 可导出：
$$H_p(x)=-\sum _{x}p(x)lo{g}_{2}p(x)(离散)$$

$$H_p(x)=-\int p(x)lo{g}_{2}p(x)dx(连续)$$

（二）机器人环境交互

机器人与环境之间的交互通俗点讲，就是机器人感知外界环境，根据环境信息做出行为决策，循环往复。

1.状态

状态是所有会对未来产生影响的机器人及其环境的所有方面因素，包括静态和动态两类。通俗讲，就是会对slam过程产生影响的因素。本书中状态用x来表示，与时间有关则表示为x_t。经典状态变量如下：

·机器人位姿：包括机器人相对于全局坐标的位置和方向。三维刚性机器人包括3个直角坐标值、3个角度方向（俯仰、横滚和偏航）。二维刚性机器人包括2个平面坐标值及朝向（偏航）。

·机器人配置:机器人执行机构配置。如某关节自由度等。

·机器人速度及加速度。

周围环境的位置及特征。

移动物体和人的位置、速度。

其他影响机器人运行的状态变量如电池状态等。

状态完整性概念即x_t包括过去状态测量及控制信息，可以最好地预测未来。（理想化，因为现实不可能获取所有要求的状态信息）。

2.环境交互

机器人与环境之间存在两种基本交互类型：1.机器人通过执行机构影响环境状态；2.机器人通过传感器收集有关状态的信息。

环境传感器测量机器人通过传感器（摄像机、测距扫描等）获取环境状态信息。

控制动作改变环境机器人通过对环境施加作用力来改变周围状态。

假设机器人可以保持所有过去的传感器测量和控制动作记录，将其记录称为数据。则机器人共获得两种不同的数据流：

环境测量数据包含环境的暂态信息，例如摄像机图像、测距扫描距离等。

​ 本书中，大多数情况都为简单假定机器人在一个时间点进行测量，即针对一个时间点进行扩展。
$$z_{t_1:t_2}=z_{t_1},z_{t_1+1},z_{t_1+2},\cdot \cdot \cdot z_{t_2}$$
​ 上式表示从时间t1到t2获得的所有测量集合，t1<=t2.

控制数据携带环境中关于状态改变的信息比如机器人速度、里程计等信息。

两种不同的数据发挥了本质上不同的作用，但是感知和控制是同时发生的。

3.概率生成法则

状态和测量的演变由概率法则支配。

（1）状态值

通常状态x_t是随机地由状态x_(t-1)产生的，且某时刻状态x_t是以所有过去的状态、测量和控制为条件的，故表征状态演变的概率法则可表示为：
$$p(x_t|x_{0:t-1},z_{1:t-1},u_{1:t})$$
重要的：如果状态x是完整的，则它是所有以前时刻发生的所有状态的充分总结。则x_{t-1}是直到t-1时刻所有控制和测量的充分总结。则有以下等式表达：
$$p(x_t|x_{0:t-1},z_{1:t-1},u_{1:t})=p(x_t|x_{t-1},u_t)$$
该表达的特性就是条件独立，这一思想贯穿本书。即如果知道了第三组变量（条件独立）的值，则该变量就是独立于其他变量的。

（2）测量值

如果x_t是完整的，则有如下很重要的条件独立：
$$p(z_t|x_{0:t},z_{1:t-1},u_{1:t})=p(z_t|x_t)$$
简述表达为：用状态x_t足以预测（有潜在噪声的）测量z_t。如果x_t是完整的，则任何其他变量信息，如过去的测量、控制、状态都与之无关。

状态转移概率：
$$p(x_t|x_{t-1},u_t)$$
指出环境状态作为机器人控制u_t的函数如何随时间变化。

测量概率：

$$p(z_t|x_t)$$
上述两概率一起描述了机器人及其环境组成的动态随机系统。可用动态贝叶斯网络展现由这些概率定义的状态和测量的演变。时刻 t 的状态随机依赖 t-1时刻的状态和控制u_t。测量z_t随机地依赖时刻t的状态，这样的时间生成模型也成为隐马尔可夫模型或动态贝叶斯网络，如下图：

​ 图1 表征控制、状态和测量演变特征的动态贝叶斯网络

4.置信分布

置信度反映了机器人有关环境状态的内部信息。通俗点讲，机器人的状态并不能直接测量，一般都从数据中进行推测，因此要从位姿的内部置信度辨别出真正的状态。置信度则表示该推测是正确的可能性。

机器人通过条件概率分布表示置信度。置信度分布是以可获得数据作为条件的关于状态变量的后验概率。这里用bel(x_t)表示状态变量x_t的置信度：
$$bel(x_t)=p(x_t|z_{1:t},u_{1:t})$$
这个后验是时刻t下状态x_t的概率分布，以所有过去测量z{1:t}和所有过去控制u{1:t}为条件。

另外，后验概率的预测形式为（即不包括测量z_t,因为其表示t时刻的测量)：
$$/bel(x_t)=p(x_t|z_{1:t-1},u_{1:t})$$

(三)贝叶斯滤波

1.贝叶斯滤波算法

该算法根据测量和控制数据计算置信度分布bel()。

贝叶斯滤波算法是递归的，即时刻t的置信度bel(x_t)由时刻t-1的置信度bel(x_t-1)来计算。

​ 算法输入为：时刻t-1的置信度、最近的控制作用u_t和最近的测量z_t；

​ 算法输出为：时刻t的置信度bel(x_t)。

利用伪代码形式可描述基本贝叶斯滤波过程（单次递归）：

图2 贝叶斯滤波过程
贝叶斯滤波算法具有两个步骤：

​ 控制更新也就是根据上一个状态和当前的控制预测当前的状态。

​ 测量更新根据当前的测量结果去修正上一步的预测结果。这一步主要是根据贝叶斯法则来实现的,也就是条件概率。

2.马尔可夫假设

马尔可夫假设又称完整状态假设，即如果知道当前状态x_t，马尔可夫假设设定过去和未来数据都是独立的。