CN113534185A - 一种设备定位方法和装置 - Google Patents

Abstract

本发明公开了一种设备定位方法和装置，涉及计算机技术领域。该方法的一具体实施方式包括：获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿；以所述估计的位姿为初始位姿，使用扫描匹配方法对所述初始位姿进行修正，得到扫描匹配位姿；将所述扫描匹配位姿作为所述目标设备的定位结果。该实施方式融合了AMCL和扫描匹配方法，避免了ICP及其相关方法中迭代计算关联信息困难的缺点，同时提出了用于检测定位可靠性的指标及判断条件，保证输出准确的位姿。

Description

一种设备定位方法和装置

技术领域

本发明涉及计算机技术领域，尤其涉及一种设备定位方法和装置。

背景技术

定位方法是指计算测量设备在地图中的位姿(位置和姿态)。该方法也可用于对搭载测量设备的各种移动设备进行定位，如无人车、无人机等。现有定位主要包含以下几种：自适应蒙特卡洛定位方法(A daptive Monte Carlo Localization，缩写AMCL)、AMCL与ICP(Itera tive Closest Point，迭代最近点方法)联合的方法、基于占据值(occu pancyvalue)线性插值的扫描匹配方法(简称扫描匹配方法)。

在实现本发明的过程中，发明人发现现有技术至少存在如下问题：

1、AMCL的定位精度较低，通常在5～10cm之间。虽现有可通过联合ICP方法克服，但仍存在由搜索点关联或搜索点线关联引起的计算复杂缺陷，且缺少用于判断估计结果是否可靠的指标；

2、扫描匹配方法的定位结果易丢失，且丢失后无法重新找回。

发明内容

有鉴于此，本发明实施例提供一种设备定位方法和装置，至少能够解决现有技术中定位精度低且定位结果易丢失的现象。

为实现上述目的，根据本发明实施例的一个方面，提供了一种设备定位方法，包括：

获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿；

以所述估计的位姿为初始位姿，使用扫描匹配方法对所述初始位姿进行修正，得到扫描匹配位姿；

将所述扫描匹配位姿作为所述目标设备的定位结果。

可选的，所述获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿，包括：

以所述目标设备初始位置为中心，产生多个粒子组成粒子群；其中，粒子表示所述目标设备的位姿，粒子群表示位姿的概率分布；

利用里程计获取所述目标设备的里程计数据，结合采样运动模型更新所述粒子群中每个粒子对应的位姿；

获取激光点云数据，将激光点云数据以每个粒子对应的位姿映射到网格地图中，得到当前占据的栅格地图；

计算当前占据的栅格地图与网格地图中障碍物的重合数量，将所述重合数量作为每个粒子的得分，以对所述粒子群进行重采样，最终以粒子群位姿的均值作为AMCL估计的位姿。

可选的，所述使用扫描匹配方法对所述初始位姿进行修正，得到扫描匹配位姿，包括：

确定所述目标设备处于所述初始位姿时，相应激光点云数据在世界坐标系下的坐标；

计算所述坐标在网格地图上的占据概率以及对所述初始位姿的偏导数；

基于激光点云数据、所述占据概率和所述偏导数，对激光点云数据与网格地图中的障碍物进行像素匹配，以重合像素数量最多为目标对所述初始位姿进行修正，得到所述扫描匹配位姿。

可选的，所述网格地图为一级网格地图。

可选的，所述将所述扫描匹配位姿作为所述目标设备的定位结果，还包括：

获取多级网格地图，从二级网格地图开始进行扫描匹配；

在每一级网格地图中，将所述初始位姿和上一级网格地图的扫描匹配位姿作为本次扫描匹配的输入；

使用所述扫描匹配方法分别对所述初始位姿和所述上一级网格地图的扫描匹配位姿进行修正，得到第一位姿和第二位姿；

