3、坐标系与变换：常用坐标系（机体、地面、导航、地球）定义、欧拉角、方向余弦矩阵、坐标变换链

在嵌入式飞控算法中，坐标系是描述飞行器运动状态的语言基础。无论是加速度计输出的比力、陀螺仪测量的角速度，还是GPS给出的经纬度位置，都必须映射到明确的坐标系中才有物理意义。飞控算法中的姿态解算、导航解算、控制律设计，本质上都是在不同坐标系之间反复进行矢量投影与旋转变换。若坐标系定义不清或变换顺序错误，轻则导致控制发散，重则引发飞行事故。本章将系统梳理飞控领域最常用的四类坐标系定义，深入讲解欧拉角与方向余弦矩阵（DCM）的数学本质，并构建一条完整的坐标变换链，帮助读者在工程实践中建立清晰的“坐标系思维”。

上图展示了飞控系统中四类核心坐标系之间的变换关系与姿态描述方法。从地球坐标系到机体坐标系，每一级变换都有明确的物理含义与数学表达。下面我们将逐一展开讲解。

3.1 常用坐标系定义与工程语义

核心原理：坐标系的选择决定了矢量分量的表达形式。在嵌入式飞控中，我们通常使用右手直角坐标系，但不同坐标系的原点与轴向定义差异巨大。地球坐标系（ECEF或ECI）以地心为原点，用于卫星导航与全球定位；导航坐标系（NED或ENU）以飞行器质心在地面的投影为原点，NED（北东地）系在航空领域最为常用，其X轴指向北、Y轴指向东、Z轴指向地心；地面坐标系则固定于起飞点或任务锚点，用于记录航点与返航逻辑；机体坐标系固连于飞行器，原点位于重心，X轴指向机头、Y轴指向右翼、Z轴指向机体下方（NED约定下）。

工程判断方法：在代码实现中，必须明确每个传感器数据所属的坐标系。例如，加速度计输出的是机体坐标系下的比力（Specific Force），而GPS输出的是ECEF或经纬高（LLA）坐标。若将机体系加速度直接当作导航系使用，积分得到的速度和位置将完全错误。建议在数据结构中为每个量显式标注坐标系标签，例如 acc_body、vel_ned、pos_lla。

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

核心原理：欧拉角用三个旋转角度描述刚体姿态，常见顺序为Z-Y-X（偏航-俯仰-横滚），对应飞控中的Yaw（ψ）、Pitch（θ）、Roll（φ）。旋转矩阵由三个基本旋转矩阵连乘得到：R = Rz(ψ) * Ry(θ) * Rx(φ)。欧拉角的物理意义直观，便于调试与显示，但其最大的数学缺陷是万向锁（Gimbal Lock）：当俯仰角接近±90°时，横滚与偏航的旋转轴重合，系统丢失一个自由度，导致姿态解算奇异。

操作流程：在飞控中，欧拉角通常用于人机交互与控制外环，而内部姿态解算应避免使用欧拉角微分方程。若必须使用，需限制俯仰角范围并做奇异保护。从DCM提取欧拉角的公式为：

// 从DCM提取欧拉角 (Z-Y-X顺序)
roll  = atan2(R[2][1], R[2][2]);
pitch = asin(-R[2][0]);
yaw   = atan2(R[1][0], R[0][0]);

案例拆解：某固定翼无人机在做大机动拉起时，俯仰角达到88°，此时若采用欧拉角反馈，横滚通道出现剧烈振荡。分析发现，由于俯仰接近90°，横滚与偏航通道耦合严重，控制器增益无法同时满足两个通道的稳定性。解决方案是将姿态反馈切换为四元数或DCM，并在控制律中采用基于旋转矢量的误差计算，最终消除了振荡。

3.3 方向余弦矩阵（DCM）与坐标变换

核心原理：DCM是一个3×3正交矩阵，其每一列表示目标坐标系各轴在源坐标系中的方向余弦。DCM的转置即为其逆矩阵，这使得坐标变换非常方便：v_ned = R_b_n * v_body，其中R_b_n表示从机体系到导航系的旋转矩阵。DCM的更新方程为R_dot = R * Ω_skew，其中Ω_skew是角速度的反对称矩阵。DCM没有奇异问题，但9个参数存在冗余，长期数值积分会导致非正交性漂移，需要定期正交化修正。

