坐标系与地理基础：WGS84、经纬度、高度与本地坐标

做无人机航点规划，第一个绕不开的就是坐标系。说白了，你得告诉飞机「去哪儿」，但怎么描述这个「哪儿」？用经纬度？用米？用相对位置？这里头门道不少。

我刚开始接触 ArduPilot 时，就吃过坐标系的亏。有一次在山区做测绘航线，飞机飞着飞着突然偏离航线，差点撞山。后来一查，是高度基准搞混了——我把气压高度当成了椭球高度。嗯，从那以后，我每次做航点规划前都会先确认坐标系设置。

WGS84 坐标系：全球通用的「地址本」

WGS84 是目前 GPS 使用的全球坐标系。你可以把它理解成一个标准的地球模型——一个椭球体，不是正圆。地球赤道半径约 6378km，极半径约 6357km，差了 21 公里。

ArduPilot 里所有 GPS 数据、航点位置，默认都基于 WGS84。你从 Mission Planner 里看到的经纬度，就是 WGS84 坐标。

经纬度与高度：三个数字定位置

经纬度大家都不陌生。经度范围 -180°~180°，纬度 -90°~90°。但高度这块容易乱——ArduPilot 里涉及三种高度：

高度类型	基准面	ArduPilot 中的用途	常见坑点
椭球高度	WGS84 椭球面	GPS 原始高度	与真实海拔可能差几十米
海拔高度	平均海平面（MSL）	航点高度、地形跟随	需要 EGM96 修正
相对高度	起飞点或 home 点	起飞后默认高度模式	切换 home 点后数值会变

本地坐标系：NED 与 ENU

经纬度适合描述「地球上的点」，但做航点规划时，我们经常需要描述「相对于飞机的位置」。比如「前方 10 米，右方 5 米，下方 2 米」。这时候就需要本地坐标系。

ArduPilot 主要用两种本地坐标系：

• NED（北-东-地）：X 轴指向北，Y 轴指向东，Z 轴指向地心。这是 ArduPilot 内部姿态控制用的坐标系。
• ENU（东-北-天）：X 轴指向东，Y 轴指向北，Z 轴指向天。ROS、PX4 等系统常用。

我个人习惯在航点规划脚本里用 NED 坐标系，因为和 ArduPilot 内部一致，少一次转换。但如果你和 ROS 对接，就得转成 ENU。

坐标转换实战：从经纬度到本地坐标

实际项目中，我们经常需要把 WGS84 经纬度转成本地 NED 坐标。比如你有一个航点列表（经纬度），想计算它们之间的相对距离和方向。

ArduPilot 提供了现成的转换函数，在 AP_Geodesy 库中。下面是一个 Python 示例（使用 pymavlink）：

from pymavlink import mavutil
from math import radians, cos, sin, sqrt, atan2

def wgs84_to_ned(lat1, lon1, alt1, lat2, lon2, alt2):
    """
    将 WGS84 坐标转换为相对于第一个点的 NED 坐标
    返回 (north, east, down) 单位：米
    """
    # 地球参数（WGS84）
    a = 6378137.0  # 赤道半径
    f = 1/298.257223563  # 扁率
    e2 = 2*f - f*f  # 第一偏心率平方
    
    # 将经纬度转为弧度
    lat1_r = radians(lat1)
    lon1_r = radians(lon1)
    lat2_r = radians(lat2)
    lon2_r = radians(lon2)
    
    # 计算卯酉圈曲率半径
    N1 = a / sqrt(1 - e2 * sin(lat1_r)**2)
    N2 = a / sqrt(1 - e2 * sin(lat2_r)**2)
    
    # 地心地固坐标（ECEF）
    x1 = (N1 + alt1) * cos(lat1_r) * cos(lon1_r)
    y1 = (N1 + alt1) * cos(lat1_r) * sin(lon1_r)
    z1 = (N1 * (1 - e2) + alt1) * sin(lat1_r)
    
    x2 = (N2 + alt2) * cos(lat2_r) * cos(lon2_r)
    y2 = (N2 + alt2) * cos(lat2_r) * sin(lon2_r)
    z2 = (N2 * (1 - e2) + alt2) * sin(lat2_r)
    
    # ECEF 差值
    dx = x2 - x1
    dy = y2 - y1
    dz = z2 - z1
    
    # 旋转到 NED（绕 Z 轴转经度，绕 Y 轴转纬度）
    north = -sin(lat1_r)*cos(lon1_r)*dx - sin(lat1_r)*sin(lon1_r)*dy + cos(lat1_r)*dz
    east = -sin(lon1_r)*dx + cos(lon1_r)*dy
    down = -cos(lat1_r)*cos(lon1_r)*dx - cos(lat1_r)*sin(lon1_r)*dy - sin(lat1_r)*dz
    
    return north, east, down

# 示例：从 home 点到第一个航点的 NED 偏移
home_lat, home_lon, home_alt = 40.7128, -74.0060, 10.0  # 纽约
wp_lat, wp_lon, wp_alt = 40.7135, -74.0055, 15.0

n, e, d = wgs84_to_ned(home_lat, home_lon, home_alt, 
                        wp_lat, wp_lon, wp_alt)
print(f"航点相对 home 点：北 {n:.2f}m，东 {e:.2f}m，下 {d:.2f}m")

坐标系转换流程总览

下面这张图总结了从 WGS84 到本地 NED 的完整转换链路，以及 ArduPilot 内部各模块使用的坐标系：

实际项目中的坐标系选择建议

做了几年航点规划，我总结了几条坐标系选择的原则：

• 小范围（半径 < 1km）：直接用本地 NED 坐标，计算简单，精度足够。我一般用 home 点作为原点。
• 中范围（1km ~ 10km）：用 WGS84 经纬度存储航点，计算时转 NED。注意地球曲率影响，距离越远误差越大。
• 大范围（> 10km）：必须用 WGS84，且要考虑大地线计算（测地线），不能用平面近似。

避坑指南：坐标系相关的常见问题

最后分享几个我踩过的坑，你遇到了可以直接对照排查：

1. 高度基准混乱：同一个任务里混用了椭球高度和 MSL 高度。解决办法——统一用相对高度，或者统一用 MSL 并加载 EGM96 修正文件。
2. 经纬度精度丢失：用 float 存经纬度，小数点后 6 位对应约 0.1 米精度。如果存成 double 或字符串，精度更高。我在 Python 脚本里统一用 Decimal 或 float64。
3. NED 与 ENU 搞反：Z 轴方向相反。从 ROS 拿数据时，记得把 Z 取反。我写过一个转换函数，专门处理这个。
4. home 点漂移：飞机起飞后 home 点更新，导致相对坐标变化。解决办法——在航点规划时锁定 home 点，或者用绝对坐标。