CN111160298A - 一种机器人及其位姿估计方法和装置 - Google Patents
一种机器人及其位姿估计方法和装置 Download PDFInfo
- Publication number
- CN111160298A CN111160298A CN201911418099.6A CN201911418099A CN111160298A CN 111160298 A CN111160298 A CN 111160298A CN 201911418099 A CN201911418099 A CN 201911418099A CN 111160298 A CN111160298 A CN 111160298A
- Authority
- CN
- China
- Prior art keywords
- feature points
- image
- camera image
- feature
- point
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 239000013598 vector Substances 0.000 claims abstract description 73
- 238000013519 translation Methods 0.000 claims abstract description 22
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 21
- 238000005070 sampling Methods 0.000 claims abstract description 15
- 239000011159 matrix material Substances 0.000 claims description 22
- 238000004590 computer program Methods 0.000 claims description 20
- 230000006870 function Effects 0.000 claims description 8
- 230000014509 gene expression Effects 0.000 claims description 5
- 239000013604 expression vector Substances 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 2
- 238000006073 displacement reaction Methods 0.000 abstract description 2
- 230000009286 beneficial effect Effects 0.000 abstract 1
- 238000012545 processing Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 5
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000003384 imaging method Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 239000000126 substance Substances 0.000 description 2
- 230000001131 transforming effect Effects 0.000 description 2
- 238000012937 correction Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V40/00—Recognition of biometric, human-related or animal-related patterns in image or video data
- G06V40/20—Movements or behaviour, e.g. gesture recognition
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T19/00—Manipulating 3D models or images for computer graphics
- G06T19/20—Editing of 3D images, e.g. changing shapes or colours, aligning objects or positioning parts
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- 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/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
- G06V10/757—Matching configurations of points or features
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2200/00—Indexing scheme for image data processing or generation, in general
- G06T2200/04—Indexing scheme for image data processing or generation, in general involving 3D image data
-
- 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/10004—Still image; Photographic image
- G06T2207/10012—Stereo images
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2219/00—Indexing scheme for manipulating 3D models or images for computer graphics
- G06T2219/20—Indexing scheme for editing of 3D models
- G06T2219/2016—Rotation, translation, scaling
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
一种机器人的位姿估计方法包括：通过双目相机相邻两帧图像，提取所述相邻两帧图像中当前帧图像的左、右相机图像与所述相邻两帧图像中前一帧图像中的特征点；将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点；对所述第一特征点采用随机采样一致性算法计算得到第一旋转向量；重建所述第一特征点对应的三维坐标，根据重建的三维坐标与所述第一特征点进行运动估计，得到第一平移向量；根据所述第一旋转向量和第一平移向量估计所述机器人位姿。由于第一旋转向量根据第相邻两帧图像的平面特征点计算得到，可以减少相机外参数对旋转分量带来的影响，且旋转向量通常比位移向量大，有利于提高机器人位姿估计的精度。
Description
技术领域
本申请属于机器人领域，尤其涉及一种机器人及其位姿估计方法和装置。
背景技术
在机器人自主执行任务时，获取到准确的、机器人自身的运动姿态，是机器人自主执行任务的完成质量的关键。传统的机器人位姿估计方案，包括基于轮式里程计、惯性导航、激光导航等。但是轮式里程计在复杂场景下容易产生打滑导致测量不准，精确的惯性导航单元价格较为昂贵，低端的惯性导航单元则会随时间容易产生漂移；激光导航技术受限于工作原理，在户外等大场景下容易失效。
视觉导航具有相对较好的鲁棒性。但是，在使用双目相机获取机器人位姿信息时，受限于相机外参数的影响，仍然会导致机器人的位姿估计不准确。
发明内容
有鉴于此，本申请实施例提供了一种机器人及其位姿估计方法和装置，以解决现有技术中机器人通过视觉估计位姿时，受相机外参数的影响，导致机器人位姿估计不准确的问题。
本申请实施例的第一方面提供了一种机器人的位姿估计方法，所述机器人的位姿估计方法包括：
通过双目相机相邻两帧图像，提取所述相邻两帧图像中当前帧图像的左、右相机图像与所述相邻两帧图像中前一帧图像中的特征点；
将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点；
对所述第一特征点采用随机采样一致性算法计算得到第一旋转向量；
重建所述第一特征点对应的三维坐标，根据重建的三维坐标与所述第一特征点进行运动估计，得到第一平移向量；
根据所述第一旋转向量和第一平移向量估计所述机器人位姿。
结合第一方面，在第一方面的第一种可能实现方式中，所述将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点的步骤包括：
将当前帧左相机图像的特征点与当前帧右相机图像的特征点进行匹配，得到第二特征点；
将当前帧左相机图像的特征点与前一帧右相机图像的特征点进行匹配，得到第三特征点；
将当前帧左相机图像的特征点与前一帧左相机图像的特征点进行匹配，得到第四特征点；
将前一帧左相机图像的特征点与前一帧右相机图像的特征点进行匹配，得到第五特征点；
根据第二特征点、第三特征点、第四特征点和第五特征点，生成第一特征点。
结合第一方面的第一种可能实现方式，在第一方面的第二种可能实现方式中，在将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点的步骤之后，所述方法还包括：
根据所述第一特征点的位置，将所述第一特征点更新至与图像对应的预设网格中的方格中；
根据方格中的特征点的数量、特征点的生成时间、特征点所对应的描述子的健壮性，更新所述方格中的特征点，根据方格中的特征点对所述第一特征点进行更新。
结合第一方面的第二种可能实现方式，在第一方面的第三种可能实现方式中，所述根据方格中的特征点的数量、特征点的生成时间、特征点所对应的描述子的健壮性，更新所述方格中的特征点的步骤包括：
如果所述方格中的特征点的生成时间大于预设时长，则删除该特征点；
如果方格中特征点的数量大于预定数量且生成时间相同，则根据特征点的描述子的健壮性删除超出预定数量的特征点。
结合第一方面，在第一方面的第四种可能实现方式中，所述对所述第一特征点采用随机采样一致性算法计算得到第一旋转向量的步骤包括：
根据五点法和随机一致性算法计算所述第一特征点所对应的本质矩阵；
根据八点法和随机一致性算法计算所述第一特征点所对应的单应矩阵；
分解所述本质矩阵和所述单应矩阵，并选择重投影误差和旋转误差较小的旋转向量作为第一旋转向量。
结合第一方面的第四种可能实现方式，在第一方面的第五种可能实现方式中，所述方法还包括：
通过线性插值的方式，对所述第一旋转向量进行平滑运动估计。
结合第一方面，在第一方面的第六种可能实现方式中，所述重建所述第一特征点对应的三维坐标，根据重建的三维坐标与所述第一特征点进行运动估计，得到第一平移向量的步骤包括：
通过非线性优化方法，最小化损失函数
的方式获取所述第一平移向量，其中，n为匹配的特征点数量，pi为当前帧特征点在世界坐标系下的三维坐标，pi’为上一帧特征点在世界坐标系下的三维世界坐标系，ξ为位姿的李代数表达向量，ξ^为位姿的李代数表达式的反对称矩阵。
本申请实施例的第二方面提供了一种机器人的位姿估计装置，其特征在于，所述机器人的位姿估计装置包括：
图像获取单元，用于通过双目相机相邻两帧图像，提取所述相邻两帧图像中当前帧图像的左、右相机图像与所述相邻两帧图像中前一帧图像中的特征点；
特征点匹配单元，用于将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点；
旋转向量获取单元，用于对所述第一特征点采用随机采样一致性算法计算得到第一旋转向量；
平移向量获取单元，用于重建所述第一特征点对应的三维坐标，根据重建的三维坐标与所述第一特征点进行运动估计，得到第一平移向量；
位姿估计单元，用于根据所述第一旋转向量和第一平移向量估计所述机器人位姿。
本申请实施例的第三方面提供了一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如第一方面任一项所述机器人的位姿估计方法的步骤。
本申请实施例的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如第一方面任一项所述机器人的位姿估计方法的步骤。
本申请实施例与现有技术相比存在的有益效果是：通过采用双目相机采集的相邻两帧图像的特征点进行匹配得到第一特征点，并根据匹配的第一特征点采用随机采样一致性算法计算得到第一旋转向量，通过第一特征点重建对应的三维坐标，根据重建的三维坐标和第一特征点进行运动估计得到第一平移向量，根据第一平移向量和第一旋转向量估计所述机器人位姿。由于第一旋转向量根据相邻两帧图像的平面特征点计算得到，可以减少相机外参数对旋转分量带来的影响，有利于提高位姿估计的精度。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的一种机器人的位姿估计方法的实现流程示意图；
图2是本申请实施例提供的一种机器人的位姿估计装置的示意图；
图3是本申请实施例提供的机器人的示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本申请。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。
为了说明本申请所述的技术方案，下面通过具体实施例来进行说明。
图1为本申请实施例提供的一种机器人的位姿估计方法的实现流程示意图，详述如下：
在步骤S101中，通过双目相机相邻两帧图像，提取所述相邻两帧图像中当前帧图像的左、右相机图像与所述相邻两帧图像中前一帧图像中的特征点；
其中，所述当前帧图像可以为第k帧图像，相邻两帧图像中的前一帧图像可以为第k-1帧图像，k为自然数。
为了便于对双目相机采集的图像进行匹配，在获取到双目相机的图像后，可以对双目相机所获取的图像按照时间对齐，使得双目相机中的左相机所采集的第k帧图像，与右相机所采集的第k帧图像的时间点相同。通过时间对齐后，左相机所采集的第i帧图像的时间，与右相机采集的第i帧图像的时间相同。
为了便于对采集的图像进行位姿估计，还可以对采集的图像进行畸变校正和/或高斯模糊处理等操作。
根据成像过程中的坐标系的转换，即将空间中的点由世界坐标系转换到摄像机坐标系，确定如果再将其投影到成像平面的图像坐标系，最后再将成像平面上的数据转换到图像平面的像素坐标系，在坐标系的转换过程中，真实图像经过畸变后得到畸变图像，可以通过预先设定的畸变数学模型，对畸变图像进行校正，使其恢复为真实图像。
通过高斯模糊对图像进行降采样处理，可以得到图像高斯金字塔，可以通过所述图像高斯金字塔提取第k帧图像的左、右相机图像以及第k-1帧的左、右相机图像的特征点。所提取的特征点可以为第k帧左、右相机图像和第k-1帧左、右相机图像通过图像高斯金字塔处理后所提取的FAST(Features From Accelerated Segment Test)特征点，并可生成所述特征点所对应的描述子。
所提取的特征点，可包括当前采集的图像的特征点，以及当前帧的前一帧图像的特征点，即第k帧图像的左、右相机所采集的图像，以及第k-1帧左、右相机所采集的图像。
在步骤S102中，将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点；
在获取第k帧图像和第k-1帧图像所包括的特征点后，可以根据所提取的特征点进行匹配，包括将第k帧的左相机图像的特征点与第k帧右相机图像的特征点匹配、第k-1帧左相机图像与第k-1帧右相机图像的特征点匹配、将第k帧左相机图像与第k-1帧左相机图像的特征点匹配，将第k帧左相机图像与第k-1帧右相机图像的特征点匹配，以及第k帧右相机图像与第k-1帧左相机图像的特征匹配，得到多个匹配的特征点，比如可以包括：
将第k帧左相机图像的特征点与第k帧右相机图像的特征点进行匹配，得到第二特征点；
将第k帧左相机图像的特征点与第k-1帧右相机图像的特征点进行匹配，得到第三特征点；
将第k帧左相机图像的特征点与第k-1帧左相机图像的特征点进行匹配，得到第四特征点；
将第k-1帧左相机图像的特征点与第k-1帧右相机图像的特征点进行匹配，得到第五特征点；
根据第二特征点、第三特征点、第四特征点和第五特征点，生成第一特征点，并可记录特征点的生成时间和对应的描述子。
根据所匹配的特征点构成第一特征点，可以将匹配到的特征点，根据第一特征点所在的位置，将其存放在预定大小的网格中的方格中。比如，所述网格可以为50*50大小，每个网格对应图像中的预定区域的像素。可以将图像横向和纵向分别划分为预定数量等份，形成由多个方格构成的网格，每个方格对应一定区域面积的像素。当第k帧图像与第k-1帧图像的特征点进行匹配得到特征点时，可以根据匹配到的特征点的位置，填充到所述图像对应的网格中的方格中，并根据不断采集的图像更新所述网格中的特征点。
在对所述网格中的特征点进行更新时，可以根据特征点的数量、特征点的生成时间、特征点所对应的描述子的健壮性，更新所述方格中的特征点，根据方格中的特征点对所述第一特征点进行更新。
比如，可以预先设定每个方格中存储的特征点的个数，当方格中存储的特征点的数量大于预定的存储数量时，可以根据特点的生成时间，来删除生成时间较早的特征点。
又或者，可以根据特征点的生成时间范围对所述方格中存储的特征的个数进行更新。可以预先设定方格中存储的特征点的生成时间范围，当特征点的生成时间超出所预设的时间范围时，则删除生成时间超出所预设的时间范围的特征点。比如，所述预设的时间范围可以为10帧时长，如果特征点的生成时间为10帧时长之前，则删除该特征点。
当方格中的特征点数量超出预定数量，并且需要删除的特征点的生成时间相同时，则可以根据特征点所对应的搭配子的健壮性，来确定需要删除的特征点。比如，可以将描述子的健壮性较弱的特征点优先删除。
在步骤S103中，对所述第一特征点采用随机采样一致性算法计算得到第一旋转向量；
根据所生成的第一特征点，结合随机采样一致性算法计算当前采集的第k帧图像的本质矩阵，计算第k帧图像所对应的单应矩阵，并将所述本质矩阵和单应矩阵分解得到两组旋转平移向量，可以选择重投影误差较小的一组旋转平移向量。
比如，可以通过开启线程1，使用五点法和随机采样一致性算法，计算第k帧图像所对应的本质矩阵
可以通过开启线程2，使用八点法和随机采样一致性算法，计算第k帧图像所对应的单应矩阵
其中，
表示第k帧相对于第k-1帧的本质矩阵，
表示第k帧相对于第k-1帧的单应矩阵。
在一种实现方式中，还可以通过线性插值的方式，对得到第一旋转向量进行平滑运动估计，假设旋转向量可以表示如下：
其中，
是指当前帧k和前一帧k-1的特征点通过分解本质矩阵
和单应矩阵
得到的旋转向量或旋转矩阵。
那么，可以通过多次迭代的方式实现线性插值平滑运动估计。比如，根据旋转向量的表示方式，可以得到不同次迭代的表示式：
第一次迭代：
第二次迭代：
第三次迭代：
通过多次迭代，可引入当前时间之前的旋转向量，从而对当前的旋转向量能够得到平滑运动估计。
其中，五点算法是基于完全针模型而提出的求解本质矩阵的新的研究方向，最早由Sarnoff公司提出，当摄像机在两幅图像之间的运动为纯平移运动时，给定5对图像对应点，则可以线性确定本质矩阵。
所述八点算法包括求线性解和奇异性约束两个步骤，是用于计算单应矩阵的最简单的方法，为了提高解的稳定性和精度，往往会对输入点集的坐标先进行归一化处理。
在步骤S104中，重建所述第一特征点对应的三维坐标，根据重建的三维坐标与所述第一特征点进行运动估计，得到第一平移向量；
可以将所述第一特征点进行三角化处理，即通过不同位置观察到三维点的二维投影点，利用三角关系，恢复出三维点的深度信息。
根据重建的第一特征点对应的三维坐标，结合所述第一特征点进行运动估计，得到第一平移向量。比如，可以通过非线性优化的方法，最小化损失函数：
的方式获取所述第一平移向量，其中，n为匹配的特征点数量，pi为当前帧特征点在世界坐标系下的三维坐标，pi’为上一帧特征点在世界坐标系下的三维世界坐标系，ξ为位姿的李代数表达向量，ξ^为位姿的李代数表达式的反对称矩阵
在步骤S105中，根据所述第一旋转向量和第一平移向量估计所述机器人位姿。
根据第k帧图像和第k-1帧图像所提取的特征点进行匹配计算旋转分量，可以使得对位姿的第一旋转分量估计时，不需要依赖相机外参数，有利于提高第一旋转向量的精度，并且对于大部分场景下旋转向量比位移大，因此，在提高了第一旋转向量的精度时，有利于更为准确的对机器人位姿进行估计。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。
图2为本申请实施例提供的一种机器人的位姿估计装置的结构示意图，详述如下：
所述机器人的位姿估计装置包括：
图像获取单元201，用于通过双目相机相邻两帧图像，提取所述相邻两帧图像中当前帧图像的左、右相机图像与所述相邻两帧图像中前一帧图像中的特征点；
特征点匹配单元202，用于将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点；
旋转向量获取单元203，用于对所述第一特征点采用随机采样一致性算法计算得到第一旋转向量；
平移向量获取单元204，用于重建所述第一特征点对应的三维坐标，根据重建的三维坐标与所述第一特征点进行运动估计，得到第一平移向量；
位姿估计单元205，用于根据所述第一旋转向量和第一平移向量估计所述机器人位姿。
图2所述机器人的位姿估计装置，与图1所述的机器人的位姿估计方法对应。
图3是本申请一实施例提供的机器人的示意图。如图3所示，该实施例的机器人3包括：处理器30、存储器31以及存储在所述存储器31中并可在所述处理器30上运行的计算机程序32，例如机器人的位姿估计程序。所述处理器30执行所述计算机程序32时实现上述各个机器人的位姿估计方法实施例中的步骤。或者，所述处理器30执行所述计算机程序32时实现上述各装置实施例中各模块/单元的功能。
示例性的，所述计算机程序32可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器31中，并由所述处理器30执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序32在所述机器人3中的执行过程。例如，所述计算机程序32可以被分割成：
图像获取单元，用于通过双目相机相邻两帧图像，提取所述相邻两帧图像中当前帧图像的左、右相机图像与所述相邻两帧图像中前一帧图像中的特征点；
特征点匹配单元，用于将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点；
旋转向量获取单元，用于对所述第一特征点采用随机采样一致性算法计算得到第一旋转向量；
平移向量获取单元，用于重建所述第一特征点对应的三维坐标，根据重建的三维坐标与所述第一特征点进行运动估计，得到第一平移向量；
位姿估计单元，用于根据所述第一旋转向量和第一平移向量估计所述机器人位姿。
所述机器人可包括，但不仅限于，处理器30、存储器31。本领域技术人员可以理解，图3仅仅是机器人3的示例，并不构成对机器人3的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述机器人还可以包括输入输出设备、网络接入设备、总线等。
所称处理器30可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器31可以是所述机器人3的内部存储单元，例如机器人3的硬盘或内存。所述存储器31也可以是所述机器人3的外部存储设备，例如所述机器人3上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器31还可以既包括所述机器人3的内部存储单元也包括外部存储设备。所述存储器31用于存储所述计算机程序以及所述机器人所需的其他程序和数据。所述存储器31还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括是电载波信号和电信信号。
以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。
Claims (10)
1.一种机器人的位姿估计方法，其特征在于，所述机器人的位姿估计方法包括：
通过双目相机相邻两帧图像，提取所述相邻两帧图像中当前帧图像的左、右相机图像与所述相邻两帧图像中前一帧图像中的特征点；
将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点；
对所述第一特征点采用随机采样一致性算法计算得到第一旋转向量；
重建所述第一特征点对应的三维坐标，根据重建的三维坐标与所述第一特征点进行运动估计，得到第一平移向量；
根据所述第一旋转向量和第一平移向量估计所述机器人位姿。
2.根据权利要求1所述的机器人的位姿估计方法，其特征在于，所述将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点的步骤包括：
将当前帧左相机图像的特征点与当前帧右相机图像的特征点进行匹配，得到第二特征点；
将当前帧左相机图像的特征点与前一帧右相机图像的特征点进行匹配，得到第三特征点；
将当前帧左相机图像的特征点与前一帧左相机图像的特征点进行匹配，得到第四特征点；
将前一帧左相机图像的特征点与前一帧右相机图像的特征点进行匹配，得到第五特征点；
根据第二特征点、第三特征点、第四特征点和第五特征点，生成第一特征点。
3.根据权利要求2所述的机器人的位姿估计方法，其特征在于，在将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点的步骤之后，所述方法还包括：
根据所述第一特征点的位置，将所述第一特征点更新至与图像对应的预设网格中的方格中；
根据方格中的特征点的数量、特征点的生成时间、特征点所对应的描述子的健壮性，更新所述方格中的特征点，根据方格中的特征点对所述第一特征点进行更新。
4.根据权利要求3所述的机器人位姿估计方法，其特征在于，所述根据方格中的特征点的数量、特征点的生成时间、特征点所对应的描述子的健壮性，更新所述方格中的特征点的步骤包括：
如果所述方格中的特征点的生成时间大于预设时长，则删除该特征点；
如果方格中特征点的数量大于预定数量且生成时间相同，则根据特征点的描述子的健壮性删除超出预定数量的特征点。
5.根据权利要求1所述的机器人位姿估计方法，其特征在于，所述对所述第一特征点采用随机采样一致性算法计算得到第一旋转向量的步骤包括：
根据五点法和随机一致性算法计算所述第一特征点所对应的本质矩阵；
根据八点法和随机一致性算法计算所述第一特征点所对应的单应矩阵；
分解所述本质矩阵和所述单应矩阵，并选择重投影误差和旋转误差较小的旋转向量作为第一旋转向量。
6.根据权利要求5所述的机器人的位姿估计方法，其特征在于，所述方法还包括：
通过线性插值的方式，对所述第一旋转向量进行平滑运动估计。
7.根据权利要求1所述的机器人的位姿估计方法，其特征在于，所述重建所述第一特征点对应的三维坐标，根据重建的三维坐标与所述第一特征点进行运动估计，得到第一平移向量的步骤包括：
通过非线性优化方法，最小化损失函数
的方式获取所述第一平移向量，其中，n为匹配的特征点数量，pi为当前帧特征点在世界坐标系下的三维坐标，pi’为上一帧特征点在世界坐标系下的三维世界坐标系，ξ为位姿的李代数表达向量，ξ^为位姿的李代数表达式的反对称矩阵。
8.一种机器人的位姿估计装置，其特征在于，所述机器人的位姿估计装置包括：
图像获取单元，用于通过双目相机相邻两帧图像，提取所述相邻两帧图像中当前帧图像的左、右相机图像与所述相邻两帧图像中前一帧图像中的特征点；
特征点匹配单元，用于将所述当前帧图像的左、右相机图像，以及前一帧图像的左、右相机图像的特征点进行匹配，得到匹配的第一特征点；
旋转向量获取单元，用于对所述第一特征点采用随机采样一致性算法计算得到第一旋转向量；
平移向量获取单元，用于重建所述第一特征点对应的三维坐标，根据重建的三维坐标与所述第一特征点进行运动估计，得到第一平移向量；
位姿估计单元，用于根据所述第一旋转向量和第一平移向量估计所述机器人位姿。
9.一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述机器人的位姿估计方法的步骤。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述机器人的位姿估计方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911418099.6A CN111160298B (zh) | 2019-12-31 | 2019-12-31 | 一种机器人及其位姿估计方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911418099.6A CN111160298B (zh) | 2019-12-31 | 2019-12-31 | 一种机器人及其位姿估计方法和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111160298A true CN111160298A (zh) | 2020-05-15 |
| CN111160298B CN111160298B (zh) | 2023-12-01 |
Family
ID=70560406
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911418099.6A Active CN111160298B (zh) | 2019-12-31 | 2019-12-31 | 一种机器人及其位姿估计方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111160298B (zh) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112950709A (zh) * | 2021-02-21 | 2021-06-11 | 深圳市优必选科技股份有限公司 | 一种位姿预测方法、位姿预测装置及机器人 |
| CN113253748A (zh) * | 2021-06-08 | 2021-08-13 | 深圳市优必选科技股份有限公司 | 并联构型仿人机器人的状态估计方法、装置、设备及介质 |
| CN113379850A (zh) * | 2021-06-30 | 2021-09-10 | 深圳市银星智能科技股份有限公司 | 移动机器人控制方法、装置、移动机器人及存储介质 |
| CN113643365A (zh) * | 2021-07-07 | 2021-11-12 | 紫东信息科技(苏州)有限公司 | 一种相机位姿估计方法、装置、设备以及可读存储介质 |
| CN114170306A (zh) * | 2021-11-17 | 2022-03-11 | 埃洛克航空科技（北京）有限公司 | 图像的姿态估计方法、装置、终端及存储介质 |
| WO2022143626A1 (zh) * | 2020-12-31 | 2022-07-07 | 深圳市优必选科技股份有限公司 | 移动机器人控制方法、计算机实现的存储介质和移动机器人 |
| WO2023005457A1 (zh) * | 2021-07-29 | 2023-02-02 | Oppo广东移动通信有限公司 | 位姿计算方法和装置、电子设备、可读存储介质 |
| CN115937002A (zh) * | 2022-09-09 | 2023-04-07 | 北京字跳网络技术有限公司 | 用于估算视频旋转的方法、装置、电子设备和存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106384383A (zh) * | 2016-09-08 | 2017-02-08 | 哈尔滨工程大学 | 一种基于fast和freak特征匹配算法的rgb‑d和slam场景重建方法 |
| CN107423772A (zh) * | 2017-08-08 | 2017-12-01 | 南京理工大学 | 一种新的基于ransac的双目图像特征匹配方法 |
| CN109141396A (zh) * | 2018-07-16 | 2019-01-04 | 南京航空航天大学 | 辅助信息与随机抽样一致算法融合的无人机位姿估计方法 |
| CN109741372A (zh) * | 2019-01-10 | 2019-05-10 | 哈尔滨工业大学 | 一种基于双目视觉的里程计运动估计方法 |
| CN109978042A (zh) * | 2019-03-19 | 2019-07-05 | 厦门理工学院 | 一种融合特征学习的自适应快速K-means聚类方法 |
-
2019
- 2019-12-31 CN CN201911418099.6A patent/CN111160298B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106384383A (zh) * | 2016-09-08 | 2017-02-08 | 哈尔滨工程大学 | 一种基于fast和freak特征匹配算法的rgb‑d和slam场景重建方法 |
| CN107423772A (zh) * | 2017-08-08 | 2017-12-01 | 南京理工大学 | 一种新的基于ransac的双目图像特征匹配方法 |
| CN109141396A (zh) * | 2018-07-16 | 2019-01-04 | 南京航空航天大学 | 辅助信息与随机抽样一致算法融合的无人机位姿估计方法 |
| CN109741372A (zh) * | 2019-01-10 | 2019-05-10 | 哈尔滨工业大学 | 一种基于双目视觉的里程计运动估计方法 |
| CN109978042A (zh) * | 2019-03-19 | 2019-07-05 | 厦门理工学院 | 一种融合特征学习的自适应快速K-means聚类方法 |
Non-Patent Citations (3)
| Title |
|---|
| 刘世蔷;黄影平;胡兴;慈文彦;: "基于自适应外点剔除与解耦算法的视觉里程计", 信息与控制, no. 02 * |
| 徐国庆;: "在线模板的人脸特征点对齐" * |
| 徐国庆;: "在线模板的人脸特征点对齐", 计算机工程与设计, no. 11 * |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2022143626A1 (zh) * | 2020-12-31 | 2022-07-07 | 深圳市优必选科技股份有限公司 | 移动机器人控制方法、计算机实现的存储介质和移动机器人 |
| CN112950709B (zh) * | 2021-02-21 | 2023-10-24 | 深圳市优必选科技股份有限公司 | 一种位姿预测方法、位姿预测装置及机器人 |
| CN112950709A (zh) * | 2021-02-21 | 2021-06-11 | 深圳市优必选科技股份有限公司 | 一种位姿预测方法、位姿预测装置及机器人 |
| CN113253748A (zh) * | 2021-06-08 | 2021-08-13 | 深圳市优必选科技股份有限公司 | 并联构型仿人机器人的状态估计方法、装置、设备及介质 |
| CN113253748B (zh) * | 2021-06-08 | 2022-02-22 | 深圳市优必选科技股份有限公司 | 并联构型仿人机器人的状态估计方法、装置、设备及介质 |
| CN113379850A (zh) * | 2021-06-30 | 2021-09-10 | 深圳市银星智能科技股份有限公司 | 移动机器人控制方法、装置、移动机器人及存储介质 |
| CN113379850B (zh) * | 2021-06-30 | 2024-01-30 | 深圳银星智能集团股份有限公司 | 移动机器人控制方法、装置、移动机器人及存储介质 |
| CN113643365A (zh) * | 2021-07-07 | 2021-11-12 | 紫东信息科技(苏州)有限公司 | 一种相机位姿估计方法、装置、设备以及可读存储介质 |
| CN113643365B (zh) * | 2021-07-07 | 2024-03-19 | 紫东信息科技(苏州)有限公司 | 一种相机位姿估计方法、装置、设备以及可读存储介质 |
| WO2023005457A1 (zh) * | 2021-07-29 | 2023-02-02 | Oppo广东移动通信有限公司 | 位姿计算方法和装置、电子设备、可读存储介质 |
| CN114170306A (zh) * | 2021-11-17 | 2022-03-11 | 埃洛克航空科技（北京）有限公司 | 图像的姿态估计方法、装置、终端及存储介质 |
| CN115937002A (zh) * | 2022-09-09 | 2023-04-07 | 北京字跳网络技术有限公司 | 用于估算视频旋转的方法、装置、电子设备和存储介质 |
| CN115937002B (zh) * | 2022-09-09 | 2023-10-20 | 北京字跳网络技术有限公司 | 用于估算视频旋转的方法、装置、电子设备和存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111160298B (zh) | 2023-12-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111160298B (zh) | 一种机器人及其位姿估计方法和装置 | |
| CN110363858B (zh) | 一种三维人脸重建方法及系统 | |
| CN110221690B (zh) | 基于ar场景的手势交互方法及装置、存储介质、通信终端 | |
| CN107292949B (zh) | 场景的三维重建方法、装置及终端设备 | |
| CN111598993B (zh) | 基于多视角成像技术的三维数据重建方法、装置 | |
| CN110108258B (zh) | 一种单目视觉里程计定位方法 | |
| CN111145238A (zh) | 单目内窥镜图像的三维重建方法、装置及终端设备 | |
| CN111311632B (zh) | 一种物体位姿跟踪方法、装置及设备 | |
| CN109241844B (zh) | 三维物体的姿态估计方法、装置、设备及存储介质 | |
| CN107862733B (zh) | 基于视线更新算法的大规模场景实时三维重建方法和系统 | |
| CN110378250B (zh) | 用于场景认知的神经网络的训练方法、装置及终端设备 | |
| CN110599586A (zh) | 一种半稠密场景重建方法、装置、电子设备及存储介质 | |
| CN104318552B (zh) | 基于凸包投影图匹配的模型配准方法 | |
| CN112435193A (zh) | 一种点云数据去噪的方法、装置、存储介质和电子设备 | |
| CN115205383A (zh) | 相机位姿的确定方法、装置、电子设备及存储介质 | |
| CN110930444B (zh) | 一种基于双边优化的点云匹配方法、介质、终端和装置 | |
| CN113610918A (zh) | 位姿计算方法和装置、电子设备、可读存储介质 | |
| CN116433843A (zh) | 基于双目视觉重建路线的三维模型重建方法及装置 | |
| KR102315696B1 (ko) | 관절 모델 정합 장치 및 방법 | |
| CN113298870B (zh) | 一种物体的姿态跟踪方法、装置、终端设备和存储介质 | |
| CN115937002B (zh) | 用于估算视频旋转的方法、装置、电子设备和存储介质 | |
| CN115170418B (zh) | 符合退化的低秩高维图像填充模型及其填充方法与系统 | |
| CN111179408B (zh) | 三维建模的方法及设备 | |
| CN106651950B (zh) | 一种基于二次曲线透视投影不变性的单相机位姿估计方法 | |
| CN113781583B (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 |