betaFeedback

WorksComputer Science

Contact-based optimization for collision-free motion planning for a million robots

Abstract

Multi-agent robotic systems have attracted growing interest in recent years, with applications ranging from coordinated drone shows to autonomous operations on land, in the air, and on water. At the planning level, robot motion is often modeled using double-integrator dynamics (with low-level controllers handling the physical dynamics during execution) with a minimum-energy objective subject to collision-avoidance constraints and limits on position, velocity, and acceleration. The complexity of the problem resides in the constraints, which couple the trajectories and make the optimization problem nonconvex. Existing numerical approaches typically discretize and optimize entire trajectories, which becomes expensive for large fleets. We exploit the structure of optimal trajectories by organizing optimization around contacts, at which robots reach the minimum allowed separation. We formulate a coupled algebraic system in contact variables and reconstruct trajectories analytically between contacts. Our algorithm, sequential contact insertion (SCI), starts from unconstrained trajectories, inserts contacts at separation violations, and jointly adjusts their times and states. Numerical experiments demonstrate planning for one million robots in minutes and thousands of robots in fractions of a second. On problems feasible for a fixed-grid sequential convex programming baseline, SCI achieves median speedups of several orders of magnitude at matching solution accuracy.

Introduction

Multi-agent robotic systems have attracted growing interest in recent years, with applications ranging from coordinated drone shows to autonomous operations on land [24], in the air, and on water [17]. Planning collision-free motion for a robot fleet is difficult because each robot’s trajectory constrains the motion of the others. We consider transitions between prescribed initial and final states over a fixed time horizon with double-integrator dynamics, a squared-acceleration objective, collision-avoidance and box constraints on position, velocity, and acceleration. This model is commonly used in the literature for fleet motion planning [1, 3, 4, 17, 18], typically paired with low-level controllers that handle the physical dynamics during execution. The complexity of the problem resides in the pairwise separation constraints that make the joint optimization problem nonconvex. The problem is therefore often solved numerically [15, 21, 23], with sequential convex programming (SCP) as a popular choice [1, 10, 14, 20, 25].

A different route to efficient planning, and the one explored in this paper, is to exploit the structure of optimal trajectories. State-constrained optimal control supplies conditions for unconstrained and constrained trajectory pieces and for matching them [6, 13]. Shooting methods use these conditions to construct a finite system of equations for these pieces and the times at which they meet [6]. For collision avoidance, Beaver et al. [4] derive such matching conditions for one agent relative to a prescribed reference trajectory. For single-robot planning around fixed obstacles, Beaver, Tron, and Cassandras [5] solve trajectory equations within a search over obstacle-vertex sequences. For fleets, Beaver and Malikopoulos [3] solve a square algebraic system for each robot’s trajectory while holding higher-priority trajectories fixed. Finally, Kamm et al. [16] completely characterize optimal two-robot motion with zero initial and final velocities for the squared-acceleration objective considered here. We exploit this characterization to construct solutions for larger fleets.

In this work. We formulate fleet planning in terms of pairwise contacts, at which robots reach the prescribed minimum separation. The stationarity conditions yield a coupled algebraic system for the contact times and states, from which the trajectories between contacts are reconstructed analytically. The contact graph decomposes this system into independent groups of interacting robots. Our approach accounts for position, velocity, and acceleration box constraints. Sequential contact insertion (SCI) starts from independent robot trajectories, inserts contacts at separation violations, and jointly adjusts their times and states. Figure 1 illustrates the algorithm and a grid-to-double-helix transfer for N=1000N = 1000 robots. We scale SCI to N=106N = 10^6 robots, which is, to our knowledge, unprecedented for joint continuous-time fleet trajectory optimization. For fleet sizes amenable to existing methods, SCI obtains median speedups of several orders of magnitude over an SCP baseline at matching accuracy.

Sequential contact insertion and grid-to-double-helix transition

Figure 1. Sequential contact insertion. (a) Initialization with unconstrained trajectories. (b) Algorithm loop: detect separation violations, insert a contact, and solve the algebraic system until collision-free. Colored tubes in (a,b) show planar robot trajectories in space-time, with time as the vertical axis; black dots mark contacts. (c) Grid-to-double-helix transition for N=1000N = 1000 robots on a 40×2540 \times25 grid (Experiment 1): snapshots at t=0,50,100t = 0, 50, 100 s; trails show the preceding 20 s. Blue and rust identify the two target strands.

Problem setup

Fleet model

