无人机 —— dogfight —— 经纬度与笛卡尔坐标系的相互转换、方位角的计算

给出代码：

import math

def latlon_to_xy(lat, lon, lat0, lon0, radius=6371000.0):
    """
    将经纬度坐标转换为局部平面笛卡尔坐标 (ENU 东北天坐标系)
    
    参数:
        lat, lon: 目标点的纬度、经度 (度)
        lat0, lon0: 原点经纬度 (度)
        radius: 地球半径，默认 6371000 米
    
    返回:
        x: 东方向 (East)  坐标，米
        y: 北方向 (North) 坐标，米
    """
    # 角度转弧度
    lat_rad = math.radians(lat)
    lon_rad = math.radians(lon)
    lat0_rad = math.radians(lat0)
    lon0_rad = math.radians(lon0)

    dlat = lat_rad - lat0_rad
    dlon = lon_rad - lon0_rad

    # 局部平面近似（小范围非常准）
    x = radius * dlon * math.cos(lat0_rad)   # 东
    y = radius * dlat                        # 北

    return x, y

# 原点（比如你的起飞点/仿真原点）
lat0 = 39.9139
lon0 = 116.3588

# 目标点经纬度
lat = 39.9145
lon = 116.3593

# 转换为平面坐标 (x东, y北)
# 目标点：(lat, lon)           原点:(lat0, lon0)
# 以(lat0, lon0)为原点，给出目标点(lat, lon)转换为笛卡尔坐标系下的坐标
x, y = latlon_to_xy(lat, lon, lat0, lon0)

print(f"经纬度({lat}, {lon})转换后的平面坐标 x={x:.2f} m, y={y:.2f} m")




import math

def xy_to_latlon(x, y, lat0, lon0, earth_radius=6371000.0):
    """
    局部平面坐标 (相对笛卡尔坐标) (东北天坐标系 ENU) 转回经纬度
    
    参数：
        x: 东向距离（米）
        y: 北向距离（米）
        lat0, lon0: 原点经纬度（度）
    返回：
        lat, lon: 目标点经纬度（度）
    """
    lat0_rad = math.radians(lat0)
    lon0_rad = math.radians(lon0)

    # 纬度变化 = 北向距离 / 地球半径
    dlat = y / earth_radius
    lat_rad = lat0_rad + dlat

    # 经度变化 = 东向距离 / (地球半径 * cos(纬度))
    dlon = x / (earth_radius * math.cos(lat0_rad))
    lon_rad = lon0_rad + dlon

    lat = math.degrees(lat_rad)
    lon = math.degrees(lon_rad)

    return lat, lon


lat, lon = xy_to_latlon(x, y, lat0, lon0)

print(f"平面坐标 x={x:.2f} m, y={y:.2f} m 转换后的经纬度({lat}, {lon})")



def calculate_bearing(lat1, lon1, lat2, lon2):
    """
    计算(lat1,lon1)到(lat2,lon2)的方位角并归一化到[-180,180)

    参数:
        lat1,lon1: 点1的经纬度
        lat2,lon2: 点2的经纬度
    返回:
        点1到点2的方位角，0表示点2在点1正北方，90正东，-90正西，-180正南

    
    输入：起点经纬度、目标点经纬度（度）
    输出：从起点指向目标的方位角，单位度
    0° = 正北
    90° = 正东
    -90° = 正西
    180° /-180° = 正南
    范围严格：[-180, 180)，非常适合做强化学习的角度差奖励
    """
    # 转弧度
    lat1 = math.radians(lat1)
    lon1 = math.radians(lon1)
    lat2 = math.radians(lat2)
    lon2 = math.radians(lon2)

    delta_lon = lon2 - lon1

    x = math.sin(delta_lon) * math.cos(lat2)
    y = (math.cos(lat1) * math.sin(lat2) -
            math.sin(lat1) * math.cos(lat2) * math.cos(delta_lon))

    bearing = math.degrees(math.atan2(x, y))
    bearing = (bearing + 180) % 360 - 180
    return bearing


import math

def bearing_from_latlon(lat1, lon1, lat2, lon2):
    """
    计算从点1 (lat1, lon1) 指向 点2 (lat2, lon2) 的方位角（真北顺时针）
    返回角度已归一化到 [-180, 180) 度

    输入：起点经纬度、目标点经纬度（度）
    输出：从起点指向目标的方位角，单位度
    0° = 正北
    90° = 正东
    -90° = 正西
    180° /-180° = 正南
    范围严格：[-180, 180)，非常适合做强化学习的角度差奖励
    """
    lat1 = math.radians(lat1)
    lon1 = math.radians(lon1)
    lat2 = math.radians(lat2)
    lon2 = math.radians(lon2)

    dlon = lon2 - lon1

    # 方位角公式
    y = math.sin(dlon) * math.cos(lat2)
    x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
    theta = math.atan2(y, x)

    # 弧度转度，并转到 [0, 360)
    bearing = math.degrees(theta)
    bearing = bearing % 360.0

    # 归一化到 [-180, 180)
    if bearing > 180.0:
        bearing -= 360.0

    return bearing

# 起点
lat1, lon1 = 30.0, 120.0
# 终点
lat2, lon2 = 30.001, 120.001

angle = calculate_bearing(lat1, lon1, lat2, lon2)
print(f"方位角: {angle:.2f} °")

angle = bearing_from_latlon(lat1, lon1, lat2, lon2)
print(f"方位角: {angle:.2f} °")

经纬度(39.9145, 116.3593)转换后的平面坐标 x=42.64 m, y=66.72 m
平面坐标 x=42.64 m, y=66.72 m 转换后的经纬度(39.9145, 116.35929999999999)
方位角: 40.89 °
方位角: 40.89 °