移动系统中的非确定性与贝叶斯滤波

第6章 移动系统中的非确定性事件

6.1 引言

现实世界情况中的确定性事件很少见。让我们来看一些例子。

考虑一种测量车辆速度的情况。由于速度传感器本身存在不确定性（灵敏度、有限分辨率、有限测量范围、物理限制等），车辆的真实速度无法被绝对精确地确定。传感器测量值容易受到环境噪声和干扰的影响，传感器的性能（特性）通常会随时间发生变化，且传感器有时会发生故障或损坏。因此，传感器可能无法提供关于我们所测量量的有用信息。

与传感器类似，执行器也可以得出相似的结论。例如，考虑电机驱动的情况。由于噪声、摩擦、磨损、未知负载或机械故障，无法根据输入激励确定实际的角速度。

此外，某些算法本质上具有不确定性。许多算法通常以有限精度（对于特定问题而言已足够）的方式设计，因为需要在有限的时间内获得结果。在移动机器人领域，如同许多其他需要实时运行的系统一样，计算速度通常比绝对精度更为重要。鉴于计算资源有限，若不降低算法精度，则无法实现在期望响应时间内完成许多算法的实时实现。

此外，移动智能体（轮式移动机器人）的工作环境本质上也具有不确定性。在结构化环境（例如，具有平面地板和墙壁的人造建筑等）中，不确定性通常较低；而在动态时变环境（例如，家庭环境、道路交通、人类公共空间、森林等）中，不确定性则更高。

自主轮式移动系统的发展必须解决来自各种源的不确定性问题。

6.2 概率基础

设 X 为一个随机变量，x 是随机变量 X 的一个结果。如果随机变量 X 的样本（结果）空间是一个具有有限（可数）结果数量的集合（例如，抛硬币只有两种可能结果：正面或反面），则 X 是离散随机变量。如果样本空间是实数集合（例如，硬币的重量），则 X 是连续随机变量。本节简要回顾概率论中的基本概念。有关该主题更详细的概述，可在许多统计学教科书（例如，[1]）中找到。

6.2.1 离散随机变量

离散随机变量 X 具有有限或可数样本空间，该空间包含随机变量 X 的所有可能结果。离散随机变量 X 的结果等于 x 的概率记为

P(X= x)

或简写为 P(x)= P(X= x)。概率函数 P(x) 在随机变量 X 的样本空间内每个取值上的取值范围均位于 0 和 1 之间的有界区间内，即 P(x) ∈[0, 1] ∀ x ∈ X。随机变量 X 所有可能结果的概率之和等于 1：

∑
x∈X
P(x)= 1 (6.1)

两个或多个事件同时发生（例如，随机变量 X 取值为 x 且随机变量 Y 取值为 y）的概率由联合概率描述：

P(x, y)= P(X= x, Y= y) (6.2)

如果随机变量 X 和 Y 相互独立，则联合概率(6.2)就是边缘概率的乘积：

P(x, y)= P(x)P(y)

条件概率，记为 P(x|y)，表示在已知随机变量 Y 取值为 y 的先验条件下，随机变量 X 取值为 x 的概率。如果 P(y)>0，则该条件概率可由下式得到

P(x|y)= P(x, y) P(y) (6.3)

对于独立的随机变量 X 和 Y，关系式(6.3)变为平凡：

P(x|y)= P(x, y) P(y)= P(x)P(y) P(y)= P(x)

概率论中的一个基本结果是全概率定理：

P(x)=∑
y∈Y
P(x, y) (6.4)

在条件概率可用的情况下，定理（6.4）可以以另一种形式给出：

P(x)=∑
y∈Y
P(x|y)P(y)= p T(x|Y)p(Y) (6.5)

在（6.5）中介绍了离散概率分布，定义为列向量 p(Y)：

p(Y)=[P(y1), P(y2),…, P(yN)]T

其中 N ∈ N 是随机变量 Y 可能取的所有状态的数量。类似地，列向量 p(x|Y) 定义如下：

p(x|Y)=[P(x|y1), P(x|y2),…, P(x|yN)]T

离散概率分布可以用直方图进行可视化（图6.1）。根据公式（6.1），直方图中所有柱子的高度总和为1。当随机变量的所有状态都是同等可能时，该随机变量可以用均匀分布来描述（图6.1B）。

6.2.2 连续随机变量

连续随机变量的范围属于实数的一个区间（有限或无限）。在连续情况下，成立

P(X= x)= 0，因为连续随机变量X具有无限样本空间。因此引入了一个在0到1之间的有界范围内的概率密度函数p(x)，即p(x) ∈[0, 1]。连续随机变量X的结果小于a的概率为

P(X< a)= ∫ a −∞
p(x) dx

概率密度函数的示例如图6.2所示。类似于离散随机变量的关系式(6.1)，连续随机变量X的概率密度函数在整个样本空间上的积分也等于1：

P(−∞< X<+∞)= ∫+∞ p(x) dx= 1

表6.1 概率论中离散和连续随机变量的选方程

描述	离散随机变量	连续随机变量
全概率	∑x∈X位姿估计协方差矩阵初值(位姿估计初值)= 1 位姿估计协方差矩阵初值(位姿估计初值)= ∑ y∈ Y位姿估计协方差矩阵初值(位姿估计初值,y) 位姿估计协方差矩阵初值(位姿估计初值)= ∑ y∈ Y位姿估计协方差矩阵初值(x| y )位姿估计协方差矩阵初值(y) μX=∑x∈X粒子位置(位姿估计初值) σ 2 X=∑x∈X(位姿估计初值 −μX)2位姿估计协方差矩阵初值(位姿估计初值)	p(位姿估计初值)= ∫−+∞∞ p(位姿估计初值,y)dy 位姿估计协方差矩阵初值(位姿估计初值)= ∫+∞ −∞位姿估计协方差矩阵初值(位姿估计初值| y )位姿估计协方差矩阵初值(y)dy μX= ∫−+∞∞ xP(x)dx σ X2= ∫−+∞∞(x −μX)2p(x)dx
全概率定理
概率
Mean
方差

在第6.2.1节中介绍的离散随机变量所满足的类似关系也可推广到概率密度函数。离散和连续随机变量的一些重要关系并列汇总于 Table 6.1。

随机变量的概率分布很少通过各种统计量来描述。连续随机变量X的均值定义为数学期望：

μX= E{X}= ∫+∞ −∞
xp(x) dx (6.6)

描述分布形状的基本参数之一是方差，其定义如下：

σ 2 X= var{X}= E{(X −E{X})2}= ∫+∞ −∞
(x −μX)2p(x) dx (6.7)

这些性质也可以为离散随机变量定义，如表6.1所示。

均值μ和方差 σ 2是唯一描述最重要的概率密度函数之一—— 正态分布（图6.3）所需的两个参数，该分布以高斯函数表示：

p(x)= 1
√2πσ 2 e−
1 2 (x−μ) 2 σ 2 (6.8)

在多维情况下，当随机变量为向量 x 时，正态分布具有以下形式：

