Kinodynamic motion planning for a multirobot system with different dynamics and actuation limits is a challenging problem. The difficulty increases with the presence of aerodynamic interaction forces that occur when aerial robots fly in close proximity. Due to these complexities, existing planners either rely on simplified assumptions (like ignoring robot dynamics and interaction forces) or produce highly suboptimal solutions. This article presents a kinodynamic motion planner for a heterogeneous team of robots that respects robot dynamics, scales well to 16 robots, and directly reasons about interaction forces between aerial robots operating in close proximity. Our method, db-ECBS, generalizes the multiagent path-finding method Enhanced Conflict-Based Search (ECBS) to the continuous domain by using the single-robot kinodynamic motion planner discontinuity-bounded A. The planner 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 interrobot collisions or 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 an 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 particularly important for very dense scenarios.

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 aerodynamic interaction forces that occur when aerial robots fly in close proximity. Due to these complexities, existing planners either rely on simplified assumptions (like ignoring robot dynamics and interaction forces) or produce highly suboptimal solutions. This paper presents a kinodynamic motion planner for a heterogeneous team of robots that respects robot dynamics, scales well to 16 robots, 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*. The planner 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 or 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 an 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 particularly important for very dense scenarios.

Motion planning for robotic systems with complex dynamics is a challenging problem. While recent sampling-based algorithms achieve asymptotic optimality by propagating random control inputs, their empirical convergence rate is often poor, especially in high-dimensional systems such as multirotors. An alternative approach is to first plan with a simplified geometric model and then use trajectory optimization to follow the reference path while accounting for the true dynamics. However, this approach may fail to produce a valid trajectory if the initial guess is not close to a dynamically feasible trajectory. In this article, we present Iterative Discontinuity Bounded A* (iDb-A*), a novel kinodynamic motion planner that combines search and optimization iteratively. The search step utilizes a finite set of short trajectories (motion primitives) that are interconnected while allowing for a bounded discontinuity between them. The optimization step locally repairs the discontinuities with trajectory optimization. By progressively reducing the allowed discontinuity and incorporating more motion primitives, our algorithm achieves asymptotic optimality with excellent any-time performance. We provide a benchmark of 43 problems across eight different dynamical systems, including different versions of unicycles and multirotors. Compared to state-of-the-art methods, iDb-A* consistently solves more problem instances and finds lower-cost solutions more rapidly.

The Model AI Assignments session seeks to gather and dis- seminate the best assignment designs of the Artificial In- telligence (AI) Education community. Recognizing that as- signments form the core of student learning experience, we here present abstracts of five AI assignments from the 2024 session that are easily adoptable, playfully engaging, and flexible for a variety of instructor needs. Assignment spec- ifications and supporting resources may be found at http://modelai.gettysburg.edu.

This paper presents a multi-robot kinodynamic motion planner that enables a team of robots with different dynamics, actuation limits, and shapes to reach their goals in challenging environments. We solve this problem by combining Conflict-Based Search (CBS), a multi-agent path finding method, and discontinuity-bounded A*, a single-robot kinodynamic motion planner. Our method, db-CBS, operates in three levels. Initially, we compute trajectories for individual robots using a graph search that allows bounded discontinuities between precomputed motion primitives. The second level identifies inter-robot collisions 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. Our approach is anytime, probabilistically complete, asymptotically optimal, and finds near-optimal solutions quickly. Experimental results with robot dynamics such as unicycle, double integrator, and car with trailer in different settings show that our method is capable of solving challenging tasks with a higher success rate and lower cost than the existing state-of-the-art.