Consider NN robots in Rd\mathbb{R}^d, d≥2d \ge2, and a fixed time horizon T>0T > 0. Robot ii has position xi(t)x_i(t) and prescribed initial and terminal states (xi,0,x˙i,0)(x_{i,0}, \dot{x}_{i,0}) and (xi,T,x˙i,T)(x_{i,T}, \dot{x}_{i,T}). Let R>0R > 0 be the prescribed minimum separation distance between any two robots. Throughout, we assume that the endpoint configurations are strictly separated: ∥xi(t)−xj(t)∥>R\lVert x_i(t) - x_j(t) \rVert> R for i<ji < j and t∈{0,T}t \in\{0,T\}. Componentwise position, velocity, and acceleration limits are prescribed by constant, nonempty boxes Bi(k)={z∈Rd:b‾i(k)≤z≤b‾i(k)}\mathcal{B}_i^{(k)} = \{z \in\mathbb{R}^d : \underline{b}_i^{(k)} \le z \le\overline{b}_i^{(k)}\}, k=0,1,2k = 0,1,2, respectively; the bounds b‾i(k)\underline{b}_i^{(k)} and b‾i(k)\overline{b}_i^{(k)} are vectors in R‾d\overline{\mathbb{R}}^d, R‾=R∪{−∞,∞}\overline{\mathbb{R}} = \mathbb{R} \cup\{-\infty,\infty\}, and the inequalities are understood componentwise.

We use double-integrator dynamics and optimize position trajectories with absolutely continuous velocities and square-integrable accelerations. We seek the fleet trajectory x=(x1,…,xN)x = (x_1,\ldots,x_N) that minimizes total squared acceleration subject to endpoint states, collision avoidance, and box bounds:

min⁡xJ[x]=∑i=1N∫0T∥x¨i(t)∥2 dts.t.(xi(0),x˙i(0))=(xi,0,x˙i,0),(xi(T),x˙i(T))=(xi,T,x˙i,T),∥xi(t)−xj(t)∥≥R,xi(t)∈Bi(0),x˙i(t)∈Bi(1),x¨i(t)∈Bi(2).(1)\begin{aligned} \min_x\quad&J[x] = \sum_{i=1}^{N}\int_0^T \lVert\ddot{x}_i(t)\rVert^2\,dt\\ \mathrm{s.t.}\quad&(x_i(0),\dot{x}_i(0)) = (x_{i,0},\dot{x}_{i,0}),\\ &(x_i(T),\dot{x}_i(T)) = (x_{i,T},\dot{x}_{i,T}),\\ &\lVert x_i(t)-x_j(t)\rVert\ge R,\\ &x_i(t)\in\mathcal{B}_i^{(0)},\quad\dot{x}_i(t)\in\mathcal{B}_i^{(1)},\quad\ddot{x}_i(t)\in\mathcal{B}_i^{(2)}. \tag*{(1)} \end{aligned}

Endpoint conditions and box bounds apply to every robot, and separation constraints apply to every pair i<ji < j. Separation, position, and velocity bounds hold for all t∈[0,T]t \in[0,T]; acceleration bounds hold almost everywhere.

Barycentric decomposition

Define the barycenter xˉ=N−1∑i=1Nxi\bar{x} = N^{-1}\sum_{i=1}^{N}x_i and centered positions x~i=xi−xˉ\tilde{x}_i = x_i - \bar{x}. The pairwise relative positions rij=xi−xj=x~i−x~jr_{ij} = x_i-x_j=\tilde{x}_i-\tilde{x}_j determine separation, while the acceleration cost splits as ∑i=1N∥x¨i(t)∥2=N∥xˉ¨(t)∥2+∑i=1N∥x~¨i(t)∥2\sum_{i=1}^{N}\lVert\ddot{x}_i(t)\rVert^2 = N\lVert\ddot{\bar{x}}(t)\rVert^2+\sum_{i=1}^{N}\lVert\ddot{\tilde{x}}_i(t)\rVert^2. The boxes constrain xi=xˉ+x~ix_i=\bar{x}+\tilde{x}_i and its derivatives, coupling barycentric and centered motion. When all boxes equal Rd\mathbb{R}^d, the barycenter minimizes its own squared-acceleration cost with fixed endpoint states. At every local minimum of this specialization, it is the cubic Hermite interpolant xˉH\bar{x}^{\mathrm{H}} of the mean endpoint positions and velocities [9 Section 2.5].

Necessary conditions

For each pair, consider the times at which ∥rij(t)∥=R\lVert r_{ij}(t)\rVert= R. An isolated such time is an isolated contact; an interval of positive length represents sustained contact. We represent the dual multipliers for continuous collision, position, and velocity constraints by finite Radon measures. Collision multipliers satisfy μji=μij≥0\mu_{ji}=\mu_{ij}\ge0 and are supported at contact times. For position (k=0k=0) and velocity (k=1k=1) bounds, write νi(k)=νi(k),+−νi(k),−\nu_i^{(k)}=\nu_i^{(k),+}-\nu_i^{(k),-}, where the nonnegative component measures are supported on the corresponding active lower and upper bounds by complementary slackness [13]. At a local minimum admitting normal multipliers, stationarity reads

x¨i=proj⁡Bi(2)wi,Dt2wi=∑j≠irij μij+12(νi(0)−Dtνi(1)).(2)\ddot{x}_i=\operatorname{proj}_{\mathcal{B}_i^{(2)}}w_i,\qquad D_t^2w_i=\sum_{j\ne i}r_{ij}\,\mu_{ij}+\frac{1}{2}\left(\nu_i^{(0)}-D_t\nu_i^{(1)}\right). \tag*{(2)}

