We present a lightweight, decentralized algorithm for navigating multiple nonholonomic agents through challenging environments with narrow passages. Our key idea is to allow agents to yield to each other in large open areas instead of narrow passages, to increase the success rate of conventional decentralized algorithms. At pre-processing time, our method computes a medial axis for the freespace. A reference trajectory is then computed and projected onto the medial axis for each agent. During run time, when an agent senses other agents moving in the opposite direction, our algorithm uses the medial axis to estimate a Point of Impact (POI) as well as the available area around the POI. If the area around the POI is not large enough for yielding behaviors to be successful, we shift the POI to nearby large areas by modulating the agent's reference trajectory and traveling speed. We evaluate our method on a row of 4 environments with up to 15 robots, and we find our method incurs a marginal computational overhead of 10–30 ms on average, achieving real-time performance. Afterward, our planned reference trajectories can be tracked using local navigation algorithms to achieve up to a 100% higher success rate over local navigation algorithms alone.

We present a centralized algorithm for labeled, disk-shaped Multi-Robot Path Planning (MPP) in a continuous planar workspace with polygonal boundaries. Our method automatically transform the continuous problem into a discrete, graph-based variant termed the pebble motion problem, which can be solved efficiently. To construct the underlying pebble graph, we identify inscribed circles in the workspace via a medial axis transform and organize robots into layers within each inscribed circle. We show that our layered pebble-graph enables collision-free motions, allowing all graph-restricted MPP instances to be feasible. MPP instances with continuous start and goal positions can then be solved via local navigations that route robots from and to graph vertices. We tested our method on several environments with high robot-packing densities (up to 61.6% of the workspace). For environments with narrow passages, such density violates the well-separated assumptions made by state-of-the-art MPP planners, while our method achieves an average success rate of 83%.

Given an articulated robot arm, we present a method to identify two regions with non-empty interiors. The first region is a subset of the configuration space where every point in the region is manipulable. The second region is a subset of the workspace where every point in the region is reachable by the end-effector. Our method expresses the kinematic state of the robot arm using the maximal coordinates, so that the kinematic constraints take polynomial forms. We then reformulate the optimization-based inverse kinematics (IK) algorithm as gradient flows. Finally, we use sum-of-squares (SOS) programming to certify the convergence of each gradient flow. Our main result shows that the feasibility of an SOS programming problem is a sufficient condition for the manipulability and reachability of the sublevel sets of polynomial functions. Our method can be used to certify manipulable or reachable regions by solving a set of linear matrix inequalities (LMIs) or to maximize the volume of a region by solving a set of bilinear matrix inequalities (BMIs). These identified regions can then be used in various motion planning problems as hard safety constraints.

We present a novel approach for collision-free global navigation for continuous-time multi-agent systems with general linear dynamics. Our approach is general and can be used to perform collision-free navigation in 2D and 3D workspaces with narrow passages and crowded regions. As part of pre-computation, we compute multiple bridges in the narrow or tight regions in the workspace using kinodynamic RRT algorithms. Our bridge has certain geometric properties that enable us to calculate a collision-free trajectory for each agent using simple interpolation at runtime. Moreover, we combine interpolated bridge trajectories with local multi-agent navigation algorithms to compute global collision-free paths for each agent. The overall approach combines the performance benefits of coupled multi-agent algorithms with the precomputed trajectories of the bridges to handle challenging scenarios. In practice, our approach can perform global navigation for tens to hundreds of agents on a single CPU core in 2D and 3D workspaces.

We present a decentralized algorithm for group-based coherent and reciprocal multi-agent navigation. In addition to generating collision-free trajectories for each agent, our approach is able to simulate macroscopic group movements and proxemic behaviors that result in coherent navigation. Our approach is general, makes no assumptions about the size or shape of the group, and can generate smooth trajectories for the agents. Furthermore, it can dynamically adapt to obstacles or the behavior of other agents. The additional overhead of generating proxemic group behaviors is relatively small and our approach can simulate hundreds of agents in real-time. We highlight its benefits on different benchmarks.

We present a fast algorithm for continuous collision detection between deformable models. Our approach performs no precomputation and can handle general triangulated models undergoing topological changes. We present a fast decomposition algorithm that represents the mesh boundary using hierarchical clusters and only needs to perform inter-cluster collision checks. The key idea is to compute such clusters quickly and merge them to generate a dynamic bounding volume hierarchy. The overall approach reduces the overhead of computing the hierarchy and also reduces the number of false positives. We highlight the the algorithm's performance on many complex benchmarks generated from medical simulations and crash analysis. In practice, we observe 1.4 to 5 times speedup over prior CCD algorithms for deformable models in our benchmarks.