Waypoint Navigation

[Ted Meyers]

I'm currently working on improving my waypoint navigation code.  Specifically, I have had problems detecting when the rover has reached a waypoint.   Way back, I wrote a piece of code something like this:

WaypointReached = (current_position == waypoint_position);   // yeah, this is clearly pseudocode

It was quickly obvious that this would not work, as it is extremely unlikely that the current position will ever be exactly the same as the waypoint.

To fix this, I wrote some code to check if the position was within some threshold distance of the waypoint.  This works, but has some issues.  First, if the threshold is too large, the driving can be imprecise.  And second, if the threshold is too small, we are back to the previous problem -- the rover starts circling the waypoint, trying unsuccessfully to get to it.  I did have some success tuning the threshold to get something that usually works well, but still occasionally ran into problems.

Next I tried detecting the missed/circling waypoint issue using trigonometry to determine if the position is "beyond" the waypoint.  The downside here was that this approach uses a lot of math computation.  And there were a lot of edge cases, especially when I tried to take shortcuts in the math.

Now, I think I have found a clever solution to the whole problem: don't look at how close the rover is to the waypoint, look at how far the rover is from the initial distance to the waypoint.  In other words, when starting navigation to a waypoint, compute the distance to that waypoint; then drive that distance and assume that the waypoint was reached.  Okay, the rover could get in trouble if it goes in a completely wrong direction, but if that is the case, it has bigger problems anyway.  So, we can safely assume that the navigation code is at least trying to steer towards the waypoint.  In which case, it is always getting closer to the waypoint, so we can assume that when it has driven the correct distance from the starting point, it has reached the waypoint, or close enough.  This simplifies everything.

(Oh, and I think that it is safe to work in units of distance squared, so that the square root doesn't need to be calculated.  As long as the numbers don't get too big and overflow.)

I'm sure that I'm not the first to have thought of this, and I'm wondering if anyone else has tried it?  Or does anyone see any big problems with it?  I've tried it in my rover simulator and it seems to work, but haven't had a chance to try it in real world conditions yet.

Anyway, I'm open to comments.

Ted

[Joep Suijs]

Hi Ted,

Next I tried detecting the missed/circling waypoint issue using trigonometry to determine if the position is "beyond" the waypoint.  The downside here was that this approach uses a lot of math computation.  And there were a lot of edge cases, especially when I tried to take shortcuts in the math.

This is what I use. It works much better than remaining distance because - like you wrote - the robot will pass the destination at some distance. I don't consider this 'a lot of math computing' though. Basically some additions, subtractonds and an arctan. You're done when the driving direction differs more than 90 degrees from the vector from the robot to the waypoint (except maybe at the beginning). 

 

Now, I think I have found a clever solution to the whole problem: don't look at how close the rover is to the waypoint, look at how far the rover is from the initial distance to the waypoint.  In other words, when starting navigation to a waypoint, compute the distance to that waypoint; then drive that distance and assume that the waypoint was reached.  Okay, the rover could get in trouble if it goes in a completely wrong direction, but if that is the case, it has bigger problems anyway.  So, we can safely assume that the navigation code is at least trying to steer towards the waypoint.  In which case, it is always getting closer to the waypoint, so we can assume that when it has driven the correct distance from the starting point, it has reached the waypoint, or close enough.  This simplifies everything.

Clever indeed! For maximal accuracy, you need to calculate the distance between the last waypoint and the robot. The actual distance traveled could be longer because of an arc while changing direction when passing the previous waypoint.

Joep

[Jon Watte]

What I've done, which is reasonably efficient, and guarantees that I go "past" the waypoint, is:

Pre-calculation each time I want to find a new waypoint:

1) When determining the next waypoint (WP), calculate the vector from current position to next waypoint. This is only used when "switching to" the waypoint.

2) Normalize this vector (VN), and remember the start point (SP).

3) Calculate length along this vector between target waypoint and start point: D1 = dot(WP-SP, VN)

Navigation determination:

4) Calculate the distance of the rover position to the start position along the determined vector. If it's greater than D1, I passed the plane determined by the V1 normal and the WP point.

DR = dot(RP-SP, VN)

time_for_next_waypoint = DR > D1

"normalize," "dot" and "vector difference" are all super simple in 2D;

normalize(x,y) = (x*scale, y*scale) where scale = 1.0/Sqrt(x*x+y*y)

dot((x1,y1),(x2,y2)) = (x1*x2,y1*y2)

