【粒子滤波定位】（二）基于概率的定位方法

1 引言

估计的任务，即从带有随机误差的观测数据中估计出某些参数或某些状态变量。

估计问题一般分三类：
滤波：从当前和过去的观测值来估计信号的当前值。
预测或外推：从过去的观测值来估计信号的现在值或将来值。
平滑或内插：从过去的观测值来估计过去的信号值。

2 基于概率的定位方法

2.1 状态空间模型

利用概率密度函数表示机器人位置变化，即运动概率模型
$$P(x_k|x_{k-1},a_{k-1})$$
表示机器人在$k-1$时刻执行了运动$a_{k-1}$后，$k$时刻机器人从$x_{k-1}$位置到达位置$x_k$的概率。

机器人的感知模型
$$P(z_k|x_k)$$
（个人：表示机器人已经到达$x_k$位置后，感知量为$z_k$的概率密度函数）
感知数据的高维数导致感知模型的概率不容易计算，我们将原始感知数据$s$投影到低维的特征向量$z$上。

2.2 贝叶斯滤波原理

$k$表示时间
x 0 : k = { x i : i = 0 , 1 , … , k } x_{0:k}=\{x_i:i=0,1,\dots,k\} x0:k​={xi​:i=0,1,…,k}代表系列未知变量
z 1 : k = { z i : i = 1 , 2 , … , k } z_{1:k}=\{z_i:i=1,2,\dots,k\} z1:k​={zi​:i=1,2,…,k}代表传感器感知系列变量

若给定系列感知量$z_{1:k}$，则未知量$x_{0:k}$的条件概率分布根据贝叶斯原理有
$$p(x_{0:k}|z_{1:k})=\frac{p(z_{1:k}|x_{0:k})p(x_{0:k})}{\int p(z_{1:k}|x_{0:k})p(x_{0:k})d{x}_{0:k}}$$
其中，$p(x_{0:k})$是先验概率密度，包含未获得新感知数据之前未知状态变量的所有信息。
$p(z_{1:k}|x_{0:k})$是感知量为$z_{1:k}$时的似然概率密度，表达了获得传感器感知数据的前提下未知状态变量出现的概率。
$p(x_{0:k}|z_{1:k})$是后验概率密度。

贝叶斯定理描述了先利用先验概率密度，获得新的传感器感知数据修正先验知识，得到修正后的后验概率密度的过程。
由于不断得到新的感知数据的修正，后验概率密度比先验概率密度更加接近于被估计变量的真实概率分布。
而后验概率密度将作为下一时刻估计变量的先验概率密度，如此递归求解的后验概率密度$p(x_{0:k}|z_{1:k})$是贝叶斯估计的完全解。

利用$p(x_{0:k}|z_{1:k})$计算出后验滤波概率密度$p(x_k|z_{1:k})$为$$p(x_k|z_{1:k})=\iint \cdots \int p(x_{0:k}|z_{1:k})d{x}_{0}d{x}_1\dots d{x}_{k-1}$$

为提高计算效率，采用递推更新的方法获得后验滤波概率密度：

（1）运动预测

已知$k-1$时刻的后验滤波概率密度$p(x_{k-1}|z_{1:k-1})$，$k$时刻，利用系统运动模型求出$x_k$的先验滤波概率密度$p(x_k|z_{1:k-1})$。

（2）感知更新

$k$时刻，利用传感器获得感知量$z_k$修正运动预测阶段获得的先验滤波概率密度，修正后得到$k$时刻的后验滤波概率密度$p(x_k|z_{1:k})$。

设$k-1$时刻滤波概率密度为$p(x_{k-1}|z_{1:k-1})$，假设系统感知$z_k$独立且系统状态$x_k$服从一阶马尔可夫过程。
具体步骤：

$h$应该是测量函数
Dirac Delta函数在除了零以外的点函数值都等于零，而其在整个定义域上的积分等于1：

递推式计算过程中，需要知道运动模型、感知模型和机器人初始信度。