分别确定所述第一位姿和所述第二位姿下激光点云数据的第一匹配率和第二匹配率，将匹配率较大的所述第一位姿或所述第二位姿作为本次扫描匹配的扫描匹配位姿；

通过多级网格地图的扫描匹配，将最后一级匹配率较大的第三位姿作为所述目标设备的定位结果。

可选的，还包括：

对于所述第一位姿，获取激光点云数据在世界坐标系下的位置；

利用占据值函数计算每个激光点云数据所在位置在相应级别网格地图中的占据值，若占据值大于或等于预订占据阈值，则确定位置被占据；

统计位置被占据的激光点云数据数量，计算所述数量在激光点云数据总数量中的占比，将所述占比作为激光点云数据的第一匹配率。

可选的，所述将最后一级匹配率较大的第三位姿作为所述目标设备的定位结果，包括：对于最后一级所得匹配率较大的第三位姿，若匹配率大于或等于预订匹配率阈值，则将所述第三位姿作为所述目标设备的定位结果。

可选的，在将所述第三位姿作为所述目标设备的定位结果之后，还包括：利用所述第三位姿对所述AMCL中的滤波器状态进行重新赋值，以对所述AMCL进行初始化处理。

可选的，所述目标设备为点云测量设备。

为实现上述目的，根据本发明实施例的另一方面，提供了一种设备定位装置，包括：

初始位姿模块，用于获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿；

位姿修正模块，用于以所述估计的位姿为初始位姿，使用扫描匹配装置对所述初始位姿进行修正，得到扫描匹配位姿；

位姿确定模块，用于将所述扫描匹配位姿作为所述目标设备的定位结果。

可选的，所述初始位姿模块，用于：

以所述目标设备初始位置为中心，产生多个粒子组成粒子群；其中，粒子表示所述目标设备的位姿，粒子群表示位姿的概率分布；

利用里程计获取所述目标设备的里程计数据，结合采样运动模型更新所述粒子群中每个粒子对应的位姿；

获取激光点云数据，将激光点云数据以每个粒子对应的位姿映射到网格地图中，得到当前占据的栅格地图；

计算当前占据的栅格地图与网格地图中障碍物的重合数量，将所述重合数量作为每个粒子的得分，以对所述粒子群进行重采样，最终以粒子群位姿的均值作为AMCL估计的位姿。

可选的，所述位姿修正模块，用于：

确定所述目标设备处于所述初始位姿时，相应激光点云数据在世界坐标系下的坐标；

计算所述坐标在网格地图上的占据概率以及对所述初始位姿的偏导数；

基于激光点云数据、所述占据概率和所述偏导数，对激光点云数据与网格地图中的障碍物进行像素匹配，以重合像素数量最多为目标对所述初始位姿进行修正，得到所述扫描匹配位姿。

可选的，所述网格地图为一级网格地图。

可选的，还包括多级扫描匹配模块，用于：

获取多级网格地图，从二级网格地图开始进行扫描匹配；

在每一级网格地图中，将所述初始位姿和上一级网格地图的扫描匹配位姿作为本次扫描匹配的输入；

使用所述扫描匹配装置分别对所述初始位姿和所述上一级网格地图的扫描匹配位姿进行修正，得到第一位姿和第二位姿；

分别确定所述第一位姿和所述第二位姿下激光点云数据的第一匹配率和第二匹配率，将匹配率较大的所述第一位姿或所述第二位姿作为本次扫描匹配的扫描匹配位姿；

通过多级网格地图的扫描匹配，将最后一级匹配率较大的第三位姿作为所述目标设备的定位结果。

可选的，所述多级扫描匹配模块，还用于：

对于所述第一位姿，获取激光点云数据在世界坐标系下的位置；

利用占据值函数计算每个激光点云数据所在位置在相应级别网格地图中的占据值，若占据值大于或等于预订占据阈值，则确定位置被占据；

统计位置被占据的激光点云数据数量，计算所述数量在激光点云数据总数量中的占比，将所述占比作为激光点云数据的第一匹配率。

