CN112734843B - 一种基于正十二面体的单目6d位姿估计方法 - Google Patents

一种基于正十二面体的单目6d位姿估计方法 Download PDF

Info

Publication number
CN112734843B
CN112734843B CN202110022822.XA CN202110022822A CN112734843B CN 112734843 B CN112734843 B CN 112734843B CN 202110022822 A CN202110022822 A CN 202110022822A CN 112734843 B CN112734843 B CN 112734843B
Authority
CN
China
Prior art keywords
regular dodecahedron
aruco
current frame
code
dodecahedron
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
CN202110022822.XA
Other languages
English (en)
Other versions
CN112734843A (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.)
Hebei University of Technology
Original Assignee
Hebei University of Technology
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 Hebei University of Technology filed Critical Hebei University of Technology
Priority to CN202110022822.XA priority Critical patent/CN112734843B/zh
Publication of CN112734843A publication Critical patent/CN112734843A/zh
Application granted granted Critical
Publication of CN112734843B publication Critical patent/CN112734843B/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
    • 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
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30204—Marker
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种基于正十二面体的单目6D位姿估计方法。本方法将ArUco码与正十二面体相结合，再使用单目相机采集被ArUco码标记的正十二面体图像，计算ArUco码的中心点的空间三维坐标，获得其内切球的稀疏点云，进而利用多元非线性拟合以及迭代最近点算法比对相机坐标处的虚拟正十二面体，进而计算出当前正十二面体的空间位移量以及旋转角度，间接计算出被测物体的位姿。本方法克服了现有方法对光照、环境及设备的高要求，检测迅速，提升了稳定性；不需要在前期投入大量的精力进行模板采集或模型训练，显著降低了位姿估计成本，保证了位姿估计精度并加强通用性，并且能够进行物体大幅度运动的位姿估计。

Description

一种基于正十二面体的单目6D位姿估计方法
技术领域
本发明属于图像识别位姿检测领域，具体是一种基于正十二面体的单目6D位姿估计方法。
背景技术
目标物体的位姿估计是保证机器人在工作空间中实现随机操作的关键技术之一。目前工业自动化行业发展迅猛，对机器人的状态进行实时检测并反馈的要求也不断提高，而基于视觉的目标位姿估计对机器人性能的提高有重要意义。单目视觉***仅使用一台相机，结构简单成本低廉，应用比较广泛。目前目标的位姿估计的主要方法有：
使用特征点匹配方法估计位姿。具体做法是首先提取图像内的特征点，并匹配不同图像中的特征点，根据特征点的运动估计目标姿态变化。如文献《G.Lu,X.Wong andJ.McBride.From Mapping to Localization:A Complete Framework to VisuallyEstimate Position and Attitude for Autonomous Vehicles[J].2019 IEEEInternational Conference on Image Processing(ICIP),Taipei,China,2019pp.3103-3107,doi:10.1109/ICIP.2019.8803326.》中提出了一种利用特征点提取和匹配技术估计自动驾驶车位姿的方法。然而，特征点匹配技术本身具有局限性，在光照条件较差的情况下特征点跟踪将会受到较大影响。
使用基于深度学习的方法，近年来深度学习的应用非常广泛，通过训练深度神经网络并构建损失函数以达到对图像中的物体进行位姿识别的效果。文献《REDMON J,DIVVALA S,GIRSHICK R,et al.You only look once:Unified,real-time objectdetection[C]//Proceedings of the IEEE conference on computer vision andpattern recognition.2016:779-788.》中采用YOLO网络，通过图像中目标二维边框恢复其位姿，但是基于深度学习的方法需要大量的样本资源来构建大量的样本数据并进行真实位姿的标注，并需要大量计算资源进行网络的训练，难以控制成本，且大多为针对性的开发，无法满足通用性。
使用基于模板法，构建三维模板并寻找与实际位姿最接近的，用于估计当前目标位姿。如文献《AUBRY M,MATURANA D,EFROS A A,et al.Seeing 3d chairs:exemplarpart-based 2d-3d alignment using a large dataset of cad models[C].Proceedingsof the IEEE conference on computer vision and pattern recognition.2014:3762-3769.》中利用大量三维CAD模型将图像中的目标分类检测定位作为一种二维图像到三维模型的对齐问题。然而模板精度性取决与模板库大小，且一个模板适用于一种场景，缺乏通用性。
发明内容
针对现有技术的不足，本发明拟解决的技术问题是，提供一种基于正十二面体的单目6D位姿估计方法。
本发明解决所述技术问题的技术方案是，提供一种基于正十二面体的单目6D位姿估计方法，其特征在于，该方法包括以下步骤：
第一步、使用ArUco码对正十二面体进行标记，得到ArUco码标记的正十二面体；标定单目相机，再通过标定后的单目相机观测ArUco码标记的正十二面体；
第二步，分别对当前帧和初始帧中所有观测到的若干个ArUco码进行定位，再分别获得当前帧和初始帧中所有观测到的ArUco码的编码idi,i＝1,2,3,...,n和ArUco码中心点的空间坐标Ai(xi,yi,zi),i＝1,2,3,...,n，n表示某一帧观测到的ArUco码的数量；
第三步、对第二步得到的当前帧ArUco码中心点的空间坐标进行多元非线性拟合计算出当前帧正十二面体内切球球心坐标作为当前帧正十二面体体心的空间坐标t0，对初始帧ArUco码中心点的空间坐标进行多元非线性拟合计算出初始帧正十二面体内切球球心坐标作为初始帧正十二面体体心的空间坐标tO；再通过公式trel＝t0-tO求得当前帧正十二面体体心相对于初始帧的相对空间坐标trel；
在相机坐标系原点处构建一个虚拟正十二面体，虚拟正十二面体体心与相机坐标系原点重合，再使用ArUco码对虚拟正十二面体进行标记并得到虚拟正十二面体所有ArUco码的编码及其中心点的空间坐标AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,12；再将第二步得到的当前帧ArUco码中心点的空间坐标及其编码与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,12通过迭代最近点方法计算出当前帧正十二面体的旋转矩阵R，将初始帧ArUco码中心点的空间坐标及其编码与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,12通过迭代最近点方法计算出初始帧正十二面体的旋转矩阵RO，再通过公式
Figure BDA0002889184010000022
计算得到当前帧正十二面体相对于初始帧的相对旋转矩阵Rrel；
再根据相对旋转矩阵Rrel和相对空间坐标trel，得到当前帧正十二面体相对于初始帧正十二面体的位姿矩阵Trel：
Figure BDA0002889184010000021
式(2)中，SE(3)表示矩阵的属性；
第四步、首先构建被测物体坐标系，将正十二面体固定于被测物体上，并计算正十二面体相对于被测物体的相对空间坐标tref和相对旋转矩阵Rref，得到正十二面体相对于被测物体的位姿矩阵Tref：
Figure BDA0002889184010000031
则被测物体的相对位姿矩阵T：
Figure BDA0002889184010000032
得到当前帧被测物体相对于初始帧的空间姿态变化。
与现有技术相比，本发明有益效果在于：
(1)本方法创造性地将ArUco码与正十二面体相结合，将位姿识别从二维标签识别扩展为对三维空间体进行识别，大幅度提升了位姿识别的性能，再使用单目相机采集被ArUco码标记的正十二面体图像，利用PnP算法计算ArUco码的中心点的空间三维坐标，获得其内切球的稀疏点云，进而利用多元非线性拟合以及迭代最近点算法比对相机坐标处的虚拟正十二面体，将位置求解和旋转矩阵求解问题转化为优化问题，进而计算出当前正十二面体的空间位移量以及旋转角度，间接计算出被测物体的位姿。
(2)本方法克服了现有方法对光照、环境及设备的高要求，检测迅速，提升了稳定性，平均每帧图像识别时间约为10ms左右；不需要在前期投入大量的精力进行模板采集或模型训练，显著降低了位姿估计成本，保证了位姿估计精度并加强通用性；使用了三维物体作为标志，能够进行空间内物体大幅度运动的位姿估计。
(3)本发明采用的正十二面体是三维物体，能够实现空间任意角度以及大幅度运动的位姿识别。正十二面体相对于同体积的正二十面体每个面面积更大，每个面可以放下更大的ArUco码供计算机视觉检测，能够在更远的位置保证识别的有效性。
(4)相较于基于特征点的方法，本发明采用单目相机识别ArUco码的方法，克服了其对光照、环境等因素的敏感程度，可以在多数环境下稳定运行。
(5)相较于基于模板的方法，本发明使用正十二面体间接测量目标位姿，通用性较强，并且不需要构建大量的模板库，节省内存空间与工作量。
(6)相较于基于深度学习的方法，本发明算法不需要相对较强的硬件性能，节省成本，满足通用性。
附图说明
图1为本发明的ArUco码标记的正十二面体的示意图；
图2为本发明的正十二面体及其内切球的示意图；
图3为本发明的虚拟正十二面体与ArUco码标记的实际正十二面体的位置关系示意图。
具体实施方式
下面给出本发明的具体实施例。具体实施例仅用于进一步详细说明本发明，不限制本申请权利要求的保护范围。
本发明提供了一种基于正十二面体的单目6D位姿估计方法(简称方法)，其特征在于，该方法包括以下步骤：
第一步、使用ArUco码对正十二面体进行标记，得到ArUco码标记的正十二面体(如图1所示)；标定单目相机，再通过标定后的单目相机观测ArUco码标记的正十二面体；
优选地，第一步中，使用ArUco码对正十二面体进行标记的过程为：使用OpenCV图像处理库通过C++代码生成12个ArUco码，将12个ArUco码按编码顺序分别贴附于正十二面体的12个面上，且ArUco码的几何中心与其贴附的面的几何中点重合。
优选地，第一步中，单目相机的标定使用棋盘网格标准标定板，并使用OpenCV图像处理库进行相机内部参数标定，生成相机内参矩阵K；
所述相机内参矩阵K为将空间内坐标转化为平面坐标的矩阵，标准化公式为：
Figure BDA0002889184010000041
式(1)中，fx、fy为相机的焦距参数；cx、cy为像素的平移量。
第二步，使用图像处理算法分别对当前帧和初始帧中所有可观测到的若干个ArUco码进行定位，再使用图像处理算法分别获得当前帧和初始帧中所有观测到的ArUco码的编码idi,i＝1,2,3,...,n以及使用位姿计算方法获得当前帧和初始帧中所有观测到的ArUco码中心点的空间坐标Ai(xi,yi,zi),i＝1,2,3,...,n，n表示某一帧观测到的ArUco码的数量；
当前帧是正十二面体当前所在位置的图像，初始帧是单目相机拍摄的正十二面体初始位置的图像；
优选的，第二步中，ArUco码的定位过程是：对单目相机采集到的图像进行灰度处理、中值滤波及自适应阈值分割，从分割后的图像中提取ArUco码轮廓，并过滤非凸型和非正方形的图像，进而得到符合要求的候选区域。
优选的，第二步中，ArUco码的编码的获得：首先对候选区域应用透视变换获得规范正方形标记，使用Otsu阈值处理算法分离黑白色位，并根据标记大小分为不同的单元格；再根据每一个单元格上数量最多的像素所对应的颜色来确定该单元格的颜色，最后根据颜色转化为二进制数值以确定该标记的编码。
优选的，第二步中，ArUco码中心点的空间坐标的获得：根据候选区域的四个角点信息求出ArUco码中心点的空间坐标，使用OpenCV框架中的ArUco库函数的位姿计算函数，将四个角点信息作为输入，使用PnP方式(即2D-3D匹配算法)即可得到ArUco码中心点在相机坐标系下的空间坐标。
第三步、对第二步得到的当前帧所有ArUco码中心点的空间坐标进行多元非线性拟合计算出当前帧正十二面体内切球球心坐标作为当前帧正十二面体体心的空间坐标t0＝(x0,y0,z0)，对初始帧ArUco码中心点的空间坐标进行多元非线性拟合计算出初始帧正十二面体内切球球心坐标作为初始帧正十二面体体心的空间坐标tO＝(xO,yO,zO)；再用当前帧正十二面体体心的空间坐标t0＝(x0,y0,z0)减去初始帧正十二面体体心的空间坐标tO＝(xO,yO,zO)，通过公式trel(x,y,z)＝t0(x0,y0,z0)-tO(xO,yO,zO)求得当前帧正十二面体体心相对于初始帧的相对空间坐标trel；
在相机坐标系原点处构建一个虚拟状态的正十二面体，其与实际的正十二面体的位置关系如图3所示，虚拟正十二面体体心与相机坐标系原点Oc重合，再使用ArUco码对虚拟正十二面体进行标记并得到虚拟正十二面体所有ArUco码的编码及其中心点的空间坐标AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,12；再将第二步得到的当前帧ArUco码中心点的空间坐标及其编码Ai(xi,yi,zi,idi),i＝1,2,3,...,n与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,12通过迭代最近点方法计算出当前帧正十二面体的旋转矩阵R，将初始帧ArUco码中心点的空间坐标及其编码与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,12通过迭代最近点方法计算出初始帧正十二面体的旋转矩阵RO，再通过公式
Figure BDA0002889184010000052
计算得到当前帧正十二面体相对于初始帧的相对旋转矩阵Rrel；
再根据相对旋转矩阵Rrel和相对空间坐标trel，得到当前帧正十二面体相对于初始帧正十二面体的位姿矩阵Trel：
Figure BDA0002889184010000051
式(2)中，SE(3)表示李群，是一种数学符号表达，表示矩阵的属性；
优选地，第三步中，当前帧正十二面体体心的空间坐标的获得：由于ArUco码的几何中心与其贴附的面的几何中点重合，而正十二面体的内切球与正十二面体的交点正好位于正十二面体每个面的几何中心处(如图2所示)，所以能够将第二步得到的单目相机在当前帧观测到的正十二面体部分面上的每个ArUco码中心点的空间坐标Ai(xi,yi,zi),i＝1,2,3,...,n视为当前帧正十二面体内切球球面点(xi,yi,zi)的稀疏点云坐标；当单目相机获得了大于三个ArUco码中心点的空间坐标后即满足多元非线性拟合的数据条件(一般情况下，单目相机每次观测可以获得6-8个ArUco码中心点坐标，即每次观测均能够满足条件)，将当前帧正十二面体内切球球面点(xi,yi,zi)与内切球半径视为参数，则内切球的标准方程为：
(xi-x0)2+(yi-y0)2+(zi-z0)2-R内 2＝0 (3)
式(3)中，R内为正十二面体内切球半径，
Figure BDA0002889184010000061
a为正十二面体的棱长；
利用非线性最小二乘，对式(3)进行该球面多元非线性函数的球心拟合，构建损失函数如下：
Figure BDA0002889184010000062
由于多元非线性拟合对初始点要求较高，极容易陷入局部极小值中，因此利用已知球面点坐标(xi,yi,zi)对内切球球心坐标生成一组多元非线性拟合的初始值R初：
Figure BDA0002889184010000063
再将R初代入式(4)中获得损失函数J的一个值，随后不断更改R初的值使得式(4)中损失函数J的值不断减小，经过多次迭代求解后可以获得一组使得损失函数J最小的(x0,y0,z0)即为当前帧内切球球心坐标(即当前帧正十二面体体心在相机坐标系下的空间坐标t0＝(x0,y0,z0))。
优选地，第三步中，初始帧正十二面体体心的空间坐标的获得：将第二步得到的单目相机在初始帧观测到的正十二面体部分面上的每个ArUco码中心点的空间坐标视为初始帧正十二面体内切球球面点的稀疏点云坐标，与当前帧正十二面体体心的空间坐标的获得同理，即可得到初始帧正十二面体体心在相机坐标系下的空间坐标tO＝(xO,yO,zO)。
优选地，第三步中，当前帧正十二面体的旋转矩阵的获得：
搜索AWi中与第二步得到的当前帧实际正十二面体的ArUco码的编码对应的虚拟正十二面体的ArUco码的编码，将编码相同的进行匹配，进而获得空间中的一组匹配点：
(AWi,Ai),i＝1,2,3,...,n(6)
该组匹配点由实际正十二面体上观测到的ArUco码中心点的空间坐标与虚拟正十二面体上与之对应的ArUco码中心点的空间坐标组成，通过式(7)分别将其去质心化(即将实际正十二面体上观测到的ArUco码中心点的空间坐标与虚拟正十二面体上与之对应的ArUco码中心点的空间坐标的质心均平移至原点处)：
Figure BDA0002889184010000071
得到的qi为实际正十二面体ArUco码中心点的空间坐标的去质心点，qi'为虚拟正十二面体ArUco码中心点的空间坐标的去质心点，进而得到qi和qi'的关系如下：
Figure BDA0002889184010000072
定义误差项：
Figure BDA0002889184010000073
根据式(9)求取使得误差项E最小的当前帧正十二面体的旋转矩阵R，式(9)中第一项
Figure BDA0002889184010000074
与优化目标无关，第二项中RTR＝I亦与优化目标无关，则误差项变为：
Figure BDA0002889184010000075
式(10)中，tr表示矩阵的迹；
为求解式(10)中的优化目标R，定义矩阵：
Figure BDA0002889184010000076
对式(11)中的W进行奇异值分解，得到：
W＝UΣVT (12)
式(12)中，Σ为W的特征值矩阵，U与V为对角矩阵，则当前帧正十二面体的旋转矩阵R为：
R＝UVT (13)。
优选地，第三步中，初始帧正十二面体的旋转矩阵的获得：搜索AWi中与第二步得到的初始帧实际正十二面体的ArUco码的编码对应的虚拟正十二面体的ArUco码的编码，将编码相同的进行匹配，与当前帧正十二面体的旋转矩阵的获得同理，即可获得初始帧正十二面体的旋转矩阵RO。
第四步、根据正十二面体与被测物体的空间相对位置关系间接计算出被测物体的6D位姿。
优选地，第四步具体是：首先构建被测物体坐标系，将正十二面体固定于被测物体上，并计算正十二面体与被测物体之间的相对位姿关系(即计算正十二面体相对于被测物体的相对空间坐标tref和相对旋转矩阵Rref)，得到正十二面体相对于被测物体的位姿矩阵Tref：
Figure BDA0002889184010000081
则被测物体的相对位姿矩阵T：
Figure BDA0002889184010000082
即得到当前帧被测物体相对于初始帧的空间姿态变化。
所述相机标定方法、ArUco码的生成方法、图像处理算法、Otsu阈值处理算法、位姿计算方法是本领域公知方法。
本发明未述及之处适用于现有技术。

