【粒子滤波定位】（三）机器人粒子滤波定位方法

粒子滤波器的基本思想来源于Monte Carlo方法，利用一组带有权重值的随机粒子集来估算后验概率密度。当粒子密度趋向于无穷大时，这种估计将等同于后验概率密度。

1 粒子滤波原理

（1）贝叶斯重要性采样（BIS）

随机变量的后验概率密度往往无法直接得到，BIS核心思想是利用一系列带权值的随机样本近似表示后验概率密度，再进一步得到所需的数学期望或方差等统计量的估计值。

求得后验概率密度之后，接下来找到该后验概率密度服从某个概率密度分布的函数的期望值或方差的估计值，利用该后验概率密度计算函数的期望值或方差。

假设状态变量$x_{0:k}$的任意函数为$g(x_{0:k})$，那么函数$g(x_{0:k})$的数学期望为$$E[g(x_{0:k})]=\int g(x_{0:k})p(x_{0:k}|z_{1:k})d{x}_{0:k}$$
（状态量$x_{0:k}$的函数值，乘以，在已知感知量为$z_{1:k}$时，状态量$x_{0:k}$的后验概率密度，作为数学期望）

根据Monte Carlo仿真原理，当$N$足够大时，从后验概率密度$p(x_{0:k}|z_{1:k})$中抽取$N$个独立同分布的粒子 { x 0 : k ( i ) , i = 1 , 2 , … , N } \{x_{0:k}(i),i=1,2,\dots,N\} {x0:k​(i),i=1,2,…,N}，使得数学期望可以近似为$$E[g(x_{0:k})]\approx \frac{1}{N}\sum _{i=1}^{N}g[x_{0:k}(i)]$$
（不乘上面的概率密度函数了，根据蒙特卡洛采样，就是随机采样，最后再平均，也能得到近似于期望值的结果）

