CN113850866A - 一种基于特征反向投影的自主空中加油锥套位姿解算方法 - Google Patents

Abstract

一种基于特征反向投影的自主空中加油锥套位姿解算方法，步骤包括：(1)获取锥套目标图像；(2)提取图像中锥套目标结构特征；(3)特征空间反向投影；(4)基于锥套结构约束解算位姿；该方法以单目视觉图像为基础运用图像识别手段精确提取锥套结构特征，以二维图像结构特征为基础进行空间反向投影，结合锥套目标的结构约束解算锥套相对位姿，提高了自主空中加油锥套目标单目视觉位姿测量精度。

Description

一种基于特征反向投影的自主空中加油锥套位姿解算方法

技术领域

本发明涉及一种单目视觉相对位姿测量方法，尤其涉及一种基于特征反向投影的自主空中加油锥套位姿解算方法。

背景技术

基于视觉的目标位置和姿态测量是在航空目标测量、工业自动化生成、自动驾驶等方面的具有广泛的应用。特别在自主空中加油领域中，加油锥套相对于受油机探管之间的相对位姿精确测量是自主空中加油任务成功的首要条件。

自主空中加油锥套相对位姿的视觉测量分为合作目标和非合作目标两种类型，合作目标测量在自主空中加油视觉测量技术中，通过在锥套目标上安装发光二极管(LightEmitting Diodes LED)或对锥套进行着色处理构建特征进行相对位姿测量，这种测量方式需要对目标进行改装来实现。非合作目标测量则通过分析目标具有的不变性特征进行相对位姿测量，目前已采用过的特征有特征点、特征线、特征圆等多种几何特征。通过对特征进行识别提取，从而利用提取特征构建约束关系解算处目标的相对位姿。

根据Carol Martinez等人的《Avision-based strategy for autonomous aerialrefueling tasks》(参见《Robotics andAutonomous Systems》2013年,61(8):876-895)，采用了锥套底面圆成像大小的最小包围矩形框的四个角点作为特征点构建单应性矩阵解算锥套的相对位置，但未涉及锥套结构特征反向投影解算位姿。根据马跃博的《基于卷积神经网络的自主空中加油识别测量技术研究》(参见2020年硕博论文库)，采用了锥套的底面圆投影椭圆反向建立了空间旋转曲面求解锥套的相对位姿，但未涉及锥套结构特征点的反向投影、底面圆半径约束、中心位置约束和迭代优化位姿解算。专利CN202010065420.3中提出了一种基于可变视场角的加油锥套位姿精确测量方法，将双目相机左右图像的标志灯进行匹配，获得标志灯在左相机坐标系下的空间坐标，计算输出锥套的位姿信息，未涉及锥套结构特征的单目视觉相对位姿测量。

发明内容

本发明解决的技术问题是：提供一种基于特征反向投影的自主空中加油锥套位姿解算方法，用于与自主空中加油中锥套类似的锥台构型目标的单目视觉位姿测量。该方法利用锥台在位姿变化时保持不变的结构特征进行反向特征投影，通过结构约束关系构建目标函数并进行迭代解算位姿。通过迭代优化目标函数，有效利用了锥套结构底面圆约束和中心位置关系的不变性特征，从而提高了位姿解算精度。

本发明技术方案如下：

一种基于特征反向投影的自主空中加油锥套位姿解算方法，该方法包括如下步骤：

步骤(1)：对锥套目标进行成像，并获取目标图像；

所述锥套目标具有锥台构型，该锥台构型的上下平行的两个端面分别为圆面和圆环，即顶面圆和底面圆，已知该两个端面的圆的半径和中心所在目标坐标系中的三维坐标，根据中心坐标和半径获取锥套的结构特征参数如下：

p_cb＝(x_cb,y_cb,z_cb)z_cb＝0

p_ct＝(0,0,d)

其中，x_w、y_w和z_w为锥套的目标坐标系坐标，r_b为锥套底面圆半径，r_t为锥套顶面圆半径，d为锥台高度，p_cb为底面圆中心坐标，p_ct为顶面圆中心坐标。

步骤(2)：提取锥套结构特征：

将步骤(1)获取的目标图像进行特征识别，提取图像中目标的结构特征点坐标集合，所述结构特征点坐标集合包括：底面圆特征点集{P_b}、底面圆中心点成像位置集{P_cb}、顶面圆中心点成像位置集{P_ct}；