sub((x2,y2),(x1,y1)) = (x2-x1,y2-y1)

So, this is "distance away from start point" but only counts distance along the vector from start to waypoint. If you go sideways, you'll have to go further to "pass the gate."

Sincerely,

jw

Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

--
You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/CABTh0a1%3DCFzx3ABuxoTbO%2B5_VMty2mCOU2HurJK5djqiucVjMA%40mail.gmail.com.

For more options, visit https://groups.google.com/d/optout.

[Ted Meyers]

Thanks for the comments!

Joep:  yeah, I'm sure that I made my calculations a lot more involved than I needed to (trying to calculate turning radius intersections, etc).  It appears that you came up with a much more elegant solution.

Jon:  I like your solution, it makes a lot of sense to me; along the lines of what I was thinking, but with the added sophistication of projecting the distance onto the driving vector.

To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

[Michael Shimniok]

On Jan 27, 2018 2:04 PM, "Jon Watte" <jwa...@gmail.com> wrote:

Pre-calculation each time I want to find a new waypoint:

1) When determining the next waypoint (WP), calculate the vector from current position to next waypoint. This is only used when "switching to" the waypoint.

2) Normalize this vector (VN), and remember the start point (SP).

3) Calculate length along this vector between target waypoint and start point: D1 = dot(WP-SP, VN)

That's what I do too, more or less. My code calls a nav / steer update routine periodically that:

. determines current position, 

. projects that onto the line between previous and next waypoint via dot product,

. determine distance from projected point to waypoint

. If distance < wpt threshold distance, waypoint++

And then for nav and steering,

. Add lookahead distance to projected point

. Compute radius of Arc between current and lookahead point

. Compute turning radius to steer that arc

When switching waypoints the path following algorithm above treats it as track error, steers accordingly, and exhibits nice smooth transitions between waypoints without any added code.

Lookahead and waypoint thresholds are independent config settings by necessity.

(forgive me but I haven't looked at that code in 3+ years and I am not blessed with a good enough memory to guarantee the above is 100% accurate. See my blog posts on data bus for verification) :)

[Jon Russell]

I do something similar... I have a PID controller that tries to keep the steering on the vector to the next waypoint. If the distance to the way point was less than 2 or 3 meters, we’re very close. At this point these it a danger the bot will drive in circles, as the turning radius is greater that the heading it needs to hit the way point exactly. So, once we get close, I don't change the steering to steer to the waypoint, instead, continue on the heading I was on just before we got close. You can assume the heading I was on would hit the waypoint pretty close. So, if I stay on that heading, and monitor the GPS vector to the waypoint, as I pass the way point I should see the GPS vector increase to 90 degrees as I pass by the waypoint. i.e. the vector will go from straight ahead, to 10, 20, 30, 45, eventually 90 degrees. At this point I’m within X meters and its 45 degrees to my left or right, so, that’s a pretty good assumption of hitting the way point, so I assume I’ve passed it, and move on to the next way point.

        if ((GPSDistanceTo < nMinDistance) & (!bTooClose)){
            // within Xm of waypoint...
            sprintf(szTmp, "%.2f meters to waypoint %d (I'm at %f %f)", GPSDistanceTo, CurrentWaypoint, gps.latitude, gps.longitude);
            LogFile(szTmp);
            bTooClose = true;
            fLastGoodCourse = GPSCourseTo;
        }


        // PID Input is delta from CourseTo.
        PID_Input = CompassHeading - GPSCourseTo;


        if (bTooClose){
        // if within Xm
            if ((PID_Input*PID_Input)>=2000) {
                // if waypoint is more than 45 deg, then we've gone past it.
                bTooClose = false;
                CurrentWaypoint++;  // next waypoint
                sprintf(szTmp, "Next waypoint : %d (I'm at %f %f)", CurrentWaypoint, gps.latitude, gps.longitude);
                LogFile(szTmp);
            } else {
                // if waypoint isn't more than 45 deg, keep going straight. Override PID_Input
                PID_Input = CompassHeading - fLastGoodCourse;
            }
        }

[Michael Shimniok]

Here's the relevant sections of my code in case you want to look more closely...

The waypoint threshold / transition magic is in updater.cpp, within update() which is called on an timer interrupt:

bearing = cartHere.bearingTo(config.cwpt[nextWaypoint]);

distance = cartHere.distanceTo(config.cwpt[nextWaypoint]);

float prevDistance = cartHere.distanceTo(config.cwpt[lastWaypoint]);

if (distance < config.waypointDist) { // Check if we're close enough to waypoint to switch

  lastWaypoint = nextWaypoint;

  nextWaypoint++;

  cteI = 0;

}

I misspoke earlier; the above only looks at plain old distance from current position to next waypoint. Only the path following uses the dot product to project current position onto line between prev/next waypoint (the current 'leg', that is), etc.

The steering angle calculations & path following are found in Steering.cpp -- the pathPursuitSA() member function is the one I use (the other options are experimental).

float Steering::pathPursuitSA(float hdg, float Bx, float By, float Ax, float Ay, float Cx, float Cy)

{

  float SA;

  // Leg vector

  float Lx = Cx - Ax;

  float Ly = Cy - Ay;

  // Robot vector

  float Rx = Bx - Ax;

  float Ry = Cy - Ay;

  // Find the goal point, a projection of the bot vector onto the current leg, moved ahead

  // along the path by the lookahead distance

  float legLength = sqrtf(Lx*Lx + Ly*Ly); // ||L||

  // R dot L/||L||, projection magnitude, bot vector onto leg vector

  float proj = (Lx*Rx + Ly*Ry)/legLength; 

  float LAx = (proj+_intercept)*Lx/legLength + Ax; // find projection point + lookahead, along leg

  float LAy = (proj+_intercept)*Ly/legLength + Ay;

  // Compute a circle that is tangential to bot heading and intercepts bot

  // and goal point (LAx,LAy), the intercept circle. Then compute the steering

  // angle to trace that circle.

  float brg = 180*atan2(LAx-Rx,LAy-Ry)/PI;

  float relBrg = brg-hdg;

  if (relBrg < -180.0)

    relBrg += 360.0;

  if (relBrg >= 180.0)

    relBrg -= 360.0;

  float sign = (relBrg < 0) ? -1 : 1; // found it best to use absolute value, restore sign later

  // The equation peaks out at 90* so clamp to 90. if theta >= 90, we select max steering

  if (relBrg > 90.0) relBrg = 90.0;

  // Compute radius based on intercept distance and specified angle

  float radius = _intercept/(2*sin(fabs(relBrg)*PI/180));

  // Now compute the steering angle to achieve the circle of 

  // Steering angle is based on wheelbase and track width

  SA = sign * angle_degrees(asin(_wheelbase / (radius - _track/2)));

  return SA;

}

The last equation is how I was able to run the same exact code on Data Bus (1:10 scale RC car) and Troubled Child (1:1 scale 86 Jeep Grand Wagoneer) simply by changing the configuration file values for track width, wheelbase, and a couple other parameters. :)  

