僚机编队飞行控制律设计

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 坐标下得到(,,)u

L x y z 。

(2) 在UTM 坐标系下将(,,)u

L x y z 和(,,)F L e x y z →相加得到僚机的期望UTM 坐标

(,,)u c F x y z 。

(3) 将(,,)u

c 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 ↔，近距离用c

L F L ↔表示，远距离用f

L F L ↔表示。则僚机期望位置的计算公式为：

(,,)(,,)(,,)(,,)f

c tr L F L

F

c f

c 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)

下面以f

L 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)

11

sin(1802)

sin c

F F c

L R ϕϕ↔=

- (3.34) 结合式(3.33)和式(3.34)，可以求解出腰长c R 。如果将c R 直接作为僚机的期望转弯半径，再根据定坡度转弯条件便可以求解出僚机的期望滚转角F c φ，但当僚机不断靠近期望点时，

c R 不断较小，由于1/F

c c R φ∝,外界微小的扰动就会造成僚机期望滚转角剧烈的震荡。因此

本文的想法是加入长机的转弯半径实时反馈。已知长机的速度为L v ，滚转角为L φ。则长机的实时转弯半径L R 为：

2tan L

L L

v R g φ=⋅ (3.35)

将L R 加入反馈后，可以得到：

2

c L

OTR R R R +=

(3.36) OTR R 便称为最优转弯半径。最后可以得到僚机的期望滚转角为：

22=arctan()+F e F

c p OTR

v k g R ψφϕ⋅⋅ (3.37)

式中e p k ψ为航迹误差控制比例系数，由式(3.37)可知，当长机转弯时，如果僚机跟踪在期望点附近，僚机会快速跟踪长机行为，使编队队形快速收敛。当长机沿直线段稳定飞行时，

0L L OTR R R φ→⇒→∞⇒→∞,F

c

φ的值主要由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)改为：

+()2

c L

OTR F L R R R e x →+=

(3.38)

2

c

僚机

图3.12 三角形编队转弯