where wiw_i is an auxiliary momentum and DtkD_t^k denotes the distributional derivative on (0,T)(0,T). The projection clips each component of wiw_i to the corresponding acceleration interval [2].

Unrestricted boxes: When all boxes equal Rd\mathbb{R}^d, the state-bound multipliers vanish and wi=x¨iw_i=\ddot{x}_i, reducing (2) to

Dt4xi=∑j≠irij μij.(3)D_t^4x_i=\sum_{j\ne i}r_{ij}\,\mu_{ij}. \tag*{(3)}

This extends the two-robot contact conditions of Kamm et al. [16] to fleets. The sum couples all encounters involving a robot. On an interval without contacts involving robot ii, the right-hand side vanishes and xi(4)=0x_i^{(4)}=0, so its motion is cubic.

The acceleration is continuous, and the jerk xi(3)x_i^{(3)}, taken as the right-continuous representative of Dt3xiD_t^3x_i, has one-sided limits. Write ⟦f⟧t=f(t+)−f(t−)\llbracket f\rrbracket_t=f(t^+)-f(t^-) for a jump in ff between these limits. Then, at an isolated contact tℓt_\ell involving only the pair p<qp<q, define the contact weight mℓ=μpq({tℓ})≥0m_\ell=\mu_{pq}(\{t_\ell\})\ge0 and set ϵp=1\epsilon_p=1, ϵq=−1\epsilon_q=-1, and ϵi=0\epsilon_i=0 for all other robots. The adjoining pieces satisfy ⟦x¨i⟧tℓ=0\llbracket\ddot{x}_i\rrbracket_{t_\ell}=0 and ⟦xi(3)⟧tℓ=mℓϵirpq(tℓ)\llbracket x_i^{(3)}\rrbracket_{t_\ell}=m_\ell\epsilon_i r_{pq}(t_\ell). Thus only the contacting robots have jerk jumps, equal and opposite along their relative position. At an interior contact, separation reaches a local minimum, giving ⟨rpq(t),r˙pq(t)⟩=0\langle r_{pq}(t),\dot{r}_{pq}(t)\rangle=0.

Contact-based trajectory reconstruction

Finite multiplier representation

We approximate the collision multipliers by a finite list of point contacts. Contact ℓ\ell has a prescribed pair (pℓ,qℓ)(p_\ell,q_\ell), time tℓt_\ell, and weight mℓ≥0m_\ell\ge0: μpq=∑ℓ:(pℓ,qℓ)=(p,q)mℓδtℓ\mu_{pq}=\sum_{\ell:(p_\ell,q_\ell)=(p,q)}m_\ell\delta_{t_\ell}. Here δtℓ\delta_{t_\ell} is the unit Dirac measure at tℓt_\ell. We approximate the dual multipliers associated with constraints on position by atoms, and those associated with constraints on velocity by atoms and constant interval densities.

For a fixed contact list, the stationarity equations can be solved independently on each connected component of the contact graph. This graph has one vertex per robot and an edge for each listed pair. We call its connected components the contact components. Box bounds act on individual robots, so they preserve this decomposition.

To start, take all boxes to be Rd\mathbb{R}^d. Within a component of NN robots, use local labels 1,…,N1,\ldots,N and distinct contact times 0<t1<⋯<tm<T0<t_1<\cdots<t_m<T. We use robot 1 as a reference and store the N−1N-1 relative positions r1jr_{1j}, j=2,…,Nj=2,\ldots,N. Together with the fixed cubic Hermite barycenter, these determine every robot’s position. Set r11=0r_{11}=0. Then rpq=r1q−r1pr_{pq}=r_{1q}-r_{1p}, xˉ1=xˉ+N−1∑j=2Nr1j\bar{x}_1=\bar{x}+N^{-1}\sum_{j=2}^{N}r_{1j}, and xj=x1−r1jx_j=x_1-r_{1j} for j≥2j\geq2.

A contact between robots p<qp<q at time tℓt_\ell has relative state rpq(tℓ)=Rnℓr_{pq}(t_\ell)=Rn_\ell and r˙ℓ=r˙pq(tℓ)\dot{r}_\ell=\dot{r}_{pq}(t_\ell), with ∥nℓ∥2=1\|n_\ell\|^2=1 and ⟨nℓ,r˙ℓ⟩=0\langle n_\ell,\dot{r}_\ell\rangle=0.

Each contact vector cℓc_\ell stores tℓt_\ell, nℓn_\ell, r˙ℓ\dot{r}_\ell, mℓm_\ell, and the states of N−2N-2 other relative coordinates. We omit coordinate qq, since the contact state determines it from coordinate pp:

cℓ=(tℓ,nℓ,r˙ℓ,mℓ,(r1j(tℓ),r˙1j(tℓ))j∈{2,…,N}∖{q}).c_\ell=\left(t_\ell,n_\ell,\dot{r}_\ell,m_\ell,\left(r_{1j}(t_\ell),\dot{r}_{1j}(t_\ell)\right)_{j\in\{2,\ldots,N\}\setminus\{q\}}\right).

