[发明专利]基于粒子滤波的定位方法、系统、设备及存储介质

说明书

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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