Pure Pursuit

Pure pursuit is a navigation algorithm used for non-holonomic vehicles to track a target or follow a path. It works by calculating the closest point on the path to the vehicle’s current position and steering towards that point.

I implemented a pure pursuit algorithm for autonomous racing on F1TENTH. I’ve done many tweaks to push the limits of the car, see more below. Code can be found here: https://github.com/CL2-UWaterloo/f1tenth_ws/tree/main/src/pure_pursuit

Resources:

• Coursera lecture by the UofT professor https://www.coursera.org/learn/intro-self-driving-cars/lecture/44N7x/lesson-2-geometric-lateral-control-pure-pursuit
• https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html
• https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf
• https://dingyan89.medium.com/three-methods-of-vehicle-lateral-control-pure-pursuit-stanley-and-mpc-db8cc1d32081 (also talks about Stanley Method and MPC)

The Problem with Pure Pursuit

After talking with Daniel Chia, I realized that there is a key tradeoff between responsiveness (better tracking) and stability (less oscillations). In pure pursuit, this is tuned in 2 ways:

• $K_p$ value (lower is more stable)
• Lookahead distance (higher is more stable)

These stability issues are exacerbated when the car goes very fast, as you’ll start seeing the car oscillating a LOT. To help combat this, we tune the parameters (reduce $K_p$, increase lookahead distance).

However, this comes at a cost of a less responsive of the car. I notice that the car undershoots the turn, and not track my racing line super well.

An improvement for the Pure Pursuit controller is using the Stanley Method.

Running Pure Pursuit on the car:

ros2 run pure_pursuit pure_pursuit_node

From this pure pursuit lecture (slides) we derive the steering angle $\theta$ from the following equation: $\theta =\frac{1}{r}=\frac{2|y|}{L^2}$

Using this equation to calculate $\theta$ results in a much stabler drive.

How did I do it before?

Before, I just drew a simple triangle and got $\theta ={\tan }^{-1}(\frac{y}{x})$. However, this is not good because the vehicle is non-holonomic. We need to think in terms of an arc instead of a line, because that is how the vehicle actually drives.

Personal Modifications to Pure Pursuit Algorithm

I personally made some modifications to get the car to drive as fast as possible, which being reliable enough not to crash into walls. Specifically, the original pure pursuit algorithm resulted in lots of oscillations.

These are:

1. Lowering the $K_p$ value
2. Variable Lookahead
3. Velocity Choice

1. Using $K_p$

$K_p$ is sort of a gain of how much weight you want to turn. Learn about PID Tuning. I noticed that with higher $K_p$, the car oscillates a lot more. However, at some point when $K_p$ is too low, the car doesn’t steer enough, and ends up crashing in the wall.

• For my case, $0.25$ is the perfect value for the square track

So my steering angle $\theta$ is given by

$\theta =K_p\frac{2y}{L^2}$

2. Dynamic Lookahead

The idea for this is that your lookahead distance should be proportional to your current velocity. So if you go twice as fast, you should look twice as much ahead, similar to how F1 Drivers race.

The tradeoff in lookahead distances

• Small lookahead = more accurate tracking, but more oscillations
• Big lookahead = less oscillations, but poor tracking

The following image illustrates the tradeoffs in lookahead:

We can thus use a dynamic lookahead distance to get the best out of both worlds depending on how fast we are going. The idea is that we want to be more accurate in tracking when we are going slow, but much more accurate when we go fast.

lookahead = min(max(min_lookahead, max_lookahead * curr_velocity / lookahead_ratio), max_lookahead);

• The min and max function are just there to make sure the lookahead is within the min_lookahead and max_lookahead

So we define the following parameters:

min_lookahead: 0.5
max_lookahead: 4.0 
lookahead_ratio: 8.0

Then, if the car is going at 4.0m/s, then the lookahead distance will be $4.0m\ast \left(\frac{4.0m/s}{8.0m/s}\right)=2.0m$

However, we also define a minimum lookahead, so that when the car goes too slow, we still choose a point in front of it, to determine a correct steering angle

Other Implementation (I saw this in the RRT node implementation), but maybe too complicated for my use case?

angle = np.clip(angle, -np.radians(self.steering_limit), np.radians(self.steering_limit))
 
# determine velocity
logit = np.clip(
	(2.0 / (1.0 + np.exp(-5 * (np.abs(np.degrees(angle)) / self.steering_limit)))) - 1.0,
	0.0,
	1.0
)
velocity = self.velocity_max - (logit * (self.velocity_max - self.velocity_min))
 

3. Velocity Choice

When it comes to getting the velocity, we should use the velocity profile of the closest point, rather than the one that is given by our lookahead point. The velocity profile is given by the Raceline Optimization

• I noticed that if I used the velocity of the lookahead point, the vehicle would brake too early

// Find the closest point to the car, and use the velocity index for that
double shortest_distance = p2pdist(waypoints.X[0], x_car_world, waypoints.Y[0], y_car_world);
velocity_i = 0;
for (int i=0; i<waypoints.X.size(); i++) {
	if (p2pdist(waypoints.X[i], x_car_world, waypoints.Y[i], y_car_world) <= shortest_distance) {
		shortest_velocity_distance = p2pdist(waypoints.X[i], x_car_world, waypoints.Y[i], y_car_world);
		velocity_i = i;
	}
}

Pipeline for running pure pursuit

1. Create a new map using SLAM in ROS2. See slam_toolbox
2. Create a list of waypoints using a global planner by recording waypoints driven by teleop using the waypoint_generator node that you wrote here
3. Move the .csv file generated into the right folder
4. Run pure_pursuit

To visualize: The path that is generated

TODO:

• Reverse deadman’s switch

Things I tweaked to get this working:

• Play with different lookahead distances $\to$ 2.0 works perfectly for now, in simulation 2.0 runs into a way
  • Setting a min and max lookahead distance
• Incrementally increase speed
• The main source of error and crashes is when the loop closes

Future things to do:

• Change Lookahead distance based on velocity
• use the formula that they propose instead of the one I derived personally

Pseudocode of final version? So that this pure pursuit code works out of the box for any code

1. Get the average distance between each point in the set of waypoints
2. Use that as an indicator of how many points to look ahead at.

for i to the next 500 waypoints (ciclical):
if p2

The next 500 waypoints is important because we don’t want to lock onto the point that is behind the car.

TODO: INSERT Drawing

Lessons

Working on this robotics project makes me realize how there are so many different moving parts to robotics. Each of them have to work in symbiosis.

But it shouldn’t happen that the position suddenly teleports. LOOKAHEAD distance should be a function of speed

Waypoint Following

Pure Pursuit is basically waypoint following for non-holonomic vehicles.

Actually, waypoint following is slightly different, because you way to be very precise to hit your waypoint. I have a scenario with the Create3 where the robot just gets stuck, because it is there, but it’s off by just a little bit, and can’t get the perfect positining.

You can easily follow waypoints in nav2. If you want to follow a custom set of waypoints, follow this tutorial https://automaticaddison.com/how-to-send-waypoints-to-the-ros-2-navigation-stack-nav-2/

Or see this? https://github.com/ros-planning/navigation2/tree/main/nav2_waypoint_follower

They’re basically the same.

Next

• Stanley Method