Claims (6)

1.一种基于正十二面体的单目6D位姿估计方法，其特征在于，该方法包括以下步骤：
第一步、使用ArUco码对正十二面体进行标记，得到ArUco码标记的正十二面体，且ArUco码的几何中心与其贴附的面的几何中心重合；标定单目相机，再通过标定后的单目相机观测ArUco码标记的正十二面体；
第二步，分别对当前帧和初始帧中所有观测到的若干个ArUco码进行定位，再分别获得当前帧和初始帧中所有观测到的ArUco码的编码idi,i＝1,2,3,...,n和ArUco码中心点的空间坐标Ai(xi,yi,zi),i＝1,2,3,...,n，n表示某一帧观测到的ArUco码的数量；
第三步、对第二步得到的当前帧ArUco码中心点的空间坐标进行多元非线性拟合计算出当前帧正十二面体内切球球心坐标作为当前帧正十二面体体心的空间坐标t0，对初始帧ArUco码中心点的空间坐标进行多元非线性拟合计算出初始帧正十二面体内切球球心坐标作为初始帧正十二面体体心的空间坐标tO；再通过公式trel＝t0-tO求得当前帧正十二面体体心相对于初始帧的相对空间坐标trel；
在相机坐标系原点处构建一个虚拟正十二面体，虚拟正十二面体体心与相机坐标系原点重合，再使用ArUco码对虚拟正十二面体进行标记并得到虚拟正十二面体所有ArUco码的编码及其中心点的空间坐标AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,12；再将第二步得到的当前帧ArUco码中心点的空间坐标及其编码与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,12通过迭代最近点方法计算出当前帧正十二面体的旋转矩阵R，将初始帧ArUco码中心点的空间坐标及其编码与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,12通过迭代最近点方法计算出初始帧正十二面体的旋转矩阵RO，再通过公式
Figure FDA0003982173450000011
计算得到当前帧正十二面体相对于初始帧的相对旋转矩阵Rrel；
再根据相对旋转矩阵Rrel和相对空间坐标trel，得到当前帧正十二面体相对于初始帧正十二面体的位姿矩阵Trel：
Figure FDA0003982173450000012
式(2)中，SE(3)表示矩阵的属性；
当前帧正十二面体体心的空间坐标的获得：将第二步得到的当前帧观测到的正十二面体部分面上的ArUco码中心点的空间坐标视为当前帧正十二面体内切球球面点(xi,yi,zi)的稀疏点云坐标；当单目相机获得了大于三个ArUco码中心点的空间坐标后，将当前帧正十二面体内切球球面点(xi,yi,zi)与内切球半径视为参数，则内切球的标准方程为：
(xi-x0)2+(yi-y0)2+(zi-z0)2-R内 2＝0 (3)
式(3)中，R内为正十二面体内切球半径，
Figure FDA0003982173450000021
a为正十二面体的棱长；
利用非线性最小二乘，对式(3)进行该球面多元非线性函数的球心拟合，构建损失函数如下：
Figure FDA0003982173450000022
由于多元非线性拟合对初始点要求较高，极容易陷入局部极小值中，因此利用已知球面点坐标(xi,yi,zi)对内切球球心坐标生成一组多元非线性拟合的初始值R初：
Figure FDA0003982173450000023
再将R初代入式(4)中获得损失函数J的一个值，随后不断更改R初的值使得式(4)中损失函数J的值不断减小，经过多次迭代求解后可以获得一组使得损失函数J最小的(x0,y0,z0)即为当前帧内切球球心坐标；
当前帧正十二面体的旋转矩阵的获得：
搜索AWi中与第二步得到的当前帧实际正十二面体的ArUco码的编码对应的虚拟正十二面体的ArUco码的编码，将编码相同的进行匹配，进而获得空间中的一组匹配点：
(AWi,Ai),i＝1,2,3,...,n (6)
该组匹配点由实际正十二面体上观测到的ArUco码中心点的空间坐标与虚拟正十二面体上与之对应的ArUco码中心点的空间坐标组成，通过式(7)分别将其去质心化：
Figure FDA0003982173450000024
得到的qi为实际正十二面体ArUco码中心点的空间坐标的去质心点，qi'为虚拟正十二面体ArUco码中心点的空间坐标的去质心点，进而得到qi和qi'的关系如下：
Figure FDA0003982173450000025
定义误差项：
Figure FDA0003982173450000026
根据式(9)求取使得误差项E最小的当前帧正十二面体的旋转矩阵R，式(9)中第一项
Figure FDA0003982173450000031
与优化目标无关，第二项中RTR＝I亦与优化目标无关，则误差项变为：
Figure FDA0003982173450000032
式(10)中，tr表示矩阵的迹；
为求解式(10)中的优化目标R，定义矩阵：
Figure FDA0003982173450000033
对式(11)中的W进行奇异值分解，得到：
W＝UΣVT (12)
式(12)中，Σ为W的特征值矩阵，U与V为对角矩阵，则当前帧正十二面体的旋转矩阵R为：
R＝UVT (13)；
第四步、首先构建被测物体坐标系，将正十二面体固定于被测物体上，并计算正十二面体相对于被测物体的相对空间坐标tref和相对旋转矩阵Rref，得到正十二面体相对于被测物体的位姿矩阵Tref：
Figure FDA0003982173450000034
则被测物体的相对位姿矩阵T：
Figure FDA0003982173450000035
得到当前帧被测物体相对于初始帧的空间姿态变化。
2.根据权利要求1所述的基于正十二面体的单目6D位姿估计方法，其特征在于，第一步中，使用ArUco码对正十二面体进行标记的过程为：使用OpenCV图像处理库生成12个ArUco码，将12个ArUco码按编码顺序分别贴附于正十二面体的12个面上。
3.根据权利要求1所述的基于正十二面体的单目6D位姿估计方法，其特征在于，第一步中，单目相机的标定使用棋盘网格标准标定板，并使用OpenCV图像处理库进行相机内部参数标定，生成相机内参矩阵K；
所述相机内参矩阵K为将空间内坐标转化为平面坐标的矩阵，标准化公式为：
Figure FDA0003982173450000036
式(1)中，fx、fy为相机的焦距参数；cx、cy为像素的平移量。
4.根据权利要求1所述的基于正十二面体的单目6D位姿估计方法，其特征在于，第二步中，ArUco码的定位过程是：对单目相机采集到的图像依次进行灰度处理、中值滤波及自适应阈值分割，从分割后的图像中提取ArUco码轮廓，并过滤非凸型和非正方形的图像，进而得到符合要求的候选区域。
5.根据权利要求4所述的基于正十二面体的单目6D位姿估计方法，其特征在于，第二步中，ArUco码的编码的获得：首先对候选区域应用透视变换获得规范正方形标记，使用Otsu阈值处理算法分离黑白色位，并根据标记大小分为不同的单元格；再根据每一个单元格上数量最多的像素所对应的颜色来确定该单元格的颜色，最后根据颜色转化为二进制数值以确定该标记的编码。
6.根据权利要求4所述的基于正十二面体的单目6D位姿估计方法，其特征在于，第二步中，ArUco码中心点的空间坐标的获得：根据候选区域的四个角点信息求出ArUco码中心点的空间坐标，使用OpenCV框架中的ArUco库函数的位姿计算函数，将四个角点信息作为输入，使用PnP方式即可得到ArUco码中心点在相机坐标系下的空间坐标。
CN202110022822.XA 2021-01-08 2021-01-08 一种基于正十二面体的单目6d位姿估计方法 Active CN112734843B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110022822.XA CN112734843B (zh) 2021-01-08 2021-01-08 一种基于正十二面体的单目6d位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110022822.XA CN112734843B (zh) 2021-01-08 2021-01-08 一种基于正十二面体的单目6d位姿估计方法