p(x)= det(2π) −
1
2 e−
1 2(x−μ) T
−1 (x−μ)

其中 是协方差矩阵。二维高斯函数的一个示例如 图6.4所示。协方差是一个对称矩阵，其元素

在第i行和第j列的元素是随机变量Xi和Xj之间的协方差cov{Xi,Xj}。

协方差 cov{X, Y} 是衡量随机变量 X 和 Y 之间线性关系的指标：

σ 2 XY= cov{X, Y}= E{(X −μX)(Y −μY)}
= ∫+∞ −∞ ∫+∞ (X −μX)(Y −μY)p(x, y) dx dy (6.9)

其中 p(x, y)是 X 和 Y 的联合概率密度函数。关系式（6.9）可简化为

σ
2 XY= E{XY} −μXμY (6.10)

如果随机变量X和Y是独立的，则 E{XY}=E{X}E{Y}且协方差（6.10） 为零： σ X2Y= 0。然而，协方差为零并不意味着随机变量是独立的。两个相同随机变量的协方差等于其方差：cov{X, X}= var{X}= σ 2(X)。

6.2.3 贝叶斯规则

贝叶斯规则是概率机器人学中的基本支柱之一。贝叶斯规则的定义是

p(x|y)= p(y|x)p(x) p(y) (6.11)

对于连续空间和

P(x|y)=
P(y|x)P(x) P(y) (6.12)

在离散空间中。贝叶斯规则通常使我们能够基于相对容易获得的概率来计算难以确定的概率。

示例 6.1

这是在移动系统中使用贝叶斯规则(6.11)的一个引例。设随机变量X
表示我们希望基于测量Y进行估计的移动系统的状态（例如，移动 系统到障碍物的距离），其中测量X在随机意义上依赖于随机状态
X= x，由于测量结果具有不确定性，我们希望知道基于测量Y=
y的估计状态的概率密度。

我们希望知道概率 p(x|y)，这可以通过公式 (6.11) 计算得到。概率 p(x) 描述了在进行测量 y 之前关于随机变量 X 的知识，因此被称为 先验估计。条件概率 p(x|y) 给出了在进行测量之后关于随机变量 X 的知识，因此也被称为后验估计。概率 p(y|x) 包含了状态 X 对测量 Y 影响的信息，因此它表示传感器的模型（例如，当移动系统距离障碍物特定距离时，到障碍物的距离测量的概率分布）。概率 p(y) 包含了测量 y 的分布，即置信度

示例 6.2

有三条不同路径都通向相同目标位置。移动系统在10次中有7次选择第一条 路径，在10次中有1次选择第二条路径，在5次中有1次选择第三条路径。第 一条、第二条和第三条路径上遇到障碍物的概率分别为5%、10%和8%。

1. 确定移动系统在朝目标行进时遇到障碍物的概率。
2. 移动系统遇到障碍物 . 这种情况发生在第一条路径上的概率是多少？

我们用符号A1, A2,和A3,分别表示第一条路径、第二条路径和第三 条路径，用符号B表示任意路径上的障碍物。选择特定路径的概率如 下：P(A1)= 0.7，P(A2)= 0.1，以及P(A3)= 0.2。在第一条路径上
遇到障碍物的概率为P(B|A1)= 0.05，在第二条路径上为P(B|A
2)= 0.1，在第三条路径上为P(B|A3)= 0.08。

1. 移动系统遇到障碍物的概率为
   P(B)= P(B|A1)P(A1)+ P(B|A2)P(A2)+ P(B|A3)P(A3) = 0.05 · 0.7+ 0.1 · 0.1+ 0.08 · 0.2 = 0.061

2. 可以使用贝叶斯规则（6.12）计算移动系统在第一条路径上卡住的概 率：
   P(A1|B)=
   P(B|A1)P(A1) P(B)
   =
   P(B|A1)P(A1)+ P(B|A2)P(A2)+ P(B|A3)P(A3)
   =
   0.05 · 0.7 0.05 · 0.7+ 0.1 · 0.1+ 0.08 · 0.2 = 0.5738

示例 6.3

一台移动机器人配备了一个灰尘检测传感器，该传感器可以检测移 动机器人下方地板的清洁度（图 6.5）。根据传感器读数，我们希望 判断移动机器人下方的地面是干净还是脏污；因此，离散随机状态 有两个可能的取值。地面是干净的概率为40%。关于该灰尘检测传 感器，有以下描述：如果地面是干净的，传感器在80%的情况下能 正确判断地面状态为干净；如果地面是脏污的，传感器在90%的情 况下能正确判断地面状态为脏污。错误测量的概率较小：当地面是 干净时，传感器每5次中有1次会错误判断地面状态；而当地面是脏 污时，传感器性能更佳，每10次中仅有1次会进行错误测量。当传感 器检测到地面为干净时，实际地面是干净的概率是多少？

让 我们用离散随机变量X ∈{地面是干净的,脏污}表示地面状态，并 用随机变量Z ∈{地面是干净的,脏污}表示传感器测量值。因此，地 面是干净的概率可表示为P(X=地面是干净的)= 0。4，传感器测 量模型可用如下数学方式表示：

P(Z= clean|X= clean)= 0.8 P(Z= dirty|X= dirty)= 0.9

让我们引入一个更简洁的表示法：

P(x)= P(X= clean)= 0.4 P(x¯)= P(X= dirty)= 1 − P(x)= 0.6 P(z|x)= P(Z= clean|X= clean)= 0.8 P(z¯|x)= P(Z= dirty|X= clean)= 1 − P(z|x)= 0.2 P(z¯|x¯)= P(Z= dirty|X= dirty)= 0.9 P(z|x¯)= P(Z= clean|X= dirty)= 1 − P(z¯|x¯)= 0.1

我们希望确定 P(x|z)，该值可通过贝叶斯规则（6.12）计算：

P(x|z)=
P(z|x)P(x) P(z)

利用全概率定理，传感器检测地板为清洁的概率可以计算如下：

P(z)= P(z|x)P(x)+ P(z|x¯)P(x¯) = 0.8 · 0.4+ 0.1 · 0.6 = 0.38

我们所寻找的概率是

P(x|z)=
0.8 · 0.4 0.38= 0.8421

当传感器检测到地板清洁时，得到的地面清洁的概率较高。然而， 仍有超过15%的可能性地板实际上是脏污的。因此，移动系统可能 错误地认为无需进行清洁；并且如果移动机器人不采取任何清洁动 作，地板将保持脏污状态。

6.3 状态估计

本节介绍了状态估计。状态估计是一个基于测量数据和系统知识来 估计系统真实状态的过程。即使系统状态可以直接测量，测量数据 通常也会受到噪声和其他干扰的影响。这使得原始测量数据在没有 适当的信号滤波情况下通常不适合进一步使用（例如直接用于控制 误差的计算）。在许多情况下，即使系统状态无法直接测量，也可 以对其进行估计。当系统具有可观测性时，便可实现这种估计。在 实现状态估计算法时，应检查系统的可观测性。状态估计算法最重 要的特性是估计收敛性和估计偏差。因此，本节概述了在实施特定 状态估计算法之前应考虑的一些实际因素。

