Reach a waypoint using GPS/Compass/Accelerometer - Algorithm?

Question

I currently have a robot with some sensors, like a GPS, an accelerometer and a compass. The thing I would like to do is my robot to reach a GPS coordinate that I enter. I wondered if any algorithm to do that already existed. I don't want a source code, which wouldn't have any point, just the procedure to follow for my robot to do so, for me to be able to understand what I do... At the moment, let's imagine that I can access the GPS coordinate everytime, so no need of a Kalman filter. I know it's unrealistic, but I would like to programm it step by step, and Kalman is the next step.

If anyone has an idea...

Answer 1

To get a bearing (positive angle east of north) between two lat-long points use:

bearing=mod(atan2(sin(lon2-lon1)*cos(lat2),(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1)),2*pi)

Note - angles probably have to be in radians depending on your math package.

But for small distances you can just calculate how many meters in one degree of lat and long at your position and then treat them as flat X,Y coords.

For typical 45deg latitudes it's around 111.132 km/deg lat, 78.847 km/deg lon.

Answer 2

1) orient your robot toward its destination.

2) Move forward until the distance between you and your destination is increasing where you should go back to 1)

3) BUT ... if you are close enough (under a threshold), consider that you arrived at the destination.

Answer 3

You can use the Location class. It's BearingTo function computes the bearing you have to follow to reach another location.

Answer 4

There is a very nice page explaining the formulas between GPS-based distance, bearing, etc. calculation, which I have been using:

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

I am currently trying to do these calculations myself, and just found out that in Martin Becket answer there is an error. If you compare to the info of that webpage, you will see that the part in the middle:

(lat1)*sin(lat2)

should actually be:

cos(lat1)*sin(lat2)

Would have left a comment, but don't have the reputation yet...