如何使用 Gps 和指南针开发对点移动算法答案

【问题标题】：How to develop to-point moving algorithm using Gps and compass如何使用 Gps 和指南针开发对点移动算法
【发布时间】：2020-10-16 01:06:48
【问题描述】：

我正在尝试开发一种控制船舵的算法... 我迷失在地理算法中...... 功能不正确。

direction WhereToMove(double CurrentLatitude, double CurrentLongitude, double TargetLatitude, double TargetLongitude, double azimuth) {
        double azimuthHysteresis = 5; //straight if the deviation is less than 5 degrees
        double pi = 2 * asin(1.0);
        double target = atan2(TargetLatitude - CurrentLatitude, TargetLongitude - CurrentLongitude) * 180 / pi;
        double delta = azimuth - target;
        if (delta > 180) delta -= 360;
        if (delta < -180) delta += 360;
        if (delta < -2) { 
            return right;
        }
        if (delta > 2) {
            return left;
        }
        return straight; // go straight
    }

【问题讨论】：

请修正代码格式

标签： c++ algorithm arduino gps topology

【解决方案1】：

最终版本

direction WhereToMove(double CurrentLatitude, double CurrentLongitude, double TargetLatitude, double TargetLongitude, double azimuth) {
    double azimuthHysteresis = 2; //straight if the deviation is less than 2 degrees
    double target = atan2(remainder(TargetLongitude - CurrentLongitude, 360.0), remainder(TargetLatitude - CurrentLatitude, 360.0)) * 180 / M_PI;
    double delta = remainder(azimuth - target, 360.0);
    double deltaModule = sqrt(delta * delta);
    if (deltaModule <= azimuthHysteresis) //deviation is small go straight
    {
        return straight;
    }
    if (delta <= -azimuthHysteresis) {
        return right;
    }
    if (delta >= azimuthHysteresis) {
        return left;
    }
    return straight; // go straight
}

【讨论】：

在将其传递给 atan2 之前，通过 coslat 缩放对数差异会更准确，特别是在高纬度地区，我所在的地方 cosLat 约为 0.5，但在北极地区它变得非常小。 fabs(delta) 比 sqrt(delta*delta) 简单
所以这段代码没有考虑到，纬度和经度是球体上的坐标。也许你真正需要的是bearing calculation的经典算法。跨度>

【解决方案2】：

几点：

你可以使用常数 M_PI 来表示 pi

我想你希望你的角度从北方顺时针测量。 atan2 给出从 x 轴逆时针方向的角度。这很容易修复，使用

atan2( dLon, dLat)

而不是

atan2( dLat, dLon)

一个经度表示的距离，大致是cos(lat)乘以一个纬度表示的距离。因此，您应该通过 cos( M_PI/180.0 * lat) 在上面缩放您的 dlon。（cos 和所有处理角度的数学函数一样，将弧度作为参数）。

您可以通过使用数学库函数剩余来简化计算方位角和目标的差异，如

delta = remainder( azimuth-target, 360.0)

这将给出一个介于 -180 和 180 之间的增量

我不知道您的代码是否会在 180E 附近使用。我会说你应该计算经度的差异，就好像它可能一样，即使用

remainder( TargetLongitude - CurrentLongitude, 360.0)

而不是

TargetLongitude - CurrentLongitude

这可能看起来很麻烦，但我发现（很难）养成始终以这种方式计算经度差的习惯比在代码中的任何地方追查这种差异的发生时间要容易得多您的代码在 180E 中使用。

【讨论】：