CN104180808B - 一种用于自主空中加油的圆形锥套视觉位姿解算方法 - Google Patents

Abstract

本发明公开了一种用于自主空中加油的圆形锥套视觉位姿解算方法，该方法利用加油锥套的结构特点，先建立原点为摄像机光心、XY平面与加油口平面平行的世界坐标系，然后分别在摄像机坐标系和世界坐标系下建立圆形投影锥面方程，并根据“旋转变换不改变系数矩阵特征值”这一特性，计算世界坐标系下圆形投影锥面方程系数矩阵中的未知参数，进而可求解受油插头和加油锥套之间的位置和姿态信息。该方法思路清晰，计算过程简单，通用性和便利性好，可获得位姿参数的精确解。

Description

一种用于自主空中加油的圆形锥套视觉位姿解算方法

技术领域

本发明涉及一种用于自主空中加油的圆形锥套视觉位姿解算方法。

背景技术

空中加油能够延长航空器的飞行时间和作用范围，而自主空中加油(AerialAutonomous Refueling，AAR)可为有人机空中加油降低风险，还可用于无人机，是现代航空领域的重要挑战之一。目前我国主要采用插头-锥套式(软式)空中加油方式，即加油机拖曳一根软管，软管末端挂载伞状的加油锥套，受油机通过飞行控制使得安装在机头区域的受油插头与加油锥套完成对接，进而实现油料的空中传送。

在此过程中，较为关键的一步是受油机在近距(距离在1m～20m之间)的情况下如何获得加油锥套与受油插头之间的精确相对位置和姿态信息，一般至少要求在对接时(即距离为1m以内)的位置误差要小于10cm，加油锥套端面中心与受油插头顶点的连线和加油口中轴线之间的角度偏差要小于10°。考虑到加油机尾流和大气紊流的影响，INS(InertialNavigation System，惯性导航系统)和GPS(Global Positioning System，全球定位系统)等传统的导航方式定位精度较低，通常采用近距定位精度高、价格便宜、重量轻的视觉导航系统。为了提高系统的可靠性和精度，也会利用INS与视觉导航相组合的方式。

现有的大多数AAR视觉方法常采用特殊的光学标记安装在加油锥套上，包括LED光标、人工特殊颜色标记等。如美国国防预研局和NASA Dryden飞行研究中心早期联合发起的自主空中加油项目(AAR)中就使用了一套VisNav系统，该VisNav系统就需要在加油锥套上安装多个LED光标，而后利用半导体位置探测器PSD进行图像定位(见董新民，徐跃鉴，陈博，《自动空中加油技术研究进展与关键问题》，《空军工程大学学报(自然科学版)》，2008(12)，9(6)：1-5)。另外，国内空军工程大学的王旭峰等建立了自主空中加油的一个视觉相对导航半物理地面试验平台，并在加油锥套断面区域加装了红色标识环带，以增强特征区域与背景图像的对比度，从而可通过色彩识别获得图像定位信息(见王旭峰，董新民，孔星炜，《机器视觉辅助的插头锥套式无人机自主空中加油仿真》，《科学技术与工程》，2013(6)，13(18)：5245-5250)。而北航的解洪文等则在加油锥管端面上放置多个光学标记，并采用双目视觉的方式求解相对位置和姿态参数(见解洪文，王宏伦，《基于双目视觉的自动空中加油近距导航方法》，《北京航空航天大学学报》，2011，37(2)：206-209)。

上述这些方法要求加油锥套安装额外的光学标记，特别是需要供电的LED光标，增加了空中加油操作的风险。因此，一种不依靠额外标记、仅利用加油锥套本身特征的AAR视觉方法就具有更好的通用性、便利性和安全性。

通过对加油锥套形状特征的分析，可知其内部加油口为圆形的，半径约为13cm，成像后在图像中呈现明显的黑色圆块或椭圆块，利用这一明显的自身形状特征可降低图像定位处理的计算量，精确的图像定位结果能够为进一步的圆形特征视觉位姿解算提高精度。随着无人机的发展，自主空中加油的需求也就越来越迫切，而不依靠额外光学标记的自主空中加油视觉导航方法具有较高的通用性和便利性，特别是基于加油锥套内部圆形加油口的计算机视觉方法特征明显，图像处理速度快，后续的视觉位姿解算过程计算量小。

发明内容

