Plan Optimal Collision-Free Trajectories With Non-Convex Cost Functions Using Graphs of Convex Sets

The recently developed approach to motion planning in Graphs of Convex Sets (GCS) provides an efficient framework for computing shortest-distance collision-free paths using convex optimization. This new motion planner is notably more computationally efficient than popular sampling-based motion planners, but it does not support non-convex cost functions. This paper develops a novel motion planning algorithm, Graph of Convex Sets with General Costs (GCSGC), to solve this problem. A given non-convex cost function is accurately approximated by a multiple-layer ReLU neural network and the configuration space is decomposed into a set of linear-cost regions using the hidden layers of the neural network. These linear-cost regions are intersected with a set of collision-free regions, and the resulting collision-free linear-cost regions are intersected to form the vertices and edges of the motion planner's underlying graph structure. The edge costs have a closed-form solution within each collision-free linear-cost region, but it is non-convex, so the McCormick relaxation is applied to convexify the edge costs. Finally, a graph pre-processing technique is developed to compute a representative graph structure that acts as a heuristic for the edge costs of the underlying GCS and then simplify the underlying graph structure by removing cycles and high-cost paths, which can significantly improve the efficiency of the planner and quality of the produced trajectories. The proposed motion planner is first validated in a 2D configuration space with comparisons between different sized neural networks with and without pre-processing, comparisons between optimal trajectories from GCSGC with shortest-distance trajectories, and comparisons between GCSGC and GCS-SLP. The GCSGC planner is further validated in a complex 7D configuration space by comparing to state-of-the-art multi-query (PRM*, GCS-SLP) and single-query (TrajOpt, BIT*, AIT*, RRT*) planners. The results show that the proposed motion planner is very competitive in terms of computational efficiency, trajectory cost, and memory footprint. Two physical experiments further validate the effectiveness of the proposed motion planner in real-world motion planning applications.