The omitted coordinate follows from r1q(tℓ)=r1p(tℓ)+Rnℓr_{1q}(t_\ell)=r_{1p}(t_\ell)+Rn_\ell and r˙1q(tℓ)=r˙1p(tℓ)+r˙ℓ\dot{r}_{1q}(t_\ell)=\dot{r}_{1p}(t_\ell)+\dot{r}_\ell. Between consecutive contact times, each r1jr_{1j} is reconstructed by cubic Hermite interpolation [16]. The first and last pieces use the prescribed endpoint states. Thus c=(c1,…,cm)c=(c_1,\ldots,c_m) determines the component trajectory.

Matching equations

In the relative coordinates r1jr_{1j}, j=2,…,Nj=2,\ldots,N, the conditions for acceleration continuity and jerk jumps from Section II-C become [r¨1j]tℓ=0[\ddot{r}_{1j}]_{t_\ell}=0 and [r...1j]tℓ=mℓR(ϵ1−ϵj)nℓ[\dddot{r}_{1j}]_{t_\ell}=m_\ell R(\epsilon_1-\epsilon_j)n_\ell. When ϵ1−ϵj=0\epsilon_1-\epsilon_j=0, continuity of acceleration and jerk makes the adjoining pieces of r1jr_{1j} the same cubic.

Appending normalization and tangency to these conditions gives F(c)=0F(c)=0, a system of m[2d(N−1)+2]m[2d(N-1)+2] scalar equations in as many unknowns. The equations at tℓt_\ell involve only the two adjacent cubic pieces, hence only cℓc_\ell, its neighboring contacts, and the fixed endpoint data. Accordingly, the Jacobian of FF is block tridiagonal, with blocks of size 2d(N−1)+22d(N-1)+2.

Consider stationary trajectories admitting multipliers with finite support and at most one pair with a nonzero multiplier per instant in each contact component.

Proposition 1 (Contact representation). The continuously feasible reconstructions of roots of F(c)=0F(c)=0 with mℓ≥0m_\ell\geq0 are exactly the stationary fleet trajectories in this construction.

Proof. For a continuously feasible reconstruction of a root, the reconstruction formulas and matching conditions give continuous acceleration and the jerk jumps of Section II-C. Its fourth distributional derivative is therefore the atomic right-hand side of (3), with nonnegative weights and support on contacts. Conversely, summing (3) over each component cancels the multiplier contributions, fixing its barycenter to the prescribed Hermite cubic. Between atoms, trajectories are cubic and recovered by Hermite interpolation. At atoms, feasibility gives tangency and the stationarity equation gives the matching conditions. Reading off contact states and weights yields a root reconstructing the trajectory. ■

Sustained contact: If the relative position changes during sustained contact, the motion can be represented by finitely many cubic pieces. Approximating it with vanishing error requires growing contact counts. SCI approximates sustained contact by clusters of point contacts, checking separation between them to a prescribed tolerance.

Box bounds: With the finite multiplier representation, (2) gives piecewise-affine momenta wiw_i. We project wiw_i onto the acceleration box, then integrate once to obtain velocity and again to obtain position. Clipping an affine function produces constant and affine pieces, so the resulting trajectories remain piecewise polynomial.

Algorithm: SCI

In Section III, we reconstructed trajectories from a prescribed contact list. To find this list, we start from unconstrained trajectories and insert contacts where separation fails. We then solve the matching equations to adjust the contact times and states jointly and reconstruct the trajectories. If a contact has a negative weight, we remove it and solve again for the remaining contacts before checking separation.

In Algorithm 1, we use absolute tolerances ϵF\epsilon_F for matching and endpoint states, ϵm\epsilon_m for multiplier nonnegativity, and ϵR\epsilon_R for separation. We return unresolved if matching or insertion fails or the time budget is exhausted.

Algorithm 1 SCI without box bounds
Input: Endpoint states, TT, RR, ϵF\epsilon_F, ϵm\epsilon_m, ϵR\epsilon_R
Output: (c,s)(c,s), s∈{success,unresolved}s\in\{\text{success},\text{unresolved}\}
1: c←()c\leftarrow()
2: loop
3: &nbsp;&nbsp;&nbsp;&nbsp;loop
4: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;G←Groups⁡(c)\mathcal{G}\leftarrow\operatorname{Groups}(c)
5: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;(c,s)←Match⁡(c,G)(c,s)\leftarrow\operatorname{Match}(c,\mathcal{G})
6: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;if s=unresolveds=\text{unresolved} then return (c,s)(c,s)
7: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;if c=()c=() or min⁡mℓ≥−ϵm\min m_\ell\geq-\epsilon_m then break
8: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;ℓ←arg⁡min⁡jmj\ell\leftarrow\arg\min_j m_j
9: &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;c←(cj)j≠ℓc\leftarrow(c_j)_{j\ne\ell}
10: &nbsp;&nbsp;&nbsp;&nbsp;end loop
11: &nbsp;&nbsp;&nbsp;&nbsp;(p,q,t∗,ϱ)←Closest⁡(c)(p,q,t_*,\varrho)\leftarrow\operatorname{Closest}(c)
12: &nbsp;&nbsp;&nbsp;&nbsp;if ϱ≥R−ϵR\varrho\geq R-\epsilon_R then return (c,success)(c,\text{success})
13: &nbsp;&nbsp;&nbsp;&nbsp;(c,s)←Insert⁡(c,p,q,t∗)(c,s)\leftarrow\operatorname{Insert}(c,p,q,t_*)
14: &nbsp;&nbsp;&nbsp;&nbsp;if s=unresolveds=\text{unresolved} then return (c,s)(c,s)
15: end loop