步骤(3)：实施特征空间反向投影：

已知单目相机等效焦距为f，定义所述目标的结构特征点坐标集合在相机坐标系下的坐标集分别为底面圆边缘点坐标集合{A_b}、底面圆中心点坐标{A_cb}和顶面圆中心点坐标{A_ct}，锥套从目标坐标系到相机坐标系的位姿转移矩阵为T，由步骤(2)获取的目标结构特征点的坐标为P_i＝(u_i,v_i)，则相机坐标系下的三维坐标表示为(P_i,f)＝(u_i,v_i,f)，相机坐标系下的特征点坐标根据小孔成像原理，结合位姿转移矩阵T和相机的内参数矩阵K将特征点反向投影到三维空间；

其中，

其中，u₀和v₀为相机主点位置，R为旋转矩阵，t为平移向量，T为转移矩阵。

特征点反向投影到三维空间的表达式为：

根据先验知识，由于锥台结构的旋转不变性，设置旋转角为0，得到反向投影方程为：

f(θ,φ,t_x,t_y,t_z,x′_w,y′_w,z′_w|u,v)＝0

上式中，θ、φ分别为目标坐标系相对于相机坐标系的俯仰角和偏航角，能构成旋转姿态矩阵R，t_x、t_y和t_z构成平移向量t＝[t_x,t_y,t_z]^T，p_w＝(x′_w,y′_w,z′_w)为图像特征点反向投影在三维空间中坐标值，(u,v)为图像中锥套结构特征的坐标；

同时根据锥台构型顶面圆的中心图像特征点(u_ct,v_ct)和目标坐标系锥台高度约束d对所述反向投影方程化简得到：

f₁(θ,φ,t_z,x′_w,y′_w,z′_w|u,v)＝0

根据该底面圆的约束方程可知z′_w＝0，结合简化后的反向投影方程计算得到反向投影方程为：

f₂(θ,φ,t_z,x′_w,y′_w|u,v)＝0；

步骤(4)：基于锥套结构约束关系解算位姿：

基于锥套结构的约束关系构建目标函数：

其中，r_b为底面圆半径，

为估计底面圆半径，x′_w和y′_w为估计的目标坐标系下底面圆轮廓坐标。

通过对待求位姿参数变量X＝[θ,φ,t_z]^T的求导得到目标函数的雅可比矩阵J，从而计算得到位姿参数的修正量ΔX，过程表示如下：

J_XΔX＝J

其中J_X为雅可比矩阵对X的偏导；

根据上述计算得到的修正量ΔX，对位姿参数进行迭代解算：

X^k＝X^k-1+ΔX

经过k次迭代，当所有的底面圆约束和中心点位置约束都能同时满足时，目标函数迭代过程逐渐收敛为零，从而解算得到最优的位姿参数变量X；

在得到X的基础之上根据上述反向投影方程简化方程计算得到目标的姿态角

和平移向量t＝(t_x,t_y,t_z)。

进一步的，所述目标函数的求解最少需要图像底面圆特征点4个。

进一步的，步骤(4)中，进行迭代解算时，通过底面圆中心位置和顶面圆中心位置关系约束消除特征反向投影的二义性。

进一步的，所述中心位置关系约束为σ＝-sign(u_ct-u_cb)。

本发明的有益效果为：本发明提供了一种针对自主空中加油中锥套类似的锥台构型目标的单目视觉位姿测量方法。该方法利用锥台在位姿变化时保持不变的结构特征进行反向特征投影，并建立结构约束关系构建目标函数进行迭代解算位姿方法。通过迭代优化目标函数，有效利用了锥套结构底面圆约束和中心位置关系的不变性特征，从而提高了位姿解算精度。

附图说明

图1为本发明具体流程图；

图2为仿真生成的锥套目标模型图；

图3(1)-图3(3)为多组仿真生成的锥套目标图像；

图4为仿真实验锥套目标相对位姿解算结果及误差图表。

具体实施方式

下面结合具体实施例，对本发明方法进一步说明。

本发明具体流程如图1所示。

