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

说明书

本发明涉及机器人定位技术领域，具体涉及一种基于粒子群定位算法的定位方法，包括步骤：S1、取前一次定位时的样本粒子分布集合；S2、根据传感器数据计算集合中每个粒子的权重值；S3、重新进行采样，根据权重值重新调整粒子分布，产生新的集合X；S4、根据权重值取可能性最大的粒子作为当前的估计位姿p；S5、进行二次采样流程，二次采样完毕后返回S1；S6、输出定位结果。本发明通过进行二次采样对粒子分布集合进行修正，使其尽可能地覆盖到位姿所有的可能性，解决了现有技术中真实的机器人位姿可能被忽略，从而导致定位精度不足、收敛速度不高的技术问题。

技术领域

本发明涉及机器人定位技术领域，具体涉及一种基于粒子群定位算法的定位方法。

背景技术

随着科学技术与人工智能的发展，移动机器人的应用越来越广泛。对于移动机器人而言，需要高精度的定位才能够执行任务。为了实现对移动机器人实时、准确、高效定位，目前广泛自适应蒙特卡洛定位算法(adaptive MonteCarlo localization，AMCL)。虽然说，AMCL算法相对稳定，但是总体定位的精度不高，同时需要大量粒子来维持较高的定位精度，因而有必要对AMCL算法做适当的修正。

对此，文件CN110927740A公开了一种移动机器人定位方法，包括将搭载二维激光雷达的机器人置于当前定位的环境中，并获取障碍物的地图点云；采用自适应蒙特卡洛定位算法，估计出机器人当前的位姿；利用机器人当前的位姿将二维激光雷达的实时数据转换为与地图点云同一坐标系下的点云PTCloudscan；将每一帧点云PTCloudscan作为迭代最近点的输入，得到当前帧点云PTCloudscan相对于地图点云的旋转矩阵R和平移矩阵T；根据旋转矩阵R和平移矩阵T，计算出机器人最终的位姿。通过将采用自适应蒙特卡洛定位算法得到的位姿作为参考值变换二维激光雷达的点云坐标系，使其与当前的地图点云足够近，加快了迭代最近点的匹配速度，提高了定位精度。

对于鲁棒性高的粒子群算法而言，例如AMCL算法，是将机器人各种可能的位姿(位置和方向)采样分布到空间中，空间中每个样本粒子代表一个可能的位姿，取其中权重值最大的粒子，即可能性最大的位姿作为机器人当前的估计位姿。

由于粒子代表的是机器人的各种可能位姿是离散采样的，而集合容量有限，故而粒子分布集合无法完全覆盖位姿所有的可能性，真实的机器人位姿可能被忽略，导致定位精度不足。此外，下一次定位是基于前一次定位产生的集合，前一次集合分布的准确性会直接影响下一次定位的准确性，这不但会进一步影响定位精度，同时还会降低收敛速度。

发明内容

本发明提供一种基于粒子群定位算法的定位方法，解决了现有技术中真实的机器人位姿可能被忽略，从而导致定位精度不足、收敛速度不高的技术问题。

本发明提供的基础方案为：一种基于粒子群定位算法的定位方法，包括步骤：

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

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

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

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

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

S6、输出定位结果。