2、数学基础：三维空间旋转表示（欧拉角、旋转矩阵、四元数）、坐标系定义（机体坐标系、世界坐标系）、旋转的复合与插值

IMU姿态解算的核心任务，是实时估计传感器所在刚体（如无人机、机器人、手机）相对于某个参考坐标系的旋转状态。要完成这一任务，我们必须先建立一套严谨的数学语言来描述“旋转”本身。本章将系统梳理三维空间旋转的三种主流表示方法——欧拉角、旋转矩阵与四元数，明确机体坐标系与世界坐标系的定义与转换关系，并深入探讨旋转的复合运算与插值策略。这些内容不仅是后续加速度计与陀螺仪数据融合的基石，也直接决定了姿态解算算法的精度、稳定性与计算效率。

2.1 坐标系定义：世界坐标系与机体坐标系

2.1.1 核心原理

姿态解算必须明确“相对于谁旋转”。我们定义两个右手直角坐标系：世界坐标系（World Frame, W），通常固定于地面，其Z轴指向天顶（与重力方向相反），X轴指向北方（或任意水平基准方向），Y轴由右手定则确定。对于大多数无人机和机器人应用，世界坐标系采用“东北天（ENU）”或“北东地（NED）”约定。另一个是机体坐标系（Body Frame, B），原点位于IMU的测量中心，随载体运动而旋转，其X轴指向载体前方，Y轴指向载体右方，Z轴指向载体下方（或上方，取决于具体约定）。

两个坐标系之间的相对关系，就是我们需要估计的姿态。从数学上看，姿态可以表示为从机体坐标系到世界坐标系的旋转映射。例如，一个向量在机体坐标系中的坐标为 v_B，在世界坐标系中的坐标 v_W 满足：

v_W = R_WB · v_B

其中 R_WB 是一个3×3的旋转矩阵，它同时包含了姿态的全部信息。理解这两个坐标系的定义，是后续所有计算的前提。

2.1.2 判断方法与操作流程

在实际工程中，确定坐标系约定需要遵循以下步骤：

1. 明确应用场景：无人机通常使用NED（北东地）坐标系，因为其Z轴向下与重力方向一致，便于高度控制；而室内机器人和手机则常用ENU（东北天）坐标系。
2. 定义机体轴方向：将IMU的X轴对准载体的前进方向，Y轴对准右方，Z轴根据右手定则确定。注意加速度计和陀螺仪的极性必须与定义的轴方向一致。
3. 验证初始对齐：将载体水平放置，此时机体坐标系与世界坐标系的Z轴应平行（或反平行），读取加速度计输出，检查重力分量是否落在预期的轴上。
4. 记录约定文档：在代码注释和设计文档中明确写出坐标系定义，避免团队协作时产生歧义。

2.1.3 案例拆解：无人机起飞前的坐标系校准

假设我们正在调试一架四旋翼无人机，IMU的Z轴向下安装。初始上电时，无人机水平放置于地面。此时加速度计读数应为 (0, 0, +1g)（若采用NED约定，Z轴向下，重力加速度为+1g）。如果读数为 (0, 0, -1g)，则说明Z轴方向定义反了，需要调整代码中的符号。这个简单的检查步骤，能避免后续姿态解算出现180°的翻转错误。进一步地，我们还可以将无人机绕Z轴旋转90°，观察陀螺仪Z轴积分结果是否与旋转角度一致，以验证陀螺仪的极性。

2.1.4 常见误区与总结

总结：坐标系定义是姿态解算的“宪法”，必须在算法设计前就明确并验证。建议在代码中定义枚举类型（如 enum FrameConvention { NED, ENU }），并在初始化时进行自检。

2.2 欧拉角：直观但存在万向锁

2.2.1 核心原理

欧拉角通过三个依次绕不同坐标轴的旋转角度来描述姿态，最常见的约定是Z-Y-X（偏航-俯仰-横滚）顺序：先绕Z轴旋转偏航角（Yaw, ψ），再绕新的Y轴旋转俯仰角（Pitch, θ），最后绕新的X轴旋转横滚角（Roll, φ）。这三个角度直接对应载体的航向、抬头低头和左右倾斜，物理意义非常直观。旋转矩阵可由三个基本旋转矩阵相乘得到：

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

