一种融合机器视觉和激光参照点信息的目标位姿测量方法与流程

1.本发明属于空间目标位姿测量领域，特别涉及一种融合机器视觉和激光参照点信息的目标位姿测量方法，应用于空间近场操作及在轨服务中的非合作目标位姿测量过程。


背景技术：

2.精确测量空间目标的相对位置和姿态(统称为位姿)是完成空间交会对接、攻防对抗、在轨捕获与维护等重大尖端国防及航天任务的关键，而基于单目视觉的位姿测量方法相对双目及多目视觉测量方法具有系统结构简单、摄像机标定步骤简明、测量视场范围广、成本低以及测量实时效率高等优点。凭借以上优点，基于单目视觉的位姿测量方法被广泛应用于空间非合作目标位姿测量领域。基于单目视觉的位姿测量方法大体上可以分为两大类：一类是基于待测目标的已知几何形状求解位姿参数，如圆环椭圆度法、目标长宽比法等方法。另一类是基于目标特征标志信息的位姿求解算法。前者需要已知目标的特定几何形状，而后者不受目标几何形状的限制，不仅适用于规则目标，也适用于各种形状不规则的目标，相比而言，后者具有更高的研究和实用价值，因而逐渐成为单目视觉的位姿测量领域的研究热点。而根据所用目标特征标志信息的不同，后者又可以分为基于点特征的单目视觉位姿测量技术和基于直线特征的单目视觉位姿测量技术。
3.基于点特征的目标位姿估计方法也被称为透视n点定位问题(perspective
‑
n
‑
point，pnp)，即给定摄像机内部参数、n个空间特征点在目标坐标系下的坐标信息及n个空间特征点对应像点在图像坐标系下的像素坐标，然后求解目标在摄像机坐标系下的位姿信息。按照求解方法来分类，pnp问题可分为非迭代方法和迭代方法。最经典的非迭代方法为dlt法，该方法最初是用于摄像机标定，改动后可以基于至少4个共面特征点或者至少6个非共面特征点求解位姿信息。但是dlt法不能保证旋转矩阵的正交性，对噪声十分敏感，鲁棒性不高。迭代方法通常利用像面重投影误差作为目标函数，然后利用gauss
‑
newton法或levenberg
‑
marquardt法等非线性优化算法进行迭代求解，通过最小化目标函数求解位姿信息，精度和鲁棒性较高。但缺点在于位姿解算精度与迭代算法中的旋转矩阵初始值关系较大，且迭代算法往往计算量大，算法效率较低。针对这一缺点，dementhon等提出了posit(pose from orthography and scaling with ierations)算法，该算法利用弱透视投影模型得到旋转矩阵初始值，再利用相关迭代算法求解位姿信息，posit算法精度较高，但其得到的旋转矩阵往往不是最优旋转矩阵。horaud提出一种基于平行透视迭代的位姿求解方法，该方法将弱透视投影模型用平行透视模型来代替，算法稳定性得到提高，并且能获得较好的求解速度。但该算法同样无法保证旋转矩阵的正交性，需要再采用其他方法求解最接近的正交矩阵，但得到的矩阵往往接近真实旋转矩阵但仍然不是最优的旋转矩阵。针对这一问题，lu提出了正交迭代算法(fast andglobally convergent pose estimation from video images"(c.p.lu,g.hager,e.mjolsness,ieee trans.pattern analysis and machine intelligence,vol.22,no.5,pp.610
‑
622，2000)，该算法以目标空间共线误差(物方共线误差)作为目标函数，可直接求解正交旋转矩阵，由于该算法具有精度高、鲁棒性好、
全局收敛以及算法精度不依赖于旋转矩阵初始值的优点，该算法逐渐成为基于单目视觉的位姿测量领域的研究热点之一。但该方法本质上是仍然一种迭代算法，这就决定了其运算效率较非迭代算法必然要低，而对于面向空间目标位姿测量的空间摄像机而言，其对算法的计算效率要求更高，因此需要改进传统的迭代过程，提升其运算效率。


技术实现要素：

