CN116255992A - 一种同时定位与建图的方法和装置 - Google Patents

一种同时定位与建图的方法和装置 Download PDF

Info

Publication number
CN116255992A
CN116255992A CN202310239527.9A CN202310239527A CN116255992A CN 116255992 A CN116255992 A CN 116255992A CN 202310239527 A CN202310239527 A CN 202310239527A CN 116255992 A CN116255992 A CN 116255992A
Authority
CN
China
Prior art keywords
map
features
sub
feature
processor
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
CN202310239527.9A
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.)
Beijing Tusen Weilai Technology Co Ltd
Original Assignee
Beijing Tusen Weilai Technology 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
Priority claimed from US15/684,389 external-priority patent/US10565457B2/en
Priority claimed from US15/684,414 external-priority patent/US10223807B1/en
Application filed by Beijing Tusen Weilai Technology Co Ltd filed Critical Beijing Tusen Weilai Technology Co Ltd
Publication of CN116255992A publication Critical patent/CN116255992A/zh
Pending legal-status Critical Current

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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/761—Proximity, similarity or dissimilarity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/764—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using classification, e.g. of video objects

Abstract

本发明公开一种同时定位与建图的方法和装置，以解决现有技术视觉SLAM定位方法存在定位漂移、定位不稳定的问题。该方法包括：SLAM装置获取一个环境的感知数据，感知数据中包括图像数据、点云数据和惯性导航数据；根据图像数据和惯性导航数据建立该环境的一个3D子地图，根据点云数据和惯性导航数据建立该环境的一个3D全局地图；从3D子地图和3D全局地图中分别抽取多个特征；根据从3D子地图和3D全局地图中分别抽取的多个特征，优化3D子地图中的特征的位置，得到用于提供定位信息的3D子地图。

Description

