CN110009680B - 基于圆特征及异面特征点的单目图像位置、姿态测量方法 - Google Patents

Abstract

本发明涉及基于圆特征及异面特征点的单目图像位置、姿态测量方法，采用基于朝向变换的椭圆检测方法定位图像中目标上圆特征对应的成像椭圆，结合圆特征圆心坐标、法向及半径信息，求取出相对位置、姿态参数；引入异面点特征，利用异面点与圆特征之间的空间关系，消除基于单目图像圆特征位置、姿态参数求解的二义性；引入基于全局约束的搜索策略，对序列图像中圆特征成像椭圆的自动跟踪，实现对目标位置、姿态参数的连续测量。本发明利用目标圆特征及异面特征点，仅基于单目图像完成目标位置、姿态测量，算法步骤简单明了，计算复杂度低，易于应用。

Description

基于圆特征及异面特征点的单目图像位置、姿态测量方法

技术领域

本发明主要涉及计算机视觉、摄像测量、机械自动化领域，特指一种利用目标圆特征及异面特征点的单目图像目标位置、姿态测量方法。

技术背景

自动化操作在航天、工业机器人领域中得到越来越普遍的应用，其中设备与目标之间相对位置、姿态关系的测量是实现自动化操作的关键所在。随着计算机视觉、摄像测量技术的发展，基于视觉的相对位置、姿态测量方法受到了越来越多的重视，基于视觉的测量技术具有精度高、成本低等优势。

已有视觉相关测量方法中包含两个主要步骤：特征提取与位置、姿态参数解算。特征提取旨在建立二维-三维之间的对应集合，基于建立的对应集合，实现相对位置、姿态参数的解算。针对待测量目标的不同特点，不同的特征提取方法及位置、姿态解算方法被采用。

点特征被广泛应用于相对位置、姿态的测量中，基于点特征的位置、姿态测量即为n点透视问题(PnP)。常用的点特征提取方法有Harris、SITF、SURF等，通过点特征之间的匹配建立二维-三维之间对应。对于PnP问题的求解，当n≥6时，该问题存在唯一线性最小二乘解，PnP问题求解的最小配置对应数为n＝3，因此，已有研究主要集中于3≤n≤5的情况，正交迭代等优化方法被采用已改善解算结果的准确性。点特征依赖于目标的纹理、角点特征等，易受到光照、杂乱背景等因素的干扰。相比于点特征，直线特征对干扰具有较强的鲁棒性，类似于PnP问题，基于直线特征的位置、姿态测量问题记为n直线透视问题(PnL)，PnL问题的边界条件及求解方法类似于PnP问题，这里不再赘述。基于直线特征的方法仅能适用于具有丰富直线结构的目标，限制了方法的使用范围。一般轮廓特征被引入到位置、姿态参数的跟踪中，此类方法对目标结构没有过多要求，使用范围较广，但需要给定目标的初始位置、姿态参数，不易实现完全自主测量。

深度学习技术被应用于目标位置、姿态参数的测量，相关方法可归结于以下两个主要类别：(1)类似于传统方法，深度学习技术用于学习特征描述，建立二维-三维对应，进而采用传统位置、姿态参数解算方法求取相对关系参数；(2)充分发挥深度学习技术端到端的优势，直接基于图像信息，输出目标位置、姿态参数，无需显式地建立二维-三维对应。已有测试结果表明，第一类方法参数求解精度由于第二类方法，另外，深度学习技术依赖于数据驱动，需要大量标注的训练数据，限制了其应用。

发明内容

本专利针对具有圆特征的目标的相对位置、姿态测量问题，发明了一种基于圆特征及异面特征点的单目图像位置、姿态测量方法：通过基于朝向变换的椭圆检测方法实现图像中圆特征对应成像椭圆的检测，结合圆特征圆心、法向及半径信息，求解目标圆特征位置、姿态参数；采用模板匹配的方法定位图像中异面特征点，结合特征点与圆特征之间的空间关系，消除单目图像中圆特征位置、姿态参数求解的二义性，得到最终的位置、姿态测量结果；利用基于全局约束的搜索策略，对序列图像中圆特征对应的成像椭圆的自动跟踪，实现对目标位置、姿态参数的连续测量。

1、本发明的基于圆特征及异面特征点的单目图像位置、姿态测量方法的实施过程

本发明实施过程如附图2所示，具体如下：

