Basic GPS algorithms ?

[Steve " 'dillo" Okay]

Does anybody out there, looking at you RoboMagellan competitors in particular, know of any docs/HOWTOs, etc. for doing basic GPS point-to-point algorithms.

Repeated Google searches turn up lots of guides on street-navigation, treating blocks as nodes on a graph, which is great if you're trying to build a delivery-bot or doing one of the self-driving car nano-degrees. What I'm really after is help w/ off-road, overland navigation for Tenacity(and subsequent rovers).

I had something mostly working around 2014/2015 for the one or two times I did RoboMagellan but that code seems to be lost to the ages now. 

My thinking here is I can probably write my own point-to-point solution in less time than it would take to get somebody else's stack, tear it apart, then beat it to fit & paint it to match.

Thanks,

'dillo

[Alan Federman]

Subtract where you are from where you want to go to get a direction for a heading. Rotate, move forward, take another reading until you get to where you want to be.

--
You received this message because you are subscribed to the Google Groups "HomeBrew Robotics Club" group.
To unsubscribe from this group and stop receiving emails from it, send an email to hbrobotics+...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/hbrobotics/975df05a-91a0-4610-9fc8-316e84f90e3an%40googlegroups.com.

[camp .]

There's probably a more sophisticated way to do this but this is what I do.

----------------------- snip --------------------------

Loop()

Set Goal Target GPS Latitude and Longitude.

Read robot's Actual GPS Latitude and Longitude.

Compare Target Latitude with Actual Latitude.

Compare Target Longitude with Actual Longitude.

If the Target Latitude is greater than the Actual Latitude the robot needs to travel North.

If the Target longitude is greater than the actual longitude the robot needs to travel East.

Determine direction robot needs to travel: North, NorthEast, East, SouthEast, South, SouthWest, West, or NorthWest in World coordinates.

// Bearing cones of travel (~45%)

// North = 338-359 or 0-22      // (45°)
// NorthEast = 23-67        // (45°)
// East = 68-112                        // (45°)
// SouthEast = 113-157      // (45°)
// South = 158-202          // (45°)
// SouthWest = 203-247      // (45°)
// West = 248-292           // (45°)
// NorthWest = 293-337      // (45°)

Read the robot's direction (bearing) to determine what direction the robot is traveling.

Turn so that robot is oriented in the Target coordinates direction (Goal) and travel forward avoiding obstacles along the way.

loop();
----------------------- snip --------------------------

- Camp

Sent from Yahoo Mail on Android

[Sergei Grichine]

Point-to-point is easy, if you REALLY know your pose (attitude) and location. The fun begins when your GPS is jumping around within 3-30 meters of your actual location, and your magnetometer (compass) is randomly off about 10 degrees, and sometimes goes crazy. 

In real life you need imu9250 or similar and an "EKF2 attitude estimator" for your robot's heading. Google that. Extra compass, like RM3100, helps a lot.

The GPS problem is even worse. I personally don't want to deal with it, and my solution is to use RTK GPS. Saves you a lot of headaches. 

When it comes to the code, I'd say learn from the best - Ardupilot and PX4 is Open Source, and there's probably plenty in ROS.

Best Regards, 

To view this discussion on the web visit https://groups.google.com/d/msgid/hbrobotics/372754241.454426.1654279526030%40mail.yahoo.com.

[Chris Albertson]

What do you need to know?  I worked on a geographic information system project for about 10 years as a software engineer.  Most of what you need is just basic math, although spherical geometry is not something that is taught to most students.   But for a small area like where a robot would need to move, you can assume the Earth is flat.   It is different if you are moving over 100 kilometers.   For small areas, just assume a plane.

So if you assume a flat Earth, the basic thing you do first is finding your latitude and then compute the ratio of meters per degree of longitude and meters per degree of latitude.   Then you can easily convert (lat. long) to (x,y)  And from there on you can use plain geometry.

