CN114674320A - 基于粒子滤波的定位方法、系统、设备及存储介质 - Google Patents

Abstract

本发明提供了一种基于粒子滤波的定位方法，定位方法包括：获取粒子集合中各个粒子的权重系数；根据各个粒子的权重系数对各个粒子进行N次重采样，以生成目标粒子集合；将目标粒子集合形成的位姿与预设激光模板匹配，以生成机器人的初始位姿；根据机器人的轮子码盘的读数更新机器人的初始位姿；根据更新后的初始位姿以及激光雷达实时采集的数据，更新动态地图层；在动态地图层与当前区域内的静态地图层的形状以及朝向相同的情形下，将动态地图层与静态地图层进行融合，以生成室内场景地图；本发明实施例提供的基于粒子滤波的定位方法，使用激光模板匹配的方法来帮助粒子滤波进行收敛，能够有效解决定位模式中的动态与长期稳定定位的问题。

Description

基于粒子滤波的定位方法、系统、设备及存储介质

技术领域

本发明涉及机器人定位领域，尤其涉及一种基于粒子滤波的定位方法、系统、设备及存储介质。

背景技术

机器人根据实时采集的传感器数据，来确定机器人在地图中的位置与朝向。定位问题主要分为位姿跟踪，全局定位，意外绑架等三种类型：已知机器人的初始位姿，来跟踪机器人运动过程中的位姿变化，属于位姿跟踪问题；未知机器人的初始位姿，需要从地图上可能的空间中来找到机器人的初始位姿，并进行跟踪，属于全局定位问题。全局定位问题中，机器人是知道自己未知初始位姿的，也就是该初始位姿的置信度比较低；如果机器人错误地相信自己的初始位姿，但是该位姿却是错误的，也就是机器人被意外地带到了另外一个地方，则属于意外绑架问题。

定位问题的的现有解决方案中，大都采用的是基于高斯模型假设的卡尔曼滤波方法；卡尔曼滤波是假设机器人的运动过程中，状态变化模型是基于高斯模型即正态分布的，而且状态变化方程是可线性近似的；所以通过不断的预测-更新的迭代过程，来估算机器人的位姿。

卡尔曼滤波方法存在的问题是：由于事实上状态的描述不一定符合高斯概率分布，且变化过程存在投影变换的正弦与余弦函数，是个非线性模型，所以线性化近似就不可避免的引入近似误差问题。另外，卡尔曼滤波方法所解决的问题主要是位姿跟踪问题，对于初始位姿不确定的全局定位问题和绑架问题，该方法就很难处理。

发明内容

本发明的目的是提供一种基于粒子滤波的定位方法、系统、设备及存储介质，用于解决在机器人初始位姿发生较大变化的场景下，长时间的稳定定位问题。

本发明实施例的一个方面提供了一种基于粒子滤波的定位方法，所述方法包括：

获取粒子集合中各个粒子的权重系数；所述粒子集合用于反映机器人的位姿且均匀分布在预设地图中；

根据所述各个粒子的权重系数对各个粒子进行N次重采样，以生成目标粒子集合；其中N为正整数且大于1，重采样到各个粒子的概率与该粒子的权重系数成正比；

将所述目标粒子集合形成的位姿与预设激光模板匹配，以生成机器人的初始位姿；

根据所述机器人的轮子码盘的读数更新所述机器人的初始位姿；

根据更新后的初始位姿以及激光雷达实时采集的数据，更新动态地图层；

在所述动态地图层与当前区域内的静态地图层的形状以及朝向相同的情形下，将所述动态地图层与静态地图层进行融合，以生成室内场景地图。

优选地，获取粒子集合中的各个粒子的权重系数的步骤，包括：

获取各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角；其中一个粒子对应于所述预设地图中的一个栅格；

根据各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角，计算各个粒子与预设地图的匹配度；

根据各个粒子与预设地图的匹配度，得到粒子集合中的各个粒子的权重系数。

优选地，根据各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角，计算各个粒子与预设地图的匹配度的步骤包括：

根据各个粒子的航向角，通过以下公式，计算各个粒子的世界坐标：

其中，所述p′为粒子集合中的粒子k的世界坐标，R为粒子集合中的粒子k的航向角；t为机器人运动的时间，p为粒子集合中的粒子k的空间坐标；

