[发明专利]一种基于粒子群定位算法的定位方法

权利要求书

1.一种基于粒子群定位算法的定位方法，其特征在于，包括步骤：

S1、取前一次定位时的样本粒子分布集合；

S2、根据传感器数据计算集合中每个粒子的权重值；

S3、重新进行采样，根据权重值重新调整粒子分布，产生新的集合X；

S4、根据权重值取可能性最大的粒子作为当前的估计位姿p；

S5、进行二次采样流程，二次采样完毕后返回S1；

S6、输出定位结果。

2.如权利要求1所述的基于粒子群定位算法的定位方法，其特征在于，S5具体包括：

S51、提取在二次采样之前产生的集合X以及估计位姿p；

S52、以p为中心，形成新的局部样本粒子集合Xs；

S53、根据传感器数据计算Xs中每个粒子的权重值；

S54、取Xs中权重最大的粒子pmax；

S55、以pmax为均值形成高斯分布粒子集合Xg；

S56、将Xg添加到X中；

S57、以p作为本次定位的估计位姿。

3.如权利要求2所述的基于粒子群定位算法的定位方法，其特征在于，在S52之后、S53之前，将X权重中最大的粒子添加到Xs中。

4.如权利要求3所述的基于粒子群定位算法的定位方法，其特征在于，在S52之后、S53之前，将上一次定位最终确定的估计位姿添加到Xs中。

5.如权利要求4所述的基于粒子群定位算法的定位方法，其特征在于，S52中，以p为中心，以规定间隔和数量对邻域位姿进行采样，形成新的局部样本粒子集合Xs。

6.如权利要求5所述的基于粒子群定位算法的定位方法，其特征在于，S52中，间隔和数量可以由用户调整设置。

7.如权利要求6所述的基于粒子群定位算法的定位方法，其特征在于，S53中，以该位姿为基础，融合障碍物的距离和角度数据，计算与给定地图中障碍物位置的匹配度，根据匹配度计算权重值。

8.如权利要求7所述的基于粒子群定位算法的定位方法，其特征在于，S55中，按照规定数量、规定方差，以pmax为均值形成高斯分布粒子集合Xg。

9.如权利要求8所述的基于粒子群定位算法的定位方法，其特征在于，S56中，将Xg更新到X中。

10.如权利要求9所述的基于粒子群定位算法的定位方法，其特征在于，S57中，以pmax作为本次定位的估计位姿。