CN111258231A - 仿猛禽视觉导航的自主空中加油对接半物理系统及其方法 - Google Patents

Abstract

本发明公开一种仿猛禽视觉导航的自主空中加油对接半物理系统及其方法；系统包括加/受油机抗干扰底层控制模块、多干扰软管‑锥套稳定控制模块、视景与显示模块、使能与选择模块、仿生视觉近距相对导航模块、受油机相对位置精确控制模块。本发明提高了锥套区域及标志点的检测效率及准确性；给出了仿生视觉导航与GPS导航源的切换逻辑，提供三种不同视觉导航方法，解决锥套距离较远或标志点被部分遮挡情况下的视觉导航问题且视觉导航精度较高，能够实现对接区域全过程视觉导航，提高视觉导航抗干扰性、稳定性；设计了GPS与视觉导航信号下的受油机相对位置控制律，能较好抑制近距对接过程中受油机受到的不同风干扰，提高相对位置控制精度。

Description

仿猛禽视觉导航的自主空中加油对接半物理系统及其方法

技术领域

本发明涉及一种仿猛禽视觉导航的自主空中加油对接半物理系统及其方法，属于无人机自主导航与控制领域。

背景技术

自主空中加油(Autonomous Aerial Refueling,AAR)技术是解决无人机在载重和续航能力上的缺陷提高无人机性能的一种有效途径，空中加油技术通过提升无人机的续航能力可以显著的提高无人机的任务执行能力，提高无人机的自主性。

软式自主空中加油主要分为会合、对接、加油和分离四个阶段，相比于其它阶段，软式空中加油对接阶段面临的情况最复杂，对导航与控制系统的要求最高、最难。软式自主空中加油对接阶段不仅需要准确获取多种风干扰下加油锥套的位姿信息，而且需要对受油机进行相对位置精确控制，因此设计合理高效的无人机自主空中加油导航与控制方法至关重要。本发明旨在通过设计一种基于仿猛禽视觉导航的自主空中加油对接系统及其方法，提高无人受油机在对接过程中的近距导航与相对位置控制精度，促进软式自主空中加油的成功实现，提高无人机的自主能力。

目前，自主空中加油过程中常用的导航技术包括惯性导航系统、差分GPS(GlobalPositioning System，GPS)系统等。惯性导航系统存在随着时间不断累加误差的缺陷；GPS技术虽然使用起来非常方便，但是由于其依靠卫星来定位，精度和抗干扰能力有待加强。再者，由于与加油机相连的锥套在多种风干扰下相对于加油机的位置和姿态不断变化，即使通过惯性和GPS导航得到了加/受油机之间的相对位置和姿态信息，也无法实现加油锥套和受油探头之间的准确对接，因此传统的导航方式无法实现加油锥套与受油探头的近距相对导航。同时，在自主空中加油不同阶段受油机的控制目标不同，对接阶段的前后存在GPS与视觉信号的切换，因此需要设计无人受油机前向、侧向与高度方向的精确相对位置控制。本发明针对现有软式自主空中加油导航方法在加油锥套与受油探头近距导航上的可行性、抗干扰性以及精确性等方面的不足，并且考虑GPS与视觉切换的受油机位置精确控制率，基于仿生视觉设计了仿猛禽视觉导航的自主空中加油对接半物理系统及其方法。

视觉导航技术通过图像处理来提取目标的特征点进行位姿测量，从而为无人机提供导引信息。相对于其它导航系统，视觉传感器具有轻便、低功耗、体积小、重量轻、成本低等特点，此外视觉导航系统的工作波段远离当前电磁对抗的频率范围，在近距测量锥套的位置与姿态信息时精度较高。鉴于视觉导航能够满足多风干扰下加油锥套摆动过程中的位姿测量要求，与对接阶段近距相对导航的稳定性、精确性、抗干扰性等要求有着紧密的契合之处，本发明提出了一种基于仿猛禽视觉导航的自主空中加油对接半物理系统及其方法，以解决现有软式自主空中加油对接阶段锥套位姿精确测量与受油机抗多风干扰位置精确控制上的不足，中国发明专利申请公开号为CN109085845A，名称为一种自主空中加油对接仿生视觉导航控制系统及方法的发明专利申请，给出了一种针对自主空中加油对接导航与控制问题的系统及方法，但其缺陷是系统采用全数字仿真，仿真系统不够真实；同时对近距导航过程中远距标志点检测不到及近距标志点遮挡两种情况采用相同的处理方式，造成近距遮挡时视觉导航重投影误差较大；并且控制系统在近距对接过程中没有考虑多风干扰的影响，所设计的控制系统适用性较差。本发明提出的半物理系统及方法从更加全面、真实的角度解决自主空中加油对接过程中的导航与控制问题，以促进软式自主空中加油近距对接地顺利实现，有效提高无人机自主性。

发明内容

本发明目的是提供一种仿猛禽视觉导航的自主空中加油对接半物理系统及其方法，旨在提高自主空中加油对接阶段近距相对导航的可靠性、抗干扰性以及精确性，并且设计与之相匹配的带有导航源切换的相对位置精确控制方法，从而提高近距导航与控制的精度，促进软式自主空中加油的顺利实现，提高无人机的自主能力水平。

本发明一种仿猛禽视觉导航的自主空中加油对接半物理系统及其方法，具体如下：

系统框架如图1所示，基于仿猛禽视觉导航的自主空中加油对接半物理系统由6部分组成，分别为：1)加/受油机抗干扰底层控制模块、2)多干扰软管-锥套稳定控制模块、3)视景与显示模块、4)使能与选择模块、5)仿生视觉近距相对导航模块、6)受油机相对位置精确控制模块，其中1)、2)、3)的计算机间UDP通信、4)、5)、6)模块在任务处理计算机上搭建，3)的FlightGear视景与视景显示在另一台视景显示计算机上搭建。

1)加/受油机抗干扰底层控制模块，包括受油机控制与加油机控制。对于受油机控制，采用反步设计方法将受油机高阶非线性系统分为从外到内四个递归的控制回路，分别为位置、航迹、气流角、姿态角速率控制回路，来实现受油机高度方向与侧向位置的精确控制。同时，为降低三个方向位置共同控制的复杂度，将前向位置控制从其它两个方向位置控制中分离出来，对应受油机底层速度控制，通过控制速度来消除前向位置偏差。姿态角速率控制得到副翼、方向舵、升降舵的偏转量，角速率控制采用等效滑模控制形式，并采用线性干扰观测器对角速率干扰量(风干扰、模型不确定性等)进行估计对等效滑模控制的切换控制权值进行自适应，以降低等效滑模控制的抖振。自内向外的控制回路即气流角、航迹、位置控制回路，以及速度控制回路，在“观测器+控制器”的二元抗干扰控制理论框架下，采用自抗扰控制控制形式，线性干扰观测器对各个控制回路的干扰量进行观测，并将观测干扰量引入控制律中，控制律采用反馈线性化的方式得到相应内环的控制量，实现对干扰量的抑制和期望输出的响应。本发明加油机假设为定直平飞状态，俯仰角保持配平状态值，滚转角、偏航角保持为零。

2)多干扰软管-锥套稳定控制模块，包括软管-锥套模型、多种大气扰动模型(大气紊流、受油机头波、加油机尾流)以及锥套位置稳定控制。具体如图2所示，软管-锥套模型，包括软管多刚体动力学与运动学、锥套动力学与运动学，软管-锥套组合体与加油机固连在一起；多种大气扰动模型，包括大气紊流、加油机尾流、受油机头波模型，建立的软管-锥套模型由多段质量集中的连杆与可控锥套组成，各段连杆与锥套容易受到多种大气扰动的影响，多种大气扰动的合成风速会叠加到软管及锥套的质心处，进而影响软管与锥套受到的气动力，因此软管与锥套的稳定位置会有较大的震荡，很难实现自主对接；锥套位置稳定控制，包括锥套侧向位置分数阶PID控制器、垂向位置分数阶PID控制器、锥套作动器分配，为了降低自主对接的难度，根据锥套稳定位置分别设计锥套位置侧向与垂向的分数阶PID控制器，得到侧向与垂向的期望主动控制力，并根据锥套气动，分配锥套相应的作动器产生实际的主动控制力，减小锥套飘摆的范围，降低自主对接的难度。

3)视景与显示模块，包括FlightGear视景、视景显示以及计算机间UDP通信。由于单个计算机计算能力有限，将FlightGear视景以及视景显示在视景显示计算机上搭建。采用3D 构图软件绘制加/受油机以及软管-锥套的3D模型，导入FlightGear构建空中加油视景，并且经计算机显示器实现视景显示的作用。导航与控制其它模块在任务处理计算机上，FlightGear 视景驱动数据在任务处理计算机由导航与控制其它模块计算得到，并通过计算机间的UDP 通信程序发送至视景显示计算机上控制自主空中加油视景的正常运行。

4)使能与选择模块，包括视觉使能、视觉导航方法选择、导航源选择。视觉使能(①)，使得仿猛禽视觉导航系统工作，通过工业相机获取视景计算机显示器上的图像，进而进行视觉导航处理。首先进行视觉导航方法选择(②)，视觉导航方法选择，包括工业相机获取视觉图像、Matlab相机接口、仿猛禽视觉标志点检测、判断是否检测到所有标志点、判断加/ 受油机相对距离是否小于某一阈值、与操作，具体来说，工业相机采集视景计算机FlightGear 加油场景图像传输至任务处理计算机，任务处理计算机通过Matlab相机数据获取接口得到相机采集图像，对工业相机采集到的图像进行仿猛禽视觉锥套标志点检测，根据是否检测到所设计的锥套上的所有标志点，判断是使用锥套标志点检测正常的导航方法还是锥套遮挡或距离远的导航方法，然后根据加/受油机相对距离判断选取锥套近距遮挡还是锥套距离远的导航方法，当锥套距离较远时，锥套标志点较小无法检测到，使用锥套距离远的导航方式，当锥套近距时，标志点被受油插头或者受油机机头部分遮挡，选择锥套近距遮挡的导航方式；导航源选择(③)，根据受油插头与加油锥套之间的视觉解算位置差以及加/受油机之间的距离，判断使用GPS还是视觉导航信号。

5)仿生视觉近距相对导航模块，包括锥套检测正常、锥套距离远、锥套近距遮挡三种情况。根据视觉导航方法选择结果，如果锥套标志点检测正常则进行标志点匹配，进而进行精确位姿估计；如果锥套距离过远，造成标志点不能全部检测到，则根据锥套颜色信息进行椭圆拟合，进而进行位姿估计；当锥套近距出现标志点部分遮挡，根据标志点的先验信息进行标志点预测、匹配，然后进行位姿估计。根据选择的不同导航方法获得加油锥套相对相机的位置后，进行坐标系转换，先将相机坐标系下的加油锥套位置转换到受油机机体坐标系下，再转换到地面惯性坐标系下，为受油机控制系统提供导航信息。

6)受油机相对位置精确控制模块，包括获取不同导航信号、对接路径生成、导航源切换防抖与权值计算策略、受油机前向、侧向位置以及高度控制。根据导航源选择(③)结果，获取视觉导航或者GPS信号，当进入加/受油机视觉导航近距对接时，设计受油机按指数曲线接近加油机的对接路径，选择跟踪路径点，降低视觉导航下受油机位置超调，同时设计导航源切换防抖策略，防止不同导航源在阈值边界处来回切换，并根据导航源切换规则得到不同导航源切换的权值，以降低不同导航源切换带来的受油机位置超调。受油机前向位置控制以底层速度控制为内环，根据前向位置控制得到加/受油机对接速度值；受油机侧向位置与高度控制，以受油机底层其它四个嵌套的控制回路为内环，侧向位置与高度控制输入为经导航源切换权值加权的相对位置信息，两个方向的控制分别根据不同的位置控制要求设计模糊规则，得到受油机底层位置控制的控制量。通过反馈加/受油机之间的相对位置，进行相对位置精确控制。

一种仿猛禽视觉导航的自主空中加油对接半物理方法，实现步骤如下：

步骤一：受油机、软管-锥套、多种风干扰建模，并给定初始状态

建立加/受油机六自由度仿射非线性模型，不加推导直接给出：

f_Vk＝(-D-Cβ_w+Lα_w-mgsinγ)/m，B_Vk＝T_maxcos(α+σ)cosβ/m；

其中，V_k为受油机地速；S₁,S₂,S₃,S₄为受油机状态量；κ为状态中间变量；δ_T,δ_a,δ_e,δ_r分别为油门开度、副翼偏转量、升降舵偏转量、方向舵偏转量；y_R,z_R,γ,χ,α,β,μ,p,q,r分别为受油机惯性系侧向位置、垂向位置、航迹倾角、航迹偏角、迎角、侧滑角、速度滚转角、滚转角速度、俯仰角速度、偏航角速度；V₀为加油机速度即受油机配平速度；Q＝0.5ρV²为受油机动压，ρ为大气密度；L,D,C,

M,N分别为升力、阻力、侧力、气动滚转力矩、俯仰力矩、偏航力矩；T为发动机推力；σ为发动机安装角；T_max为发动机最大推力；m为受油机质量；α_w,β_w为风干扰引起的受油机迎角、侧滑角；α_k,β_k为在航迹坐标系中的迎角、侧滑角；S为机翼面积；

为受油机翼展；

为平均气动弦长。

建立软管-锥套多刚体分段模型，将软管分为N段，

各段状态角可以表示为

(k＝1,2,…,N；i＝1,2)，不加推导直接给出：

其中，d_k为软管k-1指向软管k的距离矢量；a_k为软管k段的加速度；l_k为软管k段的长度；α_w为牵连角加速度；ω_w为牵连角速度；偏导数

建立多种风干扰的模型，包括加油机尾流、受油机头波、大气紊流的模型。加油机尾流采用Hallock-Burnham尾涡模型；受油机头波采用兰金半体模型；大气紊流采用Dryden紊流模型。软管-锥套在对接过程中受到加油机尾流、受油机头波、大气紊流的影响；受油机在对接过程中受到加油机尾流、大气紊流的影响。

步骤二：锥套位置稳定控制

根据锥套在稳定流场(自由流与尾流)中的稳定位置，分别设计可控锥套侧向与垂向位置分数阶PID稳定控制，通过锥套作动器分配，得到可控锥套四个舵面的控制量，改变锥套受到的气动力，抵抗多风干扰对加油锥套位置的影响，维持加油锥套在较小的范围内摆动，控制架构如图2所示。

步骤三：加/受油机抗干扰底层控制

加油机底层控制较为简单，不需要复杂的控制，采用PID控制的形式稳定速度、俯仰角、滚转角、偏航角。

针对受油机角速度回路，将与标准非线性模型不同的那部分总干扰F₄扩展为系统的内部状态量，因此得到增广后的角速度运动方程：

基于公式(5)，设计角速度回路的线性扩张状态观测器：

其中，

为状态量S₄的观测值；

为状态量F₄的观测值；ξ₄₁＝[2ζ₄,2ζ₄,2ζ₄]，

为角速度回路的线型扩张状态观测器增益，ζ₄为观测器带宽。

设计等效滑模控制器为：

其中，

为角速度回路指令

的微分，可以通过跟踪微分器得到；X＝ce(c＞0)为滑模函数，跟踪误差

为经跟踪微分器得到的角速度指令；为降低等效滑模控制的抖振，选取sat(·)饱和函数，K(t)自适应增益，具体如下：

其中，Δ为边界层厚度。

其中，

为观测到角速度回路的集总干扰。

采用非线性跟踪微分器，公式如下：

其中，参数r_td决定跟踪快慢；h_td为滤波因子；v为输入信号；v₁为v的滤波输出信号；v₂为 v的近似微分信号；fhan(·)为非线性函数：

其中，sign(·)为符号函数。

对于气流角、航迹、位置以及速度回路，增广运动方程以及观测器设计形式与公式(5)、(6)相同。控制器根据自抗扰控制理论，采用反馈线性化得到：

其中，

步骤四：加/受油机相对位置计算，视觉使能判断

对接初始阶段，将加油机位置x_T,y_T,h_T与受油机位置x_R,y_R,h_R做差，得到相对位置Δx,Δy,Δh。判断是否使能视觉导航系统，如果Δx＞Δx_thr，选择GPS导航源数据进行加/受油机相对位置精确控制，视觉导航使能标志E_vision＝0，进行步骤十二～十三；否则，仿猛禽视觉导航系统开始工作，进行步骤五～十三。

步骤五：利用仿猛禽颜色视觉进行导航方法选择

视景显示计算机FlightGear视景中，加油锥套设计为红色并且在外围添加一个绿色标志点与六个蓝色标志点，如图3所示。

利用仿猛禽颜色视觉双拮抗机制构建仿鹰眼颜色区域及标志点检测，提取加油锥套图像中不同波段信号信息。由于目前常用的图像传感器感光元件所采集得到的图像多编码为RGB (Red-Green-Blue)颜色空间的图像，但为了使颜色拮抗的计算过程更接近猛禽生理特性，将RGB颜色空间转换至长波、中波、短波LMS(Long-Medium-Short)颜色空间，进一步构建长波-中波单拮抗及短波-长波单拮抗，并转换到单拮抗空间，具体公式如下：

其中，R,G,B表示RGB空间中三个通道的灰度值；L,M,S分别为LMS空间中长波、中波、短波数值；I_LM为L兴奋、M抑制的L-M单拮抗；I_ML为M兴奋、L抑制的M-L单拮抗；I_SL为S兴奋、L抑制的S-L单拮抗；I_LS为L兴奋、S抑制的L-S单拮抗。

用二维各向同性高斯滤波核模拟神经节细胞感受野空间结构，以L兴奋、M抑制的L-M单拮抗细胞为例，对单拮抗机制进行近似细胞响应：

C_L+M-(x,y；σ_c)＝I_LM(x,y)*G(x,y；σ_c) (17)

其中，C_L+M-(x,y；σ_c)表示在图像坐标(x,y)处以σ_c为感受野大小的细胞响应；

G(x,y；σ_c)为二维各向同性高斯滤波核，σ为高斯滤波核的标准差；*表示卷积操作。同理可以计算单拮抗细胞响应C_M+L-(x,y；σ_c)、C_S+L-(x,y；σ_c)、C_L+S-(x,y；σ_c)。

根据双拮抗细胞响应的结构，用两个不同感受野大小的拮抗细胞响应构造双拮抗细胞的响应，以L-M双拮抗细胞为例，可表示为：

D_LM(x,y；σ_c)＝C_L+M-(x,y；σ_c)+k·C_M+L-(x,y；λσ_c) (19)

其中，σ_c和λσ_c表示双拮抗细胞中心和周边感受野的尺度，k为周边感受野的比重。同理可以计算双拮抗细胞响应D_SL(x,y；σ_c)。

用长波-中波双拮抗细胞响应D_LM(x,y；σ_c)可以得到红色锥套感兴趣区域；用短波-长波双拮抗细胞响应D_SL(x,y；σ_c)可以得到锥套感兴趣区域内的蓝色标志点。

绿色标志点根据检测效果采用简单双拮抗的形式，具体如下：

D_M＝max(M_thr,min(G-R,G-B)) (20)

其中,D_M为中波通道简单双拮抗输出响应；M_thr为中波阈值；max(s₁,s₂)表示取s₁与s₂的最大值；min(s₁,s₂)表示取s₁与s₂的最小值。

在对工业相机采集到的FlightGear视景进行视觉导航处理之前需要对相机进行标定，在视景显示计算机上全屏显示棋盘标定板(如图4所示)，用工业相机SDK(SoftwareDevelopment Kit)软件，改变采集图像的大小以减少相机采集干扰区域，记录实际使用图像的起始像素点以及图像像素长宽，将记录数据写入任务处理计算机Matlab相机采集接口，保存改变大小后的棋盘格标定板图像，运行棋盘格找角点程序得到标定棋盘格角点(如图5所示)，然后通过计算预先采集得到的屏幕像素点数据与标定棋盘格角点像素值的仿射变换，得到对应像素点之间的仿射变换矩阵，并得到仿射变换矩阵的逆，将两组数据保存到视觉导航处理程序文件中，用于准确位姿估计。

