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

本发明涉及机器人及导航技术领域，尤其涉及一种基于粒子滤波器的多机器人编队定位方法及机器人设备。

背景技术：

随着嵌入式计算和通信能力的提高,以及分布式或者非集中式思想的发展,人们越来越认识到相比于单个智能体,多智能体系统能够以更小的代价完成更复杂的任务，多机器人编队的应用需求越来越多，特别是机器人编队的自主导航功能。自动导航系统首先要载入建图系统生成的栅格地图，并在该地图实现机器人自定位和导航。因此，无论在什么样的环境中，机器人都需要非常准确地定位到自己的位置，以使自动导航系统快速生效。

基于激光的单个机器人自定位算法通常采用粒子滤波算法，通用的方法是amcl(自适应蒙特卡洛定位)。2d激光的位姿(通常是一个点)包括机器人的位置和朝向信息，然后机器人在地图范围内进行自定位和导航。该方法的问题有：

单个机器人只依赖自己的里程计信息和雷达信息进行粒子滤波，无法有效校正自己的位姿；

现有技术中，cn201811205306.5(一种多机器人协同定位与控制方法)提供了一种基于多机器人协同控制的定位方法，该方法根据若干个机器人中的世界坐标系坐标与传感器获得的测距信息通过梯度下降算法对机器人位置进行优化，该方法需要机器人获取很多数据，条件要求很强，同时需要全局大量计算。

因此，本领域的技术人员致力于开发一种基于粒子滤波器的多机器人编队定位方法及机器人设备，以解决现有技术中因单个机器人里程计误差、激光雷达扫描不准等因素造成的定位不准、校正不足的问题。

技术实现要素：

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是解决现有技术中因单个机器人里程计误差、激光雷达扫描不准等因素造成的定位不准、校正不足的问题。

为实现上述目的，本发明提供了一种基于粒子滤波器的多机器人编队定位方法包括以下步骤：

步骤1、建立栅格地图；

步骤2、多机器人实现协同编队运动；

步骤3、粒子滤波器迭代更新，包括采样粒子产生过程的迭代更新和重要性采样过程的迭代更新；

步骤4、所述机器人实时获取所述编队中相邻所述机器人的所述粒子平均权重；

步骤5、所述机器人利用相邻所述机器人的所述粒子平均权重实时对自己的所述平均权重进行校正；

步骤6、重复步骤3，并实时输出所述机器人的位姿，直到达到迭代终止条件时停止迭代更新。

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

进一步地，步骤2实现方法包括：集中式、分布式或混合式。

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

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

进一步地，步骤4的实现方法包括：

步骤4.1、所述机器人获取的所述粒子平均权重，具体为：利用里程计的预测信息和激光雷达的测量数据融合得到的所述粒子的平均权值，用于去除低置信度的所述粒子并更新机器人位姿；

步骤4.2、从集中式处理器实时接收数据或与相邻所述机器人直接实时通信，如所述机器人通过邻接矩阵的形式实现与相邻所述机器人的通信，则获取相邻所述机器人的所述粒子平均权重。

进一步地，步骤5包括：使用加权融合或差值修正，如利用差值修正平均权重的方法：

其中，aij的值表示机器人编队中编号为i与j的机器人之间是否通信，是机器人j在k时刻的粒子平均权重，为机器人j在k时刻校正后的粒子权重，rij表示与机器人i与机器人j在编队队形中的相对位置相关的参数。

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

本发明还提供了一种基于粒子滤波器的多机器人编队定位的机器人设备，其特征在于，包括：栅格地图建立模块，编队通信模块，编队控制模块，粒子滤波器模块，和校正模块。

进一步地，所述栅格地图建立模块，用于建立所述栅格地图；所述编队通信模块，用于与相邻所述机器人通信或与集中式处理器通信；所述编队控制模块，用于实现多机器人协同并按照固定队形运动；所述粒子滤波器模块，用于所述粒子滤波器迭代更新，包括采样粒子产生过程和重要性采样过程的迭代更新；所述校正模块，用于利用相邻所述机器人的所述粒子平均权重对自己的所述粒子平均权重进行校正。

本发明多机器人编队能在导航中的任意位置快速准确地定位自己的位置，并且能够校正丢失的机器人，经过静止状态下的粒子滤波器采样、重采样，得到更为准确的位置估计，基于这种定位方法可以实现更加精准的编队控制。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

附图说明

图1是本发明基于粒子滤波器的多机器人编队定位方法的流程图；

图2是本发明基于粒子滤波器的多机器人编队定位的机器人设备的结构示意图。

具体实施方式

以下参考说明书附图介绍本发明的多个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。

实施例一

图1是本发明基于粒子滤波器的多机器人编队定位方法的流程图，包括：

步骤1、建立栅格地图。

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

步骤2、多机器人实现协同编队运动。

多机器人协同编队运动的方法，包括：集中式、分布式或混合式，如多个机器人通过领航者和跟随者的分布式控制方法实现在工作环境中以某种固定的队型进行运动。

步骤3、粒子滤波器迭代更新，包括采样粒子产生过程和重要性采样过程。

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

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

步骤4、机器人实时获取编队中相邻机器人的粒子平均权重。

机器人实时获取编队中相邻机器人的粒子平均权重的方法，包括：从集中式处理器实时接收数据或与相邻机器人直接实时通信，如机器人通过邻接矩阵的形式实现与相邻机器人的通信，获取相邻机器人的粒子平均权重。

机器人获取的粒子平均权重，具体为：利用里程计的预测信息和激光雷达的测量数据融合得到的粒子的平均权值，用于去除低置信度的粒子并更新机器人位姿。

步骤5、机器人利用相邻机器人的粒子平均权重实时对自己的平均权重进行校正，重复步骤3，并实时输出机器人位姿，直到达到迭代终止条件时停止迭代更新。

机器人利用相邻机器人的粒子平均权重对自己的粒子平均权重进行校正的方法，包括：使用加权融合或差值修正，如利用差值修正平均权重的方法：

其中，aij的值表示机器人编队中编号为i与j的机器人之间是否通信，是机器人j在k时刻的粒子平均权重，为机器人j在k时刻校正后的粒子权重，rij表示与机器人i与机器人j在编队队形中的相对位置相关的参数。

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

根据本发明实施例一提供的方法，多机器人编队能在导航中的任意位置快速准确地定位自己的位置，并且能够校正丢失的机器人，经过静止状态下的粒子滤波器采样、重采样，得到更为准确的位置估计，基于这种定位方法可以实现更加精准的编队控制。

实施例二

图2是本发明基于粒子滤波器的多机器人编队定位的机器人设备的结构示意图，包括：

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

编队通信模块，用于与相邻机器人或集中式处理器通信；

编队控制模块，用于实现多机器人协同并按照固定队形运动；

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

校正模块，用于利用相邻机器人的粒子平均权重对自己的粒子平均权重进行校正。

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

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

通过本发明实施例二提供的一种基于粒子滤波器的多机器人编队定位方法的机器人设备，多机器人编队能在导航中的任意位置快速准确地定位自己的位置，并且能够校正丢失的机器人，经过静止状态下的粒子滤波器采样、重采样，得到更为准确的位置估计，基于这种定位方法可以实现更加精准的编队控制。

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

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。