Integrating Higher-Order Dynamics and Roadway-Compliance into
  Constrained ILQR-based Trajectory Planning for Autonomous Vehicles
Hanxiang Li Jiaqiao Zhang Sheng Zhu Dongjian Tang Donghao Xu
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.