behavior planning——15.cost function design weightTweaking

Designing cost functions is difficult and getting them all to cooperate to produce reasionable vehicle behavior is hard.

some of the difficulties associated with cost function design include solving new problems without unsolving old ones.

when you are working on a self-driving car,you may find that the vehicle is behaving reasonably well except for some

particular situations.Maybe it is not being aggressive enough about making left turns at traffic lights.

So, in an effort to solve this problem,  you either add new cost funcions, tweak existing  ones, or modify weights.But every time you do,  there is  chance that you will introduce  some breaking change into something  that already works.

in pracice ,we solve this throuth regression testing,  where we define some set of situations, Then ,whenever we make a change,  we simulate the viheicle in all of our test cases and make sure that it still behaves  expected.

We won't  say more about testing here,  but it is  an important part of developing  software in a  safety-cricical application.

The next difficulty is balancing costs of drastically different magnitudes.

Because, yes ,we want to get to our destination efficiently, but if   we are in  a situation   where safety   is an issue,

we want to  solve that problem and not think about efficiency at all.   one way to do that  is to have weights which  reflect

the type of problem the cost function addresses.  so  we want to most heavily penalize any behavior which simply is not possible due to physics,  then we want to think about safety ,legality ,comfort.

And only once those are satisfied do we want to think about efficiency. But we alse may want to adjust the relative importance if these  weights depending on situation.  If a light turns red ,for example,legality becomes a much more relevant

concern  than when we engage  in normal highway driving . this leads us to out last difficulty, reasoning about individual cost functions. Ideally, each cost function will serve a very specific responsibility.

which is something we do not do in our earlier example of s speed cost function.

We were trying to balance out desire to drive quickly, which has to do with efficiency,  with out desire to not exceed the speed limit , which is legality.  In practice, we might want to difine several cost functions associated with vehicle speed.

In which case we might have a binary cost function which just checks to see if we are  breaking the speed limit and the continuous cost function which pulls us towards out target speed.

By assigning each cost function to a very specific role, like safety versus legality versus efficiency, we can then standardize the output of all cost functions to be between -1 and 1.  Additionlly, It is helpful to paraetrize whenever possible.

This allows us to use some parameter optimization technique like gradient descent along with our set of  regression tests to

programmatically tweak our  cost functions. Finally ,thinking in terms of vehicle state is helpful. The things we can indirestly

control about  our vehicle are it is position, velocity, and acceleration.  It  can be helpful to keep these in mind when coming up with cost functions. Let is walk through an example.

Say we want to think about the following classer of cost functions.

And to make it easier, to keep everything straight,let is think in terms of position, velocity ,and acceleration.

The binary "Are we exceeding the speed? "cost functin would go here, then the cost function that wanted to keep us close

to the speed limit would go here. and now ,instead of being that weird discontinuous gragh we made earlier, this could just be  some parabola like this.

And even though the cost of this is low even for speeds that exceed the speed limit,  that is OK because we have our

binary cost function which would prevent that behavior.  Continuing to think about speed,  we  also might want to try to

drive at a speed that  is close to the average speed of traffic, for safety reasons ,even if  that speed is above or below the

speed limit. And that would go here.

In the position column, we would have an obvious feasibility concern which is collision avoidance.

We can not drive somewhere if there is already a car there. Then, for safety reasons,  wo would want the buffer distance,

which tries to keep us far from other vehicles, and the cost function which checks to make sure we are driving on the road

near the center of our lane and in a lane that is close to our goal lane.

For acceleration, we would first want to make sure we only consider behaviors that  the car can execute,

Make sure  we only consider behaviors that the car can execute, and then we would want to avoid having any rapid changes in acceleration because those are perceived as uncomfortable.

This is also known as jerk as which you will learn more about in the next lesson.

consider a merge onto a highway, for example.

This is a potentially dangerous situation where we really want to get up to traffic speed as quickly as possible.

So this cost function may become more relevant than it mormally is.  But we alse want to make sure that  we yield if there

is  not really a gap,  so we want to make sure this cost funcion and this one are weighted sufficiently high.

and we can compare these merge priorities to a different situation.

for examle, a car approaching a green light that suddenly turns yellow.

In this situation, we would probably want to boost the weights associated with  legality and would probably need to

add  a whole new cost function for obeying traffic rules.

now that do not fall neatly into the position, velocity ,or acceleration classes, so I will just put it over here.

If this is all starting to feel like it is getting pretty complex ,well,you are right.

It is pretty hard to aviod this exploding complexity when using finite state machines.

Partially that is because of the finite state machine itself, but we are also trying to solve a very hard problem and

some complexity is unavoidable no matter what solution approach you take.