一种基于单目相机的圆形靶标位姿估计方法技术

本发明专利技术提出一种基于单目相机的圆形靶标位姿估计方法，包括：步骤一、靶标成像椭圆方程求解；步骤二、靶标法向量计算；步骤三、靶标中心三维坐标计算；步骤四、靶标法向量解耦；步骤五、靶标位姿优化。本发明专利技术给出了求解靶标位置和平面法向量的完备的解析形式，能够在单个毫秒内完成解算，具有高速高精度的特性，同时采用单目相机降低了测量对系统硬件设备的要求，提高了系统的灵活性和适应性。系统中采用合作靶标的方法，既能高鲁棒性地解决法向量解耦问题，也能提高靶平面法向量计算的精度，由于合作靶标配置方案在测量中很容易实现，也是实际测量中常用的测量策略，因此该方法在合作测量中具有广泛的应用前景。中具有广泛的应用前景。中具有广泛的应用前景。

【技术实现步骤摘要】
S.Ahmad，3D location of circular and spherical features by monocular model
‑
based vision，In Proc.IEEE Int.Conf.Syst.，Man Cybern.，1989，pp.576
‑
581(Y.C.Shiu和S.艾哈迈德，基于单目视觉的圆形和球形特征位置确定，电气与电子工程师协会人与控制论国际会议，页码576
‑
581，1989年))，能够给出一个解析解，但是求出的法向量具有两个无法解耦。为此后续很多工作集中在如何从求的两个法向量中求解真实的法向量，代表性的工作有：通过添加共面点的约束(D.He and B.Benhabib，Solving the orientation
‑
duality problem for a circular feature in motion，IEEE Trans.Syst.，Man，Cybern.A，Syst.Humans，vol.28，no.4，pp.506
>‑
515本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于单目相机的圆形靶标位姿估计方法，其特征在于：包括如下步骤：步骤一、靶标成像椭圆方程求解；给定圆形靶标投影的椭圆，提取椭圆的边缘点，对边缘点进行椭圆拟合得到椭圆的参数方程，将椭圆参数方程变换为二次型的形式；步骤二、靶标法向量计算；将像素坐标系下的二次型变换为投影坐标系下的二次型，然后通过构建含有三变元的正交变换矩阵将投影坐标系下的二次型变换为目标坐标系下的二次型，此二次型应具有圆的特征，利用该约束求解得到三变元，进而得到椭圆所在平面的法向量；步骤三、靶标中心三维坐标计算；根据步骤二计算所得的正交变换矩阵得到靶圆所在射影坐标系中的代数流形，设圆所在平面为该代数流形的仿射图，并设定该放射图在Z轴上的坐标已知，通过计算得到圆形标志点中心距离投影坐标系的距离，根据投影椭圆的中心点和相机的内参数得到圆心所在的投影坐标系中的反投影射线，进而得到靶圆中心在投影坐标系中的三维坐标；步骤四、靶标法向量解耦通过以上三个步骤分别计算出两个共面靶标的法向量和三维坐标，利用两靶标点中心连线和靶标法向量垂直的约束计算出每个标志点的真实法向量，并通过最小二乘法求得最优的靶标法平面向量；步骤五、靶标位姿优化通过以上步骤得到靶标的初始位姿信息，利用靶标的初始位姿和直径信息计算其在相机中投影形成的椭圆，将步骤一得到的椭圆边缘点在计算投影椭圆方程上的误差作为目标函数，将靶标的三维坐标作为优化参数，采用优化的方法得到是目标函数最小的靶标三维坐标；最终得到圆形靶标的位姿信息。2.根据权利要求1所述的一种基于单目相机的圆形靶标位姿估计方法，其特征在于：在步骤一中，具体为：给定椭圆边缘点坐标对序列(x
i
，y
i
)，i＝1，2，...，n；其中x
i
，y
i
分别为第i个边缘点在横轴和纵轴的像素坐标值，n为边缘像素总个数；设椭圆的参数方程为Ax2+Bxy+Cy2+Dx+Ey+F＝0，其中A＝1，[B，C，D，E，F]为未知量；则对于任何一个边缘点对(x
i
，y
i
)有方程对于所有边缘点得到方程组H[B，C，D，E，F]
T
＝
‑
b，其中采用最小二乘法得到[B，C，D，E，F]
T
＝
‑
inv(H
T
H)H
T
b,其中inv(
·
)为矩阵求逆运算，令[A，B，C，D，E，F]
T
＝sign(C)[A，B，C，D，E，F]
T
，其中sign(
·
)为符号函数；椭圆的参数方程表示成二次型的形式[x，y，1]F[x，y，1]
T
＝0,其中如果C|F|≥0，则椭圆为退化的无法求取标志圆所在圆平面的法向量，其中|
·
|为求矩阵行列式运算；当所求椭圆为非退化时，即C|F|＜0,由C＞0得到|F|＜0；由椭圆的参数方程Ax2+Bxy+Cy2+Dx+Ey+F＝0得到椭圆中心所在的像素齐次坐标为PO＝((2CD
‑
BE)/(B2‑
4AC)，(2AE
‑
BD)/(B2‑
4AC)，1)，进而由相机成像原理得到靶标圆心在相机投影坐标系下的反投影射线向量为PV＝PO*inv(M)，其中函数inv(
·
)为矩阵求逆运算，矩阵M
为相机的内参数矩阵。3.根据权利要求1所述的一种基于单目相机的圆形靶标位姿估计方法，其特征在于：在步骤二中，具体为：设定相机投影坐标系为OXYZ1，对应的坐标表示为XYZ1，该坐标系经过射影变换后的坐标系为OXYZ2，对应的坐标表示为OXYZ2，使得向量[0,0,1]与圆所在平面的法向量平行；其中，摄影变换表示为正交变换矩阵TR，使得XYZ1＝XYZ2RT，求中x0，y0，z0，θ分别为未知的变量，且z0≠0；给定相机内参数矩阵M，由[x，y，1]＝[X1/Z1，Y1/Z1，1]M和[x，y，1]F[x，y，1]
T
＝0得到此齐次方程为圆边缘点齐次坐标在射影坐标系OXYZ1下的代数流形；通过射影变换TR得到圆形边缘点齐次坐标在射影坐标系OXYZ2下的代数流形令MFM
T
＝QλQ
T
，其中Q为正交矩阵，λ1≥λ2≥λ3为矩阵MFM
T
特征值矩阵，由步骤一给定的|F|＜0，得到|QλQ
T
|＝|Q|2|λ|＝|MFM
T
|＝|M|2|F|＜0,得到|λ|＜0，同时由于F是二次型，因此λ1≥λ2＞0＞λ3；由上得到TMFM
T
T
T
＝(TQ)λ(TQ)
T
，由矩阵T，Q都是正交矩阵，得出矩阵TQ也是正交矩阵，令其中x2+y2+z2＝1，z≠1，则由于在坐标系OXYZ2中向量[0,0,1]与圆所在平面的法向量平行，因此二次型矩阵SλS
T
因为是对称矩阵且满足方程组其中f1，f2分别为两方程的标号；由于该方程组含有三个未知数，且只有两个非线性方程，采用分支定界的方法求取方程组的解析解。4.根据权利要求3所述的一种基于单目相机的圆形靶标位姿估计方法，其特征在于：由方程f1得到，使其成立的条件共有λ1＝λ2，x＝0，y＝0，z＝0这四种情况；设λ1＝λ2，由方程f...

【专利技术属性】
技术研发人员：白相志，郭胜，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：