Multi-robot Path Planning with Rapidly-exploring Random Disjointed-Trees

Traditional path planning algorithms can be mainly divided into two categories, namely graph-based algorithms and sampling-based algorithms. Graph search algorithms search the space to find the shortest path. These algorithms perform well in cases with small search spaces. However, their computational complexity increases significantly when the search space gets larger [3]. On the other hand, sampling-based algorithms generate paths via random sampling in the state space. The representative algorithm is Rapidly-exploring Random Tree* (RRT*) [4]. These algorithms can utilize randomness, require less computation, and quickly generate feasible paths.

For multi-robot path planning, there are two main methods, the centralized method and the decentralized method. A central planner acquires information about the environment and all robots. It uses this information to plan paths for each robot [5]. These methods typically use graph search algorithms, such as A* [6], and combine them with conflict-based search methods to find collision-free paths for robots, such as the Conflict-Based Search (CBS) algorithm and its variants [7], [8], [9]. However, these algorithms are computationally complex. And they can be seriously limited by the space. In large maps, these methods can experience significant increases in computation time. On the other hand, the decentralized approach decomposes the problem into multiple sub-problems. It allows each robot to independently solve its sub-problem and collaborate to complete the task. Each robot needs to find a feasible path for itself. Meanwhile, each robot regards other robots as dynamic obstacles and makes decisions based on its own local information [10]. Robots can perceive each other’s position information to coordinate their movements. This algorithm mainly consists of global path planning and local collision avoidance. In global path planning, sampling-based algorithms are mainly used to enable each robot to find its feasible path. For example, the Multi-agent RRT* (MA-RRT*) algorithm is based on RRT*, an asymptotic optimal sampling algorithm [11]. In MA-RRT* algorithm, each robot uses the RRT* algorithm to generate its own path. Some decentralized methods utilize the improved version of RRT* as the global path planning module to increase speed or reduce memory [12], [13], [14].

Considering the advantages of the decentralized algorithm’s fast speed and low computational complexity, this article adopts the sampling-based path planning algorithm to solve the multi-robot path planning problem. This article aims to find the global path in the shortest possible time. With the RRdT* algorithm as the basis [15], we call the proposed algorithm MA-RRdT* algorithm. Experiment results demonstrate that the proposed algorithm improves the performance of multi-robot path planning significantly.

The RRdT* algorithm is a sampling-based path planning algorithm based on RRT*. It has the advantages of fast speed and is suitable for complex maps. RRdT* employs multiple trees to search the space effectively. These trees originate from randomly distributed root nodes within the map and are iteratively expanded to explore local connectivity. Within the RRdT* algorithm, two primary types of exploration trees exist: the root tree, which originates from the start point, and the disjointed tree, which is randomly generated within the space. The algorithm generates a root tree at the start and multiple disjointed trees throughout the map. Each tree maintains a probability value that is modified based on the success or failure of samplings. Continuously failed samplings result in a decreased probability value, indicating reduced significance within the path planning procedure. The algorithm utilizes the restart mechanism of disjointed trees and the chained directed sampling during the sampling process. The detail of these two methods is described as follows:

The MA-RRdT* algorithm implements bidirectional root trees and shared disjointed trees to ensure efficient path planning for each robot. Bidirectional root trees mean that a dedicated root tree is generated for each start and goal. Therefore, in scenarios involving $n$ robots, the map contains a total of $2n$ root trees along with several disjointed trees. Once a node from a root tree reaches sufficient proximity to a node from the root tree of the same group (and no obstacles obstruct their path), it indicates that a feasible path for that specific group is found. Once a path is generated, the corresponding robot starts moving along the path.

In addition, the algorithm shares the location information obtained from exploring the disjointed trees on the map. The MA-RRdT* algorithm uses multiple disjointed trees to explore the space. And the spatial information obtained by these trees can be accessed by each group of start and goal. In the RRdT* algorithm, if a disjointed tree is sufficiently close to a root tree, it will be merged with the root tree (all nodes in the disjointed tree are added to the root tree) and restarted. However, in the MA-RRdT* algorithm, when a disjointed tree is merged with the root tree, a new node is generated at each node’s position, and these new nodes are joined to the root tree. This aims to ensure that a disjointed tree’s information can be shared. These new nodes carry the same location information as the original nodes. After the first merging occurs, the disjointed tree is marked as inactive and stops growing (simultaneously, new disjointed trees are generated at random locations on the map). Once another root tree is sufficiently close to the inactive disjointed tree, the location information of all nodes in the tree is shared with the root tree (i.e., new nodes that carry the location information about the disjointed tree are added to the root tree). This approach allows the spatial information recorded by the disjointed tree to be shared among all root trees. It is important to note that the spatial information of the same disjointed tree can only be added to one of the root trees in each group. This ensures that redundant spatial information is not added within the same group of root trees.