对工业相机采集的图像进行仿猛禽颜色视觉机制的颜色分割，选择长波-中波双拮抗细胞响应通道的输出响应图像进行二值化处理，从而得到只包括红色区域的二值图像；接着对只包括红色区域的二值图像进行孔洞填充，滤除杂点，得到圆形锥套二值图像；将圆形锥套二值图像与原图像(即工业相机采集的图像)进行相与操作得到只包含圆形锥套的彩色图像；利用基于仿猛禽颜色视觉机制的图像颜色分割方法对只包含圆形锥套的彩色图像再次进行颜色分割，首先选择中波通道简单双拮抗输出响应图像进行二值化处理，得到绿色圆形标志点二值图像，然后进行斑点检测，得到绿色圆形标志点数量以及所对应标志点的中心像素坐标，再选择短波-长波双拮抗细胞响应通道的输出响应图像进行二值化处理，得到蓝色圆形标志点二值图像，进行斑点检测，得到蓝色圆形标志点数量以及所对应标志点的中心像素坐标。将检测到的绿色与蓝色标志点数量相加，判断是否检测到全部的七个标志点。如果检测到所有标志点，进行步骤六；否则，检测到部分或者没有检测到标志点，进行步骤七～八。

步骤六：锥套所有标志点检测正常的近距视觉导航

如果检测到锥套上的所有标志点，进行标志点的匹配。根据步骤五检测到的所有标志点的坐标，首先进行凸包变换把所有标志点的坐标顺时针排序；然后判定初始绿色标志点，根据步骤五检测到的绿色标志点的坐标，依次对所有七个标志点坐标做平方差，平方差最小值对应的坐标点即为绿色志点的坐标；最后按照顺时针依次对剩下的六个蓝色标志点进行匹配，得到所有七个标志点的匹配结果。

对匹配正确的所有标志点，利用RPnP(A Robust Solution to the Perspective-n-Point Problem)算法进行精确位姿测量，算法具体实施不再详细叙述，得到相机坐标系下的加油锥套位置，然后进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到加油锥套与受油插头在地面惯性系下的位置差Δx_v,Δy_v,Δh_v。

步骤七：锥套距离远标志点检测不到的近距视觉导航

当锥套距离较远时，由于标志点太小，会检测不到部分甚至全部标志点。因此，需要进行近距对接相对距离远时标志点检测不到的近距视觉导航。

判断如果没有检测到加油锥套所有标志点并且加/受油机前向相对距离

说明锥套距离远，检测到的标志点数量太少。此时，首先对工业相机采集到的图像进行仿猛禽颜色视觉机制的颜色分割，选择长波-中波双拮抗细胞响应通道的输出响应图像进行二值化处理，从而得到只包括红色区域的二值图像；接着对只包括红色区域的二值图像进行孔洞填充，滤除杂点，得到圆形锥套二值图像；将圆形锥套二值图像与原图像(即工业相机采集的图像)进行相与操作得到只包含圆形锥套的彩色图像；然后对只包含圆形锥套的彩色图像提取锥套边缘点；再对边缘像素点进行椭圆拟合，得到拟合椭圆的外接矩形四个顶点的像素点；最后，根据锥套实际尺寸的外接矩形对四个顶点像素点进行匹配。

利用匹配好的顶点像素点进行RPnP精确位姿测量，得到相机坐标系下的加油锥套位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到加油锥套与受油插头在地面惯性系下的位置差Δx_v,Δy_v,Δh_v。

步骤八：锥套近距标志点部分遮挡的近距视觉导航

当锥套距离较近时，由于受油机插头以及机头在相机视场中，会出现加油锥套的部分标志点被受油插头或机头遮挡，出现标志点检测不全，此时如果用步骤七中的视觉导航方法，会造成视觉处理误差较大，因此，当标志点被遮挡数量不多时，需要进行锥套近距标志点部分遮挡的近距视觉导航。

判断如果没有检测到加油锥套标志点数量大于等于5小于7并且加/受油机前向相对距离

说明锥套距离较近，标志点被部分遮挡；如果检测标志点数量小于5，用步骤七视觉导航方法。此时，根据步骤五检测到的加油锥套部分标志点的坐标，首先进行凸包变换把检测到的所有标志点坐标顺时针排序{P_i}(i＝1,2,…,5or1,2,…,6)。如果检测标志点中有绿色标志点，采用步骤六中的方法区分第一个绿色标志点，以绿色标志点坐标为始，其它标志点坐标顺时针保存，得到新的标志点坐标点集{PP_i}(i＝1,2,…,5or1,2,…,6)；如果检测到的标志点中不存在绿色标志点或者遮挡非连续标志点，计算凸包变换后顺时针排序标志点集相邻标志点间的距离{L_i}(i＝1,2,…,5or1,2,…,6)，并判断得到最小的标志点距离l_min，计算所有标志点距离与最小标志点距离的比例

(i＝1,2,…,5or1,2,…,6)。根据加油锥套标志点的实际坐标，计算得到不同数量标志点被遮挡时的相邻标志点距离比。坐标点集{P_i}(i＝1,2,…,5or1,2,…,6)相邻坐标点间被遮挡标志点数量判断：1)如果

(i＝1,2,…,5or1,2…,…,6)，相邻标志点间不存在遮挡标志点；2)如果

(i＝1,2,…,5or1,2,…,6)，相邻标志点间存在一个遮挡标志点；3)如果

(i＝1,2,…,5or1,2,…,6)，相邻标志点间存在两个遮挡标志点；4)如果

(i＝1,2,…,5or1,2,…,6)，相邻标志点间存在三个遮挡标志点；5)由于锥套标志点分布原因(如图3所示)，终点为绿色的比例段默认遮挡标志点减一。

当检测到的标志点数量为6(全部检测到标志点数量为7)时，如果绿色标志点被遮挡，计算距离比

(i＝1,2,…,6)，选择其中距离比最大的末端点为初始标志点坐标，其它标志点坐标顺时针保存，得到新的标志点坐标点集

(i＝1,2,…,6)；如果存在绿色标志点，如上所述，选择排序后的坐标点集

(i＝1,2,…,6)。

用

(i＝1,2,…,6；j＝1or2)分别计算与预存的遮挡一个点的七组标志点模板的仿射变换矩阵，并根据得到的仿射变换矩阵反投影模板像素点得到坐标点集

(i＝1,2,…,6)，并计算坐标点集

(i＝1,2,…,6；j＝1or2)与反投影点集

(i＝1,2,…,6)对应各个坐标点的误差和，选择两个点集误差和最小的预存标志点模板(即确定了被遮挡点的序号)并计算仿射变换矩阵，然后用计算得到的仿射变换矩阵与预存的7个标志点全部检测到的模板做反投影变换，得到存在7个标志点的反投影变换坐标点集

(i＝1,2,…,7)，接着根据被遮挡点的序号选择反投影坐标点集

(i＝1,2,…,7)中对应序号的坐标点插入实际检测到的坐标点集

(i＝1,2,…,6；j＝1or2)相应坐标点序号处，得到可用于位姿估计的新坐标点集

(i＝1,2,…,7)。

当检测到的标志点数量为5时，为简化描述只对存在绿色标志点的情况表述。当存在绿色标志点时，有两种情况即标志点连续遮挡、标志点非连续遮挡。计算排序后的坐标点集 {PP_i}(i＝1,2,…,5)距离比

(i＝1,2,…,5)，判断遮挡标志点非零段的数目，如果为 1段，说明标志点连续遮挡；如果大于1段，说明标志点非连续遮挡。如果标志点连续遮挡，坐标点集{PP_i}(i＝1,2,…,5)分别计算与预存的遮挡两个连续坐标点的七组标志点模板的仿射变换矩阵，经过与遮挡一个标志点后续相同的步骤后，得到可用于位姿估计的新坐标点集

(i＝1,2,…,7)。如果标志点非连续遮挡，根据距离比

(i＝1,2,…,5)遮挡标志点非零段出现的顺序确定被遮挡非连续标志点的序号，然后根据被遮挡标志点的序号，选择预测的非连续遮挡标志点的模板，计算此模板与坐标点集{PP_i}(i＝1,2,…,5)的仿射变换矩阵，然后用计算得到的仿射变换矩阵与预存的7个标志点全部检测到的模板做反投影变换，得到存在7个标志点的反投影变换坐标点集

(i＝1,2,…,7)，接着根据被遮挡点的序号选择反投影坐标点集

(i＝1,2,…,7)中对应序号的坐标点插入实际检测到的坐标点集{PP_i}(i＝1,2,…,5)相应坐标点序号处，得到可用于位姿估计的新坐标点集

(i＝1,2,…,7)。

选择坐标点集

(i＝1,2,…,7；j＝1or2or3)进行RPnP精确位姿测量，得到相机坐标系下的加油锥套位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到加油锥套与受油插头在地面惯性系下的位置差Δx_v,Δy_v,Δh_v。

步骤九：导航源切换防抖与权值计算策略

对接过程中存在GPS与视觉导航源的切换，在切换阈值边界会存在不同导航源的来回切换，同时直接切换导航源会造成受油机位置的不稳定，需要设计导航源切换防抖与权值计算策略。

导航源切换防抖策略设计为：当视觉导航数据第一次出现

时，保存此时视觉导航x方向的数据

当Δx_v抖动回比

的值时，导航源切换防抖策略输出

为

当

时，

为实时的Δx_v。

导航源切换权值计算策略设计为：确定一个导航源切换距离d_swh，切换权值u_swh计算如下

步骤十：判断使用GPS还是视觉导航数据进行控制

如果加/受油机前向相对距离差Δx≤Δx_thr但

依旧按照GPS信号进行导航控制，视觉导航使能标志E_vision＝0；如果Δx≤Δx_thr且

则按照视觉导航信号进行导航控制，视觉导航使能标志E_vision＝1。

步骤十一：对接路径生成与路径点选择

当使用视觉导航数据进行导航控制时，为减少受油机位置响应超调，设计近距对接路径。将加/受油机前向相对距离分为N_links段即得到N_links个路径点，并且设置末端直接跟踪锥套位置的目标点个数N_drogue，对接路径选择前向相对距离段的2/3指数函数。记录开始使用视觉导航数据时的加/受油机相对位置Δx^s,Δy^s,Δh^s，根据实时的相对位置Δx,Δy,Δh，计算加 /受油机相对位置改变量ΔΔx,ΔΔy,ΔΔh，并补偿到实时的视觉导航位置，公式如下：

