CN104880178A - 一种基于四面体边长和体积加权约束的单目视觉位姿测量方法 - Google Patents

Abstract

本发明公开了一种基于四面体边长和体积加权约束的单目视觉位姿测量方法，该方法的步骤包括：步骤(1)、获取四面体目标图像；步骤(2)、提取图像中四面体目标特征点；步骤(3)、建立四面体体积和边长特征加权约束关系；步骤(4)、基于四面体体积边长加权约束关系解算位姿。该方法运用图像识别手段，以四面体体积和边长特征建立加权误差约束关系，提高了目标单目视觉位姿测量精度。

Description

一种基于四面体边长和体积加权约束的单目视觉位姿测量方法

技术领域

本发明涉及一种单目视觉位姿测量方法，尤其涉及一种基于四面体边长和体积加权约束的单目视觉位姿测量方法。

背景技术

目标位姿测量在工业控制、航天操作等领域具有广泛的应用。其中基于视觉的目标位姿(位置、姿态)测量具有非接触、设备成本低、监视直观等优点。尤其在航天领域中，利用视觉对飞行器位姿进行那个位姿精确测量是多项空间任务的首要条件。

视觉位姿测量可分为合作目标测量和非合作目标测量，合作目标测量由于具有精度高、测量技术成熟、可靠等优势，在空间领域广泛采用。目前已采用过的合作标志器形态多种多样，有利用特征点、特征线、特征圆等多种几何特征的，通过对合作标志器的特征识别提取，从而利用提取的特征点坐标重投影约束、特征直线斜率约束、特征圆的半径约束等解算出目标位姿。

根据秦丽娟等人的《基于矩形的三维物体位姿估计研究》(参见《计算机工程与科学》，2009年31卷(4)49-51页)，利用了矩形对边平行等条件构造了位姿解析解算方法，但未涉及利用四面体边长和体积等几何特征。根据王晓剑等人的《基于双平行线特征的位姿估计解析算法》(参见《仪器仪表学报》，2008年29卷(3)600-604页)，利用了平行四边形的面积等特征构造位姿解析解算方法，但同样未涉及四面体边长和体积特征。专利CN201010563504.6中提出了一种基于特征线的运动目标位姿光学测量方法，方法中利用了两条相交直线上4个特征点不重合且不共线条件，解算目标位置姿态，未涉及利用四个点构成四面体的边长和体积特征。综上所述，上述方法均未涉及利用四面体边长和体积特征加权关系构造位姿迭代解算方法。

发明内容

本发明要解决的技术问题是：提供一种基于四面体边长和体积加权约束的单目视觉位姿测量方法。该方法利用4个不共面特征点构造了四面体，并利用四面体目标在位姿变化过程中其边长和体积保持不变的几何特征建立了加权误差约束关系，建立了对刚体形状约束，同时建立目标位姿进行约束，构造了迭代解算位姿方法。通过对四面体边长和体积的误差约束迭代解算目标位姿方法，有效利用了四面体目标刚体结构不变性特征，从而提高了位姿解算结果准确性。

本发明采用的技术方案如下：一种基于四面体边长和体积加权约束的单目视觉位姿测量方法，该方法包括如下步骤：

(1)对目标成像，并获取目标图像。首先设置目标由4个不共面特征点构成四面体，已知4个目标特征点在目标坐标系中3维坐标集{W₁,W₂,W₃,W₄}，获取四面体边长参数L，体积参数V，

$L_{\mathrm{ij}}=\left|\stackrel{\→}{W_i{W}_j}\right|$

$V=\frac{1}{6}\·(\stackrel{\→}{W_i{W}_j}\×\stackrel{\→}{W_i{W}_g})\·\stackrel{\→}{W_i{W}_h}$

其中i≠j≠g≠h,且i,j,g,h∈{1,2,3,4}；

(2)识别四面体目标特征点。将步骤(1)获取的图像进行识别并提取图像中目标特征点坐标{a_i}(i＝1…4)；