发明目的：为了克服现有技术中存在的自主空中加油方法中需要安装额外光学标记的不足、额外的带电装置带来更高的空中加油危险性的不足等，本发明从仅依赖加油锥套自身特征出发，提供一种用于自主空中加油的圆形锥套视觉位姿解算方法，该方法从投影变换关系出发，分别在摄像机坐标系和世界坐标系下建立圆形投影锥面方程，并根据“旋转变换不改变系数矩阵特征值”这一特性，计算世界坐标系下圆形投影锥面方程系数矩阵中的未知参数，进而可求解摄像机和加油锥套之间的位置和姿态信息，进一步可获得受油机受油插头和加油机加油锥套之间的相对位姿参数。

技术方案：为实现上述目的，本发明采用的技术方案为：

一种用于自主空中加油的圆形锥套视觉位姿解算方法，仅利用加油锥套自身内部圆形加油口特征，分别在摄像机坐标系和世界坐标系下建立圆形投影锥面方程，并根据旋转变换不改变系数矩阵特征值这一特性，计算世界坐标系下圆形投影锥面方程系数矩阵中的未知参数，进而可求解受油插头和加油锥套之间的位置和姿态信息，具体包括如下步骤：

步骤1、载入内部加油口成像椭圆在图像坐标系下的平面方程、摄像机标定矩阵和圆形特征的半径，具体包括：

步骤101、根据加油锥套内部加油口的图像定位结果，提取内部加油口成像椭圆的边缘像素点坐标，并利用最小二乘法确定椭圆平面方程系数：

f(p_x,p_y)＝(p¹)^TA(p¹)＝0

式中，p＝(p_x,p_y)^T为图像坐标系下内部加油口成像椭圆的边缘像素点坐标，上标1表示齐次坐标，即p¹＝(p^T,1)^T＝(p_x,p_y,1)^T，A＝(a_ij)、i,j＝1,2,3，A为3×3系数矩阵；

步骤102、进行摄像机标定实验，并载入摄像机标定矩阵K，形式如下：

式中，(f_cx,f_cy)为焦距长度，其单位为像素，(c_cx,c_cy)为中心点坐标，其单位为像素，α_c为偏离率，无量纲；

步骤103、载入圆形特征的半径r，单位为m；

步骤2、计算圆形射影锥面在摄像机坐标系下的方程系数P，具体包括：

步骤201、利用摄像机标定矩阵，计算在单位焦距情况下成像椭圆的平面方程：

g(q_x,q_y)＝(q¹)^TB(q¹)＝0，B＝K^TAK

式中，q＝(q_x,q_y)^T为单位焦距对应的图像坐标系下内部加油口成像椭圆的边缘像素点坐标；

步骤202、根据摄像机坐标系与单位焦距图像坐标系之间点的投影变换关系：

c＝c_zq¹

式中，c＝(c_x,c_y,c_z)^T为摄像机坐标系下内部加油口的圆形投影锥面上除原点以外的所有点，可确定圆形投影锥面在摄像机坐标系下的方程如下：

即c^TPc＝0，P＝B

步骤3、计算圆形投影锥面在世界坐标系下的方程系数M，具体包括：

步骤301、建立世界坐标系O_w-X_wY_wZ_w，使得XY平面与圆形加油口平面平行，坐标原点在摄像机光心位置，圆形加油口圆心的Y坐标为0，且X坐标为非负值；

步骤302、确定圆形加油口边缘点在世界坐标系下的方程：

式中，w＝(w_x,w_y,w_z)^T为世界坐标系下圆形加油口边缘点坐标，w₀＝(m,0,h)^T是世界坐标系下内部加油口圆心坐标，其中m和h是未知数，且m≥0；

步骤303、根据投影变换关系，计算圆形投影锥面在世界坐标系下的方程：

写成矩阵形式，即：

w^T·M·w＝0，

式中，w＝(w_x,w_y,w_z)^T为世界坐标系下内部加油口的圆形投影锥面上除原点以外的所有点；

步骤4、确定世界坐标系下的内部加油口圆心坐标，具体包括：

步骤401、计算系数矩阵M的特征值，有如下关系：

η₁＝1,

即：

式中，(η₁,η₂,η₃)为M的特征值，且根据η₂η₃<0和(1-η₂)(1-η₃)≤0设η₂≥η₁>0>η₃；

步骤402、计算系数矩阵B的特征值，并按照如下顺序排列：

λ₁,λ₂,λ₃且λ₂≥λ₁>λ₃

式中，λ₁,λ₂,λ₃为B的特征值；

步骤403、根据旋转变换不改变系数矩阵的特征值，可知系数矩阵M和系数矩阵B的特征值满足如下关系，

即

其中，μ为常数因子；

步骤404，根据步骤401中的关系式可确定矩阵M中的未知数m和h：

进而可确定世界坐标系下的内部加油口圆心坐标w₀＝(m,0,h)^T；

步骤5、确定世界坐标系与摄像机坐标系之间的旋转变换矩阵R，具体包括：

步骤501、对系数矩阵M对角化，

M＝U₁Λ₁U₁ ^T，

式中，U₁为正交矩阵，其行列式为1；

步骤502、对系数矩阵P对角化：

P＝U₂Λ₂U₂ ^T，

式中，U₂为正交矩阵，其行列式为1；

步骤503、确定世界坐标系与摄像机坐标系之间的旋转变换矩阵R：

w＝Rc，R＝U₁UU₂ ^T

式中，U依次取以下4个矩阵，以得到R的四个解：

步骤6、计算摄像机坐标系下内部加油口的圆心坐标和所在平面的法线向量，具体包括：

步骤601、计算摄像机坐标系下内部加油口的圆心坐标：

c₀＝(c_x0,c_y0,c_z0)^T＝R^Tw₀

由步骤503可确定R的四个解，根据约束c_z0<0以剔除其中两个解，保留两个解；

步骤602、确定摄像机坐标系下内部加油口所在平面的法线向量：

u₀＝(u_x0,u_y0,u_z0)^T＝R^T·(0,0,1)^T

此时u₀有两个解，考虑到加油机处于平稳飞行阶段，进一步可根据当前飞机飞行姿态可剔除掉其中一个解，即保留唯一确定的R；

步骤603、根据唯一确定的R，确定摄像机坐标系下内部加油口的圆心坐标：

c₀＝(c_x0,c_y0,c_z0)^T＝R^Tw₀

步骤7、输出受油插头与加油锥套之间的相对位姿参数，具体包括：

步骤701、预先标定摄像机坐标系下摄像机光心与受油插头之间的三维相对位置：

c_b0＝(c_xb0,c_yb0,c_zb0)^T

以及世界坐标系下圆形加油口圆心与加油锥套端面中心之间的三维相对位置：

w_b0＝(w_xb0,w_yb0,w_zb0)^T

步骤702、根据位置偏移关系，输出摄像机坐标系下受油插头与加油锥套之间的相对位置：

c_r＝(c_xr,c_yr,c_zr)^T＝c₀+R^Tw_b0-c_b0

步骤703、定义受油机机体坐标系如下：飞机质心为原点、X轴指向机头方向、Z轴在机体对称面内并指向地面；

定义加油锥套固连坐标系如下：加油口圆心为原点、Z轴平行于加油口平面法向量并指向软管、Y轴在加油口平面内并与当地水平面平行；

定义加油锥套相对于受油机的姿态角如下：受油机机体坐标系先绕Z轴旋转α(航向角)，接着绕Y轴旋转β(俯仰角)，最后绕X轴旋转γ(横滚角)，使得机体坐标系三个轴与加油锥套固连坐标系平行；

预先标定摄像机坐标系与受油机机体坐标系之间的旋转变换关系R_cb，计算受油机机体坐标系下内部加油口的圆心坐标和所在平面的法线向量：

u_b0＝(u_bx0,u_by0,u_bz0)^T＝R_cb ^T·u₀

步骤704、输出受油机与加油锥套之间的相对姿态，包括航向角α和俯仰角β，

有益效果：本发明提供的用于自主空中加油的圆形锥套视觉位姿解算方法，克服了现有自主空中加油方法中需要安装额外光学标记的不足、额外的带电装置带来更高的空中加油危险性等；该方法仅利用加油锥套自身特征，具有更好的通用性、便利性和安全性；该方法从投影变换关系出发，分别在摄像机坐标系和世界坐标系下建立圆形投影锥面方程，并根据“旋转变换不改变系数矩阵特征值”这一特性，计算世界坐标系下圆形投影锥面方程系数矩阵中的未知参数，进而求解受油插头和加油锥套之间的位置和姿态信息；该方法思路清晰，计算过程简单，可获得位姿参数的精确解。