其中，

表示根据实时视觉导航数据更新的起始路径点坐标。相邻路径点x 方向的距离为

也就是将x方向相对距离以x_single为间隔划分得到一个序列

以序列X_v为基础，也可以计算得到y,h方向的序列Y_v、H_v，因此得到路径点序列P_v，路径起始点坐标

其它路径点的更新方式，如公式(23)～(25)。

其中，X_v(N_links-N_drogue)为序列X_v中的点；K_y,K_h分别为路径y,h方向的比例系数。进一步更新全部路径点：

其中，N表示正整数；X_v(i),Y_v(i),H_v(i)为序列X_v、Y_v、H_v中点。再对路径点进行平移得到最终的路径点序列POS_v＝[XX_v,YY_v,HH_v]：

接下来进行跟踪路径点[ΔVisX,ΔVisY,ΔVisH]的选择，设定到达所选路径点x方向的阈值为d_pos，选择前向最邻近路径点。

步骤十二：确定加/受油机相对位置精确控制输入

根据步骤五～十一得到视觉导航数据Δx_v,Δy_v,Δh_v、切换标志E_vision以及权值u_swh，再结合实时GPS相对位置Δx,Δy,Δh，确定加/受油机相对位置精确控制的三个方向的输入

[PosX_in,PosY_in,PosH_in]，具体如下：

其中，Δx_des,Δy_des,Δh_des分别为GPS导航下三个方向的期望位置点。

步骤十三：加/受油机相对位置前向、侧向、高度精确控制

分别设计加/受油机前向、侧向、高度的相对位置精确控制率。

加/受油机相对前向位置精确控制，当前向相对位置

时，受油机以较高的对接速度ΔV_k1接近加油机；当前向相对位置

时，受油机减速到近距对接速度

减速过程中对接速度ΔV_k计算如下：

当前向相对位置

时，受油机保持近距对接速度

实现锥管与锥套的对接。

加/受油机侧向位置、高度精确控制，采用模糊控制方式，通过模糊控制得到

和

侧向模糊控制输入输出论域分别为[-5,5]、[-0.01,0.01]，高度模糊控制输入输出论域分别为[-2,2]、[-0.01,0.01]，隶属度函数采用zmf与smf函数(模糊控制成熟隶属度函数，不在赘述)，输入输出模糊集分别为{N,P}、{NN,PP}，具体参见图6～图9。侧向位置、高度模糊推理决策算法设计为：

1)If(input1 is N)then(output1 is NN)

2)If(input1 is P)then(output1 is PP)

其中，N,P,NN,PP的取值参见图6～图9。

采用centroid面积重心法(成熟反模糊化方法)解模糊得到实际的侧向位置、高度精确

控制量即

和

最后侧向指令

积分得到

高度指令

积分后取反得到

这里设计的加/受油机相对位置前向、侧向、高度精确控制适用于两种不同的导航源数据，只是在输入不同导航源数据上有区别，步骤十二介绍了不同导航源数据输入控制器方式。

步骤十四：判断对接是否成功

当视觉导航

时，判断Δy_v与Δh_v是否小于

如果Δy_v与Δh_v小于

表示对接成功，结束对接过程；否则，表示对接失败，则受油机减速，将前向偏离目标设为

使得加/受油机相对距离回到

重新进行对接过程。

本发明的仿猛禽视觉导航的自主空中加油对接半物理系统及其方法，其优点及功效在于：一、本发明提供了完整软式自主空中加油对接阶段导航与控制半物理系统框架及其工作流程，系统框架以及流程合理、高效，能够满足软式自主空中加油对接功能指标；二、本发明模仿猛禽颜色视觉机制构建仿猛禽颜色区域及标志点检测方法，提高了锥套区域及标志点的检测效率及准确性；三、本发明给出了仿生视觉导航与GPS导航源的切换逻辑，提供了三种不同的视觉导航方法，解决了锥套距离较远或标志点被部分遮挡情况下的视觉导航问题且视觉导航精度较高，能够实现对接区域全过程的视觉导航，提高视觉导航的抗干扰性、稳定性；四、本发明设计了GPS与视觉导航信号下的受油机相对位置控制律，能够较好地抑制近距对接过程中受油机受到的不同风干扰，提高了相对位置控制精度；

附图说明

图1基于仿猛禽视觉导航的自主空中加油对接半物理系统框架图。

图2自主空中加油软管-锥套位置稳定控制结构图。

图3自主空中加油锥套标志点示意图。

图4自主空中加油对接系统视景计算机标定板工业相机采集图。

图5自主空中加油对接系统视景工业相机采集图像角点结果图。

图6自主空中加油对接系统侧向输入模糊隶属度函数示意图。

图7自主空中加油对接系统侧向输出模糊隶属度函数示意图。

图8自主空中加油对接系统高度输入模糊隶属度函数示意图。

图9自主空中加油对接系统高度输出模糊隶属度函数示意图。

图10基于仿猛禽视觉导航的自主空中加油对接方法流程图。

图11a、b、c基于仿猛禽视觉导航的自主空中加油对接系统位置响应曲线。

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

(Δx,Δy,Δh)——加油机相对受油机的位置

(x_R,y_R,h_R)——受油机的位置

(x_T,y_T,h_T)——加油机的位置

(Δx_v,Δy_v,Δh_v)——仿猛禽视觉导航得到的锥套位置

(w_u,w_v,w_w)——合成风干扰的三个分量

a——加油机加速度

ω——加油机角速度

(x_dro,y_dro,z_dro)——锥套的位置

F_s——锥套侧向期望主动控制力

F_v——锥套垂向期望主动控制力

(act₁,act₂,act₃,act₄)——锥套四个作动器张开角

——软管第k段两个状态角

a_k——软管第k段的加速度

Q_dro——锥套受到的气动力

①～⑦——锥套标志点序号

N——模糊语言值1

P——模糊语言值2

NN——模糊语言值3

PP——模糊语言值4

具体实施方式

见图1至图11a、b、c，下面通过一个具体的基于仿猛禽视觉导航的自主空中加油对接实例来验证本发明所提出的系统及其方法的有效性。基于仿猛禽视觉导航的自主空中加油对接系统及其方法，其实现流程如图10所示，该系统及方法具体步骤如下：

步骤一：受油机、软管-锥套、多种风干扰建模，并给定初始状态

按照公式(1)～(4)，分别建立受油机以及软管-锥套的模型。以机翼后缘两尾涡中心间连线的中点为原点O_v，Y_v轴与中心连线重合，正方向指向右侧翼尖，X_v与Z_v轴分别平行于加油机机体坐标系对应的两个轴，建立尾涡坐标系O_vX_vY_vZ_v。加油机尾流Hallock-Burnham 模型风速为v_vor＝[v_vorx v_vory v_vorz]，根据模型性质v_vorx≈0，其它两个分量具体计算公式如下：

其中，(x_rel y_rel z_rel)为尾涡坐标系中某一点的坐标；Γ₀为尾涡初始强度，Γ₀＝4G_T/(πρV_Tb_T)，G_T为加油机重力，ρ为大气密度，V_T为加油机速度，b_T为加油机翼展；r_c为尾涡半径，

受油机头波兰金半体模型，可以由均流与点源流的叠加获得，以点源流的重心为原点 O_b，X_b、Y_b、Z_b分别平行于受油机机体坐标系对应的三个轴，建立头波坐标系O_bX_bY_bZ_b。头波风速可以表示为v_bow＝[v_bowx v_bowy v_bowz]，三个分量的具体表达式为：

其中，(x_b y_b z_b)为头波坐标系中某一点的坐标；头波模型在O_b为原点的极坐标系统中，径向速度v_r与周向速度v_θ可以表示为：

v_η＝-Usinη (32)

其中，U为均流的速度；Q_b为点源流的强度，Q_b＝2πUb_n，b_n＝h_nose/π，h_nose为机身在宽度方向上的最大半径；r为极径；η为极角。

给定加/受油机初始状态，自主会合结束后，受油机高度h_R＝6990m，加油机高度 h_T＝7010m，加/受油机速度V_R＝V_T＝200m/s，加油机相对受油机初始距离Δx＝120m、Δy＝2m、Δh＝20m，软管长度l＝22.86m，软管段数N＝20，软管k段的长度 l_k＝l/N＝1.143m，加油机质量m_T＝120000kg，翼展b_T＝39.88m，U＝V_T， h_hose＝0.4m，ρ＝1.293kg/m³，受油机其它模型参数均采用文献中的数据，不再赘述。设定仿真步长0.02s，仿真时间200s，当

时，仿真停止，采用轻度紊流强度。

步骤二：锥套位置稳定控制

仿真时间前50s为软管-锥套由初始给定状态稳定的过程，加/受油机不运动，保持静止的相对位置。

在仿真时间前50s，软管受到自由流与加油机尾流得到相对稳定位置。仿真时间50s 之后，加入受油机头波、大气紊流的影响，大气紊流为轻度紊流。

根据仿真时间t＝50s时的加油锥套稳定位置，分别设计锥套侧向与垂向位置分数阶 PID控制器，通过作动器分配，得到可控锥套的四个舵面的控制量，改变锥套受到的气动力，抵抗风干扰对锥套位置的影响。

步骤三：加/受油机抗干扰底层控制

按照公式(5)～(14)以及设计步骤，构建干扰观测器、跟踪微分器、参数自适应等效滑膜控制器以及反馈线性化控制器，建立稳定的受油机抗干扰底层控制，实现对受油机速度、角速度、气流角、航迹、位置的多环稳定控制，能够在抗多风干扰的情况下控制受油机位置。

受油机底层控制相关参数选择为：ζ₁＝5、ζ₂＝10、ζ₃＝20、ζ₄＝40、ζ_Vk＝40； k₁＝[0.7,0.65]、k₂＝[2,2]、k₃＝[5,4,4.5]、k_Vk＝1.2，K(t)由公式(9)计算得到；

按照传统的加油机底层控制方法设计加油速度、俯仰角、滚转角、偏航角稳定控制。

步骤四：加/受油机相对位置计算，视觉使能判断

