AVC: Heading Errors, Path Following Woes

A little over two weeks to go as I write this. That's not much time.

Testing uncovered a couple of obvious problems. The robot is experiencing heading errors and the pure pursuit path following is still working poorly.

Heading Error

I'm seeing unpredictable heading deviations possibly suggesting heading drift. The robot's ending position was incorrect, rotated counterclockwise, on three test runs.

The problem manifests most apparently in the first leg of the square course. In three autonomous runs plus one manual run, I could see in the GCS that heading was drifting off.

Time to look at logs and see what is going on. Looking at a plot of the log data you can see some problems right away.

The correct initial heading --from the starting point to the first waypoint-- was around 254 degrees. This plot shows activity from about 1 second after logging starts.

The red line, GPS heading, won't report a correct heading until it hits about 2-3m/s which occurs just after 2 seconds into the run. My code won't use GPS heading until then, relying only on gyro, which is corrected for bias at the starting line.

The blue line shows the time-lagged heading estimate based on gyro and GPS heading. The green line is the current heading estimate. The gyro is used to compute a new heading and error corrected with the time-lagged estimate.

Both blue and green lines drift off the initial heading. Whether this is actual vehicle motion or gyro drift I don't know yet and will have to investigate.

Crystal clear is that the gyro and Kalman heading estimates are almost 5 degrees in error versus the GPS heading in this first stretch of the course. That's pretty terrible, and may explain what I witnessed in field testing.

What's going on? Possibly the code is fusing in GPS data before it converges to the correct heading. Or I need to include bias as a state in my Kalman Filter. Also, I'm using a new uBlox GPS with 4Hz update rate instead of the previous Venus GPS' 10Hz rate, so GPS heading will have less influence over gyro bias.

But what of that strange spike in the green and blue plots at just after 3 seconds? These spikes are all over the plots for my runs. It appears in the gyro estimate, current heading estimate, and time-lagged estimates.

The plot above shows the spikes pretty clearly. It also shows the Z-axis gyro values. I don't see any significant correlation between the estimate spikes and the gyro values. So the spikes aren't caused by shock, apparently.

There may simply be a bug in my computations. The gyro-only estimate is particularly bad with massive spikes appearing at seemingly random intervals.

That's the first place I'll start looking. The Kalman Filter smooths out this noise and error correction reduces the effect on the current heading estimate. The spikes don't appear to gravely affect heading estimate, in the end. But I don't see how a robot can hope to win with these kinds of errors present.

Path Following Instability

Path following is unstable. The robot has a hard time staying on the path. I tried a few different lookahead distances for the virtual 'rabbit' the robot chases around the course.

With a 2m lookahead distance, the robot experiences quite a bit of cross track error, particularly after the 2nd waypoint, with some fairly wide oscillation. This is what I saw several months ago and have procrastinated dealing with.

A longer lookahead distance should result in improved stability. I tried 3m which provided no appreciable change. With a 4m lookahead, the robot didn't turn fast enough to follow the path, but it was much more stable.

To verify the other extreme, a short lookahead of 1m the path following oscillated and was nearly unstable, as expected. I can try further tuning of the lookahead distance, sure.

Meanwhile, Josh Pieper of Savage Solder fame suggested lag as a possible problem. This paper [HEREDIA-2007-ADVANCED ROBOTICS.pdf] suggests instability problems when there's a lag between sensor reading and control actuation that's caused by computation delay and mechanical delay. I verified this awhile back with simulations in Processing.

I'm seeing about 1.4ms lag between gyro read and when the steering is updated. The robot updates the steering position every 50ms. The servo is an entry level Futaba S3003 with a typical slow speed of 60° in 0.19s.

I'm not yet sure what the answer is for path following. Until I get the heading issues figured out there's little point in working on this issue.