(1)圆特征提取：针对首帧输入图像，采用基于朝向变换的椭圆提取方法检测图像中目标圆特征对应的成像椭圆，对于后续输入图像，采用基于全局约束的搜索策略，实现成像椭圆的连续跟踪；

(2)异面特征点提取：针对输入图像，基于模板匹配的方法实现图像中对角标志标识的异面特征点成像的提取；

(3)基于圆特征的位置、姿态参数求解：依据步骤(1)中提取的椭圆特征，结合圆特征圆心坐标、法向及半径信息，求取出相机与目标之间的相对位置、姿态关系；

(4)基于异面特征点的圆特征位置、姿态参数二义性消除：利用异面点与圆特征之间已知的空间关系，消除基于单目图像圆特征位置、姿态参数求解的二义性。

2、本发明的基于圆特征及异面特征点的单目图像位置、姿态测量方法

(1)圆特征对应成像椭圆的检测与跟踪

圆特征在图像中成像可为圆、椭圆及直线段三种情况，圆和直线段可视为椭圆的极端情况，因此，这里以椭圆为例进行阐述:

针对输入图像中成像椭圆的检测，采用基于朝向变换的椭圆检测方法予以实现。基于朝向变换的椭圆检测首先采用朝向变换筛选出图像有效曲线段，进而采用启发式搜索的方法对曲线段进行组合，以实现椭圆的检测，最终对得到的椭圆检测结果采用赫尔姆霍茨准则进行验证，得到最终的椭圆检测结果；

为实现对目标位置、姿态参数的连续测量，需对图像中特征进行连续检测。考虑目标运动的连续性，在完成首帧图像中椭圆目标的检测后，在后续图像中，采用全局约束的搜索策略对椭圆特征进行跟踪，如附图3所示，对上一帧图像中检测到椭圆特征进行离散采样，记得到的采样点集合为{m_i}_N，针对每一个采样点m_i，在其法线方向上基于梯度强度筛选出对应的候选对应点集合

每一个采样点对应的候选对应点数目M_i可能不同，基于椭圆对应采样点的连续性，通过最小化能量函数E(A)，确定真实对应点，

式中N为采样点数目，A＝(α₁，α₂，…，α_N)，

α_ij标识第i个采样点对应的第j个候选点是否有效，α_ij＝1则第i个采样点对应的第j个候选点有效，反之无效，j 及k为候选点下标，E_d及E_s分别为数据项及平滑项，由对应位置的像素值及距离计算得到，采用动态规划方法可实现对上述问题的高效求解。基于得到的真实对应点集合，采用最小二乘椭圆拟合算法，得到跟踪的椭圆参数；

(2)异面特征点检测

对于异面特征点，本发明选用对角标志，并采用归一化相关方法实现对角标志的检测。为适应对角标志的旋转，本发明事先沿顺时针方向制备不同角度的K个模板，针对每一个模板，采用滑动窗口的方式对图像完成处理，最终的响应图中每个位置的响应值 R(x,y)为S个模板对应响应值的最大值，如公式(2)所示，

式中T_k和I分别为第s个模板图像和输入图像，S为本发明采用的模板总数，s为每一个模板的下标，T_k(x',y')为坐标(x',y')处的像素灰度值，I(x+x',y+y')为坐标(x+x',y+y') 处的像素灰度值，(x',y')为模板图像中坐标，(x,y)为输入图像中坐标。

定位响应图中最大响应值R_max对应的位置，若R_max＞Thresh_value，则将此位置作为一面特征点检测结果，反之，认为当前图像中无设置的异面特征点，Thresh_value为设定的响应阈值；

(3)位置、姿态参数解算

基于椭圆特征及异面特征点的检测结果，实现目标位置、姿态参数的解算。圆特征成像示意图如附图4所示，O_c-X_cY_cZ_c及O-UV分别为相机坐标系及图像坐标系， (x₀,y₀,z₀)及(n_x,n_y,n_z)分别为圆特征在相机坐标系下的圆心坐标及法向；

图像中椭圆特征可表示为

au²+bv²+cuv+du+ev+f＝0 (3)

其中a～f为空间椭圆方程的参数，可由椭圆特征检测与跟踪方法得到。

图像坐标系下坐标(u,v)与相机坐标系下坐标(x,y,z)具有如下关系

u＝f₀x/z,v＝f₀y/z (4)

f₀为焦距。

