CN109345588A - 一种基于Tag的六自由度姿态估计方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于计算机视觉技术领域，尤其涉及一种基于Tag的六自由度姿态估计方法。

背景技术

三维物体的姿态直观地反映了三维物体的特征，所以三维姿态作为三维物体的重要特征之一，一直以来都是国内外研究人员的研究重点。如果可以对空间目标进行三维姿态的判断，就可以粗略判断出空间目标的目的，对空间目标做很好的分类。

姿态估计问题就是确定某一三维目标物体的方位指向问题。姿态估计在机器人视觉、动作跟踪和单照相机定标等很多领域都有应用。随着计算机视觉技术的广泛应用，在固定相机的情况下，拍摄场景中自由运动的物体，并对物体姿态做出估计，已经成为一个重要的研究方向。目前现有技术在姿态估计问题上有多种解决方案，例如基于模型来做检测或基于红外线来做检测。

目前，基于模型来做检测在检测前需要对被检测的物体进行建模，然后考虑如何将目标模型与实际图像中的物体进行匹配，在匹配时是根据模型点集到图像边缘点集的平均距离作为匹配的度量，再求解出图像中目标的位姿。基于模型的姿态估计方法是通过比对真实图像和合成图像，进行相似度计算更新物体姿态。目前基于模型的方法为了避免在全局状态空间中进行优化搜索，一般都将优化问题先降解成多个局部特征的匹配问题，非常依赖于局部特征的准确检测。当噪声较大无法提取准确的局部特征的时候，该方法的鲁棒性受到很大影响。而基于红外线来做检测时，红外容易受环境的温度影响，该方法的鲁棒性也会受到很大影响

发明内容

本发明的目的是提供一种基于Tag的六自由度姿态估计方法，通过在物体上添加Tag来辅助检测，通过相机识别出物体上的Tag，帮助SLAM完成初始化，通过检测图像中的ORB特征点完成相机的位姿估计，再把相机的位姿转换成物体的位姿，在成像质量差以及物体高速运动时有较好的鲁棒性，并且有很高的姿态估计精度。

为了实现上述目的，本发明技术方案如下：

一种基于Tag的六自由度姿态估计方法，所述基于Tag的六自由度姿态估计方法，包括：

接收输入的图像流，检测图像中运动物体上贴的二维码标签，完成初始化，以初始化完成时的图像帧为初始关键帧；

在初始化后，持续对每帧图像提取特征点，并根据上一帧对应的速度矩阵是否为空，进行相机位姿估计；

以相机位姿估计得到的值为初值，采用特征点对应的地图点重投影到图像坐标系的重投影误差函数作为目标函数，进行相机位姿优化，得到优化好的相机位姿及特征点对应的地图点；

比较当前帧与当前关键帧共同的地图点，在当前帧与当前关键帧共同的地图点不超过设定的阈值时，将当前帧作为新的当前关键帧；

在得到优化好的相机位姿后，再根据当前帧到当前帧关键帧的相机位姿、当前关键帧与初始化关键帧的相机位姿来求出当前帧的相机位姿，将相机位姿转换为运动物体的位姿。

进一步地，所述基于Tag的六自由度姿态估计方法，还包括：

当生成新的当前关键帧时，还依次检测所有关键帧，如果检测到有关键帧与至少三个其它关键帧之间，共同的地图点超过设定的阈值，就剔除这个关键帧。

进一步地，所述进行相机位姿估计，包括：

如果上一帧对应的速度矩阵为空，则将当前帧的相机位姿赋值成当前关键帧的相机位姿；

如果上一帧对应的速度矩阵不为空，则当前帧的相机位姿为上一帧对应的速度矩阵乘以上上一帧的相机位姿；

所述上一帧对应的速度矩阵为上一帧的相机位姿乘以上上一帧的相机位姿。

进一步地，所述目标函数为：

其中，