(3)建立四面体边长和体积加权约束关系；已知摄像机等效焦距为f，定义4个目标特征点在摄像机坐标系下坐标集为{A₁,A₂,A₃,A₄}，定义特征点到摄像机光心p的距离为{l₁,l₂,l₃,l₄}；由步骤(2)获取了目标特征点在图像平面成像点坐标a_i＝(u_i,v_i)(i＝1…3)，则成像点的3维坐标表示为(a_i,f)＝(u_i,v_i,f)，则：

$\stackrel{\→}{u_i}=\frac{\stackrel{\→}{p(a_i,f)}}{\left|\stackrel{\→}{p(a_i,f)}\right|}$

$A_i=l_i\stackrel{\→}{u_i}$

应用特征点构造边长L和体积V作为对刚体目标的约束。构成了基于边长L和体积V加权约束的误差项E_LV。1个四面体有6条边，则共形成6组边长L误差约束项，对应形成6组约束加权误差约束项E_LV。

$e_L={\left(l_i\right)}^2+{\left(l_j\right)}^2-2l_i{l}_j(\stackrel{\→}{u_i}\·\stackrel{\→}{u_j})-L_{\mathrm{ij}}$

$e_V=[(l_j\stackrel{\→}{u_j}-l_i\stackrel{\→}{u_i})\×(l_g\stackrel{\→}{u_g}-l_i\stackrel{\→}{u_i})]\·(l_h{\stackrel{\→}{u}}_h-l_i\stackrel{\→}{u_i})-V$

E_LV＝αe_L+βe_V

其中i≠j≠g≠h,且i,j,g,h∈{1,2,3,4}；E为第k次迭代后由e_L和e_V形成的加权误差矩阵；加权系数α和β满足α+β＝1；

(4)基于四面体边长和体积约束关系解算位姿。首次迭代解算出{l_i}的修正量x：

$J_l=\frac{{\∂E}_{\mathrm{LV}}}{\∂l}$

J_lx＝E_LV

x＝(J_l ^TJ_l)^-1J_l ^-1E_LV

其中J_l为雅可比(Jacobian)矩阵，在迭代中解算出{l_i}的修正量x，从而最终解算出准确的{l_i}。

L^(k+1)＝L^(k)-x

其中L^(k)是第k次迭代后的{l_i}。当所有的边长和体积约束都能同时被满足，误差E_LV在迭代过程中将会逐渐收敛到零，且最终解算得到最优的{l_i}。另一方面，此非线性最优化问题可以被认为是局部线性的，反复迭代可以被解算得到{l_i}。在{l_i}得到的基础上，解算得到目标特征点在摄像机坐标系中坐标{A_i}，再由最小二乘平差方法，解算得到目标姿态角度θ＝(A_x,A_y,A_z)与目标平移向量T＝(t_x,t_y,t_z)。

本发明与现有技术相比的优点在于：提供一种针对四面体目标的单目视觉位姿测量方法。该方法利用4个非共面特征点构造了四面体，并利用四面体目标在位姿变化过程中其边长和体积保持不变的几何特征建立了加权误差约束关系，在对四面体边长和体积误差约束转换为对目标位姿的约束，最终构造了迭代解算位姿方法。通过对四面体边长和体积误差约束迭代解算目标位姿方法，有效利用了四面体边长和体积不变性特征，从而提高了位姿解算结果准确性。

附图说明

图1为本发明具体流程图；

图2为仿真生成四面体目标模型；

图3为仿真生成四面体目标图像。

具体实施方式

下面结合具体实施例，对本发明方法进一步说明。

本发明具体流程如图1所示。(1)对目标成像，并获取目标图像。仿真生成四面体目标模型(如图2所示)，并获取目标图像(如图3所示)。首先设置目标由4个不共面圆形特征点构成四面体，已知4个目标特征点在目标坐标系中3维坐标集{W_i}(i＝1…n,n＝4)，单位为mm，

