Multi-Agent Motion Planning for Simultaneous Arrival using Time-Reversed Search and Distributed Optimal Control

Anja Hellander, Daniel Axehill · 2026

Read Multi-Agent Motion Planning for Simultaneous Arrival using Time-Reversed Search and Distributed Optimal Control by Anja Hellander, Daniel Axehill (2026) in the Tower of Babel, a free online library of public-domain works preserved in clean, searchable text. No sign-up. The full text is served as a plain-text file at a predictable URL, where text is present: https://antarctic-labs.com/tower-of-babel/arxiv-2605-02019.txt.

Introduction

Anja Hellander and Daniel Axehill plan motion for many agents that must reach their goals at the same time. They treat the special case where all agents start at rest. Their two-step method first finds a feasible plan by running a leading search method backward, using conflict-based search and safe interval planning with interval projection.

That search is complete, and the authors give conditions for when it is also optimal. A receding-horizon optimal control step then improves the plan, warm-started from the first step. To scale, they solve that control step in a distributed way with a nonlinear multiplier method. Tests on a car-like vehicle illustrate the framework and its behaviour.

Catalog record

CREATORAnja Hellander, Daniel Axehill
YEAR2026
COLLECTIONPAPERS
FORMATTXT
FILE SIZE41 KB
LICENSEhttp://creativecommons.org/licenses/by/4.0/
RIGHTSCC BY
SOURCEarXiv

Source: https://arxiv.org/abs/2605.02019

Browse the full library · About the Tower of Babel

Full text

The opening of the work, from the library's hosted text:

Multi-Agent Motion Planning for Simultaneous Arrival using Time-Reversed Search and Distributed Optimal Control Anja Hellander, Daniel Axehill Abstract In this work we consider the multi-agent motion planning (MAMP) problem with the constraint that agents arrive at their respective goals at the same time. For the special case where all agents are initially at rest we propose a two-step method for finding optimized and kinematically feasible solutions. The first step finds an initial feasible solution by applying a state-of-the-art MAMP algorithm (conflict-based search and safe interval path planning with interval projection) backward. The algorithm is complete, and we provide necessary conditions for when it is also optimal. The second step is an improvement step where a receding-horizon optimal control problem (OCP) is posed and the solution found in the first step is used to warm-start the solver. To improve scalability we propose to solve the OCP in a distributed manner using the nonlinear alternating direction method of multipliers (NADMM). We evaluate the proposed framework in numerical experiments on a car-like vehicle. The results show that the backward planning algorithm successfully finds feasible and collision-free solutions, and that the improvement step further improves the quality of the solutions. Compared to solving the OCPs in a centralized manner, using nonlinear ADMM reduces the computation time. I INTRODUCTION The problem of finding feasible and collision-free trajectories for autonomous agents has been well-studied. In this paper we consider the problem of finding locally optimal feasible and collision-free trajectories for multiple agents under the constraint that all agents reach their goal simultaneously. I-A Related work The multi-agent pathfinding problem, where collision-free paths for multiple agents are computed, has been well-studied. Current state-of-the-art methods include the complete and optimal Conflict-Based Search (CBS) [ 20 , 16 , 6 ] and the incomplete and suboptimal Priority-Based Search (PBS) [ 17 ] . Both these methods rely on repeatedly solving single-agent pathfinding problems using, e.g., A* search [ 11 ] or safe interval path planning (SIPP) [ 21 ] . More recently there has been increased interest in multi-agent motion planning (MAMP), an extension of MAPF that considers kinodynamic constraints on the agents. Examples of such work are [ 27 , 14 , 28 , 18 , 23 ] . In [ 1 ] an extension of SIPP called safe interval path planning with safe intervals (SIPP-IP) is presented that considers that real-life agents cannot instantaneously transition between maximum velocity and standstill. For some applications it may be desirable that all agents reach their goal simultaneously. This has been considered in [ 13 , 22 , 30 ] . Most work considers fixed-wing UAVs and model the agents as Dubins car. A common approach is then to compute Dubins paths and adjust the arrival time by modifying the velocity and/or the minimum turning radius [ 31 , 9 ] . Similarly, [ 19 ] uses Dubins paths with clothoid arcs. The problem with many of these approaches is that they first compute collision-free paths and then modify the velocity/turning radius without ascertaining that the resulting trajectories are still collision-free. This is, however, done in [ 3 ] which uses a single-agent planner similar to probabilistic roadmaps and performs resampling of the roadmap if it fails to construct collision-free trajectories. As solving motion-planning problems to (global) optimality is generally intractable for non-holonomic agents in the presence of obstacles many approaches rely on first computing a candidate solution and then improving it. For single-agent planning this is done in, e.g., [ 5 ] . Our previous work [ 12 ] extended the framework in [ 4 ] to multiple agents, but as the resulting optimization problem is solved centrally the computational effort and time required scales poorly with the number of agents. Trajectory optimization for multiple agents can be performed in a distributed manner. For a more complete overview on distributed optimization and its applications to multi-robot systems, see [ 29 , 10 ] . In [ 8 ] and [ 25 ] , the nonlinear alternating direction method of multipliers (NADMM) [ 24 ] is used for distributed trajectory optimization. Like in this work, this is done in a receding-horizon fashion. I-B Contributions • We show that multi-agent motion planning problems under simultaneous arrival constraints where agents initially have zero velocity can be solved using standard MAMP algorithms by applying backwards search. Further, we proved sufficient conditions for when using an optimal MAMP algorithm for the backwards search guarantees that the solution is optimal with respect to the initial simultaneous arrival MAMP problem, • We propose a framework for multi-agent motion planning with simultaneous arrival consisting of two steps: a computation step where an initial feasible solution is computed followed by an improvement step where the initial solution is improved by posing and solving an optimal control problem (OCP) in a receding-horizon manner. • We show that NADMM can be applied to solve the OCP in the improvement step in a distributed manner. We show using numerical examples that the computation time scales better with the number of agents than if the OCP is solved in a centralized manner. II Preliminaries This section presents the MAMP problem with simultaneous arrival constraints as well as a state-of-the-art algorithm for MAMP. II-A Problem formulation We consider K agents operating within a workspace W\subset\mathcal{R}^{2} . The (nonlinear) dynamics of each agent i are described by \dot{x}_{i}(t)=f_{i}(x_{i}(t),u_{i}(t)),\quad x(t_{0})=x_{s,i} (1) where x_{i}\in\mathcal{X}_{i}\subset\mathcal{R}^{n_{i}} is the state vector, u_{i}\in\mathcal{U}_{i}\subset\mathcal{R}^{m_{i}} is the control input, t\geq 0 is the time elapsed, and x_{s,i} is the initial state of the agent. Further, we denote the area occupied by agent i while it is in state x_{i} by R_{i}(x_{i})\subset\mathcal{R}^{2} and the state space of each agent by \mathcal{X}_{i}=\{x_{i}\in\mathcal{R}^{n_{i}}|R_{i}(x_{i})\subset W\} . The workspace also contains an obstacle region O\in W that the agents may not collide with. The obstacle space for each agent is then \mathcal{X}_{i,\text{obs}}=\{x_{i}\in\mathcal{X}_{i}|R_{i}(x)\cap O\neq\emptyset\} . Agents must therefore satisfy x_{i}(t)\in\mathcal{X}_{i}\setminus\mathcal{X}_{i,\text{obs}}=\mathcal{X}_{i,\text{free}}, (2) where \mathcal{X}_{i,\text{free}} is the free space. Additionally, agents may not collide with each other and must therefore satisfy R_{i}(x_{i}(t))\cap R_{j}(x_{j}(t))=\emptyset\quad i\neq j. (3) Given the initial states x_{s,i} and final states x_{f,i} for each agent, the multi-agent motion-planning problem consists of finding a set of feasible and collision-free trajectories (x_{i}(t),u_{i}(t),t_{f,i}) such that x_{i}(t_{0})=x_{s,i} and x_{i}(t_{f,i})=x_{f,i} for all agents i . The MAMP problem with simultaneous arrival requires that t_{f,1}=\dots=t_{f,K}=t_{f} . The optimal MAMP problem with simultaneous arrival for k agents can then be posed as \displaystyle\minimize_{\{x_{i}(t),u_{i}(t)\}_{i=1}^{K},t_{f}}\quad \displaystyle J=\sum_{i=1}^{k}J_{i}(x_{i},u_{i},t_{f}) (4a) \displaystyle\text{s.t.}\quad\dot{x}_{i}(t) \displaystyle=f_{i}(x_{i}(t),u_{i}(t)) (4b) \displaystyle x_{i}(t) \displaystyle\in\mathcal{X}_{i,\text{free}}\quad i=1,\dots,K (4c) \displaystyle u_{i}(t) \displaystyle\in\mathcal{U}_{i} (4d) \displaystyle R_{i}(x_{i}(t))\cap R_{j}(x_{j}(t)) \displaystyle=\emptyset\quad i\neq j (4e) \displaystyle x_{i}(t_{0}) \displaystyle=x_{s,i} (4f) \displaystyle x_{i}(t_{f}) \displaystyle=x_{f,i} (4g) where the cost functions J_{i} are defined as J_{i}=\int_{t_{0}}^{t_{f}}l(x_{i}(t),u_{i}(t))\,dt (5) where l(x(t),u(t))\geq 0 is the user-defined running cost. For example, selecting l(x,u)=1 results in a minimum-time problem. II-B Solving the MAMP problem A popular MAMP algorithm is CBS. The idea behind CBS is to plan for each agent separately using a single-agent motion planner and to create a search tree where each node corresponds to a set of constraints on the trajectories. When conflicts (collisions) between agents occur new nodes are created with additional constraints to solve them. A general outline of this is shown in Algorithm 1 . First, the root

Read the complete text