Multi-goal path planning using multiple random trees

TL;DR

This work addresses multi-goal path planning by decoupling the visiting order (solved via TSP) from the path planning between targets. It introduces Space-Filling Forest (SFF*), a sampling-based planner that grows multiple trees from target locations, records multiple near-target connections as virtual edges, and applies RRT*-style rewiring to minimize path costs. The key contributions are the open/close node management, priority-queue bias toward promising target directions, and the ability to form and reuse multiple connections between targets, improving target-to-target path quality and, consequently, TSP costs. Empirically, SFF* yields shorter target-to-target paths and lower final TSP costs than several baselines in 2D and 3D benchmarks, with a practical trade-off of longer runtimes, making it advantageous for scenarios where path quality drives mission efficiency.

Abstract

In this paper, we propose a novel sampling-based planner for multi-goal path planning among obstacles, where the objective is to visit predefined target locations while minimizing the travel costs. The order of visiting the targets is often achieved by solving the Traveling Salesman Problem (TSP) or its variants. TSP requires to define costs between the individual targets, which - in a map with obstacles - requires to compute mutual paths between the targets. These paths, found by path planning, are used both to define the costs (e.g., based on their length or time-to-traverse) and also they define paths that are later used in the final solution. To enable TSP finding a good-quality solution, it is necessary to find these target-to-target paths as short as possible. We propose a sampling-based planner called Space-Filling Forest (SFF*) that solves the part of finding collision-free paths. SFF* uses multiple trees (forest) constructed gradually and simultaneously from the targets and attempts to find connections with other trees to form the paths. Unlike Rapidly-exploring Random Tree (RRT), which uses the nearest-neighbor rule for selecting nodes for expansion, SFF* maintains an explicit list of nodes for expansion. Individual trees are grown in a RRT* manner, i.e., with rewiring the nodes to minimize their cost. Computational results show that SFF* provides shorter target-to-target paths than existing approaches, and consequently, the final TSP solutions also have a lower cost.

Figures (6)

• Figure 1: Example of SFF* expansion for trees $t_1,t_2,t_3$ rooted at target locations (red). The nodes in the open list are in green, the nodes in the close list are in black. Let assume the node $e$ is going to be expanded, so its vicinity is sampled in distance $l$ from $e$ (brown) (a). The candidate $1$ is discarded as it approaches other nodes of the tree closer than $e$. The candidate $4$ is discarded as it is not collision-free (b). If the candidate $r_{new} = 2$ is added to the tree, it becomes member of the open list and no other action is made (b). However, when candidate $r_{new} = 3$ is added to the tree, it is already too close to the node $f \in t_3$ because $\varrho(r_{new},f) < d$. Therefore, the trees $t_1$ and $t_2$ are virtually connected (blue edge) via new node $r_{new}$, i.e., edge $(r_{new},f)$ is added to the list of connections $E$ (c).
• Figure 2: The difference between Multi-T-RRT Devaurs_TRRT_TSP and SFF* in a map with the target locations $r_1$ and $r_2$. In classic RRT (and also in Devaurs_TRRT_TSP), the trees are expanded towards the random samples due to Voronoi-bias. In the depicted scenario, more samples are generated on the left from targets than in the right (a). Consequently, the RRT-based method, e.g., Devaurs_TRRT_TSP, prefers to grow both trees towards the left zone. The connection of these trees more likely happens also in the left zone, which results in a long path between the targets (blue) (b). Moreover, as Devaurs_TRRT_TSP connects the trees only once, no other path can be found even if the number of samples is increased. Contrary, SFF* can find multiple connections between the trees and therefore, it can also discover the shorter path (red) (c).
• Figure 3: Behavior of SFF* depending on the parameter $p_{q}$ and with the increasing number of targets. Graphs are made using 20 measurements in V-Dense (a) and Dense (b) maps.
• Figure 4: Histograms of cumulative costs of target-to-target paths, the cost path (horizontal axis) is the map units.
• Figure 5: Final TSP solution on the V-Dense map (a) and on the Triangles map (b).