[发明专利]一种基于局部粒子滤波的重定位方法

摘要

本发明公开了一种基于局部粒子滤波的重定位方法，包括：步骤1：用户操作机器人在工作环境下进行地图构建。步骤2：开启定位导航模式，启动重定位。步骤3：生成随机初始位姿集合。步骤4：更新粒子滤波器。步骤5：判定粒子滤波器输出是否满足初始定位需求。步骤6：计算实际观测与地图间的偏差。本发明避免用户直接接触机器人底层部件，降低用户操作门槛，提高用户体验，采用上述技术方案使得机器人应用层面可以更加友好的与底层模块进行交互。

主权项

1.一种基于局部粒子滤波的重定位方法，其特征在于，包括以下步骤：步骤1：用户操作机器人在工作环境下进行地图构建，在构图过程，用户指令当前位姿P₁标注为L₁，位姿P_i对应标注L_i，位姿P_n对应标注L_n，不同的位姿对应不同的标注，形成标注集合将构建完成的地图保存和标注集合保存至本地，供后续步骤使用；步骤2：开启定位导航模式，启动重定位，用户控制机器人至标注集合中L_i附近，指令告知机器人靠近L_i，机器人检索得到当前大致位姿P_i为(x_i,y_i,θ_i)；步骤3：生成随机初始位姿，设置粒子范围阈值r，设置粒子数目为m，粒子位姿集合开始为空集合；使用下列公式生成粒子位姿P(x,y,θ)：x＝xi+rand(‑1,1)×ry＝yi+rand(‑1,1)×rθ＝rand(0,1)×2π其中，rand(a,b)表示[a,b]范围内随机数生成器，将P(x,y,θ)加入到得到粒子位姿集合判定中粒子位姿个数n是否大于m，如果是，则进行步骤4，否则继续生成随机初始位姿；步骤4：使用粒子位姿集合初始化粒子滤波器，并通过程序向机器人发送机器人原地旋转指令，旋转过程中，粒子滤波器更新粒子状态；步骤5：判定粒子滤波器输出是否满足初始定位需求，若满足初始定位需求进入步骤6，否则返回步骤3；步骤6：计算实际观测与地图间的偏差，偏差符合要求，重定位成功，否则返回步骤3。