使用GPS /指南针/加速计 - 算法到达航点？

Posted 2021-04-16

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

如果有人有想法......

答案

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

bearing=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。

另一答案

1）将机器人定位到目的地。

2）向前移动直到你和目的地之间的距离增加，你应该回到1）

3）但是......如果你足够近（在一个门槛下），请考虑你到达目的地。

另一答案

你可以使用Location类。这是BearingTo函数计算你必须遵循的轴承到达另一个位置。

另一答案

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

http://www.movable-type.co.uk/scripts/latlong.html

我目前正在尝试自己进行这些计算，并且发现在Martin Becket的回答中有一个错误。如果您与该网页的信息进行比较，您会看到中间的部分：

(lat1)*sin(lat2)

应该是：

cos(lat1)*sin(lat2)

本来会留下评论，但还没有声誉......