So, on power-up, find your ligation in (lat, long) and remember it.  This is your "zero point of origin.  Computer the correct degrees to meters for you latitude.   Then after this, for every (lat, long) pair you get subtract the zero point and convert to meters.   this gives you (x,y) relative to the place where you first applied power.  This works of level ground if you stall within 10 or 20 or so miles

--
You received this message because you are subscribed to the Google Groups "HomeBrew Robotics Club" group.
To unsubscribe from this group and stop receiving emails from it, send an email to hbrobotics+...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/hbrobotics/975df05a-91a0-4610-9fc8-316e84f90e3an%40googlegroups.com.

--

Chris Albertson
Redondo Beach, California

[Chris Albertson]

Sergei, told you the best solution.  Just ignore this problem and let ROS' "localization" package do the work for you.  It has a Kalman filter and will give the best possible location estimates given the data to send it.

It is best to send it data from GPS and a good 9-DOF IMU and wheel odometry (even if the wheels do slip).  You can even install two IMUs to get even better data  The most data sources toy send it the better it will be at estimating location.

But if you are writing your own code, Do what I said and convert to meters and assume a flat Earth.    Then write your own Kalman Filter

 kind of competition, then you REALLY want to use this

http://docs.ros.org/en/api/robot_localization/html/index.html

because if you don't, the others will use it and beat you.

To view this discussion on the web visit https://groups.google.com/d/msgid/hbrobotics/CA%2BKVXVNHdqnbtJT5XAKNgHiihzRsgFao49M%2Bq8DpsEENxxnb8g%40mail.gmail.com.

[Ralph Hipps]

Steve,

Obviously there are two basic approaches, use ROS or don't.    =)

For us non-ROS types, the way to calculate course & distance to your next waypoint is basically what Alan said.

You can calculate the new course and distance to your next waypoint (wayPointIndex + 1) from your current GPS coordinates (wayPointIndex) like so:

#include <math.h>

// waypoint[0] is starting point
int wayPointIndex;            // to track next waypoint destination
float wayPoint[8][2] = {0,0}; // lat (n/s) & lon (e/w) gps coords for path planning
int wpCone[8];   // is there a cone at next wp?

float deltaLAT, deltaLATdistance;  // change in lat, distance in feet to next waypoint
float deltaLON, deltaLONdistance;  // change in lon, distance in feet to next waypoint

int newCourse;  // North is 0°, east is 90°, south is 180°, and west is 270°.
float distance;  // in feet

long ftpdegLat = 364320L;  // feet per degree of latitude
long ftpdegLon = 289800L;  // feet per degree of longitude in San Jose, CA

    // determine lat/lon changes from GPS coordinates for new course
    deltaLAT = wayPoint[(wayPointIndex + 1)][0] - wayPoint[wayPointIndex][0];
    deltaLON = wayPoint[(wayPointIndex + 1)][1] - wayPoint[wayPointIndex][1];

    // calculate course to destination waypoint
    newCourse = int(atan2(deltaLON, deltaLAT) * 57.296);  // 360/2pi  // lon/lat to match compass headings
    //  newCourse = atan2(deltaLAT, deltaLON) * 57.296;   // 360/2pi

    if (newCourse < 0) {
      newCourse = 360 + newCourse;  // North is 0°, east is 90°, south is 180°, and west is 270°
    }

    //How many feet are in 0.001 minutes? 6.074, 4.83
    deltaLATdistance = deltaLAT * ftpdegLat;
    deltaLONdistance = deltaLON * ftpdegLon;

    distance = sqrt(sq(deltaLATdistance) + sq(deltaLONdistance));  // a^2 + b^2 = c^2 for a right triangle

As Sergei & Chris have mentioned, there are issues with raw GPS for navigation and the trick is to figure out how you want to solve that problem.

You can spend big $$ on exotic cm accurate RX, or use kalman filters and/or ROS, etc., etc.

I tried a relatively low cost GPS RX from DF Robot and it was basically useless due to the scattering issue, so the struggle is real.

[Ralph Hipps]

perhaps useful info:

https://learn.sparkfun.com/tutorials/what-is-gps-rtk/all

[Chris Albertson]

I think there is an error below.  "atan2(deltaLON, deltaLAT)" only works if you are very near the equator you need to convert from angles to linear units like meters BEFORE computing the atan2.  So try this atan2(deltaLONdistance, deltaLATdistance)  But of course, also move its down to after you have computed deltaLONdistance, deltaLATdistance.   Finally remember that "ftpdegLon" is not a constant but is a function of your Latitude.

But using raw GPS like this will give very poor results if you are close (10 meters or so) to the waypoint.  You REALLY must fuse the IMU and odometry BEFORE finding the heading and distance.

Finally,  The OTHER thing you can do with this GPS data is find you actual headinthat the robot drove. Do the same as you do for "newCourse" but use the last locaton and the current location and compute something called perhaps "oldCourse"  This will tell you where the robot is facing.    Then subtract them to find how much you need to turn.

Also concider leaving the course in radians.   Why convert to degrees unless it is for a user display.  Any kind of computation will require redians

But in any case, no mater what you decide, you need to convert to distance before you do the atan2() functon.

 

To view this discussion on the web visit https://groups.google.com/d/msgid/hbrobotics/63ff8f61-1a19-4545-b235-9c6587d69560n%40googlegroups.com.

[Ralph Hipps]

Chris,

Appreciate the feedback. I haven't used this code in years, so there could always be room for improvement.

I didn't mention that I also used an IMU for compass headings, so the old course info would be known. I used that to compute which way to turn to my new course.

Agree about ftpdegLon, I calculated what it would be here in San Jose and used that as a fixed number.

And defnly agree about the poor results, which is why I haven't used this approach in years.    =)

Ralph

>+<

 |    /\
 |___/  \
 [_@_]   `<
 O O O          DL Robotics!

------------------------------------------------------------------------
In the twenty-first century, the robot will take the place

which slave labor occupied in ancient civilization.

 -- Nikola Tesla
------------------------------------------------------------------------

You received this message because you are subscribed to a topic in the Google Groups "HomeBrew Robotics Club" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/hbrobotics/Z8U8QRZmEk4/unsubscribe.
To unsubscribe from this group and all its topics, send an email to hbrobotics+...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/hbrobotics/CABbxVHs5ZsveCF5ms3UHMgdTddcOs8-gtTEK7Swug_WFDJArGw%40mail.gmail.com.

[Steve " 'dillo" Okay]

I've made some pretty good progress with object recognition, object tracking, etc. using the OAK-D camera and some onboard OpenCV code of my own

but I also have this really nice Microstrain GPS/GNSS/IMU(Which does its own on-device sensor fusion) that's onboard and hooked up but I haven't done much with and with it being summer and all,

I felt like I should really get back to working on autonomy. Since I'm not doing a delivery-bot, I was looking for a more generalized lat/long source/destination computing approach.

I don't have the encoders on the motors hooked up(mostly because the wiring rats-nest that involves) and have been looking at optical flow for visual odometry. 

Thanks for all the replies, it looks like I have multiple paths/mechanisms forward.

'dillo

[Steve " 'dillo" Okay]

If I recall correctly, it's been good enough to get you to the cone and where you need to go, so it can't be that bad! :)

'dillo

[Ralph Hipps]

Well, I didn't use this approach to achieve that, I had switched to a more basic 'course & distance' approach using my IMU compass and wheel odometry.

I have a routine I can use to calculate course & distance to each waypoint from the GPS coordinates of each cone, or I can also walk the course and count steps, use my iPhone compass for headings, etc.

I just needed to get close enough so that the PixyCam could see the cone and take over driving the last 10-15' or so.

Ralph

>+<

 |    /\
 |___/  \
 [_@_]   `<
 O O O          DL Robotics!

------------------------------------------------------------------------
In the twenty-first century, the robot will take the place

which slave labor occupied in ancient civilization.

 -- Nikola Tesla
