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

TL;DR

KRRF is a novel algorithm that efficiently plans collision-free, kinodynamically feasible multi-goal trajectories by combining tree growth, heuristics, and TSP optimization, outperforming existing methods in cost and speed.

Contribution

The paper introduces KRRF, a new approximate method that integrates kinodynamic tree growth with TSP solving for multi-goal motion planning.

Findings

KRRF produces shorter target-to-target trajectories.

KRRF achieves 1.1-2 times lower trajectory costs.

KRRF is computationally faster than existing approaches.

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.