一种基于Tag的六自由度姿态估计方法技术

本发明专利技术公开了一种基于Tag的六自由度姿态估计方法，通过在物体上添加Tag来辅助检测，通过相机识别出物体上的Tag，帮助SLAM完成初始化，在初始化后，持续对每帧图像提取特征点，并根据上一帧对应的速度矩阵是否为空，进行相机位姿估计，以相机位姿估计得到的值为初值，采用特征点对应的地图点重投影到图像坐标系的重投影误差函数作为目标函数，进行相机位姿优化，得到优化好的相机位姿及特征点对应的地图点，再把相机的位姿转换成物体的位姿。本发明专利技术的方法在成像质量差以及物体高速运动时有较好的鲁棒性，并且有很高姿态估计精度。

【技术实现步骤摘要】
一种基于Tag的六自由度姿态估计方法
本专利技术属于计算机视觉
，尤其涉及一种基于Tag的六自由度姿态估计方法。
技术介绍
三维物体的姿态直观地反映了三维物体的特征，所以三维姿态作为三维物体的重要特征之一，一直以来都是国内外研究人员的研究重点。如果可以对空间目标进行三维姿态的判断，就可以粗略判断出空间目标的目的，对空间目标做很好的分类。姿态估计问题就是确定某一三维目标物体的方位指向问题。姿态估计在机器人视觉、动作跟踪和单照相机定标等很多领域都有应用。随着计算机视觉技术的广泛应用，在固定相机的情况下，拍摄场景中自由运动的物体，并对物体姿态做出估计，已经成为一个重要的研究方向。目前现有技术在姿态估计问题上有多种解决方案，例如基于模型来做检测或基于红外线来做检测。目前，基于模型来做检测在检测前需要对被检测的物体进行建模，然后考虑如何将目标模型与实际图像中的物体进行匹配，在匹配时是根据模型点集到图像边缘点集的平均距离作为匹配的度量，再求解出图像中目标的位姿。基于模型的姿态估计方法是通过比对真实图像和合成图像，进行相似度计算更新物体姿态。目前基于模型的方法为了避免在全局状态空间中进行优化搜索，一般都将优化问题先降解成多个局部特征的匹配问题，非常依赖于局部特征的准确检测。当噪声较大无法提取准确的局部特征的时候，该方法的鲁棒性受到很大影响。而基于红外线来做检测时，红外容易受环境的温度影响，该方法的鲁棒性也会受到很大影响
技术实现思路
本专利技术的目的是提供一种基于Tag的六自由度姿态估计方法，通过在物体上添加Tag来辅助检测，通过相机识别出物体上的Tag，帮助SLAM完成初始化，通过检测图像中的ORB特征点完成相机的位姿估计，再把相机的位姿转换成物体的位姿，在成像质量差以及物体高速运动时有较好的鲁棒性，并且有很高的姿态估计精度。为了实现上述目的，本专利技术技术方案如下：一种基于Tag的六自由度姿态估计方法，所述基于Tag的六自由度姿态估计方法，包括：接收输入的图像流，检测图像中运动物体上贴的二维码标签，完成初始化，以初始化完成时的图像帧为初始关键帧；在初始化后，持续对每帧图像提取特征点，并根据上一帧对应的速度矩阵是否为空，进行相机位姿估计；以相机位姿估计得到的值为初值，采用特征点对应的地图点重投影到图像坐标系的重投影误差函数作为目标函数，进行相机位姿优化，得到优化好的相机位姿及特征点对应的地图点；比较当前帧与当前关键帧共同的地图点，在当前帧与当前关键帧共同的地图点不超过设定的阈值时，将当前帧作为新的当前关键帧；在得到优化好的相机位姿后，再根据当前帧到当前帧关键帧的相机位姿、当前关键帧与初始化关键帧的相机位姿来求出当前帧的相机位姿，将相机位姿转换为运动物体的位姿。进一步地，所述基于Tag的六自由度姿态估计方法，还包括：当生成新的当前关键帧时，还依次检测所有关键帧，如果检测到有关键帧与至少三个其它关键帧之间，共同的地图点超过设定的阈值，就剔除这个关键帧。进一步地，所述进行相机位姿估计，包括：如果上一帧对应的速度矩阵为空，则将当前帧的相机位姿赋值成当前关键帧的相机位姿；如果上一帧对应的速度矩阵不为空，则当前帧的相机位姿为上一帧对应的速度矩阵乘以上上一帧的相机位姿；所述上一帧对应的速度矩阵为上一帧的相机位姿乘以上上一帧的相机位姿。进一步地，所述目标函数为：其中，其中，η表示当前帧与上一帧所有特征匹配点的集合，R，t的初值为估计得到的相机位姿，即相机的旋转矩阵和平移向量，ρ表示Huber能量函数，Σ表示优化帧对应关键帧的协方差矩阵，xi表示第i个特征点的图像坐标，Xi表示第i个特征点的地图点坐标，EcPro表示相机估计出来的位姿变换求出的三维点重投影回图像后与图像中特征点的重投影误差，EtagPro表示相机估计出来的位姿变换与获取的Tag位姿变换分别重投到图像上的重投影误差，Rtag和ttag分别表示获取的Tag旋转矩阵和平移向量，π用如下定义表示：其中，(fx,fy)表示相机焦距，(cx,cy)表示相机光圈，(X,Y,Z)表示空间点的世界坐标系坐标。ρ(a)为Huber能量函数：a表示误差值，δ为常数。本专利技术提出了一种基于Tag的六自由度姿态估计方法，通过在物体上添加Tag来辅助检测，通过相机识别出物体上的Tag，帮助SLAM完成初始化，通过检测图像中的ORB特征点完成相机的位姿估计，再把相机的位姿转换成物体的位姿，在成像质量差以及物体高速运动时有较好的鲁棒性，并且有很高姿态估计精度。附图说明图1为本专利技术一种基于Tag的六自由度姿态估计方法流程图；图2为二维码标签Tag的图形；图3为Tag标签坐标示意图；图4为相机坐标系示意图；图5为初始化时相机坐标系相对于标签坐标系示意图。具体实施方式下面结合附图和实施例对本专利技术技术方案做进一步详细说明，以下实施例不构成对本专利技术的限定。如图1所示，给出了一种基于Tag的六自由度姿态估计方法的实施例，在本实施例中，在待估计的运动物体上贴上二维码标签，该二维码标签携带有标签识别号的二维码信息，包括如下步骤：步骤S1、接收输入的图像流，检测图像中运动物体上贴的二维码标签，完成初始化，以初始化完成时的图像帧为初始关键帧。本实施例初始化过程，包括：检测图像中运动物体上贴的二维码标签的角点，建立世界坐标系；提取初始化时图像帧的特征点，计算特征点对应的地图点。本技术方案在固定相机的情况下，拍摄场景中运动物体，获取运动物体图像，输入图像流。本实施例运动物体上贴有二维码标签，检测运动物体上贴的二维码标签(Tag检测)，首先对拍摄的运动物体图像进行二值化，获取到灰度图，从而可以在灰度图中寻找候选标签，对寻找到的每个候选标签进行解码，以确定是否为有效的二维码标签。本专利技术的一种实施例，检测二维码标签的过程如下：在灰度图中，I(i，j)是图像在坐标(i，j)点像素的值，该点的梯度值为：G(i，j)＝Ix+Iy(1)其中Ix，Iy分别表示图像水平方向和垂直方向的梯度：Ix＝I(i+1，j)-I(i，j),Iy＝I(i，j+1)-I(i，j)(2)根据Harris算法来提取图片中方形的角点，首先计算图像像素点在水平和垂直方向上的梯度，以及两者的乘积，得到一个2×2的矩阵M：其中，Ix2＝Ix×Ix，Iy2＝Iy×Iy。然后，对图像进行高斯滤波，得到新的M。接下来利用M计算每个像素的角点响应函数Re：Re＝[Ix2×Iy2-(Ix，Iy)2]-k(Ix2+Iy2)2(4)其中，k为常系数，一般取值范围为0.04～0.06。然后寻找极大值点，若Re的响应值大于阈值，则视为角点。本实施例采用霍夫变换检测图像中的直线，以确定检测到的角点为Tag边缘的角点，从而可以得到四个角点的图像坐标。如图2所示，Tag为黑白相间二维码方块，不同的Tag包含了不同的数字编码信息，然后根据检测到的直线判断是否为方形区域。如果是方形区域则作为候选区域，将候选区域与Tag库匹配，判断这些方形区域是否为Tag。容易理解的是，在图像中检测二维码标签，在现有技术中已经是比较成熟的技术，这里不再赘述。如图3所示Tag本身带有一个标签坐标系，相机坐标系是以相机光心(相机光轴与图像平面的交点)为原点的坐标系(如图4所示)，根据外参，将相机上建立的相机坐标本文档来自技高网...

