During the Spring 2021 semester, I joined the F1Tenth team at Penn to conduct an interest project on multi-vehicle coordination. To get prepared for the project, I self-learned and completed Lab 1, 2, 3, 4, and 6 of the “F1Tenth” course.
Overview
This post contains my results for Lab 6, where the Pure Pursuit algorithm for path tracking is implemented for the race car so that it can follow the designated waypoints, completing a loop of the Levine Hall smoothly. See the video below for a cool demonstration of the Pure Pursuit algorithm:
Demonstration
Of course the above is the situation where I pushed the algorithm to its limit, so that the vehicle starts drifting in order to complete the loop as fast as possible. The vehicle can track the waypoints better if we drive slower as in lab3 and lab4, as shown in video below:
Procedure
I first generated the waypoints by localizing the vehicle in the desired map (Levine Hall) using a Particle Filter and recording the locations with a Waypoint Logger. Notice that the Particle Filter implementation provided in the link requires Python 2 environment to build. However, from my experience, the node will run successfully even in a Python 3 environment as long as the codes are modified to their “Python 3” version (such as “print”), a modified version can be found here.
Another note is that a predefined map is required for generating the waypoints. In my case, I could not get in touch with a real car due to the COVID-19 pandemic, so I chose to use a provided map for the simulation. However, it is possible to generate a map ourselves with SLAM. Check out Google Cartographer for more information.
With the waypoints, the Pure Pursuit algorithm can be summarized in the following steps:
- Localize the vehicle with some localization methods, for example, particle filter. This allows the vehicle to identify its nearby waypoints and their relative position in future steps.
- Choose the next appropriate waypoint according to some lookahead distance L. In my code implementation, I chose the closest waypoint that is in the “half plane” that the vehicle is oriented and has a distance greater than L to the vehicle. Note that the lookahead distance L can be tuned to achieve desired performance.
- Calculate the required steering angle to reach the desired point. This calculation is non-trivial due to the non-holonomic nature of cars. See the picture below, due to the velocity and heading of the vehicle, we would not reach the goal point smoothly (possibly with large overshoot) if we just set the steering angle to the exact direction of the goal point. Therefore, we want to find an arc between the current position and the goal point for smooth movement of the vehicle. Since there will be infinitely many arcs possible to this purpose, we define our desired arc as the one whose center of radius lies on the y-axis of the vehicle’s coordinate frame. The curvature of the arc can then be calculated with the equation in the picture below. Finally, the required steering angle was set to be proportional to this curvature, subject to a scaling factor KP, just as a P controller where the gain KP can be tuned to obatin desired performance.
Steering angle calculation to reach a goal point.
- Publish the desired steering angle and speed to the vehicle.
More technical details can be found in the below prompt document:
My source codes for this lab can be found on github through this link.
Reference
The lecture video can be found below: