CN112123343A - 点云匹配方法、设备及存储介质 - Google Patents

点云匹配方法、设备及存储介质 Download PDF

Info

Publication number
CN112123343A
CN112123343A CN202011338505.0A CN202011338505A CN112123343A CN 112123343 A CN112123343 A CN 112123343A CN 202011338505 A CN202011338505 A CN 202011338505A CN 112123343 A CN112123343 A CN 112123343A
Authority
CN
China
Prior art keywords
matching
point cloud
icp
map
probability
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.)
Granted
Application number
CN202011338505.0A
Other languages
English (en)
Other versions
CN112123343B (zh
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.)
Syrius Technology Shenzhen Co Ltd
Original Assignee
Syrius Technology Shenzhen 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 Syrius Technology Shenzhen Co Ltd filed Critical Syrius Technology Shenzhen Co Ltd
Priority to CN202011338505.0A priority Critical patent/CN112123343B/zh
Publication of CN112123343A publication Critical patent/CN112123343A/zh
Application granted granted Critical
Publication of CN112123343B publication Critical patent/CN112123343B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种点云匹配方法、设备及存储介质，通过利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿；达到了在动态变化的环境中完成精准而稳定的激光点云匹配的目的，提高了动态变化环境中点云匹配的精度。

Description

点云匹配方法、设备及存储介质
技术领域
本发明涉及移动机器人定位技术领域，特别涉及一种点云匹配方法、设备及存储介质。
背景技术
移动机器人的定位问题是实现机器人自主移动的关键问题之一，激光雷达由于自身精度高等优势成为了室内定位最受欢迎的传感器之一，而激光雷达对应的点云匹配是室内机器人定位的关键步骤。
目前关于点云匹配的算法主要分为两大类，一类是：点到点的匹配，如ICP和PL-ICP等变种算法；另一类是：关联匹配，如cartographer中所用到的关键技术就是关联匹配的方法。但是，在真实的工作场景中（比如仓库），移动机器人的工作环境是可能发生变化的。在这种工作环境发生变化的情况下，点云匹配会因环境的动态变化而受到影响，比如点云的匹配过程会出现拼接错误等，最终会影响移动机器人的定位精度。现有技术的解决方案中，通常会采用视觉定位算法，但视觉定位算法通常对光照和场景的特征点要求较高，但定位精度不如激光雷达的定位精度高。
发明内容
针对上述移动机器人定位技术领域中激光点云匹配存在的弊端，本发明提供一种点云匹配方法、设备及存储介质，用以在动态变化的环境中完成精准而稳定的激光点云匹配。
第一方面，本发明提供了一种点云匹配方法，所述点云匹配方法包括：
利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；
对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；
根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿。
第二方面，本发明提供了一种点云匹配装置，所述点云匹配装置包括：
概率地图构建模块，用于：
利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；
对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；
点云匹配模块，用于：根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿。
第三方面，本发明还提供了一种电子设备，所述电子设备包括存储器和处理器，所述存储器上存储有可在所述处理器上运行的点云匹配程序，所述点云匹配程序被所述处理器运行时，执行所述的点云匹配方法。
第四方面，本发明还提供了一种计算机可读存储介质，所述存储介质上存储有点云匹配程序，所述点云匹配程序可以被一个或者多个处理器执行，以实现所述的点云匹配方法的步骤。
本发明一种点云匹配方法、设备及存储介质，通过利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿；达到了在动态变化的环境中完成精准而稳定的激光点云匹配的目的，提高了动态变化环境中点云匹配的精度，为后续的导航和定位提供了重要的技术基础；本发明提供的技术方案是将ICP点云匹配算法与关联匹配方法进行结合，利用了算法本身互补的优势，解决了ICP算法过程中对于环境变化鲁棒性不高的问题。
附图说明
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。
在附图中：图1是本发明点云匹配方法的一种实施方式的流程示意图。
图2a是实际环境改变后的地图中环境边界示意图。
图2b是可能的点云匹配结果示意图。
图2c是降低分辨率模糊环境边界后的地图边界示意图。
图3是本发明点云匹配方法中利用多分辨率进行点云搜索匹配过程示意图。
图4是本发明点云匹配装置的一种实施方式的功能模块示意图。
图5是本发明电子设备的一种实施方式的内部结构示意图。
具体实施方式
以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。
本发明提供了一种点云匹配方法、设备及存储介质，将ICP点云匹配和关联匹配结合在一起，在动态变化的室内环境中完成精准而稳定的激光点云匹配，提高了匹配的准确度和匹配精度，为移动机器人的建图和定位提供重要的技术基础。
如图1所示，图1是本发明点云匹配方法的一种实施方式的流程示意图；本发明一种点云匹配方法可以实施为如下描述的步骤S10-S30。
步骤S10、利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图。
本发明实施例中，在构建导航地图时，利用ICP点云匹配算法进行，选择点到点的ICP点云拼接方法，得到移动机器人对应的导航地图。设置对应的关键帧，并同时记录关键帧对应的包含位姿信息和点云位置等的关键帧信息。由于在建图过程中，机器人的位姿和点云的位置会随着后端算法的优化而逐渐发生变化，因此，当机器人的位姿和点云的位置等关键帧信息发生变化时，随之更新对应的关键帧信息。同时，记录所述关键帧中点云所在栅格的位置。在机器人处于定位阶段时，通过计算关键帧中点云所在栅格被击中的次数来统计栅格是障碍物的概率，从而生成概率地图。
步骤S20、对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔。
对生成的上述概率地图的分辨率进行处理，比如将生成的概率地图按照一定的比例降低分辨率，从而根据具体应用场景的不同环境需求，生成不同分辨率的栅格地图。根据配置的金字塔所需的层数，构建对应的概率地图金字塔。在构建所述概率地图金字塔时，按照从粗到精、从低分辨率到高分辨率的原则，构建得到所述概率地图金字塔；也就是说，在所述概率地图金字塔的最低一层，其分辨率最低，越往上分辨率越高。即：所述概率地图金字塔的地图分辨率随着金字塔层数的增高，其分辨率也越来越高，即位于所述概率地图金字塔最底层的分辨率最低，位于所述概率地图金字塔最顶层的分辨率最高。
步骤S30、根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿。
根据对机器人下发的目标信息，比如机器人所要到达的目标点，先采用ICP点云匹配算法与步骤S10中构建的导航地图进行匹配定位。若采用ICP点云匹配成功，则直接输出对应的位姿；若采用ICP点云匹配不成功，则采用关联匹配的方式，结合步骤S20中构建的概率地图金字塔，将ICP点云匹配与关联匹配二者进行结合，得到机器人的位姿并输出对应的位姿信息。
参照2a、图2b和图2c，针对实际环境发生改变时，如图2a所示，图2a是实际环境改变后的地图中环境边界示意图。当仅采用IPC点云匹配方法进行点云匹配时，可能的点云匹配结果如图2b所示，图2b是可能的点云匹配结果示意图，由图2b可知，仅当采用IPC点云匹配算法进行点云匹配时，存在较大的误差。因此，如图1所述的步骤30中，当采用ICP点云匹配不成功时，则采用关联匹配的方式。在进行关联匹配时，结合步骤S20中构建的概率地图金字塔，由于概率地图金字塔是由多分辨率的概率地图组成，因此，在对步骤S10中构建的概率地图进行分辨率处理比如降低分辨率处理，则降低分辨率模糊边界后的地图如图2c所示，通过将ICP点云匹配与关联匹配二者进行结合，即可得到机器人的位姿并输出对应的位姿信息。
进一步地，在一个实施例中，图1所述实施例的步骤S10中，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图，可以按照如下技术手段实施。
根据记录的所述关键帧的位姿信息和对应的点云信息，获取所述关键帧中点云所在的栅格的位置；在机器人定位阶段，遍历所有的所述关键帧对应的点云信息，计算所述关键帧中点云所在的栅格被击中的次数；根据所述关键帧中点云所在的栅格被击中的次数，统计所述栅格是障碍物的概率值；根据所述概率值的统计结果，生成所述概率地图。
比如，在一个具体的应用场景中，根据所述关键帧中点云所在的栅格被激光击中的次数，来更新栅格是障碍物的概率。假设每个栅格初始被击中或者没有被击中的概率都是0.5，即Phit = Pmiss = 0.5。当所述关键帧中点云所在的栅格被观测到是否被击中时，可以根据如下计算公式来更新所述栅格被击中的概率：
Figure 250578DEST_PATH_IMAGE001
上述公式中，
Figure 354800DEST_PATH_IMAGE002
为击中的概率值，通常为一个经验值，用来更新所述关键帧中点云所在的栅格的概率值。clamp是将所述概率值限定在一个范围内的函数；Mold（x）为所述栅格对应的前一次的概率值，
Figure 104450DEST_PATH_IMAGE003
为所述栅格对应的当前次的概率值。当遍历至最后一帧关键帧时，所述概率地图生成成功。
进一步地，在一个实施例中，图1所述实施例的步骤S20中，对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔，可以按照如下技术手段实施。
配置所需要的金字塔层数；根据配置的金字塔的具体层数，对生成的所述概率地图按照预设比例降低分辨率，得到不同分辨率分别对应的与所述金字塔层数的数量一致的栅格地图；按照分辨率由低到高的顺序，由底层到高层配置所述栅格地图，构建并得到所述概率地图金字塔。其中，所述概率地图金字塔的地图分辨率随着金字塔层数的增高，其分辨率也越来越高，即位于所述概率地图金字塔最底层的分辨率最低，位于所述概率地图金字塔最顶层的分辨率最高。
本发明实施例中，配置的所述金字塔的具体层数，可以根据所述点云匹配方法的具体应用场景和/或点云匹配算法与所述关联算法对应的经验值进行具体配置。本发明实施例对所述金字塔的具体层数的取值不进行限定。
进一步地，在一个实施例中，图1所述实施例的步骤S30中，所述根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿，可以按照如下技术手段实施。
根据对机器人下发的目标信息比如目标点对应的位置信息，使用ICP点云匹配算法与所述导航地图进行ICP匹配定位，判断ICP匹配定位是否定位成功；若ICP匹配定位成功，则输出机器人的位姿；若ICP匹配定位不成功，则利用构建的概率地图金字塔，使用关联匹配算法进行点云匹配定位，输出机器人的位姿。
其中，针对根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，判断ICP匹配定位是否定位成功，可以采用如下描述的技术手段实施。
根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行ICP匹配定位；同时，在ICP匹配定位的过程中，机器人维护一个概率子地图，并根据ICP点云匹配算法得到的位姿更新所述概率子地图；在判断ICP匹配定位是否定位成功时，可以根据预先设置的ICP匹配成功的条件，通过判断是否满足ICP匹配成功的条件，来判断ICP匹配定位是否定位成功。在一个实施例中，预先设置的ICP匹配成功的条件包括：当匹配到的点云大于或者等于预设阈值时，则认为ICP匹配成功，输出机器人对应的位姿。
比如，机器人判断利用ICP点云匹配算法匹配到的点云是否大于预设阈值；若大于或者等于所述预设阈值，则判断出ICP匹配定位成功；若小于所述预设阈值，则继续利用ICP点云匹配算法进行点云匹配；若利用ICP点云匹配算法匹配到的点云小于所述预设阈值的次数，达到预设次数，则判断出ICP匹配定位不成功。比如，在一个具体的应用场景中，当ICP匹配定位时，因环境变化过大导致ICP匹配定位失败超过三次，则认为ICP匹配定位不成功。
在ICP匹配定位不成功时，引入关联匹配方式。在一个实施例中，在ICP匹配定位不成功时，利用构建的概率地图金字塔，使用关联匹配算法进行点云匹配定位，输出机器人的位姿，可以采用如下描述的技术手段实施。
若ICP匹配定位不成功，则将ICP匹配定位过程中维护的所述概率子地图，与构建的所述概率地图金字塔按照关联匹配算法，进行关联匹配，输出机器人的位姿。
在一个具体的应用场景中，机器人将ICP匹配定位过程中维护的所述概率子地图，与构建的所述概率地图金字塔按照关联匹配算法，进行关联匹配，输出机器人的位姿，可以按照如下描述的技术手段实施。
机器人按照预设搜索范围和搜索步长，利用所述概率子地图，从所述概率地图金字塔中最低分辨率的地图开始搜索匹配；针对所述概率子地图与所述概率地图金字塔的每次搜索匹配，计算待匹配点云被转到匹配点云坐标系下时与所述待匹配点云距离最近的所有点云对应的匹配得分；将计算得到的所有匹配得分中分值最大的数值作为本次搜索的匹配分值。
判断本次搜索对应的所述匹配分值是否大于预设匹配值；若本次搜索对应的所述匹配分值大于或者等于所述预设匹配值，则将所述匹配分值对应的点云作为关联匹配结果，输出对应的位姿；若本次搜索对应的所述匹配分值小于所述预设匹配值，则继续向上搜索并与所述概率地图金字塔中分辨率高的概率地图进行关联匹配，直至关联匹配成功，输出对应的位姿。
比如，在一个具体的应用场景中，机器人在进行多分辨率概率地图匹配时，即将定位导航过程中维护的所述概率子地图，与构建的多分辨率的概率地图金字塔进行匹配时，为了提升搜索匹配的效率，可以采用滑动窗口算法进行。比如，可以选择一个固定的窗口在地图上滑动，例如选择窗口x、y、θ的大小，并设定在窗口内x、y、θ的搜索步长，则有：
Figure 635926DEST_PATH_IMAGE004
其中，
Figure 979182DEST_PATH_IMAGE005
是窗口x方向的搜索步长，
Figure 887095DEST_PATH_IMAGE006
是窗口y方向的搜索步长，
Figure 756831DEST_PATH_IMAGE007
是窗口内角度的搜索范围；r是窗口x方向以及窗口y方向上每次搜索的搜索范围，
Figure 459208DEST_PATH_IMAGE008
是窗口内每次对于角度搜索对应的搜索步长。
机器人在将所述概率子地图与构建的所述概率地图金字塔进行匹配时，先从所述概率地图金字塔所包含的最低分辨率的地图开始搜索匹配，如图3所示，图3是本发明点云匹配方法中利用多分辨率进行点云搜索匹配过程示意图。
在将所述概率子地图与所述概率地图金字塔进行搜索匹配时，每次搜索匹配时均会计算得到一个匹配得分，其计算过程如下：
Figure 289761DEST_PATH_IMAGE009
其中，
Figure 735786DEST_PATH_IMAGE010
表示：待匹配点云转到被匹配点云坐标系下时，与所述待匹配点云最近的所有点云对应的匹配得分；
Figure 961493DEST_PATH_IMAGE011
表示转换矩阵，将激光帧转换到参考帧下；
Figure 834771DEST_PATH_IMAGE012
表示该激光帧的第k个点云；
Figure 152620DEST_PATH_IMAGE013
表示索引j位于
Figure 136756DEST_PATH_IMAGE014
的搜索窗内。从计算得到的所有匹配得分中选择搜索匹配到的最大值，并将所述匹配得分中的最大值作为本次搜索匹配的匹配分值。设定得分阈值即预设匹配值，判断本次搜索对应的所述匹配分值是否大于预设匹配值。
若本次搜索对应的所述匹配分值大于或者等于所述预设匹配值，则不再向上搜索，并将所述匹配分值对应的点云作为关联匹配结果，输出对应的位姿。
若本次搜索对应的所述匹配分值小于所述预设匹配值，则继续向上搜索并与所述概率地图金字塔中分辨率较高的概率地图进行关联匹配，直至关联匹配成功，输出对应的位姿。
在本发明点云匹配方法中，以上实施例所描述的ICP点云匹配算法包含了ICP的各种变种算法；另外，在本发明点云匹配方法中，也可以将关联匹配算法与其他匹配精度高的算法结合在一起。
本发明基于多分辨率的点云匹配方法，通过利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿；达到了在动态变化的环境中完成精准而稳定的激光点云匹配的目的，提高了动态变化环境中点云匹配的精度，为后续的导航和定位提供了重要的技术基础；本发明提供的技术方案是将ICP点云匹配算法与关联匹配方法进行结合，利用了算法本身互补的优势，解决了ICP算法过程中对于环境变化鲁棒性不高的问题。
对应于以上实施例所描述的点云匹配方法，本发明还提供了一种点云匹配装置。如图4所示，图4是本发明点云匹配装置的一种实施方式的功能模块示意图，图4所述实施例仅仅从功能上描述本发明点云匹配装置。
在图4所述实施例中，本发明点云匹配装置在功能上包括：概率地图构建模块100和点云匹配模块200。
在一个实施例中，所述概率地图构建模块100用于：利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔。
所述点云匹配模块200用于：根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿。
在一个实施例中，所述点云匹配模块200用于：根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行ICP匹配定位，判断ICP匹配定位是否定位成功；若ICP匹配定位成功，则输出机器人的位姿；若ICP匹配定位不成功，则利用构建的概率地图金字塔，使用关联匹配算法进行点云匹配定位，输出机器人的位姿。
在一个实施例中，所述点云匹配模块200用于：根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行ICP匹配定位；同时，在ICP匹配定位的过程中，维护一个概率子地图，并根据ICP点云匹配算法得到的位姿更新所述概率子地图；判断利用ICP点云匹配算法匹配到的点云是否大于预设阈值；若大于或者等于所述预设阈值，则判断出ICP匹配定位成功；若小于所述预设阈值，则继续利用ICP点云匹配算法进行点云匹配；若利用ICP点云匹配算法匹配到的点云小于所述预设阈值的次数，达到预设次数，则判断出ICP匹配定位不成功。
在一个实施例中，所述点云匹配模块200用于：若ICP匹配定位不成功，则将ICP匹配定位过程中维护的所述概率子地图，与构建的所述概率地图金字塔按照关联匹配算法，进行关联匹配，输出机器人的位姿。
在一个实施例中，所述点云匹配模块200用于：按照预设搜索范围和搜索步长，利用所述概率子地图，从所述概率地图金字塔中最低分辨率的地图开始搜索匹配；针对所述概率子地图与所述概率地图金字塔的每次搜索匹配，计算待匹配点云被转到匹配点云坐标系下时与所述待匹配点云距离最近的所有点云对应的匹配得分；将计算得到的所有匹配得分中分值最大的数值作为本次搜索的匹配分值；判断本次搜索对应的所述匹配分值是否大于预设匹配值；若本次搜索对应的所述匹配分值大于或者等于所述预设匹配值，则将所述匹配分值对应的点云作为关联匹配结果，输出对应的位姿；若本次搜索对应的所述匹配分值小于所述预设匹配值，则继续向上搜索并与所述概率地图金字塔中分辨率高的概率地图进行关联匹配，直至关联匹配成功，输出对应的位姿。
在一个实施例中，所述概率地图构建模块100用于：根据记录的所述关键帧的位姿信息和对应的点云信息，获取所述关键帧中点云所在的栅格的位置；在机器人定位阶段，遍历所有的所述关键帧对应的点云信息，计算所述关键帧中点云所在的栅格被击中的次数；根据所述关键帧中点云所在的栅格被击中的次数，统计所述栅格是障碍物的概率值；根据所述概率值的统计结果，生成所述概率地图。
在一个实施例中，所述概率地图构建模块100用于：配置需要的金字塔层数；对生成的所述概率地图按照预设比例降低分辨率，得到不同分辨率分别对应的与所述金字塔层数的数量一致的栅格地图；按照分辨率由低到高的顺序，由底层到高层配置所述栅格地图，构建对应的概率地图金字塔。
其中，所述概率地图金字塔的地图分辨率随着金字塔层数的增高，其分辨率也越来越高，即位于所述概率地图金字塔最底层的分辨率最低，位于所述概率地图金字塔最顶层的分辨率最高。
本发明点云匹配装置，通过利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿；达到了在动态变化的环境中完成精准而稳定的激光点云匹配的目的，提高了动态变化环境中点云匹配的精度，为后续的导航和定位提供了重要的技术基础；本发明提供的技术方案是将ICP点云匹配算法与关联匹配方法进行结合，利用了算法本身互补的优势，解决了ICP算法过程中对于环境变化鲁棒性不高的问题。
本发明还提供了一种电子设备，所述电子设备可以按照图1所述的点云匹配方法进行点云匹配和定位。如图5所示，图5是本发明电子设备的一种实施方式的内部结构示意图。
在本实施例中，电子设备1可以是PC（Personal Computer，个人电脑），也可以是智能手机、平板电脑、便携计算机等终端设备。该电子设备1至少包括存储器11、处理器12，通信总线13，以及网络接口14。
其中，存储器11至少包括一种类型的可读存储介质，所述可读存储介质包括闪存、硬盘、多媒体卡、卡型存储器（例如，SD或DX存储器等）、磁性存储器、磁盘、光盘等。存储器11在一些实施例中可以是电子设备1的内部存储单元，例如该电子设备1的硬盘。存储器11在另一些实施例中也可以是电子设备1的外部存储设备，例如电子设备1上配备的插接式硬盘，智能存储卡（Smart Media Card, SMC），安全数字（Secure Digital, SD）卡，闪存卡（Flash Card）等。进一步地，存储器11还可以既包括电子设备1的内部存储单元也包括外部存储设备。存储器11不仅可以用于存储安装于电子设备1的应用软件及各类数据，例如点云匹配程序01的代码等，还可以用于暂时地存储已经输出或者将要输出的数据。
处理器12在一些实施例中可以是一中央处理器（Central Processing Unit,CPU）、控制器、微控制器、微处理器或其他数据处理芯片，用于运行存储器11中存储的程序代码或处理数据，例如执行点云匹配程序01等。
通信总线13用于实现这些组件之间的连接通信。
网络接口14可选的可以包括标准的有线接口、无线接口（如WI-FI接口），通常用于在该电子设备1与其他电子设备之间建立通信连接。
可选地，该电子设备1还可以包括用户接口，用户接口可以包括显示器（Display）、输入单元比如键盘（Keyboard），可选的用户接口还可以包括标准的有线接口、无线接口。可选地，在一些实施例中，显示器可以是LED显示器、液晶显示器、触控式液晶显示器以及OLED（Organic Light-Emitting Diode，有机发光二极管）触摸器等。其中，显示器也可以适当的称为显示屏或显示单元，用于显示在电子设备1中处理的信息以及用于显示可视化的用户界面。
图5仅示出了具有组件11-14以及点云匹配程序01的电子设备1，本领域技术人员可以理解的是，图5示出的结构并不构成对电子设备1的限定，可以包括比图示更少或者更多的部件，或者组合某些部件，或者不同的部件布置。
基于图1、图2a、图2b、图2c、图3和图4所述实施例的描述，在图5所示的电子设备1实施例中，存储器11中存储有点云匹配程序01；所述存储器11上存储的点云匹配程序01可在所述处理器12上运行，所述点云匹配程序01被所述处理器12运行时实现如下步骤：利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿。
本发明电子设备，通过利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿；达到了在动态变化的环境中完成精准而稳定的激光点云匹配的目的，提高了动态变化环境中点云匹配的精度，为后续的导航和定位提供了重要的技术基础；本发明提供的技术方案是将ICP点云匹配算法与关联匹配方法进行结合，利用了算法本身互补的优势，解决了ICP算法过程中对于环境变化鲁棒性不高的问题。
此外，本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有点云匹配程序，所述点云匹配程序可以被一个或者多个处理器执行，以实现如下操作：利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿。
本发明计算机可读存储介质具体实施方式与上述点云匹配方法、点云匹配装置和电子设备对应的各实施例的实施原理基本相同，在此不作累述。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (10)