Solving a Task-and-Motion Planning (TAMP) problem can be represented as a sequential (meta-) decision process, where early decisions concern the skeleton (sequence of logic actions) and later decisions concern what to compute for such skeletons (e.g., action parameters, bounds, RRT paths, or full optimal manipulation trajectories). We consider the general problem of how to schedule compute effort in such hierarchical solution processes. More specifically, we introduce infinite completion trees as a problem formalization, where before we can expand or evaluate a node, we have to solve a preemptible computational sub-problem of a priori unknown compute effort. Infinite branchings represent an infinite choice of random initializations of computational sub-problems. Decision making in such trees means to decide on where to invest compute or where to widen a branch. We propose a heuristic to balance branching width and compute depth using polynomial level sets. We show completeness of the resulting solver and that a round robin baseline strategy used previously for TAMP becomes a special case. Experiments confirm the robustness and efficiency of the method on problems including stochastic bandits and a suite of TAMP problems, and compare our approach to a round robin baseline. An appendix comparing the framework to bandit methods and proposing a corresponding tree policy version is found on the supplementary webpage1.

Rapidly-exploring Random Trees (RRT) and its variations have emerged as a robust and efficient tool for finding collision-free paths in robotic systems. However, adding dynamic constraints makes the motion planning problem significantly harder, as it requires solving two-value boundary problems (computationally expensive) or propagating random control inputs (uninformative). Alternatively, Iterative Discontinuity Bounded A* (iDb-A*), introduced in our previous study, combines search and optimization iteratively. The search step connects short trajectories (motion primitives) while allowing a bounded discontinuity between the motion primitives, which is later repaired in the trajectory optimization step.Building upon these foundations, in this paper, we present iDb-RRT, a sampling-based kinodynamic motion planning algorithm that combines motion primitives and trajectory optimization within the RRT framework. iDb-RRT is probabilistically complete and can be implemented in forward or bidirectional mode. We have tested our algorithm across a benchmark suite comprising 30 problems, spanning 8 different systems, and shown that iDb-RRT can find solutions up to 10x faster than previous methods, especially in complex scenarios that require long trajectories or involve navigating through narrow passages.

Mobile robots frequently navigate on roadmaps, i.e., graphs where edges represent safe motions, in applications such as healthcare, hospitality, and warehouse automation. Often the environment is quasi-static, i.e., it is sufficient to construct a roadmap once and then use it for any future planning queries. Roadmaps are typically used with graph search algorithm to find feasible paths for the robots. Therefore, the roadmap should be well-connected, and graph searches should produce near-optimal solutions with short solution paths while simultaneously be computationally efficient to execute queries quickly.We propose a new method to construct roadmaps based on the Gray-Scott reaction diffusion system and Delaunay triangulation. Our approach, GSRM, produces roadmaps with evenly distributed vertices and edges that are well-connected even in environments with challenging narrow passages. Empirically, we compare to classical roadmaps generated by 8-connected grids, probabilistic roadmaps (PRM, SPARS2), and optimized roadmap graphs (ORM). Our results show that GSRM consistently produces superior roadmaps that are well-connected, have high query efficiency, and result in short solution paths.

We propose a motion planner for cable-driven payload transportation using multiple unmanned aerial vehicles (UAVs) in an environment cluttered with obstacles. Our planner is kinodynamic, i.e., it considers the full dynamics model of the transporting system including actuation constraints. Due to the high dimensionality of the planning problem, we use a hierarchical approach where we first solve for the geometric motion using a sampling-based method with a novel sampler, followed by constrained trajectory optimization that considers the full dynamics of the system. Both planning stages consider inter-robot and robot/obstacle collisions. We demonstrate in a software-in-the-loop simulation and real flight experiments that there is a significant benefit in kinodynamic motion planning for such payload transport systems with respect to payload tracking error and energy consumption compared to the standard methods of planning for the payload alone. Notably, we observe a significantly higher success rate in scenarios where the team formation changes are needed to move through tight spaces.

We consider time-optimal motion planning for dynamical systems that are translation-invariant, a property that holds for many mobile robots, such as differential-drives, cars, airplanes, and multirotors. Our key insight is that we can extend graph-search algorithms to the continuous case when used symbiotically with optimization. For the graph search, we introduce discontinuity-bounded A* (db-A*), a generalization of the A* algorithm that uses concepts and data structures from sampling-based planners. Db-A* reuses short trajectories, so-called motion primitives, as edges and allows a maximum user-specified discontinuity at the vertices. These trajectories are locally repaired with trajectory optimization, which also provides new improved motion primitives. Our novel kinodynamic motion planner, kMP-db-A*, has almost surely asymptotic optimal behavior and computes near-optimal solutions quickly. For our empirical validation, we provide the first benchmark that compares search-, sampling-, and optimization- based time-optimal motion planning on multiple dynamical systems in different settings. Compared to the baselines, kMP- db-A* consistently solves more problem instances, finds lower- cost initial solutions, and converges more quickly.

