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

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

在电动垂直起降（eVTOL）飞控系统的算法架构中，坐标系定义与姿态表示方法是所有导航、制导与控制（GNC）逻辑的数学基石。无论是加速度计的比力方程、陀螺仪的角速度积分，还是控制律中的姿态误差计算，都依赖于对“物体在空间中如何描述”这一根本问题的精确回答。本章将深入剖析地球坐标系与机体坐标系的物理含义，系统对比欧拉角、旋转矩阵与四元数三种主流姿态表示方法的数学本质、工程适用场景及相互转换关系，并结合飞控代码中的典型实现进行拆解。

3.1 坐标系定义与物理基准

3.1.1 地球坐标系（NED）

地球坐标系采用北-东-地（North-East-Down, NED）正交右手系，原点通常定义在飞行器起飞点或地面站位置。X轴指向地理北极，Y轴指向正东，Z轴垂直于当地水平面指向地心。该坐标系是惯性导航解算的基准框架，GPS/RTK提供的经纬高数据需经高斯-克吕格投影或通用横轴墨卡托（UTM）投影转换为NED下的平面坐标。在eVTOL城市空中交通（UAM）场景中，NED系还承担着空域管理、航线规划与障碍物规避的空间参考职责。

  工程要点：NED系与机体坐标系之间的转换矩阵是导航解算的核心。飞控计算机以100-500Hz的频率执行姿态更新，每次更新均需将机体坐标系下的加速度计测量值通过旋转矩阵变换至NED系，进而完成速度与位置积分。该过程的数值精度直接决定悬停精度与航线跟踪误差。

3.1.2 机体坐标系（Body Frame）

机体坐标系固连于eVTOL机体，原点位于飞行器重心（CG），X轴指向机头方向，Y轴指向右翼，Z轴垂直于机体平面向下。该坐标系是惯性测量单元（IMU）的直接测量基准：三轴加速度计测量比力、三轴陀螺仪测量角速度均在机体坐标系下表达。对于多旋翼构型eVTOL，电机推力方向通常沿机体Z轴负方向；对于倾转旋翼构型，还需额外定义电机倾转轴与机体轴的几何关系。

特性	地球坐标系（NED）	机体坐标系（Body）
原点	起飞点/地面站	飞行器重心
X轴	地理北极	机头方向
Y轴	正东	右翼方向
Z轴	垂直向下（指向地心）	机体垂直向下
主要用途	导航解算、航迹规划	IMU测量、控制律执行
测量传感器	GPS/RTK、气压计	加速度计、陀螺仪

3.2 姿态表示方法数学原理

3.2.1 欧拉角表示

欧拉角通过三次连续旋转描述机体坐标系相对于NED系的姿态，常用Z-Y-X（偏航-俯仰-横滚）顺序。偏航角ψ绕Z轴旋转，俯仰角θ绕Y轴旋转，横滚角φ绕X轴旋转。欧拉角的物理意义直观，适合人机交互显示与初始姿态设定。然而，当俯仰角接近±90°时，偏航与横滚旋转轴重合，导致旋转自由度丢失，即万向节锁（Gimbal Lock）现象。在eVTOL垂直起降阶段，若飞行器出现大俯仰姿态（如过渡飞行中的倾转机动），欧拉角表示将产生数值奇异。

// 欧拉角到旋转矩阵（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φ]

3.2.2 旋转矩阵

旋转矩阵R是一个3×3正交矩阵，满足R^TR=I且det(R)=+1。它将机体坐标系下的向量变换至NED系：v_NED = R · v_Body。旋转矩阵的优点是运算精度高、无奇异问题，适合连续旋转的复合计算。但其包含9个元素，存在6个冗余约束，直接用于状态估计时计算量较大，且数值漂移后难以直观修正。

3.2.3 四元数表示

四元数q = [w, x, y, z]^T（或标量+矢量部分）是一种超复数表示，满足||q||=1。它用4个参数描述三维旋转，避免了欧拉角的奇异问题，且比旋转矩阵更紧凑。四元数乘法对应旋转复合，计算效率高，是飞控系统中姿态解算的主流选择。单位四元数与旋转矩阵的转换关系为：

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.3 转换关系与工程实现