其中，η表示当前帧与上一帧所有特征匹配点的集合，R，t的初值为估计得到的相机位姿，即相机的旋转矩阵和平移向量，ρ表示Huber能量函数，Σ表示优化帧对应关键帧的协方差矩阵，xⁱ表示第i个特征点的图像坐标，Xⁱ表示第i个特征点的地图点坐标，E_cPro表示相机估计出来的位姿变换求出的三维点重投影回图像后与图像中特征点的重投影误差，E_tagPro表示相机估计出来的位姿变换与获取的Tag位姿变换分别重投到图像上的重投影误差，R_tag和t_tag分别表示获取的Tag旋转矩阵和平移向量，π用如下定义表示：

其中，(f_x,f_y)表示相机焦距，(c_x,c_y)表示相机光圈，(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)＝I_x+I_y (1)

其中I_x，I_y分别表示图像水平方向和垂直方向的梯度：

I_x＝I(i+1，j)-I(i，j),I_y＝I(i，j+1)-I(i，j) (2)

根据Harris算法来提取图片中方形的角点，首先计算图像像素点在水平和垂直方向上的梯度，以及两者的乘积，得到一个2×2的矩阵M：

其中，I_x ²＝I_x×I_x，I_y ²＝I_y×I_y。

然后，对图像进行高斯滤波，得到新的M。接下来利用M计算每个像素的角点响应函数Re：

Re＝[I_x ²×I_y ²-(I_x，I_y)²]-k(I_x ²+I_y ²)² (4)

其中，k为常系数，一般取值范围为0.04～0.06。然后寻找极大值点，若Re的响应值大于阈值，则视为角点。

本实施例采用霍夫变换检测图像中的直线，以确定检测到的角点为Tag边缘的角点，从而可以得到四个角点的图像坐标。如图2所示，Tag为黑白相间二维码方块，不同的Tag包含了不同的数字编码信息，然后根据检测到的直线判断是否为方形区域。如果是方形区域则作为候选区域，将候选区域与Tag库匹配，判断这些方形区域是否为Tag。容易理解的是，在图像中检测二维码标签，在现有技术中已经是比较成熟的技术，这里不再赘述。

如图3所示Tag本身带有一个标签坐标系，相机坐标系是以相机光心(相机光轴与图像平面的交点)为原点的坐标系(如图4所示)，根据外参，将相机上建立的相机坐标系通过旋转平移与Tag的标签坐标系重合，建立系统的唯一的世界坐标系。世界坐标系是系统的绝对坐标系，图像上所有点的坐标都是以该坐标系的原点来确定各自的位置的。

SLAM(Simultaneous Localization And Mapping)，即时定位与地图构建，在本发明中实现估计相机位姿的功能。需要两帧图像进行初始化，先测量出Tag的真实尺寸为2h×2h，以Tag的中心为标签坐标系原点，则Tag四个角的3D坐标则如图3所示，根据四个角点对应的2D图像坐标(在Tag检测的时候得到)，通过PnP算法分别求得两帧图像中Tag和相机之间的外参，即其之间的旋转矩阵和平移向量，从而可以进行旋转平移，建立唯一的世界坐标系。

本实施例对于输入的图像流中的每一帧，都需要提取特征点，并通过后续的步骤获取特征点对应的地图点，地图点为SLAM计算图像上二维特征点的深度形成该特征点的三维坐标点。

对于运动物体图片，本实施例还进行特征提取，目前提取特征点的方法有FAST、SURF、SIFT等算法，由于FAST算法提取特征点速度快，能满足系统实时性的要求，本实施例采用FAST算法来检测特征点。一般来说，基于特征点周围一圈的图像灰度值与该特征点的灰度值差别足够大，认为该候选点就是一个特征点：

其中，I(x)为圆周circle(p)上任意一点的灰度，I(p)为圆心的灰度，ε为灰度差的阈值，如果N大于给定阈值，则认为p是个特征点。

得到特征点后，用BRIEF算法来计算一个特征点的描述子。本实施例以特征点为中心，取S×S的领域窗口，在窗内随机选取一对点，比较两者像素值的大小，定义τ，进行如下二进制赋值：