根据各个粒子的世界坐标，以及各个粒子与预设地图中的对应栅格之间的距离，通过以下公式，计算粒子集合中的各个粒子与预设地图的匹配度：

其中，所述g_k为粒子k到栅格g之间的距离，p′_k为粒子k的世界坐标，F为粒子k的匹配度。

优选地，根据各个粒子与预设地图的匹配度，得到粒子集合中的各个粒子的权重系数的步骤，包括：

根据以下公式，得到粒子集合中的各个粒子的权重系数：

c＝exp(-ρ²)

其中所述ρ为粒子k的匹配度，所述c为粒子k的权重系数。

优选地，根据所述各个粒子的权重系数对各个粒子进行N次重采样，以生成目标粒子集合的步骤，包括：

根据所述各个粒子的权重系数对预设地图内的粒子进行有放回的N次重复采样；其中每次重复采样的粒子数量与所述粒子集合中的粒子数量相同；

将第N次重复采样后的多个粒子作为目标粒子集合。

优选地，将所述目标粒子集合形成的位姿与预设激光模板匹配，以生成机器人的初始位姿的步骤，包括：

将所述目标粒子集合形成的位姿与数据库中的多个激光模板进行匹配；

将匹配度高于预设阀值的目标激光模板输入到所述目标粒子集合中进行迭代收敛，得到所述机器人的初始位姿。

优选地，根据所述机器人的轮子码盘的读数更新所述机器人的初始位姿的步骤，包括：

根据机器人的轮子码盘的读数，得到轮子转动的角位移；

根据所述角位移，获取机器人行走的相对位移；

将所述相对位移转换为世界坐标，更新所述机器人的初始位姿。

本发明实施例的一个方面又提供一种基于粒子滤波的定位系统，包括，

获取模块，用于获取粒子集合中各个粒子的权重系数；所述粒子集合用于反映机器人的位姿且均匀分布在预设地图中；

采样模块，用于根据所述各个粒子的权重系数对各个粒子进行N次重采样，以生成目标粒子集合；其中N为正整数且大于1，重采样到各个粒子的概率与该粒子的权重系数成正比；

匹配模块，用于将所述目标粒子集合形成的位姿与预设激光模板匹配，以生成机器人的初始位姿；

更新模块，用于根据所述机器人的轮子码盘的读数更新所述机器人的初始位姿；并根据更新后的初始位姿以及激光雷达实时采集的数据，更新动态地图层；

融合模块，用于在所述动态地图层与当前区域内的静态地图层的形状以及朝向相同的情形下，将所述动态地图层与静态地图层进行融合，以生成室内场景地图。

本发明实施例的一个方面又提供了一种计算机设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述基于粒子滤波的定位方法的步骤。

本发明实施例的一个方面又提供了一种计算机可读存储介质，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述基于粒子滤波的定位方法的步骤。

本发明实施例提供的基于粒子滤波的定位方法、系统、设备及存储介质，充分利用了粒子滤波方法的多模态假设特性，对粒子的位姿进行跟踪，评估和收敛；同时使用激光模板匹配的方法来帮助粒子滤波进行收敛。本方案能够有效解决定位模式中的动态与长期稳定的问题。这样即使存在机器人运行环境出现一定程度的动态变化，而导致实时采集的传感器数据与参考地图无法准确匹配的情况，也能够进行稳定定位；同时可以在新构建的动态地图层中通过共同部分，来与所加载的静态地图层建立起关联，从而使整张地图得到更新。

附图说明

图1示意性示出了根据本发明实施例一的基于粒子滤波的定位方法的流程图；

图2为图1中步骤S100的子步骤图；

图3为图1中步骤S102的子步骤图；

图4示意性示出了根据本发明实施例二的基于粒子滤波的定位系统的框图；及

图5示意性示出了根据本发明实施例三的适于实现基于粒子滤波的定位方法的计算机设备的硬件架构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，在本发明实施例中涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

在本发明的描述中，需要理解的是，步骤前的数字标号并不标识执行步骤的前后顺序，仅用于方便描述本发明及区别每一步骤，因此不能理解为对本发明的限制。

名词解释：