可选的，所述多级扫描匹配模块，用于：对于最后一级所得匹配率较大的第三位姿，若匹配率大于或等于预订匹配率阈值，则将所述第三位姿作为所述目标设备的定位结果。

可选的，还包括初始化模块，用于：利用所述第三位姿对所述AMCL中的滤波器状态进行重新赋值，以对所述AMCL进行初始化处理。

可选的，所述目标设备为点云测量设备。

为实现上述目的，根据本发明实施例的再一方面，提供了一种设备定位电子设备。

本发明实施例的电子设备包括：一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现上述任一所述的设备定位方法。

为实现上述目的，根据本发明实施例的再一方面，提供了一种计算机可读介质，其上存储有计算机程序，所述程序被处理器执行时实现上述任一所述的设备定位方法。

根据本发明所述提供的方案，上述发明中的一个实施例具有如下优点或有益效果：提供了一种定位精度高、易于计算、定位不轻易丢失、即使定位丢失后也可恢复的定位方法，该方法融合了AMCL和扫描匹配方法，避免了ICP及其相关方法中迭代计算关联信息困难的缺点，同时提出了用于检测定位可靠性的指标及判断条件，保证输出准确的位姿。

上述的非惯用的可选方式所具有的进一步效果将在下文中结合具体实施方式加以说明。

附图说明

附图用于更好地理解本发明，不构成对本发明的不当限定。其中：

图1是根据本发明实施例的一种设备定位方法的主要流程示意图；

图2是根据本发明实施例的一种可选的设备定位方法的流程示意图；

图3是根据本发明实施例的另一种可选的设备定位方法的流程示意图；

图4是根据本发明实施例的一具体地设备定位方法的流程示意图；

图5是根据本发明实施例的一种设备定位装置的主要模块示意图；

图6是本发明实施例可以应用于其中的示例性系统架构图；

图7是适于用来实现本发明实施例的移动设备或服务器的计算机系统的结构示意图。

具体实施方式

以下结合附图对本发明的示范性实施例做出说明，其中包括本发明实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本发明的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。

参见图1，示出的是本发明实施例提供的一种设备定位方法的主要流程图，包括如下步骤：

S101：获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿；

S102：以所述估计的位姿为初始位姿，使用扫描匹配方法对所述初始位姿进行修正，得到扫描匹配位姿；

S103：将所述扫描匹配位姿作为所述目标设备的定位结果。

上述实施方式中，对于步骤S101，点云测量设备通常固定在机器人上的指定位姿，由此根据点云测量设备的位姿可以推算机器人的位姿。本发明中所提及的目标设备为点云测量设备，例如激光雷达、摄像头等一切可以产生点云数据的测量设备。

AMCL是一种成熟定位方法，该方法以粒子滤波器为框架，使用粒子群表示设备位姿的概率分布，每个粒子附带一个权重值，表示该粒子的正确程度。

AMCL具有定位结果不易丢失、且即使定位丢失后也可以重新找回的优点。作为整个定位方法的第一步骤，其输出的位姿(参见后续图2所示描述)将被传递给扫描匹配方法，作为扫描匹配方法的初始位姿。

本发明假定里程计数据、点云测量设备的激光点云数据以及网格地图数据是提前配置且可以利用的。AMCL估计目标设备位姿的主要步骤如下：

1)通过里程计获取目标设备的里程计数据，例如轮式编码器，从而根据前后时刻的位姿变化量和前一时刻的位姿来估计出当前时刻设备的位姿和运行速度。其中，里程计是预先预备的，在定位算法中通常是单独的一部分，由轮速计、惯导元器件、算法等构成。

2)在所得里程计数据的基础上，通过采样运动模型更新粒子群中每个粒子的位姿。每个粒子都是一个单独的滤波器，均需要估计位姿与新粒子的分布情况。该过程已现有，在此不再赘述。

