【Probabilistic Robotics】【概率机器人】【SLAM】Chapter 2 “Recursive State Estimation“ ---- 总结、心得与扩展阅读

序：此博客的主要内容是《Probabilistic Robotics》的第二章节“迭代状态估计”，目的与我上一篇文章一致。读者若没有读过第一篇可以点击下面的链接跳转。由于时间仓促，恐有疏漏，还望各位批评指正。

点击我进行跳转

正文

第二章分为七个部分，其中第三、第四部分内各有四个小部分，如下：

1. Introduction
2. Basic Concepts in Probability
3. Robot Environment Interaction
   3.1 State
   3.2 Environment Interaction
   3.3 Probabilistic Generative Laws
   3.4 Belief Distributions
4. Bayes Filters
   4.1 The Bayes Filter Algorithm
   4.2 Example
   4.3 Mathematical Derivation of the Bayes Filter
   4.4 The Markov Assumption
5. Representation and Computation
6. Summary
7. Bibliographical Remarks

Introduction

   本节介绍了该章的结构。其中，贝叶斯滤波器是重点，是本书大部分算法的基础。

Basic Concepts in Probability

   该部分介绍了一些概率论的基础知识。有一定基础的朋友可以跳过该部分。

1. 随机变量：设X为一随机变量，x代表X中一可能发生的特定事件，p(x)代表随机变量X的值为x时的概率。一个标准的例子是抛硬币：

$$p(X=正面)=p(X=背面)=1\text{\hspace{1em}(2.1)}$$

   推广至多事件，可以表达为：

$$\sum _{x}p(X=x)=1（离散情况）\text{\hspace{1em}(2.2)}$$

2. 正态分布：分为标量和多维向量两种情况（当多维公式维度为1时与标量等价）

p ( x ) = ( 2 π σ ) − 1 2 e x p { − 1 2 ( x − μ ) 2 σ 2 } （标量） (2.3) \displaystyle p(x) = (2\pi \sigma)^{-\frac{1}{2}} exp\{{-\frac{1}{2} \frac{(x-\mu)^{2}}{\sigma ^{2}}}\}（标量）\tag{2.3} p(x)=(2πσ)−21​exp{−21​σ2(x−μ)2​}（标量）(2.3)

   其中，（2.3）可简写为 $N(x;\mu ,{\sigma }^2)$，$\mu$和${\sigma }^2$为随机变量的均值和方差。

p ( x ) = d e t ( 2 π Σ ) − 1 2 e x p { − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) } （多维向量） (2.4) \displaystyle p(x) = det(2\pi \Sigma )^{-\frac{1}{2}} exp\{{-\frac{1}{2} (x-\mu)^T \Sigma ^{-1}(x-\mu)}\}（多维向量）\tag{2.4} p(x)=det(2πΣ)−21​exp{−21​(x−μ)TΣ−1(x−μ)}（多维向量）(2.4)

   其中，（2.4）可简写为 $N(x;\mu ,\Sigma )$，$\mu$和$\Sigma$为随机变量的均值和协方差矩阵（该矩阵正半定对称）。

3. 一些常见公式：

   3.1 联合概率分布：

$$\begin{gathered} p(x,y)=p(X=xandY=y)=p(x)p(y) \\ (第一个等号无条件，但是第二个等号要求变量X,Y相互独立)\text{\hspace{1em}(2.5)} \end{gathered}$$

   多变量推广：当 $x$ 和 $y$ 条件独立时（在 $Z=z$ 发生前提下，讨论$X$和$Y$的情况），

$$p(x,y|z)=p(x|z)p(y|z)\text{\hspace{1em}(2.6)}$$

   需要注意的是，绝对独立和条件独立没有关系，即（2.6）和（2.5）不能互相推导。只有在巧合的时候才会出现（2.5）（2.6）相等。

   3.2 条件概率，当 $Y=y$ 时，$X=x$ 的概率：