激光雷达(英文：Laser Radar)，是以发射激光束探测目标的位置、速度等特征量的雷达系统。其工作原理是向目标发射探测信号(激光束),然后将接收到的从目标反射回来的信号(目标回波)与发射信号进行比较,作适当处理后,就可获得目标的有关信息,如目标距离、方位、高度、速度、姿态、甚至形状等参数,从而对飞机、导弹等目标进行探测、跟踪和识别。它由激光发射机、光学接收机、转台和信息处理系统等组成，激光器将电脉冲变成光脉冲发射出去，光接收机再把从目标反射回来的光脉冲还原成电脉冲，送到显示器。

粒子滤波：通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被形象的称为“粒子”，故而叫粒子滤波。

本实施例主要是基于现有粒子滤波定位的优化方案，下面以计算机设备为执行主体，具体阐述本发明实施方案。

实施例一

图1示意性示出了根据本发明的一种基于粒子滤波的定位方法的流程图。

如图1所示，该粒子滤波的定位方法可以包括步骤S100～S105，其中：

步骤S100：获取粒子集合中各个粒子的权重系数；所述粒子集合用于反映机器人的位姿且均匀分布在预设地图中。

具体地，每个粒子都关联了一个机器人位姿。可根据粒子集合中所有粒子相关联的位姿的平均值，以此来作为机器人的位姿。同样，统计学意义上的位姿不确定性相关联的协方差矩阵，就是计算出粒子群集合中各个粒子所关联的位姿，相对于均值位姿之间的偏差程度。

进一步具体地，本实施例中可通过观测模型对粒子的权重系数进行评估，观测模型是针对传感器的测量模型，用于计算相应位姿的权重系数，也就是来描述相应位姿准确度大小的数值：取值在0与1之间，越靠近0表示权重系数越小，越靠近1表示权重系数越大。观测模型将激光雷达数据中每个激光束的末端点(即各个粒子)到机器人的距离与各个粒子的航向角的原始数据，根据观测出的不确定性数值，按照正态分布构建出来以观测数据为均值，以描述不确定性的协方差矩阵的正态分布函数。在此基础上，将预设地图的数据与上述正态分布函数进行比对，从而计算出来各个粒子的权重系数。

进一步优选地，如图2所示，获取粒子集合中的各个粒子的权重系数的步骤具体包括：

步骤S100-1：获取各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角；其中一个粒子对应于所述预设地图中的一个栅格；

具体的，预设地图优选为栅格地图，栅格地图由若干个依次排列的矩形栅格组成，其中，本实施例中获取到的是各个粒子与对应的矩形栅格的距离以及航向角均可通过激光雷达获取。

步骤S100-2：根据各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角，计算各个粒子与预设地图的匹配度；

为了获取到各个粒子与预设地图的匹配度，首先需要将粒子的原始数据(即各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角)转换为世界坐标；根据各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角，计算各个粒子与预设地图的匹配度的步骤包括：

步骤S100-1A：根据各个粒子的航向角，通过以下公式，计算各个粒子的世界坐标：

其中，所述p′为粒子集合中的粒子k的世界坐标，R为粒子集合中的粒子k的航向角；t为机器人运动的时间，p为粒子集合中的粒子k的空间坐标。

具体地，通过激光雷达获取的带空间坐标的采样点的集合，即粒子集合，而通过激光雷达采集到的粒子集合带有空间坐标，所以首先需要将空间坐标通过矩阵转换为世界坐标；然后再通过以下方法，计算粒子集合中的各个粒子与预设地图的匹配度。

步骤S100-1B：根据各个粒子的世界坐标，以及各个粒子与预设地图中的对应栅格之间的距离，通过以下公式，计算粒子集合中的各个粒子与预设地图的匹配度：

其中，所述g_k为粒子k到栅格g之间的距离，p′_k为粒子k的世界坐标，F为粒子k的匹配度。

可以以上公式来计算粒子与预设地图的匹配程度。如果距离比较小，也就是变换后世界坐标与栅格地图对应点坐标之间的偏差二次型比较小，说明根据机器人坐标变换后的粒子与预设地图的契合程度一致性较好，即粒子与预设地图的匹配度较高。在匹配度较高的情况，就对应于该粒子所代表的机器人位姿比较准确的情形；也就是说，该粒子的权重系数较大。

