使用两个GPS设备计算航向

嗨，我计划使用两个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);

不管怎样，即使我旋转机器人，角度也总是保持不变

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-向西

你能试试这个吗

0-向北行驶， 90-向东， 180-向南，

---

270-向西。

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

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

添加一些您的机器人捕获的

x1

、

y1

、

x2

和

y2

值的示例可能会很有用。我的怀疑是，考虑到可能的分辨率和设备的分离，两个GPS单元可能返回相同的值。添加一些您的机器人捕获的

x1

、

y1

、

x2

和

y2

值的示例可能会很有用。我怀疑两个GPS装置可能返回相同的值，考虑到它们可能的分辨率和设备的分离。