一种基于正态分布的粒子滤波定位方法

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

S3、观测模型更新：根据粒子位姿的观测数据，采用正态分布模型计算粒子权重及粒子总权重—基于粒子s

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

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

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

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

进一步的，所述连续定位方法具体如下：

将局部窗口的重定位结果输送给里程计，基于里程计在当前时刻与上一时刻内的位移及转角完成对所有粒子位姿的预测；

获取当前时刻的激光帧，基于步骤S3至步骤S5来获取当前的最佳位姿。

进一步的，粒子s

x

其中，x

本发明提出的基于正态分布的粒子滤波定位方法，仅仅依靠激光传感器，且不需要人工干预就可原地实现重定位，过程中可快速高效地实现定位，同时一定程度克服使用里程计时存在的打滑问题；此外，移动机器人出现异常情况后快速实现全局定位以及仅使用激光传感器实现精确定位。

附图说明

图1为本发明实施例提供的基于正态分布的粒子滤波定位方法流程图。

具体实施方式

下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

图1为本发明实施例提供的基于正态分布的粒子滤波定位方法流程图，该方法具体包括如下步骤：

步骤1)全局地图预处理：加载系统所需的全局地图(即栅格地图)，并记录全局地图中空闲区域、占用区域和未知区域。基于地图边界对地图进行裁剪，保留有效的地图区域。空闲区域即为激光雷达探测不存在障碍物的区域，占用区域即为激光雷达探测存在障碍物的区域，未知区域即为激光雷达无法探测的区域。

步骤2)构建栅格地图的正态分布概率模型：在栅格地图内根据地图边界以及栅格地图分辨率划分若干大区域。对于每个大区域内部划分4个子区域，要求四个子区域相互重叠，相邻两子区域重叠面积的50％，计算每个子区域内栅格点的坐标均值和坐标方差，如公式(1)、(2)所示。

μ

步骤3)初始化粒子分布：构建粒子滤波器，将空闲区域作为有效区域，在有效区域产生随机粒子。

步骤4)观测模型更新：根据粒子位姿的观测数据，采用正态分布模型计算粒子权重及粒子总权重，基于粒子s

x

其中，x

步骤5)重采样：根据粒子权重对粒子进行筛选、复制及保留，同时随机采样以以下概率增加，增加随机粒子。如公式(5)、(6)所示。

max{0.0，1.0-w

w

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

步骤7)里程计的运动模型更新：基于当前重定位的最佳位姿作为中心，划定合适的局部窗口，在局部窗口内根据高斯模型随机分布粒子，基于步骤4)至步骤6)实现局部窗口重定位。将此时的局部重定位结果输送给里程计的运动模型，基于里程计在当前时刻与上一时刻内的位移及转角完成对所有粒子位姿的预测。

步骤8)通过激光传感器观测数据，基于步骤4)～步骤6)完成粒子滤波器迭代更新后获得最佳位姿。利用正态分布模型计算粒子权重，进行重采样操作，复制高权重粒子，舍弃低权重粒子，当完成粒子滤波器迭代更新后获得最佳位姿。

本方案提出的基于正态分布的粒子滤波定位方法，仅仅依靠激光传感器，且不需要人工干预就可原地实现重定位，过程中可快速高效地实现定位，同时一定程度克服使用里程计时存在的打滑问题；此外，移动机器人出现异常情况后快速实现全局定位以及仅使用激光传感器实现精确定位。

有益技术效果的描述角度：

1)基于正态分布的粒子滤波重定位方法能够减少对初始地图最近障碍物距离的依赖，提高重定位的时间；窗口内的正态粒子滤波重定位方法能够输送结果给里程计运动模型，较少对硬件里程计的依赖。

2)基于正态分布的粒子滤波定位方法能够在移动机器人出现异常情况快速实现全局定位，可以仅依靠激光传感器实现精确定位，一定程度克服使用里程计时存在的打滑问题。

并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。