Skip to content

Pure Pursuit

Jump to bottom
edf42001 edited this page Apr 2, 2018 · 2 revisions

Pure Pursuit

Pure Pursuit is an algorithm for making a robot follow a path. There is also a Path Drawing application, which is used to create the paths for the robot to follow.

Table of Contents

Purpose of this Guide

The purpose of this guide is to explain Pure Pursuit to YOU, the user, so that you can understand and make this code work on you robot.

Pure Pursuit

The pure pursuit algorithm is extremely robust. It doesn't matter if your tuning numbers are a bit off, as long as you have accurate odometry your robot will be able to follow the path.

The bare-bones algorithm takes in the path, the robot's location, the velocity to travel, the wheelbase of the robot, and the lookahead distance. The robot finds the point on the path that is ahead of it by the lookahead distance, the lookahead point. It calculates the curvature of the arc it needs to travel to get from the robot's location to the lookahead point. From this curvature, the wheelbase, and the target velocity, it calculates the target velocity for the left and right wheels. A PIDF controller is used to get the wheels to the target velocity. 20ms pass, the robot moves forward a fraction of an inch, a new lookahead point is found, and the algorithm continues until the robot reaches the end of the path.

Detailed Walkthrough of Code

The following is a description of the implementation of the algorithm. All VIs are described in more detail in the Description of VIs section. Our code was implemented using LabVIEWs Command and Control Framework, however the underlying algorithm can be used in either framework.

The Drive Path VI command is used to start the algorithm. It takes in the specifications of the path to follow. It sends this data to the Drive Controller.

In the Drive Controller, a loop separate from the main control loop runs at 50Hz. It uses EncodersGyroToXY to keep track of the XY location of the robot. The loop also calculates the velocity of the left and right wheels. It sends the robot's XY location, angle, and left/right wheel velocities to the main controller so the algorithm can use them. The loop resets the measured XY Location and gyro angle when autonomous starts. It sets the XY location to one of the 4 preset locations for autonomous that is selected on the Dashboard, and it sets the gyro to 0.

When running the Pure Pursuit algorithm, the main control loop runs at 50Hz. The PurePursuit VI executes the algorithm. It takes in the path data, the data about the state of the robot, a reset signal, and PIDF gains for left/right wheel velocity. It outputs a boolean saying when the robot is done the path.

Reset should be true on the first iteration of the PurePursuit VI. This is done by testing if the iterations of the Drive Path command == 0. Command iterations is given by the Command and Control framework. When Reset is true, the algorithm reinitializes the feedback nodes used in RateLimiter, FindLookaheadPoint, FindClosestPoint, and CalcLeftRightVelocities. When Reset is true, things about the path that are needed but are not given to Drive Path command VI are calculated and stored in a feedback node. Currently, what is calculated is the target velocity for each point along the path and the distance is point is from the end along the path. The target velocity is lower when the path has a high curvature.

CalcDistanceFromEnd goes throught he path to measure how far each point is from the end of the path, measured along the path.

Then, by taking in lumps three points on the path and feeding them to CalcCircleWithThreePoints, the code in PurePursuit finds the curvature of the path at each point. This is multiplied by a constant to get the target velocity at each point. These target velocities are fed into CalcMaxVelAtPoints, along with the maximum velocity and acceleration.

CalcMaxVelAtPoints works backwards and adjusts the velocity at each point so they obey the max acceleration and velocity limits. It does not traverse the path forward, as limiting acceleration in this direction is handled by a RateLimiter on target velocity.

Now everything is ready for the algorithm to begin. FindClosestPoint finds the closest point to the robot. It uses the index of this point to find the robot's distance to end and the target velocity.

FindLookaheadPoint finds the lookahead point. It takes in the path, lookahead distance, robot location, and the average point spacing. (Note: point spacing may be deprecated).

CalcCurvature takes in the lookahead point and robot location. It calculates the curvature of the arc the robot needs to travel to get to the lookahead point.

Target velocity and curvature from above along with the robot track width are fed into a formula node which calculates the target left/right wheel velocities.

