基于粒子滤波器的快速初始定位方法及机器人设备与流程

本发明涉及机器人及导航技术领域，尤其涉及基于粒子滤波器的快速初始定位方法及机器人设备。

背景技术：

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

基于激光的机器人自定位算法通常采用粒子滤波算法，在机器人操作系统ros(robotoperatingsystem)中的相应模块为amcl(自适应蒙特卡洛定位)。2d激光的初始位姿首先需要用户在栅格地图中指定(通常是一个点)，包括机器人的位置和朝向信息，然后机器人可以在地图范围内进行自定位和导航。该方法的问题有：

没有为栅格地图分区，用户指定初始位置不方便；

用户指定的初始位置与真实位置的差距无法感知，因此无论用户指定的初始位置在何处，粒子滤波器的初始参数(粒子数、初始分布方差)都不会变化，同时amcl需要在机器人运动模型中进行采样和重采样，因此初始位置是根据粒子滤波器的初始参数迭代一次进行采样得到的。初始参数的不准以及没有重采样，造成初始位置估计不是特别准确。

现有技术中，cn201810307437.8(一种基于局部粒子滤波的重定位方法)提供了一种基于局部粒子滤波的重定位方法，该方法对地图中的一些位置进行了标注，机器人只能在这些标注位置进行初始定位，同时需要控制机器人在原地旋转最终得到初始位置估计。

综上所述，现有技术存在因用户指定初始位置不方便、初始化位置受限制、粒子滤波器初始化参数不准确、初始定位需要机器人运动等因素造成的初始位置不准确的问题。

技术实现要素：

本发明提出了基于粒子滤波器的快速初始定位方法及机器人设备，用于解决现在技术存在的因用户指定初始位置不方便、初始化位置受限制、粒子滤波器初始化参数不准确、初始定位需要机器人运动等因素造成的初始位置不准确的问题。

本发明通过以下技术方案来实现上述目的：

基于粒子滤波器的快速初始定位方法，包括以下步骤：

步骤1：建立栅格地图；

步骤2：建立栅格地图分区；

步骤3：为机器人指定初始位置区域，可以为一个或多个栅格地图分区；

步骤4：机器人根据初始位置区域初始化粒子滤波器参数；

步骤5：粒子滤波器迭代更新，包括采样粒子产生过程和重要性采样过程；当达到迭代终止条件时，停止迭代更新，并输出机器人的初始位置信息。

进一步地，所述建立栅格地图的方法，包括：基于激光或视觉传感器，机器

人自主移动或由用户控制机器人在工作环境中进行地图构建。

进一步地，所述建立栅格地图分区的方法，包括：程序自动建立均匀分区，如正方形区域，支持用户指定正方形区域边长，程序计算各分区的分布均值和方差。

进一步地，所述建立栅格地图分区的方法，包括：用户基于栅格地图划定分区，如根据栅格地图中的重要地标进行分区，程序计算各分区的分布均值和方差。

进一步地，机器人根据初始位置区域初始化粒子滤波器参数，包括：

根据初始位置区域的分布均值和方差得到粒子滤波器的粒子初始分布。

根据初始位置区域的方差得到粒子滤波器的粒子数，原则为方差越大，粒子数越多；或由用户指定得到粒子滤波器的粒子数。

进一步地，所述采样粒子产生过程，具体为：根据粒子初始分布或重要性采样过程得到的粒子分布，产生新的采样粒子。

进一步地，所述重要性采样过程，具体为：根据采样粒子位置的观测及机器人真实位置的观测，计算采样粒子的权值，并进行低方差重采样，等到更新的粒子分布。

进一步地，所述迭代终止条件，至少包括达到迭代次数或粒子分布的方差小

于一定阈值。

本发明实施例提供的另一种基于粒子滤波器的快速初始定位的机器人设备，包括：

栅格地图建立模块，用于建立栅格地图；

栅格地图分区模块，用于建立栅格地图分区；