附图说明

图1为安装在加油机上的加油锥套照片；其中Inner Refueling Port为内部加油口；

图2为本发明方法中所涉及到的坐标系示意图，其中点O_c(O_w)为摄像机光心，也是世界坐标系O_w-X_wY_wZ_w和摄像机坐标系O_c-X_cY_cZ_c的原点，A为成像平面，平行于X_cY_c平面，Z_c垂直于成像面，B为成像椭圆，D为内部圆形加油口，C为加油口所在的平面，X_wY_w平面平行于加油口所在的平面C；

图3为本发明方法的计算流程图；

图4为本发明方法中所涉及各位置点的相对关系图；其中1为受油机，2为加油锥套，A为受油插头顶点，B为端面中心，C为摄像机光心，D为加油口圆心，θ为角度偏差，①为AB连线，②为AC连线，③为CD连线，④为DB连线；

图5为视景仿真实验中受油机摄像头与加油机锥套之间的相对运动轨迹；其中Start Point表示起点，End Point表示终端，3D Trajectory表示受油机摄像头与加油机锥套之间的三维相对运动轨迹，2D Projectiong表示三维相对运动轨迹在YZ平面的投影；

图6为视景仿真实验中采用的参考轨迹与本发明方法解算轨迹之间的对比曲线，6(a)为X轴对比曲线，6(b)为Y轴对比曲线，6(c)为Z轴对比曲线；其中Reference表示参考轨迹，Solved表示本发明方法解算轨迹；

图7为视景仿真实验中本发明方法的XY平面定位误差，7(a)为X轴定位误差，7(b)为Y轴定位误差；其中X Error表示X轴定位误差，Y Error表示Y轴定位误差，Frame表示帧数；

图8为视景仿真实验中本发明方法的距离解算误差和方位角误差，8(a)为距离解算误差，8(b)为方位角误差；其中Z Error表示距离结算误差，Aangle Error表示方位角误差，Frame表示帧数

具体实施方式

下面结合附图对本发明作更进一步的说明。

从图1可以明显看出，加油锥套内部加油口呈现圆形特征，若光轴与加油口平面法线向量的夹角较小，成像结果为近似圆形。

图2中展示了本发明方法中所涉及到的几个坐标系，包括摄像机坐标系和世界坐标系，特别是世界坐标系的建立较为严格。

一种用于自主空中加油的圆形锥套视觉位姿解算方法，仅利用加油锥套自身内部圆形加油口特征，分别在摄像机坐标系和世界坐标系下建立圆形投影锥面方程，并根据旋转变换不改变系数矩阵特征值这一特性，计算世界坐标系下圆形投影锥面方程系数矩阵中的未知参数，进而可求解受油插头和加油锥套之间的位置和姿态信息，如图3所示，该方法具体包括如下步骤：

步骤1、载入内部加油口成像椭圆在图像坐标系下的平面方程、摄像机标定矩阵和圆形特征的半径，具体包括：

步骤101、根据加油锥套内部加油口的图像定位结果，提取内部加油口成像椭圆的边缘像素点坐标，并利用最小二乘法确定椭圆平面方程系数：

f(p_x,p_y)＝(p¹)^TA(p¹)＝0

式中，p＝(p_x,p_y)^T为图像坐标系下内部加油口成像椭圆的边缘像素点坐标，上标1表示齐次坐标，即p¹＝(p^T,1)^T＝(p_x,p_y,1)^T，A＝(a_ij)、i,j＝1,2,3，A为3×3系数矩阵；

步骤102、进行摄像机标定实验，并载入摄像机标定矩阵K，形式如下：

式中，(f_x,f_y)为焦距长度，其单位为像素，(c_cx,c_cy)为中心点坐标，其单位为像素，α_c为偏离率，无量纲；

步骤103、载入圆形特征的半径r，单位为m；

步骤2、计算圆形射影锥面在摄像机坐标系下的方程系数P，具体包括：

步骤201、利用摄像机标定矩阵，计算在单位焦距情况下成像椭圆的平面方程：

g(q_x,q_y)＝(q¹)^TB(q¹)＝0，B＝K^TAK

式中，q＝(q_x,q_y)^T为单位焦距对应的图像坐标系下内部加油口成像椭圆的边缘像素点坐标；

