2. PX4概述与开发环境
各位同学，欢迎来到第二章。这一章我们聊聊坐标系。
说实话，我刚入行做飞控那会儿，觉得坐标系这东西太简单了——不就是三个轴嘛。结果第一次调试无人机偏航角的时候，飞机在天上转圈，我在地上抓狂。后来才发现，是坐标系定义搞反了。
嗯，坐标系搞错了，飞机就敢跟你玩漂移。所以这一章，咱们把坐标系这件事彻底讲清楚。
2.1 为什么需要多个坐标系？
你想想看，无人机在空中飞行，需要知道自己在哪里、往哪飞、姿态怎么样。但“位置”和“姿态”是相对的——你得先选一个参考系。
举个例子：
- 你要告诉飞控“往北飞10米”，这个“北”是相对于地球的，不是相对于机头的。
- 但你要控制电机转速让飞机抬头，这个“抬头”是相对于飞机本身的。
所以，飞控里至少需要两套坐标系：一套管位置（世界坐标系），一套管姿态（机体坐标系）。
核心概念：坐标系变换的本质，就是把同一个向量在不同参考系下“翻译”出来。飞控里90%的数学运算，都是在做这种翻译。
2.2 世界坐标系（World Frame）
世界坐标系，也叫地球固联坐标系。它的原点通常选在起飞点，或者某个已知的地理位置。
在PX4里，世界坐标系遵循 北-东-地（NED） 约定：
- X轴：指向正北
- Y轴：指向正东
- Z轴：指向地心（向下）
为什么Z轴要向下？我记得第一次看到NED约定时也很疑惑。后来做了一次高度融合实验才明白：因为重力加速度是向下的，Z轴向下可以让重力分量直接取正，省去一个负号。说白了，就是为了计算方便。
| 坐标系 | X轴 | Y轴 | Z轴 | 典型用途 |
|---|---|---|---|---|
| 世界坐标系（NED） | 北 | 东 | 地 | GPS位置、航点规划 |
| 世界坐标系（ENU） | 东 | 北 | 天 | ROS、仿真环境 |
注意：PX4使用NED，但ROS和很多仿真工具使用ENU（东-北-天）。做仿真时，一定要在数据接口层做转换。我曾经见过一个团队，仿真里飞得好好的，真机一飞就炸——就是因为坐标系没转。
2.3 机体坐标系（Body Frame）
机体坐标系固定在无人机上，跟着飞机一起动。原点在飞机的重心位置。
PX4的机体坐标系约定：
- X轴：指向机头方向（前进方向）
- Y轴：指向飞机右侧（右翼方向）
- Z轴：指向飞机下方（符合右手定则）
这个坐标系有什么用？IMU（惯性测量单元）测量的加速度和角速度，就是基于机体坐标系的。你想啊，加速度计装在飞机上，它测到的“向前加速”是相对于机头的，不是相对于北的。
2.4 导航坐标系（Navigation Frame）
导航坐标系，其实可以理解为世界坐标系的一个“本地版本”。
在PX4里，导航坐标系和世界坐标系基本是同一个东西——都是NED。但严格来说，导航坐标系的原点可以随任务移动，而世界坐标系的原点是固定的。
我个人习惯这样理解：
- 世界坐标系：起飞点就是原点，整个飞行过程中不变
- 导航坐标系：用于解算导航方程，原点可以重新对齐（比如做零偏修正时）
实际工程中，你不需要太纠结这个区别。PX4的代码里，local_position 用的就是导航坐标系，原点在起飞点。
2.5 姿态表示：欧拉角与四元数
好了，现在有了两个坐标系，怎么描述飞机相对于世界的姿态？
两种主流方法：欧拉角和四元数。
2.5.1 欧拉角
欧拉角用三个角度来描述姿态：
- 滚转角（Roll，φ）：绕X轴旋转
- 俯仰角（Pitch，θ）：绕Y轴旋转
- 偏航角（Yaw，ψ）：绕Z轴旋转
PX4里，欧拉角的旋转顺序是 Z-Y-X（先偏航，再俯仰，最后滚转）。这个顺序很重要，换一个顺序，结果就完全不一样。
避坑指南：我曾经在写姿态解算时，用了不同的旋转顺序，结果飞机悬停时一直往一边偏。查了两天才发现，是旋转顺序和PX4的约定不一致。所以，做坐标变换前，先确认旋转顺序。
欧拉角有个著名的缺陷——万向锁（Gimbal Lock）。当俯仰角达到±90°时，滚转和偏航会失去一个自由度。对于固定翼飞机，这可能是致命的。
2.5.2 四元数
四元数用四个数来表示旋转，没有万向锁问题。形式是：
q = w + x*i + y*j + z*k其中 w 是实部，x, y, z 是虚部。满足 w² + x² + y² + z² = 1。
PX4内部全部使用四元数。为什么？
- 没有万向锁
- 插值平滑（做姿态控制时很重要）
- 计算效率高（只有乘法和加法，没有三角函数）
但四元数对人类不友好。你看到 [0.707, 0, 0.707, 0]，能想象出飞机是什么姿态吗？反正我不能。所以调试时，我们会把四元数转成欧拉角来看。
2.6 坐标变换矩阵
坐标变换矩阵，就是把一个向量从一个坐标系“搬”到另一个坐标系。
从机体坐标系到世界坐标系的旋转矩阵（用欧拉角表示）：
R = Rz(ψ) * Ry(θ) * Rx(φ)
其中：
Rz(ψ) = [cosψ -sinψ 0]
[sinψ cosψ 0]
[0 0 1]
Ry(θ) = [cosθ 0 sinθ]
[0 1 0 ]
[-sinθ 0 cosθ]
Rx(φ) = [1 0 0 ]
[0 cosφ -sinφ]
[0 sinφ cosφ]用四元数表示旋转矩阵会更简洁。PX4里用的是这个公式：
R = [1-2(y²+z²) 2(xy-wz) 2(xz+wy) ]
[2(xy+wz) 1-2(x²+z²) 2(yz-wx) ]
[2(xz-wy) 2(yz+wx) 1-2(x²+y²) ]嗯，公式看着复杂，但代码里就是几行乘加运算。你不需要背，知道怎么用就行。
2.7 PX4中的坐标系约定
最后，我们把PX4里的坐标系约定总结一下。这部分是硬规则，写代码时必须遵守：
| 模块 | 使用的坐标系 | 姿态表示 | 备注 |
|---|---|---|---|
| 位置估计（EKF2） | NED（世界） | 四元数 | 内部全部用四元数 |
| 姿态估计 | 机体→世界 | 四元数 | 输出给控制模块 |
| 控制模块 | 机体坐标系 | 四元数/欧拉角 | 调试时转欧拉角 |
| GPS驱动 | NED | — | 经纬度转本地坐标 |
| MAVLink通信 | NED | 欧拉角 | 地面站显示用 |
关键点：PX4内部统一使用NED + 四元数。所有外部输入（如GPS、磁力计）都会被转换到这个统一框架下。你写自定义模块时，也要遵守这个约定。
2.8 知识体系总览
下面这张图，把本章的核心逻辑串起来了：
这张图你看懂了吗？从上往下看：世界坐标系和机体坐标系是基础，导航坐标系是世界坐标系的本地化版本，姿态表示描述两者的相对关系，坐标变换矩阵完成数学转换，最后全部统一到PX4的约定框架里。