--

You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/16d544ea-6a2c-4aa5-a454-1b8b4d46a782%40googlegroups.com.

[Ted Meyers]

Thanks for the all the code everyone, I'm in the process of slowing assimilating the best of it!

I really like how the dot product can be used to set a pursuit point and also be used to detect waypoint completion.  Angle to waypoint also works for detecting waypoints, but I probably won't use it -- it is just too different than what I'm used to doing.  

Computing steering angle (like Michael does) is also something I had on my list of things to look into.  I'm currently using a PID on the pursuit point heading from the current heading (like Jon R. has in his code).  It works quite well, so I don't really have a need to calculate the actual steering angle, unless I'm missing something.

I think I am very close to the point where my waypoint navigation works almost flawlessly, at least for the software part (it could be helped by better heading sensors).  The big problem is obstacles.  This last AVC was the first time that the barrels where moved around before the heat.  Meaning that I couldn't just program the waypoints to go around them.  My AVC runs did not go well.  I did have some some code hacked in to detect when an obstacle was hit and backup and try to go around, but it wasn't very robust.  I'm still trying to figure out how to plot a path through obstacles (assuming their locations are found) and then get back on the waypoints.  And determining which waypoints if any should be skipped.  I should probably look at SLAM some more, but from what I remember, it is more about position and mapping than navigation.

[Ted Meyers]

On Monday, January 29, 2018 at 8:56:27 AM UTC-7, Michael Shimniok wrote:

The waypoint threshold / transition magic is in updater.cpp, within update() which is called on an timer interrupt:

Michael,

I'm curious how you are able to do the update on a timer interrupt?  Maybe you do a lot less inside of your update?  In mine, I read sensors, check for collisions, update the PIDs, update the position and heading, update the pursuit point, calculate the steering and throttle, update the steering and throttle output, log data, check buttons and set status LEDs and LCDs (and I'm sure I missed something).