一种同时定位与建图的方法和装置
本申请为申请号为201810508563.X、申请日为2018年5月24日的发明名称为“一种同时定位与建图的方法和装置”的中国专利申请的分案申请。
技术领域
本发明视觉同时定位与建图(Simultaneous Localization and Mapping，SLAM)领域，特别涉及一种同时定位和建图的方法与装置。
背景技术
近年来智能车辆或者自动驾驶车辆逐渐得到推广。在自动驾驶车辆的多种应用中，一个关键的问题是如何在一个大规模的户外环境中进行稳定和平稳的定位。对于在户外环境中运行的陆地车辆例如一个自动驾驶车辆，获取定位信息的最主要的传感器是全球定位系统(Global Positioning System，GPS)。但是一个众所周知的问题是，在城市环境中GPS卫星信号不稳定，卫星信号的准确性也会由于例如城市高楼或者树木遮挡造成的多径效应而受到影响。
针对于此，许多辅助定位方法得到发展，用于弥补城市环境中无法通过GPS信号进行定位的问题。
基于视觉SLAM的方法通过建模地图并使用惯性导航系统根据构建的地图进行定位。但是现有的基于视觉SLAM进行定位的方法，在长时间运行后存在漂移现象，也即定位得到的位置与真实位置之间的差值越来越大。
可见，现有的视觉SLAM定位方法存在定位漂移、定位不稳定的问题。
发明内容
有鉴于此，本发明实施例提供了一种同时定位和建图的方法和装置，用以解决现有技术中视觉SLAM存在的定位漂移、定位不稳定的问题。
根据本申请的一个方面，提供了一种同时定位和建图的方法，包括：
SLAM装置获取一个环境的感知数据，感知数据中包括图像数据、点云数据和惯性导航数据；
根据图像数据和惯性导航数据建立该环境的一个3D子地图，根据点云数据和惯性导航数据建立该环境的一个3D全局地图；
从3D子地图和3D全局地图中分别抽取多个特征；
根据从3D子地图和3D全局地图中分别抽取的多个特征，优化3D子地图中的特征的位置，得到用于提供定位信息的3D子地图。
根据本申请的另一个方面，提供了一种同时定位和建图的装置，包括：一个处理器和至少一个存储器，至少一个存储器中存储有至少一条机器可执行指令，处理器读取并执行至少一个机器可执行指令以实现：
获取一个环境的感知数据，感知数据中包括图像数据、点云数据和惯性导航数据；
根据图像数据和惯性导航数据建立该环境的一个3D子地图，根据点云数据和惯性导航数据建立该环境的一个3D全局地图；
从3D子地图和3D全局地图中分别抽取多个特征；
根据从3D子地图和3D全局地图中分别抽取的多个特征，优化3D子地图中的特征的位置，得到用于提供定位信息的3D子地图。
根据本申请的另一个方面，提供了一种同时定位和建图的装置，包括：
数据获取模块，用于获取一个环境的感知数据，感知数据中包括图像数据、点云数据和惯性导航数据；
建图模块，用于根据图像数据和惯性导航数据建立该环境的一个3D子地图，根据点云数据和惯性导航数据建立该环境的一个3D全局地图；
定位模块，用于从3D子地图和3D全局地图中分别抽取多个特征；根据从3D子地图和3D全局地图中分别抽取的多个特征，优化3D子地图中的特征的位置，得到用于提供定位信息的3D子地图。
根据本申请实施例提供的技术方案，SLAM装置根据图像数据和惯性导航数据建立一个环境的3D子地图，根据点云数据和惯性导航数据建立该环境的3D全局地图，从3D子地图和3D全局地图中分别抽取多个特征，并根据抽取的特征优化3D子地图中特征的位置，得到用于提供定位信息的3D子地图。其中，3D全局地图是根据点云数据建立得到的，点云数据的物理测量信息相比图像数据更为准确，根据提取的特征优化3D子地图中的特征的位置，能够使得3D子地图具有更为准确的物理测量信息，相比现有技术的视觉SLAM仅通过图像数据进行定位，能够提供更为准确的位置信息；从而能够解决现有技术中视觉SLAM存在的定位漂移、定位不稳定的问题。
附图说明
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。
图1为本申请实施例提供的同时定位和建图的方法的处理流程图；
图2为在图1中的步骤103之后对齐3D子地图和3D全局地图的处理流程图；
图3为图1中步骤105的处理流程图；
图4为图1中步骤107的处理流程图；
图5为图4中步骤1071的处理流程图；
图6为图4中步骤1072的处理流程图；
图7为本申请实施例提供的同时定位和建图的装置的结构框图；
图8为本申请实施例提供的同时定位和建图的装置的另一个结构框图。
具体实施方式
为了使本技术领域的人员更好地理解本发明中的技术方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
针对现有技术中视觉SLAM存在的定位漂移、定位不稳定的问题，本申请实施例提供了一种SLAM的方法和装置，用以解决该问题。根据本申请实施例提供的技术方案，SLAM装置根据图像数据和惯性导航数据建立一个环境的3D子地图，根据点云数据和惯性导航数据建立该环境的3D全局地图，从3D子地图和3D全局地图中分别抽取多个特征，并根据抽取的特征优化3D子地图中的特征的位置，得到用于提供定位信息的3D子地图。其中，3D全局地图是根据点云数据建立得到的，点云数据的物理测量信息相比图像数据更为准确，根据提取的特征优化3D子地图中的特征的位置，能够使得3D子地图具有更为准确的物理测量信息，相比现有技术的视觉SLAM仅通过图像数据进行定位，能够提供更为准确的位置信息；从而能够解决现有技术中视觉SLAM存在的定位漂移、定位不稳定的问题。
以上是本发明的核心思想，为了使本技术领域的人员更好地理解本发明实施例中的技术方案，并使本发明实施例的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明实施例中技术方案作进一步详细的说明。
图1中示出了本申请实施例提供的同时定位和建图的方法的处理流程，包括：
步骤101、SLAM装置获取一个环境的感知数据，感知数据中包括图像数据、点云数据和惯性导航数据；
其中，图像数据可以是通过至少一个摄像头获取到的，点云数据可以是通过LiDAR获取到的，惯性导航数据可以是通过全球导航卫星系统-惯性测量单元(GlobalNavigation Satellite System-Inertial Measurement Unit，GNSS-IMU)获取到的位姿数据；
在SLAM装置获取感知数据之前，还可以先对相关的传感器和摄像头进行校正和时间同步，校正和时间同步的方法可以使用在本申请之前或者之后的方法，本申请不做具体限定；
步骤103、根据图像数据和惯性导航数据建立该环境的一个3D子地图，根据点云数据和惯性导航数据建立该环境的一个3D全局地图；
其中，3D子地图可以使用视觉SLAM技术根据图像数据和惯性导航数据建立得到，3D全局地图可以使用LiDAR建图技术根据点云数据和惯性导航数据建立得到；建立3D子地图和3D全局地图的方法，可以使用本申请之前的地图建立方法或者本申请之后的地图建立方法，本申请对此不做严格限制；
步骤105、从3D子地图和3D全局地图中分别抽取多个特征；
步骤107、根据从3D子地图和3D全局地图中分别抽取的多个特征，优化3D子地图中的特征的位置，得到用于提供定位信息的3D子地图。
根据图1所示的方法，3D全局地图是根据点云数据建立得到的，点云数据的物理测量信息相比图像数据更为准确，根据提取的特征来优化3D子地图中的特征的位置，能够使得3D子地图具有更为准确的物理测量信息，相比现有技术的视觉SLAM仅通过图像数据进行定位，能够提供更为准确的位置信息；从而能够解决现有技术中视觉SLAM存在的定位漂移、定位不稳定的问题。
下面对图1中的处理过程进行详细说明。
在图1所示方法的基础上，在一些实施例中，在上述步骤103建立了3D子地图和3D全局地图之后，还将3D子地图与3D全局地图进行对齐，该处理包括如图2所示的流程：
步骤103a、选取3D子地图中的至少一个点；
其中，选取的至少一个点可以是3D子地图中的任意点，例如选取一个中心点或者多个特征点，或者选取中心点和多个特征点；
步骤103b、根据惯性导航数据分别确定选取的至少一个点的经度、纬度和高度数据；
步骤103c、根据选取的至少一个点的经度、维度和高度数据，将3D子地图的坐标系转换为3D全局地图的坐标系。
其中，进行坐标系转换的方法可以使用本申请之前的坐标系方法或者本申请之后的方法，本申请这里不做严格限制。将3D子地图与3D全局地图对齐，能够使3D子地图的位置信息更为准确，为后续的处理提供更好的处理基础。
图3中示出上述步骤105的详细处理过程，包括：
步骤1051、按照预定的体素尺寸，对3D子地图和3D全局地图进行体素化，得到分别包括多个体素的3D子地图和3D全局地图；
体素化也即按照预定的体素(Voxel)尺寸，将3D空间划分为立体网格，一个立体网格即为一个体素；
步骤1052、确定3D子地图中各体素包括的3D点和3D全局地图中的各体素包括的3D点；
其中，3D子地图是根据图像数据建立得到的，将图像数据中的特征或者纹理投影到3D空间中，可以得到表达该特征或者纹理的3D点；
步骤1053、确定各体素中3D点的分布所表达的特征；
特征即为由多个聚集的3D点所表达出来的物体，这些特征可以包括直线、曲线、平面、曲面等等；
其中，可以使用概率模型估计体素中3D点的分布所表达的特征，并且可以使用本申请之前的概率模型或者本申请之后的概率模型，本申请对此不做严格限制；
步骤1054、在体素所包括的3D点所表达的特征是预定义的特征的情况下，确定该体素为特征体素；
其中，预定义的特征可以根据实际应用场景的需要而具体设定；
步骤1055、分别抽取3D子地图和3D全局地图的特征体素包括的特征；
步骤1056、按照预定义的特征类别，确定每一个抽取的特征所属的类别。
通过图3所示的处理，可以为3D子地图和3D全局地图中的每个体素确定一个特征类别，也即为每个体素确定一个语义类别，使得体素具有语义信息。
图4中示出了图1中步骤107的处理流程，包括：
步骤1071、对从3D子地图中抽取的多个特征和从3D全局地图中抽取的多个特征进行匹配，建立3D子地图的特征和3D全局地图的特征之间的对应关系；
步骤1072、根据所建立的3D子地图的特征和3D全局地图的特征之间的对应关系，优化3D子地图中的特征的位置，得到用于提供定位信息的3D子地图。
图4所示处理，根据匹配的具有对应关系的特征，对齐3D子地图和3D全局地图，能够使3D子地图中的特征具有更准确的、通过物理测量得到的位置信息。
对于图4中步骤1071，对从3D子地图中抽取的多个特征和从3D全局地图中抽取的多个特征进行匹配，建立3D子地图的特征和3D全局地图的特征之间的对应关系，图5中示出了的该步骤的处理流程，包括：
步骤51、对于属于同一个类别的特征，分别计算3D子地图中的该类别各抽取的特征与3D全局地图中该类别各抽取的特征之间的匹配分数；
其中，在一些实施例中，两个特征之间的匹配分数包括两个特征之间的相似度；
则，分别计算3D子地图中的该类别各抽取的特征与3D全局地图中该类别各抽取的特征之间的匹配分数，可以是分别计算3D子地图中该类别各抽取的特征与3D全局地图中该类别各抽取的特征之间的相似度；
例如，对于类别A，3D子地图中包括3个类别为A的特征，即特征1、特征2、特征3，3D全局地图中包括4个类别为A的特征，即特征a、特征b、特征c、特征d，分别确定特征1和特征a的相似度、特征1和特征b的相似度、特征1和特征c的相似度、特征1和特征d的相似度，同理，分别计算特征2和特征a、b、c、d之间的相似度，分别计算特征3和特征a、b、c、d之间的相似度。
对于匹配分数，本申请中还可以包括其它能够衡量两个特征之间的相似程度的量，并且可以根据具体应用场景的需要来设置。
步骤52、对于3D子地图中的各抽取的特征，在3D全局地图中选取出与该特征匹配分数最高的特征，作为待选特征对；
接上例，在特征1和特征a的相似度、特征1和特征b的相似度、特征1和特征c的相似度、特征1和特征d的相似度中，选取相似度的值最高的特征，例如，特征1和特征c的相似度最高，则选取特征1和特征c作为待选特征对。
步骤53、确定各个待选特征对中的两个特征之间的距离，在距离小于或等于预定阈值的情况下，确定选取的这两个特征为有效特征，建立这两个特征之间的对应关系。
具体地，在步骤53之前，还可以进一步根据如图2所示的处理流程对3D子地图和3D全局地图进行对齐，并在步骤53中，根据对齐后的3D子地图和3D全局地图，确定各个待选特征对中的两个特征之间的距离；由于3D子地图和3D全局地图已经进行了对齐操作，基于对齐后的3D子地图和3D全局地图，能够确定得到准确的待选特征对中两个特征之间的距离。
接上例，确定特征1在3D子地图中的位置，确定特征c在3D全局地图中的位置，如果这两个位置之间的差值小于或等于一个预定阈值，则特征1和特征c是有效特征，建立特征1和特征c之间的对应关系；否则，特征1和特征c是无效特征。可以丢弃无效特征或者不对无效的特征对做后续的处理。
对于图4中的步骤1072，其中，优化3D子地图中的特征的位置的操作，如图6所示，可以具体包括：
步骤61、对于具有对应关系的3D子地图中的特征和3D全局地图中的特征，将具有对应关系的特征的位置作为一个预定义的目标函数的输入，该目标函数输出一个代价(cost)值，该cost值为具有对应关系的特征间的距离的总和；其中，目标函数是一个根据3D子地图中和3D全局地图中具有对应关系的特征的位置，来表达3D子地图和3D全局地图之间的相对位置关系的函数；
其中，输入到目标函数中的具有对应关系的特征的位置，可以选择全部的具有对应关系的特征的位置，也可以选择部分具有对应关系的特征的位置，可以根据具体应用场景的需要来进行选择。
步骤62、在cost值大于一个预定的第一收敛阈值的情况下，进行迭代优化处理：即迭代修改3D子地图中特征的位置，并且迭代更新目标函数的cost值；在cost值小于或等于第一收敛阈值的情况下，或者相邻两次迭代处理之间的cost值的差值小于或等于一个预定的第二收敛阈值的情况下，结束迭代处理。
其中，可以采用迭代优化算法来实现迭代优化处理。迭代优化算法可以是本申请之前的优化算法，也可以是本申请之后的优化算法，本申请对此不做具体限定；例如可以采用Leverberg-Mariquardt算法(以下简称为L-M算法)对3D子地图中特征的位置进行迭代修改，使得3D子地图中特征的位置不断逼近3D全局地图中的位置。
并且，由于目标函数是根据具有对应关系的特征的位置来进行表达的函数，对3D子地图的位置进行修改后，目标函数也随之得到修改。优化修改后的目标函数能够更为准确地表达3D子地图和3D全局地图之间的相对位置关系。
通过上述迭代处理，根据建立的3D子地图和3D全局地图中的特征的对应关系，对3D子地图的位置进行优化，能够不断地将3D子地图中的特征的位置修改得逼近3D全局地图中对应的特征的准确的位置，得到具有准确位置信息的3D子地图，该3D子地图可用于提供位置信息。
从而，本申请实施例提供的上述方法，能够使得3D子地图具有更为准确的物理测量信息，相比现有技术的视觉SLAM仅通过图像数据进行定位，能够提供更为准确的位置信息；从而能够解决现有技术中视觉SLAM存在的定位漂移、定位不稳定的问题。
基于相同的发明构思，本申请实施例还提供了一种同时定位与建图的装置。
图7示出了本申请实施例提供的同时定位与建图的装置的结构，该装置包括一个处理器71和至少一个存储器72，至少一个存储器72中存储有至少一条机器可执行指令，处理器71读取并执行至少一个机器可执行指令以实现：
获取一个环境的感知数据，感知数据中包括图像数据、点云数据和惯性导航数据；
根据图像数据和惯性导航数据建立该环境的一个3D子地图，根据点云数据和惯性导航数据建立该环境的一个3D全局地图；
从3D子地图和3D全局地图中分别抽取多个特征；
根据从3D子地图和3D全局地图中分别抽取的多个特征，优化3D子地图中的特征的位置，得到用于提供定位信息的3D子地图。
在一些实施例中，处理器71执行至少一条机器可执行指令实现建立该环境的一个3D子地图和一个3D全局地图之后，还包括对齐3D子地图和3D全局地图，包括：选取3D子地图中的至少一个点；根据惯性导航数据分别确定选取的至少一个点的经度、纬度和高度数据；根据选取的至少一个点的经度、维度和高度数据，将3D子地图的坐标系转换为3D全局地图的坐标系。
在一些实施例中，处理器71执行至少一条机器可执行指令实现从3D子地图和3D全局地图中分别抽取多个特征，包括：按照预定的体素尺寸，对3D子地图和3D全局地图进行体素化，得到分别包括多个体素的3D子地图和3D全局地图；确定3D子地图中各体素包括的3D点和3D全局地图中的各体素包括的3D点；确定各体素中3D点的分布所表达的特征；在体素所包括的3D点所表达的特征是预定义的特征的情况下，抽取该特征；按照预定义的特征类别，确定每一个抽取的特征所属的类别。
在一些实施例中，处理器71执行至少一条机器可执行指令实现确定各体素中3D点的分布所表达的特征，包括：使用概率模型估计体素中3D点的分布所表达的特征。
在一些实施例中，处理器71执行至少一条机器可执行指令实现根据从3D子地图和3D全局地图中分别抽取的多个特征，优化3D子地图中的特征的位置，包括：对从3D子地图中抽取的多个特征和从3D全局地图中抽取的多个特征进行匹配，建立3D子地图的特征和3D全局地图的特征之间的对应关系；根据所建立的3D子地图的特征和3D全局地图的特征之间的对应关系，优化3D子地图中的特征的位置。
在一些实施例中，处理器71执行至少一条机器可执行指令实现对从3D子地图中抽取的多个特征和从3D全局地图中抽取的多个特征进行匹配，建立3D子地图的特征和3D全局地图的特征之间的对应关系，包括：对于属于同一个类别的特征，分别计算3D子地图中的该类别各抽取的特征与3D全局地图中该类别各抽取的特征之间的匹配分数；对于3D子地图中的各抽取的特征，在3D全局地图中选取出与该特征匹配分数最高的特征，作为待选特征对；确定各个待选特征对中的两个特征之间的距离，在距离小于或等于预定阈值的情况下，确定选取的这两个特征为有效特征，建立这两个特征之间的对应关系。
在一些实施例中，处理器71执行至少一条机器可执行指令实现在确定各个待选特征对中的两个特征之间的距离之前，还包括：对齐3D子地图和3D全局地图；则，处理器71执行至少一条机器可执行指令实现确定各个待选特征对中的两个特征之间的距离，包括：根据对齐后的3D子地图和3D全局地图，确定各个待选特征对中的两个特征之间的距离。
在一些实施例中，两个特征之间的匹配分数包括两个特征之间的相似度。
在一些实施例中，处理器71执行至少一条机器可执行指令实现根据建立的特征之间的对应关系，优化3D子地图中的特征的位置，包括：将具有对应关系的特征的位置作为一个预定义的目标函数的输入，该目标函数输出一个代价值，该代价值为具有对应关系的特征间的距离的总和；其中，目标函数是一个根据3D子地图中和3D全局地图中具有对应关系的特征的位置，来表达3D子地图和3D全局地图的相对位置关系的函数；在代价值大于一个预定的第一收敛阈值的情况下，迭代修改3D子地图中特征的位置；在代价值小于或等于第一收敛阈值的情况下，或者在相邻两次迭代处理之间的代价值的差值小于或等于一个预定的第二收敛阈值的情况下，结束迭代处理。
在一些实施例中，处理器71执行至少一条机器可执行指令实现迭代修改3D子地图中特征的位置，包括：采用Leverberg-Mariquardt算法迭代修改3D子地图中特征的位置。
通过本申请实施例提供的上述装置，能够使得3D子地图具有更为准确的物理测量信息，相比现有技术的视觉SLAM仅通过图像数据进行定位，能够提供更为准确的位置信息；从而能够解决现有技术中视觉SLAM存在的定位漂移、定位不稳定的问题。
基于相同的发明构思，本申请实施例还提供了一种同时定位与建图的装置。
图8示出了本申请实施例提供的同时定位与建图的装置的结构，该装置包括：
数据获取模块81，用于获取一个环境的感知数据，感知数据中包括图像数据、点云数据和惯性导航数据；
建图模块82，用于根据图像数据和惯性导航数据建立该环境的一个3D子地图，根据点云数据和惯性导航数据建立该环境的一个3D全局地图；
定位模块83，用于从3D子地图和3D全局地图中分别抽取多个特征；根据从3D子地图和3D全局地图中分别抽取的多个特征，优化3D子地图中的特征的位置，得到用于提供定位信息的3D子地图。
在一些实施例中，建图模块82在建立该环境的一个3D子地图和一个3D全局地图之后，还用于对齐3D子地图和3D全局地图，具体包括：选取3D子地图中的至少一个点；根据惯性导航数据分别确定选取的至少一个点的经度、纬度和高度数据；根据选取的至少一个点的经度、维度和高度数据，将3D子地图的坐标系转换为3D全局地图的坐标系。
在一些实施例中，定位模块83从3D子地图和3D全局地图中分别抽取多个特征，包括：按照预定的体素尺寸，对3D子地图和3D全局地图进行体素化，得到分别包括多个体素的3D子地图和3D全局地图；确定3D子地图中各体素包括的3D点和3D全局地图中的各体素包括的3D点；确定各体素中3D点的分布所表达的特征；在体素所包括的3D点所表达的特征是预定义的特征的情况下，抽取该特征；按照预定义的特征类别，确定每一个抽取的特征所属的类别。
在一些实施例中，定位模块83确定各体素中3D点的分布所表达的特征，包括：使用概率模型估计体素中3D点的分布所表达的特征。
在一些实施例中，定位模块83根据从3D子地图和3D全局地图中分别抽取的多个特征，优化3D子地图中的特征的位置，包括：对从3D子地图中抽取的多个特征和从3D全局地图中抽取的多个特征进行匹配，建立3D子地图的特征和3D全局地图的特征之间的对应关系；根据所建立的3D子地图的特征和3D全局地图的特征之间的对应关系，优化3D子地图中的特征的位置。
在一些实施例中，定位模块83对从3D子地图中抽取的多个特征和从3D全局地图中抽取的多个特征进行匹配，建立3D子地图的特征和3D全局地图的特征之间的对应关系，包括：对于属于同一个类别的特征，分别计算3D子地图中的该类别各抽取的特征与3D全局地图中该类别各抽取的特征之间的匹配分数；对于3D子地图中的各抽取的特征，在3D全局地图中选取出与该特征匹配分数最高的特征，作为待选特征对；确定各个待选特征对中的两个特征之间的距离，在距离小于或等于预定阈值的情况下，确定选取的这两个特征为有效特征，建立这两个特征之间的对应关系。
在一些实施例中，定位模块83在确定各个待选特征对中的两个特征之间的距离之前，还用于：对齐3D子地图和3D全局地图；则，定位模块83确定各个待选特征对中的两个特征之间的距离，包括：根据对齐后的3D子地图和3D全局地图，确定各个待选特征对中的两个特征之间的距离。
在一些实施例中，两个特征之间的匹配分数包括两个特征之间的相似度。
在一些实施例中，定位模块83根据建立的特征之间的对应关系，优化3D子地图中的特征的位置，包括：对于具有对应关系的3D子地图中的特征和3D全局地图中的特征，将具有对应关系的特征的位置作为一个预定义的目标函数的输入，该目标函数输出一个代价值，该代价值为具有对应关系的特征间的距离的总和；其中，目标函数是一个根据3D子地图中和3D全局地图中具有对应关系的特征的位置，来表达3D子地图和3D全局地图的相对位置关系的函数；在代价值大于一个预定的第一收敛阈值的情况下，迭代修改3D子地图中特征的位置；在代价值小于或等于第一收敛阈值的情况下，或者在相邻两次迭代处理之间的代价值的差值小于或等于一个预定的第二收敛阈值的情况下，结束迭代处理。
在一些实施例中，定位模块83迭代修改3D子地图中特征的位置，包括：采用Leverberg-Mariquardt算法迭代修改3D子地图中特征的位置。
通过本申请实施例提供的上述装置，能够使得3D子地图具有更为准确的物理测量信息，相比现有技术的视觉SLAM仅通过图像数据进行定位，能够提供更为准确的位置信息；从而能够解决现有技术中视觉SLAM存在的定位漂移、定位不稳定的问题。
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (20)

