第1章：三维空间基础——坐标系定义与旋转矩阵

各位同学，欢迎来到《姿态解算算法：从零到精通》的第一章。

说实话，姿态解算这个领域，说难也难，说简单也简单。但不管你是做四轴飞行器、机器人还是VR设备，坐标系和旋转这块基础必须打牢。我见过太多工程师，算法跑飞了都不知道是坐标系搞反了。

今天我们就从最基础的三维空间说起。

1.1 世界坐标系——我们站在哪里看？

先问个问题：你如何描述一架飞机的位置？

「它在我的东北方向，高度500米。」

这句话里其实就隐含了一个坐标系——世界坐标系。它是个固定不动的参考系，通常用 W系 或 n系 表示。

我个人习惯用 东北天（ENU） 坐标系：

• X轴：指向东（East）
• Y轴：指向北（North）
• Z轴：指向天（Up）

当然，也有用 北东地（NED） 的，比如很多飞控默认就是NED。这个没有绝对的对错，但一定要统一。我在项目中遇到过，两个模块一个用ENU一个用NED，结果融合出来的姿态直接反了180度……嗯，排查了整整一下午。

1.2 机体坐标系——飞机自己怎么看？

现在换个视角。你坐在飞机驾驶舱里，你的「前」「右」「下」分别对应什么？

这就是机体坐标系，也叫 b系。它固连在飞行器上，随飞行器一起运动。

标准定义是这样的：

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

你想想看，为什么Z轴要指向下？因为这样符合右手定则——X叉乘Y得到Z。很多初学者在这里搞混，我建议你拿个手机比划一下：屏幕朝上，手机头朝前，右手边就是Y，屏幕朝下就是Z。

// 机体坐标系轴定义
#define AXIS_X_FORWARD  0
#define AXIS_Y_RIGHT    1
#define AXIS_Z_DOWN     2

1.3 欧拉角——俯仰、横滚、偏航

好，现在我们有世界坐标系和机体坐标系了。那怎么描述飞机相对于地面的姿态呢？

最直观的方式就是欧拉角。说白了，就是三个角度：

这里有个坑，我必须提醒你：旋转顺序很重要！

常用的顺序是 Z-Y-X，也就是先偏航、再俯仰、最后横滚。为什么是这个顺序？因为这样能避免「万向锁」问题——嗯，这个我们后面章节会详细讲，今天先记住顺序。

1.4 旋转矩阵——数学工具登场

欧拉角虽然直观，但做数学运算不方便。比如你要把机体坐标系下的加速度转换到世界坐标系，总不能手动算三个角度吧？

这时候就需要旋转矩阵了。

旋转矩阵是一个3x3的正交矩阵，它的作用就是把一个向量从一个坐标系旋转到另一个坐标系。

以绕Z轴旋转ψ角度为例，旋转矩阵是：

Rz(ψ) = [cosψ  -sinψ   0]
        [sinψ   cosψ   0]
        [  0      0     1]

绕Y轴旋转θ角度：

Ry(θ) = [cosθ   0   sinθ]
        [  0    1    0  ]
        [-sinθ  0   cosθ]

绕X轴旋转φ角度：

Rx(φ) = [1    0      0   ]
        [0  cosφ  -sinφ ]
        [0  sinφ   cosφ ]

那么，从机体坐标系到世界坐标系的完整旋转矩阵就是：

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

注意乘法顺序！因为矩阵乘法不满足交换律，先旋转的矩阵在右边。我们刚才说的Z-Y-X顺序，就是先绕X转、再绕Y转、最后绕Z转，所以Rz在最左边。

1.5 代码实现——把数学变成C语言

光说不练假把式。我们来看看怎么在代码里实现旋转矩阵。

#include <math.h>

// 定义3x3旋转矩阵
typedef struct {
    float m[3][3];
} RotationMatrix;

// 根据欧拉角计算旋转矩阵 (Z-Y-X顺序)
RotationMatrix eulerToRotationMatrix(float roll, float pitch, float yaw) {
    RotationMatrix R;
    float cr = cos(roll);
    float sr = sin(roll);
    float cp = cos(pitch);
    float sp = sin(pitch);
    float cy = cos(yaw);
    float sy = sin(yaw);

    // 计算 R = Rz * Ry * Rx
    R.m[0][0] = cy * cp;
    R.m[0][1] = cy * sp * sr - sy * cr;
    R.m[0][2] = cy * sp * cr + sy * sr;
    
    R.m[1][0] = sy * cp;
    R.m[1][1] = sy * sp * sr + cy * cr;
    R.m[1][2] = sy * sp * cr - cy * sr;
    
    R.m[2][0] = -sp;
    R.m[2][1] = cp * sr;
    R.m[2][2] = cp * cr;

    return R;
}

// 使用旋转矩阵转换向量
void rotateVector(RotationMatrix *R, float *v_in, float *v_out) {
    v_out[0] = R->m[0][0] * v_in[0] + R->m[0][1] * v_in[1] + R->m[0][2] * v_in[2];
    v_out[1] = R->m[1][0] * v_in[0] + R->m[1][1] * v_in[1] + R->m[1][2] * v_in[2];
    v_out[2] = R->m[2][0] * v_in[0] + R->m[2][1] * v_in[1] + R->m[2][2] * v_in[2];
}

1.6 本章小结

今天我们讲了三个核心概念：

1. 世界坐标系：固定不动的参考系，常用ENU或NED
2. 机体坐标系：固连在飞行器上，随飞行器运动
3. 欧拉角：俯仰、横滚、偏航，描述姿态最直观的方式
4. 旋转矩阵：数学工具，用于坐标系之间的向量转换

下一章我们会讲四元数——一种比欧拉角更优雅、没有万向锁问题的姿态表示方法。到时候你会发现，旋转矩阵和四元数之间可以互相转换，非常有意思。

好了，今天就到这里。记得动手写写代码，把旋转矩阵算一遍。纸上得来终觉浅，绝知此事要躬行。

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