加油机与受油机位置做差，得到加油机相对受油机的位置Δx,Δy,Δh。如果Δx＞80m (加油锥套与受油插头之间的距离大约在40m)，选择GPS导航源数据进行加/受油机相对位置精确控制，视觉导航使能标志E_vision＝0，进行步骤十二～十三；否则，仿猛禽视觉导航系统开始工作，进行步骤五～十三。

步骤五：利用仿猛禽颜色视觉进行导航方法选择

在对工业相机采集到的FlightGear视景进行视觉导航处理之前需要对相机进行标定，在视景显示计算机上全屏显示棋盘标定板(如图4所示)，用工业相机SDK(SoftwareDevelopment Kit)软件，改变采集图像的大小以减少相机采集干扰区域，记录实际使用图像的起始像素点以及图像像素长宽，将记录数据写入任务处理计算机Matlab相机采集接口，保存改变大小后的棋盘格标定板图像，运行棋盘格找角点程序得到标定棋盘格角点(如图5所示)，然后通过计算预先采集得到的屏幕像素点数据与标定棋盘格角点像素值的仿射变换，得到对应像素点之间的仿射变换矩阵，并得到仿射变换矩阵的逆，将两组数据保存到视觉导航处理程序文件中，用于准确位姿估计。

仿猛禽双拮抗机制颜色检测可以描述为公式(15)～(20)，仿猛禽颜色视觉双拮抗模型 M_thr＝20，σ_c＝3，λ＝3，k＝0.2。对工业相机采集的图像进行仿猛禽颜色视觉双拮抗机制的颜色分割，选择长波-中波双拮抗细胞响应通道的输出响应图像进行二值化处理，从而得到只包括红色区域的二值图像；接着对只包括红色区域的二值图像进行孔洞填充，滤除杂点，得到圆形锥套二值图像；将圆形锥套二值图像与原图像(即工业相机采集的图像)进行相与操作得到只包含圆形锥套的彩色图像；利用基于仿猛禽颜色视觉机制的图像颜色分割方法对只包含圆形锥套的彩色图像再次进行颜色分割，首先选择中波通道简单双拮抗输出响应图像进行二值化处理，得到绿色圆形标志点二值图像，然后进行斑点检测，得到绿色圆形标志点数量以及所对应标志点的中心像素坐标，再选择短波-长波双拮抗细胞响应通道的输出响应图像进行二值化处理，得到蓝色圆形标志点二值图像，进行斑点检测，得到蓝色圆形标志点数量以及所对应标志点的中心像素坐标。将检测到的绿色与蓝色标志点数量相加，判断是否检测到全部的七个标志点。如果检测到所有标志点，进行步骤六；否则，检测到部分或者没有检测到标志点，进行步骤七～八。

步骤六：锥套所有标志点检测正常的近距视觉导航

对检测到锥套上的所有标志点，进行标志点的匹配。根据步骤五检测到的所有标志点的坐标，首先进行凸包变换把所有标志点的坐标顺时针排序；然后判定初始绿色标志点，根据步骤五检测到的绿色标志点的坐标，依次对所有七个标志点坐标做平方差，平方差最小值对应的坐标点即为绿色志点的坐标；最后按照顺时针依次对剩下的六个蓝色标志点进行匹配，得到所有七个标志点的匹配结果。

利用RPnP位姿估计算法进行加油锥套精确位姿估计：1)建立锥套标志点四阶多项式，在锥套坐标系下，选择两个标志点的连线记作旋转轴，以该条连线的中点为坐标原点，连线所在方向为Z轴方向建立新的坐标系O_aX_aY_aZ_a，并将原锥套坐标系下的7个三维特征点的坐标转换到坐标系O_aX_aY_aZ_a下，将7个在新坐标系下的三维标志点进行三三组合分为5个不同的点集。根据三角几何原理每三个点都可以构造一个四阶多项式，5个点集可以形成由5个四阶多项式形成的方程组；2)标志点在摄像机坐标系下坐标的求解，将四阶多项式方程组中的每一个四阶多项式平方求和，构建一个八阶的代价函数，通过对该八阶代价函数进行求导，再求代价函数导数的零解，得到至多四个特征根，由这些特征根计算出7个三维标志点在摄像机坐标系下的坐标，由RPnP算法第一步中确定旋转轴的两个标志点在摄像机坐标系下所对应的坐标

所确定的Z_c轴为

3)位姿求解，已知坐标系O_aX_aY_aZ_a的Z_a轴在摄像机坐标系O_cX_cY_cZ_c下所对应的Z_c轴，则这两个坐标系之间的旋转矩阵R可以表示成一个旋转角度ξ的表达式，这样R中只有一个参数ξ，表达式如下：

其中，H是一个任意的正交旋转矩阵，它的第三列[h₇ h₈ h₉]^T等于Z_c，rot(Z_c,ξ)表示绕Z_c轴的旋转角度ξ。根据摄像机成像原理，三维特征点到二维归一化图像平面的投影可以表示为：

其中，(u_i,v_i)为特征点的像素坐标，(X_i,Y_i,Z_i)为特征点在O_aX_aY_aZ_a坐标系下的坐标， t＝[t_x t_y t_z]^T为坐标系O_aX_aY_aZ_a与摄像机坐标系O_cX_cY_cZ_c之间的平移向量。为了求解参数[cosξsinξt_x t_y t_z 1]^T将公式(34)进行化解，则每个特征点可以构造一个2×6的方程组，将全部特征点多构成的方程组进行组合，并利用奇异值分解的方法对组合后的方程组进行求解，从而求出两个坐标系之间的旋转矩阵R和平移向量t。

最终得到相机坐标系下的加油锥套位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到加油锥套与受油插头在地面惯性系下的位置差Δx_v,Δy_v,Δh_v。

步骤七：锥套距离远标志点检测不到的近距视觉导航

当锥套距离较远时，由于标志点太小，会检测不到部分甚至全部标志点。因此，需要进行近距对接相对距离远时标志点检测不到的近距视觉导航。

判断如果没有检测到加油锥套所有标志点并且加/受油机前向相对距离Δx＞40m，说明锥套距离远，检测到的标志点数量太少。此时，首先对工业相机采集到的图像进行仿猛禽颜色视觉机制的颜色分割，选择长波-中波双拮抗细胞响应通道的输出响应图像进行二值化处理，从而得到只包括红色区域的二值图像；接着对只包括红色区域的二值图像进行孔洞填充，滤除杂点，得到圆形锥套二值图像；将圆形锥套二值图像与原图像(即工业相机采集的图像)进行相与操作得到只包含圆形锥套的彩色图像；然后对只包含圆形锥套的彩色图像提取锥套边缘点；再对边缘像素点进行椭圆拟合，得到拟合椭圆的外接矩形四个顶点的像素点；最后，根据锥套实际尺寸的外接矩形对四个顶点像素点进行匹配。

利用匹配好的顶点像素点进行RPnP精确位姿测量，得到相机坐标系下的加油锥套位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到加油锥套与受油插头在地面惯性系下的位置差Δx_v,Δy_v,Δh_v。

步骤八：锥套近距标志点部分遮挡的近距视觉导航

当锥套距离较近时，由于受油机插头以及机头再相机视场中，会出现加油锥套的部分标志点被受油插头或机头遮挡，出现标志点检测不全，此时如果用步骤七中的视觉导航方法，会造成视觉处理误差较大，因此，当标志点被遮挡数量不多时，需要进行锥套近距标志点部分遮挡的近距视觉导航。

判断如果没有检测到加油锥套标志点数量大于等于5小于7并且加/受油机前向相对距离Δx≤40m，说明锥套距离较近，标志点被部分遮挡；如果检测标志点数量小于5，用步骤七视觉导航方法。此时，根据步骤五检测到的加油锥套部分标志点的坐标，首先进行凸包变换把检测到的所有标志点坐标顺时针排序{P_i}(i＝1,2,…,5or1,2,…,6)。如果检测标志点中有绿色标志点，采用步骤六中的方法区分第一个绿色标志点，以绿色标志点坐标为始，其它标志点坐标顺时针保存，得到新的标志点坐标点集{PP_i}(i＝1,2,…,5or1,2,…,6)；如果检测到的标志点中不存在绿色标志点或者遮挡非连续标志点，计算凸包变换后顺时针排序标志点集相邻标志点间的距离{L_i}(i＝1,2,…,5or1,2,…,6)，并判断得到最小的标志点距离l_min，计算所有标志点距离与最小标志点距离的比例

(i＝1,2,…,5or1,2,…,6)。根据加油锥套标志点的实际坐标，计算得到不同数量标志点被遮挡时的相邻标志点距离比。坐标点集{P_i}(i＝1,2,…,5or1,2,…,6)相邻坐标点间被遮挡标志点数量判断：1)如果

(i＝1,2,…,5or1,2,…,6)，相邻标志点间不存在遮挡标志点；2)如果

(i＝1,2,…,5or1,2,…,6)，相邻标志点间存在一个遮挡标志点；3)如果

(i＝1,2,…,5or1,2,…,6)，相邻标志点间存在两个遮挡标志点；4)如果

(i＝1,2,…,5or1,2,…,6)，相邻标志点间存在三个遮挡标志点；5)由于锥套标志点分布原因(如图3所示)，终点为绿色的比例段默认遮挡标志点减一。

当检测到的标志点数量为6(全部检测到标志点数量为7)时，如果绿色标志点被遮挡，计算距离比

(i＝1,2,…,6)，选择其中距离比最大的末端点为初始标志点坐标，其它标志点坐标顺时针保存，得到新的标志点坐标点集

(i＝1,2,…,6)；如果存在绿色标志点，如上所述，选择排序后的坐标点集

(i＝1,2,…,6)。

用

(i＝1,2,…,6；j＝1or2)分别计算与预存的遮挡一个点的七组标志点模板的仿射变换矩阵，并根据得到的仿射变换矩阵反投影模板像素点得到坐标点集

(i＝1,2,…,6)，并计算坐标点集

(i＝1,2,…,6；j＝1or2)与反投影点集

(i＝1,2,…,6)对应各个坐标点的误差和，选择两个点集误差和最小的预存标志点模板(即确定了被遮挡点的序号)并计算仿射变换矩阵，然后用计算得到的仿射变换矩阵与预存的7个标志点全部检测到的模板做反投影变换，得到存在7个标志点的反投影变换坐标点集

(i＝1,2,…,7)，接着根据被遮挡点的序号选择反投影坐标点集

