2. 坐标系与姿态表示：常用坐标系、欧拉角与旋转矩阵、四元数基础、姿态解算原理

各位同学，咱们今天聊点硬核的。做飞控，坐标系就是你的“世界观”。

我刚开始搞飞控那会儿，觉得坐标系这玩意儿太简单了，不就是XYZ嘛。结果第一次试飞，飞机在天上乱转，我盯着数据看了半天，才发现是坐标系搞反了。嗯，从那以后，我再也不敢小看这一章了。

2.1 常用坐标系：你得知道你在哪儿

飞控里常用的坐标系，说白了就三种：惯性系、机体系、导航系。我习惯把它们比作“上帝视角”、“飞机视角”和“地图视角”。

2.1.1 惯性坐标系（i系）

惯性系，就是那个“绝对静止”的参考系。在飞控里，我们通常用地球中心作为原点，或者用起飞点作为原点。它不跟着飞机转，也不跟着地球转（近似）。

• 定义：原点在地心或起飞点，X轴指向北极，Y轴指向东，Z轴指向地心（或天）。
• 作用：它是所有姿态解算的“基准”。你想想看，飞机说“我转了30度”，是相对于谁转的？就是相对于惯性系。
• 我的经验：在GPS信号好的时候，惯性系很好用。但一旦进入室内或隧道，惯性系就“失效”了，因为没法用GPS修正。这时候就得靠IMU自己积分，误差会慢慢累积。

2.1.2 机体坐标系（b系）

机体系，就是“绑在飞机上”的坐标系。原点在飞机重心，X轴指向机头，Y轴指向右翼，Z轴指向下（符合右手定则）。

• 定义：原点在重心，Xb向前，Yb向右，Zb向下。
• 作用：所有传感器（陀螺、加速度计）测量的数据，都是在这个坐标系下的。比如陀螺测到的角速度，就是绕Xb、Yb、Zb的旋转速度。
• 避坑指南：我曾经遇到过一个问题，加速度计的数据在机体系下是[0, 0, -g]，但有人把它当成了[0, 0, g]，结果姿态解算直接炸了。记住，重力方向在机体系下是负Z。

2.1.3 导航坐标系（n系）

导航系，也叫“东北天”或“北东地”坐标系。它跟惯性系很像，但更“接地气”。原点在起飞点，X轴指向北，Y轴指向东，Z轴指向天（或地）。

• 定义：原点在起飞点，Xn向北，Yn向东，Zn向天（或地）。
• 作用：我们最终要控制飞机在“地图”上飞，所以导航系是控制器的“语言”。比如你让飞机“向东飞10米”，这个“东”就是在导航系下说的。
• 我的习惯：我个人习惯用“北东地”（NED）坐标系，因为跟机体系的Z轴方向一致（都向下），这样转换起来少一个负号，不容易出错。

2.2 欧拉角与旋转矩阵：怎么描述“转”

飞机在天上转，怎么描述这个“转”？最直观的就是欧拉角。但欧拉角有个大坑，叫“万向锁”。

2.2.1 欧拉角

欧拉角就是三个角度：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。

• 定义：
  • 横滚角 φ：绕X轴旋转，范围 ±180°
  • 俯仰角 θ：绕Y轴旋转，范围 ±90°
  • 偏航角 ψ：绕Z轴旋转，范围 ±180°
• 旋转顺序：我建议用Z-Y-X顺序（先偏航，再俯仰，最后横滚）。这个顺序在飞控里最常用。
• 万向锁问题：当俯仰角接近±90°时，横滚和偏航会“锁死”，丢失一个自由度。你想想看，飞机垂直向上飞的时候，你分不清它是横滚还是偏航。这就是万向锁。

2.2.2 旋转矩阵

旋转矩阵，就是把一个向量从一个坐标系转到另一个坐标系。比如从机体系转到导航系。

从机体系到导航系的旋转矩阵 R_b^n 可以写成：

