3. 坐标系与变换：地理坐标系、机体坐标系、坐标系旋转与转换
做姿态估计，说白了就是搞清楚「物体朝哪边看」。但这个问题有个前提——你得先定义好「朝哪边」是相对于什么说的。坐标系，就是我们给空间定下的那把尺子。
我刚开始做惯导时，被坐标系绕晕过好几次。明明算出来的角度是对的，但飞机就是往反方向飞。后来才明白，不是算法错了，是坐标系定义搞反了。嗯，这节课咱们就把这事彻底理清楚。
3.1 地理坐标系：我们生活的参考系
地理坐标系，也叫导航坐标系，通常用 n系 表示。它固定在地球上，原点就在载体所在位置。
常见的定义方式是这样的：
- X轴：指向东（E）
- Y轴：指向北（N）
- Z轴：指向天（U），也就是垂直向上
这个叫「东北天」坐标系，英文缩写 ENU。当然也有「北东地」NED，但做无人机和机器人，ENU 更常见。
关键点：地理坐标系是惯性导航的「锚」。你所有的姿态、速度、位置，最终都要换算到这个系里才有意义。
我个人习惯把地理坐标系想象成「大地给你的绝对参考」。不管飞机怎么翻，地理系永远东是东、北是北。这就像你站在操场上，不管怎么转圈，教学楼的方向是不变的。
3.2 机体坐标系：跟着载体一起动
机体坐标系，也叫 b系，它牢牢固定在载体上。原点在载体的重心，三个轴跟着载体一起转。
对于常见的飞行器：
- X轴：指向机头方向（前进方向）
- Y轴：指向右侧机翼
- Z轴：指向机体下方（满足右手定则）
你想想看，IMU（惯性测量单元）就装在机体上。它测到的加速度和角速度，天然就是机体坐标系下的数据。所以我们要做的，就是把机体系的数据「翻译」到地理系去。
我的经验：有一次我在调试四旋翼，发现水平飞行时加速度计读出的Z轴一直是 -1g。当时我以为是传感器坏了，后来才意识到——机体坐标系下，静止时Z轴感受到的就是重力，方向向下所以是负的。这个坑，我踩过。
3.3 坐标系旋转：从机体到地理
现在问题来了：机体系测到了角速度和加速度，但我们要的是地理系下的姿态。怎么转？
坐标系旋转，本质上就是用一个 旋转矩阵 把向量从一个系映射到另一个系。这个矩阵通常记作 Cbn，意思是从b系到n系的旋转。
旋转矩阵有三个基本形式，分别绕三个轴转：
| 旋转轴 | 旋转矩阵（绕θ角） |
|---|---|
| 绕X轴 |
|
| 绕Y轴 |
|
| 绕Z轴 |
|
实际应用中，我们通常用 欧拉角 来描述旋转顺序：先绕Z轴转（偏航角 ψ），再绕Y轴转（俯仰角 θ），最后绕X轴转（横滚角 φ）。这个顺序叫 Z-Y-X，也是航空领域最常用的。
完整的旋转矩阵长这样：
C_b^n =
[cosθ·cosψ, sinφ·sinθ·cosψ - cosφ·sinψ, cosφ·sinθ·cosψ + sinφ·sinψ]
[cosθ·sinψ, sinφ·sinθ·sinψ + cosφ·cosψ, cosφ·sinθ·sinψ - sinφ·cosψ]
[ -sinθ, sinφ·cosθ, cosφ·cosθ ]注意：欧拉角有个著名的「万向锁」问题。当俯仰角接近 ±90° 时，偏航和横滚会耦合在一起，导致自由度丢失。我曾经在某个项目中因为这个原因，把欧拉角换成了四元数。后面章节会详细讲四元数，这里先留个印象。
3.4 坐标系转换的实战应用
理论说完了，咱们看看实际怎么用。假设你从IMU读到了加速度数据 ab = [ax, ay, az]T，这是机体系下的。要得到地理系下的加速度 an，只需要做一次矩阵乘法：
a_n = C_b^n · a_b就这么简单。但要注意，旋转矩阵是正交矩阵，它的逆等于它的转置。所以从n系到b系的转换就是：
C_n^b = (C_b^n)^T这个性质在代码里非常有用，不用每次都去算逆矩阵。
避坑指南：我曾经在写代码时，直接把旋转矩阵的转置当逆用，结果发现算出来的角度全反了。后来检查才发现——旋转矩阵确实是正交的，但前提是你用的必须是标准正交基。如果你的坐标系定义里三个轴不是严格正交，那转置就不等于逆。所以，定义坐标系时一定要保证右手定则，三个轴互相垂直。
3.5 知识体系总览
为了让你更直观地理解这一章的知识结构，我画了一张图：
这张图把本章的核心逻辑串起来了。左边是地理系，右边是机体系，中间靠旋转矩阵连接。下面三个框分别是欧拉角、基本旋转矩阵和坐标转换公式。最底下是那个绕不开的万向锁警告。
3.6 小结
这一章我们聊了三件事：
- 地理坐标系：固定在地球上的绝对参考，ENU 或 NED 两种定义
- 机体坐标系：跟着载体转，IMU 数据天然在这个系里
- 坐标系旋转：用旋转矩阵把机体系数据转到地理系，核心是欧拉角和三个基本旋转矩阵
说实话，坐标系这部分内容看起来枯燥，但它是整个姿态估计的基石。我见过太多人算法公式背得滚瓜烂熟，结果坐标系搞反了，整个系统全崩。所以，花点时间把坐标系定义清楚，比什么都重要。
一个小建议：写代码时，在注释里明确标注每个变量所在的坐标系。比如 acc_b 表示机体系加速度，acc_n 表示地理系加速度。这个习惯帮我省了无数调试时间。
公众号:蓝海资料掘金营，微信deep3321