步骤S100-3：根据各个粒子与预设地图的匹配度，得到粒子集合中的各个粒子的权重系数。

由于粒子集合中的各个粒子与预设地图的匹配度与权重系数呈正比例关系，所以可以通过以下公式计算粒子集合中的各个粒子的权重系数：

c＝exp(-ρ²)

其中所述ρ为粒子k的匹配度，所述c为粒子k的权重系数。

步骤S101，根据所述各个粒子的权重系数对各个粒子进行N次重采样，以生成目标粒子集合；其中N为正整数且大于1，重采样到各个粒子的概率与该粒子的权重系数成正比。

具体地，重采样是进行与粒子集合的粒子个数数目相同的采样过程；其中每一次采样时，被采样出来的粒子的概率，是与该粒子的权重系数成正比例。

优选地，根据所述各个粒子的权重系数对各个粒子进行N次重采样，以生成目标粒子集合的步骤，包括：

步骤S101-1：根据所述各个粒子的权重系数对预设地图内的粒子进行有放回的N次重复采样；其中每次重复采样的粒子数量与所述粒子集合中的粒子数量相同；

将第N次重复采样后的多个粒子作为目标粒子集合。

具体地，权重系数较大的粒子，在每次采样过程中更可能被筛选出来；同样，权重系数较大的粒子，在多次重复的有放回重采样过程中，更可能被筛选出来多次。这样基于权重系数进行的重采样，就可以保证该过程可以筛选出与采样前的数目相同，但是其中对应权重系数较大的粒子出现更多的集合。每次重采样过程，都是基于当时所计算出来的权重系数来进行操作的；上述计算出的权重系数是为重采样过程所服务的。采样结束后，并不再立刻重新计算权重系数。而是等到下一次的机器人运动更新后，再根据观测模型来重新进行计算权重系数。上述的过程不断循环往复周而复始的过程中的一个环节；这种“运动预测-观测更新”环节的不断重复迭代，就构成了粒子滤波方法来对于动态系统进行状态估计的基本途径。

步骤S102：将所述目标粒子集合形成的位姿与预设激光模板匹配，以生成机器人的初始位姿。

具体地，通过激光模板匹配的方法来检索到所加载激光模板数据库中与当前雷达数据最相似的激光模板，并将当前雷达数据匹配到所检索到的激光模板数据上，来得到调整后的优化位姿。

优选地，如图3所示，将所述目标粒子集合形成的位姿与预设激光模板匹配，以生成机器人的初始位姿的步骤，包括：

步骤S102-1：将所述目标粒子集合形成的位姿与数据库中的多个激光模板进行匹配；

步骤S102-2：将匹配度高于预设阀值的目标激光模板输入到所述目标粒子集合中进行迭代收敛，得到所述机器人的初始位姿。

具体地，本实施例中的激光模板是指相应的激光模板数据库构建过程中，将原始的激光雷达数据进行预处理，根据几何形状特征，找到相关曲率变化较大的位置等来构建出词袋，再通过激光雷达数据所对应的词汇，构建出数据库，数据库中包括若干个可供匹配的激光模板。其作用是将激光雷达数据进行标准化，后续使用实时获取的激光雷达数据(即目标粒子集合形成的位姿)进行检索的时候，只要比对激光模板数据库中相应模板，就可以判断实时获取的激光雷达数据是否与激光模板数据库中的激光模板具备关联度，从而找出与实时获取的激光雷达数据所对应的模板库中激光模板，完成模板匹配的工作。最初始的激光模板匹配可以得到多个备选的激光模板，通过激光模板匹配可以筛选掉匹配度较低的情况，只留下匹配程度较好的激光模板用于优化位姿，作为位姿先验的条件，输入到传统的粒子滤波框架里面，进行迭代收敛，最后得到机器人的初始位姿。

步骤S103：根据所述机器人的轮子码盘的读数更新所述机器人的初始位姿。

具体地，所述机器人的相对位移的数值来自于机器人本身的里程计数据，其中里程计数据来自于底盘的航迹推算运动学模型。

进一步具体地，根据所述机器人的轮子码盘的读数更新所述机器人的初始位姿的步骤，包括：