工程实现要点：在嵌入式平台上，DCM更新需要9个浮点乘法，计算量适中。但更常用的是四元数更新（仅4个参数），在需要DCM时再转换。从四元数到DCM的转换公式为：

// 四元数 q = (q0, q1, q2, q3) 转 DCM
R[0][0] = 1 - 2*(q2*q2 + q3*q3);
R[0][1] = 2*(q1*q2 - q0*q3);
R[0][2] = 2*(q1*q3 + q0*q2);
R[1][0] = 2*(q1*q2 + q0*q3);
R[1][1] = 1 - 2*(q1*q1 + q3*q3);
R[1][2] = 2*(q2*q3 - q0*q1);
R[2][0] = 2*(q1*q3 - q0*q2);
R[2][1] = 2*(q2*q3 + q0*q1);
R[2][2] = 1 - 2*(q1*q1 + q2*q2);

实战场景：在多旋翼的加速度计融合中，我们需要将机体系加速度转换到导航系以消除重力影响。假设机体系加速度为a_body = (0, 0, -9.8)（悬停时），通过DCM变换后应得到a_ned = (0, 0, -9.8)。若DCM存在误差，则导航系加速度会出现水平分量，导致速度积分漂移。因此，DCM的精度直接决定了导航解算的长期稳定性。

3.4 坐标变换链的完整构建

核心原理：完整的坐标变换链从地球系到机体系通常包含四级变换：ECEF→导航系（经纬高转换）、导航系→地面系（平移与航向对齐）、地面系→机体系（姿态旋转）。在实际飞控中，我们往往将地面系与导航系合并处理，因为起飞点通常与导航系原点重合。变换链的数学表达为：v_body = R_n_b * R_e_n * v_ecef。

操作流程：构建变换链时，应遵循以下步骤：

1. 明确每个数据源的坐标系标签，建立数据字典。
2. 确定变换顺序，通常为Z-Y-X（偏航-俯仰-横滚）。
3. 编写坐标变换函数库，统一输入输出接口。
4. 在仿真环境中验证变换链的正确性（如使用旋转前后矢量模长不变的性质）。
5. 在硬件在环测试中对比变换前后的数据曲线。

案例拆解：某垂直起降固定翼飞机在过渡飞行阶段（从悬停转为平飞），飞控需要同时使用机体系和导航系数据。初始现象是：当飞机倾斜超过30°时，高度估计出现跳变。分析发现，高度融合算法中使用了气压计和加速度计，但加速度计数据未经过坐标变换，直接将机体系Z轴加速度当作垂直加速度。修正方案是在融合前将加速度计数据通过DCM变换到导航系，并减去重力分量。修改后，高度估计平滑，过渡飞行控制稳定。

3.5 常见误区与工程复盘

误区一：混淆NED与ENU坐标系。在PX4中默认NED，而ROS的REP-103建议使用ENU。若将两者混用，会导致偏航角相差180°，俯仰与横滚符号反转。复盘经验：在项目启动时，应在代码仓库中明确标注坐标系约定，并在接口处添加静态断言。

误区二：欧拉角旋转顺序不统一。不同文献可能使用不同的旋转顺序（如Z-X-Y或Y-Z-X），导致同一组欧拉角对应不同的姿态矩阵。复盘经验：在文档中明确写出旋转顺序，并在代码注释中标注。

误区三：忽略DCM的正交化修正。长时间运行后，DCM的列向量可能不再正交，导致变换后的矢量模长改变。复盘经验：每1000次更新后执行一次Gram-Schmidt正交化，或直接使用四元数避免该问题。

总结：坐标系与变换是飞控算法的基石。工程实践中，建议采用“四元数做姿态更新、DCM做坐标变换、欧拉角做显示与控制接口”的混合策略。每次新增传感器或控制模块时，务必先明确其坐标系定义，再编写变换代码。通过建立完整的坐标变换链并辅以严格的测试，可以大幅减少因坐标系混乱导致的疑难故障。