3. 坐标系与变换：地理坐标系（NED/ENU）、机体坐标系、坐标系旋转与变换矩阵、四元数与旋转矩阵互转
做姿态解算，说白了就是搞清楚「飞机/机器人现在是什么姿势」。但姿势这东西，得有个参考系才行。你总不能说「它朝东偏了30度」，你得先定义什么是「东」，什么是「北」。这就是坐标系存在的意义。
我刚开始做飞控的时候，被坐标系绕得晕头转向。NED和ENU傻傻分不清，四元数更是看得头皮发麻。后来踩的坑多了，慢慢就摸清了门道。今天咱们就把这块硬骨头啃下来。
3.1 地理坐标系：NED 与 ENU
地理坐标系，就是用来描述物体在地球上位置的参考系。常见的两种是NED和ENU。
核心区别：NED的Z轴指向地心（向下），ENU的Z轴指向天顶（向上）。
| 特性 | NED（北-东-地） | ENU（东-北-天） |
|---|---|---|
| X轴 | 指向地理北 | 指向地理东 |
| Y轴 | 指向地理东 | 指向地理北 |
| Z轴 | 指向地心（向下） | 指向天顶（向上） |
| 常见领域 | 航空、无人机 | 机器人、自动驾驶 |
我个人习惯在无人机项目里用NED。为什么？因为高度方向跟重力方向一致，加速度计测出来的重力加速度是正的，处理起来很直观。但做地面机器人时，ENU更常见，因为Z轴向上符合人的直觉。
小技巧：如果你在两个坐标系之间切换，记住一个简单的变换：绕X轴旋转180度，NED就变成了ENU。我在项目中经常用这个技巧做数据对齐。
3.2 机体坐标系
机体坐标系是固定在飞行器或机器人身上的坐标系。原点在质心，方向跟着机体走。
对于四旋翼无人机，通常这样定义：
- X轴（Roll轴）：指向机头方向
- Y轴（Pitch轴）：指向机身右侧
- Z轴（Yaw轴）：指向机身下方（NED约定）
你想想看，IMU传感器就装在机身上，它测出来的加速度和角速度，天然就是机体坐标系下的数据。但我们做控制的时候，需要知道机体相对于地面的姿态。这就涉及到坐标系之间的变换了。
注意：不同厂家的IMU，机体坐标系定义可能不一样。我遇到过一款IMU，它的Z轴是朝上的，跟我的飞控定义反了。结果一上电，飞机直接翻了个跟头。嗯，从那以后，我每次拿到新硬件，第一件事就是检查坐标系定义。
3.3 坐标系旋转与变换矩阵
坐标系旋转，说白了就是把一个向量从一个坐标系「搬」到另一个坐标系。数学上，我们用旋转矩阵来做这件事。
绕三个轴的基本旋转矩阵长这样：
// 绕X轴旋转角度φ
R_x(φ) = [1, 0, 0 ]
[0, cos(φ), -sin(φ)]
[0, sin(φ), cos(φ)]
// 绕Y轴旋转角度θ
R_y(θ) = [cos(θ), 0, sin(θ)]
[0, 1, 0 ]
[-sin(θ), 0, cos(θ)]
// 绕Z轴旋转角度ψ
R_z(ψ) = [cos(ψ), -sin(ψ), 0]
[sin(ψ), cos(ψ), 0]
[0, 0, 1]
完整的旋转矩阵就是这三个矩阵的乘积。但顺序很重要！不同的旋转顺序，结果完全不同。我习惯用Z-Y-X顺序（先偏航，再俯仰，最后横滚），这也是航空领域最常用的顺序。
关键点：旋转矩阵是正交矩阵，它的逆等于它的转置。这意味着从机体到地理坐标系的旋转矩阵，就是地理到机体的转置。这个性质在代码里非常有用，能省不少计算量。
3.4 四元数与旋转矩阵互转
四元数这东西，刚接触时觉得挺玄乎。但用久了你会发现，它比欧拉角好使多了——没有万向锁，插值平滑，计算效率高。
一个四元数q = [w, x, y, z]，其中w是实部，[x, y, z]是虚部。它表示绕单位向量[u_x, u_y, u_z]旋转角度θ：
q = [cos(θ/2), u_x*sin(θ/2), u_y*sin(θ/2), u_z*sin(θ/2)]
从四元数到旋转矩阵的转换公式：
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)
实战经验：我在嵌入式平台上实现四元数更新时，发现直接用浮点运算太慢了。后来改用定点数，把四元数归一化放在最后做，性能提升了30%。另外，记得定期做归一化，否则误差会累积，姿态会漂移。
3.5 知识体系总览
下面这张图，把本章的核心逻辑串起来了。你看一遍，应该能有个整体印象。
你看，整个知识体系其实就三块：地理坐标系、机体坐标系，以及连接它们的数学工具（旋转矩阵和四元数）。搞懂了这三者的关系，姿态解算的底层逻辑你就拿下了。
一句话总结：姿态解算，就是不断在「地理坐标系下的参考向量」和「机体坐标系下的测量值」之间做变换，找到那个最合适的旋转关系。
公众号:蓝海资料掘金营，微信deep3321