My concern is that other timers and interrupts need to also work, so you can't just disable them for the whole update (wheel encoders, PWM, etc)  And you have to deal with variable volatility and atomic operations.  So?  Maybe these issues aren't as big a concern as I thought?  Or more likely, you have a lot less in your update function and do the other stuff somewhere else?

The way I do it is that for any function that takes any significant amount of time to execute, I just periodically sprinkle in calls to update (and inside update is a time check that immediately returns if the time period since last called is too short).  This is an annoying way to have to code, but it does work.

[Jon Watte]

For the barrels, I think that two ultrasound sensors (slight-left, slight-right) could be all you need. If you stuck them on a servo, you could even stop and sense the best path to go.

Simple barrier evasion would likely be effective because the barrelled part is so short, you don't need real path planning.

Sincerely,

jw

Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

--
You received this message because you are subscribed to the Google Groups "diyrovers" group.

To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/c80b0de5-ca30-4fe3-9969-6bc8be3ced0a%40googlegroups.com.

[Joep Suijs]

Op wo 31 jan. 2018 om 20:14 schreef Ted Meyers <ted.m...@gmail.com>:

The way I do it is that for any function that takes any significant amount of time to execute, I just periodically sprinkle in calls to update (and inside update is a time check that immediately returns if the time period since last called is too short).  This is an annoying way to have to code, but it does work.

A time check in each update makes sense if you have independent 'tasks'. I found that many functions on my robot are not independent, but best called once per control cycle in a specific order: First read sensors, then update position, etc, up to writing new PWM values. You could do one time-check to start the control cycle and then call all functions.

Joep

 

[Michael Shimniok]

I'm curious how you are able to do the update on a timer interrupt?  Maybe you do a lot less inside of your update?

That is part of it...

  In mine, I read sensors, check for collisions, update the PIDs, update the position and heading, update the pursuit point, calculate the steering and throttle, update the steering and throttle output, log data, check buttons and set status LEDs and LCDs (and I'm sure I missed something).

I do the logging and LCD display updates outside the timer interrupt handler. I found through testing that writing to microSD card can take much longer than even the most involved calculations. Since it isn't super time critical, do it in the main while loop. Same with the LCD and serial output for telemetry. I basically have a circular buffer of a struct called system_state (or something) that contains all the system state variables like speed, position, heading, etc.

Like you my update() is doing the position and heading estimation, path following and steering, throttle PID, and reading sensors (GPS and gyro).

The timer runs at 100Hz and I time it using a microsecond timer and execution time leaves a more than comfortable margin to get everything done.

However, at one point during 2012 development, I recall running into issues with the position calculations taking too long, so I converted to Cartesian coordinates instead of lat/lon. Even then, this was only an issue when I started correcting gyro bias.

My concern is that other timers and interrupts need to also work, so you can't just disable them for the whole update (wheel encoders, PWM, etc)

Right. My only time-critical interrupt besides the update() routine is the wheel encoder pin change interrupt. If memory serves me (after 5 years lol) the pin change interrupt is higher priority, by default, than the timer interrupt on LPC1768. 

Unlike the Arduino Servo library implementation that uses a timer, on mbed the Servo library PWM is handled completely in hardware. So no interrupts to worry about there. On Arduino, one could use (or write) a hardware-only Servo library.

LPC1768 uses a nested, vectored interrupt controller (NVIC). Nested: where one interrupt can be serviced immediately in the middle of executing an interrupt handler for another interrupt. Otherwise, the flag is set and the interrupt is serviced after the current handler returns. If nested interrupts are disabled, you would want to make sure update() execution time is less than the time between encoder interrupts so that you don't miss any. Also, you can change the interrupt priority if you want. I didn't need to.

 And you have to deal with variable volatility and atomic operations.

Indeed, I did have to think about both concerns. I'd have to go back and review the code in more detail. I would think any global variable modified by an interrupt handler would need to be declared volatile. And one would have to look at race conditions for same. In my code, only the wheel encoder counters and the system state buffer fall into that category.

  So?  Maybe these issues aren't as big a concern as I thought?  Or more likely, you have a lot less in your update function and do the other stuff somewhere else?

Not spending lots of time writing to log file (varied in my case from a few ms to 150ms (YMMV)) probably made things simpler. And then carefully selecting how best to "share" variables between ISRs and main code. 

The way I do it is that for any function that takes any significant amount of time to execute, I just periodically sprinkle in calls to update (and inside update is a time check that immediately returns if the time period since last called is too short).  This is an annoying way to have to code, but it does work.

Sure. Cooperative multitasking :) Ala old MacOS :)

