坐标系与旋转表示：地理坐标系、机体坐标系、欧拉角、旋转矩阵、四元数基础与转换

说实话，飞控算法里最容易让人头晕的，就是坐标系和旋转表示。我当年刚入行时，被欧拉角万向锁坑过好几次。后来才明白，这东西就像开车——你得先搞清楚自己在哪，面朝哪，才能谈怎么走。

今天咱们就把这块硬骨头啃下来。我会结合自己踩过的坑，把地理坐标系、机体坐标系、欧拉角、旋转矩阵、四元数这些概念串起来讲。

1. 两个核心坐标系：你站在哪，飞机朝哪

飞控里最常用的就两个坐标系。一个是地理坐标系，一个是机体坐标系。

地理坐标系（NED系）：说白了就是地面上的参考系。N指北，E指东，D指地。我习惯把原点放在飞机起飞点，这样计算位置偏移特别直观。

机体坐标系（Body系）：这个跟着飞机走。x轴朝机头，y轴朝右翼，z轴朝下。你想想看，飞机上的陀螺仪和加速度计，测出来的数据都是相对于机体坐标系的。

嗯，这里要注意：两个坐标系之间的转换，就是飞控算法的核心。你要把机体上测到的加速度，转换到地理坐标系下，才能算出位置。

2. 欧拉角：最直观，但最危险

欧拉角用三个角度描述旋转：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。我刚开始学飞控时，觉得这玩意儿太友好了，一看就懂。

但后来在项目里吃过亏。有一次做固定翼的自主飞行，飞机做大机动时突然姿态解算炸了。查了半天，发现是万向锁问题——当俯仰角接近±90°时，横滚和偏航就分不清了。

欧拉角的旋转顺序也很关键。飞控里常用的是ZYX顺序：先偏航，再俯仰，最后横滚。对应的旋转矩阵长这样：

// ZYX顺序的旋转矩阵
R = Rz(yaw) * Ry(pitch) * Rx(roll)

// 具体展开
R = [
  [cos(y)cos(p),  cos(y)sin(p)sin(r)-sin(y)cos(r),  cos(y)sin(p)cos(r)+sin(y)sin(r)],
  [sin(y)cos(p),  sin(y)sin(p)sin(r)+cos(y)cos(r),  sin(y)sin(p)cos(r)-cos(y)sin(r)],
  [-sin(p),        cos(p)sin(r),                     cos(p)cos(r)]
]

3. 旋转矩阵：数学上完美，计算上沉重

旋转矩阵是个3x3的正交矩阵。它的好处是：没有奇点，可以连续旋转。坏处是：9个参数，计算量大，而且容易因为数值误差不再是正交矩阵。

我一般在做坐标变换时用旋转矩阵。比如把机体加速度转到地理系：

// 机体加速度转到地理系
acc_ned = R * acc_body

// 其中R是当前姿态对应的旋转矩阵
// acc_body是IMU测到的三轴加速度

但如果你要做姿态更新，我建议别用旋转矩阵。每次更新都要重新正交化，太麻烦。

4. 四元数：飞控工程师的瑞士军刀

四元数是个超复数，用四个参数表示旋转：q = w + xi + yj + zk。其中w是实部，x、y、z是虚部。

我个人习惯用四元数做姿态更新和插值。原因有三：

• 无奇点：不会出现万向锁
• 计算快：只需要4个参数，乘法比矩阵快
• 易插值：球面线性插值（SLERP）非常平滑

四元数更新姿态的代码，我一般这么写：

// 四元数姿态更新
// gyro是陀螺仪角速度，dt是时间步长
void quaternionUpdate(float q[4], float gyro[3], float dt) {
    float wx = gyro[0] * dt / 2.0f;
    float wy = gyro[1] * dt / 2.0f;
    float wz = gyro[2] * dt / 2.0f;
    
    float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
    
    q[0] = q0 - q1*wx - q2*wy - q3*wz;
    q[1] = q1 + q0*wx + q2*wz - q3*wy;
    q[2] = q2 + q0*wy - q1*wz + q3*wx;
    q[3] = q3 + q0*wz + q1*wy - q2*wx;
    
    // 归一化，防止数值误差
    float norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
    q[0] /= norm; q[1] /= norm; q[2] /= norm; q[3] /= norm;
}

5. 三种表示的转换关系

面试时经常被问：给我写个四元数转欧拉角的函数。其实公式不难，但要注意象限判断。

转换类型	公式/方法	注意事项
四元数 → 欧拉角	roll = atan2(2(wx+yz), 1-2(x²+y²)) pitch = asin(2(wy-zx)) yaw = atan2(2(wz+xy), 1-2(y²+z²))	pitch接近±90°时，roll和yaw不稳定
欧拉角 → 四元数	q = qz(yaw) * qy(pitch) * qx(roll)	注意旋转顺序要与定义一致
旋转矩阵 → 四元数	从矩阵迹计算，或用Shoemake方法	矩阵必须是正交的，否则结果不准

代码实现时，我习惯这样写四元数转欧拉角：

// 四元数转欧拉角（ZYX顺序）
void quatToEuler(float q[4], float* roll, float* pitch, float* yaw) {
    float w = q[0], x = q[1], y = q[2], z = q[3];
    
    // 俯仰角
    float sinp = 2.0f * (w*y - z*x);
    if (fabs(sinp) >= 1.0f) {
        *pitch = copysign(M_PI/2.0f, sinp); // 处理万向锁
    } else {
        *pitch = asin(sinp);
    }
    
    // 横滚角和偏航角
    *roll = atan2(2.0f*(w*x + y*z), 1.0f - 2.0f*(x*x + y*y));
    *yaw = atan2(2.0f*(w*z + x*y), 1.0f - 2.0f*(y*y + z*z));
}

6. 知识体系总览

下面这张图，是我自己总结的坐标系与旋转表示的知识结构。面试前看一遍，心里就有底了。

7. 面试常见追问

面试官一般会顺着这个话题往下挖。我整理了几个高频问题：

1. 为什么飞控里不用欧拉角做姿态更新？

   因为欧拉角微分方程里有三角函数，计算复杂，而且万向锁会导致数值不稳定。四元数微分方程只有加减乘除，简单可靠。

2. 四元数怎么表示旋转？

   一个旋转可以用单位四元数表示：q = cos(θ/2) + u·sin(θ/2)，其中u是旋转轴单位向量，θ是旋转角度。旋转一个向量v：v' = q * v * q⁻¹。

3. 旋转矩阵的正交性怎么保证？

   我一般用Gram-Schmidt正交化，或者用四元数转矩阵，因为四元数天然是单位化的。

好了，坐标系和旋转表示这块，核心就是这些。记住：理解物理意义比死记公式重要得多。我在项目里见过太多人公式背得滚瓜烂熟，但一遇到实际问题就懵了。你想想看，面试官问你的目的，不就是看你有没有真正理解吗？

公众号:蓝海资料掘金营，微信deep3321