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

概率机器人贝叶斯滤波

状态空间模型

现代控制理论的基础都是建立在状态空间模型之上的,具体的状态空间模型的理论可以参考现代控制理论方面的相关内容。下面给出状态空间模型的方程:

状态方程:

$$X_t = AX_{t-1}+BU_t$$

观测方程:

$$Y_t = CX_t$$

但是现实中的系统不可避免的会有误差产生,但是上面的方程并没有反映出误差对系统的影响。下面对状态方程和观测方程进行完善后,可以得到如下的状态空间模型:

状态方程:

$$X_t = A X_{t-1} + B U_t + \xi_t$$

观测方程:

$$Y_t = C X_t + \delta_t$$

从上面的方程中可以看出,状态方程式关于状态量 $X$ 的一个递推关系,因为在误差的不确定性,这就使得 $X_t$ 成为了一个随机变量。因此,状态 $X_t$ 会有一个概率值,这个概率值由概率论的知识可以得到是 $X_t$ 在发生了 $X_{t-1}$$U_t$ 的条件下发送的。数学公式表示为 $P(X_t|X_{t-1},U_t)$

从抽象一点角度来看 $P(X_t|X_{t-1},U_t)$$X_{t-1}$$U_t$ 的作用下转移到 $X_t$ 的概率,因此叫做状态转移概率。

根据观测方程,同样有测量概率 $P(Y_t|X_t)$ 。状态转移概率和测量概率一起描述了机器人及其环境组成的动态随机系统。

这样的时间生成模型,可以称为隐马尔可夫模型。隐马尔可夫模型的可视化图模型这里就不提了,网上有很多资料可以查阅。

贝叶斯滤波算法

下面先简单介绍一下贝叶斯滤波算法之后,再针对算法进行数据推导。

对于所有的 $X_t$ 有下面的算法:

$$ \begin{align} \overline{bel}(X_t) ={} & \int{P(X_t|X_{t-1},U_t)bel(X_{t-1})dX_{t-1}} \\ bel(X_t) ={} & \eta P(Y_t|X_t) \overline{bel}(X_t) \end{align} $$

贝叶斯滤波的数学推导

$$ \begin{align} P(X_i|Y_{1:t},U_{1;t}) ={} & \frac{P(Y_t|X_t,Y_{1:t-1},U_{1:t-1})P(X_t|Y_{1:t-1},U_{1:t-1})}{P(Y_t|Y_{1:t-1},U_{1:t})}\\ ={} & \eta {P(Y_t|X_t,Y_{1:t-1},U_{1:t-1})P(X_t|Y_{1:t-1},U_{1:t-1})} \end{align} $$

由条件独立有:

$$ P(Y_t|X_t,Y_{1:t-1},U_{1:t-1})=P(Y_t|X_t) $$

因此:

$$ bel(x_t) = \eta P(Y_t|X_t) \overline{bel}(X_t) $$

由全概率公式可以得:

$$ \begin{align} \overline{bel} (X_t) ={} & P(X_t|Y_{1:t-1},U_{1:t-1})\\ ={} & \int P(X_t|X_{t-1},Y_{1:t-1},U_{1:t})P(X_{t-1}|Y_{1:t-1},U_{1:t})dX_{t-1} \end{align} $$

所以得到算法的第二步。 在第二步的公式中,注意状态的完整性和条件独立的运用。