第1章：坐标系与姿态表示

做IMU融合这些年，我最大的体会就是——坐标系搞不清楚，后面全是白搭。你想想看，传感器输出的数据是相对于它自己的，而你要控制的是整个载体的运动。这两者之间怎么对齐？这就是本章要解决的问题。

1.1 导航坐标系与载体坐标系

先说两个最基础的坐标系。我个人习惯把它们比作「世界地图」和「手机屏幕」。

导航坐标系（n系），也叫世界坐标系。它固定在地面上，通常取北-东-地（NED）或者东-北-天（ENU）。我最早做无人机项目时用的就是NED，因为飞控算法里重力方向是正Z，用起来很直观。

载体坐标系（b系），固定在运动物体上。原点在载体质心，X轴指向载体前方，Y轴指向右侧，Z轴指向下方。说白了，就是「我坐在飞机里，往前看的方向就是X」。

嗯，这里要注意：两个坐标系之间的转换，就是姿态估计的核心任务。

我曾经在标定一个六轴IMU时，发现数据总是对不上。折腾了两天，最后发现是坐标系定义反了——我把载体坐标系的Z轴朝上了，而算法里默认朝下。这种低级错误，犯过一次就再也不会忘了。

1.2 欧拉角：俯仰、横滚、偏航

欧拉角是描述姿态最直观的方式。说白了就是三个角度：

• 俯仰角（Pitch, θ）：绕Y轴旋转，抬头低头
• 横滚角（Roll, φ）：绕X轴旋转，左右倾斜
• 偏航角（Yaw, ψ）：绕Z轴旋转，左右转向

旋转顺序我建议用Z-Y-X（偏航→俯仰→横滚），这是航空航天领域的标准。为什么是这个顺序？因为偏航角变化最频繁，放在最后旋转可以减少耦合。

欧拉角的微分方程也很重要：

// 欧拉角速率与角速度的关系
// ω = [p, q, r]^T 是载体坐标系下的角速度
// [φ̇, θ̇, ψ̇]^T 是欧拉角速率

φ̇ = p + q·sinφ·tanθ + r·cosφ·tanθ
θ̇ = q·cosφ - r·sinφ
ψ̇ = (q·sinφ + r·cosφ) / cosθ

你看，当θ接近90°时，cosθ趋近于0，ψ̇会变得无穷大。这就是万向锁的数学本质。

1.3 旋转矩阵

旋转矩阵是一个3×3的正交矩阵，它把一个向量从一个坐标系旋转到另一个坐标系。我习惯用R_bⁿ表示「从载体坐标系到导航坐标系的旋转」。

单次旋转的矩阵很简单：

// 绕X轴旋转φ角
R_x(φ) = [1,    0,     0;
          0,  cosφ, -sinφ;
          0,  sinφ,  cosφ]

// 绕Y轴旋转θ角
R_y(θ) = [cosθ,  0,  sinθ;
          0,     1,     0;
         -sinθ,  0,  cosθ]

// 绕Z轴旋转ψ角
R_z(ψ) = [cosψ, -sinψ,  0;
          sinψ,  cosψ,  0;
          0,     0,     1]

完整的旋转矩阵就是三个矩阵的乘积（Z-Y-X顺序）：

R = R_z(ψ) · R_y(θ) · R_x(φ)

1.4 四元数基础

说实话，我刚开始接触四元数时也觉得头大。但用久了你会发现，它比欧拉角和旋转矩阵都好用。为什么？

• 没有万向锁
• 计算量小（只有4个参数）
• 插值平滑（做姿态平滑时特别有用）

四元数可以理解为「绕一个单位向量n旋转θ角」的数学表示：

q = [cos(θ/2), n_x·sin(θ/2), n_y·sin(θ/2), n_z·sin(θ/2)]^T
   = [q0, q1, q2, q3]^T

其中q0是标量部分，[q1, q2, q3]是矢量部分。注意，四元数必须是单位四元数（模长为1），否则旋转会带缩放。

四元数的乘法（用于组合旋转）：

q = q1 ⊗ q2

q0 = q1_0·q2_0 - q1_1·q2_1 - q1_2·q2_2 - q1_3·q2_3
q1 = q1_0·q2_1 + q1_1·q2_0 + q1_2·q2_3 - q1_3·q2_2
q2 = q1_0·q2_2 - q1_1·q2_3 + q1_2·q2_0 + q1_3·q2_1
q3 = q1_0·q2_3 + q1_1·q2_2 - q1_2·q2_1 + q1_3·q2_0

嗯，这个公式看着复杂，但写成代码就是几行乘法加法。我建议你直接封装成函数，别每次都手算。

1.5 三种表示方法的对比

特性	欧拉角	旋转矩阵	四元数
参数数量	3	9	4
直观性	★★★★★	★★☆☆☆	★★☆☆☆
万向锁	有	无	无
计算效率	高	低	高
插值平滑	差	一般	好
适用场景	人机交互、显示	坐标变换	姿态解算、滤波

我个人在实际项目中，通常这样搭配使用：

• 算法内部用四元数做姿态解算和滤波
• 需要显示或记录时，转成欧拉角
• 做坐标变换时，转成旋转矩阵

最后分享一个我踩过的坑：有一次做IMU标定，我用了欧拉角做姿态更新，结果在俯仰角接近90°时，数据直接炸了。从那以后，我所有IMU融合算法都改用四元数。你想想看，一个万向锁就能让整个系统崩溃，这代价太大了。

可可精品资料库