[发明专利]一种基于正态分布的粒子滤波定位方法

说明书

本发明公开了一种基于正态分布的粒子滤波定位方法，包括：基于及栅格地图分辨率将栅格地图划分若干大区域，每个大区域内部划分4个子区域，相邻子区域面积重叠，计算每个子区域内栅格点的位置均值和位置方差；构建粒子滤波器，将空闲区域作为有效区域，在有效区域内产生随机粒子；根据粒子位姿的观测数据，采用正态分布模型计算粒子权重及粒子总权重；根据粒子权重对粒子进行筛选、复制及保留，同时增加随机粒子；重采样后，更新粒子分布，使得每个粒子的权重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权重粒子簇的平均位姿即为最佳位姿，即完成重定位。一定程度克服使用里程计时存在的打滑问题。

技术领域

本发明属于定位技术领域，更具体地，本发明涉及一种基于正态分布的粒子滤波定位方法。

背景技术

基于路标的激光定位导航技术需要在环境中布设大量的人工路标，物料成本高，对环境有一定的要求，不能做到完全的柔性导航。目前，移动机器人同时定位与地图构建(SLAM)技术成为机器人定位导航研究的热点问题，是移动机器人自主导航的基础和前提。求解SLAM问题的方法有基于kalman滤波器的方法，基于粒子滤波的方法和基于图优化的方法。SLAM方法可以提高移动机器人的自主能力和环境适应能力，使移动机器人可以在未知的环境中进行自主定位和导航。实现移动机器人的自动导航需要加载建图系统生成的栅格地图，并在该地图实现机器人自定位和导航。

现有的技术中存在划分地图区域和粒子滤波进行定位的方法：为机器人指定初始位置区域，可以为一个或多个栅格地图分区；机器人根据初始位置区域初始化粒子滤波器参数；粒子滤波器迭代更新，包括采样粒子产生过程和重要性采样过程；当达到迭代终止条件时，停止迭代更新，并输出机器人的初始位置信息。该方案使得机器人能在任意位置、不需运动的情况下，由用户方便快捷地指定初始位置、并获取相对准确的粒子滤波器初始化参数，经过静止状态下的粒子滤波器采样、重采样，得到更为准确的初始位置估计。该方案存在如下问题：

建立栅格地图分区采用的是程序自动建立均匀分区和用户基于栅格地图中重要地标进行分区。这种分区方法可能会因为所选的区域包含的周围信息缺少或者定位点没有重要地标而导致重定位失败。而本方案采用的基于正态分布的粒子滤波定位方法，仅仅依靠激光传感器，且不需要人工干预就可原地实现重定位，连续定位过程中可快速高效地实现定位，同时一定程度克服使用里程计时存在的打滑问题。

发明内容

本发明提供了一种基于正态分布的粒子滤波定位方法，旨在改善上述问题。

本发明是这样实现的，一种基于正态分布的粒子滤波定位方法，所述方法具体包括如下步骤：

S1、构建地图正态分布概率模型：基于及栅格地图分辨率将栅格地图划分若干大区域，对于每个大区域内部划分4个子区域，相邻两子区域面积50％重叠，计算每个子区域内栅格点的坐标均值和坐标方差；

S2、初始化粒子分布：构建粒子滤波器，将空闲区域作为有效区域，在有效区域内产生随机粒子；

S3、观测模型更新：根据粒子位姿的观测数据，采用正态分布模型计算粒子权重及粒子总权重—基于粒子s_i的位姿将当前激光帧中的点云转换至栅格地图中，计算当前激光帧中各点云落入相应4个子区域的得分和，即为对应粒子s_i的权重；

S4、重采样：根据粒子权重对粒子进行筛选、复制及保留，同时增加随机粒子；

S5、粒子滤波器迭代更新：重采样后，更新粒子分布，使得每个粒子的权重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权重粒子簇的平均位姿即为最佳位姿，即完成重定位。

进一步的，在步骤S1之后还包括：

里程计的运动模型更新：基于当前重定位的最佳位姿作为中心，划定局部窗口，在局部窗口内根据高斯模型随机分布粒子，基于步骤S3至步骤S5实现局部窗口内的最优位姿，即再次完成重定位。