Incrementally constrained dynamic optimization: A computational framework for lane change motion planning of connected and automated vehicles

Lane change is a basic and critical element of complicated driving maneuvers such as overtaking, merging, and exit. Improper lane change is a primary cause for car crashes. This article focuses on the cooperative lane change motion planning task for multiple connected and automated vehicles (CAVs). To describe this task in a straightforward, accurate, and generic way, a centralized optimal control problem should be formulated. This optimal control problem consists of a cost function and many constraints, including the vehicle kinematic constraints, collision-avoidance constraints, two-point boundary conditions, and so forth. However, this optimal control problem is difficult to solve, because the constraints related to all of the CAVs must be considered simultaneously. In order to facilitate the numerical solving process, an incrementally constrained dynamic optimization method is proposed. In this method, a series of subproblems are defined and solved in a sequence such that each one is more difficult than its former one by incorporating more collision-avoidance constraints. The optimum of each one subproblem serves as the initial guess, which warmly starts the solution process of the next subproblem in the sequence. This sequential process continues until the optimal solution to the originally formulated problem is derived. In this way, the difficulties in the original problem are dispersed into multiple parts and then tackled incrementally. Unification, effectiveness, and efficiency of the proposed method have been investigated through a series of simulation tests.