(i＝1,2,…,7)中对应序号的坐标点插入实际检测到的坐标点集

(i＝1,2,…,6；j＝1or2)相应坐标点序号处，得到可用于位姿估计的新坐标点集

(i＝1,2,…,7)。

当检测到的标志点数量为5时，为简化描述只对存在绿色标志点的情况表述。当存在绿色标志点时，有两种情况即标志点连续遮挡、标志点非连续遮挡。计算排序后的坐标点集 {PP_i}(i＝1,2,…,5)距离比

(i＝1,2,…,5)，判断遮挡标志点非零段的数目，如果为 1段，说明标志点连续遮挡；如果大于1段，说明标志点非连续遮挡。如果标志点连续遮挡，坐标点集{PP_i}(i＝1,2,…,5)分别计算与预存的遮挡两个连续坐标点的七组标志点模板的仿射变换矩阵，经过与遮挡一个标志点后续相同的步骤后，得到可用于位姿估计的新坐标点集

(i＝1,2,…,7)。如果标志点非连续遮挡，根据距离比

(i＝1,2,…,5)遮挡标志点非零段出现的顺序确定被遮挡非连续标志点的序号，然后根据被遮挡标志点的序号，选择预测的非连续遮挡标志点的模板，计算此模板与坐标点集{PP_i}(i＝1,2,…,5)的仿射变换矩阵，然后用计算得到的仿射变换矩阵与预存的7个标志点全部检测到的模板做反投影变换，得到存在7个标志点的反投影变换坐标点集

(i＝1,2,…,7)，接着根据被遮挡点的序号选择反投影坐标点集

(i＝1,2,…,7)中对应序号的坐标点插入实际检测到的坐标点集{PP_i}(i＝1,2,…,5)相应坐标点序号处，得到可用于位姿估计的新坐标点集

(i＝1,2,…,7)。

选择坐标点集

(i＝1,2,…,7；j＝1or2or3)进行RPnP精确位姿测量，得到相机坐标系下的加油锥套位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到加油锥套与受油插头在地面惯性系下的位置差Δx_v,Δy_v,Δh_v。

步骤九：导航源切换防抖与权值计算策略

设计导航源切换防抖与权值计算策略，导航源切换防抖策略，当视觉导航数据第一次出现

时，保存此时视觉导航x方向的数据

当Δx_v抖动回比35小的值时，导航源切换防抖策略输出

为

当Δx_v＜35时，

为实时的Δx_v；导航源切换权值计算，确定一个导航源切换距离d_swh＝5m，按公式(21)计算得到切换权值u_swh。

步骤十：判断使用GPS还是视觉导航数据进行控制

如果加/受油机前向相对距离差Δx≤80但Δx_v≥35，依旧按照GPS信号进行导航控制，视觉导航使能标志E_vision＝0；如果Δx≤80且Δx_v＜35，则按照视觉导航信号进行导航控制，视觉导航使能标志E_vision＝1。

步骤十一：对接路径生成与路径点选择

对接路径生成，将加/受油机前向相对距离分为N_links＝35段即得到35个路径点，并且设置末端直接跟踪锥套位置的目标点个数N_drogue＝10，对接路径选择前向相对距离段的2/3指数函数。记录开始使用视觉导航数据时的加/受油机相对位置Δx^s,Δy^s,Δh^s，根据实时的相对位置Δx,Δy,Δh，按照公式(22)计算加/受油机相对位置改变量ΔΔx,ΔΔy,ΔΔh，

并补偿到实时的视觉导航位置得到

相邻路径点x方向的距离为

也就是将x方向相对距离以x_single为间隔划分得到一个序列

以序列X_v为基础，也可以计算得到y,h方向的序列Y_v、H_v，因此得到路径点序列P_v，路径起始点坐标

按照公式(23)～(25)更新其它路径点。

接下来进行跟踪路径点的选择，设定到达所选路径点x方向的阈值为d_pos＝0.2m，选择前向最邻近跟踪路径点[ΔVisX,ΔVisY,ΔVisH]。

步骤十二：确定加/受油机相对位置精确控制输入

根据步骤五～十一得到视觉导航数据Δx_v,Δy_v,Δh_v、切换标志E_vision以及权值u_swh，再结合实时GPS相对位置Δx,Δy,Δh，根据公式(26)确定加/受油机相对位置精确控制的三个方向的输入[PosX_in,PosY_in,PosH_in]。

步骤十三：加/受油机相对位置前向、侧向、高度精确控制

分别设计加/受油机前向、侧向、高度的相对位置精确控制率。

加/受油机相对前向位置精确控制，当前向相对位置

时，受油机以较高的对接速度ΔV_k1＝5m/s接近加油机；当前向相对位置

时，受油机减速到近距对接速度

按照公式(27)得到ΔV_k；当前向相对位置Δx＜90时，受油机保持近距对接速度1.2m/s，实现锥管与锥套的对接。

加/受油机侧向位置、高度精确控制，采用模糊控制方式，通过模糊控制得到

和

侧向模糊控制输入输出论域分别为[-5,5]、[-0.01,0.01]，高度模糊控制输入输出论域分别为[-2,2]、[-0.01,0.01]，隶属度函数采用zmf与smf函数(模糊控制成熟隶属度函数，不在赘述)，输入输出模糊集分别为{N,P}、{NN,PP}，具体参见图6～图9。侧向位置、高度模糊推理决策算法设计为：

1)If(input1 is N)then(output1 is NN)

2)If(input1 is P)then(output1 is PP)。

采用centroid面积重心法(成熟反模糊化方法)解模糊得到实际的侧向位置、高度精确控制量即

和

最后侧向指令

积分得到

高度指令

积分后取反得到

这里设计的加/受油机相对位置前向、侧向、高度精确控制适用于两种不同的导航源数据，按照步骤十二得到不同导航源数据输入控制器方式。

步骤十四：判断对接是否成功

当视觉导航Δx_v＜0.1m时，判断Δy_v＜0.25m与Δh_v＜0.25m是否成立。如果成立，表示对接成功，结束对接过程；否则，表示对接失败，则受油机减速，将前向偏离目标设为35m，使得加/受油机相对距离回到35m，重新进行对接过程。对接成功过程中，视觉导航控制加油锥套位置响应曲线，如图11a、b、c所示。

Claims (2)

1.一种仿猛禽视觉导航的自主空中加油对接半物理系统，该系统包括：加/受油机抗干扰底层控制模块、多干扰软管-锥套稳定控制模块、视景与显示模块、使能与选择模块、仿生视觉近距相对导航模块、受油机相对位置精确控制模块；其特征在于：

1)加/受油机抗干扰底层控制模块，包括受油机控制与加油机控制；对于受油机控制，采用反步设计方法将受油机高阶非线性系统分为从外到内四个递归的控制回路，分别为位置、航迹、气流角、姿态角速率控制回路，实现受油机高度方向与侧向位置的精确控制；同时将前向位置控制从其它两个方向位置控制中分离出来，对应受油机底层速度控制，通过控制速度来消除前向位置偏差；姿态角速率控制得到副翼、方向舵、升降舵的偏转量，角速率控制采用等效滑模控制形式，并采用线性干扰观测器对角速率干扰量进行估计，对等效滑模控制的切换控制权值进行自适应；自内向外的控制回路即气流角、航迹、位置控制回路，以及速度控制回路，线性干扰观测器对各个控制回路的干扰量进行观测，并将观测干扰量引入控制律中，控制律采用反馈线性化的方式得到相应内环的控制量，实现对干扰量的抑制和期望输出的响应；加油机假设为定直平飞状态，俯仰角保持配平状态值，滚转角、偏航角保持为零；

2)多干扰软管-锥套稳定控制模块，包括软管-锥套模型、多种大气扰动模型以及锥套位置稳定控制；其中，锥套位置稳定控制，包括锥套侧向位置分数阶PID控制器、垂向位置分数阶PID控制器、锥套作动器分配；

3)视景与显示模块，包括FlightGear视景、视景显示以及计算机间UDP通信；由于单个计算机计算能力有限，将FlightGear视景以及视景显示在视景显示计算机上搭建；采用3D构图软件绘制加/受油机以及软管-锥套的3D模型，导入FlightGear构建空中加油视景，并且经计算机显示器实现视景显示的作用；导航与控制其它模块在任务处理计算机上，FlightGear视景驱动数据在任务处理计算机由导航与控制其它模块计算得到，并通过计算机间的UDP通信程序发送至视景显示计算机上控制自主空中加油视景的正常运行；

4)使能与选择模块，包括视觉使能、视觉导航方法选择、导航源选择；视觉使能通过工业相机获取视景计算机显示器上的图像，进而进行视觉导航处理；首先进行视觉导航方法选择，视觉导航方法选择，包括工业相机获取视觉图像、Matlab相机接口、仿猛禽视觉标志点检测、判断是否检测到所有标志点、判断加/受油机相对距离是否小于某一阈值、与操作；工业相机采集视景计算机FlightGear加油场景图像传输至任务处理计算机，任务处理计算机通过Matlab相机数据获取接口得到相机采集图像，对工业相机采集到的图像进行仿猛禽视觉锥套标志点检测，根据是否检测到所设计的锥套上的所有标志点，判断是使用锥套标志点检测正常的导航方法还是锥套遮挡或距离远的导航方法，然后根据加/受油机相对距离判断选取锥套近距遮挡还是锥套距离远的导航方法，当锥套距离较远时，锥套标志点较小无法检测到，使用锥套距离远的导航方式，当锥套近距时，标志点被受油插头或者受油机机头部分遮挡，选择锥套近距遮挡的导航方式；导航源选择，根据受油插头与加油锥套之间的视觉解算位置差以及加/受油机之间的距离，判断使用GPS还是视觉导航信号；

5)仿生视觉近距相对导航模块，包括锥套检测正常、锥套距离远、锥套近距遮挡三种情况；其中，当锥套近距出现标志点部分遮挡，根据标志点的先验信息进行标志点预测、匹配，然后进行位姿估计；根据选择的不同导航方法获得加油锥套相对相机的位置后，进行坐标系转换，先将相机坐标系下的加油锥套位置转换到受油机机体坐标系下，再转换到地面惯性坐标系下，为受油机控制系统提供导航信息；

