FrmRRT*: Sampling-based Multi-robot Formation Safe Path Planning in Obstacle-rich Environments

This paper presents a new sampling-based safe path planning method, FrmRRT*, for multi-robot systems to safely navigate confined and obstacle-rich environments. Specifically, the formation shape and the detected in-front navigable width are considered, and a new path representation of intersecting circles is proposed to ensure safer formation keeping and flexible transformation. A new scoring strategy is proposed to adjust the safe distance to avoid potential formation collisions. Upon detecting the traversable width, the currently best formation shape can be matched from the predefined shape library online, and then the respective path for each robot can be derived efficiently. Gazebo simulations and parameter sensitivity studies demonstrate that our FrmRRT* outperforms existing methods in safely and efficiently navigating formation through various obstacle-rich environments.