Carnegie Mellon Racing Path Planning Library

What is Path Planning?

Path planning is responsible for determining a route for the car to take in order to navigate the track. This process involves two primary parts: determining the midline and employing SLAM (Simultaneous Localization and Mapping) to determine the structure of the racetrack and the location of the car. Determining the midline refers to the process of generating a route between the blue and yellow cones observed at the current time step. The midline is necessary when the car is competing its first lap around the track because without knowledge of the track’s layout, the car is forced to naively navigate the track. As the car completes its first lap, a SLAM algorithm is employed to refine its position in the track and the position of previously seen cones (localization) based on newly observed cones while also mapping out the racetrack (mapping).

Our Implementation

Our midline calculation relies on Support Vector Machines (SVMs). Using the cones received from our lidar, the SVM generates a decision boundary using a cubic polynomial kernel. This decision boundary is converted into a series of points that represents the path the car should take. The SLAM algorithm used is iSAM2, a form of factor graph SLAM. We chose this algorithm after considering 1.) iSAM2 is capable of incrementally optimizing its estimates for previous cone position esimates and pose estimates, as opposed to optimizing only at loop closure, 2.) iSAM2’s performance made it a clear choice for SLAM, and 3.) we are able to work closely with the author of iSAM2, Professor Michael Kaess. Thus, we would like to take this opportunity to thank Profesor Michael Kaess for dedicating his time and efforts to assisting our implementation of SLAM. For more information on SVMs, see the Perceptions Pipeline documentation. For more information on iSAM2, See “What is Factor Graph SLAM?”

Note

Midline calculation is written in Python, and because perceptions is written in python, midline is written in the Perceptions pipeline and compiled in the Perceptions package. SLAM on the other hand is done in C++ and is compiled in the Path Planning package.

This is made possible by processing observed cone positions received from our Perceptions pipeline into a path the car can follow. The Planning pipelines is then responsible for a.) determining the midline, a series of points representing the path the car should take b.) SLAM. The path is published and received by the Controls Pipeline where control actions will be determined to instruct the car to follow the route indicated by the spline. The map of the racetrack generated by SLAM will be used in the laps following the first lap to navigate the track.

Contents

• What is Factor Graph SLAM?
• How to run iSAM2 on the EUFS simulator
  • Prerequisites
  • Setup
• More CMR Driverless

Source Files

• isam2.cpp
• isam2Node.cpp
• Scripts