基于地理坐标计算转向角的方法及问题咨询

问题描述 #

给定三个近距离GPS坐标点：

p1 = (p1_lon, p1_lat)
p2 = (p2_lon, p2_lat)
p3 = (p3_lon, p3_lat)

路径定义：p1→p2为线段C，p2→p3为线段A，p1→p3为线段B。疑问点：

1. 转向角是线段C与A的夹角（对向线段B），还是需要用180°（或π弧度）减去该夹角？
2. 用余弦定理计算角度时出现NaN，推测是小距离/小角度场景的精度问题，想了解哈弗辛距离的定义、适用性，以及更稳定的角度计算方法。

一、转向角的正确定义 #

转向角指的是行驶过程中在p2点的实际转向角度，即入向路径的反方向（p2→p1）与出向路径（p2→p3）的夹角，范围为0°~180°。

• 你用余弦定理算出的θ是线段C（p1→p2）和A（p2→p3）的夹角，这个角度和转向角的关系是：转向角 = 180° - θ（当θ≤180°时）。
• 直观例子：若三点共线，C和A的夹角为180°，转向角为0°，符合“直线行驶无转向”的实际情况；若在p2点左转90°，C和A的夹角为90°，转向角就是90°。

二、余弦定理出现NaN的原因 #

核心是浮点精度误差：

• 当三点距离极近或几乎共线时，计算式(B² - A² - C²)/(-2*A*C)的结果可能因为浮点计算误差超出np.arccos()的定义域[-1, 1]（比如出现1.0000000002或-1.0000000002），直接返回NaN。
• 这种问题在小距离场景下尤为明显，因为距离的平方值差异极小，放大了精度误差。

三、哈弗辛距离的定义与适用性 #

• 哈弗辛距离是基于球面几何的大圆距离计算方法，专门用于计算地球表面两点间的最短距离（地球近似为球体），核心公式实现：

  def haversine(lon1, lat1, lon2, lat2):
      R = 6378137.0  # 地球半径（米）
      lon1, lat1, lon2, lat2 = map(np.radians, [lon1, lat1, lon2, lat2])
      dlon = lon2 - lon1
      dlat = lat2 - lat1
      a = np.sin(dlat/2)**2 + np.cos(lat1)*np.cos(lat2)*np.sin(dlon/2)**2
      c = 2*np.arctan2(np.sqrt(a), np.sqrt(1-a))
      return R*c
  

• 但哈弗辛距离只是两点距离的计算方式，和解决转向角的NaN问题没有直接关联——无论用测地距离还是哈弗辛距离，代入余弦定理都可能遇到精度问题。

四、避免NaN的转向角计算方法 #

推荐用向量法结合atan2，鲁棒性更强，不会出现NaN：
因为三点距离近，可以忽略地球曲率，将GPS坐标转换为局部平面坐标系（以p2为原点的东北坐标系），再通过向量的点积、叉积计算角度：

import numpy as np

def calculate_turn_angle(p1_lon, p1_lat, p2_lon, p2_lat, p3_lon, p3_lat):
    R = 6378137.0  # 地球半径（米）
    # 转换为弧度
    p1_lon_rad = np.radians(p1_lon)
    p1_lat_rad = np.radians(p1_lat)
    p2_lon_rad = np.radians(p2_lon)
    p2_lat_rad = np.radians(p2_lat)
    p3_lon_rad = np.radians(p3_lon)
    p3_lat_rad = np.radians(p3_lat)

    # 计算p1相对于p2的东北坐标系坐标（入向向量：p2→p1）
    delta_lon1 = p1_lon_rad - p2_lon_rad
    x_in = R * delta_lon1 * np.cos(p2_lat_rad)
    y_in = R * (p1_lat_rad - p2_lat_rad)

    # 计算p3相对于p2的东北坐标系坐标（出向向量：p2→p3）
    delta_lon3 = p3_lon_rad - p2_lon_rad
    x_out = R * delta_lon3 * np.cos(p2_lat_rad)
    y_out = R * (p3_lat_rad - p2_lat_rad)

    # 计算点积和叉积
    dot_product = x_in * x_out + y_in * y_out
    cross_product = x_in * y_out - y_in * x_out

    # 计算转向角（弧度），范围[-π, π]，取绝对值后转为0~180°
    turn_angle_rad = np.arctan2(cross_product, dot_product)
    turn_angle_deg = np.degrees(np.abs(turn_angle_rad))
    return turn_angle_deg, turn_angle_rad

• 优势：atan2的定义域是全体实数，不会因精度误差返回NaN；同时能直接区分左转、右转（通过cross_product的正负），无需额外计算。

内容的提问来源于stack exchange，提问作者M. Fire