2. 坐标系与运动学：地球坐标系、机体坐标系、欧拉角与四元数、无人机运动学模型、编队相对运动学
各位同学，欢迎来到第二章。
这一章，咱们要啃一块硬骨头——坐标系与运动学。说实话，这部分内容在很多人看来就是一堆枯燥的数学公式。但我得告诉你，这恰恰是编队飞行的地基。地基不稳，楼盖得再高也得塌。
我自己带过不少项目，见过太多团队在编队控制上栽跟头。追根溯源，十有八九是坐标系搞混了，或者运动学模型没建对。所以这一章，我会把我在实际项目中踩过的坑、总结的经验，都揉碎了讲给你听。
核心要点：坐标系是描述无人机状态的“语言”，运动学模型是描述无人机行为的“语法”。编队飞行，就是让多架无人机用同一种“语言”和“语法”协同工作。
2.1 地球坐标系与机体坐标系
先说说坐标系。说白了，坐标系就是我们给无人机定位置的参考框架。
地球坐标系，也叫导航坐标系。我习惯用北-东-地（NED）系。原点通常选在起飞点，X轴指向北，Y轴指向东，Z轴指向地心。为什么用NED？因为无人机上的GPS和磁罗盘直接输出北向和东向信息，省去了一次坐标变换。
机体坐标系，是固定在无人机身上的。原点在质心，X轴指向机头，Y轴指向右翼，Z轴指向下。这个坐标系下，IMU（惯性测量单元）直接测量的是机体坐标系下的加速度和角速度。
嗯，这里要注意：两个坐标系之间的转换，是编队控制中最容易出错的地方。我曾经在一个项目中，因为把机体坐标系下的加速度当成了地球坐标系下的加速度，结果无人机编队飞出去就散架了……
2.2 欧拉角与四元数
从机体坐标系转到地球坐标系，需要知道无人机的姿态。描述姿态，最直观的就是欧拉角。
欧拉角有三个：横滚角（φ）、俯仰角（θ）、偏航角（ψ）。你可以想象成：先绕Z轴转ψ，再绕Y轴转θ，最后绕X轴转φ。这个顺序叫Z-Y-X，是无人机领域的惯例。
但欧拉角有个大问题——万向锁。当俯仰角接近±90°时，横滚和偏航会耦合在一起，丢失一个自由度。这在编队飞行中非常危险，尤其是做特技动作时。
所以，我建议在实际代码中，姿态表示一律用四元数。四元数没有奇点，计算效率也高。它用四个数（q0, q1, q2, q3）表示旋转，满足q0² + q1² + q2² + q3² = 1。
个人经验：我在PX4和ArduPilot的代码里，内部姿态估计全部用四元数。只在给用户显示或者做简单调试时，才转成欧拉角。你想想看，如果编队中有10架无人机，每架都要做姿态解算，用四元数能省下不少计算资源。
欧拉角转四元数的公式如下（Z-Y-X顺序）：
q0 = cos(ψ/2) * cos(θ/2) * cos(φ/2) + sin(ψ/2) * sin(θ/2) * sin(φ/2)
q1 = cos(ψ/2) * cos(θ/2) * sin(φ/2) - sin(ψ/2) * sin(θ/2) * cos(φ/2)
q2 = cos(ψ/2) * sin(θ/2) * cos(φ/2) + sin(ψ/2) * cos(θ/2) * sin(φ/2)
q3 = sin(ψ/2) * cos(θ/2) * cos(φ/2) - cos(ψ/2) * sin(θ/2) * sin(φ/2)2.3 无人机运动学模型
有了坐标系和姿态，接下来就是运动学模型。说白了，就是描述无人机怎么动。
对于四旋翼无人机，我通常用质点模型加姿态动力学来近似。为什么？因为编队控制更关注位置和速度，而不是每个电机的转速。
位置运动学方程（在地球坐标系下）：
ẋ = vx
ẏ = vy
ż = vz速度运动学方程（考虑推力、重力、阻力）：
m * v̇x = (cosφ * sinθ * cosψ + sinφ * sinψ) * T
m * v̇y = (cosφ * sinθ * sinψ - sinφ * cosψ) * T
m * v̇z = (cosφ * cosθ) * T - m * g这里T是总推力，g是重力加速度。你看，推力在三个轴上的分量，就是通过欧拉角（或四元数）从机体坐标系投影到地球坐标系的。
避坑指南：我曾经在仿真中直接用这个模型，结果发现实际飞行时跟踪误差很大。后来排查发现，我忽略了空气阻力。对于高速编队飞行（速度>10m/s），阻力项必须加进去。建议在模型中加入阻尼项 -kv * v，其中kv是阻尼系数。
2.4 编队相对运动学
编队飞行，核心是相对运动学。也就是一架无人机相对于另一架无人机怎么动。
假设有两架无人机：长机（Leader）和僚机（Follower）。僚机要保持在长机后方5米、右侧3米、下方2米的位置。这个相对位置，是在长机的机体坐标系下定义的。
相对位置向量 p_rel = [dx, dy, dz]^T，在长机机体坐标系下表示为：
p_rel_body = R(ψ_L) * (p_F - p_L)其中R(ψ_L)是长机偏航角的旋转矩阵，p_F和p_L分别是僚机和长机在地球坐标系下的位置。
相对速度呢？不仅要考虑位置差的变化，还要考虑长机自身的旋转。公式如下：
v_rel = v_F - v_L - ω_L × p_rel这里ω_L是长机的角速度，×表示叉乘。这个公式我建议你手推一遍，理解其中的物理含义。
为什么要有这一项？因为长机在转弯时，即使僚机保持相对位置不变，它在惯性系下的速度也会变化。如果不补偿这一项，编队就会在转弯时散开。
实战要点：在编队控制器中，我通常把相对运动学模型写成状态空间形式：
ẋ_rel = A * x_rel + B * u_rel其中x_rel = [dx, dy, dz, dvx, dvy, dvz]^T，u_rel是僚机的控制输入。这样设计控制器时，可以直接用线性控制理论，比如LQR或MPC。
2.5 本章知识体系
为了让你更直观地理解本章的知识结构，我画了一张图：
这张图清晰地展示了本章的脉络：从坐标系和姿态表示出发，建立单机运动学模型，最后扩展到编队相对运动学。每一步都环环相扣。
2.6 小结
这一章的内容确实有点多，但都是硬通货。总结一下：
- 地球坐标系用NED，机体坐标系用前-右-下。两者之间的转换靠旋转矩阵。
- 欧拉角直观但有万向锁，四元数无奇点且计算高效。代码里用四元数，调试时转欧拉角。
- 运动学模型是编队控制的基础。别忘了加阻力项，否则实际飞行会偏差很大。
- 编队相对运动学要考虑长机旋转带来的影响。那个叉乘项ω_L × p_rel，是编队转弯不散架的关键。
嗯，这些内容你消化一下。下一章我们会基于这些模型，开始设计编队控制器。到时候你会发现，今天打下的基础，全都能用上。