CN105825505B - 一种面向硬式空中加油的视觉测量方法 - Google Patents

Abstract

本发明提出了一种面向硬式空中加油的视觉测量方法，一种面向硬式空中加油的视觉测量方法，该方法的步骤如下：步骤一：基于发光二极管(LED)的受油口目标识别；步骤二：LED标志点中心点匹配；步骤三：利用高斯最小二乘微分校正算法对受油口相对相机的平移矩阵和旋转矩阵进行求解。本方法利用视觉传感器，对受油口位姿进行测量，利用测量得到的位姿关系对受油机以及加油杆实现精确控制，实现加油对接。本方法鲁棒性好，精确性高，可大大提高硬式空中加油的安全性和可靠性。

Description

一种面向硬式空中加油的视觉测量方法

【技术领域】

本发明是一种面向硬式空中加油的视觉测量方法，属于计算机视觉测量技术领域。

【背景技术】

空中加油是指在飞行过程中一架飞机向另一架或多架飞机(或直升机)传输燃油的活动。空中加油自出现起，就以其在军事行动中的重要作用而日益受到各国的广泛重视。长航时无人机进行一次空中加油，其续航时间能增加80％以上，这也为携带更多任务载荷提供了可能；无人机进行一次空中加油，航时可提高30％～40％，从而为解决燃油与武器装载之间的矛盾提供了有效途径。

空中加油技术是提高飞机远程作战效能的关键技术之一，是增强战斗力的放大器。空中加油不仅改变了以往的作战模式，而且还可以提高战绩的综合作战效能，已成为先进军用飞机完成作战任务不可缺少的保障措施。相对于软管式加油，硬管式空中加油因为加油量大、受油机操作负担轻、受油机受油设备简单等特点而具有明显的优势，因而成为未来空中加油技术的发展方向。

目前，空中加油技术中发展比较成熟并被广泛采用的空中加油系统可分为下属四种：一种是插头-锥套式(软管)加油系统，另一种是飞桁式(硬管)加油系统，还有两种方式是将上述两种系统结合起来的混合式及多系统式。

自上世纪五十年代，人工操纵的空中加油技术已经比较成熟，但人工操纵的空中加油技术效率较低、对飞行员的驾驶技术要求很高，易受到驾驶员心理和生理以及技术战术状态的影响。因此开展自主空中加油技术的研究具有重要的意义。20世纪80年代以来，伴随着神经科学、计算神经科学、解剖学的发展，人类对脑科学有了更加深入的了解。在所感知的外部世界信息中，大约80％的部分来自视觉，这种客观的事实使得科学家对视觉信息的各种处理机制进行了非常深入的研究，并取得了一系列的视觉信息处理理论成果。这些成果的取得，使得计算机视觉科研人员有条件从事模拟生物视觉功能的研究，也为改善目前的机器视觉识别性能提供了生物学上的依据。机器(计算机)视觉是指用计算机实现类生物的视觉功能，即对客观世界三维场景的感知、识别、理解。因此，机器视觉是计算机科学和智能科学交叉而形成的学科，是以模拟生物视觉功能或信息处理机制为主要研究内容，构建仿生视觉的计算模型，利用计算机对描述自然景物的图像数据进行处理，以实现类似于生物视觉感知功能为目标。

相对于其它导航方式，视觉传感器具有轻便、低功耗、体积小、重量轻等优点，同时能够提供丰富的周围环境信息。

【发明内容】

1、发明目的：

本发明提出了一种面向硬式空中加油的视觉测量方法，其目的是提供一种更具实用价值的硬式空中加油测量方法，为自主空中加油系统提供精确可靠的测量信息，革新当前的人工操纵加油技术，提高空中加油效率，最大程度避免因操作员误操作造成的机毁人亡的重大事故。

2、技术方案：

本发明针对硬式空中加油需求，提出了一种鲁棒性强、精确度高的视觉测量方法，前期工作需要对相机的参数进行确定和对空中硬式加油视觉测量图像的获取。该方法通过对图像序列进行图像处理操作解算受油口相对相机的平移矩阵和旋转矩阵。图像序列是通过可见光相机拍摄获取的，相机的内参是解算位姿关系时的必备参数，相机参数可以通过张正友标定法确定。测量方法步骤构成见附图1所示。