Grouping: In Groups⁡(c)\operatorname{Groups}(c), we update only the contact components affected by insertion or removal. When a new contact links two groups, we combine their matching equations into one system; after removal, we check whether the affected group splits into smaller groups that we can solve independently. Matching: In MATCH⁡(c,G)\operatorname{MATCH}(c,\mathcal{G}), we first solve each group’s system F(c)=0F(c)=0 by Newton iteration [8], using the analytical block-tridiagonal Jacobian from Section III-B and banded linear solves. We use backtracking to shorten each Newton step until it decreases the residual without changing the chronological order of the contacts. If Newton iteration fails, we use the cubic Green function [16] to express the intermediate positions and velocities in terms of the contact times, normals, and weights (tℓ,nℓ,mℓ)ℓ=1m(t_\ell,n_\ell,m_\ell)_{\ell=1}^{m}. We then solve this smaller system with Powell’s hybrid method [19] and trust-region least squares. If these attempts repeatedly converge to roots with negative weights, we use shifted deflation [12] to rescale the residual near those roots and direct subsequent solves toward another root. We accept a match only when ∥F(c)∥∞≤εF\lVert F(c)\rVert_\infty\le\varepsilon_F and every endpoint position and velocity component has absolute error at most εF\varepsilon_F.

Detection: In CLOSEST⁡(c)\operatorname{CLOSEST}(c), we find the closest pair (p,q)(p,q) over the full horizon and return its separation ϱ\varrho and the time t∗t_\ast at which it occurs. We use Bernstein convex-hull bounds [11] to enclose trajectory segments over adaptively subdivided time intervals. We group these enclosures in bounding-volume hierarchies and compare whole groups at once, excluding distant pairs without checking each trajectory individually (dual-tree traversal [7]). For the remaining pairs, we find the minimum squared distance by evaluating it at segment endpoints and at times when its derivative vanishes.

Insertion: In INSERT⁡(c,p,q,t∗)\operatorname{INSERT}(c,p,q,t_\ast), we initialize the new contact’s state from the current trajectory at t∗t_\ast and set its weight to mℓ=0m_\ell=0. To handle symmetric and antipodal cases, we slightly shift t∗t_\ast when it coincides with an existing contact time. We process violations in order of increasing separation, adding at most one contact involving each component before solving the matching equations again.

Position, velocity, and acceleration limits: With box constraints, we minimize JJ under endpoint conditions, acceleration bounds throughout the motion, and position, velocity, and separation inequalities at selected times. For a fixed unit separation direction nℓn_\ell, we impose ⟨nℓ,rpℓqℓ(tℓ)⟩≥R\langle n_\ell,r_{p_\ell q_\ell}(t_\ell)\rangle\ge R, allowing the robots to move farther apart than RR. For velocity, we also impose interval constraints (xi(b)−xi(a))/(b−a)∈Bi(1)(x_i(b)-x_i(a))/(b-a)\in B_i^{(1)}, a<ba<b; the associated dual multipliers represent constant densities on [a,b][a,b]. These constraints bound interval averages; we check instantaneous velocity after each solve. By fixing these times, intervals, and directions during each update, we obtain a convex subproblem. We solve its dual in the multipliers associated with endpoint conditions and the selected inequalities, requiring nonnegative multipliers for the latter. After each update, we check endpoint accuracy, every box bound, and pairwise separation over the full horizon. We add constraints where violations remain, remove constraints with zero multipliers, and update the separation directions before solving again. We accept the trajectory only when all checks pass.

Numerical experiments

Unless stated otherwise, the experiments use planar fleets with R=0.5R=0.5 and zero initial and final velocities. We sample initial and final configurations independently in a disk of radius 5N/25\sqrt{N/2}, rejecting configurations with pairwise distances below 0.80.8. The horizon is T=10N/2T=10\sqrt{N/2}. The measurements were obtained on an Apple M3 Pro with 36 GB of memory. We use the current SCI implementation and SCP measurements on the same instances. The scaling and baseline comparisons use unrestricted boxes; Experiment 7 tests finite position, velocity, and acceleration limits.