W₁＝(11.5,0,0)

W₂＝(-11.5,0,0)

W₃＝(0,11.5,0)

W₄＝(0,0,-11.5)

根据{W_i}获取四面体边长参数L，体积参数V，

L₁₂＝23mm

L₂₃＝16.3mm

L₃₁＝16.3mm

L₄₁＝16.3mm

L₄₂＝16.3mm

L₄₃＝16.3mm

V＝507mm³

(2)识别四面体目标特征点。将步骤(1)获取的图像进行识别并提取图像中目标特征点质心坐标{a_i}(i＝1…4)；

(3)建立四面体边长和体积加权约束关系。已知摄像机等效焦距为f＝3654.5pixel，定义4个目标特征点在摄像机坐标系下坐标集为{A₁,A₂,A₃,A₄}，定义特征点到摄像机光心p的距离为{l₁,l₂,l₃,l₄}。由步骤(2)获取了目标特征点在图像平面成像点质心坐标aⁱ＝(u_i,v_i)(i＝1…3)，则成像点的3维坐标表示为(a_i,f)＝(u_i,v_i,f)。则：

$\stackrel{\→}{u_i}=\frac{\stackrel{\→}{p(a_i,f)}}{\left|\stackrel{\→}{p(a_i,f)}\right|}$

$A_i=l_i\stackrel{\→}{u_i}$

应用特征点构造边长L和体积V作为对刚体目标的约束。构成了基于边长L和体积V加权约束的误差项E_l。1个四面体有6条边即确定误差项，对应形成6组加权误差约束项E_l。

$e_{L_{\mathrm{ij}}}=[{\left(l_i\right)}^2+{\left(l_j\right)}^2-2l_i{l}_j(\stackrel{\→}{u_i}\·\stackrel{\→}{u_j})]-L_{\mathrm{ij}}$

$e_V=\frac{1}{6}[(l_j\stackrel{\→}{u_j}-l_i\stackrel{\→}{u_i})\×(l_g\stackrel{\→}{u_g}-l_i\stackrel{\→}{u_i})]\·(l_h{\stackrel{\→}{u}}_h-l_i\stackrel{\→}{u_i})-V$

E_l＝αe_L+βe_V

其中E为第k次迭代后由e_L和e_V形成的加权误差矩阵，设置加权系数α＝0.4和β＝0.6，满足α+β＝1；

(4)基于四面体边长和体积约束关系解算位姿。首次迭代解算出{l_i}的修正量x：

$J_l=\frac{{\∂E}_l}{\∂L}$

J_lx＝E_l

x＝(J_l ^TJ_l)^-1J_l ^-1E_l

其中J_l为雅可比(Jacobian)矩阵，在迭代中解算出{l_i}的修正量x，从而最终解算出准确的{l_i}。

L^(k+1)＝L^(k)-x

其中L^(k)是第k次迭代后的{l_i}。由于边长和体积不变性，当所有的边长和体积约束都能同时被满足，误差E_l在迭代过程中将会逐渐收敛到零，且最终解算得到最优的{l_i}。另一方面，此非线性最优化问题可以被认为是局部线性的，反复迭代可以被解算得到{l_i}。在{l_i}得到的基础上，解算得到目标特征点在摄像机坐标系中坐标{A_i}，再由最小二乘平差方法，解算得到目标姿态角度θ＝(A_x,A_y,A_z)与目标平移向量T＝(t_x,t_y,t_z)，如表1所示，分别统计3组试验位姿解算结果及平均误差。

表1 仿真试验位姿解算结果及误差图表

由于充分利用了四面体边长和体积信息，并且将两种几何特征进行加权关联，建立起非线性最优化约束条件，使位姿解算结果更符合目标本身刚体特性，从而提高位姿解算准确性。

本发明未详述部分属于本技术领域的公知技术。以上所述仅为本发明的具体实例而已，并不用于以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种基于四面体边长和体积加权约束的单目视觉位姿测量方法，其特征在于该方法包括如下步骤：

