The goal of this project is to run the car in the following manner:
- Keep the speed as close to 50 MPH as possible but not to exceed it.
- The car should not experience total acceleration over 10 m/s^2 and jerk that is greater than 50 m/s^3.
- Change the lane when appropriate.
- Avoid accidents.
This project is run on a car simulator. Detailed instructions can be found here.
My path planning algorithm is based on the finite-state machine approach. These states are:
- Staying on the middle lane
- Staying on the right lane
- Staying on the left lane
- Stopping
In the first three states. the car will try to reach the desired speed (44 MPH or 20 m/s). Therefore, this implementation does not have an explicit state for acceleration process. At any instant, the car reads the car's localization and the sensor fusion data to determine whether it should stay on its lane, switch to a adjacent lane, or reduce its speed.
The cost function is in the following form:
L(state)=C1/(time to next possible collision)+C2(desired_speed-current_speed).
C1 and C2 are coefficients that can be altered to achieve the most desirable results. I simply choose C1=C2=1.
I use the localization data (x, y, vx, vy) given by sensor_fusion to calculate the time that a collision would happen for each state.
This assumes the adjacent cars moves in a constant speed.
The car trajectory in the this implementation is mainly generated by spline fitting. This is easy to implement and very efficient.
The fitting comprises of the following steps:
- Select the end point of the remain trajectory as the reference point s_0
- Find the way points after this reference point on the map.
- Convert these waypoints from global coordinates to car reference coordinates.
- Fit these waypoints to obtain a spline fitted model s'
- Starting from the reference point. Estimate an appropriate length of segment to the next point, say \Delta s.
- Calculate the x coordinate that s+\Delta s projects on.
- Find the y coordinate from the fitted spline, namely s'(x+\Delta s)
- Convert the trajectory from car reference coordinates back to global coordinates.
Note that acceleration is included in the generation of trajectory. For each interval, the instantaneous speed of the car is recalculated with the following iterative equations:
where v_{N} and v_{N+1} are the instantaneous speed of the car at the N-th and (N+1)-th step, respectively, whereas x_{N+1} is the car position and the N-th step.
-
Evalulate the states using the Markov decision process: the current implementation chooses the next state with the lowest cost. This is essentially a Greedy approach. It would be interesting to search the deeper states to make the car more intelligent.
-
My current model assumes that the neighboring cars move with constant speed. It would be more precise to calculate the car position of the car probabilistically rather than deterministically.