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

本申请为申请号为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存在的定位漂移、定位不稳定的问题。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。