其中，p(x)，p(y)分别是随机点x，y的像素值。重复公式(6)，取256对点，形成一个二进制编码，这个编码就是对特征点的描述。

对于初始化时的帧图像，如图5所示，通过第一帧与Tag之间的外参R₁和第二帧的与Tag之间的外参R₂，可求得第一帧与第二帧之间的外参，即两帧间的相机位姿R₁₂：

R₁₂＝R₁ ^-1×R₂ (5)

已知两帧特征点的二维坐标和两帧间的位姿，通过三角化来计算出特征点的深度值信息，即该特征点在世界坐标系下对应点的Z轴数据。

本发明通过Tag的真实尺寸，能解决单目SLAM的尺度问题(生成的地图在现实世界中真实的尺寸无法得知)，且在初始化时形成高精度的地图点。当系统长时间运行时，物体的平移距离的测量仍能保持高精度。

此外，本实施例还以初始化完成时的图像帧为初始关键帧。

步骤S2、在初始化后，持续对每帧图像提取特征点，并根据上一帧对应的速度矩阵是否为空，进行相机位姿估计。

初始化后，本实施例持续对图像流进行处理，对当前处理的图像帧称为当前帧，对当前帧进行特征点提取，关于特征点提取前面已经描述，这里不再赘述。

对于相机位姿的估计，本实施例中如果用每一张图来建立或更新地图，计算量太大，因此本实施例通过关键帧来计算。本实施例将初始化完成时的图像帧作为初始关键帧，并在获取每一帧图像特征点对应的地图点后，更新当前关键帧，用于后续位姿估计。

当前关键帧是指在处理当前帧时对应的关键帧，在初始化后，初始化完成时的图像帧为初始关键帧，本实施例对关键帧进行管理，具体如下：

本实施例对于每帧运动物体图像，在获取每一帧图像特征点对应的地图点后，比较当前帧与当前关键帧共同的地图点，在当前帧与当前关键帧共同的地图点不超过设定的阈值(例如90％)时，将当前帧作为新的当前关键帧。

此外，对于所有关键帧，当生成新的当前关键帧时，还依次检测所有关键帧，如果检测到有关键帧与至少三个其它关键帧之间，共同的地图点超过设定的阈值(例如有至少90％的地图点重复)，就剔除这个关键帧，以减少维护关键帧的计算量。

本实施例进行相机位姿估计，用估计的相机位姿作为后续优化的初值，本发明技术方案不限于具体的相机位姿估计方法，例如可以采用当前关键帧的相机位姿作为估计的相机位姿，并作为后续优化的初值。

本发明的一种实施例，进行相机位姿估计，包括：

如果上一帧对应的速度矩阵为空，则根据当前关键帧进行相机位姿估计，否则根据运动模型进行相机位姿估计。上一帧对应的速度矩阵为得到上一帧的相机位姿后，用上一帧的相机位姿乘以上上一帧的相机位姿。

本实施例在进行相机位姿估计时，如果上一帧的速度矩阵不为空，则根据运动模型进行相机位姿估计。运动模型假设的是相机运动速度不变，即在估计当前帧的相机位姿时为：当前帧的相机位姿等于上一帧对应的速度矩阵乘以上上一帧的相机位姿。

需要说明的是，上一帧的速度矩阵为得到上一帧的相机位姿后，用上一帧的相机位姿乘以上上一帧的相机位姿。本实施例在计算得到一帧的相机位姿后，还更新速度矩阵，以用于后续帧的相机位姿估计。

如果上一帧的速度矩阵为空，则根据关键帧进行相机位姿估计，即直接将当前帧的相机位姿赋值成当前关键帧的相机位姿，作为后续相机位姿优化的初值。

步骤S3、以相机位姿估计得到的值为初值，采用特征点对应的地图点重投影到图像坐标系的重投影误差函数作为目标函数，进行相机位姿优化，得到优化好的相机位姿及特征点对应的地图点。