【技术保护点】
1.一种基于Tag的六自由度姿态估计方法，其特征在于，所述基于Tag的六自由度姿态估计方法，包括：接收输入的图像流，检测图像中运动物体上贴的二维码标签，完成初始化，以初始化完成时的图像帧为初始关键帧；在初始化后，持续对每帧图像提取特征点，并根据上一帧对应的速度矩阵是否为空，进行相机位姿估计；以相机位姿估计得到的值为初值，采用特征点对应的地图点重投影到图像坐标系的重投影误差函数作为目标函数，进行相机位姿优化，得到优化好的相机位姿及特征点对应的地图点；比较当前帧与当前关键帧共同的地图点，在当前帧与当前关键帧共同的地图点不超过设定的阈值时，将当前帧作为新的当前关键帧；在得到优化好的相机位姿后，再根据当前帧到当前帧关键帧的相机位姿、当前关键帧与初始化关键帧的相机位姿来求出当前帧的相机位姿，将相机位姿转换为运动物体的位姿。

【技术特征摘要】
1.一种基于Tag的六自由度姿态估计方法，其特征在于，所述基于Tag的六自由度姿态估计方法，包括：接收输入的图像流，检测图像中运动物体上贴的二维码标签，完成初始化，以初始化完成时的图像帧为初始关键帧；在初始化后，持续对每帧图像提取特征点，并根据上一帧对应的速度矩阵是否为空，进行相机位姿估计；以相机位姿估计得到的值为初值，采用特征点对应的地图点重投影到图像坐标系的重投影误差函数作为目标函数，进行相机位姿优化，得到优化好的相机位姿及特征点对应的地图点；比较当前帧与当前关键帧共同的地图点，在当前帧与当前关键帧共同的地图点不超过设定的阈值时，将当前帧作为新的当前关键帧；在得到优化好的相机位姿后，再根据当前帧到当前帧关键帧的相机位姿、当前关键帧与初始化关键帧的相机位姿来求出当前帧的相机位姿，将相机位姿转换为运动物体的位姿。2.如权利要求1所述的基于Tag的六自由度姿态估计方法，其特征在于，所述基于Tag的六自由度姿态估计方法，还包括：当生成新的当前关键帧时，还依次检测所有关键帧，如果检测到有关键帧与至少三个其它关键帧之间，共同的地图点超过设定的阈值，就剔除这个关键帧。3.如权利要求1所述的基于Tag的六自由度姿态估计方法，其特...

【专利技术属性】
技术研发人员：刘盛，柯程远，冯缘，王杨庆，沈康，陈胜勇，
申请(专利权)人：浙江工业大学，
类型：发明
国别省市：浙江,33