6.3.1 干扰和噪声

系统模型中未考虑的所有系统动态、所有不可测量信号以及建模误差都 可以被视为

系统干扰。在线性假设下，所有干扰可以表示为一个单一的项 n(t)，该 单项被添加到真实信号 y0(t) 中：

y(t)= y0(t)+ n(t) (6.13)

干扰可以分为几类：高频、准平稳随机信号（例如，测量噪声）、 低频、非平稳信号（例如，漂移）、周期信号或其他类型的信号
（例如，尖峰、数据异常值）。其中最重要的随机信号之一是white
noise。

频谱和信号分布是描述信号最重要的特性。信号分布给出了幅 值取特定值的概率。最常见的两种信号分布是均匀分布和高斯（正 态）分布（参见第6.2节）。信号的频谱表示信号在每一时刻之间的 相互依赖关系，这与构成信号的频率成分的分布有关。对于白噪声 而言，所有频率成分的分布是均匀的，因此每个时间步的信号值与 先前信号的值无关。

6.3.2 估计收敛性和偏差

如前所述，状态估计基于测量的输入输出信号、变量之间的关系 （系统模型）以及信号的某些统计特性（例如方差）和系统的其他 信息，提供内部状态的估计。所有这些信息应以某种方式融合，以 获得对内部状态的准确且精确的估计。然而，由于噪声、干扰、寄 生动态、对系统模型的错误假设以及其他误差源的存在，测量值、 模型和信号的先验已知特性本质上具有不确定性。因此，状态估计 通常与实际状态不同。

上述所有问题都会导致信号中存在一定程度的不确定性。从数 学上讲，可以在随机环境中处理这一问题，将信号视为随机变量。
随后，信号可通过其概率密度函数表示，有时也可简化为均值和方 差。在随机环境中，一个重要问题是某个状态估计的质量。特别是 需要分析状态估计向真实值的收敛性。具体而言，应提出两个重要 问题：

1. 估计的数学期望是否与真实值相同？如果是，则该估计是无偏的。
   如果估计随着时间（更大的观测区间）而改善，并且当观测区间趋 于无限时收敛到真实值，则该估计是一致的。
2. 当观测时间趋于无穷时，估计误差的方差是否收敛于零？如果是， 并且估计是一致的，则该估计为均方一致。这意味着随着观测时间 的增加，估计的准确度和精确度都变得非常好（所有估计值都接近 真实值）。

当对系统的假设较为简单时（如系统模型完美、高斯噪声等）， 上述问题的答案相对容易获得。然而，当处理更复杂的问题时，这 些问题的答案变得极为困难，且无法再获得解析解。但需要记住的 是，如果一些重要假设未被违反，状态估计器仍能提供令人满意的 结果。因此，在使用特定的状态估计算法时，仔细阅读细则至关重要。否则，这些算法提供的估计结果可能与实际状态相差甚远，而 问题在于我们通常甚至意识不到这一点。

6.3.3 可观测性

需要估计的状态通常是隐藏的，因为关于这些状态的信息通常无法 直接获取。可以根据对系统输出的测量来估计这些状态，而系统输 出直接或间接依赖于系统状态。在实现估计算法之前，应首先回答 以下问题：能否在有限时间内通过观测系统输出唯一地估计出这些 状态？该问题的答案与系统的可观测性相关。如果系统是可观测的， 则可以通过观测系统输出来估计状态。系统可能仅是部分可观测的； 在这种情况下，只能估计一部分系统状态，其余的系统状态无法被 估计。如果系统是完全可观测的，则所有状态都是输出可连接的。

在进入可观测性的定义之前，应先介绍不可区分的状态这一概
念。考虑如下形式的一般非线性系统（x ∈ R n、u ∈ R m 和 y ∈ R l）：

x˙(t)=f(x(t), u(t)) y(t)= h(x(t)) (6.14)

两个状态 x0和 x1是不可区分的，如果对于有限时间区间 t ∈[t0,t上每个 容许的输入 u(t)都得到相同的输出 [2]：

y(t, x0) ≡ y(t, x1) ∀ t ∈[t0, t1] (6.15)

从状态x0出发的所有不可区分状态的集合记为 I(x0)。可观测性的定 义现可表述如下：若在x0处，不可区分状态集 I(x0)仅包含状态x 0, ，即 I(x0)={x0}，则称系统在x0处是可观测的。若对于定义
域内的每一个状态x，不可区分状态集 I(x)都只包含状态x，即 I(x)={x} ∀x，则称系统是可观测的。需要注意的是，可观测性并不 意味着对于每个输入u(t)，t ∈[t0,t1]，都可以通过观测输出来估计 x。此外，可能需要较长的观测时间才能区分不同状态。已有多种可观测性形式被定义[2]：局部可观测性（更强的可观测性概念）、弱 可观测性（较弱的可观测性概念）以及局部弱可观测性。对于自治 线性系统，这些不同的可观测性形式都是等价的。

检查一般非线性系统（6.14）的可观测性需要高级数学分析。
该系统的局部弱可观测性可通过一个简单的代数测试来检验。为此， 引入一个李导数算子，表示沿系统轨迹h关于x的时间导数：

Lf[h(x)]= ∂h(x) ∂x f(x) (6.16)

考虑系统（6.14）是自治的（u(t)= 0对于所有 t）。为了检验局部 弱可观测性，需要对系统输出 y进行多次微分（直到稍后定义的矩 阵 Q 的秩增加为止）：

y= h(x)= L0 f[h(x)]
y˙= ∂h(x)
∂x dx dt= ∂h(x)
∂x
f(x)= Lf[h(x)]=L 1 f[h(x)]
y¨= ∂ f(x))f(x)= Lf[Lf[h(x)]]= L2 f[h(x)]
…
di y dti= Li f[h(x)]
(6.17)

系统输出的6.17的时间导数(x)可以排列成矩阵

L(x)= ⎡⎢⎢⎢⎢⎢⎢⎣ L0 f[h(x)]
L1 f[h(x)]
L2 f[h(x)]
…
Li f[h(x)]
⎤⎥⎥⎥⎥⎥⎥⎦ (6.18)

矩阵Q(x0)= ∂ ∂xL(x)|x0的秩决定了系统在x0处的局部弱可观测性。如 果该矩阵Q(x0)的秩等于状态数，即rank(Q(x0))= n，则称系统在 xx0,处满足可观测性秩条件，这是系统在x0处具有局部弱可观测性 的充分但非必要条件。如果对于定义域内的每一个x都满足可观测性 秩条件，则系统是局部且弱可观测的。关于可观测性主题更深入的研 究可参见例如[2–4]。

对于时不变线性系统，可观测性秩条件得以简化。对于具有n个状态且 形式为 ˙x(t)= Ax(t)+Bu(t),y(t)= Cx(t)的系统，式(6.18)中的李导数为

