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

Question

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

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
    }

Answer 1

几点：

圆周率

可以使用常量 M_PI

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

atan2( dLon, dLat)

而不是

atan2( dLat, dLon)

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

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

delta = remainder( azimuth-target, 360.0)

这将给出 -180 和 180 之间的增量

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

remainder( TargetLongitude - CurrentLongitude, 360.0)

而不是

TargetLongitude - CurrentLongitude

这似乎是 OTT，但我发现（困难的方法）养成总是以这种方式计算经度差异的习惯比在代码中到处追踪这种差异要容易得多当您的代码在 180E 期间使用时拍摄。

Answer 2

最终版本

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
}

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

How to develop to-point moving algorithm using Gps and compass

c++

algorithm

arduino

gps

topology