Integrating Higher-Order Dynamics and Roadway-Compliance into Constrained ILQR-based Trajectory Planning for Autonomous Vehicles

TL;DR

This paper enhances trajectory planning for autonomous vehicles by integrating higher-order vehicle dynamics and roadway compliance into a constrained ILQR framework, improving safety, control, and adherence to road rules in real-time scenarios.

Contribution

It introduces a novel augmented vehicle model with higher-order derivatives and a relaxed barrier function into CILQR, addressing controllability and roadway compliance for better trajectory planning.

Findings

Improved trajectory controllability with higher-order dynamics.

Enhanced roadway adherence in trajectory planning.

Validated real-time performance in simulations and experiments.

Abstract

This paper addresses the advancements in on-road trajectory planning for Autonomous Passenger Vehicles (APV). Trajectory planning aims to produce a globally optimal route for APVs, considering various factors such as vehicle dynamics, constraints, and detected obstacles. Traditional techniques involve a combination of sampling methods followed by optimization algorithms, where the former ensures global awareness and the latter refines for local optima. Notably, the Constrained Iterative Linear Quadratic Regulator (CILQR) optimization algorithm has recently emerged, adapted for APV systems, emphasizing improved safety and comfort. However, existing implementations utilizing the vehicle bicycle kinematic model may not guarantee controllable trajectories. We augment this model by incorporating higher-order terms, including the first and second-order derivatives of curvature and longitudinal jerk. This inclusion facilitates a richer representation in our cost and constraint design. We also address roadway compliance, emphasizing adherence to lane boundaries and directions, which past work often overlooked. Lastly, we adopt a relaxed logarithmic barrier function to address the CILQR's dependency on feasible initial trajectories. The proposed methodology is then validated through simulation and real-world experiment driving scenes in real time.