第四节：坐标系定义——机体坐标系、导航坐标系、地球坐标系及其转换

各位同学，今天我们来聊聊坐标系。说实话，坐标系这个东西，刚入行的时候我觉得特别简单——不就是三个轴嘛。但真正做项目之后才发现，坐标系搞不清楚，姿态解算就是空中楼阁。

我记得有一次调试四旋翼，飞机一解锁就往一边翻。查了半天，最后发现是机体坐标系和导航坐标系的转换矩阵写反了。嗯，从那以后，我每次写代码前都会先把坐标系定义画在纸上。

4.1 为什么要定义坐标系？

说白了，无人机在空中飞，我们需要知道它「朝哪看」、「往哪飞」。但这两个问题是在不同参考系下描述的。

  • 朝哪看——描述飞机自身的姿态，用机体坐标系
  • 往哪飞——描述飞机相对于地面的运动，用导航坐标系
  • 在哪飞——描述飞机在地球上的位置，用地球坐标系

你想想看，如果没有统一的坐标系定义，你说「飞机向右转」，到底是机头向右，还是地面观察者的右手边？这就是问题的根源。

4.2 机体坐标系（Body Frame）

机体坐标系是固定在无人机上的。我习惯用 Ob-XbYbZb 来表示。

  • 原点 Ob：通常取飞机的重心
  • Xb 轴：指向机头方向（前进方向）
  • Yb 轴：指向飞机右侧（右翼方向）
  • Zb 轴：指向飞机下方（符合右手定则）

注意：这里 Zb 轴指向下方，是航空领域的惯例。有些教材用 Z 轴向上，但我在实际项目中统一用「东北地」的右手系，这样和 IMU 传感器的输出方向一致。

机体坐标系有什么用？IMU 传感器（加速度计、陀螺仪）测量的数据，默认就是在机体坐标系下的。比如陀螺仪输出的角速度 ωx、ωy、ωz，就是绕机体三个轴的旋转速度。

4.3 导航坐标系（Navigation Frame）

导航坐标系，也叫地理坐标系。我常用 On-XnYnZn 表示。

  • 原点 On：通常取无人机起飞点
  • Xn 轴：指向正北（N）
  • Yn 轴：指向正东（E）
  • Zn 轴：指向地心（D，向下）

这就是经典的 NED（北-东-地）坐标系。我个人习惯用 NED，因为和 GPS 输出的方向一致，省去很多转换麻烦。

小技巧：如果你用 ENU（东-北-天）坐标系，注意 Z 轴向上。两种都可以，但一定要和你的传感器、控制算法统一。我曾经见过团队里两个人用不同约定，结果融合出来的姿态差了 90 度。

4.4 地球坐标系（Earth Frame）

地球坐标系是描述无人机在大地中的绝对位置。常用的是 ECEF（地心地固坐标系）。

  • 原点 Oe：地球质心
  • Xe 轴：指向本初子午线与赤道的交点
  • Ye 轴：指向东经 90 度方向
  • Ze 轴：指向北极

说实话，做多旋翼姿态解算时，我们很少直接用 ECEF 坐标。更多是用 GPS 输出的经纬高，然后转换到导航坐标系下的局部位置。但理解 ECEF 有助于你理解 GPS 定位原理。

4.5 坐标系之间的转换

这是重点中的重点。姿态解算的核心，就是把机体坐标系下的传感器数据，转换到导航坐标系下使用。

4.5.1 旋转矩阵

从导航坐标系到机体坐标系的旋转矩阵，记作 Cnb。反过来，从机体到导航的矩阵是 Cbn，两者互为转置。

旋转矩阵由三个欧拉角构成：

  • 偏航角 ψ（Yaw）：绕 Zn 轴旋转
  • 俯仰角 θ（Pitch）：绕 Yn 轴旋转
  • 横滚角 φ（Roll）：绕 Xn 轴旋转

旋转顺序我习惯用 Z-Y-X（偏航→俯仰→横滚），这是航空标准。

