KRRF: Kinodynamic Rapidly-exploring Random Forest algorithm for multi-goal motion planning

TL;DR

The paper tackles kinodynamic multi-goal motion planning with an unknown visiting order by integrating two ideas: (i) growing multiple kinodynamic RRT-like trees rooted at each target to discover target-to-target trajectories and obtain a cost-based distance matrix, and (ii) solving a TSP on these costs to determine an efficient visiting order, followed by guided sampling to assemble a continuous, kinodynamic final trajectory. The proposed Kinodynamic Rapidly-exploring Random Forest (KRRF) leverages cross-tree heuristic information to accelerate target-to-target exploration and uses guided sampling along the TSP-derived sequence to ensure feasibility without explicit BVP solutions. Empirical results across car-like, diff-drive, and bike-like models on several maps show KRRF achieves 1.1–2x reductions in trajectory cost compared with state-of-the-art baselines, often with faster runtimes, and demonstrates robustness to varying target counts. The work provides an effective, general-purpose open-source solution for multi-goal kinodynamic planning in cluttered environments, with potential applicability to 3D and high-DOF systems.

Abstract

The problem of kinodynamic multi-goal motion planning is to find a trajectory over multiple target locations with an apriori unknown sequence of visits. The objective is to minimize the cost of the trajectory planned in a cluttered environment for a robot with a kinodynamic motion model. This problem has yet to be efficiently solved as it combines two NP-hard problems, the Traveling Salesman Problem~(TSP) and the kinodynamic motion planning problem. We propose a novel approximate method called Kinodynamic Rapidly-exploring Random Forest~(KRRF) to find a collision-free multi-goal trajectory that satisfies the motion constraints of the robot. KRRF simultaneously grows kinodynamic trees from all targets towards all other targets while using the other trees as a heuristic to boost the growth. Once the target-to-target trajectories are planned, their cost is used to solve the TSP to find the sequence of targets. The final multi-goal trajectory satisfying kinodynamic constraints is planned by guiding the RRT-based planner along the target-to-target trajectories in the TSP sequence. Compared with existing approaches, KRRF provides shorter target-to-target trajectories and final multi-goal trajectories with $1.1-2$ times lower costs while being computationally faster in most test cases. The method will be published as an open-source library.

Figures (7)

• Figure 1: Phases of the KRRF method connecting six targets (circles). First, target-to-target trajectories are simultaneously planned among all pairs of targets with a motion planning tree rooted in each target (a). The minimum-length TSP sequence to visit the targets (marked by numbers) is found based on the lengths of the planned trajectories shown in red (b). The trajectories connecting the ordered targets (red) guide the RRT-based motion planner (c) to find the final multi-goal trajectory (d).
• Figure 2: KRRF
• Figure 3: Four types of tree expansions. Expansion of tree $T_j$ using Monte Carlo simulation and selecting the nearest configuration to $q_{\text{rand}}$ (red path in (a)). Similarly for $T_i$ expansion in (b) green path is selected. In (c) an expansion of $T_i$ from node $q_{\text{pop}}$ towards the node $q_{\text{best}} \in T_j$ in its neighborhood with lowest $\varrho(q_{\text{pop}}, q_{\text{best}}) + \mathrm{cost}(q_{\text{best}}, r_j)$ (green path). Random expansion of tree $T_i$ in (d) is just a Monte Carlo simulation with one simulation trial.
• Figure 4: ExpandTrees
• Figure 5: GuidedSampling