[发明专利]定位方法和定位装置及计算机可读存储介质

说明书

本公开公开了一种定位方法和定位装置及计算机可读存储介质，涉及定位领域。其中的方法包括：根据待定位设备的至少一种定位传感器的定位位置信息确定待定位设备本次定位的初始定位位置信息；随机分布若干粒子，其中的部分粒子分布在初始定位位置信息的周围；根据每个粒子与地图的匹配程度选出最优粒子；将最优粒子的位置信息与本次定位的初始定位位置信息进行融合，将融合后的定位位置信息确定为待定位设备本次定位的最终定位位置信息。从而，解决定位发散问题，即使由于环境复杂而错选最优粒子，最优粒子依然在真实位置附近，不会产生太大的定位误差。

技术领域

本公开涉及定位领域，特别涉及一种定位方法和定位装置及计算机可读存储介质。

背景技术

基于激光雷达的定位方案中，粒子滤波算法是最常用的减小误差方法之一。粒子滤波算法从大量粒子中选取一个最优的粒子作为待定位设备的真实位置。

发明内容

发明人发现，在复杂的定位环境中，例如，地面不平、环境几何信息特征不明显，粒子滤波算法可能会出现错误的选出最优粒子的情况，下一次定位时，会以上次选出的最优粒子为基础分布粒子，从而造成连续多次选出错误的最优粒子，出现定位发散现象。如图1所示，第一次定位由于环境信息的干扰导致粒子滤波选出一个错误的“最优”粒子，第二次定位将第一次定位所选的“最优”粒子作为自动驾驶设备的“真实位置”，导致粒子重分布时自动驾驶设备真正的真实位置附近几乎没有粒子，由于必须要选出一个最优粒子，所以又错误的选出了一个“最优”粒子，第三次定位的定位位置已经完全发散，定位位置离真实位置越来越远。

本公开实施例所要解决的一个技术问题是：定位发散问题。

本公开的一个方面提出一种定位方法，包括：根据待定位设备的至少一种定位传感器的定位位置信息确定待定位设备本次定位的初始定位位置信息；随机分布若干粒子，其中的部分粒子分布在初始定位位置信息的周围；根据每个粒子与地图的匹配程度选出最优粒子；将最优粒子的位置信息与本次定位的初始定位位置信息进行融合，将融合后的定位位置信息确定为待定位设备本次定位的最终定位位置信息。

其中，在随机分布的若干粒子中，还有部分粒子分布在上一次定位输出的最优粒子的周围。

可选地，将每个粒子对应的所有激光点扫描到的障碍物到地图上最近障碍物的距离之和最小的粒子确定为最优粒子。

可选地，融合的方法采用卡尔曼滤波算法。

可选地，该方法还包括：判断最优粒子是否合格，并将合格的最优粒子的位置信息与初始定位位置信息进行融合；其中，若最优粒子满足与地图上障碍物匹配较好且连续的激光点不少于预设数量N_min、与地图上匹配较差的激光点不多于预设数量N_max中的至少一项，则判定最优粒子合格，其中，匹配较好是指第i个激光点扫描的障碍物到地图上最近障碍物的距离d_i小于预设的最小距离阈值d_min，连续是指一条激光束中至少相邻三个激光点扫描的障碍物到地图上最近障碍物的距离同时小于d_min，匹配较差是指d_i大于预设的最大距离阈值d_max。

可选地，其中，所述待定位设备包括自动驾驶设备，所述定位传感器包括全球定位系统、惯性测量单元、里程计。

本公开的再一个方面提出一种定位装置，包括：初始定位模块，用于根据待定位设备的至少一种定位传感器的定位位置信息确定待定位设备本次定位的初始定位位置信息；粒子分布模块，用于随机分布若干粒子，其中的部分粒子分布在初始定位位置信息的周围；最优粒子模块，用于根据每个粒子与地图的匹配程度选出最优粒子；融合定位模块，用于将最优粒子的位置信息与本次定位的初始定位位置信息进行融合，将融合后的定位位置信息确定为待定位设备本次定位的最终定位位置信息。

其中，在随机分布的若干粒子中，还有部分粒子分布在上一次定位输出的最优粒子的周围。