GPS 坐标计算方向角 & 夹角 & 距离

发表于 2022-05-01 更新于 2022-05-07 分类于 GIS 阅读次数： Disqus：本文字数： 9.1k 阅读时长 ≈ 8 分钟

这是使用GPS坐标来计算距离和方位角的全文转载,附加了夹角计算,及一些 py 代码实现.
资料来源:
https://johnnyqian.net/blog/gps-locator.html
https://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/
https://blog.csdn.net/weixin_30293135/article/details/96275339

更新

1 2	2022.05.01 初始 2022.05.07 添加夹角计算

导语

GPS 坐标计算方向角 & 夹角 & 距离

根据地球上任意两地的经纬度，可以计算它们在球面上的最短距离（Great-circle Distance / Orthodromic Distance）及相对始末位置的方位角（Bearing）。

基本概念

行星的自转使得其呈现“椭球型”：在赤道上凸起，在极点平坦。所以赤道半径比极半径大。

地球的赤道半径a，或长半轴，是从地球中心至赤道的距离，大约为6378.1370km。

地球的极半径b, 或短半轴，是从地球中心至南极或北极的距离，大约为6356.7523km。

在地理学上，椭球体的平均半径计算公式为：

mean_Earth_radius

对于地球而言，平均半径为6371.0088km。

mean_earth_radius

经纬线

经纬线是人类为度量方便而假设出来的辅助线。纬线定义为地球表面某点随地球自转所形成的轨迹。任何一根纬线都是圆形而且两两平行。纬线的长度是赤道的周长乘以纬线的纬度的余弦，所以赤道最长，离赤道越远的纬线，周长越短，到了两极就缩为0。

经线也称子午线，和纬线一样是人类为度量而假设出来的辅助线，定义为地球表面连接南北两极的大圆线上的半圆弧。任两根经线的长度相等，相交于南北两极点。每一根经线都有其相对应的数值，称为经度。经线的长度大约为20003.93km。

有如下的表格：

纬度度量	相差距离
1度	111.13km
1分	1.85km
1秒	30.87m
0.1度（6分）	11.11km
0.01度（36秒）	1111.33m

因此，单从纬度这个角度来看，如果要达到10m左右的误差，纬度坐标需要达到0.0001度这个精度。

计算任意两点间的距离

地理空间距离计算方法较多，目前可以分为两类：

椭球模型，该模型最贴近真实地球，精度也最高，但计算较为复杂
球面模型，这种模型将地球看成一个标准球体，球面上两点之间的距离即为弧长，这种方法使用较广
简化模型，这种模型适用于两点距离较近的情形，可以认为两点在一个二维平面上，并且经纬线相互垂直

下面我们只探讨球面模型和简化模型。

球面模型

earth model

假设地球上有A(ja,wa)，B(jb,wb)两点（ja和jb分别是A和B的经度，wa和wb分别是A和B的纬度），A和B两点的球面距离就是AB的弧长，也叫做大圆距离，AB弧长=R*角AOB（角AOB是A跟B的弧度，O是地球的球心，R是地球半径）。如何求出角AOB呢？可以先求三角形AOB的最大边AB的长度，再根据余弦定律可以求夹角。

1）根据经纬度，以及地球半径R，将A、B两点的经纬度坐标转换成球面坐标

daum_equation_1

2）根据A、B两点的三维坐标求AB长度

daum_equation_2

3）根据余弦定理求出角AOB

daum_equation_3

4）于是得到AB弧长=R*角AOB

daum_equation_4

此公式也可以直接使用三面角的余弦定理（Spherical law of cosines）得到，在下文求解两点间的方位角时，我们也会用到这个余弦定理。

这个公式被称为大圆距离公式，但是这个计算公式有有个比较大的问题 ── 当两点相距较近时，cos(jb-ja)的误差会比较大。那么如何解决这个问题？

Haversine 公式

首先，我们先介绍一个三角函数 versine.