步骤(1)、对目标成像，并获取目标图像；首先设置目标由4个不共面特征点构成四面体，已知4个目标特征点在目标坐标系中3维坐标集{W₁,W₂,W₃,W₄}，获取四面体边长参数L，体积参数V，

$L_{\mathrm{ij}}=\left|\stackrel{\→}{W_i{W}_j}\right|$

$V=(\stackrel{\→}{W_i{W}_j}\×\stackrel{\→}{W_i{W}_g})\·\stackrel{\→}{W_i{W}_h}$

其中i≠j≠g≠h,且i,j,g,h∈{1,2,3,4}；

步骤(2)、识别四面体目标特征点；将步骤(1)获取的图像进行识别并提取图像中目标特征点坐标{a_i}(i＝1…4)；

步骤(3)、建立四面体边长和体积加权约束关系；已知摄像机等效焦距为f，定义4个目标特征点在摄像机坐标系下坐标集为{A₁,A₂,A₃,A₄}，定义特征点到摄像机光心p的距离为{l₁,l₂,l₃,l₄}；由步骤(2)获取了目标特征点在图像平面成像点坐标a_i＝(u_i,v_i)(i＝1…3)，则成像点的3维坐标表示为(a_i,f)＝(u_i,v_i,f)，则：

${\stackrel{\→}{u}}_i=\frac{\stackrel{\→}{p(a_i,f)}}{\left|\stackrel{\→}{p(a_i,f)}\right|}$

$A_i=l_i\stackrel{\→}{u_i}$

应用特征点构造边长L和体积V作为对刚体目标的约束，构成了基于边长L和体积V加权约束的误差项E_LV，1个四面体有6条边，则共形成6组边长L误差约束项，对应形成6组约束加权误差约束项E_LV；

$e_L={\left(l_i\right)}^2+{\left(l_j\right)}^2-{2l}_i{l}_j({\stackrel{\→}{u}}_i\·{\stackrel{\→}{u}}_j)-L_{\mathrm{ij}}$

$e_V=[(l_j\stackrel{\→}{u_j}-l_i\stackrel{\→}{u_i})\×(l_g\stackrel{\→}{u_g}-l_i\stackrel{\→}{u_i})]\·(l_h\stackrel{\→}{u_h}-l_i\stackrel{\→}{u_i})-V$

E_LV＝αe_L+βe_V

其中i≠j≠g≠h,且i,j,g,h∈{1,2,3,4}；E为第k次迭代后由e_L和e_V形成的加权误差矩阵；

步骤(4)、基于四面体边长和体积约束关系解算位姿；首次迭代解算出{l_i}的修正量x：

$J_l=\frac{{\∂E}_{\mathrm{LV}}}{\∂L}$

J_lx＝E_LV

x＝(J_l ^TJ_l)^-1J_l ^-1E_LV

其中J_l为雅可比(Jacobian)矩阵，在迭代中解算出{l_i}的修正量x，从而最终解算出准确的{l_i}；

L^(k+1)＝L^(k)-x

其中L^(k)是第k次迭代后的{l_i}；当所有的边长和体积约束都能同时被满足，误差E_LV在迭代过程中将会逐渐收敛到零，且最终解算得到最优的{l_i}；另一方面，此非线性最优化问题可以被认为是局部线性的，反复迭代可以被解算得到{l_i}；在{l_i}得到的基础上，解算得到目标特征点在摄像机坐标系中坐标{A_i}，再由最小二乘平差方法，解算得到目标姿态角度θ＝(A_x,A_y,A_z)与目标平移向量T＝(t_x,t_y,t_z)。

2.根据权利要求1所述的一种基于四面体边长和体积加权约束的单目视觉位姿测量方法，其特征在于：所述步骤(3)中E_LV为第k次迭代后由e_L和e_V形成的加权误差矩阵，具体应为：加权系数α和β满足α+β＝1。