[发明专利]一种基于三角形边长、内角、形状因子加权约束的单目视觉位姿测量方法

摘要

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

主权项

一种基于三角形边长、内角、形状因子加权约束的单目视觉位姿测量方法，其特征在于该方法包括如下步骤：步骤(1)、对目标成像，并获取目标图像；首先设置的目标为由3个目标特征点构成的三角形，已知3个目标特征点在目标坐标系中3维坐标集为{W1,W2,W3}，获取三角形边长参数L，内角参数Ang，形状因子F，其中i≠j≠g,且i,j,g∈{1,2,3}；步骤(2)、识别三角形目标特征点；将步骤(1)获取的图像进行识别并提取图像中目标特征点坐标{ai}，其中，i＝1、2、3；步骤(3)、建立三角形边长、内角和形状因子加权约束关系；已知摄像机等效焦距为f，定义3个目标特征点在摄像机坐标系下坐标集为{A1,A2,A3}，定义3个目标特征点到摄像机光心p的距离为{l1,l2,l3}；由步骤(2)获取了目标特征点在图像平面成像点坐标ai＝(ui,vi)，其中，i＝1、2、3，则成像点的3维坐标表示为(ai,f)＝(ui,vi,f)，则：Ai=liui→]]>应用目标特征点构造边长参数L、内角参数Ang、形状因子F作为对刚体目标的约束，同时也构造了对刚体目标位姿的约束；构成了基于边长参数L、内角参数Ang、形状因子F加权约束的误差项ELF和EAF；1个三角形有3条边，3个内角，其中每条边或每个内角与形状因子加权共形成6组约束加权误差约束项ELAF；ELF＝αeL+βeFEAF＝αeAng+βeFELAF=ELFEAF]]>其中i≠j≠g,且i,j,g∈{1,2,3},ELAF为第k次迭代后由eL、eAng、eF形成的加权误差矩阵，α和β为加权系数；步骤(4)、基于三角形边长、内角、形状因子约束关系解算位姿，首次迭代解算出{li}的修正量x：Jlx＝ELAFx＝(JlTJl)‑1Jl‑1ELAF其中Jl为雅可比(Jacobian)矩阵，在迭代中解算出{li}的修正量x，从而最终解算出准确的{li}；L(k+1)＝L(k)‑x其中L(k)是第k次迭代后的{li}，当所有的边长、内角、形状因子约束都能同时被满足，刚体形状满足约束，同时位姿解满足约束，误差约束项ELAF在迭代过程中将会逐渐收敛到零，且最终解算得到最优的{li}；另一方面，此非线性最优化问题被认为是局部线性的，反复迭代可以被解算得到{li}；在{li}得到的基础上，解算得到目标特征点在摄像机坐标系中坐标{Ai}，再由最小二乘平差方法，解算得到目标姿态角度θ＝(Ax,Ay,Az)与目标平移向量T＝(tx,ty,tz)。