坐标系与姿态表示：导航坐标系、机体坐标系、欧拉角、旋转矩阵、四元数

IMU（惯性测量单元）输出的原始数据是加速度计和陀螺仪在机体坐标系下的测量值，而姿态解算、导航定位、控制决策等几乎所有上层应用，都需要在导航坐标系下完成。因此，理解坐标系定义以及姿态的数学表示方法，是进行IMU数据处理的第一步，也是后续滤波算法（如互补滤波、卡尔曼滤波）建模的基础。本节将系统梳理两种核心坐标系的定义与转换关系，并深入对比欧拉角、旋转矩阵、四元数三种姿态表示方法的数学原理、工程适用场景与相互转换方法。

3.1 导航坐标系与机体坐标系的定义与工程意义

3.1.1 核心原理：为什么需要两个坐标系？

IMU传感器（加速度计、陀螺仪、磁力计）直接固连在载体上，其测量轴与载体的几何轴对齐，因此原始数据天然定义在机体坐标系（body frame，b系）中。然而，用户关心的是载体相对于地面、地球或惯性空间的运动状态，这就需要引入导航坐标系（navigation frame，n系）。导航坐标系通常选取当地水平坐标系（如北-东-地，NED），其原点位于载体质心，X轴指向地理北向，Y轴指向东向，Z轴垂直于当地水平面向下。

两个坐标系之间的差异本质上是旋转关系。当载体姿态发生变化时，同一物理量（如重力加速度、地磁场矢量）在两个坐标系下的投影分量不同。姿态解算的核心任务，就是实时估计从b系到n系的旋转关系，从而将IMU测量值转换到导航坐标系下进行积分、融合和导航解算。

3.1.2 判断方法：如何选择坐标系约定？

工程实践中，坐标系的选择直接影响后续算法的复杂度。以下是常用约定：

• NED（北-东-地）：航空、无人机领域最常用，Z轴向下，与重力方向一致，便于高度解算。
• ENU（东-北-天）：机器人、自动驾驶领域常用，Z轴向上，与视觉SLAM的坐标习惯兼容。
• 机体坐标系：X轴沿载体纵轴向前，Y轴向右，Z轴向下（右手定则），与NED构成右手系。

3.1.3 案例拆解：坐标系定义错误导致的飞控异常

背景：某四旋翼无人机开发团队在移植开源飞控代码时，发现飞机起飞后迅速侧翻。排查过程中，地面站显示的姿态角在静止状态下正常，但一旦推油门，滚转角快速发散。

分析过程：检查代码发现，开源飞控默认使用NED坐标系，而团队在传感器驱动中误将加速度计的Z轴方向配置为向上（ENU习惯）。这导致重力加速度在n系中的投影符号错误，姿态解算中的补偿项方向相反，控制器输出正反馈，最终引发侧翻。

解决措施：将传感器安装方向与坐标系定义对齐，并在驱动层增加坐标轴重映射函数，确保加速度计输出与NED约定一致。修改后，无人机飞行稳定。

复盘：坐标系定义是IMU工程的第一道关口，任何不一致都会在闭环控制中被放大。建议在硬件调试阶段使用水平静止数据验证加速度计输出是否满足“Z轴约等于+1g（NED）”或“-1g（ENU）”的预期。

3.2 欧拉角：直观但存在万向锁问题

3.2.1 核心原理：三次基本旋转的合成

欧拉角通过三次绕不同坐标轴的旋转来描述姿态，常用的旋转顺序为Z-Y-X（偏航-俯仰-滚转，即yaw-pitch-roll）。设偏航角为ψ，俯仰角为θ，滚转角为φ，则从b系到n系的旋转矩阵可分解为：

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

其中Rz、Ry、Rx分别为绕Z、Y、X轴的基本旋转矩阵。欧拉角的物理意义直观，便于人机交互和可视化显示，是地面站、调试工具中最常用的姿态表示。

3.2.2 判断方法：万向锁的识别与规避

当俯仰角θ接近±90°时，偏航轴与滚转轴在空间中重合，系统丢失一个自由度，这种现象称为万向锁（Gimbal Lock）。此时，偏航角和滚转角无法唯一确定，且角速度到欧拉角速率的变换矩阵出现奇异（分母为cosθ）。

3.2.3 实战场景：欧拉角在姿态显示中的应用

在无人机地面站软件中，通常使用欧拉角实时显示飞行姿态。开发者需要将四元数转换为欧拉角，转换公式如下（Z-Y-X顺序）：

roll  = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
pitch = asin(2*(w*y - z*x))
yaw   = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))

注意，pitch的计算使用asin函数，当输入超出[-1,1]范围时（数值误差导致），会产生NaN。工程上应做钳位处理：pitch = asin(max(-1, min(1, 2*(w*y - z*x))))。

3.3 旋转矩阵：数学严谨但参数冗余