Task and Motion Planning has made great progress in solving hard sequential manipulation problems. However, a gap between such planning formulations and control methods for reactive execution remains. In this paper we pro-pose a model predictive control approach dedicated to robustly execute a single sequence of constraints, which corresponds to a discrete decision sequence of a TAMP plan. We decompose the overall control problem into three sub-problems (solving for sequential waypoints, their timing, and a short receding horizon path) that each is a non-linear program solved online in each MPC cycle. The resulting control strategy can account for long-term interdependencies of constraints and reactively plan for a timing-optimal transition through all constraints. We additionally propose phase backtracking when running constraints of the current phase cannot be fulfilled, leading to a fluent re-initiation behavior that is robust to perturbations and interferences by an experimenter.

We presentNeural-Swarm2, a learning-based method for motion planning and control that allows heterogeneous multirotors in a swarm to safely fly in close proximity. Such operation for drones is challenging due to complex aerodynamic interaction forces, such as downwash generated by nearby drones and ground effect. Conventional planning and control methods neglect capturing these interaction forces, resulting in sparse swarm configuration during flight. Our approach combines a physics-based nominal dynamics model with learned deep neural networks with strong Lipschitz properties. We make use of two techniques to accurately predict the aerodynamic interactions between heterogeneous multirotors: 1) Spectral normalization for stability and generalization guarantees of unseen data and 2) heterogeneous deep sets for supporting any number of heterogeneous neighbors in a permutation-invariant manner without reducing expressiveness. The learned residual dynamics benefit both the proposed interaction-aware multirobot motion planning and the nonlinear tracking control design because the learned interaction forces reduce the modelling errors. Experimental results demonstrate thatNeural-Swarm2is able to generalize to larger swarms beyond training cases and significantly outperforms a baseline nonlinear tracking controller with up to three times reduction in worst-case tracking errors.

The Model AI Assignments session seeks to gather and disseminate the best assignment designs of the Artificial Intelligence (AI) Education community. Recognizing that assignments form the core of student learning experience, we here present abstracts of nine AI assignments from the 2020 session that are easily adoptable, playfully engaging, and flexible for a variety of instructor needs. Assignment specifications and supporting resources may be found at http://modelai.gettysburg.edu.

In this paper, we present Neural-Swarm, a nonlinear decentralized stable controller for close-proximity flight of multirotor swarms. Close-proximity control is challenging due to the complex aerodynamic interaction effects between multirotors, such as downwash from higher vehicles to lower ones. Conventional methods often fail to properly capture these interaction effects, resulting in controllers that must maintain large safety distances between vehicles, and thus are not capable of close-proximity flight. Our approach combines a nominal dynamics model with a regularized permutation-invariant Deep Neural Network (DNN) that accurately learns the high-order multi-vehicle interactions. We design a stable nonlinear tracking controller using the learned model. Experimental results demonstrate that the proposed controller significantly outperforms a baseline nonlinear tracking controller with up to four times smaller worst-case height tracking errors. We also empirically demonstrate the ability of our learned model to generalize to larger swarm sizes.

The Multi-Agent Pickup and Delivery (MAPD) problem models applications where a large number of agents attend to a stream of incoming pickup-and-delivery tasks. Token Passing (TP) is a recent MAPD algorithm that is efficient and effective. We make TP even more efficient and effective by using a novel combinatorial search algorithm, called Safe Interval Path Planning with Reservation Table (SIPPwRT), for single-agent path planning. SIPPwRT uses an advanced data structure that allows for fast updates and lookups of the current paths of all agents in an online setting. The resulting MAPD algorithm TP-SIPPwRT takes kinematic constraints of real robots into account directly during planning, computes continuous agent movements with given velocities that work on non-holonomic robots rather than discrete agent movements with uniform velocity, and is complete for wellformed MAPD instances. We demonstrate its benefits for automated warehouses using both an agent simulator and a standard robot simulator. For example, we demonstrate that it can compute paths for hundreds of agents and thousands of tasks in seconds and is more efficient and effective than existing MAPD algorithms that use a post-processing step to adapt their paths to continuous agent movements with given velocities.