$$p(x|y)=p(X=x|Y=y)=\frac{p(x,y)}{p(y)}\text{\hspace{1em}(2.7)}$$

   3.3 全概率公式，分为离散和连续两个部分：

$$p(x)=\sum _{y}p(x|y)p(y)(离散)\text{\hspace{1em}(2.8)}$$

$$p(x)=\int p(x|y)p(y)dy(连续)\text{\hspace{1em}(2.9)}$$

   3.4 贝叶斯法则 （重点）

$$p(x|y)=\frac{p(y|x)p(x)}{p(y)}\text{\hspace{1em}(2.10)}$$

$$p(y)=\sum _{x^{\prime }}p(y|x^{\prime })p(x^{\prime })(离散)\text{\hspace{1em}(2.11)}$$

$$p(y)={\int }_{}p(y|x^{\prime })p(x^{\prime })d{x}^{\prime }(连续)\text{\hspace{1em}(2.12)}$$

   个人理解：$p(x)$ 是先验概率分布，是系统在获得新信息前的随机变量的旧分布。$p(y)$ 是新信息，用来更新旧分布，可以视为传感器获得的新信息，$p(x|y)$ 是后验概率分布，是原来$p(x)$更新后的新分布。

   3.5 贝叶斯法则的简化

   根据公式（2.10）和（2.11）我们可知，$p(y)$ 的值不受 $x$ 的影响。所以，$p(y{)}^{-1}$ 可以视为常数。通常来说，我们把这个常数视作正则化或标准化变量，用 $\eta$ 表示，（2.9）化为：

$$p(x|y)=\eta p(y|x)p(x)\text{\hspace{1em}(2.13)}$$

   用辅助变量$au{x}_{x|y}$和$au{x}_y$来简化表达贝叶斯法则：

$$au{x}_{x|y}=p(y|x)p(x)\text{\hspace{1em}(2.14)}$$

$$au{x}_y=\sum _{x}au{x}_{x|y}\text{\hspace{1em}(2.15)}$$

$$p(x|y)=\frac{au{x}_{x|y}}{au{x}_y}\text{\hspace{1em}(2.16)}$$

   这么做的目的是简洁。$\eta$ 可能在不同式子中的赋值不一样，这样会导致${\eta }^{\prime }$，${\eta }^{\prime \prime }$…等符号的使用，为了防止乱套，我们都用 $\eta$ 去表示，即使在不同式子中的赋值不一样。这么做是可以的，因为 $\eta$ 的作用是归一化，只要最终结果归一化，运算过程中的 $\eta$ 值不重要。

   3.6 期望与方差，$E$为期望，$Cov$为方差：

$$E[aX+b]=aE[X]+b\text{\hspace{1em}(2.17)}$$

$$Cov[X]=E[X-E[X]{]}^2=E[X^2]-E[X{]}^2\text{\hspace{1em}(2.18)}$$

   **3.7 熵（略读）：

   熵是一个来源于信息论的概念。在本书中，熵将用于表示机器人信息收集的程度（即概率），以表达机器人在执行特定动作 $x$ 时可能接收到的信息，表达方式如下：

$$H(P)=E[-lo{g}_{2}p(x)]=-\sum _{x}p(x)lo{g}_{2}p(x)\text{\hspace{1em}(2.19)}$$

   $-lo{g}_{2}p(x)$ 是使用最佳编码方法对 $x$ 进行编码所需的位数，$p(x)$ 是观测到 $x$ 的概率。

Robot Environment Interaction

   如图1所示，机器人通过传感器感知环境，通过执行器影响环境。那么，如何用精确的语言描述这种“感知”和“影响”呢？我们从基础概念——状态开始：

图1 机器人与环境的互动