1.一种系统，包括：
至少一个处理器；以及
至少一个存储器，包括计算机程序指令，所述计算机程序指令在由所述至少一个处理器执行时引起所述系统至少：
基于来自相机的多个图像，生成包括第一多个特征的第一地图；
基于来自光测距传感器的数据，生成包括第二多个特征的第二地图；以及
基于所述第一多个特征和所述第二多个特征的比较，确定所述第一地图相对于所述第二地图的位置。
2.根据权利要求1所述的系统，其中，所述至少一个存储器进一步包括计算机程序指令，所述计算机程序指令在由所述至少一个处理器执行时引起所述系统至少：
将所述第一多个特征与所述第二多个特征进行比较以产生多个分数，所述多个分数中的每个分数包括所述第一多个特征中的一个特征与所述第二多个特征中的一个特征之间的距离。
3.根据权利要求2所述的系统，其中，所述至少一个存储器进一步包括计算机程序指令，所述计算机程序指令在由所述至少一个处理器执行时引起所述系统至少：
确定所述多个分数中的一个分数中包括的第一距离大于阈值距离；以及
响应于所述第一距离大于所述阈值距离的确定，去除所述第一多个特征中的一个特征与所述第二多个特征中的一个特征之间的对应关系。
4.根据权利要求1所述的系统，其中，为确定所述第一地图相对于所述第二地图的位置，所述至少一个存储器进一步包括计算机程序指令，所述计算机程序指令在由所述至少一个处理器执行时引起所述系统至少：
从所述第一地图中抽取所述第一多个特征；以及
从所述第二地图中抽取所述第二多个特征。
5.根据权利要求4所述的系统，其中，从所述第一地图中抽取的所述第一多个特征包括结构化特征和非结构化特征，并且其中，从所述第二地图中抽取的所述第二多个特征包括结构化特征和非结构化特征。
6.根据权利要求5所述的系统，其中，所述结构化特征包括平面、直线和曲线中的至少一者，并且其中，所述非结构化特征包括三维点。
7.根据权利要求4所述的系统，其中，为从所述第一地图中抽取所述第一多个特征以及从所述第二地图中抽取所述第二多个特征，所述至少一个存储器进一步包括计算机程序指令，所述计算机程序指令在由所述至少一个处理器执行时引起所述系统至少：
对所述第一地图进行体素化以产生第一多个体素；
对所述第二地图进行体素化以产生第二多个体素；以及
基于概率模型，生成所述第一多个体素和所述第二多个体素内三维点的分布。
8.根据权利要求4所述的系统，其中，为从所述第一地图中抽取所述第一多个特征以及从所述第二地图中抽取所述第二多个特征，所述至少一个存储器进一步包括计算机程序指令，所述计算机程序指令在由所述至少一个处理器执行时引起所述系统至少：
对所述第一多个特征和所述第二多个特征进行分类。
9.根据权利要求1所述的系统，其中，所述至少一个存储器进一步包括计算机程序指令，所述计算机程序指令在由所述至少一个处理器执行时引起所述系统至少：
基于从惯性导航模块接收到的信息，确定所述第一地图相对于所述第二地图的位置。
10.根据权利要求1所述的系统，其中，为生成所述第一地图，所述至少一个存储器进一步包括计算机程序指令，所述计算机程序指令在由所述至少一个处理器执行时引起所述系统至少：
执行视觉同时定位与建图。
11.根据权利要求1所述的系统，其中，所述第二地图包括城市规模三维地图。
12.根据权利要求1所述的系统，其中，为生成所述第二地图，所述至少一个存储器进一步包括计算机程序指令，所述计算机程序指令在由所述至少一个处理器执行时引起所述系统至少：
从惯性导航模块接收车辆姿态信息，所述车辆姿态信息包括车辆位置和取向信息；以及
基于所述接收到的车辆姿态信息生成所述第二地图。
13.一种方法，包括：
基于来自相机的多个图像，生成包括第一多个特征的第一地图；
基于来自光测距传感器的数据，生成包括第二多个特征的第二地图；以及
基于所述第一多个特征和所述第二多个特征的比较，确定所述第一地图相对于所述第二地图的位置。
14.根据权利要求13所述的方法，进一步包括：
将所述第一多个特征与所述第二多个特征进行比较以产生多个分数，所述多个分数中的每个分数包括所述第一多个特征中的第一特征与所述第二多个特征中的对应的第二特征之间的距离。
15.根据权利要求14所述的方法，其中，所述第一特征与所述对应的第二特征之间的距离由经训练的分类器确定。
16.根据权利要求14所述的方法，其中，确定所述第一地图相对于所述第二地图的位置包括执行对所述第一地图相对于所述第二地图的位置的迭代估计，直到所述第一多个特征与所述第二多个特征中的对应特征之间的距离小于阈值距离。
17.根据权利要求13所述的方法，进一步包括：
从所述第一地图中抽取所述第一多个特征；
从所述第二地图中抽取所述第二多个特征；以及
将所述第一多个特征和所述第二多个特征分类成多个类别。
18.根据权利要求17所述的方法，其中，从所述第一地图中抽取的所述第一多个特征包括结构化特征和非结构化特征，并且其中，从所述第二地图中抽取的所述第二多个特征包括结构化特征和非结构化特征。
19.根据权利要求17所述的方法，其中，从所述第一地图中抽取所述第一多个特征以及从所述第二地图中抽取所述第二多个特征进一步包括：
对所述第一地图进行体素化以产生第一多个体素；
对所述第二地图进行体素化以产生第二多个体素；以及
基于概率模型，生成所述第一多个体素和所述第二多个体素内三维点的分布。
20.一种非暂时性计算机可读介质，其存储指令，所述指令在由至少一个处理器执行时引起包括以下各者的操作：
基于来自相机的多个图像，生成包括第一多个特征的第一地图；
基于来自光测距传感器的数据，生成包括第二多个特征的第二地图；以及
基于所述第一多个特征和所述第二多个特征的比较，确定所述第一地图相对于所述第二地图的位置。
CN202310239527.9A 2017-08-23 2018-05-24 一种同时定位与建图的方法和装置 Pending CN116255992A (zh)

