Two-Stage Hierarchical Motion Planning with Basis-Splines in Highway Environments

Road environments can include several local optimal solutions to be explored by a motion planning algorithm, which can be computationally complex. This work proposes a two-stage approach, where each stage solves the same basis-spline parameterized optimal control problem. In the first step, a solution is selected from a discrete set of trajectory candidates. In the second one, the trajectory is refined by a nonlinear optimization algorithm in the continuous solution space. The non-uniform basis-spline parameterization guarantees the trajectory's time-continuous feasibility. Final costs are introduced to ensure convergence towards a final manifold. The approach is evaluated in a highway scenario where other vehicles must be circumnavigated to reach a specified velocity and lane center.