CN108375374A - 基于自适应迭代容积粒子滤波的蒙特卡罗定位算法 - Google Patents

Abstract

本发明提供了一种基于自适应迭代容积粒子滤波的蒙特卡罗定位算法，属于机器人定位方法技术领域，本发明在CMCL(容积粒子滤波的蒙特卡罗定位)算法的基础上进行改进，针对当前CMCL算法存在的计算量大、实时处理能力较差的问题，提出了一种新的定位算法。本发明使用高斯‑牛顿迭代容积卡尔曼滤波器生成重要性提议分布的容积粒子滤波器；并且利用Kullback‑Leibler距离准则对CPF进行增强，以自适应地选择粒子数量。本发明可以有效避免标准容积粒子滤波的高阶截断误差的限制，减小定位误差及算法计算量，提高处理能力。

Description

基于自适应迭代容积粒子滤波的蒙特卡罗定位算法

技术领域

本发明涉及机器人定位方法技术领域，具体为一种基于自适应迭代容积粒子滤波的蒙特卡罗定位算法。

背景技术

移动机器人定位利用先验环境地图信息、前一时刻位姿估计以及传感器的观测信息，经过一系列的处理和变换，产生对当前位姿的估计，从而确定其在工作环境中所处位置。传感器能够测量机器人各个方向和最近障碍物之间的距离。

基于粒子滤波的蒙特卡罗定位(Monte Carlo Localization，MCL)算法是以先验分布代替后验分布进行采样，并结合观测似然函数来评估每个粒子的重要性权重，忽略了当前移动机器人环境的观测信息对其状态估计的修正作用，使预测粒子集分布在观测似然函数的尾部，因此导致粒子集退化问题。为解决这一问题，国内外学者们作了大量研究工作，如:Khan等将马尔可夫蒙特卡罗采样引入粒子滤波，解决了粒子滤波在高维空间中采样效率低的问题，并在一定程度上避免了粒子集的退化；Pfaff等通过平滑观测似然函数使粒子滤波中的有效粒子增加。van de Merwe等利用Unscented卡尔曼滤波(Unscented KalmanFilter，UKF)设计粒子滤波器的提议分布，提出了Unscented粒子滤波(UnscentedParticle Filter，UPF)算法，使粒子更加集中于高观测似然区域，能有效解决粒子退化问题。E.Y.Wu利用扩展卡尔曼滤波器设计了粒子滤波器的提议分布，提出了提议密度可变边界的马尔科夫链蒙特卡罗(MCMC)重采样方法，以提高粒子的细化能力。Sarkar提出了一种计算粒子滤波器中重要性权值的新方法，并对其在姿态跟踪中的性能进行了广泛的研究。J.Pérez提出了扩展的蒙特卡洛定位方法，当机器人位姿丢失时，它通过视觉地点识别信息检测和重置其位置。A.Alhashimi通过改进测量模型有效地减少了MCL算法的计算负荷。Q.L.Lee提出基于容积粒子滤波(CPF)的蒙特卡罗定位，它克服了粒子集退化问题，利用容积卡尔曼滤波(CKF)生成更精确的提议分布，并证明了其性能优于一般MCL和unscentedMCL算法。然而，非线性系统在容积卡尔曼滤波器中的线性化过程导致了高阶截断误差。对于MCL算法中的每一次迭代，都必须通过序贯重要性采样得到粒子集，并计算出相应的重要性权重，从而导致计算量大，实时处理能力不强。

发明内容

针对现有技术中的缺陷，本发明提供一种基于自适应迭代容积粒子滤波的蒙特卡罗定位算法，其将标准粒子滤波器与迭代容积卡尔曼滤波器(ICKF)相结合。ICKF在粒子滤波中起着提议分布的作用，缓解了忽略最新依据的不足，消除了高阶截断误差。除了采样步长的自适应执行外，本算法还基于Kullback-Leibler距离准则自适应地调整粒子数量，提高了实时处理能力。

本发明技术方案如下：

一种基于自适应迭代容积粒子滤波的蒙特卡罗定位算法，其特征在于，包括以下步骤：

步骤一、滤波初始化(t＝0)，确定系统状态初始分布N(x₀,P₀)，控制噪声Q、观测噪声R以及界限n_χ和δ，区间b尺寸Δ，最小样本容量n_min，从移动机器人状态初始分布采集n个粒子，即计算容积点；

步骤二、采用迭代容积卡尔曼滤波得到重要性函数并根据重要性函数计算粒子状态

步骤三、滤波初始化(t＝0)，确定系统状态初始分布N(x₀,P₀)，控制噪声Q、观测噪声R以及界限n_χ和δ，区间b尺寸Δ，最小样本容量n_min，从移动机器人状态初始分布采集n个粒子，即计算容积点；

步骤四、量测更新，计算每个粒子的重要性权重且将粒子集权重归一化；

步骤五、计算机器人定位所需要的粒子数；

步骤六、计算有效采样尺度N_eff，如果当前有效采样尺度N_eff小于阈值N_thr，则对粒子进行基于KLD的重采样；