L0 f[h(x)]=Cx(t)
L1 f[h(x)]=C(Ax(t)+ Bu(t))
L2 f[h(x)]=CA(Ax(t)+ Bu(t))
…
(6.19)

李导数的偏微分（公式6.19）得到卡尔曼可观测性矩阵Q：

QT=[CT A TCT ···(A T)n−1CT] (6.20)

如果可观测性矩阵的秩具有n个独立行，则系统是可观测的，即可观 测性矩阵的秩等于状态的数量：

rank(Q)= n (6.21)

6.4 贝叶斯滤波

6.4.1 马尔可夫链

让我们关注那些可以假设关于完整状态命题的系统。这意味着在 任意给定时刻，系统的所有信息都可以从系统状态中确定。系统可 以根据当前状态进行描述，这是马尔可夫过程的一个特性。在图 6.6中，展示了一个隐马尔可夫过程（链），其中状态无法直接获取， 只能通过与状态当前值随机依赖的测量值来估计。在这些假设下 （马尔可夫过程），系统的当前状态仅依赖于前一状态，而不依赖 于状态的全部历史：

p(xk|x0,…, xk−1)= p(xk|xk−1)

类似地，如果仅知道当前状态，则假设测量与系统状态的整个历史无关：

p(zk|x0,…, xk)= p(zk|xk)

隐马尔可夫过程的结构，其中当前状态xk不仅依赖于前一状态 xk−1，还依赖于系统输入uk−1，如图6.7所示。输入uk−1是最新的外 部动作，影响从时间步k− 1到当前时间步k的内部状态。

6.4.2 基于观测的状态估计

贝叶斯滤波器是用于计算概率分布的最通用形式的算法。贝叶斯滤 波器是一种强大的统计工具，可用于在存在系统和测量不确定性的 情况下进行位置（系统状态）估计[5]。

在已知传感器的统计模型 p(z|x)（给定已知状态下的测量值的 概率分布）以及测量值的概率分布 p(z)的情况下，可以估计测量后 状态的概率分布（p(x|z)）。这在示例 6.3 中针对离散随机变量进行 了说明。

现在让我们来看一下在获得一系列测量值x时估计状态z的情
况。我们希望估计p(xk|z1, …, zk)，即考虑直到当前时间步所有测量
值序列的情况下，时间步k时状态x的概率分布。贝叶斯公式可以写 成递归形式：

p(xk|z1,…, zk)=
p(zk|xk, z1,…, zk−1)p(xk|z1,…, zk−1)
p(zk|z1,…, zk−1)

可以重写为更短的形式：

p(xk|z1:k)=
p(zk|xk, z1:k−1)p(xk|z1:k−1) p(zk|z1:k−1) (6.22)

式（6.22）中各符号的含义如下：
• p(xk|z1:k)是在时间步k利用测量数据更新后的估计状态概率分布。
• p(zk|xk,z1:k−1)是在已知当前状态xk以及至时间k−1为止的先前测 量的情况下，时间步k的测量概率分布。
• p(xk|z1:k−1)是基于先前测量的预测状态概率分布。• p(zk|z1:k−1)
是时间步k中的测量概率（对测量的置信度）。

通常情况下，如果系统状态 xk已知，则式(6.22)中的当前测量 zk与先前测量无关（完全状态假设，马尔可夫过程）：

p(zk|xk, z1:k−1)= p(zk|xk)

因此，式(6.22)简化为以下形式：

p(xk|z1:k)= p(zk|xk)p(xk|z1:k−1) p(zk|z1:k−1) (6.23)

式(6.23)的推导见示例 6.4。

示例 6.4

作为练习，请在考虑完全状态假设的情况下，从公式(6.23)推导出公式 (6.22)（p(zk|xk,z1:k−1)= p(zk|xk)）。

式 解(6.23)的推导如下：

p(xk|z1:k)=
p(z1:k|xk)p(xk) p(z1:k) = p(zk, z1:k−1|xk)p(xk) p(zk, z1:k−1) = p(zk|z1:k−1, xk)p(z1:k−1|xk)p(xk) p(zk|z1:k−1)p(z1:k−1) = p(zk|z1:k−1, xk)p(xk|z1:k−1)p(z1:k−1)p(xk) p(zk|z1:k−1)p(z1:k−1)p(xk) = p(zk|z1:k−1, xk)p(xk|z1:k−1) p(zk|z1:k−1) = p(zk|xk)p(xk|z1:k−1) p(zk|z1:k−1)

递归公式(6.23)，其根据先前测量更新状态，包含一个预测项p(xk|z1:k−1)。状态估计可分为两个步骤：预测步骤和校正步骤。

预测步骤

预测p(xk|z1:k−1)按如下方式进行：

p(xk|z1:k−1)= ∫ p(xk|xk−1, z1:k−1)p(xk−1|z1:k−1) dxk−1

完全状态假设产生了一个更简单的形式：

p(xk|z1:k−1)= ∫ p(xk|xk−1)p(xk−1|z1:k−1) dxk−1 (6.24)

其中p(xk|xk−1)表示状态转移的概率分布，而p(xk−1|z1:k−1)是前一时 刻估计状态的校正后的概率分布。

校正步骤

在时间步 k 的测量之后的估计状态概率分布以及预测步骤中的预测 状态概率分布如下所示：

p(xk|z1:k)= p(zk|xk)p(xk|z1:k−1) p(zk|z1:k−1) (6.25)

描述测量置信度的概率p(zk|z1:k−1)可以从该关系中确定

p(zk|z1:k−1)= ∫ p(zk|xk)p(xk|z1:k−1) dxk

最可能状态估计

如何利用估计的概率密度分布p(xk|z1:k)来计算最可能状态xk？最佳 且最可能的状态估计（数学期望）E{xk}被定义为使测量的平均平方 误差最小的值：

E{xk}= ∫ xkp(xk|z1:k) dxk

此外，还可以估计使后验概率p(xk|z1:k)最大的位姿估计初值xkmax的 值：

xkmax= max
xk
p(xk|z1:k)

示例6.5

考虑 示例 6.3 并确定在时间步 k= 2传感器再次检测到地面是干净的时， 地面是干净的概率。

示例 6.5—续 解答

在时间步 k= 1的示例 6.3 中考虑了以下情况：地板是清洁的，且传感 器也检测到地板为清洁状态（z1=清洁）。计算了以下条件概率：

P(x1|z1)=
0.8 · 0.4 0.38= 0.8421

在下一个时间步 k= 2，传感器返回一个读数 z2= clean，其正确
传感器读数的概率为 P(z2|x2)= 0.8，错误传感器读数的概率为 P(z
2|¯x2)= 0.1（假设传感器特性为时不变）。

让我们首先评估基于先前测量的预测概率 P(x2|z1)，即地面是脏 的。考虑式(6.24)，其中用积分操作替代求和操作，得到

P(xk|z1:k−1)= ∑
xk−1∈X
P(xk|xk−1)P(xk−1|z1:k−1)