6)受油机相对位置精确控制模块，包括获取不同导航信号、对接路径生成、导航源切换防抖与权值计算策略、受油机前向、侧向位置以及高度控制；根据导航源选择结果，获取视觉导航或者GPS信号，当进入加/受油机视觉导航近距对接时，设计受油机按指数曲线接近加油机的对接路径，选择跟踪路径点，降低视觉导航下受油机位置超调，同时设计导航源切换防抖策略，防止不同导航源在阈值边界处来回切换，并根据导航源切换规则得到不同导航源切换的权值，以降低不同导航源切换带来的受油机位置超调；受油机前向位置控制以底层速度控制为内环，根据前向位置控制得到加/受油机对接速度值；受油机侧向位置与高度控制，以受油机底层其它四个嵌套的控制回路为内环，侧向位置与高度控制输入为经导航源切换权值加权的相对位置信息，两个方向的控制分别根据不同的位置控制要求设计模糊规则，得到受油机底层位置控制的控制量；通过反馈加/受油机之间的相对位置，进行相对位置精确控制。

2.一种仿猛禽视觉导航的自主空中加油对接半物理方法，其特征在于：该方法实现步骤如下：

步骤一：受油机、软管-锥套、多种风干扰建模，并给定初始状态建立加/受油机六自由度仿射非线性模型：

其中，V_k为受油机地速；S₁,S₂,S₃,S₄为受油机状态量；κ为状态中间变量；δ_T,δ_a,δ_e,δ_r分别为油门开度、副翼偏转量、升降舵偏转量、方向舵偏转量；y_R,z_R,γ,χ,α,β,μ,p,q,r分别为受油机惯性系侧向位置、垂向位置、航迹倾角、航迹偏角、迎角、侧滑角、速度滚转角、滚转角速度、俯仰角速度、偏航角速度；V₀为加油机速度即受油机配平速度；Q＝0.5ρV²为受油机动压，ρ为大气密度；L,D,C,L,M,N分别为升力、阻力、侧力、气动滚转力矩、俯仰力矩、偏航力矩；T为发动机推力；σ为发动机安装角；T_max为发动机最大推力；m为受油机质量；α_w,β_w为风干扰引起的受油机迎角、侧滑角；α_k,β_k为在航迹坐标系中的迎角、侧滑角；S为机翼面积；

为受油机翼展；

为平均气动弦长；

建立软管-锥套多刚体分段模型，将软管分为N段，各段状态角可以表示为θ_ki(k＝1,2,…,N；i＝1,2)，不加推导直接给出：

其中，d_k为软管k-1指向软管k的距离矢量；a_k为软管k段的加速度；l_k为软管k段的长度；α_w为牵连角加速度；ω_w为牵连角速度；偏导数

建立多种风干扰的模型，包括加油机尾流、受油机头波、大气紊流的模型；加油机尾流采用Hallock-Burnham尾涡模型；受油机头波采用兰金半体模型；大气紊流采用Dryden紊流模型；软管-锥套在对接过程中受到加油机尾流、受油机头波、大气紊流的影响；受油机在对接过程中受到加油机尾流、大气紊流的影响；

步骤二：锥套位置稳定控制

根据锥套在稳定流场中的稳定位置，分别设计可控锥套侧向与垂向位置分数阶PID稳定控制，通过锥套作动器分配，得到可控锥套四个舵面的控制量，改变锥套受到的气动力，抵抗多风干扰对加油锥套位置的影响，维持加油锥套在较小的范围内摆动；

步骤三：加/受油机抗干扰底层控制

加油机底层控制较为简单，不需要复杂的控制，采用PID控制的形式稳定速度、俯仰角、滚转角、偏航角；

针对受油机角速度回路，将与标准非线性模型不同的那部分总干扰F₄扩展为系统的内部状态量，因此得到增广后的角速度运动方程：

基于公式(5)，设计角速度回路的线性扩张状态观测器：

其中，

为状态量S₄的观测值；

为状态量F₄的观测值；ξ₄₁＝[2ζ₄,2ζ₄,2ζ₄]，

为角速度回路的线型扩张状态观测器增益，ζ₄为观测器带宽；

设计等效滑模控制器为：

其中，

为角速度回路指令

的微分，可以通过跟踪微分器得到；X＝ce(c＞0)为滑模函数，跟踪误差

为经跟踪微分器得到的角速度指令；为降低等效滑模控制的抖振，选取sat(·)饱和函数，K(t)自适应增益，具体如下：

其中，Δ为边界层厚度；

其中，

为观测到角速度回路的集总干扰；

采用非线性跟踪微分器，公式如下：

其中，参数r_td决定跟踪快慢；h_td为滤波因子；v为输入信号；v₁为v的滤波输出信号；v₂为v的近似微分信号；fhan(·)为非线性函数：

其中，sign(·)为符号函数；

对于气流角、航迹、位置以及速度回路，增广运动方程以及观测器设计形式与公式(5)、(6)相同；控制器根据自抗扰控制理论，采用反馈线性化得到：

其中，

步骤四：加/受油机相对位置计算，视觉使能判断

对接初始阶段，将加油机位置x_T,y_T,h_T与受油机位置x_R,y_R,h_R做差，得到相对位置Δx,Δy,Δh；判断是否使能视觉导航系统，如果Δx＞Δx_thr，选择GPS导航源数据进行加/受油机相对位置精确控制，视觉导航使能标志E_vision＝0，进行步骤十二～十三；否则，仿猛禽视觉导航系统开始工作，进行步骤五～十三；

步骤五：利用仿猛禽颜色视觉进行导航方法选择

视景显示计算机FlightGear视景中，加油锥套设计为红色并且在外围添加一个绿色标志点与六个蓝色标志点；

利用仿猛禽颜色视觉双拮抗机制构建仿鹰眼颜色区域及标志点检测，提取加油锥套图像中不同波段信号信息；由于目前常用的图像传感器感光元件所采集得到的图像多编码为RGB即Red-Green-Blue颜色空间的图像，但为了使颜色拮抗的计算过程更接近猛禽生理特性，将RGB颜色空间转换至长波、中波、短波LMS即Long-Medium-Short颜色空间，进一步构建长波-中波单拮抗及短波-长波单拮抗，并转换到单拮抗空间，具体公式如下：

其中，R,G,B表示RGB空间中三个通道的灰度值；L,M,S分别为LMS空间中长波、中波、短波数值；I_LM为L兴奋、M抑制的L-M单拮抗；I_ML为M兴奋、L抑制的M-L单拮抗；I_SL为S兴奋、L抑制的S-L单拮抗；I_LS为L兴奋、S抑制的L-S单拮抗；

用二维各向同性高斯滤波核模拟神经节细胞感受野空间结构，以L兴奋、M抑制的L-M单拮抗细胞为例，对单拮抗机制进行近似细胞响应：

C_L+M-(x,y；σ_c)＝I_LM(x,y)*G(x,y；σ_c) (17)

其中，C_L+M-(x,y；σ_c)表示在图像坐标(x,y)处以σ_c为感受野大小的细胞响应；G(x,y；σ_c)为二维各向同性高斯滤波核，σ为高斯滤波核的标准差；*表示卷积操作；同理可以计算单拮抗细胞响应C_M+L-(x,y；σ_c)、C_S+L-(x,y；σ_c)、C_L+S-(x,y；σ_c)；

根据双拮抗细胞响应的结构，用两个不同感受野大小的拮抗细胞响应构造双拮抗细胞的响应，以L-M双拮抗细胞为例，可表示为：

D_LM(x,y；σ_c)＝C_L+M-(x,y；σ_c)+k·C_M+L-(x,y；λσ_c) (19)

其中，σ_c和λσ_c表示双拮抗细胞中心和周边感受野的尺度，k为周边感受野的比重；同理可以计算双拮抗细胞响应D_SL(x,y；σ_c)；

用长波-中波双拮抗细胞响应D_LM(x,y；σ_c)可以得到红色锥套感兴趣区域；用短波-长波双拮抗细胞响应D_SL(x,y；σ_c)可以得到锥套感兴趣区域内的蓝色标志点；

绿色标志点根据检测效果采用简单双拮抗的形式，具体如下：

D_M＝max(M_thr,min(G-R,G-B)) (20)

其中,D_M为中波通道简单双拮抗输出响应；M_thr为中波阈值；max(s₁,s₂)表示取s₁与s₂的最大值；min(s₁,s₂)表示取s₁与s₂的最小值；

在对工业相机采集到的FlightGear视景进行视觉导航处理之前需要对相机进行标定，改变采集图像的大小以减少相机采集干扰区域，记录实际使用图像的起始像素点以及图像像素长宽，将记录数据写入任务处理计算机Matlab相机采集接口，保存改变大小后的棋盘格标定板图像，运行棋盘格找角点程序得到标定棋盘格角点，然后通过计算预先采集得到的屏幕像素点数据与标定棋盘格角点像素值的仿射变换，得到对应像素点之间的仿射变换矩阵，并得到仿射变换矩阵的逆，将两组数据保存到视觉导航处理程序文件中，用于准确位姿估计；

对工业相机采集的图像进行仿猛禽颜色视觉机制的颜色分割，选择长波-中波双拮抗细胞响应通道的输出响应图像进行二值化处理，从而得到只包括红色区域的二值图像；接着对只包括红色区域的二值图像进行孔洞填充，滤除杂点，得到圆形锥套二值图像；将圆形锥套二值图像与原图像进行相与操作得到只包含圆形锥套的彩色图像；利用基于仿猛禽颜色视觉机制的图像颜色分割方法对只包含圆形锥套的彩色图像再次进行颜色分割，首先选择中波通道简单双拮抗输出响应图像进行二值化处理，得到绿色圆形标志点二值图像，然后进行斑点检测，得到绿色圆形标志点数量以及所对应标志点的中心像素坐标，再选择短波-长波双拮抗细胞响应通道的输出响应图像进行二值化处理，得到蓝色圆形标志点二值图像，进行斑点检测，得到蓝色圆形标志点数量以及所对应标志点的中心像素坐标；将检测到的绿色与蓝色标志点数量相加，判断是否检测到全部的七个标志点；如果检测到所有标志点，进行步骤六；否则，检测到部分或者没有检测到标志点，进行步骤七～八；

步骤六：锥套所有标志点检测正常的近距视觉导航

