Algorithm 使用GPS/罗盘/加速计到达航路点-算法？

我现在有一个带有一些传感器的机器人，比如GPS、加速计和指南针。我想做的事情是我的机器人到达我输入的GPS坐标。我想知道是否已经存在这样做的算法。我不想要一个没有任何意义的源代码，只是为了让我的机器人这样做，为了让我能够理解我所做的事情而遵循的过程。。。现在，让我们想象一下，我每次都可以访问GPS坐标，因此不需要卡尔曼滤波器。我知道这是不现实的，但我想一步一步地编程，而Kalman是下一步

如果有人有想法…

1）将机器人定向到其目的地

2） 继续前进，直到您与目的地之间的距离增加，您应该返回1）

---

3） 但是。。。如果你足够接近（在一个阈值之下），考虑你到达目的地。

你可以使用<代码>位置< /C>类。它的

BearingTo

功能计算到达另一个位置必须遵循的方位。

要获得两个纬度点之间的方位角（北偏东的正角度），请使用：

轴承=mod（atan2（sin（lon2-lon1）*cos（lat2），（lat1）*sin（lat2）-sin（lat1）*cos（lat2）*cos（lon2-lon1）），2*pi）

注意-角度可能必须以弧度为单位，具体取决于您的数学软件包

但对于小距离，你只需计算出你所在位置在一个纬度和一个长度上有多少米，然后将它们视为平面X，Y坐标

---

对于典型的45度纬度，纬度约为111.132 km/deg lat，经度约为78.847 km/deg lon。

有一个非常好的页面解释了基于GPS的距离、方位等计算公式，我一直在使用：

我目前正试图自己做这些计算，刚刚发现马丁·贝克特的答案中有一个错误。如果与该网页的信息进行比较，您将看到中间的部分：

（lat1）*sin（lat2）

实际上应该是：

cos（lat1）*sin（lat2）

---

会留下评论，但还没有声誉…

嗨，谢谢你的回答。我想知道如何根据机器人及其目的地的GPS坐标计算方位角。我想有一些关于经度和纬度的计算，但我在谷歌上找不到。。