Interaction-Aware Multi-Robot Kinodynamic Motion Planning

Kinodynamic motion planning for a multi-robot system with different dynamics and actuation limits is a challenging problem. The difficulty increases with the presence of an aerodynamic interaction force that occur in aerial robots flying in close-proximity. Due to these complexities, existing planners either rely on simplified assumption like ignoring robot dynamics, interaction forces or produce highly suboptimal solutions. This paper presents a kinodynamic motion planner for a heterogeneous team of robots that respects robot dynamics and directly reasons about interaction forces between aerial robots operating in close-proximity. Our method, db-ECBS, generalizes the multi-agent path finding method Enhanced Conflict-Based Search (ECBS) to the continuous domain by using the single-robot kinodynamic motion planner discontinuity-bounded A*. Db-ECBS operates on three levels. Initially, individual robot trajectories are computed using a graph search that allows bounded discontinuities between precomputed motion primitives. The second level identifies inter-robot collisions, interaction force violations and resolves them by imposing constraints on the first level. The third and final level uses the resulting solution with discontinuities as an initial guess for a joint space trajectory optimization. The procedure is repeated with a reduced discontinuity bound resulting in a anytime, probabilistically complete, and asymptotically bounded suboptimal planner. We provide a benchmark of 65 problems with six different dynamics. We demonstrate that db-ECBS produces trajectories that are less than half the cost of existing planners. We show that the interaction-awareness is in particular important for very dense scenarios.