betaFeedback

WorksComputer Science

A complete analytical characterization of optimal collision-free motion for two robots

Abstract

Collision-free motion planning for multi-agent robotic systems remains an active research area because of the complexity of coordinating robot trajectories, with applications ranging from coordinated drone shows to autonomous operations on land, in the air, and on water. Importantly, the analytical difficulty of nonconvex collision-avoidance constraints has so far driven the development of exclusively numerical methods. We derive a complete analytical characterization of the optimal trajectories for two robots with zero initial and final velocities, under double-integrator dynamics and a squared-acceleration objective. We prove that these trajectories belong to eight distinct contact families and determine which family applies for each configuration of initial and final positions. We analyze the geometrical properties of the optimal trajectories and provide a numerical algorithm for computing them. These insights have already been adopted to improve the performance of numerical methods for multi-agent motion planning by several orders of magnitude.

Introduction

Collision-free motion planning for multi-agent robotic systems remains an active research area because of the complexity of coordinating robot trajectories, with applications ranging from coordinated drone shows to autonomous operations on land [40], in the air [2], and on water [21]. A variety of numerical methods exist to solve this problem, from sampling and graph search [22, 31, 30, 38] to joint assignment and planning [35] and hybrid search and trajectory optimization [18]. Trajectory optimizers use mixed-integer formulations [13], spline parameterizations [36, 39], sequential convex programming (SCP) [2, 29, 7, 15], learning-based methods [9, 17], distributed optimization [37, 32], and separation principles [33, 27].

Jan Kamm and Antonio Terpin contributed equally to this work.

Corresponding author: Jan Kamm ([email protected]).

ORCID: Jan Kamm, 0009-0005-6289-4198; Antonio Terpin, 0000-0002-8456-6786; Raffaello D’Andrea, 0000-0001-5287-7849

We consider collision-free motion planning under double-integrator dynamics, with fixed final time and endpoint positions. The squared-acceleration objective measures control effort and is equivalent to squared-thrust minimization [2]. This objective is commonly used for multi-agent assignment and collision avoidance [5, 4], navigation among polygonal obstacles [6], and, alongside other cost terms, distributed quadrotor planning [25]. Various drone and floating-robot experiments have demonstrated that simplified planning models generate reference trajectories amenable to tracking by low-level controllers [2, 25, 21]. However, these approaches compute trajectories numerically because nonconvex collision-avoidance constraints make an analytical solution difficult. Even for two robots, existing theory provides no complete analytical characterization of minimum-effort transitions between prescribed endpoint states over a fixed time horizon under double-integrator dynamics.

Importantly, the two-robot case isolates the pairwise interaction that underlies fleet planning, and an analytical characterization would identify the fundamental properties of optimal collision-free motion, offering new possibilities for efficient planning. Analytical characterizations exist in other settings. Hu et al. [19] construct exact optimal motions for two planar agents through a geometric analysis of the separation constraint. They minimize squared velocity and leave endpoint velocities free. Kirkpatrick and Liu [23, 24] give a complete geometric characterization for two robots minimizing total path length, with paths consisting of straight segments and circular arcs. For the squared-acceleration objective, Beaver and Malikopoulos [5] construct trajectories by successively inserting constrained arcs and solving for their joining times. Their method optimizes each agent against prescribed higher-priority trajectories and restricts sustained contact to constant relative speed. A complete characterization of the jointly optimal two-robot motion requires accounting for variable relative speed and determining which combinations of isolated contacts and sustained contact are globally optimal.

Contributions. In this paper, we provide a complete analytical characterization for the optimal trajectories of two robots with zero initial and final velocities under double-integrator dynamics and a squared-acceleration objective. We prove that the contact set consists of finitely many isolated points and boundary arcs, with cubic motion between contacts, and we derive an exact cost identity that gives a sufficient condition for global optimality (§ 2.2.1 and § 2.2.2). Then, we prove that every optimal relative trajectory lies in a plane, classify the minimizers into eight contact families, and show that their admissible construction images partition the endpoint space (§ 2.2.3). We give a certified numerical procedure that recovers the selected trajectory and its cost to any prescribed positive accuracy for algebraic endpoint data (§ 2.2.4). Our two-robot characterization underlies the contact-based fleet planner in [20], which incorporates acceleration constraints and computes collision-free trajectories for one million robots in minutes and for thousands in a fraction of a second.

Results

