Optimal Trajectory Planning and Decision Making in Lane Change Maneuvers Near a Highway Exit

Paper in proceeding, 2019

In this paper, an optimization based algorithmfor safe and efficient collaborative driving in intersections isformulated. The problem is to determine the optimal orderin which vehicles should travel through an intersection underthe assumptions that the longitudinal velocity of all vehiclescan be controlled along a predefined path. In the originalformulation one quadratic optimization program was solved foreach possible crossing order of the vehicles and collisions wereavoided by formulating constraints that only allowed one vehicleat the time inside the intersection. To make this algorithmmore effective, we formulate less restrictive collision avoidanceconstraints by introducing one critical zone for each point wheretwo predefined paths cross. It is shown that this formulationleads to a decrease in the number of quadratic optimizationprograms that need to be solved to find the best crossingorder. Further, an algorithm is provided that finds the numberof crossing sequences which yield unique formulations of theoptimization program. The results show that when simulatingmore complex scenarios, like four vehicles traveling throughan ordinary intersection, the reduction of computational timeand the total time it takes for all vehicles to make it throughthe intersection can be significantly reduced using these lessrestrictive constraints.