3、坐标系与姿态表示：NED与ENU坐标系，欧拉角、四元数与旋转矩阵，坐标系转换实战
做飞控仿真，第一件事不是调PID，而是搞清楚你的飞机到底在哪个坐标系里。
这话听起来有点夸张，但我在项目里见过太多次——仿真里飞得好好的，真机一上天就翻跟头。查到最后，往往是坐标系搞反了。说白了，坐标系就是你和飞机沟通的语言，语言不通，啥都白搭。
3.1 NED与ENU：两个最常见的坐标系
飞控领域有两个主流坐标系：NED（北-东-地）和ENU（东-北-天）。
你想想看，一个指向地面，一个指向天空，正好相反。这就是很多问题的根源。
| 坐标系 | X轴 | Y轴 | Z轴 | 常见场景 |
|---|---|---|---|---|
| NED | 北 | 东 | 地（向下） | PX4、ArduPilot、无人机导航 |
| ENU | 东 | 北 | 天（向上） | ROS、Gazebo、GPS坐标 |
核心要点：仿真环境（如Gazebo）默认用ENU，而飞控固件（如PX4）内部用NED。你必须在它们之间做转换，否则飞控收到的位置信息是反的。
我记得第一次做PX4+SITL仿真时，飞机一解锁就往地里钻。我当时还以为是推力不够，调了半天油门曲线。后来才发现，是Gazebo发出来的高度数据是ENU的Z轴向上，而PX4期望的是NED的Z轴向下。一个正一个负，飞机自然就"钻地"了。
3.2 欧拉角：直观但危险的姿态表示
欧拉角是最直观的姿态表示方式——横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。每个搞过航模的人都熟悉它。
但我要说一句：欧拉角在仿真里用用可以，真机慎用。
为什么？因为万向锁（Gimbal Lock）。当俯仰角接近±90°时，横滚和偏航会失去一个自由度，姿态解算直接崩掉。
避坑指南：我曾经在仿真里用欧拉角做姿态控制，飞机做大机动时突然失控。查了两天，发现是俯仰角超过85°后，欧拉角微分方程出现奇异。从那以后，我所有涉及大角度的仿真都改用四元数。
欧拉角的转换公式（ZYX顺序）：
// 欧拉角 → 旋转矩阵（ZYX顺序）
R = Rz(yaw) * Ry(pitch) * Rx(roll)
// 其中：
Rx(roll) = [[1, 0, 0],
[0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]]
Ry(pitch) = [[cos(pitch), 0, sin(pitch)],
[0, 1, 0],
[-sin(pitch), 0, cos(pitch)]]
Rz(yaw) = [[cos(yaw), -sin(yaw), 0],
[sin(yaw), cos(yaw), 0],
[0, 0, 1]]3.3 四元数：飞控的"真香"选择
四元数听起来高大上，其实没那么玄乎。它用四个数表示旋转：q = [w, x, y, z]，其中w是实部，x、y、z是虚部。
我个人习惯在仿真里全程用四元数。原因有三：
- 无万向锁——随便你怎么转，不会奇异
- 插值平滑——做轨迹规划时，四元数球面线性插值（SLERP）比欧拉角平滑得多
- 计算高效——乘法比三角函数快
四元数与旋转矩阵的转换：
// 四元数 → 旋转矩阵
// q = [w, x, y, z]
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²)]]
// 旋转矩阵 → 四元数
w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4 * w)
y = (R[0][2] - R[2][0]) / (4 * w)
z = (R[1][0] - R[0][1]) / (4 * w)小技巧：在代码里判断四元数是否合法，可以检查它的模长是否接近1。如果偏差超过1e-6，记得做归一化。我习惯在每个控制周期开始前都做一次归一化，防止数值误差累积。
3.4 坐标系转换实战：从Gazebo到PX4
好了，理论讲完，咱们来点实际的。下面是一个完整的坐标系转换流程，我在PX4+SITL仿真里每天都在用。
场景：Gazebo（ENU）→ PX4（NED）
- 位置转换：ENU的(x, y, z) → NED的(x', y', z')
x' = y, y' = x, z' = -z - 姿态转换：ENU的四元数q_ENU → NED的四元数q_NED
需要左乘一个旋转矩阵，将ENU对齐到NED - 速度转换：同理，交换x/y分量，取反z分量
// C++示例：ENU → NED 转换
#include <Eigen/Dense>
// 位置转换
Eigen::Vector3d pos_enu = {1.0, 2.0, 3.0}; // Gazebo位置
Eigen::Vector3d pos_ned;
pos_ned.x() = pos_enu.y(); // 北 ← 东
pos_ned.y() = pos_enu.x(); // 东 ← 北
pos_ned.z() = -pos_enu.z(); // 地 ← -天
// 姿态转换（四元数）
Eigen::Quaterniond q_enu(1.0, 0.0, 0.0, 0.0); // Gazebo姿态
// ENU→NED的旋转：绕Z轴转-90°，使X从东转到北
Eigen::Quaterniond q_enu2ned(
Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ()));
Eigen::Quaterniond q_ned = q_enu2ned * q_enu;关键点：转换顺序不能错。先转位置，再转姿态。如果先转姿态再转位置，你会得到完全错误的飞行轨迹。我刚开始写这个转换时搞反了顺序，飞机在仿真里画了个8字形，还以为是算法问题……
3.5 知识体系总览
下面这张图总结了本章的核心逻辑，我建议你保存下来，做仿真时对照着看。
3.6 本章小结
嗯，到这里坐标系和姿态表示就讲完了。我帮你理一下重点：
- NED vs ENU：记住Z轴方向相反，仿真和真机之间必须做转换
- 欧拉角：直观但危险，小角度调试可以用，大机动别碰
- 四元数：飞控的标配，无万向锁，插值平滑，建议全程使用
- 旋转矩阵：连接欧拉角和四元数的桥梁，理解它就能自由切换
- 实战转换：先位置后姿态，顺序不能错