三、编队飞行基础：编队概念、编队队形与坐标系转换
编队飞行是无人机集群作战与协同任务的核心技术形态，其本质是让多架无人机在空间上保持既定几何关系，并在时间维度上同步执行机动。无论是物流配送中的多机协同、农业植保的并行作业，还是军事侦察的分布式压制，编队飞行的底层逻辑都依赖于三个基本支柱：编队概念（定义“谁跟谁、保持什么关系”）、队形设计（定义“空间上如何排列”）以及坐标系转换（定义“如何用数学语言描述这种关系”）。本章将系统拆解这三个支柱，并重点剖析领航-跟随系下的坐标转换方法，为后续的编队控制律设计与避障策略打下坚实的数学与物理基础。
3.1 编队飞行的核心概念与任务分类
核心原理：编队飞行（Formation Flight）是指两架或以上无人机按照预设的几何约束，在飞行过程中保持相对位置与姿态稳定的协同运动模式。其数学本质是求解一个带约束的最优控制问题：在满足动力学模型、执行器饱和、通信时延和避碰安全边界的前提下，使每架飞机的实际状态向量（位置、速度、姿态）与期望状态向量的误差最小化。编队概念中最重要的三个要素是：编队参考点（通常取领航机或虚拟几何中心）、编队队形向量（描述各机相对参考点的期望偏移）以及编队刚性（队形在机动过程中保持不变的严格程度）。
判断方法：在实际工程中，我们通常根据任务需求将编队分为三类：刚性编队（要求队形严格不变，适用于精密编队表演或雷达阵列）、柔性编队（允许队形在一定范围内伸缩，适用于避障或穿越狭窄区域）以及可变编队（根据任务阶段动态切换队形，如从一字形切换为菱形以增强防御）。判断采用何种编队模式，主要依据三个指标：任务对空间覆盖的要求、机间通信带宽与可靠性、以及单机自主能力等级。
| 编队类型 | 刚性要求 | 典型应用 | 控制复杂度 | 通信依赖 |
|---|---|---|---|---|
| 刚性编队 | 高（误差<0.1m） | 编队表演、SAR成像 | 高（需全状态反馈） | 高（10Hz以上） |
| 柔性编队 | 中（误差<1m） | 物流配送、协同搜索 | 中（位置环为主） | 中（5-10Hz） |
| 可变编队 | 低（切换时允许瞬态误差） | 突防、动态避障 | 高（需模式切换逻辑） | 中（事件触发） |
3.2 典型编队队形：几何定义与适用场景
核心原理：队形设计的本质是在空间上分配每架无人机的期望位置，其数学表达为队形向量集合 D = {d_1, d_2, ..., d_n}，其中 d_i = [Δx_i, Δy_i, Δz_i]^T 表示第 i 架飞机相对于编队参考点在领航-跟随系中的偏移量。队形设计需综合考虑三个物理约束：气动干扰（前后机距离过近会导致尾流影响，通常要求纵向间距大于 3 倍翼展）、传感器视场（光电吊舱或雷达的探测角度限制）以及通信链路质量（机间距离越远，数据链误码率越高）。
操作流程：选择队形时遵循以下步骤：第一步，明确任务目标（如区域覆盖、目标跟踪、载荷协同）；第二步，根据目标确定队形几何参数（间距、角度、高度差）；第三步，进行仿真验证（在Gazebo或AirSim中模拟不同风场下的队形保持误差）；第四步，实飞测试并调整参数。以三角队形为例，其几何定义为：领航机位于顶点，两架跟随机位于底边两端，与领航机形成 60° 夹角，典型间距为 5-10 米。这种队形的优势在于每架飞机都有清晰的视野，且后机可利用前机的涡流升力（在固定翼中尤为明显）。
3.3 编队坐标系体系：惯性系、机体系与领航-跟随系
核心原理：编队飞行涉及三个核心坐标系，它们之间的转换关系是编队控制算法的数学基础。首先是惯性坐标系（Earth-fixed Frame, E系），通常采用北-东-地（NED）结构，原点固定于地面某参考点，用于描述无人机的位置和航迹。其次是机体坐标系（Body Frame, B系），原点位于无人机质心，X轴指向机头，Y轴指向右翼，Z轴指向下方，用于描述姿态角（滚转、俯仰、偏航）和机体速度。最后是领航-跟随系（Leader-Follower Frame, L系），这是一个随领航机运动的动坐标系，原点位于领航机质心，三轴方向与领航机的机体系对齐。L系的引入使得编队队形定义变得直观——我们只需在L系中指定各跟随机的期望偏移量，而无需关心领航机自身的姿态。
转换方法：坐标转换的核心是旋转矩阵与平移向量。从B系到E系的转换使用姿态旋转矩阵 R_EB，该矩阵由欧拉角（滚转φ、俯仰θ、偏航ψ）构成，标准形式为 Z-Y-X 顺序的旋转矩阵乘积。从L系到E系的转换则分为两步：先将L系中的偏移向量 d_L 通过领航机的姿态矩阵 R_EL 旋转到E系方向，再加上领航机在E系中的位置 P_L，即 P_E = P_L + R_EL * d_L。在实际代码实现中，我们通常使用四元数代替欧拉角以避免万向锁问题，转换公式为 P_E = P_L + q_L ⊗ d_L ⊗ q_L*，其中 q_L 为领航机姿态四元数。
// 伪代码：将跟随机期望位置从L系转换到E系
// 输入：领航机位置 leader_pos_E, 领航机姿态四元数 q_leader, 队形偏移 d_L
// 输出：跟随机期望位置 follower_desired_E
// 1. 将偏移向量旋转到E系（四元数旋转）
Vector3f d_E = rotateVectorByQuaternion(d_L, q_leader);
// 2. 加上领航机位置
Vector3f follower_desired_E = leader_pos_E + d_E;
// 3. 若需进一步转换到跟随机的机体系，则使用跟随机的姿态四元数 q_follower
Vector3f d_B = rotateVectorByQuaternion(follower_desired_E - follower_pos_E, conjugate(q_follower));3.4 坐标系转换的工程实现与误差分析
核心原理：在真实的飞控系统中，坐标转换并非简单的数学运算，还涉及传感器噪声、时间同步和通信延迟等工程问题。惯性导航系统（INS）提供的姿态数据通常带有漂移，GPS提供的位置存在米级噪声，而机间通信链路（如Xbee或WiFi）会引入数十毫秒的延迟。因此，工程实现中需要采用扩展卡尔曼滤波器（EKF）或无迹卡尔曼滤波器（UKF）对多源数据进行融合，以得到平滑且一致的坐标系转换结果。
操作流程：在PX4/ArduPilot中实现编队坐标转换的标准流程如下：第一步，通过MAVLink协议以20Hz频率接收领航机的状态数据（位置、速度、四元数）；第二步，在本地维护一个领航机状态缓存，利用时间戳进行延迟补偿（通常采用线性插值或零阶保持）；第三步，根据当前编队队形参数计算期望位置在L系中的坐标；第四步，调用 attitudeRotation() 函数将期望位置转换到E系；第五步，将E系期望位置输入位置控制器，生成速度与姿态指令。整个流程需要在飞控的 navigator 模块中实现，且必须保证转换计算耗时小于 2ms。
| 误差来源 | 典型量级 | 影响结果 | 补偿策略 |
|---|---|---|---|
| GPS定位噪声 | ±0.5m（单点） | 编队位置抖动 | RTK差分定位（±2cm） |
| 姿态估计漂移 | 0.1°/min | 队形缓慢变形 | 磁力计+GPS航向融合 |
| 通信延迟 | 20-100ms | 跟踪滞后 | Smith预估器或延迟补偿 |
| 时间戳不同步 | 1-10ms | 转换误差累积 | PTP精确时间同步协议 |
3.5 常见误区与总结复盘
常见误区一：将机体系与领航-跟随系混为一谈。机体系是固连于每架无人机自身的坐标系，其原点随飞机移动且方向随姿态变化；而领航-跟随系是固连于领航机的坐标系，用于描述编队内各机的相对几何关系。在编写编队控制律时，必须明确当前计算是在哪个坐标系下进行的。例如，若在跟随机的机体系中直接使用L系定义的队形偏移量，会导致严重的控制错误。
常见误区二：忽略地球曲率与椭球模型。当编队飞行范围超过 10km 时，将惯性系简化为平面直角坐标系会引入不可忽略的误差。此时应使用WGS-84椭球模型，并将经纬度坐标转换为局部切平面（ENU或NED）坐标。在课程实训中，建议使用 geographiclib 库进行高精度坐标转换。
常见误区三：队形切换时未考虑动力学约束。从一字队形切换为菱形队形时，如果直接给定新的期望位置，跟随机会产生过大的加速度指令，导致电机饱和甚至失速。正确的做法是使用轨迹规划器（如Minimum Snap轨迹）生成平滑的过渡路径，并限制最大加速度与加加速度。