3)依据点云测量设备测量激光点云数据。以激光雷达为例，向目标设备发射激光束，并对反射激光进行检测和分析，测得目标设备表面每个采样点在雷达坐标系下的坐标点，以此得到一个点的集合，称之为“点云(Point Cloud)”，其属性包括三维空间坐标位置X、Y、Z和反射强度Intensity。

4)将所测得的激光点云数据以每一个粒子对应的位姿映射到网格地图(网格地图以数据形式存储)，得到当前占据的栅格地图。

5)计算当前占据的栅格地图与地图中障碍物的重合数量，并将该重合数量作为每个粒子的得分，以每个粒子的得分作为权重自适应地对粒子群进行重采样，最终以粒子群位姿的均值作为AMCL估计的位姿。

对于步骤S102和S103，扫描匹配方法以AMCL方法估计的位姿为初始位姿，结合激光点云数据和一级网格地图，计算点云测量设备的位姿相对应初始位姿的变化量，以此修正初始位姿，最终估计得到点云测量设备的位姿，即扫描匹配位姿。

定义扫描匹配位姿为ξ＝[x,y,θ]^T，AMCL估计的位姿(即初始位姿)为ξ_A＝[x_A,y_A,θ_A]^T；其中，x和y为世界坐标系下的坐标，θ为点云测量设备坐标系相对于世界坐标系逆时针偏转的角度。

扫描匹配过程的目标是：寻找一个偏差值Δξ，令激光点云数据与网格地图中的障碍物进行像素匹配，以达到最佳对准(重合像素数量最多)，使下式成立：

对上式中的M(S_i(ξ_A+Δξ))进行一阶泰勒展开，求取展开式的最小值(对Δξ求取偏导数并令偏导数为0)，最终求得：

上式中，i表示第i个激光点云数据，每个运行周期内，点云测量设备会上报固定数量(如1440个)的激光点云数据；

函数S_i(ξ_A)是点云测量设备处于位姿ξ_A时，激光扫描点云在世界坐标系下的坐标；M(S_i(ξ_A))为地图在给定坐标点S_i(ξ_A)上的占据概率。S_i(ξ_A)对ξ_A的偏导数为：

H为Hessian矩阵：

上述实施例所提供的方法，扫描匹配方法虽可实现高精度定位且计算较为容易，但易发生定位丢失问题，通过以AMCL估计的位姿作为初始位姿，有效解决了定位易丢失的问题，进而提高了设备定位准确度。

参见图2，示出了根据本发明实施例的一种可选的设备定位方法流程示意图，包括如下步骤：

S201：获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿；

S202：以所述估计的位姿为初始位姿，使用扫描匹配方法对所述初始位姿进行修正，得到扫描匹配位姿；

S203：获取多级网格地图，从二级网格地图开始进行扫描匹配；

S204：在每一级网格地图中，将所述初始位姿和上一级网格地图的扫描匹配位姿作为本次扫描匹配的输入；

S205：使用所述扫描匹配方法分别对所述初始位姿和所述上一级网格地图的扫描匹配位姿进行修正，得到第一位姿和第二位姿；

S206：分别确定所述第一位姿和所述第二位姿下激光点云数据的第一匹配率和第二匹配率，将匹配率较大的所述第一位姿或所述第二位姿作为本次扫描匹配的扫描匹配位姿；

S207：通过多级网格地图的扫描匹配，将最后一级匹配率较大的第三位姿作为所述目标设备的定位结果。

上述实施方式中，对于步骤S201和S202可参见图1所示步骤S101和S102的描述，在此不再赘述。

上述实施方式中，对于步骤S203～S206，仅使用一次扫描匹配方法计算的位姿有可能是不可靠的，因此需要判断扫描匹配估计的位姿是否可靠，本发明采用多级扫描匹配方式进行处理。

多级扫描匹配方式依据2级至N级网格地图逐级递增分辨率，依次计算扫描匹配结果，参见图3所示。主要由输入、扫描匹配方法和定位可靠性指标三部分构成，对于每一级网格地图分别进行两次匹配：

