A biconvex method for minimum-time motion planning through sequences of convex sets

TL;DR

This paper introduces the Sequence of Convex Sets (SCS), a biconvex method for minimum-time motion planning that traverses a fixed sequence of convex sets under convex velocity and acceleration constraints. By alternately solving two convex subproblems—one with fixed transition points and one with fixed transition velocities—SCS achieves monotone convergence, completeness, and anytime behavior without line-search or trust-region tuning. The approach uses a polygonal initialization and a Bézier-based finite-dimensional parameterization to enable efficient, scalable optimization, with convex restrictions ensuring feasibility at each iteration. Empirical results show that SCS delivers high-quality trajectories substantially faster than state-of-the-art nonconvex solvers and with runtimes comparable to industry waypoint-based planners, while reducing trajectory duration in challenging scenarios such as multi-robot package transfer.

Abstract

We consider the problem of designing a smooth trajectory that traverses a sequence of convex sets in minimum time, while satisfying given velocity and acceleration constraints. This problem is naturally formulated as a nonconvex program. To solve it, we propose a biconvex method that quickly produces an initial trajectory and iteratively refines it by solving two convex subproblems in alternation. This method is guaranteed to converge, returns a feasible trajectory even if stopped early, and does not require the selection of any line-search or trust-region parameter. Exhaustive experiments show that our method finds high-quality trajectories in a fraction of the time of state-of-the-art solvers for nonconvex optimization. In addition, it achieves runtimes comparable to industry-standard waypoint-based motion planners, while consistently designing lower-duration trajectories than existing optimization-based planners.

Figures (8)

• Figure 1: Sparrow robot sorting products into bins in the Amazon warehouses.
• Figure 2: Example of the motion-planning problem. The safe convex sets to be traversed are $\mathcal{Q}_1, \ldots, \mathcal{Q}_4$. The trajectory $\bm q:[0,T] \rightarrow \mathbb R^2$ is shown in blue. The initial and terminal points are $\bm q_\mathrm{init}$ and $\bm q_\mathrm{term}$. The times $t_0, \ldots, t_4$ determine the trajectory piece assigned to each safe set.
• Figure 3: Steps of SCS during the solution of the problem in Fig. \ref{['fig:statement']}. In the initialization (1st panel), we quickly compute a feasible polygonal trajectory. Then, we alternate between a convex subproblem with fixed transition points and one with fixed transition velocities (2nd to 4th panels). We terminate when the cost decrease of an iteration is small enough (5th panel). The trajectory computed at each iteration (solid blue) is overlaid on the trajectory from the previous iteration (dashed red).
• Figure 4: A two-dimensional Bézier curve with control points $\gamma_0, \ldots, \gamma_4$. The area shaded in yellow is the convex hull of the control points.
• Figure 5: Benchmark problem with $I=5$ safe sets in $n=2$ dimensions, with $m=4$ facets each. The optimal trajectory is shown in blue.

Theorems & Definitions (2)

• Proposition 1
• proof