I wanted to make sure update() was called at a fairly strict interval for sake of estimation.

Michael

[Chuck E]

Hey Ted,

GPS waypoints, correct? I am not sure if anyone has said this, or how accurate what I am about to say - is.  

From what I understand about GPS and using GPS way points.  

GPS is accurate within 3 meters.  That is, a 9ft, by 9ft square (of course.).  When your system moves, so does your  9'x9' square.  This has troubled a lot of us in the past because in some situations if your waypoint  is inside the box, your system could end up going in any direction (worse scenarios while navigating while moving). 

Not sure if this helps at all, but it would explain why it would work in a simulator, and maybe have error outside.

I like the idea of calculating track from last position...  Obviously if this is old news, or completely wrong...  Please let me know.

[Jon Watte]

Most of us probably use a local coordinate system (I use meters East/North from origin at start line.)

GPS is used as an input to a position estimated estimator, which also takes other inputs, like IMUs, wheel encoders, optical flow pose estimators, and so forth.

Separately, relative GPS is more accurate than absolute. Establish a zero at the start point, and calculate delta. This compensates for certain kinds of errors.

If you can add an RTK correction stream, you can get to centimeter precision, even while moving. Navistar and similar products do this. Two years ago, that was the winner at AVC! But it makes it too easy (at the expense of a rather expensive product) so I think RTK is banned now.

Jon

--
You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/02e4a9be-3cbd-49f0-af1a-25cfccb257ee%40googlegroups.com.

[jesse brockmann]

I'm pretty sure Ted doesn't use GPS.  There is often a point bonus for not using gps at Sparkfun AVC.  So the majority don't use it.

Jesse

[Ted Meyers]

Yeah, I don't use GPS.  But if I did (actually when I did),  it was used with a complimentary filter, so that GPS jumpiness couldn't affect the short term position much.   You might want to try this, it will solve most of the problem you described.

I don't agree that relative GPS is any more accurate than absolute.  From what I've heard and seen they are the same,  you just have a better starting position.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/CANqFZg3PQ5C7ifywNCmvFDzp_Q1sG%2Bp0fp4eF4iGtUUq-4D0kw%40mail.gmail.com.

[Jon Watte]

Relative GPS is better for short durations.

I model GPS as periods of slow linear drift, terminated by a bigger discrete jump.

I believe the slow drift in atmospherics/movement, and the big jump happens when the receiver switches satellites.

If you can measure the drift when you stand still, or compare it to wheel encoders, you can get much better error correction than a basic Kalman filter, for typical input data. And if you get a sudden discontinuity, reset your zero to compensate (perhaps by some fraction less than 100%)

Works well for 1-5 minute races.

As you say, modern AVC is doing less GPS, and I fully expect a vision plus encoders solution to win at some point!

Jon

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/CADnZWKO5YiXB4CSWD8UOJ8yuY45Hc%3DskmWtwG7AC%2BNy7T%2B6B2g%40mail.gmail.com.

[Michael Shimniok]

Sounds like a good approach, Jon.

I found in my testing that gps heading didn't have those jumps and was quite accurate, so I only ever used GPS heading to correct the gyro.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/CAJgyHGNgwPjgRcdSEp_0d_m%3DB2p7%3DzOwC%2B1YFEAOv4LOqPOT3g%40mail.gmail.com.

[Ted Meyers]

I wonder why the heading didn't have jumps?  Maybe the GPS chip is doing some low-pass filtering?  Essentially what Jon is doing -- which I would agree is a good approach.

[Ted Meyers]

Yeah, the DIY Robocars people have been having great success with vision based driving.  I do wonder why none of them have yet used encoders and/or gyros yet (that I know of)?  Seems like it would help a lot. Possibly because it is hard enough just getting the vision algorithms working.

[Jon Watte]

Alan is using encoders (taped zebra stripes inside the wheel rims) and a Kalman filter for his car, which is currently the reigning champion.

It turns out, for the diy tracks, knowing exactly where you are isn't as important as knowing how to react to the exact circumstance in front of you.

(Although I still have some ideas about how to use the former to figure out the latter!)

The main problem with the vision is things like glare and uneven lighting; mainly physics. The "if I can see the road, I can steer" bit is reasonably well covered.

Sincerely,

jw

Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

--
You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+unsubscribe@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/508550c6-d93c-452a-af45-b5e2980d3eb2%40googlegroups.com.