1)以AMCL估计的位姿为初始位姿的匹配；

2)基于上一级网格地图扫描匹配输出的扫描匹配位姿(如ξ)的匹配。

对于每一级网格地图对应的扫描匹配，根据定位可靠性指标评估两次匹配结果，输出较为可靠的匹配结果。

需要说明的是，此处扫描匹配方法与图1中介绍的步骤相同，区别之处仅在于，图1仅使用了一级网格地图，而本实施方式使用的地图依次为2级至N级网格地图。

定义基于上一级网格地图扫描匹配的输出结果为ξ_j＝[x_j,y_j,θ_j]^T,j＝1…(N-1)，共N级网格地图。

在每层网格地图中，分别利用扫描匹配方法，对AMCL估计的初始位姿ξ_A和上一级网格地图扫描匹配输出结果ξ_j进行修正，得到第一位姿和第二位姿。

以第一位姿为例，基于第一位姿，计算每个激光点云数据在世界坐标系下的位置。然后利用占据值函数M(·)，计算每个激光点云数据在相应级别网格地图中所在位置的占据值，比对该占据值与预先设置的占据阈值，以判断该位置是障碍物还是空闲区域，若是障碍物，则确定该激光点云数据被占据。

理论上所有激光点云数据所在的位置都应该被占据，因此被占据的点云数据所在的位置越多，估计的位姿准确的可能性就越大，定位结果就越可靠。

定位可靠性指标为：

匹配率η＝被占据的激光点云数据数量/激光点云数据总数量

匹配率η被用作定位可靠性的依据。通过该匹配率，可以从第一位姿和第二位姿中选择出占据量更为可靠的位姿，并在相应网格地图不是最后一级情况下，将所确定的位姿作为下一级网格地图的输入。

以图3所示的二级网格地图为例，输入至扫描匹配方法的位姿包括：

1)AMCL估计的初始位姿ξ_A；

2)基于一级网格地图修正后的扫描匹配位姿ξ。

扫描匹配方法基于二级网格地图，修正初始位姿ξ_A得到ξ'_A，修正扫描匹配位姿ξ得到ξ'。分别比对ξ'_A和ξ'对应被占据的激光点云数据数量与激光点云数据总数量的比值，假若ξ'较大，则将ξ_A和ξ'作为三级网格地图的输入。

对于步骤S207，在最后一级网格地图中，在将所得匹配率较大的第三位姿作为点云测量设备的定位结果之前，还需要判断该位姿是否定位可靠。定位可靠的条件为：匹配率η大于某一预先设定的阈值(如60％)。反之，若小于，则说明定位失败。

如果定位可靠，整个定位方法将输出最终修正的位姿，并对AMCL进行重新初始化，即使用位姿对AMCL中的滤波器状态重新赋值，以便AMCL能够保持最准确的位姿。但如果定位不可靠，则整个定位方法不输出任何信息，也不调整AMCL，直接进行下一次位姿估计。

本发明实施例所提供的方法，AMCL算法估计出点云测量设备的位姿为粒子群的加权中心，其与点云测量设备实际位姿仍存在一定的偏差。将扫描匹配算法与AMCL结合，将AMCL算法解出的位姿作为初始位姿，在多分辨率网格地图上由粗到精逐层进行扫描匹配，从而得到更加精准的位姿。

参见图4，示出了根据本发明实施例的一种具体地的设备定位方法流程示意图，包括如下步骤：

S401：获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿；

S402：以估计的位姿为初始位姿，使用扫描匹配方法基于一级网格地图对所述初始位姿进行修正，得到扫描匹配位姿；

S403：以所述初始位姿和所述扫描匹配位姿为输入，基于多级扫描匹配方法，得到目标位姿；其中，多级扫描匹配方法对应于多级网格地图，目标位姿为最后一级网格地图的扫描匹配位姿；

