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.
(click to see the full video)
git clone https://github.com/uWebSockets/uWebSockets cd uWebSockets git checkout e94b6e1
mkdir build && cd build
cmake .. && make
(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
(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
(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
|Cartesian coordinates||Frenet coordinates||Benifits of Frenet|
Frenet coordinates make the math easy to describe the driving behaviors.
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.
[“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
//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
[“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
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.