1.1 僚机编队飞行控制律设计僚机在编队队形保持阶段需要实时跟踪动态的坐标点，并没有预定的航线。

对于固定翼飞机而言，传统的控制方法难以实现对动态目标点的实时跟踪。

为此本文提出了一种新的固定翼编队横侧向跟踪算法，取名为最优转弯半径(OTR, Optimal Turning Radius)算法，且已经验证了在长僚机模式下的编队跟踪中相比于传统PID 控制律有更好的性能。

该算法在跟踪期望点附近引入了超前跟踪点和滞后跟踪点的概念，并将跟踪距离分为远距、中距和近距三种情况。

在转弯段，僚机横侧向指令中增加长机的转弯半径实时反馈。

在长机保持直线飞行时，僚机横侧向指令主要由航迹误差生成，期望航迹根据远距、中距、近距不同情况分别计算。

在长机转弯时，僚机横侧向指令会加入长机的转弯半径实时反馈，保证转弯过程中僚机也能迅速的跟踪到期望点，进而让编队快速收敛到期望的队形。

1.1.1 OTR 算法的原理推导设僚机相对于长机的设定偏差为(,,)F L e x y z →，长机坐标为(,,)L l h λ，其中(,,)F L e x y z →属于本地通用横墨卡托格网 (UTM, Universal Transverse Mercator) 坐标系，其y 轴与长机航向重合，(,,)L l h λ表示长机在GPS 坐标系下的经纬高坐标。

根据僚机与长机的固定偏差F L e →和长机的位置L 可以计算出僚机的期望位置(,,)c F l h λ。

其计算过程分为三步：(1) 将(,,)L l h λ转化到UTM 坐标下得到(,,)uL x y z 。

(2) 在UTM 坐标系下将(,,)uL x y z 和(,,)F L e x y z →相加得到僚机的期望UTM 坐标(,,)u c F x y z 。

(3) 将(,,)uc F x y z 转化到GPS 坐标系下得到(,,)c F l h λ。

编队跟踪保持过程中一个很大的难点便是转弯过程中的期望路径震荡，表现出来的现象便是滚转角的震荡和航向偏向相反方向。

为此本文在实际期望跟踪点附近设计超前跟踪点和滞后跟踪点，并且引入长机的实时转弯半径作为反馈。

超前跟踪点的设计可以有效的解决过点转弯导致的侧向通道震荡问题，滞后跟踪点的设计使僚机在远距依旧能保持跟踪能力。

定义超前跟踪点为(,,)c l F l h λ→，滞后跟踪点为(,,)c tr F l h λ→。

定义函数(),,g f x d 表示在GPS 坐标系下以位置f 为基准点，向x 方向前进距离d 得到的经纬高位置。

设相对于期望点超前距离为c l L →，相对于期望点滞后距离为c tr L →，则：()(,,)=g (,,),,c l c c l F l h F l h L L ψλλ→→ (3.30) ()(,,)=(,,),,c tr c c tr F l h g F l h L L ψλλ→→- (3.31)式中L ψ为长机的航向角，超前点和滞后点在期望点附近沿着长机航向方向选取。

定义长机与僚机的实时距离为L F L ↔，近距离用cL F L ↔表示，远距离用fL F L ↔表示。

则僚机期望位置的计算公式为：(,,)(,,)(,,)(,,)fc tr L F LFc fc c L F L F L F c c l L F L F F l h L L F l h F l h L L L F l h L L λλλλ→↔↔↔↔↔→↔↔⎧≥⎪=<<⎨⎪≤⎩(3.32)下面以fL F L F L L ↔↔≥为例介绍，其他情况推导类似，僚机期望滚转角c φ的计算方法如图3.11：长机僚机2ϕc tr F →(,,)c l l h λ→ψψA B图3.11 滞后点跟踪示意图图中F ψ代表僚机的航向，2ϕ表示僚机当前位置与滞后点的连线与僚机航向的夹角。

1ϕ和2ϕ互余。

c F F L ↔代表僚机与期望点之间的距离。

ABC 三点组成等腰三角形，显然已知僚机当前位置、僚机航向、滞后点位置，可以求解出2ϕ。

将2ϕ作为已知条件看待，结合三角形的正弦定理有：12+=90ϕϕ (3.33)11sin(1802)sin cF F cL R ϕϕ↔=- (3.34) 结合式(3.33)和式(3.34)，可以求解出腰长c R 。

如果将c R 直接作为僚机的期望转弯半径，再根据定坡度转弯条件便可以求解出僚机的期望滚转角F c φ，但当僚机不断靠近期望点时，c R 不断较小，由于1/Fc c R φ∝,外界微小的扰动就会造成僚机期望滚转角剧烈的震荡。

因此本文的想法是加入长机的转弯半径实时反馈。

已知长机的速度为L v ，滚转角为L φ。

则长机的实时转弯半径L R 为：2tan LL Lv R g φ=⋅ (3.35)将L R 加入反馈后，可以得到：2c LOTR R R R +=(3.36) OTR R 便称为最优转弯半径。

最后可以得到僚机的期望滚转角为：22=arctan()+F e Fc p OTRv k g R ψφϕ⋅⋅ (3.37)式中e p k ψ为航迹误差控制比例系数，由式(3.37)可知，当长机转弯时，如果僚机跟踪在期望点附近，僚机会快速跟踪长机行为，使编队队形快速收敛。

当长机沿直线段稳定飞行时，0L L OTR R R φ→⇒→∞⇒→∞,Fcφ的值主要由2e p k ψϕ⋅决定，即在长机保持直线飞行时，僚机横侧向指令主要由航迹误差生成。

现在考虑如下图3.12的队形转弯情况。

图3.12为理想的转弯，图中僚机1与僚机2处于同一同心圆c o 。

其转弯半径之差为21()()F L F L e x e x →→-。

12()()F L F L e x e x →→、代表僚机1和僚机2相对于长机x 轴的给定偏差，为实现图中理想的转弯效果，将式(3.36)改为：+()2c LOTR F L R R R e x →+=(3.38)2c僚机图3.12 三角形编队转弯1.1.2 僚机纵向通道控制律设计上一节介绍了OTR 算法的原理推导，解决了在编队队形保持阶段，僚机的横侧向通道控制问题，本小节将介绍僚机纵向通道的控制律设计。

由3.3.2节可知，长机的纵向通道控制采用了TECS 算法，该算法假设飞机在飞行过程中所受的阻力不变，适合长机或者单机的自动航线飞行，其原因为在自动航线阶段一般会预先给定巡航速度(空速)，飞机达到巡航速度之后速度便一直稳定在巡航速度附近。

由于阻力正比于空速的平方，可认为这阶段飞机所受到的阻力不变，满足TECS 算法的前提假设。

但由于僚机在跟踪长机过程中，需要根据长机的位置不断的调整自身的速度，不再满足阻力不变的假设，所以需要单独设计僚机的纵向通道控制律。

从易于工程实现的角度考虑，本文决定采用经典的PID 算法来设计，距离-速度-油门通道采用串级PID 。

高度的控制方程为：()Fc L F L h h e z →=+ (3.39)F Fc F h h h ∆=- (3.40)FFF kFc p F iFd F i k h k hT k h θα==⋅∆+⋅∆⋅∆+⋅∆∆∑ (3.41)式中Fc h 为僚机期望高度，L h 为长机实际高度，F h 为僚机实际高度，()F L e z →为僚机相对于长机z 轴给定偏差，F F F p i d k k k 、、为PID 控制系数。

图3.13给出了速度油门通道的控制框图。

将僚机位置与期望点的距离c F F L ↔当做控制输入量，经过一次二阶低通滤波处理，目的为消除噪音。

F v 为僚机地速，L v 为长机地速，Fp δ为僚机期望的油门输出量，'Fp δ为油门配平值，一般取在中立油门附近。

图3.13 僚机纵向通道控制图速度油门通道的控制律为：c c c kF pl F F il F F dl F F i v k L k L T k L α↔↔↔=∆=⋅+⋅∆+⋅∆∑ (3.42)=vF F L F e v v v ∆∆+- (3.43)0kFp pv vF iv vF dv vF i k e k e T k e δα=∆=⋅∆+∆⋅∆+⋅∆∆∑ (3.44)'+Fp Fp Fpδδδ=∆ (3.45) 1.1.3 SITL 仿真验证与分析在3.4.1中详细介绍了本文提出的一种新的编队跟踪算法(OTR 算法)，现将其与传统的PID 算法进行对比测试。

图3.14给出了PID 算法跟踪一条直线的情况，图中S P 为期望路径的起点，E P 为期望路径的终点，P 为飞机当前位置，ce D 为侧偏距。

ρ为期望航迹角，ie δ为实际航迹角。

北东EP EP P L ↔ρ∆ρceD δieS P SN NENSE E EE P图3.14 PID 算法跟踪直线情况由上图可知，飞机当前位置与终点的直线距离E P P L ↔=。

侧偏距ce D 的计算公式为：=sin sin()E E ce P P P P ie D L L ρρδ↔↔∆=- (3.46)PID 算法的控制方程为：FD D D cececekc p ce iced ce i k D k DT k D φα•==⋅+⋅∆+∆∑ (3.47)为满足编队跟踪要求，PID 算法同样加入超前滞后模型，具体形式为：((,,),(,,))=((,,),(,,))((,,),(,,))F fpid c tr L F L F c fc pid c tr c L F L F L F c pid c tr c l L F L F g F l h F l h L L g F l h F l h L L L g F l h F l h L L λλφλλλλ→↔↔→↔↔↔→→↔↔⎧≥⎪<<⎨⎪≤⎩(3.48)式中(,,)F l h λ为僚机实际位置，1(,)pid i i g P P -为PID 算法跟踪直线的期望滚转角计算函数，1i P -代表目标直线的起点，i P 代表目标直线的终点。

本节用SITL 仿真测试对比OTR 和PID 算法，测试条件为：长机为自动模式下飞行矩形航路，航线长边为2km ，短边1km 。

巡航速度为22m/s 。

3机组成等边三角形编队，边长为60米，超前距离=100c l L →，滞后距离=120c tr L →，PID 纵向控制律和参数与3.4.2中介绍一致，横侧向控制参数为：=0.750.06,0.06D D D cececep i d k k k ==，选择位于长机右侧的僚机跟踪曲线进行分析。

图中紫色线段为僚机实际走过的路径，黄色线段为长机的航线。

图3.15和图3.16为最优转弯半径跟踪控制律和PID 控制律的轨迹跟踪示意图。

图3.15 最优转弯半径控制律跟踪轨迹图图3.16 PID 控制律跟踪轨迹图50100150200250300飞行距离 (米)水平距离(米)图3.17 转弯段PID 与OTR 水平距离对比图3.17中横坐标为转弯之后僚机飞行的距离，纵坐标为飞机当前点与期望轨迹的水平距离。