Fast, Safe and Robust Motion Planning for Autonomous Vehicles Based on Robust Control Invariant Tubes

Abstract This paper tackles uncertainties between planning and actual models. It extends the concept of RCI (robust control invariant) tubes, originally a parameterized representation of closed-loop control robustness in traditional feedback control, to the domain of motion planning for autonomous vehicles. Thus, closed-loop system uncertainty can be preemptively addressed during vehicle motion planning. This involves selecting collision-free trajectories to minimize the volume of robust invariant tubes. Furthermore, constraints on state and control variables are translated into constraints on the RCI tubes of the closed-loop system, ensuring that motion planning produces a safe and optimal trajectory while maintaining flexibility, rather than solely optimizing for the open-loop nominal model. Additionally, to expedite the solving process, we were inspired by $${\mathcal{L}}_{2}$$ <mml:math xmlns:mml="http://www.w3.org/1998/Math/MathML"> <mml:msub> <mml:mi>L</mml:mi> <mml:mn>2</mml:mn> </mml:msub> </mml:math> gain to parameterize the RCI tubes and developed a parameterized explicit iterative expression for propagating ellipsoidal uncertainty sets within closed-loop systems. Furthermore, we applied the pseudospectral orthogonal collocation method to parameterize the optimization problem of transcribing trajectories using high-order Lagrangian polynomials. Finally, under various operating conditions, we incorporate both the kinematic and dynamic models of the vehicle and also conduct simulations and analyses of uncertainties such as heading angle measurement, chassis response, and steering hysteresis. Our proposed robust motion planning framework has been validated to effectively address nearly all bounded uncertainties while anticipating potential tracking errors in control during the planning phase. This ensures fast, closed-loop safety and robustness in vehicle motion planning.