一、姿态测量基础：坐标系定义与旋转表达
做飞控这么多年，我最大的体会就是——坐标系搞不清楚，后面全白搭。你想想看，无人机在天上飞，你说它“朝北”，这个“北”是相对于谁的？是地面站的北，还是它自己机头的北？所以，咱们得先把坐标系这个“度量衡”统一了。
1.1 三大坐标系：惯性系、机体系、导航系
我个人习惯把坐标系分成三类，每一类都有它特定的用途。说白了，就是给不同的“参考对象”找个固定的尺子。
- 惯性坐标系（i系）：这是最“绝对”的坐标系。它固定在天上，不随地球自转或飞机运动而改变。原点在地心或地面某点，Z轴指向北极，X、Y轴在赤道平面内。嗯，这里要注意，实际工程中我们很少用严格的地心惯性系，而是用“当地水平惯性系”近似，因为地球自转的影响在短时间飞行中可以忽略。
- 机体坐标系（b系）：这个坐标系“长”在飞机上。原点在飞机重心，X轴指向机头，Y轴指向右翼，Z轴指向机腹（右手定则）。我在项目中遇到过一个问题：有个同事把Z轴搞反了，结果姿态解算出来的俯仰角符号全反了，飞机一推杆就抬头……嗯，从那以后我每次建系都要画个图确认。
- 导航坐标系（n系）：这是飞控最常用的“工作坐标系”。通常取“北-东-地”（NED）或“东-北-天”（ENU）。我建议初学者用NED，因为和惯性导航的惯用约定一致。导航系的作用，就是把机体系下的测量值转换到地面参考系中。
核心要点：三个坐标系之间的转换，就是姿态测量的全部秘密。机体系→导航系的旋转，就是我们常说的“姿态”。
1.2 欧拉角：直观但小心“万向锁”
欧拉角是最直观的姿态表达方式。你想想看，飞机怎么转？先绕Z轴转个偏航角（ψ），再绕Y轴转个俯仰角（θ），最后绕X轴转个滚转角（φ）。这就是经典的“Z-Y-X”旋转顺序。
但是，欧拉角有个大坑——万向锁。当俯仰角接近±90°时，偏航和滚转的旋转轴会重合，丢失一个自由度。我曾经在调试一个固定翼无人机时，遇到俯仰角超过85°的情况，结果姿态解算直接炸了，航向角乱跳。后来我改用四元数，问题就解决了。
避坑指南：如果你在做全姿态飞行（比如特技无人机、火箭回收），千万别只用欧拉角。四元数才是王道。
1.3 四元数：无奇异的优雅表达
四元数，说白了就是一个“超复数”。它由四个数组成：q = [q0, q1, q2, q3]，其中q0是标量部分，[q1, q2, q3]是矢量部分。它表示绕某个轴旋转某个角度，而且没有万向锁问题。
我建议你记住这个公式：
// 四元数表示绕单位向量 [x, y, z] 旋转角度 θ
q0 = cos(θ/2)
q1 = x * sin(θ/2)
q2 = y * sin(θ/2)
q3 = z * sin(θ/2)四元数的好处是：连续、无奇异、计算效率高。坏处是：不够直观。你没法一眼看出飞机是抬头还是低头。所以实际工程中，我通常用四元数做解算，用欧拉角做显示。
个人经验：四元数归一化很重要！每次更新后都要做一次归一化，否则累积误差会让姿态越偏越远。我习惯在每次姿态更新后加一行：q = q / norm(q)。
1.4 旋转矩阵推导：从欧拉角到矩阵
旋转矩阵，就是把一个向量从一个坐标系转到另一个坐标系的“转换器”。以Z-Y-X顺序为例，从机体系到导航系的旋转矩阵可以这样推导：
- 先绕Z轴转偏航角ψ：
R_z(ψ) = [[cosψ, -sinψ, 0], [sinψ, cosψ, 0], [0, 0, 1]] - 再绕Y轴转俯仰角θ：
R_y(θ) = [[cosθ, 0, sinθ], [0, 1, 0], [-sinθ, 0, cosθ]] - 最后绕X轴转滚转角φ：
R_x(φ) = [[1, 0, 0], [0, cosφ, -sinφ], [0, sinφ, cosφ]]
最终的旋转矩阵是：C_b^n = R_x(φ) * R_y(θ) * R_z(ψ)。注意顺序！是右乘，先转的在后。
我在项目中遇到过一个问题：有个同事把旋转顺序搞反了，结果姿态解算出来的角度全错。嗯，这里要记住：旋转矩阵的乘法不满足交换律，顺序错了，结果就全错了。
1.5 知识体系总览
下面这张图，是我自己总结的姿态测量基础的知识结构。你把它看懂了，后面学角速率控制就轻松多了。
1.6 实用技巧：欧拉角与四元数互转
实际工程中，我们经常需要在欧拉角和四元数之间来回转换。下面是我常用的转换公式：
| 转换方向 | 公式 |
|---|---|
| 欧拉角→四元数 |
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) |
| 四元数→欧拉角 |
φ = atan2(2(q0q1 + q2q3), 1 - 2(q1² + q2²)) θ = asin(2(q0q2 - q3q1)) ψ = atan2(2(q0q3 + q1q2), 1 - 2(q2² + q3²)) |
避坑指南：四元数转欧拉角时，注意asin函数的输入范围是[-1, 1]。如果计算出的值超出这个范围，说明四元数没有归一化。我曾经因为这个bug排查了整整两天……嗯，从那以后我每次转换前都会检查归一化。
好了，这一章的内容就到这里。坐标系定义、欧拉角、四元数、旋转矩阵，这四个概念是姿态测量的基石。你把这些搞清楚了，后面学角速率控制、卡尔曼滤波就会轻松很多。