1. State

  在本书中，我们将状态视作机器人及其环境的所有特征的集合，它的符号是 $x$ ，当时间为 $t$ 时，状态为 $x_t$。其中，状态有动静、内外之分。例如，大街上人的位置是动态的；建筑物的墙是静态的；机器人自身的姿态，位置，速度是内部状态，环境变量为外部的。

  此外，如果一个状态包含完整的过去状态、测量或控制信息，而且不具有对未来进行准确预测的信息，我们称这个状态是完整的。反之，如果一个状态不包含完整的信息，我们称之为不完整的状态。

  在一个大多数机器人系统中，状态通常是连续的，比如机器人的位姿，速度等；但有时，状态也可以是离散的，例如判断传感器是否损坏，0为损坏，1为良好等。对于既包含连续状态空间，又包含离散状态空间的状态空间，我们称为混合状态空间。

  此外，如果在一个过程中， $x_{t-1}$ 的信息只对 $x_t$ 产生影响，不对 $x_{t+1}$ 产生影响时，我们称满足这个条件的过程为马尔可夫链。

2. Environment Interaction

   从图1我们易得，机器人与其环境之间的交互有两种基本类型：I. 通过执行器影响环境状态 II. 通过传感器收集状态信息。下面我们分开解释其中的重点部分：

   I. 传感器测量：传感器测量值到达时会有一定的延迟，比如相机获得的图像需要时间处理才能使用。

   II. 控制器：控制的例子包括机器人自身的运动和对物体的操控。在噪音的影响下，即使机器人本身不执行任何动作，状态通常也会发生变化。

   根据两种不同的环境交互方式，机器人可以访问两种不同的数据流：I. 测量数据 II.控制数据

   I. 测量数据：时间 $t$ 处的测量数据用 $z_t$ 表示。在本书中，忽略小的时序影响。比如，大多数激光雷达传感器以非常高的速度顺序扫描环境，但我们将简单地假设它只对应于特定的时刻。

   II. 控制数据：时间 $t$ 处的控制数据用 $u_t$ 表示。我们假设每个时间步 $t$ 恰好有一个控制项 $u_t$。控制数据的主要信息与状态的变化有关。举个例子，将机器人的速度设置为每秒 10 厘米并持续 5 秒，表明执行此运动命令后机器人的位姿比命令执行前的位姿提前约 50 厘米。

  需要注意的是，理解测量和控制之间的区别至关重要，因为两种类型的数据在根本上发挥着不同的作用。感知提供了有关环境状态的信息，因此它往往会增加机器人的感知信息；另一方面，由于机器人驱动中固有的噪声和机器人环境的随机性，运动往往会导致机器人的感知信息损失。

3. Probabilistic Generative Laws

  状态和测量的演化受概率定律影响。在这里，我们假设状态 $x_t$ 的出现可能以所有过去的状态、测量和控制为条件，表达形式如下所示：

$$p(x_t|x_{0:t-1},z_{1:t-1},u_{1:t})\text{\hspace{1em}(2.20)}$$

  假如状态 $x$ 是完整的，那么该状态包括了之前步骤中出现的所有信息。以 $x_{t-1}$ 为例，它包含了 $z_{1:t-1},u_{1:t-1}$ 的信息。所以，（2.20）可以化简为如下：

$$p(x_t|x_{t-1},u_t)\text{\hspace{1em}(2.21)}$$

  概率式（2.21）是状态转移概率，它表明了状态如何随着时间的推移而演变。因为状态是随机的，所以状态转移是概率的，而不是确定性的。

  同理，我们对生成测量值的过程进行建模，假如状态 $x_t$ 是完整的，则：

$$p(z_t|x_{0:t},z_{1:t-1},u_{1:t})=p(z_t|x_t)\text{\hspace{1em}(2.22)}$$

  概率式（2.22）是测量概率，它表明了从状态 $x$ 生成测量 $z$ 的概率定律。而且，（2.22）的等号也说明了如果当前状态是完整的，那么该状态足以测量此时的新信息 $z_t$；

  我们不难发现，时间 $t$ 时的状态随机依赖于时间 $t-1$ 时的状态和控制 $u_t$。 测量值 $z_t$ 随机取决于时间 $t$ 时的状态。这种时间生成模型也称为隐马尔可夫模型（HMM）或动态贝叶斯网络（DBN）。 为了完全指定模型，我们还需要初始状态分布 $p(x_0)$。