其中每个基本旋转矩阵都是绕单一轴的旋转。欧拉角的优势在于人机交互友好，调试时可以直接读取角度值。

2.2.2 判断方法与操作流程

使用欧拉角时，必须注意旋转顺序和角度范围。偏航角通常定义为 [-π, π] 或 [0, 2π]，俯仰角限制在 [-π/2, π/2] 以避免奇异性，横滚角限制在 [-π, π]。当俯仰角接近 ±90° 时，偏航和横滚的旋转轴重合，导致自由度丢失，即万向锁（Gimbal Lock）现象。此时，系统无法唯一确定偏航和横滚的分配，姿态解算会失效。

2.2.3 案例拆解：固定翼飞机的姿态显示

在固定翼飞机的姿态指示器（ADI）中，俯仰角范围通常为 ±90°，横滚角为 ±180°。当飞机垂直爬升（俯仰角接近90°）时，仪表上的偏航和横滚指示会变得不稳定，这就是万向锁的直观表现。对于这种应用，如果仅使用欧拉角进行姿态解算，在极端姿态下会出现数值跳变。因此，工程上通常使用四元数进行内部解算，仅在显示层将四元数转换为欧拉角。

2.2.4 常见误区与总结

总结：欧拉角是理解姿态最直观的工具，但其奇异性（万向锁）和三角函数计算开销使其不适合作为核心解算表示。下表对比了三种表示方法的关键特性：

特性	欧拉角	旋转矩阵	四元数
存储量	3个角度	9个元素	4个元素
奇异性	有（万向锁）	无	无
复合运算	复杂（需矩阵乘法）	矩阵乘法	四元数乘法
插值	线性插值不自然	不适用	SLERP平滑
计算效率	中（含三角函数）	高（矩阵乘法）	高（仅乘加）
直观性	最直观	不直观	不直观

2.3 旋转矩阵：精确但冗余

2.3.1 核心原理

旋转矩阵是一个3×3的正交矩阵，满足 R^TR = I 且 det(R) = +1。它描述了坐标系旋转的线性变换。旋转矩阵的优点是：可以直接用于向量变换，复合运算就是简单的矩阵乘法，且不存在奇异性。缺点是：9个元素之间存在6个约束条件（正交性和单位行列式），存储冗余，且数值漂移后需要重新正交化，增加了计算负担。

2.3.2 判断方法与操作流程

在IMU解算中，旋转矩阵常用于将加速度计测量的比力从机体坐标系转换到世界坐标系，以便计算重力方向。操作流程如下：

1. 根据当前姿态（四元数或欧拉角）构造旋转矩阵 R_WB。
2. 将加速度计读数 a_B 左乘旋转矩阵：a_W = R_WB · a_B。
3. 在世界坐标系中，将 a_W 与重力向量 g_W = (0, 0, 1) 比较，得到姿态误差。

2.3.3 案例拆解：互补滤波中的矩阵应用

在经典的互补滤波器中，我们利用旋转矩阵将加速度计读数转换到世界坐标系，然后与重力参考向量叉积得到误差。例如，若载体水平，加速度计读数应为 (0, 0, 1)。如果实际读数为 (0.1, 0, 0.995)，则叉积误差为 (0, 0.1, 0)（近似），该误差用于修正陀螺仪积分漂移。这个过程中，旋转矩阵的精度直接影响误差计算的准确性。

2.3.4 常见误区与总结

总结：旋转矩阵是理论分析的有力工具，但在嵌入式平台上，由于存储和计算效率的考虑，通常不直接作为姿态状态量，而是作为中间转换媒介。

2.4 四元数：高效且无奇异性

2.4.1 核心原理

四元数是一个超复数，形式为 q = w + xi + yj + zk，其中 w 是标量部分， (x, y, z) 是向量部分。单位四元数（模长为1）可以表示三维旋转。其物理意义是：绕单位轴 n = (nx, ny, nz) 旋转角度 θ 的四元数为：

q = [cos(θ/2), nx·sin(θ/2), ny·sin(θ/2), nz·sin(θ/2)]

四元数的乘法（Hamilton积）可以复合旋转，且不存在万向锁问题。其逆（共轭）表示反向旋转。四元数在插值（SLERP）和微分方程（姿态更新）中表现出色，是IMU融合算法（如Mahony、Madgwick）的首选表示。

2.4.2 判断方法与操作流程

使用四元数进行姿态更新时，核心流程为：

1. 初始化：根据初始欧拉角构造四元数。
2. 陀螺仪积分：读取角速度 ω = (ωx, ωy, ωz)，构造增量四元数 q_delta = [1, ωx·dt/2, ωy·dt/2, ωz·dt/2]（小角度近似）。
3. 更新姿态：q_new = q_old ⊗ q_delta，然后归一化。
4. 融合修正：利用加速度计和磁力计计算误差，修正四元数。

2.4.3 案例拆解：四元数SLERP插值在云台控制中的应用

在相机云台控制中，需要平滑地从当前姿态旋转到目标姿态。如果使用欧拉角线性插值，会导致旋转路径不自然（可能出现抖动）。而使用四元数的球面线性插值（SLERP），可以保证旋转轴恒定、角速度均匀。例如，云台从 (0°, 0°, 0°) 旋转到 (90°, 45°, 0°)，SLERP会生成一条平滑的弧线路径，而欧拉角插值则可能产生不稳定的中间姿态。SLERP公式为：

q(t) = (sin((1-t)Ω) / sinΩ) · q1 + (sin(tΩ) / sinΩ) · q2

其中 Ω 是 q1 和 q2 之间的夹角，t 从0到1。

2.4.4 常见误区与总结

总结：四元数在计算效率、存储和插值平滑性上全面优于欧拉角和旋转矩阵，是IMU姿态解算的工业标准。

2.5 旋转的复合与插值：工程实践要点

2.5.1 核心原理

旋转的复合是指将多个旋转依次作用于刚体。在四元数中，复合运算为乘法：q_total = q2 ⊗ q1（先应用 q1，再应用 q2）。注意乘法顺序不可交换。旋转的插值则是在两个姿态之间生成平滑过渡的中间姿态。四元数的SLERP插值能保证最短路径和恒定角速度，而欧拉角的线性插值则可能产生不自然的旋转。

2.5.2 判断方法与操作流程

在IMU解算中，旋转复合用于将陀螺仪的增量旋转累加到当前姿态上。操作流程为：

1. 读取陀螺仪角速度，乘以采样周期 dt，得到角增量 Δθ。
2. 构造增量四元数 q_inc = [cos(|Δθ|/2), (Δθ/|Δθ|)·sin(|Δθ|/2)]。
3. 更新姿态：q_current = q_current ⊗ q_inc。
4. 归一化四元数。

2.5.3 案例拆解：多传感器融合中的旋转复合

在视觉-惯性导航系统中，相机位姿（旋转）和IMU姿态需要对齐。假设IMU坐标系到相机坐标系的固定旋转为 q_BC（标定得到），则相机在世界坐标系中的姿态为 q_WC = q_WB ⊗ q_BC。这个复合运算将IMU的姿态转换到相机坐标系，用于视觉特征点的投影。如果复合顺序错误，会导致视觉和惯性数据严重不匹配。

2.5.4 常见误区与总结

总结：旋转的复合与插值是姿态解算的日常操作。四元数提供了最优雅的数学工具，但需要开发者深刻理解其运算规则和物理含义。下表总结了不同表示方法在复合与插值中的适用性：

操作	欧拉角	旋转矩阵	四元数
复合	需转换为矩阵，计算复杂	矩阵乘法，直接但冗余	四元数乘法，高效
插值	线性插值不自然，有抖动	不适用	SLERP平滑，最短路径
微分方程	存在奇异点	需正交化修正	无奇异，计算简单

在实际工程中，推荐采用“四元数为核心，欧拉角为显示接口”的策略。初始化时使用欧拉角设定初始姿态，然后转换为四元数进行实时解算，最终在需要输出给用户或上层控制时，再将四元数转换回欧拉角。这种混合方案兼顾了直观性与计算鲁棒性。