[发明专利]一种基于改进迭代容积粒子滤波算法的定位方法

说明书

本发明公开了一种基于改进迭代容积粒子滤波算法的定位方法，该方法首先产生由多个粒子组成的粒子集，然后基于在已知环境中观测到一个或多个路标来对粒子集进行多次迭代更新，每次迭代过程进行多次状态均值的更新，但状态协方差只在对一个路标的最后一次迭代后更新一次，最后基于得到的经更新的状态均值估计值和状态协方差估计值来完成每个粒子的重要性采样，并实现所述移动机器人的定位。该算法利用容积数值积分原则估计出高斯先验的非线性转移密度，使得粒子集较为集中的分布于观测似然函数的尾部，克服了传统粒子滤波定位算法中粒子集退化的问题，使用较少的粒子即可实现对移动机器人高精度高效率的定姿、定位。

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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