1.一种点云匹配方法，其特征在于，所述点云匹配方法包括：
利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；
对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；
根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿。
2.如权利要求1所述的点云匹配方法，其特征在于，所述根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿，包括：
根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行ICP匹配定位，判断ICP匹配定位是否定位成功；
若ICP匹配定位成功，则输出机器人的位姿；
若ICP匹配定位不成功，则利用构建的概率地图金字塔，使用关联匹配算法进行点云匹配定位，输出机器人的位姿。
3.如权利要求2所述的点云匹配方法，其特征在于，所述根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，判断ICP匹配定位是否定位成功，包括：
根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行ICP匹配定位；同时，在ICP匹配定位的过程中，维护一个概率子地图，并根据ICP点云匹配算法得到的位姿更新所述概率子地图；
判断利用ICP点云匹配算法匹配到的点云是否大于预设阈值；若大于或者等于所述预设阈值，则判断出ICP匹配定位成功；
若小于所述预设阈值，则继续利用ICP点云匹配算法进行点云匹配；若利用ICP点云匹配算法匹配到的点云小于所述预设阈值的次数，达到预设次数，则判断出ICP匹配定位不成功。
4.如权利要求3所述的点云匹配方法，其特征在于，所述若ICP匹配定位不成功，则利用构建的概率地图金字塔，使用关联匹配算法进行点云匹配定位，输出机器人的位姿，包括：
若ICP匹配定位不成功，则将ICP匹配定位过程中维护的所述概率子地图，与构建的所述概率地图金字塔按照关联匹配算法，进行关联匹配，输出机器人的位姿。
5.如权利要求4所述的点云匹配方法，其特征在于，所述将ICP匹配定位过程中维护的所述概率子地图，与构建的所述概率地图金字塔按照关联匹配算法，进行关联匹配，输出机器人的位姿，包括：
按照预设搜索范围和搜索步长，利用所述概率子地图，从所述概率地图金字塔中最低分辨率的地图开始搜索匹配；
针对所述概率子地图与所述概率地图金字塔的每次搜索匹配，计算待匹配点云被转到匹配点云坐标系下时与所述待匹配点云距离最近的所有点云对应的匹配得分；
将计算得到的所有匹配得分中分值最大的数值作为本次搜索的匹配分值；
判断本次搜索对应的所述匹配分值是否大于预设匹配值；
若本次搜索对应的所述匹配分值大于或者等于所述预设匹配值，则将所述匹配分值对应的点云作为关联匹配结果，输出对应的位姿；
若本次搜索对应的所述匹配分值小于所述预设匹配值，则继续向上搜索并与所述概率地图金字塔中分辨率高的概率地图进行关联匹配，直至关联匹配成功，输出对应的位姿。
6.如权利要求1至5任一项所述的点云匹配方法，其特征在于，所述根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图，包括：
根据记录的所述关键帧的位姿信息和对应的点云信息，获取所述关键帧中点云所在的栅格的位置；
在机器人定位阶段，遍历所有的所述关键帧对应的点云信息，计算所述关键帧中点云所在的栅格被击中的次数；
根据所述关键帧中点云所在的栅格被击中的次数，统计所述栅格是障碍物的概率值；
根据所述概率值的统计结果，生成所述概率地图。
7.如权利要求1至5任一项所述的点云匹配方法，其特征在于，所述对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔，包括：
配置需要的金字塔层数；
对生成的所述概率地图按照预设比例降低分辨率，得到不同分辨率分别对应的与所述金字塔层数的数量一致的栅格地图；
按照分辨率由低到高的顺序，由底层到高层配置所述栅格地图，构建对应的概率地图金字塔；
其中，所述概率地图金字塔的地图分辨率随着金字塔层数的增高，其分辨率也越来越高，即位于所述概率地图金字塔最底层的分辨率最低，位于所述概率地图金字塔最顶层的分辨率最高。
8.一种点云匹配装置，其特征在于，所述点云匹配装置包括：
概率地图构建模块，用于：
利用ICP点云匹配算法构建导航地图，设置关键帧并记录对应的关键帧信息，根据记录的所述关键帧信息，统计关键帧中点云所在的栅格是障碍物的概率，生成概率地图；
对生成的所述概率地图的分辨率进行处理，得到不同分辨率的栅格地图，构建对应的概率地图金字塔；
点云匹配模块，用于：根据对机器人下发的目标信息，使用ICP点云匹配算法与所述导航地图进行匹配定位，根据ICP点云匹配结果，结合构建的所述概率地图金字塔，得到机器人的位姿。
9.一种电子设备，其特征在于，所述电子设备包括存储器和处理器，所述存储器上存储有可在所述处理器上运行的点云匹配程序，所述点云匹配程序被所述处理器运行时，执行如权利要求1至7中任一项所述的点云匹配方法。
10.一种计算机可读存储介质，其特征在于，所述存储介质上存储有点云匹配程序，所述点云匹配程序可以被一个或者多个处理器执行，以实现如权利要求1至7中任一项所述的点云匹配方法的步骤。
CN202011338505.0A 2020-11-25 2020-11-25 点云匹配方法、设备及存储介质 Active CN112123343B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011338505.0A CN112123343B (zh) 2020-11-25 2020-11-25 点云匹配方法、设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011338505.0A CN112123343B (zh) 2020-11-25 2020-11-25 点云匹配方法、设备及存储介质