如果检测到锥套上的所有标志点，进行标志点的匹配；根据步骤五检测到的所有标志点的坐标，首先进行凸包变换把所有标志点的坐标顺时针排序；然后判定初始绿色标志点，根据步骤五检测到的绿色标志点的坐标，依次对所有七个标志点坐标做平方差，平方差最小值对应的坐标点即为绿色志点的坐标；最后按照顺时针依次对剩下的六个蓝色标志点进行匹配，得到所有七个标志点的匹配结果；

对匹配正确的所有标志点，利用RPnP算法进行精确位姿测量，得到相机坐标系下的加油锥套位置，然后进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到加油锥套与受油插头在地面惯性系下的位置差Δx_v,Δy_v,Δh_v；

步骤七：锥套距离远标志点检测不到的近距视觉导航

判断如果没有检测到加油锥套所有标志点并且加/受油机前向相对距离

说明锥套距离远，检测到的标志点数量太少；此时，首先对工业相机采集到的图像进行仿猛禽颜色视觉机制的颜色分割，选择长波-中波双拮抗细胞响应通道的输出响应图像进行二值化处理，从而得到只包括红色区域的二值图像；接着对只包括红色区域的二值图像进行孔洞填充，滤除杂点，得到圆形锥套二值图像；将圆形锥套二值图像与原图像进行相与操作得到只包含圆形锥套的彩色图像；然后对只包含圆形锥套的彩色图像提取锥套边缘点；再对边缘像素点进行椭圆拟合，得到拟合椭圆的外接矩形四个顶点的像素点；最后，根据锥套实际尺寸的外接矩形对四个顶点像素点进行匹配；

利用匹配好的顶点像素点进行RPnP精确位姿测量，得到相机坐标系下的加油锥套位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到加油锥套与受油插头在地面惯性系下的位置差Δx_v,Δy_v,Δh_v；

步骤八：锥套近距标志点部分遮挡的近距视觉导航

判断如果没有检测到加油锥套标志点数量大于等于5小于7并且加/受油机前向相对距离

说明锥套距离较近，标志点被部分遮挡；如果检测标志点数量小于5，用步骤七视觉导航方法；此时，根据步骤五检测到的加油锥套部分标志点的坐标，首先进行凸包变换把检测到的所有标志点坐标顺时针排序{P_i}(i＝1,2,…,5or1,2,…,6)；如果检测标志点中有绿色标志点，采用步骤六中的方法区分第一个绿色标志点，以绿色标志点坐标为始，其它标志点坐标顺时针保存，得到新的标志点坐标点集{PP_i}(i＝1,2,…,5or1,2,…,6)；如果检测到的标志点中不存在绿色标志点或者遮挡非连续标志点，计算凸包变换后顺时针排序标志点集相邻标志点间的距离{L_i}(i＝1,2,…,5or1,2,…,6)，并判断得到最小的标志点距离l_min，计算所有标志点距离与最小标志点距离的比例

根据加油锥套标志点的实际坐标，计算得到不同数量标志点被遮挡时的相邻标志点距离比；坐标点集{P_i}(i＝1,2,…,5or1,2,…,6)相邻坐标点间被遮挡标志点数量判断：

1)如果

相邻标志点间不存在遮挡标志点；

2)如果

相邻标志点间存在一个遮挡标志点；

3)如果

相邻标志点间存在两个遮挡标志点；

4)如果

相邻标志点间存在三个遮挡标志点；

5)由于锥套标志点分布原因，终点为绿色的比例段默认遮挡标志点减一；

当检测到的标志点数量为6时，如果绿色标志点被遮挡，计算距离比

选择其中距离比最大的末端点为初始标志点坐标，其它标志点坐标顺时针保存，得到新的标志点坐标点集

如果存在绿色标志点，如上所述，选择排序后的坐标点集

用

分别计算与预存的遮挡一个点的七组标志点模板的仿射变换矩阵，并根据得到的仿射变换矩阵反投影模板像素点得到坐标点集

并计算坐标点集

与反投影点集

对应各个坐标点的误差和，选择两个点集误差和最小的预存标志点模板并计算仿射变换矩阵，然后用计算得到的仿射变换矩阵与预存的7个标志点全部检测到的模板做反投影变换，得到存在7个标志点的反投影变换坐标点集

接着根据被遮挡点的序号选择反投影坐标点集

中对应序号的坐标点插入实际检测到的坐标点集

相应坐标点序号处，得到可用于位姿估计的新坐标点集

当检测到的标志点数量为5时，为简化描述只对存在绿色标志点的情况表述；当存在绿色标志点时，有两种情况即标志点连续遮挡、标志点非连续遮挡；计算排序后的坐标点集{PP_i}(i＝1,2,…,5)距离比

判断遮挡标志点非零段的数目，如果为1段，说明标志点连续遮挡；如果大于1段，说明标志点非连续遮挡；如果标志点连续遮挡，坐标点集{PP_i}(i＝1,2,…,5)分别计算与预存的遮挡两个连续坐标点的七组标志点模板的仿射变换矩阵，经过与遮挡一个标志点后续相同的步骤后，得到可用于位姿估计的新坐标点集

如果标志点非连续遮挡，根据距离比

遮挡标志点非零段出现的顺序确定被遮挡非连续标志点的序号，然后根据被遮挡标志点的序号，选择预测的非连续遮挡标志点的模板，计算此模板与坐标点集{PP_i}(i＝1,2,…,5)的仿射变换矩阵，然后用计算得到的仿射变换矩阵与预存的7个标志点全部检测到的模板做反投影变换，得到存在7个标志点的反投影变换坐标点集

接着根据被遮挡点的序号选择反投影坐标点集

中对应序号的坐标点插入实际检测到的坐标点集

相应坐标点序号处，得到可用于位姿估计的新坐标点集

选择坐标点集

进行RPnP精确位姿测量，得到相机坐标系下的加油锥套位置，并进行相机坐标系、机体坐标系、地面惯性系的坐标转换，得到加油锥套与受油插头在地面惯性系下的位置差Δx_v,Δy_v,Δh_v；

步骤九：导航源切换防抖与权值计算策略

导航源切换防抖策略设计为：当视觉导航数据第一次出现

时，保存此时视觉导航x方向的数据

当Δx_v抖动回比

的值时，导航源切换防抖策略输出

为

当

时，

为实时的Δx_v；

导航源切换权值计算策略设计为：确定一个导航源切换距离d_swh，切换权值u_swh计算如下

步骤十：判断使用GPS还是视觉导航数据进行控制

如果加/受油机前向相对距离差Δx≤Δx_thr但

依旧按照GPS信号进行导航控制，视觉导航使能标志E_vision＝0；如果Δx≤Δx_th且

则按照视觉导航信号进行导航控制，视觉导航使能标志E_visio_n＝1；

步骤十一：对接路径生成与路径点选择

当使用视觉导航数据进行导航控制时，为减少受油机位置响应超调，设计近距对接路径；将加/受油机前向相对距离分为N_links段即得到N_links个路径点，并且设置末端直接跟踪锥套位置的目标点个数N_drogue，对接路径选择前向相对距离段的2/3指数函数；记录开始使用视觉导航数据时的加/受油机相对位置Δx^s,Δy^s,Δh^s，根据实时的相对位置Δx,Δy,Δh，计算加/受油机相对位置改变量ΔΔx,ΔΔy,ΔΔh，并补偿到实时的视觉导航位置，公式如下：

其中，

表示根据实时视觉导航数据更新的起始路径点坐标；相邻路径点x方向的距离为

也就是将x方向相对距离以x_single为间隔划分得到一个序列

以序列X_v为基础，也可以计算得到y,h方向的序列Y_v、H_v，因此得到路径点序列P_v，路径起始点坐标

其它路径点的更新方式，如公式(23)～(25)；

其中，X_v(N_links-N_drogue)为序列X_v中的点；K_y,K_h分别为路径y,h方向的比例系数；进一步更新全部路径点：

其中，N表示正整数；X_v(i),Y_v(i),H_v(i)为序列X_v、Y_v、H_v中点；再对路径点进行平移得到最终的路径点序列POS_v＝[XX_v,YY_v,HH_v]：

接下来进行跟踪路径点[ΔVisX,ΔVisY,ΔVisH]的选择，设定到达所选路径点x方向的阈值为d_pos，选择前向最邻近路径点；

步骤十二：确定加/受油机相对位置精确控制输入

根据步骤五～十一得到视觉导航数据Δx_v,Δy_v,Δh_v、切换标志E_vision以及权值u_swh，再结合实时GPS相对位置Δx,Δy,Δh，确定加/受油机相对位置精确控制的三个方向的输入[PosX_in,PosY_in,PosH_in]，具体如下：

其中，Δx_des,Δy_des,Δh_des分别为GPS导航下三个方向的期望位置点；

步骤十三：加/受油机相对位置前向、侧向、高度精确控制

分别设计加/受油机前向、侧向、高度的相对位置精确控制率；

加/受油机相对前向位置精确控制，当前向相对位置

时，受油机以较高的对接速度ΔV_k1接近加油机；当前向相对位置

时，受油机减速到近距对接速度

减速过程中对接速度ΔV_k计算如下：

当前向相对位置

时，受油机保持近距对接速度

实现锥管与锥套的对接；

加/受油机侧向位置、高度精确控制，采用模糊控制方式，通过模糊控制得到

和

侧向模糊控制输入输出论域分别为[-5,5]、[-0.01,0.01]，高度模糊控制输入输出论域分别为[-2,2]、[-0.01,0.01]，隶属度函数采用zmf与smf函数，输入输出模糊集分别为{N,P}、{NN,PP}；侧向位置、高度模糊推理决策算法设计为：

1)If(input1 is N)then(output1 is NN)

2)If(input1 is P)then(output1 is PP)

采用centroid面积重心法解模糊得到实际的侧向位置、高度精确控制量即

和

最后侧向指令

积分得到

高度指令

积分后取反得到

步骤十四：判断对接是否成功

当视觉导航

时，判断Δy_v与Δh_v是否小于

如果Δy_v与Δh_v小于

表示对接成功，结束对接过程；否则，表示对接失败，则受油机减速，将前向偏离目标设为

使得加/受油机相对距离回到

重新进行对接过程。

Images