(1)对锥套目标成像，并获取目标图像。共进行了3组实验，如图3(1)-图3(3)所示为，仿真生成的锥套目标在不同位姿下的3组图像。设置的锥套目标是由平行的圆环和圆面构成的锥台构型如图2所示，已知锥台底面圆半径500mm和中心三维坐标为(0，0，0)，顶面圆中心坐标为(0，0，600)单位mm。根据中心坐标和空间圆半径可获取锥套的结构特征参数；

p_cb＝(0,0,0)

p_ct＝(0,0,600)

(2)提取锥套结构特征。将步骤(1)获取的图像进行特征识别提取图像中目标的结构特征点坐标集合{P_b}(底面圆特征点集)、{P_cb}(底面圆中心点成像位置)、{P_ct}(顶面圆中心点成像位置)；

(3)特征空间反向投影。单目相机等效焦距为f＝1773.62pixel，定义锥套结构特征点集在相机坐标系下的坐标集分别为{A_b}、{A_cb}和{A_ct}，锥套目标坐标系到相机坐标系的位姿转移矩阵为T，由步骤(2)获取了目标结构特征点的坐标为P_i＝(u_i,v_i)，则相机坐标系下的三维坐标可表示为(P_i,f)＝(u_i,v_i,f)，相机坐标系下的特征点坐标根据小孔成像原理，结合位姿转移矩阵T和相机的内参数矩阵K将特征点反向投影到三维空间；

根据先验知识，由于锥台结构的旋转不变性，设置旋转角为0，可得到反向投影方程为：

f(θ,φ,t_x,t_y,t_z,x′_w,y′_w,z′_w|u,v)＝0

上式中θ、φ分别为目标坐标系相对于相机坐标系的俯仰角和偏航角，可构成旋转姿态矩阵R，t_x、t_y和t_z构成平移向量t＝[t_x,t_y,t_z]^T，p_w＝(x′_w,y′_w,z′_w)为图像特征点反向投影在三维空间中坐标值，(u,v)为图像中锥套结构特征的坐标。

同时根据顶面圆中心的图像特征点(u_ct,v_ct)和目标坐标系中(0,0,d)可以对反向投影方程化简：

f₁(θ,φ,t_z,x′_w,y′_w,z′_w|u,v)＝0

其中底面圆轮廓坐标可由简化后的反向投影方法计算得到，根据底面圆的约束方程可知z′_w＝0，从而得到：

f₂(θ,φ,t_z,x′_w,y′_w|u,v)＝0

(4)基于锥套结构约束关系解算位姿。基于锥套结构的约束关系底面圆半径构建目标函数：

目标函数的求解最少需要图像底面圆特征点4个，通过对待求位姿参数变量X＝[θ,φ,t_z]^T的求导得到目标函数的雅可比矩阵J，从而计算得到参数的修正量ΔX。

J_XΔX＝J

根据计算得到修正量ΔX，对位姿参数进行迭代解算，同时由于进行迭代解算的底面圆特征点反向投影具有二义性，即存在对称的底面圆对应于相同的图像特征，因此通过底面圆中心位置和顶面圆中心的位置关系σ约束可以消除特征反向投影的二义性。

X^k＝X^k-1+ΔX

经过k次迭代，当所有的底面圆约束和中心点位置约束都能同时满足时，目标函数迭代过程逐渐收敛为零，从而得到解算得到最优的位姿参数变量X。在得到X的基础之上根据上述反向投影方程简化过程公式计算得到目标的姿态角

和平移向量t＝(t_x,t_y,t_z)。如图4所示，分别统计了3组实验位姿解算结果以及平均误差。

由于充分利用所有构建锥套结构特征的特征点信息，将几何特征和空间关系进行关联，建立起目标函数进行非线性最优化迭代解算，提高相对目标位姿解算精度。

本发明未详述部分属于本技术领域的公知技术。以上所述仅为本发明的具体实例而已，并不用于以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于特征反向投影的自主空中加油锥套位姿解算方法，其特征在于：

该方法包括如下步骤：

步骤(1)：对锥套目标进行成像，并获取目标图像；

所述锥套目标具有锥台构型，该锥台构型的上下平行的两个端面分别为圆面和圆环，即顶面圆和底面圆，已知该两个端面的圆的半径和中心所在目标坐标系中的三维坐标，根据中心坐标和半径获取锥套的结构特征参数如下：