目前假设移动系统仅能检测地面状态，但无法影响地面状态（即移 动系统不进行任何清洁操作，也不会使地板变脏）。因此，状态转 移概率简单为 P(x2|x1)= 1 和 P(x2|¯x1)= 0。故

P(x2|z1:1)= P(x2|x1)P(x1|z1)+ P(x2|x¯1)P(x¯1|z1) = 1 · 0.8421+ 0 · 0.1579 = 0.8421

这是一个合乎逻辑的结果，因为如果不考虑测量值 z2，则没有获得 关于系统的任何新信息。因此，地面状态在时间步 k= 2 的概率与 前一时刻 k= 1 的状态概率相同。

测量值可以在校正步中使用关系（6.25）与当前估计进行融合：

P(x2|z1:2)=
P(z2|x2)P(x2|z1:1) P(z2|z1:1) = 0.8 · 0.8421 P(z2|z1:1)

其中归一化因子的值需要进行计算。缺失的因子是时间步 k= 2 内地面 清洁的概率，且它能够

示例6.6

再次考虑 示例 6.3 并确定如果地面是干净的且传感器进行了三次测 量 z1:3=(清洁,清洁,脏污)。

解
The 前fi两个条件概率已在示例 6.3 和6.5中计算过，第三个是

P(x3|z1:3)=
P(z3|x3)P(x3|z1:2) P(z3|z1:2)
=
0.2 · 0.9771 0.2 · 0.9771+ 0.9 ·(1 −0.9771) = 0.9046

其中 P(Z=脏污|X=清洁)= 1 −P(Z=清洁|X=清洁)。在时 间步i= 1,2,3 处获取的三次连续状态测量中的状态概率因此为

P(xk|z1:i)=(0.8421,
0.9771, 0.9046)

时间步 k= 1、2、3 的后验状态概率分布也在 图 6.8中显示。在 Matlab 中的解决方案实现如 清单6.2所示。

清单 6.2 示例 6.6
1 % 选择第一条、第二条或第三条路径的概率：2 % p(A) =[P(A1), P(A2), P(A3)]3 p_A=[0.7 0.1 0.2]4 % 在第一条、第二条和5 % 第三条路径上遇到障碍物的概率：p(B|A)=[P(B|A1), P(B|A2), P(B|A3)]6 p_BA =[0.05 0.1 0.08]7 8 % 出现障碍物的概率：P(B) 9 P_B= p_BA*p_A.’ 10 11 % 移动机器人在第一条、第二条和第三条12 % 路径上受阻的概率：p(A|B) =[P(A1|B), P(A2|B), P(A3|B)]13 p_AB =(p_BA.*p_A)./P_B
p_状态转移雅可比矩阵 =0.7 0.1 0.2 p_BA =0.05 0.1 0.08 P B=_0.0610 p_AB =0.5738 0.1639 0.2623

示例 6.7

一个移动机器人配备了一个传感器，该传感器可以检测地板是否干 净（Z ∈{地面是干净的,脏污}）。移动系统还配备了一个清洁系统 （一组刷子、一个真空泵和一个集尘盒），该系统可以在机器人认为地板需要清洁时启动进行清洁（U ∈{清洁,null}）。我们再次希望确定地板是否干净（X ∈{地面是干净的,脏污}）。

地面是干净的初始概率（置信度）是

bel(X0= clean)= 0.5

传感器测量的正确性由传感器的统计模型给出：

P(Zk= clean|Xk= clean)= 0.8, P(Zk= dirty|Xk= clean)= 0.2 P(Zk= dirty|Xk= dirty)= 0.9, P(Zk= clean|Xk= dirty)= 0.1

如果机器人决定执行地板清洁，其结果概率如下：

P(Xk= clean|Xk−1= clean, Uk−1= clean)= 1 P(Xk= dirty|Xk−1= clean, Uk−1= clean)= 0 P(Xk= clean|Xk−1= dirty, Uk−1= clean)= 0.8 P(Xk= dirty|Xk−1= dirty, Uk−1= clean)= 0.2

如果清洁系统未被激活，则可以假设以下结果概率：

P(Xk= clean|Xk−1= clean, Uk−1= null)= 1 P(Xk= dirty|Xk−1= clean, Uk−1= null)= 0 P(Xk= clean|Xk−1= dirty, Uk−1= null)= 0 P(Xk= dirty|Xk−1= dirty, Uk−1= null)= 1

假设移动系统首先执行一个动作，然后接收测量数据。根据所 执行的动作（预测）确定置信度belp(xk) ，并根据测量值bel(xk) （校 正）确定置信度bel(xk) ，针对以下动作和测量序列：

k Uk−1 Zk
1 null脏污 2清洁清洁 3清洁清洁

例6.7—继续 解法

不失一般性，我们引入以下符号表示：Xk ∈{清洁,脏污} 表示为 Xk ∈{xk, ¯xk}， Zk ∈{清洁, 脏污} 表示为 Zk ∈{zk, ¯zk}，以及 Uk ∈(清洁,null) 表示为 Uk ∈{ uk, ¯uk让}。我们使用 算法3。在时间步k= 1，当动作 ¯u0= null处于激 活状态时，我们可以确定地面是干净的预测信念如下：

belp(x1)= ∑
x0∈X
P(x1|x0, u¯0)bel(x0) = P(x1|x¯0, u¯0)bel(x¯0)+ P(x1|x0, u¯0)bel(x0) = 0 · 0.5+ 1 · 0.5 = 0.5

以及对地面是脏的的预测信念：

belp(x¯1)= ∑
x0∈X
P(x¯1|x0, u¯0)bel(x0) = P(x¯1|x¯0, u¯0)bel(x¯0)+ P(x¯1|x0, u¯0)bel(x0) = 1 · 0.5+ 0 · 0.5 = 0.5

由于未执行任何动作，状态概率保持不变。基于测量值 ¯z1=脏污 ， 可以对置信度进行修正：

bel(x1)= ηp(z¯1|x1)belp(x1)= η0.2 · 0.5= η0.1 and bel(x¯1)= ηp(z¯1|x¯1)belp(x¯1)= η0.9 · 0.5= η0.45

在评估归一化因子 η之后：

η= 1 0.1+ 0.45= 1.82

信念的最终值可以被确定：

bel(x1)= 0.182, bel(x ¯1)= 0.818

该过程可以重复用于时间步k = 2，其中u1= clean且z2= clean：

belp(x2)= 0.8364, belp(x¯2)= 0.1636 bel(x2)= 0.9761, bel(x¯2)= 0.0239

并且可以继续用于时间步 k= 3，其中u2= clean且 z3=clean：

belp(x3)= 0.9952, belp(x¯3)= 0.0048 bel(x3)= 0.9994, bel(x¯3)= 0.0006

示例 6.7 的解在 Matlab 中的实现见清单 6.3。

