Pure Pursuit
Adaptive pure pursuit is a adaptive path following algorithm for vehicle navigation. The algorithm follows a path as a human driver would by looking ahead at a path and in the direction, updating the point you are looking at to maintain a lookahead distance from you to the point. This algorithm is useful for autonomous and programming skills to follow a very smooth and fast path to your target, eliminating jerky movements that create inaccuracies in odometry, faster paths, flexibility, and a more robust system that can react to outside forces that can impact a robots autonomous routine.
The adaptive pure pursuit algorithm controls a robot on a curved path, to a selected goal point. The algorithm finds a point, a lookahead distance away from the robot, and pursues that point, continuously updating as the robot follows that path. This algorithm mimics one way a human driver would follow a path, by looking ahead at a consistent distance on the road, and pursuing that point.
- lookahead distance - The distance that points will be from the robot
- lookahead point - The current point on the path that is one lookahead distance away from the robot
- next point - The point on the path that is next after the current lookahead point.
- goal point - The end point
- current position - The live position of the robot, acquired from the odometry.
- Odometry
- Finding the lookahead point
- Calculate robot movement, and move robot
We have 3 wheel odometry on our current robot that uses 3 perpendicular to the live position of the robot during autonomous operation of the robot. There is more detail about this subject in the 3 Wheel Odometry section of the notebook.
Finding the lookahead point is one of the most challenging parts of pure pursuit. We have accomplished this by handling all of the path mathematics in the path class. To find the appropriate lookahead point we find the intersection of a circle with the same position of the robot and a radius that equals the lookahead distance, and a of the line segment. To find the intersections we a quadratic equation to find if there are any intersections, and calculate the valid results of the calculations.
If the discriminant is more than 0, we know there is a valid answer so we continue, if there is no valid answer we will return nothing. t1 is the first intersection, and t2 is the second intersection.
The result of this is that we have 2 intersections with the line, t1, and t2, although they might not both be on the line segment the largest one that is on the line segment we will return, if none are on the line segment we will return nothing.
To calculate the robot movement we have to get the vector of the lookahead point to the robot’s position to command the robot. To do this we will use our vector utility class to create a vector that equals the lookahead point - current position.
We will put this value through a PID to get a better movement. To do this we will have to scale the vector to match the lookahead distance to make the PID profiles work similar with different lookahead distances. We send this magnitude to a PID to smooth out the values, then send that value to the motors.
The main variable that you can tune in the pure pursuit algorithm is the lookahead distance. A smaller lookahead distance will regain the path faster, but will have oscillations. A larger lookahead distance will regain path slower in a faster path, and less oscillations. This is illustrated below.
