3. 地理空间基础与坐标系

做无人机航线规划，第一个绕不开的就是坐标系。说白了，你得知道飞机在哪儿，往哪儿飞，姿态怎么样。我见过不少新手，代码写得挺溜，结果飞机一上天就偏航，最后发现是坐标系搞混了。嗯，这节课咱们就把这些基础打牢。

3.1 WGS84 坐标系：全球定位的基准

WGS84，全称 World Geodetic System 1984。你手机里的 GPS、手里的遥控器、飞机上的飞控，用的都是它。它是一个地心地固坐标系，原点在地球质心。

它的核心参数就两个：经纬度。纬度范围 -90° 到 +90°，经度范围 -180° 到 +180°。但要注意，WGS84 不是完美的球体，它是个椭球体。赤道半径约 6378.137 km，极半径约 6356.752 km。

3.2 经纬度与 UTM 互转

经纬度虽然直观，但做距离计算很麻烦。一度经度在赤道是 111 km，到北极就变成 0 了。所以实际规划航线时，我更喜欢用 UTM 坐标。

UTM 把地球分成 60 个带，每个带 6° 宽。坐标单位是米，计算距离、面积非常方便。

互转公式比较复杂，但 ArduPilot 提供了现成的库。我个人习惯用 AP_Geodesy 库里的函数：

// 经纬度转UTM
Location loc(40.7128, -74.0060); // 纽约
float utm_x, utm_y;
char utm_zone;
loc.to_utm(utm_x, utm_y, utm_zone);

// UTM转经纬度
Location loc2;
loc2.from_utm(utm_x, utm_y, utm_zone);

3.3 航向角 / 俯仰角 / 横滚角定义

这三个角定义了飞机的姿态。你想想看，飞机在空中不是平着飞的，它得转弯、爬升、侧倾。

角度	英文	定义	范围
航向角	Yaw	机头与真北的夹角，顺时针为正	0° ~ 360°
俯仰角	Pitch	机头与水平面的夹角，抬头为正	-90° ~ +90°
横滚角	Roll	机体绕纵轴旋转的角度，右倾为正	-180° ~ +180°

我在项目中遇到过一个问题：飞控输出的航向角是 0-360°，但我的算法需要 -180° 到 180°。如果不做转换，航线会突然反向。记得加个 wrap 函数。

3.4 NED 与 ENU 坐标系

这两个是导航常用的局部坐标系。很多新手在这里栽跟头。

• NED：北-东-地。X 轴指向北，Y 轴指向东，Z 轴指向地心。ArduPilot 的 EKF 和惯性导航用的就是 NED。
• ENU：东-北-天。X 轴指向东，Y 轴指向北，Z 轴指向天。ROS、PX4 和一些地面站软件喜欢用 ENU。

转换其实很简单，就是旋转和取反：

// NED 转 ENU
enu_x = ned_y;
enu_y = ned_x;
enu_z = -ned_z;

3.5 地磁偏角与真北修正

磁北和真北不是一回事。磁北在地理北极附近，但每年都在漂移。地磁偏角就是磁北与真北的夹角。

ArduPilot 里有个参数 COMPASS_DEC，就是用来设置地磁偏角的。如果你不设，飞控默认用磁北，航线会整体偏一个角度。

真北修正的公式也很简单：

真北航向 = 磁北航向 + 地磁偏角
// 东偏为正，西偏为负

3.6 知识体系总览

下面这张图把本章的核心逻辑串起来了。你从 GPS 拿到 WGS84 经纬度，转成 UTM 做计算，同时要知道飞机的姿态角和坐标系，最后用地磁偏角修正航向。

3.7 实战建议

最后给几个实在的建议：

1. 统一坐标系：整个项目里只用一种局部坐标系，要么 NED 要么 ENU。混用必出 bug。
2. 验证偏角：起飞前，用地面站检查 COMPASS_DEC 是否正确。我习惯在 Mission Planner 的「初始设置」里看一眼。
3. 测试转换：写个简单的测试脚本，把已知经纬度转成 UTM，再转回来，看误差是否在 1 米以内。