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

权利要求书

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)。