步骤202、根据摄像机坐标系与单位焦距图像坐标系之间点的投影变换关系：

c＝c_zq¹

式中，c＝(c_x,c_y,c_z)^T为摄像机坐标系下内部加油口的圆形投影锥面上除原点以外的所有点，可确定圆形投影锥面在摄像机坐标系下的方程如下：

即c^TPc＝0，P＝B

步骤3、计算圆形投影锥面在世界坐标系下的方程系数M，具体包括：

步骤301、建立世界坐标系O_w-X_wY_wZ_w，使得XY平面与圆形加油口平面平行，坐标原点在摄像机光心位置，圆形加油口圆心的Y坐标为0，且X坐标为非负值；

步骤302、确定圆形加油口边缘点在世界坐标系下的方程：

式中，w＝(w_x,w_y,w_z)^T为世界坐标系下圆形加油口边缘点坐标，w₀＝(m,0,h)^T是世界坐标系下内部加油口圆心坐标，其中m和h是未知数，且m≥0；

步骤303、根据投影变换关系，计算圆形投影锥面在世界坐标系下的方程：

写成矩阵形式，即：

w^T·M·w＝0，

式中，w＝(w_x,w_y,w_z)^T为世界坐标系下内部加油口的圆形投影锥面上除原点以外的所有点；

步骤4、确定世界坐标系下的内部加油口圆心坐标，具体包括：

步骤401、计算系数矩阵M的特征值，有如下关系：

η₁＝1,

即：

式中，(η₁,η₂,η₃)为M的特征值，且根据η₂η₃<0和(1-η₂)(1-η₃)≤0设η₂≥η₁>0>η₃；

步骤402、计算系数矩阵B的特征值，并按照如下顺序排列：

λ₁,λ₂,λ₃且λ₂≥λ₁>λ₃

式中，λ₁,λ₂,λ₃为B的特征值；

步骤403、根据旋转变换不改变系数矩阵的特征值，可知系数矩阵M和系数矩阵B的特征值满足如下关系，

即

其中，μ为常数因子；

步骤404，根据步骤401中的关系式可确定矩阵M中的未知数m和h：

进而可确定世界坐标系下的内部加油口圆心坐标w₀＝(m,0,h)^T；

步骤5、确定世界坐标系与摄像机坐标系之间的旋转变换矩阵R，具体包括：

步骤501、对系数矩阵M对角化，

M＝U₁Λ₁U₁ ^T，

式中，U₁为正交矩阵，其行列式为1；

步骤502、对系数矩阵P对角化：

P＝U₂Λ₂U₂ ^T，

式中，U₂为正交矩阵，其行列式为1；

步骤503、确定世界坐标系与摄像机坐标系之间的旋转变换矩阵R：

w＝Rc，R＝U₁UU₂ ^T

式中，U依次取以下4个矩阵，以得到R的四个解：

步骤6、计算摄像机坐标系下内部加油口的圆心坐标和所在平面的法线向量，具体包括：

步骤601、计算摄像机坐标系下内部加油口的圆心坐标：

c₀＝(c_x0,c_y0,c_z0)^T＝R^Tw₀

由步骤503可确定R的四个解，根据约束c_z0<0以剔除其中两个解，保留两个解；

步骤602、确定摄像机坐标系下内部加油口所在平面的法线向量：

u₀＝(u_x0,u_y0,u_z0)^T＝R^T·(0,0,1)^T

此时u₀有两个解，考虑到加油机处于平稳飞行阶段，进一步可根据当前飞机飞行姿态可剔除掉其中一个解，即保留唯一确定的R；

步骤603、根据唯一确定的R，确定摄像机坐标系下内部加油口的圆心坐标：

c₀＝(c_x0,c_y0,c_z0)^T＝R^Tw₀

步骤7、输出受油插头与加油锥套之间的相对位姿参数，具体包括：

步骤701、预先标定摄像机坐标系下摄像机光心与受油插头之间的三维相对位置：

c_b0＝(c_xb0,c_yb0,c_zb0)^T

以及世界坐标系下圆形加油口圆心与加油锥套端面中心之间的三维相对位置：

w_b0＝(w_xb0,w_yb0,w_zb0)^T

步骤702、根据图4所示的位置偏移关系，输出摄像机坐标系下受油插头与加油锥套之间的相对位置：

c_r＝(c_xr,c_yr,c_zr)^T＝c₀+R^Tw_b0-c_b0

步骤703、定义受油机机体坐标系如下：飞机质心为原点、X轴指向机头方向、Z轴在机体对称面内并指向地面；

定义加油锥套固连坐标系如下：加油口圆心为原点、Z轴平行于加油口平面法向量并指向软管、Y轴在加油口平面内并与当地水平面平行；

定义加油锥套相对于受油机的姿态角如下：受油机机体坐标系先绕Z轴旋转α(航向角)，接着绕Y轴旋转β(俯仰角)，最后绕X轴旋转γ(横滚角)，使得机体坐标系三个轴与加油锥套固连坐标系平行；

预先标定摄像机坐标系与受油机机体坐标系之间的旋转变换关系R_cb，计算受油机机体坐标系下内部加油口的圆心坐标和所在平面的法线向量：

u_b0＝(u_bx0,u_by0,u_bz0)^T＝R_cb ^T·u₀

步骤704、输出受油机与加油锥套之间的相对姿态，包括航向角α和俯仰角β，

为了评价本发明提出的圆形视觉位姿解算方法的性能，设计了AAR视景仿真验证系统，该验证系统工作流程如下：

(1)设计飞行轨迹，获得摄像机与加油锥套之间的相对运动轨迹；

(2)建立加油锥套三维模型，并建立加油机三维模型；

(3)预设摄像机参数，并采用视景仿真程序生成摄像机采集图像序列；

(4)对采集图像序列进行锥套图像定位，获得精确的锥套图像边缘信息；

(5)利用本发明方法进行视觉位姿解算，并与预设的运动轨迹进行比较。

利用上述AAR视景仿真验证系统，进行了相关实验。实验时，选取图像采集帧率为25帧/秒，像素个数为768×576，摄像机焦距为10mm，单位像素物理尺寸为5.5μm，锥套内部加油口半径为14cm。图5为实验用的摄像机与加油锥套之间的相对运动轨迹；

采用本发明方法解算的定位结果如图6所示，图中将解算结果与预设轨迹进行对比，可以看出两者基本重合。图7和图8给出了解算误差曲线，图7为XY平面的定位误差曲线，可以看出平面定位误差小于2cm，从第250帧(相对距离为8m)开始下降，直到第390帧(相对距离为2.5m)以后误差小于5mm。图8为解算距离误差和方位角误差，其中方位角误差定义为圆形所在平面法线向量的参考值与解算值间的夹角，可以看出，距离误差随距离减小逐渐减小，从第370帧(相对距离为3.3m)开始距离误差小于0.1m；而方位角误差在整个过程中的平均值为6.39°，标准差为3.69°，且第250帧(相对距离为8m)开始，方位角误差要基本小于10°，均值为4.28°，标准差为2.07°。上述实验结果验证了本发明方法的正确性和准确度。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (1)

1.一种用于自主空中加油的圆形锥套视觉位姿解算方法，其特征在于：仅利用加油锥套自身内部圆形加油口特征，分别在摄像机坐标系和世界坐标系下建立圆形投影锥面方程，并根据旋转变换不改变系数矩阵特征值这一特性，计算世界坐标系下圆形投影锥面方程系数矩阵中的未知参数，进而可求解受油插头和加油锥套之间的位置和姿态信息，具体包括如下步骤：

步骤1、载入内部加油口成像椭圆在图像坐标系下的平面方程、摄像机标定矩阵和圆形特征的半径，具体包括：

步骤101、根据加油锥套内部加油口的图像定位结果，提取内部加油口成像椭圆的边缘像素点坐标，并利用最小二乘法确定椭圆平面方程系数：

f(p_x,p_y)＝(p¹)^TA(p¹)＝0

式中，p＝(p_x,p_y)^T为图像坐标系下内部加油口成像椭圆的边缘像素点坐标，上标1表示齐次坐标，即p¹＝(p^T,1)^T＝(p_x,p_y,1)^T，A＝(a_ij)、i,j＝1,2,3，A为3×3系数矩阵；

步骤102、进行摄像机标定实验，并载入摄像机标定矩阵K，形式如下：

