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

摘要

本发明公开了一种基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法，包括：(1)利用平方根转换无味卡尔曼滤波器对最优粒子提议分布进行估计，并对机器人位姿状态进行采样；(2)利用平方根容积卡尔曼滤波器更新每个粒子对应的特征地图信息；(3)利用基于相对熵的自适应粒子重采样方法确定当前时刻所需的粒子个数；(4)根据重采样后的粒子集确定机器人的位姿状态和路标特征地图信息。本发明同时从采样粒子的质量和数量两个方面对传统的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)根据重采样后的粒子集确定机器人的位姿状态和路标特征地图信息。