Applications Claiming Priority (5)

Application Number Priority Date Filing Date Title
US15/684,389 2017-08-23
US15/684,389 US10565457B2 (en) 2017-08-23 2017-08-23 Feature matching and correspondence refinement and 3D submap position refinement system and method for centimeter precision localization using camera-based submap and LiDAR-based global map
US15/684,414 2017-08-23
US15/684,414 US10223807B1 (en) 2017-08-23 2017-08-23 Feature extraction from 3D submap and global map system and method for centimeter precision localization using camera-based submap and lidar-based global map
CN201810508563.XA CN109425348B (zh) 2017-08-23 2018-05-24 一种同时定位与建图的方法和装置

Related Parent Applications (1)

Application Number Title Priority Date Filing Date
CN201810508563.XA Division CN109425348B (zh) 2017-08-23 2018-05-24 一种同时定位与建图的方法和装置

Publications (1)

Publication Number Publication Date
CN116255992A true CN116255992A (zh) 2023-06-13

Family

ID=65514481

Family Applications (2)

Application Number Title Priority Date Filing Date
CN202310239527.9A Pending CN116255992A (zh) 2017-08-23 2018-05-24 一种同时定位与建图的方法和装置
CN201810508563.XA Active CN109425348B (zh) 2017-08-23 2018-05-24 一种同时定位与建图的方法和装置

