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

TL;DR

This paper introduces a two-step multi-agent motion planning framework that ensures simultaneous arrival, combining backward search algorithms with distributed optimal control to improve scalability and solution quality.

Contribution

It proposes a novel combination of backward search and distributed optimal control for multi-agent planning with simultaneous arrival constraints.

Findings

Backward planning algorithm finds feasible, collision-free solutions.

The improvement step enhances solution quality.

Distributed OCP reduces computation time compared to centralized methods.

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.