versine

Sine, cosine, and versine of angle θ in terms of a unit circle with radius 1, centered at O.

Versine，中文称之为正矢，在三角函数之中被定义为 versine_formula_1 ，值域在0～2之间。

根据正弦的半角公式：

正矢可以变换为：

由此，我们可以定义正矢的一半，即半正矢（half versine，记为hav）为：

在上述的大圆距离公式的基础上，我们记：

1
2
3

δ=角AOB，即球心角
∆φ=wb−wa
∆λ=jb−ja

有如下的推导过程：

formula derivation

结合上述对半正矢的定义，我们就可以得出如下的Haversine 公式：

haversine formula

Haversine 公式描述了球面三角形的特性，我们这里不展开叙述。

我们记a为：

substitution

由此，我们可以得到：

substitution

进而得到球心角δ为：

substitution

这里atan2是反正切函数arctan的一个变种。

基于值域为的反正切函数，该函数定义如下：

工程上经常使用atan2，目的是确保得到的角度在正确的象限中。

简化模型

简化模型适用于两点距离较近的情形，可以认为两点在一个二维平面上，并且经纬线相互垂直。如图所示，要求A(116.8, 39,78)和B(116.9, 39.68)两点的距离，我们可以先求出南北方向距离AM，然后求出东西方向距离BM，最后求矩形对角线距离。

atan formula

工程实现

下面是计算地球上任意两点间的距离的工程实现，使用C#语言，包括球面模型和简化模型：

公式中所用到的角度都是弧度单位（rad），因此常规的经纬度需要先转换。

public class Distance
{
    const double R = 6371;
    const double PI = Math.PI;

    static Func<double, double> rad = Radians;
    static Func<double, double> sin = Math.Sin;
    static Func<double, double> cos = Math.Cos;
    static Func<double, double> sqrt = Math.Sqrt;
    static Func<double, double, double> atan2 = Math.Atan2;

    /// <summary>
    /// Convert degrees to Radians
    /// </summary>
    /// <param name="x">Degrees</param>
    /// <returns>The equivalent in radians</returns>
    public static double Radians(double x)
    {
        return x * PI / 180;
    }

    /// <summary>
    /// Calculate the Great-circle Distance between two points using Haversine formula.
    /// </summary>
    /// <param name="lat1"></param>
    /// <param name="lon1"></param>
    /// <param name="lat2"></param>
    /// <param name="lon2"></param>
    /// <returns>The distance in kilometers.</returns>
    public static double Complex(double lat1, double lon1, double lat2, double lon2)
    {
        var havLat = sin(rad(lat1 - lat2) / 2);
        var havLon = sin(rad(lon1 - lon2) / 2);

        var a = havLat * havLat + cos(rad(lat1)) * cos(rad(lat2)) * havLon * havLon;

        return 2 * R * atan2(sqrt(a), sqrt(1 - a));
    }

    /// <summary>
    /// Calculate the distance between two points using simplified model.
    /// </summary>
    /// <param name="lat1"></param>
    /// <param name="lon1"></param>
    /// <param name="lat2"></param>
    /// <param name="lon2"></param>
    /// <returns>The distance in kilometers.</returns>
    public static double Simplified(double lat1, double lon1, double lat2, double lon2)
    {
        var avgLat = rad(lat1 + lat2) / 2;
        var disLat = R * cos(avgLat) * rad(lon1 - lon2);
        var disLon = R * rad(lat1 - lat2);

        return sqrt(disLat * disLat + disLon * disLon);
    }
}

球面距离计算 python

输入是通常经纬度

