Path Planning for Self driving car

Teaching a self driving car to change lane by itself may sound straightforward, but actually this is one of the deepest and hardest problems for a self-driving car, including lots of aspects to pay attention to like Safety, Comfort and Efficiency.

Path planning belong to SAE level 4, which have fully autonomous functional skills, and its process involve various modules.

Sensor fusion collects data around the car and send it to Localization. Then Prediction and Localization modules sends input data as vehicle state, Map, Speed limit and Localization data to the Behavior Planner which compute the maneuver recommendation and send it to the Trajectory planner. The Trajectory also gets data like polynomial trajectory generation from the Prediction module, and send smoothed trajectories to the Motion control. The interaction between Prediction, Behavior and Trajectory is called Path Planning.

The Behaviour planning section includes Safety & Comfort aspects to evaluate the feasibility of moving to a lane in the immediate future, and Efficiency aspect that estimate a score (cost function) for each lane to determine the best lane for the vehicle to be in.

In this Path planning project I created a C++ program that safely navigate around a virtual highway with other traffic that is driving up to 50 MPH. The program communicate with a simulator which provides the car's localization, sensor fusion data and the sparse map list of waypoints around the highway. The car try to go as close as possible to the 50 MPH speed limit, which means passing slower traffic when possible.

Cheers,

Shmulik Willinger