KRRF: Kinodynamic Rapidly-exploring Random Forest algorithm for multi-goal motion planning (2505.06126v1)

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.

• The paper presents KRRF, a Kinodynamic Rapidly-exploring Random Forest algorithm that uses multiple trees to efficiently solve multi-goal planning while respecting robot kinodynamic constraints.
• Experimental evaluation shows KRRF provides up to two times lower trajectory costs and enhanced computational speed compared to existing approaches like LazyTSP and SFF*.
• The KRRF algorithm's effectiveness in managing diverse kinodynamic constraints makes it suitable for practical applications in robotics, including multi-agent systems and autonomous exploration.

Kinodynamic Rapidly-exploring Random Forest Algorithm for Multi-goal Motion Planning

Overview

The problem of multi-goal motion planning is concerned with finding a trajectory that connects multiple target locations by determining an optimal sequence of visits and planning a collision-free path, fulfilled by the robot's kinodynamic motion capabilities. This complex optimization task involves solving two NP-hard problems simultaneously: the Traveling Salesman Problem (TSP) for sequencing targets and the kinodynamic motion planning problem for trajectory generation. Traditional approaches often struggle with the inherent complexity introduced by the kinodynamic constraints and dynamic environments, leading to inefficiencies in path computation.

Kinodynamic Rapidly-exploring Random Forest (KRRF)

KRRF presents an innovative method to address these multi-goal path planning challenges. The approach builds upon the principles of sampling-based algorithms, notably extending the functionalities of the rapidly-exploring random trees (RRT) to accommodate multiple trees forming a forest structure, simultaneously growing from all target locations. This forest explores the configuration space by leveraging the kinodynamic model of the robot and facilitates heuristic-guided sampling, enhancing the efficiency of pathfinding.

The algorithm comprises two primary phases:

1. Forest Construction: In this phase, KRRF constructs kinodynamic trees from each target using the RRT methodology, incorporating the heuristic derived from neighboring trees to assist expansion toward potential connections. This exploitation-exploration mechanism ensures rapid and cost-effective tree growth, circumventing the need for explicit two-point boundary value problem solutions. The target-to-target connection costs inform the TSP solver, which computes the optimal sequence of visits.
2. Trajectory Reconstruction: This phase utilizes the target sequences from the previous phase, employing guided sampling techniques to ensure that the robot’s motion constraints are adhered to during the assembly of the final multi-goal trajectory. The sampling is biased along the target-to-target paths, thereby fostering efficient tree connectivity without compromising on kinodynamic feasibility.

Experimental Evaluation

KRRF demonstrates significant improvements over existing approaches, such as LazyTSP and SFF*, providing solutions with up to two times lower trajectory costs and enhanced computational speed across varied test environments. The algorithm's capability to handle diverse kinodynamic models, including non-standard robots, underscores its versatility and practicality.

1. Path Quality: KRRF consistently yields shorter trajectories due to its heuristic-driven tree expansion and effective sampling strategies, ensuring optimal path cost minimization.
2. Computation Efficiency: The algorithm exhibits stable computational time across varying numbers of targets, notably outpacing other state-of-the-art methods, especially in complex obstacle-laden environments.

Implications and Future Work

KRRF's approach aligns well with the ongoing advancements in autonomous systems requiring efficient navigation through complex terrains involving multiple objectives. Its effectiveness in managing diverse kinodynamic constraints expands its potential applications in robotics, particularly in areas such as cooperative multi-agent systems, active perception tasks, and autonomous exploration and inspection regimes.

Future research may focus on further optimizing the heuristics used in KRRF, exploring machine learning integration to predict promising expansion nodes or refine trajectory cost predictions dynamically. Additionally, the method could be extended to accommodate more complex environmental factors, such as dynamic obstacles and variable target settings.

Overall, KRRF offers a robust framework extending the capabilities of sampling-based motion planning algorithms, ensuring they can address increasingly complex multi-goal path scenarios in real-time applications.