Servo - March 2010

FIGURE 4. Simple map of the earth showing latitude/longitude lines.

well as the current coordinates. The destination coordinates are often known in advance and programmed into the robot ahead of time.

This same concept is the basis for the RoboMagellan competition where robots are given access to the GPS coordinates of the waypoints on the course, and the robot must navigate from its current position to the point or points on the map. Each waypoint is designated by a large orange traffic cone allowing the robot to confirm it has reached that waypoint visually.

Going Back To School

In order to calculate the heading to the next waypoint from the current position, you have to do a little math. To simplify things, let’s assume we won’t be crossing the equator or the prime meridian since this is unlikely anyway. By doing this, we can treat all latitude and longitude values as absolute. So, to calculate distance and heading you will plot a point at the coordinates you are heading to relative to the coordinates you are at. Looking at Figure 5, we will

FIGURE 5. Pythagorian Theorem and formulas.

56 SERVO 03.2010

say that the point where x and y intersect is our home point. This represents the current coordinates. Point A represents the point where our waypoint destination is.

Essentially, what we’ve done is created a virtual right triangle which will allow us to get two important values. By treating all values as absolute, you can simply subtract the x values to get a and subtract the y values to get b. Using the Pythagorean Theorem, you can now calculate c. Remember, a2 + b2 = c2. Now we have our distance which can be used in a robot with wheel encoders to ensure we travel far enough. Now to calculate the heading.

Some trigonometry is involved in this step. In order to obtain θ (Theta), you will take the inverse of the formulae within Figure 5. This will give you the degrees of the heading you need to be on. Based on your current heading, you can then calculate the new heading. For the most part, you won’t have to worry about the calculations. They should all be tucked into a nice subroutine. About the only thing not covered here is some filtering to smooth out changes. Once things are dialed in, the robot shouldn’t veer much. As a safety, we can make it stop whenever there is no GPS lock if need be.

Final Thoughts

The specifics of handling the data from the compass module will vary depending on which compass module you use and how its data is formatted. The specifics of dealing with the distance data will vary some as well, depending on your microcontroller, drive system, encoders, and overall resolution. In Part 2, we will explore a real world example of having a robot navigate through several waypoints to reach a destination. Based on the information provided here, I can almost see the light bulbs over your heads and the wheels turning (both figuratively and literally). You’ve got a month to implement these ideas. Until then, brush up on your math and have fun experimenting! SV