I'm building an Autonomous Ground Vehicle and it's coming along nicely except I'm having 2 problems---One) I don't know how to actually get the (rc) car to go from one place to the next---Two) I'm having trouble with obstacle avoidance
But right now I really need help with the GPS. I'm using the 66 Channel LS20031 GPS 5Hz Receiver from sparkfun to get the GPS coordinates, and the HMC6352 digital compass to orient the car. How would I go about making the car orient itself and go from coordinates "x" to coordinates "y"?
I'm programming with an Arduino mega 2560, and using the chassis of the duratrax evader ext-2. I'm using a traxxas 12T 550 dc motor with the traxxas xl-5 esc. I'm using the stock servo for the duratrax evader as well. The car also has 5 IR sensors (Infrared Proximity Sensor - Sharp GP2Y0A21YK) to help avoid obstacles.
If anyone can, please help me with this!!! I appreciate it greatly!!
Also, at any request, I'll post pictures (if that's of any help)
You can calculate the approximate bearing from point A to point B using atan2() by plugging in the polar coordinates (N, E values for Y, X).
Note that you will get radians out of atan2(), so you have to convert to degrees.
Then, figure out which direction you are headed; if less than the target, turn right; if greater than the target, turn left.
Something like:
int turn(double curN, double curE, double targetN, double targetE, double curHeading) {
double target = atan2(targetN - curN, targetE - curE) * 180 / M_PI;
double delta = curHeading - target;
if (delta > 180) delta -= 360;
if (delta < -180) delta += 360;
if (delta < -2) return -1; // turn left
if (delta > 2) return 1; // turn right
return 0; // go straight
}
Presumably, "curHeading" comes from the compass, and is in the same space as the atan2() coordinate space. atan2() thinks X/E is 0, and Y/N is 90 degrees. If you have traditional compass heading, you can conveniently convert by just swapping the y and x arguments.
Notice the forms on that page, entitled "Calculate the great circle distance between two points". That's what you want to do; you want to take a starting set of lat/lon coordinates (your current position), and find out how far and what direction you need to head (your bearing) to get to the next set of lat/lon coordinates (your desired position). Keep doing that over and over, and you can get from A, to B, to C, etc. BTW - these points are called "waypoints" in navigation parlance.
Now - take a look at the source code to that page; try to find the form, and note the names of the form fields (I trust you understand how to do this; if you don't, the following may or may not make any sense). Go to the top of the source code, and you'll see where it is including a couple of javascript files; the important one is:
That file contains all the code the page uses to calculate the values. There are a few functions in there to become familiar with; you may want to do some googling:
function Vincenty_Distance(lat1,lon1,lat2,lon2,us,meters_only)
function Haversine_Distance(lat1,lon1,lat2,lon2,us)
function Bearing(lat1,lon1,lat2,lon2,radians)
You may also want to look at this site (where these were pulled from):
All in all, though - what you want to do I've already described; take a reading from your GPS, use the lat/lon coordinates along with your first waypoint to calculate where you want to go, get your bearing and distance, and head in that direction. When you get so far along the route (that will be up to you to decide - you can choose to do this often, or once every few minutes, or maybe after you have traveled so far, etc), take another calculation based on where you are and where you want to be; if the bearing is off, adjust your course and continue on.
Note that it isn't likely that you'll hit your waypoint exactly; you'll only be able to get "so close" because of the inaccuracies in the GPS system. If you get withing 10 meters or so, conside that "good enough" and continue on. Also note that there is a difference between "straight line" distance and "great circle" distance; the latter is considered more accurate, but the former is fine for short distances (so for your small vehicle case, it would be OK - it would -not- be ok if you were running the DARPA Grand Challenge, however, as the distances are too large).
Ok - well, that should cover (or at least get you going) on GPS waypoint navigation. You've probably seen more than a few "unfamiliar" terms used, so I advise that you do more googling and learning about the subject as you experiment. As far as obstacle avoidance, that's a whole 'nother can of worms. If the obstacles are just small things in the way, you can probably easily go around them, then take a new bearing and head off in that direction. If, however, they are something larger (a building, perhaps), then you need some intelligence, most likely.
The way this is commonly done is by a system known as SLAM (Simultaneous Localization and Mapping):
Check out that page, especially at the bottom there is a paper (PDF) you can download entitled "SLAM for Dummies".
If you are really serious about this project, then I would also advise you to take the time to participate in this online course:
This online course is taught in Python, by Professor Sebastian Thrun - if you want to learn the basics of SLAM, you can't get a better course, IMHO. I took this course earlier this year in the springtime; it was a wonderful experience, well worth all the blood, sweat, and tears I put into it.
Good luck with your project, and I hope this helps...
Over short distances I've found the round off error from the Arduino's floating point libraries to be a problem.
Using simple trig over short distances, like around the block, seems to be more accurate.
One involving sin(), cos(), and/or tan(). Get out a geometry book and start reading. You have a known location (latitude and longitude) that you want to go to. You have a know location that you want to go to. Delta latitude and delta longitude are easy enough to determine. From that, finding the angle is quite simple.
Would I have to use some sort of "for" loop to get the servo to turn the wheels gradually? I've never really used for loops before, and It's been recommended that I use them....and how would I determine the angle? I know how to get the distance (distance formula---the square root of (x2-x1) squared + (y2-y1)squared----)
Local, flat earth approximation
If you stay in the vicinity of a given fixed point (lat0,lon0), it may be a good enough approximation to consider the earth as "flat", and use a North, East, Down rectangular coordinate system with origin at the fixed point. If we call the changes in latitude and longitude dlat=lat-lat0, dlon=lon-lon0 (Here treating North and East as positive!), then
a is the equatorial radius of the earth (=6378.137000km for WGS84), and e^2=f*(2-f) with the flattening f=1/298.257223563 for WGS84.
In the spherical model used elsewhere in the Formulary, R1=R2=R, the earth's radius. (using R=1 we get distances in radians, using R=60*180/pi distances are in nm.)
In the flat earth approximation, distances and bearings are given by the usual plane trigonometry formulae, i.e:
distance = sqrt(distance_North^2 + distance_East^2)
bearing to (lat,lon) = mod(atan2(distance_East, distance_North), 2*pi)
(= mod(atan2(cos(lat0)dlon, dlat), 2pi) in the spherical case)
These approximations fail in the vicinity of either pole and at large distances. The fractional errors are of order (distance/R)^2.
I took the Micromega examples and changed the great circle method to the one above.
