CN105300383A - 一种基于回溯搜索的无人机空中加油位姿估计方法 - Google Patents

Abstract

本发明是一种基于回溯搜索的无人机空中加油位姿估计方法，其实施步骤为：步骤一：获取进行位姿估计所需信息；步骤二：初始化回溯搜索算法参数；步骤三：设计代价函数；步骤四：利用鲁棒透视n点算法进行位姿估计；步骤五：利用变异交叉算子进行寻优；步骤六：储存结果并验证。本发明提出的位姿估计算法具有很高的准确性和鲁棒性，可在无人机空中加油的精确导引过程中使用，保证对接过程的安全顺利进行。

Description

一种基于回溯搜索的无人机空中加油位姿估计方法

技术领域

本发明涉及一种基于回溯搜索的无人机空中加油位姿估计方法，属于无人机自主控制领域。

背景技术

无人机(UnmannedAerialVehicle,UAV)自走上历史舞台以来已在军事民用方面得到了广泛的应用，其在军事上的作用更是随着军事化需求的不断升级而越来越重要。无人机自主空中加油(AutonomousAerialRefueling,ARR)技术可以显著提升无人机的作战效能和作战机动性，已逐渐成为无人机的必备能力之一，是未来无人机技术发展的重要方向。实现无人机自主空中加油的关键是实现受油机和加油机之间相对位置与姿态的准确实时测量，并有效控制受油机和加油机，从而实现自主加油精确导引，保证整个加油过程的顺利进行。为使受油机和加油机能够安全无碰撞地进行紧密对接，其相对位姿信息的准确性显得尤为重要，是整个过程中进行飞行控制的基础。

基于计算机视觉的无人机位姿测量近年来成为了无人机导航领域的一大研究热点，并逐渐为该问题提供切实可行的解决方法。与传统的测量方法相比，视觉测量价格低、功耗低、提供的信息量大，可远离电磁对抗的频率范围，具有很强的应用现实性。基于计算机视觉的无人机位姿测量***主要由三部分组成：目标检测、特征点提取以及位姿估计。本专利主要针对无人机空中加油的位姿估计模块，提出一种基于回溯搜索的估计方法。

透视n点定位(Perspective-n-Point，PnP)问题起源于摄像机标定，其根据空间中n点在摄像机模型下的2维与3维映射关系，以及n点在参考坐标系和相机坐标系下的不同坐标表示，来决定摄像机的位置与方位，是目前计算机视觉领域解决位姿估计问题的常用方法。ShiqiLi等人提出的鲁棒透视n点(RobustPerspective-n-Point,RPnP)算法为PnP问题提供了一个鲁棒的解析解，适用面广，能在普通3维点、共面点、拟奇异点等多种空间特征点结构下进行位姿估计。

回溯搜索算法是由PinarCivicioglu于2013年提出的一种新型的基于群体智能的启发式优化算法。该算法继承了差分进化算法的总体框架，结构简单，但搜索过程中的变异和交叉操作又与差分进化算法有着本质的区别，其通过产生历史种群并控制搜索方向和边界，大大地提高了算法的优化效率。在该算法的迭代过程中，历史种群能够不断设置为本次迭代之前的种群位置，从而实现记忆功能。在历史种群的基础上，回溯搜索算法将两个基本算子结合起来解决优化问题。

(1)变异算子

不同于其他的优化算法，在种群的变异过程中，历史种群P^old的位置对搜索方向起到了决定性的作用，具体的更新规则如下式所示：

M＝P+F(P^old-P)(1)

式中，P和M分别是当前种群位置和变异后的种群位置，变异尺度系数F的大小控制了在(P^old-P)方向上搜索的幅度。

(2)交叉算子

交叉算子是在变异后的种群和当前种群之间进行的操作，为了更好地保持种群多样性，提高算法跳出局部最优的能力，回溯搜索算法通过定义一个map矩阵来控制种群间实际进行交叉的个体个数，具体步骤如下式所示：

$T_{i,j}=\left\{\begin{array}{cc}{M}_{i,j},& {\mathrm{map}}_{i,j}=1\\ P_{i,j},& {\mathrm{map}}_{i,j}=0\end{array}\right.---\left(2\right)$

式中的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。

进行完上述两个基本算子后，回溯搜索算法将采用贪婪选择策略在交叉后的种群T与当前种群P中选择出适应度更高的个体作为新的种群个体，再根据一定的规则更新历史种群P^old，进入下一轮的迭代优化，直到满足停止迭代的条件。回溯搜索算法的整体流程图如图1所示。

发明内容

1、发明目的：

本发明提出了一种基于回溯搜索的无人机空中加油位姿估计方法，其目的是提供一种基于视觉测量的位姿估计方法，为无人机空中加油的精确导引提供一种解决方案，以弥补其他测量方法在某些情况下的不足，提高无人机位姿测量的鲁棒性和精度，为空中加油的顺利实现提供有效保障。

该方法利用Matlab软件编写相应的位姿估计算法程序，利用Simulink三维视景模拟整个空中加油环境，通过仿真得到空中加油过程中加油机和受油机之间相对六自由度位姿信息的测量误差，在此误差基础上构建优化问题目标函数，利用回溯搜索算法求解出最优的姿态估计值。

2、技术方案：

本发明利用基于群体智能的回溯搜索算法的全局搜索能力强，收敛速度快等特点，开发一种基于回溯搜索的无人机空中加油位姿估计方法，该方法的具体步骤如下：

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

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

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

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

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

(2)初始化种群数量N

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

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

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

$\left\{\begin{array}{c}{P}_{i,j}~U\left({\mathrm{low}}_j,{\mathrm{up}}_j\right)\\ {P^{old}}_{i,j}~U\left({\mathrm{low}}_j,{\mathrm{up}}_j\right)\end{array}\right.---\left(4\right)$

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

(4)设置算法重要参数

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

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

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

步骤三：设计适应度函数

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

$fitness\left(P_i\right)=\frac{\left|\right|T_i-T_0\left|\right|}{T_0}+\max \left(\arccos \left({R_{i1}}^T\&CenterDot;R_{01}\right),\arccos \left({R_{i2}}^T\&CenterDot;R_{02}\right),\arccos \left({R_{i3}}^T\&CenterDot;R_{03}\right)\right)---\left(5\right)$

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

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

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

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

$\left\{\begin{array}{c}{f}_1\left(x\right)={\mathrm{\&alpha;}}_1{x}^4+{\mathrm{\&beta;}}_1{x}^3+{\mathrm{\&chi;}}_1{x}^2+{\mathrm{\&delta;}}_{1}x+e_1=0,\\ f_2\left(x\right)={\mathrm{\&alpha;}}_2{x}^4+{\mathrm{\&beta;}}_2{x}^3+{\mathrm{\&chi;}}_2{x}^2+{\mathrm{\&delta;}}_{2}x+e_2=0,\\ ...,\\ f_{n-2}\left(x\right)={\mathrm{\&alpha;}}_{n-2}{x}^4+{\mathrm{\&beta;}}_{n-2}{x}^3+{\mathrm{\&chi;}}_{n-2}{x}^2+{\mathrm{\&delta;}}_{n-2}x+e_{n-2}=0.\end{array}\right.---\left(6\right)$

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

$R_i=Rrot(Z_a,\mathrm{\&alpha;})=\left[\begin{array}{ccc}{r}_1& r_4& r_7\\ r_2& r_5& r_8\\ r_3& r_6& r_9\end{array}\right]\left[\begin{array}{ccc}cos\mathrm{\&alpha;}& -sin\mathrm{\&alpha;}& 0\\ sin\mathrm{\&alpha;}& \cos \mathrm{\&alpha;}& 0\\ 0& 0& 1\end{array}\right]---\left(7\right)$

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

λp_j＝R_iP_j+T_i(8)

式中，P_j(j＝1,2,...,n)是空间三维点，p_j(j＝1,2,...,n)是空间三维点对应的图像坐标，最后，将 $R_i=Rrot(Z_a,\mathrm{\&alpha;})=\left[\begin{array}{ccc}{r}_1& r_4& r_7\\ r_2& r_5& r_8\\ r_3& r_6& r_9\end{array}\right]\left[\begin{array}{ccc}cos\mathrm{\&alpha;}& -sin\mathrm{\&alpha;}& 0\\ sin\mathrm{\&alpha;}& \cos \mathrm{\&alpha;}& 0\\ 0& 0& 1\end{array}\right]$ 式(7)带入式(8)，求解出个体i对应的旋转矩阵R_i和平移向量T_i。

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

利用初始化的群体和历史群体的位置，根据M＝P+F(P^old-P)式(1)对当前群体进行变异操作，历史群体决定了当前群体变异的方向，变异尺度系数限制了在该方向上变异的幅度。得到变异群体M后，进行交叉操作；式中，P和M分别是当前种群位置和变异后的种群位置，变异尺度系数F的大小控制了在(P^old-P)方向上搜索的幅度。根据 $T_{i,j}=\left\{\begin{array}{cc}{M}_{i,j},& {\mathrm{map}}_{i,j}=1\\ P_{i,j},& {\mathrm{map}}_{i,j}=0\end{array}\right.$ 式(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算法在该个体所选择的两个特征点建立起的正交坐标系的基础上对相对位姿的估计误差，具体计算过程见步骤三和步骤四。如果交叉后的个体适应度值小于当前个体的适应度值，则将交叉后个体作为新种群中的个体，反之则不改变当前个体。继续下一轮寻优，当运行次数大于算法设定的最大迭代次数时，停止寻优过程。

步骤六：储存结果并验证

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

3、优点及效果：

本发明提出了一种基于回溯搜索的无人机空中加油位姿估计方法，其目的是提供一种基于视觉测量的位姿估计方法，为无人机空中加油的精确导引提供一种解决方案。本方法有效地降低了位姿估计的误差，提高了位姿测量的鲁棒性和准确性，为无人机空中加油的精确导引过程提供了有效保障。

附图说明

图1回溯搜索算法整体流程图。

图2利用种群个体建立正交坐标系示意图。

图3X轴相对位置估计误差示意图。

图4Y轴相对位置估计误差示意图。

图5Z轴相对位置估计误差示意图。

图6X轴相对姿态估计误差示意图。

图7Y轴相对姿态估计误差示意图。

图8Z轴相对姿态估计误差示意图。

图中标号及符号说明如下：

O_c——摄像机光轴中心

P_i0P_j0——个体所选择的两点

O_aX_aY_aZ_a——基于个体所选择的两点建立的正交坐标系

P_j——空间3维点

p_j——空间3维点在图像平面的投影

具体实施方式

下面通过一个具体的无人机位姿估计实例来验证本发明所提出的位姿估计方法的有效性。本实例中的受油机使用的是B-2模型，标志物安装在受油机的上表面，其中包括用来进行位姿估计的7个特征点。虚拟相机模型安装在飞行在受油机前面的加油机尾部，不断地获取受油机图像。本实例中加油机在受油机坐标系下匀速前进接近受油机，没有相对姿态变化，两架飞机相对速度为0.1m/s，仿真时间50s，相对距离由-33m到-28m。实验计算机配置为i5-3230M处理器，2.60GHz主频，4G内存，软件环境为R2012b版本的Matlab。

本实例的具体实现步骤如下：

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

本实例中，受油机标志物上的特征点总数为7个，它们在受油机坐标系下的坐标见表一，经过目标检测和特征提取，这7个特征点在虚拟相机拍摄到的图像中的对应图像坐标见表二。该虚拟相机的内参矩阵如下：

$K=\left[\begin{array}{ccc}483& 0& 200\\ 0& 483& 200\\ 0& 0& 1\end{array}\right]---\left(9\right)$

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

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

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

(2)初始化种群数量N

由于本实例中的特征点数量较少，优化相对容易，同时为了降低计算复杂度，种群数量N设置为10。种群数量应与实际应用中的特征点数目总体上成正比关系，因此在特征点数量较多的实例中，种群数量N应适当增大。

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

见图1，回溯搜索算法在本方法中用来从所有特征点中选择最优的两个特征点，以此建立正交坐标系进行位姿估计，因此种群的搜索空间应为两个不同特征点的所有组合。故low和up为特征点的序号最小值和最大值，本例中low设为1，up设为7。定义好搜索空间后，按照 $\left\{\begin{array}{c}{P}_{i,j}~U({\mathrm{low}}_j,{\mathrm{up}}_j)\\ {P^{\mathrm{old}}}_{i,j}~U({\mathrm{low}}_j,{\mathrm{up}}_j)\end{array}\right.$ 式(4)随机初始化种群P和历史种群P^old。

(4)设置算法重要参数

本实例中，变异尺度系数F的大小直接控制了变异算子操作中的搜索幅度，为保持种群的多样性，利用随机数设置变异尺度系数为F＝3×rnd，其中rnd为(0,1)区间均匀分布的随机数，混合比例参数mr设置为1。

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

本实例的搜索空间不大，相应的迭代次数可尽量减少，基于对算法优化性能的了解，设置迭代次数NC为30。

步骤三：设计适应度函数

本实例中，适应度函数直接设计为估计出的位姿结果与真值之间的误差，如下式所示：

$fitness\left(P_i\right)=\frac{\left|\right|T_i-T_0\left|\right|}{T_0}+\max \left(\arccos \left({R_{i1}}^T\&CenterDot;R_{01}\right),\arccos \left({R_{i2}}^T\&CenterDot;R_{02}\right),\arccos \left({R_{i3}}^T\&CenterDot;R_{03}\right)\right)---\left(10\right)$

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

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

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

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

$\left\{\begin{array}{c}{f}_1\left(x\right)={\mathrm{\&alpha;}}_1{x}^4+{\mathrm{\&beta;}}_1{x}^3+{\mathrm{\&chi;}}_1{x}^2+{\mathrm{\&delta;}}_{1}x+e_1=0,\\ f_2\left(x\right)={\mathrm{\&alpha;}}_2{x}^4+{\mathrm{\&beta;}}_2{x}^3+{\mathrm{\&chi;}}_2{x}^2+{\mathrm{\&delta;}}_{2}x+e_2=0,\\ ...,\\ f_{n-2}\left(x\right)={\mathrm{\&alpha;}}_{n-2}{x}^4+{\mathrm{\&beta;}}_{n-2}{x}^3+{\mathrm{\&chi;}}_{n-2}{x}^2+{\mathrm{\&delta;}}_{n-2}x+e_{n-2}=0.\end{array}\right.---\left(11\right)$

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

$R_i=Rrot(Z_a,\mathrm{\&alpha;})=\left[\begin{array}{ccc}{r}_1& r_4& r_7\\ r_2& r_5& r_8\\ r_3& r_6& r_9\end{array}\right]\left[\begin{array}{ccc}cos\mathrm{\&alpha;}& -sin\mathrm{\&alpha;}& 0\\ sin\mathrm{\&alpha;}& \cos \mathrm{\&alpha;}& 0\\ 0& 0& 1\end{array}\right]---\left(12\right)$

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

λp_j＝R_iP_j+T_i(13)

式中，P_j(j＝1,2,...,n)是空间三维点，p_j(j＝1,2,...,n)是空间三维点对应的图像坐标，最后，将 $R_i=Rrot(Z_a,\mathrm{\&alpha;})=\left[\begin{array}{ccc}{r}_1& r_4& r_7\\ r_2& r_5& r_8\\ r_3& r_6& r_9\end{array}\right]\left[\begin{array}{ccc}cos\mathrm{\&alpha;}& -sin\mathrm{\&alpha;}& 0\\ sin\mathrm{\&alpha;}& \cos \mathrm{\&alpha;}& 0\\ 0& 0& 1\end{array}\right]$ 式(12)带入λp_j＝R_iP_j+T_i式(13)，求解出个体i对应的旋转矩阵R_i和平移向量T_i。

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

利用初始化的群体和历史群体的位置，根据M＝P+F(P^old-P)式(1)对当前群体进行变异操作，历史群体决定了当前群体变异的方向，变异尺度系数限制了在该方向上变异的幅度。得到变异群体M后，进行交叉操作。根据 $T_{i,j}=\left\{\begin{array}{cc}{M}_{i,j},& {\mathrm{map}}_{i,j}=1\\ P_{i,j},& {\mathrm{map}}_{i,j}=0\end{array}\right.$ 式(2)在变异群体和当前群体之间进行交叉操作，实际交叉的个体取决于map矩阵的生成。在交叉算子使用之后，计算每个交叉后个体的适应度值，即RPnP算法在该个体所选择的两个特征点建立起的正交坐标系的基础上对相对位姿的估计误差，具体计算过程见步骤三和步骤四。如果交叉后的个体适应度值小于当前个体的适应度值，则将交叉后个体作为新种群中的个体，反之则不改变当前个体。继续下一轮寻优，当运行次数大于算法设定的最大迭代次数时，停止寻优过程。

步骤六：储存结果并验证

在停止寻优后的当前群体中选择出全局最优值，即适应度值最小的个体，将此结果保存，此结果所对应的位姿估计值为估计误差最小的值。在后续对该特征点集的位姿估计中，可直接采用此结果建立正交坐标系，进行位姿解算，不需要重复寻优过程，得到的位姿估计结果仍然具有高精度。用此优化结果进行连续50s的动态实时位姿估计，XYZ三轴的相对位置估计误差如图3-5所示，绕XYZ三轴的相对姿态估计误差如图6-8所示。

由实例中的仿真结果可以看出，本发明提出的位姿估计方法能够实时准确地对无人机相对位姿进行估计，其中，对于相对位置的估计，X轴方向平均估计误差为0.0136米，标准差为0.0029米，Y轴方向的平均估计误差为0.0095米，标准差为0.0069米，Z轴方向的平均估计误差为0.0091米，标准差为0.0046米；对于相对姿态的估计，绕X轴方向的平均估计误差为0.0858度，标准差为0.1338度，绕Y轴方向的平均估计误差为-0.0442度，标准差为0.1092度，绕Z轴方向的平均估计误差为-0.0428度，标准差为0.0874度。可见本发明提出的位姿估计算法具有很高的准确性和鲁棒性，可在无人机空中加油的精确导引过程中使用，保证对接过程的安全顺利进行。

特征点在受油机坐标系下的坐标见下表一：

表一(单位：米)

特征点对应的图像坐标见下表二：

表二(单位：像素)

Claims (1)

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

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

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

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

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

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

(2)初始化种群数量N

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

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

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

$\{\begin{array}{c}{P}_{i,j}~U\left({\mathrm{low}}_j,{\mathrm{up}}_j\right)\\ {P^{old}}_{i,j}~U\left({\mathrm{low}}_j,{\mathrm{up}}_j\right)\end{array}---\left(4\right)$

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

(4)设置算法重要参数

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

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

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

步骤三：设计适应度函数

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

$fitness\left(P_i\right)=\frac{\left|\right|T_i-T_0\left|\right|}{T_0}+\max \left(\arccos \left({R_{i1}}^T\&CenterDot;R_{01}\right),\arccos \left({R_{i2}}^T\&CenterDot;R_{02}\right),arc\cos \left({R_{i3}}^T\&CenterDot;R_{06}\right)\right)---\left(5\right)$

式中，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}，根据三点约束，得到如下多项式：

$\left\{\begin{array}{c}{f}_1\left(x\right)={\mathrm{\&alpha;}}_1{x}^4+{\mathrm{\&beta;}}_1{x}^3+{\mathrm{\&chi;}}_1{x}^2+{\mathrm{\&delta;}}_{1}x+e_1=0,\\ f_2\left(x\right)={\mathrm{\&alpha;}}_2{x}^4+{\mathrm{\&beta;}}_2{x}^3+{\mathrm{\&chi;}}_2{x}^2+{\mathrm{\&delta;}}_{2}x+e_2=0,\\ ...,\\ f_{n-2}\left(x\right)={\mathrm{\&alpha;}}_{n-2}{x}^4+{\mathrm{\&beta;}}_{n-2}{x}^3+{\mathrm{\&chi;}}_{n-2}{x}^2+{\mathrm{\&delta;}}_{n-2}x+e_{n-2}=0.\end{array}\right.---\left(6\right)$

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

$R_i=Rrot(Z_a,\mathrm{\&alpha;})=\left[\begin{array}{ccc}{r}_1& r_4& r_7\\ r_2& r_5& r_8\\ r_3& r_6& r_9\end{array}\right]\left[\begin{array}{ccc}cos\mathrm{\&alpha;}& -sin\mathrm{\&alpha;}& 0\\ sin\mathrm{\&alpha;}& \cos \mathrm{\&alpha;}& 0\\ 0& 0& 1\end{array}\right]---\left(7\right)$

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

λp_j＝R_iP_j+T_i(8)

式中，P_j(j＝1,2,...,n)是空间三维点，p_j(j＝1,2,...,n)是空间三维点对应的图像坐标，最后，将 $R_i=Rrot(Z_a,\mathrm{\&alpha;})=\left[\begin{array}{ccc}{r}_1& r_4& r_7\\ r_2& r_5& r_8\\ r_3& r_6& r_9\end{array}\right]\left[\begin{array}{ccc}cos\mathrm{\&alpha;}& -sin\mathrm{\&alpha;}& 0\\ sin\mathrm{\&alpha;}& \cos \mathrm{\&alpha;}& 0\\ 0& 0& 1\end{array}\right]$ 式(7)带入式(8)，求解出个体i对应的旋转矩阵R_i和平移向量T_i；

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

利用初始化的群体和历史群体的位置，根据M＝P+F(P^old-P)式(1)对当前群体进行变异操作，历史群体决定了当前群体变异的方向，变异尺度系数限制了在该方向上变异的幅度，得到变异群体M后，进行交叉操作，式中，P和M分别是当前种群位置和变异后的种群位置，变异尺度系数F的大小控制了在(P^old-P)方向上搜索的幅度；根据 $T_{i,j}=\left\{\begin{array}{cc}{M}_{i,j},& {\mathrm{map}}_{i,j}=1\\ P_{i,j},& {\mathrm{map}}_{i,j}=0\end{array}\right.$ 式(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算法在该个体所选择的两个特征点建立起的正交坐标系的基础上对相对位姿的估计误差，具体计算过程见步骤三和步骤四；如果交叉后的个体适应度值小于当前个体的适应度值，则将交叉后个体作为新种群中的个体，反之则不改变当前个体，继续下一轮寻优，当运行次数大于算法设定的最大迭代次数时，停止寻优过程；

步骤六：储存结果并验证

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