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

权利要求书

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

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

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

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

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

$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_{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形成的加权误差矩阵；E_LV为第k次迭代后由e_L和e_V形成的误差约束项，具体应为：加权系数α和β满足α+β＝1；

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

$J_l=\frac{\∂E_{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)。