The baseline transcribes the same fixed-horizon problem with prescribed terminal states, following Kamm et al. [17], Section III-CI, with piecewise-constant accelerations on K=200K=200 equal intervals and OSQP [22] for the quadratic subproblems. In these comparisons, both methods start from the unconstrained solution. Unless varied below, εR=10−3\varepsilon_R=10^{-3}, or 0.2%0.2\% of RR. For cubic contact matching, εF=10−7\varepsilon_F=10^{-7}, εm=10−9\varepsilon_m=10^{-9}, and the Newton iteration limit is 100100. The baseline uses at most 2020 outer iterations with position-change tolerance 10−210^{-2}; OSQP uses absolute tolerance 10−410^{-4}, zero relative tolerance, and at most 10410^4 iterations. The comparison studies use a 300 300\,s time limit.

Success requires solver convergence, satisfaction of the imposed box bounds, and pairwise separation of at least R−εRR-\varepsilon_R throughout [0,T][0,T] for SCI, and at the enforced grid points for SCP. The continuous-time separation success rate counts successful runs whose piecewise-polynomial trajectories also meet this bound throughout [0,T][0,T]. Both rates use all attempted instances, including failures and timeouts, in their denominator; these values coincide for SCI. In plots with shaded bands, lines show medians; darker and lighter bands show the interquartile and central 95% ranges across seeds.

Experiment 1 (A million robots). We compute transitions from a planar grid to a double helix for fleets of 500 to 1,000,000 robots (Figure 1 and Figure 2). The workspace grows with fleet size while the initial grid spacing remains 2 m. We use a horizon of 100 s and a separation tolerance of 1 cm. Results. For 1,000 robots, we compute trajectories with 118 contacts in 0.124 s. For 10610^6 robots, we obtain 151,071 contacts in 525.4 s: we plan continuous-time motion for a million robots in under nine minutes on a single workstation. Sparse interactions keep the largest final contact component to only 16 robots, allowing us to represent the million-robot solution through small coupled systems.

Figure 2. Grid-to-double-helix scaling (Experiment 1): contact counts, robots in contacts, and computation time versus fleet size, with one run per size.

Experiment 2 (Fleet size). We study the effect of fleet size on runtime and success rate, using 50 seeds for each N∈{2,4,…,20}N \in\{2,4,\ldots,20\}. SCI has a 100% success rate across 500 instances. At N=20N=20, its median runtime is 0.009 s (Figure 3, left), with medians of 6 final contacts and 65 matching unknowns across the contact components. At N=20N=20, the baseline has a 98% success rate and median runtime 106.6 s; its continuous-time separation success rate is 0%. Its median continuous-time separation deficit is 2.8% of RR. At N=20N=20, SCI computes trajectories 12289 times faster than SCP. The baseline’s high convergence rate on the fixed grid therefore substantially overstates continuous-time feasibility at this resolution.

Figure 3. Runtime and separation accuracy. Left: computation time for solver-converged runs over 50 seeds per fleet size (Experiment 2). Right: continuous-time separation success rates over 50 instances at N=16N=16 versus runtime budget (Experiment 3). Colors identify methods in both panels; line styles on the right identify separation targets relative to RR.

Experiment 3 (Separation accuracy and cost). For 50 instances at N=16N=16, we set εR∈{10−2,10−3,10−4}\varepsilon_R \in\{10^{-2},10^{-3},10^{-4}\}, corresponding to 2%, 0.2%, and 0.02% of RR. The baseline is run at K∈{100,200,400,800,1600}K \in\{100,200,400,800,1600\} with OSQP absolute tolerance εR/10\varepsilon_R/10. For each instance and target, the baseline curve uses the fastest successful run that meets the continuous-time separation target. Results. Across a hundredfold tightening of the separation tolerance, SCI retains a 100% success rate and a nearly unchanged median runtime of 0.006 s, with a median contact count of 4. Isolated contacts satisfy the same matching equations at every separation tolerance. By contrast, SCP requires finer time grids, and its continuous-time separation success rate falls from 96% to 40% and 14% at the three targets (Figure 3, right). At the 2% target, the median speedup on the 45 interacting instances where both methods meet continuous-time separation is 3296. At 0.2% and 0.02%, median speedups are 43122 (17 instances) and 83487 (4 instances). Against the coarsest successful baseline grid at each target, the maximum relative cost difference across 75 paired successes is 0.019%. The large runtime gains therefore accompany nearly identical acceleration costs on these paired feasible instances.

Experiment 4 (Congestion). At fixed N=10N=10 and T=10N/2T=10\sqrt{N/2}, we vary sws∈{0.12,0.15,0.175,0.4,0.55,0.75,1,1.25,1.5}s_{\mathrm{ws}} \in\{0.12,0.15,0.175,0.4,0.55,0.75,1,1.25,1.5\}, with 50 instances per scale, spanning sparse to extreme congestion. At sws=0.12s_{\mathrm{ws}}=0.12, the endpoint exclusion disks cover 52.7% of their enclosing disk, close to the bulk random sequential adsorption (RSA) jamming coverage of 54.7%. The nominal physical robot-area fraction is 34.7%. Accepted endpoint pairs at 0.12 are also scaled to 0.15 and 0.175. Results. Both methods solve all 150 instances at the three densest levels. At sws=0.12s_{\mathrm{ws}}=0.12, all 10 robots enter a single matching component in 15/50 instances (percolation). Median SCI runtime is 0.20 s, with a maximum of 0.65 s, compared with a median of 22.87 s for SCP. SCI therefore retains subsecond runtimes even when the fleet cannot be split into smaller independent contact components.

