Superfast Configuration-Space Convex Set Computation on GPUs for Online Motion Planning

TL;DR

The paper tackles real-time motion planning in changing environments by constructing probabilistically safe convex decompositions of the robot's configuration space using GPU-accelerated EI-ZO to inflate line-segments into safe polytopes, which are then integrated with dynamic roadmaps and decomposition-based motion planners. The core contributions are the EI-ZO algorithm, its CUDA implementation, and the end-to-end pipeline that couples DRMs, EI-ZO, and DBMPs to produce fast, reliable, collision-free trajectories in 2D and 7-DOF settings, validated on simulation and hardware. The results show substantial speedups (up to about 17x faster) and improved reliability (around a 28% increase) over nonlinear trajectory optimization, with occasional trade-offs in nominal trajectory cost. Overall, the work extends DBMPs to changing environments by enabling rapid, scalable safe-space construction and integration with high-level planning to deliver practical, perception-enabled motion planning pipelines.

Abstract

In this work, we leverage GPUs to construct probabilistically collision-free convex sets in robot configuration space on the fly. This extends the use of modern motion planning algorithms that leverage such representations to changing environments. These planners rapidly and reliably optimize high-quality trajectories, without the burden of challenging nonconvex collision-avoidance constraints. We present an algorithm that inflates collision-free piecewise linear paths into sequences of convex sets (SCS) that are probabilistically collision-free using massive parallelism. We then integrate this algorithm into a motion planning pipeline, which leverages dynamic roadmaps to rapidly find one or multiple collision-free paths, and inflates them. We then optimize the trajectory through the probabilistically collision-free sets, simultaneously using the candidate trajectory to detect and remove collisions from the sets. We demonstrate the efficacy of our approach on a simulation benchmark and a KUKA iiwa 7 robot manipulator with perception in the loop. On our benchmark, our approach runs 17.1 times faster and yields a 27.9% increase in reliability over the nonlinear trajectory optimization baseline, while still producing high-quality motion plans.

Figures (7)

• Figure 1: On the left, our hardware setup with a KUKA LBR iiwa 7 R800 robotic manipulator and three Intel RealSense D415 depth cameras for perceiving obstacles is shown. In the center, the simulated system with the perceived obstacles is shown. The target end effector pose is indicated by the yellow gripper and the collision geometries of the system are shown in red. The perceived obstacles are approximated by a union of spheres shown in the center of the table. The figure on the right shows a two-dimensional slice of the seven-dimensional configuration space, along with a slice of the safe sets (blue and green), and a slice of the configuration obstacles (self-collisions in black, collisions with perceived obstacles in red). The slice lies tangent to the trajectory such that the u-direction is co-linear with the velocity and the v-direction is a random orthogonal vector. The current configuration is indicated by the black dot.
• Figure 2: A single iteration of adding hyperplanes to the polytope in the EI-ZO algorithm. The configuration obstacles are shaded in grey and the line segment seeding EI-ZO is shown in black. Starting from the left, first, a batch of configurations is sampled in the current polytope given by the black rectangle. These samples are checked for collisions. The colliding configurations, shown in red, are then used to seed a bisection search to find collisions that minimize the distance to the line segment. The resulting candidates, indicated by the red stars (center frame), are ranked by their distance to the line segment in ascending order, and used to position the tangent planes (final frame) that separate the collisions from the line segment.
• Figure 3: A DRM is a PRM with additional lookup tables that allow the PRM to rapidly be updated provided task space observations of obstacles. To this end, the task space is discretized into voxels. We associate a circumscribing sphere with each voxel as collision geometry for collision checking. A portion of these spheres are shown in purple in the right figure. Next, a lookup table is constructed, $\mathcal{M}_c$, that maps each of these spheres to nodes in the road map that would collide with the sphere. Above, a single sphere is shown in yellow on the right, along with three configurations, shown in red, green, and blue, that collide with it and are stored in $\mathcal{M}_c$. On the left, a cartoon of the configuration space is shown with the underlying PRM and configuration obstacles. The three colliding configurations are highlighted again in red, blue, and green. The configuration obstacle corresponding to the yellow sphere is highlighted in yellow.
• Figure 4: This figure depicts our motion planning pipeline. Our motion planning pipeline is split into an offline phase in which a DRM is constructed, and an online phase where we use the DRM to generate a collision-free PWL paath, inflate the path, and use a DBMP to optimize a trajectory. Starting from the left, in the first figure the DRM is constructed. In the second, we observe a new obstacle (red blob), and build the collision set (red dot). Using the updated DRM, we then solve the IK problem for the goal (yellow star), and find a collision-free path connecting the start (blue dot) to the goal. We inflate this path to an SCS using EI-ZO in the third figure. In the fourth figure, we solve the motion planning problem using DBMPs and check the trajectory for collisions, finding one inside of the green set, indicated by the red arrow. In the fifth figure, we use the recovery mechanism from §\ref{['ssec:recovery']} to modify the green set and produce a collision-free path. In general, these last two steps need to be repeated multiple times.
• Figure 5: Flowchart for recovering from collisions found in the optimized trajectory.