在实际问题中很难直接从后验概率密度中抽样，遂引入一个重要性概率密度的参考分布$q(x_{0:k}|z_{1:k})$。利用贝叶斯公式和$q(x_{0:k}|z_{1:k})$求取函数期望为
$$\begin{array}{rl}E[g(x_{0:k})]& =\int g(x_{0:k})\frac{p(x_{0:k}|z_{1:k})}{q(x_{0:k}|z_{1:k})}q(x_{0:k}|z_{1:k})d{x}_{0:k}（乘上q(x_{0:k}|z_{1:k})同时分母上除以它）\\ & =\int g(x_{0:k})\frac{p(z_{1:k}|x_{0:k})p(x_{0:k})}{p(z_{1:k})q(x_{0:k}|z_{1:k})}q(x_{0:k}|z_{1:k})d{x}_{0:k}（贝叶斯定理p(x_{0:k}|z_{1:k})=\frac{p(z_{1:k}|x_{0:k})p(x_{0:k})}{p(z_{1:k})}）\\ & =\int g(x_{0:k})\frac{w_k^{\ast }(x_{0:k})}{p(z_{1:k})}q(x_{0:k}|z_{1:k})d{x}_{0:k}（提取出一个w_k(x_{0:k})）\\ & =\frac{\int g(x_{0:k})w_k^{\ast }(x_{0:k})q(x_{0:k}|z_{1:k})d{x}_{0:k}}{p(z_{1:k})}（对x_{0:k}积分，对p(z_{1:k})作常数处理）\end{array}$$
其中，令权值$$w_k^{\ast }(x_{0:k})=\frac{p(z_{1:k}|x_{0:k})p(x_{0:k})}{q(x_{0:k}|z_{1:k})}$$
化简$p(z_{1:k})$：
$$\begin{array}{rl}p(z_{1:k})& =\int p(z_{1:k},x_{0:k})d{x}_{0:k}（边缘概率密度公式）\\ & =\int \frac{p(z_{1:k}|x_{0:k})p(x_{0:k})q(x_{0:k}|z_{1:k})}{q(x_{0:k}|z_{1:k})}d{x}_{0:k}（全概率公式p(z_{1:k},x_{0:k}=p(z_{1:k}|x_{0:k})p(x_{0:k})，同时分子分母同时乘q(x_{0:k})|z_{1:k})）\\ & =\int w_k^{\ast }(x_{0:k})q(x_{0:k}|z_{1:k})d{x}_{0:k}（提取出w_k(x_{0:k})）\end{array}$$
代入$E[g(x_{0:k})]$：
E [ g ( x 0 : k ) ] = ∫ g ( x 0 : k ) w k ∗ ( x 0 : k ) q ( x 0 : k ∣ z 1 : k ) d x 0 : k ∫ w k ∗ ( x 0 : k ) q ( x 0 : k ) ∣ z 1 : k ) d x 0 : k ≈ 1 N ∑ i = 1 N g [ x 0 : K ( i ) ] w k ∗ [ x 0 : k ( i ) ] 1 N ∑ i = 1 N w k ∗ [ x 0 : k ( i ) ] （ M o n t e   C a r l o 仿 真 原 理 ， 从 重 要 性 概 率 密 度 q ( x 0 : K ∣ z 1 : k ) 中 抽 取 N 个 独 立 同 分 布 的 样 本 { x 0 : k ( i ) , i = 1 , 2 , … , N } 近 似 表 示 数 学 期 望 ） = ∑ i = 1 N g [ x 0 : K ( i ) ] w k [ x 0 : k ( i ) ] \begin{aligned}E[g(x_{0:k})]&=\frac{\int{g(x_{0:k}){w_k^*(x_{0:k})}q(x_{0:k}|z_{1:k})dx_{0:k}}}{\int{w_k^*(x_{0:k})q(x_{0:k})|z_{1:k})dx_{0:k}}}\\&\approx\frac{\frac{1}{N}\sum_{i=1}^Ng[x_{0:K}(i)]w_k^*[x_{0:k}(i)]}{\frac{1}{N}\sum_{i=1}^Nw_k^*[x_{0:k}(i)]}（Monte\ Carlo仿真原理，从重要性概率密度q(x_{0:K}|z_{1:k})中抽取N个独立同分布的样本\{x_{0:k}(i),i=1,2,\dots,N\}近似表示数学期望）\\&=\sum_{i=1}^Ng[x_{0:K}(i)]w_k[x_{0:k}(i)]\end{aligned} E[g(x0:k​)]​=∫wk∗​(x0:k​)q(x0:k​)∣z1:k​)dx0:k​∫g(x0:k​)wk∗​(x0:k​)q(x0:k​∣z1:k​)dx0:k​​≈N1​∑i=1N​wk∗​[x0:k​(i)]N1​∑i=1N​g[x0:K​(i)]wk∗​[x0:k​(i)]​（Monte Carlo仿真原理，从重要性概率密度q(x0:K​∣z1:k​)中抽取N个独立同分布的样本{x0:k​(i),i=1,2,…,N}近似表示数学期望）=i=1∑N​g[x0:K​(i)]wk​[x0:k​(i)]​
其中，归一化权值$$w_k[x_{0:k}(i)]=\frac{w_k^{\ast }[x_{0:k}(i)]}{{\sum }_{i=1}^N{w}_k^{\ast }[x_{0:k}(i)]}$$
贝叶斯重要性采样方法中，每次利用到新的感知数据$y_k$都要重新从重要性概率密度$q(x_{0:K}|y_{1:k})$中抽取样本 { x 0 : k ( i ) , i = 1 , 2 , … , N } \{x_{0:k}(i),i=1,2,\dots,N\} {x0:k​(i),i=1,2,…,N}，重新计算每个粒子$x_{0:k}(i)$的权值$w_k[x_{0:k}(i)]$，导致计算的存储空间增大，计算量增大。

（2）序贯重要性采样（SIS）

$SequentialImportanceSampling$方法以序列形式采样，采用递推更新方式计算权值，大大提高计算效率。

对重要性概率密度进行分解：$$q(x_{0:k}|z_{1:k})=q(x_k|x_{0:k},z_{1:k})q(x_{0:k-1},z_{1:k-1})$$
相应地
$$w_{k-1}^{\ast }(x_{0:k-1})=\frac{p(z_{1:k-1}|x_{0:k-1})p(x_{0:k-1})}{q(x_{0:k-1}|z_{1:k-1})}$$
$$\begin{array}{rl}{w}_k^{\ast }(x_{0:k})& =\frac{p(z_{1:k}|x_{0:k})p(x_{0:k})}{q(x_{0:k-1}|z_{1:k})q(x_{0:k-1}|z_{1:k-1})}\\ & =w_{k-1}^{\ast }(x_{0:k})\frac{p(z_k|x_k)p(x_k|x_{k-1})}{q(x_k|x_{0:k-1},z_{1:k})}\end{array}$$
SIS算法具体实现步骤：