Due to variations in the positions of start and goal points, some groups find paths slower than others. To expedite the path planning process for all groups and ensure efficient path planning for each robot, the algorithm introduces a heuristic method to minimize the algorithmic run time. Once a group of root trees successfully finds a path, the probability values of the trees in that group are set to 0. Moreover, when identifying trees close to a newly sampled node $q_{new}$, the root trees that have found their path are excluded. This prevents the root trees in this group from sampling and merging with disjointed trees within the space. Consequently, growth is stopped for the root trees in that particular group. Thus, more exploration opportunities can be allocated to the root trees that have not found a path. Once all robots have established their respective paths, all trees involved stop growing.

After finding a path, the algorithm records the position information of all nodes along the path in a list. However, due to the stochastic nature of node sampling, the optimality of the path cannot be guaranteed, and there may be redundant positions leading to a less smooth path. To enhance the quality of the generated path, this study incorporates a Line-of-Sight (LoS) checking algorithm after path generation [17]. The LoS algorithm can assess whether two nodes can be rewired with less cost. In other words, if there are no obstacles on the straight path between two arbitrary nodes, the straight path is the optimal path connecting them. The LoS algorithm aims to minimize costs and eliminate redundant nodes. This leads to a smoother and shorter path. The algorithmic details are presented in the following section.

Starting from the start (i.e., the initial position), the algorithm designates the initial position as the current position, denoted as $p_{tmp}$. It examines each subsequent position coordinate $p_{check}$ in the list to determine whether a direct line connection between $p_{tmp}$ and $p_{check}$ would result in collisions with obstacles. Through this examination, the algorithm identifies the first coordinate after $p_{tmp}$ that conflicts with an obstacle, and the coordinate preceding it represents the farthest non-conflicting position from $p_{tmp}$ (referred to as $p_{next}$). By removing all position coordinates between $p_{tmp}$ and $p_{next}$ in the list, a direct connection is established between $p_{tmp}$ and $p_{next}$, eliminating redundant positions. Subsequently, $p_{next}$ is set as the new current position ($p_{tmp}$), and the process continues by examining the remaining position coordinates. These steps are repeated until the goal position is reached (i.e., $p_{next}$ is at the goal position). By utilizing this LoS checking algorithm, the generated path is optimized. An example of the LoS algorithm is shown in Fig. 3.

After addressing the global path planning problem, the issue of local obstacle avoidance between robots needs to be solved. In this algorithm, once a path is found for a group of start and goal, the corresponding robot starts its movement toward the destination. However, ensuring the robots are collision-free during their motion is essential. Therefore, the algorithm incorporates the Reciprocal Velocity Obstacles (RVO) algorithm as the robot’s local obstacle avoidance scheduler. In general, the RVO algorithm implements collision avoidance by calculating safe velocities based on other robots’ current velocities and positions [18]. In RVO, a set of velocity obstacles is computed for each robot, representing a range of velocities that could result in a collision if other robots maintain their velocity. By considering the relative velocity obstacles, the algorithm determines the safe velocity for each robot (the safe velocity refers to the velocity outside the relative velocity obstacles). This effectively prevents potential collisions between robots. Additionally, the algorithm considers each robot’s preferred velocity, which represents the desired movement velocity in the absence of other robots. In MA-RRdT*, the safe velocity that is closest to the robot’s preferred velocity is selected as its actual velocity. The RVO algorithm has demonstrated effectiveness in various scenarios, including crowded environments. Therefore, in this study, the RVO algorithm is utilized as the local obstacle avoidance module of the robots.

The proposed algorithm improves the RRdT* single-robot path planning algorithm to address the multi-robot path planning problem. Based on the RRdT* algorithm, MA-RRdT* adds the number of starts and goals (corresponding to the number of robots) and generates a dedicated root tree for each start and goal. To ensure computational efficiency and exploration effectiveness, the position information of the disjointed trees in the space is shared among the root trees within each group. Moreover, a heuristic method is introduced to accelerate the exploration process for distant root trees. Additionally, the LoS algorithm is utilized to optimize the generated paths. The processed paths have a shorter length and become smoother. Once the paths are generated and optimized, the corresponding robots start their motion accordingly. The RVO algorithm is utilized as the local obstacle avoidance module to ensure safe movement. Algorithm 2 shows the MA-RRdT* algorithm. Compared to RRdT* algorithm (algorithm 1), it can be found that the sharing of trees and the LoS method are added to enhance the global path planning process. Moreover, the robot motion module is added. An example of the MA-RRdT* algorithm is shown in Fig. 4. It can be observed that all the robots have found their corresponding paths and reached their respective goals.