步骤S103-1：根据机器人的轮子码盘的读数，得到轮子转动的角位移；

步骤S103-2：根据所述角位移，获取机器人行走的相对位移；

步骤S103-3：将所述相对位移转换为世界坐标，更新所述机器人的初始位姿。

具体地，在粒子滤波中使用里程计数据来进行粒子集合的位姿的更新，其中使用的是相对位移的数值。通过读取两个轮子码盘的读数，以及底盘的轮子半径，轮间距等几何尺寸，进而得到轮子转动的角位移；轮子的角位移已知后，就可以根据底盘的运动学模型，来推算出轮子转动过程中，整个底盘的水平位移和旋转角度，根据运动学模型计算水平位移和旋转角度属于现有技术，在此不再赘述；水平位移和旋转角度即可计算出相对位移，相对位移已知后，可将相对位移变换到世界坐标系中，从而得到机器人的底盘在世界坐标系中的位姿，也就是航迹推算，世界坐标系的变换方法与上述空间坐标转换为世界坐标的方法类似，在此不再赘述。

步骤S104，根据更新后的初始位姿以及激光雷达实时采集的数据，更新动态地图层；

具体地，本实施例在使用上述不断迭代的粒子滤波进行机器人定位的过程中，会使用定位跟踪的轨迹，来维护一个动态的地图层；该动态地图层是由激光雷达实时采集的数据，按照定位跟踪过程中机器人的初始位姿，变换到统一的动态地图坐标系中；具体地激光雷达实时采集的数据包括机器人在空间的坐标(即机器人对应的粒子集合的空间坐标)以及机器人的朝向，来将实时采集的在线雷达数据按照所关联的粒子对应的机器人位姿转换到地图坐标系中。

步骤S105:在所述动态地图层与当前区域内的静态地图层的形状以及朝向相同的情形下，将所述动态地图层与静态地图层进行融合，以生成室内场景地图。

具体地，动态地图与静态地图进行融合的前提是彼此对齐，具体体现就是对应到现实环境中的同一个物体，在动态地图和静态地图中能够形状与朝向都保持一致的准确对齐到一起。这意味着动态地图层来拓展静态地图层的内容时，所采用的位置与朝向两个地图图层是彼此一致的；只有这样，才能保证两个图层进行融合时不会出现地图墙壁重影等现象；进一步地，动态地图层在与静态地图层对齐之后，会复合到所加载的静态地图层上，以确保复合后的地图，既包括所加载地图的静态特性，又包括实时场景中的动态特性。复合后的地图对于机器人的定位，可以确保机器人在环境中与所加载的静态地图发生较大变化的位置，不再会出现误匹配的情形；而是在动态地图层与静态地图层能够匹配上的时候，来进行机器人的位姿对齐更新。

在本实施例中，在更新动态地图图层的过程中，机器人可以通过针对实时采集的雷达数据来进行激光匹配而进行自身位姿跟踪。对于机器人实时雷达数据在与所加载的静态地图匹配度较好的时候才进行位姿融合，可以确保能够将机器人此时的位姿挂载到所加载的静态地图上面比较准确的位置与朝向上；这样，在静态地图层上面找到了比较准确的机器人位姿，就可以将动态地图层与静态地图层进行对齐。在动态地图层能够有效准确匹配到所加载地图上时，动态与静态地图层就可以进行对齐和融合，从而保证动态场景下，与静态环境所对应的相同固定不动的物体如墙壁等，都能对正融合到一起。这样复合处理后的地图，是机器人进行准确定位的基础。

同时，对于定位过程中机器人不知道自己当前在地图中何处的情形，本方案采用了基于激光模板匹配的方法，来将实时采集的雷达数据特征，匹配于所加载地图中相关的激光雷达的特征，从而找出其中比较相似的备选帧，来进行粒子滤波的提速。这些候选帧所关联的一系列位姿，可以作为粒子滤波收敛的初始先验。有了这些候选帧位姿的先验，粒子滤波方法就可以在先验位姿附近来进行生成粒子，并计算权重系数，重采样，不断迭代粒子滤波的更新与重采样的过程，直到粒子群收敛到比较靠谱正确的位姿上。

实施例二

