2. 坐标系基础
做无人机航点任务链，第一个绕不开的坎就是坐标系。我见过不少新手，代码写得挺漂亮，结果飞机一飞就偏到隔壁山头去了——十有八九是坐标系搞混了。
说白了，坐标系就是给地球上的点贴标签。标签贴错了，导航就乱了。今天咱们把 WGS84、ENU 和它们之间的转换彻底讲透。
WGS84：地球的“全球身份证”
WGS84 全称 World Geodetic System 1984。它是目前 GPS 使用的标准坐标系。你手机里拿到的经纬度，就是 WGS84 坐标。
它的核心参数就三个：
- 经度（Longitude）：东西方向，范围 -180° ~ 180°
- 纬度（Latitude）：南北方向，范围 -90° ~ 90°
- 海拔高度（Altitude）：相对于 WGS84 椭球面的高度，单位米
注意，这里的高度是椭球高，不是海拔高。我在项目中遇到过，有人直接用 GPS 高度当海拔，结果飞机在山区飞的时候，离地面忽高忽低。嗯，这里要区分一下：椭球高 ≠ 海拔高，中间差一个大地水准面起伏。
关键点：WGS84 是一个地心地固坐标系（ECEF），原点在地球质心，Z 轴指向北极，X 轴指向本初子午线与赤道的交点。它描述的是地球上的绝对位置。
ENU：局部坐标系，飞手的好朋友
ENU 全称 East-North-Up。它是一个局部直角坐标系，原点通常设在起飞点或某个参考点。
- E 轴（东）：指向正东方向
- N 轴（北）：指向正北方向
- U 轴（天）：指向天空，垂直于当地水平面
为什么需要 ENU？你想想看，WGS84 坐标是球面上的角度，计算两点之间的距离、方向，得用球面三角公式，麻烦得很。ENU 是平面直角坐标系，加减乘除就能算，多省事。
我个人习惯，所有航点任务链的路径规划、避障计算，都在 ENU 坐标系里做。只在最后下发指令时，才转回 WGS84 给飞控。
经验之谈：ENU 坐标系的原点选在起飞点最方便。这样飞机起飞时坐标就是 (0, 0, 0)，后续所有航点都是相对于起飞点的偏移量。调试时一眼就能看出位置对不对。
坐标系转换：从球面到平面，再回来
转换的核心思路分两步：
- WGS84 → ECEF：把经纬高转成地心地固直角坐标
- ECEF → ENU：把地心坐标旋转平移成局部坐标
反过来也一样，ENU → ECEF → WGS84。
我画了一张流程图，帮你理清关系：
代码实现：Python 版转换工具
理论讲完了，上代码。这是我项目里一直在用的转换函数，经过多次飞行验证，精度够用。
import math
def wgs84_to_ecef(lat, lon, alt):
"""WGS84 转 ECEF"""
a = 6378137.0 # 地球长半轴
f = 1 / 298.257223563 # 扁率
e2 = 2*f - f*f # 第一偏心率平方
lat_rad = math.radians(lat)
lon_rad = math.radians(lon)
N = a / math.sqrt(1 - e2 * math.sin(lat_rad)**2)
X = (N + alt) * math.cos(lat_rad) * math.cos(lon_rad)
Y = (N + alt) * math.cos(lat_rad) * math.sin(lon_rad)
Z = (N * (1 - e2) + alt) * math.sin(lat_rad)
return X, Y, Z
def ecef_to_enu(X, Y, Z, lat0, lon0, alt0):
"""ECEF 转 ENU，参考点 (lat0, lon0, alt0)"""
X0, Y0, Z0 = wgs84_to_ecef(lat0, lon0, alt0)
dx = X - X0
dy = Y - Y0
dz = Z - Z0
lat0_rad = math.radians(lat0)
lon0_rad = math.radians(lon0)
# 旋转矩阵
E = -math.sin(lon0_rad) * dx + math.cos(lon0_rad) * dy
N = (-math.sin(lat0_rad) * math.cos(lon0_rad) * dx
- math.sin(lat0_rad) * math.sin(lon0_rad) * dy
+ math.cos(lat0_rad) * dz)
U = (math.cos(lat0_rad) * math.cos(lon0_rad) * dx
+ math.cos(lat0_rad) * math.sin(lon0_rad) * dy
+ math.sin(lat0_rad) * dz)
return E, N, U
def enu_to_wgs84(E, N, U, lat0, lon0, alt0):
"""ENU 转 WGS84"""
lat0_rad = math.radians(lat0)
lon0_rad = math.radians(lon0)
# 逆旋转矩阵
X = (-math.sin(lon0_rad) * E
- math.sin(lat0_rad) * math.cos(lon0_rad) * N
+ math.cos(lat0_rad) * math.cos(lon0_rad) * U) + wgs84_to_ecef(lat0, lon0, alt0)[0]
Y = (math.cos(lon0_rad) * E
- math.sin(lat0_rad) * math.sin(lon0_rad) * N
+ math.cos(lat0_rad) * math.sin(lon0_rad) * U) + wgs84_to_ecef(lat0, lon0, alt0)[1]
Z = (math.cos(lat0_rad) * N
+ math.sin(lat0_rad) * U) + wgs84_to_ecef(lat0, lon0, alt0)[2]
# ECEF 转 WGS84（迭代法）
a = 6378137.0
f = 1 / 298.257223563
e2 = 2*f - f*f
p = math.sqrt(X**2 + Y**2)
theta = math.atan2(Z * a, p * (a * (1 - f)))
lat = math.atan2(Z + e2 * a * math.sin(theta)**3,
p - e2 * a * math.cos(theta)**3)
lon = math.atan2(Y, X)
alt = p / math.cos(lat) - a / math.sqrt(1 - e2 * math.sin(lat)**2)
return math.degrees(lat), math.degrees(lon), alt
使用建议：把参考点 (lat0, lon0, alt0) 设为起飞点。起飞前记录一次，后续所有航点都用这个参考点做转换。我习惯在任务开始时调用一次 set_reference()，后面只管 ENU 坐标。
常见坐标系对比
| 坐标系 | 类型 | 原点 | 单位 | 适用场景 |
|---|---|---|---|---|
| WGS84 | 球面坐标 | 地球质心 | 度、米 | GPS 定位、航点存储 |
| ECEF | 直角坐标 | 地球质心 | 米 | 中间转换桥梁 |
| ENU | 直角坐标 | 局部参考点 | 米 | 路径规划、避障计算 |
| NED | 直角坐标 | 局部参考点 | 米 | 飞控姿态控制 |
NED 是北东地坐标系，飞控内部常用。不过做航点任务链，ENU 更直观——向上是正，符合直觉。
避坑指南：我曾经在转换时忘记把经纬度转成弧度，结果算出来的 ENU 坐标差了十万八千里。还有一次，参考点选错了，飞机起飞后直接往反方向飞。记住：参考点必须和起飞点一致，转换前先检查单位。
精度问题：什么时候需要高精度？
普通消费级无人机，GPS 精度在 2~5 米。用上面的公式足够了。但如果你做测绘、巡检这类高精度任务，需要考虑：
- RTK 定位：厘米级精度，转换公式不变，但输入数据更准
- 大地水准面模型：椭球高转海拔高，需要 EGM2008 等模型
- 地球曲率：航点距离超过 10 公里时，ENU 的平面近似误差会变大
我个人习惯，10 公里以内的任务，直接用上面的代码。超过 10 公里，我会分段设置参考点，或者改用 UTM 投影。
好了，坐标系这块就讲这么多。记住一句话：WGS84 存数据，ENU 做计算。转换代码写一次，后面所有航点任务都能复用。