Highway Path Planning
This article is about my approach for Udacity Self-driving Cars Nanodegree Path Planning project. It was the most exciting and challenging project so far. And I can say I am totally in love with path planning. One more positive thing about this project is a chance to take part in Bosch Challenge. Unfortunately, I did not pay enough attention to coordinates conversions and my car crashed into the first car on Bosch final track. Though it gave me enough motivation to correct my project, and finally I was able to run my car around all three tracks and get 135.27 sec result for the final Bosch track.
I liked the approach from the Path Planning Walkthrough. I mean we control car’s velocity instead of acceleration. If we do not increase speed more than 0.224 mph in 0.02 sec time frame, our car will not suffer from longitudinal jerks. If we use spline library to smooth our path, we will not suffer from latitudinal jerks. This approximation is rough but sufficient for our toy track (actually, sometimes I still able to see maximum jerk violation message).
To analyze and predict car’s behavior I work with frenet coordinates. It is not easy to determine from Cartesian coordinates whether two cars are in the same lane. It is better to use frenet coordinates for this. The idea behind frenet coordinates is pretty simple. If we take the middle of the road as y axis and define d as a distance from the y axis, we get frenet coordinates. Now to determine whether two cars are in the same lane, we just need to compare d-value of frenet coordinates.
In my initial approach, I checked d-value every time to find car’s lane. Unfortunately, provided transformation between frenet and Cartesian coordinates has poor accuracy, and information about busy lanes was incorrect pretty often that led to accidents.
As a result, I decided to store lane number as a variable of the car and update the value only after some threshold. Of course, it is a trade-off. Too large threshold would lead to accidents in case some car changed the lane in front of my car. And too small threshold would not help me to ignore conversion issues. I have chosen something in the middle. Unfortunately, I am still able to see some issues related to wrong coordinates transformation, and sometimes my car is not able to detect the vehicle in front it when it changed lane.
To find the best car’s behavior, I use a final state machine. It is not easy to decide which states lead to a good result and it is itself a large topic for discussion. I decided to take the approach from the lectures and ended up with five states:
As it is shown in the picture, my car is in the initial state only at the very first moment. Later it never returns to the original state. One more thing to notice is that I can be in state change lane only one time in a raw. Then I have to move to keep lane state.
A brief description of states:
- CS (constant speed) — initial state of the car. When the car leaves this state it never returns to it.
- KL (keep lane) — car should stay in the same lane and adjust speed according to conditions (e.g., set maximum speed or set the same speed as the car ahead).
- PLCL, PLCR (prepare lane change left, prepare lane change right) — after some thought I decided that the only thing the car does here is increasing speed up to the maximum value (we do not care about cars in front of us since the car is going to change the lane in near future). Only when some car in front of me is too close, I slow down.
- LCL, LCR (lane change left, lane change right) —the car change current lane
The most important part of the project is to define cost functions. Let’s say we are on a highway with no other cars around. In such situation, we should stay in the same lane. Constant lane changing is extremely uncomfortable, so my first cost function is change_lane_cost. We penalized our trajectory if we want to change lane. Honestly, I did a small trick for Bosch challenge. I did not penalize the trajectory if I want to move in the middle lane. It gives me more freedom with maneuvers. Otherwise, I can be stuck in the left-most and right-most lanes when my lane and middle lanes are busy.
My next task was to force a car to change state from KL to PLCL or PLCR states. When we see a car in front of us, we should slow down the speed. However, in prepare lane change state we still have maximum available speed. So, if we penalize trajectory with lower speed, our car should prefer to switch to prepare change lane states. As a result, I added inefficiency_cost cost function.
It was not enough since speed difference was not significant enough and I was not able to defeat cost of change_lane_cost. So, I added one more cost function free_line_cost. We find the closest obstacle in the lane and penalize the trajectory according to this distance.
But what if we have another car in the proposed lane? How can we avoid the accident? I decided to add another cost function collision_cost. We just check that change lane is safe in this situation using some basic physics. If it is not, we penalized the trajectory heavily.
OK, now we in PLCL or PLCR state. How and when should we force our car to change the lane? At this point, PLCL(PLCR) and LCL(LCR) have the same cost! After some consideration, I decided to add buffer_cost cost function. We find the closest car in my current lane (for LCL it is the same as a proposed lane, and for PLCL it is an original one). If the closest car is too close, I penalized the trajectory. And the car finally change the lane! This cost function also helps me to avoid accidents when the car from the next lane suddenly decides to change the lane in front of me!
In my implementation, I have a prediction module as well. I try to predict car’s position in some time in future, and then I can use this information to predict my car behavior. Prediction is not crucial for this toy project, and I do not use future positions (only current one) in my trajectory planning. Though, it is easy to turn on this functionality.
Notes
We were provided with helper functions to convert between Frenet and Cartesian coordinates. They are useful and reduce boilerplate, though their accuracy is poor. It is possible to notice odd car behavior on windy parts of the road due to the lack of accuracy. Sometimes it can lead to accidents since I believe the other car is in my lane though it is not.
I spend most of the time on tuning my collision_cost function. It is a crucial part of the project and could be tricky to implement correctly. Collision detection is still not perfect, and I can face car accidents from time to time. Though in major cases it is caused by poor transformation accuracy.
After spending a lot of time tuning parameters for my cost functions, I finally got the result, which I found satisfying. My code base can be found on GitHub. Initially, I had two branches: one for Path Planning project and another one for Bosch challenge. Then I stick to one solution for all tracks (it is required to set a corresponding .csv file to switch between tracks).
Also, I’d like to notice that car’s behavior depends on my computer’s mood. Even for final Bosch track with the same cars trajectories, I can get 5–6 different results. If my computer does not have enough free computational power at the moment, I can even face some accidents.
Udacity track:
Bosch challenge 2.75 mile track:
Bosch challenge 1.75 mile final track:
