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

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

A relocation method based on local particle filter

The invention discloses a relocation method based on local particle filter, which comprises the following steps: a user manipulates a robot to construct a map in a working environment. Step 2: open the location navigation mode and start repositioning. Step 3: generate random initial pose sets. Step 4: update the particle filter. Step 5: determine whether the output of the particle filter meets the initial location requirement. Step 6: calculate the deviation between actual observation and map. The invention avoids the user directly touching the bottom parts of the robot, reduces the user's operation threshold, and improves the user experience. By adopting the technical scheme, the robot application layer can interact with the bottom module more friendly.

【技术实现步骤摘要】
一种基于局部粒子滤波的重定位方法
本专利技术涉及机器人重定位
，具体涉及一种基于局部粒子滤波的重定位方法。
技术介绍
基于2d激光的机器人自主移动系统通常包括构建地图和定位导航两个部分。构建地图通常由操作人员控制机器人，在该机器人工作环境下行走，使用相应的构图算法获得工作环境下的地图。对于2d激光来说，得到的地图通常为栅格地图，存储为文件，供定位导航系统及后续应用使用。定位导航系统载入构图系统生成的栅格地图，并在该地图中实现机器人自定位和导航。基于激光的机器人自定位算法通常采用粒子滤波算法，在机器人操作系统ROS(RobotOperatingSystem)中的相应模块为amcl。2d激光的初始位姿首先需要用户在栅格地图中指定，包括机器人的位置和朝向信息，然后机器人可以在地图范围内进行自定位和导航。初始位姿的给定需要用户面对栅格地图，需要一定的操作技巧，提高了机器人使用门槛，降低了用户体验。实际应用时，机器人需要将地图中的若干位置进行特殊的标注，例如商场服务机器人需要标注每个店的位置、厕所等位置，以便引导用户至相应的目的地。本专利技术旨在通过这些特定的位置辅助机器人进行初始位姿的给定。从而避免用户直接接触栅格地图等底层部件，并与机器人实际应用进行更好的对接。
技术实现思路
本专利技术所要解决的技术问题是：针对现有的基于2d激光的机器人重定位需要用户在栅格地图上进行，导致操作门槛高，用户体验差的问题，提出了一种基于局部粒子滤波的重定位方法。本专利技术所述标注集合其中P＝(x,y,θ)表示地图中的特定位置，L表示上述位置对应的标注，位姿P1标注为L1，位姿Pi对应标注Li，位姿Pn对应标注Ln，不同的位姿对应不同的标注，例如充电桩、厕所等常用名词。初始该集合为空。中n表示中元素的个数。本专利技术所述粒子位姿集合其中P的定义与上述标注集合中相同。本专利技术所述激光观测scanM是指激光传感器单次扫描采集到的数据，scanM＝{(l1,θ1)...(li,θi)...(lM,θM)}。其中M表示激光传感器单次扫描获得的数据点数目，θi表示第i个数据点的偏转角，li表示第i个数据点的距离。本专利技术采用的技术方案如下：一种基于局部粒子滤波的重定位方法，包括以下步骤：步骤1：用户操作机器人在工作环境下进行地图构建，在构图过程，用户指令当前位姿P1标注为L1，位姿Pi对应标注Li，位姿Pn对应标注Ln，不同的位姿对应不同的标注，形成标注集合将构建完成的地图保存和标注集合保存至本地，供后续步骤使用；步骤2：开启定位导航模式，启动重定位，用户控制机器人至标注集合中Li附近，指令告知机器人靠近Li，机器人检索得到当前大致位姿Pi为(xi,yi,θ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]范围内随机数生成器，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,θ)表示地图中的特定位置，P1为(x1,y1,θ1)，Pi为(xi,yi,θi)，Pn为(xn,yn,θn)，L表示P对应的标注，标注集合位姿P1对应标注L1，位姿Pi对应标注Li，位姿Pn对应标注Ln，1、i、n为不同的序号。步骤4中，以amcl为例，对于每个位姿根据实际激光观测与栅格地图的一致程度更新粒子滤波器，使得粒子位姿趋向于尽可能使实际激光观测与栅格地图一致。步骤5中，粒子滤波器通常输出最终位姿以及所有滤波器样本位姿的协方差矩阵cov：将该矩阵进行特征值分解，得到cov特征值为(λ1,λ2,λ3)，其中λi越小表示粒子滤波器的粒子位姿相似程度越高，得到的位姿也越准确。如果对于λi∈(λ1,λ2,λ3)，满足λi<ε，其中ε为设定阈值，则进行步骤6，否则返回步骤3；步骤6中，对于最终位姿将激光观测scanM＝{(l1,θ1)...(li,θi)...(lM,θM)}投影到栅格地图上，其中M表示激光传感器单次扫描获得的数据点数目，θi表示第i个数据点的偏转角，li表示第i个数据点的距离；对于(li,θi)，得到该点在栅格地图中的坐标为：在栅格地图中查找距离(gx,gy)最近的障碍物坐标(ox,oy)，计算两者之间的距离dist：dist＝sqrt((gx-ox)2+(gy-oy)2)如果dist小于阈值S，则判定该观测点为内点，否则为异常点。最终计算scanM中内点所占的比例，如果该比例大于阈值R，则重定位成功，否则返回步骤3。综上所示，由于采用了上述技术方案，本专利技术的有益效果是：本专利技术避免用户直接接触机器人底层部件，降低用户操作门槛，提高用户体验。采用上述技术方案使得机器人应用层面可以更加友好的与底层模块进行交互。附图说明图1为本专利技术基于局部粒子滤波的重定位方法的流程示意图。具体实施方式本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任意方式组合。如图1所示，一种基于局部粒子滤波的重定位方法，包括：步骤1：用户操作机器人在工作环境下进行地图构建。步骤2：开启定位导航模式，启动重定位。步骤3：生成随机初始位姿集合。步骤4：更新粒子滤波器。步骤5：判定粒子滤波器输出是否满足初始定位需求。步骤6：计算实际观测与地图间的偏差。实施例1：实施例以商用服务机器人为例，特定位置包括前台，厕所等常用地名。基于ros的navigationstack进行实现。步骤1：用户操作机器人在工作环境下，使用gmapping进行地图构建。在构图过程，用户指令当前位姿P标注为L，将cov加入到集合Setlabel。将构建完成的地图保存和标注集合保存至本地，供后续步骤使用。步骤2：开启定位导航模式。打开amcl模块，用户控制机器人至集合中Li附近，指令告知机器人靠近Li，机器人检索得到当前大致位置为(xi,yi,θi)。步骤3：设置粒子范围阈值r，设置粒子数目为m。为空集合。使用下列公式生成粒子位姿：x＝xi+rand(-1,1)×ry＝yi+rand(-1,1)×rθ＝rand(0,1)×2π其中rand(a,b)表示[a,b]范围内随机数生成器。将(x,y,θ)加入到判定n是否大于m，如果是，则进行步骤4，否则继续生成随机初始位姿。步骤4：使用粒子位姿集合初始化amcl，并通过程序向机器人发送机器人原地旋转指令。旋转过程中，粒子滤波器更新粒子状态。对于每个位姿根据实际激光观测与栅格地图的一致程度更新粒子本文档来自技高网...

【技术保护点】
1.一种基于局部粒子滤波的重定位方法，其特征在于，包括以下步骤：步骤1：用户操作机器人在工作环境下进行地图构建，在构图过程，用户指令当前位姿P1标注为L1，位姿Pi对应标注Li，位姿Pn对应标注Ln，不同的位姿对应不同的标注，形成标注集合

【技术特征摘要】
1.一种基于局部粒子滤波的重定位方法，其特征在于，包括以下步骤：步骤1：用户操作机器人在工作环境下进行地图构建，在构图过程，用户指令当前位姿P1标注为L1，位姿Pi对应标注Li，位姿Pn对应标注Ln，不同的位姿对应不同的标注，形成标注集合将构建完成的地图保存和标注集合保存至本地，供后续步骤使用；步骤2：开启定位导航模式，启动重定位，用户控制机器人至标注集合中Li附近，指令告知机器人靠近Li，机器人检索得到当前大致位姿Pi为(xi,yi,θ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：判定粒子滤波器输出...

【专利技术属性】
技术研发人员：张清泉，王军南，余饶东，唐杭生，
申请(专利权)人：浙江大承机器人科技有限公司，
类型：发明
国别省市：浙江,33