Experiment 5 (Algorithmic ablations). Using the same 50 seeds at N=10N=10 and sws∈{0.4,1,1.5}s_{\mathrm{ws}} \in\{0.4,1,1.5\}, we replace the analytical Jacobian with finite differences, or insert at the earliest violating local distance minimum instead of the worst violation. Results. At sws=0.4s_{\mathrm{ws}}=0.4, finite differences increase runtime by a median factor of 12.48 on jointly successful instances. Earliest insertion stagnates on 4 of the 50 dense instances; worst-violation insertion solves all 50. Analytical derivatives therefore improve speed, while worst-violation insertion avoids the observed stagnation.

Experiment 6 (Spatial dimension). We test d∈{2,3,5,10}d \in\{2,3,5,10\} and N∈{5,10}N \in\{5,10\} with T=10T=10. Endpoints are sampled in a dd-dimensional ball, whose radius is calibrated on 256 separate seeds to match the planar mean number of unconstrained interacting pairs. Each (d,N)(d,N) is evaluated on 50 further seeds. Results. SCI succeeds on all instances. At N=10N=10, its median runtime stays within 0.00254–0.0035 s across dimensions. Its speedup over SCP increases with dimension (d=2d=2: 2734×, d=3d=3: 4308×, d=5d=5: 27397×), using median runtimes. At d=10d=10, SCP times out on 37 out of 50 instances. The stable runtimes show that contact matching remains practical in higher dimensions when encounter frequency is controlled.

Experiment 7 (Box bounds). We solve (1) with SCI on 102 instances with N=3N=3–12 and feasible SCP reference trajectories. The suite contains 68 planar and 34 spatial cases: crossing chains, circle swaps, and corridors, with T=6.5T=6.5–14.9 s and R∈{0.30,0.42,0.50}R \in\{0.30,0.42,0.50\} m. In 101 cases, the componentwise limits are symmetric: (bˉ(1),bˉ(2))=(0.8,0.65)(\bar{b}^{(1)},\bar{b}^{(2)})=(0.8,0.65) in 74 cases, (0.65,0.4)(0.65,0.4) in 24, and (0.75,0.55)(0.75,0.55) in 3, in m/s and m/s2^2, respectively. One case uses asymmetric robot- and axis-dependent bounds. The 23 corridor cases also impose ∣x1,2(t)∣≤0.04|x_{1,2}(t)| \le0.04 m; all other position coordinates are unrestricted. Results. All 102 bounded instances pass continuous feasibility checks at tolerance 2×10−62 \times10^{-6}. At this tolerance, position, velocity, and acceleration limits are active in 23, 85, and 58 cases, respectively. The median runtime across the bounded instances is 0.20 s.

Conclusion

By optimizing sparse contact components, SCI makes million-agent planning computationally feasible while retaining continuous-time separation checks. An extension with fixed virtual robots could encode static obstacles as unions of balls, enforcing separation only for pairs involving a physical robot. This would let large agent counts represent environmental detail as well as physical fleets.