// 旋转矩阵 C_n^b (从导航到机体)
// 假设 psi = yaw, theta = pitch, phi = roll
// 使用 Z-Y-X 顺序

float C[3][3];

C[0][0] = cos(theta) * cos(psi);
C[0][1] = cos(theta) * sin(psi);
C[0][2] = -sin(theta);

C[1][0] = sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi);
C[1][1] = sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi);
C[1][2] = sin(phi) * cos(theta);

C[2][0] = cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi);
C[2][1] = cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi);
C[2][2] = cos(phi) * cos(theta);

避坑指南：我曾经在代码里把 sin 和 cos 写反了，结果飞机悬停时一直往一个方向漂。排查了整整两天才发现是旋转矩阵的符号问题。建议你写完矩阵后，用单位向量测试一下：把 [1,0,0] 转过去，看看结果对不对。

4.5.2 四元数表示

欧拉角有万向锁问题，所以实际工程中我更推荐用四元数。四元数 q = [q0, q1, q2, q3] 可以避免奇异性。

// 四元数旋转向量
// 将机体坐标系下的向量 v_b 转换到导航坐标系 v_n

void quat_rotate(float q[4], float v_b[3], float v_n[3]) {
    float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
    
    v_n[0] = (q0*q0 + q1*q1 - q2*q2 - q3*q3) * v_b[0] 
           + 2*(q1*q2 - q0*q3) * v_b[1] 
           + 2*(q1*q3 + q0*q2) * v_b[2];
           
    v_n[1] = 2*(q1*q2 + q0*q3) * v_b[0] 
           + (q0*q0 - q1*q1 + q2*q2 - q3*q3) * v_b[1] 
           + 2*(q2*q3 - q0*q1) * v_b[2];
           
    v_n[2] = 2*(q1*q3 - q0*q2) * v_b[0] 
           + 2*(q2*q3 + q0*q1) * v_b[1] 
           + (q0*q0 - q1*q1 - q2*q2 + q3*q3) * v_b[2];
}

4.6 实际应用中的坐标系转换流程

在飞控代码里，坐标系转换的典型流程是这样的：

  1. 读取 IMU 数据：加速度计和陀螺仪输出的是机体坐标系下的值
  2. 姿态解算：用陀螺仪积分 + 加速度计/磁力计修正，得到当前姿态（四元数或欧拉角）
  3. 加速度转换：把机体坐标系下的加速度，通过旋转矩阵转到导航坐标系
  4. 去除重力：在导航坐标系下减去重力分量 [0, 0, g]
  5. 位置积分：对去重力后的加速度进行二次积分，得到位置

核心要点：控制算法（PID）通常在导航坐标系下计算期望的力或加速度，然后通过旋转矩阵逆变换到机体坐标系，再分配给各个电机。这个来回转换，就是姿态解算的桥梁。

4.7 常见坐标系转换错误

错误类型 现象 原因
旋转方向反了 飞机朝反方向运动 旋转矩阵的符号取反
坐标轴顺序错 横滚和俯仰混在一起 Z-Y-X 顺序写成了 X-Y-Z
重力方向搞错 高度积分发散 Z 轴向上/向下约定不一致
四元数归一化遗漏 姿态慢慢漂移 四元数累积误差未归一化

我的调试习惯：每次写完坐标系转换代码，先用 MATLAB 或 Python 做离线仿真。把 IMU 的静态数据录下来，在电脑上跑一遍算法，看看转换后的加速度是不是 [0, 0, g]（静止时）。如果是，说明转换对了。

4.8 小结

坐标系定义是姿态解算的地基。地基不稳，上面盖的楼再漂亮也会塌。我个人建议你：

  • 画图！把三个坐标系画在纸上，标清楚轴的方向
  • 统一！整个项目组用同一套约定（NED + Z-Y-X）
  • 测试！写一个简单的向量旋转测试函数，验证每个转换

下一节我们会讲欧拉角和四元数的具体计算，到时候你会看到坐标系转换是如何在代码里落地的。嗯，今天就到这里，有问题随时交流。

← 上一章 📖 返回目录 下一章 →