坐标系基础：地理坐标系（WGS84）、局部坐标系（ENU）、机体坐标系、坐标系转换原理

做无人机航点规划，第一个绕不开的坎就是坐标系。我见过不少新手，代码写得挺漂亮，结果飞机一上天就乱飞——十有八九是坐标系搞混了。说白了，坐标系就是给飞机一个「我在哪、要去哪」的数学框架。今天咱们把这几个坐标系彻底捋清楚。

1. 地理坐标系（WGS84）——地球的“全球地址”

WGS84 是目前全球卫星导航系统（GPS、北斗、GLONASS）通用的坐标基准。它用经度、纬度和海拔高度来描述地球上任意一点的位置。你手机里那个定位图标，背后就是 WGS84。

我记得刚入行时，总觉得 WGS84 就是经纬度，很简单。直到有一次在极地附近做测试，发现经度线收敛导致定位跳变，才意识到这东西没那么简单。

2. 局部坐标系（ENU）——以你为中心的“东北天”

ENU 坐标系，全称 East-North-Up，就是以东为 X 轴、北为 Y 轴、天为 Z 轴的右手坐标系。它把地球表面的一小块区域近似为平面，非常适合做局部路径规划。

为什么需要 ENU？因为 WGS84 是球面坐标，计算距离和角度非常麻烦。而 ENU 是直角坐标，加减乘除直接算，简单粗暴。你想想看，让飞机飞 100 米向东，用 ENU 就是 (100, 0, 0)，多清爽。

我在项目中习惯把起飞点设为 ENU 原点。这样所有航点都相对于起飞点描述，调试时一眼就能看出飞机偏了多少。

3. 机体坐标系（Body Frame）——飞机自己的“前后左右”

机体坐标系是固定在飞机上的。通常 X 轴指向机头（前），Y 轴指向右翼（右），Z 轴指向下方（下）。注意，Z 轴朝下是航空界的惯例，和 ENU 的 Z 轴朝上正好相反。

这个坐标系用来描述飞机的姿态（俯仰、横滚、偏航）和机体速度。比如「飞机向右滚转 30 度」，就是在机体坐标系下说的。

4. 坐标系转换原理——把“全球地址”变成“本地坐标”

坐标系转换，说白了就是数学上的旋转和平移。咱们从最常用的 WGS84 → ENU 转换说起。

4.1 WGS84 → ENU 转换

这个转换分两步：先把 WGS84 转成地心地固坐标系（ECEF），再把 ECEF 转成 ENU。

// 伪代码示例：WGS84 转 ENU
// 输入：纬度 lat, 经度 lon, 高度 alt（WGS84）
// 输出：东向 e, 北向 n, 天向 u（ENU）

// 第一步：WGS84 转 ECEF
x_ECEF = (N + alt) * cos(lat) * cos(lon)
y_ECEF = (N + alt) * cos(lat) * sin(lon)
z_ECEF = (N * (1 - e²) + alt) * sin(lat)

// 第二步：ECEF 转 ENU（以参考点 lat0, lon0, alt0 为原点）
dx = x_ECEF - x_ECEF_ref
dy = y_ECEF - y_ECEF_ref
dz = z_ECEF - z_ECEF_ref

e = -sin(lon0)*dx + cos(lon0)*dy
n = -sin(lat0)*cos(lon0)*dx - sin(lat0)*sin(lon0)*dy + cos(lat0)*dz
u = cos(lat0)*cos(lon0)*dx + cos(lat0)*sin(lon0)*dy + sin(lat0)*dz

嗯，这里要注意：N 是卯酉圈曲率半径，e 是地球偏心率。这些值在 WGS84 标准里是固定的，直接查表用就行。

4.2 ENU ↔ 机体坐标系转换

这个转换靠的是姿态角（偏航、俯仰、横滚）。说白了就是把 ENU 下的向量，绕 Z 轴转偏航角，再绕 Y 轴转俯仰角，最后绕 X 轴转横滚角。

我建议你直接用旋转矩阵，别自己手算欧拉角，容易出万向锁问题。我在一个项目中手写了欧拉角转换，结果在俯仰 90 度时炸了，后来老老实实换了四元数。

5. 知识体系总览

下面这张图把本章的核心逻辑串起来了。你一看就明白：WGS84 是全局定位，ENU 是局部规划，机体坐标系是姿态控制，三者通过转换公式连接。

6. 避坑指南

最后分享几个我踩过的坑，希望能帮你省点时间。

• 海拔问题：WGS84 椭球高 ≠ 真实海拔。做地形跟随任务时，一定要用 EGM96 模型修正。我曾经在青藏高原测试，椭球高和真实海拔差了 60 多米，差点撞山。
• 原点漂移：ENU 原点选在起飞点，但飞机飞远了（比如 10 公里以上），地球曲率就不能忽略了。这时候要么分段规划，要么换回 WGS84 计算。
• 万向锁：用欧拉角做姿态转换时，俯仰角接近 ±90° 会丢失一个自由度。我建议内部统一用四元数，只在日志和调试时转成欧拉角。
• 精度损失：WGS84 转 ENU 时，如果参考点和目标点距离很远，浮点精度会下降。我习惯在每次航点更新时重新计算转换矩阵，而不是用固定的。