4. Belief Distributions

  概率机器人学的另一个关键概念是置信度，而且有些状态我们无法直接测量。比如，我们可以通过GPS获得机器人在世界坐标系的位置，但我们无法知道机器人的姿态。所以，我们必须从数据推断其姿态。

  置信分布为每个可能的关于真实姿态的假设分配一个概率（或密度值）。 置信分布是基于可用数据的状态变量的后验概率，用 $bel(x_t)$ 来表示对状态变量 $x_t$ 的置信度：

$$bel(x_t)=p(x_t|z_{1:t},u_{1:t})\text{\hspace{1em}(2.23)}$$

  该后验是时间 $t$ 状态 $x_t$ 的概率分布，以所有过去的测量 $z_{1:t}$ 和所有过去的控制 $u_{1:t}$ 为条件。

  有后验概率就有先验概率，在 $x_t$ 时刻，在机器人接收传感器的数据 $z_t$ 前，先验概率为：

$$\overline{bel}(x_t)=p(x_t|z_{1:t-1},u_{1:t})\text{\hspace{1em}(2.24)}$$

  先验概率反应了在合并时间 $t$ 的测量之前，根据先前的状态后验预测时间 $t$ 的状态。从 $bel(x_t)$ 计算 $\overline{bel}(x_t)$ 称为校正或测量更新。

Bayes Filters

The Bayes Filter Algorithm

  直接给出贝叶斯滤波器的伪代码形式：

1. \quad\quad\quad Algorithm_Bayes_Filter$(bel(x_{t-1})$,$u_t$,$z_t)$
2. \quad\quad\quad\quad for all $x_t$ do
3. \quad\quad\quad\quad\quad $\overline{bel}(x_t)=\int p(x_t|u_t,x_{t-1})bel(x_{t-1})dx$
4. \quad\quad\quad\quad\quad $bel(x_t)=\eta p(z_t|x_t)\overline{bel}(x_t)$
5. \quad\quad\quad\quad end
6. \quad\quad\quad\quad return $bel(x_t)$

  先看结构，该算法很明显是迭代的，通过 $bel(x_{t-1})$ 计算 $bel(x_t)$。

  再看输入输出，它的输入是时间 $t-1$ 时的置信度 $bel(x_{t-1})$，以及最近的控制 $u_t$ 和最近的测量 $z_t$；输出是时间 $t$ 时的置信度 $bel(x_t)$。

  再看核心，很明显是步骤3和步骤4。

  在步骤3中，方程左边是先验置信，那么右边一定含有$x_{t-1}$和$u_t$，而不含有$z_t$。再看右边，实际上是状态转移概率与上个时刻的置信分布的乘积的积分。这个很好理解，已知上一个时刻的后验置信分布了，我想获得这个时刻的置信分布，怎么办呢？状态转移概率可以帮助我们，它的含义就是状态从 $t-1$ 时刻到 $t$ 时刻发生改变的概率，和我们的需求一致。

  在步骤4中，贝叶斯滤波器将置信度 $bel(x_t)$ 乘以测量 $z_t$ 在此时状态下可能已被观察到的概率。这一步也被称为测量更新。从信息损益的角度来看，第3步控制量的存在使机器人的状态真实性下降（噪音引入），而为了平衡，第4步观测器数据的加入可以增加机器人的状态的置信度。这样看也是符合常理的。另外，常数 $\eta$ 的引入目的是归一化，因为在乘积和积分的过程中，$bel(x_t)$可能不会积分到 1。

  最后看初始值，一共三种情况。I. 已知 $x_0$ 的值。这样，$bel(x_0)$ 的值集中在正确的 $x_0$ 值上，并在其他任何地方分配零概率。II. 不知道$x_0$ 的值。这样，$bel(x_0)$ 通常设置为 $x_0$ 域上的均匀分布。III. 已知$x_0$ 的部分信息。这样可以用非均匀分布表达。

