Path and Motion Planning for Autonomous Vehicles

Udacity - Self-Driving Car NanoDegree

One of the primary purposes of a self-driving vehicle is to be able to arrive at a target location without human intervention. To accomplish this task, the vehicle must be able to figure out a route to arrive at the destination position (Path Planning) and must be able to navigate the obstacles (Motion Planning) to arrive safely at the intended location. For example, if the car wants to arrive at your home, it should first use the GPS and find the shortest route. From this time has been calculated it is required to get context information such as lanes and other vehicles to navigate safely to your house [1]


Image by Free-Photos from Pixabay

In this project[2], we have implemented a planner that allows the simulated vehicle to navigate through three lanes of a highway, avoiding: Collide with other cars, exceed speed, acceleration, and/or jerk, and maintaining the lane until is safe to maneuver.


Change Lanes

The planner::change_lanes method uses a state machine to decide what maneuver must be executed. Using the Frenet coordinate system the vehicle calculates a safe path to drive through traffic.

  1. Uses sensor fusion to identify cars that are on the same lane, if a vehicle is detected on the same lane

  2. When a car is detected first checks that are possible to use the left lane to the overpass. It checks that there are no cars in front or behind within a safe distance(including acceleration)

  3. After checks that is possible to use the right lane to the overpass. It checks that there are no cars in front or behind within a safe distance(including acceleration)

  4. Finally, if a car is detected to be too close, the vehicle slows down, and when it is possible turns to the left or right lanes

Waypoint Planner

The planner::waypoint_planner method verify when is safe to drive using the change_lanes method and drives using the map coordinates.

  1. Gets the car position and map waypoints in S, X, Y

  2. Calculates the yaw

  3. Convert the frenet points into map coordinates

  4. Calculating with the actual lane, the next points required to navigate

  5. Transform to vehicle coordinates

  6. Smooths the navigation, yaw, and path using the spline library

  7. Finally converts back to the X, and Y coordinates to be pass to the vehicle control system


Throughout this project, we apply the knowledge gathered through multiple lessons and partial implementations of algorithms for breadth/depth-first search, Hybrid A*, naive algorithms, cost functions, and Jerk minimizing trajectories which are some of the basis of modern self-driving vehicles. However, much remains to be done to achieve a complete self-autonomy level 5.

As with many of the projects before, I have enjoyed implementing the algorithms and understanding their part in the bigger picture.


[1] A Survey of Motion Planning and ControlTechniques for Self-driving Urban Vehicles

[2] Highway Path Planning Implementation