What is Pure Pursuit?

Pure pursuit is a tracking algorithm that works by calculating the curvature that will move a vehicle from its current position to some goal position.

The whole point of the algorithm is to choose a goal position that is some distance ahead of the vehicle on the path.

The name pure pursuit comes from the analogy that we use to describe the method. We tend to think of the vehicle as chasing a point on the path some distance ahead of it - it is pursuing that moving point. That analogy is often used to compare this method to the way humans drive. We tend to look some distance in front of the car and head toward that spot. This look ahead distance changes as we drive to reflect the twist of the road and vision occlusions.

Theoretical Derivation

Determining the curvature that will drive the vehicle to a chosen path point, termed the goal point
An arc that joins the current point and the goal point is constucted
chord length = lookahead distance

Conside the lookahead distance to be analogous to the distance to a spot in front of a car that a human driver might look toward to track the roadway.

Lets say the vehicle at origin wanted to move to the point (x,y).

If x = y, Then d = 0 and y = x = r

$l$ is the locus of possible goal points for the vehicle. $\space \space \space l = \sqrt{x^2+y^2}$
the radius of the arc and the x-offset are independent. $\space \space \space x + d = r$
the radius of the arc can also be found using $l$ and $x$ : $\space \space \space r = \frac{l^2}{2x}$
curvature $\gamma = \frac{1}{r} = \frac{2x}{l^2}$

Representation and Communication

The Implementation is quite straightforward.

The only real implementation problems are :

Deciding how to deal with the path information
- communication
- graphics
- updating the path with new information from the planner

Path Representation

A path is represented as a set of discrete points. (need to be stored in memory)

Typically a path point is of some PATH_TYPE that is a struct containing:

x location in global coordinates
y location in global coordinates
heading in global coordinates (Head Direction, may known as $\omega$ )
curvature of the path at this point
distance (along a straight line) of this point from the beginning of the path

//pseudocode

struct PathPoint {
    float x;
    float y;
    float w;
  	float curvature;
    float distance;
};

Communication and Path Management

The tracker usually runs on one machine while the planner runs on another. The idea is that during a navigation cycle the planner finds a path segment through the newly perceived terrain.

Pure Pursuit Algorithm

The Pure Pursuit Algorithm can be outlined as follows:

Determine the current location of the vehicle (x,y,w)
Find the path point closest to the vehicle
Find the goal point
Transform the goal point to vehicle’s local coordinates
Calculate the curvature and request the vehicle to set the steering to that curvature
Update the vehicle’s position

The path tracking problem that we most often have to address is that of staying on a path, rather than getting onto a path.

Effects of Changing the Lookahead Distance

Lookahead distance $(l)$ is a parameter in the pure pursuit algorithm.

The effects of changing the lookahead distance must be considered within the context of one of two problems:

Regaining a Path (The vehicle is a “large” distance from the path and must attain the path)
Maintaining a Path (The vehicle is on the path and wants to remain on the path)

Regaining a Path

Long (large) lookahead distances $(l)$ tend to converge to the path more gradually and with less oscillation
Short (small) lookahead distances $(l)$ tend to with more oscillation

Maintaining a Path

Long (large) lookahead distances $(l)$ , less curvy of a path that can be followed
Short (small) lookahead distances $(l)$ , more curvy of a path that can be followed

If the path between the vehicle and the goal point is sufficiently ‘‘curvy’’ then there is no single arc that joins the two points; any driven arc will induce error.

Choosing Lookahead Distance

Given a lookahead distance we can define a curvature, but given a curvature we can’t define the lookahead distance.

lookahead distances ranging between $0$ and $2r$ are all equally admissable.

Limitation of Pure Pursuit

The method does not model the capability of the vehicle or of its actuators, and so assumes perfect response to requested curvatures. This causes two problem.

A sharp change in curvature can be requested at a high speed, causing the vehicle’s rear end to skid.
The vehicle will not close on thepath as quickly as desired because of the first order lag in steering.

Implementation of the Adaptive Pure Pursuit

The pure pursuit controller gives a robust method for quickly, accurately, and reliably following a path. Because the algorithm is so reliable, you don’t have to worry about finangling with the constants, which saves time and effort. Your robot can wear down and the carpet may be bumpy, but as long as the encoder and gyro readings are accurate it will still follow the path.

Odometry

Odometry is finding the robot’s XY location.

Use encoders attached to the drive wheels to measure how far the robot moved
Use gyro to measure in which direction
or use other forms of odometry (Vision, LIDAR)

To get a fair approximation of its current location:

//pseudocode

float x_loc = 0, y_loc = 0;

