坐标系与空间变换：从地面到天空的数学桥梁
做无人机定位仿真，说白了就是跟坐标系打交道。我刚开始接触这个领域时，觉得坐标系不就是几个箭头嘛，有什么好研究的？结果第一次联调就栽了跟头——飞机明明在仿真里飞得好好的，实际数据一进来，位置全乱套了。后来才明白，坐标系选不对、变换做错，后面所有算法都是白搭。
这一章，咱们就把坐标系和空间变换彻底讲透。你想想看，无人机在天上飞，它的位置、姿态、速度，都得用某个坐标系来描述。不同坐标系之间怎么转换？欧拉角和四元数到底怎么用？这些搞清楚了，后面的仿真联调才能走得顺。
核心要点：坐标系是定位的“语言”，空间变换是“翻译器”。两者缺一不可。
2.1 三大核心坐标系
无人机定位里，最常用的坐标系有三个。我习惯把它们分成“地球级”、“导航级”和“机载级”。
2.1.1 地心地固坐标系（ECEF）
ECEF 坐标系，原点在地球质心。Z 轴指向北极，X 轴指向本初子午线与赤道的交点，Y 轴按右手定则确定。说白了，它是一个跟地球一起转的坐标系。
为什么需要它？因为 GPS 卫星给出来的位置，默认就是 ECEF 坐标。我记得第一次做 GPS 数据解析时，看到那串几百万米量级的数字，差点以为自己算错了。后来才反应过来——ECEF 的坐标值确实很大，因为它是以地心为原点的。
| 坐标系 | 原点 | 特点 | 典型用途 |
|---|---|---|---|
| ECEF | 地球质心 | 随地球自转，坐标值大 | GPS 定位、卫星轨道 |
| 导航坐标系 | 载体所在位置 | 北东地或东北天 | 惯性导航、航向计算 |
| 机体坐标系 | 无人机质心 | 前右下或前左上 | 姿态控制、传感器安装 |
2.1.2 导航坐标系（NED / ENU）
导航坐标系，也叫当地水平坐标系。原点在无人机当前位置，X 轴指向北（或东），Y 轴指向东（或北），Z 轴指向地心（或天顶）。
我个人习惯用 NED（北东地）坐标系，因为跟大多数飞控的默认设置一致。但要注意，有些传感器用的是 ENU（东北天），转换时千万别搞反了。我曾经在项目里因为 NED 和 ENU 混用，导致飞机在仿真里一直往地下钻，排查了整整两天才找到问题。
小技巧：做仿真联调时，建议统一用 NED 坐标系。如果传感器输出的是 ENU，记得在数据入口处做一次转换，别等到算法层再处理。
2.1.3 机体坐标系（Body Frame）
机体坐标系固定在无人机上。原点在质心，X 轴指向机头，Y 轴指向右翼，Z 轴指向下方（右手定则）。
这个坐标系最直观——你坐在飞机里，前后左右上下，就是机体坐标系的方向。IMU（惯性测量单元）输出的加速度和角速度，默认就是机体坐标系下的数据。
2.2 欧拉角与四元数
描述无人机姿态，有两种主流方式：欧拉角和四元数。嗯，这里要注意，选哪个直接影响到你的算法稳定性和计算效率。
2.2.1 欧拉角
欧拉角用三个角度描述姿态：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。
- 横滚角（φ）：绕 X 轴旋转，范围 ±180°
- 俯仰角（θ）：绕 Y 轴旋转，范围 ±90°
- 偏航角（ψ）：绕 Z 轴旋转，范围 ±180°
欧拉角的好处是直观，一看就懂。但有个致命问题——万向锁。当俯仰角接近 ±90° 时，横滚和偏航会耦合，导致自由度丢失。我在做某款固定翼无人机仿真时，就遇到过俯仰角超过 85° 后姿态解算直接发散的情况。
警告：如果无人机可能做大机动飞行（比如特技、翻滚），千万别只用欧拉角做姿态控制。否则万向锁会让你吃大亏。
2.2.2 四元数
四元数用四个数描述姿态：q = [q0, q1, q2, q3]。它没有万向锁问题，计算效率高，适合做插值和滤波。
四元数看起来抽象，但用起来其实很简单。我一般把它当作“旋转的代数表示”。你不需要理解它的几何意义，只要会用公式就行。
// 欧拉角转四元数（ZYX 顺序）
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 坐标变换矩阵
坐标系之间的转换，靠的就是变换矩阵。说白了，就是把一个向量从一个坐标系“搬”到另一个坐标系。
2.3.1 从机体坐标系到导航坐标系
这个变换矩阵也叫方向余弦矩阵（DCM），由三个欧拉角构成。我习惯用 ZYX 顺序：先偏航，再俯仰，最后横滚。
C_b^n = R_z(ψ) * R_y(θ) * R_x(φ)
其中：
R_x(φ) = [1, 0, 0; 0, cosφ, -sinφ; 0, sinφ, cosφ]
R_y(θ) = [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]
R_z(ψ) = [cosψ, -sinψ, 0; sinψ, cosψ, 0; 0, 0, 1]这个矩阵有什么用？举个例子：IMU 测到的加速度是机体坐标系下的，但导航算法需要的是导航坐标系下的加速度。用 C_b^n 乘一下，就转换好了。
2.3.2 从导航坐标系到地心地固坐标系
这个转换需要经纬度信息。公式稍微复杂一点，但原理一样——把当地水平坐标系下的向量，旋转到地球固定坐标系下。
我记得有一次做多机协同仿真，需要把每架飞机的相对位置统一到 ECEF 下。当时偷懒没做这个转换，结果两架飞机在仿真里明明相距 10 米，实际算出来的距离却是 100 米。嗯，从那以后我再也不敢跳过坐标变换了。
2.4 知识体系总览
下面这张图，把本章的核心知识点串起来了。你可以把它当作坐标系变换的“导航图”。
2.5 实战建议
做仿真联调时，我建议你按这个流程来：
- 统一坐标系约定：项目一开始就定好用 NED 还是 ENU，用四元数还是欧拉角。别等到代码写了一半再改。
- 写一个坐标变换工具库：把 ECEF ↔ NED ↔ Body 的转换封装好，单元测试覆盖所有边界情况。
- 日志里同时记录原始数据和转换后数据：方便排查问题。我曾经靠这个习惯，半小时就定位到了一个 IMU 安装方向反了的 bug。
- 仿真环境里加一个“坐标系可视化”功能：把三个坐标系画出来，看着飞机转，比看数字直观多了。
再次提醒：四元数归一化！四元数归一化！四元数归一化！每次更新完四元数，记得做归一化。否则累积误差会让你的姿态越算越偏。