3、坐标系与姿态表示：导航坐标系、机体坐标系、欧拉角、旋转矩阵、四元数的定义与转换关系
在IMU（惯性测量单元）姿态解算中，坐标系与姿态表示方法是整个算法的数学基石。无论是加速度计、陀螺仪还是磁力计，其原始输出都是相对于某个坐标系的物理量；而姿态解算的目标，就是描述机体坐标系相对于导航坐标系的旋转关系。本章将系统讲解导航坐标系与机体坐标系的定义、欧拉角与旋转矩阵的物理含义、四元数的数学本质，以及它们之间的转换关系，为后续的互补滤波和卡尔曼滤波实战打下坚实基础。
3.1 导航坐标系与机体坐标系的定义
3.1.1 导航坐标系（n系）
导航坐标系是进行惯性导航解算的参考基准，其原点通常取在载体（如无人机、机器人）的质心位置。根据应用场景的不同，导航坐标系有两种常见取法：NED（北-东-地）坐标系和ENU（东-北-天）坐标系。NED坐标系中，X轴指向地理北、Y轴指向地理东、Z轴垂直指向地面；而ENU坐标系中，X轴指向东、Y轴指向北、Z轴垂直指向天空。在航空、航天及大多数地面机器人应用中，NED坐标系更为常见，因为它与重力方向一致，便于处理加速度数据。
导航坐标系的选择直接影响后续的姿态解算公式和传感器数据符号约定。例如，在NED坐标系下，静止时加速度计测得的比力为[0, 0, -g]（g为重力加速度），而在ENU坐标系下则为[0, 0, +g]。初学者若混淆坐标系约定，会导致姿态角符号错误或解算发散。
3.1.2 机体坐标系（b系）
机体坐标系与载体固连，随载体一起运动。其原点同样位于载体质心，X轴沿载体纵轴指向机头（或前进方向），Y轴垂直于X轴指向载体右侧（右翼方向），Z轴垂直于XOY平面指向载体下方，构成右手直角坐标系。IMU的三个轴（陀螺仪、加速度计）通常与机体坐标系的三个轴对齐安装，因此IMU的原始输出就是机体坐标系下的物理量。
需要特别注意的是，机体坐标系的定义与IMU的安装方向必须严格对应。如果IMU在PCB上的安装方向与机体坐标系不一致（例如旋转了90°安装），则需要在软件中进行轴重映射，否则姿态解算结果将完全错误。在实际工程中，我们通常通过标定或轴映射矩阵来修正这种安装偏差。
| 坐标系 | 原点 | X轴 | Y轴 | Z轴 | 典型应用 |
|---|---|---|---|---|---|
| 导航坐标系（NED） | 载体质心 | 北 | 东 | 地 | 航空、无人机、地面机器人 |
| 导航坐标系（ENU） | 载体质心 | 东 | 北 | 天 | 部分车载导航、室内定位 |
| 机体坐标系（b系） | 载体质心 | 机头方向 | 右翼方向 | 机体下方 | IMU测量基准 |
3.2 欧拉角：直观的姿态描述
3.2.1 欧拉角的定义与旋转顺序
欧拉角通过三个依次旋转的角度来描述刚体在三维空间中的姿态。在航空领域，最常用的欧拉角定义为：横滚角φ（Roll）、俯仰角θ（Pitch）和航向角ψ（Yaw）。其旋转顺序通常为：先绕Z轴旋转ψ（航向），再绕新的Y轴旋转θ（俯仰），最后绕新的X轴旋转φ（横滚），即Z-Y-X顺序。这个顺序并非随意选择，而是与航空器的运动特性相关——航向变化最频繁，俯仰次之，横滚相对独立。
欧拉角的物理意义非常直观：当无人机水平飞行时，横滚角描述其绕机体X轴的倾斜程度，俯仰角描述机头相对于水平面的仰俯角度，航向角描述机头相对于地理北的偏转角度。这种直观性使得欧拉角非常适合用于人机交互显示和简单的姿态控制。
3.2.2 欧拉角的局限性：万向节锁
欧拉角存在一个著名的数学缺陷——万向节锁（Gimbal Lock）。当俯仰角θ接近±90°时，第一次旋转（航向）和第三次旋转（横滚）的旋转轴会重合，导致系统丢失一个旋转自由度。此时，航向角和横滚角无法独立区分，姿态解算会出现奇异。例如，当无人机垂直爬升（θ=90°）时，航向和横滚的区分变得模糊，任何航向变化都可以被解释为横滚变化，反之亦然。
在实际飞行中，固定翼飞机或旋翼机很少会达到俯仰±90°的极端姿态，但对于全姿态飞行器（如特技无人机、导弹）或某些机器人应用，万向节锁是必须避免的。因此，在姿态解算的数学内核中，我们通常不直接使用欧拉角进行积分和滤波，而是使用四元数或旋转矩阵。
3.3 旋转矩阵：姿态的数学表达
3.3.1 旋转矩阵的定义与性质
旋转矩阵R是一个3×3的正交矩阵，满足RTR = I且det(R) = 1。它描述了机体坐标系相对于导航坐标系的旋转关系。若一个向量在机体坐标系下的坐标为vb，则其在导航坐标系下的坐标vn可通过vn = R · vb计算得到。旋转矩阵的每一列表示机体坐标系的基向量在导航坐标系中的投影。
由欧拉角（Z-Y-X顺序）构造旋转矩阵的公式为：
R = Rz(ψ) · Ry(θ) · Rx(φ)
其中：
Rz(ψ) = [cosψ -sinψ 0; sinψ cosψ 0; 0 0 1]
Ry(θ) = [cosθ 0 sinθ; 0 1 0; -sinθ 0 cosθ]
Rx(φ) = [1 0 0; 0 cosφ -sinφ; 0 sinφ cosφ]展开后得到完整的旋转矩阵，其元素包含三个欧拉角的三角函数组合。旋转矩阵的优点是：无奇异、可直接用于向量坐标变换、可方便地进行矩阵乘法组合多个旋转。缺点是：9个元素存在冗余（实际只有3个自由度），且每次更新需要处理9个数值，计算量相对较大。
3.3.2 从旋转矩阵提取欧拉角
在实际工程中，我们经常需要从旋转矩阵中提取欧拉角用于显示或控制。提取公式为：
φ = atan2(R[2][1], R[2][2])
θ = -asin(R[2][0])
ψ = atan2(R[1][0], R[0][0])其中R[i][j]表示旋转矩阵的第i行第j列元素（从0开始计数）。需要注意的是，当θ接近±90°时，atan2的参数会退化，此时提取的φ和ψ不再可靠，这正是万向节锁在数值上的体现。
3.4 四元数：高效无奇异的姿态表示
3.4.1 四元数的数学定义
四元数是由一个实部和三个虚部组成的超复数，形式为q = w + xi + yj + zk，通常写作q = [w, x, y, z]或q = [cos(θ/2), sin(θ/2)·nx, sin(θ/2)·ny, sin(θ/2)·nz]，其中n = [nx, ny, nz]是旋转轴的单位向量，θ是绕该轴的旋转角度。四元数满足约束条件w² + x² + y² + z² = 1，即单位四元数。
四元数表示旋转的核心优势在于：无万向节锁、计算效率高（仅需4个标量）、便于插值（球面线性插值slerp）。在姿态解算中，我们使用四元数作为姿态的“内部表示”，陀螺仪积分、互补滤波、卡尔曼滤波的状态向量都使用四元数。
3.4.2 四元数与旋转矩阵的转换
从四元数q = [w, x, y, z]构造旋转矩阵的公式为：
R = [1-2(y²+z²) 2(xy-wz) 2(xz+wy);
2(xy+wz) 1-2(x²+z²) 2(yz-wx);
2(xz-wy) 2(yz+wx) 1-2(x²+y²)]反之，从旋转矩阵提取四元数的算法有多种，常用的基于矩阵迹的方法为：
t = R[0][0] + R[1][1] + R[2][2]
if t > 0:
s = 0.5 / sqrt(t+1)
w = 0.25 / s
x = (R[2][1] - R[1][2]) * s
y = (R[0][2] - R[2][0]) * s
z = (R[1][0] - R[0][1]) * s
else:
# 处理t≤0的情况，选择最大对角元素对应分支
...从四元数提取欧拉角的公式为：
φ = atan2(2(wx+yz), 1-2(x²+y²))
θ = asin(2(wy-zx))
ψ = atan2(2(wz+xy), 1-2(y²+z²))3.5 姿态更新：四元数微分方程
3.5.1 四元数微分方程
在IMU姿态解算中，陀螺仪输出的是机体坐标系下的角速度ω = [ωx, ωy, ωz]。四元数随时间的变化率与角速度的关系为：
dq/dt = 0.5 · q ⊗ [0, ωx, ωy, ωz]其中⊗表示四元数乘法。写成矩阵形式为：
[dw/dt] [0 -ωx -ωy -ωz] [w]
[dx/dt] = [ωx 0 ωz -ωy] [x]
[dy/dt] [ωy -ωz 0 ωx] [y]
[dz/dt] [ωz ωy -ωx 0 ] [z]在实际代码中，我们使用一阶龙格-库塔法（即欧拉法）进行离散化更新：
q_new = q_old + (dt/2) · q_old ⊗ [0, ωx, ωy, ωz]
然后归一化：q_new = q_new / ||q_new||归一化是必须的，因为数值积分误差会导致四元数偏离单位范数，进而引起旋转矩阵非正交，最终导致姿态漂移。
3.5.2 陀螺仪积分与误差累积
单纯使用陀螺仪积分进行姿态更新，会因陀螺仪零偏、噪声和积分误差而随时间累积漂移。例如，一个零偏为0.01°/s的陀螺仪，在1小时内会累积36°的姿态误差。因此，实际系统中必须融合加速度计和磁力计的数据来修正陀螺仪积分带来的漂移。这正是后续章节中互补滤波和卡尔曼滤波的核心目的。
3.6 案例拆解：从IMU原始数据到欧拉角输出
下面通过一个完整的案例，演示如何将IMU原始数据转换为欧拉角输出。假设我们使用一款常见的六轴IMU（如MPU6050），其陀螺仪量程为±2000°/s，加速度计量程为±8g，采样频率为100Hz。
步骤1：读取原始数据并转换为物理量
// 假设从寄存器读取的原始值为raw_gyro_x, raw_gyro_y, raw_gyro_z
// 陀螺仪灵敏度：131 LSB/(°/s)（量程±2000°/s时）
float gyro_x = raw_gyro_x / 131.0f; // 单位：°/s
float gyro_y = raw_gyro_y / 131.0f;
float gyro_z = raw_gyro_z / 131.0f;
// 加速度计灵敏度：4096 LSB/g（量程±8g时）
float accel_x = raw_accel_x / 4096.0f; // 单位：g
float accel_y = raw_accel_y / 4096.0f;
float accel_z = raw_accel_z / 4096.0f;步骤2：初始化四元数（假设初始水平，航向为0）
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;步骤3：陀螺仪积分更新四元数（dt = 0.01s）
float norm = sqrt(gyro_x*gyro_x + gyro_y*gyro_y + gyro_z*gyro_z);
float half_dt = 0.005f;
// 四元数微分方程（简化版，未包含角速度归一化）
float dq0 = half_dt * (-q1*gyro_x - q2*gyro_y - q3*gyro_z);
float dq1 = half_dt * ( q0*gyro_x + q2*gyro_z - q3*gyro_y);
float dq2 = half_dt * ( q0*gyro_y - q1*gyro_z + q3*gyro_x);
float dq3 = half_dt * ( q0*gyro_z + q1*gyro_y - q2*gyro_x);
q0 += dq0; q1 += dq1; q2 += dq2; q3 += dq3;
// 归一化
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;步骤4：从四元数提取欧拉角
float roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2)) * 180/M_PI;
float pitch = asin(2*(q0*q2 - q3*q1)) * 180/M_PI;
float yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3)) * 180/M_PI;结果分析：在静止状态下，陀螺仪积分会使姿态缓慢漂移。例如，若陀螺仪零偏为0.5°/s，则10秒后航向角将漂移约5°。为了抑制漂移，需要引入加速度计数据修正横滚和俯仰角。具体方法将在后续章节的互补滤波中详细讲解。
| 姿态表示方法 | 参数数量 | 有无奇异 | 计算效率 | 直观性 | 适用场景 |
|---|---|---|---|---|---|
| 欧拉角 | 3 | 有（±90°俯仰） | 中 | 高 | 显示、简单控制 |
| 旋转矩阵 | 9 | 无 | 低 | 低 | 坐标变换、多旋转组合 |
| 四元数 | 4 | 无 | 高 | 中 | 姿态解算、滤波、插值 |
3.7 常见误区与总结复盘
误区一：混淆坐标系定义。NED与ENU的选择会影响加速度计数据的符号和姿态角的定义。在代码中必须明确注释坐标系约定，并在整个工程中保持一致。
误区二：直接对欧拉角积分。欧拉角微分方程包含三角函数且存在奇异，不适合用于姿态更新。正确做法是使用四元数或旋转矩阵进行积分，仅在需要时转换为欧拉角。
误区三：忘记四元数归一化。四元数积分后必须归一化，否则旋转矩阵不再正交，会导致姿态误差累积。建议每次更新后立即归一化。
误区四：忽略陀螺仪零偏。陀螺仪零偏是姿态漂移的主要来源。在系统启动时应进行静态零偏校准，并在运行中通过加速度计/磁力计数据持续修正。
误区五：轴映射错误。IMU安装方向与机体坐标系不一致时，必须进行轴重映射。建议在硬件设计阶段就明确IMU的安装方向，并在软件中提供轴映射配置接口。