S404：判断目标位姿的匹配率是否大于或等于预订匹配率阈值，若是，则将其作为目标设备的定位结果，否则定位失败，直接进入下一次位姿估计。

本发明实施例提供了一种定位精度高、易于计算、定位不轻易丢失、即使定位丢失后也可恢复的定位方法，主要由AMCL、扫描匹配方法、多级扫描匹配方法和定位可靠性判断条件组成。该方法融合了AMCL和扫描匹配方法，避免了ICP及其相关方法中迭代计算关联信息困难的缺点，同时提出了用于检测定位可靠性的指标及判断条件，保证输出准确的位姿。

参见图5，示出了本发明实施例提供的一种设备定位装置500的主要模块示意图，包括：

初始位姿模块501，用于获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿；

位姿修正模块502，用于以所述估计的位姿为初始位姿，使用扫描匹配装置对所述初始位姿进行修正，得到扫描匹配位姿；

位姿确定模块503，用于将所述扫描匹配位姿作为所述目标设备的定位结果。

本发明实施装置中，所述初始位姿模块501，用于：

以所述目标设备初始位置为中心，产生多个粒子组成粒子群；其中，粒子表示所述目标设备的位姿，粒子群表示位姿的概率分布；

利用里程计获取所述目标设备的里程计数据，结合采样运动模型更新所述粒子群中每个粒子对应的位姿；

获取激光点云数据，将激光点云数据以每个粒子对应的位姿映射到网格地图中，得到当前占据的栅格地图；

计算当前占据的栅格地图与网格地图中障碍物的重合数量，将所述重合数量作为每个粒子的得分，以对所述粒子群进行重采样，最终以粒子群位姿的均值作为AMCL估计的位姿。

本发明实施装置中，所述位姿修正模块502，用于：

确定所述目标设备处于所述初始位姿时，相应激光点云数据在世界坐标系下的坐标；

计算所述坐标在网格地图上的占据概率以及对所述初始位姿的偏导数；

基于激光点云数据、所述占据概率和所述偏导数，对激光点云数据与网格地图中的障碍物进行像素匹配，以重合像素数量最多为目标对所述初始位姿进行修正，得到所述扫描匹配位姿。

本发明实施装置中，所述网格地图为一级网格地图。

本发明实施装置还包括多级扫描匹配模块504(图中未标出)，用于：

获取多级网格地图，从二级网格地图开始进行扫描匹配；

在每一级网格地图中，将所述初始位姿和上一级网格地图的扫描匹配位姿作为本次扫描匹配的输入；

使用所述扫描匹配装置分别对所述初始位姿和所述上一级网格地图的扫描匹配位姿进行修正，得到第一位姿和第二位姿；

分别确定所述第一位姿和所述第二位姿下激光点云数据的第一匹配率和第二匹配率，将匹配率较大的所述第一位姿或所述第二位姿作为本次扫描匹配的扫描匹配位姿；

通过多级网格地图的扫描匹配，将最后一级匹配率较大的第三位姿作为所述目标设备的定位结果。

本发明实施装置中，所述多级扫描匹配模块504，还用于：

对于所述第一位姿，获取激光点云数据在世界坐标系下的位置；

利用占据值函数计算每个激光点云数据所在位置在相应级别网格地图中的占据值，若占据值大于或等于预订占据阈值，则确定位置被占据；

统计位置被占据的激光点云数据数量，计算所述数量在激光点云数据总数量中的占比，将所述占比作为激光点云数据的第一匹配率。

本发明实施装置中，所述多级扫描匹配模块504，用于：对于最后一级所得匹配率较大的第三位姿，若匹配率大于或等于预订匹配率阈值，则将所述第三位姿作为所述目标设备的定位结果。

本发明实施装置还包括初始化模块505(图中未标出)，用于：

利用所述第三位姿对所述AMCL中的滤波器状态进行重新赋值，以对所述AMCL进行初始化处理。

