Two-stage trajectory planning for automated highway driving
Loading...
Date
2025
Authors
Journal Title
Journal ISSN
Volume Title
Publisher
Alternative Title(s)
Hierarchically combined trajectory sampling and numerical optimization subject to semi-infinite constraints
Abstract
Automated driving is often considered a technology of the future. Assistance systems already support drivers today and can temporarily take over the driving task. Many technical challenges have already been solved. Nevertheless, there is still room for improvement in terms of safety and control performance. Especially on highways, computationally efficient algorithms are needed that ensure consistent driving behavior and make predictive decisions.
The control software of an automated vehicle is usually divided into several modules. Two central components are the behavior planning and the motion planning modules. The behavior planner selects the best maneuver while the motion planner calculates a safe and comfortable trajectory. In this work, the control tasks of both modules are formulated mathematically as a continuous-time optimal control problem. A cost functional describes the preferences of the passengers. Constraints ensure collision avoidance, compliance with traffic rules, and consideration of the planning goal and the vehicle dynamics. In addition, an integer variable encodes the possible driving maneuvers. A set of terminal vehicle states, consisting of local targets along the lane centers, guarantees convergence to the desired vehicle states. Terminal cost enables a far-sighted selection of the next local target on the way to the global target. The future vehicle trajectory is described by polynomial splines based on the differential flatness of the vehicle model. The spline coefficients form convex hulls around the spline functions, which ensure the trajectory's feasibility under continuous-time constraints.
A two-stage algorithm is developed to solve the optimal control problem efficiently. The first stage constructs and evaluates a graph containing a discrete set of candidate trajectories. A graph search algorithm selects the best trajectory and the corresponding maneuver. However, achieving high control performance in the first stage requires a high computational effort. Therefore, the second stage solves a nonlinear program using a numerical optimization algorithm to locally improve the solution of the first stage under the same costs and constraints. Consequently, the first stage always provides a feasible initial solution for the second stage. A shrinking control horizon and a non-uniform spline trajectory parameterization guarantee the convergence of the vehicle state to the terminal set.
The solution algorithms in the two stages, along with their combination, are examined in simulations of various highway scenarios. The evaluation shows that the automated vehicle can be reliably stabilized in the global target in the presence of other vehicles. At the same time, traffic rules and passenger preferences are implemented as far as possible. Reducing the degrees of freedom of the trajectory leads to shorter computing times, but can influence progress toward the destination. Safety and stability are always guaranteed.
Description
Table of contents
Keywords
Automated driving, Model predictive control, Semi-infinite program
Subjects based on RSWK
Autonomes Fahren