We consider two robots with zero initial and final velocities, strict initial separation and terminal contact allowed. Their relative position r(t)r(t) must remain outside the interior of an exclusion ball whose radius RR is the required separation. We call an encounter at the minimum separation a contact. The relative trajectory may touch its boundary at isolated points or follow it along an arc, representing sustained contact (Figure 1(d–g)). Every global optimum belongs to one of eight contact families (Figure 1(a–c)). When the endpoint segment avoids the disk’s interior, the free cubic is optimal (F). Otherwise, the relative trajectory joins distinct cubic pieces at one, two, or three isolated contacts (P, PP, PPP), or follows a single boundary arc (A), possibly with one isolated contact before (PA), after (AP), or on each side (PAP). Figure 1(a–c) illustrates numerically how the optimal contact pattern depends on the robots’ initial and final relative positions. Every globally minimizing relative trajectory lies in a plane (Proposition 2.6). In that plane, the initial and final relative positions are (d0,0)(d_0,0) and (rT,1,rT,2)(r_{T,1},r_{T,2}), respectively, in units of RR. In the following we present the key mathematical results that make this characterization precise.

Figure 1. Two-robot contact structure in relative coordinates. (a–c) Numerical contact-family map for R=T=1R = T = 1: transition surfaces and slices at d0=1.15d_0 = 1.15 and d0=1.343d_0 = 1.343; the inset magnifies the narrow PPP region. The cylinder excludes ∥rT∥<1\lVert r_T \rVert< 1. (d–g) Representative trajectories with R=1R = 1. Blue denotes free motion; red dots and segments mark isolated contacts and boundary arcs. Filled and open dots mark initial and final positions.

Problem formulation

Fix a time horizon T>0T>0. For integers d≥1d\geq1 and k≥0k\geq0, L2((0,T);Rd)L^2((0,T);\mathbb{R}^d) is the space of square-integrable functions, and Hk((0,T);Rd)H^k((0,T);\mathbb{R}^d) consists of functions whose weak derivatives through order kk are square-integrable [1 Chapters 2–3]. We write Ck([0,T];Rd)C^k([0,T];\mathbb{R}^d) for functions with kk continuous derivatives. For z,w∈Rdz,w\in\mathbb{R}^d and f∈L2((0,T);Rd)f\in L^2((0,T);\mathbb{R}^d), set ⟨z,w⟩:=∑ℓ=1dzℓwℓ\langle z,w\rangle:=\sum_{\ell=1}^{d}z_\ell w_\ell, ∥z∥:=⟨z,z⟩1/2\lVert z\rVert:=\langle z,z\rangle^{1/2}, and ∥f∥L2((0,T);Rd):=(∫0T∥f(t)∥2 dt)1/2\lVert f\rVert_{L^2((0,T);\mathbb{R}^d)}:=\left(\int_0^T\lVert f(t)\rVert^2\,\mathrm{d}t\right)^{1/2}. We omit the norm subscript when the argument makes its meaning unambiguous. We write ∣A∣\lvert A\rvert for the cardinality of a set AA and ∣a∣\lvert a\rvert for the absolute value of a scalar aa. The vectors e1,…,ede_1,\ldots,e_d denote the orthonormal coordinate vectors of the chosen spatial frame. Square brackets enclose function or trajectory arguments; parentheses indicate evaluation at a time or a finite-dimensional argument. Write DtkD_t^k for the kkth distributional time derivative, including Dt1D_t^1 for the first. Ordinary time derivatives are denoted by r˙\dot r, r¨\ddot r, r(3)r^{(3)}, and r(4)r^{(4)}, with the same convention for other trajectories. For Sobolev trajectories, these symbols denote the corresponding weak derivatives. Dots on normalized trajectories refer to their displayed time variable. Time differentiation acts componentwise on vector-valued functions. We write M+((0,T])\mathcal{M}_+((0,T]) for finite nonnegative Radon measures and δs\delta_s for the Dirac measure with unit mass at ss. The support supp⁡(μ)\operatorname{supp}(\mu) consists of points whose every neighborhood in [0,T][0,T] has positive μ\mu-measure. For a function with finite one-sided limits, write f(t−):=lim⁡s↑tf(s)f(t^-):=\lim_{s\uparrow t}f(s), f(t+):=lim⁡s↓tf(s)f(t^+):=\lim_{s\downarrow t}f(s), and [ ⁣[f] ⁣]t:=f(t+)−f(t−)[\![f]\!]_t:=f(t^+)-f(t^-). The minus and plus signs indicate limits from the left and right, respectively; f(0+)f(0^+) and f(T−)f(T^-) are the endpoint limits from inside the time interval. A right-continuous representative takes the value f(t+)f(t^+) at each interior time. In measure integrals, unmarked limits include endpoints, while a lower limit a+a^+ excludes aa and an upper limit b−b^- excludes bb. For example, ∫0+tf(s) dμ(s)\int_{0^+}^{t}f(s)\,\mathrm{d}\mu(s) integrates over 0<s≤t0<s\leq t, whereas ∫0+T−f(s) dμ(s)\int_{0^+}^{T^-}f(s)\,\mathrm{d}\mu(s) integrates over 0<s<T0<s<T.

Two-robot model

Let r,b∈H2((0,T);Rd)r,b \in H^2((0,T); \mathbb{R}^d), d≥2d \ge2, denote the position of the first robot relative to the second and their barycenter, respectively. Both trajectories have prescribed endpoint positions r0,rTr_0,r_T and b0,bTb_0,b_T, and zero initial and final velocities. The endpoint data satisfy ∥r0∥>R\lVert r_0\rVert> R and ∥rT∥≥R\lVert r_T\rVert\ge R, where R>0R > 0 is the minimum separation. The cost is 2∥b¨∥2+12∥r¨∥22\lVert\ddot{b}\rVert^2 + \frac{1}{2}\lVert\ddot{r}\rVert^2. Since separation constrains only rr, the barycentric and relative trajectories minimize their costs independently. The unique optimal barycenter is the cubic Hermite interpolant [14 Section 2.5] bH(t)=b0+(bT−b0)t^2(3−2t^)b^H(t)=b_0+(b_T-b_0)\hat{t}^2(3-2\hat{t}), where t^=t/T\hat{t}=t/T. The relative trajectory solves

min⁡r12∥r¨∥2s.t. r(0)=r0, r(T)=rT, r˙(0)=r˙(T)=0, ∥r(t)∥≥R, t∈[0,T].(1)\min_r \frac{1}{2}\lVert\ddot{r}\rVert^2 \quad\text{s.t. } r(0)=r_0,\ r(T)=r_T,\ \dot{r}(0)=\dot{r}(T)=0,\ \lVert r(t)\rVert\ge R,\ t\in[0,T]. \tag*{(1)}

Let M∗\mathcal{M}^* denote its set of global minimizers.

Remark 2.1 (Nondimensionalization). For t^=t/T∈[0,1]\hat{t}=t/T\in[0,1], define r^(t^):=r(Tt^)/R\hat{r}(\hat{t}):=r(T\hat{t})/R. The inverse r(t)=Rr^(t/T)r(t)=R\hat{r}(t/T) identifies feasible trajectories with those of (1) at T=R=1T=R=1, with endpoint positions r0/Rr_0/R, rT/Rr_T/R and zero initial and final velocities. Moreover, ∥r¨∥L2((0,T);Rd)2=R2T−3∥r^¨∥L2((0,1);Rd)2\lVert\ddot{r}\rVert_{L^2((0,T);\mathbb{R}^d)}^2=R^2T^{-3}\lVert\ddot{\hat{r}}\rVert_{L^2((0,1);\mathbb{R}^d)}^2. Thus minimizers correspond bijectively. Hats denote normalization; we omit them when T=R=1T=R=1.

Remark 2.2 (Existence and nonuniqueness). The direct method [28 Theorem 2.3] guarantees a global minimizer of (1). This solution may not be unique. In fact, uniqueness fails for opposite endpoint vectors, since symmetry gives equally costly optimal trajectories passing on opposite sides of the separation ball.

Optimal trajectory characterization

To characterize the structure of the optimal trajectories, we first derive optimality conditions (Proposition 2.3) and construct a global certificate (§ 2.2.2). Using the optimality conditions, we characterize the active set, namely the times at which the robots attain the minimum separation, as finitely many isolated contacts and boundary arcs (Proposition 2.4). We then prove that every optimal relative trajectory lies in a plane and establish side selection to complete the catalogue of eight contact families and determine their regions in endpoint space (§ 2.2.3). We conclude with a numerical procedure to recover the selected optimal trajectory and its cost (§ 2.2.4).

Contact laws

Write A={t∈[0,T]:∥r(t)∥=R}\mathcal{A}=\{t\in[0,T]:\lVert r(t)\rVert=R\} for the active set and g(t)=R2−∥r(t)∥2g(t)=R^2-\lVert r(t)\rVert^2 for the clearance. The connected components of A\mathcal{A} are the contacts. An isolated point is an isolated contact; a contact of positive length is a boundary arc, necessarily a closed interval [a,b][a,b] with a<ba<b. A measure represents both isolated contacts and contact maintained over an interval.

