CN112767476A - 一种快速定位系统、方法及应用 - Google Patents

Abstract

本申请属于定位导航技术领域，特别是涉及一种快速定位系统、方法及应用。现有的一些比较快速的重定位方法，通常需要视觉信息的辅助，机器人配置比较复杂，而且，视觉的方法并不完全可靠，受到场景光照变化的影响较大。本申请提供了一种快速定位系统，包括子图构建模块、区域特征提取模块、区域特征匹配模块和粒子群搜索匹配定位模块；子图构建模块，用于获取局部区域360度的地图；区域特征提取模块，用于预先提取该区域的结构特征，并用直方图、有序数据序列的形式表示；区域特征匹配模块，用于实现当前子图特征与地图多个区域的特征进行匹配，快速筛选出最近似的区域；粒子群搜索匹配定位模块，用于获取精确定位。能够快速的实现重定位。

Description

一种快速定位系统、方法及应用

技术领域

本申请属于定位导航技术领域，特别是涉及一种快速定位系统、方法及应用。

背景技术

机器人在运行过程中，不可避免的会因为人为干扰、环境干扰(如，绑架问题、任意位置开机等等)等因素，暂时失去自身的位置，从而导致无法自主移动。在外界干扰去除后，如何实现快速的定位是机器人研发的关键技术。

目前，多数自主移动机器人采用激光传感器构建的地图实现定位导航，而已有单独基于激光实现重定位的传统方法，主要直接采用粒子滤波匹配等方式进行，由于激光数据的直接匹配算法计算量较大，而且激光数据表述的场景本身具有信息量小重复度高的缺陷，因此，传统方法存在候选位置过多，成功率不高、收敛慢的问题；

现有的一些比较快速的重定位方法，通常需要视觉信息的辅助，机器人配置比较复杂，而且，视觉的方法并不完全可靠，受到场景光照变化的影响较大。

发明内容

1.要解决的技术问题

基于激光导航的移动服务机器人在实际运行过程中，因为人为干扰、场景变化、自身测量噪声等的影响，不可避免的会出现定位丢失的情况。如何在定位丢失的情况下，重新找回自身位置，成为需要解决的问题，本申请提供了一种快速定位系统、方法及应用。

2.技术方案

为了达到上述的目的，本申请提供了一种快速定位系统，包括子图构建模块、区域特征提取模块、区域特征匹配模块和粒子群搜索匹配定位模块；所述子图构建模块，用于获取局部区域360度的地图；所述区域特征提取模块，用于预先提取该区域的结构特征，并用直方图、有序数据序列的形式表示，便于快速比对和精确匹配；所述区域特征匹配模块，用于实现当前子图特征与地图多个区域的特征进行匹配，快速筛选出最近似的区域，用于粒子群搜索匹配定位；所述粒子群搜索匹配定位模块，用于获取精确定位。

2、本申请提供的另一种实施方式为：所述区域特征提取模块包括区域选取子模块，区域预处理子模块、区域特征提取子模块和区域特征描述子模块；所述区域选取子模块，用于遍历地图选取不同的区域；所述区域预处理子模块，用于对每一个所述区域进行标识；所述区域特征提取子模块，用于计算每个所述区域直方图特征；所述区域特征描述子模块，用于存储地图所有区域特征计算的结果。

本申请提供的另一种实施方式为：所述区域特征匹配模块包括粗匹配子模块和精细匹配子模块；所述粗匹配子模块，用于对待匹配对象的直方图特征进行快速比对；所述精细匹配子模块，当直方图特征比对通过，对所述待匹配对象的有序数据序列进行循环移位精确比对。

本申请还提供一种快速定位方法，采用所述快速定位系统进行定位。

本申请提供的另一种实施方式为：所述方法包括如下步骤：步骤1：构建全局栅格地图，对所述全局栅格地图的区域进行划分，提取每个所述区域的特征一；步骤2：构建局部子图，提取所述子图的特征二；步骤3：将所述特征一与所述特征二进行快速匹配，得到符合阈值的候选位姿集合；步骤4：对所述候选集合进行粒子滤波匹配跟踪得到精确定位。