清单 6.3 示例 6.7 的解决方案的实现示例 6.7
1 % 符号说明：X===X(k)，X’=== X(k) -1) 2 disp(‘对地板清洁与脏污的初始信念’) 3 bel_Xc= 0.5; % bel(X=clean) 4 bel_X =[bel_ Xc 1‐bel_Xc]% bel(X=clean), bel(X=dirty) 5 6 disp(‘灰尘传感器测量的条件概率’) 7 P_ZcXc = 0.8; % P(Z=clean|X=clean) 8 P_ZdXc= 1‐P_ZcXc; % P(Z=dirty|X=clean) 9 P_ZdXd = 0.9; % P(Z=dirty|X=dirty) 10 P_ZcXd= 1‐P_ZdXd; % P(Z=clean|X=dirty) 11 p_ZX =[P_ZcXc, P_ ZcXd; … 12 P_ZdXc, P_ZdXd]13 14 disp(‘执行清洁操作时的结果概率’) 15 P_XcXcUc = 1; % P(X=clean|X’=clean,U’=clean) 16 P_XdXcUc= 1‐P_XcXcUc; % P(X=dirty|X’=clean,U’= clean) 17 P_XcXdUc = 0.8; % P(X=clean|X’=dirty,U’=clean) 18 P_XdXdUc= 1‐P_ XcXdUc; % P(X=dirty|X’=dirty,U’=clean) 19 p_ZXUc =[P_XcXcUc, P_XdXcUc; … 20 P_ XcXdUc, P_XdXdUc]21 22 disp(‘未执行清洁操作时的结果概率’) 23 P_XcXcUn = 1; % P(X= clean|X’=clean,U’=null) 24 P_XdXcUn= 1‐P_XcXcUn; % P(X=dirty|X’=clean,U’= null) 25 P_XcXdUn = 0; % P(X=clean|X’=dirty,U’=null) 26 P_XdXdUn= 1‐P_XcXdUn; % P(X=dirty|X’=dirty,U’=null) 27 p_ZXUn =[P_XcXcUn, P_XdXcUn; … 28 P_XcXdUn, P_XdXdUn]29 30 U ={‘null’, ‘clean’, ‘clean’}; 31 Z ={‘dirty’, ‘clean’, ‘clean’}; 32 for k=1: length(U) 33 fprintf(‘预测步骤: U(%d)=%s\n’, k‐1, U{k}) 34 if strcmp(U(k), ‘clean’) 35 belp_X = bel_X*p_ZXUc 36 else 37 belp_X= bel_X*p_ZXUn 38 end 39 40 fprintf(‘校正步骤: Z(% d)=%s\n’, k, Z{k}) 41 if strcmp(Z(k), ‘clean’) 42 bel_X= p_ZX(1,:).*belp_X;
43 否则 44 bel_X=p_ZX(2,:).*belp_X;45 结束 46 bel_X= bel_X/和(bel_X)47 结束
初始置信度：清洁与脏污floorbel X= 0.5000 0.5000 灰尘传感器测量的条件概率 p_ZX =0.8000 0.1000 0.2000 0.9000 执行清洁操作时的结果概率 p_ZXUc = 1.0000 0 0.8000 0.2000 未执行清洁操作时的结果概率 p_ZXUn =1 0 0 1 预测步骤： U(0)=nullbelp_X =0.5000 0.5000 校正步骤：Z(1)=dirty bel X=_0.1818 0.8182 预测步骤：U(1)=clean belp_X =0.8364 0.1636 校正步骤：Z(2)=clean bel X= 0.9761 0.0239 预测步骤：U(2)=clean belp_X =0.9952 0.0048 校正步骤：Z(3)= clean bel X=_0.9994 0.0006

6.4.4 定位示例

一个简单的例子将用于阐述定位原理，以展示蒙特卡洛定位算法背 后的基本思想。考虑一个在环境中移动并具备环境感知能力的移动 系统。基于传感器读数和执行的移动动作信息，定位算法应确定该 移动系统在环境中的位姿。为了实现该定位算法，需使用全概率定 理和贝叶斯规则

使用了这些概念，它们是贝叶斯滤波器中的基本部分。在接下来的 章节中，首先介绍存在传感器不确定性时的测量过程，然后介绍存 在执行器不确定性时的动作过程。移动系统通过执行动作来改变环 境的状态（移动系统的位姿随着其移动而发生变化）。

示例6.8

考虑一条环形路径，移动系统可以在其上沿前进或后退方向移动。
该路径由可数数量的亮色和暗色方块以某种随机顺序组成，地砖宽度与路径宽度相同。移动系统可以位于标有编号的任意一个单元格上。不失一般性，假设路径由五个单元格组成，如图6.9所示。每个 地砖代表一个移动系统可能处于其中的单元格；因此，这是一种对 环境的离散表示。移动系统知道环境地图（即亮色与暗色地砖的序 列已知），但不知道自身当前占据的是哪一个单元格。因此，移动 系统位置的初始置信度为均匀分布

示例6.8—续

分布，因为每个单元格的概率相等。该移动系统具有用于检测亮色 和暗色方块的传感器，但传感器测量存在不确定性。该移动系统具 备在前进或后退方向上移动指定数量方块的功能，但移动本身具有 不确定性（移动系统可能移动不足或过度）。此示例案例将用于解 释环境检测过程和在环境中移动过程的基本概念。

观测雅可比 矩阵

本示fi例中考虑的配置位于示例 6.9 至 6.14中，其中给出了后续 提出的特定问题的解。

6.4.5 环境感知

在环境中进行的测量可用于改进对该环境中状态X（例如，位置） 的估计。想象一下，我们在半夜梦游时在房子里走动。当我们醒来 时，可以通过感官（视觉、触觉等）来确定自己所在的位置。

从数学上讲，关于环境的初始知识可以用概率分布 p(x)（先验） 来描述。当获得新的测量值 z（其置信度为 p(z|x)）时，若能确定测 量后的概率 p(x|z)，则可改进该分布。这可以通过贝叶斯规则 p(x|z )= bel(x)= p(z|x)p(x) p(z) 实现。其中概率 p(z|x) 表示传感器的 统计模型，而 bel(x) 是测量完成后状态估计的置信度。在感知过程 中，将对贝叶斯滤波器的校正步骤进行评估。

示例6.9

考虑 示例6.8，假设移动系统检测到一个暗单元格 Z= dark。该移动系统以 0.6的概率检测到暗单元格，以0.2的概率发生错误（将亮单元格误检为暗单 元格）。因此，

p(Z= dark|X= xd)= 0.6, d ∈{3, 4} p(Z= dark|X= xb)= 0.2, b ∈{1, 2, 5}

示例 6.9—续