Publications (2)

Publication Number Publication Date
CN112123343A true CN112123343A (zh) 2020-12-25
CN112123343B CN112123343B (zh) 2021-02-05

Family

ID=73852304

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011338505.0A Active CN112123343B (zh) 2020-11-25 2020-11-25 点云匹配方法、设备及存储介质

Country Status (1)

Country Link
CN (1) CN112123343B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113311827A (zh) * 2021-05-08 2021-08-27 东南大学 一种提高存储效率的机器人室内地图及其生成方法
CN113432533A (zh) * 2021-06-18 2021-09-24 北京盈迪曼德科技有限公司 一种机器人定位方法、装置、机器人及存储介质
CN113674351A (zh) * 2021-07-27 2021-11-19 追觅创新科技(苏州)有限公司 一种机器人的建图方法及机器人
CN114271856A (zh) * 2021-12-27 2022-04-05 开普云信息科技股份有限公司 三维超声影像生成方法、装置、存储介质及设备

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104897161A (zh) * 2015-06-02 2015-09-09 武汉大学 基于激光测距的室内平面地图制图方法
CN107526360A (zh) * 2017-09-26 2017-12-29 河南科技学院 一种未知环境下排爆机器人多阶自主导航探测系统及方法
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN109545072A (zh) * 2018-11-14 2019-03-29 广州广电研究院有限公司 地图构建的位姿计算方法、装置、存储介质和系统
US20190179332A1 (en) * 2017-12-08 2019-06-13 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for outputting obstacle information

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104897161A (zh) * 2015-06-02 2015-09-09 武汉大学 基于激光测距的室内平面地图制图方法
CN107526360A (zh) * 2017-09-26 2017-12-29 河南科技学院 一种未知环境下排爆机器人多阶自主导航探测系统及方法
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
US20190179332A1 (en) * 2017-12-08 2019-06-13 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for outputting obstacle information
CN109545072A (zh) * 2018-11-14 2019-03-29 广州广电研究院有限公司 地图构建的位姿计算方法、装置、存储介质和系统

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113311827A (zh) * 2021-05-08 2021-08-27 东南大学 一种提高存储效率的机器人室内地图及其生成方法
CN113432533A (zh) * 2021-06-18 2021-09-24 北京盈迪曼德科技有限公司 一种机器人定位方法、装置、机器人及存储介质
CN113432533B (zh) * 2021-06-18 2023-08-15 北京盈迪曼德科技有限公司 一种机器人定位方法、装置、机器人及存储介质
CN113674351A (zh) * 2021-07-27 2021-11-19 追觅创新科技(苏州)有限公司 一种机器人的建图方法及机器人
CN113674351B (zh) * 2021-07-27 2023-08-08 追觅创新科技(苏州)有限公司 一种机器人的建图方法及机器人
CN114271856A (zh) * 2021-12-27 2022-04-05 开普云信息科技股份有限公司 三维超声影像生成方法、装置、存储介质及设备