References

  1. [1]R. A. Adams and J. J. F. Fournier, Sobolev Spaces, vol. 140 of Pure and Applied Mathematics, Academic Press, Amsterdam, 2nd ed., 2003.
  2. [2]F. Augugliaro, A. P. Schoellig, and R. 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), IEEE, 2012, pp. 1917–1922.DOI
  3. [3]S. Basu, Algorithms in real algebraic geometry: A survey, in Real Algebraic Geometry, K. Bekka, G. Fichou, J.-P. Monnier, and R. Quarez, eds., vol. 51 of Panoramas et Synthèses, Société Mathématique de France, Paris, 2017, pp. 107–153.
  4. [4]L. E. Beaver, M. Dorothy, C. Kroninger, and A. A. Malikopoulos, Energy-optimal motion planning for agents: Barycentric motion and collision avoidance constraints, in 2021 American Control Conference (ACC), IEEE, 2021, pp. 1040–1045.arxiv.org/abs/2009.00588
  5. [5]L. E. Beaver and A. A. Malikopoulos, An energy-optimal framework for assignment and trajectory generation in teams of autonomous agents, Syst. Control Lett., 138 (2020), 104670.DOI
  6. [6]L. E. Beaver, R. Tron, and C. G. Cassandras, A graph-based approach to generate energy-optimal robot trajectories in polygonal environments, IFAC-PapersOnLine, 56 (2023), pp. 7000–7005.arxiv.org/abs/2211.05251
  7. [7]R. Bonalli, A. Cauligi, A. Bylard, and M. Pavone, GuSTO: Guaranteed sequential trajectory optimization via sequential convex programming, in 2019 International Conference on Robotics and Automation (ICRA), IEEE, 2019, pp. 6741–6747.arxiv.org/abs/1903.00155
  8. [8]J. F. Bonnans and A. Hermant, Second-order analysis for optimal control problems with pure state constraints and mixed control-state constraints, Ann. Inst. H. Poincaré C Anal. Non Linéaire, 26 (2009), pp. 561–598.
  9. [9]Y. F. Chen, M. Liu, M. Everett, and J. P. How, Decentralized non-communicating multiagent collision avoidance with deep reinforcement learning, in 2017 IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2017, pp. 285–292.arxiv.org/abs/1609.07845
  10. [10]F. Colonius, A note on the existence of Lagrange multipliers, Appl. Math. Optim., 10 (1983), pp. 187–191.DOI
  11. [11]A. L. Dontchev and I. V. Kolmanovsky, State constraints in the linear regulator problem: Case study, J. Optim. Theory Appl., 87 (1995), pp. 323–347.DOI
  12. [12]R. G. Downey and A. G. Melnikov, Computably compact metric spaces, Bull. Symb. Log., 29 (2023), pp. 170–263.DOI
  13. [13]M. G. Earl and R. D’Andrea, Iterative MILP methods for vehicle-control problems, IEEE Trans. Robot., 21 (2005), pp. 1158–1167.
  14. [14]M. Egerstedt and C. Martin, Control Theoretic Splines: Optimal Control, Statistics, and Path Planning, Princeton Series in Applied Mathematics, Princeton University Press, Princeton, NJ, 2010.
  15. [15]P. Elango, D. Luo, A. G. Kamath, S. Uzun, T. Kim, and B. Açıkmeşe, Continuous-time successive convexification for constrained trajectory optimization, Automatica, 180 (2025).DOI
  16. [16]G. B. Folland, Real Analysis: Modern Techniques and Their Applications, Pure and Applied Mathematics, John Wiley & Sons, New York, 2nd ed., 1999.
  17. [17]P. D. Grontas, A. Terpin, E. Balta, R. D’Andrea, and J. Lygeros, Innet: Optimizing hard-constrained neural networks with orthogonal projection layers, in International Conference on Learning Representations (ICLR), 2026.
  18. [18]W. Hönig, J. A. Preiss, T. K. S. Kumar, G. S. Sukhatme, and N. Ayanian, Trajectory planning for quadrotor swarms, IEEE Trans. Robot., 34 (2018), pp. 856–869.DOI
  19. [19]J. Hu, M. Prandini, and S. Sastry, Optimal coordinated motions of multiple agents moving on a plane, SIAM J. Control Optim., 42 (2003), pp. 637–668.DOI
  20. [20]J. Kamm, A. Terpin, and R. D’Andrea, Contact-based optimization for collision-free motion planning for a million robots, 2026. arXiv:2610.41548.arxiv.org/abs/2610.41548
  21. [21]J. Kamm, A. Terpin, R. D’Andrea, and A. 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
  22. [22]S. Karaman and E. Frazzoli, Sampling-based algorithms for optimal motion planning, Int. J. Robot. Res., 30 (2011), pp. 846–894.arxiv.org/abs/1105.1186
  23. [23]D. Kirkpatrick and P. Liu, Characterizing minimum-length coordinated motions for two discs, in Proceedings of the 28th Canadian Conference on Computational Geometry (CCCG), 2016, pp. 252–259. Extended version: arXiv:1607.04005.arxiv.org/abs/1607.04005
  24. [24]D. Kirkpatrick and P. Liu, Minimum-length coordinated motions for two convex centrally-symmetric robots, 2025. arXiv:2503.02010.arxiv.org/abs/2503.02010
  25. [25]C. E. Luis and A. P. Schoellig, Trajectory generation for multiagent point-to-point transitions via distributed model predictive control, IEEE Robot. Autom. Lett., 4 (2019), pp. 375–382.DOI
  26. [26]R. Ma, D. Yan, and L. Wei, Multiplicity of nodal solutions for fourth order equation with clamped beam boundary conditions, Electron. J. Qual. Theory Differ. Equ., 2020 (2020), pp. 1–14.DOI
  27. [27]L. Pezzetti, N. Lanzetti, A. Terpin, F. Dörfler, and G. Ramponi, A separation principle for cooperative multi-agent reinforcement learning, in Nineteenth European Workshop on Reinforcement Learning, 2026.arxiv.org/abs/2611.09644
  28. [28]F. Rindler, Calculus of Variations, Universitext, Springer, Cham, 2018.
  29. [29]J. Schulman, J. Ho, A. Lee, I. Awwal, H. Bradlow, and P. Abbeel, Finding locally optimal, collision-free trajectories with sequential convex optimization, in Proceedings of Robotics: Science and Systems IX, Berlin, Germany, 2013.DOI
  30. [30]G. Sharon, R. Stern, A. Felner, and N. R. Sturtevant, Conflict-based search for optimal multi-agent pathfinding, Artif. Intell., 219 (2015), pp. 40–66.DOI
  31. [31]R. Shome, K. Solovey, A. Dobson, D. Halperin, and K. E. Bekris, dRRT*: Scalable and informed asymptotically-optimal multi-robot motion planning, Auton. Robots, 44 (2020), pp. 443–467.DOI
  32. [32]A. Terpin, S. Fricker, M. Perez, M. Hudoba de Badyn, and F. Dörfler, Distributed feedback optimisation for robotic coordination, in 2022 American Control Conference (ACC), IEEE, 2022, pp. 3710–3715.arxiv.org/abs/2109.14486
  33. [33]A. Terpin, N. Lanzetti, and F. Dörfler, Dynamic programming in probability spaces via optimal transport, SIAM J. Control Optim., 62 (2024), pp. 1183–1206.
  34. [34]W. F. Trench, Introduction to Real Analysis, Pearson Education, Upper Saddle River, NJ, 2003.
  35. [35]M. Turpin, N. Michael, and V. Kumar, CAPT: Concurrent assignment and planning of trajectories for multiple robots, Int. J. Robot. Res., 33 (2014), pp. 98–112.DOI
  36. [36]W. VAN LOOCK, G. PIPELEERS, AND J. SWEVERS, B-spline parameterized optimal motion trajectories for robotic systems with guaranteed constraint satisfaction, Mech. Sci., 6 (2015), pp. 163–171.DOI
  37. [37]R. VAN PARYS AND G. PIPELEERS, Distributed MPC for multi-vehicle systems moving in formation, Robot. Auton. Syst., 97 (2017), pp. 144–152.DOI
  38. [38]G. WAGNER AND H. CHOSET, Subdimensional expansion for multirobot path planning, Artif. Intell., 219 (2015), pp. 1–24.DOI
  39. [39]Z. WANG, X. ZHOU, C. XU, AND F. GAO, Geometrically constrained trajectory optimization for multicopters, IEEE Trans. Robot., 38 (2022), pp. 3259–3278.arxiv.org/abs/2103.00190
  40. [40]P. R. WURMAN, R. D’ANDREA, AND M. MOUNTZ, Coordinating hundreds of cooperative, autonomous vehicles in warehouses, AI Mag., 29 (2008), pp. 9–19.

Paper details

vlnc:W_u4qxnwy9
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