本发明实施装置中，所述目标设备为点云测量设备。

另外，在本发明实施例中所述装置的具体实施内容，在上面所述方法中已经详细说明了，故在此重复内容不再说明。

图6示出了可以应用本发明实施例的示例性系统架构600。

如图6所示，系统架构600可以包括终端设备601、602、603，网络604和服务器605(仅仅是示例)。网络604用以在终端设备601、602、603和服务器605之间提供通信链路的介质。网络604可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。

用户可以使用终端设备601、602、603通过网络604与服务器605交互，以接收或发送消息等。终端设备601、602、603上可以安装有各种通讯客户端应用。

终端设备601、602、603可以是具有显示屏并且支持网页浏览的各种电子设备，包括但不限于智能手机、平板电脑、膝上型便携计算机和台式计算机等等。

服务器605可以是提供各种服务的服务器，例如对用户利用终端设备601、602、603所浏览的购物类网站提供支持的后台管理服务器(仅为示例)。

需要说明的是，本发明实施例所提供的方法一般由服务器605执行，相应地，装置一般设置于服务器605中。

应该理解，图6中的终端设备、网络和服务器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端设备、网络和服务器。

下面参考图7，其示出了适于用来实现本发明实施例的终端设备的计算机系统700的结构示意图。图7示出的终端设备仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。

如图7所示，计算机系统700包括中央处理单元(CPU)701，其可以根据存储在只读存储器(ROM)702中的程序或者从存储部分708加载到随机访问存储器(RAM)703中的程序而执行各种适当的动作和处理。在RAM 703中，还存储有系统700操作所需的各种程序和数据。CPU 701、ROM 702以及RAM 703通过总线704彼此相连。输入/输出(I/O)接口705也连接至总线704。

以下部件连接至I/O接口705：包括键盘、鼠标等的输入部分706；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分707；包括硬盘等的存储部分708；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分709。通信部分709经由诸如因特网的网络执行通信处理。驱动器710也根据需要连接至I/O接口705。可拆卸介质711，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器710上，以便于从其上读出的计算机程序根据需要被安装入存储部分708。

特别地，根据本发明公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本发明公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分709从网络上被下载和安装，和/或从可拆卸介质711被安装。在该计算机程序被中央处理单元(CPU)701执行时，执行本发明的系统中限定的上述功能。

需要说明的是，本发明所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本发明中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。

附图中的流程图和框图，图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

描述于本发明实施例中所涉及到的模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的模块也可以设置在处理器中，例如，可以描述为：一种处理器包括初始位姿模块、位姿修正模块、位姿确定模块。其中，这些模块的名称在某种情况下并不构成对该模块本身的限定，例如，初始位姿模块还可以被描述为“AMCL初始位姿模块”。

作为另一方面，本发明还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的设备中所包含的；也可以是单独存在，而未装配入该设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被一个该设备执行时，使得该设备包括：

获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿；

以所述估计的位姿为初始位姿，使用扫描匹配装置对所述初始位姿进行修正，得到扫描匹配位姿；

将所述扫描匹配位姿作为所述目标设备的定位结果。

根据本发明实施例的技术方案，提供了一种定位精度高、易于计算、定位不轻易丢失、即使定位丢失后也可恢复的定位方法，主要由AMCL、扫描匹配方法、多级扫描匹配方法和定位可靠性判断条件组成。该方法融合了AMCL和扫描匹配方法，避免了ICP及其相关方法中迭代计算关联信息困难的缺点，同时提出了用于检测定位可靠性的指标及判断条件，保证输出准确的位姿。

上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，取决于设计要求和其他因素，可以发生各种各样的修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。

Claims (12)

1.一种设备定位方法，其特征在于，包括：

获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿；

以所述估计的位姿为初始位姿，使用扫描匹配方法对所述初始位姿进行修正，得到扫描匹配位姿；

将所述扫描匹配位姿作为所述目标设备的定位结果。