一种面向硬式空中加油的视觉测量方法，该方法的步骤如下：

步骤一：基于发光二极管(LED)的受油口目标识别

在视觉测量时，需要对受油口进行识别，在受油口附近设置7个红色LED光标记点的方法，利用计算机视觉实现对光标记点的检测识别。

(1)将图像序列进行彩色空间(RGB)到色相、饱和度和明度(HSV)化处理：

通过相机获取光标记点的位图像，将光标记点设置成红色，利用这个颜色特征信息进行点提取。首先，将相机采集到的RGB图像变换到HSV空间。对于H、S、V三个通道的图像，选取其中光标记点最明显的S通道图像进行中值滤波，并由给定阈值进行分割，得到二值化图像。该二值化图像可能存在噪声，可以用腐蚀，膨胀等形态处理，将该噪声去掉，检测到光斑区域，但光斑的数目及光斑中心点(光标记点)的坐标位置需要精确求解。根据均值平移(meanshift)的聚类思想，采用mean shift聚类算法求解这些光标记点的图像坐标，为最后受油口与相机相对位置测量做准备。

色调H表示颜色的色相，用角度来度量，取值范围为0°～360°，从红色才是按逆时针方向计算，红色为0°，绿色为120°，蓝色为240°；饱和度S表示颜色的纯度，其取值范围为0.0～1.0；亮度V表示颜色的亮度，取值范围为0.0(黑色)～1.0(白色)，圆锥的顶面对应于V＝1，代表的颜色最亮。令max为R、G、B三个分量的最大值，min为三个分量的最小值，r、g、b分别表示三个分量的值，RGB到HSV的变换的数学公式如下所示：

V＝max (3)

(2)对颜色空间变换后的图像分别在H、S、V通道进行阈值分割：

由于检测对象是受油口附近红色光标记点，通过RGB颜色空间到HSV空间的变换得到三个通道H、S、V通道图像，其中S通道光标记点最为明显，选择S通道的图像进行特定阈值分割，即可检测到目标光斑。分割得到的二值图可能存在噪声点，可以通过中值滤波及形态学的腐蚀膨胀算子进行处理。

(3)光标记点中心定位：

在经过以上操作之后，能够获取LED标志点在图像中的区域，在这一基础上，还需要确定每个区域的中心位置，也就是确定光标记点的中心。不同于背景像素有很多相似块区域分布在图像各处，显著点往往是聚集在一起。所以可以认为像素块p_i和与其相似的块距离都很近，则p_i是显著的，当相似块都分布在较远区域，则p_i的显著性不高。因此用d_position(p_i,p_j)来度量像素块p_i和p_j之间的距离，即：

式中，d_position(p_i,p_j)是像素块p_i和像素块p_j之间的距离，和表示分别是像素块p_i和像素块p_j的图像坐标。

结合上面两个特征，对于两个像素块，采用一个相异度计算方法：

式中c为系数，d_position(p_i,p_j)含义与上式相同，d_color(p_i,p_j)表示像素块p_i和像素块p_j在颜色空间上的距离。

在计算像素块p_i的显著程度时，选择K个与它最相似的像素块作为参考集，如果它与参考集的差异度很大，则可认为它与图像其他像素块的相异度都很大。因此定义显著性

式中r为尺度。

1)多尺度显著度增强

由于背景像素块在多尺度上有很多相似的块，而显著像素块只在几个尺度上有类似的块，故引入多尺度方法来加强显著区域与非显著区域的对比度。因此修正如下：

其中r_m∈{R|R＝{r₁,r₂...r_M}}，r_m为尺度，M为尺度的数目。

可得出像素p_i的显著度为它在不同尺度上的均值：

2)区域显著值修正

根据三分法(Rule of thirds)特性，可以认为在全局视角上，像素离交点的距离越近，它的显著度越高，所以显著度公式修正为

其中d_foci(i)表示像素块p_i离交点的距离。

由上述方法可以找到显著度最大的像素块，取其中心作为光标记点中心。

步骤二：LED标志点中心点匹配

