FIGURE 4. Simple map of the earth showing
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.
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