2. 多旋翼动力学基础：坐标系定义与姿态表示
各位同学，欢迎来到第二章。这一章我们要啃的，是飞控算法里最基础、也最容易出错的部分——坐标系和姿态表示。
说实话，我当年刚入行时，在这上面栽过不少跟头。有一次调试避障算法，飞机明明应该向左躲，结果它向右冲过去了。查了两天，最后发现是坐标系搞反了。嗯，从那以后，我对坐标系定义就格外小心。
2.1 为什么要定义坐标系？
你想想看，多旋翼在天上飞，它要感知自己的位置、速度、姿态。这些量怎么描述？总得有个参考吧。
说白了，坐标系就是一把尺子。我们用不同的尺子量不同的东西：
- 世界坐标系：用来描述飞机在空间中的绝对位置
- 机体坐标系：用来描述飞机自身的朝向和受力
这两把尺子之间怎么换算？这就是姿态表示要解决的问题。
核心思想：所有传感器数据最终都要统一到同一个坐标系下处理。这是飞控算法设计的第一原则。
2.2 世界坐标系（World Frame）
世界坐标系，也叫惯性坐标系或导航坐标系。我习惯用 W系 或 NED系 来称呼它。
在无人机领域，最常用的世界坐标系是 北-东-地（NED）：
- X轴：指向正北
- Y轴：指向正东
- Z轴：指向地心（向下）
为什么Z轴向下？这跟飞机的高度测量有关。气压计测的是海拔，GPS给的是海拔高度，Z轴向下正好对应「高度越低，数值越小」的直觉。
我的习惯：在代码里，我通常用 world_frame 或 ned_frame 来命名世界坐标系相关的变量。这样一看就知道是哪个坐标系。
2.3 机体坐标系（Body Frame）
机体坐标系是固定在飞机上的。我一般叫它 B系。
定义方式遵循右手定则：
- X轴：指向机头方向
- Y轴：指向飞机右侧
- Z轴：指向飞机下方
为什么要这样定义？因为IMU（惯性测量单元）的安装方向通常就是这样的。加速度计和陀螺仪输出的数据，默认就是机体坐标系下的。
注意：不同厂家对机体坐标系的定义可能不同。比如PX4和ArduPilot的坐标系定义就有细微差别。我在项目中吃过这个亏——移植代码时忘了检查坐标系定义，结果姿态解算全乱了。
2.4 欧拉角与旋转矩阵
好了，现在有了两个坐标系。怎么把一个坐标系下的向量转换到另一个坐标系下？
最直观的方法就是 欧拉角。
欧拉角用三个角度来描述旋转：
- 偏航角（Yaw, ψ）：绕Z轴旋转
- 俯仰角（Pitch, θ）：绕Y轴旋转
- 横滚角（Roll, φ）：绕X轴旋转
旋转顺序我习惯用 Z-Y-X（偏航→俯仰→横滚）。这个顺序在航空领域是标准做法。
旋转矩阵长这样：
// 绕Z轴旋转（偏航）
R_yaw = [cos(ψ) -sin(ψ) 0
sin(ψ) cos(ψ) 0
0 0 1]
// 绕Y轴旋转（俯仰）
R_pitch = [cos(θ) 0 sin(θ)
0 1 0
-sin(θ) 0 cos(θ)]
// 绕X轴旋转（横滚）
R_roll = [1 0 0
0 cos(φ) -sin(φ)
0 sin(φ) cos(φ)]
// 完整旋转矩阵：R = R_yaw * R_pitch * R_roll有了这个矩阵，我们就可以把机体坐标系下的向量转到世界坐标系：
v_world = R * v_body避坑指南：我曾经在MPC控制器里直接用欧拉角做状态量，结果在俯仰角接近90度时出现了万向锁。飞机直接失控了。从那以后，我只要做全姿态控制，就一定用四元数。
2.5 四元数基础
四元数是什么？说白了，它是一个四维复数，用来表示三维空间中的旋转。
形式很简单：
q = w + xi + yj + zk其中 w 是实部，x, y, z 是虚部。约束条件是 w² + x² + y² + z² = 1。
为什么用四元数？三个理由：
- 无万向锁：这是最大的优势。欧拉角在俯仰±90度时会丢失一个自由度，四元数不会。
- 计算效率高：四元数乘法比矩阵乘法快。在嵌入式平台上，这点很重要。
- 插值平滑：做轨迹规划时，四元数的球面线性插值（SLERP）非常平滑。
四元数跟旋转矩阵的转换关系：
// 四元数 → 旋转矩阵
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²)]
// 旋转矩阵 → 四元数
w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4*w)
y = (R[0][2] - R[2][0]) / (4*w)
z = (R[1][0] - R[0][1]) / (4*w)我的建议：在代码里，我通常用 Eigen::Quaterniond 或自己封装一个四元数类。不要手写四元数运算，容易出错。我见过有人把乘法顺序搞反，结果姿态解算出来的角度全是反的。
2.6 三种姿态表示方法的对比
| 方法 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 欧拉角 | 直观、易理解 | 万向锁、计算复杂 | 地面站显示、简单控制 |
| 旋转矩阵 | 数学性质好 | 9个参数、冗余 | 坐标变换、视觉SLAM |
| 四元数 | 无奇点、计算快 | 不够直观 | 飞控核心、MPC控制器 |
2.7 知识体系总览
下面这张图是我自己画的，把这一章的核心逻辑串起来了：
这张图把整个知识体系串起来了。你从世界坐标系和机体坐标系出发，通过姿态表示（欧拉角、旋转矩阵、四元数）做转换，最终应用到MPC控制器中。
总结一下：坐标系定义是飞控算法的地基。地基不稳，上面盖的房子再漂亮也得塌。我个人建议，在写任何飞控代码之前，先把坐标系定义写清楚，注释写明白。这样后面调试时能省很多时间。