Publications (2)

Publication Number Publication Date
CN112734843A CN112734843A (zh) 2021-04-30
CN112734843B true CN112734843B (zh) 2023-03-21

Family

ID=75591304

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110022822.XA Active CN112734843B (zh) 2021-01-08 2021-01-08 一种基于正十二面体的单目6d位姿估计方法

Country Status (1)

Country Link
CN (1) CN112734843B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107291093A (zh) * 2017-07-04 2017-10-24 西北工业大学 基于视觉slam的复杂环境下无人机自主降落区域选择方法
CN108734737A (zh) * 2018-06-14 2018-11-02 哈尔滨工业大学 基于视觉slam估计空间旋转非合作目标转轴的方法
CN110962127A (zh) * 2019-12-10 2020-04-07 南京航空航天大学 一种用于机械臂末端位姿辅助标定装置及其标定方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018145291A1 (en) * 2017-02-10 2018-08-16 SZ DJI Technology Co., Ltd. System and method for real-time location tracking of drone
CN107292927B (zh) * 2017-06-13 2020-09-04 厦门大学 一种基于双目视觉的对称运动平台位姿测量方法
CN108168504B (zh) * 2017-12-26 2020-05-22 中国人民解放军战略支援部队信息工程大学 一种基于拟合球心法的天线旋转中心标校方法
CN109345588B (zh) * 2018-09-20 2021-10-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
CN111307146B (zh) * 2020-03-02 2023-07-18 北京航空航天大学青岛研究院 一种基于双目相机和imu的虚拟现实头戴显示设备定位***
CN111981982B (zh) * 2020-08-21 2021-07-06 北京航空航天大学 一种基于加权sfm算法的多向合作靶标光学测量方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107291093A (zh) * 2017-07-04 2017-10-24 西北工业大学 基于视觉slam的复杂环境下无人机自主降落区域选择方法
CN108734737A (zh) * 2018-06-14 2018-11-02 哈尔滨工业大学 基于视觉slam估计空间旋转非合作目标转轴的方法
CN110962127A (zh) * 2019-12-10 2020-04-07 南京航空航天大学 一种用于机械臂末端位姿辅助标定装置及其标定方法

