Diffeomorphism-Transformed Iterative Linear Quadratic Regulator for Constrained Motion Planning in Autonomous Driving

Ensuring safe driving and real-time execution is a crucial requirement in the motion planning process for autonomous vehicles. Hence, there is a compelling demand for advanced motion planning algorithms that exhibit effective management of inequality constraints and exceptional computational performance. This paper investigates a diffeomorphism-transformed iterative linear quadratic regulator (DTiLQR) algorithm for addressing constrained motion planning problems in autonomous vehicles with nonlinear dynamics and multiple inequality constraints. With regard to the state and input constraints, a novel state-and-input diffeomorphism is proposed to transform the constrained state/input space into an unconstrained one. Subsequently, these inequality constraints are systematically incorporated into the vehicle dynamics, thereby leading to the newly constructed system in this context. Then, we reformulate and incorporate the obstacle avoidance constraint into the objective function using state diffeomorphism and logarithmic barrier function. With this, the original optimization problem is converted to the unconstrained counterpart, adhering only to the constructed system dynamics. In this sense, featuring a streamlined single-loop architecture (which is essentially different from the dual-loop algorithmic design of existing constrained iLQR algorithms), DTiLQR is used to solve the optimization problem effectively while maintaining motion performance and constraint satisfaction for the resulting optimal trajectory. Ultimately, case studies across various driving situations showcase the effectiveness and exceptional computational efficiency of the proposed DTiLQR algorithm.