[发明专利]基于粒子自收敛的移动机器人重定位方法

权利要求书

1.一种基于粒子自收敛的移动机器人重定位方法，其特征在于，所述方法具体包括如下步骤：

S1、基于栅格地图生成格网粒子集合set_a，格网粒子集合set_a由格网粒子组成，格网粒子即为各格网点p_i(x_i,y_i)的最优位姿；

S2、更新格网粒子集合set_a中的粒子及对应的打分；

S3、对格网粒子集合set_a中的粒子进行重采样，将重采样的粒子复制至新的粒子集合set_b，粒子集合set_b中的粒子权重相等；

S4、计算粒子集合set_b中所有粒子的位姿标准差，检测是否所有粒子的位姿标准差都小于预设的阈值，若检测结果为 是，将粒子集合中set_b所有粒子的平均位姿作为机器人的当前重定位的位姿，若检测结果为否，则执行步骤S2；

格网点p_i(x_i,y_i)最优位姿的获取方法具体如下：

从0度到360度，每个设定的角度步进Δθ取一个角度θ_j，将角度θ_j与格网点p_i(x_i,y_i)组成一个位姿p_i,j(x_i,y_i,θ_j)；

利用似然域模型对位姿p_i,j(x_i,y_i,θ_j)进行打分，遍历格网点p_i(x_i,y_i)所有的角度，最高打分位姿即为格网点p_i(x_i,y_i)的最优位姿；

格网粒子集合set_a中的粒子更新方法具体包括如下步骤：

S21、格网粒子集合set_a中顺序选择一个粒子p_i(x_i,y_i,θ_i)，并在粒子p_i(x_i,y_i,θ_i)周围生成k个高斯随机位姿；

S22、利用似然域模型计算粒子p_i(x_i,y_i,θ_i)及k个随机位姿集合的得分，得到最高得分w_max和对应的位姿p_max，令当前位姿等于得分最高的位姿p_i＝p_max，当前位姿的权重等于最高得分，即w_i＝w_max；

S23、遍历粒子格网粒子集合set_a中所有的粒子，完成格网粒子集合set_a中的更新与打分。

2.如权利要求1所述基于粒子自收敛的移动机器人重定位方法，其特征在于，格网粒子集合set_a的生成方法具体包括如下步骤：

S11、在栅格地图上每隔设定距离Δd取一个格网点p_i(x_i,y_i)；

S12、检测取格网点所在的栅格是否被障碍物占据；

S13、若检测结果为否，则获取格网点p_i(x_i,y_i)的最优位姿，若检测结果为是，则基于设定距离Δd在栅格地图上取下一个格网点，并执行步骤S12；

S14、遍历栅格地图上的所有格网点，输出格网粒子集合set_a。

3.如权利要求1所述基于粒子自收敛的移动机器人重定位方法，其特征在于，粒子重采样过程具体如下：

S31、对格网粒子集合中的粒子权重进行降序排列；

S32、基于粒子复制比率计算待复制粒子的数量copy_count，设定粒子复制比率为r，当前格网粒子集合中的粒子数set_count，计算得到待复制粒子的数copy_count＝r*set_count；

S33、构建空的粒子集合set_b，将排名靠前的copy_count个粒子复制到粒子集合set_b，并将粒子集合set_b中的所有粒子权重设为1/copy_count；

S34、将粒子集合set_b替代格网粒子集合set_a。