3.3.1 核心原理：方向余弦矩阵（DCM）

旋转矩阵（又称方向余弦矩阵，DCM）是一个3×3的正交矩阵，其列向量表示b系各轴在n系中的方向余弦。任意矢量在b系和n系之间的转换关系为：

v_n = R_nb * v_b

旋转矩阵的9个元素并非独立，它们满足正交约束（R^T R = I）和行列式为1的条件。旋转矩阵的优点是：无奇异性、可直接用于矢量旋转、组合旋转只需矩阵乘法。缺点是参数冗余，且随时间积分时需不断正交化修正，否则矩阵会逐渐偏离正交性。

3.3.2 操作流程：旋转矩阵的更新与正交化

在捷联惯导系统中，旋转矩阵的微分方程为：

dR/dt = R * Ω

其中Ω为陀螺仪角速度的反对称矩阵。数值积分后，矩阵会因积分误差失去正交性，需定期进行正交化修正：

1. 计算误差矩阵：E = R^T * R - I
2. 修正：R_corrected = R * (I - E/2)
3. 重新归一化各列向量。

3.4 四元数：紧凑、无奇异、计算高效

3.4.1 核心原理：超复数与旋转的对应关系

四元数由一个实部和三个虚部组成：q = w + xi + yj + zk，其中w为标量部分，(x,y,z)为矢量部分。单位四元数（模长为1）可以表示三维空间中的旋转。设旋转轴为单位向量(u_x, u_y, u_z)，旋转角为θ，则对应的四元数为：

q = [cos(θ/2), u_x*sin(θ/2), u_y*sin(θ/2), u_z*sin(θ/2)]

四元数具有以下优势：

• 紧凑性：仅4个参数，无冗余。
• 无奇异性：不存在万向锁问题。
• 计算高效：乘法仅需16次乘加运算，比矩阵乘法快。
• 易于插值：球面线性插值（SLERP）可实现平滑姿态过渡。

3.4.2 操作流程：四元数姿态更新与转换

四元数的姿态更新微分方程为：

dq/dt = 0.5 * q ⊗ ω

其中ω为陀螺仪角速度的四元数形式（实部为0），⊗表示四元数乘法。工程上常用一阶龙格-库塔法进行离散积分：

q_new = q + dt * 0.5 * q ⊗ ω

积分后需对四元数进行归一化处理，防止模长漂移。四元数与旋转矩阵的转换关系为：

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²)]]

3.4.3 案例拆解：四元数在姿态解算中的完整闭环

背景：设计一款两轮自平衡机器人，要求姿态更新频率为500Hz，MCU主频168MHz，需在有限算力下实现高精度姿态估计。

方案设计：

1. 陀螺仪输出角速度，转换为四元数增量，进行姿态预测。
2. 加速度计输出比力，通过重力矢量对比，计算姿态误差。
3. 采用互补滤波融合：q_fused = q_gyro * (1-α) + q_acc * α，其中α为权重系数（通常取0.02~0.1）。
4. 输出四元数，同时转换为欧拉角用于显示和控制。

关键参数：陀螺仪零偏稳定性0.01°/s，加速度计噪声密度0.2mg/√Hz，互补滤波系数α=0.05。

结果：静态姿态误差小于0.5°，动态跟随延迟小于10ms，满足平衡控制需求。

复盘：四元数在整个解算过程中避免了欧拉角的奇异问题，且计算量仅为旋转矩阵的1/3左右。在资源受限的嵌入式平台上，四元数是姿态解算的首选表示。

3.5 三种表示方法的对比与工程选型建议

3.5.1 综合对比表

3.5.2 工程选型建议

• 内部状态量：一律使用四元数，避免奇异性和冗余。
• 对外接口：根据需求输出欧拉角（显示）或旋转矩阵（坐标变换）。
• 存储与传输：四元数4个浮点数即可完整表示姿态，适合无线传输和日志记录。
• 多传感器融合：在卡尔曼滤波中，四元数作为状态向量可避免协方差矩阵奇异。

3.5.3 总结复盘：坐标系与姿态表示的工程闭环

本节从IMU数据处理的源头出发，明确了导航坐标系与机体坐标系的定义和转换需求，深入剖析了欧拉角、旋转矩阵、四元数三种姿态表示方法的数学本质与工程特性。核心结论如下：

1. 坐标系是前提：任何IMU数据都必须明确其参考坐标系，否则后续处理毫无意义。
2. 四元数是核心：在姿态解算、滤波、控制等核心算法中，四元数是最优选择。
3. 欧拉角用于交互：仅在需要人机交互或可视化时，将四元数转换为欧拉角。
4. 旋转矩阵用于变换：当需要将矢量从b系转换到n系时，临时构建旋转矩阵。

掌握这些基础概念后，下一阶段将进入IMU误差分析与校准方法的学习，为后续滤波算法设计奠定数据质量基础。