[发明专利]基于粒子滤波器的定位恢复方法及机器人设备

说明书

本发明涉及机器人及导航领域，公开了一种基于粒子滤波器的定位恢复方法及机器人设备，用于解决现在技术存在的因全局地图较大、没有采用定位丢失过程中的先验信息等因素造成的失效恢复的用时过长、效率低的问题。本发明的方法包括：定位恢复判断模块，用于计算定位质量，并判断是否启动定位恢复；定位恢复区域计算模块，用于当启动定位恢复时，根据定位质量计算定位恢复区域；定位恢复执行模块，用于粒子滤波器迭代更新，其中粒子的撒点区域包括所述定位恢复区域。本发明适用于机器人导航。

技术领域

本发明涉及机器人及导航领域，特别涉及基于粒子滤波器的定位恢复方法及机器人设备。

背景技术

随着机器人相关技术的快速发展，人们对机器人的需求越来越高，特别是机器人的自主导航功能。自动导航系统首先要载入建图系统生成的栅格地图，并在该地图实现机器人自定位和导航。因此，无论是机器人开始启动自动导航系统或自动导航系统失效后重启，机器人都需要快速的定位到自己的初始位置，以使自动导航系统快速生效。

基于激光的机器人自定位算法通常采用粒子滤波算法，在机器人操作系统ROS(RobotOperating System)中的相应模块为amcl(自适应蒙特卡洛定位)，其中采用Augmented_MCL算法在全局地图上增加随机粒子，能在一定程度上解决失效恢复的问题。

算法流程见附图1，基本的蒙特卡罗定位算法用M个粒子的集合表示置信度bel(x_t)。图1所示的第5行从运动模型采样，以当前置信度为起点使用粒子。运动模型包括速度采样模型和里程计模型。测量模型应用于第6行，以确定粒子的重要性权重。测量模型包括测距仪的波束模型和似然域模型。第10、11行分别计算粒子的短期似然平均和长期似然平均。在重采样过程中，第13行以一定概率进行全局地图的随机采样(撒点)。

然而该方法存在的问题包括：机器人运动过程中，定位往往是逐渐丢失的情况，现有方法没有采用定位丢失过程中的先验信息，仅在全局地图上增加随机粒子，当全局地图较大时，失效恢复的用时过长，效率低。

发明内容

本发明要解决的技术问题是：提供一种基于粒子滤波器的定位恢复方法及机器人设备，用于解决现在技术存在的因全局地图较大、没有采用定位丢失过程中的先验信息等因素造成的失效恢复的用时过长、效率低的问题。

为解决上述问题，本发明采用的技术方案是：基于粒子滤波器的定位恢复方法，包括：

计算定位质量，并判断是否启动定位恢复；

当启动定位恢复时，根据定位质量计算定位恢复区域；

粒子滤波器迭代更新，其中粒子的撒点区域包括所述定位恢复区域。

进一步地，所述计算定位质量并判断是否启动定位恢复的方法，可包括：计算粒子的平均权重，当平均权重小于启动定位恢复阈值时，启动定位恢复。

进一步地，所述计算定位质量并判断是否启动定位恢复的方法，可包括：计算粒子的短期似然平均和长期似然平均，当短期似然平均和长期似然平均之比小于启动定位恢复阈值时，启动定位恢复。

进一步地，所述计算定位恢复区域的方法，可包括：以里程计输出的当前位置为均值，根据粒子的平均权重计算方差和/或粒子数。

进一步地，所述计算定位恢复区域的方法，可包括：以里程计输出的当前位置为均值，根据粒子的短期似然平均和长期似然平均之比计算方差和/或粒子数。

进一步地，为了更准确定位，所述粒子滤波器迭代更新时，粒子的撒点区域可包括原里程计模型计算出的区域、所述定位恢复区域和全局区域。

对应于以上的定位恢复方法，本发明还提供了一种基于粒子滤波器进行定位恢复的机器人设备，包括：

定位恢复判断模块，用于计算定位质量，并判断是否启动定位恢复；