// this function needs to be repeated 50 times per second
void getCurrLocation(){
  float distance = (change_in_left_encoder_value + change_in_right_encoder_value)/2;
	x_loc += distance * cos(robot_angle);
  y_loc += distance * sin(robot_angle);
  //^^This gives the location of the robot relative to its starting location
}

Path Generation

A path is a series of XY coordinates (called waypoints).

Path Generation Consist of:

Creating a few waypoints for the robot to follow
Then injects more waypoints into the path and smooths the path.
Then the following information is calculated and stored for every waypoint in the path:
- Distance that point is along the path
- Target velocity of the robot at that point
- Curvature of the path at that point

When creating a path, the user also sets the maximum velocity and acceleration of the robot.

Creating waypoints

Just define a few waypoint for the robot to go

Injecting waypoints

Having closely spaced points increases the accuracy of the velocity setpoints and curvature measurements.

The Injection Algorithm:

calculates how many evenly spaced points would fit within each line segment of the path
- (including the start point, excluding the end point)
then inserts them along the segment
When it gets to the end, it has still not added the endpoint of the path, so it adds it to the list of points.

//pseudocode

spacing = any_value
new_points = array_to_put_new_points_into

for(int i = 0; i < line; i++)
{
	vector = end_point - start_point;
  num_points_that_fit = Math.ceil(vector.magnitude() / spacing)
  vector = vector.normalize() * spacing
  for (i = 0; i<num_points_that_fit; i++)
  {
    Add(start_point + vector* i);
  }
}

Add(last_point);

Smoothing

The algorithm takes in a 2D array of XY coordinates (path) and returns a smoother version of the coordinates (newPath).

Suggested values for parameters:

weight_smooth: b = 0.75 ~ 0.98
weight_data: a = 1 - b
tolerance = 0.001

//pseudocode

public double[][] smoother(double[][] path, double a, double b, double tolerance) {
    //copy array
    double[][] newPath = doubleArrayCopy(path);
    double change = tolerance;
    while (change >= tolerance) {
        change = 0.0;
        for (int i = 1; i < path.length - 1; i++)
            for (int j = 0; j < path[i].length; j++) {
                double aux = newPath[i][j];
                newPath[i][j] += a * (path[i][j] - newPath[i][j]) + b * (newPath[i - 1][j] + newPath[i + 1][j] - (2.0 * newPath[i][j]));
            }
    }
    return newPath;
}
change += Math.abs(aux - newPath[i][j]);

Distances Between Points

Finding the distance along the path to a point is as simple as keeping a running sum of the distances between points:

$\text {distance at point } i=\text {distance at point }(i-1)+\text { distance formula}(\text {point } i, \text { point }(i-1))$

Curvature of Path

Get the curvature at a point by finding the radius of the circle that intersects the point and the two points on either side of it.
Curvature = 1/radius
You need to avoid divide-by-zero error. If Curvature = 0, radius = inf, and the path is a straight line.

Velocities

Part 1: Maximum velocities

Each point on the path will have a target velocity the robot tries to reach.

The robot uses the target velocity of the point closest to it when calculating the target left and right wheel speeds
When calculating the target velocity for a point we take into account the curvature at the point so the robot slows down around sharp turns

The maximum velocity defined for the path is also taken into account. Each point’s velocity is set to the minimum of：

$\text { (path max velocity, k / curvature at point) }$

where k is a constant around 1 ~ 5, based on how slow you want the robot to go around turns.

Part 2: Acceleration limits

As our robot travels along the path, we want it to follow the velocity setpoints, but also obey a maximum acceleration.

The robot actually starts moving since the target velocity of the start point is non-0. But since the target velocities no longer obey the maximum acceleration, we have to pass them through a rate limiter. In real time, as the robot travels the path, the rate limiter constrains the rate of change of the input target velocity so the actual target velocity is smooth, but with the robot actually trying to move. In essence, it turns the second orange line back into the first. The logic for the rate limiter is described after the equations for calculating velocity.

Part 3: Calculations

To find the target velocities:

$v_{f}=\sqrt{v_{i}^{2}+2 * a * d}$

where

$v_i$ = velocity t prior point
$a$ = max acceleration
$d$ = distance between the points

To calculate the new target velocities, first set the velocity of the last point to 0. Then, with index i starting at the second to last point and moving backwards:

$\text {distance}=\text {distance formula}(\text {point}(i+1), \text { point i})$

$\text { new velocity at point } i=\min \left(\right.\text { old target velocity at point } i, \sqrt{(\text { velocity at point }(i+1))^{2}+2 * a * \text { distance })}$

Rate Limiter:

The rate limiter takes in the input you want to limit and the max rate of change for the input, and returns an output that tries to get to the same value as the input but is limited in how fast it can change. Constrain caps the first parameter to be in the range of the second two.

$\text { max change }=\text { change in time between calls to rate limiter } * \text { max rate }$

