[发明专利]一种基于回溯搜索的无人机空中加油位姿估计方法

权利要求书

1.一种基于回溯搜索的无人机空中加油位姿估计方法，其特征在于：该方法的具体步骤如下：

步骤一：获取进行位姿估计所需信息

用透视n点定位解法进行位姿估计，需要获取参与位姿估计运算的所有特征点的信息，包括特征点的总个数，这些特征点在受油机坐标系下的位置信息、在安装在加油机上的摄像机中的图像坐标值以及两者的对应关系，获取摄像机的内参信息；

步骤二：初始化回溯搜索算法参数

(1)初始化优化参数维数D

用群体智能算法在所有的特征点中寻找两个最优的特征点来建立正交坐标系，因此，D设置为2；

(2)初始化种群数量N

种群数量N与算法的优化效果关系紧密，较大的种群数量能加速优化算法的收敛速度，但同时也会增加计算复杂度，根据实际问题选择一个适当的种群数量，兼顾算法优化的准确性和效率；

(3)初始化种群P和历史种群P^old位置

由于回溯搜索算法中初始值对优化性能的影响较小，故在定义好的搜索空间中随机产生种群个体，以初始化种群和历史种群；low为搜索空间的下限，up为搜索空间的上限，则种群和历史种群中个体的每一维被初始化为：

{Pi,j~U(lowj,upj)Poldi,j~U(lowj,upj)---(4)]]>

式中，i∈[1,2,3,...N]，j∈[1,2,3,...,D]，U是随机均匀分布函数；

(4)设置算法重要参数

对于随机搜索算法，一些重要参数的设置会影响算法的优化性能，变异尺度系数F的大小直接控制了变异算子操作中的搜索幅度，混合比例参数mr将影响种群间交叉的个体个数；

(5)设置算法迭代次数NC

算法迭代次数将直接影响算法的优化结果，设置算法的迭代次数应考虑算法对于实际问题的平均收敛速度，以及应用中对于算法优化性能和时间的要求；

步骤三：设计适应度函数

适应度函数是群智能优化方法的核心，决定优化结果的实用性，适应度函数直接设计为估计出的位姿结果与真值之间的误差，如下式所示：

fitness(Pi)=||Ti-T0||T0+max(arccos(Ri1T·R01),arccos(Ri2T·R02),arccos(Ri3T·R06))---(5)]]>

式中，T_i和R_i＝[R_i1R_i2R_i3]分别为第i个个体所对应的平移向量和旋转矩阵的估计值，T₀和R₀＝[R₀₁R₀₂R₀₃]分别为平移向量和旋转矩阵的真实值；

步骤四：利用RPnP算法进行位姿估计

种群中的每一个个体分别代表了两个不同的特征点，基于该两点组成的线段能建立一个正交坐标系O_aX_aY_aZ_a，定义的中点作为该坐标系的原点O_a，且Z_a轴的正方向与方向相同，建立好正交坐标系后，将所有特征点的坐标值从受油机坐标系转换到该坐标系下，则加油机相对于受油机的相对位姿估计转换为相机坐标系相对于坐标系O_aX_aY_aZ_a的位姿解算问题；

为求解该问题，将n个特征点分为(n-2)个子集，每一个子集包括三个不同的特征点，如{P_i0P_j0P_k|k≠i0,k≠j0}，根据三点约束，得到如下多项式：

f1(x)=α1x4+β1x3+χ1x2+δ1x+e1=0,f2(x)=α2x4+β2x3+χ2x2+δ2x+e2=0,...,fn-2(x)=αn-2x4+βn-2x3+χn-2x2+δn-2x+en-2=0.---(6)]]>

求解上述多项式，得到正交坐标系O_aX_aY_aZ_a中P_i0和P_j0之间的深度，从而计算出旋转轴Z_a以及绕Z_a轴旋转的旋转角α；确定旋转轴后，从坐标系O_aX_aY_aZ_a到相机坐标系的旋转矩阵表示如下：

Ri=Rrot(Za,α)=r1r4r7r2r5r8r3r6r9cosα-sinα0sinαcosα0001---(7)]]>

式中，R为第三列元素与Z_a相等的正交旋转矩阵，再根据相机参数λ和空间三维点的投影关系，得下式：

λp_j＝R_iP_j+T_i(8)

式中，P_j(j＝1,2,...,n)是空间三维点，p_j(j＝1,2,...,n)是空间三维点对应的图像坐标，最后，将Ri=Rrot(Za,α)=r1r4r7r2r5r8r3r6r9cosα-sinα0sinαcosα0001]]>式(7)带入式(8)，求解出个体i对应的旋转矩阵R_i和平移向量T_i；

步骤五：利用变异交叉算子进行寻优

利用初始化的群体和历史群体的位置，根据M＝P+F(P^old-P)式(1)对当前群体进行变异操作，历史群体决定了当前群体变异的方向，变异尺度系数限制了在该方向上变异的幅度，得到变异群体M后，进行交叉操作，式中，P和M分别是当前种群位置和变异后的种群位置，变异尺度系数F的大小控制了在(P^old-P)方向上搜索的幅度；根据Ti,j=Mi,j,mapi,j=1Pi,j,mapi,j=0]]>式(2)在变异群体和当前群体之间进行交叉操作，实际交叉

的个体取决于map矩阵的生成；式中的map为一个取值为0或1的二元矩阵，该矩阵的生成过程为：首先将所有元素初始化为1，再按照如下方式进行随机初始化：

式中，D为种群的维数，rnd为(0,1)区间均匀分布的随机数，mr为混合比例参数；u为向量[1,2,...,D]进行随机排序后的整数向量，代表个体维数的索引；当随机数a<b时，选出向量u中排在前个的元素，对map_i向量相应的维数位置进行置零操作；当a≥b时，map_i向量仅在randi(D)维上为0，其余元素均为初始化的值1；

在交叉算子使用之后，计算每个交叉后个体的适应度值，即RPnP算法在该个体所选择的两个特征点建立起的正交坐标系的基础上对相对位姿的估计误差，具体计算过程见步骤三和步骤四；如果交叉后的个体适应度值小于当前个体的适应度值，则将交叉后个体作为新种群中的个体，反之则不改变当前个体，继续下一轮寻优，当运行次数大于算法设定的最大迭代次数时，停止寻优过程；

步骤六：储存结果并验证

在停止寻优后的当前群体中选择出全局最优值，即适应度值最小的个体，将此结果保存，此结果所对应的位姿估计值为估计误差最小的值；在后续对该特征点集的位姿估计中，直接采用此结果建立正交坐标系，进行位姿解算，不需要重复寻优过程，得到的位姿估计结果仍然具有高精度。