2.根据权利要求1所述的方法，其特征在于，所述获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿，包括：

以所述目标设备初始位置为中心，产生多个粒子组成粒子群；其中，粒子表示所述目标设备的位姿，粒子群表示位姿的概率分布；

利用里程计获取所述目标设备的里程计数据，结合采样运动模型更新所述粒子群中每个粒子对应的位姿；

获取激光点云数据，将激光点云数据以每个粒子对应的位姿映射到网格地图中，得到当前占据的栅格地图；

计算当前占据的栅格地图与网格地图中障碍物的重合数量，将所述重合数量作为每个粒子的得分，以对所述粒子群进行重采样，最终以粒子群位姿的均值作为AMCL估计的位姿。

3.根据权利要求1所述的方法，其特征在于，所述使用扫描匹配方法对所述初始位姿进行修正，得到扫描匹配位姿，包括：

确定所述目标设备处于所述初始位姿时，相应激光点云数据在世界坐标系下的坐标；

计算所述坐标在网格地图上的占据概率以及对所述初始位姿的偏导数；

基于激光点云数据、所述占据概率和所述偏导数，对激光点云数据与网格地图中的障碍物进行像素匹配，以重合像素数量最多为目标对所述初始位姿进行修正，得到所述扫描匹配位姿。

4.根据权利要求3所述的方法，其特征在于，所述网格地图为一级网格地图。

5.根据权利要求1所述的方法，其特征在于，所述将所述扫描匹配位姿作为所述目标设备的定位结果，还包括：

获取多级网格地图，从二级网格地图开始进行扫描匹配；

在每一级网格地图中，将所述初始位姿和上一级网格地图的扫描匹配位姿作为本次扫描匹配的输入；

使用所述扫描匹配方法分别对所述初始位姿和所述上一级网格地图的扫描匹配位姿进行修正，得到第一位姿和第二位姿；

分别确定所述第一位姿和所述第二位姿下激光点云数据的第一匹配率和第二匹配率，将匹配率较大的所述第一位姿或所述第二位姿作为本次扫描匹配的扫描匹配位姿；

通过多级网格地图的扫描匹配，将最后一级匹配率较大的第三位姿作为所述目标设备的定位结果。

6.根据权利要求5所述的方法，其特征在于，还包括：

对于所述第一位姿，获取激光点云数据在世界坐标系下的位置；

利用占据值函数计算每个激光点云数据所在位置在相应级别网格地图中的占据值，若占据值大于或等于预订占据阈值，则确定位置被占据；

统计位置被占据的激光点云数据数量，计算所述数量在激光点云数据总数量中的占比，将所述占比作为激光点云数据的第一匹配率。

7.根据权利要求5所述的方法，其特征在于，所述将最后一级匹配率较大的第三位姿作为所述目标设备的定位结果，包括：

对于最后一级所得匹配率较大的第三位姿，若匹配率大于或等于预订匹配率阈值，则将所述第三位姿作为所述目标设备的定位结果。

8.根据权利要求7所述的方法，其特征在于，在将所述第三位姿作为所述目标设备的定位结果之后，还包括：

利用所述第三位姿对所述AMCL中的滤波器状态进行重新赋值，以对所述AMCL进行初始化处理。

9.根据权利要求1-8中任一项所述的方法，其特征在于，所述目标设备为点云测量设备。

10.一种设备定位装置，其特征在于，包括：

初始位姿模块，用于获取自适应蒙特卡洛定位AMCL对目标设备估计的位姿；

位姿修正模块，用于以所述估计的位姿为初始位姿，使用扫描匹配装置对所述初始位姿进行修正，得到扫描匹配位姿；

位姿确定模块，用于将所述扫描匹配位姿作为所述目标设备的定位结果。

11.一种电子设备，其特征在于，包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序，

当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-9中任一所述的方法。

12.一种计算机可读介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现如权利要求1-9中任一所述的方法。

Images