The target left/right wheel velocities along with PIDF gains and the current left/right wheel velocities are fed into the CalcPowerPurePursuits. These VIs runs a PIDF controller, including FeedForward on acceleration, to calculate the power to send to the left/right motors. The target acceleration for the FeedForward Acceleration is calculated in real time by the rate of change of target velocity.

The left/right motor powers are then outputted to the left/right motors.

List Of VIs

Tuning Tips

Tuning the PIDF Controller

The left/right velocities are controlled by a PIDF controller. This requires tuning 5 numbers:

  • Kp: Proportional constant
  • Ki: Integral constant
  • Kd: derivative constant
  • Fv: Feedforward on velocity constant
  • Fa: Feedforward on acceleration constant

Luckily, the pure pursuit algorithm is robust, so it doesn't matter if your gains are not tuned perfectly.

Follow this tuning process to tune your PIDF loop:

  • Have a graph of target velocity vs measured velocity (should be on front panel of PurePursuit).
  • Create a straight path (as long as possible) for your robot to travel. Set target velocity to something near normal operating range, say 80 in/s.
  • Set Kp, Ki, Kd, Fa to 0. Guess a reasonable initial value for Fv: (0.007).
  • Run the path. If the measured velocity does not match the target velocity when the robot is not accelerating, adjust Kv until it does.
  • Next, pick a reasonable initial value for Fa (0.001). Adjust it until the measured and target velocities match closely when the robot is accelerating.
  • Make Kp 0.002. Increase it as much as you want, but the more you do the jerkier it gets.

Other Numbers you Need to Mess With

This is a list of all important, changeable numbers in the Pure Pursuit code. The format is as follows:

Name - Location - Description

  • Width - Path Drawer FP, Constants Global Variable - The width of the robot, including bumpers. If RobotWidth changes, all the paths need to be redrawn because it affects where the center of rotation starts.
  • Length - Path Drawer FP, Constants Global Variable- The length of the robot, including bumpers. If RobotLength changes, all the paths need to be redrawn because it affects where the center of rotation starts.
  • CenterRotOffset - Path Drawer FP, Constants Global Variable - The distance from the back of the robot to the center of rotation. Determine experimentally. If it changes, all the paths need to be redrawn because it affects where the center of rotation starts.
  • Lookahead - Path Drawer FP - How far ahead on the path the robot is trying to get to. This is changed on a path to path basis. When smaller, robot will follow sharp turns more accurately but will oscillate more. When large, the opposite is true. Average range should be 11-30 depending on curvature of path.
  • PointSpacing - Path Drawer FP - How far apart the points in the path are. After the path is smoothed, this becomes a rougher estimate. 6 inches is good, and should not need to be changed
  • MaxVel - Path Drawer FP - The maximum velocity the robot will go. Note, individual wheel velocities can be greater when going around turns.
  • MaxAccel - Path Drawer FP - The maximum acceleration the robot will do.
  • Roundedness - Path Drawer FP - Determines how smooth the path will be. Obviously, path smoothness also depends on where you put the nodes.
  • Starting Position Coordinates - Path Drawer BD ("Starting Position Preset": Value Change case), Drive Controller BD (middle loop) - This is a big one. This is an array of the coordinates of the edge of the robot's bumpers for the 4 starting positions. For positions 1 and 3, the center of rotation is below the coordinate, for 2 and 4 it is above it. The pixel measurements stored on the Path Drawer BD are converted into inches by subtracting pos 4's coordinates, and multiplying by the inch/px ratio. The new numbers are what goes in the Drive Controller, so the robot knows where it is starting. Pos 4's starting coords are also in the bottom left of the main loop. It'd be nice if there was a central location that changes to would reflect in both locations, but I don't know how.
  • Inch/px ratio - Path Drawer BD - How many inches of real world a pixel of the picture drawn on represents. With our picture, this is equal to 0.92969870875.
  • Not that these really matter, but you can delete a node in the Path Drawer when your mouse is within 10px of it, and pressing "I" inserts adds a new node to the end of your path at (300, 200).
  • PIDF gains - Drive Controller FP - See Tuning the PIDF controller
  • Trackwidth - Drive Controller FP - The distance between the two sets of wheels on your robot. When it is larger, the robot turns more vigorously. Normally a number a bit bigger than your robot's physical trackwidth works for me, because of turning scrub, but you can determine this number experimentally be spinning the robot in place and using the equation Δtheta = (ΔLeft - ΔRight)/Trackwidth.
  • Speed/Curve - Path Drawer FP - The conversion from curvature of a point on the path to the maximum velocity of the robot at this point. The robot will obey the smallest of this speed or the maximum velocity set in the Path Drawer. Good values are 1.5 - 3.
  • DistanceSlowAtEnd - Path Drawer FP - The distance the robot will travel at a slower speed at the end of the path. The robot does this to prevent any overshoot from momentum.
  • SlowSpeed - Path Drawer FP - The speed to go at the end of the path.
  • 0.5, 1000 - The velocity and time, respectively, that if the robot is going slower then the velocity for the time it will end the path. This is in case the robot gets stuck against something at the end. Currently not used.
  • 10 - LookAheadPoint BD - The range (units: points in path) in front of the current lookahead point the robot looks for the next point

