CN113822996A - 机器人的位姿估计方法及装置、电子设备、存储介质 - Google Patents

机器人的位姿估计方法及装置、电子设备、存储介质 Download PDF

Info

Publication number
CN113822996A
CN113822996A CN202111384555.7A CN202111384555A CN113822996A CN 113822996 A CN113822996 A CN 113822996A CN 202111384555 A CN202111384555 A CN 202111384555A CN 113822996 A CN113822996 A CN 113822996A
Authority
CN
China
Prior art keywords
dimensional
straight line
point cloud
points
list
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
CN202111384555.7A
Other languages
English (en)
Other versions
CN113822996B (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.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202111384555.7A priority Critical patent/CN113822996B/zh
Publication of CN113822996A publication Critical patent/CN113822996A/zh
Application granted granted Critical
Publication of CN113822996B publication Critical patent/CN113822996B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/253—Fusion techniques of extracted features
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/06—Topological mapping of higher dimensional structures onto lower dimensional surfaces
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Software Systems (AREA)
  • Geometry (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Remote Sensing (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computer Graphics (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种机器人的位姿估计方法及装置、电子设备、存储介质，包括：获取三维点云地图和机器人采集的二维图片；从所述二维图片中，提取二维直线；使用视觉里程计计算所述二维图片，获取位姿数据和三维特征点；使用点云的强度信息对所述三维点云地图进行平面分割，获得三维平面结构；从所述三维平面结构中获取三维直线；根据所述位姿数据，对所述三维特征点和所述三维点云地图进行最近邻搜索，得到点与点的第一对应关系；根据所述位姿数据，对所述二维直线和所述三维直线进行匹配筛选，得到二维与三维直线的第二对应关系；通过所述第一对应关系和所述第二对应关系，构建代价函数；通过所述代价函数，得到机器人的位姿。

Description

机器人的位姿估计方法及装置、电子设备、存储介质
技术领域
本申请涉及机器人视觉定位领域，特别是涉及一种机器人的位姿估计方法及装置、电子设备、存储介质。
背景技术
基于视觉的导航任务，在现实世界的机器人应用中显示出巨大的潜力，并吸引了越来越多的兴趣。为了实现成功的导航，导航器需要时刻获取当前机器人准确的位姿信息。基于激光雷达定位方案通常具有准确的定位结果，但是其价格昂贵，体积大，不便安装在小平台上。基于全球定位系统(Global Positioning System，GPS)的定位方案在室内和密集高楼无法使用。而相机因其价格便宜，质量轻，便携安装等优点广泛地用于自动驾驶和机器人的领域。
在实现本发明的过程中，发明人发现现有技术中至少存在如下问题：
现有的视觉在点云中定位的工作主要采用单一的点或线特征，而依靠单一的点或线特征的视觉定位算法对环境的比较敏感，适用范围受限。
发明内容
本申请实施例的目的是提供一种机器人的位姿估计方法及装置、电子设备、存储介质，以解决相关技术中存在的依靠单一的点或线特征的视觉定位算法对环境的比较敏感，适用范围受限的技术问题。
根据本申请实施例的第一方面，提供一种机器人的位姿估计方法，包括：
获取三维点云地图和机器人采集的二维图片；
从所述二维图片中，提取二维直线；
使用视觉里程计计算所述二维图片，获取位姿数据和三维特征点；
使用点云的强度信息对所述三维点云地图进行平面分割，获得三维平面结构；
从所述三维平面结构中获取三维直线；
根据所述位姿数据，对所述三维特征点和所述三维点云地图进行最近邻搜索，得到点与点的第一对应关系；
根据所述位姿数据，对所述二维直线和所述三维直线进行匹配筛选，得到二维与三维直线的第二对应关系；
通过所述第一对应关系和所述第二对应关系，构建代价函数；
通过所述代价函数，得到机器人的位姿。
进一步地，使用视觉里程计计算所述二维图片，获取位姿数据和三维特征点，包括：
使用视觉里程计算法，根据二维图片计算得到当前机器人的位姿数据；
通过视觉里程计算法中的三角化方法恢复二维图片中特征点的三维坐标信息，得到三维特征点。
进一步地，使用点云的强度信息对所述三维点云地图进行平面分割，获得三维平面结构，包括：
计算所述三维点云地图中每个点的特征信息；
根据所述特征信息，将所述三维点云地图的每个点放入不同列表中；
对所述列表内的点，在三维点云地图中搜索满足临近关系的点，将满足临近关系的点加入自身的列表中，删除满足临近关系的点所在的列表；
迭代进行满足临近关系的点加入自身的列表中的步骤，直到不存在满足临近关系的三维地图点，最终获得一个列表，迭代进行列表产生的过程，最终获得一组列表；
计算一组列表中每个列表内点集的特征信息，将该特征信息作为列表融合的判断信息；
对所述列表寻找临近列表，根据所述判断信息，决定所述列表和所述临近列表之间是否融合为新的列表，直到所有列表之间无法融合为止，最终获得一组新的列表；
根据所述一组新的列表，得到三维平面结构。
进一步地，从所述三维平面结构中获取三维直线，包括：
根据所述三维平面结构，构建局部二维坐标系；
将所述三维平面结构投影到二维坐标系中，得到所述三维平面结构上每个点对应的灰度值，形成灰度图；
提取所述灰度图的边缘；
将所述边缘反投影到三维空间中，获得三维直线。
进一步地，根据所述位姿数据，对所述三维特征点和所述三维点云地图进行最近邻搜索，得到点与点的第一对应关系，包括：
通过所述位姿数据，分割出三维点云地图中的局部子图；
将所述局部子图放入最近邻树中；
对所有三维特征点搜索所述最近邻树中的最近点，得到点与点的第一对应关系。
进一步地，根据所述位姿数据，对所述二维直线和所述三维直线进行匹配筛选，得到二维与三维直线的第二对应关系，包括：
通过所述位姿数据和相机内参，将所述三维直线投影到所述二维图片中；
根据所述二维图片的可视范围筛选出可用直线，并通过暴力搜索的方式，获得直线间最小距离的匹配对，该匹配对为第二对应关系。
进一步地，通过所述第一对应关系和所述第二对应关系，构建代价函数，包括：
根据所述第一对应关系，构建三维点与点的重投影误差的代价函数；
根据所述第二对应关系，构建直线距离的代价函数；
将所述三维点与点的重投影误差的代价函数和所述直线距离的代价函数相融合，作为最终的代价函数。
根据本申请实施例的第二方面，提供一种机器人的位姿估计装置，包括：
第一获取模块，用于获取三维点云地图和机器人采集的二维图片；
提取模块，用于从所述二维图片中，提取二维直线；
第二获取模块，用于使用视觉里程计计算所述二维图片，获取位姿数据和三维特征点；
第三获取模块，用于使用点云的强度信息对所述三维点云地图进行平面分割，获得三维平面结构；
第四获取模块，用于从所述三维平面结构中获取三维直线；
搜索模块，用于根据所述位姿数据，对所述三维特征点和所述三维点云地图进行最近邻搜索，得到点与点的第一对应关系；
筛选模块，用于根据所述位姿数据，对所述二维直线和所述三维直线进行匹配筛选，得到二维与三维直线的第二对应关系；
构建模块，用于通过所述第一对应关系和所述第二对应关系，构建代价函数；
计算模块，用于通过所述代价函数，得到机器人的位姿。
根据本申请实施例的第三方面，提供一种电子设备，包括：
一个或多个处理器；
存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的方法。
根据本申请实施例的第四方面，提供一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现如第一方面所述方法的步骤。
本申请的实施例提供的技术方案可以包括以下有益效果：
由上述实施例可知，本申请通过联合三维特征点与三维点云地图、二维直线与三维直线两种匹配关系，构建代价函数求解位姿的方法，解决仅靠单一特征求解位姿出现精度低，易退化的问题，可以提高视觉定位技术的准确性和鲁棒性。并且使用视觉传感器在三维点云地图定位获得与激光传感器定位近似的效果，但降低传感器成本，提高传感器配置的灵活性。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理。
图1是根据一示例性实施例示出的一种机器人的位姿估计方法的流程图。
图2是步骤S13的流程图。
图3是步骤S14的流程图。
图4是步骤S15的流程图。
图5是步骤S16的流程图。
图6是步骤S17的流程图。
图7是步骤S18的流程图。
图8是根据一示例性实施例示出的一种机器人的位姿估计装置框图。
具体实施方式
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。
在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
应当理解，尽管在本申请可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”。
图1是根据一示例性实施例示出的一种机器人的位姿估计方法的流程图，如图1所示，本发明实施例提供的一种机器人的位姿估计方法可以包括以下步骤：
步骤S11，获取三维点云地图和机器人采集的二维图片；
步骤S12，从所述二维图片中，提取二维直线；
步骤S13，使用视觉里程计计算所述二维图片，获取位姿数据和三维特征点；
步骤S14，使用点云的强度信息对所述三维点云地图进行平面分割，获得三维平面结构；
步骤S15，从所述三维平面结构中获取三维直线；
步骤S16，根据所述位姿数据，对所述三维特征点和所述三维点云地图进行最近邻搜索，得到点与点的第一对应关系；
步骤S17，根据所述位姿数据，对所述二维直线和所述三维直线进行匹配筛选，得到二维与三维直线的第二对应关系；
步骤S18，通过所述第一对应关系和所述第二对应关系，构建代价函数；
步骤S19，通过所述代价函数，得到机器人的位姿。
由上述实施例可知，本申请通过联合三维特征点与三维点云地图、二维直线与三维直线两种匹配关系，构建代价函数求解位姿的方法，解决仅靠单一特征求解位姿出现精度低，易退化的问题，可以提高视觉定位技术的准确性和鲁棒性。并且使用视觉传感器在三维点云地图定位获得与激光传感器定位近似的效果，但降低传感器成本，提高传感器配置的灵活性。
在步骤S11的具体实施中，获取三维点云地图和机器人采集的二维图片；
具体地，提前使用基于多传感器融合的同时定位与建图算法，构建机器人环境精确的三维点云地图，并通过体素滤波除去三维点云地图中的噪点，保存在硬盘中。当机器人进行视觉定位时，加载上述的三维点云地图，作为先验的环境信息。此时，机器上实时获得二维图片，用于计算机器人的位姿数据。
在步骤S12的具体实施中，从所述二维图片中，提取二维直线；
具体地，使用基于深度学习的LSD算法提取所述二维图片中的直线结构，获取得二维直线。使用传统的LSD算法检测二维图像中的直线会出现大量的琐碎的高噪声的直线段，在构建二维直线与三维直线匹配对时产生大量的异常匹配对，而使用基于深度学习的LSD算法提取二维图片中的鲁棒、准确的长直线，有利于求解位姿数据。解决传统的LSD算法提取短小的噪声直线问题，从而更适用于三维点云地图直线的匹配。
在步骤S13的具体实施中，使用视觉里程计计算所述二维图片，获取位姿数据和三维特征点；如图2所示，该步骤包括以下子步骤：
步骤S131，使用视觉里程计算法，根据二维图片计算得到当前机器人的位姿数据；
具体地，使用最先进的视觉里程计(Visual Odometry, VO)算法，根据机器人相机获得的二维图片，计算机器人的位姿数据。
步骤S132，通过视觉里程计算法中的三角化方法恢复二维图片中特征点的三维坐标信息，得到三维特征点。
具体地，订阅VO算法恢复的三维特征点，用于与三维点云地图做最近邻匹配。
在步骤S14的具体实施中，使用点云的强度信息对所述三维点云地图进行平面分割，获得三维平面结构；如图3所示，该步骤包括以下子步骤：
步骤S141，计算所述三维点云地图中每个点的特征信息；
具体地，对于每个地图点
Figure 682003DEST_PATH_IMAGE001
，使用KD树搜索其最近邻的N个地图点
Figure 412193DEST_PATH_IMAGE002
。计算
Figure 988668DEST_PATH_IMAGE002
的法 向量
Figure 180615DEST_PATH_IMAGE003
、曲率
Figure 557105DEST_PATH_IMAGE004
、尺度
Figure 176306DEST_PATH_IMAGE005
、强度信息
Figure 990809DEST_PATH_IMAGE006
作为
Figure 986447DEST_PATH_IMAGE001
的局部信息。
步骤S142，根据所述特征信息，将所述三维点云地图的每个点放入不同列表中；
具体地，根据三维地图点的曲率，进行升序排列，然后将每个点
Figure 452063DEST_PATH_IMAGE001
放入列表
Figure 992897DEST_PATH_IMAGE007
中，设 三维地图有
Figure 809543DEST_PATH_IMAGE008
个地图点，
Figure 77714DEST_PATH_IMAGE009
。
步骤S143，对所述列表内的点，在三维点云地图中搜索满足临近关系的点，将满足临近关系的点加入自身的列表中，删除满足临近关系的点所在的列表；
具体地，对列表
Figure 148569DEST_PATH_IMAGE007
中的每个点
Figure 640730DEST_PATH_IMAGE010
，在三维点云地图中使用最近邻搜素，寻找最近的 点
Figure 426896DEST_PATH_IMAGE011
。如果
Figure 498757DEST_PATH_IMAGE011
满足公式(1)-(4)，则将
Figure 938966DEST_PATH_IMAGE011
加入到列表
Figure 821602DEST_PATH_IMAGE007
中，并删除
Figure 347262DEST_PATH_IMAGE011
所在列表
Figure 973546DEST_PATH_IMAGE012
。式中
Figure 268261DEST_PATH_IMAGE013
=50
Figure 836646DEST_PATH_IMAGE014
。
Figure 600334DEST_PATH_IMAGE015
(1)
Figure 279577DEST_PATH_IMAGE016
(2)
Figure 182460DEST_PATH_IMAGE017
(3)
Figure 921746DEST_PATH_IMAGE018
(4)
步骤S144，迭代进行满足临近关系的点加入自身的列表中的步骤，直到不存在满足临近关系的三维地图点，最终获得一个列表，迭代进行列表产生的过程，最终获得一组列表；
具体地，当获得一个列表后，对第二个列表进行相同的步骤，产生第二个列表。循 环所有列表，产生一组列表。进一步优选地，如果列表内点的数量小于阈值
Figure 156419DEST_PATH_IMAGE019
(
Figure 124506DEST_PATH_IMAGE020
的值根据三 维点云地图分辨率自适应)，则删除该列表和列表内的点，这样可以去除噪点。
步骤S145，计算一组列表中每个列表内点集的特征信息，将该特征信息作为列表融合的判断信息；
具体地，每个列表
Figure 659392DEST_PATH_IMAGE007
代表一个平面
Figure 320312DEST_PATH_IMAGE021
，计算
Figure 42280DEST_PATH_IMAGE021
的特征信息，该特征信息包括平均强 度值
Figure 63326DEST_PATH_IMAGE022
、法向量
Figure 672293DEST_PATH_IMAGE023
、曲率
Figure 753381DEST_PATH_IMAGE024
和尺度
Figure 976028DEST_PATH_IMAGE025
，将该特征信息作为列表融合的判断信息。
步骤S146，对所述列表寻找临近列表，根据所述判断信息，决定所述列表和所述临近列表之间是否融合为新的列表，直到所有列表之间无法融合为止，最终获得一组新的列表；
具体地，对每个平面
Figure 800764DEST_PATH_IMAGE021
内的点分配相同的标签
Figure 779085DEST_PATH_IMAGE026
。遍历平面
Figure 516228DEST_PATH_IMAGE021
内点
Figure 212788DEST_PATH_IMAGE027
，如果
Figure 575636DEST_PATH_IMAGE028
中存 在点
Figure 424775DEST_PATH_IMAGE029
与
Figure 316507DEST_PATH_IMAGE027
标签不同，则认为
Figure 500364DEST_PATH_IMAGE027
为边缘点，
Figure 417636DEST_PATH_IMAGE029
所属的平面
Figure 104969DEST_PATH_IMAGE030
为
Figure 440705DEST_PATH_IMAGE021
的相邻平面。如果相邻平 面满足公式(5)-(7)则，对相邻平面融合为新的平面。迭代所有相邻平面，直到平面数量不 再增加，即获得一组新的列表，也就是三维点云地图中的三维平面结构。
Figure 846278DEST_PATH_IMAGE031
。
Figure 550929DEST_PATH_IMAGE032
Figure 109080DEST_PATH_IMAGE033
Figure 139353DEST_PATH_IMAGE034
步骤S147，根据所述一组新的列表，得到三维平面结构。
在步骤S15的具体实施中，从所述三维平面结构中获取三维直线；如图4所示，该步骤包括以下子步骤：
步骤S151，根据所述三维平面结构，构建局部二维坐标系；
具体地，三维平面结构的视为一个列表，计算局部的中心作为二维平面的原点
Figure 766644DEST_PATH_IMAGE035
。 原点与列表的第一个点
Figure 25718DEST_PATH_IMAGE036
组成的向量为二维平面的
Figure 687643DEST_PATH_IMAGE037
轴，
Figure 623238DEST_PATH_IMAGE038
轴为三维平面结构法向量与
Figure 754137DEST_PATH_IMAGE037
轴 叉乘获得。
步骤S152，将所述三维平面结构投影到二维坐标系中，得到所述三维平面结构上每个点对应的灰度值，形成灰度图；
步骤S153，提取所述灰度图的边缘；
具体地，使用边缘提取算法提取灰度图的边缘。
步骤S154，将所述边缘反投影到三维空间中，获得三维直线。
具体地，通过反投影，将二维平面灰度图中检测的边缘投影到三维空间，获得三维直线。
在步骤S16的具体实施中，根据所述位姿数据，对所述三维特征点和所述三维点云地图进行最近邻搜索，得到点与点的第一对应关系；如图5所示，该步骤包括以下子步骤：
步骤S161，通过所述位姿数据，分割出三维点云地图中的局部子图；
具体地，将三维点云地图分割成固定大小的二维栅格，根据机器人当前的平面位置和朝向角，加载机器人周围二维栅格所属的三维点云地图，获得局部子图。
步骤S162，将所述局部子图放入最近邻树中；
具体地，将局部子图放入KD树中，用于点的最近邻搜索。
步骤S163，对所有三维特征点搜索所述最近邻树中的最近点，得到点与点的第一对应关系。
在步骤S17的具体实施中，根据所述位姿数据，对所述二维直线和所述三维直线进行匹配筛选，得到二维与三维直线的第二对应关系；如图6所示，该步骤包括以下子步骤：
步骤S171，通过所述位姿数据和相机内参，将所述三维直线投影到所述二维图片中；
步骤S172，根据所述二维图片的可视范围筛选出可用直线，并通过暴力搜索的方式，获得直线间最小距离的匹配对，该匹配对为第二对应关系。
在步骤S18的具体实施中，通过所述第一对应关系和所述第二对应关系，构建代价函数；如图7所示，该步骤包括以下子步骤：
步骤S181，根据所述第一对应关系，构建三维点与点的重投影误差的代价函数；
具体地，构建第一个代价函数，即为三维点与点的重投影误差，
Figure 535011DEST_PATH_IMAGE039
，
Figure 317022DEST_PATH_IMAGE040
为三维地图点，
Figure 436900DEST_PATH_IMAGE041
为三维特征点，
Figure 304362DEST_PATH_IMAGE042
为三维特征点到三维点 云地图的位姿变换。
步骤S182，根据所述第二对应关系，构建直线距离的代价函数；
具体地，构建第二个代价函数，即为
Figure 905239DEST_PATH_IMAGE043
，其 中
Figure 541757DEST_PATH_IMAGE044
表示直线匹配对的数量
Figure 819154DEST_PATH_IMAGE045
表示相机内参，
Figure 659065DEST_PATH_IMAGE046
表示二维直线参数，
Figure 312901DEST_PATH_IMAGE047
为视觉里 程计输出位姿，
Figure 554657DEST_PATH_IMAGE007
为三维直线。
步骤S183，将所述三维点与点的重投影误差的代价函数和所述直线距离的代价函数相融合，作为最终的代价函数。
具体地，将所述三维点与点的重投影误差的代价函数和所述直线距离的代价函数 相加，作为最终的代价函数，最终的代价函数为
Figure 737377DEST_PATH_IMAGE048
。
由于结合点特征匹配和线特征匹配两种约束关系构建的代价函数，克服仅靠单一特征匹配定位的泛化能力弱，精度低的问题，进而可以提高视觉定位技术的准确性和鲁棒性。
与前述的机器人的位姿估计方法的实施例相对应，本申请还提供了机器人的位姿估计装置的实施例。
图8是根据一示例性实施例示出的一种机器人的位姿估计装置框图。参照图8，该装置包括：
第一获取模块11，用于获取三维点云地图和机器人采集的二维图片；
提取模块12，用于从所述二维图片中，提取二维直线；
第二获取模块13，用于使用视觉里程计计算所述二维图片，获取位姿数据和三维特征点；
第三获取模块14，用于使用点云的强度信息对所述三维点云地图进行平面分割，获得三维平面结构；
第四获取模块15，用于从所述三维平面结构中获取三维直线；
搜索模块16，用于根据所述位姿数据，对所述三维特征点和所述三维点云地图进行最近邻搜索，得到点与点的第一对应关系；
筛选模块17，用于根据所述位姿数据，对所述二维直线和所述三维直线进行匹配筛选，得到二维与三维直线的第二对应关系；
构建模块18，用于通过所述第一对应关系和所述第二对应关系，构建代价函数；
计算模块19，用于通过所述代价函数，得到机器人的位姿。
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本申请方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
相应的，本申请还提供一种电子设备，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述的机器人的位姿估计方法。
相应的，本申请还提供一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现如上述的机器人的位姿估计方法。
本领域技术人员在考虑说明书及实践这里公开的内容后，将容易想到本申请的其它实施方案。本申请旨在涵盖本申请的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本申请的一般性原理并包括本申请未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本申请的真正范围和精神由下面的权利要求指出。
应当理解的是，本申请并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本申请的范围仅由所附的权利要求来限制。

Claims (10)

1.一种机器人的位姿估计方法，其特征在于，包括：
获取三维点云地图和机器人采集的二维图片；
从所述二维图片中，提取二维直线；
使用视觉里程计计算所述二维图片，获取位姿数据和三维特征点；
使用点云的强度信息对所述三维点云地图进行平面分割，获得三维平面结构；
从所述三维平面结构中获取三维直线；
根据所述位姿数据，对所述三维特征点和所述三维点云地图进行最近邻搜索，得到点与点的第一对应关系；
根据所述位姿数据，对所述二维直线和所述三维直线进行匹配筛选，得到二维与三维直线的第二对应关系；
通过所述第一对应关系和所述第二对应关系，构建代价函数；
通过所述代价函数，得到机器人的位姿。
2.根据权利要求1所述的方法，其特征在于，使用视觉里程计计算所述二维图片，获取位姿数据和三维特征点，包括：
使用视觉里程计算法，根据二维图片计算得到当前机器人的位姿数据；
通过视觉里程计算法中的三角化方法恢复二维图片中特征点的三维坐标信息，得到三维特征点。
3.根据权利要求1所述的方法，其特征在于，使用点云的强度信息对所述三维点云地图进行平面分割，获得三维平面结构，包括：
计算所述三维点云地图中每个点的特征信息；
根据所述特征信息，将所述三维点云地图的每个点放入不同列表中；
对所述列表内的点，在三维点云地图中搜索满足临近关系的点，将满足临近关系的点加入自身的列表中，删除满足临近关系的点所在的列表；
迭代进行满足临近关系的点加入自身的列表中的步骤，直到不存在满足临近关系的三维地图点，最终获得一个列表，迭代进行列表产生的过程，最终获得一组列表；
计算一组列表中每个列表内点集的特征信息，将该特征信息作为列表融合的判断信息；
对所述列表寻找临近列表，根据所述判断信息，决定所述列表和所述临近列表之间是否融合为新的列表，直到所有列表之间无法融合为止，最终获得一组新的列表；
根据所述一组新的列表，得到三维平面结构。
4.根据权利要求1所述的方法，其特征在于，从所述三维平面结构中获取三维直线，包括：
根据所述三维平面结构，构建局部二维坐标系；
将所述三维平面结构投影到二维坐标系中，得到所述三维平面结构上每个点对应的灰度值，形成灰度图；
提取所述灰度图的边缘；
将所述边缘反投影到三维空间中，获得三维直线。
5.根据权利要求1所述的方法，其特征在于，根据所述位姿数据，对所述三维特征点和所述三维点云地图进行最近邻搜索，得到点与点的第一对应关系，包括：
通过所述位姿数据，分割出三维点云地图中的局部子图；
将所述局部子图放入最近邻树中；
对所有三维特征点搜索所述最近邻树中的最近点，得到点与点的第一对应关系。
6.根据权利要求1所述的方法，其特征在于，根据所述位姿数据，对所述二维直线和所述三维直线进行匹配筛选，得到二维与三维直线的第二对应关系，包括：
通过所述位姿数据和相机内参，将所述三维直线投影到所述二维图片中；
根据所述二维图片的可视范围筛选出可用直线，并通过暴力搜索的方式，获得直线间最小距离的匹配对，该匹配对为第二对应关系。
7.根据权利要求1所述的方法，其特征在于，通过所述第一对应关系和所述第二对应关系，构建代价函数，包括：
根据所述第一对应关系，构建三维点与点的重投影误差的代价函数；
根据所述第二对应关系，构建直线距离的代价函数；
将所述三维点与点的重投影误差的代价函数和所述直线距离的代价函数相融合，作为最终的代价函数。
8.一种机器人的位姿估计装置，其特征在于，包括：
第一获取模块，用于获取三维点云地图和机器人采集的二维图片；
提取模块，用于从所述二维图片中，提取二维直线；
第二获取模块，用于使用视觉里程计计算所述二维图片，获取位姿数据和三维特征点；
第三获取模块，用于使用点云的强度信息对所述三维点云地图进行平面分割，获得三维平面结构；
第四获取模块，用于从所述三维平面结构中获取三维直线；
搜索模块，用于根据所述位姿数据，对所述三维特征点和所述三维点云地图进行最近邻搜索，得到点与点的第一对应关系；
筛选模块，用于根据所述位姿数据，对所述二维直线和所述三维直线进行匹配筛选，得到二维与三维直线的第二对应关系；
构建模块，用于通过所述第一对应关系和所述第二对应关系，构建代价函数；
计算模块，用于通过所述代价函数，得到机器人的位姿。
9.一种电子设备，其特征在于，包括：
一个或多个处理器；
存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-7任一项所述的方法。
10.一种计算机可读存储介质，其上存储有计算机指令，其特征在于，该指令被处理器执行时实现如权利要求1-7中任一项所述方法的步骤。
CN202111384555.7A 2021-11-22 2021-11-22 机器人的位姿估计方法及装置、电子设备、存储介质 Active CN113822996B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111384555.7A CN113822996B (zh) 2021-11-22 2021-11-22 机器人的位姿估计方法及装置、电子设备、存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111384555.7A CN113822996B (zh) 2021-11-22 2021-11-22 机器人的位姿估计方法及装置、电子设备、存储介质

Publications (2)

Publication Number Publication Date
CN113822996A true CN113822996A (zh) 2021-12-21
CN113822996B CN113822996B (zh) 2022-02-22

Family

ID=78918013

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111384555.7A Active CN113822996B (zh) 2021-11-22 2021-11-22 机器人的位姿估计方法及装置、电子设备、存储介质

Country Status (1)

Country Link
CN (1) CN113822996B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114413881A (zh) * 2022-01-07 2022-04-29 中国第一汽车股份有限公司 高精矢量地图的构建方法、装置及存储介质
CN114800504A (zh) * 2022-04-26 2022-07-29 平安普惠企业管理有限公司 机器人位姿分析方法、装置、设备及存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105045263A (zh) * 2015-07-06 2015-11-11 杭州南江机器人股份有限公司 一种基于Kinect的机器人自定位方法
CN108242079A (zh) * 2017-12-30 2018-07-03 北京工业大学 一种基于多特征视觉里程计和图优化模型的vslam方法
CN109752003A (zh) * 2018-12-26 2019-05-14 浙江大学 一种机器人视觉惯性点线特征定位方法及装置
CN110470298A (zh) * 2019-07-04 2019-11-19 浙江工业大学 一种基于滚动时域的机器人视觉伺服位姿估计方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105045263A (zh) * 2015-07-06 2015-11-11 杭州南江机器人股份有限公司 一种基于Kinect的机器人自定位方法
CN108242079A (zh) * 2017-12-30 2018-07-03 北京工业大学 一种基于多特征视觉里程计和图优化模型的vslam方法
CN109752003A (zh) * 2018-12-26 2019-05-14 浙江大学 一种机器人视觉惯性点线特征定位方法及装置
CN110470298A (zh) * 2019-07-04 2019-11-19 浙江工业大学 一种基于滚动时域的机器人视觉伺服位姿估计方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
PAUL J.BESL等: "A Method for Registration of 3-D Shapes", 《IEEE TRANSACTIONS ON PATTERN ANALYSIS AND MACHINE INTERLLIGENCE》 *
吴乃亮等: "基于视觉里程计的移动机器人三维场景重构", 《华中科技大学学报（自然科学版）》 *
熊雄: "基于RGB-D相机的机器人室内导航技术研究", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114413881A (zh) * 2022-01-07 2022-04-29 中国第一汽车股份有限公司 高精矢量地图的构建方法、装置及存储介质
CN114413881B (zh) * 2022-01-07 2023-09-01 中国第一汽车股份有限公司 高精矢量地图的构建方法、装置及存储介质
CN114800504A (zh) * 2022-04-26 2022-07-29 平安普惠企业管理有限公司 机器人位姿分析方法、装置、设备及存储介质

Also Published As

Publication number Publication date
CN113822996B (zh) 2022-02-22

Similar Documents

Publication Publication Date Title
Dai et al. Rgb-d slam in dynamic environments using point correlations
CN110135455B (zh) 影像匹配方法、装置及计算机可读存储介质
Huang et al. Clusterslam: A slam backend for simultaneous rigid body clustering and motion estimation
CN113168717B (zh) 一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达
Taneja et al. City-scale change detection in cadastral 3D models using images
Wendel et al. Natural landmark-based monocular localization for MAVs
Yin et al. Dynam-SLAM: An accurate, robust stereo visual-inertial SLAM method in dynamic environments
JP2020067439A (ja) 移動体位置推定システムおよび移動体位置推定方法
Ding et al. Vehicle pose and shape estimation through multiple monocular vision
EP3274964B1 (en) Automatic connection of images using visual features
CN113822996B (zh) 机器人的位姿估计方法及装置、电子设备、存储介质
Singh et al. Fusing semantics and motion state detection for robust visual SLAM
Lu et al. A lightweight real-time 3D LiDAR SLAM for autonomous vehicles in large-scale urban environment
CN115239899B (zh) 位姿图生成方法、高精地图生成方法和装置
Jiang et al. Icp stereo visual odometry for wheeled vehicles based on a 1dof motion prior
Sohn et al. Sequential modelling of building rooftops by integrating airborne LiDAR data and optical imagery: preliminary results
Das et al. Taming the north: Multi-camera parallel tracking and mapping in snow-laden environments
Zhu et al. LVIF: a lightweight tightly coupled stereo-inertial SLAM with fisheye camera
CN113570713B (zh) 一种面向动态环境的语义地图构建方法及装置
Djordjevic et al. An accurate method for 3D object reconstruction from unordered sparse views
Zhu et al. Toward the ghosting phenomenon in a stereo-based map with a collaborative RGB-D repair
Bobkov et al. Noise-resistant Unsupervised Object Segmentation in Multi-view Indoor Point Clouds.
Hokmabadi et al. Accurate and Scalable Contour-based Camera Pose Estimation Using Deep Learning with Synthetic Data
Pal et al. Evolution of simultaneous localization and mapping framework for autonomous robotics—a comprehensive review
Knyaz Deep learning performance for digital terrain model generation

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