import numpy as np
_R = 6373000  # Earth radius in meters
def get_dis(lat1, lon1, lat2, lon2):
    """ 计算距离
    """
    lat1 = np.deg2rad(lat1)  # type: ignore
    lon1 = np.deg2rad(lon1)  # type: ignore
    lat2 = np.deg2rad(lat2)  # type: ignore
    lon2 = np.deg2rad(lon2)  # type: ignore

    dlat = lat1 - lat2  # type: ignore
    dlon = lon1 - lon2  # type: ignore

    a = np.sin(
        dlat / 2)**2 + np.cos(lat1) * np.cos(lat2) * np.sin(  # type: ignore
            dlon / 2)**2  # type: ignore
    c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a))  # type: ignore

    return _R * c

计算任意两点间的方位角

方位角是从某点的指北经线起，依顺时针方向到目标方向线之间的水平夹角（如图所示θ，可以将其看成是指南针所指示的角度），也即是OPN平面与OPQ平面的所构成的二面角大小。

earth model

以北极点N为顶点，N-PQO构成了一个三面角。

二面角N-PQ-O的大小为θ，其平面角为π/2 - φ2;
二面角p-ON-Q的大小为λ2−λ1，其平面角为δ;

由三面角正弦定理可得：

bearing_equation_1

由三面角余弦定理可得：

bearing_equation_2

由此可得：

bearing_equation_3

结合上述在求解两点间的距离时得到的结果：

bearing_equation_4

可得到：

bearing_equation_5

进而得到方位角：

bearing_equation_6

二面角
从一条直线出发的两个半平面所组成的图形，叫做二面角。这条直线叫做二面角的棱，每个半平面叫做二面角的面。以二面角的公共直线上任意一点为端点，在两个面内分别作垂直于公共直线的两条射线，这两条射线所成的角叫做二面角的平面角。二面角的大小, 可以用它的平面角来度量。

三面角
从一点出发并且不在同一平面内的三条射线，其中每相邻两射线可以决定一个平面，这样的三个平面所围成的立体图形叫做三面角。其中，这三条射线叫做三面角的棱，这些射线的公共端点叫做三面角的顶点，相邻两棱所夹的平面部分叫做三面角的面，在每个面内两条棱所形成的角叫做三面角的面角，过每一条棱的两个面所形成的二面角叫做三面角的二面角。一个三面角可以用它的顶点的字母来表示，例如“三面角S”；或在顶点的字母之后加一短划，并顺次写上每一条棱上的一个字母，例如“三面角S-ABC”。

工程实现

下面是计算地球上任意两点间的方位角的工程实现，使用C#语言：

计算公式适用于地球上任意两点，请注意经纬度分正负值。对于经度来说，东经为正，西经为负；对于纬度来说，北纬为为正，南纬为负。

public class Bearing
{
    const double PI = Math.PI;

    static Func<double, double> rad = Radians;
    static Func<double, double> abs = Math.Abs;
    static Func<double, double> sin = Math.Sin;
    static Func<double, double> cos = Math.Cos;
    static Func<double, double, double> atan2 = Math.Atan2;

    /// <summary>
    /// Convert degrees to radians
    /// </summary>
    /// <param name="x">Degrees</param>
    /// <returns>The equivalent in radians</returns>
    public static double Radians(double x)
    {
        return x * PI / 180;
    }

    /// <summary>
    /// Calculate the bearing between two points using spherical laws(Spherical law of sines and cosines).
    /// </summary>
    /// <param name="lat1"></param>
    /// <param name="lon1"></param>
    /// <param name="lat2"></param>
    /// <param name="lon2"></param>
    /// <returns>The bearing in degrees.</returns>
    public static double Complex(double lat1, double lon1, double lat2, double lon2)
    {
        var numerator = sin(rad(lon2 - lon1)) * cos(rad(lat2));
        var denominator = cos(rad(lat1)) * sin(rad(lat2)) 
                        - sin(rad(lat1)) * cos(rad(lat2)) * cos(rad(lon2 - lon1));

        var x = atan2(abs(numerator), abs(denominator));
        var result = x;

        if (lon2 > lon1) // right quadrant
        {
            if (lat2 > lat1) // first quadrant
                result = x;
            else if (lat2 < lat1) // forth quadrant
                result = PI - x;
            else
                result = PI / 2; // in positive-x axis
        }
        else if (lon2 < lon1) // left quadrant
        {
            if (lat2 > lat1) // second quadrant
                result = 2 * PI - x;
            else if (lat2 < lat1) // third quadrant
                result = PI + x;
            else
                result = PI * 3 / 2; // in negative-x axis
        }
        else // same longitude
        {
            if (lat2 > lat1) // in positive-y axis
                result = 0;
            else if (lat2 < lat1)
                result = PI; // in negative-y axis
            else
                throw new Exception("Shouldn't be same location!");
        }

        return result * 180 / PI;
    }
}

