CN113129379A - 自动移动设备的全局重定位方法以及装置 - Google Patents

自动移动设备的全局重定位方法以及装置 Download PDF

Info

Publication number
CN113129379A
CN113129379A CN202110669388.4A CN202110669388A CN113129379A CN 113129379 A CN113129379 A CN 113129379A CN 202110669388 A CN202110669388 A CN 202110669388A CN 113129379 A CN113129379 A CN 113129379A
Authority
CN
China
Prior art keywords
score
current
optimal solution
solution
feasible
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110669388.4A
Other languages
English (en)
Inventor
崔锦
李想
谭杨
宋永博
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nuctech Co Ltd
Original Assignee
Nuctech Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nuctech Co Ltd filed Critical Nuctech Co Ltd
Priority to CN202110669388.4A priority Critical patent/CN113129379A/zh
Publication of CN113129379A publication Critical patent/CN113129379A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Databases & Information Systems (AREA)
  • Electromagnetism (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Image Analysis (AREA)

Abstract

本发明涉及一种自动移动设备的全局重定位方法以及装置。自动移动设备的全局重定位方法包括：栅格地图获取步骤，获取当前检测区域在多个层级的预设分辨率下的栅格地图；位姿数据获取步骤，基于自动移动设备上的距离传感器的检测数据而获取多组位姿数据；评分排序步骤，针对每组位姿数据，计算在最低分辨率下的栅格地图中的预定位置的得分，并将所述位姿数据作为可行解按得分从大到小排序生成可行解集；以及最优解搜索步骤，在第一个可行解的得分高于当前最低得分阈值时，以所述第一个可行解的位置为起点，在分辨率由低到高的栅格地图中依次扩展来递归搜索最优解。根据上述，能够减少搜索数据量，提高搜索速度。

Description

自动移动设备的全局重定位方法以及装置
技术领域
本发明涉及自动移动设备的全局重定位方法以及装置，特别涉及利用多分辨率地图进行快速定位的自动移动设备的全局重定位方法以及装置。
背景技术
随着人工智能技术的发展，机器人的使用逐渐增多。机器人的全局重定位也是当前使用中经常遇到的问题。在机器人关机后被人为移动位置、或者在定位过程中发生了定位丢失等情况下，为了准确地确定机器人在当前地图中的位姿，根据机器人自身的传感器的检测数据，在全局地图中进行搜索，重新获取在当前地图中的准确位姿。
目前机器人的全局重定位方法大体上可以分为基于激光雷达的全局重定位技术和基于视觉如摄像头的全局重定位技术。
基于激光雷达的全局重定位技术包括以下两种方法。一种是粒子滤波算法，在已知栅格地图的全局生成随机粒子，计算所有粒子的激光雷达数据与当前地图的匹配程度，选取最优粒子进行复制，移动机器人后再对所有粒子重新采样，多次迭代后粒子群收敛的位置即为机器人真实位姿。另一种是通过使用当前雷达数据与当前的栅格地图进行匹配的方式，如通过将整帧激光雷达数据进行特征提取，与人工定义的几何特征进行匹配，或者将整帧激光雷达数据与地图进行ICP（Iterative Closest Point，迭代最近点）匹配等。
基于视觉的全局重定位技术包括以下两种方法。一种是通过对拍摄的图片进行特征点提取，计算特征点和描述子的词袋向量，将词袋向量与提前训练好的视觉词典进行对比与匹配，找到符合要求的相似帧，从而进行全局重定位。另一种是通过神经网络的方法进行全局重定位。
但是，基于激光雷达进行粒子滤波进行全局重定位时，需要让机器人不停的运动，直至粒子收敛，耗费时间长，而且还不一定能够找到正确的位姿。另外，使用匹配的方法需要遍历生成的每一个雷达数据，非常费时。
基于视觉的全局重定位技术虽然计算准确度较高，但是在分析图像时需要很高的计算能力，需要更昂贵的计算设备。另外，目前大多数机器人的导航方式是使用激光雷达的，使用视觉方法进行全局重定位还需要引入一个摄像头，这提高了生成成本与计算设备的成本。
发明内容
本发明考虑了上述问题，提出了一种自动移动设备的全局重定位方法以及装置，以解决上述至少一个问题。
本发明的第一方面涉及一种自动移动设备的全局重定位方法，包括：栅格地图获取步骤，获取当前检测区域在多个层级的预设分辨率下的栅格地图；位姿数据获取步骤，基于自动移动设备上的距离传感器的检测数据而获取多组位姿数据；评分排序步骤，针对每组位姿数据，计算在最低分辨率下的栅格地图中的预定位置的得分，并将所述位姿数据作为可行解按得分从大到小排序生成可行解集；以及最优解搜索步骤，在第一个可行解的得分高于当前最低得分阈值时，以所述第一个可行解的位置为起点，在分辨率由低到高的栅格地图中依次扩展来递归搜索最优解。
根据第一方面涉及的全局重定位方法，所述最优解搜索步骤包括：可行解扩展步骤，针对当前得分最高且得分大于所述当前最低得分阈值的所述第一个可行解，从所在的可行解集中删除，并以该删除的所述第一个可行解的位置为起点，以低一层级分辨率为移动步长，获取包含所述第一个可行解在内的、在所述低一层级分辨率的栅格地图中的扩展数据的集合作为扩展数据集；扩展数据集排序步骤，对所述扩展数据集中的位姿数据在所述低一层级分辨率的栅格地图中进行评分并排序，获取将位姿数据按得分从大到小排序的当前可行解集；以及最优解返回步骤，重复所述可行解扩展步骤以及所述扩展数据集排序步骤，在搜索到最低层级时、或者在当前可行解集中的可行解的个数为零时、或者在当前可行解集中的第一个可行解的得分小于等于所述当前最低得分阈值时，获取当前层级的最优解及其得分，并向上一层级返回，从而从低层级向高层级逐层返回最优解。
根据第一方面涉及的全局重定位方法，在所述递归搜索中，当获取得分高于所述当前最低得分阈值的最优解时，利用所述最优解的得分更新所述当前最低得分阈值。
根据第一方面涉及的全局重定位方法，在所述栅格地图获取步骤中，所述多个层级的预设分辨率的值从小到大按照固定的偶数倍数线性地增长。
根据第一方面涉及的全局重定位方法，在所述位姿数据获取步骤中，包括：旋转数据获取步骤：将所述检测数据以预定的角度步长旋转一周而获取多个旋转检测数据；以及平移步骤，将所述旋转检测数据平移到最低分辨率的栅格地图的预定位置来获取所述位姿数据，所述预定位置为所述起点 。
根据第一方面涉及的全局重定位方法，还包括最优解判断步骤，判断在所述最优解搜索步骤中获取的最优解的得分是否高于预先设定的最优解阈值，如果所述最优解的得分大于所述最优解阈值，则将所述最优解确定为所述自动移动设备的位姿。
根据第一方面涉及的全局重定位方法，将最低分辨率下的栅格地图的中心位置设为所述预定位置。
本发明的第二方面涉及 一种自动移动设备的全局重定位装置，包括：
栅格地图获取单元，获取当前检测区域在多个层级的预设分辨率下的栅格地图；位姿数据获取单元，基于自动移动设备上的距离传感器的检测数据而获取多组位姿数据；评分排序单元，针对每组位姿数据，计算在最低分辨率下的栅格地图中的预定位置的得分，并将所述位姿数据作为可行解按得分从大到小排序生成可行解集；以及最优解搜索单元，在第一个可行解的得分高于当前最低得分阈值时，以所述第一个可行解的位置为起点，在分辨率由低到高的栅格地图中依次扩展来递归搜索最优解。
根据本发明第二方面涉及的全局重定位装置，所述最优解搜索单元包括：可行解扩展单元，针对当前得分最高且得分大于所述当前最低得分阈值的所述第一个可行解，从所在的可行解集中删除，并以该删除的所述第一个可行解的位置为起点，以低一层级分辨率为移动步长，获取包含所述第一个可行解在内的、在所述低一层级分辨率的栅格地图中的扩展数据的集合作为扩展数据集；扩展数据集排序单元，对所述扩展数据集中的位姿数据在所述低一层级分辨率的栅格地图中进行评分并排序，获取将位姿数据按得分从大到小排序的当前可行解集；以及最优解返回单元，重复所述可行解扩展单元以及所述扩展数据集排序单元的动作，在搜索到最低层级时、或者在当前可行解集中的可行解的个数为零时、或者在当前可行解集中的第一个可行解的得分小于等于所述当前最低得分阈值时，获取当前层级的最优解及其得分，并向上一层级返回，从而逐层返回最优解。
根据本发明第二方面涉及的全局重定位装置，在所述递归搜索中，当获取得分高于所述当前最低得分阈值的最优解时，利用所述最优解的得分更新所述当前最低得分阈值。
根据本发明第二方面涉及的全局重定位装置，在所述栅格地图获取单元中，所述多个层级的预设分辨率的值从小到大按照固定的偶数倍数线性地增长。
根据本发明第二方面涉及的全局重定位装置，在所述位姿数据获取单元中，包括：旋转数据获取单元：将所述检测数据以预定的角度步长旋转一周而获取多个旋转检测数据；以及平移单元，将所述旋转检测数据平移到最低分辨率的栅格地图的预定位置来获取所述位姿数据，所述预定位置为所述起点 。
根据本发明第二方面涉及的全局重定位装置，还包括最优解判断单元，判断在所述最优解搜索单元中获取的最优解的得分是否高于预先设定的最优解阈值，如果所述最优解的得分大于所述最优解阈值，则将所述最优解确定为所述自动移动设备的位姿。
根据本发明第二方面涉及的全局重定位装置，将最低分辨率下的栅格地图的中心位置设为所述预定位置。
根据上述，应用多分辨率栅格地图与递归快速搜索算法，实现了秒级的快速全局重定位，解决了进行全局重定位耗时长的问题。另外，根据本发明，由于不需要使用摄像头及高计算能力的计算设备，因此降低了成本。
附图说明
图1是本发明涉及的全局重定位方法的流程图；
图2是本发明涉及的最优解搜索步骤的流程图；
图3是本发明涉及的栅格地图搜索数据模型的示意图；
图4是本发明涉及的进行递归搜索的一例流程图；
图5是本发明涉及的返回最优解时的判断的流程图；
图6是本发明涉及的全局重定位装置的结构图。
具体实施方式
下面将详细描述本发明的各个方面的特征和示例性实施例，为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及具体实施例，对本发明进行进一步详细描述。应理解，此处所描述的具体实施例仅被配置为解释本发明，并不被配置为限定本发明。对于本领域技术人员来说，本发明可以在不需要这些具体细节中的一些细节的情况下实施。下面对实施例的描述仅仅是为了通过示出本发明的示例来提供对本发明更好的理解。
下面，对本发明的自动移动设备的全局重定位方法进行说明。本发明应用了多分辨率栅格地图与利用了递归运算的快速搜索树法结合的快速全局重定位方法。
关于多分辨率栅格地图，设定分辨率值从大到小的多个层级的分辨率，获取各个分辨率下的栅格地图。在搜索中从最低分辨率的栅格地图开始，依次逐渐提高分辨率，进行精细搜索。这里，在本说明书中，分辨率高是指分辨率的值小，分辨率低是指分辨率的值大。
关于快速搜索树法，使用了递归算法。获取将位姿数据的按照得分排序后的可行解集，针对得分高于当前最低得分阈值的第一个可行解，从最高得分的第一个可行解开始以低一层级的分辨率在该低一层级的分辨率的栅格地图中扩展，获取新的可行解集，再对新的可行解集按照在该低一层级的分辨率的栅格地图中的得分进行排序，再一次从最高得分的可行解开始以再低一层级的分辨率扩展。依次类推，直到搜索到最低层级而获取最高分辨率对应的可行解集，或者直到当前可行解集中的可行解的个数为零，或者直到当前可行解集中的所有可行解的得分均是当前最低得分阈值以下，根据当前可行解集获取当前层级的最优解及其得分，并向上一层级返回，从而从低层级向高层级逐层返回最优解。这里，最初时，最低得分阈值是预先设定的值，之后，在递归搜索中，当获取得分高于当前最低得分阈值的最优解时，利用该最优解的得分更新当前最低得分阈值，即该最低得分阈值不断被高于当前最低得分阈值的可行解的得分更新。
根据上述，能够从得分最高的可行解进行搜索，且能够不对得分低于当前最低得分阈值的可行解进行搜索。因此减少了搜索的数据量，大幅度地提高了搜索速度。另外，能够不断对当前最低得分阈值进行更新，来提高最低得分阈值，因此能够进一步减少进行搜索的数据量，提高搜索速度，实现秒级搜索。
图1是本发明涉及的全局重定位方法的流程图。本发明的自动移动设备的全局重定位方法包含以下步骤：栅格地图获取步骤S11，获取当前检测区域在多个层级的预设分辨率下的栅格地图；位姿数据获取步骤S12，基于自动移动设备上的距离传感器的检测数据而获取多组位姿数据；评分排序步骤S13，针对每组位姿数据，计算在最低分辨率下的栅格地图中的预定位置的得分，并将所述位姿数据作为可行解按得分从大到小排序生成可行解集；以及最优解搜索步骤S14，在所述第一个可行解的得分高于当前最低得分阈值时，以所述第一个可行解的位置为起点，在分辨率由低到高的栅格地图中依次扩展来递归搜索最优解。
下面，以对自动移动的机器人进行全局重定位为例进行详细说明。
首先，对栅格地图获取步骤S1进行说明。确定机器人的移动区域来作为检测区域，获取当前检测区域在多个层级的预设分辨率下的栅格地图。
可选地，多个层级的预设分辨率的值从小到大按照固定的比例线性地增长。更优选地，多个层级的预设分辨率的值从小到大按照固定的偶数倍数线性地增长。
这里，以多个分辨率按照两倍的比例进行变化为例进行说明。例如，当前的栅格地图的分辨率 r为0.05米，则再生成分辨率r分别为0.1米、0.2米、0.4米、0.8米、1.6米、3.2米的栅格地图，即获得分辨率从高到低的七个栅格地图。这里，设定了七个分辨率的栅格地图，但是可以根据需要适当地设定，并不进行特别限制。
还执行位姿数据获取步骤S2。机器人具有激光雷达，可以基于机器人上的激光雷达的检测数据而获取多组位姿数据。
在上述位姿数据获取步骤S2中，包括：旋转数据获取步骤，将激光雷达的检测数据以预定的角度步长旋转一周而获取多个旋转检测数据；以及平移步骤，将所述旋转检测数据平移到最低分辨率3.2m的栅格地图的预定位置来获取所述位姿数据，所述预定位置为进行位置搜索的起点。
该激光雷达检测周边环境，检测出距周边环境中的物体的距离，获取一组距离数据，这里设为一组距离数据为D（d1，d2，……，dn）。
为了获取机器人准确的位姿数据，即获取机器人的准确位置以及姿态，对该组距 离数据进行旋转。在栅格地图的最高分辨率r=0.05m、激光雷达检测到的距离数据中的最大 值为dmax时，以旋转角度的角度步长
Figure 981732DEST_PATH_IMAGE001
进行旋转，获取对
Figure 476036DEST_PATH_IMAGE002
取整而得的个数的、对检测 数据进行旋转后的旋转检测数据。这里，旋转的角度步长
Figure 521352DEST_PATH_IMAGE001
如下式（1）所示。
Figure 447720DEST_PATH_IMAGE003
（1）
这里设为获取m个旋转检测数据，m为整数。
例如获取包含以下旋转检测数据的数据集C。
旋转0*
Figure 793251DEST_PATH_IMAGE001
，获取数据（
Figure 694342DEST_PATH_IMAGE004
，D0），即C0；旋转1*
Figure 23692DEST_PATH_IMAGE001
，获取数据（
Figure 3018DEST_PATH_IMAGE005
，D1），即C1；旋转2*
Figure 937476DEST_PATH_IMAGE001
，获 取数据（
Figure 461998DEST_PATH_IMAGE006
，D2），即C2；……；旋转（m-1）*
Figure 747486DEST_PATH_IMAGE001
，获取数据（
Figure 766389DEST_PATH_IMAGE007
，Dm-1），即Cm-1。
在获取包含m 个旋转检测数据的数据集后，将该m个旋转检测数据进行平行移动，并移动到最低分辨率的栅格地图的预定位置，获取m个位姿数据。即，以最低分辨率的栅格地图的预定位置为起点进行搜索。
可选地，以最低分辨率下的栅格地图的中心位置为起点进行搜索。这里，将数据集C移动到3.2m分辨率的栅格地图中的中心位置。
接着，在评分排序步骤S13中，针对每组位姿数据，计算在3.2m分辨率下的栅格地图中的得分，并将位姿数据按得分从大到小的顺序排序，生成可行解集C’。这里，该得分反映了该组位姿数据为最优解的概率，得分越高，越接近最优解。对m个位姿数据按照得分进行排序生成可行解集C’。以下，将C’包含的排序后的m个位姿数据称为可行解。通过以上，由于能够将各组位姿数据按照得分从高到低的顺序进行排序，从而在以下的搜索中，能够从机器人所处的可能性高的位置开始搜索，从而以更少的运算更快地获取最优解。
接着，执行最优解搜索步骤S14。在最优解搜索步骤S14中，对于得分高于当前最低得分阈值的可行解集中的第一个可行解，以其所在的位置为起点，在分辨率由3.2m到0.05m变化的栅格地图中依次扩展来递归搜索最优解。
具体地，图2是本发明涉及的最优解搜索步骤的流程图。上述最优解搜索步骤S14包括：可行解扩展步骤S141、扩展数据集排序步骤S142、以及最优解返回步骤S143。
在可行解扩展步骤S141中，针对当前得分最高且得分大于当前最低得分阈值的第一个可行解，从所在的可行解集中删除，并以该删除的第一个可行解的位置为起点，以低一层级分辨率为移动步长，获取包含该第一个可行解在内的、在该低一层级分辨率的栅格地图中的扩展数据的集合作为扩展数据集。
在扩展数据集排序步骤S142中，对扩展数据集中的位姿数据在该低一层级分辨率的栅格地图中进行评分并排序，获取将扩展数据集中的位置数据按得分从大到小排序的当前可行解集。
在最优解返回步骤S143中，重复可行解扩展步骤S141以及扩展数据集排序步骤，在搜索到最低层级时、或者在当前可行解集中的可行解的个数为零时、或者在当前可行解集中的第一个可行解的得分小于等于所述当前最低得分阈值时，获取当前层级的最优解及其得分，并向上一层级返回，从而从低层级向高层级逐层返回最优解。
在本实施方式中，使用利用了递归搜索的快速树搜索，获取可行解集C’中的第一个可行解，即得分最高的可行解Cmax，将该Cmax从可行解集C’中删除。在该可行解Cmax的得分大于当前最低得分阈值的情况下，以可行解Cmax的位置为起点，即初始时从栅格地图的中心点起，以下一层级的1.6m分辨率下的1.6m为移动步长，获取包含上述第一个可行解Cmax在内的、在1.6m分辨率的栅格地图中的扩展数据集。对该扩展数据集的所有位姿数据在该1.6m分辨率的栅格地图中进行评分，获取扩展数据集中的各个位姿数据的得分。接着，对该扩展数据集中的各个位姿数据按照得分进行排序，获取排序后的当前可行解集。
针对排序后的当前可行解集，删除得分最高的当前第一个可行解。以该删除的当前第一个可行解为起点，以0.8m分辨率下的0.8m为移动步长，获取包含该当前第一个可行解在内的在0.8m分辨率栅格地图中的扩展数据集。对该扩展数据集在该0.8m分辨率的栅格地图中进行评分，获取扩展数据集中的各个位姿数据的得分。接着，对该扩展数据集中的各个位姿数据按照该得分进行排序，获取排序后的当前可行解集。
依次类推，直至扩展到分辨率为0.05m的栅格地图上，获取扩展后的各位姿数据在分辨率0.05m的栅格地图中的得分，并获取将扩展后的位姿数据按得分从大到小进行排序而得的当前可行解集。
这里，在每次扩展之前，将用于进行扩展的当前第一个可行解从其所在的可行解集中删除，然后再向低一层级的分辨率的栅格地图中进行扩展。通过该删除，能够始终对可行解集中的得分最高的第一个可行解进行判断搜索。
图3是本发明涉及的栅格地图搜索数据模型的示意图。通过上述，从分辨率为3.2m的栅格地图的第七层向下搜索，直至到分辨率为0.05m的栅格地图的第一层。
由于0.05m分辨率的栅格地图是最高分辨率的栅格地图，因此将0.05m分辨率层级下当前可行解集中的当前第一个可行解作为0.05m分辨率层级的最优解而向上一层返回。
将在0.05m分辨率栅格地图中的最优解向上一层返回，此时，在该0.1m分辨率的层级中，将在0.05m分辨率的栅格地图中返回的最优解的得分与当前最低得分阈值进行比较，在该最优解的得分高于当前最低得分阈值时，用返回的该最优解的得分更新当前最低得分阈值。否则，保持该当前最低得分阈值不变。
将当前0.1m分辨率下的栅格地图对应的当前可行解集中的当前第一个可行解与当前最低得分阈值进行比较，在该当前第一个可行解大于当前最低得分阈值时，以当前该第一个可行解的位置为起点在0.05m分辨率的栅格地图中进行扩展，来递归搜索最优解，并删除该0.1m分辨率下的可行解集中的当前第一个可行解。接着，判断从0.05m分辨率层级返回的当前第一个可行解是否优于当前最优解，如果优于当前最优解，则更新当前最优解，并且利用最优解的得分更新当前最低得分阈值；否则继续保持该当前最优解。返回当前最优解，重复上述的最优解搜索，直至0.1m分辨率下的可行解集中不存在得分大于当前最低得分阈值的可行解。
接着，向上一层，即0.2m分辨率的层级返回在0.1分辨率下的当前最优解。以与在0.1m分辨率层级相同的方式搜索并向上一层级返回最优解，直至返回到3.2m分辨率下的层级。
接着基于与对之前3.2m分辨率下的可行解集中的第一个可行解搜索最优解相同的方式，开始对3.2m分辨率下的、已删除原始的第一个可行解后的可行解集的第一个可行解进行递归搜索。在此过程中，当前最优解被更新或者保持，直至最后3.2m分辨率层级下的可行解集中的剩余的当前第一可行解的得分小于等于当前最低得分阈值、或者3.2m分辨率层级下的可行解集中不存在可行解，结束递归搜索，并返回当前最优解。
上述按照分辨率层级来逐层递归搜索最优解，并将返回最优解的得分作为当前最低得分阈值，即，通过上述递归循环搜索，能够删除大量得分低于当前得分阈值的可行解，减少了运算量，提高了运算速度。从而实现了快速最优解。
可选地，还本发明的重定位方法还包括最优解判断步骤，判断所获取的最优解的得分是否高于预先设定的最优解阈值，如果所述最优解的得分大于所述最优解阈值，则将所述最小阈值对应的所述位置确定为所述最优解。即，求得的最优位姿数据的得分如果大于预先设定的最优解阈值，则认为全局重定位成功，即找到了当前激光雷达数据在当前栅格地图上的位姿，即完成了全局重定位过程。否则此次重定位搜索失败。这里的最优解阈值是大于预先设定的最低得分阈值的值。
图4是本发明涉及的递归搜索的一例流程图。
在步骤S1中，首先进行初始化，输入可行解集C’，预先设定分辨率的层级depth=7，预先确定最低得分阈值min_score=0.4，并设定当前最优解得分best_score=0，并设置当前最优解参数C_best。
在步骤S2中，判断栅格地图的层级depth是否为1，当depth不等于1时（即，为“否”时），进入到步骤S3。
在步骤S3中，令当前最优解得分best_score的值为预先确定的最低得分阈值min_score，然后进入到步骤S4。
在步骤S4中，判断当前可行解集C’中的可行解的个数是否为0。在当前可行解集C’中的可行解的个数不为0时（即，为“否”时），进入到步骤S5。
在步骤S5中，取可行解集C’中的第一个可行解Cmax，并将该可行解从可行解集中删除，进入到步骤S6。
在步骤S6中，判断可行解Cmax的得分Cmax_score是否是最低得分阈值min_score以下。在可行解Cmax的得分Cmax_score大于预定最低得分阈值min_score时（即，为“否”时），进入到步骤S7。
在步骤S7中，将可行解Cmax以depth-1层分辨率的步长在depth-1层栅格地图中进行预定的平移，来生成新的可行解集C11，然后进入到步骤S8。例如，在直角坐标系中，分别在X与Y两个方向上进行平移来生成新的可行解集C11。
在步骤S8中，对当前新的可行解集C11的各位姿数据在depth-1层分辨率的栅格地图中进行评分，并按照得分从高到低对各位姿数据进行排序，得到排序后的可行解集C12，然后进入到步骤S9。
在步骤S9中，使C=C12，depth=depth-1，min_score=best_score，并返回到步骤S1进行向下一层级扩展。
另外，在步骤S2中，当depth为0时（即，为“是”时），表明已经完成最高分辨率的层级的搜索，则进入到步骤S10，获取该可行解集中的第一个可行解，即得分最高的可行解及其得分并返回该可行解及其得分。
另外，在上述步骤S4中，在当前可行解集C中可行解的个数为0时（即，为“是”时），则进入到步骤S11，获取当前分辨率层级下的搜索结果C_result及其得分，并根据该搜索结果返回最优解C_best及其得分C_best_score。
另外，在上述步骤S6中，在当前最大可行解Cmax的得分Cmax_score小于等于最低得分阈值min_score时（即，为“是”时），获取在该Cmax所在层级的搜索结果C_result及其得分C_result_score，并根据该搜索结果及其得分返回最优解及其得分。
根据上述可知，通过向下进行递归搜索，能够在满足当前层级的可行解集的可行解的得分大于当前最低得分阈值的情况下，不断地向更高分辨率的层级进行递归搜索。在达到最高分辨率对应的层级时，根据当前可行解集C中的第一个可行解返回最优解；或者在当前的可行解集的最高得分小于等于最低得分阈值min_score时、或者在当前的可行解集中的可行解的个数为零时，获取的搜索结果，根据搜索结果返回最优解。
由于利用递归算法而逐层向下扩展，因此在返回最优解时逐层向上层返回。下面对步骤S11的根据搜索结果返回最优解的步骤进行详细说明。
图5是本发明涉及的返回最优解的流程图。在步骤S111中，获取搜索结果C_result及其得分C_result_score，进入到步骤S112。
在步骤S112中，判断当前的搜索结果C_result的得分C_result_score是否大于best_score。
在判断为是的情况下，进入到步骤S114，C_best=C_result、best_score=C_result_score，即。利用得分更高的可行解及其得分来更新C_best以及对应的当前最高得分best_score。
在判断为否的情况下，进入到步骤S113，不对C_best以及对应的最高得分best_score进行更新，而直接返回当前的C_best以及best_score。
由于采用递归运算，因此在步骤S113和步骤S114中的返回，是向低分辨率的上一层级返回当前最优解C_best以及对应的最高得分best_score。通过向上一层级返回最优解，将该最优解的得分作为当前最低得分阈值对该上一层级的可行解集中的当前第一个可行解的得分进行判断，在该当前第一个可行解的得分高于当前最低得分阈值时，继续对该可行解向下搜索，直到满足返回条件，根据搜索结果判断是否更新当前最优解及其得分。
通过从最高得分的可行解进行搜索，并对低于当前最低得分阈值的可行解，不再进行搜索而直接删除，因此减少了大量的数据，减少搜索量，提高了搜索速度。另外，由于当前最优解不断被更新，当前得分阈值也不短被更新，因此当前得分阈值越来越大，因此对于可行解中的得分较低的数据能够大幅度地进行删除，也大幅度地减少了运算量，提高了搜索速度。
因此，根据上述，每次获取的可行解集均是按得分从大到小排序而得，每次从得分最大的可行解开始判断，在要判断当前可行解低于当前的最低得分阈值时，不需要对当前可行解集中的当前可行解以及后面所有的可行解进行判断，从而减少了大量的运算量，提高了速度。
另外，由于采用了多分辨率地图，如上图3所示，每一层的宽度代表着搜索空间的大小。从第七层开始进行搜索与评分，通过对比当前层级的当前第一个可行解的得分与当前最低得分阈值，如果当前可行解的得分小于等于当前最低得分阈值，则不再对当前可行解进行扩展，并将当前可行解及其该当前可行解集中的其他可行解删除，也不再进行计算；如果当前可行解的得分大于当前最优解的得分，则将这个可行解进行扩展，在更精细的分辨率上求取可行解的得分，从而大幅度地加速了搜索速度，实现了秒级的搜索。即，从高层级开始搜索，随着层级降低，各层级的搜索空间从小到大变化，在越高的层级中删除数据，越能减少搜索的数据量，提高搜索速度。而由于当前最低得分阈值不断被更大的值更新，低于当前最低得分阈值的可行解的数据将被删除，从而大幅减少了所要搜索的数据，提高了搜索速度。
以上，对本发明的自动移动设备的全局重定位方法进行说了说明。下面对本发明涉及的自动移动设备的全局重定位装置进行说明。
图6是本发明涉及的全局重定位装置的结构图。自动移动设备的全局重定位装置1包括：栅格地图获取单元11，获取当前检测区域在多个层级的预设分辨率下的栅格地图；位姿数据获取单元12，基于自动移动设备上的距离传感器的检测数据而获取多组位姿数据；评分排序单元13，针对每组位姿数据，计算在最低分辨率下的栅格地图中的预定位置的得分，并将所述位姿数据作为可行解按得分从大到小排序生成可行解集；以及最优解搜索单元14，在第一个可行解的得分高于当前最低得分阈值时，以所述第一个可行解的位置为起点，在分辨率由低到高的栅格地图中依次扩展来递归搜索最优解。具体的细节描述参考前述的图1，这里不再赘述。通过从得分最高的可行解进行搜索，能够加快搜索的速度，减少搜索的数据量。
可选地，最优解搜索单元14包括：可行解扩展单元，针对当前得分最高且得分大于所述当前最低得分阈值的所述第一个可行解，从所在的可行解集中删除，并以该删除的所述第一个可行解的位置为起点，以低一层级分辨率为移动步长，获取包含所述第一个可行解在内的、在所述低一层级分辨率的栅格地图中的扩展数据的集合作为扩展数据集；扩展数据集排序单元，对所述扩展数据集中的位姿数据在所述低一层级分辨率的栅格地图中进行评分并排序，获取将位姿数据按得分从大到小排序的当前可行解集；以及最优解返回单元，重复所述可行解扩展单元以及所述扩展数据集排序单元的动作，在搜索到最低层级时、或者在当前可行解集中的可行解的个数为零时、或者在当前可行解集中的第一个可行解的得分小于等于所述当前最低得分阈值时，获取当前层级的最优解及其得分，并向上一层级返回，从而从低层级向高层级逐层返回最优解。
根据上述，能够从得分最高的可行解进行搜索，且能够不对得分低于当前最低得分阈值的可行解进行搜索。因此减少了搜索的数据量，大幅度地提高了搜索速度。
可选地，在所述递归搜索中，当获取得分高于所述当前最低得分阈值的最优解时，利用所述最优解的得分更新所述当前最低得分阈值。
由于当前最优解不断被更新，当前最低得分阈值也不断被更新，因此当前最低得分阈值越来越大，因此对于后面得分较低的数据能够大幅度地进行删除，也大幅度地减少了运算量，提高了搜索速度。
可选地，在所述栅格地图获取单元11中，多个层级的预设分辨率的值从小到大按照固定的偶数倍数线性地增长。
可选地，在位姿数据获取单元12中，包括：旋转数据获取单元：将所述检测数据以预定的角度步长旋转一周而获取多个旋转检测数据；平移单元，将所述旋转检测数据平移到最低分辨率的栅格地图的预定位置来获取所述位姿数据，所述预定位置为所述起点。
可选地，重定位装置还包括最优解判断单元，判断在所述最优解搜索单元中获取的最优解的得分是否高于预先设定的最优解阈值，如果所述最优解的得分大于所述最优解阈值，则将所述最优解确定为所述自动移动设备的位姿。
可选地，将最低分辨率下的栅格地图的中心位置设为所述预定位置。
对于自动移动设备的全局重定位装置的具体的细节描述，由于与上述对自动移动设备的全局重定位方法的图1至图5的描述分别对应，这里就不在重复说明。
在上述实施方式中，以自动移动设备是自动移动的机器人为例进行了说明，但是自动移动设备也可以是无人车等，只要是能够在无人的情况下自动移动的设备即可，不进行限定。
另外，在上述实施例中，以机器人安装有激光雷达进行测距为例进行说明。但是该机器人只要安装有距离传感器即可，该距离传感器只要能够获取环境距离值即可，不进行特别限定。
另外，在上述实施例中示出的流程图仅是一个示例，只要能够实现本发明的搜索即可，并不进行特别限定。
以上，虽然结合附图描述了本发明的实施方式和具体实施例，但是本领域技术人员可以在不脱落本发明的精神和范围的情况下做出各种修改和变形，这样的修改和变形均落入由所述权利要求所限定的范围之内。

Claims (14)

1.一种自动移动设备的全局重定位方法，其特征在于，包括：
栅格地图获取步骤，获取当前检测区域在多个层级的预设分辨率下的栅格地图；
位姿数据获取步骤，基于自动移动设备上的距离传感器的检测数据而获取多组位姿数据；
评分排序步骤，针对每组位姿数据，计算在最低分辨率下的栅格地图中的预定位置的得分，并将所述位姿数据作为可行解按得分从大到小排序生成可行解集；以及
最优解搜索步骤，在第一个可行解的得分高于当前最低得分阈值时，以所述第一个可行解的位置为起点，在分辨率由低到高的栅格地图中依次扩展来递归搜索最优解。
2.如权利要求1所述的全局重定位方法，其特征在于，
所述最优解搜索步骤包括：
可行解扩展步骤，针对当前得分最高且得分大于所述当前最低得分阈值的所述第一个可行解，从所在的可行解集中删除，并以该删除的所述第一个可行解的位置为起点，以低一层级分辨率为移动步长，获取包含所述第一个可行解在内的、在所述低一层级分辨率的栅格地图中的扩展数据的集合作为扩展数据集；
扩展数据集排序步骤，对所述扩展数据集中的位姿数据在所述低一层级分辨率的栅格地图中进行评分并排序，获取将位姿数据按得分从大到小排序的当前可行解集；以及
最优解返回步骤，重复所述可行解扩展步骤以及所述扩展数据集排序步骤，在搜索到最低层级时、或者在当前可行解集中的可行解的个数为零时、或者在当前可行解集中的第一个可行解的得分小于等于所述当前最低得分阈值时，获取当前层级的最优解及其得分，并向上一层级返回，从而从低层级向高层级逐层返回最优解。
3.如权利要求1或2所述的全局重定位方法，其特征在于，
在所述递归搜索中，当获取得分高于所述当前最低得分阈值的最优解时，利用所述最优解的得分更新所述当前最低得分阈值。
4.如权利要求1或2所述的全局重定位方法，其特征在于，
在所述栅格地图获取步骤中，所述多个层级的预设分辨率的值从小到大按照固定的偶数倍数线性地增长。
5.如权利要求1或2所述的全局重定位方法，其特征在于，
在所述位姿数据获取步骤中，包括：
旋转数据获取步骤：将所述检测数据以预定的角度步长旋转一周而获取多个旋转检测数据；以及
平移步骤，将所述旋转检测数据平移到最低分辨率的栅格地图的预定位置来获取所述位姿数据，所述预定位置为所述起点 。
6.如权利要求1或2所述的全局重定位方法，其特征在于，
还包括最优解判断步骤，判断在所述最优解搜索步骤中获取的最优解的得分是否高于预先设定的最优解阈值，如果所述最优解的得分大于所述最优解阈值，则将所述最优解确定为所述自动移动设备的位姿。
7.如权利要求1或2所述的全局重定位方法，其特征在于，
将最低分辨率下的栅格地图的中心位置设为所述预定位置。
8.一种自动移动设备的全局重定位装置，其特征在于，包括：
栅格地图获取单元，获取当前检测区域在多个层级的预设分辨率下的栅格地图；
位姿数据获取单元，基于自动移动设备上的距离传感器的检测数据而获取多组位姿数据；
评分排序单元，针对每组位姿数据，计算在最低分辨率下的栅格地图中的预定位置的得分，并将所述位姿数据作为可行解按得分从大到小排序生成可行解集；以及
最优解搜索单元，在第一个可行解的得分高于当前最低得分阈值时，以所述第一个可行解的位置为起点，在分辨率由低到高的栅格地图中依次扩展来递归搜索最优解。
9.如权利要求8所述的全局重定位装置，其特征在于，
所述最优解搜索单元包括：
可行解扩展单元，针对当前得分最高且得分大于所述当前最低得分阈值的所述第一个可行解，从所在的可行解集中删除，并以该删除的所述第一个可行解的位置为起点，以低一层级分辨率为移动步长，获取包含所述第一个可行解在内的、在所述低一层级分辨率的栅格地图中的扩展数据的集合作为扩展数据集；
扩展数据集排序单元，对所述扩展数据集中的位姿数据在所述低一层级分辨率的栅格地图中进行评分并排序，获取将位姿数据按得分从大到小排序的当前可行解集；以及
最优解返回单元，重复所述可行解扩展单元以及所述扩展数据集排序单元的动作，在搜索到最低层级时、或者在当前可行解集中的可行解的个数为零时、或者在当前可行解集中的第一个可行解的得分小于等于所述当前最低得分阈值时，获取当前层级的最优解及其得分，并向上一层级返回，从而从低层级向高层级逐层返回最优解。
10.如权利要求8或9所述的全局重定位装置，其特征在于，
在所述递归搜索中，当获取得分高于所述当前最低得分阈值的最优解时，利用所述最优解的得分更新所述当前最低得分阈值。
11.如权利要求8或9所述的全局重定位装置，其特征在于，
在所述栅格地图获取单元中，所述多个层级的预设分辨率的值从小到大按照固定的偶数倍数线性地增长。
12.如权利要求8或9所述的全局重定位装置，其特征在于，
在所述位姿数据获取单元中，包括：
旋转数据获取单元：将所述检测数据以预定的角度步长旋转一周而获取多个旋转检测数据；以及
平移单元，将所述旋转检测数据平移到最低分辨率的栅格地图的预定位置来获取所述位姿数据，所述预定位置为所述起点 。
13.如权利要求8或9所述的全局重定位装置，其特征在于，
还包括最优解判断单元，判断在所述最优解搜索单元中获取的最优解的得分是否高于预先设定的最优解阈值，如果所述最优解的得分大于所述最优解阈值，则将所述最优解确定为所述自动移动设备的位姿。
14.如权利要求8或9所述的全局重定位装置，其特征在于，
将最低分辨率下的栅格地图的中心位置设为所述预定位置。
CN202110669388.4A 2021-06-17 2021-06-17 自动移动设备的全局重定位方法以及装置 Pending CN113129379A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110669388.4A CN113129379A (zh) 2021-06-17 2021-06-17 自动移动设备的全局重定位方法以及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110669388.4A CN113129379A (zh) 2021-06-17 2021-06-17 自动移动设备的全局重定位方法以及装置

Publications (1)

Publication Number Publication Date
CN113129379A true CN113129379A (zh) 2021-07-16

Family

ID=76783037

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110669388.4A Pending CN113129379A (zh) 2021-06-17 2021-06-17 自动移动设备的全局重定位方法以及装置

Country Status (1)

Country Link
CN (1) CN113129379A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113607160A (zh) * 2021-08-24 2021-11-05 湖南国科微电子股份有限公司 视觉定位恢复方法、装置、机器人和可读存储介质
CN113960999A (zh) * 2021-07-30 2022-01-21 珠海一微半导体股份有限公司 移动机器人重定位方法、系统及芯片

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107831765A (zh) * 2017-10-23 2018-03-23 广州视源电子科技股份有限公司 定位方法、装置、设备及存储介质
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN109978925A (zh) * 2017-12-27 2019-07-05 深圳市优必选科技有限公司 一种机器人位姿的识别方法及其机器人
CN111486855A (zh) * 2020-04-28 2020-08-04 武汉科技大学 一种具有物体导航点的室内二维语义栅格地图构建方法
CN111765884A (zh) * 2020-06-18 2020-10-13 北京海益同展信息科技有限公司 机器人重定位方法、装置、电子设备及存储介质
CN112179330A (zh) * 2020-09-14 2021-01-05 浙江大华技术股份有限公司 移动设备的位姿确定方法及装置
CN112180937A (zh) * 2020-10-14 2021-01-05 中国安全生产科学研究院 一种地铁车厢消毒机器人及其自动导航方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107831765A (zh) * 2017-10-23 2018-03-23 广州视源电子科技股份有限公司 定位方法、装置、设备及存储介质
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN109978925A (zh) * 2017-12-27 2019-07-05 深圳市优必选科技有限公司 一种机器人位姿的识别方法及其机器人
CN111486855A (zh) * 2020-04-28 2020-08-04 武汉科技大学 一种具有物体导航点的室内二维语义栅格地图构建方法
CN111765884A (zh) * 2020-06-18 2020-10-13 北京海益同展信息科技有限公司 机器人重定位方法、装置、电子设备及存储介质
CN112179330A (zh) * 2020-09-14 2021-01-05 浙江大华技术股份有限公司 移动设备的位姿确定方法及装置
CN112180937A (zh) * 2020-10-14 2021-01-05 中国安全生产科学研究院 一种地铁车厢消毒机器人及其自动导航方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113960999A (zh) * 2021-07-30 2022-01-21 珠海一微半导体股份有限公司 移动机器人重定位方法、系统及芯片
CN113960999B (zh) * 2021-07-30 2024-05-07 珠海一微半导体股份有限公司 移动机器人重定位方法、系统及芯片
CN113607160A (zh) * 2021-08-24 2021-11-05 湖南国科微电子股份有限公司 视觉定位恢复方法、装置、机器人和可读存储介质
CN113607160B (zh) * 2021-08-24 2023-10-31 湖南国科微电子股份有限公司 视觉定位恢复方法、装置、机器人和可读存储介质

Similar Documents

Publication Publication Date Title
CN112179330B (zh) 移动设备的位姿确定方法及装置
CN108256574B (zh) 机器人定位方法及装置
CN111429574B (zh) 基于三维点云和视觉融合的移动机器人定位方法和系统
CN111652934B (zh) 定位方法及地图构建方法、装置、设备、存储介质
CN111508002B (zh) 一种小型低飞目标视觉检测跟踪系统及其方法
CN113467456B (zh) 一种未知环境下用于特定目标搜索的路径规划方法
CN113129379A (zh) 自动移动设备的全局重定位方法以及装置
WO2016119117A1 (en) Localization and mapping method
JP2006190191A (ja) 情報処理装置および方法、並びにプログラム
CN109323697B (zh) 一种针对室内机器人任意点启动时粒子快速收敛的方法
CN112785705B (zh) 一种位姿获取方法、装置及移动设备
CN111783722B (zh) 一种激光点云的车道线提取方法和电子设备
CN111709317B (zh) 一种基于显著性模型下多尺度特征的行人重识别方法
WO2011085435A1 (en) Classification process for an extracted object or terrain feature
CN115546116A (zh) 全覆盖式岩体不连续面提取与间距计算方法及系统
CN114187418A (zh) 回环检测方法、点云地图构建方法、电子设备及存储介质
CN115307622A (zh) 在动态环境下基于深度学习的自主建图方法和系统
Li et al. An SLAM Algorithm Based on Laser Radar and Vision Fusion with Loop Detection Optimization
CN115586767A (zh) 一种多机器人路径规划方法和装置
CN111724438B (zh) 一种数据处理方法、装置
CN113592976A (zh) 地图数据的处理方法、装置、家用电器和可读存储介质
CN108090961B (zh) 一种三维激光点云成像中的快速平差方法
Dos Santos et al. Automatic Building Boundary Extraction from Airborne LiDAR Data Robust to Density Variation
CN117058358B (zh) 一种场景边界检测方法和移动平台
CN114236552B (zh) 基于激光雷达的重定位方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination