The old, simple path following algorithm worked but didn't correct cross track error. The new algorithm chases a virtual rabbit, computing where the rabbit is and how to intercept it.
The Pure Pursuit algorithm is conceptually elegant and simple. It's easy to program, it's popular, and, clearly, it works quite well.
Pure Pursuit Algorithm
The 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!
Turning
The 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.
Conclusion
So, despite planning to test this on a small 1:10 RC car, I ended up proving it out on the full size Jeep. Once the turning radius and lookahead distances are set reasonably, it works a treat!If I get some time I'll post up the calculations for converting from arc radius to steering angle to servo signal. Suffice it to say that determining the correct steering angle to traverse the intercept arc is relatively simple to figure out, using basic geometry.
References
Path Tracking for a Miniature Robot.pdf
coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf
