[发明专利]一种用于自主空中加油的圆形锥套视觉位姿解算方法

权利要求书

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=fcxαcfcxccx0fcyccy001]]>

式中，(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为摄像机坐标系下内部加油口的圆形投影锥面上除原点以外的所有点，可确定圆形投影锥面在摄像机坐标系下的方程如下：

g(cxcz,cycz)=1cz2cTBc=0,]]>即c^TPc＝0，P＝B

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

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

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

(wx-m)2+wy2=r2wz=h]]>

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

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

(wx-wzhm)2+wy2=(wzhr)2]]>

写成矩阵形式，即：

w^T·M·w＝0，M=10-m/h010-m/h0(m2-r2)/h2]]>

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

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

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

η₁＝1,η2+η3=m2-r2h2+1,η2η3=-r2h2]]>

即：(1-η2)(1-η3)=-m2h2≤0]]>

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

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

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

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

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

1η2η3=μλ1λ2λ3,]]>即η2=λ2λ1,η3=λ3λ1]]>

其中，μ为常数因子；

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

h=r/-η2η3,m=r(1-η2)(1-η3)/(η2η3)]]>

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

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

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

M＝U₁Λ₁U₁^T，Λ1=η1η2η3]]>

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

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

P＝U₂Λ₂U₂^T，Λ2=λ1λ2λ3]]>

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

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

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

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

U(1)=111,U(2)=1-1-1,U(3)=-11-1,U(4)=-1-11]]>

步骤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、输出受油机与加油锥套之间的相对姿态，包括航向角α和俯仰角β，α=arctan(-uby0ubx0),β=arcsin(uby0sinα).]]>