Nav using GPS and the Prop: What to Expect?
Ken Gracey
Posts: 7,392
Hey guys,
I'm slowly making progress on my project: the PropBot. This is a robot I built just to study the Propeller. I feel strongly about having a project as a reason to learn anything.·The objective of this robot has been to make an autonomous robot that navigates around the Parallax building parking lot via·four waypoints, avoiding any parked cars or square curbs·along the way. Ideally, I'll sit in my office and watch the bot lap the office every·two minutes or so till the battery dies.
I've made this work one step at a time. I've made each piece work on its own, then integrated them together into the Propeller. These pieces include two HB-25s, five Ping sensors and associated signal conditioning, and GPS. I'm not navigating via GPS, but I've got it working well in smart mode and displaying the data on TV or via Parallax Serial Terminal.
I'm ready to start navigating via GPS. And I seek any input you may have to help me get the best results. My approach is as follows:
Add HM55B compass for bearing
Put four GPS waypoints into Prop as constants
Do the math (using Float32) to calculate distance from existing location to waypoint (found some trig calcs to do this)
Calculate heading
Turn robot to the right direction
Advance towards waypoint, making the same calculation every few seconds
When waypoint is reached, increment to the next waypoint
I will also send coordinates back via radio using Phil's modem app.
The waypoints I choose should have about 10-15' on either side of the robot until square curbs or parked cars are encountered.
Cam's math library is essential to the project.
Any advice, from any perspective? I have such limited time for programming and I want to make the best use of my time.
Thanks,
Ken Gracey
·
I'm slowly making progress on my project: the PropBot. This is a robot I built just to study the Propeller. I feel strongly about having a project as a reason to learn anything.·The objective of this robot has been to make an autonomous robot that navigates around the Parallax building parking lot via·four waypoints, avoiding any parked cars or square curbs·along the way. Ideally, I'll sit in my office and watch the bot lap the office every·two minutes or so till the battery dies.
I've made this work one step at a time. I've made each piece work on its own, then integrated them together into the Propeller. These pieces include two HB-25s, five Ping sensors and associated signal conditioning, and GPS. I'm not navigating via GPS, but I've got it working well in smart mode and displaying the data on TV or via Parallax Serial Terminal.
I'm ready to start navigating via GPS. And I seek any input you may have to help me get the best results. My approach is as follows:
Add HM55B compass for bearing
Put four GPS waypoints into Prop as constants
Do the math (using Float32) to calculate distance from existing location to waypoint (found some trig calcs to do this)
Calculate heading
Turn robot to the right direction
Advance towards waypoint, making the same calculation every few seconds
When waypoint is reached, increment to the next waypoint
I will also send coordinates back via radio using Phil's modem app.
The waypoints I choose should have about 10-15' on either side of the robot until square curbs or parked cars are encountered.
Cam's math library is essential to the project.
Any advice, from any perspective? I have such limited time for programming and I want to make the best use of my time.
Thanks,
Ken Gracey
·
Comments
Try to simulate a differential GPS by software. To do this you have to know the precise Lat, Lon, (Alt) coordinates of the starting position. Store the difference between the measured position and the starting position and use this difference during the travel to the other, precisely given or incremental, waypoints. This method assumes that the travel takes not more time than 2-5 minutes. To locate the first waypoint again you can use an IR seeker, for example, to home in and to update the GPS error for the next travel.
By the way, how the cars will avoid the robot? (Well, during working hours there shouldn't be too much traffic there.) To begin an 'escape or avoid' sequence in time, you will need to make the position calculations and environment sensing more frequently than 'every few seconds'. Some emergency sound signal might be useful.·
Can the hardware estimate the travelled distance or the angle of a turn from the rotation of the motors/wheels? That would help much.
Some grid memory of the occupied territories can be useful. After that you can use Lee-algorithm to find shortest route among obstacles. On the next run the robot can re-position itself on the grid with some AI method.
What if one puts a small but strong magnet on the robot by accident? In other words, the robot should be able to calibrate its magnetic sensor, sometime, or, the algorithm should recognize magnetic perturbations. (Wheels are running parallely, but sensed magnetization changes abruptly. Or, during a longer straight run, the difference between GPS and magnetic heading·is more tha acceptable.)
www.sparkfun.com/commerce/product_info.php?products_id=8416
I access it via FullDuplexSerial
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Visit my site -> [url=Http://www.rawcircuits.com]www.rawcircuits.com[/url]
Thanks much so far for the input. You've given me a lot to think about this afternoon, so I'll do a bit of reading before I do any programming.
Philldapill, sure! Charging station would be cool. It's another project of complexity considering all that I've got going on with this robot already, but I'll save the thought and if I achieve the other portions of the project then a charging station will be added.
Chuck and cessnapilot: also thanks!
I'll be back with more questions after I have something to show.
Ken Gracey
My robot actually uses 3 Propellers: one dedicated to proximity sensors, one main unit that also handles telemetry and motor control, and the third is strictly for navigation & GPS. I've actually added a Memsic 2 axis accelerometer to the setup you described, to keep track of motion for better position extrapolation between GPS updates. I realized I wasn't going to have enough cogs left over in the main unit's Propeller, so I put the navcomp (navigation computer) into it's own box with a dedicated Propeller and an LCD display. I actually just finished building the whole navcomp last Saturday. The nice thing about it is that I can take the navcomp box off the robot, stick a 9V battery in it, and test it separately from the robot by just walking around outside holding it!
I got the LCD, accelerometer, and compass working yesterday, but I can't get any serial data from the GPS in raw mode, and I can't seem to find an object on the object exchange for running the GPS in smart mode--? I added Beau Schwabe's transistor level shifter after reading a forum thread where he says the GPS module requires 4 volts for an "on" bit so the Propeller can't Tx to it through a resistor alone. Maybe I made a mistake building the level shifter circuit, or perhaps I just need to cut the level shifter out and use the GPS in raw mode with a resistor alone. What kind of circuit are you using with your GPS?
Ken, why don't we collaborate on the code? I've got my compass working, you've got your GPS working, we can put those together and work on waypoint navigation. We could make a general purpose navigation computer object that can be configured for custom setups or used on the QuadRover.
Instead of navigating blindly to waypoints, I'm thinking of the waypoints as describing lines that the robot tries to follow. That way if it can't physically make it to the waypoint (due to obstacles and/or GPS inaccuracies) it will settle for following parallel to the line until a path clears, trying to get closer to the line only if it can, so it will continue to advance rather than stopping at one bad waypoint. You could think of the lines as describing a potentially infinite number of waypoints, so that the robot follows a course rather than trying to get to a fixed point.
A friend of mine suggested that if an obstacle is in the path, you could add temporary waypoints to make a box around the obstacle, which would cause the robot to follow a rectangular path around the obstacle before returning to the path. So some functions of the navigation object might be:
Init()
SetWaypoint()
GetDirectionToWaypoint() ' degrees; waypoints are points
GetDistanceToWaypoint()
GetDirectionToPath() ' waypoints are lines
GetDistanceToPath()
AvoidObstacle(direction) ' Generate temporary path. direction = degree heading where obstacle was detected
Backtrack() ' Try to follow the temporary course backwards to get back to the main path
I anticipate we might want to implement different navigation strategies on top of the basics; if we disagree on the algorithms that's good because it means we'll generate a more general purpose code that can be used by others too. What we can do is, we make a basic waypoint navigation object that does the lowest common denominator, and we can wrap that object with extra algorithms if we want.
So many ideas, So many projects, So little time.
I think the GPS module has an integrated 220 ohm resistor on the data line, which is just enough to keep it from damaging the Propeller or itself, but might be a little on the small side (usually you would use 1-2K I think). But then, what do I know: my setup currently isn't working! I might just try connecting it directly. It is, after all, only a worst case draw of 10 mA, and I'm sure the SX inside the Parallax GPS unit can handle it.
Are you using the GPS float objects from the OBEX? They have separate constants for Tx pin and Rx pin - do you just set them to be the same pin? I was wondering if maybe that code won't work if the Tx and Rx pins are the same.
http://geology.heroy.smu.edu/~dpa-www/robo/challenge/examples.html
I have a file of his tutorials, but they are 10,000 miles away.
John Abshier
I'm trying to do the same thing.· In order to avoid the intimidating Latitude and Longitude numbers, I use Google Earth to select a point just to the Southwest of the area where my robot will be running.·I call this point the "local origin".· All of my GPS readings are measurements relative to that point.· Also, the meters per degree Latitude and Meters per degree Longitude are calculated at the local origin.· All of these numbers are manually entered as constants in my spin program.
Last weekend I was at CIRC Bot Brawl in Peoria and gave a static display of my robot.· The attached PeoriaCalc is the Excel calculations for the local origin and several other points in the area where the Bot Brawl was held.· Peoria Spin is the list of constants that were entered.
One note here.· I have chosen to use decimeters as a standard in my program for all of the measurements.· I know that it is rather optimistic to use decimeters for GPS measurements, but it works well for sonar and encoders, and I didn't want to worry about length conversions in he program.
The last file, ReadNema.spin, is the program that reads the GGA and RMC sentences from an EM-406A at 56kbits.· It saves the fix and # of satellites from the GGA and Latitude, Longitude, Speed over ground, and Course over ground from the RMA sentence.· Note that the atoi calculations are only for the differences between the present location and the local origin and that the distance is calculated at the same time.
The last piece of the puzzle is the arcTan.· I'm using some code that Mike Green posted awhile back.· It is accurate to within one degree most of the time, which is good enough for me.
The code was working very well last fall.· I'm just waiting for warm, dry days to start playing outside again.
······ Rick Brooks
Post Edited (Rick Brooks) : 3/10/2009 6:49:56 AM GMT
Im doing a bit of gps nav myself here... Im starting to understand it sort off...
I'm working on the first step which is converting the·lat / lon·from ddmm.mmmm to dd.dddd
i found some great pages with help. my code i think is basically·moving·in the·right direction·but the math is pretty off when i check it with an online converter.. basicaly this is the basic math im doing. Note: this isnt the exact syntax i just stripped out the float stuff to make it easier to understand.
'latitude := 3819.5882 'lat_dd := trunc(latitude / 100) 'lat_mm := lat - (lat_dd * 100) 'lat_mm := lat_mm / 60 'lat_dec := lat_dd + lat_mmmy lat is say.. 3819.5882 from my gps
my decimal output from the·previous basic code is.. 38.3265 ish (i say ish cuse the lat from the gps bounces a little obviously). This is much·different from online converters which give me around 38.332875.
I followed online procudures to get the translation.. But im guessing it might have something to do with not taking into account another decimal place.. Am i supposed to be figuring out the extra decimal place ex:· 3819.58*.*82· ?
did that make sense? [noparse]:)[/noparse]
hope so thanks
Jesse
·
Ken, I don't think you need the compass. The GPS system knows where North is.
Jack
It uses the F32_full object
lat1 := 28.963105 'coordinates have north bearing 0
long1 := -81.471019
lat2 := 28.963763
long2 := -81.471019
r := F32.atan2(f32.fsub(F32.fmul(F32.cos(f32.radians(f32.fsub(90.0,lat1))),F32.sin(f32.radians(f32.fsub(90.0,lat2)))),F32.fmul(F32.fmul(F32.sin(f32.radians(f32.fsub(90.0,lat1))),F32.cos(f32.radians(f32.fsub(90.0,lat2)))),f32.cos(F32.Radians(F32.Fsub(long2,long1))))),F32.fmul(F32.sin(F32.Radians(F32.Fsub(long2,long1))),F32.cos(f32.radians(f32.fsub(90.0,lat2)))))
degree := F32.degrees(r) ' degree comes out 0:east -90:north 90:south 180,-180:west
Degree comes out as a negative number between 0-180 for the northern half
and a postive number 0-180 for the southern half, both start at east and go to +-180 West.
I used an if statement to covert it into 360 degrees rotation with north 0/360
It seems fairly accurate.
Would you mind sharing your code to convert from ddmm.mmmm to dd.dddd? seems simple enough but as you can see, a couple posts up i seemed to have even messed that up.
·· The heading provided in the $GPRMC string is calculated in this manner.· But like altitude, it can vary in accuracy and so high end GPS system include dedicated compass and pressure sensor for altitude.· Take care.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Chris Savage
Parallax Engineering
Haven't got there yet. I don't even have a GPS unit yet.
I was hoping I could get the coordinates out of the GPS in dd.dddd format. But now I am guessing NOT?
'lat_dd := trunc(latitude / 100)
'lat_mm := lat - (lat_dd * 100)
'lat_mm := lat_mm / 60
'lat_dec := lat_dd + lat_mm
What is the lat variable on the third line?
Yep, you're absolutely right, Dennis. About a second I posted my message, I realized that the GPS, though it does indeed know where North is, it doesn't know
your attitude <mine is awful some days [noparse];)[/noparse]>. If you're moving, it knows your velocity, so you can infer your heading. But if you're standing still, it can't tell your heading.
You just pointed out a case I hadn't even thought of, which is turning in place. Airplanes and missiles don't do that <grin>.
Ken, forget I even posted. Everything I said was wrong; you do need a compass.
Jack
and to hammer it in one more time for clarity... thats·basically the·right math to convert it, but i think i have to do something extra because when you enter a lat into an online converter it makes you split up the 3819.5882· into somthing like· 38 19 58.82· and im not taking into account that last decimal. but im probably just totally clueless what im supposed to do with it.
heres a new page i just found with some good info though... http://home.online.no/~sigurdhu/Deg_formats.htm
Post Edited (s2jesse) : 3/11/2009 5:38:45 AM GMT
so anyway that should be correct for anyone who needs to convert their ddmm.mmmm to dd.dddd
1. Why bother with floating point at all? Couldn't you just take 123.8854 degrees and ignore the decimal point altogether, leaving 1238854? Better yet, as another poster suggested, pick a reference point, say 123 degrees, and subtract it from all your readings, leaving 8854 in the example above. Unless you're navigating the world, you dont need those extra digits.
2. Arctangent: who needs it? The HM55B provides direction as (x,y) (i.e. cos/sin) coordinates. The difference between where you are and where you want to be is also specified in (x,y) coordinates. If you're pointed toward your target, the direction sines and cosines between your heading and the target bearing will be equal. If they're not, you can figure out which way to correct without knowing the actual angle. These trig identities will give you the turn information:
····sin(turn) = sin(bearing - heading) = sin(bearing)cos(heading) - cos(bearing)sin(heading)
····cos(turn) = cos(bearing - heading) = cos(bearing)cos(heading) + sin(bearing)sin(heading)
... no actual angles or arctans required! Besides, once you start dealing with actual angles, modulo arithmetic (e.g. 270 + 180 == 90) rears its ugly head.
-Phil
You are correct on all points.
1. Simplify the GPS math with a reference point.· Remember that floating point normally gobbles up a spare cog or two.· Once the project gets close to being finished, you will need all of the cogs that you can get.
2.· ArcTan is not required.· I would not use it if it took up extra cogs or took too much time.· The arctan function that I use is simple, reasonably fast, reasonably accurate, and does not eat cogs.· The Lego RoboMagellan competitors use a vey simple method that ignores all trig functions (uses just x and y differences) and is very, very·inaccurate, but it works surprisingly well.
Modulo arithmetic must be considered in your programming.· Also, make sure that your functions work in all four quadrants.· Finally, watch for strange things happening at 0, 90, 180, or 270 degrees.
··· Rick Brooks
Fixed my GPS connection. After tearing down my level shifter circuit, trying direct connection to different Propeller pins, and even analyzing it all on a scope, finally realized that in fact the problem was that I had the Tx and Rx pins set to be the same in the GPS Float object. The driver sets the tx pin high, then the GPS can't pull it low.
I quite agree with Phil, it seems remarkable to me that someone would haul in floating point on a microcontroller program just to handle what is basically a cartesian rectangular coordinate system. For gosh sakes people it's the same coordinate system used to call out squares in the board game "Battleship"; there's no reason to make it complicated. Just because there is a "." in the lat/longitude strings, does not mean it has to be floating point! Furthermore, why represent values in degrees, requiring a divide by 60? Why not use minutes as the basic unit, then you can multiply degrees by 60 and add to the minutes.
Edit: You can find out the ratio of minutes to cm using this online calculator: www.ngs.noaa.gov/cgi-bin/Inv_Fwd/inverse2.prl. It will be different for different latitudes, so you would adjust a CON section in the object for what part of the world you're in. The way to get the calculator to tell you the ratio is to pick a GPS lat/long near where you are, and enter it, then for the second point just enter that same number +.0001 on the latitude. The distance it calculates for you is your 1/10000th of a minute to cm ratio; repeat same process for longitude.
Post Edited (Dennis Ferron) : 3/14/2009 1:47:24 AM GMT
Great work on your utilities.. I· spent like 2 days trying to figure out the spherical Law of Cosines method as well. It was driving me nuts. The haversine meathod works like a charm! The distances are accurate to within what apears the gps's error margins.
After that I was trying out your bearing code as well but i cant seem to get·clean output from it. Im converting the miles formated distance back to rads using... (not concerned with optimzing anything at this point obvioulsy [noparse]:)[/noparse]
·before i feed it into the bearing code but i seem to output 6.805647e+38.·Have you succuselfully got the bearing code to work for you in your spin code?
longitudinal distance only. I rewrote the bearing routine, but I do not
have a board right now to test on. Give the following a try if you get
a chance. My math skills are pretty rusty, so if there is a mathematician
in the crowd, an algorithm check would be welcome.
Seems right on the money! verfied my output with google earth.