将块拟合之后的点与标志点的世界坐标进行拟合，计算每个提到的点的图像坐标与所有标志点的世界坐标转换成图像坐标之后的图像坐标之间的欧式距离，与哪个标志点最近，就认为提到的某点为该标志点，从而可以赋予其世界坐标进行后面的计算。

特征点匹配(Point Feature Matching)，即匹配标记连续帧图像对应特征点的位置。因为已经给出了光标记点P_j在图像上的投影方程，假定从摄像机采集图像提取特征点得到的检测点集为{p₁,p₂,…p_n}，其中p_j＝(u_j,v_j)是图像坐标，而投影点集为同时设对应的投影点集为其中是利用针孔投影成像模型计算得到的在像素坐标系中的坐标。

当检测特征点与投影特征点匹配上时，两个点集的欧式距离最小。检测点集{p₁,p₂,…p_n}和投影点集之间的欧式距离矩阵Err如下，其矩阵的维数为m×n。

点集间对应点的匹配问题可以用经典的数学指派问题来描述，可描述为：

假设有两个点集A,B,S_ik表示集合A中的点i与集合B中的点k之间的欧式距离，求解两个点集的最小欧式距离，即可表示为如下公式：

S＝{s_ij|s_ij＝dist(a_i,b_j),a_i∈A,b_j∈B,i＝1…N,j＝1…M} (12)

s_ik、s_ij和dist(a_i,b_j)表示点之间的欧氏距离，x_ik为示性函数，当点匹配上时值为1，否则为0。

求解上述最小欧式距离的解，可利用匈牙利算法Hungarian，限于求解方阵即点数相同的两个点集，以及改进算法Munkres，求解非方阵，即两个点数不一样多的点集。

步骤三：利用高斯最小二乘微分校正算法对受油口相对相机的平移矩阵和旋转矩阵进行求解

(1)相机成像模型及各个坐标系定义说明：

针对硬管式空中加油问题，假定相机放置在加油机机身尾部下方伸缩管附近某一特定位置，并事先已标定好。为了获取受油机相对加油机的位置和姿态信息，可在受油口附近设置多个光标记点，用于对受油机机背上受油口的识别和定位，并由事先标定好的光标记点的相对几何位置关系及摄像机成像系统模型来求解上述位置和姿态信息。

基于视觉的导航系统为加油机和受油机的引导控制系统提供受油机相对加油机的位置和姿态信息，对这些信息的求解首先需要定义加油机机体坐标系、受油机机体坐标系、摄像机成像系统坐标系，及受油机坐标系，通过各个坐标系之间的关系来描述其相对几何位姿关系。各个坐标系的定义如下：

O_b1-x_b1y_b1z_b1：受油机机体坐标系；

O_b1-x_b2y_b2z_b2：加油机机体坐标系

O_c-x_cy_cz_c：安装在加油机上的摄像机(伸缩管附近)成像坐标系

O_d-x_dy_dz_d：受油口(目标)坐标系

O_e-x_ey_ez_e：大地坐标系

视觉测量的主要功能就是求解受油机上的受油口坐标系O_d-x_dy_dz_d的中心O_d到加油机伸缩管附近摄像机坐标系中心O_c的距离d_D，最后转换为受油机和加油机质心间的相对距离和姿态。

加油机上的相机成像原理可采用针孔摄像机数学模型来描述。为了从代数上描述上述几何投影关系，需要建立摄像机坐标系和图像平面坐标系。空间中某一三维空间点成像到图像平面之间的成像过程如附图2所示。

图中摄像机坐标表示为O_c-x_cy_cz_c，摄像机的焦距为f。空间点P_c在摄像机坐标系中的欧氏坐标记为(x_c,y_c,z_c)，它的像点p在图像坐标系中的坐标记为(x,y)，根据三角关系，得到下述关系：

上式可表述为：

而图像坐标系与图像物理坐标系的关系如附图3所示。

图像物理坐标系以摄像机光轴与像平面的交点为原点，以毫米为单位。假设每一个像素在x轴与y轴方向的物理尺寸为dx.dy，图像物理坐标系原点在图像坐标系中坐标为(u₀,v₀)，则在图像坐标系上的点(u,v)与图像物理坐标系中对应点(x,y)的转换关系可表示为：