3.3.1 欧拉角 ↔ 四元数

从欧拉角（Z-Y-X顺序）到四元数的转换公式为：

w = cos(φ/2)cos(θ/2)cos(ψ/2) + sin(φ/2)sin(θ/2)sin(ψ/2)
x = sin(φ/2)cos(θ/2)cos(ψ/2) - cos(φ/2)sin(θ/2)sin(ψ/2)
y = cos(φ/2)sin(θ/2)cos(ψ/2) + sin(φ/2)cos(θ/2)sin(ψ/2)
z = cos(φ/2)cos(θ/2)sin(ψ/2) - sin(φ/2)sin(θ/2)cos(ψ/2)

反向转换时需注意象限判断，工程中常使用atan2函数保证角度范围在(-π, π]内。

3.3.2 四元数姿态更新

飞控系统中，陀螺仪输出的角速度ω用于四元数微分方程更新：

dq/dt = 0.5 * q ⊗ [0, ωx, ωy, ωz]
其中⊗表示四元数乘法。离散化后采用一阶或二阶龙格-库塔法积分。

数值稳定性风险：四元数积分后必须进行归一化处理（q = q/||q||），否则模长漂移将导致旋转矩阵失去正交性，进而引发姿态解算发散。建议在每个控制周期（如1ms）内执行归一化，并监控模长偏差是否超过1e-6阈值。

3.4 案例拆解：eVTOL过渡飞行中的姿态表示切换

背景与约束：某复合翼eVTOL在从垂直起降向固定翼巡航过渡时，俯仰角需从90°逐渐减小至5°。飞控系统采用PX4开源软件栈，姿态控制器内部使用四元数运算，但地面站显示与日志记录需输出欧拉角。

初始现象：在过渡机动中，当俯仰角穿越85°-95°区间时，日志中欧拉角出现跳变，偏航角瞬间翻转180°，导致地面站姿态显示异常，但飞行器实际控制正常。

分析判断：该现象是典型的万向节锁问题。当θ接近90°时，欧拉角转换公式中的cosθ趋近于0，atan2计算中x与y分量比值不稳定，导致ψ与φ的数值解耦失败。飞控内部四元数运算未受影响，但日志输出层的欧拉角转换未做奇异保护。

实际操作步骤：

在姿态输出模块中增加奇异检测：当|cosθ| < 1e-3时，锁定当前ψ值，仅更新φ与θ。
将日志记录格式改为四元数+欧拉角双通道存储，便于事后分析。
地面站显示端增加欧拉角平滑滤波，抑制跳变毛刺。

结果对比：修复后，过渡飞行全程欧拉角输出连续无跳变，地面站显示与实际姿态一致。该问题暴露了多模块间姿态表示不一致的隐患，后续在代码评审中增加了坐标系与姿态表示规范文档。

经验复盘：在eVTOL飞控开发中，应遵循“内部运算用四元数、人机交互用欧拉角、数学推导用旋转矩阵”的原则。所有接口函数必须明确标注姿态表示类型，并在关键路径上增加断言检查（如四元数模长偏差、旋转矩阵正交性校验）。

3.5 常见误区与总结

误区一：将欧拉角直接用于大角度姿态控制。当俯仰角超过±60°时，欧拉角微分方程中的三角函数项将导致控制增益非线性畸变。
误区二：忽略旋转矩阵的正交化修正。长时间运行后，数值积分误差会使R^TR ≠ I，需定期执行施密特正交化或采用四元数替代。
误区三：混淆NED系与ENU系（东-北-天）。若传感器数据或外部接口采用ENU约定，而飞控内部使用NED，将导致位置与速度解算出现系统性偏差。
误区四：四元数更新步长过大。在高速旋转场景（如eVTOL桨叶失效后的应急机动）中，过大的积分步长将引入不可忽略的旋转近似误差。

总结而言，坐标系与姿态表示是eVTOL飞控算法的基础设施。工程师需深刻理解三种表示方法的数学本质与适用边界，在工程实现中建立统一的姿态数据流规范。下一阶段将基于本章的姿态框架，进一步推导动力学方程与控制律设计，但在此之前，务必确保姿态解算模块的数值鲁棒性与跨平台一致性。