Reflections on Designing a Virtual Highway Path Planner (Part 3/3)
Right now it swerves like a crazy wishy-washy undecided driver and ironically too conservative at keeping its lane at the same time.
Read: Part 1 of 3 | Read: Part 2 of 3| Read: Part 3 of 3
Deciding Which Behavior
Given our sensor information, how do we decide whether to turn left, turn right, or stay in lane? This got me thinking, there are many things we can consider. We can consider the speed and acceleration of nearby objects for example. For example, if leading car in that lane is faster than the leading car in our lane, that’s awesome. If the car at the back of us in that considered lane that we are cutting is moving slow then even better. But do we really have to think that complex? Everything must be made simple but not simpler. Think.
IMPORTANT NOTE: The Udacity Lessons recommends using five states including prepare to change left and prepare to change right. But I decided to try just having three states and just add the two other states if I can’t get away with just three states. I got away with having just three state but to be honest, I think it might be better to have those five states if you want to make a more intelligent path planner.
Maybe a human driver would think that the single most important thing to consider is how much “gap” there is between our vehicle and the vehicle in front of us. As much as possible we want to stay just in one lane and not bump the vehicle in front of us. But if we are going at the required speed but we are already so near the vehicle in front of us then that means we should think about moving left or right. But where should we move? Left or right? Is it even possible to turn anyway? Maybe there’s just cars in that lane that we wouldn’t have space to turn. So we have to consider how much space there is between the nearest vehicle in our desired lane both in front of us and in our back.
Also, I think we should prioritize being in the middle lane most of the time because it provides more freedom. As a concrete example suppose, I’m in the rightmost lane and the front car is slow, so I want to change lane. I see that the leftmost lane has no cars in it but the middle lane also has heavy traffic. I can’t switch from the right lane to the middle lane, so I’m stuck. This wouldn’t happen if I’m in the middle lane.
Okay, I know now that this is how I think. Basically if we are too near the front vehicle we should consider switching lane. It only makes sense to switch if the gap between the potential leading vehicle is larger than our current leading vehicle. It’s only possible to switch lane if there is sufficient gap between us and the nearest front vehicle and that there is sufficient gap between us and the nearest vehicle at the back in the lane we which to switch to avoid bumping to vehicles. Of course if we can turn both at the left or right we want to choose the one with the larger gaps. Also, all things equal, we’d rather stay in the middle lane. We also don’t want to switch lanes all the time because that would just be rude and inefficient.
So how do I translate this to something a computer can understand and process? We can design something like a cost function or equation that considers all of this. What is the cost of staying in the lane, turning left, or turning right? The higher the cost the less we are inclined to do that thing. We choose the lowest cost. It would be best to use a sigmoid or logistic function because it maps values nicely from zero to one. But that’s just a little bit too unintuitive for me. Make everything simple if you can get away with it.
IMPORTANT NOTE: Right now I do think it’s really so much better to sigmoid / logistic cost function so that the the returned cost value is normalized between 0 and 1 or -1 and 1. Maybe I will do that in the future.
It’s impossible to do (there is no road on our right stupid!) then make the cost the highest possible. If the gap between us and the back or front vehicle is below or above a certain threshold then let’s just not risk it, make cost the highest possible. The larger the gap, the better. So let’s make the cost inversely proportional to the gap. When we are considering turning, we consider the gap at the front and the gap at the back, but the gap at the front is more important. So let’s assign add those two components with different weights. For staying in lane, let’s just consider the front gap because if the vehicle at our back is behaving properly we shouldn’t worry about it. Also this means that given equal front gaps, staying in lane would lead a lower cost because there is no back gap component. Let’s test this in our simulator.
With some tweaking I realized that I did not consider prioritizing the middle lane. Let’s add a factor that rewards turning towards the middle lane, this is a fraction less than one that gets multiplied to cost when turning with makes it smaller. Now it’s better. I see that it’s swerving all the time when the gap between the lanes are all high so that means the differences between the costs are almost negligible. We need to do something about this. Let’s add a turn penalty factor to penalize switching turns even more. Still swerving but at least we are not crashing into anyone. I guess this can be tuned further.
Generating an Actual Path
Now we know what behavior/action to do. Either go left, go right, or stay in lane. What location exactly along the forward and sideways direction of the road do we want to end up at at how what period of time? How many seconds? By that time, what speed do we want to have? What acceleration? Oh my god, so many choices. Let’s make everything simple if we can get away with it. Let’s just make the period of time constant. Okay let’s try 1 second if it’s too fast to switch lane let’s try 1.25, 1.5, 1.75 and 2 seconds.
When changing lane let’s just make our goal velocity the same as our starting velocity along the road moving forward. That seems to make sense. Let’s make acceleration zero both along the road and sideways of the road. Once we reach the center of the target road we don’t want be moving sideways anymore so no speed components along that direction, no acceleration either. Zero, zero. We know our target location along the side of the road which is the center of the desired lane, how about along the direction of the road moving forward? I think it’s safe to estimate a desired distance to be the distance taken given our average velocity at that period of time traversed. I found that 1.6 seconds is a decent time to switch lane so that’s cool. The desired distance traveled formula also works well.
When moving straight and there’s sufficient gap or if the car in front of us is moving really really fast, we can go as fast as we can which I say a little below the speed limit to be safe. But if we are too near the front vehicle, just to be safe, let’s just be a little slower than it. But if that vehicle is like super slow like a turtle, let’s not be that slow. Let’s have a minimum threshold of slowness because to be super slow is just too ridiculous.
Okay so we have our starting state, goal state and duration. That’s great! We can use this to generate a path using our `jmt` object I discussed above. Let’s also save this target state, we can use as our starting state later.
No Predictions, No Feasibility Checks???
You might have smelled something fishy here. Three smelly fishy things actually.
First, why didn’t I analyze what the other cars are doing? Why didn’t I predict their intended behaviors and possible trajectory? Isn’t this important to make sure that there are no possible collisions for safety purposes? Well, I probably should’ve. But prediction is so complicated! (Or maybe I’m just lazy) I wanted to make everything simple if I can get away with it, and I did get away with it. To make sure there wouldn’t be collisions, I assumed that the vehicles are driving within speed limit and I made sure that my distance from nearby vehicles is sufficiently high when changing lanes so that I don’t have to worry about collisions. This simplification comes of course with a cost. Most of the time even though the gap is sufficient enough, my car is still afraid to turn.
How about feasibility checks? How am I sure that I won’t exceed the instantaneous speed, acceleration, and jerk limits? Shouldn’t I check for that too? Yeah, I guess we should. But you see I specifically chose conservative target values for final position, speed, acceleration and over a really short time span of 1.6 seconds. That’s why it’s almost improbable (maybe not impossible) that it goes outside the accepted boundaries.
Third, in the first place, why are we waiting for the whole plan to be executed? Shouldn’t we update our path plan as much as possible? Yeah, we probably should. But if we can make things simpler and we can get away with it then that’s probably best. 1.6 seconds isn’t that long anyway.
Converting the Path to what the Controller Understands
Yay, we have our path now! But this path is given with respect to the road along the road and sideways of the road. We have to transform it to global cartesian map coordinates with X, Y axes. How to we do this? We are given a sufficient number of waypoints in the X, Y coordinates and we know how they are mapped along the road. We also have a vector with respect to the global X, Y axes pointing outwards the loop.
We can use a lightweight spline library to estimate a cubic polynomial fthat passes all the way points as a function of the road distance from the starting point. We can use this convert any position expressed as a number along the the road and sideways of the road. So given our path function we can generate the X,Y coordinates every 0.02 second increments (which the controller expects) over a span of the traversed time. Let’s pass this to our controller! We’re done!
Final Statement
As I’ve said earlier, I think so far this is the most difficult project yet. It isn’t as straightforward as our previous projects. This article is about how I approached this project.
It works pretty well but it can always be improved by tuning parameters, designing better cost functions, designing a better estimator of target states, and even considering multiple target states and choosing which is best.
There are also assumptions that is based on the simulator given to us that might not be applicable in other cases. For example, for the first four seconds I assumed that there are no cars going to block us as I accelerated from a speed of zero to my target speed. I am waiting for most of my path points to be executed before appending my new calculated path to be sent. This makes things simpler as given the perfect controller and latency; the state of the starting path is the same as the last desired goal target of the last path. This makes everything simple and works at this case, but will most likely not work at a real environment.
Overall, I think this is an interesting project that I loved (and hated!!) working on. My solution is not the best and makes many assumptions not applicable in most other scenarios so take it with a grain of salt!
*Most images here are from the Udacity lessons and Wikipedia