Quadrotor stabilizing controllers often require careful, model-specific tuning for safe operation. We use reinforcement learning to train policies in simulation that transfer remarkably well to multiple different physical quadrotors. Our policies are low-level, i.e., we map the rotorcrafts' state directly to the motor outputs. The trained control policies are very robust to external disturbances and can withstand harsh initial conditions such as throws. We show how different training methodologies (change of the cost function, modeling of noise, use of domain randomization) might affect flight performance. To the best of our knowledge, this is the first work that demonstrates that a simple neural network can learn a robust stabilizing low-level quadrotor controller (without the use of a stabilizing PD controller) that is shown to generalize to multiple quadrotors. The video of our experiments can be found at https://sites.google.com/view/sim-to-multi-quad.

The Multi-Agent Pickup and Delivery (MAPD) problem models applications where a large number of agents attend to a stream of incoming pickup-and-delivery tasks. Token Passing (TP) is a recent MAPD algorithm that is efficient and effective. We make TP even more efficient and effective by using a novel combinatorial search algorithm, called Safe Interval Path Planning with Reservation Table (SIPPwRT), for single-agent path planning. SIPPwRT uses an advanced data structure that allows for fast updates and lookups of the current paths of all agents in an online setting. The resulting MAPD algorithm TP-SIPPwRT takes kinematic constraints of real robots into account directly during planning, computes continuous agent movements with given velocities that work on non-holonomic robots rather than discrete agent movements with uniform velocity, and is complete for well-formed MAPD instances. We demonstrate its benefits for automated warehouses using both an agent simulator and a standard robot simulator. For example, we demonstrate that it can compute paths for hundreds of agents and thousands of tasks in seconds and is more efficient and effective than existing MAPD algorithms that use a post-processing step to adapt their paths to continuous agent movements with given velocities. This paper was published at AAAI 2019.

We describe a trajectory planning method for heterogeneous mobile robot teams in known environments. We consider two core problems that arise with heterogeneous robot teams: asymmetric inter-robot collision constraints and varying dynamic limits. Asymmetric collision constraints are important for close-proximity flight of rotorcraft due to the downwash effect, which complicates spatial coordination. Varying dynamic limits complicate temporal coordination between robots and must be taken into account during planning. Our method builds upon a hybrid planner that combines graph-planning techniques with trajectory optimization and scales well to large homogeneous robot teams. We extend the hybrid planning approach to include the additional spatial and temporal coordination to support heterogeneous teams. Our method scales well with the number of robots and robot types and we demonstrate our approach on a team of 15 physical robots of 4 different types, including quadrotors and differential drive robots.

Collaboration between two or more geographically dispersed teams has applications in research and training. In many cases specialized devices, such as robots, may need to be combined between the collaborating groups. However, it would be expensive or even impossible to collocate them at a single physical location. We describe the design of a mixed reality test bed which allows dispersed humans and physically embodied agents to collaborate within a single virtual environment. We demonstrate our approach using Unity's networking architecture as well as open source robot software and hardware. In our scenario, a total of 3 humans and 6 drones must move through a narrow doorway while avoiding collisions in the physical spaces as well as virtual space.

We describe a method for multirobot trajectory planning in known, obstacle-rich environments. We demonstrate our approach on a quadrotor swarm navigating in a warehouse setting. Our method consists of following three stages: 1) roadmap generation that generates sparse roadmaps annotated with possible interrobot collisions; 2) discrete planning that finds valid execution schedules in discrete time and space; 3) continuous refinement that creates smooth trajectories. We account for the downwash effect of quadrotors, allowing safe flight in dense formations. We demonstrate computational efficiency in simulation with up to 200 robots and physical plausibility with an experiment on 32 nano-quadrotors. Our approach can compute safe and smooth trajectories for hundreds of quadrotors in dense environments with obstacles in a few minutes.