Also Published As

Publication number Publication date
CN112734843A (zh) 2021-04-30

Similar Documents

Publication Publication Date Title
CN111775152B (zh) 基于三维测量引导机械臂抓取散乱堆叠工件的方法及***
CN107590836B (zh) 一种基于Kinect的充电桩动态识别与定位方法及***
CN109344882B (zh) 基于卷积神经网络的机器人控制目标位姿识别方法
CN108460779B (zh) 一种动态环境下的移动机器人图像视觉定位方法
CN112734844B (zh) 一种基于正八面体的单目6d位姿估计方法
CN107314771B (zh) 基于编码标志点的无人机定位以及姿态角测量方法
CN105021124B (zh) 一种基于深度图的平面零件三维位置和法向量计算方法
CN111476841B (zh) 一种基于点云和图像的识别定位方法及***
CN107977996B (zh) 基于靶标标定定位模型的空间目标定位方法
CN112907735B (zh) 一种基于点云的柔性电缆识别与三维重建方法
CN103727930A (zh) 一种基于边缘匹配的激光测距仪与相机相对位姿标定方法
CN112857215B (zh) 一种基于正二十面体的单目6d位姿估计方法
CN111640158A (zh) 基于对应掩模的端到端的摄像头与激光雷达外参标定方法
Zheng et al. Industrial part localization and grasping using a robotic arm guided by 2D monocular vision
CN113237434B (zh) 一种基于阶梯形标定物的激光轮廓传感器eye-in-hand标定方法
Ma et al. Binocular vision object positioning method for robots based on coarse-fine stereo matching
Sun et al. Robotic grasping using semantic segmentation and primitive geometric model based 3D pose estimation
CN113295171B (zh) 一种基于单目视觉的旋转刚体航天器姿态估计方法
Sun et al. Automatic targetless calibration for LiDAR and camera based on instance segmentation
CN112734843B (zh) 一种基于正十二面体的单目6d位姿估计方法
CN115760984A (zh) 一种立方星基于单目视觉的非合作目标位姿测量方法
CN116021519A (zh) 一种基于tof相机采摘机器人手眼标定方法以及装置
CN108694348B (zh) 一种基于自然特征的跟踪注册方法及装置
Wu et al. Object Pose Estimation with Point Cloud Data for Robot Grasping
CN109242910B (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