基于粒子滤波器的定位恢复方法及机器人设备与流程

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

背景技术：

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

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

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

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

技术实现要素：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

具体的，定位恢复判断模块判断需要启动定位恢复的情形可包括：

粒子的平均权重小于启动定位恢复阈值时，或者，粒子的短期似然平均和长期似然平均之比小于启动定位恢复阈值时。

具体的，定位恢复区域计算模块计算定位恢复区域的方法可以为：

以里程计输出的当前位置为均值，根据粒子的平均权重计算方差和/或粒子数；或者，

以里程计输出的当前位置为均值，根据粒子的短期似然平均和长期似然平均之比计算方差和/或粒子数。

具体的，为了更准确定位，粒子的撒点区域具体包括：原里程计模型计算出的区域、所述定位恢复区域以及全局区域。

本发明的有益效果是：机器人在运动过程中，采用定位丢失过程中的先验信息，增加定位恢复区域的准确性，减少失效恢复的用时，提高失效恢复的效率，或者说增加粒子更新过程中撒点的准确性，提高粒子滤波器抗丢失的鲁棒性。

附图说明

图1示出了现有技术的augmented_mcl算法的流程图。

图2示出了本发明一种基于粒子滤波器的快速初始定位方法的实施例一的流程图。

具体实施方式

下面将结合本发明实施例的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一

图2示出了本发明一种基于粒子滤波器的定位恢复方法的实施例一的流程图，包括，

步骤1、计算定位质量，并判断是否启动定位恢复。

具体实现时，可以有多种方法来表示定位质量，比如用所有粒子的平均权重，或粒子的短期似然平均和长期似然平均等等。

以粒子的平均权重为例，计算粒子的平均权重，当平均权重小于启动定位恢复阈值时，启动定位恢复。平均权重越小，说明粒子离真实位置越远，因此最简单的做法是当平均权重小于某个值时(定义为启动定位恢复阈值)，就应该启动定位恢复操作。注意：本发明中说的定位恢复，并不代表定位就真正失效了，在定位有一定偏差时，也可以启动本算法，即进行定位恢复操作，这在一定程度上可以预防机器人的定位丢失。

启动定位恢复阈值的具体取值，根据传感器的精度和环境而定。

以粒子的似然平均为例，计算粒子的短期似然平均和长期似然平均，当短期似然平均和长期似然平均之比小于启动定位恢复阈值时，启动定位恢复。设计原理与粒子的平均权重类似。

步骤2、当启动定位恢复时，计算定位恢复区域。

机器人在运动过程中，定位往往是逐渐丢失的。通过步骤1计算定位质量，我们能大概估计出机器人是什么时候开始定位变差、直到定位丢失的。因此我们可以借助定位质量给出的先验知识，帮助机器人在粒子更新时，把粒子分布在更可能包含机器人真实位置的区域。而不是等机器人定位丢失后，在全局地图进行撒点。因此，根据定位质量，计算定位恢复区域，是关键的步骤。

根据定位质量的不同表征，计算定位恢复区域的方法也有所不同。

以粒子的平均权重为例，计算定位恢复区域的方法，包括：以里程计输出的当前位置为均值，根据粒子的平均权重计算方差和/或粒子数。原则为粒子的平均权重越小，方差越大，粒子数越多。比如为：(ωavg为平均权重，αfix为固定方差)。注意：这里方差包括位置的方差和角度的方差。

以粒子的似然平均为例，计算定位恢复区域的方法，包括：以里程计输出的当前位置为均值，根据粒子的短期似然平均和长期似然平均之比计算方差和/或粒子数。原则为上述比值越小，方差越大，粒子数越多。具体设计方法与粒子的平均权重类似。

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

该算法可以与附图1中的原始算法进行融合，即撒点时，粒子的撒点区域包括原里程计模型计算出的区域和/或所述定位恢复区域和/或全局区域。

根据本发明实施例提供的方法，机器人在运动过程中，采用定位丢失过程中的先验信息，增加定位恢复区域的准确性，减少失效恢复的用时，提高失效恢复的效率，或者说增加粒子更新过程中撒点的准确性，提高粒子滤波器抗丢失的鲁棒性。

实施例二

实施例二提供了一种基于粒子滤波器进行定位恢复的机器人设备，包括：

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

可以有多种方法来表示定位质量，比如用所有粒子的平均权重，或粒子的短期似然平均和长期似然平均等等。

以粒子的平均权重为例，计算粒子的平均权重，当平均权重小于启动定位恢复阈值时，启动定位恢复。平均权重越小，说明粒子离真实位置越远，因此最简单的做法是当平均权重小于某个值时(定义为启动定位恢复阈值)，就应该启动定位恢复操作。启动定位恢复阈值的具体取值，根据传感器的精度和环境而定。

以粒子的似然平均为例，计算粒子的短期似然平均和长期似然平均，当短期似然平均和长期似然平均之比小于启动定位恢复阈值时，启动定位恢复。设计原理与粒子的平均权重类似。

定位恢复区域计算模块，用于当启动定位恢复时，计算定位恢复区域。

根据定位质量的不同表征，计算定位恢复区域的方法也有所不同。

以粒子的平均权重为例，计算定位恢复区域的方法，包括：以里程计输出的当前位置为均值，根据粒子的平均权重计算方差和/或粒子数。原则为粒子的平均权重越小，方差越大，粒子数越多。比如为：(ωavg为平均权重，αfix为固定方差)。注意：这里方差包括位置的方差和角度的方差。

以粒子的似然平均为例，计算定位恢复区域的方法，包括：以里程计输出的当前位置为均值，根据粒子的短期似然平均和长期似然平均之比计算方差和/或粒子数。原则为上述比值越小，方差越大，粒子数越多。具体设计方法与粒子的平均权重类似。

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

该算法可以与附图1中的原始算法进行融合，即撒点时，粒子的撒点区域包括原里程计模型计算出的区域和/或所述定位恢复区域和/或全局区域。

需要说明的是，本实施例中的各模块(或单元)是逻辑意义上的，具体实现时，多个模块(或单元)可以合并成一个模块(或单元)，一个模块(或单元)也可以拆分成多个模块(或单元)。

根据本发明实施例提供的机器人设备，机器人在运动过程中，采用定位丢失过程中的先验信息，增加定位恢复区域的准确性，减少失效恢复的用时，提高失效恢复的效率，或者说增加粒子更新过程中撒点的准确性，提高粒子滤波器抗丢失的鲁棒性。

本领域普通技术人员可以理解，实现上述实施例方法中的全部或部分流程是可以通过程序来指令相关的硬件来完成，所述的程序可以存储于计算机可读取存储介质中，该程序在执行时，可包括如上各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(read-onlymemory,rom)或随机存储记忆体(randomaccessmemory,ram)等。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均应包括在本发明的保护范围之内。