使用两个 GPS 设备计算航向

Question

您好，我打算使用两个 RTK-GPS 设备来获取户外机器人的位置和航向。两个设备分别放置在机器人的左右两侧，距离均为2m。标题应该是 0 到 2*PI 之间的值。如果机器人朝北，则为 0。

我正在尝试以下操作：

double x1 = gps_left->latitude;
double y1 = gps_left->longitude;
double x2 = gps_right->latitude;
double y2 = gps_right->longitude;

double dx = x2-x1;
double dy = y2-y1;

// get the normal of the line
double nx = dy*-1;
double ny = dx;

double angle = atan2(ny ,nx);

即使我旋转机器人，角度也始终保持不变

Answer 1

double x1 = gps_left->latitude;
double y1 = gps_left->longitude;
double x2 = gps_right->latitude;
double y2 = gps_right->longitude;

double dy = y2-y1;

double nx = cos(x2)*sin(dy);
double ny = cos(x1)*sin(dy) - sin(x1)*cos(x2)*cos(dy);

double angle = atan2(nx,ny);

你能试试这个吗？

0 - 向北行驶， 90 - 向东行驶， 180 - 向南行驶， 270 - 向西前进。

Answer 2

像@Mark Booth 一样，分辨率可能还不够。我们遇到了类似的问题，所以我们使用磁力计来解决这个问题。使用来自磁力计和 GPS 的航向信息，我们能够成功导航。