Reducing Collision Checking for Sampling-Based Motion Planning Using Graph Neural Networks

1 Introduction . Sampling-based planning is a popular approach to high-dimensional continuous motion planning in robotics [ 36 , 14 , 31 , 27 , 19 , 53 , 35 ] . The idea is to iteratively sample configurations of the robots and construct one or multiple exploration trees to probe the free space , such that the start and goal states are eventually connected by some collision-free path through the sampled states , ideally with path cost minimized . This motion planning problem is hard , theoretically PSPACE-complete [ 48 ] , and existing algorithms are challenged when planning motions of robots with a few degrees of freedom [ 34 , 15 , 1 , 39 , 9 , 15 ] . In particular , the planning algorithms need to repeatedly check whether an edge connecting two sample states is feasible , i.e. , that no state along the edge collides with any obstacle . This collision checking operation is the major computational bottleneck in the planning process and by itself NP-hard in general [ 28 , 7 ] . For instance , consider the 7D Kuka arm planning problem in the environment shown in Figure 1 . The leading sampling-based planning algorithm BIT * [ 19 ] spends about 28.6 seconds to find a complete motion plan for the robot , in which 20.2s ( 70.6 % of time ) is spent on collision checking . In comparison , it only takes 0.06s ( 0.2 % of time ) for sampling all the probing states needed for constructing random graphs for completing the search . Learning-based approaches have become popular for accelerating motion planning . Many recent approaches learn patterns of the configuration spaces to improve the sampling of the probing states , typically through reinforcement learning or imitation learning [ 29 , 25 , 65 , 47 , 8 ] . For instance , Ichter et al . [ 25 ] and motion planning networks [ 47 ] apply imitation learning on collected demonstrations to bias the sampling process . The NEXT algorithm [ 8 ] provides a state-of-the-art design for embedding high-dimensional continuous state spaces into low-dimensional representations , while balancing exploration and exploitation in the sampling process . It has demonstrated clear benefits of using learning-based components to reduce samples and accelerate planning . However , we believe two 35th Conference on Neural Information Processing Systems ( NeurIPS 2021 ) . aspects in NEXT can be improved , if we shift the focus from reducing sample complexity to reducing collision checking . First , instead of taking the grid-based encoding of the entire workspace as input , we can use the graphs formed by batches of samples from the free space to better capture the geometric patterns of an environment . Second , having access to the entire graph formed by samples allows us to better prioritize edge exploration and collision checking based on relatively global patterns , and avoid getting stuck in local regions . In short , with reasonably relaxed budget of samples taken uniformly from the space , we can better exploit global patterns to reduce the more expensive collision checking operations instead . Figure 1 shows a typical example of how the trade-off benefits overall planning , and more thorough comparisons are provided in the experimental results section . ( a ) ( b ) Figure 1 : Performance on 7D Kuka arm . Left : Trajectory generated by the proposed GNNbased approach . Right : NEXT getting stuck at a local region . Both methods were trained on the same training set . We design two novel learning-based components that utilize Graph Neural Networks ( GNNs ) to accelerate the search for collision-free paths in batch samplingbased motion planning algorithms . The first component is the GNN Path Explorer , which is trained to find collision-free paths given the environment configuration and a random geometric graph ( RGG ) formed by probing samples . The second component is the GNN Path Smoother , which learns to optimize the path obtained from the explorer . In both models , we rely on the expressiveness and permutation invariance of GNNs as well as attention mechanisms to identify geometric patterns in the RGGs formed by samples , and accelerate combinatorial search . The proposed learning-based components can accelerate batch sampling-based planning algorithms without compromising probabilistic completeness properties . The methods achieve higher success rate , much lower rate of collision checking , and accelerate the overall planning compared to state-ofthe-art methods . We evaluate the proposed approach in a variety of planning environments from 2D mazes to 14D dual KUKA arms . Experiments show that our method significantly reduces collision checking , improves the overall planning efficiency , and scales well to high-dimensional tasks . The paper is organized as follows . We review related work and preliminaries in Section 2 and 3 . We describe the detailed design of the GNN architectures in Section 4 , followed by the training of GNN explorer and smoother in Section 5 . We discuss experimental results in Section 6 . 2 Related Work . Learning-based Motion Planning . Learning-based approaches typically consider motion planning as a sequential decision-making problem , which could be naturally tackled with reinforcement learning or imitation learning . With model-based reinforcement learning , DDPG-MP [ 29 ] integrates the known dynamic of robots and trains a policy network . Strudel et al . [ 54 ] improves the obstacle encoding with the position and normal vectors . OracleNet [ 2 ] learns via oracle imitation and encodes the trajectory history by an LSTM [ 23 ] . Other than predicting nodes which are expected to greedily form a trajectory , various approaches have been designed to first learn to sample vertices , then utilize sampling-based motion planning for further path exploration through these samples . L2RRT [ 24 ] first embeds high-dimensional configuration into low-dimensional representation , then performs RRT [ 36 ] on top of that . Ichter et al . [ 25 ] uses conditional VAE to sample nodes . Zhang et al . [ 65 ] learns a rejection sampling distribution . Madaan [ 40 ] encodes the explored tree with an RNN . Motion Planning Networks [ 47 ] utilizes the dropout-based stochastic auto-encoder for biased sampling . NEXT [ 8 ] projects the high-dimensional planning spaces into low-dimensional embedding discrete spaces , and further applies Gated Path Planning Networks [ 37 ] to predict the samples . Existing learning-based approaches have considered improving collision detection . Fastron [ 13 ] and ClearanceNet [ 6 ] learn function approximators as a proxy to collision detection , which is disparate from our focus on reducing the steps that are needed to the collision checker , and can be improved further potentially if combined together . Another recent line of work focuses on learning to explore edges given fixed graph . Value Iteration Networks [ 55 ] and Gated Path Planning Networks [ 37 ] apply convolutional neural networks ( CNN ) on discrete maps , then predict the policy with a weighted attention sum over neighborhoods . Generalized Value Iteration Networks [ 44 ] and Velickovic et al . [ 60 ] extend this approach for nontrivial graph by replacing CNN with GNN . However , the construction of such graphs requires ground-truth collision status for every edge on the graph at inference time . It should be noted that other than sampling-based approaches , trajectory optimization [ 30 , 68 , 57 , 51 , 67 ] , and motion primitives [ 43 , 66 ] are standard choices for more structured problems such as for autonomous cars and UAVs , while sampling-based methods are important for navigating high-dimensional cluttered spaces such as for manipulators and rescue robots . Graph Neural Networks for Motion Planning . Graph neural networks are permutation invariant to the orders of nodes on graph , which become a natural choice for learning patterns in graph problems . They have been successfully applied in robotics applications such as decentralized control [ 38 ] . For sampling-based motion planning , Khan et al . [ 33 ] utilizes GNN to identify critical samples . We focus on the different aspect of collision checking with given random geometric graphs , and can be combined with existing techniques without affecting probabilistic completeness . More broadly , GNNs have been used for learning patterns in general graph-structured problems , e.g . graph-based exploration [ 12 , 52 ] , combinatorial optimization [ 32 , 16 , 5 ] , neural algorithm execution [ 60 , 61 , 63 , 59 ] . Other than to use GNN for high-dimensional planning , several works propose to first learn neural metrics , then build explicit graphs upon the learned metric which is used later to search the path [ 50 , 18 , 17 , 64 ] . While sharing similar interests , our work specifically focuses on how to reduce the collision checking steps for sampling-based motion planning . Informed Sampling for Motion Planning . A main focus in motion planning is on developing problem-independent heuristic functions for prioritizing the samples or edges to explore . Approaches include Randomized A * [ 14 ] , Fast Marching Trees ( FMT * ) [ 27 ] , Sampling-based A * ( SBA * ) [ 45 ] , Batch Informed Trees ( BIT * ) [ 19 ] . These methods are orthogonal to our learning-based approach , which can further exploit the problem distribution and recognize patterns through offline training to improve efficiency . Recent work in motion planning has made significant progress in reducing collision checking through batch sampling and incremental search , such as in BIT * [ 19 ] and AIT * [ 53 ] . The idea is to start with batches of probing random samples in the free space , and focus on reducing collision checking to edges that are likely on good paths to the goal , which also inspires our work . Lazy Motion Planning . Lazy motion planning also focuses on reducing collision checking , typically with hand-crafted heuristics or features . LazyPRM and LazySP [ 4 , 21 ] construct an RGG first and check the edge lazily only if that edge is on the global shortest path to the goal . Instead of calculating a complete shortest path , LWA * uses one-step lookahead to prioritize certain edges [ 11 ] . LRA * interleaves between LazySP and LWA * , with a predefined horizon to lookahead [ 41 ] . These approaches use hand-crafted heuristics and do not utilize data-dependent information from specific tasks . Recently , GLS and StrOLL [ 42 , 3 ] leverage experiences to learn to select the edge to check with either fixed graphs or hand-crafted features . Our GNN-based approach proposes the use of new representations for the learning-based components , with the goal of directly recognizing patterns using samples from the configuration space . 3 Preliminaries . Motion Planning . We focus on motion planning in continuous spaces , where the configuration space is C ⊆ Rn . The configuration space includes all degrees of freedom of a robot ( e.g . all joints of a robotic arm ) and is different from the workspace where the robot physically resides in , which is at most 3-dimensional . For planning problem on a graph G = 〈V , E〉 , we denote the start vertex and goal vertex as vs , vg ∈ C. A path from vs to vg is a finite set of edges π = { ei : ( vi , v′i ) } i∈ [ 0 , k ] such that v0 = vs , v′k = vg , and v ′ i = vi+1 for all i ∈ [ 0 , k − 1 ] . An environment for a motion planning problem consists of a set of obstacles Cobs ⊆ C and free space Cfree = C \ Cobs . Note that Cobs is the projection of 3D objects in the higher-dimensional configuration space , and typically has complex geometric structures that can not be efficiently represented . A sample state v ∈ C in the configuration space is free if v ∈ Cfree , i.e. , it is not contained in any obstacle . An edge connecting two samples is free if e ⊆ Cfree . Namely , for every point v on the edge e , v ∈ Cfree . A path π is free if all its edges are free . A random geometric graph ( RGG ) is a r-disc or k-nearest-neighbor ( k-NN ) graph G [ 20 , 62 ] , where nodes are randomly sampled from the free space Cfree . In this paper we consider the RGG as a k-NN graph . Given a random geometric graph G and a pair of start and goal configuration ( vs , vg ) , the goal of agent is to find a free path π from vs to vg . Without loss of generality , we consider the cost of a path to be the total length over all edges in it . Graph Neural Networks and Attention . Let G = 〈V , E〉 be a finite graph where each vertex vi is labeled by data xi ∈ Rn . A graph neural network ( GNN ) learns the representation hi of node vi by aggregating the information from its neighbors N ( vi ) . Given fully-connected networks f and g , a typical GNN encodes the representation h ( k+1 ) i of the node vi after k aggregation as : c ( k ) i = ⊕ ( k ) ( { f ( h ( k ) i , h ( k ) j ) | ( vi , vj ) ∈ E } ) and h ( k+1 ) i = g ( h ( k ) i , c ( k ) i ) ( 1 ) where h ( 1 ) i = xi and ⊕ is some permutation-invariant aggregation function on sets , such as max , mean , or sum . We will also use the attention mechanism when we need to encode a varied number of obstacles as inputs . Suppose there are n keys each with dimension dq : K ∈ Rn×dk , each key corresponding to a value V ∈ Rn×dv . Given m query vectors Q ∈ Rm×dk , we use a typical attention function Att ( K , Q , V ) for each query as Att ( K , Q , V ) = softmax ( QKT / √ dk ) V [ 58 ] . The function is also permutation-invariant so the order of obstacles does not affect the output .

Summary: At a high-level, this work proposes GNNs as models for biasing the sampling of edges and nodes to connect in a random geometric graph. The chief goal is to reduce the amount of collision-checking necessary to plan a path from a start to a goal. Two models are presented: one that performs the above, and another that then smooths the returned path in order to further reduce path cost.  Specifically, the proposed approach first constructs a random geometric graph (RGG) by sampling points from free space. Then, a GNN-parameterized policy iteratively guides the selection of RGG edges for collision-checking. A collision-free path is found when the start and goal nodes are connected on the RGG. The algorithm then leverages another GNN to smooth the solution path. Both GNNs are learned by imitating the solutions of existing oracles. The proposed approach achieves higher success rates, lower collision checks, and comparable path costs on a set of benchmark motion planning tasks compared to existing traditional and learning-/sampling-based algorithms.