2. 坐标系与姿态表示：地理坐标系、载体坐标系、欧拉角、四元数、旋转矩阵、坐标系转换关系

做IMU融合算法，说白了就是在跟坐标系打交道。我刚开始接触这行的时候，觉得坐标系转换就是个数学游戏，直到有一次在跑车实验里，姿态解算直接飞了——后来发现是坐标系定义搞反了。嗯，从那以后我再也不敢小看这一章了。

今天咱们就把坐标系和姿态表示这件事彻底捋清楚。你想想看，IMU输出的原始数据是加速度和角速度，但这些数据是在传感器自己身上的坐标系里测的。我们要用它来算载体的姿态、位置，就必须在不同坐标系之间来回倒腾。这就是整个融合算法的地基。

  核心要点： 坐标系定义错了，后面所有算法都是白搭。我建议你在项目一开始，就把坐标系约定写死在代码注释里。

2.1 地理坐标系（n系）

地理坐标系，也叫导航坐标系，通常用 n 表示。它固定在地球上，是我们描述载体位置和姿态的参考基准。

常见的定义方式有两种：

东北天（ENU）：X轴指向东，Y轴指向北，Z轴指向天。我在无人机项目里常用这个。
北东地（NED）：X轴指向北，Y轴指向东，Z轴指向地。飞机和导弹领域更常见。

我个人习惯用东北天（ENU），因为Z轴朝上，跟直觉比较吻合。但如果你做的是航空惯导，大概率会碰到NED。记住，没有对错，只有约定。

小技巧： 我建议你在代码里用枚举类型把坐标系定义写清楚，比如 enum CoordFrame { ENU, NED }，避免后续混淆。

2.2 载体坐标系（b系）

载体坐标系固连在运动物体上，用 b 表示。原点在载体的质心，三个轴跟载体的几何结构对齐。

典型的定义：

X轴：指向载体前进方向（前）
Y轴：指向载体右侧（右）
Z轴：指向载体下方（下），构成右手系

IMU传感器就安装在这个坐标系里。它输出的加速度和角速度，都是相对于载体坐标系的。所以，我们要把b系的数据转换到n系，才能做导航解算。

注意： 我曾经遇到过一个坑——IMU的安装方向跟载体坐标系定义不一致。比如IMU的X轴朝后，那你就得在代码里做一次固定的旋转补偿。否则，融合结果会完全错误。

2.3 欧拉角

欧拉角是最直观的姿态表示方法。它用三个角度来描述载体坐标系相对于地理坐标系的旋转：

航向角（Yaw, ψ）：绕Z轴旋转，范围通常[-π, π]或[0, 2π)
俯仰角（Pitch, θ）：绕Y轴旋转，范围[-π/2, π/2]
横滚角（Roll, φ）：绕X轴旋转，范围[-π, π]

旋转顺序一般是 Z-Y-X（航向→俯仰→横滚）。这个顺序很重要，换一个顺序，结果就完全不一样了。

欧拉角的好处是直观，看一眼就知道载体大概是什么姿态。但坏处也很明显——万向锁。当俯仰角接近±90°时，航向和横滚会耦合，丢失一个自由度。我在做四轴飞行器的时候遇到过这个问题，飞控直接乱转。

  避坑指南： 如果你做的是全姿态运动（比如无人机、机器人），尽量不要用欧拉角做内部解算。它只适合做显示和人工交互。

2.4 四元数

四元数是我个人最喜欢的姿态表示方式。它没有万向锁，计算效率高，插值平滑。说白了，它就是工程上的最优解。

四元数是一个四维复数：

q = w + xi + yj + zk

其中 w 是实部，x、y、z 是虚部。它满足约束条件：

w² + x² + y² + z² = 1

四元数表示旋转的核心思想是：绕单位向量 (x, y, z) 旋转角度 θ，对应的四元数为：

q = [cos(θ/2), x*sin(θ/2), y*sin(θ/2), z*sin(θ/2)]

用四元数旋转一个向量 v 的公式是：

v' = q * v * q_conj

其中 q_conj 是 q 的共轭。这个公式在代码里实现起来非常简洁。

经验之谈： 我建议你在代码里用四元数做所有内部姿态更新。只在最后输出给用户时，才转成欧拉角。这样既避免了万向锁，又保持了可读性。

2.5 旋转矩阵

旋转矩阵是坐标系转换的数学工具。从b系到n系的旋转矩阵记作 C_b^n，它是一个3×3的正交矩阵，满足：

C_b^n * (C_b^n)^T = I

用欧拉角构建旋转矩阵（Z-Y-X顺序）：