为了使用方便，用齐次坐标与矩形形式来表示：

而摄像机坐标(x_c,y_c,z_c)同受油口坐标(x_d,y_d,z_d)的坐标转换是两个三维坐标系之间的变换，可以用一个旋转矩阵R与一个位移矩阵t来描述，转换关系的数学表达式如下：

(2)基于高斯最小二乘微分校正算法的位姿测量

高斯最小二乘微分校正算法将高斯-牛顿法应用于表示估计点与检测点之间偏差非线性代价函数的最小化上。选择这种算法是因为它代表了一类广泛用于摄影测量的算法。高斯最小二乘微分校正算法结构简单，实时性好，已经被应用于多种场景下的位姿估计问题。

设图像序列在k时刻检测、标记的特征点j的坐标是[u_j v_j 1]^T，旋转矩阵R和位移矩阵t均为未知向量X(k)的估计值的函数，利用将第j个坐标点投影到成像平面，记作重新安排特征点坐标，得到：

式中前三个参数分别是偏航角、俯仰角、滚转角，后三个参数为相对位移在x,y,z方向上的投影。

k时刻处理算法提取达到的像素点集记为G₀(k)，视觉估计误差ΔG(k)定义为：

G₀(k)＝[u₁,v₁,……v_m] (17)

高斯最小二乘微分校正算法迭代更新迭代公式为：

其中，i为迭代次数，W(k)为控制矩阵，为R_i(k)的逆，为A_i(k)的转置。R_i(k)和A_i(k)定义如下：

为偏导数符号。

迭代过程结束后，即估计点和检测点的位置偏差充分小时，可得出估计值的最优解，即解算出旋转矩阵R和位移矩阵t，从而估计出飞机的位姿。

3、优点及效果：

本发明提出了一种面向硬式空中加油的视觉测量方法，提出了从目标识别，到特征点匹配，再到位姿测量的一整套测量方案，其目的是提供一种具有实用价值的硬式空中加油测量方法。本方法利用视觉传感器，对受油口位姿进行测量，利用测量得到的位姿关系对受油机以及加油杆实现精确控制，实现加油对接。本方法鲁棒性好，精确性高，可大大提高硬式空中加油的安全性和可靠性。

【附图说明】

图1为面向硬式空中加油的视觉测量系统示意图。

图2为摄像机成像模型示意图。

图3为图像坐标系与图像物理坐标系关系图。

图4为硬式空中加油受油口示意图。

图5(a)、图5(b)和图5(c)为姿态角变化曲线。

图6(a)、图6(b)和图6(c)为位移变化曲线。

图7为重投影误差曲线。

图中标号及符号说明如下：

R——旋转矩阵；

t——位移矩阵；

Y——满足条件(是)；

O_wX_wY_wZ_w——世界坐标系；O_cX_cY_cZ_c——摄像机坐标系；

O₀UV——图像坐标系；OXY——图像物理坐标系。

【具体实施方式】

下面通过一个具体的视觉测量实例来验证本发明所提出的设计方法的有效性。本实例中使用两架无人机做测试试验，一架无人机作为加油机，一架作为受油机。受油机上安装如图4所示的3D打印受油口模型作为测量对象，受油口周围的7个凸台涂上红色作为测量标志点，在加油机上安装工业相机作为视觉传感器获取受油口的可见光图像，使用机载微型电脑搭建的视觉测量系统对获取图像序列进行处理。实例选取任意一帧图像作为说明。实验计算机配置为i5-4210M处理器，2.60Ghz主频，4G内存，软件为visual studio 2010版本。

本实例的具体实现步骤如下：

前期工作：相机的参数确定和空中硬式加油视觉测量图像获取

通过张正友标定法确定公式(16)中的相机参数矩阵为

然后用标定好的相机拍摄，获取加油相机序列，进行下一步图像处理。

步骤一：基于发光二极管(LED)的受油口目标识别

本实例中，采用在受油口附近设置7个光标记点的方法，利用计算机视觉实现对光标记点的检测识别。

