[发明专利]基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法

权利要求书

1.一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，包括以下步骤：

(1)将运动噪声和测量噪声同时增广至机器人位姿状态向量中，利用平方根转换无味卡尔曼滤波器对最优粒子提议分布进行估计，并根据该粒子提议分布对机器人位姿状态进行采样，具体过程为：

(1-1)将k时刻的机器人位姿和路标特征地图的联合后验概率表示为如下N_k个粒子组成的粒子集：

其中，i表示粒子序号，表示k时刻第i个粒子的权重，和分别表示k时刻第i个机器人位姿状态的估计值及其协方差平方根因子；和分别表示k时刻第i个粒子中关于第m个路标特征路标的位置均值和相应的协方差平方根因子；

(1-2)将运动噪声和测量噪声信息同时增广至机器人位姿状态向量及其协方差矩阵的平方根因子中，保证协方差的半正定性；

(1-3)根据机器人运动函数、增广后的机器人位姿状态向量均值及其协方差矩阵的平方根因子，计算机器人位姿状态的预测均值及其协方差平方根因子，具体过程为：

(1-3-1)根据增广后的机器人位姿状态向量均值和协方差矩阵的平方根因子，计算转换无味Sigma点集：

其中，和分别为增广后的机器人位姿状态向量均值和协方差矩阵的平方根因子，表示增广状态向量的维度，表示第j个转换无味Sigma点，每个转换无味Sigma点包含了机器人位姿状态分量运动噪声分量和测量噪声分量为n_a维向量，其元素由下式确定：

其中，r表示向量中的元素序号，表示取下整运算符，当n_a为奇数时，最后一个元素为

(1-3-2)根据机器人运动函数生成机器人位姿状态的预测转换无味Sigma点：

其中，f(·)表示给定的机器人运动函数，u_k表示当前时刻的控制输入；

(1-3-3)根据预测的转换无味Sigma点计算机器人位姿状态的预测均值及其协方差平方根因子：

其中，qr(·)表示矩阵的QR分解运算，Π_v，k|k-1定义为以下加权中心偏差矩阵：

(1-4)当第l个路标特征被机器人再次成功观测到时，根据路标特征测量值z_k，计算机器人位姿状态后验估计均值及其协方差矩阵平方根因子；

(1-5)计算粒子的提议分布，并根据此分布采样生成新一代粒子；

(2)利用平方根容积卡尔曼滤波器更新每个粒子对应的路标特征地图信息；

(3)在粒子重采样阶段，利用基于相对熵的自适应粒子重采样方法确定当前时刻所需的粒子个数；

(4)根据重采样后的粒子集确定机器人的位姿状态和路标特征地图信息。

2.如权利要求1所述基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，其特征在于：步骤(1-4)的具体步骤为：

(1-4-1)当第l个路标特征被机器人再次成功观测到时，根据测量函数h(·)生成测量值Sigma点：

其中，表示第l个路标特征在k-1时刻的位置状态估计值；h(·)表示给定的机器人观测函数；

(1-4-2)根据测量值Sigma点计算预测测量值均值：

(1-4-3)利用QR分解，计算预测测量值均值的协方差矩阵的平方根因子：

其中，Π_z，k|k-1定义为以下加权中心偏差矩阵：

(1-4-4)计算机器人位姿状态的交叉协方差矩阵和卡尔曼增益：

(1-4-5)根据路标特征的测量值z_k，计算机器人位姿状态后验估计均值及其协方差矩阵平方根因子：

其中，cholupdate(·)函数表示矩阵的Cholesky更新分解运算，‘-’表示对参数先进行矩阵减法运算。