SIS算法是一种递推的、样本权值采用递归更新方式的贝叶斯重要性采样方法。
粒子滤波算法就是基于SIS的。

（3）重要性函数选择

重要性函数$q(x_k|x_{k-1}^i,z_k)$直接影响粒子的重要性分布，合理的选择保障粒子集的重要性权值方差最小，同时使粒子集的采样变得容易。

Doucet提出一种最优的重要性分布选择办法，即
$$q(x_k|x_{k-1}^i,z_k{)}_{opt}=p(x_k|x_{k-1}^i,z_k)$$
选择真实分布作为重要性分布，此时任意粒子$x_{k-1}^i$的权重值$w_k^i$都等于$1/N$，$var(x_k^i)=0$，则权重值更新为
$$w_k^i=w_{k-1}^i\int p(z_k|x_k^i)p(x_k^i|x_{k-1})d{x}_k^i$$
主要缺点是：
通常无法得到真实分布$p(x_k|x_{k-1}^i,z_k)$；
计算权重值时的积分一般无法求解。

为避免上述缺点，可选择重要性分布函数为
$$q(x_k|x_{k-1}^i,z_k)=p(x_k|x_{k-1}^i)$$
则权重值更新为
$$w_k^i=x_{k-1}^{i}p(z_k,x_k^i)$$
该重要性分布函数没有融入最新的传感器感知信息，以先验密度函数进行抽样得到的权重值系数的方差大得多，但由于形式简单易于实现，广泛应用于粒子滤波算法中。

（4）重要性重采样

粒子退化问题，即经过几次迭代之后许多粒子的权重值变得很小，而权重值小的粒子对定位估计$p(x_k|z_{1:k})$的贡献小，而计算量耗费在权值小的粒子上导致算法效率不高，所以要对粒子集进行重采样来解决退化现象。
如果每个迭代周期中都对粒子集进行重采样，会导致粒子贫乏。
通过判断粒子集的有效粒子数目，可以进行自适应的重采样。

判断粒子权重值接近零的数目的方法，变异系数（Coefficient of Variation）$$c{v}_t^2=\frac{var[w_{(}i)]}{E^2[w_t(i)]}=\frac{1}{N}\sum _{i=1}^N[Mw(i)-1]$$
有效粒子集数目$$ES{S}_t=\frac{N}{1+c{v}_t^2}$$
重采样方法本质就是复制和保留权重值大的粒子，淘汰权重值小的粒子。把一非等权重值的粒子集 { X k ( i ) , π k ( i ) } \{X_k^{(i)},\pi_k^{(i)}\} {Xk(i)​,πk(i)​}映射成一个权重值相等的新粒子集 { X k ( i ) , N − 1 } \{X_k^{(i)},N^{-1}\} {Xk(i)​,N−1}。

根据粒子权重值的大小比例来选择复制粒子是其中一个最简单的重采样方法。具体实现过程是：

• 计算出当前粒子集的累积和；
• 选择均匀分布在$[0,1]$之间的$N$个随机数，并按照从小到大排序；
• 粒子将要被复制的次数就是在累积和每个区间内出现的已排序随机数的个数。
  如果一个粒子的权重值比较大，则对应的累积和区间也比较大，因此随机数处于此区间的概率就比较大，此粒子就会被复制多次而保留下来。

2 基于PF的机器人定位算法

核心思想就是利用$N$个带权重的离散粒子 S k = { ( x k j , w k j ) ∣ j = 1 , 2 , … , N } S_k=\{(x_k^j,w_k^j)|j=1,2,\dots,N\} Sk​={(xkj​,wkj​)∣j=1,2,…,N}来表示机器人位姿的后验概率密度$$p(x_k|z_{1:k})=\sum w_k^j\delta (x_k-x_k^j)$$
其中$k$时刻，机器人在位姿状态$x_k$的粒子为$x_k^j$，权值$w_k^j$表示$k$时刻机器人处于$x_k^j$状态的概率。

粒子滤波定位方法的优点主要表现为：

• 克服了基于卡尔曼滤波器的定位方法的缺点，能够处理多峰分布问题，可以很好地解决全局定位问题；
• 与基于网格的马尔可夫定位方法相比，很大程度上降低了内存空间的占用，能将最新的传感器感知量融合到机器人状态估计中；
• 与固定网格大小的马尔可夫定位方法相比，粒子滤波定位方法适用于多种传感器，易于实现，而且定位方法更高。