$\text { output }+=\text { constrain }(\text { input }-\text { last output },-\text { max change, max change })$

Following the Path

The algorithm to follow the path is as follows:

Find the closest point
Find the lookahead point
Calculate the curvature of the arc to the lookahead point
Calculate the target left and right wheel velocities
Use a control loop to achieve the target left and right wheel velocities

Closest Point

Finding the closest point is a matter of calculating the distances to all points, and taking the smallest. We suggest starting your search at the index of the last closest point, so the closest point can only travel forward along the path. Later, we will need to lookup the target velocity for this point.

Lookahead Point

The lookahead point is the point on the path that is the lookahead distance from the robot.
We find the lookahead point by finding the intersection point of the circle of radius lookahead distance centered at the robot’s location, and the path segments.

E is the starting point of the line segment
L is the end point of the line segment
C is the center of circle (robot location)
r is the radius of that circle (lookahead distance)
d = L - E (Direction vector of ray, from start to end)
f = E - C (Vector from center sphere to ray start)

//pseudocode

a = d.Dot(d)
b = 2 * f.Dot(d)
c = f.Dot(f) - r * r
discriminant = b * b - 4 * a * c
if (discriminant < 0) { 
  // no intersection
} 
else {
    discriminant = sqrt(discriminant) 
    t1 = (-b - discriminant) / (2 * a) 
    t2 = (-b + discriminant) / (2 * a)
    if (t1 >= 0 && t1 <= 1) { 
      //return t1 intersection
    }
    if (t2 >= 0 && t2 <= 1) {
      //return t2 intersection 
    }    
  //otherwise, no intersection 
}

Then to find the intersection point:

$\text { Point }=E+(t \text { value of intersection }) * d$

To find the lookahead point, iterate through the line segments of the path to see if there is a valid intersection point ( 0 ≤ t ≤ 1 ) between the segment and the lookahead circle. The new lookahead point is the first valid intersection whose fractional index is greater then the index of the last lookahead point. This insures the lookahead point never goes backwards. If no valid lookahead point is found, use the last lookahead point. To optimize your search you can start at the index of the last lookahead point.

Curvature of Arc

curvature $\gamma = \frac{1}{r} = \frac{2x}{l^2}$

We get to choose $l$ , the lookahead distance, but how do we find x?

x is the horizontal (relative to the robot) distance to the lookahead point. This can be found by calculating the distance to the lookahead point from the imaginary line defined by the robot’s location and direction.

We use the point-slope formula to find the equation of the “robot line”:

$(y-\text {robot } y) /(x-\text {robot } x)=\tan (\text {robot angle})$

Converting this to the form ax + by + c = 0 gives:

$\begin{array}{l} a=-\tan (\text {robot angle}) \\ b=1 \\ c=\tan (\text {robot angle}) \times \text {robot } x-\text {robot } y \end{array}$

The point-line distance:

$x=\mid a * \text { lookahead } x+b * \text { lookahead } y+c \mid / \sqrt{a^{2}+b^{2}}$

Wheel Velocities

While we can not directly control the curvature our robot drives at, we can control the left and right wheel speeds.

In order to calculate the target left/right speeds that make the robot drive at the correct curvature with the correct velocity, we will need to know three things:

curvature
- Curvature we calculated abov
target velocity
- To get the target velocity take the target velocity associated with the closest point, and constantly feed this value through the rate limiter to get the acceleration-limited target velocity.
the track width (the horizontal distance between the wheels on the robot)
- Due to turning scrub, you want to use a track width a few inches larger than the real one

$\begin{array}{l} V=(L+R) / 2 \\ W=(L-R) / T \\ V=W / C \end{array} \\ \begin{array}{l} L=V *(2+C T) / 2 \\ R=V *(2-C T) / 2 \end{array}$

where

V = target robbot velocity
L = target left wheel’s speed
R = target right wheel’s speed
C = curvature of arc
W = angular velocity of robot
T = track width

Controlling Wheel Velocities

$F F=K_{v} * \text { target vel }+K_{a} * \text { target accel }$

Tuning

Stopping

if your path ends on a sharp turn, it makes it hard for the robot to stop in the right location while facing the right direction.

To fix this, you can:

Reduce the lookahead distance
Adjust the path so there is no longer a sharp turn
Keep the sharp turn, but adjust the path until the robot ends up where you want anyway
Virtually extend the path past the actual endpoint of the path, so the effective lookahead distance doesn’t shrink to 0

Importance of Visualizations

Visualizing data is extremely helpful in programming the pure pursuit controller. Every step of the algorithm benefits from graphs or diagrams. It makes it easy to see if the math is working as it should. Even more useful is a robot simulator that allows you to visualize your algorithm at work and test changes before putting them on a real robot.