通过摄像机获取光标记点图像，利用红色标记点的颜色特征信息进行点的提取。首先，进行颜色空间变换，摄像头采集图像进行彩色空间(RGB)到色相、饱和度和明度(HSV)化处理，得到H、S、V三个通道的图像。RGB到HSV的变换数学公式见公式(1)-(3)。

然后对变换之后的图像进行阈值分割处理。选取其中光标记点最明显的S通道图像进行中值滤波，并由给定阈值进行分割，得到二值化图像。

该二值化图像可能存在噪声，可以用腐蚀，膨胀等形态处理，将该噪声去掉，检测到光斑区域，但光斑的数目及光斑中心点(光标记点)的坐标位置需要精确求解。根据meanshift的聚类思想，采用mean shift聚类算法求解这些光标记点的图像坐标，为最后受油口与摄像机相对位置测量做准备。

最后，由公式(4)-(9)计算出光标记点区域每个像素在不同尺度上的显著度均值，由此得到每个光标记点区域的中心位置。每个标记点中心的图像坐标为：

步骤二：LED标志点中心点匹配

在得到每个光标记点区域的中心坐标之后，需要将其与标志点的世界坐标进行拟合才可以进行位姿的解算。

利用公式(10)计算检测点集{p₁,p₂,…p_n}和投影点集之间的欧式距离矩阵Err，利用公式(11)得到两点集的欧式距离。再利用munkres算法，求解匹配结果。匹配矩阵为：assign＝[1,2,3,4,5,6,7]^T。

步骤三：利用高斯最小二乘微分校正算法对受油口相对相机的平移矩阵和旋转矩阵进行求解

要得到图像坐标与摄像机坐标系的关系，首先需要知道相机参数，根据选用的相机型号，公式16中的相机内参矩阵为

从公式(17)可以看出，对受油口位姿进行精确测量，实际上就是求解公式中的R和T矩阵，设图像序列在k时刻检测、标记的特征点j的坐标是[u_j v_j 1]^T，参数矩阵R和T为未知向量X(k)的估计值的函数，利用将第j个坐标点投影到成像平面，记作利用公式(15)-(20)，设置迭代最小偏差为1×10^-5，最大迭代次数为50，迭代求解出受油口位姿。求解出该帧的位移矩阵为：位姿角矩阵为利用计算得到的位姿关系，对7个标志点进行重投影，与P_i矩阵做差，得到重投影误差为0.07。

接下来对连续图像序列中的多幅图像进行位姿求解，求解出的三个姿态角变化曲线见附图5(a)、图5(b)和图5(c)所示，三个方向的位移见附图6(a)、图6(b)和图6(c)所示，根据解算的结果通过重投影反解回标志点得到的中心点象素与步骤一得到的象素做差，可计算得到重投影误差，误差曲线见附图7。通过试验结果可以发现，本发明能够对硬式空中加油模拟受油口的位姿进行精确测量。

Claims (1)

1.一种面向硬式空中加油的视觉测量方法，其特征在于，该方法的步骤如下：

步骤一：基于发光二极管LED的受油口目标识别

在视觉测量时，需要对受油口进行识别，在受油口附近设置7个红色LED光标记点，利用计算机视觉实现对光标记点的检测识别；

1.1将图像序列进行彩色空间RGB到色相、饱和度和明度HSV化处理：

通过相机获取光标记点的位图像，将光标记点设置成红色，利用这个颜色特征信息进行点提取；首先，将相机采集到的RGB图像变换到HSV空间；对于H、S、V三个通道的图像，选取其中光标记点最明显的S通道图像进行中值滤波，并由给定阈值进行分割，得到二值化图像；该二值化图像存在噪声，用腐蚀、膨胀的形态处理，将该噪声去掉，检测到光斑区域，但光斑的数目及光斑中心点的坐标位置需要精确求解；根据均值平移mean shift的聚类思想，采用mean shift聚类算法求解这些光标记点的图像坐标，为最后受油口与相机相对位置测量做准备；

