定位方法和定位装置及计算机可读存储介质
技术领域
本公开涉及定位领域,特别涉及一种定位方法和定位装置及计算机可读存储介质。
背景技术
基于激光雷达的定位方案中,粒子滤波算法是最常用的减小误差方法之一。粒子滤波算法从大量粒子中选取一个最优的粒子作为待定位设备的真实位置。
发明内容
发明人发现,在复杂的定位环境中,例如,地面不平、环境几何信息特征不明显,粒子滤波算法可能会出现错误的选出最优粒子的情况,下一次定位时,会以上次选出的最优粒子为基础分布粒子,从而造成连续多次选出错误的最优粒子,出现定位发散现象。如图1所示,第一次定位由于环境信息的干扰导致粒子滤波选出一个错误的“最优”粒子,第二次定位将第一次定位所选的“最优”粒子作为自动驾驶设备的“真实位置”,导致粒子重分布时自动驾驶设备真正的真实位置附近几乎没有粒子,由于必须要选出一个最优粒子,所以又错误的选出了一个“最优”粒子,第三次定位的定位位置已经完全发散,定位位置离真实位置越来越远。
本公开实施例所要解决的一个技术问题是:定位发散问题。
本公开的一个方面提出一种定位方法,包括:根据待定位设备的至少一种定位传感器的定位位置信息确定待定位设备本次定位的初始定位位置信息;随机分布若干粒子,其中的部分粒子分布在初始定位位置信息的周围;根据每个粒子与地图的匹配程度选出最优粒子;将最优粒子的位置信息与本次定位的初始定位位置信息进行融合,将融合后的定位位置信息确定为待定位设备本次定位的最终定位位置信息。
其中,在随机分布的若干粒子中,还有部分粒子分布在上一次定位输出的最优粒子的周围。
可选地,将每个粒子对应的所有激光点扫描到的障碍物到地图上最近障碍物的距离之和最小的粒子确定为最优粒子。
可选地,融合的方法采用卡尔曼滤波算法。
可选地,该方法还包括:判断最优粒子是否合格,并将合格的最优粒子的位置信息与初始定位位置信息进行融合;其中,若最优粒子满足与地图上障碍物匹配较好且连续的激光点不少于预设数量Nmin、与地图上匹配较差的激光点不多于预设数量Nmax中的至少一项,则判定最优粒子合格,其中,匹配较好是指第i个激光点扫描的障碍物到地图上最近障碍物的距离di小于预设的最小距离阈值dmin,连续是指一条激光束中至少相邻三个激光点扫描的障碍物到地图上最近障碍物的距离同时小于dmin,匹配较差是指di大于预设的最大距离阈值dmax。
可选地,其中,所述待定位设备包括自动驾驶设备,所述定位传感器包括全球定位系统、惯性测量单元、里程计。
本公开的再一个方面提出一种定位装置,包括:初始定位模块,用于根据待定位设备的至少一种定位传感器的定位位置信息确定待定位设备本次定位的初始定位位置信息;粒子分布模块,用于随机分布若干粒子,其中的部分粒子分布在初始定位位置信息的周围;最优粒子模块,用于根据每个粒子与地图的匹配程度选出最优粒子;融合定位模块,用于将最优粒子的位置信息与本次定位的初始定位位置信息进行融合,将融合后的定位位置信息确定为待定位设备本次定位的最终定位位置信息。
其中,在随机分布的若干粒子中,还有部分粒子分布在上一次定位输出的最优粒子的周围。
可选地,所述最优粒子模块,用于将每个粒子对应的所有激光点扫描到的障碍物到地图上最近障碍物的距离之和最小的粒子确定为最优粒子。
可选地,所述融合定位模块采用卡尔曼滤波算法进行融合处理。
可选地,所述初始定位模块中的定位传感器包括全球定位系统、惯性测量单元、里程计。
可选地,该装置还包括:合格粒子模块,用于判断最优粒子是否合格,所述融合定位模块,用于将合格的最优粒子的位置信息与初始定位位置信息进行融合,将融合后的定位位置信息确定为待定位设备的最终定位位置信息;其中,若最优粒子满足与地图上障碍物匹配较好且连续的激光点不少于预设数量Nmin、与地图上匹配较差的激光点不多于预设数量Nmax中的至少一项,则判定最优粒子合格,其中,匹配较好是指第i个激光点扫描的障碍物到地图上最近障碍物的距离di小于预设的最小距离阈值dmin,连续是指一条激光束中至少相邻三个激光点扫描的障碍物到地图上最近障碍物的距离同时小于dmin,匹配较差是指di大于预设的最大距离阈值dmax。
本公开的又一个方面提出一种定位装置,包括:存储器;以及耦接至所述存储器的处理器,所述处理器被配置为基于存储在所述存储器中的指令,执行前述的定位方法。
本公开的另一个方面提出一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现前述的定位方法的步骤。
本公开每次定位时在定位传感器确定的初始定位位置周围分布一些粒子,并将最优粒子位置与初始定位位置融合后的位置信息确定为最终定位位置,可以解决定位发散问题,即使由于环境复杂而错选最优粒子,最优粒子依然在真实位置附近,不会产生太大的定位误差。
附图说明
下面将对实施例或相关技术描述中所需要使用的附图作简单地介绍。根据下面参照附图的详细描述,可以更加清楚地理解本公开,
显而易见地,下面描述中的附图仅仅是本公开的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1示出定位发散现象示意图。
图2示出本公开基于粒子滤波实现的定位方法一个实施例的流程图。
图3示出本公开粒子对应的激光点与地图的匹配情况示意图。
图4示出本公开解决定位发散问题的效果示意图。
图5为本公开基于粒子滤波实现的定位方法再一个实施例的流程图。
图6为本公开定位装置的一个实施例的结构图。
图7为本公开定位装置的再一个实施例的结构图。
具体实施方式
下面将结合本公开实施例中的附图,对本公开实施例中的技术方案进行清楚、完整地描述。
粒子滤波,是指通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数,用样本均值代替积分运算,进而获得系统状态的最小方差估计的过程,这些样本被形象的称为“粒子”,故而叫粒子滤波。
在本公开的待定位设备(如自动驾驶设备)的定位中,会用大量粒子模拟待定位设备的定位传感器测量的误差分布,根据每个粒子与地图的匹配程度选出最优粒子,以最优粒子的位置代替待定位设备的真实位置。
图2为本公开基于粒子滤波实现的定位方法一个实施例的流程图。如图2所示,每次定位时执行以下步骤:
步骤210,根据待定位设备的至少一种定位传感器的定位位置信息确定待定位设备本次定位的初始定位位置信息。
其中,定位传感器例如包括全球定位系统、惯性测量单元、里程计。其中,惯性测量单元(IMU)是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的,一个IMU包含了三个单轴的加速度计和三个单轴的陀螺,加速度计检测物体在载体坐标系统独立三轴的加速度信号,而陀螺检测载体相对于导航坐标系的角速度信号,测量物体在三维空间中的角速度和加速度。
例如,如果是第一次定位,可以主要根据全球定位系统的定位位置信息来确定本次定位的初始定位位置信息。如果不是第一次定位,可以主要根据上一次定位的最终定位位置信息,以及里程计和惯性测量单元的定位位置信息来确定本次定位的初始定位位置信息。
步骤220,根据初始定位位置信息重新设计粒子分布,即,随机分布若干粒子,其中有部分粒子分布在本次定位的初始定位位置信息的周围,还有部分粒子分布在上一次定位输出的最优粒子的周围。
其中,如果是第一次定位,则没有上一次定位的输出,可以将粒子主要分布在本次定位的初始定位位置信息的周围。
步骤230,根据每个粒子与地图的匹配程度选出最优粒子。
例如,假设每个粒子在地图中的位置为待定位设备在地图坐标系下的位置,这样每个粒子便得到当前激光传感器的每个激光点扫描的障碍物在地图坐标系下的位置,可以计算每个激光点扫描的障碍物到地图上最近障碍物的距离,然后将每个粒子对应的所有激光点扫描到的障碍物到地图上最近障碍物的距离之和最小的粒子确定为最优粒子。图3示出粒子对应的激光点与地图的匹配情况示意图。公式表示如下:
其中,di表示第i个激光点扫描的障碍物到地图上最近障碍物的距离,n为一束激光中的激光点个数,Ds表示所有激光点扫描到的障碍物到地图上最近障碍物的距离之和。Ds最小的粒子确定为最优粒子。
其中,可以根据初始定位位置信息加载相应位置的地图,以减小加载的信息量,提高加载速度。
步骤240,将最优粒子的位置信息与本次定位的初始定位位置信息进行融合,将融合后的定位位置信息确定为待定位设备本次定位的最终定位位置信息。本次定位结束。
其中,融合的方法例如可以采用卡尔曼滤波算法,或者,采用平均或加权平均等方法。卡尔曼滤波算法是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。
每次定位时,重复执行步骤210~240完成各次定位。
上述实施例,每次定位时在定位传感器确定的初始定位位置周围分布一些粒子,并将最优粒子位置与初始定位位置融合后的位置信息确定为最终定位位置,可以解决定位发散问题,即使由于环境复杂而错选最优粒子,最优粒子依然在真实位置附近,不会产生太大的定位误差。
图4示出本公开解决定位发散问题的效果示意图。假设第一次定位由于环境信息的干扰选错“最优”粒子。在第二次定位时,即使前一次定位选错“最优”粒子,按照本公开的方案,仍然使一部分粒子分布在定位传感器确定的初始定位位置附近,避免错误继续发散。在第三次定位时,假设定位环境好转,选择了正确的最优粒子,使粒子滤波定位重新收敛到真实位置附近。
图5为本公开基于粒子滤波实现的定位方法再一实施例的流程图。如图5所示,每次定位时执行以下步骤(其中,步骤510、520、530、550的具体实现可以分别参考步骤210、220、230、240,这里不再赘述)。
步骤510,根据待定位设备的至少一种定位传感器的定位位置信息确定待定位设备本次定位的初始定位位置信息。
例如,如果是第一次定位,可以主要根据全球定位系统的定位位置信息来确定本次定位的初始定位位置信息。如果不是第一次定位,可以主要根据上一次定位的最终定位位置信息,以及里程计和惯性测量单元的定位位置信息来确定本次定位的初始定位位置信息。
步骤520,根据初始定位位置信息重新设计粒子分布,即,随机分布若干粒子,其中有部分粒子分布在本次定位的初始定位位置信息的周围,还有部分粒子分布在上一次定位输出的最优粒子的周围。
其中,如果是第一次定位,则没有上一次定位的输出,可以将粒子主要分布在本次定位的初始定位位置信息的周围。
步骤530,根据每个粒子与地图的匹配程度选出最优粒子。
步骤540,判断最优粒子是否合格,若合格,执行步骤550,若不合格,不输出本次粒子滤波的定位结果,可选择地,例如可以用本次的初始定位位置信息作为待定位设备本次定位的最终定位位置信息。
例如,若最优粒子满足与地图上障碍物匹配较好且连续的激光点不少于预设数量Nmin、与地图上匹配较差的激光点不多于预设数量Nmax中的至少一项,则判定最优粒子合格。若两项均满足,则最优粒子更可靠。作为一种示例,Nmin例如可以设置为n的10%,Nmax例如可以设置为n的50%,n为一束激光中的激光点个数,但不限于所举示例。
其中,匹配较好是指第i个激光点扫描的障碍物到地图上最近障碍物的距离di小于预设的最小距离阈值dmin,连续是指一条激光束中至少相邻三个激光点扫描的障碍物到地图上最近障碍物的距离同时小于dmin,匹配较差是指di大于预设的最大距离阈值dmax。
公式表示如下:
设N1为最优粒子所对应的一束激光中匹配较好且连续的激光点个数,N2为匹配较差的激光点个数,n为一束激光中激光点的总个数。
其中,条件1例如为di-1<dmin,di<dmin,di+1<dmin。
其中,条件2例如为di>dmax。
若N1≥Nmin、N2≤Nmax中的至少一项满足,可以判定最优粒子合格。优选地,两项均满足。若两项均满足,则最优粒子更可靠。
步骤540的一种示例性的实现流程如下:
步骤541,计算最优粒子的每个激光点i扫描的障碍物到地图上最近障碍物的距离di,然后分别进入步骤542与步骤544。
步骤542,判断最优粒子中是否有三个以上连续激光点扫描到的障碍物到地图上最近障碍物的距离di同时小于最小值dmin,如果是,进入步骤543,如果否,返回步骤510。
其中,dmin为预设的在激光扫描过程中允许最优粒子每个激光点扫描的障碍物到地图上最近障碍物的距离的最小值。
步骤543,运用公式2与公式3运算得出数值N1,进入步骤546。
其中,N1为最优粒子所对应的一束激光中匹配较好且连续的激光点个数。
步骤544,判断最优粒子中每个激光点所对应的di是否大于最大距离阈值dmax,如果是,进入步骤545,如果否,返回步骤510。
其中,dmax为预设的在激光扫描过程中允许最优粒子每个激光点扫描的障碍物到地图上最近障碍物的距离的最大值。
步骤545,运用公式4与公式5运算得出数值N2,进入步骤546。
其中,N2为匹配较差的激光点个数。
步骤546,根据最优粒子需要满足的其中一种示例性的条件,判断N1≥Nmin、N2≤Nmax是否同时满足。若同时满足,最优粒子合格,执行步骤550,否则,最优粒子不合格,不输出本次粒子滤波的定位结果,可选择地,例如可以用本次的初始定位位置信息作为待定位设备本次定位的最终定位位置信息(步骤547)。
步骤550,将合格的最优粒子的位置信息与本次定位的初始定位位置信息进行融合,将融合后的定位位置信息确定为待定位设备本次定位的最终定位位置信息。本次定位结束。
每次定位时,重复执行步骤510~550完成各次定位。
上述实施例,在判断最优粒子合格后,再将最优粒子位置与初始定位位置融合后的位置信息确定为最终定位位置,可以使参与位置融合的最优粒子更加可靠,进而使确定的最终定位位置更加可靠。
图6为本公开定位装置的一个实施例的结构图。如图6所示,该实施例的装置60包括:存储器610以及耦接至该存储器610的处理器620,处理器620被配置为基于存储在存储器610中的指令,执行前述任意一个实施例中的定位方法。
其中,存储器610例如可以包括系统存储器、固定非易失性存储介质等。系统存储器例如存储有操作系统、应用程序、引导装载程序(Boot Loader)以及其他程序等。
装置60还可以包括输入输出接口630、网络接口640、存储接口650等。这些接口630,640,650以及存储器610和处理器620之间例如可以通过总线660连接。其中,输入输出接口630为显示器、鼠标、键盘、触摸屏等输入输出设备提供连接接口。网络接口640为各种联网设备提供连接接口。存储接口650为SD卡、U盘等外置存储设备提供连接接口。
图7为本公开定位装置的再一个实施例的结构图。如图7所示,该实施例的装置70包括:
初始定位模块710,用于根据待定位设备的至少一种定位传感器的定位位置信息确定待定位设备本次定位的初始定位位置信息;
粒子分布模块720,用于随机分布若干粒子,其中的部分粒子分布在初始定位位置信息的周围;
最优粒子模块730,用于根据每个粒子与地图的匹配程度选出最优粒子;
融合定位模块740,用于将最优粒子的位置信息与本次定位的初始定位位置信息进行融合,将融合后的定位位置信息确定为待定位设备本次定位的最终定位位置信息。
其中,初始定位模块710中的定位传感器包括全球定位系统、惯性测量单元、里程计。
其中,最优粒子模块730,用于将每个粒子对应的所有激光点扫描到的障碍物到地图上最近障碍物的距离之和最小的粒子确定为最优粒子。
其中,融合定位模块740采用卡尔曼滤波算法进行融合处理;
如图7所示,装置70还包括:合格粒子模块735,用于判断最优粒子是否合格,其中,若最优粒子满足与地图上障碍物匹配较好且连续的激光点不少于预设数量Nmin、与地图上匹配较差的激光点不多于预设数量Nmax中的至少一项,则判定最优粒子合格,其中,匹配较好是指第i个激光点扫描的障碍物到地图上最近障碍物的距离di小于预设的最小距离阈值dmin,连续是指一条激光束中至少相邻三个激光点扫描的障碍物到地图上最近障碍物的距离同时小于dmin,匹配较差是指di大于预设的最大距离阈值dmax。相应的,融合定位模块740,用于将合格的最优粒子的位置信息与初始定位位置信息进行融合,将融合后的定位位置信息确定为待定位设备的最终定位位置信息。
上述实施例,每次定位时在定位传感器确定的初始定位位置周围分布一些粒子,并将最优粒子位置与初始定位位置融合后的位置信息确定为最终定位位置,可以解决定位发散问题,即使由于环境复杂而错选最优粒子,最优粒子依然在真实位置附近,不会产生太大的定位误差。此外,还可以在判断最优粒子合格后,再将最优粒子位置与初始定位位置融合后的位置信息确定为最终定位位置,可以使参与位置融合的最优粒子更加可靠,进而使确定的最终定位位置更加可靠。
本公开还提出一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现前述的定位方法的步骤。
本领域内的技术人员应当明白,本公开的实施例可提供为方法、系统、或计算机程序产品。因此,本公开可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用非瞬时性存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本公开是参照根据本公开实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解为可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
以上所述仅为本公开的较佳实施例,并不用以限制本公开,凡在本公开的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本公开的保护范围之内。