2. 单机运动学与动力学基础

编队控制的所有推导，最终都要落到“每架无人机在空间中处于什么位置、朝哪个方向、以什么速度运动”这几个量上。如果单机的坐标约定、状态变量和输入输出关系没有统一，多机之间的相对位置、期望间距、队形误差就无从谈起。本节先把单机模型讲清楚，为后续编队建模提供统一的数学底座。

一、坐标系定义：先约定，再计算

描述一架四旋翼的运动，至少需要两个坐标系。第一个是惯性坐标系（世界系），记为 {E}，原点固定在地面某点，x_E 指向东、y_E 指向北、z_E 垂直向上，符合右手定则。第二个是机体坐标系，记为 {B}，原点取在无人机质心，x_B 指向机头前方，y_B 指向机身右侧，z_B 垂直机身向上。

两个坐标系之间的姿态关系用旋转矩阵 R ∈ SO(3) 表示，它把机体系下的向量变换到世界系：p_E = R · p_B。若用欧拉角描述，取 Z-Y-X 顺序（先偏航 ψ、再俯仰 θ、最后滚转 φ），则旋转矩阵为：

R = Rz(ψ)·Ry(θ)·Rx(φ)
  = [ cψcθ,  cψsθsφ - sψcφ,  cψsθcφ + sψsφ
      sψcθ,  sψsθsφ + cψcφ,  sψsθcφ - cψsφ
      -sθ,   cθsφ,           cθcφ          ]

其中 c 表示 cos，s 表示 sin。这里必须强调：全文及所有图示统一采用“世界系 z 轴向上、机体系 z 轴向上、Z-Y-X 欧拉角”这一约定。若切换到 NED 坐标系（z 轴向下），重力方向、高度符号、推力方向都会反号，编队的高度保持逻辑必须同步修改，否则会出现“越飞越低”的隐蔽错误。

二、刚体运动模型：位置与姿态的耦合

四旋翼是典型的六自由度刚体：三个平动自由度（世界系位置 p = [x, y, z]ᵀ）和三个转动自由度（欧拉角 η = [φ, θ, ψ]ᵀ）。平动方程由牛顿第二定律给出：

m·p̈ = R·[0, 0, T]ᵀ - [0, 0, m·g]ᵀ + F_d

其中 m 为质量，T 为四个旋翼产生的总推力，g 为重力加速度，F_d 为气动阻力等扰动。这个式子揭示了四旋翼最本质的特性：推力方向始终沿机体 z 轴，因此要产生水平方向的加速度，必须先倾斜机身。也就是说，位置控制无法直接输出水平力，只能通过改变姿态间接实现——这就是“欠驱动”的根源。

转动方程可写为 J·ω̇ = τ - ω × (J·ω)，其中 J 为转动惯量矩阵，ω 为机体角速度，τ 为三轴力矩。在编队飞行中，姿态环通常作为内环快速跟踪，外环的位置/速度指令由编队控制器给出，因此后续建模多采用“位置-速度-加速度”层面的简化模型。

三、旋翼无人机简化模型：从六自由度到双积分

当姿态内环带宽远高于位置外环时，可以认为姿态指令被瞬时跟踪，此时单机模型可简化为世界系下的双积分形式：

ṗ = v
v̇ = a_cmd + [0, 0, -g]ᵀ

其中 a_cmd 是位置控制器输出的期望加速度。这个简化模型是编队算法最常用的对象：每架无人机被抽象为一个受控的二阶系统，编队控制器只需计算 a_cmd，再由底层姿态控制器解算为推力与力矩。需要提醒的是，简化模型成立的前提是姿态响应足够快、倾角不过大（一般小于 30°），否则水平加速度与倾角之间的近似关系会明显失真。

四、状态变量与输入输出关系

把上述内容整理成标准状态空间形式，单机状态量取 12 维：

x = [x, y, z, ẋ, ẏ, ż, φ, θ, ψ, p, q, r]ᵀ

前 6 维是位置与速度，后 6 维是姿态角与机体角速度。控制输入通常取 4 维：总推力 T 与三轴力矩 [τ_φ, τ_θ, τ_ψ]ᵀ。输出量则根据任务需要选取，编队控制最关心的是位置 p 和偏航角 ψ，因为队形由各机的位置和朝向共同决定。

下面这张图把“坐标系—状态—输入—输出”的关系串起来，帮助建立单机模型的整体印象：

五、为编队建模打基础：从单机到多机

有了单机模型，编队建模就变得清晰。假设编队中有 N 架无人机，第 i 架的状态为 p_i, v_i，期望队形由相对位置 d_ij 描述，即 p_i - p_j → d_ij。编队控制器要做的，就是根据邻居的状态偏差计算每架的 a_cmd,i，再通过单机模型映射为实际运动。

这里有一个容易忽略的细节：编队误差必须在同一坐标系下计算。如果某架无人机的位置来自 GPS（世界系），而另一架来自视觉里程计（局部系），直接相减得到的相对位置就包含坐标系偏差。工程上通常做法是：所有无人机共享同一世界系原点，或通过相对定位模块统一到同一参考系后再做队形误差计算。