[发明专利]基于改进重采样方法和粒子选取的FastSLAM算法

权利要求书

1.一种基于改进重采样方法和粒子选取的FastSLAM算法，包括以下步骤：

1)机器人根据输入控制数据，依据机器人运动预测模型进行机器人位姿预测，根据机器人外部传感器的测量模型结合测量数据进行机器人位姿和路标位置更新；

2)机器人位姿预测采用粒子滤波器进行计算，根据有效粒子数，粒子权重协方差和粒子测量残余一致性来进行粒子重采样判定标准的修正；

所述的粒子权重协方差的分布w_cov按照式(1)进行计算：

M表示粒子的总数，w_i表示粒子i的权重，T表示矩阵转置操作；

所述的粒子测量残余一致性按照式(2)进行计算：

表示根据测量模型使用扩展卡尔曼滤波器所获得的路标i和粒子m中的机器人位姿之间的相对位置数据， 为机器人外部传感器所检测到的路标i和机器人位姿之间的测量数据；residule_cov_t，m表示测量残余数据的协方差，其计算方法如式(3)所示：

H_t，m，i为测量模型对路标i位置向量的雅可比矩阵， 为粒子m中路标i在上一时刻被检测到时所获得的位置协方差矩阵，T表示矩阵转置操作，Q_t表示测量模型的噪声分布协方差；

3)在重采样条件满足以下情况时，使用指数等级方法和交叉算子进行新粒子的生成：使用当前粒子有效数，粒子权重协方差，以及粒子测量残余一致性综合进行判断，当有效粒子数小于总粒子数的75％，粒子权重协方差大于粒子权重均值，并且粒子测量残余一致性在95％的一致性区域之外； 

所述的指数等级方法和交叉算子包括：首先根据粒子权重对粒子排序，再使用指数方法对粒子权重进行调整，之后使用随机数生成方法选择两个父粒子，最后依据父粒子和交叉算子进行新粒子的生成；

4)依据生成的新粒子进行机器人的定位和地图创建。