3. 坐标系与运动学模型：常用坐标系、无人机运动学与动力学模型、编队控制中的坐标变换
多无人机协同控制的核心挑战在于：每架无人机都在自身局部坐标系中感知环境、执行控制，而编队任务却要求在全局坐标系中定义队形、分配目标、协调动作。如果坐标系定义不统一、变换关系不清晰，任何高级编队算法都将失去物理基础。本章从坐标系定义出发，系统梳理无人机运动学与动力学模型，并重点讲解编队控制中坐标变换的工程实现方法。
3.1 常用坐标系定义与选择原则
3.1.1 核心原理：三类坐标系的角色分工
在多无人机协同系统中，坐标系不是单一概念，而是分层嵌套的参考框架。工程实践中必须明确区分以下三类坐标系：
- 全局坐标系（Global Frame）：通常采用NED（北-东-地）或ENU（东-北-天）惯性系，原点固定于地面站或任务起始点。全局坐标系是编队队形定义、任务规划、航迹规划的基准。NED系符合航空惯例，x轴指向北，z轴指向地心；ENU系则更贴近机器人领域习惯，z轴指向天顶。
- 机体坐标系（Body Frame）：原点位于无人机质心，x轴指向机头方向，y轴指向右翼，z轴指向机腹（NED约定）或机顶（ENU约定）。机体坐标系是IMU、陀螺仪、加速度计等机载传感器的直接参考系，也是飞控系统内环姿态控制的执行基准。
- 编队坐标系（Formation Frame）：原点通常定义在编队几何中心或领机质心，各轴方向与全局坐标系保持一致，或随领机姿态旋转。编队坐标系用于描述僚机相对于领机的期望位置偏差，是编队控制器设计的主要工作空间。
选择坐标系时需遵循三个原则：任务匹配原则（全局任务用全局系，局部避障用机体系）、传感器对齐原则（测量数据在哪个坐标系产生，就应在哪个坐标系处理）、计算效率原则（频繁变换会增加计算负担，应尽量减少不必要的坐标转换）。
3.1.2 判断方法：坐标系选择决策流程
- 明确任务阶段：起飞/降落阶段以机体坐标系为主，编队巡航阶段以全局坐标系为主，编队机动阶段需同时使用编队坐标系与机体坐标系。
- 识别传感器输出：GPS输出全局系位置，IMU输出机体系角速度与加速度，视觉SLAM输出相机系位姿——需统一转换到控制器所需坐标系。
- 评估机动幅度：小角度机动（<30°）可用欧拉角线性化近似；大角度机动（>45°）必须使用四元数或旋转矩阵完整表达。
- 确定通信拓扑：集中式编队以编队坐标系为统一基准，分布式编队需各机维护局部编队坐标系并定期同步。
pos_global、vel_body），并在函数命名中包含坐标系前缀（如 globalToBody()）。这能显著减少因坐标系混淆导致的调试时间。
3.2 无人机运动学与动力学模型
3.2.1 运动学模型：位置与姿态的几何关系
运动学模型描述无人机位置、速度、姿态角之间的纯几何关系，不涉及力与力矩。对于固定翼无人机，运动学方程可简化为：
ẋ = V·cos(ψ)·cos(θ)
ẏ = V·sin(ψ)·cos(θ)
ż = V·sin(θ)
ψ̇ = ω_z其中V为空速，θ为俯仰角，ψ为偏航角，ω_z为偏航角速度。对于旋翼无人机，运动学模型通常简化为三自由度质点模型，忽略姿态动力学细节，直接以加速度指令作为输入：
p̈ = a_cmd这种简化在编队控制中非常常见，因为编队算法关注的是位置协同而非姿态内环。
3.2.2 动力学模型：力与运动的因果关系
动力学模型描述推力、重力、气动力与加速度之间的关系。以四旋翼为例，其动力学方程可写为：
m·a = R·T_total - m·g·e_z - D·v其中R为机体到全局的旋转矩阵，T_total为总推力向量，D为阻力系数矩阵。在编队控制中，动力学模型主要用于两个目的：一是生成可行的加速度指令（考虑推力限制），二是进行前馈补偿（如领机机动时的惯性力补偿）。
| 模型类型 | 状态变量 | 输入 | 适用场景 |
|---|---|---|---|
| 质点运动学 | 位置、速度 | 加速度指令 | 编队队形规划、避障 |
| 刚体运动学 | 位置、速度、姿态角 | 角速度、线速度 | 编队协同转弯、姿态同步 |
| 简化动力学 | 位置、速度、加速度 | 推力、力矩 | 控制器设计、仿真验证 |
| 全量动力学 | 位置、速度、姿态、角速度 | 电机转速、舵面偏角 | 高保真仿真、硬件在环测试 |
3.3 编队控制中的坐标变换
3.3.1 核心原理：从全局到编队的变换链
编队控制中的坐标变换本质上是刚体变换的级联应用。设领机在全局坐标系中的位姿为 (p_L, R_L)，僚机在编队坐标系中的期望位置为 Δp_F，则僚机的全局期望位置为：
p_desired_G = p_L + R_L · Δp_F这里R_L是领机姿态对应的旋转矩阵，它将编队坐标系中的偏移量旋转到全局坐标系方向。若编队坐标系与全局坐标系方向一致（不随领机旋转），则R_L退化为单位矩阵，变换简化为纯平移。
3.3.2 操作流程：坐标变换的工程实现步骤
- 定义编队队形：在编队坐标系中定义各僚机的期望偏移量 Δp_F = [Δx, Δy, Δz]^T，如三角形编队可定义为 Δp_1=[0,0,0]、Δp_2=[-5,3,0]、Δp_3=[-5,-3,0]（单位：米）。
- 获取领机状态：通过通信链路获取领机的全局位置 p_L 和姿态四元数 q_L（或欧拉角）。
- 计算旋转矩阵：将四元数转换为旋转矩阵 R_L，或直接使用欧拉角构建 R = Rz(ψ)·Ry(θ)·Rx(φ)。
- 执行变换：计算僚机全局期望位置 p_desired_G = p_L + R_L · Δp_F。
- 生成控制指令：将 p_desired_G 与僚机当前全局位置比较，生成位置误差，经控制器转换为机体坐标系的加速度指令。
背景与约束：5架四旋翼无人机执行区域侦察任务，需保持V形编队（领机在前，左右各2架僚机，间距5m，侧向偏移3m）以30°坡度角协同转弯。通信链路为2.4GHz数传，延迟约50ms，GPS定位精度±1m。
初始现象：直线飞行时编队保持良好，但进入转弯后僚机出现明显滞后，队形扭曲，左右僚机不对称。
分析判断：问题根源在于坐标变换未考虑领机姿态变化。原实现中直接使用 p_desired_G = p_L + Δp_F，忽略了R_L旋转项。转弯时领机偏航角变化，编队坐标系随领机旋转，但僚机仍按全局系偏移量计算期望位置，导致外侧僚机跟不上、内侧僚机过度修正。
实际操作：① 在编队控制器中加入领机姿态四元数订阅；② 将队形偏移量左乘领机旋转矩阵：p_desired_G = p_L + R_L·Δp_F；③ 增加前馈项：在转弯指令中加入领机角速度前馈，补偿向心加速度需求；④ 将位置环带宽从0.5Hz提升至1.0Hz，以应对转弯动态。
结果对比：修改后编队最大位置误差从2.8m降至0.6m，左右僚机对称性恢复，转弯过程队形保持完整。
复盘经验：编队控制中坐标变换必须完整包含旋转项，不能因直线飞行时旋转矩阵为单位阵就省略该计算。同时，通信延迟导致的领机状态滞后需通过预测补偿（如线性外推）解决。
3.3.3 常见误区与总结复盘
- 误区一：混淆NED与ENU——NED系z轴向下，ENU系z轴向上，若混用会导致高度方向控制反向。解决方法是统一约定并在代码中强制检查。
- 误区二：欧拉角万向锁——当俯仰角接近±90°时，偏航与横滚耦合，旋转矩阵奇异。编队大机动时应切换为四元数表示。
- 误区三：忽略领机姿态的旋转效应——如案例所示，编队转弯时队形偏移量必须随领机旋转，否则队形扭曲。
- 误区四：坐标系变换的累积误差——多次变换会累积浮点误差，建议定期用全局GPS数据校正局部估计。
本章内容为后续编队控制算法奠定了数学基础。理解坐标系定义、运动学/动力学模型以及坐标变换的工程实现，是设计鲁棒编队控制器的前提。建议读者在仿真环境中（如PX4+Gazebo）实际搭建三机编队，通过修改坐标变换代码观察队形变化，以加深对本章概念的理解。