R_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]

举个例子，如果飞机机体系下的加速度是 a_b = [0, 0, -g]，那么导航系下的加速度 a_n = R_b^n * a_b。如果飞机水平（φ=0, θ=0, ψ=0），那么 a_n = [0, 0, -g]，跟预期一样。

2.3 四元数基础：为什么用它？

四元数，说白了就是“没有万向锁的欧拉角”。它用四个数表示旋转，避免了奇异性。

2.3.1 四元数的定义

一个四元数 q = [q0, q1, q2, q3]，其中 q0 是标量部分，[q1, q2, q3] 是矢量部分。它满足：

q0^2 + q1^2 + q2^2 + q3^2 = 1

这个叫“单位四元数”。只有单位四元数才能表示旋转。

2.3.2 四元数与旋转矩阵的转换

从四元数到旋转矩阵：

R = [1-2(q2^2+q3^2), 2(q1q2-q0q3), 2(q1q3+q0q2);
     2(q1q2+q0q3), 1-2(q1^2+q3^2), 2(q2q3-q0q1);
     2(q1q3-q0q2), 2(q2q3+q0q1), 1-2(q1^2+q2^2)]

从欧拉角到四元数：

q0 = cos(φ/2)cos(θ/2)cos(ψ/2) + sin(φ/2)sin(θ/2)sin(ψ/2)
q1 = sin(φ/2)cos(θ/2)cos(ψ/2) - cos(φ/2)sin(θ/2)sin(ψ/2)
q2 = cos(φ/2)sin(θ/2)cos(ψ/2) + sin(φ/2)cos(θ/2)sin(ψ/2)
q3 = cos(φ/2)cos(θ/2)sin(ψ/2) - sin(φ/2)sin(θ/2)cos(ψ/2)

2.4 姿态解算原理：从传感器到姿态

姿态解算，就是利用陀螺、加速度计、磁力计的数据，算出飞机的姿态（四元数或欧拉角）。

2.4.1 陀螺积分

陀螺测量的是角速度 ω = [ωx, ωy, ωz]。通过积分，可以得到姿态变化：

q_dot = 0.5 * q ⊗ [0, ωx, ωy, ωz]

其中 ⊗ 表示四元数乘法。然后：

q_new = q_old + q_dot * dt

但陀螺有漂移，积分久了会偏。所以需要其他传感器来修正。

2.4.2 加速度计修正

加速度计测量的是重力在机体系下的分量。如果飞机水平，加速度计应该测到 [0, 0, -g]。如果测到的不是这个值，说明飞机有倾斜。

具体做法：

1. 用当前四元数算出“预测的重力方向”在机体系下的值。
2. 跟加速度计实测值做差，得到误差。
3. 用这个误差去修正四元数（比如用互补滤波或卡尔曼滤波）。

2.4.3 磁力计修正

磁力计测量的是地磁场方向。它用来修正偏航角。但磁力计很容易受干扰（比如电机电流），所以一般只在低速或悬停时用。

2.5 知识体系图

下面这张图，是我自己画的，把这一章的核心逻辑串起来了。你仔细看看，应该能理解坐标系、欧拉角、四元数、姿态解算之间的关系。

2.6 本章小结

这一章的内容，说白了就是三件事：

1. 坐标系：知道传感器在哪儿（机体系），知道目标在哪儿（导航系）。
2. 姿态表示：用欧拉角直观理解，用四元数做计算，用旋转矩阵做转换。
3. 姿态解算：用陀螺积分做预测，用加速度计和磁力计做修正。

嗯，这里要注意，姿态解算不是一次性的，而是每个控制周期都在做。你想想看，飞机每秒钟要算几百次姿态，所以算法必须简单、高效、稳定。

我个人习惯，在代码里把姿态解算封装成一个函数，输入是陀螺、加速度计、磁力计的原始数据，输出是四元数。这样上层控制逻辑就不用关心底层细节了。