2.3 基于贝叶斯滤波的定位

贝叶斯滤波理论提供一个利用概率方法递归进行状态预估的抽象的理论框架。

其中，连续型和离散型是概率的表达方式。

（1）线性卡尔曼滤波器

系统运动状态模型和传感器感知模型都是线性的且符合高斯分布，同时噪声也符合高斯分布，此时线性卡尔曼滤波器定位结果最优。
利用均值和方差描述机器人位姿的概率分布
$$\begin{array}{rl}Bel(x_k)& =N(x_k;u_k,{\Sigma }_k)\\ & =\frac{1}{(2\pi {)}^{d/2}|{\Sigma }_k{|}^{1/2}}exp[-\frac{1}{2}(x_k-u_k{)}^T{\Sigma }_k^{-1}(x_k-u_k)]\end{array}$$
概率分布的均值$u_k$，协方差矩阵${\Sigma }_k$，维数为$d$；
$N(x_k;u_k,{\Sigma }_k)$表示符合高斯分布的$x_k$的概率分布。
协方差越大，不确定性越高。

典型的线性系统的运动预测方程和感知方程用线性方程描述为
$$\begin{array}{rl}& x_k=A{x}_{k-1}+w_{k-1}\\ & z_k=H{x}_k+{\nu }_k\end{array}$$
其中，$A$是一个$n\times n$的矩阵，系统噪声为$w_{k-1}$，$H$为$m\times n$矩阵，${\nu }_k$表示传感器感知噪声。
预测与校正不断迭代的过程，具体步骤：

其中，$K_k$为卡尔曼增益，卡尔曼增益越大，后验信度的计算中将融合的新的感知信息越多。

局限性在于只能表示单峰的概率分布，系统的状态误差不能太大。
无法应对出现一定信度的错误感知数据的情况，无法实现全局定位。

（2）扩展卡尔曼滤波器

为应对非线性的机器人定位系统的系统模型。

核心本质：利用泰勒展开式在参考点对非线性系统进行线性化，再利用卡尔曼滤波器理论实现系统预估与校正的过程。

定义非线性系统的状态方程和感知模型为
$$\begin{array}{rl}& x_k=f(x_{k-1})+w_{k-1}\\ & z_k=h(x_k)+{\nu }_k\end{array}$$

具体步骤为

雅可比矩阵$A_k$和$H_k$利用最新的状态估计值来计算。

如果系统模型的非线性程度比较大，扩展卡尔曼滤波器将无法进行有效预估，针对这个问题就提出了无迹卡尔曼滤波器。

（3）无迹卡尔曼滤波器

$UKF$是在$EKF$框架下对运动状态转移函数和感知函数进行无迹变换的改进，通过$Sigma$点控制分布采样来提高滤波器对非线性系统的状态估计精度。

系统状态空间模型的运动状态转移函数
$$x_{k+1}=F(x_k,u_k,{\nu }_k)$$
$k$时刻状态量$x_k$；第$k$步运动控制量$u_k$；第$k$步运动噪声${\nu }_k$，服从高斯分布$$p({\nu }_k)=N(0,R^{\nu })$$
$R^{\nu }$为$\nu$协方差。

运动预测模型$$p(x_k|x_{k-1})=N[F(x_{k-1},u_{k-1}),R^{\nu }]$$

传感器感知模型$$p(z_k|x_k)=N[H(x_k,n_k),R^n]$$第$k$步感知测量值$z_k$,感知噪声$n_k$，服从高斯分布$$p(n_k)=N(0,R^n)$$$R^n$为$n$协方差。

系统感知函数$$z_k=H(x_k,n_k)$$
据全概率公式，系统初始化条件$$p(x_0|z_0)=\frac{p(z_0|x_0)p(x_0)}{p(z_0)}$$

$x$和$Z$的均值和协方差的精度都达到了2阶。
$UKF$精度和鲁棒性都比$EKF$好，且计算量增加不大。