$K=\left[\begin{array}{ccc}{f}_{cx}& {\mathrm{\&alpha;}}_c{f}_{cx}& c_{cx}\\ 0& f_{cy}& c_{cy}\\ 0& 0& 1\end{array}\right]$

式中，(f_cx,f_cy)为焦距长度，其单位为像素，(c_cx,c_cy)为中心点坐标，其单位为像素，α_c为偏离率，无量纲；

步骤103、载入圆形特征的半径r，单位为m；

步骤2、计算圆形投影锥面在摄像机坐标系下的方程系数P，具体包括：

步骤201、利用摄像机标定矩阵，计算在单位焦距情况下成像椭圆的平面方程：

g(q_x,q_y)＝(q¹)^TB(q¹)＝0，B＝K^TAK

式中，q＝(q_x,q_y)^T为单位焦距对应的图像坐标系下内部加油口成像椭圆的边缘像素点坐标；

步骤202、根据摄像机坐标系与单位焦距图像坐标系之间点的投影变换关系：

c＝c_zq¹

式中，c＝(c_x,c_y,c_z)^T为摄像机坐标系下内部加油口的圆形投影锥面上除原点以外的所有点，可确定圆形投影锥面在摄像机坐标系下的方程如下：

即c^TPc＝0，P＝B

步骤3、计算圆形投影锥面在世界坐标系下的方程系数M，具体包括：

步骤301、建立世界坐标系O_w-X_wY_wZ_w，使得XY平面与内部加油口平面平行，坐标原点在摄像机光心位置，内部加油口圆心的Y坐标为0，且X坐标为非负值；

步骤302、确定内部加油口边缘点在世界坐标系下的方程：

