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

摘要

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

主权项

一种基于四面体边长和体积加权约束的单目视觉位姿测量方法，其特征在于该方法包括如下步骤：步骤(1)、对目标成像，并获取目标图像；首先设置目标由4个不共面特征点构成四面体，已知4个目标特征点在目标坐标系中3维坐标集{W₁,W₂,W₃,W₄}，获取四面体边长参数L，体积参数V，$L_{\mathrm{ij}}=\left|\overrightarrow{W_i{W}_j}\right|$$V=(\overrightarrow{W_i{W}_j}\times \overrightarrow{W_i{W}_g})·\overrightarrow{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)，则：${\overrightarrow{u}}_i=\frac{\overrightarrow{p(a_i,f)}}{\left|\overrightarrow{p(a_i,f)}\right|}$$A_i=l_i\overrightarrow{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({\overrightarrow{u}}_i·{\overrightarrow{u}}_j)-L_{\mathrm{ij}}$$e_V=[(l_j\overrightarrow{u_j}-l_i\overrightarrow{u_i})\times (l_g\overrightarrow{u_g}-l_i\overrightarrow{u_i})]·(l_h\overrightarrow{u_h}-l_i\overrightarrow{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{{\partial E}_{\mathrm{LV}}}{\partial L}$J_lx＝E_LVx＝(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)。