一种基于回溯搜索的无人机空中加油位姿估计方法
【技术领域】
[0001] 本发明涉及一种基于回溯搜索的无人机空中加油位姿估计方法，属于无人机自主 控制领域。
【背景技术】
[0002] 无人机（Unmanned Aerial Vehicle, UAV)自走上历史舞台以来已在军事民用方面 得到了广泛的应用，其在军事上的作用更是随着军事化需求的不断升级而越来越重要。无 人机自主空中加油（Autonomous Aerial Refueling, ARR)技术可以显著提升无人机的作战 效能和作战机动性，已逐渐成为无人机的必备能力之一，是未来无人机技术发展的重要方 向。实现无人机自主空中加油的关键是实现受油机和加油机之间相对位置与姿态的准确实 时测量，并有效控制受油机和加油机，从而实现自主加油精确导引，保证整个加油过程的顺 利进行。为使受油机和加油机能够安全无碰撞地进行紧密对接，其相对位姿信息的准确性 显得尤为重要，是整个过程中进行飞行控制的基础。
[0003] 基于计算机视觉的无人机位姿测量近年来成为了无人机导航领域的一大研究热 点，并逐渐为该问题提供切实可行的解决方法。与传统的测量方法相比，视觉测量价格低、 功耗低、提供的信息量大，可远离电磁对抗的频率范围，具有很强的应用现实性。基于计算 机视觉的无人机位姿测量系统主要由三部分组成：目标检测、特征点提取以及位姿估计。本 专利主要针对无人机空中加油的位姿估计模块，提出一种基于回溯搜索的估计方法。
[0004] 透视η点定位（Perspective-n-Point，PnP)问题起源于摄像机标定，其根据空间 中η点在摄像机模型下的2维与3维映射关系，以及η点在参考坐标系和相机坐标系下的 不同坐标表示，来决定摄像机的位置与方位，是目前计算机视觉领域解决位姿估计问题的 常用方法。Shiqi Li等人提出的鲁棒透视11点（Robust Perspective-n_Point，RPnP)算法 为PnP问题提供了一个鲁棒的解析解，适用面广，能在普通3维点、共面点、拟奇异点等多种 空间特征点结构下进行位姿估计。
[0005] 回溯搜索算法是由Pinar Civicioglu于2013年提出的一种新型的基于群体智能 的启发式优化算法。该算法继承了差分进化算法的总体框架，结构简单，但搜索过程中的变 异和交叉操作又与差分进化算法有着本质的区别，其通过产生历史种群并控制搜索方向和 边界，大大地提高了算法的优化效率。在该算法的迭代过程中，历史种群能够不断设置为本 次迭代之前的种群位置，从而实现记忆功能。在历史种群的基础上，回溯搜索算法将两个基 本算子结合起来解决优化问题。
[0006] (1)变异算子
[0007] 不同于其他的优化算法，在种群的变异过程中，历史种群P°ld的位置对搜索方向起 到了决定性的作用，具体的更新规则如下式所示：
[0008] M = P+F (Pold-P) (1)
[0009] 式中，P和M分别是当前种群位置和变异后的种群位置，变异尺度系数F的大小控 制了在（p° ld-p)方向上搜索的幅度。
[0010] ⑵交叉算子
[0011] 交叉算子是在变异后的种群和当前种群之间进行的操作，为了更好地保持种群多 样性，提高算法跳出局部最优的能力，回溯搜索算法通过定义一个map矩阵来控制种群间 实际进行交叉的个体个数，具体步骤如下式所示：
[0013] 式中的map为一个取值为0或1的二元矩阵，该矩阵的生成过程为：首先将所有元 素初始化为1，再按照如下方式进行随机初始化：
[0015] 式中，D为种群的维数，rnd为（0, 1)区间均匀分布的随机数，mr为混合比例参数； u为向量[1，2，...，D]进行随机排序后的整数向量，代表个体维数的索引。当随机数a〈b 时，:「/wwm/·/) i)选出向量u中排在前「嫌个的元素，对map^量相应的维数 位置进行置零操作；当a多b时，量仅在randi (D)维上为0,其余元素均为初始化的 值1。
[0016] 进行完上述两个基本算子后，回溯搜索算法将采用贪婪选择策略在交叉后的种群 T与当前种群P中选择出适应度更高的个体作为新的种群个体，再根据一定的规则更新历 史种群P°ld，进入下一轮的迭代优化，直到满足停止迭代的条件。回溯搜索算法的整体流程 图如图1所示。
【发明内容】
[0017] 1、发明目的：
[0018] 本发明提出了一种基于回溯搜索的无人机空中加油位姿估计方法，其目的是提供 一种基于视觉测量的位姿估计方法，为无人机空中加油的精确导引提供一种解决方案，以 弥补其他测量方法在某些情况下的不足，提高无人机位姿测量的鲁棒性和精度，为空中加 油的顺利实现提供有效保障。
[0019] 该方法利用Matlab软件编写相应的位姿估计算法程序，利用Simulink三维视景 模拟整个空中加油环境，通过仿真得到空中加油过程中加油机和受油机之间相对六自由度 位姿信息的测量误差，在此误差基础上构建优化问题目标函数，利用回溯搜索算法求解出 最优的姿态估计值。
[0020] 2、技术方案：
[0021] 本发明利用基于群体智能的回溯搜索算法的全局搜索能力强，收敛速度快等特 点，开发一种基于回溯搜索的无人机空中加油位姿估计方法，该方法的具体步骤如下：
[0022] 步骤一：获取进行位姿估计所需信息
[0023] 本方法中用透视η点定位解法进行位姿估计，因此需要获取参与位姿估计运算的 所有特征点的信息，包括特征点的总个数，这些特征点在受油机坐标系下的位置信息、在安 装在加油机上的摄像机中的图像坐标值以及两者的对应关系，获取摄像机的内参信息等。
[0024] 步骤二：初始化回溯搜索算法参数
[0025] (1)初始化优化参数维数D
[0026] 本方法中用群体智能算法在所有的特征点中寻找两个最优的特征点来建立正交 坐标系，因此D设置为2。
[0027] (2)初始化种群数量N
[0028] 种群数量N与算法的优化效果关系紧密，较大的种群数量可能加速优化算法的收 敛速度，但同时也会增加计算复杂度，根据实际问题选择一个适当的种群数量，兼顾算法优 化的准确性和效率。
[0029] (3)初始化种群P和历史种群Fld位置
[0030] 由于回溯搜索算法中初始值对优化性能的影响较小，故在定义好的搜索空间中随 机产生种群个体，以初始化种群和历史种群。low为搜索空间的下限，up为搜索空间的上 限，则种群和历史种群中个体的每一维被初始化为：
[0032] 式中，i e [1，2,3，···Ν]，j e [1，2,3，...，D]，U 是随机均匀分布函数。
[0033] (4)设置算法重要参数
[0034] 对于随机搜索算法，一些重要参数的设置会影响算法的优化性能，本方法中，变异 尺度系数F的大小直接控制了变异算子操作中的搜索幅度，混合比例参数mr将影响种群间 交叉的个体个数。
[0035] (5)设置算法迭代次数NC
[0036] 算法迭代次数将直接影响算法的优化结果，设置算法的迭代次数应考虑算法对于 实际问题的平均收敛速度，以及应用中对于算法优化性能和时间的要求。
[0037] 步骤三：设计适应度函数
[0038] 适应度函数是群智能优化方法的核心，决定优化结果的实用性。本方法中，适应度 函数直接设计为估计出的位姿结果与真值之间的误差，如下式所示：
[0040] 式中，！\和R [R u R12 R13]分别为第i个个体所对应的平移向量和旋转矩阵的 估计值，T。和R。= [Rm Rra R1J分别为平移向量和旋转矩阵的真实值。
[0041] 步骤四：利用RPnP算法进行位姿估计
[0042] 种群中的每一个个体分别代表了两个不同的特征点，基于该两点组成的线段 ，可建立一个正交坐标系O aXaYaZa,建立方法如图2所示：定义^；的中点作为该坐 标系的原点oa，且Zjft的正方向与巧巧；;方向相同。建立好正交坐标系后，将所有特征点的 坐标值从受油机坐标系转换到该坐标系下，则加油机相对于受油机的相对位姿估计转换为 相机坐标系相对于坐标系OaXaYaZa的位姿解算问题。
[0043] 为求解该问题，将η个特征点分为（η-2)个子集，每一个子集包括三个不同的特征 点，如{Pi〇Pj〇P klk乒i〇, k乒jO}，根据三点约束，可得到如下多项式：
[0045] 求解上述多项式