概率机器人粒子滤波

分类于: Robotics
关键词: 概率机器人

概率机器人粒子滤波

基本算法

粒子滤波(particle filter)是贝叶斯滤波的另一种非参数实现。粒子滤波的主要思想是用一系列从后验得到的随机状态采样表示后验 $bel(x_t)$ 。

粒子滤波中，后验分布的样本叫作粒子(particles)，有

$$ \chi_t := x_t^{[1]}, x_t^{[2]}, \dots, x_t^{[m]} $$

每一个粒子 $x_t^{[m]}(1 \leq m \leq M)$ 是状态在时刻t的一个具体实例。

粒子滤波的直观感觉就是用一系列粒子 $\chi_t$ ,来近似置信度 $bel(x_t)$ 。理想情况下，状态假设 $x_t$ 包含在粒子集 $\chi_t$ 中的可能性与其贝叶斯滤波的后验 $bel(x_t)$ 成比例：

$$ x_t^{[m]} \sim p(x_t|z_{1:t}, u_{1:t}) $$

Algorithm Particle_filter

算法输入：( $\chi_{t-1}, u_t, z_t$ )

$$ \overline{\chi}_t=\chi_t=\phi $$

for $m = 1$ to $M$ do sample $x_t^{[m]} \sim p(x_t|u_t, x_{t-1}^{[m]})$ $w_t^{[m]} = p(z_t|x_t^{[m]})$ $\overline{\chi}_t = \overline{\chi}_t + $ endfor

for $m=1$ to $M$ do draw $i$ with probability $\propto w_t^{[i]}$ add $x_t^{[i]}$ to $\chi_t$ endfor

return $\chi_t$

重要性采样

由于概率机器人这本书上讲的重要性采样我感觉讲得不是特别清楚，所以，我参考了B站上徐亦达教授讲解的粒子滤波部分的视频（徐亦达机器学习：Particle Filter 粒子滤波【2015年版-全集】）。虽然，我还是不能完全搞懂粒子滤波，至少，对粒子滤波使用重要性采样的原因有了一个大概的认识：使用已知的分布进行抽样，避免寻找状态的实际分布。

个人简单的理解是要从 $f(x)$ 的分布中进行采样来表示该函数的置信度。很多情况下，分布 $f(x)$ 是无法知道其详细形式的。利用下面的公式：

$$ E_f[I(x \in A)] = \int f(x)I(x \in A) dx = \int \frac{f(x)}{g(x)} g(x)I(x \in A) dx $$

将 $w(x) = \frac{f(x)}{g(x)}$ 用来说明 $f$ 和 $g$ 之间的不匹配性。重要性采样算法采用了这种转换，将从 $f$ 中获得一个样本，转换到从 $g$ 中获取一个样本。

与 $g$ 密度相关的分布称为建议分布，与 $f$ 密度相关的分布称为目标分布。

数学推导

假设粒子为状态序列： $x_{0:t}^{[m]} = x_0^{[m]}, x_1^{[m]}, \dots, x_t^{[m]}$ 。

在所有状态序列上计算后验： $bel(x_{0:t}) = p(x_{0:t}|u_{1:t}, z_{1:t})$

推导过程：

$$ \begin{align} p(x_{0:t}|u_{1:t}, z_{1:t}) &= \eta p(z_t|x_{0:t}, z_{1:t}, u_{1:t}) p(x_{0:t}|z_{1:t-1}, u_{1:t})\\ &= \eta p(z_t|x_t) p(x_{0:t}|z_{1:t}, u_{1:t})\\ &= \eta p(z_t|x_t) p(x_t|x_{0:t-1}, z_{1:t-1}, u_{1:t})p(x_{0:t-1}|z_{1:t-1}, u_{1:t})\\ &= \eta p(z_t|x_t) p(x_t|x_{t-1}, u_t) p(x_{0:t-1}|z_{1:t-1}, u_{1:t-1}) \end{align} $$

注意：此次表示的是从初始状态到目前状态的所有状态，并不只是最近时刻的状态。

$$ w_t^{[m]} = \frac{f(x)}{g(x)} = \frac{\eta p(z_t|x_t) p(x_t|x_{t-1}, u_t) p(x_{0:t-1}|z_{1:t-1}, u_{1:t-1})}{p(x_t|x_{t-1}, u_t)P(x_{0:t-1}|z_{0:t-1}, u_{0:t-1})} = \eta p(z_t|x_t) $$

因此，可以获得下列等式：

$$ \eta w_t^{[m]} P(x_t|x_{t-1}, u_t)p(x_{0:t-1}|z_{0:t-1}, u_{0:t-1}) = bel(x_{0:t}) $$

粒子滤波的实际考虑和特性

密度提取

粒子滤波下的样本集表示了连续置信度的离散近似。但是，许多应用需要获得连续估计。从这样的样本中提取一个连续密度的问题称为密度估计。

抽样方差

粒子滤波误差的一个重要来源与随机采样固有变化有关。每当有限样本数最来自概率密度时，从这些样本提取的统计特性与初始密度的统计特性就会稍有不同。

重采样

抽样方差是通过重复的重采样放大的。为了理解这个问题，考虑极端情况是很有用的，即机器入的状态不发生变化。

采样偏差

即使实际仅采用有限多个粒子也会在后验估计中引入系统偏差(bias)。

粒子缺乏

即使有大最的粒子，但是在正确的状态附近没有粒子的情况也是有可能发生的。