本申请提供的另一种实施方式为：所述步骤1中所述区域处于所述全局栅格地图中的无障碍区域。

本申请提供的另一种实施方式为：根据所述特征一得到位置-场景特征集合。

本申请提供的另一种实施方式为：所述步骤2中基于当前激光扫描数据，构建所述局部子图。

本申请提供的另一种实施方式为：所述局部子图采用360°激光雷达，直接使用一帧激光雷达数据，构建所述局部子图；或者采用在原地旋转一周，通过局部区域的同步定位与地图绘制算法，构建所述局部子图。

本申请提供的另一种实施方式为：所述步骤4将所述候选位子作为粒子滤波定位的初始位置，采用基于激光雷达的传统蒙特卡洛-粒子滤波匹配跟踪定位算法，进行定位跟踪。

本申请还提供一种快速定位方法的应用，将所述的快速定位方法应用于机器人重定位中。

3.有益效果

与现有技术相比，本申请提供的一种快速定位系统、方法及应用的有益效果在于：

本申请提供的快速定位方法，涉及一种激光地图场景结构特征。

本申请提供的快速定位方法，具体涉及一种运用激光雷达传感器数据，在栅格地图中实现快速定位的方法。通过该方法，使得机器人在因任意位置启动、人为干扰等因素导致的定位跟踪丢失情况下，能够快速的实现重定位。

本申请提供的快速定位方法，为一种基于场景结构特征提取与匹配的方法，仅仅依靠激光数据就可以实现，配置使用简单，而且多数场景下收敛速度较快，比较实用。

本申请提供的快速定位方法的应用，针对机器人在运行过程中可能出现的定位丢失、定位失败问题，本申请基于已有地图和激光扫描测距传感器数据，提出一种新的地图场景结构特征：对角距离向量特征及其直方图特征，同过该特征的匹配与搜索，可以实现快速的候选位置查找，进而大大加快机器人重定位的速度与可靠性。相比于多数基于激光导航的机器人定位方法，本申请方法在多数场景下可实现快速而可靠的重定位，且无需视觉辅助。

本申请提供的快速定位方法，相比于传统的单一激光雷达数据直接匹配定位的方法，收敛速度与可靠性可以大大提升。

本申请提供的快速定位方法，不依赖视觉辅助，系统与算法结构可以大大简化，而且不受到光照变化影响。

附图说明

图1是本申请的快速定位方法流程示意图。

具体实施方式

在下文中，将参考附图对本申请的具体实施例进行详细地描述，依照这些详细的描述，所属领域技术人员能够清楚地理解本申请，并能够实施本申请。在不违背本申请原理的情况下，各个不同的实施例中的特征可以进行组合以获得新的实施方式，或者替代某些实施例中的某些特征，获得其它优选的实施方式。

参见图1，本申请提供一种快速定位系统，包括子图构建模块、区域特征提取模块、区域特征匹配模块和粒子群搜索匹配定位模块；所述子图构建模块，用于获取局部区域360度的地图；所述区域特征提取模块，用于预先提取该区域的结构特征，并用直方图、有序数据序列的形式表示，便于快速比对和精确匹配；所述区域特征匹配模块，用于实现当前子图特征与地图多个区域的特征进行匹配，快速筛选出最近似的区域，用于粒子群搜索匹配定位；所述粒子群搜索匹配定位模块，用于获取精确定位。

该子图构建模块通过机器人在原地或者小范围360度旋转，采用多个连续帧激光雷达数据，构建小范围地图；当机器人使用360度可探测的激光雷达或者激光雷达组合时，可以直接采用当前帧的激光数据，生成局部区域360度的地图。

该粒子群搜索匹配定位模块，对于匹配过程中，将与当前子图区域匹配度高的若干区域，取其在全地图的中心位置及方向角，作为机器人位置猜测的若干初始粒子，采用基于蒙特卡洛粒子滤波匹配的方法，计算机器人最终的精确定位。

进一步地，所述区域特征提取模块包括区域选取子模块，区域预处理子模块、区域特征提取子模块和区域特征描述子模块。其中，区域选取子模块，主要用于从地图中遍历的选取不同的区域，区域之间可以允许有交集，但子区域的合集应该覆盖地图全部可通行区域。区域预处理子模块，主要用于对每一个区域，进行障碍物、非障碍物、中心点的标识。区域特征提取子模块，主要用于对每个区域，以中心点为原点，依次计算其对角方向上障碍点之间的距离，并按角度顺序排序，作为有序数据序列特征，并进一步计算直方图特征。区域特征描述子模块，主要用于存储地图所有区域特征计算的结果，包括区域中心位置、有序数据序列特征、直方图特征等。

该区域选取子模块：对于全地图，采用固定尺寸大小的窗口，从地图中按照一定的步长截取多个区域，且区域中心点必需位于地图上的无障碍区域；对于该构建的当前子图，直接采用子图整体作为需要提取特征的区域，子图形成的区域以机器人作为区域中心。

该区域的预处理子模块：对于区域中空白的位置(无障碍的位置)标识为0，对于区域中有障碍的位置，标识为1，对于区域的边界上，如果其为无障碍，标识为2。

该区域的特征提取子模块：以区域中心为原点，计算相反方向对的障碍点之间的距离d，例如，可取如下方向对：(0°，180°)，(1°，181°)，(2°，182°)，...，(179°，359°)，将d记录。当相反方向对上找不到障碍点位置时(沿着某方向一直搜索非0点，但找不到，直到标识为2的边界)，可分为如下两种情况：一种是其中一个方向找不到障碍点，将d记为DA，另一种是两个方向上都找不到，将d记为DB。从而组成对角距离向量特征[d1，d2，...，dn]。

该区域特征的描述子模块：基于前述对角距离向量特征，将d按照大小量化分类，并统计同一个分类下的点对个数，计算直方图特征，用于快速匹配。对角距离向量特征用于精确匹配。

进一步地，所述区域特征匹配模块包括粗匹配子模块和精细匹配子模块。其中，粗匹配子模块主要对两个待匹配对象的直方图特征进行快速比对；当直方图特征比对通过的前提下，进入精细匹配子模块，即，对两个待匹配对象的有序数据序列进行循环移位精确比对。

该粗匹配子模块，对于2个区域(其中一个为当前子图区域，另一个为全地图中获取的区域)，直接比对直方图特征，低于阈值的舍弃，高于阈值的保留。

该精细匹配子模块，前一步骤通过的区域图，循环比对特征d序列，匹配度高于阈值的，保留，并提取旋转角度差，计算方向角。

本申请还提供一种快速定位方法，所述方法包括如下步骤：步骤1：构建全局栅格地图，对所述全局栅格地图的区域进行划分，提取每个所述区域的特征一；步骤2：构建局部子图，提取所述子图的特征二；步骤3：将所述特征一与所述特征二进行快速匹配，得到符合阈值的候选位姿集合；步骤4：对所述候选集合进行粒子滤波匹配跟踪得到精确定位。

进一步地，所述步骤1中所述区域处于所述全局栅格地图中的无障碍区域。

进一步地，根据所述特征一得到位置-场景特征集合。

进一步地，所述步骤2中基于当前激光扫描数据，构建所述局部子图。

进一步地，所述局部子图采用360°激光雷达，直接使用一帧激光雷达数据，构建所述局部子图；或者采用在原地旋转一周，通过局部区域的同步定位与地图绘制算法，构建所述局部子图。

进一步地，所述步骤4将所述候选位子作为粒子滤波定位的初始位置，采用基于激光雷达的传统蒙特卡洛-粒子滤波匹配跟踪定位算法，进行定位跟踪。