References

  1. [1]Federico Augugliaro, Angela P. Schoellig, and Raffaello D’Andrea. Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach. In 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 1917–1922, IEEE, 2012.DOI
  2. [2]Heinz H. Bauschke, Regina S. Burachik, and C. Yalçın Kaya. Constraint splitting and projection methods for optimal control of double integrator. In Heinz H. Bauschke, Regina S. Burachik, and D. Russell Luke, editors, Splitting Algorithms, Modern Operator Theory, and Applications, pages 45–68. Springer, Cham, 2019.arxiv.org/abs/1804.03767
  3. [3]Logan E. Beaver and Andreas A. Malikopoulos. An energy-optimal framework for assignment and trajectory generation in teams of autonomous agents. Syst. Control Lett., 138:104670, 2020.DOI
  4. [4]Logan E. Beaver, Michael Dorothy, Christopher Kroninger, and Andreas A. Malikopoulos. Energy-optimal motion planning for agents: Barycentric motion and collision avoidance constraints. In 2021 American Control Conference (ACC), pages 1040–1045. IEEE, 2021.arxiv.org/abs/2009.00588
  5. [5]Logan E. Beaver, Roberto Tron, and Christos G. Cassandras. A graph-based approach to generate energy-optimal robot trajectories in polygonal environments. IFAC-PapersOnLine, 56(2):7000–7005, 2023.arxiv.org/abs/2211.05251
  6. [6]J. Frédéric Bonnans and Audrey Hermant. Well-posedness of the shooting algorithm for state constrained optimal control problems with a single constraint and control. SIAM J. Control Optim., 46(4):1398–1430, 2007.DOI
  7. [7]Ryan R. Curtin, William B. March, Parkishit Ram, David V. Anderson, Alexander G. Gray, and Charles L. Isbell, Jr. Tree-independent dual-tree algorithms. In Proceedings of the 30th International Conference on Machine Learning, volume 28(3) of Proceedings of Machine Learning Research, pages 1435–1443. PMLR, 2013.arxiv.org/abs/1304.4327
  8. [8]John E. Dennis, Jr. and Robert B. Schnabel. Numerical Methods for Unconstrained Optimization and Nonlinear Equations, volume 16 of Classics in Applied Mathematics. Society for Industrial and Applied Mathematics, Philadelphia, PA, 1996.
  9. [9]Magnus Egerstedt and Clyde Martin. Control Theoretic Splines: Optimal Control, Statistics, and Path Planning. Princeton Series in Applied Mathematics. Princeton University Press, Princeton, NJ, 2010. ISBN 978-0-691-13296-9.
  10. [10]Purnanand Elango, Dayou Luo, Abhinav G. Kamath, Samet Uzun, Taewan Kim, and Behçet Açıkmeşe. Continuous-time successive convexification for constrained trajectory optimization. Automatica, 180, 2025.DOI
  11. [11]Rida T. Farouki. The Bernstein polynomial basis: A centennial retrospective. Comput. Aided Geom. Des., 29 (6):379–419, 2012.DOI
  12. [12]Patrick E. Farrell, Ásgeir Birkisson, and Simon W. Funke. Deflation techniques for finding distinct solutions of nonlinear partial differential equations. SIAM J. Sci. Comput., 37(4):A2026–A2045, 2015.arxiv.org/abs/1410.5620
  13. [13]Richard F. Hartl, Suresh P. Sethi, and Raymond G. Vickson. A survey of the maximum principles for optimal control problems with state constraints. SIAM Rev., 37(2):181–218, 1995.DOI
  14. [14]Kris Hauser. Semi-infinite programming for trajectory optimization with non-convex obstacles. Int. J. Robot. Res., 40(10–11):1106–1122, 2021.DOI
  15. [15]Wolfgang Hönig, James A. Preiss, T. K. Satish Kumar, Gaurav S. Sukhatme, and Nora Ayanian. Trajectory planning for quadrotor swarms. IEEE Trans. Robot., 34 (4):856–869, 2018.DOI
  16. [16]Jan Kamm, Antonio Terpin, and Raffaello D’Andrea. A complete analytical characterization of optimal collision-free motion for two robots. arXiv:2610.41558, 2026.arxiv.org/abs/2610.41558
  17. [17]Jan Kamm, Antonio Terpin, Raffaello D’Andrea, and Aswin Ramachandran. Motion planning for hundreds of floating robots. In 2026 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2026. Accepted for publication; arXiv:2606.09620.arxiv.org/abs/2606.09620
  18. [18]Carlos E. Luis and Angela P. Schoellig. Trajectory generation for multiagent point-to-point transitions via distributed model predictive control. IEEE Robot. Autom. Lett., 4(2):375–382, 2019.DOI
  19. [19]Michael J. D. Powell. A hybrid method for nonlinear equations. In Philip Rabinowitz, editor, Numerical Methods for Nonlinear Algebraic Equations, pages 87–114. Gordon and Breach, London, 1970.
  20. [20]John Schulman, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow, and Pieter Abbeel. Finding locally optimal, collision-free trajectories with sequential convex optimization. In Proceedings of Robotics: Science and Systems IX, Berlin, Germany, 2013.DOI
  21. [21]Rahul Shome, Kiril Solovey, Andrew Dobson, Dan Halperin, and Kostas E. Bekris. dRRT*: Scalable and informed asymptotically-optimal multi-robot motion planning. Auton. Robots, 44(3–4):443–467, 2020.DOI
  22. [22]Bartolomeo Stellato, Goran Banjac, Paul Goulart, Alberto Bemporad, and Stephen Boyd. OSQP: An operator splitting solver for quadratic programs. Math. Program. Comput., 12(4):637–672, 2020.
  23. [23]Wannes Van Loock, Goele Pipeleers, and Jan Swevers. B-spline parameterized optimal motion trajectories for robotic systems with guaranteed constraint satisfaction. Mech. Sci., 6(2):163–171, 2015.DOI
  24. [24]Peter R. Wurman, Raffaello D’Andrea, and Mick Mountz. Coordinating hundreds of cooperative, autonomous vehicles in warehouses. AI Mag., 29(1):9–19, 2008.
  25. [25]Duo Zhang, Chen Liang, Xifeng Gao, Kui Wu, and Zherong Pan. Provably feasible semi-infinite program under collision constraints via subdivision. IEEE Trans. Robot., 40:2602–2619, 2024.DOI

Paper details

vlnc:W_hmv36f6a
Version
v1 · latest
Published
Fresh — added to the Hub within the last 30 days
Added
License
CC BY 4.0
Usage

views · 0

downloads · 0

A view counts once per browser per day, a download each time the PDF is requested.

  • Computer Science

Not recorded for this paper

View statement
Looking for related papers…
Contents