C_b^n = R_z(ψ) * R_y(θ) * R_x(φ)

其中：

R_z(ψ) = [cosψ  -sinψ  0;  sinψ  cosψ  0;  0  0  1]
R_y(θ) = [cosθ  0  sinθ;  0  1  0;  -sinθ  0  cosθ]
R_x(φ) = [1  0  0;  0  cosφ  -sinφ;  0  sinφ  cosφ]

旋转矩阵的好处是直观，而且可以串联多个旋转。但缺点是9个参数有冗余，而且数值更新时容易失去正交性，需要定期重新正交化。

注意： 旋转矩阵的更新要用到角速度，公式是 C_dot = C * [ω×]。这个 [ω×] 是角速度的反对称矩阵。我刚开始写代码时忘了这个，结果姿态越算越偏。

2.6 坐标系转换关系

好了，现在我们把所有东西串起来。假设IMU在b系测得了加速度 a_b 和角速度 ω_b，我们要把它们转换到n系：

加速度转换：a_n = C_b^n * a_b
角速度转换：角速度本身就是向量，直接转换：ω_n = C_b^n * ω_b
姿态更新：用四元数微分方程：q_dot = 0.5 * q ⊗ ω_b

这里 ⊗ 表示四元数乘法。实际代码里，我们通常用离散化的方式更新四元数：

// 假设 dt 是采样间隔
// ω 是角速度向量 [ωx, ωy, ωz]
double norm = sqrt(ωx*ωx + ωy*ωy + ωz*ωz);
double half_theta = 0.5 * norm * dt;

// 如果角速度很小，用一阶近似
if (norm < 1e-6) {
    q.w += 0.5 * (-q.x*ωx - q.y*ωy - q.z*ωz) * dt;
    q.x += 0.5 * ( q.w*ωx - q.z*ωy + q.y*ωz) * dt;
    q.y += 0.5 * ( q.z*ωx + q.w*ωy - q.x*ωz) * dt;
    q.z += 0.5 * (-q.y*ωx + q.x*ωy + q.w*ωz) * dt;
} else {
    double sin_half = sin(half_theta) / norm;
    double cos_half = cos(half_theta);
    
    // 四元数乘法更新
    double dq_w = cos_half;
    double dq_x = ωx * sin_half;
    double dq_y = ωy * sin_half;
    double dq_z = ωz * sin_half;
    
    // q_new = q ⊗ dq
    double w = q.w*dq_w - q.x*dq_x - q.y*dq_y - q.z*dq_z;
    double x = q.w*dq_x + q.x*dq_w + q.y*dq_z - q.z*dq_y;
    double y = q.w*dq_y - q.x*dq_z + q.y*dq_w + q.z*dq_x;
    double z = q.w*dq_z + q.x*dq_y - q.y*dq_x + q.z*dq_w;
    
    q = {w, x, y, z};
}

// 归一化，保证四元数单位长度
double norm_q = sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
q.w /= norm_q;
q.x /= norm_q;
q.y /= norm_q;
q.z /= norm_q;

  关键点： 每次更新完四元数，一定要做归一化。数值误差会慢慢累积，不归一化的话，姿态会漂移。我吃过这个亏，调试了整整两天才发现是归一化的问题。

2.7 知识体系总览

下面这张图把本章的核心逻辑串起来了。你仔细看一遍，应该能理解坐标系和姿态表示在整个IMU融合算法中的位置。

这张图里，左边是地理坐标系，右边是载体坐标系，中间是三种姿态表示方式。它们通过旋转矩阵和四元数相互转换，最终服务于IMU融合算法。你把这个逻辑理清了，后面学滤波器和融合算法就会轻松很多。

我的建议： 刚开始做IMU项目时，先把坐标系转换的单元测试写好。用几个已知的简单旋转（比如绕单轴转90°）验证代码正确性。这一步做好了，后面调试融合算法时能省一半时间。

好了，坐标系和姿态表示就讲到这里。这些东西是IMU融合算法的基本功，你花时间把它吃透，后面会越走越顺。记住，工程上四元数是首选，欧拉角只做显示，旋转矩阵做中间转换。这个套路我用了好多年，没出过问题。

📚 课程目录

2. 坐标系与姿态表示：地理坐标系、载体坐标系、欧拉角、四元数、旋转矩阵、坐标系转换关系

2.1 地理坐标系（n系）

2.2 载体坐标系（b系）

2.3 欧拉角

2.4 四元数

2.5 旋转矩阵

2.6 坐标系转换关系

2.7 知识体系总览