Skip to content

Pure Pursuit

Why Do Robots Need Pure Pursuit?

Imagine you have a robot car, and you want it to follow a winding road/curved path. If the robot just tries to go straight or only follows the exact points on the path, it might turn too sharply, wobble, or even go off track!

How Does Pure Pursuit Help?

Instead of following the path like a dot-to-dot drawing, the robot looks a little ahead and aims for a point in front of it. This way, it turns smoothly and follows the path naturally, just like how you ride a bike or drive a car. This can be compared to a donkey chasing a carrot (as the donkey will keep moving forward to reach the carrot, but never actually reach it and eat it, till the path ends).

Mapping Diagram

Advantages/Disadvantages

Although Pure Pursuit is not the most accurate path-following algorithm to follow a path as the one shown below, it has a decent amount of accuracy when used, and compared to other algorithms, has a speed advantage.

Mapping Diagram
What is the main purpose of the Pure Pursuit algorithm in robotics?

The Math

Draw a Circle With the Robot as the center

\[(x - h)^2 + (y - k)^2 = r^2\]

This equation represents a circle where:

  • \((h, k)\) is the center of the circle and the robot.
  • \(r\) is the "lookahead" distance, which is how far ahead on the path we want to look ahead. This is also the radius of the circle. A smaller lookahead distance makes the robot more reactive, while a larger lookahead distance makes it more stable but less responsive to path changes.
  • \((x, y)\) represents any point on the circle we draw around our robot.

Going to the point

  • We then want to use a control algorithm such as Pid to head towards the point of intersection with the Circle and the path.

Algorithm Steps

The Pure Pursuit algorithm follows these steps:

  1. Draw a circle around the robot with your chosen lookahead-value (Bigger lookahead means it will follow the path less close and smaller means it will follow the path more close).
  2. See on the path where the circle intersects.
  3. Use an error-based control algorithm such as PID, to make the robot head towards that point.
  4. Adjust the robot’s movement accordingly.
  5. Repeat the process at each timestep until you reach your end point (Note if the circle goes past the ending point just set the next point to head to as the ending point).
If the lookahead distance is increased, what is the likely effect on the robot's path following?

The Code

View Pure Pursuit code on GitHub

Here we find the lookahead point using the intersection of the circle and the path. We use the "discriminant" to tell the amount of intersection points(if any) with the circle. 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
     def find_lookahead_point(self): # finding the next lookahead point
        robot_position = self.jackal_robot.localize() # localizes properly
        for i in range(self.current_index, len(self.path) - 1):
            start, end = self.path[i], self.path[i + 1] # looking in the line between waypoints
            # Compute quadratic coefficients for lookahead
            dx, dy = end[0] - start[0], end[1] - start[1]
            fx, fy = start[0] - robot_position[0], start[1] - robot_position[1]
            a, b, c = dx**2 + dy**2, 2 * (fx * dx + fy * dy), fx**2 + fy**2 - self.lookahead**2
            discriminant = b**2 - 4 * a * c

            if discriminant >= 0: # finding and returning intersection/updating the points crossed to make sure it doesn't go backwards
                discriminant_sqrt = math.sqrt(discriminant)
                t1 = (-b + discriminant_sqrt) / (2 * a)
                t2 = (-b - discriminant_sqrt) / (2 * a)

                if 0 <= t1 <= 1:
                    self.current_index = i  # Update current index
                    return (start[0] + t1 * dx, start[1] + t1 * dy)
                if 0 <= t2 <= 1:
                    self.current_index = i  # Update current index
                    return (start[0] + t2 * dx, start[1] + t2 * dy)

        return self.path[-1] # returns last point if nothing found(shouldnt ever occur but fault tolerance)

View More Pure Pursuit code (simulation) on GitHub

In this file we find the lookahead point and utilize PID to make the robot head toward the lookahead point found using the previous code. 

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
  goal=self.follower.find_lookahead_point()
                self.x_goal=goal[0]
                self.y_goal=goal[1]


                # showing the lookahead intersection point
                p.addUserDebugLine([self.x_goal,self.y_goal-0.1,0.05],
                                    [self.x_goal,self.y_goal+0.1 , 0.05],
                                    lineColorRGB=[1, 0, 0], 
                                    lineWidth=300)

                self.x_error=self.x_goal-self.current_x
                self.y_error=self.y_goal-self.current_y
                self.h_error=self.angle_wrap(math.atan2(self.y_error,self.x_error)-self.current_h)

                # calculate with pid for the velocity to go to the lookahead
                self.linear_velocity=self.linear_pid.calculateVelocity(self.x_error)
                self.angular_velocity=self.angular_pid.calculateVelocity(self.h_error)

                # setting the velocities
                self.jackal_robot.inverse_kinematics(self.linear_velocity,self.angular_velocity)
                self.jackal_robot.setVelocity()