Also Published As

Publication number Publication date
CN112123343B (zh) 2021-02-05

Similar Documents

Publication Publication Date Title
CN112123343B (zh) 点云匹配方法、设备及存储介质
CN110866496B (zh) 基于深度图像的机器人定位与建图方法和装置
JP7196382B2 (ja) 更新道路を識別する方法、装置、デバイス及びコンピュータ記憶媒体
US9245193B2 (en) Dynamic selection of surfaces in real world for projection of information thereon
CN106951484B (zh) 图片检索方法及装置、计算机设备及计算机可读介质
AU2010328337B2 (en) Matching an approximately located query image against a reference image set
CN108279670B (zh) 用于调整点云数据采集轨迹的方法、设备以及计算机可读介质
CN112179330A (zh) 移动设备的位姿确定方法及装置
CN111340890B (zh) 相机外参标定方法、装置、设备和可读存储介质
CN110866497B (zh) 基于点线特征融合的机器人定位与建图方法和装置
CN109855628A (zh) 一种室内或楼宇间的定位、导航方法及装置、以及计算机可读写介质和电子设备
US20170039450A1 (en) Identifying Entities to be Investigated Using Storefront Recognition
WO2021027692A1 (zh) 视觉特征库的构建方法、视觉定位方法、装置和存储介质
US20210264155A1 (en) Visual positioning method and apparatus, and computer-readable storage medium
CN114236552A (zh) 基于激光雷达的重定位方法及系统
US9910878B2 (en) Methods for processing within-distance queries
KR20210065901A (ko) 이미지에서의 키 포인트 위치의 인식 방법, 장치, 전자기기 및 매체
CN113191323A (zh) 一种语义元素处理的方法、装置、电子设备及存储介质
WO2022252482A1 (zh) 机器人及其环境地图构建方法和装置
CN111929694B (zh) 点云匹配方法、设备及存储介质
CN112836077B (zh) 特定人物检索方法、系统、设备及存储介质
CN114674307B (zh) 一种重定位方法及电子设备
CN111832483A (zh) 一种兴趣点有效性识别方法、装置、设备以及存储介质
CN116698040A (zh) 智能设备的定位方法、装置、智能设备及介质
CN113358124A (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
GR01 Patent grant
GR01 Patent grant
CP02 Change in the address of a patent holder
CP02 Change in the address of a patent holder

Address after: 518000 Room 401, block D, building 7, Shenzhen International Innovation Valley, Dashi 1st Road, Xili community, Xili street, Nanshan District, Shenzhen, Guangdong

Patentee after: JUXING TECHNOLOGY (SHENZHEN) Co.,Ltd.

Address before: 518000 building 101, building R3b, Gaoxin industrial village, No.018, Gaoxin South 7th Road, community, high tech Zone, Yuehai street, Nanshan District, Shenzhen City, Guangdong Province

Patentee before: JUXING TECHNOLOGY (SHENZHEN) Co.,Ltd.