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

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，jul.1998(d.he和贝诺
·
本哈比卜，求解运动圆形特征法向量对偶问题，电气与电子工程师协会人与控制论国际会议，第28卷，第4篇，页码506-515，1998年7月))，或者通过增加共面线的约束(c.meng，j.xue，and z.hu，monocular position-pose measurement based on circular and linear features，in proc.ieee int.conf.digit.image comput.，techn.appl.，2015，pp.1-8(孟偲，薛娇和胡詹，基于圆和线特征的单目位姿估计，数字图像计算国际会议：技术与应用，2015年，页码1-8))。


技术实现要素：

6.本发明提供了一种基于单目相机的圆形靶标位姿估计技术，通过在射影几何框架下研究相机成像过程，将圆形靶标和投影椭圆看作同一代数流形在不同射影坐标系下的z轴上投影形成的仿射图，将靶标位置和法平面估计问题简化为射影坐标系正交变换矩阵求解问题，然后利用共面靶标的约束求得真实平面法向量。该方法给出了求解靶标位置和平面法向量的完备的解析形式，能够在单个毫秒内完成解算，具有高速高精度的特性，同时采用单目相机降低了测量对系统硬件设备的要求，提高了系统的灵活性和适应性。
7.本发明采取以下技术方案：
8.步骤一、靶标成像椭圆方程求解；
9.给定圆形靶标投影的椭圆，提取椭圆的边缘点，对边缘点进行椭圆拟合得到椭圆的参数方程，将椭圆参数方程变换为二次型的形式。
10.步骤二、靶标法向量计算；
11.将像素坐标系下的二次型变换为投影坐标系下的二次型，然后通过构建含有三变元的正交变换矩阵将投影坐标系下的二次型变换为目标坐标系下的二次型，此二次型应具有圆的特征，利用该约束求解得到三变元，进而得到椭圆所在平面的法向量。
12.步骤三、靶标中心三维坐标计算；
13.根据步骤二计算所得的正交变换矩阵可得靶圆所在射影坐标系中的代数流形，假设圆所在平面为该代数流形的仿射图，并假定该放射图在z轴上的坐标已知，通过计算得到圆形标志点中心距离投影坐标系的距离，根据投影椭圆的中心点和相机的内参数可得圆心所在的投影坐标系中的反投影射线，进而可得到靶圆中心在投影坐标系中的三维坐标。
14.步骤四、靶标法向量解耦
15.通过以上三个步骤分别计算出两个共面靶标的法向量和三维坐标，利用两靶标点中心连线和靶标法向量垂直的约束计算出每个标志点的真实法向量，并通过最小二乘法求得最优的靶标法平面向量。
下的代数流形令mfm
t
＝qλq
t
，其中q为正交矩阵，为矩阵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分别为两方程的标号。由于该方程组含有三个未知数，且只有两个非线性方程，采用分支定界的方法求取方程组的解析解，即：由方程f1可知，使其成立的条件共有λ1＝λ2，x＝0，y＝0，z＝0这四种情况。假设λ1＝λ2，由方程f2可得λ1z2 λ3(x2 y2)＝λ1，由x2 y2 z2＝1可得λ1(x2 y2)＝λ3(x2 y2)，由x2 y2≠0可得λ1＝λ3，这与λ1＞λ3相矛盾，因此λ1≠λ2；当x＝0时，由方程f2可得λ2y2z2 λ3y4＝λ1y2，y≠0，可得(λ
3-λ2)y2＝λ
1-λ2，由于λ
3-λ2＜0，λ
1-λ2＜0，可知等式不成立，因此x≠0；当z＝0时，由方程f2可得λ1y2 λ2x2＝λ3，由x2 y2 z2＝1可得(λ
2-λ1)x2＝λ
3-λ1，即不成立，因此z≠0；当y＝0时，由方程f2可得λ1x2z2 λ3x4＝λ2x2，可得由x2 y2 z2＝1可得，由tq＝s可得t＝sq
t
，则[x0，y0，z0]＝[x，y，z]q
t
，由于[x，y，z]共有四组解，因此靶标所在平面的法向量[x0，y0，z0]也共有四组。由于向量[x0，y0，z0]为圆所在平面法向量在坐标系oxyz2中的坐标，同时取法向量方向与坐标系z轴方向成钝角设定，因此可得靶标所在平面向量在坐标系oxyz1下的两组解其中v1，v2，分别为靶标所在平面的法向量，令则
[0024]
步骤三、靶标中心三维坐标计算；
之间的旋转矩阵为令坐标系cxyz1为以靶点圆心为坐标原点，以靶点法向量为z轴的坐标系，令坐标系cxyz2为以坐标系oxyz2的z轴与靶点所在平面的交点为原点，以靶点法向量为z轴的坐标系，则坐标系cxyz1和cxyz2的坐标c
xyz1
，c
xyz2
满足c
xyz2
＝c
xyz1
tc，其中tc＝coor*r
t
。在坐标系cxyz1中，靶圆边缘点满足方程[c
x1
，c
y1
，c
z1
]f[c
x1
，c
y1
，c
z1
]
t
＝0，其中r为圆形靶点的半径，其在坐标系cxyz2中表示为其中其在坐标系oxyz2中表示为令则令p
xyz2
＝[p
x2
，p
y2
，p
z2
]，则其在cxyz1中表示为令q
xyz
＝p
xyz1
m，其中m为相机的内参数矩阵，则对于步骤一中椭圆的边缘点(xi，yi)(i＝1，2，...，n)，构建其齐次坐标[yi，xi，1](i＝1，2，...，n)，令为优化的目标函数，优化变量为coor和vo，构建优化问题其中符号||
·
||为取向量模值。对于上述优化问题，采用内点法迭代求解，求解得到的最优值coor
opt
和vo
opt
即为靶点的三维坐标和平面法向量。
[0031]
本发明的优点及功效：本发明基于单个相机和两个共面圆形靶标来计算靶标在相机坐标系下的空间位置和平面法向量，采用射影变换来计算出靶平面法向量组，并通过代数流形和仿射图之间的变换关系得到靶圆中心与投影中心的距离，进而得到靶圆中心在相机坐标系的三维坐标，利用共面靶标中心连线与靶平面正交的关系解耦出正确的靶平面法向量，对于得到的靶点位姿的初始值，将靶点投影到相机成像平面上得到计算投影椭圆方程，利用真实椭圆边缘点在计算投影椭圆上的误差作为目标函数，对靶点位姿进行优化。本发明给出了求解靶标位置和平面法向量的完备的解析形式，能够在单个毫秒内完成解算，具有高速高精度的特性，同时采用单目相机降低了测量对系统硬件设备的要求，提高了系统的灵活性和适应性。系统中采用合作靶标的方法，既能高鲁棒性地解决法向量解耦问题，也能提高靶平面法向量计算的精度，由于合作靶标配置方案在测量中很容易实现，也是实际测量中常用的测量策略，因此该方法在合作测量中具有广泛的应用前景。
附图说明
[0032]
图1为本发明用于具体实施介绍的应用实例。
[0033]
图2为本方法中坐标系示意图。
[0034]
图3a为实验1待检测圆形靶点的原始输入图像。
[0035]
图3b为基于本发明的计算出的结果经过投影后在图像中的展示。其中，圆心代表三维坐标在成像平面上的投影，箭头代表法向量的在成像平面上的投影。
[0036]
图4a为实验2待检测圆形靶点的原始输入图像。
[0037]
图4b为基于本发明的计算出的结果经过投影后在图像中的展示。其中，圆心代表三维坐标在成像平面上的投影，箭头代表法向量的在成像平面上的投影。
具体实施方式
[0038]
为了更好地理解本发明的技术方案，以下结合附图1中的具体应用实例对本发明的实施方式作进一步描述，在本例中步骤一到步骤三中只对附图1中靶标c1c2进行处理，对于其它靶点也是按照这种方式处理。为了进一步展示本方法的有效性，我们在附图3和附图4中分别展示了本方法的两次实验结果，其中图3(a)和图4(a)分别为两次实验的待检测靶点的输入图像，图3(b)和图4(b)分别为两次实验采用本方法检测到的靶点中心位置和法向量展示图，
[0039]
本发明具体实施步骤如下：
[0040]
步骤一、靶标成像椭圆方程求解
[0041]
给定附图1中靶标c1的成像椭圆边缘点坐标对序列(xi，yi)(i＝1，2，...，n)，其中xi，yi分别为第i个边缘点在横轴和纵轴的像素坐标值，n在样例中为200。假设椭圆的参数方程为ax2 bxy cy2 dx ey f＝0，其中a＝1，[b，c，d，e，f]为未知量。则对于任何一个边缘点对(xi，yi)有方程对于所有边缘点可得方程组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]＝sign(c)[a，b，c，d，e，f]，其中sign(
·
)为符号函数，在本样例中参数[a，b，c，d，e，f]的值为[1，-0.042，0.8767，-129.4594，-93.3336，5657.9]。椭圆的参数方程可以表示成二次型的形式[x，y，1]f[x，y，1]
t
＝0，其中当所求椭圆退化为圆时即|f|＝0，其中|
·
|为求矩阵行列式运算，则圆的所在平面的法向量为[0，0，-1]；如果椭圆退化为一条直线|f|＞0，则无法求取靶标所在平面的法向量；当所求椭圆为非退化的，即c|f|《0，由c＞0可知|f|＜0。当本例中|f|＝-1020.2，故椭圆是非退化的。由椭圆的参数方程ax2 bxy cy2 dx ey f＝0可以得到椭圆中心的像素齐次坐标为po＝[(2cd-be)/(b
2-4ac)，(2ae-bd)/(b
2-4ac)，1]＝[723.881，926.8062，1]，进而由相机成像原理可得靶标圆心在相机投影坐标系下的反投影射线向量为pv＝po*inv(m)＝[0.0751，-0.0287，0.9968]，其中函数inv(
·
)为矩阵求逆运算，矩阵m为相机的内参数矩阵，在本例中
[0042]
步骤二、靶标平面法向量计算；
[0043]
如图2所示，假定相机投影坐标系为oxyz1，对应的坐标表示为xyz1，该坐标系经过射影变换后的坐标系为oxyz2，对应的坐标表示为xyz2，使得向量[0，0，1]与圆所在平面的法向量平行，其中摄影变换表示为正交变换矩阵tr，使得xyz1＝xyz2rt，求中rt，求中x0，y0，z0，θ分别为未知的变量，且给定相机内参数矩阵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为正交矩阵，为矩阵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，则＝1，z≠1，则其中符号～所在位置的值在本方法中未用到省略不写出。由于在坐标系oxyz2中向量[0，0，1]与圆所在平面的法向量平行，因此二次型矩阵sλs
t
因为对称矩阵且满足方程组其中f1，f2分别为两方程的标号。由于该方程组含有三个未知数，且只有两个非线性方程，采用分支定界的方法求取方程组的解析解。由方程f1可知，使其成立的条件共有λ1＝λ2，x＝0，y＝0，z＝0这四种
情况的组合，假设λ1＝λ2，由方程f2可得λ
1z2
λ3(x2 y2)＝λ1，由x2 y2 z2＝1可得λ1(x2 y2)＝λ3(x2 y2)，由x2 y2≠0可得λ1＝λ3这与λ1＞λ3相矛盾，因此λ1≠λ2；当x＝0时，由方程f2可得λ2y2z2 λ3y4＝λ1y2，y≠0，可得(λ
3-λ2)y2＝λ
1-λ2，由于λ
3-λ2＜0，λ
1-λ2≥0，可知等式不成立，因此x≠0；当z＝0时，由方程f2可得λ1y2 λ2x2＝λ3，由x2 y2 z2＝1可得(λ
2-λ1)x2＝λ
3-λ1，即不成立，因此z≠0；当y＝0时，由方程f2可得λ1x2z2 λ3x4＝λ2x2，可得由x2 y2 z2＝1可得，在本例中x＝
±
0.4354，z＝
±
0.9002。由tq＝s可得t＝sq
t
，则[x0，y0，z0]＝[x，y，z]q
t
，由于[x，y，z]共有四组解，因此靶标所在平面的法向量[x0，y0，z0]也共有四组，在本例中[x，y，z]四组解为[0.3599，0，-0.933]，[0.3599，0，0.933]，[-0.3599，0，-0.933]和[-0.3599，0，0.933]，对应的[x0，y0，z0]的四组解为[-0.1501，0.1428，-0.9783]，[0.1501，-0.1428，0.9783]，[0.5403，0.2475，-0.8042]和[-0.5403，-0.2475，0.8042]。由于向量[x0，y0，z0]为圆所在平面法向量在坐标系oxyz2中的坐标，同时取法向量方向与坐标系z轴方向成钝角设定，因此可得靶标所在平面向量在坐标系oxyz1下的两组解其中v1，v2，分别为靶标所在平面的法向量，在本例中v1＝[0.5403，0.2475，-0.8042]，v2＝[-0.1501，0.1428，-0.9783]。令则则
[0044]
步骤三、靶标中心三维坐标计算；
[0045]
由步骤二可知圆形边缘点在坐标系oxyz2下的齐次坐标满足代数流形令xyz3＝xyz2r则有其中t为步骤二已经得到的任一可行解，在本例中为令其中a，c，d，f为参数，可得在本例中a＝1.9198e7，c＝-0.7405e7，d＝0，f＝0.2855e7。求取该流形在坐标系oxyz3下z轴k处的仿射图可得令分别为圆形边缘点齐次坐标在仿射图上的仿射坐标，则即即可知圆在仿射坐标下的圆心坐标为圆的直径为
由于靶标直径已知给定为r，则得在本例中靶标圆的半径为25mm。圆心到坐标系中心0的距离在本例中靶标圆的半径为25mm。圆心到坐标系中心0的距离在本例中d＝3360.9mm。根据步骤一得到的反投影射线向量pv＝[0.0753，-0.0288，1]和上述计算得到靶标中心到投影中心的距离d＝3360.9mm，可以得到靶标的在投影坐标系下的三维坐标向量
[0046]
步骤四、靶标法向量解耦
[0047]
给定同一平面内两个圆形靶标c1，c2，分别按照步骤一计算得到对应成像椭圆反投影射线向量pv1＝[-0.0751，-0.0287，0.9968]，pv2＝[-0.0192，-0.0222，0.9996]，根据步骤二计算得到对应靶平面的法向量组二计算得到对应靶平面的法向量组根据步骤三计算得到靶标在投影坐标系下的三维坐标向量coor1＝[-252.3，96.5，3350]，coor2＝[-66.1，-76.5，3448.2]。由于靶标c1，c2在同一平面内，因此法向量组v1，v2中真实法向量应该与靶标c1，c2中心的连线垂直，令cv＝coor
1-coor2＝[-186.1665，-20.0332，-98.2164]，并对其进行长度归一化处理得到98.2164]，并对其进行长度归一化处理得到对于任意一个靶标的向量组v1，v2，分别找出与向量cv夹角最接近90
°
的向量作为该靶标的法向量即：
[0048]048]
在本例中，vt1＝0.5403，0.2476，-0.8042，vt2＝0.6139，0.2319，-0.7546。
[0049]
对于得到的目标法向量采用vt1、vt2最小二乘法拟合出最优的靶平面法向量vo，vo＝min
vo
(∑
i＝1，2
(vt
i-vo)(vt
i-vo)
t
)，
[0050]
其解析解为vo＝(vt1 vt2)/2＝[0.5771，0.2398，-0.7794]，模归一化后的靶平面法向量为vo＝vo/|vo|＝[0.5777，0.24，-0.7802]。
[0051]
综上得到了靶标c1，c2位姿信息，即中心坐标coor1，coor2，以及他们坐在平面的法向量vo。
[0052]
步骤五、靶标位姿优化
[0053]
对于步骤四中的任意一个靶标c∈(c1，c2)，计算得到的初始位姿为：三维坐标coor∈(coor1，coor2)，法向量vo。令(x0，y0，z0)＝vo，tc＝coor*r
t
，则坐标系oxyz1和坐标系oxyz2之间的旋转矩阵为令坐标系cxyz1为以靶点圆心为坐标原
点，以靶点法向量为z轴的坐标系，令坐标系cxyz2为以坐标系oxyz2的z轴与靶点所在平面的交点为原点，以靶点法向量为z轴的坐标系，则坐标系cxyz1和cxyz2的坐标c
xyz1
，c
xyz2
满足c
xyz2
＝c
xyz1
tc，其中tc＝coor*r
t
。在坐标系cxyz1中，靶圆边缘点满足方程[c
x1
，c
y1
，c
z1
]f[c
x1
，c
y1
，c
z1
]
t
＝0，其中r为圆形靶点的半径，其在坐标系cxyz2中表示为其中其在坐标系oxyz2中表示为令则令p
xyz2
＝[p
x2
，p
y2
，p
z2
]，则其在cxyz1中表示为令q
xyz
＝p
xyz1
m，其中m为相机的内参数矩阵，则对于步骤一中椭圆的边缘点(xi，yi)(i＝1，2，...，n)，构建其齐次坐标[yi，xi，1](i＝1，2，...，n)，令为优化的目标函数，优化变量为coor和vo，构建优化问题其中符号||
·
||为取向量模值。对于上述优化问题，采用内点法迭代求解，求解得到的最优值coor
opt
和vo
opt
即为靶点的三维坐标和平面法向量。
[0054]
对于本示例中的靶点c1，c2的位姿分别为：coor1＝[-251.3，95.2，3351.1]，coor2＝[-66.3，-76.1，3449.9]，vo1＝[0.5765，0.232，-0.7813]，vo2＝[0.5765，0.232，-0.7813]。