编队系列(二)｜固定翼如何依靠邻居信息保持队形？一致性控制、通信拓扑与分布式仿真实现，附仿真代码

上一篇[《编队系列（一）｜固定翼领航-跟随如何实现？虚拟结构与 LQR 控制，附仿真代码》](#《编队系列（一）｜固定翼领航-跟随如何实现？虚拟结构与 LQR 控制，附仿真代码》)采用一架领航机发布状态、其余飞机跟踪编队槽位的方式。该结构的信息关系清晰，适合双机伴飞和小规模编队验证；但随着成员数量增加，领航机的通信负担、单点失效以及长距离状态转发开始成为需要单独处理的问题。

一致性方法提供了另一种协同方式：每架飞机在本地计算控制量，只与通信图中的邻居交换必要状态，并利用邻居误差逐步形成共同的协同变量。该变量可以是位置、速度、航向、到达时间或路径参数。经典一致性理论给出了通信拓扑、拉普拉斯矩阵与收敛条件之间的关系 $1$ $2$ ，但固定翼无法像单积分器质点一样直接沿任意方向移动，因此还需要把一致性项与路径跟踪、速度边界和姿态控制接口结合起来。

本文以迅翼现有六机固定翼一致性仿真例程为基础：每架飞机根据自身位置、航迹方向和预设槽位反推出一个虚拟编队参考点，通过环形通信拓扑交换参考点坐标；一致性项负责减小各机参考点之间的差异，路径跟踪项负责使编队沿闭合折线路径飞行，LQR 和姿态映射再把期望状态转换为 PX4 Offboard 可以执行的指令。

一、从领航机状态转向邻居信息

设系统中共有 N 架固定翼无人机，第 i 架飞机的位置、速度和航迹角分别为：

pi = $*xi* , *yi*$ T， vi = $*vx,i* , *vy,i*$ T， χi = atan2(vy,i , vx,i)

Leader--follower 中，第 i 架跟随者主要使用领航机状态生成本机参考；一致性编队中，第 i 架飞机根据自身状态以及邻居集合 Ni 中的状态完成计算：

ui = fi (xi , {xj | j ∈ Ni})

这里的"分布式"是指控制量在各机本地计算，并依赖邻居之间的信息交换。它不表示系统完全不需要通信，也不表示任务路径、编队槽位和安全规则必须在线独立生成。本文仿真通过配置文件为所有节点预设共同路径、队形和通信矩阵，运行时则不再由唯一领航节点集中计算所有成员的编队控制量。

这种结构带来三个直接变化：

通信对象发生变化：每架飞机只订阅图中邻居的参考点信息，不需要持续接收同一架领航机的全部状态。
控制器形式统一：所有成员运行相同的路径跟踪、一致性计算、LQR 和姿态映射逻辑，成员差异主要由编号、槽位和邻居集合决定。
故障影响取决于拓扑：单个成员退出后，只要剩余通信图仍满足相应连通条件，其他成员仍可能继续完成状态协调；若退出导致网络分裂，各连通分量会分别演化，不能继续保证全编队一致。

二、通信拓扑如何写成拉普拉斯矩阵

一致性算法首先需要明确"谁能收到谁的信息"。通信网络可以表示为图：

G = (V , E , A)

其中，V = {1, 2, ..., N } 是节点集合，每个节点对应一架飞机；E 是通信边集合；A = $*aij*$ 是邻接矩阵。对于无向无权图：

aij = 1（当 (i , j ) ∈ E ）， aij = 0（当 (i , j ) ∉ E）

若 aij = 1，第 i 架飞机在一致性计算中使用第 j 架飞机的信息。无向图满足 aij = aji ；加权图则允许 aij 取非负权重，用于描述不同通信关系或控制增益。

教案中的五节点无向图对应邻接矩阵：

第 i 个节点的度为 di = Σj aij，度矩阵写成：

D = diag(3, 2, 2, 3, 2)

矩阵的第 i 行直接给出本机与邻居的误差组合：

$*Lz*$ i = Σj ∈ Ni aij (zi − zj)

因此，程序不需要单独遍历所有误差项，只需取 L 的第 i 行与全体协同状态相乘。实际分布式实现并不要求本机接收所有节点的信息，因为第 i 行中非零的非对角元素只对应本机邻居。

1. 一阶平均一致性

先考虑一维一阶系统：

ż = u ， z = $*z* 1, *z* 2, ..., *zN*$ T

采用一致性协议：

ui = −kc Σj ∈ Ni aij (zi − zj)

写成矩阵形式即 ż = −kcLz 。对于固定、无向且连通的通信图，L 只有一个零特征值，其余特征值为正。系统状态最终收敛到初值的算术平均：

limt →∞ zi (t ) = (1/N ) Σj =1N zj(0)

参数 kc > 0 调整理想模型中的收敛时间尺度，但增大 kc 也会提高控制变化和对通信噪声的敏感度。工程中还会受到消息周期、时延、控制限幅和飞行器动态的共同限制。

**注意："图连通即可"需要说明适用范围。**固定无向图在连通条件下可以实现上述一阶平均一致性。有向图通常需要存在有向生成树才能保证状态趋于共同值，而收敛到算术平均还需要权重平衡等附加条件；切换拓扑、通信时延和丢包也需要分别满足相应假设，不能只用一次静态连通性检查代替 $1$ $2$ 。

三、普通一致性怎样变成编队一致性

若直接令所有位置 pi 达到一致，最终结果是所有飞机趋向同一个位置，这与保持安全间隔的编队目标相矛盾。编队控制需要先从各机状态中构造一个应当相同的协同变量，再对该变量执行一致性。

本文采用虚拟结构参考点。设第 i 架飞机在编队坐标系中的槽位偏置为 ri，二维旋转矩阵为：

R (χi ) = $cos*χi* , -sin*χi* ; sin*χi* , cos*χi*$

第 i 架飞机根据自身位置和航迹方向反推出虚拟编队参考点：

ci = pi − R (χi )ri

当所有 ci 收敛到同一位置，并且各机对编队朝向的估计一致时，有：

pi − pj = R (χc )(ri − rj)

这意味着飞机之间保持由槽位差定义的相对几何关系。虚拟结构方法用于描述队形，一致性协议用于协调各机对结构参考点的估计，两者承担不同功能 $3$ 。

1. 两种可选的一致性变量

协调项给出以下两种实现路线：

参考点坐标一致性 ：各机交换二维参考点 ci = $*cx,i* , *cy,i*$ T，分别对 x 、y 坐标计算一致性项。方法直观，适合一般折线或平面轨迹，但至少需要传输两个连续量，且还要单独保证公共参考点收敛到任务路径。
路径参数一致性 ：先把参考点投影到公共参数路径 p (τ )，再对弧长、相位或虚拟参数 τi 进行一致性。通信变量可以降为一维，形成后的编队也自然附着在公共路径上，但需要解决闭合路径参数回绕、不同曲率区间的速度映射以及参数投影唯一性。

迅翼六机仿真例程采用第一种方案。每架飞机通过 ROS 发布自身虚拟参考点坐标，并接收环形拓扑中两个邻居的参考点。设 cx = $*cx,1* , ..., *cx,N*$ T，cy = $*cy,1* , ..., *cy,N*$ T，二维一致性速度为：

vc,x = −kcLcx ， vc,y = −kcLcy

第 i 架飞机只需计算对应分量：

vc,i = −kc Σj ∈ Ni aij (ci − cj)

四、把路径跟踪与一致性速度合成

一致性项只能减小各机虚拟参考点之间的差异，不能独立决定编队沿哪条任务路径飞行。因此，每架飞机还需要根据本机参考点 ci 计算路径投影和切向速度。

设公共路径为 P ，qi 是 ci 在路径上的最近点：

qi = arg minq ∈ P ‖q − ci‖

路径切向单位向量记为 ti ，巡航速度为 Vd。用于参考点路径跟踪的速度可以写成：

vp,i = Vdti + kp (qi − ci)

程序会对合成结果重新归一化，使路径跟踪部分保持给定的名义速度。若路径存在曲率，槽位随编队朝向转动还会产生附加速度。定义平面内旋转 90° 的矩阵 J = $0, -1; 1, 0$ ，槽位旋转速度可以写成：

vr,i = ωiJR (χp,i )ri

其中 χp,i 是路径切向方向，ωi 是编队参考方向的角速度。该式等价于程序中先把二维槽位扩展为三维向量，再计算 $0, 0, *ωi*$ T × ri。于是，飞机的期望速度可以概括为：

vid = vp,i + vr,i + vc,i

三项分别负责沿路径前进、补偿槽位旋转以及减小邻居参考点误差。本文折线路径示例采用零路径半径，合成速度主要由路径切向速度、路径收敛速度和参考点一致性速度构成。对于连续圆弧或大尺度转弯，曲率与槽位旋转速度会直接影响内外侧成员的速度分配，此时应将 vr,i 纳入期望速度计算。

1. 从期望速度到固定翼姿态指令

与上一期相同，单机水平运动采用双积分近似：

ẋi = Axi + Bai ， xi = $*pi* ; *vi*$

根据路径投影和槽位偏置构造期望位置 pid ，再与 vid 组成期望状态 xid = $*pid* ; *vid*$ 。LQR 反馈生成水平期望加速度：

aid = −Ki (xi − xid)

将 aid 分解为沿速度方向和法向的分量：

a ∥,i = (aid )Tvi / ‖vi ‖， a ⊥,i = aid − $(*aid* )T*vi* / (*viTvi* )$ vi

协调转弯近似下，滚转角参考为：

φid = sgn(vx,iay,id − vy,iax,id ) · arctan(‖a ⊥,i ‖ / g)

期望航迹方向由路径切向速度给出，高度误差转换为俯仰角参考，最终通过 [roll, pitch, yaw, a] 姿态接口发送给固定翼底层控制器。固定翼输入与速度约束会改变理想一致性系统的收敛过程，因此不能仅凭 ż = −Lz 的理论结果判断真机编队性能。固定翼集群研究通常需要把协同律与最小空速、最大航向角速度和滚转角等约束同时分析 $4$ 。

**从一致性算法到固定翼控制接口。**迅翼仿真平台已经连通通信拓扑、协同算法、ROS 消息与 PX4 固定翼控制接口。研究者可以在统一环境中替换编队策略、比较通信结构和调整控制参数，把精力集中在协同方法与飞行表现上。

五、迅翼仿真平台中的六机 ROS 实现

该例程由 ConsensusFollow.py 实现二维编队与 LQR 计算，consensus_uav.py 完成 ROS 通信、三维状态更新和姿态指令发送，spawn_uav.py 封装各机的 MAVROS/Gazebo 状态与控制接口。整体程序结构如下：

主要文件职责可以概括为：

文件	主要职责
`consensus_uav.py`	发布本机参考点、订阅邻居参考点、更新三维状态并发送姿态指令
`ConsensusFollow.py`	计算虚拟参考点、路径期望状态、一致性速度、LQR 加速度与滚转角
`DesirePath.py`	计算折线路径最近点、路径切向速度和距离信息
`FormationFun.py`	根据飞机数量和间距生成圆形编队槽位
`spawn_uav.py`	读取无人机位置、速度与姿态，并封装 PX4/MAVROS 控制消息
`6_consensus_plane.launch`	启动 6 个一致性节点和 6 个固定翼底层控制节点

1. 环形拓扑如何进入 ROS 订阅关系

示例通过 CircleCommunicationMGenerate() 生成环形图拉普拉斯矩阵。对于 N 架飞机，每个节点与前后两个节点通信：

Lii = 2， L i ,(i −1) mod N = −1， L i ,(i +1) mod N = −1

每个节点发布话题 /uav{i}/consensus_data，消息类型为 geometry_msgs/Vector3，其中 x、y 保存本机虚拟参考点坐标，z 保存当前高度。节点只为 Lij ≠ 0 的邻居建立订阅，从而使通信关系与拉普拉斯矩阵一致。

一致性计算在代码中对应两行核心操作：

python 复制代码

consensus_v = -self.L[self.id, :] @ all_agent_pos * self.consensus_k
desire_v = path_v + converge_v + consensus_v

实际代码先对 path_v + converge_v 归一化，再叠加 consensus_v，并把一致性速度限制在 4 m/s 内。这项限幅用于避免初始参考点误差较大时产生过高速度修正，但限幅也会延长编队形成时间。

2. 六机示例的主要参数

参数	示例设置	作用
飞机数量	6	`6_consensus_plane.launch` 启动数量
通信拓扑	无向环形图	每架飞机与两个邻居交换参考点
任务路径	1000 m × 1000 m 闭合折线	公共路径参考
名义巡航速度	20 m/s	路径切向速度
编队间距参数	20 m	圆形槽位尺度
一致性增益	kc = 0.1	参考点误差修正强度
路径收敛参数	`converge_k = 500`	数值越大，横向收敛速度越小
一致性速度上限	4 m/s	限制邻居误差引起的速度修正
水平加速度上限	5 m/s²	限制 LQR 输出
名义高度	80 − i m	按编号设置 1 m 高度差
控制周期	约 0.1 s	ROS 主循环周期

高度按编号错开可以减少初始集结阶段轨迹完全重合的概率，但 1 m 高度差不能替代正式的机间避碰策略。外场试验仍需根据翼展、定位精度、尾涡、通信时延和应急机动能力设置安全间隔。

六、从仿真现象理解一致性编队

该六机例程把通信拓扑、虚拟结构、一致性误差和固定翼控制接口放在同一场景中。通过改变初始位置、通信关系和控制参数，可以直观看到以下几类现象：

一致性收敛与队形形成是两个过程 ：‖ci − c̄‖ 减小说明各机对虚拟结构参考点的估计逐渐接近；实际相对位置是否满足槽位关系，还要结合编队几何误差判断。参考点已经接近时，飞机仍可能因为速度、滚转角和路径跟踪动态而处于队形调整阶段。
通信拓扑决定误差如何传播：环形图中，每架飞机只与前后邻居交换信息，局部误差会沿链路逐步传递。成员数量增加或链路减少时，拉普拉斯矩阵的代数连通度通常下降，编队达到共同参考所需的时间也会发生变化。
一致性增益需要与飞行约束匹配 ：增大 kc 可以提高理想模型中的误差修正速度，但过大的修正量可能更频繁地触发速度、加速度或滚转角限制。此时，理论收敛速度不会直接等同于飞机的实际队形形成速度。
路径几何会影响编队暂态：在折线路径的拐角处，路径切向方向快速变化，各机还要同时完成槽位旋转和一致性修正。路径误差与编队误差可能在转弯阶段同步增大，内外侧成员所需的速度也不相同。
网络连通性决定协同范围：单个节点退出后，环形图通常会变为仍然连通的链式图，剩余成员可以继续交换信息；如果多条链路失效并使网络分裂，不同连通分量将分别形成各自的共同状态，无法继续维持全编队一致。

这些现象说明，一致性协议解决的是邻居信息如何形成共同协同变量，而完整的固定翼编队还需要路径跟踪、状态估计、通信管理和单机控制共同作用。大尺度曲线转弯时，槽位旋转速度 ωiJR (χp,i )ri 会影响内外侧成员的速度分配；在成员交叉或链路变化的场景中，还要结合安全间隔、冲突预测和失效处置策略分析飞行过程。

因此，阅读或比较一致性编队实验时，可以从三个层次观察结果：协同层关注参考点一致性误差和编队几何误差；通信层关注消息周期、时延与拓扑变化；飞行层关注路径误差、空速以及姿态和控制量是否接近限制。一致性 UAV 实现综述也指出，理论协议进入飞行平台后，需要与状态估计、目标或路径跟踪、通信管理和底层控制器共同评价 $5$ 。这种分层观察方式能够区分"数学状态已经趋同"和"固定翼编队已经稳定可飞"两个结论。

七、从一致性例程到固定翼编队项目

迅翼 SwiftWing 仿真平台已经集成 leader--follower 与一致性固定翼编队例程。研究者可以修改飞机数量、通信拓扑、槽位、路径、LQR 权重和一致性增益，也可以进一步替换为路径参数一致性、切换拓扑、编队重构或分布式 MPC，而不必重新搭建单机 PX4、ROS、Gazebo 和状态接口。

结合 SwiftWing S7 固定翼科研平台，现有例程可以继续延伸到机载代码适配、多机通信联调、半实物仿真和外场飞行验证。针对高校、科研院所和项目团队的具体研究目标，迅翼可进一步评估算法移植、接口适配、通信方案、试验设计与定制平台的实现范围。

需要配套代码、仿真平台资料或一致性编队方案的读者，可以在公众号后台回复 "一致性编队"，并说明计划验证的飞机数量、通信拓扑、算法阶段和试验目标。

八、总结

一致性编队将通信关系写入邻接矩阵和拉普拉斯矩阵，各机根据本机与邻居的协同变量误差生成局部修正量。普通位置一致性会使成员趋向同一点；固定翼编队需要对带槽位偏置的虚拟参考点、航迹角、速度或路径参数执行一致性，再由路径跟踪和单机制导层生成可飞参考。

本文例程对二维虚拟参考点坐标进行一致性，并将路径速度、路径收敛速度和一致性速度组合为期望运动，再复用 LQR 与姿态控制接口。该结构适合验证静态环形拓扑下的多机编队，但其收敛与安全性仍受消息有效性、通信时延、拓扑变化、速度饱和、转弯曲率和机间避碰约束影响。

参考文献

OLFATI-SABER R, FAX J A, MURRAY R M. Consensus and cooperation in networked multi-agent systems $J$ . Proceedings of the IEEE, 2007, 95(1): 215-233. DOI: 10.1109/JPROC.2006.887293.
REN W, BEARD R W. Distributed consensus in multi-vehicle cooperative control: Theory and applications $M$ . London: Springer, 2008. DOI: 10.1007/978-1-84800-015-5.
LEWIS M A, TAN K H. High precision formation control of mobile robots using virtual structures $J$ . Autonomous Robots, 1997, 4(4): 387-403. DOI: 10.1023/A:1008814708459.
CHEN H, WANG X K, SHEN L C, et al. Formation flight of fixed-wing UAV swarms: A group-based hierarchical approach $J$ . Chinese Journal of Aeronautics, 2021, 34(2): 504-515. DOI: 10.1016/j.cja.2020.03.006.
LIZZIO F F, CAPELLO E, GUGLIERI G. A review of consensus-based multi-agent UAV implementations $J$ . Journal of Intelligent & Robotic Systems, 2022, 106: 43. DOI: 10.1007/s10846-022-01743-9.