31 lines
1.0 KiB
Python
31 lines
1.0 KiB
Python
import math
|
|
|
|
def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt):
|
|
"""
|
|
将 WGS-84 经纬高 (LLA) 转换为局部 NED 坐标 (x, y, z)
|
|
|
|
Args:
|
|
lat, lon, alt: 目标点的纬度(deg)、经度(deg)、海拔(m)
|
|
ref_lat, ref_lon, ref_alt: 参考点（起飞点）的纬度、经度、海拔
|
|
|
|
Returns:
|
|
(x, y, z): NED 坐标 (米)
|
|
x: 北向距离 (正北为正)
|
|
y: 东向距离 (正东为正)
|
|
z: 向下距离 (向下为正，即向上为负)
|
|
"""
|
|
# WGS-84 椭球参数（赤道半径，单位：米）
|
|
EARTH_RADIUS = 6378137.0
|
|
# 1. 纬度差 -> 北向距离 (x)
|
|
dlat = math.radians(lat - ref_lat)
|
|
x = dlat * EARTH_RADIUS
|
|
|
|
# 2. 经度差 -> 东向距离 (y)，需乘以纬度的余弦值修正
|
|
dlon = math.radians(lon - ref_lon)
|
|
y = dlon * EARTH_RADIUS * math.cos(math.radians(ref_lat))
|
|
|
|
# 3. 高度差 -> NED Z (向下为正)
|
|
# 若目标海拔高于参考点，则 z 为负（向上）
|
|
z = -(alt - ref_alt)
|
|
|
|
return x, y, z |