CN108507579B - 一种基于局部粒子滤波的重定位方法 - Google Patents

Abstract

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

Description

一种基于局部粒子滤波的重定位方法

技术领域

本发明涉及机器人重定位技术领域，具体涉及一种基于局部粒子滤波的重定位方法。

背景技术

基于2d激光的机器人自主移动系统通常包括构建地图和定位导航两个部分。构建地图通常由操作人员控制机器人，在该机器人工作环境下行走，使用相应的构图算法获得工作环境下的地图。对于2d激光来说，得到的地图通常为栅格地图，存储为文件，供定位导航系统及后续应用使用。

定位导航系统载入构图系统生成的栅格地图，并在该地图中实现机器人自定位和导航。基于激光的机器人自定位算法通常采用粒子滤波算法，在机器人操作系统ROS(RobotOperating System)中的相应模块为amcl。2d激光的初始位姿首先需要用户在栅格地图中指定，包括机器人的位置和朝向信息，然后机器人可以在地图范围内进行自定位和导航。初始位姿的给定需要用户面对栅格地图，需要一定的操作技巧，提高了机器人使用门槛，降低了用户体验。

实际应用时，机器人需要将地图中的若干位置进行特殊的标注，例如商场服务机器人需要标注每个店的位置、厕所等位置，以便引导用户至相应的目的地。本发明旨在通过这些特定的位置辅助机器人进行初始位姿的给定。从而避免用户直接接触栅格地图等底层部件，并与机器人实际应用进行更好的对接。

发明内容

本发明所要解决的技术问题是：针对现有的基于2d激光的机器人重定位需要用户在栅格地图上进行，导致操作门槛高，用户体验差的问题，提出了一种基于局部粒子滤波的重定位方法。

本发明所述标注集合

其中P＝(x,y,θ)表示地图中的特定位置，L表示上述位置对应的标注，位姿P₁标注为L₁，位姿P_i对应标注L_i，位姿P_n对应标注L_n，不同的位姿对应不同的标注，例如充电桩、厕所等常用名词。初始该集合为空。

中n表示

中元素的个数。

本发明所述粒子位姿集合

其中P的定义与上述标注集合

中相同。

本发明所述激光观测scan^M是指激光传感器单次扫描采集到的数据，scan^M＝{(l₁,θ₁)...(l_i,θ_i)...(l_M,θ_M)}。其中M表示激光传感器单次扫描获得的数据点数目，θ_i表示第i个数据点的偏转角，l_i表示第i个数据点的距离。

本发明采用的技术方案如下：

一种基于局部粒子滤波的重定位方法，包括以下步骤：

步骤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]范围内随机数生成器，rand(-1,1)表示[-1,1]范围内随机数生成器，rand(0,1)表示[0,1]范围内随机数生成器，将P(x,y,θ)加入到

得到粒子位姿集合

判定

中粒子位姿个数n是否大于m，如果是，则进行步骤4，否则继续生成随机初始位姿；

步骤4：使用粒子位姿集合

初始化粒子滤波器，并通过程序向机器人发送机器人原地旋转指令，旋转过程中，粒子滤波器更新粒子状态；

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

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

步骤7：开始自主定位和导航。

步骤1中，P＝(x,y,θ)表示地图中的特定位置，P₁为(x₁,y₁,θ₁)，P_i为(x_i,y_i,θ_i)，P_n为(x_n,y_n,θ_n)，L表示P对应的标注，标注集合

位姿P₁对应标注L₁，位姿P_i对应标注L_i，位姿P_n对应标注L_n，1、i、n为不同的序号。

步骤4中，以amcl为例，对于每个位姿

根据实际激光观测与栅格地图的一致程度更新粒子滤波器，使得粒子位姿趋向于尽可能使实际激光观测与栅格地图一致。

步骤5中，粒子滤波器通常输出最终位姿

以及所有滤波器样本位姿的协方差矩阵cov：

将该矩阵进行特征值分解，得到cov特征值为(λ₁,λ₂,λ₃)，其中λ_i越小表示粒子滤波器的粒子位姿相似程度越高，得到的位姿也越准确。如果对于λ_i∈(λ₁,λ₂,λ₃)，满足λ_i<ε，其中ε为设定阈值，则进行步骤6，否则返回步骤3；

步骤6中，对于最终位姿

将激光观测scan^M＝{(l₁,θ₁)...(l_i,θ_i)...(l_M,θ_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。

综上所示，由于采用了上述技术方案，本发明的有益效果是：

本发明避免用户直接接触机器人底层部件，降低用户操作门槛，提高用户体验。采用上述技术方案使得机器人应用层面可以更加友好的与底层模块进行交互。

附图说明

图1为本发明基于局部粒子滤波的重定位方法的流程示意图。

具体实施方式

本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任意方式组合。

如图1所示，一种基于局部粒子滤波的重定位方法，包括：

步骤1：用户操作机器人在工作环境下进行地图构建。

步骤2：开启定位导航模式，启动重定位。

步骤3：生成随机初始位姿集合。

步骤4：更新粒子滤波器。

步骤5：判定粒子滤波器输出是否满足初始定位需求。

步骤6：计算实际观测与地图间的偏差。

实施例1：

实施例以商用服务机器人为例，特定位置包括前台，厕所等常用地名。基于ros的navigation stack进行实现。

步骤1：用户操作机器人在工作环境下，使用gmapping进行地图构建。在构图过程，用户指令当前位姿P标注为L，将cov加入到集合Set_label。将构建完成的地图保存和标注集合保存至本地，供后续步骤使用。

步骤2：开启定位导航模式。打开amcl模块，用户控制机器人至集合

中L_i附近，指令告知机器人靠近L_i，机器人检索

得到当前大致位置为(x_i,y_i,θ_i)。

步骤3：设置粒子范围阈值r，设置粒子数目为m。

为空集合。使用下列公式生成粒子位姿：

x＝x_i+rand(-1,1)×r

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

θ＝rand(0,1)×2π

其中rand(a,b)表示[a,b]范围内随机数生成器。将(x,y,θ)加入到

判定n是否大于m，如果是，则进行步骤4，否则继续生成随机初始位姿。

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

根据实际激光观测与栅格地图的一致程度更新粒子滤波器，使得粒子位姿趋向于尽可能使实际激光观测与栅格地图一致。

步骤5：判定粒子滤波器输出是否满足初始定位需求。

粒子滤波器通常输出最终位姿

以及所有滤波器样本位姿的协方差矩阵cov：

将该矩阵进行特征值分解，得到cov特征值为(λ₁,λ₂,λ₃)，其中λ_i越小表示粒子滤波器的粒子位姿相似程度越高，得到的位姿也越准确。如果对于λ_i∈(λ₁,λ₂,λ₃)，满足λ_i<ε，其中ε为设定阈值，则进行步骤6，否则返回步骤3。

步骤6：计算实际观测与地图间的偏差。

对于最终位姿

将激光观测scan^M＝{(l₁,θ₁)...(l_i,θ_i)...(l_M,θ_M)}投影到栅格地图上。对于(l_i,θ_i)，得到该点在栅格地图中的坐标为：

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

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

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

步骤7：开始自主定位和导航。

Claims (2)

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。

Images