Example

  该节提供了一个贝叶斯滤波器的简单例子。

  我们使用让机器人采用贝叶斯滤波算法，并使用其摄像头估计门的状态。为简化问题，假设门只有打开或关闭两种状态，而且只有机器人可以改变门的状态。我们设初始条件为假设机器人最初并不知道门的状态，则初始置信分布如下：

$$bel(X_0=open)=bel(X_0=open)=0.5\text{\hspace{1em}(2.25)}$$

  传感器有可能获得的信息如下：

$$\begin{gathered} p(Z_t=sense\_open|X_t=is\_open)=0.6 \\ p(Z_t=sense\_close|X_t=is\_open)=0.4 \\ p(Z_t=sense\_open|X_t=is\_close)=0.2 \\ p(Z_t=sense\_close|X_t=is\_close)=0.8\text{\hspace{1em}(2.26)} \end{gathered}$$

  这些概率表明机器人的传感器在检测关闭的门时相对可靠，因为错误概率为 0.2。

  最后我们设定状态转移概率，分为两种情况，什么也不做和开门：

$$\begin{gathered} p(X_t=is\_open|U_t=push|X_{t-1}=is\_open)=1 \\ p(X_t=is\_closed|U_t=push|X_{t-1}=is\_open)=0 \\ p(X_t=is\_open|U_t=push|X_{t-1}=is\_closed)=0.8 \\ p(X_t=is\_closed|U_t=push|X_{t-1}=is\_closed)=0.2\text{\hspace{1em}(2.27)} \end{gathered}$$

$$\begin{gathered} p(X_t=is\_open|U_t=do\_nothing|X_{t-1}=is\_open)=1 \\ p(X_t=is\_closed|U_t=do\_nothing|X_{t-1}=is\_open)=0 \\ p(X_t=is\_open|U_t=do\_nothing|X_{t-1}=is\_closed)=0 \\ p(X_t=is\_closed|U_t=do\_nothing|X_{t-1}=is\_closed)=1\text{\hspace{1em}(2.27)} \end{gathered}$$

  下面，我们令输入为 $u_1=do\_nothing$，测量为 $z_1=sense\_open$，按照伪代码第3行计算：
$$\begin{array}{rl}\overline{bel}(x_1)=& \int p(x_1|u_1,x_0)bel(x_0)d{x}_0\\ =& p(x_1|u_1=do\_nothing,x_0=is\_open)bel(x_0=is\_open)\\ & +p(x_1|u_1=do\_nothing,x_0=is\_closed)bel(x_0=is\_closed)\end{array}\text{\hspace{1em}(2.28)}$$

  分别计算 $\overline{bel}(x_1=is\_open)$ 和 $\overline{bel}(x_1=is\_closed)$ ：

$$\begin{array}{rl}& \overline{bel}(x_1=is\_open)\\ =& p(x_1=is\_open|u_1=do\_nothing,x_0=is\_open)bel(x_0=is\_open)+\\ & p(x_1=is\_open|u_1=do\_nothing,x_0=is\_closed)bel(x_0=is\_closed)\\ =& 1\ast 0.5+0\ast 0.5=0.5\end{array}\text{\hspace{1em}(2.29)}$$

$$\begin{array}{rl}& \overline{bel}(x_1=is\_closed)\\ =& p(x_1=is\_closed|u_1=do\_nothing,x_0=is\_open)bel(x_0=is\_open)+\\ & p(x_1=is\_closed|u_1=do\_nothing,x_0=is\_closed)bel(x_0=is\_closed)\\ =& 0\ast 0.5+1\ast 0.5=0.5\end{array}\text{\hspace{1em}(2.30)}$$

  然后按照伪代码第4行计算后验分布：