步骤七、以粒子集数学期望作为当前时刻t的移动机器人位姿状态输出

与现有技术相比本发明具有如下有益效果：

本发明提供了一种基于自适应迭代容积粒子滤波的蒙特卡罗定位算法，该算法在CMCL算法上进行改进，将标准粒子滤波器与迭代容积卡尔曼滤波器(ICKF)相结合。ICKF在粒子滤波中起着提议分布的作用，缓解了忽略最新依据的不足，消除了高阶截断误差。除了采样步长的自适应执行外，本算法还利用Kullback-Leibler距离准则对CPF进行增强，以自适应地选择粒子数量，Kullback-Leibler距离准则能够自适应地调整粒子数量，提高了实时处理能力。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍。在所有附图中，类似的元件或部分一般由类似的附图标记标识。附图中，各元件或部分并不一定按照实际的比例绘制。

图1为本发明方法的流程示意图；

具体实施方式

下面结合附图对本发明的实施例作进一步详细说明。

如图1所示的一种基于自适应迭代容积粒子滤波的蒙特卡罗定位算法，包括以下步骤：

步骤一、滤波初始化(t＝0)，确定系统状态初始分布N(x₀,P₀)，控制噪声Q、观测噪声R以及界限n_χ和δ，区间b尺寸Δ，最小样本容量n_mi_n，从移动机器人状态初始分布采集n个粒子，即计算容积点；

步骤二、采用迭代容积卡尔曼滤波得到重要性函数并根据重要性函数计算粒子状态

步骤三、滤波初始化(t＝0)，确定系统状态初始分布N(x₀,P₀)，控制噪声Q、观测噪声R以及界限n_χ和δ，区间b尺寸Δ，最小样本容量n_min，从移动机器人状态初始分布采集n个粒子，即计算容积点；

步骤四、量测更新，计算每个粒子的重要性权重且将粒子集权重归一化；

步骤五、计算机器人定位所需要的粒子数；

步骤六、计算有效采样尺度N_eff，如果当前有效采样尺度N_eff小于阈值N_thr，则对粒子进行基于KLD的重采样；

步骤七、以粒子集数学期望作为当前时刻t的移动机器人位姿状态输出

上述步骤二中具体过程如下：

根据上一次的增广粒子状态和上一次的增广粒子状态协方差计算容积粒子集，

其中，增广粒子状态满足高斯分布，

利用系统状态转移函数f得到转换后的容积粒子集；

结合球面-径向规则，利用容积粒子集计算预测粒子状态和协方差矩阵。

上述步骤四中量测更新，具体过程如下：

迭代更新能够有效的降低容积卡尔曼滤波中非线性系统线性化引起的高阶截断误差，采用高斯-牛顿迭代更新来减少这种误差，可以提高滤波的准确性，迭代更新方程为：

x_t,it＝x_t|t-1+K_t,it-1(z_t-z_t|t-1-H_t,it-1(x_t|t-1-x_t|it))；

式中it是迭代更新的次数，K_t,it-1是it-1次迭代之后的卡尔曼滤波器增益，H_t,it-1是it-1次迭代后测量模型的雅可比行列式；将迭代更新方程中的卡尔曼滤波增益和量测雅可比矩阵改写为：

K_t,it-1＝cov(x,z)(cov(z,z)+R)^-1

用统计线性化来代替一阶线性化来计算第i-t次容积粒子的状态迭代更新，根据预测粒子状态和协方差矩阵计算容积粒子集：

在等式满足初始化条件的情况下，

使用由测量函数h传递的量测容积粒子集计算量测预测：

使用容积粒子集计算协方差矩阵，得的粒子状态更新：

it-t次迭代之后的第i-t个粒子的卡尔曼滤波器增益是：

迭代更新的状态和状态协方差矩阵是：

在容积卡尔曼滤波器迭代更新后执行序贯重要性采样；

如果机器人同时获得多个环境特征，容积粒子集中的每个粒子的状态迭代更新后，执行序贯重要性采样并根据以下公式计算每个粒子的重要性权重：

其中，当迭代次数it＝1时，容积粒子集的迭代更新将退化为CPF更新过程。

上述步骤六中，基于KLD的重采样，包括：

Kullback Lerbler距离(KLD)采样已成功地应用于诸如非线性状态估计等领域，KLD采样核心思想是：在粒子滤波的每次迭代中，以概率1-δ使真实后验概率和基于样本的估计概率密度之间的误差小于ε，由此来确定重采样样本数目，该误差是通过计算Kullback-Lerbler距离(K-L距离)来确定的。K-L距离用来表示两个概率分布p和q之间的逼近误差，即

K-L距离取值非负，是衡量两个概率分布之间差异的标准，当且仅当值为零时两个分布相同。由概率理论分析，当n满足一定数量时，可以保证真实概率密度与估计概率密度之间的K-L距离小于阈值ε，从而保证两者之间的逼近误差最小，此时n的值为

其中χ²分布的分位数通过下式确定

利用Wilson-Hilferty变换近似可以得到

