｜固定翼如何依靠邻居信息保持队形？一致性控制、通信拓扑与分布式仿真实现，附仿真代码)
上一篇《编队系列（一）｜固定翼领航-跟随如何实现？虚拟结构与 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= Σjaij，度矩阵写成：
D= diag(3, 2, 2, 3, 2)
矩阵的第i行直接给出本机与邻居的误差组合：
[Lz]i= Σj∈Niaij(zi−zj)
因此，程序不需要单独遍历所有误差项，只需取L的第i行与全体协同状态相乘。实际分布式实现并不要求本机接收所有节点的信息，因为第i行中非零的非对角元素只对应本机邻居。
1. 一阶平均一致性
先考虑一维一阶系统：
ż=u，z= [z1,z2, …,zN]T
采用一致性协议：
ui= −kcΣj∈Niaij(zi−zj)
写成矩阵形式即ż= −kcLz。对于固定、无向且连通的通信图，L只有一个零特征值，其余特征值为正。系统状态最终收敛到初值的算术平均：
limt→∞zi(t) = (1/N) Σj=1Nzj(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∈Niaij(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)Tvi/ (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，Li,(i−1) modN= −1，Li,(i+1) modN= −1
每个节点发布话题/uav{i}/consensus_data，消息类型为geometry_msgs/Vector3，其中x、y保存本机虚拟参考点坐标，z保存当前高度。节点只为Lij≠ 0 的邻居建立订阅，从而使通信关系与拉普拉斯矩阵一致。
一致性计算在代码中对应两行核心操作：
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 −im | 按编号设置 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.