本实施例对当前估计的相机位姿做光束平差法(Bundle Adjustment)，优化单帧的相机位姿以提高相机位姿的精度，得到准确当前帧的相机位姿。

本实施例首先通过特征点的描述子来做当前帧与其对应的关键帧图像特征点的匹配，已知匹配的2D特征点坐标(即图像坐标)和其形成的地图点即其3D坐标点(世界坐标系坐标)，把3D的坐标按照当前的相机位姿重新投影回2D的图像上，把投影回来的点与图像上原来对应的2D点做比较，优化当前帧的相机姿态，采用3D-2D的重投影误差构成优化的目标函数。

本方法引进的一个新的误差项E_tagPro，融合新数据，用重投影误差结合Tag的位姿联合优化相机位姿，求得使重投影误差最小的旋转矩阵R，平移向量t，以及地图点集X：

其中，η表示当前帧与上一帧所有特征匹配点的集合，R，t的初值为估计得到的相机位姿，即相机的旋转矩阵和平移向量，ρ表示Huber能量函数，Σ表示优化帧对应关键帧的协方差矩阵，xⁱ表示第i个特征点的图像坐标，Xⁱ表示第i个特征点的地图点坐标，E_cPro表示相机估计出来的位姿变换求出的三维点重投影回图像后与图像中特征点的重投影误差，E_tagPro表示相机估计出来的位姿变换与获取的Tag位姿变换分别重投到图像上的重投影误差，R_tag和t_tag分别表示获取的Tag旋转矩阵和平移向量，π用如下定义表示：

其中，(f_x,f_y)表示相机焦距，(c_x,c_y)表示相机光圈，(X,Y,Z)表示空间点的世界坐标系坐标。

ρ(a)为Huber能量函数：

a表示误差值，δ为常数，一般取值为1。

通过上述优化方式，得到优化好的相机位姿，优化好的相机位姿是重投影误差最小的相机位姿。

需要说明的是，上述优化方法中加入了新的误差项E_tagPro，提高了优化的精度。当不加入该新的误差项E_tagPro时，也可以进行优化，不过优化精度没有加入新的误差项高。对于不加入该新的误差项的具体优化过程，这里不再赘述。

SLAM用当前帧相对于世界坐标系原点的旋转矩阵和平移向量来表示当前相机位姿，也即当前帧相对于初始关键帧的外参，这里不再赘述。

本实施例对于初始化以后的特征点的世界坐标系坐标，根据上述优化得到的特征点的世界坐标系坐标，即对应的地图点。对深度值未知的特征点，在刚形成当前关键帧时，其深度赋初值为1，然后根据公式(8)的重投影误差，在后续帧中不断跟踪这些特征点，优化得到这些特征点的深度值。如果深度值收敛，则形成新的世界坐标系坐标，否则丢弃该特征点，不会形成世界坐标系坐标。

容易理解的是，在获取到当前帧的特征点对应的地图点后，则会比较当前帧与当前关键帧共同的地图点，更新当前关键帧，这里不再赘述。

步骤S4、在得到优化好的相机位姿后，再根据当前帧到当前帧关键帧的相机位姿、当前关键帧与初始化关键帧的相机位姿来求出当前帧的相机位姿，将相机位姿转换为运动物体的位姿。

在得到优化好的相机位姿后，再根据当前帧到当前帧关键帧的相机位姿、当前关键帧与初始化关键帧的相机位姿来求出当前帧的相机位姿，这样避免上一帧相机位姿估计误差对当前帧的相机位姿估计带来影响。

T_camera＝T_ref×T_camToRef (12)

其中T_camera为当前帧的相机位姿，T_ref为当前关键帧与初始化关键帧的相机位姿，T_camToRef为当前帧到当前帧关键帧的相机位姿。

需要说明的是，计算出当前帧的相机位姿后，可以用来计算速度矩阵，这里不再赘述。