------------------------------------------------------------------------

You received this message because you are subscribed to a topic in the Google Groups "HomeBrew Robotics Club" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/hbrobotics/Z8U8QRZmEk4/unsubscribe.
To unsubscribe from this group and all its topics, send an email to hbrobotics+...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/hbrobotics/c400d7bc-cd4b-4555-bf3e-276649c3240bn%40googlegroups.com.

[Ralph Hipps]

sorry, replied before seeing the context, not shown in gmail.

Ralph

>+<

 |    /\
 |___/  \
 [_@_]   `<
 O O O          DL Robotics!

------------------------------------------------------------------------
In the twenty-first century, the robot will take the place

which slave labor occupied in ancient civilization.

 -- Nikola Tesla
------------------------------------------------------------------------

[Chris Albertson]

There is no way your IMU will be aligned to GPS heading.   Even with a magnetomotor, there is magnetic declination.  This is on order of about a 10-degree error.  But even worse, the gyros in the IMU drift badly and must be continuously recalibrated.   There are some good ways to do this.  Look up "AHRS" algorithms.   A low cost IMU is not useful without this.  Here is a good intro https://learn.adafruit.com/ahrs-for-adafruits-9-dof-10-dof-breakout/sensor-fusion-algorithms.  The IMU become dramatically more reliable after applying one of the AHRS filters.    

These $20 IMUs are bad enough that if you program a robot to follow an IMU heading it will drive in a circle.  It's worse with my robt-dog.  If I use the IUM to keep the body level, then over time the robot will lean and tip over.  We have to measure and compensate for drift.  The algorithm in the above link does this.

But still, True North and Magnetic North are not the same and the only way to compare them is to look up the variation for your location.  (The magnetic pole is someplace in Greenland and it moves.)

This is all good if you want to learn how it works.  I guess there are a few different options

1) don't care about absolute location, navigate based on vision

2) learn about this and write your own code to do sensor calibration and fusion

3) Use someone else's code to do the above but this means Linux/ROS and so on

4) Spend some money and buy an GPS RTK system and works if you have a good signal.

To view this discussion on the web visit https://groups.google.com/d/msgid/hbrobotics/CADjBhasAbaqzHznwLnorSCdJJ5K_HPwNtYkXY1KVkw_dt90MLw%40mail.gmail.com.

[dpa]

The equations used to calculate the distance and heading between two lat/lon locations used by seismologists and such use the angular distance from the center of the earth, which works the same anywhere on the globe.   Probably not needed for our little 'bots that travel only a few miles.   But if you want to send your autonomous robo-boat or weather-balloon to distant shores you'll need it.  <grin>

For reference here is the common solution:

Distance and heading between two latitude/longitude coordinates. 

First coordinate pair is lat1,lon1, and second pair is lat2,lon2.

a. Define degrees per radian, nautical miles per degree and meters per nautical mile:

#define RADS (180/M_PI)
#define NAUTICAL_MILES 3437.7387
#define METERS_PER_NM 1852

b. Calculate angular distance between two lat,lon pairs:

a_distance = acos(sin(lat1/RADS)*sin(lat2/RADS) + cos(lat1/RADS)*cos(lat2/RADS)*cos((lon1-lon2)/RADS));
n_distance = a_distance * NAUTICAL_MILES; 

c. Calculate azimuth angle from lat1,lon1 to lat2,lon2:

 

azimuth = acos((sin(lat2/RADS) - sin(lat1/RADS) * cos(a_distance)) / (cos(lat1/RADS) * sin(a_distance)));

d. Bearing and distance from lat1,lon1 to lat2,lon2 in degrees and meters are:

 

degrees_azimuth = azimuth * RADS;
meters_distance = n_distance * METERS_PER_NM;

David

[dpa]

Brain freeze.   

#define NAUTICAL_MILES  59.2957795

sorry...

dpa

[dpa]

Been a while since I looked at this!   Angular distance is in radians, so the original post is correct.   Think I'll take a nap now.