OPTIMIZATION FUNCTION FOR TURN PLANNING

Embodiments herein include an oversized automated vehicle having a trailer portion and tractor portion performing a J-hook turn at an intersection. The automated vehicle includes an autonomy system for gathering and processing various types of data. The autonomy system generates a candidate trajectory, including the trailer path and the tractor path performing a J-hook turn, and generates driving instructions for the automated vehicle according to a satisfactory candidate trajectory. The autonomy system applies an optimization function for iteratively generating and modifying the candidate trajectory. Using the map data for the intersection, the optimization function modifies the candidate trajectory, such that if the tractor or the trailer were to drive along the predicted tractor path and trailer path, then the tractor and the trailer would perform the J-hook turn while fitting within the drivable surface boundary and inner boundaries (e.g., driving lanes).