使用GPS坐标来计算距离和方位角

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

敬威

7627人浏览 · 2023-04-08 14:32:22

敬威 · 2023-04-08 14:32:22 发布

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

基本概念

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

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

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

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

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

经纬线

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

计算任意两点间的距离

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

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

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

球面模型

假设地球上有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两点的经纬度坐标转换成球面坐标

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

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

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

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

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

Haversine 公式

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

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

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

根据正弦的半角公式：

正矢可以变换为：

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

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

1
2
3

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

有如下的推导过程：

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

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

我们记a为：

由此，我们可以得到：

进而得到球心角δ为：

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

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

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

简化模型

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

工程实现

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

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);
    }
}

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

计算任意两点间的方位角

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

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

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

由三面角正弦定理可得：

由三面角余弦定理可得：

由此可得：

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

可得到：

进而得到方位角：

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

三面角
从一点出发并且不在同一平面内的三条射线，其中每相邻两射线可以决定一个平面，这样的三个平面所围成的立体图形叫做三面角。其中，这三条射线叫做三面角的棱，这些射线的公共端点叫做三面角的顶点，相邻两棱所夹的平面部分叫做三面角的面，在每个面内两条棱所形成的角叫做三面角的面角，过每一条棱的两个面所形成的二面角叫做三面角的二面角。一个三面角可以用它的顶点的字母来表示，例如“三面角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;
    }
}