二面角的 python 实现,输入通常经纬度.

def get_heading(lat1, lon1, lat2, lon2):
    """ 计算方向角,直观理解为指南针的角度.
        相关资料参考: https://johnnyqian.net/blog/gps-locator.html
    """
    lat1 = np.deg2rad(lat1)  # type: ignore
    lon1 = np.deg2rad(lon1)  # type: ignore
    lat2 = np.deg2rad(lat2)  # type: ignore
    lon2 = np.deg2rad(lon2)  # type: ignore

    dl = lon2 - lon1  # type: ignore

    X = np.cos(lat2) * np.sin(dl)  # type: ignore
    Y = np.cos(lat1) * np.sin(lat2) - np.sin(lat1) * np.cos(  # type: ignore
        lat2) * np.cos(dl)  # type: ignore

    return np.rad2deg(np.arctan2(X, Y))  # type: ignore

Reference

Earth radius
Meridian
Great-circle distance
what-is-the-difference-between-atan-and-atan2
Dihedral angle
三角恒等式
根据两点的经纬度求方位角和距离
万能公式求二面角——三面角第一余弦定理
震中距、方位角和反方位角的计算

夹角计算

内容来自计算3个地理坐标点之间的夹角,将代码转换成了 numpy 实现,方便 pandas 调用.

原理不加赘述,3 点坐标转换为两个向量,计算向量夹角.

注意: 这里是把地理位置当成平面正交坐标系,地球是椭球体,这里的夹角会存在误差,仅在距离较近 3 点使用.

def geo2xyz(lat, lng, r=6400):
    """geo2xyz 将地理经纬度转换成笛卡尔坐标系

    Args:
        lat (_type_): _description_
        lng (_type_): _description_
        r (int, optional): 地球半径,求角度所以无所谓

    Returns:
        _type_: _description_
    """
    thera = (np.pi * lat) / 180
    fie = (np.pi * lng) / 180
    x = r * np.cos(thera) * np.cos(fie)  # type: ignore
    y = r * np.cos(thera) * np.sin(fie)  # type: ignore
    z = r * np.sin(thera)  # type: ignore
    return [x, y, z]


def get_angle(latf, lonf, lat, lon, late, lone):
    """get_angle 3个经纬度 求夹角

    Args:
        latf (_type_): _description_
        lonf (_type_): _description_
        lat (_type_): _description_
        lon (_type_): _description_
        late (_type_): _description_
        lone (_type_): _description_

    Returns:
        _type_: _description_
    """
    p1 = geo2xyz(latf, lonf)
    p2 = geo2xyz(lat, lon)
    p3 = geo2xyz(late, lone)

    _P1P2 = np.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2 +
                    (p2[2] - p1[2])**2)  # type: ignore
    _P2P3 = np.sqrt((p3[0] - p2[0])**2 + (p3[1] - p2[1])**2 +
                    (p3[2] - p2[2])**2)  # type: ignore
    P = (p1[0] - p2[0]) * (p3[0] - p2[0]) + (p1[1] - p2[1]) * (
        p3[1] - p2[1]) + (p1[2] - p2[2]) * (p3[2] - p2[2])  # type: ignore
    angle = (np.arccos(P / (_P1P2 * _P2P3)) / np.pi) * 180  # type: ignore
    return angle