Implement a simple real-time path planner in C++ to navigate a car around a simulated highway scenario

View the Project on GitHub

Path Planner for Highway Autonomous Driving


Implement a simple real-time path planner in C++ to navigate a car around a simulated highway scenario, including other traffic, given waypoint, and sensor fusion data.

The planned path should be safe and smooth, so that tha car avoids collisions with other vehicles, keeps within a lane (aside from short periods of time while changing lanes), and drive according to the speed limits (50mph). Besides, the car must not violate a set of motion constraints, e.g. maximum acceleration (10 m/s^2), and maximum jerk (50 m/s^3). As a results, the car is drive safely and the passengers feel comfortable.

Demo (video playbacks 4-time faster than real)


(click to see the full video)

1 Code & Files

1.1 My project includes the following files and folders

1.2 Dependencies & my environment

1.3 How to run the code

  1. Clone this repo.
  2. Make a build directory: mkdir build && cd build
  3. Compile: cmake .. && make
  4. Run it: ./path_planning.
  5. Open the simulator, which is downloaded from here

1.4. Release History

2 Approaches

2.1 Algorithm Steps

(1) Utilize the sensor fusion data to check each lane’s availability

Go through all the detected vehicle’s information and status, and check the availabilities of each lanes. See code from line 256-306 in src/main.cpp.

(2) Based on the lanes’ availability to plan the next state of the car

Choose one of the following states for the car, based on the lane’s availability which is obtained from the sensor fusion. See code from line 310-324 in src/main.cpp.

(3) Use anchor points to interpolate a spline curve

To create a smooth path, two last points from the previous planned path and another 3 predicted future points are used together for computing a smooth spline curve, which is visualized as planned path. Then break the entire spline curve into 50 points which is used to control the vehicle’s speed (our car visits them sequentially every 0.02 seconds). See code from line 332-443 in src/main.cpp.

2.2 Frenet coordinates

Cartesian coordinates Frenet coordinates Benifits of Frenet

Frenet coordinates make the math easy to describe the driving behaviors.

3 Data format

3.1 The map of the highway is in data/highway_map.txt

Each waypoint in the list contains [x,y,s,dx,dy] values. x and y are the waypoint’s map coordinate position, the s value is the distance along the road to get to that waypoint in meters, the dx and dy values define the unit normal vector pointing outward of the highway loop. The highway’s waypoints loop around so the frenet s value, distance along the road, goes from 0 to 6945.554.

3.2 Main car’s localization Data

[“x”] The car’s x position in map coordinates

[“y”] The car’s y position in map coordinates

[”s”] The car’s s position in frenet coordinates

[“d”] The car’s d position in frenet coordinates

[“yaw”] The car’s yaw angle in the map

[“speed”] The car’s speed in MPH

3.3 Previous path data given to the Planner

//Note: Return the previous list but with processed points removed, can be a nice tool to show how far along the path has processed since last time.

[“previous_path_x”] The previous list of x points previously given to the simulator

[“previous_path_y”] The previous list of y points previously given to the simulator

3.4 Previous path’s end s and d values

[“end_path_s”] The previous list’s last point’s frenet s value

[“end_path_d”] The previous list’s last point’s frenet d value

4 Discussion

The autonomous car is able to complete loop around the 6946m highway without collisions with other vehicles and uncomfortable jerk, and its follows the road speed limits with gentle acceleration. Besides, the car is able to take over the slow vehicles when there are suitable opportunities, e.g. enough spacefs to both leading and following vehicles.

Currently, the lane changes algorithm is quite simple and to further improve the path planner, we can build a finite state machine to move between actions, and build a cost function to determine the best action.