一种基于改进迭代容积粒子滤波算法的定位方法与流程

本发明涉及一种定位方法，尤其涉及一种基于改进迭代容积粒子滤波算法的定位方法。

背景技术：

机器人定位技术是移动机器人利用运动信息和环境观测信息估算自身在环境中所处的位置和姿态，该技术在被广泛应用在路径规划、避障等方面，也是移动机器人自主导航研究中最基本、最重要的问题。其中基于粒子滤波的蒙特卡洛定位(montecarlolocalization，mcl)算法是当前学术界研究移动机器人定位问题的主流方法之一，mcl用离散粒子集来近似移动机器人位姿的后验概率分布，适应于任意系统动态、任意噪声分布系统的后验信度估计。在基于粒子滤波的蒙特卡洛定位算法中，粒子提议分布的计算是影响算法性能的关键因素之一。而算法的性能直接影响到定位的准确性和效率。

基于粒子滤波的定位算法中的粒子提议分布的计算常常用到卡尔曼滤波器技术。随着卡尔曼滤波器技术的发展，迭代扩展卡尔曼滤波和容积卡尔曼滤波相继被引入到粒子滤波定位算法中的粒子提议分布的计算中。这些算法在一定程度上克服了基于粒子滤波定位方法的粒子集退化问题，但是仍存在一些问题。例如迭代扩展卡尔曼滤波方法在处理非线性问题时，通过对非线性函数的泰勒展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性，但是在该过程中引入了截断误差，很可能会导致滤波发；容积卡尔曼滤波利用三阶球面-相径容积规则来近似非线性高斯滤波中的高斯积分，避免了迭代扩展卡尔曼算法对非线性函数的线性化处理，但是在量测方程为非线性函数时，基于容积卡尔曼的粒子滤波定位算法利用基于线性贝叶斯估计量的高斯近似滤波器逼近状态后验概率密度函数时，达到的效果却不理想。

技术实现要素：

发明目的：针对现有技术的不足，本发明提出一种基于改进迭代容积粒子滤波算法的定位方法，以在确保定位准确度的同时提高定位效率。

技术方案：本发明提出的基于改进迭代容积粒子滤波算法的定位方法包括下列步骤：

(1)产生由多个粒子组成的粒子集并初始化，每一粒子包含如下参数：移动机器人的状态均值估计值、状态协方差估计值和该粒子的权重；

(2)基于先前的状态均值估计值和状态协方差估计值、运动信息、噪声控制信息及机器人运动模型来计算当前的状态均值预测值和状态协方差预测值；

(3)如果当前在已知环境中观测到一个或多个路标，针对每个路标对所述粒子集进行m次迭代更新，m为设定的整数；每次迭代的过程包括：

(3.1)基于路标的高斯密度函数、当前的状态均值预测值和状态协方差预测值来计算量测容积点集；

(3.2)将量测容积点集分解为机器人状态分量、路标状态分量和量测分量；

(3.3)基于机器人状态分量、路标状态分量、量测分量和已知的观测模型来计算转换量测容积点集；

(3.4)基于所述转换量测容积点集计算路标量测值的预测值；

(3.5)基于所述转换量测容积点集、路标量测值的预测值、机器人状态分量和当前的状态均值预测值来计算量测新息的协方差、机器人状态和量测值之间的互协方差以及卡尔曼滤波参数；

(3.6)基于量测新息的协方差、机器人状态和量测值之间的互协方差、卡尔曼滤波参数以及已知的路标的量测值完成对当前的状态均值估计值的一次更新，当前的状态协方差估计值保持不变，将更新后的当前的状态均值估计值作为下一次迭代所需的当前的状态均值预测值，将当前的状态协方差估计值作为下一次迭代所需的状态协方差预测值；

基于对当前路标进行的m次迭代后的机器人状态和量测值之间的互协方差、卡尔曼滤波参数和状态协方差预测值来更新每个粒子中的状态协方差估计值；当某一时刻观测到多个路标时，将针对当前路标进行m次迭代后最终得到的经更新的状态均值估计值和状态协方差估计值作为针对下一路标进行第一次迭代前的状态均值预测值和状态协方差预测值；

(4)基于步骤(3)中最终得到的经更新的状态均值估计值和状态协方差估计值来完成每个粒子的重要性采样，并基于采样值来更新每个粒子的权值，以实现所述移动机器人的定位。

有益效果：本发明与现有技术相比，其优点为：

1、采用多个容积点来代替传统迭代扩展卡尔曼滤波线性化截断过程，杜绝了截断误差的引入，进而提高了量测更新过程似然函数的逼近精度。同时，相比于迭代扩展卡尔曼滤波在一个滤波周期内要进行m次状态均值和状态协方差更新，本发明专利方法对状态均值进行了m次计算，而状态协方差只在最后一次迭代过程中更新，从而减小了计算量，提高了运行速率。

2、在对迭代步长因子进行优化时，本发明方法使用了蜘蛛群居算法，该算法模拟群居蜘蛛的协作行为，与大多数群智能算法不同的是，在蜘蛛群算法中搜索个体是按雌、雄性别分为两类的，寻优过程中也依照不同的搜索准则进行。该搜索模式有效避免了个体在优势群体周围的聚集，可快速搜索到迭代步长因子的最优值并保证了该值为全局最优值。

附图说明

图1是本发明的定位方法的流程图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

如图1所示，本发明的基于改进迭代容积粒子滤波算法的定位方法包括以下步骤：

(1)根据粒子集确定机器人的位姿：产生由多个粒子组成的粒子集并初始化，其中表示k时刻第i个粒子对机器人状态均值的估计值，表示表示k时刻第i个粒子对机器人状态协方差的估计值，表示k时刻第i个粒子的权重。每个粒子中的和的初始值可以是人为设定的。

(2)计算状态均值预测值和状态协方差预测值

首先对机器人的位姿进行增维：结合机器人的运动信息uk-1和控制噪声矩阵q对粒子中的状态均值估计值和状态协方差估计值进行增维：

其中a和pa分别表示增维后的机器人状态均值估计值和增维后的状态协方差估计值，pk-1表第i个粒子k-1时刻对机器人状态协方差的估计值。增维后的机器人位姿状态满足高斯分布：n(a，pa)。

然后产生移动机器人状态预测过程所需的2(ns+nu)个状态预测容积点

其中状态预测容积点又可分解为机器人位姿分量和控制分量ns表示机器人位姿的维数，nu表示机器人运动控制信息uk-1的维数，j表示容积点的索引值，且

接下来，根据各状态预测容积点的位姿分量和控制分量计算机器人的状态均值预测值和状态协方差预测值

其中表示预测转换的容积点，f(·)表示机器人运动模型。

(3)多次迭代：如果当前在已知环境中观测到一个或多个路标，针对每个路标对所述粒子集进行m次迭代更新。其中，m的值可以根据需要人为设定，m的值越大，迭代更新次数越多，定位越精确，但计算量也将随之增大。

每次迭代更新的过程大致如下：

(a)对机器人状态均值预测值和状态协方差预测值进行增维

在针对索引值为m的路标进行滤波时，利用该路标的高斯密度函数对机器人的状态均值预测值和状态协方差预测值进行增维：

其中表示增维后的机器人状态均值预测值，表示增维后的状态协方差预测值，nμ表示路标的状态信息维数，nz表示路标真实观测信息的维数。

(b)产生量测容积点

根据增维后的状态均值预测值b的高斯密度n(b，pb)计算量测容积点集

其中量测容积点又可分解为机器人状态分量路标状态分量和量测分量j^*表示测量容积点的索引值，且

根据机器人状态分量路标状态分量和量测分量计算转换量测容积点集

其中h(·)表示观测模型。

再根据得到的转换量测容积点集计算路标量测值的预测值

然后分别计算量测新息，即的协方差机器人状态与量测值之间的互协方差和卡尔曼滤波增益ks：

(c)机器人状态均值估计值的多次更新、迭代步长因子的更新和状态协方差估计值的一次更新：

首先根据步骤(b)求解出的量测新息的协方差机器人状态与量测值之间的互协方差和卡尔曼滤波增益ks对机器人状态均值估计值进行更新：

其中l＝1，...，m表示滤波过程的迭代次数，al表示迭代步长因子，表示观测到的索引值为m的路标的量测值。每次迭代过程中都使用蜘蛛群居算法对迭代步长因子al进行优化更新。假设al，t为第l次迭代滤波中第t次更新的步长因子，其更新方式为al，t＝βal，t直到：

其中，

为的雅克比矩阵，为黑塞矩阵，并且0＜β＜1，0＜γ＜0.5。

在针对当前路标进行m次迭代更新后，完成机器人状态协方差估计值的更新：

当机器人某一时刻同时观测到了多个路标特征，针对当前路标进行m次迭代后得到的经更新的状态均值估计值和状态协方差估计值为针对下一路标进行第一次迭代更新前的状态均值预测值和状态协方差预测值。

(f)根据步骤(e)中经过m次迭代后获取的经更新的机器人状态均值估计值和只在最后一次迭代过程后更新获取的状态协方差估计值进行重要性采样，得到第i个粒子对应的经采样状态均值估计值

重要性采样完成后根据以下式子计算每个粒子重采样后对应的权值

然后对各个权值进行归一化，得到归一化后的权值：

最后根据每个粒子对应的经采样状态均值估计值及归一化后的权值便可以最终确定机器人当前的位姿状态sk：

应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。