Navigation cost computation for lane changes before a critical intersection

A non-transitory computer-readable medium having program code recorded thereon to select between tactical driving decisions of a controlled ego vehicle to reach a target destination, the program code being executed by a processor and comprising:

• program code to determine upcoming tactical driving decisions of the controlled ego vehicle to reach the target destination according to a mission plan;
• program code to rank upcoming tactical driving maneuvers associated with each of the upcoming tactical driving decisions;
• program code to select a tactical driving maneuver prior to a critical intersection according to the ranking of the upcoming tactical driving maneuvers to reach the target destination according to the mission plan, in which the critical intersection comprises an intersection in which the ego vehicle is to make a turn based on the mission plan to reach the target destination;
• program code to perform, by the controlled ego vehicle, the selected tactical driving maneuver prior to the critical intersection;
• and
• program code to turn the ego vehicle at the intersection and to perform one or more selected ones of the upcoming tactical driving maneuvers to reach the target destination according to the mission plan.

A system to select between tactical driving decisions of a controlled ego vehicle to reach a target destination, the system comprising:

• a vehicle perception module, including a convolutional neural network configured to determine upcoming tactical driving decisions of the controlled ego vehicle to reach the target destination according to a mission plan;
• a navigation cost function module configured to rank upcoming tactical driving maneuvers associated with each of the upcoming tactical driving decisions;
• a vehicle control selection module configured to select a tactical driving maneuver prior to a critical intersection according to the ranking of the upcoming tactical driving maneuvers to reach the target destination according to the mission plan, in which the critical intersection comprises an intersection in which the ego vehicle is to make a turn based on the mission plan to reach the target destination;
• and
• a vehicle behavior controller to perform, by the controlled ego vehicle, the selected tactical driving maneuver prior to the critical intersection, and to turn the ego vehicle at the intersection and to perform one or more selected ones of the upcoming tactical driving maneuvers to reach the target destination according to the mission plan.