📚 课程目录第3章坐标系与姿态表示

3、坐标系与姿态表示：地球坐标系、机体坐标系、欧拉角、方向余弦矩阵、四元数及其转换关系

姿态控制是无人机飞控算法的核心，而姿态描述则是姿态控制的第一块基石。在工程实践中，我们通常需要在地球坐标系（导航系）与机体坐标系（机体系）之间反复切换，以描述无人机的朝向、角速度和加速度。本章将系统梳理这两大坐标系的定义、欧拉角、方向余弦矩阵（DCM）与四元数三种姿态表示方法，并重点讲解它们之间的转换关系与工程选型建议。理解本章内容，是后续进行姿态解算、控制器设计与传感器融合的前提。

在飞控系统中，姿态描述直接决定了控制精度与稳定性。欧拉角直观易懂，但存在万向节死锁问题；方向余弦矩阵无奇异，但参数冗余；四元数则兼顾了计算效率与数值稳定性。下面我们逐一展开。

3.1 地球坐标系与机体坐标系的定义

核心原理：地球坐标系（导航坐标系，n系）通常采用北-东-地（NED）右手直角坐标系，原点位于无人机重心在地面的投影点，X轴指向地理北，Y轴指向地理东，Z轴垂直向下指向地心。机体坐标系（b系）固连于无人机机体，原点位于重心，X轴指向机头前方，Y轴指向右翼，Z轴垂直机体向下。两个坐标系之间的相对旋转关系，就是无人机的姿态。

判断方法：在工程中，我们通过惯性测量单元（IMU）中的加速度计和陀螺仪来感知机体坐标系下的物理量，而导航、路径规划则在NED系下进行。因此，所有传感器数据融合与控制律设计，都离不开这两个坐标系之间的转换。

坐标系	原点	X轴	Y轴	Z轴	主要用途
地球坐标系（n系）	地面固定点	北	东	地	导航、定位、路径规划
机体坐标系（b系）	无人机重心	机头前方	右翼	机体下方	IMU测量、姿态控制

3.2 欧拉角：直观但存在死锁

核心原理：欧拉角通过三次绕不同轴的旋转来描述姿态，常见的Z-Y-X顺序依次为偏航角（Yaw，绕Z轴）、俯仰角（Pitch，绕Y轴）、横滚角（Roll，绕X轴）。其物理意义明确，非常适合人机交互和调试显示。

操作流程：从n系到b系的旋转矩阵可由三个基本旋转矩阵连乘得到：
R = Rz(Yaw) * Ry(Pitch) * Rx(Roll)。需要注意的是，旋转顺序不可交换，工程中必须约定统一标准。

风险提示：当俯仰角接近±90°时，偏航与横滚的旋转轴重合，导致自由度丢失，即万向节死锁。此时欧拉角微分方程出现奇异，无法唯一确定姿态。因此，欧拉角仅适用于水平姿态变化较小的场景（如固定翼平飞），不适用于全姿态飞行（如垂直起降、翻滚特技）。

3.3 方向余弦矩阵（DCM）：无奇异的坐标变换利器

核心原理：DCM是一个3×3的正交矩阵，其每一列表示机体坐标系的单位矢量在导航坐标系下的投影。它完整描述了任意姿态，不存在奇异点。DCM的微分方程为：dC/dt = C * Ω，其中Ω为机体角速度的反对称矩阵。

判断方法：当需要频繁进行矢量坐标变换（如将重力加速度从n系转换到b系）时，DCM是最直接的工具。但DCM有9个参数，存在6个约束条件，直接积分会导致数值漂移，需要频繁正交化修正。

工程经验：在早期的飞控中，DCM算法（如Mahony互补滤波的DCM实现）曾广泛使用。其优势在于无需三角函数运算，适合嵌入式平台。但现代飞控更倾向于使用四元数，因为四元数仅需4个参数，且归一化约束更简单。

3.4 四元数：高效且稳定的姿态表示

核心原理：四元数由一个实部和三个虚部组成：q = q0 + q1*i + q2*j + q3*k，其中q0为标量部分，表示旋转角的一半的余弦，虚部表示旋转轴的方向。单位四元数（模长为1）可以唯一表示三维空间中的旋转。其微分方程为：dq/dt = 0.5 * q ⊗ ω，其中ω为机体角速度四元数（实部为0）。

操作流程：四元数到DCM的转换公式为：

R = [
  1-2(q2²+q3²),   2(q1q2 - q0q3),   2(q1q3 + q0q2);
  2(q1q2 + q0q3), 1-2(q1²+q3²),     2(q2q3 - q0q1);
  2(q1q3 - q0q2), 2(q2q3 + q0q1),   1-2(q1²+q2²)
]

从DCM提取欧拉角时，可使用Yaw = atan2(R[1][0], R[0][0])，Pitch = asin(-R[2][0])，Roll = atan2(R[2][1], R[2][2])。

3.5 案例拆解：四元数在姿态解算中的实战应用

场景：某四旋翼无人机在悬停时受到阵风扰动，飞控需要实时解算姿态并输出控制量。初始姿态为水平（欧拉角全为0），陀螺仪测得机体角速度（单位rad/s）为：p=0.1, q=-0.05, r=0.02。

分析过程：若采用欧拉角法，需对三个角度分别积分，且存在死锁风险；若采用DCM，需维护9个参数并做正交化。我们选择四元数法：初始四元数为q=[1,0,0,0]。根据微分方程，在Δt=0.01s内更新：

ω = [0, p, q, r] = [0, 0.1, -0.05, 0.02]
dq/dt = 0.5 * q ⊗ ω = 0.5 * [1,0,0,0] ⊗ [0,0.1,-0.05,0.02]
      = 0.5 * [0, 0.1, -0.05, 0.02]
q_new = q + dq/dt * Δt = [1, 0.0005, -0.00025, 0.0001]

归一化后得到新的四元数。随后将其转换为欧拉角用于地面站显示：Pitch ≈ -0.0286°，Roll ≈ 0.0573°，Yaw ≈ 0.0115°。整个过程无奇异，计算量小。

结果对比：若使用欧拉角法，在俯仰角接近90°时会出现数值发散；而四元数法在任意姿态下均保持稳定。该案例验证了四元数在工程中的优越性。

复盘经验：在实际飞控中，我们通常将四元数作为姿态解算的核心，仅在需要显示或与外部设备通信时转换为欧拉角。同时，需定期对四元数进行归一化处理，以消除累积误差。

3.6 常见误区与总结复盘

误区一：混淆旋转顺序。不同旋转顺序（如Z-Y-X与Z-X-Y）会产生不同的欧拉角，必须统一标准。
误区二：忽视万向节死锁。在垂直起降或特技飞行中，欧拉角法会失效，应使用四元数或DCM。
误区三：四元数未归一化。非单位四元数会导致旋转矩阵非正交，进而引起姿态漂移。
误区四：DCM正交化过于频繁。过度修正会引入额外噪声，应结合传感器噪声特性合理设置修正频率。

总结：三种姿态表示各有优劣：欧拉角直观但存在死锁，DCM无奇异但参数冗余，四元数高效且稳定。在飞控工程中，推荐以四元数作为核心姿态表示，DCM用于坐标变换，欧拉角用于人机交互。掌握它们之间的转换关系，是设计鲁棒姿态控制器的关键基础。