色调H表示颜色的色相，用角度来度量，取值范围为0°～360°，从红色才是按逆时针方向计算，红色为0°，绿色为120°，蓝色为240°；饱和度S表示颜色的纯度，其取值范围为0.0～1.0；亮度V表示颜色的亮度，取值范围为0.0黑色～1.0白色，圆锥的顶面对应于V＝1，代表的颜色最亮；令max为R、G、B三个分量的最大值，min为三个分量的最小值，r、g、b分别表示三个分量的值，RGB到HSV的变换的数学公式如下所示：

$S=\left\{\begin{array}{cc}0,& ifmax=0\\ \frac{\max -\min }{\max }=1-\frac{\min }{\max },& otherwise\end{array}\right.---\left(2\right)$

V＝max (3)

1.2对颜色空间变换后的图像分别在H、S、V通道进行阈值分割：

由于检测对象是受油口附近红色光标记点，通过RGB颜色空间到HSV空间的变换得到三个通道H、S、V通道图像，其中S通道光标记点最为明显，选择S通道的图像进行特定阈值分割，检测到目标光斑；分割得到的二值图存在噪声点，通过中值滤波及形态学的腐蚀膨胀算子进行处理；

1.3光标记点中心定位：

在经过以上操作之后，能够获取LED标志点在图像中的区域，在这一基础上，还需要确定每个区域的中心位置，也就是确定光标记点的中心；不同于背景像素有很多相似块区域分布在图像各处，显著点往往是聚集在一起；所以认为像素块p_i和与其相似的块距离都很近，则p_i是显著的，当相似块都分布在较远区域，则p_i的显著性不高；因此用d_position(p_i,p_j)来度量像素块p_i和p_j之间的距离：

$d_{\mathrm{position}}(p_i,p_j)=\left|\right|{\mathrm{position}}_{p_i}-{\mathrm{position}}_{p_j}\left|\right|---\left(4\right)$

式中，d_position(p_i,p_j)是像素块p_i和像素块p_j之间的距离，和表示分别是像素块p_i和像素块p_j的图像坐标；

结合上面两个特征，对于两个像素块，采用一个相异度计算方法：

$d(p_i,p_j)=\frac{\log (1+d_{color}(p_i,p_j))}{1+c\·d_{position}(p_i,p_j)}---\left(5\right)$

式中c为系数，d_position(p_i,p_j)含义与上式相同，d_color(p_i,p_j)表示像素块p_i和像素块p_j在颜色空间上的距离；

在计算像素块p_i的显著程度时，选择K个与它最相似的像素块作为参考集，如果它与参考集的差异度很大，则认为它与图像其他像素块的相异度都很大；因此定义显著性

$S_i^r=1-\exp \{-\frac{1}{K}\underset{k=1}{\overset{K}{\Σ}}d(p_i^r,q_k^r)\}---\left(6\right)$

式中r为尺度；

1.31多尺度显著度增强

由于背景像素块在多尺度上有很多相似的块，而显著像素块只在几个尺度上有类似的块，故引入多尺度方法来加强显著区域与非显著区域的对比度；因此修正如下：

$S_i^r=1-\exp \{-\frac{1}{K}\underset{k=1}{\overset{K}{\Σ}}d(p_i^r,q_k^{r_m})\}---\left(7\right)$

其中r_m∈{R|R＝{r₁,r₂...r_M}}，r_m为尺度，M为尺度的数目；

得出像素p_i的显著度为它在不同尺度上的均值：

${\stackrel{\‾}{S}}_i=\frac{1}{M}\underset{r_m}{\Σ}{S}_i^{r_m}---\left(8\right)$

1.32区域显著值修正

根据三分法的特性，认为在全局视角上，像素离交点的距离越近，它的显著度越高，所以显著度公式修正为

${\hat{S}}_i={\stackrel{\‾}{S}}_i(1-d_{foci}(i\left)\right)---\left(9\right)$

其中d_foci(i)表示像素块p_i离交点的距离；

因此能找到显著度最大的像素块，取其中心作为光标记点中心；

步骤二：LED标志点中心点匹配

将块拟合之后的点与标志点的世界坐标进行拟合，计算每个提到的点的图像坐标与所有标志点的世界坐标转换成图像坐标之后的图像坐标之间的欧式距离，与哪个标志点最近，就认为提到的某点为该标志点，从而赋予其世界坐标进行后面的计算；

特征点匹配是匹配标记连续帧图像对应特征点的位置；因为已经给出了光标记点P_j在图像上的投影方程，假定从摄像机采集图像提取特征点得到的检测点集为{p₁,p₂,...p_n}，其中p_j＝(u_j,v_j)是图像坐标，而投影点集为同时设对应的投影点集为其中是利用针孔投影成像模型计算得到的在像素坐标系中的坐标；

当检测特征点与投影特征点匹配上时，两个点集的欧式距离最小；检测点集{p₁,p₂,...p_n}和投影点集之间的欧式距离矩阵Err如下，其矩阵的维数为m×n；

$Err=\left[\begin{array}{cccc}d({\hat{p}}_1,p_1)& d({\hat{p}}_1,p_2)& ...& d({\hat{p}}_1,p_n)\\ d({\hat{p}}_2,p_1)& d({\hat{p}}_2,p_2)& ...& d({\hat{p}}_2,p_n)\\ \·& \·& \·& \·\\ \·& \·& \·& \·\\ \·& \·& \·& \·\\ d({\hat{p}}_m,p_1)& d({\hat{p}}_m,p_2)& ...& d({\hat{p}}_m,p_n)\end{array}\right]---\left(10\right)$

点集间对应点的匹配问题用数学指派问题来描述，描述为：

假设有两个点集A,B,S_ik表示集合A中的点i与集合B中的点k之间的欧式距离，求解两个点集的最小欧式距离，表示为如下公式：

$\left\{\begin{array}{c}{\mathrm{min\Σ}}_{i=1}^N{\mathrm{\Σ}}_{k=1}^N{s}_{ik}{x}_{ik}\\ {\mathrm{\Σ}}_{i=1}^N{x}_{ik}=1,k=1...N\\ {\mathrm{\Σ}}_{i=1}^N{x}_{ik}=1,i=1...N\\ S_{ik}>0\\ x_{ik}\∈\{0,1\}\end{array}\right.---\left(11\right)$

S＝{s_ij|s_ij＝dist(a_i,b_j),a_i∈A,b_j∈B,i＝1...N,j＝1...M} (12)

s_ik、s_ij和dist(a_i,b_j)表示点之间的欧氏距离，x_ik为示性函数，当点匹配上时值为1，否则为0；

求解上述最小欧式距离的解，利用匈牙利算法Hungarian，限于求解方阵点数相同的两个点集，以及改进算法Munkres，求解非方阵，是两个点数不一样多的点集；

步骤三：利用高斯最小二乘微分校正算法对受油口相对相机的平移矩阵和旋转矩阵进行求解

3.1相机成像模型及各个坐标系定义说明：

针对硬管式空中加油问题，假定相机放置在加油机机身尾部下方伸缩管附近某一特定位置，并事先已标定好；为了获取受油机相对加油机的位置和姿态信息，在受油口附近设置多个光标记点，用于对受油机机背上受油口的识别和定位，并由事先标定好的光标记点的相对几何位置关系及摄像机成像系统模型来求解上述位置和姿态信息；

基于视觉的导航系统为加油机和受油机的引导控制系统提供受油机相对加油机的位置和姿态信息，对这些信息的求解首先需要定义加油机机体坐标系、受油机机体坐标系、摄像机成像系统坐标系，及受油机坐标系，通过各个坐标系之间的关系来描述其相对几何位姿关系；各个坐标系的定义如下：

O_b1-x_b1y_b1z_b1：受油机机体坐标系；

O_b1-x_b2y_b2z_b2：加油机机体坐标系；

O_c-x_cy_cz_c：安装在加油机上的摄像机成像坐标系；

O_d-x_dy_dz_d：受油口坐标系；

O_e-x_ey_ez_e：大地坐标系；

视觉测量的功能就是求解受油机上的受油口坐标系O_d-x_dy_dz_d的中心O_d到加油机伸缩管附近摄像机坐标系中心O_c的距离d_D，最后转换为受油机和加油机质心间的相对距离和姿态；

加油机上的相机成像原理采用针孔摄像机数学模型来描述；为了从代数上描述上述几何投影关系，需要建立摄像机坐标系和图像平面坐标系；

设摄像机坐标表示为O_c-x_cy_cz_c，摄像机的焦距为f；空间点P_c在摄像机坐标系中的欧氏坐标记为(x_c,y_c,z_c)，它的像点p在图像坐标系中的坐标记为(x,y)，根据三角关系，得到下述关系：

$\left\{\begin{array}{c}x=\frac{f}{z_c}{x}_c\\ y=\frac{f}{z_c}{y}_c\end{array}\right.---\left(13\right)$

上式表述为：

$z_c\left(\begin{array}{c}x\\ y\\ 1\end{array}\right)=\left(\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right)\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right)---\left(14\right)$

图像物理坐标系以摄像机光轴与像平面的交点为原点，以毫米为单位；假设每一个像素在x轴与y轴方向的物理尺寸为dx.dy，图像物理坐标系原点在图像坐标系中坐标为(u₀,v₀)，则在图像坐标系上的点(u,v)与图像物理坐标系中对应点(x,y)的转换关系表示为：

$\left\{\begin{array}{c}u=\frac{x}{dx}+u_0\\ v=\frac{y}{dy}+v_0\end{array}\right.---\left(15\right)$

为了使用方便，用齐次坐标与矩形形式来表示：

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}\frac{1}{dx}& 0& u_o\\ 0& \frac{1}{dy}& v_o\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}x\\ y\\ 1\end{array}\right)---\left(16\right)$