图4示意性示出了根据本发明实施例二的基于粒子滤波的定位系统，该定位系统可以被分割成程序模块，一个或者多个程序模块被存储于存储介质中，并由处理器所执行，以完成本发明实施例。本发明实施例所称的程序模块是指能够完成特定功能的一系列计算机程序指令段，以下描述将具体介绍本实施例中各程序模块的功能。

如图4所示，该定位系统130包括获取模块131、采样模块132、匹配模块133、更新模块134、融合模块135；

获取模块131，用于获取粒子集合中各个粒子的权重系数；所述粒子集合用于反映机器人的位姿且均匀分布在预设地图中；

采样模块132，用于根据所述各个粒子的权重系数对各个粒子进行N次重采样，以生成目标粒子集合；其中N为正整数且大于1，重采样到各个粒子的概率与该粒子的权重系数成正比；

匹配模块133，用于将所述目标粒子集合形成的位姿与预设激光模板匹配，以生成机器人的初始位姿；

更新模块134，用于根据所述机器人的轮子码盘的读数更新所述机器人的初始位姿；并根据更新后的初始位姿以及激光雷达实时采集的数据，更新动态地图层；

融合模块135，用于在所述动态地图层与当前区域内的静态地图层的形状以及朝向相同的情形下，将所述动态地图层与静态地图层进行融合，以生成室内场景地图。

所述获取模块131还用于获取各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角；其中一个粒子对应于所述预设地图中的一个栅格；

根据各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角，计算各个粒子与预设地图的匹配度；

根据各个粒子与预设地图的匹配度，得到粒子集合中的各个粒子的权重系数。

所述获取模块131还用于根据各个粒子的航向角，通过以下公式，计算各个粒子的世界坐标：

其中，所述p′为粒子集合中的粒子k的世界坐标，R为粒子集合中的粒子k的航向角；t为机器人运动的时间，p为粒子集合中的粒子k的空间坐标；

根据各个粒子的世界坐标，以及各个粒子与预设地图中的对应栅格之间的距离，通过以下公式，计算粒子集合中的各个粒子与预设地图的匹配度：

其中，所述g_k为粒子k到栅格g之间的距离，p′_k为粒子k的世界坐标，F为粒子k的匹配度。

所述获取模块131还用于根据以下公式，得到粒子集合中的各个粒子的权重系数：

c＝exp(-ρ²)

其中所述ρ为粒子k的匹配度，所述c为粒子k的权重系数。

所述采样模块132还用于根据所述各个粒子的权重系数对预设地图内的粒子进行有放回的N次重复采样；其中每次重复采样的粒子数量与所述粒子集合中的粒子数量相同；将第N次重复采样后的多个粒子作为目标粒子集合。

所述匹配模块133还用于将所述目标粒子集合形成的位姿与数据库中的多个激光模板进行匹配；将匹配度高于预设阀值的目标激光模板输入到所述目标粒子集合中进行迭代收敛，得到所述机器人的初始位姿。

所述更新模块134还用于根据机器人的轮子码盘的读数，得到轮子转动的角位移；根据所述角位移，获取机器人行走的相对位移；将所述相对位移转换为世界坐标，更新所述机器人的初始位姿。

实施例三

图5示意性示出了根据本发明实施例三的适于实现基于粒子滤波的定位方法的计算机设备6的硬件架构示意图。本实施例中，计算机设备6是一种能够按照事先设定或者存储的指令，自动进行数值计算和/或信息处理的设备。例如，可以是智能手机、平板电脑、笔记本电脑、台式计算机、机架式服务器、刀片式服务器、塔式服务器或机柜式服务器(包括独立的服务器，或者多个服务器所组成的服务器集群)、网关等。如图5所示，计算机设备6至少包括但不限于：可通过系统总线相互通信链接存储器141、处理器142、网络接口143。其中：