$$\begin{array}{rl}& bel(x_1=is\_open)\\ =& \eta p(z_1=sense\_open|x_1=is\_open)\overline{bel}(x_0=is\_open)\\ =& \eta 0.6\ast 0.5=0.3\eta \end{array}\text{\hspace{1em}(2.31)}$$

$$\begin{array}{rl}& bel(x_1=is\_closed)\\ =& \eta p(z_1=sense\_open|x_1=is\_closed)\overline{bel}(x_0=is\_closed)\\ =& \eta 0.2\ast 0.5=0.1\eta \end{array}\text{\hspace{1em}(2.32)}$$

  归一化因子 $\eta$ 的值为$1/(0.3+0.1)=2.5$。因此，我们获得下式：

$$\begin{gathered} bel(x_1=is\_open)=0.75 \\ bel(x_1=is\_closed)=0.25\text{\hspace{1em}(2.33)} \end{gathered}$$

  贝叶斯滤波器第一次迭代结束。按照这种方式，我们可以继续计算下去。

Mathematical Derivation of the Bayes Filter

  该节是主要讲述如何使用贝叶斯法则推导贝叶斯滤波器，推导中使用了状态的完整性和马尔科夫链的概念。

  根据贝叶斯法则，我们可知：

$$\begin{array}{rl}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})\end{array}\text{\hspace{1em}(2.34)}$$

  因为我们假设 $t$ 时刻的状态是完整的。即 $x_t$ 之前没有变量可能影响 $x_{t+1}$ 的随机演化，（2.34）可以简化为：

$$p(x_t|z_{1:t},u_{1:t})=\eta p(z_t|x_t)p(x_t|z_{1:t-1},u_{1:t})\text{\hspace{1em}(2.35)}$$

  因此，（2.35）为伪代码第4行。

  根据全概率公式展开 $p(x_t|z_{1:t-1},u_{1:t})$ ，即$\overline{bel}(x_t)$：

$$\begin{array}{rl}\overline{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-1},u_{1:t})d{x}_{t-1}\end{array}\text{\hspace{1em}(2.36)}$$

  因为状态是完整的，所以 $x_{t-1}$ 包含过去的测量和控制信息——$z_{1:t-1}$ 和 $u_{1:t-1}$，（2.36）简化为：

$$\overline{bel}(x_t)=\int p(x_t|x_{t-1},u_t)p(x_{t-1}|z_{1:t-1},u_{1:t})d{x}_{t-1}\text{\hspace{1em}(2.37)}$$

The Markov Assumption

  马尔可夫假设假设，如果知道当前状态 $x_t$，过去和未来的数据是独立的。但这个假设的要求很高，在一些情况下假设会不成立，例如：

1. 建模状态不能包含全部真实状态
2. 状态转移概率和测量概率的误差
3. 置信函数的近似误差

Representation and Computation

  在一般的机器人问题中，置信分布是近似的。为了计算的准确性，近似函数的选择也很重要。通常会考虑以下几个方面：

1. 计算效率
2. 近似的准确性
3. 计算的简易性

Summary

  在本节中，我们介绍了机器人技术中贝叶斯过滤器的基本思想，作为估计环境状态（包括机器人本身的状态）的方法。

1. 机器人与其环境的交互被建模为耦合动力系统，其中机器人可以通过选择控制来操纵其环境，并且可以通过传感器测量来感知其环境。
2. 状态转移概率和测量概率
3. 贝叶斯过滤器是递归的； 时间 t 的置信度是根据时间 t − 1 的置信度计算得出的。
4. 马尔可夫假设
5. 近似函数的评估涉及其准确性、效率和实施的难易程度。

Bibliographical Remarks

  略

扩展阅读

1. 状态和状态空间
2. 马尔可夫链
3. 卡尔曼滤波（可能需要vpn）