In this project the goal is to safely navigate around a virtual highway with other traffic that is driving +/-10 MPH of the 50 MPH speed limit. The planning algorithm is provided the car’s localization and sensor fusion data and welll as a sparse map of waypoints around the highway. The outlined algorithm satisfies all the key performance indicator (KPI) requirements provided as input.
The KPIs for this project are:
-
the car should go as close as possible to the 50 MPH speed limit. Since the car moves 50 times a second, a distance of 0.5m per move will create a velocity of 25 m/s. 25 m/s is close to 50 MPH.
-
As a consequence of the speed requirement, the algorithm should demonstrate passing slower traffic when possible in the presence of other cars that try to change lanes too.
-
The car should avoid hitting other cars at all cost as well as driving inside of the marked road lanes at all times, unless going from one lane to another.
-
The car should be able to make one complete loop around the 6946m highway.
-
The car should not experience total acceleration over 10 m/s^2 and jerk that is greater than 10 m/s^3.
-
Acceleration is calculated by comparing the rate of change of average speed over .2 second intervals. Part of the total acceleration is the normal component, AccN which measures the centripetal acceleration from turning. The tighter and faster a turn is made, the higher the AccN value will be.
-
The jerk is calculated as the average acceleration over 1 second intervals.
-
The map of the highway is provided in data/highway_map.txt file. It contains a total of 181 waypoints, with the last waypoint mapping back around to the first. The waypoints are in the middle of the double-yellow diving line in the center of the highway.
Each waypoint in the list contains \( [x, y, s, dx, dy] \) values. \( x \) and \( y \) are the waypoint’s global map coordinate position.The Frenet s value is the distance along the road to get to that waypoint in meters. The Frenet d unit normal vector is split up into the x component (dx) , and the y component (dy). 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. The first waypoint has an s value of 0 because it is the starting point. Since the car is trying to go 50 MPH, it should take a little over 5 minutes to complete 1 loop.
The highway has 6 lanes total - 3 heading in each direction. Each lane is 4 m wide and the car should only ever be in one of the 3 lanes on the right-hand side. The car should always be inside a lane unless doing a lane change. The d vector has a magnitude of 1 and points perpendicular to the road in the direction of the right-hand side of the road. The d vector can be used to calculate lane positions. For example, if you want to be in the left lane at some waypoint just add the waypoint’s (x,y) coordinates with the d vector multiplied by 2. Since the lane is 4 m wide, the middle of the left lane (the lane closest to the double-yellow diving line) is 2 m from the waypoint. If you would like to be in the middle lane, add the waypoint’s coordinates to the d vector multiplied by 6 = (2+4), since the center of the middle lane is 4 m from the center of the left lane, which is itself 2 m from the double-yellow diving line and the waypoints.
Here is the data provided from the Simulator to the C++ Program
| Variable | Description |
|---|---|
["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_deg"] |
The car’s yaw_deg angle in the map |
["speed"] |
The car’s speed in MPH |
| Variable | Description |
|---|---|
["previous_path_x"] |
The previous list of x points previously given to the simulator. There will be some latency between the simulator running and the path planner returning a path, with optimized code usually its not very long maybe just 1-3 time steps. During this delay the simulator will continue using points that it was last given. Because of this its a good idea to store the last points used so we can have a smooth transition. previous_path_x, and previous_path_y can be helpful for this transition since they show the last points given to the simulator controller with the processed points already removed. You would either return a path that extends this previous path or make sure to create a new path that has a smooth transition with this last path. |
["previous_path_y"] |
The previous list of y points previously given to the simulator. See above for explanation. |
Note: Returning 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.
Variable |
Description |
["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 |
| Variable | Description |
|---|---|
["sensor_fusion"] |
A 2d vector of cars and then that car’s [car’s unique ID, global car’s x position in map coordinates, global car’s y position in map coordinates, global car’s x velocity in m/s, global car’s y velocity in m/s, car’s s position in Frenet coordinates, car’s d position in Frenet coordinates. The vx, vy values can be useful for predicting where the cars will be in the future. For instance, if you were to assume that the tracked car kept moving along the road, then its future predicted Frenet s value will be its current s value plus its (transformed) total velocity (m/s) multiplied by the time elapsed into the future (s). |
The car uses a perfect controller and every 20ms will visit every (x,y) point in the list it receives. The units for the (x,y) points are in meters and the spacing of the points determines the speed of the car. The vector going from a point to the next point in the list dictates the angle of the car. Acceleration both in the tangential and normal directions is measured along with the jerk, the rate of change of total acceleration. Currently jerk is over a 0.2 second interval, it would probably be better to average total acceleration over 1 second and measure jerk from that.
The algorithm must create paths that can smoothly changes lanes. Any time the vehicle approaches a car in front of it that is moving slower than the speed limit, the vehicle should consider changing lanes.
The car should only change lanes if such a change would be safe, and also if the lane change would help it move through the flow of traffic better.
For safety, a lane change path should optimize the distance away from other traffic. For comfort, a lane change path should also result in low acceleration and jerk. In this implementation we created smooth trajectories using spline interpolation between 5 points 2 points at the end of the previous path and 3 new points spaced along the map waypoints at 30m, 60m and 90m respectively The header-only implementation of spline function as implemented here http://kluge.in-chemnitz.de/opensource/spline/ was used.