Ask Your Question
0

Checking for GPS waypoint being passed?

asked 2019-01-14 01:21:39 -0500

Hypomania gravatar image

I am working on an autonomous vehicle system that navigates and avoids obstacles.

Navigation alone, it goes from waypoint to waypoint by hitting the circular threshold points, basically saying "when within this distance, increment the target waypoint.

Now, when there is an obstacle and the threshold point is "inside" the obstacle, 99.9% of the times the threshold point won't be hit as the is threshold is set to very low due to the system using RTK. I am trying to use two intersecting lines (lat and long) to check whether or not the current waypoint has passed the next one, however that doesn't seem to work very well and I can't wrap my head around how to do it. Here's my code (in the 2D array, in the second dimension 0 is for lat and 1 is for long):

void trackWaypoint(double heading, double current_coords[NUMBER_OF_WAYPOINTS][2], double next_coords[NUMBER_OF_WAYPOINTS][2], uint32_t &wp) { // This is only for N and W hemispheres

if (wp != (NUMBER_OF_WAYPOINTS - 1)) {

    if ((heading >= 0) and (heading < 90)) {

        if (((next_coords[wp + 1][0] - current_coords[wp][0]) < 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) > 0)) {
            wp++;
        }
    }
    else if ((heading >= 90) and (heading < 180)) {

        if (((next_coords[wp + 1][0] - current_coords[wp][0]) > 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) > 0)) {
            wp++;
        }
    }
    else if ((heading >= 180) and (heading < 270)) {

        if (((next_coords[wp + 1][0] - current_coords[wp][0]) > 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) < 0)) {
            wp++;
        }
    }
    else { // [270, 360)

        if (((next_coords[wp + 1][0] - current_coords[wp][0]) < 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) < 0)) {
            wp++;
        }
    }
}

}

Here's two scenarios, A and B, assuming we are operating in Northern and Western hemispheres (sorry for lousy drawing):

In A, if the system decides to take the left path, it will not be able to to determine whether or not the next waypoint has been passed due to it thinking it hasn't intersected along the longitude (x) axis.

In A, if the system decides to take the upper path, it will not be able to to determine whether or not the next waypoint has been passed due to it thinking it hasn't intersected along the latitude (y) axis.

Is there a different way of doing it? If not, how could I account for both paths passing the next waypoint?

enter image description here

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2019-01-14 01:50:45 -0500

Instead of trying to solve this problem in terms of X and Y (Lat and Long) You want to use some more relevant geometry. Referring to your diagram above, if you define an infinite line through the next two green waypoints. Then you can calculate the position of the closest point on that line to your current position. If this position is before the first green waypoint then it hasn't been passed, however if that closest point is beyond the first green waypoint then the point has been passed and you can move to the next one. The maths for this is surprisingly simple using vectors:

Define three vectors:

  • P the current location
  • W1 waypoint 1
  • W2 waypoint 2

Calculate the unit direction (D) between W2 and W1

  • D = (W2 - W1) / |W2 - W1|

then distance in front or behind W1 (Dist)

  • Dist = (P - W1).dot(D)

This will avoid the orientation problems you described. However this may mean that you could potentially drift a long way from the waypoints if there are a lot of obstacles. Hope this helps though.

edit flag offensive delete link more

Comments

Thank you so much, really appreciated. That does make sense, I am gonna try to see if I can implement that!

Hypomania gravatar image Hypomania  ( 2019-01-14 02:36:24 -0500 )edit

No problem.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-01-14 02:58:46 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-01-14 01:21:39 -0500

Seen: 91 times

Last updated: Jan 14 '19