而摄像机坐标(x_c,y_c,z_c)同受油口坐标(x_d,y_d,z_d)的坐标转换是两个三维坐标系之间的变换，用一个旋转矩阵R与一个位移矩阵t来描述，转换关系的数学表达式如下：

$\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right)=\left(\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right)\left(\begin{array}{c}{x}_d\\ y_d\\ z_d\\ 1\end{array}\right)---\left(17\right)$

3.2基于高斯最小二乘微分校正算法的位姿测量

高斯最小二乘微分校正算法将高斯-牛顿法应用于表示估计点与检测点之间偏差非线性代价函数的最小化上；

设图像序列在k时刻检测、标记的特征点j的坐标是[u_j v_j 1]^T，旋转矩阵R和位移矩阵t均为未知向量X(k)的估计值的函数，利用将第j个坐标点投影到成像平面，记作重新安排特征点坐标，得到：

$G\left(\stackrel{\‾}{X}\right(K\left)\right)=\[{\stackrel{\‾}{u}}_1,{\stackrel{\‾}{v}}_1,......,{\stackrel{\‾}{u}}_m,{\stackrel{\‾}{v}}_m\]---\left(15\right)$

式中前三个参数分别是偏航角、俯仰角、滚转角，后三个参数为相对位移在x,y,z方向上的投影；