计算出当前帧的相机位姿后，将相机位姿转换为物体的位姿：

T_object＝T_camera ^-1 (13)

其中T_object表示物体位姿。

在实际的应用中，运动物体可以是无人机，通过本技术方案的方法，将相机位姿转成无人机位姿，并输出当前无人机的三维点云图。

本发明提出了物体六自由度位姿估计方法，基于纯视觉，在物体发生快速运动的情况下，仍表现优异。通过实验，可以得到表1所示技术指标：

偏航角误差	俯仰角误差	翻转角误差	x轴方向误差	y轴方向误差	z轴方向误差
						0.1°	0.5°	0.5°	1mm	1mm	0.3mm

表1实验误差测量

本发明引入了Tag来消除系统的累计误差，通过Tag完成SLAM的一帧初始化，解决了单目SLAM会出现的尺度漂移问题。由于特征点的检测效率高，该方法可以实现最高50FPS的实时检测。

以上实施例仅用以说明本发明的技术方案而非对其进行限制，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (4)

1.一种基于Tag的六自由度姿态估计方法，其特征在于，所述基于Tag的六自由度姿态估计方法，包括：

接收输入的图像流，检测图像中运动物体上贴的二维码标签，完成初始化，以初始化完成时的图像帧为初始关键帧；

在初始化后，持续对每帧图像提取特征点，并根据上一帧对应的速度矩阵是否为空，进行相机位姿估计；

以相机位姿估计得到的值为初值，采用特征点对应的地图点重投影到图像坐标系的重投影误差函数作为目标函数，进行相机位姿优化，得到优化好的相机位姿及特征点对应的地图点；

比较当前帧与当前关键帧共同的地图点，在当前帧与当前关键帧共同的地图点不超过设定的阈值时，将当前帧作为新的当前关键帧；

在得到优化好的相机位姿后，再根据当前帧到当前帧关键帧的相机位姿、当前关键帧与初始化关键帧的相机位姿来求出当前帧的相机位姿，将相机位姿转换为运动物体的位姿。

2.如权利要求1所述的基于Tag的六自由度姿态估计方法，其特征在于，所述基于Tag的六自由度姿态估计方法，还包括：

当生成新的当前关键帧时，还依次检测所有关键帧，如果检测到有关键帧与至少三个其它关键帧之间，共同的地图点超过设定的阈值，就剔除这个关键帧。

3.如权利要求1所述的基于Tag的六自由度姿态估计方法，其特征在于，所述进行相机位姿估计，包括：

如果上一帧对应的速度矩阵为空，则将当前帧的相机位姿赋值成当前关键帧的相机位姿；

如果上一帧对应的速度矩阵不为空，则当前帧的相机位姿为上一帧对应的速度矩阵乘以上上一帧的相机位姿；

所述上一帧对应的速度矩阵为上一帧的相机位姿乘以上上一帧的相机位姿。

4.如权利要求1所述的基于Tag的六自由度姿态估计方法，其特征在于，所述目标函数为：

其中，

其中，η表示当前帧与上一帧所有特征匹配点的集合，X为地图点集，R，t的初值为估计得到的相机位姿，即相机的旋转矩阵和平移向量，ρ表示Huber能量函数，Σ表示优化帧对应关键帧的协方差矩阵，xⁱ表示第i个特征点的图像坐标，Xⁱ表示第i个特征点的地图点坐标，E_cPro表示相机估计出来的位姿变换求出的三维点重投影回图像后与图像中特征点的重投影误差，E_tagPro表示相机估计出来的位姿变换与获取的Tag位姿变换分别重投到图像上的重投影误差，R_tag和t_tag分别表示获取的Tag旋转矩阵和平移向量，π用如下定义表示：

其中，(f_x,f_y)表示相机焦距，(c_x,c_y)表示相机光圈，(X,Y,Z)表示空间点的世界坐标系坐标。

ρ(a)为Huber能量函数：

a表示误差值，δ为常数。