The Culmination of A Self Driving Car Journey (Part 1)
Path Planning
Having been slow on writing Medium posts the last few months, I’m finally getting the chance to write about the final term of the Udacity Self-Driving Car Engineer Nanodegree. It’s been an absolutely phenomenal experience between all three terms, and I hope to reflect more on the sum total of the program in the final part of this post. For now, I want to cover the first of the three projects of the final term, Path Planning.
I also want to put a quick disclaimer to start the article, as I recently started a position with Udacity as a Student Experience Lead for the Self-Driving Car Engineer Nanodegree program, where I’ll get to help more students along their own paths to the self-driving car industry. These posts will not be made as any part of my new position with Udacity, but instead from my experience as a student.
Anyway, back to Path Planning. For myself, I think this was the toughest of all the projects, and it looked like from the forums and Slack channel that this was a common occurrence for most of the students. Part of this came down to figuring out the right way to implement some of the classroom concepts, and part of it also came from potential solutions we had not covered as deeply.
Path Planning revolves around telling the car where to move on the road. Now, this probably sounds a little similar to the Behavioral Cloning project from Term 1, but this project steps it up a few notches by adding in additional lanes and traffic. The simulator vehicle will need to be capable of deciding when to change lanes to avoid slow traffic, while also deciding to keep its lane if a desired lane change is unsafe due to other cars in adjacent lanes. Note that for Path Planning, we’ll think of the road as distance along the road being a “s” value, while distance from the middle of the road toward the outside being a “d” value (the Frenet frame/space, for those interested). Therefore, an “s” value of 10 meters would mean you had traveled 10 meters along a lane from your start point, while a “d” value of 5 meters would mean you were in a lane five meters away from the center line (or wherever you are measuring the “d” value from).
One approach to this is to first start with a Behavior Planner, whereby the car 1) determines its desired speed, 2) checks whether the car in front of it (if any) is going below that speed, and therefore makes a lane change desirable, 3) checks the adjacent lanes for open space, as well as speed of vehicles in those lanes, 4) determines whether to make a lane change or stay in the same lane. Note that even if a lane has open space directly to the side of the vehicle, changing into that lane might not be a great idea. A faster car may be approaching from behind, meaning your car will get hit or hit it by the time it changes lanes, or a very slow car could be ahead in the next lane, and you’ll end up even slower than staying in your initial lane.
At this point, it might seem like we’re already good to go. However, in the simulator, there are multiple constraints to take into account to make the drive a smooth and safe experience. Along with staying on the road and not crashing into other vehicles (hopefully covered in the Behavior Planner already), there were also limits on max speed, acceleration, and jerk (jerk being the change in/derivative of acceleration). Too high of jerk would make the passengers whip wildly backwards or forwards in their seat, causing a lot of discomfort.
Trajectory generation was the main classroom concept here, which involves fitting a fifth-order polynomial for a jerk-minimizing trajectory (JMT). To take a quick step back to our “s” and “d” values above, we can think of our current state in “s” as our s-position, speed (first derivative of “s”, or change in “s” position over a given period of time), and acceleration (second derivative of “s”, or change in speed over the given period of time). Similarly, the “d” value in our state has a first derivative of “d” direction speed and second derivative of “d” direction acceleration. JMT allows you to plug in the current state [s, speed, acceleration] (or similar for d), as well as desired end state, and then fitting the polynomial function to minimize jerk, or the third derivative of the first state value (“s” in this case). JMT will return the polynomials of a line for the car to follow from point to point on the road. Note that the desired end states are fairly easy to come up with; if you want to travel 100 meters down the road, you’d increase the “s” value by 100 from your start state, put speed as how fast you want to be going, etc. The desired “d” values can be done similarly, by adding or subtracting how many meters you want to change from your current lane position respective to other lanes (or more accurately in the Udacity simulator, from the center line).
As wordy as the above might be, it allows for a lot of flexibility in generating multiple potential trajectories for your vehicle. This is because you can also send in a distribution of desired end states, and calculate the jerk minimizing trajectory for each of these. I originally chose to used a normalized distribution around my original desired end state to accomplish this.
What’s the point of having multiple possible trajectories? Well, the next step is to implement various cost functions to pick the best of your calculated trajectories. Some of the original cost functions I included were based around different “s” and “d” positions, different speeds and accelerations, high costs around collisions or running off road (in case the additional distributions I added included such trajectories), and even how much of a buffer from other vehicles was given by the trajectory.
I implemented all of the above, and continued tweaking for a couple weeks as I tried to narrow down my issues and complete a full circuit of the simulator track. One big difficulty is that the simulator for Path Planning uses a perfect controller, whereby when you feed back a given position (note that the simulator takes global X & Y coordinates, so you’ll have to convert between them and the “s” & “d” values), the car essentially teleports to that spot. So if you have your desired trajectory points too far apart, you’ll easily surpass the acceleration and jerk constraints that you cannot exceed in order to pass the project. (You could potentially implement a PID controller to mitigate this, although I never took the time to do so.)
I got close with this approach, really close. But unfortunately, I did not succeed. I had a TON of close ones, but my car always had a random lane change where it exceeded max acceleration, or got too excited in heavy traffic and decided its best option was to ram the car in front of it to make space (something some may wish to do in Bay Area traffic, but that’s of course frowned upon). I even once managed to flip my car on its roof, although because of the perfect controller in the simulator, it just keep right on driving around the track with its wheels in the sky.
So what was the solution here? It ended up being what is called a “spline”, which helps to interpolate between points on a polynomial function, or essentially creating a smooth trajectory based on a few given waypoints. For my own, after determining through my behavior planner where the car needed to end up at, I used my desired lane or “d” value, with 50 meter intervals in distance down the road, or “s” values, to determine these waypoints. After my car’s path was calculated by the spline, I could then set points along the spline based on desired speed, which I would raise or lower to my desired speed. My desired speed was based off either the car in front of me (match them) or going up to the speed limit, if not blocked.
After implementing the spline, my car was now able to make its way around the track for the necessary distance without collisions or violating any other constraints (speed, acceleration and jerk, primarily). I did notice in re-running the simulator multiple times in coming back to this article that I did see a couple random collisions however, so it appears I may still have a little room for improvement on my current algorithm, even if it does typically make it around the track just fine.
That’s it for Path Planning! The second project of Term 3 is an elective, where I could choose to do either Functional Safety or Advanced Deep Learning. I chose Advanced Deep Learning, and you can read about it here!
