如何在浮点支持较差的处理器上计算 GPS 坐标之间的距离？

Question

我需要计算 GPS 坐标之间的距离来计算行驶距离。我已经尝试过Haversine和Vincenty算法，它们在我的台式PC上运行良好，但是当我将代码移植到dsPIC时，由于缺乏浮点精度，它们对于接近的点（几米之内）返回0以及 sin 和 cos 的糟糕实现。

对于我的用例，我的点相距不超过 10 米，并且彼此之间的距离都在 10 公里以内。我尝试过以下算法，结果看起来不错：

double dist(double latA, double lonA, double latB, double lonB)
{
    double latD = fabs(latA - latB) * 111.3;
    double lonD = fabs(lonA - lonB) * 111.3 * cos(latA * 3.14159265 / 180);

    return sqrt(latD*latD + lonD*lonD) * 1000;
}

假设每1°的距离是111.3公里，我使用毕达哥拉斯定理来计算距离。有什么简单的方法可以改进我的算法吗？或者还有其他不依赖于高精度正弦/余弦的算法吗？

Answer 1

海洋 AIS 系统（在 IEC61193-2 中指定）中使用的公认算法是 Rhumb Line算法。我已经使用 Anthony 在目标上成功实现了这一点Williams 的定点数学库，它使用 CORDIC算法，我相信通常会比软件浮点性能提高约 5 倍。

然而，该库是 C++ 而不是 C，由于大量的运算符重载，这使得它易于使用，但这可能不是您想要的。然而，值得考虑对 C 代码使用 C++ 编译，只是为了这个库的好处。当然，问题是 Microchip 的 C31 编译器奇怪地不支持 C++。

但需要注意的是，log() 函数的查找表只差一个值，因此需要在末尾添加一个值为 0 的附加元素。我找到后安东尼证实了这一点，但我不相信他更新了下载。

不管怎样，答案可能是使用定点和 CORDIC。

要解析 1m 的经度或赤道弧，您将需要 8 位精度，因此单精度浮点数是不够的，使用双精度会大大减慢速度。检查 MikroElectronica 的 C 用户手册发现编译器仅支持单精度 - float、double 和 long double 都是 32 位，所以有在任何情况下，使用此编译器都无法使用内置 FP 类型来实现所需的准确性。

如果有任何用处，这是我使用安东尼库的 Rhumb Line 代码：

标题：

#if !defined cRhumbLine_INCLUDE
#define cRhumbLine_INCLUDE

#include "fixed.hpp"

//! Rhumb Line method for distance and bearing between two geodesic points
class cRhumbLine
{
public:

    //! @brief Default constructor
    //!
    //! Defines a zero length line, bearing zero
    cRhumbLine() : m_update_bearing(false), m_update_distance(false), m_distance(0), m_bearing(0) {} 

    //! @brief Constructor defining a line
    //!
    //! @param startLatDeg  Start latitude in degrees, negative values are south of equator
    //! @param startLonDeg  Start longitude in degrees, negative values are west of meridian.
    //! @param endLatDeg    End latitude in degrees, negative values are south of equator
    //! @param endLonDeg    End longitude in degrees, negative values are west of meridian.
    cRhumbLine( fixed startLatDeg, fixed startLonDeg, fixed endLatDeg, fixed endLonDeg ) 
    {
        define( startLatDeg, startLonDeg, endLatDeg, endLonDeg ) ;
    }

    //! @brief Define a start/ent point
    //!
    //! @param startLatDeg  Start latitude in degrees, negarive values are south of equator
    //! @param startLonDeg  Start longitude in degrees, negative values are west of meridian.
    //! @param endLatDeg    End latitude in degrees, negarive values are south of equator
    //! @param endLonDeg    End longitude in degrees, negative values are west of meridian.
    void define( fixed startLatDeg, fixed startLonDeg, fixed endLatDeg, fixed endLonDeg ) ;

    //! @brief Get start-end distance in nautical miles
    //! @return Start-end distance in nautical miles.
    fixed distanceNm() { return distanceMetres() / ONE_NM_IN_METRE ; }

    //! @brief Get start-end distance in metres.
    //! @return Start-end distance in metres.
    fixed distanceMetres()  ;

    //! @brief Get start-end bearing in degreed.
    //! @return Start-end bearing in degreed (0 <= x < 360).
    fixed bearingDeg()  ;

private:
    static const int ONE_NM_IN_METRE = 1852 ;

    bool m_update_bearing ;
    bool m_update_distance ;
    fixed m_distance ;
    fixed m_bearing ;

    fixed m_delta_phi ;
    fixed m_delta_lon ;
    fixed m_delta_lat ;
    fixed m_lat1_radians ;
} ;

#endif

正文：

#include "cRhumbLine.h"

void cRhumbLine::define( fixed startLatDeg, fixed startLonDeg, fixed endLatDeg, fixed endLonDeg )
{
    fixed lat1 = startLatDeg / 180 * fixed_pi ;
    fixed lon1 = startLonDeg / 180 * fixed_pi ;
    fixed lat2 = endLatDeg / 180 * fixed_pi ;
    fixed lon2 = endLonDeg / 180 * fixed_pi ;

    m_update_bearing = true ;
    m_update_distance = true ;

    m_delta_phi = log( tan( lat2 / 2 + fixed_quarter_pi ) / tan( lat1 / 2 + fixed_quarter_pi ) ) ;
    m_delta_lat = lat1 - lat2 ;
    m_delta_lon = lon1 - lon2 ;
    m_lat1_radians = lat1 ;

    // Deal with delta_lon > 180deg, take shorter route across meridian
    if( m_delta_lon.abs() > fixed_pi )
    {
        m_delta_lon = m_delta_lon > 0 ? -(fixed_two_pi - m_delta_lon) : (fixed_two_pi + m_delta_lon) ;
    }
} 


fixed cRhumbLine::distanceMetres()
{
    if( m_update_distance )
    {
        static const fixed mean_radius = 6371009 ; // in metres. Source: International Union of Geodesy and Geophysics (IUGG)

        fixed q = m_delta_phi != 0 ? m_delta_lat / m_delta_phi : cos( m_lat1_radians ) ; // Deal with lines with constant latitude

        m_distance = sqrt( ( sqr(m_delta_lat) + sqr(q) * sqr(m_delta_lon) ) ) * mean_radius ;
        m_update_distance = false ;
    }

    return m_distance ;
}


fixed cRhumbLine::bearingDeg()
{
    if( m_update_bearing )
    {
        static const fixed mean_radius = 6371009 ; // in metres. Source: International Union of Geodesy and Geophysics (IUGG)

        m_bearing = atan2( m_delta_lon, m_delta_phi ) / fixed_pi * 180 ;
        if( m_bearing == -180 )
        {
            m_bearing == 0 ;
        }
        else if( m_bearing < 0 )
        {
            m_bearing += 360 ;
        }
        m_update_bearing = false ;
    }

    return m_bearing ;
}

Answer 2

一些评论：

• 您需要指定计算的范围和精度要求。在确定使用什么方法计算余弦时，范围和精度极其重要。此外，如果纬度和经度的相对差异与到极点的角距离相比很小，您发布的毕达哥拉斯近似效果很好。如果纬度不接近，您的伪毕达哥拉斯算法在高纬度地区将无法正常工作。 （例如，在纬度 43.001 和 43.002 处效果很好，但在纬度 89.961 和 89.962 处则不然）

• 需要根据其圆度来计算经度 - 您的算法将在国际日期变更线周围失败，但这可以通过采用纵向差分对称模 360 来轻松解决，其中 smod(x,m) = mod(x+m/2,m)-m/2。 （例如 -179.5 - +179.5 = -359 度，但如果计算 smod(-359,360)，您将得到 +1 度。）

• 在设计时，充分利用您的PC。您有一个非常强大的计算器，您可以评估大量测试点的高精度答案和您的近似值，并查看它们有何不同，以评估准确性。如果您在此信息中推断出模式，则可以使用它进行二阶近似以提高准确性。

---

更新：您声明您的范围/精度要求为 +/-60 度（在一个半球内缩小范围没有优势）和 1% 的精度。 cos(x) 的良好近似值（x 为度数）在此范围内为 c₂(x) = 0.9942 - 1.39*10^-4 * x ² = 0.9942 - (0.01179x)²；在此范围内其误差最大值为0.006。

如果您想要更高的精度，请使用四次多项式 (c₄(x) = 0.999945-(0.01233015x)²+(0.007778x)⁴ 在此范围内的最大误差小于 6x10^-5，但对参数错误和算术更加敏感错误）或分成多个范围。

Answer 3

您可能想尝试使用预先计算的 sin 和 cos 表。
它使用更多的内存，可以破坏通用处理器（不是您的情况）上的缓存，但在您的处理器上具有尽可能高的准确性，并且速度相当快。

Answer 4

您（有效地）使用定点 DSP，因此您可能想研究定点函数；他们可能会表现得更好。

事实证明，Microchip 有一个可用的定点库： http://www.microchip.com/stellent/idcplg?IdcService=SS_GET_PAGE&nodeId=2680&dDocName=en552208 我不知道这会有多大帮助 - 它可能缺乏您需要的精度。

以下是如何自行操作的示例： http://www.coranac.com/2009 /07/sines

回到正轨 - Microchip 页面表明他们的编译器和库与单精度和双精度兼容 IEEE-754。除非他们说的是半真半假并且使用半精度（16 位）格式。如果您仍然没有得到您需要的结果，我会考虑提交错误报告。