本申请还提供一种快速定位方法的应用，将所述的快速定位方法应用于机器人重定位中。

实施例

下面以室内移动机器人为例，详细说明本申请方法实现的细节。

该机器人在平面移动，主要传感器为水平安装的360度激光雷达，其测量距离为8米。机器人通过cartographer、gmapping等建图算法，事先构建好了室内环境2D栅格地图，记为M；机器人在地图中的对应的位置与姿态，记为P。

本申请解决的问题可被描述为：机器人在环境中运行，当给定地图M以及每次激光测量数据scan的情况下，如何快速的确定机器人的正确位置与姿态P。

具体实施步骤如下：

(1)对地图的特征预提取：

对于地图M，以一定的步长(比如，0.2m)提取n个半径为r(r与激光雷达测量最大距离保持基本一致，此处r＝8m)的区域Si。

Si的中心应该处于地图M中的无障碍区域，Si集合的所有区域应该覆盖地图M中全部无障碍区域；

对每一个区域Si，对于区域中空白的位置(无障碍的位置)标识为0，对于区域中有障碍的位置，标识为1，对于区域Si的边界上，如果其为无障碍，标识为2；

(2)区域Si的特征Hi的提取

以Si区域中心为原点，按角度顺序依次计算相反方向对上标识为1的障碍点之间的距离d，例如，可取如下方向对：(0°，180°)，(1°，181°)，(2°，182°)，...，(179°，359°)，将d记录。

当相反方向对上找不到障碍点位置时(沿着某方向一直搜索非0点，但找不到，直到标识为2的边界)，可分为如下两种情况：一种是其中一个方向找不到障碍点，将d记为DA，另一种是两个方向上都找不到,将d记为DB。

将d按照角度顺序排序，得到对角距离向量特征Di＝[d1，d2,...,dn]，可作为区域Si的特征。同时，我们将Di中的d按照大小量化分类，并统计同一个分类下的点对个数，形成直方图，其中，DA和DB单独分类，得到对应的直方图Zi。

则，对于区域Si，其中心位置在地图M中的位置为Pi，其对应特征表述为：

Hi＝{Di，Zi}

计算所有Si的特征，从而得到机器人的位置-场景特征集合：

F＝{(Pi,Hi)}＝{(Pi，{Di，Zi})}

其中，Pi表示地图某点位置，Hi表示对应Pi位置上的特征。

步骤(1)和步骤(2)可以在构建地图后离线执行，不占用机器人正式运行时的时间。

(3)当前子图s的构建及特征h提取

当机器人正式运行时，发现自身位置丢失，则启快速重定位流程。

首先基于当前激光扫描数据，构建局部子图m。这里采用的是360°激光雷达，可直接使用一帧激光雷达数据，构建子图。如果不是360°激光雷达，则机器人可以在原地旋转一周，通过局部区域的SLAM算法，构建子图m。

对于子图m，以机器人位置为区域中心，采用步骤2的方法，提取当前子图区域的特征h＝{Dm，Zm}，其中，Dm为子图m的对角距离向量特征，Zm为对应的直方图特征。

(4)快速匹配

对于当前子图区域特征h，从位置-场景特征集合F＝{(Pi,Hi)}中，搜索最匹配若干Hi及其对应位置Pi。具体分为两个步骤：

(a)直方图快速匹配：直接对比h和每个Hi的直方图特征，即对比Zm和所有的Zi，只有匹配误差小于固定阈值的予以保留；

(b)精细匹配：对于前一步骤得到的符合条件的Hi，通过角度循环移位，提取误差最小的对应角度。具体方法如下：

设Dm和Di的长度为n，Dm_k为Dm循环移位k位后的向量，计算公式如下：

e为匹配误差，对应的偏差角为:

当匹配误差小于阈值时，保留此Hi,并且从F中索引到对应的位置Pi。从而得到一个候选位置及姿态{Pi，θ}。