Family Applications After (1)

Application Number Title Priority Date Filing Date
CN201810508563.XA Active CN109425348B (zh) 2017-08-23 2018-05-24 一种同时定位与建图的方法和装置

Country Status (1)

Country Link
CN (2) CN116255992A (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020232709A1 (en) * 2019-05-23 2020-11-26 Beijing Didi Infinity Technology And Development Co., Ltd. Method and system for evaluating quality of a point cloud map
CN110263209B (zh) * 2019-06-27 2021-07-09 北京百度网讯科技有限公司 用于生成信息的方法和装置
CN110704562B (zh) * 2019-09-27 2022-07-19 Oppo广东移动通信有限公司 地图融合方法及装置、设备、存储介质
US11725944B2 (en) * 2020-03-02 2023-08-15 Apollo Intelligent Driving Technology (Beijing) Co, Ltd. Method, apparatus, computing device and computer-readable storage medium for positioning
CN111735451B (zh) * 2020-04-16 2022-06-07 中国北方车辆研究所 一种基于多源先验信息的点云匹配高精度定位方法
CN113819914A (zh) * 2020-06-19 2021-12-21 北京图森未来科技有限公司 一种地图构建方法及装置
CN111811502B (zh) * 2020-07-10 2022-07-22 北京航空航天大学 一种运动载体多源信息融合导航方法及系统
CN112596064B (zh) * 2020-11-30 2024-03-08 中科院软件研究所南京软件技术研究院 激光与视觉融合的一体化室内机器人全局定位方法
CN113639749A (zh) * 2021-07-02 2021-11-12 天津大学 一种基于不确定性的多波束测深数据匹配检测方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7639842B2 (en) * 2002-05-03 2009-12-29 Imagetree Corp. Remote sensing and probabilistic sampling based forest inventory method
US7774158B2 (en) * 2002-12-17 2010-08-10 Evolution Robotics, Inc. Systems and methods for landmark generation for visual simultaneous localization and mapping
JP6021897B2 (ja) * 2011-05-16 2016-11-09 フグロ ロームズ ピーティーワイ リミテッド 画像データを処理する方法およびシステム
US20160054409A1 (en) * 2013-04-09 2016-02-25 Tor WAGER Fmri-based neurologic signature of physical pain
CN103389103B (zh) * 2013-07-03 2015-11-18 北京理工大学 一种基于数据挖掘的地理环境特征地图构建与导航方法
CN104848851B (zh) * 2015-05-29 2017-08-18 山东鲁能智能技术有限公司 基于多传感器数据融合构图的变电站巡检机器人及其方法
US9630619B1 (en) * 2015-11-04 2017-04-25 Zoox, Inc. Robotic vehicle active safety systems and methods
CN106679648B (zh) * 2016-12-08 2019-12-10 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN106705964A (zh) * 2017-01-06 2017-05-24 武汉大学 全景相机融合imu、激光扫描仪定位与导航系统及方法

Also Published As

Publication number Publication date
CN109425348A (zh) 2019-03-05
CN109425348B (zh) 2023-04-07

Similar Documents

Publication Publication Date Title
CN109425348B (zh) 一种同时定位与建图的方法和装置
CN110426051B (zh) 一种车道线绘制方法、装置及存储介质
KR102145109B1 (ko) 지도 생성 및 운동 객체 위치 결정 방법 및 장치
CN111199564B (zh) 智能移动终端的室内定位方法、装置与电子设备
JP6745328B2 (ja) 点群データを復旧するための方法及び装置
CN109074085B (zh) 一种自主定位和地图建立方法、装置和机器人
EP3644015A1 (en) Position estimation system and position estimation method
CN107735797B (zh) 用于确定第一坐标系与第二坐标系之间的运动的方法
EP3274964B1 (en) Automatic connection of images using visual features
CN113048980B (zh) 位姿优化方法、装置、电子设备及存储介质
EP3852065A1 (en) Data processing method and apparatus
CN111862214B (zh) 计算机设备定位方法、装置、计算机设备和存储介质
CN111274847B (zh) 一种定位方法
CN109313809B (zh) 一种图像匹配方法、装置及存储介质
CN110751722B (zh) 同时定位建图方法及装置
CN113592015B (zh) 定位以及训练特征匹配网络的方法和装置
CN112733971B (zh) 扫描设备的位姿确定方法、装置、设备及存储介质
KR102249381B1 (ko) 3차원 영상 정보를 이용한 모바일 디바이스의 공간 정보 생성 시스템 및 방법
CN115239776B (zh) 点云的配准方法、装置、设备和介质
CN115239899B (zh) 位姿图生成方法、高精地图生成方法和装置
KR102130687B1 (ko) 다중 센서 플랫폼 간 정보 융합을 위한 시스템
CN114674328B (zh) 地图生成方法、装置、电子设备、存储介质、及车辆
CN113808196A (zh) 平面融合定位方法、装置、电子设备及存储介质
CN114511590A (zh) 基于单目视觉3d车辆检测与跟踪的路口多引导线构建方法
CN114111817A (zh) 基于slam地图与高精度地图匹配的车辆定位方法及系统

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