存储器141至少包括一种类型的计算机可读存储介质，可读存储介质包括闪存、硬盘、多媒体卡、卡型存储器(例如，SD或DX存储器等)、随机访问存储器(RAM)、静态随机访问存储器(SRAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、可编程只读存储器(PROM)、磁性存储器、磁盘、光盘等。在一些实施例中，存储器141可以是计算机设备6的内部存储模块，例如该计算机设备6的硬盘或内存。在另一些实施例中，存储器141也可以是计算机设备6的外部存储设备，例如该计算机设备6上配备的插接式硬盘，智能存储卡(SmartMedia Card，简称为SMC)，安全数字(Secure Digital，简称为SD)卡，闪存卡(Flash Card)等。当然，存储器141还可以既包括计算机设备6的内部存储模块也包括其外部存储设备。本实施例中，存储器141通常用于存储安装于计算机设备6的操作系统和各类应用软件，例如基于粒子滤波的定位方法的程序代码等。此外，存储器141还可以用于暂时地存储已经输出或者将要输出的各类数据。

处理器142在一些实施例中可以是中央处理器(Central Processing Unit，简称为CPU)、控制器、微控制器、微处理器、或其他数据处理芯片。该处理器142通常用于控制计算机设备6的总体操作，例如执行与计算机设备6进行数据交互或者通信相关的控制和处理等。本实施例中，处理器142用于运行存储器141中存储的程序代码或者处理数据。

网络接口143可包括无线网络接口或有线网络接口，该网络接口143通常用于在计算机设备6与其他计算机设备之间建立通信链接。例如，网络接口143用于通过网络将计算机设备6与外部终端相连，在计算机设备6与外部终端之间的建立数据传输通道和通信链接等。网络可以是企业内部网(Intranet)、互联网(Internet)、全球移动通讯系统(GlobalSystem of Mobile communication，简称为GSM)、宽带码分多址(Wideband Code DivisionMultiple Access，简称为WCDMA)、4G网络、5G网络、蓝牙(Bluetooth)、Wi-Fi等无线或有线网络。

需要指出的是，图5仅示出了具有部件141-143的计算机设备，但是应理解的是，并不要求实施所有示出的部件，可以替代的实施更多或者更少的部件。

在本实施例中，存储于存储器141中的基于粒子滤波的定位方法还可以被分割为一个或者多个程序模块，并由处理器(本实施例为处理器142)所执行，以完成本发明实施例。

实施例四

本发明还提供一种计算机可读存储介质，计算机可读存储介质其上存储有计算机程序，计算机程序被处理器执行时实现实施例中的基于粒子滤波的定位方法的步骤。

本实施例中，计算机可读存储介质包括闪存、硬盘、多媒体卡、卡型存储器(例如，SD或DX存储器等)、随机访问存储器(RAM)、静态随机访问存储器(SRAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、可编程只读存储器(PROM)、磁性存储器、磁盘、光盘等。在一些实施例中，计算机可读存储介质可以是计算机设备的内部存储单元，例如该计算机设备的硬盘或内存。在另一些实施例中，计算机可读存储介质也可以是计算机设备的外部存储设备，例如该计算机设备上配备的插接式硬盘，智能存储卡(Smart Media Card，简称为SMC)，安全数字(Secure Digital，简称为SD)卡，闪存卡(Flash Card)等。当然，计算机可读存储介质还可以既包括计算机设备的内部存储单元也包括其外部存储设备。本实施例中，计算机可读存储介质通常用于存储安装于计算机设备的操作系统和各类应用软件，例如实施例中基于粒子滤波的定位方法的程序代码等。此外，计算机可读存储介质还可以用于暂时地存储已经输出或者将要输出的各类数据。

显然，本领域的技术人员应该明白，上述的本发明实施例的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明实施例不限制于任何特定的硬件和软件结合。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种基于粒子滤波的定位方法，其特征在于，所述方法包括：

获取粒子集合中各个粒子的权重系数；所述粒子集合用于反映机器人的位姿且均匀分布在预设地图中；

根据所述各个粒子的权重系数对各个粒子进行N次重采样，以生成目标粒子集合；其中N为正整数且大于1，重采样到各个粒子的概率与该粒子的权重系数成正比；

将所述目标粒子集合形成的位姿与预设激光模板匹配，以生成机器人的初始位姿；

根据所述机器人的轮子码盘的读数更新所述机器人的初始位姿；

根据更新后的初始位姿以及激光雷达实时采集的数据，更新动态地图层；

在所述动态地图层与当前区域内的静态地图层的形状以及朝向相同的情形下，将所述动态地图层与静态地图层进行融合，以生成室内场景地图。

2.根据权利要求1所述的基于粒子滤波的定位方法，其特征在于，获取粒子集合中的各个粒子的权重系数的步骤，包括：

获取各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角；其中一个粒子对应于所述预设地图中的一个栅格；

根据各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角，计算各个粒子与预设地图的匹配度；

根据各个粒子与预设地图的匹配度，得到粒子集合中的各个粒子的权重系数。

3.根据权利要求2所述的基于粒子滤波的定位方法，其特征在于，根据各个粒子与预设地图中的对应栅格之间的距离以及各个粒子的航向角，计算各个粒子与预设地图的匹配度的步骤包括：

根据各个粒子的航向角，通过以下公式，计算各个粒子的世界坐标：

其中，所述p′为粒子集合中的粒子k的世界坐标，R为粒子集合中的粒子k的航向角；t为机器人运动的时间，p为粒子集合中的粒子k的空间坐标；

根据各个粒子的世界坐标，以及各个粒子与预设地图中的对应栅格之间的距离，通过以下公式，计算粒子集合中的各个粒子与预设地图的匹配度：

其中，所述g_k为粒子k到栅格g之间的距离，p′_k为粒子k的世界坐标，F为粒子k的匹配度。

4.根据权利要求3所述的基于粒子滤波的定位方法，其特征在于，根据各个粒子与预设地图的匹配度，得到粒子集合中的各个粒子的权重系数的步骤，包括：

根据以下公式，得到粒子集合中的各个粒子的权重系数：

c＝exp(-ρ²)

其中所述ρ为粒子k的匹配度，所述c为粒子k的权重系数。

5.根据权利要求1所述的基于粒子滤波的定位方法，其特征在于，根据所述各个粒子的权重系数对各个粒子进行N次重采样，以生成目标粒子集合的步骤，包括：

根据所述各个粒子的权重系数对预设地图内的粒子进行有放回的N次重复采样；其中每次重复采样的粒子数量与所述粒子集合中的粒子数量相同；

将第N次重复采样后的多个粒子作为目标粒子集合。

6.根据权利要求1所述的基于粒子滤波的定位方法，其特征在于，将所述目标粒子集合形成的位姿与预设激光模板匹配，以生成机器人的初始位姿的步骤，包括：

将所述目标粒子集合形成的位姿与数据库中的多个激光模板进行匹配；

将匹配度高于预设阀值的目标激光模板输入到所述目标粒子集合中进行迭代收敛，得到所述机器人的初始位姿。

7.根据权利要求1所述的基于粒子滤波的定位方法，其特征在于，根据所述机器人的轮子码盘的读数更新所述机器人的初始位姿的步骤，包括：

根据机器人的轮子码盘的读数，得到轮子转动的角位移；

根据所述角位移，获取机器人行走的相对位移；

将所述相对位移转换为世界坐标，更新所述机器人的初始位姿。

8.一种基于粒子滤波的定位系统，其特征在于，包括，

获取模块，用于获取粒子集合中各个粒子的权重系数；所述粒子集合用于反映机器人的位姿且均匀分布在预设地图中；

采样模块，用于根据所述各个粒子的权重系数对各个粒子进行N次重采样，以生成目标粒子集合；其中N为正整数且大于1，重采样到各个粒子的概率与该粒子的权重系数成正比；

匹配模块，用于将所述目标粒子集合形成的位姿与预设激光模板匹配，以生成机器人的初始位姿；

更新模块，用于根据所述机器人的轮子码盘的读数更新所述机器人的初始位姿；并根据更新后的初始位姿以及激光雷达实时采集的数据，更新动态地图层；

融合模块，用于在所述动态地图层与当前区域内的静态地图层的形状以及朝向相同的情形下，将所述动态地图层与静态地图层进行融合，以生成室内场景地图。

9.一种计算机设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时用于实现权利要求1～7中任一项所述的基于粒子滤波的定位方法的步骤。

10.一种计算机可读存储介质，其特征在于，其内存储有计算机程序，所述计算机程序可被至少一个处理器所执行，以使所述至少一个处理器执行权利要求1～7中任一项所述的基于粒子滤波的定位方法的步骤。

Images