Seok Lee blog

Path Planning

Path_planning

Summary:

Simulation result (please click the below thumbnail to view a video):

video result

Things to consider:

Resource to tackle the remaining work:

Skill gap

  1. Finite State Machine (FSM)
  2. Cost function
  3. How to utilize Finite State Machine (FSM) (or cost function) for lane change maneuver when necessary.

Work flow (after picking up some ideas from Udacity’s course material):

 if ((lane == 1) && (!lane_0) && (!lane_2))
                {
                  //weightfactor1 = designated_wf;
                  if ((weightfactor0 < weightfactor2) && (abs(weightfactor0 - weightfactor2) > weight_tol))
                  {
                    lane = 0;
                    lane_flag = 0;
                  }
                  else if ((weightfactor0 > weightfactor2) && (abs(weightfactor0 - weightfactor2)>weight_tol))
                  {
                    lane = 2;
                    lane_flag = 1;
                  }
                  else if ((weightfactor0 == 1) && (weightfactor2 == 1)) // dummy testing 
                  {
                    lane = 1;
                    lane_flag = 1.1;
                  }
                  else
                  {
                    lane = 1;
                    lane_flag = 99999;
                  }
                }
            cout << "cl:"<<too_close<<",ln: "<<lane<<",d: "<<d<<",l_f:"<<lane_flag<<",L0: "<<lane_0<<",L2: "<<lane_2<<",L1:"<<lane_1<<",wf: "<<"["<<weightfactor0<<","<<weightfactor1<<","<<weightfactor2<<"]"<<"end_d: "<<end_path_d<<endl;
   else if ((weightfactor1 == 0) &&(weightfactor2 ==0)) // dummy rare corner case
                  {
                    lane = 1;
                    lane_flag = 11.1;
                  }
                  else
                  {
                    lane = 2;
                    lane_flag = 12;
                  }

Simulation result (please click the below thumbnail):

video result

Discussion:

References:

a. FSM

FSM strength_weakness

FSM_highway FSM_transition_function

b. Cost function:

FSM_cost_function_speed

c. Example of cost function:

cost_function

COST FUNCTION NUMBER VERBAL DESCRIPTION
1 Penalizes trajectories that attempt to accelerate at a rate which is not possible for the vehicle
2 Penalizes trajectories that drive off the road
3 Penalizes trajectories that exceed the speed limit
4 Penalizes trajectories that do not stay near the center of the lane.
5 Rewards trajectories that stay near the target lane.

d. All figures appeared in this page are taken from Udacity’s course material.