2、坐标系与坐标变换：地球坐标系、机体坐标系、欧拉角与四元数、坐标变换矩阵

说实话，坐标系这个东西，刚入门的时候很容易被绕晕。

我记得我第一次做无人机仿真，明明代码逻辑都对，飞机就是原地打转。查了两天，最后发现是坐标变换矩阵写反了。嗯，从那以后，我对坐标系就格外上心。

这一节，咱们就把坐标系和坐标变换彻底讲透。你想想看，无人机在天上飞，它得知道自己在哪里，机头朝哪，姿态怎么样。这些信息，全靠坐标系来描述。

2.1 地球坐标系：飞机到底在哪？

地球坐标系，说白了就是给地球贴个标签，让无人机知道自己的绝对位置。

最常用的有两种：

• 地理坐标系（LLA）：经度、纬度、高度。GPS直接输出的就是这个。
• 地心地固坐标系（ECEF）：以地心为原点，X轴指向本初子午线，Y轴指向东经90度，Z轴指向北极。做远距离导航时用得多。

我个人习惯，在局部路径规划时，会把LLA转成平面坐标。为什么呢？因为经纬度算距离太麻烦了，还得考虑地球曲率。

2.2 机体坐标系：飞机头朝哪？

地球坐标系是“世界”的视角。那飞机自己怎么看自己？这就用到机体坐标系了。

机体坐标系的原点在飞机的重心。通常这样定义：

• X轴：指向机头方向（前进方向）
• Y轴：指向飞机右侧（右翼方向）
• Z轴：指向飞机下方（符合右手定则）

我在项目中遇到过一个问题：IMU（惯性测量单元）安装的时候，如果没对准机头，那输出的加速度和角速度就是歪的。这时候必须做标定，把IMU的坐标系旋转到机体坐标系。

2.3 欧拉角：最直观的姿态描述

欧拉角，就是用来描述“飞机相对于地球坐标系转了多少”。

三个角度：

欧拉角的好处是直观。你一看“俯仰30度”，就知道飞机抬头了。

但它有个致命问题——万向锁。当俯仰角接近±90度时，滚转和偏航会耦合，丢失一个自由度。我曾经在调试一个特技飞行的无人机时，就遇到了万向锁，姿态解算直接崩了。

2.4 四元数：优雅的数学工具

四元数，听起来高大上，其实就是为了解决万向锁而生的。

它用四个数来表示旋转：

q = w + xi + yj + zk

其中 w 是实部，x、y、z 是虚部。约束条件是 w² + x² + y² + z² = 1。

为什么用四个数？因为它在数学上避免了奇异性，而且插值平滑，计算效率高。

我记得第一次手写四元数更新代码时，公式推导了半天。其实你不需要完全理解它的几何意义，会用就行。核心操作就两个：

1. 四元数乘法：用来组合旋转
2. 四元数归一化：防止累积误差导致模长偏离1

// 四元数更新（简化版）
void quaternionUpdate(float gx, float gy, float gz, float dt) {
    float q0, q1, q2, q3; // 当前四元数
    float dq0, dq1, dq2, dq3;
    
    dq0 = 0.5f * (-q1*gx - q2*gy - q3*gz) * dt;
    dq1 = 0.5f * ( q0*gx + q2*gz - q3*gy) * dt;
    dq2 = 0.5f * ( q0*gy - q1*gz + q3*gx) * dt;
    dq3 = 0.5f * ( q0*gz + q1*gy - q2*gx) * dt;
    
    q0 += dq0; q1 += dq1; q2 += dq2; q3 += dq3;
    
    // 归一化
    float norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;
}

2.5 坐标变换矩阵：连接不同坐标系

有了地球坐标系和机体坐标系，怎么把机体的加速度转换到地球坐标系？

答案就是坐标变换矩阵（也叫旋转矩阵）。

从机体坐标系到地球坐标系的旋转矩阵，可以用欧拉角或四元数构造。

用欧拉角构造（Z-Y-X顺序）：

R = Rz(ψ) * Ry(θ) * Rx(φ)

展开后是一个3x3的矩阵。我一般不会手算，直接调库或者用现成公式。

用四元数构造更简洁：

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

有了这个矩阵，你就可以把机体坐标系下的加速度、磁场等数据，转换到地球坐标系下使用。

2.6 总结与实用建议

好了，这一节的内容不少。我帮你理一下思路：

• 地球坐标系：告诉无人机“你在哪”
• 机体坐标系：告诉无人机“你朝哪”
• 欧拉角：直观，但有万向锁，适合水平飞行
• 四元数：无奇异性，适合全姿态，但不够直观
• 坐标变换矩阵：连接两个世界的桥梁

我个人建议，新手先从欧拉角入手，理解姿态的概念。等遇到万向锁了，再切换到四元数。这样印象更深。

下一节，咱们聊聊传感器融合，怎么把IMU、GPS、磁力计的数据揉在一起，得到准确的姿态和位置。到时候，这些坐标系知识全都会用上。