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

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

A Location Method Based on Improved Iterative Volume Particle Filtering

The invention discloses a positioning method based on an improved iterative volume particle filter algorithm. The method first generates a particle set composed of multiple particles, then iterates the particle set several times based on observing one or more landmarks in a known environment, and updates the state mean several times in each iteration process, but the state covariance is only at the end of one landmark. After the next iteration, the importance of each particle is sampled based on the updated state mean estimation and state covariance estimation, and the location of the mobile robot is realized. The algorithm uses the principle of volume numerical integration to estimate the non-linear transfer density of Gauss prior, which makes the particle set more concentrated at the tail of the observation likelihood function, overcomes the problem of particle set degradation in the traditional particle filter location algorithm, and uses fewer particles to achieve high-precision and efficient attitude determination and location of mobile robots.

【技术实现步骤摘要】
一种基于改进迭代容积粒子滤波算法的定位方法
本专利技术涉及一种定位方法，尤其涉及一种基于改进迭代容积粒子滤波算法的定位方法。
技术介绍
机器人定位技术是移动机器人利用运动信息和环境观测信息估算自身在环境中所处的位置和姿态，该技术在被广泛应用在路径规划、避障等方面，也是移动机器人自主导航研究中最基本、最重要的问题。其中基于粒子滤波的蒙特卡洛定位(MonteCarloLocalization，MCL)算法是当前学术界研究移动机器人定位问题的主流方法之一，MCL用离散粒子集来近似移动机器人位姿的后验概率分布，适应于任意系统动态、任意噪声分布系统的后验信度估计。在基于粒子滤波的蒙特卡洛定位算法中，粒子提议分布的计算是影响算法性能的关键因素之一。而算法的性能直接影响到定位的准确性和效率。基于粒子滤波的定位算法中的粒子提议分布的计算常常用到卡尔曼滤波器技术。随着卡尔曼滤波器技术的发展，迭代扩展卡尔曼滤波和容积卡尔曼滤波相继被引入到粒子滤波定位算法中的粒子提议分布的计算中。这些算法在一定程度上克服了基于粒子滤波定位方法的粒子集退化问题，但是仍存在一些问题。例如迭代扩展卡尔曼滤波方法在处理非线性问题本文档来自技高网...

【技术保护点】
1.一种基于改进迭代容积粒子滤波算法的定位方法，其特征在于，包括下列步骤：(1)产生由多个粒子组成的粒子集并初始化，每一粒子包含如下参数：移动机器人的状态均值估计值、状态协方差估计值和该粒子的权重；(2)基于先前的状态均值估计值和状态协方差估计值、运动信息、噪声控制信息及机器人运动模型来计算当前的状态均值预测值和状态协方差预测值；(3)如果当前在已知环境中观测到一个或多个路标，针对每个路标对所述粒子集进行M次迭代更新，M为设定的整数；每次迭代更新的过程包括：(3.1)基于路标的高斯密度函数、当前的状态均值预测值和状态协方差预测值来计算量测容积点集；(3.2)将量测容积点集分解为机器人状态分量、...

【技术特征摘要】
1.一种基于改进迭代容积粒子滤波算法的定位方法，其特征在于，包括下列步骤：(1)产生由多个粒子组成的粒子集并初始化，每一粒子包含如下参数：移动机器人的状态均值估计值、状态协方差估计值和该粒子的权重；(2)基于先前的状态均值估计值和状态协方差估计值、运动信息、噪声控制信息及机器人运动模型来计算当前的状态均值预测值和状态协方差预测值；(3)如果当前在已知环境中观测到一个或多个路标，针对每个路标对所述粒子集进行M次迭代更新，M为设定的整数；每次迭代更新的过程包括：(3.1)基于路标的高斯密度函数、当前的状态均值预测值和状态协方差预测值来计算量测容积点集；(3.2)将量测容积点集分解为机器人状态分量、路标状态分量和量测分量；(3.3)基于机器人状态分量、路标状态分量、量测分量和已知的观测模型来计算转换量测容积点集；(3.4)基于所述转换量测容积点集计算路标量测值的预测值；(3.5)基于所述转换量测容积点集、路标量测值的预测值、机器人状态分量和当前的状态均值预测值来计算量测新息的协方差、机器人状态和量测值之间的互协方差以及卡尔曼滤波参数；(3.6)基于量测新息的协方差、机器人状态和量测值之间的互协方差、卡尔曼滤波参数以及已知的路标的量测值完成对当前的状态均值估计值的一次更新，当前的状态协方差估计值保持不变，将更新后的当前的状态均值估计值作为下一次迭代所需的当前的状态均值预测值，将当前的状态协方差估计值作为下一次迭代所需的状态协方差预测值；基于对当前路标进行的M次迭代更新后的机器人状态和量测值之间的互协方差、卡尔曼滤波参数和状态协方差预测值来更新每个粒子中的状态协方差估计值；当某一时刻观测到多个路标时，针对当前路标进行M次迭代后最终得到的经更新的状态均值估计值和状态协方差估计值为针对下一路标进行第一次迭代更新前的状态均值预测值和状态协方差预测值；(4)基于步骤(3)中最终得到的经更新的状态均值估计值和状态协方差估计值来完成每个粒子的重要性采样，并基于采样值来更新每个粒子的权值，以实现所述移动机器人的定位。2.根据权利要求1所述的基于改进迭代容积粒子滤波算法的定位方法，其特征在于，步骤(2)包括如下步骤：(2.1)基于运动信息uk-1和噪声控制信息Q对先前的状态均值估计值和状态协方差估计值进行增维：其中，和Pk-1分别表示第i个粒子中先前的状态均值估计值和状态协方差估计值，a和Pa分别表示增维后的机器人状态均值估计值和增维后的状态协方差估计值，增维后的机器人位姿状态满足高斯分布：N(a，Pa)；(2.2)基于增维后的状态均值估计值和状态协方差估计值产生包含2(ns+nu)个容积点的状态预测容积点集其中，ns表示机器人位姿的维数，nu表示机器人运动控制信息uk-1的维数，j表示容积点的索引值，且(2.3)基于所述状态预测容积点集和机器人运动模型计算移动机器人当前的...

【专利技术属性】
技术研发人员：陈熙源，柳笛，方文辉，刘晓，马振，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32