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

权利要求书

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＝x_i+rand(-1,1)×r

y＝y_i+rand(-1,1)×r

θ＝rand(0,1)×2π

其中，rand(a,b)表示[a,b]范围内随机数生成器，将P(x,y,θ)加入到得到粒子位姿集合判定中粒子位姿个数n是否大于m，如果是，则进行步骤4，否则继续生成随机初始位姿；

步骤4：使用粒子位姿集合初始化粒子滤波器，并通过程序向机器人发送机器人原地旋转指令，旋转过程中，粒子滤波器更新粒子状态；

步骤5：判定粒子滤波器输出是否满足初始定位需求，若满足初始定位需求进入步骤6，否则返回步骤3；

粒子滤波器通常输出最终位姿以及所有滤波器样本位姿的协方差矩阵cov：

将该矩阵进行特征值分解，得到cov特征值为(λ₁,λ₂,λ₃)，如果对于λ_i∈(λ₁,λ₂,λ₃)，满足λ_i<ε，其中ε为设定阈值，则进行步骤6，否则返回步骤3；

步骤6：计算实际观测与地图间的偏差，偏差符合要求，重定位成功，否则返回步骤3。

2.根据权利要求1所述的基于局部粒子滤波的重定位方法，其特征在于，步骤6中，对于最终位姿将激光观测scan^M＝{(l₁,θ₁)...(l_i,θ_i)...(l_M,θ_M)}投影到栅格地图上，其中，θ_i表示第i个数据点的偏转角，l_i表示第i个数据点的距离；

对于(l_i,θ_i)，得到该点在栅格地图中的坐标为：

在栅格地图中查找距离(gx,gy)最近的障碍物坐标(ox,oy)，计算两者之间的距离dist：

dist＝sqrt((gx-ox)²+(gy-oy)²)

如果dist小于阈值S，则判定该观测点为内点，否则为异常点，最终计算scan^M中内点所占的比例，如果该比例大于阈值R，则重定位成功，否则返回步骤3。