其中索引b表示亮单元格，索引d表示暗单元格。在开始时，移动系统不知道其
位置，这可以用均匀概率分布P ( i)=
0
= i)= ( 2, ∈{1,…, 5}
X xbelx.i来 描述。计算单次测量后的位置概率分布。

我们希望确定在进行测量后状态估计信念的条件概率分布 p(X1|Z= dark)。该概率可通过贝叶斯滤波器的校正步骤来确定：

p(X1|Z= dark)= p(Z= dark|X1) ∗p(X1) P(Z= dark)
=[0.2, 0.2, 0.6, 0.6, 0.2]T ∗[0.2, 0.2, 0.2, 0.2, 0.2]T
P(Z= dark)
=[0.04, 0.04, 0.12, 0.12, 0.04]T
P(Z= dark)

其中，算子 ∗ 表示向量元素的逐元素乘法运算。

我们需要计算检测到暗单元格P(Z= dark)的概率。因此必须评 估全概率，即考虑所有单元格的情况下检测到暗单元格的概率：

P(Z= dark)=∑i P(Z= dark|X1= xi)P(X1= xi) = p T(Z= dark|X1)p(X1) =[0.2, 0.2, 0.6, 0.6, 0.2][0.2, 0.2, 0.2, 0.2, 0.2] T
= 0.36

因此，后验概率分布如下：

p(X1|Z= dark)=[0.11, 0.11, 0.33, 0.33, 0.11] T

因此，我们可以得出结论：移动系统的位置在单元格3或4中的概率 是其余三个单元中的三倍。概率分布在图6.10中也以图形方式显示。
本示例的解也在清单6.4中给出。

清单6.4 示例6.9的解决方案的实现示例6.9
1 显示(’传感器测量分布 p(Z=暗 | X)’) 2 p_ZdX=[0.2 0.2 0.6 0.6 0.2]3 显示(’传感器测量分布 p(Z= 暗|X)’) 4 p_ZbX= 1‐p_ZdX 5 显示(’初始分布 p(X)’) 6 p_X= ones(1,5)/5 7 8 显示(’检测到暗单元格的概率 P(Z=暗)’) 9 P_Zd= p_ ZdX*p_X.’ 10 11 显示(’后验分布 p(X|Z=暗)’) 12 p_XZd= p_ZdX.*p_ X/P_Zd 传感器测量分布 p(Z=暗|X) p_ZdX =0.2000 0.2000 0.6000 0.6000 0.2000 传感器测量分布 p(Z=亮|X) p_ZbX =0.8000 0.8000 0.4000 0.4000 0.8000 初始分布 p(X) p_X =0.2000 0.2000 0.2000 0.2000 0.2000 检测到暗单元格的概率 P(Z=暗) P Zd= 0.3600 后验分布 p(X=Z=暗) p XZd =0.1111 0.1111 0.3333 0.3333 0.1111

示例 6.10

回答以下关于 示例6.9 的问题： 1. 多次测量能否改善移动系统位置的估计（在测量之间移动系统不 移动）？ 2. 如果移动系统连续两次检测到某个瓦片为暗，移动系统 位置的概率分布是什么？ 3. 如果移动系统先检测到某个瓦片为暗， 然后检测为亮，移动系统位置的概率分布是什么？ 4. 如果移动系统 先检测到某个瓦片为暗，然后检测为亮，再检测为暗，移动系统位 置的概率分布是什么？

解
1. 如果正确测量的概率高于测量错误的概率，则多个测量值可以改 善对移动系统位置的估计。
2. 若传感器连续两次检测到单元格为暗，则其概率分布如下：

p(X2|Z1= dark, Z2= dark)= p(X2|z1, z2) = p(z2|X2) ∗p(X2|z1) P(z2|z1)
=[0.2, 0.2, 0.6, 0.6, 0.2]T ∗[0.11, 0.11, 0.33, 0.33, 0.11]T
P(z2|z1)

其中概率分布 p(X2|z1) 的第 j 个元素由 P(X2= xj |z1)= pT(X
2= xj |X1)p(X1|z1)= P(X1= xj |z1) 给出，因为我们无法影响状 态（参见示例6.5）；我们仅通过测量值来观测状态。分母中的条 件概率为

P(z2|z1)=∑
xi
P(z2|X2= xi)P(X2= xi|z1)
= p T (z2|X2)p(X2|z1) =[0.2, 0.2, 0.6, 0.6, 0.2][0.11, 0.11, 0.33, 0.33, 0.11] T
= 0.4667

示例 6.10—续

最后，解是

p(X2|z1, z2)=[0.2, 0.2, 0.6, 0.6, 0.2]T ∗[0.11, 0.11, 0.33, 0.33, 0.11]T [0.2, 0.2, 0.6, 0.6, 0.2][0.11, 0.11, 0.33, 0.33, 0.11]T =[0.0476, 0.0476, 0.4286, 0.4286, 0.0476]T

3. 亮单元格被正确检测到的概率为p(Z= bright|X= bright)= 1 −p(Z= dark|X= bright)= 0.8，错误检测的概率为p ( )= 0 = | = )= 1 −(= | = 4 Z brightX darkpZ darkX dark。第二次测量可基于概率分布p(X2|Z1=dark)进行。

p(X2|Z1= dark, Z2= bright)= p(X2|z1, z2)
=[0.8, 0.8, 0.4, 0.4, 0.8]T ∗[0.11, 0.11, 0.33, 0.33, 0.11]T
P(Z2= bright|Z1= dark) P(Z2= bright|Z1= dark)
=∑
xi
P(Z2= bright|X2= xi)P(X2= xi|Z1= dark) = p T(Z2= bright|X2)p(X2|Z1= dark) =[0.8, 0.8, 0.4, 0.4, 0.8][0.11, 0.11, 0.33, 0.33, 0.11]T= 0.533
p(X2|Z1= dark, Z2= bright)=[0.167, 0.167, 0.25, 0.25, 0.167]T

三次测量后的状态概率分布为

p(X3|Z1= dark, Z2= bright, Z3= dark) =[0.083, 0.083, 0.375, 0.375, 0.083] T

解决方案在Matlab中的实现如清单 6.5所示。三个时间步的 后验状态概率分布在图 6.11中以图形形式展示。

清单 6.5 示例 6.10 的解的实现
1 p_ZdX=[0.2 0.2 0.6 0.6 0.2]; 2 p_ZbX= 1‐p_ZdX; 3 p_X= ones(1,5)/5; 4 5 disp( ‘检测到暗块的概率 P(Z1=dark) ’ ) 6 P_z1= p_ZdX*p_X.’ 7 disp( ‘后验分布 p(X1|Z1= dark) ’ ) 8 p_Xz1= p_ZdX.*p_X/P_z1 9 10 disp( ‘检测到亮块的概率 P(Z2=bright|Z1= dark) ’ )
11 P_z2=p_ZbX*p_Xz1.’ 12 显示(’后验分布 p(X2|Z1=暗,Z2=亮)’) 13 p_Xz2= p_ZbX.*p_ Xz1/P_z2 14 15 显示(’检测到暗格子的**概率** P(Z3=暗|Z1=暗,Z2=亮)’) 16 P_z3= p_ZdX*p_ Xz2.’ 17 显示(’后验分布 p(X3|Z1=暗,Z2=亮,Z3=暗)’) 18 p_Xz3= p_ZdX.*p_Xz2/P_z3
检测到暗色格子的概率 P(Z1=dark) P z1= 0.3600 后验分布p(X1|Z1=dark) p_Xz1= 0.1111 0.1111 0.3333 0.3333 0.1111 检测到亮色格子的概率 P(Z2=bright|Z1=dark) P z2=_0.5333 后验分布p(X2|Z1=dark,Z2=bright) p_Xz2=0.1667 0.1667 0.2500 0.2500 0.1667 检测到暗色格子的概率 P(Z3=dark|Z1=dark,Z2=bright) P z3= 0.4000 后验分布p(X3|Z1=dark,Z2=bright,Z3=dark) p_Xz3=0.0833 0.0833 0.3750 0.3750 0.0833

6.4.6 环境中的运动

移动系统可以利用一些执行器（例如电机驱动轮）和控制系统在环 境中移动。每次移动都存在或多或少的不确定性；因此，移动系统 在环境中的移动会增加对其在环境中位姿状态的不确定性。

假设我们站在一个已知环境中。我们闭上眼睛并迈出几步。经 过几步之后，我们大致知道自己所在的位置，因为我们知道步长以 及行走的方向。因此，我们可以大致想象出自己的位置。然而，我 们的步长并不能精确知晓，而且方向也难以准确估计；因此，随着 步数的增加，我们对自身在空间中位姿的了解会随时间逐渐减少。

在不通过测量观测状态的情况下移动时，关系式（6.28）可以重新表述 为：

p(xk|u0:k−1)= ∫+∞ −∞ p(xk|xk−1, uk−1)p(xk−1|u1:k−2) dxk−1

新状态的置信度 p(xk|u0:k−1) 取决于前一时刻的置信度 p(xk−1|u 0:k−2) 以及条件状态转移概率 p(xk|xk−1,uk−1)。在已知动作 uk−1 的情 况下，通过对所有可能的从前一状态xk−1 转移到状态xk 的状态转移 概率 p(xk|xk−1,uk−1) 进行积分（或在离散情况下的求和），可以确 定概率分布 p(xk|,u0:k−1)。

示例6.11

再次考虑 示例6.8 并假设移动系统的初始位置在第一个单元格（X 0= x1）。初始状态可用概率分布 p(X0)=[1, 0, 0, 0, 0] 表示。移动系 统可在单元格之间移动，移动动作的结果有80%的概率正确，10% 的概率移动距离比要求少一个单元格，另有10%的概率多移动一个 单元格。这可以用以下状态转移概率描述：

P(Xk= xi|Xk−1= xj, Uk−1= u)= 0.8; fori=j+ u
P(Xk= xi|Xk−1= xj, Uk−1= u)= 0.1; fori=j+ u−1 P(Xk= xi|Xk−1= xj, Uk−1= u)= 0.1; fori=j+ u+ 1

示例 6.11—续

移动系统必须在逆时针方向上移动两个单元格（U0= 2）。确 定移动完成后移动系统的位置信念。

移动后的概率分布（置信度）可通过计算移动系统在每个单元格中 位置的概率（全概率）来确定。移动系统只能从第3个单元（过长移 动）、第4个单元（正确移动）和第5个单元（过短移动）到达第一 个单元格。这得到转移到第一个单元格的概率分布 p(X1= x1|X0, U 0= 2)=[0, 0, 0.1, 0.8, 0.1]T。移动后，移动系统位于第一个单元格 的概率为

P(X1= x1|U0= 2)=∑
xi
P(X1= x1|X0= xi, U0= 2)P(X0= xi) = p T(X1= x1|X0, U0= 2)p(X0) =[0, 0, 0.1, 0.8, 0.1][1, 0, 0, 0, 0]T= 0

移动系统在移动后位于第二个单元格的概率是

P(X1= x2|U0= 2)=∑
xi
P(X1= x2|X0= xi, U0= 2)P(X0= xi) = p T(X1= x2|X0, U0= 2)p(X0) =[0.1, 0, 0, 0.1, 0.8][1, 0, 0, 0, 0]T= 0.1

类似地，可以计算所有其他单元格的概率：

P(X1= x3|U0= 2)=[0.8, 0.1, 0, 0, 0.1][1, 0, 0, 0, 0] T= 0.8 P(X1= x4|U0= 2)=[0.1, 0.8, 0.1, 0, 0][1, 0, 0, 0, 0] T= 0.1 P(X1= x5|U0= 2)=[0, 0.1, 0.8, 0.1, 0][1, 0, 0, 0, 0] T= 0

因此，移动后的位置置信度为

p(X1|U0= 2)=[0, 0.1, 0.8, 0.1, 0] T

后验状态概率分布在 图 6.12 中以图形方式呈现。该解决方案在 Matlab 中的实现如 清单 6.6 所示。

清单 6.6 示例6.11 解的实现d
1 isp(初始 l belief p(X0)’)
2 p_X0=[1 0 0 0 0]3 4 P_xxu_null = 0.8; % P(X=i|X’=j,U’= u), i=j+u 5 1
6 _ _ = 0 1 = | = = =+ = 0 1 = = = =++1
P xxu less . ; % P(X i X’ j,U’ u), i j u‐ P_xxu_more . ; % P(X i|X’ j,U’ u), i j u 7 8 显示(’置信度 p(X1|U0=2)’); 9 p_xXu =[0 0 P_xxu_more P_xxu_null P_xxu_less];% for U=210 p_Xu= zeros(1,5); 11 for i=1:5 12 p_Xu(i)=p_xXu*p_X0.’; 13 p_xXu = p_xXu([end 1:end‐1]); 14 end 15 p_X1= p_Xu 初始置信度 p(X0) p_X0=1 0 0 0 0 置信度 p(X1|U0=2) p_X1= 0 0.1000 0.8000 0.1000 0

示例6.12

在示例6.11 中移动系统移动后，若其再次沿逆时针方向移动，但此次仅 移动一个单元格（U1= 1），此时移动系统位置的置信度是多少？

示例 6.12—续 解

移动后的概率分布（置信度）可以通过计算移动系统位于每个单元 格的概率（全概率）来再次确定。以单个单元的移动为例，第一个 单元格可以从第1个单元（过短移动）、第4个单元（过长移动）和 第5个单元（正确移动）到达。移动系统可以从第1、2、5个单元到 达第二个单元，以此类推。移动完成后，可以计算出以下概率：

P(X2=x1|U0= 2, U1= 1)=[0.1, 0, 0, 0.1, 0.8][0, 0.1, 0.8, 0.1, 0]T= 0.01 P(X2=x2|U0= 2, U1= 1)=[0.8, 0.1, 0, 0, 0.1][0, 0.1, 0.8, 0.1, 0]T= 0.01 P(X2=x3|U0= 2, U1= 1)=[0.1, 0.8, 0.1, 0, 0][0, 0.1, 0.8, 0.1, 0]T= 0.16P( X2=x4|U0= 2, U1= 1)=[0, 0.1, 0.8, 0.1, 0][0, 0.1, 0.8, 0.1, 0]T