FASTFUNNELS: Agile Multi-Robot Safe Navigation in Narrow Spaces via Adaptive and Dynamic Funnels

FastFunnels supplementary video

Click to watch the FASTFUNNELS supplementary video

Supplementary Material (Appendix)
FASTFUNNELS (Ours) ORCA GCBF DMPC-FF DEFORM DEFORM (TB3)

Abstract

In this paper, we focus on the problem of high-speed multi-robot navigation in cluttered spaces with tight spatial constraints. A key technical challenge that the literature today faces is to balance safety with agility, often trading one for the other (slowing down near a bottleneck to avoid a collision versus failing to swerve in time at high speeds)especially in car-like dynamic systems. The approach presented in this paper aims to solve this problem and belongs to the family of formation control methods where Ackermann-steering agents coordinate with each other when navigating tight spaces. Formation control techniques today, however, rely on rigid, pre-defined shapes and adaptations, initial conditions, environmental conditions, and vehicular dynamics, that are typically impractical to maintain at high speeds. For e.g. agents going through a small gap can ONLY first form a square that eventually transforms into a line using analytical equations, and then back into a square. Our approach, on the other hand, draws inspiration from a smoothly flowing droplet of water that adapts its shape continuously and dynamically through whatever space its flowing without slowing down. We propose a hierarchical algorithm that, at the high level, trains a reinforcement learning policy to produce a fast, dynamic “blob-like” elliptical patch that can morph its shape in real-time and can squeeze, stretch, and orient itself fluidly around obstacles and through tight spaces. At the lower level, the agents or robots are optimized via distributed model predictive control (DMPC) to stay inside the patch (which acts as a soft forcing function for the agents) and not collide with each other. We evaluate our approach, FASTFUNNELS, using simulated F1TENTH racing cars and show that FASTFUNNELS surpasses the baselines and achieves a completion rate of 89% at 7.73 m/s across N =1 to N =6 robots in 14 different scenarios averaged over 20 trials each, while all baselines either fail to complete the track or succeed at extremely low speeds (0.07–1.79 m/s, 40% completion rate).