AVC Path Following
I'm implementing a better path following algorithm for autonomous rovers than what Data Bus ran in 2012.
The old, simple path following algorithm worked but didn't correct cross track error. That fact may explain why the robot missed the bonus hoop way back in June at the 2012 AVC
I've been revising the path following algorithm in recent months. The new approach for following a path performs well in tests so far, is simple to program, and isn't mathematically complex.
The robot chases a virtual rabbit, computing where the rabbit is and how to intercept it. Conceptual elegance and simplicity. Read on for details...
Pure Pursuit AlgorithmThe robot follows a virtual rabbit that travels around the legs of the course, with each leg defined by two points. The rabbit is always located along the current leg of the course, A=[W0 W1].
The projection of the robot's position onto A is point P and the rabbit is located along A, a fixed distance from P. The fixed distance between P and the rabbit is called the lookahead distance.
The algorithm moves the rabbit's position at each update step, computes a relative bearing from the robot to the rabbit, computes an arc path tangential to its heading that intercepts the rabbit. The result of this unending pursuit is a smooth correction to the robot's heading and cross track error. This algorithm is called Pure Pursuit.
Here's the math I used to compute the intercept arc. The distance from W0 to P is the projection of the robot's position onto W, given by a dot product:
Where A=[W0 W1] and B=[W0 robot]. Vector A, divided by its magnitude, is the unit vector pointing along A. The dot product can be computed with trig functions (slow) or you can do it this way (fast):
Once you have a scalar from the dot product you can find your goal point, the rabbit, like this:
Even if the math looks spooky, the code is trivial. Here's the Processing code I used for my simulation.
// Leg vector
float Ax = Xw[next] - Xw[prev];
float Ay = Yw[next] - Yw[prev];
// Bot vector
float Bx = x - Xw[prev];
float By = y - Yw[prev];
// Dot product
float legLength = sqrt(Ax*Ax + Ay*Ay);
float proj = (Lx*Bx + Ly*By)/legLength;
// Goal point ("rabbit")
float Rx = (proj+lookAhead)*Ax/legLength + Xw[prev];
float Ry = (proj+lookAhead)*Ay/legLength + Yw[prev];
See? Not bad at all!
TurningThe robot now knows where the rabbit is. The robot knows it's heading and position. It can compute the relative bearing to the rabbit. But how much should the robot turn to catch the rabbit?
An elegant approach with smooth behavior that doesn't require complex programming logic or trial and error tuning is to use an intercept arc that intersects both robot and rabbit, and is tangential to the robot's heading. Here's how.
We have the robot at B, it's heading described by BC, and the rabbit or goal point at G. The distance between B and G is D.
A circle that intersects B and G and is tangential to BC with radius R will have an origin along a line that is perpendicular to BC and passes through B. We simply need to find out R, the radius for this circle. Time to break out some trigonometry.
Draw another radius line perpendicular to BG. This line will bisect BG (each line is D/2 in length). Studying the right triangles generated by these lines, notice that the relative bearing, theta, is also the angle between the new radii intersecting B and G, respectively.
We can express D/2 in terms of R and Theta, then solve for R:
The robot recomputes a new intercept arc at every update step. The result is a continuous path towards the goal point. The path is followed, the cross track error is accounted for.
I've not tested the algorithm much, but I'm sure it'll work fine; it's very well known. It's just a matter of tuning the lookahead distance. Both simulation and real world look promising but I've been sidetracked with another issue for now.
I haven't talked about steering, yet, either. Determining the correct steering angle to traverse the intercept arc is relatively simple to figure out, using basic geometry.
Path Tracking for a Miniature Robot.pdf