k时刻处理算法提取达到的像素点集记为G₀(k)，视觉估计误差ΔG(k)定义为：

$\mathrm{\Δ}G\left(k\right)=G_0\left(k\right)-G\left(\stackrel{\‾}{X}\right(k\left)\right)---\left(16\right)$

G₀(k)＝[u₁,v₁,......v_m] (17)

高斯最小二乘微分校正算法迭代更新迭代公式为：

${\stackrel{\‾}{X}}_{i+1}\left(k\right)={\stackrel{\‾}{X}}_i\left(k\right)+R_i^{-1}\left(k\right)A_i^T\left(k\right)W\left(k\right){\mathrm{\ΔG}}_i\left(k\right)---\left(18\right)$

其中，i为迭代次数，W(k)为控制矩阵，为R_i(k)的逆，为A_i(k)的转置；R_i(k)和A_i(k)定义如下：

$R_i\left(k\right)=A_i^T\left(k\right)W\left(k\right)A_i\left(k\right)---\left(19\right)$

$A_i\left(k\right)=\frac{\∂G_i\left(k\right)}{\∂X}{|}_{X=\tilde{X}\left(k\right)}---\left(20\right)$

为偏导数符号；

迭代过程结束后，估计点和检测点的位置偏差充分小时，得出估计值的最优解，解算出旋转矩阵R和位移矩阵t，从而估计出飞机的位姿。