通过快速匹配步骤，可以得到N个符合阈值条件的候选位姿。

如果N＝0，则机器人移动到下一个位置，重新进行步骤(3-4)，直到N>0为止。

(5)粒子滤波匹配跟踪

对于(4)中得到的N个候选位姿，将其作为粒子滤波定位的初始位置，采用基于激光雷达的传统蒙特卡洛-粒子滤波匹配跟踪定位算法，进行机器人的定位跟踪。如果之后出现跟踪失败，则重新启动本算法。

尽管在上文中参考特定的实施例对本申请进行了描述，但是所属领域技术人员应当理解，在本申请公开的原理和范围内，可以针对本申请公开的配置和细节做出许多修改。本申请的保护范围由所附的权利要求来确定，并且权利要求意在涵盖权利要求中技术特征的等同物文字意义或范围所包含的全部修改。

Claims (10)

1.一种快速定位系统，其特征在于：包括子图构建模块、区域特征提取模块、区域特征匹配模块和粒子群搜索匹配定位模块；

所述子图构建模块，用于获取局部区域360度的地图；

所述区域特征提取模块，用于预先提取区域的结构特征，并用直方图、有序数据序列的形式表示，便于快速比对和精确匹配；

所述区域特征匹配模块，用于实现当前子图特征与地图多个区域的特征进行匹配，快速筛选出最近似的区域；

所述粒子群搜索匹配定位模块，用于获取精确定位。

2.如权利要求1所述的快速定位系统，其特征在于：所述区域特征提取模块包括区域选取子模块，区域预处理子模块、区域特征提取子模块和区域特征描述子模块；

所述区域选取子模块，用于遍历地图选取不同的区域；

所述区域预处理子模块，用于对每一个所述区域进行标识；

所述区域特征提取子模块，用于计算每个所述区域直方图特征；

所述区域特征描述子模块，用于存储地图所有区域特征计算的结果。

3.如权利要求1所述的快速定位系统，其特征在于：所述区域特征匹配模块包括粗匹配子模块和精细匹配子模块；

所述粗匹配子模块，用于对待匹配对象的直方图特征进行快速比对；

所述精细匹配子模块，当直方图特征比对通过，对所述待匹配对象的有序数据序列进行循环移位精确比对。

4.一种快速定位方法，其特征在于：采用所述权利要求1～3中任一项所述的快速定位系统进行定位。

5.如权利要求4所述的快速定位方法，其特征在于：所述方法包括如下步骤：

步骤1：构建全局栅格地图，对所述全局栅格地图的区域进行划分，提取每个所述区域的特征一；

步骤2：构建局部子图，提取所述子图的特征二；

步骤3：将所述特征一与所述特征二进行快速匹配，得到符合阈值的候选位姿集合；

步骤4：对所述候选集合进行粒子滤波匹配跟踪得到精确定位。

6.如权利要求5所述的快速定位方法，其特征在于：所述步骤1中所述区域处于所述全局栅格地图中的无障碍区域。

7.如权利要求5所述的快速定位方法，其特征在于：根据所述特征一得到位置-场景特征集合，所述特征一采用直方图、有序数据序列的形式表示；所述步骤2中基于当前激光扫描数据，构建所述局部子图。

8.如权利要求7所述的快速定位方法，其特征在于：所述局部子图采用360°激光雷达，直接使用一帧激光雷达数据，构建所述局部子图；或者采用在原地旋转一周，通过局部区域的同步定位与地图绘制算法，构建所述局部子图。

9.如权利要求5所述的快速定位方法，其特征在于：所述步骤4将所述候选位子作为粒子滤波定位的初始位置，采用基于激光雷达的传统蒙特卡洛-粒子滤波匹配跟踪定位算法，进行定位跟踪。

10.一种快速定位方法的应用，其特征在于：将权利要求4～9中任一项所述的快速定位方法应用于机器人重定位中。

Images