Informed Steiner Trees: Sampling and Pruning for Multi-Goal Path Finding in High Dimensions

We interleave sampling based motion planning methods with pruning ideas from minimum spanning tree algorithms to develop a new approach for solving a Multi-Goal Path Finding (MGPF) problem in high dimensional spaces. The approach alternates between sampling points from selected regions in the search space and de-emphasizing regions that may not lead to good solutions for MGPF. Our approach provides an asymptotic, 2-approximation guarantee for MGPF. We also present extensive numerical results to illustrate the advantages of our proposed approach over uniform sampling in terms of the quality of the solutions found and computation speed. Note to Practitioners—MGPF is concerned with finding a collision-free, near-optimal path for a robot visiting a set of target configurations. This problem arises in applications that use robotic manipulators such as advanced manufacturing, surface inspection, package sorting, and in other logistical applications where the cost of the traveling between any two configurations of a robot cannot be readily determined a-priori. As robots are expected to perform a large number of tasks, the sequencing of these tasks become important specifically when the travel costs are challenging to estimate. This paper provides an approach to handle this problem in higher dimensions with theoretical guarantees as well as provides simulation results on a broad class of environments to corroborate its performance with respect to the state of the art.

I. Introduction

Multi-Goal Path Finding (MGPF) problems aim to find a least-cost path for a robot to travel from an origin () to a destination () such that the path visits each node in a given set of goals () at least once. In the process of finding a least-cost path, MGPF algorithms also find an optimal sequence in which the goals must be visited. When the search space is discrete (., a finite graph), the cost of traveling between any two nodes can be computed using an all-pairs shortest paths algorithm. In this case, the MGPF encodes a variant of the Steiner1 Traveling Salesman Problem (TSP) and is NP-Hard [28]. In the general case, the search space is continuous and the least cost to travel between any two nodes is not known a-priori. This least-cost path computation between any two nodes in the presence of obstacles, in itself, is one of the most widely studied problems in robot motion planning [27], [30]. We address the general case of MGPF as it naturally arises in active perception [1], [36], surface inspection [7] and logistical applications [24], [35], [40]. Specifically, industrial applications such as spot welding, spray-painting or drilling [42], [46] use robot manipulators with end-effectors to travel through a set of target configurations to perform their respective tasks. While the target configurations are known, the sequence in which these configurations must be visited are not known. In addition, the cost of traveling between any two configurations is not easy to pre-compute and is independent of the sequence in which the target configurations are visited.¹

Any node that is not required to be visited is referred to as a Steiner node. A path may choose to visit a Steiner node if it helps in either finding feasible solutions or reducing the cost of travel.