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

摘要

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

主权项

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