4.为了克服传统正交迭代算法存在的运算效率较非迭代算法低的问题，本发明提供一种融合单目视觉和激光参照点信息的空间非合作目标位姿测量方法，利用激光参照点作为单目视觉位姿测量方法的空间特征点，对传统正交迭代算法的中间过程进行整合，通过在迭代过程消除平移向量的中间值来实现正交迭代算法的加速求解，同时利用平行透视投影模型来求解正交迭代算法中的旋转矩阵初始值，使得算法的性能和效率都得到了改善。
5.本发明的技术方案是提供一种融合机器视觉和激光参照点信息的目标位姿测量方法，其特殊之处在于，包括以下步骤：
6.步骤1、将激光参照点作为空间特征点，对激光参照点成像，提取激光参照点所成像点在摄像机像素坐标系中的坐标；
7.步骤2、构造物方共线误差，并以物方共线误差的最小值作为目标函数，对位姿参数旋转矩阵r进行迭代求解；
8.步骤2.1、确定旋转矩阵初始值；
9.步骤2.2、利用公式7确定激光参照点所成像点的视线投影矩阵w
i
：
[0010][0011]
其中，其中为第i个激光参照点在目标坐标系下的坐标，为第i个激光参照点对应的归一化像点齐次坐标；
[0012]
步骤2.3、利用视线投影矩阵w
i
、旋转矩阵r的初始值及构造物方共线误差：
[0013][0014]
其中，利用公式10求解平移向量
[0015][0016]
其中i为单位矩阵；
[0017]
步骤2.4、返回步骤2.3，利用奇异值分解方法(svd)获得绝对定向问题的最优解进而迭代更新旋转矩阵r，直至满足迭代终止条件；
[0018]
步骤2.5、选取迭代更新过程中获取的最小的物方共线误差，作为位姿估计的目标函数：
[0019]
[0020]
平移向量的最优解为：
[0021][0022]
进一步地，步骤2.1使用平行透视模型求解旋转矩阵初始值：
[0023]
设激光参照点集{p
i
}的齐次坐标为p
i
＝(x
i
,y
i
,z
i
,1)
t
，对应的像点集{p
i
}的齐次坐标(归一化像点齐次坐标)为p
i
＝(x
i
,y
i
,1)
t
，激光参照点集{p
i
}的质心齐次坐标为质心像点齐次坐标为(x0,y0,1)
t
，则旋转矩阵的初始值r0为：
[0024][0025]
其中，和分别表示和对应的反对称矩阵，和分别为三维列向量，其结果可由下式确定：
[0026][0027]
进一步地，步骤1具体包括以下步骤：
[0028]
步骤1.1、记录激光参照点的空间三维坐标；
[0029]
步骤1.2、对激光参照点成像，并对激光参照点成像照片进行预处理，预处理主要包括彩色图像灰度化处理、图像滤波、直方图均衡化、边缘锐化和图像去噪处理；
[0030]
步骤1.3、采用harris角点提取算法提取激光参照点所成像点在摄像机像素坐标系中的坐标。
[0031]
进一步地，步骤2之后还包括步骤3，基于正交仿真试验，分析测量精度：
[0032]
步骤3.1、设计正交试验；
[0033]
将空间特征点图像坐标提取精度、空间特征点空间坐标精度、摄像机主点标定精度、归一化焦距标定精度和空间特征点个数这五个因素作为影响测量精度的主要因素；每个因素均选取三个典型的水平，采用l
18
(35)的正交表；
[0034]
步骤3.2、明确产品质量特性稳健性指标；
[0035]
以位姿参数的误差(δa
x
、δa
y
、δa
z
、δp
x
、δp
y
、δp
z
)分别作为产品质量特性稳健性指标；其中δa
x
、δa
y
、δa
z
分别是x、y、z三个方向的旋转角度误差；δp
x
、δp
y
、δp
z
分别是x、y、z三个方向的位置误差；
[0036]
步骤3.3、信噪比分析；
[0037]
基于l
18
(35)的正交表，建立18组仿真试验，利用18组仿真试验的结果计算不同位姿参数误差的信噪比；
[0038]
步骤3.4、计算信噪比的极差；
[0039]
对于步骤3.3中计算所得的不同位姿参数误差的信噪比，计算其极差，极差越大，
说明该影响因素的影响水平就越高，影响力就越大；
[0040]
步骤3.5、计算各个影响因素对各个位姿参数的影响力排名；
[0041]
基于步骤3.4中极差计算结果，计算每个影响因素对各个位姿参数的影响力排名，明确对于不同位姿参数，影响其精度的首要影响因素。
[0042]
本发明的有益效果是：
[0043]
1、本发明利用激光参照点作为单目视觉位姿测量方法中的空间特征点，对传统正交迭代算法进行改进，将传统正交迭代算法中每一次的迭代过程需要分别计算一次旋转矩阵r和平移向量改进为每一次的迭代过程只需要迭代更新旋转矩阵r，不需要在每次迭代中均求解平移向量的，换言之，只需最后一次迭代后输出平移向量的最优解即可。因此，在迭代过程中可通过消除的中间值来减少迭代过程中的计算量，进而提升运算效率。
[0044]
2、本发明方法使用平行透视模型进行旋转矩阵初始值的求解。理论上，旋转矩阵初始值r0可随意选定，然而旋转矩阵初始值r0的选择对算法的运行效率影响极大，旋转矩阵初始值r0选取不当时常常使得算法计算量极大，耗时较长。通过平行透视模型求解旋转矩阵初始值，进一步提升运算效率。
附图说明
[0045]
图1为本发明方法流程图；
[0046]
图2为正交迭代算法原理图。
具体实施方式
[0047]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，其中所述仅是本发明的一种实施方案而已，并非对本发明做任何形式的限制，故依据本发明的技术实质对以上实施方案所作的任何简单修改，等同变化或修改，均仍属于本发明技术方案的范围内。
[0048]
如图1所示为本发明的基本工作流程，具体实现包括以下步骤：
[0049]
步骤1、利用激光器照射目标，在目标表面形成激光参照点，将激光参照点作为空间特征点，对激光参照点成像，采用harris角点提取算法提取激光参照点所成像点在摄像机像素坐标系中的坐标。
[0050]
步骤1.1、首先记录激光参照点的空间三维坐标；
[0051]
步骤1.2、对激光参照点成像照片进行预处理，预处理主要包括彩色图像灰度化处理、图像滤波、直方图均衡化、边缘锐化和图像去噪处理。
[0052]
步骤1.3、采用基于harris角点提取算法进行角点检测，提取图像中每个激光参照点所成像点的图像像素坐标。
[0053]
步骤2、构造物方共线误差，并以物方共线误差的最小值作为目标函数，对位姿参数旋转矩阵进行迭代求解。
[0054]
步骤2.1、使用平行透视模型求解旋转矩阵初始值。
[0055]
理论上，旋转矩阵初始值r0可随意选定，然而旋转矩阵初始值r0的选择对算法的运行效率影响极大，旋转矩阵初始值r0选取不当时常常使得算法计算量极大，耗时较长。本方法中，使用平行透视模型进行旋转矩阵初始值的求解。
[0056]
在平行透视模型下，投影过程分为两步：第一步还是将物体平行投影到过质心且与像平面平行的平面上，但此时的投影线不平行于光轴，而平行于摄像机光心和质心的连线。平行透视模型可表示为：
[0057][0058]
其中，(x0,y0,z0)为质心坐标。平行透视模型的矩阵表示为：
[0059][0060]
其中
[0061][0062]
称为平行透视投影矩阵。
[0063]
设三维空间坐标为p的实际坐标为(x,y,z)
t
＝(x0 δx,y0 δy,z0 δz)
t
，平行透视模型成像误差i
err
可由泰勒公式得到：
[0064][0065]
式(4)表明，平行透视模型成像误差为三维点坐标的二阶无穷小。而弱视模型下像点误差为三维点坐标的一阶无穷小。因此，在本发明中，采用平行透视模型进行旋转矩阵初始值的求解。
[0066]
设三维空间点集{p
i
}的齐次坐标为p
i
＝(x
i
,y
i
,z
i
,1)
t
，对应的像点集{p
i
}的齐次坐标为p
i
＝(x
i
,y
i
,1)
t
，三维空间点集{p
i
}的质心齐次坐标为质心像点齐次坐标为(x0,y0,1)
t
，则旋转矩阵初始值r0为：
[0067][0068]
其中，和分别表示和对应的反对称矩阵。和分别为三维列向量，其结果可由下式确定：
[0069][0070]
由此可通过式(6)得到正交迭代算法中的旋转矩阵初始值r0。
[0071]
步骤2.2、假设目标坐标系中有n个激光参照点，其中第i个激光参照点在目标坐标系下的坐标为对应的归一化像点齐次坐标为则像点的视线投影矩阵为：
[0072][0073]
其中，w
i
称为视线投影矩阵，视线指的是从光心到像点的射线。激光参照点经视线投影矩阵作用后，即得到其在对应图像点视线上的投影点。
[0074]
步骤2.3、构造物方共线误差；
[0075]
标准正交迭代算法的基本原理就是空间特征点应与其在对应图像点视线上的投影点重合，其中关系可由目标空间共线方程描述：
[0076][0077]
其中r和即为摄像机在目标坐标系下的位姿，r为旋转矩阵，为平移向量。
[0078]
由式(8)，可构造物方共线误差：
[0079][0080]
本发明用公式10求解公式9中平移向量
[0081][0082]
其中i为单位矩阵。
[0083]
步骤2.4、返回步骤2.3，利用奇异值分解方法(svd)获得绝对定向问题的最优解进而迭代更新r，直至满足迭代终止条件，如图2；该实施例中迭代终止条件设定为物方共线误差的相对变化量小于提前设定的阈值。具体而言，在本实施例中，如果连续五次迭代中对应的物方共线误差相对变化量小于0.001mm，则意味着迭代已经稳定，整个迭代过程终止。
[0084]
步骤2.5、以迭代过程中的式(9)为基础，可得到位姿估计的目标函数：
[0085][0086]
对于式(11)，若已知旋转矩阵r，则平移向量的最优解为：
[0087][0088]
这是一个单变量的最小化寻优问题。可利用迭代法求解的最优值。迭代寻优初始值为r0，利用奇异值分解方法(svd)获得绝对定向问题的最优解进而更新r，这样一个过程就能完成r和的不断迭代优化。
[0089]
传统正交迭代算法每一次的迭代过程需要分别计算一次旋转矩阵r和平移向量但实际上，每一次更新r后，均可线性求解出最优因此，每轮迭代其实是对旋转矩阵r的迭代，不需要在每次迭代中均求解平移向量的，换言之，只需最后一次迭代后输出平移向量的最优解即可。因此，本发明在迭代过程中可通过消除的中间值来减少迭代过程中的计算量，进而提升运算效率。
[0090]
步骤3、基于正交仿真试验，对上述位姿测量方法的精度进行定量分析。
[0091]
步骤3.1、设计正交试验。
[0092]
田口方法由日本著名质量管理专家田口玄一博士创立，田口方法通过调整设计参数，使产品的功能、性能对偏差的起因不敏感，以提高产品自身的抗干扰能力。为了定量描述产品质量损失，田口提出了“质量损失函数”的概念，并以信噪比来衡量设计参数的稳健程度。田口方法强调产品的设计都必须经过系统设计，参数设计和容差设计三个阶段，参数设计是其核心过程，其主要特点是引进质量损失函数，并将其转化为信噪比，以正交实验设计为基础，然后通过对实验方案的统计分析，找出各参数值的最佳水平组合，从而提高产品的性能和质量。由于正交实验具有“均匀分散性、整齐可比”的特点，所以应用正交表来安排实验具有代表性，能够比较全面地反映各因素各水平对指标影响的大致情况。
[0093]
表1各几何因素编号及水平数
[0094][0095]
影响上述位姿测量算法的因素主要包括空间特征点图像坐标(空间特征点对应像点在摄像机像素坐标系中的坐标)提取精度、空间特征点空间坐标精度、摄像机主点标定精度、归一化焦距标定精度和空间特征点个数这五个因素，本发明以田口方法思想和仿真分析的结果为基础，通过合理设计正交实验，定量研究空间特征点图像坐标提取精度、空间特征点空间坐标精度、摄像机主点标定精度、归一化焦距标定精度、空间特征点个数对最终位姿测量结果的影响。各个因素的水平数见表1所示，每个因素均选取了三个典型的水平，若
一一计算每种组合，则一共有35＝243种不同的组合方式，逐个计算将是一项十分耗时的工作，如表2所示，本发明中采用l
18
(35)的正交表，只需计算18种组合，从而极大的加速了分析过程。
[0096]
表2 l
18
(35)正交表
[0097][0098][0099]
步骤3.2、明确产品质量特性稳健性指标。
[0100]
对于空间目标的位姿测量问题而言，位姿测量结果误差越小越好，因此，在本发明中，以位姿测量结果的误差(δa
x
、δa
y
、δa
z
、δp
x
、δp
y
、δp
z
)分别作为产品质量特性稳健性指标(即算法精度定量分析的目标函数)。
[0101]
步骤3.3、信噪比分析。
[0102]
田口方法用信噪比(snr)来衡量产品质量稳定性。其分析过程是通过使用正交试验表，以信噪比作为产品稳健性的评价指标，帮助研究者发现对于每种控制因素，哪种水平的影响是更有效的。信噪比分析过程是通过使用正交试验表，以信噪比作为产品稳健性的评价指标，帮助研究者发现对于每种控制因素，哪种水平的影响是更有效的。在本发明中，基于表2建立了18组仿真试验，18组仿真实验的结果如表3所示。
[0103]
表3仿真实验的结果
[0104]
[0105][0106]
基于表3中所示的18组仿真试验结果计算不同位姿参数误差的信噪比。下面以计算δp
z
的信噪比snr
δpz
为例来说明信噪比的计算过程。
[0107]
δp
z
的信噪比可由下式得出：
[0108][0109]
其中，n表示试验重复进行的次数。由于仿真试验的结果本身就是100次试验的均方值，因此n取1。基于式(13)，得到18组仿真试验中δp
z
的信噪比snr
δpz
，结果如表4最后一列所示，其它位姿参数误差(δa
x
、δa
y
、δa
z
、δp
x
、δp
y
)的信噪比可以同理得出，结果也展示在表4中。
[0110]
表4各个位姿参数误差的信噪比
[0111]
[0112][0113]
步骤3.4、计算信噪比的极差。对于步骤3.3中计算所得的不同位姿参数误差的信噪比，计算其极差，极差越大，说明该影响因素的影响水平就越高，影响力就越大。
[0114]
具体而言，如表5所示，t1行的第一个数字是当a因素处在第一水平时的6组试验所对应的信噪比之和。同理可得t1‑
t3行其它的数字。信噪比值的极差指的是t1‑
t3中最大值与最小值之间的差值，极差越大，说明该因子的影响水平就越高。
[0115]
表5各个参数对δp
z
的贡献率分析
[0116][0117]
步骤3.5、计算各个影响因素的影响力排名。基于步骤3.4中极差计算结果，计算每个影响因素的影响力排名，明确对于不同位姿参数，影响其精度的首要影响因素。
[0118]
每个几何因素的影响贡献率及其排名按如下方法得到：
[0119]
r
i
＝maxt
j
‑
mint
j
(j＝1,2,3)(14)
[0120][0121]
使用同样的方法对其它五个位姿测量误差(δa
x
、δa
y
、δa
z
、δp
x
、δp
y
)的信噪比以及各个因素对其影响进行了计算，由于计算过程类似，在此不再赘述。
[0122]
依据本发明提出的位姿测量算法精度定量计算方法，可以明确影响每个位姿参数的首要因素，便于针对不同的空间位姿测量任务的需求，对位姿测量结果进行针对性的优化。