p_cb＝(x_cb，y_cb，z_cb)z_cb＝0

p_ct＝(0,0,d)

其中，x_w、y_w和z_w为锥套的目标坐标系坐标，r_b为锥套底面圆半径，r_t为锥套顶面圆半径，d为锥台高度，p_cb为底面圆中心坐标，p_ct为顶面圆中心坐标；

步骤(2)：提取锥套结构特征：

将步骤(1)获取的目标图像进行特征识别，提取图像中目标的结构特征点坐标集合，所述结构特征点坐标集合包括：底面圆特征点集{P_b}、底面圆中心点成像位置集{P_cb}、顶面圆中心点成像位置集{P_ct}；

步骤(3)：实施特征空间反向投影：

已知单目相机等效焦距为f，定义所述目标的结构特征点坐标集合在相机坐标系下的坐标集分别为底面圆边缘点坐标集合{A_b}、底面圆中心点坐标{A_cb}和顶面圆中心点坐标{A_ct}，锥套目标坐标系到相机坐标系的位姿转移矩阵为T，由步骤(2)获取的目标结构特征点的坐标为P_i＝(u_i,v_i)，则相机坐标系下的三维坐标表示为(P_i,f)＝(u_i,v_i,f)，相机坐标系下的特征点坐标根据小孔成像原理，结合位姿转移矩阵T和相机的内参数矩阵K将特征点反向投影到三维空间；

其中，

其中u₀和v₀为相机主点位置，R为旋转矩阵，t为平移向量，T为转移矩阵；

特征点反向投影到三维空间的表达式为：

根据先验知识，由于锥台结构的旋转不变性，设置旋转角为0，得到反向投影方程为：

f(θ,φ,t_x,t_y,t_z,x′_w,y′_w,z′_w|u,v)＝0

上式中，θ、φ分别为目标坐标系相对于相机坐标系的俯仰角和偏航角，能构成旋转姿态矩阵R，t_x、t_y和t_z构成平移向量t＝[t_x,t_y,t_z]^T，p_w＝(x′_w,y′_w,z′_w)为图像特征点反向投影在三维空间中坐标值，(u,v)为图像中锥套结构特征的坐标；

同时根据锥台构型顶面圆的中心图像特征点(u_ct,v_ct)和目标坐标系锥台高度d对所述反向投影方程化简得到：

f₁(θ,φ,t_z,x′_w,y′_w,z′_w|u,v)＝0

根据该底面圆的约束方程可知z′_w＝0，结合简化后的反向投影方程计算得到反向投影方程为：

f₂(θ,φ,t_z,x′_w,y′_w|u,v)＝0；

步骤(4)：基于锥套结构约束关系解算位姿：

基于锥套结构的约束关系构建目标函数：

其中，r_b为底面圆半径，

为估计底面圆半径，x′_w和y′_w为估计的目标坐标系下底面圆轮廓坐标；

通过对待求位姿参数变量X＝[θ,φ,t_z]^T的求导得到目标函数的雅可比矩阵J，从而计算得到位姿参数的修正量ΔX，过程表示如下：

J_XΔX＝J

其中，J_X为雅可比矩阵对X的偏导；

根据上述计算得到的修正量ΔX，对位姿参数进行迭代解算：

X^k＝X^k-1+ΔX

经过k次迭代，当所有的底面圆约束和中心点位置约束都能同时满足时，目标函数迭代过程逐渐收敛为零，从而解算得到最优的位姿参数变量X；

在得到X的基础之上根据上述反向投影方程简化方程计算得到目标的姿态角

和平移向量t＝(t_x,t_y,t_z)。

2.根据权利要求1所述的一种基于特征反向投影的自主空中加油锥套位姿解算方法，其特征在于：

所述目标函数的求解最少需要图像底面圆特征点4个。

3.根据权利要求1所述的一种基于特征反向投影的自主空中加油锥套位姿解算方法，其特征在于：

步骤(4)中，进行迭代解算时，通过底面圆中心位置和顶面圆中心位置关系σ约束消除特征反向投影的二义性。

4.根据权利要求3所述的一种基于特征反向投影的自主空中加油锥套位姿解算方法，其特征在于：

所述中心位置关系σ约束为σ＝-sign(u_ct-u_cb)。

Images