Path Drawer

The Path Drawer is a program that runs on the computer. It is used to develop paths for the robot to traverese during auto.

How To Use

This section will walk through all the features of the Path Drawer so you can use it to draw paths. The Path Drawer has an image where paths are draw, and a tab control with two tabs: Draw and Load/Save

Setting Up

In the Draw tab:

  • Enter your robot's width and length, including bumpers, into the RobotWidth and RobotLength controls
  • Input how far the center of rotation is from the back of the robot into CenterOfRotYOffset
  • Set PointSpacing to 6.

Making a Path

In the Draw tab:

  • Make sure the polyline tool is selected (to the left of the image).
  • To start drawing a path, click on the image. Click wherever you want to place a node, and double click to finish drawing.
  • You can drag nodes by clicking on them and dragging. Or you can numerically adjust them by editing the values in the horizontal Points array.
  • If you accidentally start drawing a new path, or at any time you want to go back to the previous path, press Z
  • Hover the mouse over a node and press Backspace to remove a node
  • Press I to add a node at the end of the path. To insert one in the middle of the path, right click on an element int the Points array and do Data Operations->Insert Element
  • Adjust the Roundedness slider to make the path more or less smooth. (Tip: Round out corners by adding extra points, so the corner consists of two angles instead of one).
  • Click on StartingPositionPreset to choose the starting position. This needs to be re-clicked if you change Width or CenterOfRotYOffset. Chose "No Preset" if the path doesn't start at the alliance station wall.
  • Click Flip Path to reflect the path across the X-axis of the field.
  • Enter the maximum velocity and acceleration for your robot to obey with the MaxVel and MaxAccel controls.

Loading/Saving Paths

In the Save/Load tab:

  • Click SaveToLocal to save your path as a text file on the computer. Enter the name of your file. Do not include the file extension.
  • To export a file, select the file in the LocalFiles box. Make sure you are connected to the robot. Click ExportToRobot. Note that this saves the file on the computer, not your current path. Make sure to save the path to local before selecting and uploading it to the robot. A command window will pop up. If it asks you if you want to connect, enter "yes". The window labeled standard error will show the response of the command running. If it shows a huge block of text you probably want to click Delete known_hosts to delete the known_hosts file on your computer, then try exporting the file to the robot again.
  • To load a path, select the file in the box and click LoadFromLocal. This will remove your current path.
  • To get all the path files currently on the roboRIO onto your computer, click LoadAllFromRobot. They will go into the TempPaths folder. You have to manually copy them into the Paths folder if you want to load, edit, and reupload them. Or if you know the files on the robot match the ones in the Paths folder, you don't need to get the files from the roboRIO.
  • Click ExportAllToRobot to upload all the files in the Paths folder to the robot.
Clone this wiki locally