Time-Optimal Trajectory Planning in Highway Scenarios using Basis-Spline Parameterization

Philip Dorpmüller,Thomas Schmitz,Naveen Bejagam,Torsten Bertram

2023-10-05

Abstract:Basis splines enable a time-continuous feasibility check with a finite number of constraints. Constraints apply to the whole trajectory for motion planning applications that require a collision-free and dynamically feasible trajectory. Existing motion planners that rely on gradient-based optimization apply time scaling to implement a shrinking planning horizon. They neither guarantee a recursively feasible trajectory nor enable reaching two terminal manifold parts at different time scales. This paper proposes a nonlinear optimization problem that addresses the drawbacks of existing approaches. Therefore, the spline breakpoints are included in the optimization variables. Transformations between spline bases are implemented so a sparse problem formulation is achieved. A strategy for breakpoint removal enables the convergence into a terminal manifold. The evaluation in an overtaking scenario shows the influence of the breakpoint number on the solution quality and the time required for optimization.

Robotics

What problem does this paper attempt to address?

### Problems the Paper Attempts to Solve

This paper aims to address the time-optimal trajectory planning problem in highway scenarios. Specifically, the paper proposes a nonlinear optimization problem based on B-spline parameterization to generate an optimal vehicle trajectory that ends at a specified terminal manifold (including target speed and lane). The main contributions of this method are:

1. **Recursive Feasibility**: By optimizing the breakpoints (BPs) of the B-spline and introducing a new breakpoint removal strategy, the recursive feasibility of the trajectory is ensured. This means that if a feasible solution is found at the current step, a feasible solution can also be guaranteed at the next step.

2. **Multiple Time Scales**: Different time scales in the longitudinal and lateral directions are introduced, allowing the vehicle to reach the target speed and target lane at different times.

3. **Continuous-Time Feasibility**: Utilizing the properties of B-splines, the feasibility of the entire trajectory in continuous time is ensured, not just at discrete time points.

### Background and Motivation

Existing motion planning methods typically rely on gradient optimization and achieve planning horizon reduction through time scaling. However, these methods have the following issues:

- **Inability to Ensure Recursive Feasibility**: Existing methods cannot ensure recursive feasibility, meaning that if a feasible solution is found at the current step, a feasible solution may not be found at the next step.
- **Single Time Scale**: Existing methods typically use a single time scale, making it impossible to achieve different time planning in the longitudinal and lateral directions.
- **Feasibility Check at Discrete Time Points**: Existing methods typically perform feasibility checks at discrete time points, failing to ensure feasibility in continuous time.

### Method Overview

1. **Trajectory Representation**: The vehicle trajectory is represented as two sixth-order B-spline functions for the longitudinal and lateral directions, respectively. The coefficients and breakpoints of the B-spline are the optimization variables.
2. **Constraints**:
- **Dynamic Feasibility**: Dynamic feasibility is ensured by constraining acceleration and curvature.
- **Collision Avoidance**: Collision avoidance is ensured by approximating the rectangular vehicle shape with ellipses and using Minkowski sums.
- **Heading Angle Constraint**: The heading angle is constrained to ensure that the low heading angle assumption is not violated.
- **Terminal Manifold**: The trajectory needs to reach the target speed and target lane within the specified time.
3. **Cost Function**: The cost function is defined as the weighted sum of time and the integral of squared jerk to balance comfort and goal achievement progress.
4. **Nonlinear Optimization Problem**: All the above expressions are combined into a nonlinear optimization problem, which is solved using CasADi and the IPOPT solver.

### Experimental Evaluation

The paper conducts experimental evaluations in the Town04 highway scenario of the Carla simulator, considering up to 5 obstacle vehicles. The experimental results show:

- **Asymptotic Convergence**: The vehicle can asymptotically converge to the terminal manifold.
- **Cost Function Value**: As the number of internal breakpoints increases, the initial cost value decreases, but additional internal breakpoints provide diminishing improvements to the cost.
- **Optimization Time**: As the number of internal breakpoints and obstacle vehicles increases, the optimization time gradually increases.

### Future Work

- **Following the Lead Vehicle**: Consider using the predicted trajectory of the lead vehicle as the terminal manifold to achieve safe constant time gap following.
- **Discrete Trajectory Search**: Develop a planning algorithm that searches for B-spline trajectories in a discrete set to provide initial trajectories and terminal manifolds.

Through these methods, the paper provides an effective and reliable solution for time-optimal trajectory planning in highway scenarios.