$\left\{\begin{array}{c}{\left(w_{px}-m\right)}^2+{w_{py}}^2=r^2\\ w_{pz}=h\end{array}\right.$

式中，(w_px,w_py,w_pz)^T为世界坐标系下内部加油口边缘点坐标，w₀＝(m,0,h)^T是世界坐标系下内部加油口圆心坐标，其中m和h是未知数，且m≥0；

步骤303、根据投影变换关系，计算圆形投影锥面在世界坐标系下的方程：

${(w_{sx}-\frac{w_{sz}}{h}m)}^2+{w_{sy}}^2={\left(\frac{w_{sz}}{h}r\right)}^2$

式中，(w_sx,w_sy,w_sz)^T为世界坐标系下内部加油口的圆形投影锥面上除边缘点和圆心以外的所有点坐标；

将世界坐标系下内部加油口的圆形投影锥面上除圆心以外的点写成矩阵形式，即：

$w^T\&CenterDot;M\&CenterDot;w=0,M=\left[\begin{array}{ccc}1& 0& -m/h\\ 0& 1& 0\\ -m/h& 0& (m^2-r^2)/h^2\end{array}\right]$

式中：w＝(w_x,w_y,w_z)^T包括(w_px,w_py,w_pz)^T和(w_sx,w_sy,w_sz)^T两部分；

步骤4、确定世界坐标系下的内部加油口圆心坐标，具体包括：

步骤401、计算系数矩阵M的特征值，有如下关系：

${\mathrm{\&eta;}}_1=1,{\mathrm{\&eta;}}_2+{\mathrm{\&eta;}}_3=\frac{m^2-r^2}{h^2}+1,{\mathrm{\&eta;}}_2{\mathrm{\&eta;}}_3=-\frac{r^2}{h^2}$

即：

式中，(η₁,η₂,η)₃为M的特征值，且根据η₂η₃＜0和(1-η₂)(1-η₃)≤0设η₂≥η₁＞0＞η₃；

步骤402、计算系数矩阵B的特征值，并按照如下顺序排列：

λ₁,λ₂,λ₃且λ₂≥λ₁＞λ₃

式中，λ₁,λ₂,λ₃为B的特征值；

步骤403、根据旋转变换不改变系数矩阵的特征值，可知系数矩阵M和系数矩阵B的特征值满足如下关系，

即

其中，μ为常数因子；

步骤404，根据步骤401中的关系式可确定矩阵M中的未知数m和h：

$h=r/\sqrt{-{\mathrm{\&eta;}}_2{\mathrm{\&eta;}}_3},m=r\sqrt{(1-{\mathrm{\&eta;}}_2)(1-{\mathrm{\&eta;}}_3)/\left({\mathrm{\&eta;}}_2{\mathrm{\&eta;}}_3\right)}$

进而可确定世界坐标系下的内部加油口圆心坐标w₀＝(m,0,h)^T；

步骤5、确定世界坐标系与摄像机坐标系之间的旋转变换矩阵R，具体包括：

步骤501、对系数矩阵M对角化，

$M=U_1{\mathrm{\&Lambda;}}_1{U_1}^T,{\mathrm{\&Lambda;}}_1=\left[\begin{array}{ccc}{\mathrm{\&eta;}}_1& & \\ & {\mathrm{\&eta;}}_2& \\ & & {\mathrm{\&eta;}}_3\end{array}\right]$

式中，U₁为正交矩阵，其行列式为1；

步骤502、对系数矩阵P对角化：

$P=U_2{\mathrm{\&Lambda;}}_2{U_2}^T,{\mathrm{\&Lambda;}}_2=\left[\begin{array}{ccc}{\mathrm{\&lambda;}}_1& & \\ & {\mathrm{\&lambda;}}_2& \\ & & {\mathrm{\&lambda;}}_3\end{array}\right]$

式中，U₂为正交矩阵，其行列式为1；

步骤503、确定世界坐标系与摄像机坐标系之间的旋转变换矩阵R：

w＝Rc，R＝U₁UU₂ ^T

式中，U依次取以下4个矩阵，以得到R的四个解：

$U\left(1\right)=\left[\begin{array}{ccc}1& & \\ & 1& \\ & & 1\end{array}\right],U\left(2\right)=\left[\begin{array}{ccc}1& & \\ & -1& \\ & & -1\end{array}\right],U\left(3\right)=\left[\begin{array}{ccc}-1& & \\ & 1& \\ & & -1\end{array}\right],U\left(4\right)=\left[\begin{array}{ccc}-1& & \\ & -1& \\ & & 1\end{array}\right]$

步骤6、计算摄像机坐标系下内部加油口的圆心坐标和所在平面的法线向量，具体包括：

步骤601、计算摄像机坐标系下内部加油口的圆心坐标：

c₀＝(c_x0,c_y0,c_z0)^T＝R^Tw₀

由步骤503可确定R的四个解，根据约束c_z0＜0以剔除其中两个解，保留两个解；

步骤602、确定摄像机坐标系下内部加油口所在平面的法线向量：

u₀＝(u_x0,u_y0,u_z0)^T＝R^T·(0,0,1)^T

此时u₀有两个解，考虑到加油机处于平稳飞行阶段，进一步可根据当前飞机飞行姿态可剔除掉其中一个解，即保留唯一确定的R；

步骤603、根据唯一确定的R，确定摄像机坐标系下内部加油口的圆心坐标：

c₀＝(c_x0,c_y0,c_z0)^T＝R^Tw₀

步骤7、输出受油插头与加油锥套之间的相对位姿参数，具体包括：

步骤701、预先标定摄像机坐标系下摄像机光心与受油插头之间的三维相对位置：

c_b0＝(c_xb0,c_yb0,c_zb0)^T

以及世界坐标系下内部加油口圆心与加油锥套端面中心之间的三维相对位置：

w_b0＝(w_xb0,w_yb0,w_zb0)^T

步骤702、根据位置偏移关系，输出摄像机坐标系下受油插头与加油锥套之间的相对位置：

c_r＝(c_xr,c_yr,c_zr)^T＝c₀+R^Tw_b0-c_b0

步骤703、定义受油机机体坐标系如下：飞机质心为原点、X轴指向机头方向、Z轴在机体对称面内并指向地面；

定义加油锥套固连坐标系如下：加油口圆心为原点、Z轴平行于加油口平面法向量并指向软管、Y轴在加油口平面内并与当地水平面平行；

定义加油锥套相对于受油机的姿态角如下：受油机机体坐标系先绕Z轴旋转α，接着绕Y轴旋转β，最后绕X轴旋转γ，使得机体坐标系三个轴与加油锥套固连坐标系平行；

预先标定摄像机坐标系与受油机机体坐标系之间的旋转变换关系R_cb，计算受油机机体坐标系下内部加油口的圆心坐标和所在平面的法线向量：

u_b0＝(u_bx0,u_by0,u_bz0)^T＝R_cb ^T·u₀

步骤704、输出受油机与加油锥套之间的相对姿态，包括航向角α和俯仰角β，