Trajectory Optimization for Curvature Bounded Non-Holonomic Vehicles: Application to Autonomous Driving

TL;DR

This paper introduces a hierarchical trajectory optimization method for nonholonomic curvature-bounded vehicles, enabling smooth, collision-free paths in complex scenarios with improved computational efficiency for autonomous driving.

Contribution

The paper presents a novel hierarchical optimization routine that alternates between angular acceleration and linear velocity spaces, enhancing efficiency and accuracy in trajectory planning.

Findings

Hierarchical optimization reduces computational time compared to joint optimization.

The method produces smooth, collision-free trajectories suitable for complex driving maneuvers.

The approach outperforms traditional polygon approximation methods in efficiency and conservativeness.

Abstract

In this paper, we propose a trajectory optimization for computing smooth collision free trajectories for nonholonomic curvature bounded vehicles among static and dynamic obstacles. One of the key novelties of our formulation is a hierarchal optimization routine which alternately operates in the space of angular accelerations and linear velocities. That is, the optimization has a two layer structure wherein angular accelerations are optimized keeping the linear velocities fixed and vice versa. If the vehicle/obstacles are modeled as circles than the velocity optimization layer can be shown to have the computationally efficient difference of convex structure commonly observed for linear systems. This leads to a less conservative approximation as compared to that obtained by approximating each polygon individually through its circumscribing circle. On the other hand, it leads to optimization problem with less number of constraints as compared to that obtained when approximating polygons as multiple overlapping circles. We use the proposed trajectory optimization as the basis for constructing a Model Predictive Control framework for navigating an autonomous car in complex scenarios like overtaking, lane changing and merging. Moreover, we also highlight the advantages provided by the alternating optimization routine. Specifically, we show it produces trajectories which have comparable arc lengths and smoothness as compared to those produced with joint simultaneous optimization in the space of angular accelerations and linear velocities. However, importantly, the alternating optimization provides some gain in computational time.