CN108038139B - 地图构建方法、装置和机器人定位方法、装置、计算机设备和存储介质 - Google Patents

地图构建方法、装置和机器人定位方法、装置、计算机设备和存储介质 Download PDF

Info

Publication number
CN108038139B
CN108038139B CN201711182940.7A CN201711182940A CN108038139B CN 108038139 B CN108038139 B CN 108038139B CN 201711182940 A CN201711182940 A CN 201711182940A CN 108038139 B CN108038139 B CN 108038139B
Authority
CN
China
Prior art keywords
key frame
matched
natural feature
natural
map
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.)
Active
Application number
CN201711182940.7A
Other languages
English (en)
Other versions
CN108038139A (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.)
Visionnav Robotics Shenzhen Co Ltd
Original Assignee
Visionnav Robotics 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 Visionnav Robotics Shenzhen Co Ltd filed Critical Visionnav Robotics Shenzhen Co Ltd
Publication of CN108038139A publication Critical patent/CN108038139A/zh
Application granted granted Critical
Publication of CN108038139B publication Critical patent/CN108038139B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/40—Scenes; Scene-specific elements in video content
    • G06V20/46—Extracting features or characteristics from the video content, e.g. video fingerprints, representative shots or key frames
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/40—Scenes; Scene-specific elements in video content
    • G06V20/46—Extracting features or characteristics from the video content, e.g. video fingerprints, representative shots or key frames
    • G06V20/47—Detecting features for summarising video content
    • 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/10016—Video; Image sequence

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Databases & Information Systems (AREA)
  • Multimedia (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明提供一种地图构建方法、装置、机器人定位方法、装置、计算机设备和存储介质，通过获取图像，检测图像中的图像码和自然特征，根据图像码和自然特征建立关键帧，根据里程计信息和图像码携带的位置信息计算关键帧的相对位置和绝对位置，根据关键帧的位置信息和图像码携带的位置信息以及里程计里程同时进行处理得到自然特征的位置信息，将自然特征的位置信息的添加到全局地图中，不断的获取新的图像更新全局地图。通过将图像码的位置信息和自然特征结合起来能够有效的提高构建地图的精确度，能够提供更为稳定和精确的定位信息。

Description

地图构建方法、装置和机器人定位方法、装置、计算机设备和 存储介质
技术领域
本发明涉及机器视觉技术领域，特别是涉及一种地图构建方法、装置和机器人定位方法、装置、计算机设备和存储介质。
背景技术
“我在哪”“这是什么地方”“我怎么到我想去的地方”是智能移动机器人所需要解决的三大问题。其中，“我在哪”即移动机器人如何确定自己所在的位置，简称定位问题移动机器人的首要问题，是解决另外两个问题的前提。
目前机器定位主要是分为激光定位方法，磁条磁钉定位方法以及视觉定位方法。目前主要的视觉定位方法是基于自然特征的视觉定位方法又称视觉同步定位与地图构建该方法首先需要采集环境图像，建立自然特征的地图，然后根据建立的自然特征地图以及实时运行时采集的图像进行匹配，并得出移动机器人的位置与姿态。基于自然特征的建立的地图容易受到环境的影响，导致这个定位系统稳定性差，难以达到工业要求的定位精度。
发明内容
基于此，有必要针对上述地图的问题，提供一种地图构建方法、装置、机器人定位方法、装置、计算机设备和存储介质。
一种地图构建方法，所述方法包括：获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧加入到局部地图和全局地图中；当检测到所述局部地图中增加了下一个关键帧时，获取所述局部地图中当前关键帧和所述下一个关键帧的匹配自然特征集，将所述匹配自然特征集加入局部地图以及全局地图；获取里程计测量信息集；根据所述里程计测量信息集和所述匹配自然特征集计算得到所述匹配自然特征集与所述局部地图中所有关键帧的相对位置集和所述局部地图中所有关键帧的相对位姿集；当检测到所述全局地图中增加了所述下一个关键帧，且所述下一个关键帧存在图像码时，获取所述下一个关键帧的图像码携带的位置信息；获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集；根据所述关键帧中图像码的携带的位置信息集、所述所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计的测量信息集、所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地中所述自然特征集的位置集；重复进入获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到所述局部地图和所述全局地图，检测所述局部地图的关键帧，根据所述关键帧对所述局部地图进行更新，检测所述全局地图的关键帧，根据所述关键帧对所述全局地图进行更新，直至处理完所有获取到的图像得到所述全局地图。
在其中一个实施例中，所述获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集的步骤，包括：当检测到所述全局地图中增加了下一个关键帧，且所述下一个关键帧与所述全局地图中所有的关键帧存在匹配的关键帧时，根据所述匹配的关键帧之间共同观测的自然特征集计算得到所述下一个关键帧与所述匹配的关键帧的相对位置集。
在其中一个实施例中，所述获取里程计测量信息集的步骤之后，还包括：根据所述匹配自然特征集在所述局部地图中查找能够共同观测到预设范围内的所述匹配自然特征集的关键帧作为匹配关键帧集；根据所述里程计测量信息和所述匹配自然特征集计算得到所述匹配关键帧集与所述下一关键帧的相对位置集。
在其中一个实施例中，所述根据所述关键帧中图像码的携带的位置信息集、所述所述所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计的测量信息集、所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地中所述自然特征集的位置集的步骤，包括：根据所述关键帧中图像码的携带的位置信息集、所述所述下一个关键帧与所述匹配的关键帧的相对位置集和所述里程计的测量信息集计算得到所述全局地图中所有关键帧的位姿集；按照预设条件在所述全局地图中查找关键帧作为与所述匹配的关键帧之间共同观测的自然特征集中的自然特征对应的主要关键帧，由各个主要关键帧组成主要关键帧集；根据所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述主要关键帧集计算得到所述匹配自然特征与所述匹配自然特征对应的主要关键帧的相对位置组成匹配相对位置集；根据所述全局地图中所有关键帧的位姿集以及所述匹配相对位置集，更新全局地图中自然特征的位置集。
在其中一个实施例中，所述根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到局部地图和全局地图的步骤，包括：提取所述图像的自然特征集；检测所述图像中是否存在图像码集；当所述自然特征能够与上一个相邻关键帧自然特征成功匹配或检测到所述图像中存在图像码满足时，根据添加关键帧的预设条件将所述当前图像作为关键帧添加到所述局部地图中；根据添加的关键帧的自然特帧集和所述上一个关键帧的自然特征集进行匹配得到相匹配的自然特征集，根据所述相匹配的自然特征集和所述图像码集更新所述局部地图；将所述关键帧添加到全局地图。
在其中一个实施例中，所述根据所述当前关键的自然特帧集和所述上一个关键帧的自然特征集进行匹配得到匹配自然特征集，根据所述匹配自然特征集和所述图像码更新所述局部地图的步骤，包括：根据图像匹配算法对所述当前关键帧的自然特征集和所述上一个关键帧的自然特征集进行匹配，得到所述当前关键帧与所述上一个关键帧相同自然特征的所述匹配自然特征集；根据所述匹配自然特征集和所述里程计的测量信息计算得到所述匹配的自然特征集相对于所述当前关键帧的第一相对位置，将所述第一相对位置添加到所述局部地图中；根据所述里程计信息、所述局部地图中所有的关键帧中所有自然特征集的位置信息和所述图像码作为约束条件利用光束平差法对所述局部地图进行处理更新所述局部地图中所有的关键帧和自然特征的位置。
在其中一个实施例中，还提供一种机器人定位方法，所述方法包括：当检测到拍摄到的当前帧中存在图像码时，根据所述图像码获取机器人的初始位姿；获取里程计数据，根据所述里程计数据计算得到所述机器人的当前位置与所述机器人的上一位置的相对位置，根据所述相对位置以及机器人上一位置，计算当前位置的预测值；根据所述当前位置的预测值查询全局地图中预设范围内的目标关键帧集和目标图像码集；根据所述目标关键帧集、所述目标图像码集和所述当前帧的自然特征集计算得到所述机器人的当前位姿；获取下一帧图像作为所述当前帧，重复进入获取里程计数据，根据所述里程计数据计算得到不同时刻机器人的相对位置，根据所述相对位置和获取到的目标关键帧集和里程计信息计算得到所述机器人的当前位姿，直到处理完获取到的图像得到机器人的当前位姿。
在其中一个实施例中，所述根据所述目标关键帧集、所述目标图像码集和所述当前关键帧计算得到所述机器人的当前位置的步骤，包括：获取所述目标关键帧集中各个目标关键帧的目标关键帧自然特征组成目标关键帧自然特征集；获取所述目标图像码集的位置集；对所述自然特征集与所述目标关键帧自然特征集进行匹配得到匹配自然特征集；根据所述匹配自然特征计算得到匹配位置集；根据所述匹配位置集和所述位置集计算得到所述机器人的当前位姿。
在其中一个实施例中，提供一种地图构图装置，所述装置包括：
关键帧获取模块，用于获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧加入到局部地图和全局地图中；
匹配自然特征集计算模块，用于当检测到所述局部地图中增加了下一个关键帧时，获取所述局部地图中当前关键帧和所述下一个关键帧的匹配自然特征集，将所述匹配自然特征集加入局部地图以及全局地图；
位姿计算模块，用于获取里程计测量信息集，根据所述里程计测量信息集和所述匹配自然特征集计算得到所述匹配自然特征集与所述局部地图中所有关键帧的相对位置集和所述局部地图中所有关键帧的相对位姿集；
图像码携带的位置信息获取模块，用于当检测到所述全局地图中增加了所述下一个关键帧，且所述下一个关键帧存在图像码时，获取所述下一个关键帧的图像码携带的位置信息；
相对位置获取模块，用于获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集；
全局地更新模块，根据所述关键帧中图像码的携带的位置信息集、所述所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计的测量信息集、所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地中所述自然特征集的位置集；
地图更新模块，用于重复进入获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到所述局部地图和所述全局地图，检测所述局部地图的关键帧，根据所述关键帧对所述局部地图进行更新，检测所述全局地图的关键帧，根据所述关键帧所述全局地图进行更新，直至处理完所有获取到的图像得到所述全局地图。
在其中一个实施例中，提供一种机器人定位装置，所述装置包括：
初始位姿计算模块，用于当检测到拍摄到的当前帧中存在图像码时，根据所述图像码获取机器人的初始位姿；
预测值计算模块，用于获取里程计数据，根据所述里程计数据计算得到所述机器人的当前位置与所述机器人的上一位置的相对位置，根据所述相对位置以及机器人上一位置，计算当前位置的预测值；
自然特征集计算模块，用于根据所述当前位置的预测值查询全局地图中预设范围内的目标关键帧集和目标图像码集，并获取当前关键帧的自然特征集，所述全局地图根据关键帧中的自然特征集和图像码集建立；
当前位姿计算模块，用于根据所述目标关键帧集、所述目标图像码集和所述当前关键帧的自然特征集计算得到所述机器人的当前位姿；
当前位姿更新模块，用于获取下一帧图像作为所述当前帧，重复进入获取里程计数据，根据所述里程计数据计算得到不同时刻机器人的相对位置，根据所述相对位置和获取到的目标关键帧集和里程计信息计算得到所述机器人的当前位姿，直到处理完获取到的图像得到机器人的当前位姿。
一种计算机设备，所述计算机设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时执行上述地图构建方法或机器人定位所述的步骤。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时执行上述地图构建方法或机器人定位的步骤。
上述地图构建方法、装置、机器人定位方法、装置、计算机设备和存储介质，通过获取图像，检测图像中的图像码和自然特征，根据图像码和自然特征建立关键帧，根据里程计信息和图像码携带的位置信息计算关键帧的相对位置和绝对位置，根据关键帧的位置信息和图像码携带的位置信息以及里程计里程同时进行处理得到自然特征的位置信息，将自然特征的位置信息的添加到全局地图中，随着获取到的新图像更新全局地图。将图像码的位置信息和自然特征结合起来能够有效的提高构建地图的精确度，提供更为稳定和精确的定位信息。
附图说明
图1为一个实施例中机器人定位的应用场景图；
图2为一个实施例中地图构建方法的流程图；
图3为另一个实施例中地图构建方法的流程图；
图4为一个实施例中全局地图更新的步骤流程图；
图5为一个实施例中建立关键帧的步骤流程图；
图6为一个实施例中局部地图更新的步骤流程图；
图7为一个实施例中机器人定位方法流程图；
图8为一个实施例中机器人当前位姿计算的步骤流程图；
图9为一个实施例中一种地图构建装置的框架图；
图10为另一个实施例中地图构建装置的框架图；
图11为一个实施例中全局地图更新模块的框架图；
图12为一个实施例中关键帧获取模块的框架图；
图13为一个实施例中局部地图更新单元的框架图；
图14为一个实施例中地图构建装置的框架图；
图15为一个实施例中机器人定位装置的框架图。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不限定本发明的保护范围。
除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。
在一个实施例中，如图1所示，为机器人定更为的应用场景图，图中包含处理器510，拍照装置520，里程计530，设备540用于搭载处理器510，相机520和里程计530以及图像码550。拍照设备520用于拍摄图片，里程计530用于计算里程计的相对位置，通过移动设备540，相机520获取不同时刻不同地点的图片。图像码550作为辅助工具用于增加环境的特征，提供绝对位置信息；在处理器510计算自然特征和识别解码图像码，通过在环境中增加图像码来增加环境的稳定性是的移动设备的定位更为准确，提高系统的稳定性。里程计530的成本低，可以选择一个或者多个里程计530。通过里程计530的测量数据与根据拍照装置520拍摄的图像检测图像中的图像码和自然特征建立环境地图，根据环境地图、图像码、自然特征和里程计的测量信息计算得到移动设备的实时定位。
在一个实施例中，如图2所示，提供了一种地图构建方法，上述方法包括：
步骤S110，获取图像，根据图像包含的自然特征集和图像码集建立关键帧，将关键帧加入到局部地图和全局地图中。
具体地，获取相机等能够拍摄的图像的装置拍摄的图片，提取图像中的自然特征和识别并解码图像中的图像码，根据自然特征集和图像码集建立关键帧，其中，关键帧分为包含图像码的关键帧、包含自然特征的关键帧和既包含自然特征又包含图像码的关键帧，将关键帧和关键帧的位置以及自然特征的位置添加到局部地图和全局地图。其中，自然特征主要包括不限于角点、边缘点、纹理、形状和颜色等。自然特征提取算法提取的特征主要包括尺度不变特征变换(Scale-invariant feature transform，SIFT)、加速稳健特征(Speeded Up Robust Features，SURF)、加速的分割测试特征(Features fromAccelerated Segment Test，FAST)和二进制可靠的独立单元特征(Binary RobustIndependent Elementary Features，BRIEF)。其中，图像码可以是二维码和能够携带标识信息的编码图片。二维码可以是人们熟识的二维码(例如：QR code、Data Matrix或者Arucocode)或二维码阵，或其他种类人工标签，例如有特殊结构的反光标签，圆柱形人工标签等等。对于图像码的布置可以只是在关键位置进行布置或者按照预设的距离布置。
步骤S120，当检测到局部地图中增加了下一个关键帧时，获取局部地图中当前关键帧和下一个关键帧的匹配自然特征集，将匹配自然特征集加入局部地图以及全局地图。
其中，下一个关键帧为加入局部地图和全局地图中新的关键帧。具体地，随着图像的不断加入，提取加入的图像的自然特征集和图像码集，当满足添加关键帧时，将关键帧添加到局部地图中，对添加的关键帧中包含的自然特征集和上一个相邻关键帧中包含的自然特征集进行匹配得到匹配自然特征集，将得到的匹配自然特征集加入局部地图和全局地图中。
步骤S130，获取里程计测量信息集，根据里程计测量信息集和匹配自然特征集计算得到匹配自然特征集与局部地图中所有关键帧的相对位置集和局部地图中所有关键帧的相对位姿集。
具体地，获取里程计中得到的里程计相对位置信息集，根据里程计相对位置信息集和步骤S120中得到的匹配自然特征集计算得到局部地图中所有关键帧之间的相对位置集和局部地图中所有关键帧之间的相对位姿集。其中，里程计是一种利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法。该方法被用在许多种机器人系统(轮式或者腿式)上面，来估计，而不是确定这些机器人相对于初始位置移动的距离。这种方法对由速度对时间积分来求得位置的估计时所产生的误差十分敏感。快速、精确的数据采集，设备标定以及处理过程对于高效的使用该方法是十分必要的。
步骤S140，当检测到全局地图中增加了下一个关键帧，且下一个关键帧存在图像码时，获取下一个关键帧的图像码携带的位置信息。
具体地，随着图像的不断加入，全局地图中不断的加入新的关键帧，当检测到全局地图中增加了一个新的关键帧时，对新的关键帧的进行检测，当检测到新的关键帧中存在图像码时，获取新的关键帧中图像码携带的位置信息。其中，图像码携带的位置信息为世界坐标系中的三维位置或位姿信息。
步骤S150，获取下一个关键帧与下一个关键帧对应匹配的关键帧的相对位置集。
具体地，在全局地图中查找与新增加的关键帧的匹配关键帧，计算下一个关键帧和与之匹配的关键帧之间的相对位置。
步骤S160，根据关键帧中图像码的携带的位置信息集、下一个关键帧与匹配的关键帧的相对位置、里程计的测量信息集、匹配自然特征集相对于局部地图中所有关键帧的相对位置集更新全局地中自然特征集的位置集。
具体地，根据全局地图中的关键帧集中包含的图像码集携带的位置信息集、新加入的关键帧与新加入的关键帧匹配的关键帧的相位位置、步骤S130中获取到的里程计的相对位置集、步骤S130中得到的匹配自然特征集相对于局部中所有关键帧的相对位置集和匹配自然特征集相对于全局地图中所有关键帧的位姿集计算全局地图中所有关键帧中自然特征的位置，将得到自然特征的位置添加到全局地图中，得到新的全局地图。
步骤S170，重复进入获取图像，根据图像包含的自然特征集和图像码集建立关键帧，根据关键帧建立局部地图，根据获取到的图像计算得到对应的关键帧，根据关键帧计算得到对应的自然特征集，将自然特征添加到全局地图中，直至处理完所有获取到的图像得到全局地图。
具体地，随着拍摄设备拍摄到新图像，获取拍摄设备拍摄的新图像或者获取拍摄好的图像时序图的下一帧图像作为新图像，对新图像重复进行检测，建立新的关键帧，对新的关键帧进行检测，根据新的关键帧和全局中存在的关键帧计算自然特征的位置，将得到的自然特征的位置添加到全局地图中，不断重复上述过程更新局部地图和全局地图直到处理完拍摄设备获取到的图像，得到最新的全局地图。
上述地图构建方法，通过不断的获取图像，检测图像中的自然特征集和图像码集，根据自然特征集和图像码集建立关键帧得到更为进精确的位置信息，将关键帧添加到局部地图和全局地图，对全局地图中的关键帧与新加入局部地图中的关键帧进行匹配，得到匹配的关键帧，或者检测新加入的关键帧的图像码得到图像码，根据图像码携带的位置信息和匹配的关键帧得到的匹配自然特征的位置信息，以及里程计的测量信息计算得到全局地图中所有关键帧中的自然特征的信息，将得到的自然特征的位置信息添加到全局地图中，通过图像码携带的位置信息和自然特征的结合的方法能够在减少施工周期短同时得到更为精确的地图。
在一个实施例中，当检测到全局地图中增加了下一个关键帧，且下一个关键帧与全局地图中所有的关键帧存在匹配的关键帧时，根据匹配的关键帧之间共同观测的自然特征集计算得到下一个关键帧与匹配的关键帧的相对位置。
具体地，当检测到全局地图中增加了一个新的关键帧时，对新的关键帧的进行检测，当检测到全局地图中所有的关键帧中存在与新的关键帧相似的关键帧时，也就是能够找到匹配的关键帧时，将匹配的关键帧的共同观测到的自然特征集提取出来，利用共同观测到的自然特征集计算出相似的关键帧与新的关键帧的相对位置。
在一个实施例中，如图3所示，一种地图构建方法，还包括：
步骤S210，根据匹配自然特征集在局部地图中查找能够共同观测到预设范围内的所述匹配自然特征集的关键帧作为匹配关键帧集。
步骤S230，根据里程计测量信息和匹配自然特征集计算得到匹配关键帧集与下一关键帧的相对位置集。
具体地，根据匹配自然特征集中各个匹配自然特征在局部地图中查询能够共同观测到匹配自然特征，且观测到的匹配自然特征足够多的关键帧组成匹配关键帧集。根据里程计的相对位置信息和匹配自然特征集计算得到匹配关键帧集于局部地图中加入的新的关键帧的相对位置。
在一个实施例中，如图4所示，步骤S160包括：
步骤S162，根据关键帧中图像码的携带的位置信息集、下一个关键帧与匹配的关键帧的相对位置和里程计的测量信息集计算得到全局地图中所有关键帧的位姿集。
具体地，根据全局地图中所有的关键帧中包含的图像码携带的位置信息、新加入的关键帧与匹配的关键帧的相位位置和里程计的相对位置信息集共同进行优化得到全局地图中所有关键帧的位姿集。其中，关键帧的位姿集为世界坐标系中的位置。其中，优化的方法包括且不限于高斯牛顿法、莱文贝格-马夸特方法(Levenberg–Marquardtalgorithm)以及基于遗传算法的优化方法等。
步骤S164，按照预设条件在全局地图中查找关键帧，查找到满足预设条件的关键帧组成主要关键帧集。
其中，预设条件可以是且不限于是下列条件的任何一种：观测角度最接近平均观测角度的关键帧或自然特征描述最接近平均特征描述的关键帧。每一个自然特征都会有与之对应的主要关键帧。具体地，在全局地图中查找观测角度最接近平均观测角度的关键帧或自然特征描述最接近平均特征描述的关键帧作为与全局地图中所有关键帧中包含的所有的自然特征中各个自然特征对应的各个关键帧组成主要关键帧集。
步骤S166，根据匹配自然特征集相对于局部中所有关键帧的相对位置集和主要关键帧集计算得到匹配自然特征与匹配自然特征对应的主要关键帧的相对位置组成匹配相对位置集。
具体地，根据步骤S130中计算得到的匹配自然特征集相对于局部中所有关键帧的相对位置集和根据步骤S164中得到的主要关键帧集计算得到匹配自然特征与匹配自然特征对应的主要关键帧的相对位置组成匹配相对位置集。
步骤S168，根据全局地图中所有关键帧的位姿集以及匹配相对位置集，更新全局地图中自然特征的位置集。
具体地，根据步骤S162中计算得到的全局地图中所有关键帧的位姿和步骤S166中计算得到匹配相对位置集计算得到全局地图中所有关键帧中包含的自然特征的位置信息集，将上述自然特征的位置信息集添加到全局地图中得到新的全局地图。
在一个实施例中，如图5所示，步骤S110包括：
步骤S111，提取图像的自然特征。
具体地，采用自然特征提取算法提取图像中的自然特征。
步骤S113，检测图像中是否存在图像码。
具体地，通过图像检测算法对图像检测，检测图像中是否包含图像码。
步骤S115，当自然特征能够与上一个相邻关键帧自然特征成功匹配或检测到图像中存在图像码满足时，根据添加关键帧的预设条件将当前图像作为关键帧添加到局部地图中。
具体地，根据步骤S111中提取到的自然特征与局部地中上一个相邻关键帧的自然特征能够成果匹配时，或者检测图像中包含图像码时，当满足机器人相邻两个状态之间发生的位姿变化大于预设值，且当前图像与上一个相邻关键帧匹配的自然特征数目小于预设值，或者是根据里程计信息查找两个相邻机器人之间发生位姿变化大于预设值，且当前图像中检测到图像码时，将上述图像作为关键帧添加到局部地图中。其中，添加关键帧的预设条件是根据里程计信息查找机器人相邻两个状态之间发生的位姿变化大于预设值，且当前图像与上一个相邻关键帧匹配的自然特征数目小于预设值，或者是根据里程计信息查找相邻两个状态之间发生的运动大预设值，且当前图像中检测到图像码时。其中位姿变化大于预设值包含运动距离大于预设值和/或旋转角度大于预设值。
步骤S117，根据添加的当前关键的自然特帧和上一个关键帧的自然特征进行匹配得到相匹配的自然特征集，根据相匹配的自然特征集和图像码更新局部地图。
具体地，将步骤S115中添加的关键帧的自然特征和与局部地图中添加的关键帧相邻的上一个关键帧的自然特征进行匹配得到相匹配的自然特征集，根据得到的相匹配的自然特征集或者图像码更新局部地图。
步骤S119，将关键帧添加到全局地图。
具体地，将步骤115中得到的关键帧添加的全局地图中。
在一个实施例中，如图6所示，步骤S117包括：
步骤S1172，根据图像匹配算法对当前关键帧的自然特征集和上一个关键帧的自然特征集进行匹配，得到当前关键帧与上一个关键帧相同自然特征的匹配自然特征集。
具体地，计算两个关键帧的自然特征之间的相似度，当两个关键帧的自然特征的相似度达到预设范围时，表示两个关键帧中包含的自然特征为匹配自然特征，两个关键帧中包含的所有的匹配自然特征组成自然特征集合。其中，两个特征点是否匹配可以根据两个特征点的汉明距离确定。
步骤S1174，根据匹配自然特征集和里程计的测量信息计算得到匹配的自然特征集相对于当前关键帧的第一相对位置，将第一相对位置添加到局部地图中。
具体地，从里程计的测量信息中获取关键帧的当前位置信息和下一个关键帧的位置信息，或者当前关键帧和下一个关键帧的相对位置，根据匹配的自然特征和关键帧之间的相对位置计算得到匹配的自然特征和当前关键帧的相对位置，所有的匹配的自然特帧对应与当前关键帧的相对位置集组成第一相对位置集。通过匹配的自然特征和关键帧之间的相对位置还可以计算得到匹配的自然特征与下一个关键帧的相对位置。其中关键帧之间的相对位置可以从里程计的测量信息中获取到。
步骤S1176，根据里程计信息、局部地图中所有的关键帧中所有自然特征集的位置信息和图像码作为约束条件利用光束平差法对局部地图进行处理更新局部地图中所有的关键帧和自然特征的位置。
具体地，根据里程计信息和在局部地图中所有关键帧中包含的自然特征的观测信息以及局部地图中所有关键帧中包含的图像码作为约束条件，根据约束条件采用光束平差法对局部地图进行优化，使得观测值和根据观测值推算出来的预测值之间的误差最小，得到局部地图中所有的关键帧和关键帧包含的自然特征的位置。
在一个实施例中，读取一帧图像，然后利用ORB(Oriented FAST and RotatedBRIEF有方向的FAST特征(Features from Accelerated Segment Test，加速的分割测试特征)提取和旋转的BRIEF特征(Binary Robust Independent Elementary Features二进制可靠的独立单元特征))特征提取器提取图像中的自然特征，并检测图像中是否有二维码。如果之前未曾创建关键帧，则检查此帧图像是否满足关键帧要求，如满足，则创建第一个关键帧，如不满足，则丢弃当前数据。如果已经创建了关键帧，则与最近的关键帧进行特征匹配，如当前帧和关键帧有一定的位移，并且可以成功匹配，或者当前帧检测到了二维码，则创建新的关键帧，否则，丢弃当前的帧。紧接着，利用三角法，计算出新关键帧和旧关键帧中匹配的自然特征的三维位置，并添加至局部地图中。对于二维码，则线性的解决PNP问题，计算出相机相对于二维码的相对位置。最后，将自然特征与二维码的观测作为约束条件，利用光束平差法(Bundle adjustment)对局部地图进行优化，最大限度的降低观测误差，优化出局部地图中关键帧以及自然特征的位置，并把结果添加进全局地图。
在一个实施例中，如图7所示，提供一种机器人定位方法，包括：
步骤S410，当检测到拍摄到的当前帧中存在图像码时，根据图像码获取机器人的初始位姿。
具体地，获取里程计数据，根据里程计数据计算得到机器人的当前位置与机器人的上一位置的相对位置，根据相对位置以及机器人上一位置，计算当前位置的预测值。
步骤S430，根据当前位置的预测值查询全局地图中预设范围内的目标关键帧集和目标图像码集。
具体地，根据步骤S410中的得到的当前位置的预测值查询全局地图中附近的关集帧和图像码组成目标关键帧和目标图像码集。
步骤S450，根据目标关键帧集、目标图像码集和当前帧的自然特征集计算得到机器人的当前位姿。
具体地，根据目标关键帧集中的自然特征集与当前帧图像的自然特征进行匹配，根据匹配结果和目标图像码集作为约束条件利用光束平差法优化得到概率最大的位姿作为机器人的当前位姿。
步骤S470，获取下一帧图像作为当前帧，重复进入获取里程计数据，根据里程计数据计算得到不同时刻机器人的相对位置，根据相对位置和获取到的目标关键帧集和里程计信息计算得到机器人的当前位姿，直到处理完获取到的图像得到机器人的当前位姿。
其中，下一帧图像是通过相机或者其他的拍摄设备拍摄到的图像。具体地获取下一帧图像作为当前帧，重复执行上述步骤S410、步骤S430和步骤S450得到机器人的当前时刻的位姿。
在一个实施例中，如图8所示，步骤S450，包括：
步骤S451，获取目标关键帧集中各个目标关键帧的目标关键帧自然特征组成目标关键帧自然特征集。
具体地，根据全局地图中查询到的目标关键帧，根据目标关键帧集中包含的各个目标关键帧对应的目标关键帧自然特征组成目标关键帧自然特征集。
步骤S453，获取目标图像码集的位置集。
其中，目标图像码中携带关键帧的位置信息。具体地获取查询到的目标图像码中携带的位置信息作为位置集。
步骤S455，对自然特征集与目标关键帧自然特征集进行匹配得到匹配自然特征集。
具体地，根据特征匹配算法当期前帧图像的自然特征和目标关键帧自然特征集进行匹配，将成功匹配的自然特征作为匹配自然特征集。
步骤S457，根据匹配自然特征计算得到匹配位置集。
具体地，根据匹配自然特征计算得到匹配的自然特征的位置信息作为匹配位置集。
步骤S459，根据匹配位置集和位置集计算得到机器人的当前位姿。
具体地，根据查询到的图像码集携带的位置信息和根据匹配匹配自然特征计算得到的自然特征的位置信息计算当前机器人的位置信息，得到当前机器人的当前位姿。
在一个实施例中，机器人上安装的拍照装置拍摄图像，里程计获取机器人的位置，将拍摄到的图像和里程计的测量信息上传的处理器中，处理器对拍摄到的图像结合里程计信息进行处理得到当前时刻机器人的位姿。
在一个实施例中，如图9所示，提供一种地图构建装置100，包括：
关键帧获取模块110，用于获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧加入到局部地图和全局地图中。
匹配自然特征集计算模块120，用于当检测到所述局部地图中增加了下一个关键帧时，获取所述局部地图中当前关键帧和所述下一个关键帧的匹配自然特征集，将所述匹配自然特征集加入局部地图以及全局地图。
位姿计算模块130，用于获取里程计测量信息集，根据所述里程计测量信息集和所述匹配自然特征集计算得到所述匹配自然特征集与所述局部地图中所有关键帧的相对位置集和所述局部地图中所有关键帧的相对位姿集。
图像码携带的位置信息获取模块140，用于当检测到所述全局地图中增加了所述下一个关键帧，且所述下一个关键帧存在图像码时，获取所述下一个关键帧的图像码携带的位置信息。
相对位置获取模块150，用于获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集。
全局地图更新模块160，用于根据所述关键帧中图像码的携带的位置信息集、所述所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计的测量信息集、所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地中所述自然特征集的位置集。
地图更新模块170，用于重复进入获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到所述局部地图和所述全局地图，检测所述全局地图的关键帧，根据所述关键帧对所述局部地图进行更新，检测所述全局地图的关键帧，根据所述关键帧对所述全局地图进行更新，直至处理完所有获取到的图像得到所述全局地图。
在一个实施例中，如图10所示，地图构建装置100还包括：
匹配关键帧集获取模块210，用于根据所述匹配自然特征集在所述局部地图中查找能够共同观测到预设范围内的所述匹配自然特征集的关键帧作为匹配关键帧集。
相对位置集获取模块230，用于根据所述里程计测量信息和所述匹配自然特征集计算得到所述匹配关键帧集与所述下一关键帧的相对位置集。
在一个实施例中，如图11所示，模块160包括：
全局关键帧位姿获取单元162，用于根据所述关键帧中图像码的携带的位置信息集、所述所述下一个关键帧与所述匹配的关键帧的相对位置集和所述里程计的测量信息集计算得到所述全局地图中所有关键帧的位姿集。
主要关键帧集获取单元164，用于按照预设条件在所述全局地图中查找关键帧，查找到的满足所述预设条件的关键帧组成主要关键帧集。
匹配相对位置集获取单元166，用于根据所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述主要关键帧集计算得到所述匹配自然特征与所述匹配自然特征对应的主要关键帧的相对位置组成匹配相对位置集。
全局地图更新单元168，用于根据所述全局地图中所有关键帧的位姿集以及所述匹配相对位置集，更新全局地图中自然特征的位置集。
在一个实施例中，如图12所示，模块110包括：
自然特征提取单元111，用于提取所述图像的自然特征集。
图像码检测单元113，用于检测所述图像中是否存在图像码集。
局部关键帧添加单元115，用于当所述自然特征能够与上一个相邻关键帧自然特征成功匹配或检测到所述图像中存在图像码满足时，根据添加关键帧的预设条件将所述当前图像作为关键帧添加到所述局部地图中。
局部地图更新单元117，用于根据添加的关键帧的自然特帧集和所述上一个关键帧的自然特征集进行匹配得到相匹配的自然特征集，根据所述相匹配的自然特征集和所述图像码集更新所述局部地图。
全局关键帧添加模块119，用于将所述关键帧添加到全局地图。
在一个实施例中，如图13所示，单元117包括：
匹配自然特征集获取子单元1172，用于根据图像匹配算法对所述当前关键帧的自然特征集和所述上一个关键帧的自然特征集进行匹配，得到所述当前关键帧与所述上一个关键帧相同自然特征的所述匹配自然特征集。
局部地图相对位置添加子单元1174，用于根据所述匹配自然特征集和所述里程计的测量信息计算得到所述匹配的自然特征集相对于所述当前关键帧的第一相对位置，将所述第一相对位置添加到所述局部地图中。
局部地图优化子单元1176，用于根据所述里程计信息、所述局部地图中所有的关键帧中所有自然特征集的位置信息和所述图像码作为约束条件利用光束平差法对所述局部地图进行处理更新所述局部地图中所有的关键帧和自然特征的位置。
在一个实施例中，如图14所示，提供一种机器人定位装置400，包括：
初始位姿计算模块410，用于当检测到拍摄到的当前帧中存在图像码时，根据所述图像码获取机器人的初始位姿。
预测值计算模块430，用于获取里程计数据，根据所述里程计数据计算得到所述机器人的当前位置与所述机器人的上一位置的相对位置，根据所述相对位置以及机器人上一位置，计算当前位置的预测值。
自然特征集计算模块450，用于根据所述当前位置的预测值查询全局地图中预设范围内的目标关键帧集和目标图像码集。
当前位姿计算模块470，用于根据所述目标关键帧集、所述目标图像码集和所述当前帧的自然特征集计算得到所述机器人的当前位姿。
当前位姿更新模块490，用于获取下一帧图像作为所述当前帧，重复进入获取里程计数据，根据所述里程计数据计算得到不同时刻机器人的相对位置，根据所述相对位置和获取到的目标关键帧集和里程计信息计算得到所述机器人的当前位姿，直到处理完获取到的图像得到机器人的当前位姿。
在一个实施例中，如图15所示，模块450包括：
目标关键帧自然特征集获取单元451，用于获取所述目标关键帧集中各个目标关键帧的目标关键帧自然特征组成目标关键帧自然特征集。
位置集获取单元453，用于获取所述目标图像码集的位置集。
匹配自然特征集计算单元455，用于对所述自然特征集与所述目标关键帧自然特征集进行匹配得到匹配自然特征集。
匹配位置集计算单元457，用于根据所述匹配自然特征计算得到匹配位置集。
当前位姿计算单元459，用于根据所述匹配位置集和所述位置集计算得到所述机器人的当前位姿。
一种计算机设备，所述计算机设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时执行如下步骤：获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧加入到局部地图和全局地图中；当检测到所述局部地图中增加了下一个关键帧时，获取所述局部地图中当前关键帧和所述下一个关键帧的匹配自然特征集，将所述匹配自然特征集加入局部地图以及全局地图；获取里程计测量信息集；根据所述里程计测量信息集和所述匹配自然特征集计算得到所述匹配自然特征集与所述局部地图中所有关键帧的相对位置集和所述局部地图中所有关键帧的相对位姿集；当检测到所述全局地图中增加了所述下一个关键帧，且所述下一个关键帧存在图像码时，获取所述下一个关键帧的图像码携带的位置信息；获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集；根据所述关键帧中图像码的携带的位置信息集、所述所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计的测量信息集、所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地中所述自然特征集的位置集；重复进入获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到所述局部地图和所述全局地图，检测所述局部地图的关键帧，根据所述关键帧对所述局部地图进行更新，检测所述全局地图的关键帧，根据所述关键帧对所述全局地图进行更新，直至处理完所有获取到的图像得到所述全局地图。
在一个实施例中，所述处理器所执行的所述获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集，包括：当检测到所述全局地图中增加了下一个关键帧，且所述下一个关键帧与所述全局地图中所有的关键帧存在匹配的关键帧时，根据所述匹配的关键帧之间共同观测的自然特征集计算得到所述下一个关键帧与所述匹配的关键帧的相对位置集。
在一个实施例中，所述获取里程计测量信息集的步骤之后，所述处理器执行所述计算机程序时还用于实现以下步骤：根据所述匹配自然特征集在所述局部地图中查找能够共同观测到预设范围内的所述匹配自然特征集的关键帧作为匹配关键帧集；根据所述里程计测量信息和所述匹配自然特征集计算得到所述匹配关键帧集与所述下一关键帧的相对位置集。
在一个实施例中，所述处理器所执行的所述根据所述关键帧中图像码的携带的位置信息集、所述所述所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计的测量信息集、所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地中所述自然特征集的位置集，包括：根据所述关键帧中图像码的携带的位置信息集、所述所述下一个关键帧与所述匹配的关键帧的相对位置集和所述里程计的测量信息集计算得到所述全局地图中所有关键帧的位姿集；按照预设条件在所述全局地图中查找关键帧，查找到的满足所述预设条件的关键帧组成主要关键帧集；根据所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述主要关键帧集计算得到所述匹配自然特征与所述匹配自然特征对应的主要关键帧的相对位置组成匹配相对位置集；根据所述全局地图中所有关键帧的位姿集以及所述匹配相对位置集，更新全局地图中自然特征的位置集。
在一个实施例中，所述处理器所执行的所述根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到局部地图和全局地图，包括：提取所述图像的自然特征集；检测所述图像中是否存在图像码集；当所述自然特征能够与上一个相邻关键帧自然特征成功匹配或检测到所述图像中存在图像码满足时，根据添加关键帧的预设条件将所述当前图像作为关键帧添加到所述局部地图中；根据添加的关键帧的自然特帧集和所述上一个关键帧的自然特征集进行匹配得到相匹配的自然特征集，根据所述相匹配的自然特征集和所述图像码集更新所述局部地图；将所述关键帧添加到全局地图。
在一个实施例中，所述处理器所执行的所述根据所述当前关键的自然特帧集和所述上一个关键帧的自然特征集进行匹配得到匹配自然特征集，根据所述匹配自然特征集和所述图像码更新所述局部地图，包括：根据图像匹配算法对所述当前关键帧的自然特征集和所述上一个关键帧的自然特征集进行匹配，得到所述当前关键帧与所述上一个关键帧相同自然特征的所述匹配自然特征集；根据所述匹配自然特征集和所述里程计的测量信息计算得到所述匹配的自然特征集相对于所述当前关键帧的第一相对位置，将所述第一相对位置添加到所述局部地图中；根据所述里程计信息、所述局部地图中所有的关键帧中所有自然特征集的位置信息和所述图像码作为约束条件利用光束平差法对所述局部地图进行处理更新所述局部地图中所有的关键帧和自然特征的位置。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时执行如下步骤：获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧加入到局部地图和全局地图中；当检测到所述局部地图中增加了下一个关键帧时，获取所述局部地图中当前关键帧和所述下一个关键帧的匹配自然特征集，将所述匹配自然特征集加入局部地图以及全局地图；获取里程计测量信息集；根据所述里程计测量信息集和所述匹配自然特征集计算得到所述匹配自然特征集与所述局部地图中所有关键帧的相对位置集和所述局部地图中所有关键帧的相对位姿集；获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集；根据所述关键帧中图像码的携带的位置信息集、所述所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计的测量信息集、所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地中所述自然特征集的位置集；重复进入获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到所述局部地图和所述全局地图，检测所述局部地图的关键帧，根据所述关键帧对所述局部地图进行更新，检测所述全局地图的关键帧，根据所述关键帧对所述全局地图进行更新，直至处理完所有获取到的图像得到所述全局地图。
在一个实施例中，所述计算机程序被处理器执行的获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集，包括：当检测到所述全局地图中增加了下一个关键帧，且所述下一个关键帧与所述全局地图中所有的关键帧存在匹配的关键帧时，根据所述匹配的关键帧之间共同观测的自然特征集计算得到所述下一个关键帧与所述匹配的关键帧的相对位置集。
在一个实施例中，所述获取里程计测量信息集的步骤之后，所述计算机程序被处理器执行时还执行以下步骤：根据所述匹配自然特征集在所述局部地图中查找能够共同观测到预设范围内的所述匹配自然特征集的关键帧作为匹配关键帧集；根据所述里程计测量信息和所述匹配自然特征集计算得到所述匹配关键帧集与所述下一关键帧的相对位置集。
在一个实施例中，所述计算机程序被处理器执行的所述根据所述关键帧中图像码的携带的位置信息集、所述所述所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计的测量信息集、所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地中所述自然特征集的位置集，包括：根据所述关键帧中图像码的携带的位置信息集、所述所述下一个关键帧与所述匹配的关键帧的相对位置集和所述里程计的测量信息集计算得到所述全局地图中所有关键帧的位姿集；按照预设条件在所述全局地图中查找关键帧，查找到的满足所述预设条件的关键帧组成主要关键帧集；根据所述匹配自然特征集相对于所述局部中所有关键帧的相对位置集和所述主要关键帧集计算得到所述匹配自然特征与所述匹配自然特征对应的主要关键帧的相对位置组成匹配相对位置集；根据所述全局地图中所有关键帧的位姿集以及所述匹配相对位置集，更新全局地图中自然特征的位置集。
在一个实施例中，所述计算机程序被处理器执行的所述根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到局部地图和全局地图，包括：提取所述图像的自然特征集；检测所述图像中是否存在图像码集；当所述自然特征能够与上一个相邻关键帧自然特征成功匹配或检测到所述图像中存在图像码满足时，根据添加关键帧的预设条件将所述当前图像作为关键帧添加到所述局部地图中；根据添加的关键帧的自然特帧集和所述上一个关键帧的自然特征集进行匹配得到相匹配的自然特征集，根据所述相匹配的自然特征集和所述图像码集更新所述局部地图；将所述关键帧添加到全局地图。
在一个实施例中，所述计算机程序被处理器执行的所述根据所述当前关键的自然特帧集和所述上一个关键帧的自然特征集进行匹配得到匹配自然特征集，根据所述匹配自然特征集和所述图像码更新所述局部地图，包括：根据图像匹配算法对所述当前关键帧的自然特征集和所述上一个关键帧的自然特征集进行匹配，得到所述当前关键帧与所述上一个关键帧相同自然特征的所述匹配自然特征集；根据所述匹配自然特征集和所述里程计的测量信息计算得到所述匹配的自然特征集相对于所述当前关键帧的第一相对位置，将所述第一相对位置添加到所述局部地图中；根据所述里程计信息、所述局部地图中所有的关键帧中所有自然特征集的位置信息和所述图像码作为约束条件利用光束平差法对所述局部地图进行处理更新所述局部地图中所有的关键帧和自然特征的位置。
一种计算机设备，所述计算机设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：当检测到拍摄到的当前帧中存在图像码时，根据所述图像码获取机器人的初始位姿；获取里程计数据，根据所述里程计数据计算得到所述机器人的当前位置与所述机器人的上一位置的相对位置，根据所述相对位置以及机器人上一位置，计算当前位置的预测值；根据所述当前位置的预测值查询全局地图中预设范围内的目标关键帧集和目标图像码集；根据所述目标关键帧集、所述目标图像码集和所述当前帧的自然特征集计算得到所述机器人的当前位姿；获取下一帧图像作为所述当前帧，重复进入获取里程计数据，根据所述里程计数据计算得到不同时刻机器人的相对位置，根据所述相对位置和获取到的目标关键帧集和里程计信息计算得到所述机器人的当前位姿，直到处理完获取到的图像得到机器人的当前位姿。
在一个实施例中，所述处理器所执行的所述根据所述目标关键帧集、所述目标图像码集和所述当前关键帧计算得到所述机器人的当前位置，包括：获取所述目标关键帧集中各个目标关键帧的目标关键帧自然特征组成目标关键帧自然特征集；获取所述目标图像码集的位置集；对所述自然特征集与所述目标关键帧自然特征集进行匹配得到匹配自然特征集；根据所述匹配自然特征计算得到匹配位置集；根据所述匹配位置集和所述位置集计算得到所述机器人的当前位姿。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时执行以下步骤：当检测到拍摄到的当前帧中存在图像码时，根据所述图像码获取机器人的初始位姿；获取里程计数据，根据所述里程计数据计算得到所述机器人的当前位置与所述机器人的上一位置的相对位置，根据所述相对位置以及机器人上一位置，计算当前位置的预测值；根据所述当前位置的预测值查询全局地图中预设范围内的目标关键帧集和目标图像码集；根据所述目标关键帧集、所述目标图像码集和所述当前帧的自然特征集计算得到所述机器人的当前位姿；获取下一帧图像作为所述当前帧，重复进入获取里程计数据，根据所述里程计数据计算得到不同时刻机器人的相对位置，根据所述相对位置和获取到的目标关键帧集和里程计信息计算得到所述机器人的当前位姿，直到处理完获取到的图像得到机器人的当前位姿。
在一个实施例中，所述计算机程序被处理器执行的所述根据所述目标关键帧集、所述目标图像码集和所述当前关键帧计算得到所述机器人的当前位置，包括：获取所述目标关键帧集中各个目标关键帧的目标关键帧自然特征组成目标关键帧自然特征集；获取所述目标图像码集的位置集；对所述自然特征集与所述目标关键帧自然特征集进行匹配得到匹配自然特征集；根据所述匹配自然特征计算得到匹配位置集；根据所述匹配位置集和所述位置集计算得到所述机器人的当前位姿。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性的计算机可读取存储介质中，如本发明实施例中，该程序可存储于计算机系统的存储介质中，并被该计算机系统中的至少一个处理器执行，以实现包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(Random Access Memory，RAM)等。
据此，在一个实施例中还提供一种存储介质，其上存储有计算机程序，其中，该程序被处理器执行时实现如上述各实施例中的任意一个实施例的方法。
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (12)

1.一种地图构建方法，所述方法包括：
获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧加入到局部地图和全局地图中；
当检测到所述局部地图中增加了下一个关键帧时，获取所述局部地图中当前关键帧和所述下一个关键帧的匹配自然特征集，将所述匹配自然特征集加入局部地图以及全局地图；
获取里程计测量信息集；
根据所述里程计测量信息集和所述匹配自然特征集计算得到所述匹配自然特征集与所述局部地图中所有关键帧的相对位置集和所述局部地图中所有关键帧的相对位姿集；
当检测到所述全局地图中增加了所述下一个关键帧，且所述下一个关键帧存在图像码时，获取所述下一个关键帧的图像码携带的位置信息；
获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集；
根据所述关键帧中图像码的携带的位置信息集、所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计测量信息集、所述匹配自然特征集相对于所述局部地图中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地图中所述自然特征集的位置集；
重复进入获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到所述局部地图和所述全局地图，检测所述局部地图的关键帧，根据所述关键帧对所述局部地图进行更新，检测所述全局地图的关键帧，根据所述关键帧对所述全局地图进行更新，直至处理完所有获取到的图像得到所述全局地图。
2.根据权利要求1所述的方法，其特征在于，所述获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集的步骤，包括：
当检测到所述全局地图中增加了下一个关键帧，且所述下一个关键帧与所述全局地图中所有的关键帧存在匹配的关键帧时，根据所述匹配的关键帧之间共同观测的自然特征集计算得到所述下一个关键帧与所述匹配的关键帧的相对位置集。
3.根据权利要求1所述的方法，其特征在于，所述获取里程计测量信息集的步骤之后，还包括：
根据所述匹配自然特征集在所述局部地图中查找能够共同观测到预设范围内的所述匹配自然特征集的关键帧作为匹配关键帧集；
根据所述里程计测量信息集和所述匹配自然特征集计算得到所述匹配关键帧集与下一关键帧的相对位置集。
4.根据权利要求1所述的方法，其特征在于，所述根据所述关键帧中图像码的携带的位置信息集、所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计测量信息集、所述匹配自然特征集相对于所述局部地图中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地图中所述自然特征集的位置集的步骤，包括：
根据所述关键帧中图像码的携带的位置信息集、所述下一个关键帧与所述匹配的关键帧的相对位置集和所述里程计测量信息集计算得到所述全局地图中所有关键帧的位姿集；
按照预设条件在所述全局地图中查找关键帧，查找到的满足所述预设条件的关键帧组成主要关键帧集；
根据所述匹配自然特征集相对于所述局部地图中所有关键帧的相对位置集和所述主要关键帧集计算得到所述匹配自然特征集与所述匹配自然特征集对应的主要关键帧的相对位置组成匹配相对位置集；
根据所述全局地图中所有关键帧的位姿集以及所述匹配相对位置集，更新全局地图中自然特征的位置集。
5.根据权利要求1所述的方法，其特征在于，所述根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到所述局部地图和所述全局地图的步骤，包括：
提取所述图像的自然特征集；
检测所述图像中是否存在图像码集；
当所述自然特征集能够与上一个相邻关键帧自然特征集成功匹配或检测到所述图像中存在图像码满足时，根据添加关键帧的预设条件将当前所述图像作为关键帧添加到所述局部地图中；
根据添加的关键帧的自然特征集和所述上一个相邻关键帧自然特征集进行匹配得到相匹配的自然特征集，根据所述相匹配的自然特征集和所述图像码集更新所述局部地图；
将所述关键帧添加到全局地图。
6.根据权利要求5所述的方法，其特征在于，所述根据添加的关键帧的自然特征集和所述上一个相邻关键帧自然特征集进行匹配得到相匹配的自然特征集，根据所述相匹配的自然特征集和所述图像码集更新所述局部地图，包括：
根据图像匹配算法对当前所述关键帧的自然特征集和所述上一个相邻关键帧自然特征集进行匹配，得到当前所述关键帧与所述上一个相邻关键帧相同自然特征的相匹配的自然特征集；
根据所述相匹配的自然特征集和所述里程计测量信息集计算得到所述相匹配的自然特征集相对于当前所述关键帧的第一相对位置，将所述第一相对位置添加到所述局部地图中；
根据所述里程计测量信息集、所述局部地图中所有的关键帧中所有自然特征集的位置信息和所述图像码集作为约束条件利用光束平差法对所述局部地图进行处理更新所述局部地图中所有的关键帧和自然特征的位置。
7.一种机器人定位方法，其特征在于，所述方法包括：
当检测到拍摄到的当前帧中存在图像码时，根据所述图像码获取机器人的初始位姿；
获取里程计数据，根据所述里程计数据计算得到所述机器人的初始位姿与所述机器人的上一位置的相对位置，根据所述相对位置以及机器人上一位置，计算当前位置的预测值；
根据所述当前位置的预测值查询全局地图中预设范围内的目标关键帧集和目标图像码集；
根据所述目标关键帧集、所述目标图像码集和所述当前帧的自然特征集计算得到所述机器人的当前位姿；
获取下一帧图像作为所述当前帧，重复进入获取里程计数据，根据所述里程计数据计算得到不同时刻机器人的相对位置，根据所述相对位置和获取到的目标关键帧集和里程计信息计算得到所述机器人的当前位姿，直到处理完获取到的图像得到机器人的当前位姿。
8.根据权利要求7所述的方法，其特征在于，所述根据所述目标关键帧集、所述目标图像码集和所述当前帧的自然特征集计算得到所述机器人的当前位姿的步骤，包括：
获取所述目标关键帧集中各个目标关键帧的目标关键帧自然特征组成目标关键帧自然特征集；
获取所述目标图像码集的位置集；
对所述自然特征集与所述目标关键帧自然特征集进行匹配得到匹配自然特征集；
根据所述匹配自然特征集计算得到匹配位置集；
根据所述匹配位置集和所述位置集计算得到所述机器人的当前位姿。
9.一种地图构图装置，其特征在于，所述装置包括：
关键帧获取模块，用于获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧加入到局部地图和全局地图中；
匹配自然特征集计算模块，用于当检测到所述局部地图中增加了下一个关键帧时，获取所述局部地图中当前关键帧和所述下一个关键帧的匹配自然特征集，将所述匹配自然特征集加入局部地图以及全局地图；
位姿计算模块，用于获取里程计测量信息集，根据所述里程计测量信息集和所述匹配自然特征集计算得到所述匹配自然特征集与所述局部地图中所有关键帧的相对位置集和所述局部地图中所有关键帧的相对位姿集；
图像码携带的位置信息获取模块，用于当检测到所述全局地图中增加了所述下一个关键帧，且所述下一个关键帧存在图像码时，获取所述下一个关键帧的图像码携带的位置信息；
相位位置获取模块，获取全局地图中所述下一个关键帧与所述下一个关键帧对应的匹配的关键帧的相对位置集；
全局地图更新模块，根据所述关键帧中图像码的携带的位置信息集、所述下一个关键帧与所述匹配的关键帧的相对位置集、所述里程计测量信息集、所述匹配自然特征集相对于所述局部地图中所有关键帧的相对位置集和所述全局地图中所有关键帧的位姿集更新全局地图中所述自然特征集的位置集；
地图更新模块，用于重复进入获取图像，根据所述图像包含的自然特征集和图像码集建立关键帧，将所述关键帧添加到所述局部地图和所述全局地图，检测所述局部地图的关键帧，根据所述关键帧对所述局部地图进行更新，检测所述全局地图的关键帧，根据所述关键帧对所述全局地图进行更新，直至处理完所有获取到的图像得到所述全局地图。
10.一种机器人定位装置，其特征在于，所述装置包括：
初始位姿计算模块，用于当检测到拍摄到的当前帧中存在图像码时，根据所述图像码获取机器人的初始位姿；
预测值计算模块，用于获取里程计数据，根据所述里程计数据计算得到所述机器人的初始位姿与所述机器人的上一位置的相对位置，根据所述相对位置以及机器人上一位置，计算当前位置的预测值；
自然特征集计算模块，用于根据所述当前位置的预测值查询全局地图中预设范围内的目标关键帧集和目标图像码集，并获取当前关键帧的自然特征集，所述全局地图根据关键帧中的自然特征集和图像码集建立；
当前位姿计算模块，用于根据所述目标关键帧集、所述目标图像码集和所述当前帧的自然特征集计算得到所述机器人的当前位姿；
当前位姿更新模块，用于获取下一帧图像作为所述当前帧，重复进入获取里程计数据，根据所述里程计数据计算得到不同时刻机器人的相对位置，根据所述相对位置和获取到的目标关键帧集和里程计信息计算得到所述机器人的当前位姿，直到处理完获取到的图像得到机器人的当前位姿。
11.一种计算机设备，所述计算机设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至8中任一项所述的方法。
12.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至8中任一项所述的方法。
CN201711182940.7A 2017-11-10 2017-11-23 地图构建方法、装置和机器人定位方法、装置、计算机设备和存储介质 Active CN108038139B (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201711107108 2017-11-10
CN2017111071080 2017-11-10

Publications (2)

Publication Number Publication Date
CN108038139A CN108038139A (zh) 2018-05-15
CN108038139B true CN108038139B (zh) 2021-08-13

Family

ID=62093955

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711182940.7A Active CN108038139B (zh) 2017-11-10 2017-11-23 地图构建方法、装置和机器人定位方法、装置、计算机设备和存储介质

Country Status (1)

Country Link
CN (1) CN108038139B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109099915B (zh) * 2018-06-27 2020-12-25 未来机器人（深圳）有限公司 移动机器人定位方法、装置、计算机设备和存储介质
CN111322993B (zh) * 2018-12-13 2022-03-04 杭州海康机器人技术有限公司 一种视觉定位方法和装置
WO2020133415A1 (en) * 2018-12-29 2020-07-02 Beijing Didi Infinity Technology And Development Co., Ltd. Systems and methods for constructing a high-definition map based on landmarks
CN110335317B (zh) * 2019-07-02 2022-03-25 百度在线网络技术（北京）有限公司 基于终端设备定位的图像处理方法、装置、设备和介质
CN110561416B (zh) * 2019-08-01 2021-03-02 深圳市银星智能科技股份有限公司 一种激光雷达重定位方法及机器人
CN113701739B (zh) * 2020-05-22 2023-08-18 杭州海康机器人股份有限公司 一种地图更新方法以及装置
CN114742884B (zh) * 2022-06-09 2022-11-22 杭州迦智科技有限公司 一种基于纹理的建图、里程计算、定位方法及系统
CN116408808B (zh) * 2023-06-09 2023-08-01 未来机器人（深圳）有限公司 机器人取货检测方法及装置、机器人

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008224641A (ja) * 2007-03-12 2008-09-25 Masahiro Tomono カメラ姿勢推定システム
JP2012064131A (ja) * 2010-09-17 2012-03-29 Tokyo Institute Of Technology 地図生成装置、地図生成方法、移動体の移動方法、及びロボット装置
CN106296812B (zh) * 2016-08-18 2019-04-02 宁波傲视智绘光电科技有限公司 同步定位与建图方法
CN107160395B (zh) * 2017-06-07 2020-10-16 中国人民解放军装甲兵工程学院 地图构建方法及机器人控制系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
" Localization and navigation using QR code for mobile robot in indoor environment";Huijuan Zhang等;《2015 IEEE International Conference on Robotics and Biomimetics (ROBIO)》;20160225;全文 *

Also Published As

Publication number Publication date
CN108038139A (zh) 2018-05-15

Similar Documents

Publication Publication Date Title
CN108038139B (zh) 地图构建方法、装置和机器人定位方法、装置、计算机设备和存储介质
US10807236B2 (en) System and method for multimodal mapping and localization
CN109345588B (zh) 一种基于Tag的六自由度姿态估计方法
US20200218929A1 (en) Visual slam method and apparatus based on point and line features
CN109074085B (zh) 一种自主定位和地图建立方法、装置和机器人
US9420265B2 (en) Tracking poses of 3D camera using points and planes
US20110205338A1 (en) Apparatus for estimating position of mobile robot and method thereof
CN111489393A (zh) Vslam方法、控制器和可移动设备
US8380384B2 (en) Apparatus and method for localizing mobile robot
CN108801268B (zh) 目标对象的定位方法、装置及机器人
Palazzolo et al. Fast image-based geometric change detection given a 3d model
CN107735797B (zh) 用于确定第一坐标系与第二坐标系之间的运动的方法
JP2017526082A (ja) 動作推定方法、移動体、およびプロセッサに動作推定方法を実行させるコンピュータプログラムコードでコード化された非一時的コンピュータ可読媒体
KR101380628B1 (ko) 복수의 카메라를 사용한 객체 추적 방법 및 장치
KR20150120408A (ko) 카메라 보조 모션 방향 및 속도 추정
KR20100104581A (ko) 이동 로봇에서 자신의 위치를 추정하기 위한 방법 및 장치
CN113538410A (zh) 一种基于3d激光雷达和uwb的室内slam建图方法
KR101460313B1 (ko) 시각 특징과 기하 정보를 이용한 로봇의 위치 추정 장치 및 방법
WO2018207426A1 (ja) 情報処理装置、情報処理方法、及びプログラム
CN110570474B (zh) 一种深度相机的位姿估计方法及系统
Tamjidi et al. 6-DOF pose estimation of a portable navigation aid for the visually impaired
Palazzolo et al. Change detection in 3d models based on camera images
WO2014203743A1 (en) Method for registering data using set of primitives
CN111882663A (zh) 一种融合语义信息完成视觉slam闭环检测方法
Huang et al. Image-based localization for indoor environment using mobile phone

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