使用两个GPS设备计算航向

Question

您好我正在计划使用两台RTK-GPS设备来获取室外机器人的位置和方向。这两个设备放置在机器人的左侧和右侧，距离为2米。标题应为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的航向信息，我们能够成功导航。