其中，z₁-_δ是上分位数为1-δ的标准正态分布。

这个方法提供了在下一个时间步中连续重要性采样所需要的粒子数量的下限。为了保持性能，同时避免过多的计算工作量，样本数量被限制在[N_min,N_max]。此外，若当前颗粒的数量多于下一步所需要的数量，则丢弃最坏的粒子。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解；其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的范围，其均应涵盖在本发明的权利要求和说明书的范围当中。

Claims (4)

1.一种基于自适应迭代容积粒子滤波的蒙特卡罗定位算法，其特征在于，包括以下步骤：

步骤一、滤波初始化(t＝0)，确定系统状态初始分布N(x₀,P₀)，控制噪声Q、观测噪声R以及界限n_χ和δ，区间b尺寸Δ，最小样本容量n_min，从移动机器人状态初始分布采集n个粒子，即计算容积点；

步骤二、采用迭代容积卡尔曼滤波得到重要性函数并根据重要性函数计算粒子状态

步骤三、滤波初始化(t＝0)，确定系统状态初始分布N(x₀,P₀)，控制噪声Q、观测噪声R以及界限n_χ和δ，区间b尺寸Δ，最小样本容量n_min，从移动机器人状态初始分布采集n个粒子，即计算容积点；

步骤四、量测更新，计算每个粒子的重要性权重且将粒子集权重归一化；

步骤五、计算机器人定位所需要的粒子数；

步骤六、计算有效采样尺度N_eff，如果当前有效采样尺度N_eff小于阈值N_thr，则对粒子进行基于KLD的重采样；

步骤七、以粒子集数学期望作为当前时刻t的移动机器人位姿状态输出

2.根据权利要求1所述的基于自适应迭代容积粒子滤波的蒙特卡罗定位算法，其特征在于，步骤二中具体过程如下：

根据上一次的增广粒子状态和上一次的增广粒子状态协方差计算容积粒子集：

其中，增广粒子状态满足高斯分布，

利用系统状态转移函数f得到转换后的容积粒子集：

结合球面-径向规则，利用容积粒子集计算预测粒子状态和协方差矩阵：

3.根据权利要求1所述的基于自适应迭代容积粒子滤波的蒙特卡罗定位算法，其特征在于，其中步骤四中量测更新，具体过程如下：

迭代更新能够有效的降低容积卡尔曼滤波中非线性系统线性化引起的高阶截断误差，采用高斯-牛顿迭代更新来减少这种误差，可以提高滤波的准确性，迭代更新方程为：

x_t,it＝x_t|t-1+K_t,it-1(z_t-z_t|t-1-H_t,it-1(x_t|t-1-x_t|it))；

式中it是迭代更新的次数，K_t,it-1是it-1次迭代之后的卡尔曼滤波器增益，H_t,it-1是it-1次迭代后测量模型的雅可比行列式；将迭代更新方程中的卡尔曼滤波增益和量测雅可比矩阵改写为：

K_t,it-1＝cov(x,z)(cov(z,z)+R)^-1

用统计线性化来代替一阶线性化来计算第i-t次容积粒子的状态迭代更新，根据预测粒子状态和协方差矩阵计算容积粒子集：

在等式满足初始化条件的情况下，

使用由测量函数h传递的量测容积粒子集计算量测预测：

使用容积粒子集计算协方差矩阵，得的粒子状态更新：

；

it-t次迭代之后的第i-t个粒子的卡尔曼滤波器增益是：

迭代更新的状态和状态协方差矩阵是：

在容积卡尔曼滤波器迭代更新后执行序贯重要性采样；

如果机器人同时获得多个环境特征，容积粒子集中的每个粒子的状态迭代更新后，执行序贯重要性采样并根据以下公式计算每个粒子的重要性权重：

其中，当迭代次数it＝1时，容积粒子集的迭代更新将退化为CPF更新过程。

4.根据权利要求1所述的基于自适应迭代容积粒子滤波的蒙特卡罗定位算法，步骤六中，基于KLD的重采样，具体过程如下：

KLD采样在粒子滤波的每次迭代中，以概率1-δ使真实后验概率和基于样本的估计概率密度之间的误差小于ε，由此来确定重采样样本数目，该误差是通过计算Kullback-Lerbler距离(K-L距离)来确定的；K-L距离用来表示两个概率分布p和q之间的逼近误差，即

K-L距离取值非负，是衡量两个概率分布之间差异的标准，当且仅当值为零时两个分布相同；由概率理论分析，当n满足一定数量时，可以保证真实概率密度与估计概率密度之间的K-L距离小于阈值ε，从而保证两者之间的逼近误差最小，此时n的值为

其中χ²分布的分位数通过下式确定

利用Wilson-Hilferty变换近似可以得到

其中，z_1-δ是上分位数为1-δ的标准正态分布；

这个方法提供了在下一个时间步中连续重要性采样所需要的粒子数量的下限，为了保持性能，同时避免过多的计算工作量，样本数量被限制在[N_min,N_max]，此外，若当前颗粒的数量多于下一步所需要的数量，则丢弃最坏的粒子。