初始位置区域指定模块，用于为机器人指定初始位置区域，可以为一个或多个栅格地图分区；

粒子滤波器初始化模块，用于机器人根据初始位置区域初始化粒子滤波器参数；

粒子滤波器执行模块，用于粒子滤波器迭代更新，包括采样粒子产生过程和重要性采样过程；当达到迭代终止条件时，停止迭代更新，并输出机器人的初始位置信息。

本发明的有益效果在于：

本发明的定位方法使得机器人能在任意位置、不需运动的情况下，由用户方便快捷的指定初始位置、并获取相对准确的粒子滤波器初始化参数，经过静止状态下的粒子滤波器采样、重采样，得到更为准确的初始位置估计。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要实用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本实施例的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

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

图2示出了本发明一种基于粒子滤波器的快速初始定位的机器人设备的结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将对本发明的技术方案进行详细的描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所得到的所有其它实施方式，都属于本发明所保护的范围。

实施例一

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

步骤1、建立栅格地图。

建立栅格地图的方法，包括：基于激光或视觉传感器，机器人自主移动或由用户控制机器人在工作环境中进行地图构建。

步骤2、建立栅格地图分区。

建立栅格地图分区的方法，包括：程序自动建立均匀分区，如正方形区域，支持用户指定正方形区域边长，程序计算各分区的分布均值和方差。

建立栅格地图分区的方法，还包括：用户基于栅格地图划定分区，如根据栅格地图中的重要地标进行分区，程序计算各分区的分布均值和方差。

步骤3、为机器人指定初始位置区域，可以为一个或多个栅格地图分区；

步骤4、机器人根据初始位置区域初始化粒子滤波器参数。

根据初始位置区域的分布均值和方差得到粒子滤波器的粒子初始分布。

根据初始位置区域的方差得到粒子滤波器的粒子数，原则为方差越大，粒子数越多；或由用户指定得到粒子滤波器的粒子数。

步骤5、粒子滤波器迭代更新，包括采样粒子产生过程和重要性采样过程；当达到迭代终止条件时，停止迭代更新，并输出机器人的初始位置信息。

采样粒子产生过程，具体为：根据粒子初始分布或重要性采样过程得到的粒子分布，产生新的采样粒子。

重要性采样过程，具体为：根据采样粒子位置的观测及机器人真实位置的观测，计算采样粒子的权值，并进行低方差重采样，等到更新的粒子分布。

迭代终止条件，至少包括达到迭代次数或粒子分布的方差小于一定阈值。

根据本发明实施例提供的方法，机器人能在任意位置、不需运动的情况下，由用户方便快捷的指定初始位置、并获取相对准确的粒子滤波器初始化参数，经过静止状态下的粒子滤波器采样、重采样，得到更为准确的初始位置估计。

实施例二

图2示本发明一种基于粒子滤波器的快速初始定位的机器人设备的结构示意图，包括：

栅格地图建立模块，用于建立栅格地图；

栅格地图分区模块，用于建立栅格地图分区；

初始位置区域指定模块，用于为机器人指定初始位置区域，可以为一个或多个栅格地图分区；

粒子滤波器初始化模块，用于机器人根据初始位置区域初始化粒子滤波器参数；

粒子滤波器执行模块，用于粒子滤波器迭代更新，包括采样粒子产生过程和重要性采样过程；当达到迭代终止条件时，停止迭代更新，并输出机器人的初始位置信息。

每个模块的具体细节信息参考实施例一。

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

通过本发明实施例二提供的一种基于粒子滤波器的快速初始定位的机器人设备，机器人能在任意位置、不需运动的情况下，由用户方便快捷的指定初始位置、并获取相对准确的粒子滤波器初始化参数，经过静止状态下的粒子滤波器采样、重采样，得到更为准确的初始位置估计。

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

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合，为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。本发明的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明的思想，其同样应当视为本发明所公开的内容。