Implicit Graph Search for Planning on Graphs of Convex Sets

TL;DR

The paper tackles the computational bottleneck of Graphs of Convex Sets (GCS) in motion planning by introducing INSAT-based implicit graph search methods, IxG and IxG*. By interleaving discrete graph search with convex trajectory optimization and leveraging a Lower Bound Graph (LBG) for admissible heuristics and pruning, the approach achieves stronger completeness and optimality guarantees while dramatically reducing planning time. IxG* further enhances guarantees by allowing path revisits and employing pruning to control complexity, enabling parallelization. Empirical results across 2D mazes, 3D UAV environments, and multi-arm manipulation demonstrate substantial runtime reductions and scalable performance compared to traditional GCS, without sacrificing trajectory quality. The work thus broadens the applicability of GCS by enabling fast, anytime planning and flexible replanning using well-established graph-search techniques.

Abstract

Graphs of Convex Sets (GCS) is a recent method for synthesizing smooth trajectories by decomposing the planning space into convex sets, forming a graph to encode the adjacency relationships within the decomposition, and then simultaneously searching this graph and optimizing parts of the trajectory to obtain the final trajectory. To do this, one must solve a Mixed Integer Convex Program (MICP) and to mitigate computational time, GCS proposes a convex relaxation that is empirically very tight. Despite this tight relaxation, motion planning with GCS for real-world robotics problems translates to solving the simultaneous batch optimization problem that may contain millions of constraints and therefore can be slow. This is further exacerbated by the fact that the size of the GCS problem is invariant to the planning query. Motivated by the observation that the trajectory solution lies only on a fraction of the set of convex sets, we present two implicit graph search methods for planning on the graph of convex sets called INSATxGCS (IxG) and IxG*. INterleaved Search And Trajectory optimization (INSAT) is a previously developed algorithm that alternates between searching on a graph and optimizing partial paths to find a smooth trajectory. By using an implicit graph search method INSAT on the graph of convex sets, we achieve faster planning while ensuring stronger guarantees on completeness and optimality. Moveover, introducing a search-based technique to plan on the graph of convex sets enables us to easily leverage well-established techniques such as search parallelization, lazy planning, anytime planning, and replanning as future work. Numerical comparisons against GCS demonstrate the superiority of IxG across several applications, including planning for an 18-degree-of-freedom multi-arm assembly scenario.

Figures (17)

• Figure 1: Smooth, collision-free motions generated by INSATxGCS (IxG) for various tasks with three Motoman HC10DTP arms operating simultaneously in a 18-DoF multi-arm assembly scenario.
• Figure 2: An example of decomposition of free space into convex sets $\mathscr{Q}_1 \cup \ldots \cup \mathscr{Q}_n$ with start $q_0$ and goal $q_T$ states (above) and their relationship represented as a graph (below).
• Figure 3: Graphical illustration of INSATxGCS in contrast to GCS. Fig. \ref{['sfa']} shows the joint batch optimization performed by GCS over the entire graph. The orange regions denote the obstacles and the blue regions denote the free space decomposed into a graph of convex sets. The final trajectory is shown as a dark blue curve and the optimization over each convex set of the graph is shown as light blue curves. Fig \ref{['sfb']}-\ref{['sfd']} shows the various steps of INSATxGCS incrementally exploring the convex sets and evaluating the edges only as needed. The convex sets in dark blue denote the explored graph and the light ones denote the unvisited convex sets. The trajectories in dark blue are the incremental optimizations that reuse the light blue solutions from previous expansions for computing trajectories to the successor states. INSATxGCS finds the solution trajectory to the goal without even visiting several convex sets of the graph (Fig. \ref{['sfd']}).
• Figure 4: Working principle of GCS gcs.
• Figure 5: Working principle of INSAT

Theorems & Definitions (9)

• Definition 1
• Theorem 1
• proof
• Theorem 2
• proof
• Theorem 3
• proof
• Theorem 4
• proof