We define a system architecture for a large swarm of miniature quadcopters flying in dense formation indoors. The large number of small vehicles motivates novel design choices for state estimation and communication. For state estimation, we develop a method to reliably track many small rigid bodies with identical motion-capture marker arrangements. Our communication infrastructure uses compressed one-way data flow and supports a large number of vehicles per radio. We achieve reliable flight with accurate tracking (<; 2 cm mean position error) by implementing the majority of computation onboard, including sensor fusion, control, and some trajectory planning. We provide various examples and empirically determine latency and tracking performance for swarms with up to 49 vehicles.

Multi-Agent Path Finding (MAPF) is well studied in both AI and robotics. Given a discretized environment and agents with assigned start and goal locations, MAPF solvers from AI find collision-free paths for hundreds of agents with user-provided sub-optimality guarantees. However, they ignore that actual robots are subject to kinematic constraints (such as velocity limits) and suffer from imperfect plan-execution capabilities. We therefore introduce MAPF-POST to postprocess the output of a MAPF solver in polynomial time to create a plan-execution schedule that can be executed on robots. This schedule works on non-holonomic robots, considers kinematic constraints, provides a guaranteed safety distance between robots, and exploits slack to avoid time-intensive replanning in many cases. We evaluate MAPF-POST in simulation and on differential-drive robots, showcasing the practicality of our approach.

We describe a method for formation-change trajectory planning for large quadrotor teams in obstacle-rich environments. Our method decomposes the planning problem into two stages: a discrete planner operating on a graph representation of the workspace, and a continuous refinement that converts the non-smooth graph plan into a set of Ck-continuous trajectories, locally optimizing an integral-squared-derivative cost. We account for the downwash effect, allowing safe flight in dense formations. We demonstrate the computational efficiency in simulation with up to 200 robots and the physical plausibility with an experiment with 32 nano-quadrotors. Our approach can compute safe and smooth trajectories for hundreds of quadrotors in dense environments with obstacles in a few minutes.

When tracking multiple targets with autonomous cameras for 3D scene reconstruction, e.g., in sports, a significant challenge is handling the unpredictable nature of the targets' motion. Such a monitoring system must reposition according to the targets' movements and maintain satisfactory coverage of the targets. We propose an approximate, centralized approach for maximizing the visible boundary of dynamic targets using mobile cameras in a bounded 2D environment. Targets and obstacles translate, rotate, and deform independently, and cameras are only aware of the current position and shape of the targets and obstacles. Using current information, the environment is searched for better viewing positions, then cameras navigate to those positions while avoiding collisions with targets and obstacles. We present a benchmark and metrics to evaluate the performance of our method, and compare our approach to a simple gradient-based local method in several real-time simulations.

We study formation change for robot groups in known environments. We are given a team of robots partitioned into groups, where robots in the same group are interchangeable with each other. A formation specifies the locations occupied by each group. The objective is to find collision-free paths that move all robots from a given start formation to a given goal formation. Our algorithm TAPF* has the following features: (a) it incorporates kinematic constraints of robots in form of velocity limits; (b) it maintains a user-specified safety distance between robots; (c) it attempts to minimize the makespan; and (d) it runs efficiently for hundreds of robots and dozens of groups even in dense 3D environments with narrow corridors and other occlusions. We demonstrate the efficiency and effectiveness of TAPF* in simulation and on robots.

Mixed Reality can be a valuable tool for research and development in robotics. In this work, we refine the definition of Mixed Reality to accommodate seamless interaction between physical and virtual objects in any number of physical or virtual environments. In particular, we show that Mixed Reality can reduce the gap between simulation and implementation by enabling the prototyping of algorithms on a combination of physical and virtual objects, including robots, sensors, and humans. Robots can be enhanced with additional virtual capabilities, or can interact with humans without sharing physical space. We demonstrate Mixed Reality with three representative experiments, each of which highlights the advantages of our approach. We also provide a testbed for Mixed Reality with three different virtual robotics environments in combination with the Crazyflie 2.0 quadcopter.