将公式(4)带入公式(3)中得

Ax²+By²+Cxy+Dxz+Eyz+Fz²＝0 (5)

式中

将公式(5)改写为矩阵形式，得

Q为一对称矩阵，则必存在正交矩阵P满足

P^TQP＝diag(λ₁,λ₂,λ₃) (7)

λ₁,λ₂,λ₃为Q的特征值，对其进行排序使得它们满足λ₁与λ₂同号，与λ₃异号，且|λ₁|≥|λ₂|，λ₁,λ₂,λ₃对应的归一化特征向量记为e₁,e₂,e₃，可令P＝[e₁e₂e₃]。

定义(x',y',z')^T＝P(x,y,z)^T，得到在新坐标系下的标准圆锥体的表示

(x',y',z')diag(λ₁,λ₂,λ₃)(x',y',z')^T＝0 (8)

正交矩阵P可视为O_c-X_cY_cZ_c转换到O_c-X'Y'Z'的旋转。在O_c-X'Y'Z'中，圆锥体轴线与 Z'轴重合，结合圆特征半径R，可得圆心及法向求解结果

利用P^-1，可将公式(9)所示解转换到O_c-X_cY_cZ_c中，

记为{(x_0i,y_0i,z_0i)^T,(n_xi,n_yi,n_zi)^T}_(i＝1,2)；

如附图5所示，基于单目图像求解圆特征参数时存在二义性。本发明提出引入异面点特征予以解决。记异面点特征在O_c-X_cY_cZ_c中为P(x_p,y_p,z_p)^T，其对应成像点的齐次坐标为p(u_p,v_p,1)^T，基于共线约束可知理想情况下P位于直线上

记P 与圆特征圆心的距离为Dp，Do为Dp在圆特征法向上的投影，Dp与Do均为已知量，基于Dp与共线约束，针对公式(9)所示的两组圆心坐标{(x_0i,y_0i,z_0i)^T}_(i＝1,2)，通过求解点到直线的距离可得到P对应的四个解{P_rj}_{(j＝1,2,3,4)}，计算对应的Do_rj，通过公式 (10)确定正确的圆特征与特征点参数；

O_b-X_bY_bZ_b各坐标轴在O_c-X_cY_cZ_c中的表示为

其中

则目标位置、姿态参数如公式(12)所示：

3、本发明达到的技术效果

与现有技术相比，本发明的优点在于：

(1)本发明利用目标圆特征及异面特征点，仅基于单目图像完成目标位置、姿态测量，算法步骤简单明了，计算复杂度低，易于应用；

(2)本发明通过引入异面特征点，利用特征点与圆特征圆心、法向之间的空间关系，很好地解决了圆特征参数求解中的二义性问题；

(3)本发明采用全局约束的搜索策略对椭圆特征的跟踪，实现了对目标位置、姿态参数的高效、可靠连续测量。

附图说明

图1基于圆特征及异面特征点的单目图像位置、姿态测量示意图，

基于圆特征及异面特征点的单目图像位置、姿态测量方法基于目标圆特征及异面特征点，求取物体坐标系O_b-X_bY_bZ_b到相机坐标系O_c-X_cY_cZ_c变换[R Τ]；

图2基于圆特征及异面特征点的单目图像位置、姿态测量方法流程图，

对于首帧输入图像进行椭圆特征检测，作为圆特征初始化，对于后续图像，采用跟踪方法实现椭圆特征的跟踪；基于模板匹配的方法实现特征点检测；基于椭圆检测结果完成圆特征参数的求解，结合点特征检测结果，消除参数求解的二义性，得到最终的位置、姿态参数测量结果；

图3基于全局约束搜索策略的椭圆特征跟踪示意图，

将上一帧图像中椭圆检测或跟踪结果作为当前帧中初始化结果，离散采样，对每一采样点，沿其法线方向搜索对应点；

图4圆特征成像示意图，

O_c-X_cY_cZ_c及O-UV分别为相机坐标系及图像坐标系，(x₀,y₀,z₀)及(n_x,n_y,n_z)分别为圆特征在相机坐标系下的圆心坐标及法向。O-UV上椭圆记为圆特征在像平面上对应的椭圆成像；

图5单目图像中圆特征参数求解的二义性示意图，

P₁为像平面，由空间几何关系可知，平面P₂及P₃上上所示的圆特征均可对应P₁上的成像椭圆。

具体实施方式

以下对本发明的实施方式做进一步详细说明：

(1)圆特征提取

针对首帧输入图像，采用基于朝向变换的椭圆提取方法检测图像中目标圆特征对应的成像椭圆。基于朝向变换的椭圆检测首先采用朝向变换筛选出图像有效曲线段，进而采用启发式搜索的方法对曲线段进行组合，以实现椭圆的检测，最终对得到的椭圆检测结果采用赫尔姆霍茨准则进行验证，得到最终的椭圆检测结果；

对于后续输入图像，采用基于全局约束的搜索策略，实现成像椭圆的连续跟踪。如附图3所示，对上一帧图像中检测到椭圆特征进行离散采样，记得到的采样点集合为 {m_i}_N，针对每一个采样点m_i，在其法线方向上基于梯度强度筛选出对应的候选对应点集合

每一个采样点对应的候选对应点数目M_i可能不同，基于椭圆对应采样点的连续性，通过最小化能量函数E(A)，确定真实对应点，

式中N为采样点数目，A＝(α₁，α₂，…，α_N)，

，α_ij标识第i个采样点对应的第j个候选点是否有效，α_ij＝1则第i个采样点对应的第 j个候选点有效，反之无效，j及k为候选点下标，E_d及E_s分别为数据项及平滑项，由对应位置的像素值及距离计算得到，采用动态规划方法可实现对上述问题的高效求解。基于得到的真实对应点集合，采用最小二乘椭圆拟合算法，得到跟踪的椭圆参数；

(2)异面特征点提取

针对输入图像，基于模板匹配的方法实现图像中对角标志标识的异面特征点成像的提取。采用归一化相关作为相似性度量准则，为适应对角标志的旋转，本发明事先沿顺时针方向制备不同角度的K个模板，针对每一个模板，采用滑动窗口的方式对图像完成处理，最终的响应图中每个位置的响应值R(x,y)为K个模板对应响应值的最大值，如公式(2)所示，

式中T_k和I分别为第s个模板图像和输入图像，S为本发明采用的模板总数，s为每一个模板的下标，T_k(x',y')为坐标(x',y')处的像素灰度值，I(x+x',y+y')为坐标(x+x',y+y') 处的像素灰度值，(x',y')为模板图像中坐标，(x,y)为输入图像中坐标。定位响应图中最大响应值R_max对应的位置，若R_max＞Thresh_value，则将此位置作为一面特征点检测结果，反之，认为当前图像中无设置的异面特征点，Thresh_value为设定的响应阈值；

(3)基于圆特征的位置、姿态参数求解

依据步骤(1)中提取的椭圆特征，结合圆特征圆心坐标、法向及半径信息，求取出相机与目标之间的相对位置、姿态关系；

圆特征成像示意图如附图4所示，O_c-X_cY_cZ_c及O-UV分别为相机坐标系及图像坐标系，(x₀,y₀,z₀)及(n_x,n_y,n_z)分别为圆特征在相机坐标系下的圆心坐标及法向；

图像中椭圆特征可表示为

au²+bv²+cuv+du+ev+f＝0 (3)

其中a～f由椭圆特征检测与跟踪方法得到。其中a～f为空间椭圆方程的参数，可由椭圆特征检测与跟踪方法得到。

图像坐标系下坐标(u,v)与相机坐标系下坐标(x,y,z)具有如下关系

u＝f₀x/z,v＝f₀y/z (4)

f₀为焦距，将公式(4)带入公式(3)中得

Ax²+By²+Cxy+Dxz+Eyz+Fz²＝0 (5)

式中

将公式(5)改写为矩阵形式，可得

Q为一对称矩阵，则必存在正交矩阵P满足

P^TQP＝diag(λ₁,λ₂,λ₃) (7)

λ₁,λ₂,λ₃为Q的特征值，对其进行排序使得它们满足λ₁与λ₂同号，与λ₃异号，且 |λ₁|≥|λ₂|，λ₁,λ₂,λ₃对应的归一化特征向量记为e₁,e₂,e₃，可令P＝[e₁ e₂ e₃]。定义(x',y',z')^T＝P(x,y,z)^T，得到在新坐标系下的标准圆锥体的表示

(x',y',z')diag(λ₁,λ₂,λ₃)(x',y',z')^T＝0 (8)

正交矩阵P可视为O_c-X_cY_cZ_c转换到O_c-X'Y'Z'的旋转。在O_c-X'Y'Z'中，圆锥体轴线与 Z'轴重合，结合圆特征半径R，可得圆心及法向求解结果

利用P^-1，可将公式(9)所示解转换到O_c-X_cY_cZ_c中，

记为{(x_0i,y_0i,z_0i)^T,(n_xi,n_yi,n_zi)^T}_(i＝1,2)；

(4)基于异面特征点的圆特征位置、姿态参数二义性消除

利用异面特征点与圆特征之间已知的空间关系，消除基于单目图像圆特征位置、姿态参数求解的二义性；

如附图5所示，基于单目图像求解圆特征参数时存在二义性。本发明提出引入异面点特征予以解决。记异面点特征在O_c-X_cY_cZ_c中为P(x_p,y_p,z_p)^T，其对应成像点的齐次坐标为p(u_p,v_p,1)^T，基于共线约束可知理想情况下P位于直线上

记P与圆特征圆心的距离为Dp，Do为Dp在圆特征法向上的投影，Dp与Do均为已知量，基于Dp与共线约束，针对公式(9)所示的两组圆心坐标{(x_0i,y_0i,z_0i)^T}_(i＝1,2)，通过求解点到直线的距离可得到P对应的四个解{P_rj}_{(j＝1,2,3,4)}，计算对应的Do_rj，通过公式(10)确定正确的圆特征与特征点参数；

O_b-X_bY_bZ_b各坐标轴在O_c-X_cY_cZ_c中的表示为

其中

则目标位置、姿态参数如公式(12)所示；

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (4)

1.基于圆特征及异面特征点的单目图像位置、姿态测量方法，其特征在于，

通过基于朝向变换的椭圆检测方法实现图像中圆特征对应成像椭圆的检测，结合圆特征圆心、法向及半径信息，求解目标位置、姿态参数；采用模板匹配的方法定位图像中异面特征点，结合特征点与圆特征之间的空间关系，消除单目图像中圆特征位置、姿态参数求解的二义性，得到最终的位置、姿态测量结果；利用基于全局约束的搜索策略，对序列图像中圆特征对应的成像椭圆的自动跟踪，实现对目标位置、姿态参数的连续测量；

其步骤为：

(1)圆特征提取：针对首帧输入图像，采用基于朝向变换的椭圆提取方法检测图像中目标圆特征对应的成像椭圆，对于后续输入图像，采用基于全局约束的搜索策略，实现成像椭圆的连续跟踪；

(2)异面特征点提取：针对输入图像，基于模板匹配的方法实现图像中对角标志标识的异面特征点成像的提取；

(3)基于圆特征的圆特征位置、姿态参数求解：依据步骤(1)中提取的椭圆特征，结合圆特征圆心坐标、法向及半径信息，求取出相机与目标之间的相对位置、姿态关系；

(4)基于异面特征点的位置、姿态参数二义性消除：利用异面点与圆特征之间已知的空间关系，消除基于单目图像圆特征位置、姿态参数求解的二义性；

所述基于朝向变换的椭圆提取方法检测图像中目标圆特征对应的成像椭圆，具体为：

首先采用朝向变换筛选出图像有效曲线段，进而采用启发式搜索的方法对曲线段进行组合，以实现椭圆的检测，最终对得到的椭圆检测结果采用赫尔姆霍茨准则进行验证，得到最终的椭圆检测结果；

为实现对目标位置、姿态参数的连续测量，需对图像中特征进行连续检测，考虑目标运动的连续性，在完成首帧图像中椭圆目标的检测后，在后续图像中，采用全局约束的搜索策略对椭圆特征进行跟踪，对上一帧图像中检测到椭圆特征进行离散采样，记得到的采样点集合为{m_i}_N，针对每一个采样点m_i，在其法线方向上基于梯度强度筛选出对应的候选对应点集合

每一个采样点对应的候选对应点数目M_i可能不同，基于椭圆对应采样点的连续性，通过最小化能量函数E(A)，确定真实对应点，

式中N为采样点数目，A＝(α₁，α₂，L，α_N)，

α_ij标识第i个采样点对应的第j个候选点是否有效，α_ij＝1则第i个采样点对应的第j个候选点有效，反之无效，j及k为候选点下标，E_d及E_s分别为数据项及平滑项，由对应位置的像素值及距离计算得到，基于得到的真实对应点集合，采用最小二乘椭圆拟合算法，得到跟踪的椭圆参数。

2.根据权利要求1所述的基于圆特征及异面特征点的单目图像位置、姿态测量方法，其特征在于，所述异面特征点提取，具体为：

选用对角标志，并采用归一化相关方法实现对角标志的提取，为适应对角标志的旋转，事先沿顺时针方向制备不同角度的K个模板，针对每一个模板，采用滑动窗口的方式对图像完成处理，最终的响应图中每个位置的响应值R(x,y)为K个模板对应响应值的最大值，如公式(2)所示，

式中T_k和I分别为第k个模板图像和输入图像，K为模板总数，k为每一个模板的下标，T_k(x',y')为坐标(x',y')处的像素灰度值，I(x+x',y+y')为坐标(x+x',y+y')处的像素灰度值，(x',y')为模板图像中坐标，(x,y)为输入图像中坐标；

定位响应图中最大响应值R_max对应的位置，若R_max＞Thresh_value，则将此位置作为一面特征点检测结果，反之，认为当前图像中无设置的异面特征点，Thresh_value为设定的响应阈值。

3.根据权利要求1所述的基于圆特征及异面特征点的单目图像位置、姿态测量方法，其特征在于，所述位置、姿态参数求解，具体为：

基于椭圆特征及异面特征点的检测结果，实现目标位置、姿态参数的求解，O_c-X_cY_cZ_c及O-UV分别为相机坐标系及图像坐标系，(x₀,y₀,z₀)及(n_x,n_y,n_z)分别为圆特征在相机坐标系下的圆心坐标及法向；

椭圆特征表示为

au²+bv²+cuv+du+ev+f＝0 (3)

其中a～f为空间椭圆方程的参数，由椭圆特征检测与跟踪方法得到，

图像坐标系下坐标(u,v)与相机坐标系下坐标(x,y,z)具有如下关系

u＝f₀x/z,v＝f₀y/z (4)

f₀为焦距，

将公式(4)带入公式(3)中得

Ax²+By²+Cxy+Dxz+Eyz+Fz²＝0 (5)

式中

F＝f，将公式(5)改写为矩阵形式，得

Q为一对称矩阵，则必存在正交矩阵P满足

P^TQP＝diag(λ₁,λ₂,λ₃) (7)

λ₁,λ₂,λ₃为Q的特征值，对其进行排序使得它们满足λ₁与λ₂同号，与λ₃异号，且|λ₁|≥|λ₂|，λ₁,λ₂,λ₃对应的归一化特征向量记为e₁,e₂,e₃，令P＝[e₁ e₂ e₃]，定义(x',y',z')^T＝P(x,y,z)^T，得到在新坐标系下的标准圆锥体的表示

(x',y',z')diag(λ₁,λ₂,λ₃)(x',y',z')^T＝0 (8)

正交矩阵P为坐标系O_c-X_cY_cZ_c到坐标系O_c-X'Y'Z'的旋转矩阵，在O_c-X'Y'Z'中，圆锥体轴线与Z'轴重合，结合圆特征半径R，得圆心及法向求解结果

利用P^-1，将公式(9)所示解转换到O_c-X_cY_cZ_c中，

记为{(x_0i,y_0i,z_0i)^T，(n_xi,n_yi,n_zi)^T}_(i＝1,2)。

4.根据权利要求3所述的基于圆特征及异面特征点的单目图像位置、姿态测量方法，其特征在于，基于单目图像求解圆特征参数时存在二义性，引入异面点特征予以解决，记异面点特征在O_c-X_cY_cZ_c中为P(x_p,y_p,z_p)^T，其对应成像点的齐次坐标为p(u_p,v_p,1)^T，基于共线约束知理想情况下P位于直线上

记P与圆特征圆心的距离为Dp，Do为Dp在圆特征法向上的投影，Dp与Do均为已知量，基于Dp与共线约束，针对公式(9)所示的两组圆心坐标{(x_0i,y_0i,z_0i)^T}_(i＝1,2)，通过求解点到直线的距离可得到P对应的四个解{P_rj}_{(j＝1,2,3,4)}，计算对应的Do_rj，通过公式(10)确定正确的圆特征与特征点参数；

O_b-X_bY_bZ_b各坐标轴在O_c-X_cY_cZ_c中的表示为

其中

则目标位置、姿态参数如公式(12)所示：

Images