Optimized and kinematically feasible multi-agent motion planning

TL;DR

This paper introduces a two-step multi-agent motion planning framework combining initial feasible solutions with optimal control-based improvements, validated on tractor-trailer systems.

Contribution

It proposes a novel two-step approach integrating conflict-based search and optimal control, along with optimized motion primitive generation for multi-agent systems.

Findings

CBS outperforms PBS in success rate and runtime for tractor-trailer systems.

The framework achieves kinematically feasible and optimized solutions.

A simple lattice-based planner can outperform extended SIPP-IP in certain scenarios.

Abstract

Multi-agent motion planning (MAMP) is an important problem for autonomous systems with multiple agents. In this work we propose a two-step method for finding optimized and kinematically feasible solutions to MAMP problems. The first step finds an initial feasible solution using state-of-the-art methods such as conflict-based search (CBS) or priority-based search (PBS), and the second step is an improvement step which improves the solution by solving a multi-phase optimal control problem (OCP) where the initial solution is used to warm-start the solver. We also propose a method for generating motion primitives in an optimized way under the constraint that the primitive durations are all multiples of the same sample time. We evaluate our proposed framework on a MAMP problem for tractor-trailer systems. We extend the safe interval path planning with interval projections (SIPP-IP) algorithm so it can handle more general cost functions and larger agents, but our results show that for the tractor-trailer system a simple lattice-based planner performs better due to less conservative collision checks. Our experiments also indicate that CBS performs better than PBS for this system as it achieves a higher success rate in environments with obstacles and had a lower average runtime, although both planners achieve solutions of similar quality after the improvement step.