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

权利要求书

1.一种基于正态分布的粒子滤波定位方法，其特征在于，所述方法具体包括如下步骤：

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

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

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

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

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

2.如权利要求1所述基于正态分布的粒子滤波定位方法，其特征在于，在步骤S1之后还包括：

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

3.如权利要求2所述基于正态分布的粒子滤波定位方法，其特征在于，所述连续定位方法具体如下：

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

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

4.如权利要求2所述基于正态分布的粒子滤波定位方法，其特征在于，粒子s_i的权重p_si的计算公式如下：

x_i′＝ξ·x_i (3)

其中，x_i表示当前激光帧中第i个点在机器人本体坐标系中的坐标，ξ表示基于粒子s_i位姿生成的旋转平移矩阵，x_i′表示基于旋转平移矩阵ξ将激光帧中第i个点坐标x_i转换到地图点云后的坐标，μ_ij表示第j个区域的坐标均值μ_i，∑_ij表示第j个区域的坐标方差∑_i。