Anytime Informed Multi-Path Replanning Strategy for Complex Environments

In many real-world applications (e.g., human-robot collaboration), the environment changes rapidly, and the intended path may become invalid due to moving obstacles. In these situations, the robot should quickly find a new path to reach the goal, possibly without stopping. Planning from scratch or repairing the current graph can be too expensive and time-consuming. This paper proposes MARS, a sampling-based Multi-pAth Replanning Strategy that enables a robot to move in dynamic environments with unpredictable obstacles. The novelty of the method is the exploitation of a set of precomputed paths to compute a new solution in a few hundred milliseconds when an obstacle obstructs the robot’s path. The algorithm enhances the search speed by using informed sampling, builds a directed graph to reuse results from previous replanning iterations, and improves the current solution in an anytime fashion to make the robot responsive to environmental changes. In addition, the paper presents a multithread architecture, applicable to several replanning algorithms, to handle the execution of the robot’s trajectory with continuous replanning and the collision checking of the traversed path. The paper compares state-of-the-art sampling-based path-replanning algorithms in complex and high-dimensional scenarios, showing that MARS is superior in terms of success rate and quality of solutions found. An open-source ROS-compatible implementation of the algorithm is also provided.


I. INTRODUCTION
Moving a robot in the real world presents several challenges, including adapting the robot's motion to the dynamic nature of the environment. This problem has become very important in recent years with the spread of mobile robotics [1], social robotics [2], and human-robot collaboration (HRC) in industrial settings [3]. Path planners usually plan a path from The associate editor coordinating the review of this manuscript and approving it for publication was Zheng Chen . a start point to a goal point, considering only static obstacles. However, unstructured environments may have moving and unforeseen obstructions that invalidate the initially calculated trajectory. In these cases, path replanning algorithms are needed to enable the robot to react rapidly to environmental changes, quickly correcting the robot's route to avoid collisions and reach the goal safely. An example of a possible application is HRC, i.e., workspace sharing between humans and robots. Typically, the robot's speed is modulated according to the distance from the operator to avoid collisions.
A path replanning algorithm prevents the robot from getting stuck when the operator obstructs the robot's path for a long time.
This work proposes a sampling-based path replanning strategy that exploits multiple pre-computed paths to repair and optimize the solution over time. The method combines an anytime approach and admissible informed sampling [4] to be fast, even in complex, high-dimensional scenarios. The complexity of the search is reduced by connecting the current path to one of the available collision-free paths. The algorithm uses a heuristic to determine which nodes of the other available paths to connect to; informed sampling, subtrees reuse, and a directed graph allows for fewer calculations and a higher solutions convergence rate.

A. RELATED WORKS
Several strategies have been developed over the years to modify the robot's path on the fly.
Graph-based replanning algorithms usually derive from A* [5], a well-known algorithm to find the optimal path in a graph. For example, Lifelong Planning A* (LPA*) [6] is a modified version of A* that repeatedly finds the shortest paths from a given start node to a given goal node as the edge costs of a graph change or vertices are added or deleted. Variants of the algorithm exist [7], [8], [9], but these approaches suffer from the curse of dimensionality. Consequently, these algorithms are better suited for small-dimensional problems, such as planning for mobile robots.
Potential fields also are often investigated [10], [11], [12]. In [10], a force that depends on the distance between the robot and the obstacles deforms an initial trajectory; a subsequent force seeks to restore the trajectory. Local minima are a potential drawback of this approach. There may not be a fix if the environmental change results in the disappearance of a passage (i.e., this approach is not complete).
Reinforcement Learning (RL) is an emerging approach for online planners [13], [14]. These strategies are responsive and efficient in the environment they have been trained in. Still, they are not suitable for unfamiliar environments and completely unpredictable obstacles.
Sampling-based replanning algorithms are the most popular planners [15], [16], [17], [18], [19], [20], [21], [22], [23], [24]. Usually, these algorithms are variants of the Rapidlyexploring Random Tree (RRT) algorithm [25], its optimal counterpart RRT* [26], or the Probabilistic RoadMap (PRM) algorithm [27]. Random sampling avoids the a priori discretization of the search space. For example, Executionextended RRT (ERRT) [15] searches for a new path to the goal alternating the sampling of new states and cached waypoints. The rationale is that the real-world changes, but the changes are usually small. As a result, the path initially found can be a guideline for finding a new one. Although ERRT proves to be faster than RRT in finding the new solution, there is little information it reuses from previous planning queries. Dynamic RRT (DRRT) [16] takes a different approach, reusing the valid branches of the RRT. When an obstacle obstructs the current path, the algorithm prunes the tree and starts its construction to reach the goal again. This approach involves an initial overhead because, firstly, the algorithm must verify all tree branches. Furthermore, the tree is re-built using RRT so that the new path will be far from optimality. Efficient Bias-Goal factor RRT (EBG-RRT) [21] modifies the two aforementioned methods using an efficient and optimal waypoint cache to connect the valid part of the tree to the path beyond the obstacle.
Many sampling-based path planners combine anytime searches [28], [29], and adaptive sampling strategies [4] to compute suitable solutions quickly and rapidly refine them, and, therefore, to meet the needs of real-world applications. For example, Anytime Dynamic RRT [17] calls a DRRT when an obstacle stops the current path and uses an Anytime RRT [29] to enhance the solution. Informed sampling can speed up the search for better solutions by shrinking the sampling space with an estimation of the omniscient set, i.e., the set of samples that can improve the solution. Sampling the informed set allows considering only states with a non-null probability of improving the path [4], [30].
Among other works relevant to ours, [31] uses RRT* to repair the path given a potential collision. The nodes around the obstacle are rewired to make the node corresponding to the robot position as their parent node, and new states are sampled to find a path to the goal. Reconfigurable Random Forest (RRF) [32] is multi-query and preserves portions of the tree that are disconnected by the appearance of a new obstacle. The algorithm maintains a forest of disconnected RRTs rooted in different locations and continuously tries to connect them. Multipartite RRT [19] combines RRF, DRRT, and ERRT to maintain a forest of disconnected RRTs and repair the path to the goal. [18] provides the Multiple Parallel RRTs (MPRRT) algorithm, which continuously executes independent RRTs on each available processor core. Dynamic Roadmap (DRM) [23] adapts a PRM [27] to environmental changes by decomposing the workspace into cells and detecting the edges and nodes of the roadmap that affect each cell occupied by obstacles and then searches the graph again. Time dimension can be added to the graph, and the estimation of the movement of obstacles, if available, can be used to predict collisions [33]. Furthermore, it is possible to check vertices and edges inside a time horizon while delaying the examination of those outside it by representing obstacles as time-space volumes [34], [35]. Temporal PRM (T-PRM) [24] exploits obstacle motion prediction to incorporate into the nodes of a PRM the information about the time intervals with no collisions. Unlikely, the algorithm assumes a constant speed for the obstacles.
These methods' performance deteriorates as the search space's complexity and dimensionality increase. When the robot's path is obstructed multiple times, computing new trees from scratch each time or performing tree pruning can 4106 VOLUME 11, 2023 be costly, while path search in a PRM can take a prohibitively long time to get a real-time response from the robot.

B. CONTRIBUTION
We propose MARS, a Multi-pAth Replanning Strategy designed to quickly provide a suitable solution in complex and high-dimensional search spaces. MARS core consists of building a graph that connects paths to the same goal, allowing the exploitation of a set of paths pre-calculated. When an obstacle blocks the current path, it looks for a new one by trying to connect the current path to the other available ones. If a connection is found, it can search for better solutions during the remaining time.
Exploiting a set of pre-computed paths to find a solution is partially new in the literature. Most algorithms, indeed, involve a tree pruning phase and its subsequent reconstruction (DRRT [16], Anytime DRRT [17], RRF [32], and Multipartite RRT [19]). The few state-of-the-art approaches that exploit a population of trajectories for replanning use evolutionary computation [36], or Gaussian processes and factor graphs [37], [38]. Instead, our multi-path strategy connects the current path before the obstacle to a node closer than the goal and then uses the available sub-path from that node to the goal. Informed sampling allows to improve the plan faster over time.
The paper presents contributions beyond the method's novelty. First, the strategy is not tied to a specific sampling-based path-planning algorithm. It combines informed sampling, directed graphs, problem knowledge reuse, and lazy collision checking to reduce the computational load. Informed sampling is used to focus the search for better solutions, and the directed graph allows the results obtained in previous iterations to be exploited. Second, it introduces the firstorder connections and second-order connections to handle multiple paths to the goal in the same tree. Third, it presents a multithread architecture for executing the trajectory with continuous replanning and collision checking. This architecture can be used with various replanning algorithms and reduces the computational load on the replanner. This work represents a revision and extension of previous papers [39], [40]. There is now a great deal of reuse of the tree grown at each iteration to find solutions faster. A directed graph is introduced, which extends the tree data structure, allows intermediate connections between paths to be handled more efficiently, and allows multiple existing solutions to be searched toward the goal. The replanning architecture and its role are explained in more detail. The open-source C++ code for the proposed replanner and architecture is at [41].

II. METHOD
Denote X , X obs ⊂ X as the space of all configurations and all configurations in a collision, respectively. Given an initial configuration x start ∈ X free ≡ closure(X \ X obs ) and a goal configuration x goal ∈ X free , a path planning problem solver finds a curve σ : [0, 1] → X free such that σ (0) = x start and σ (1) = x goal . A solution curve to such a problem is a feasible path. Furthermore, an optimal path is a feasible path σ * such that: where c : free → R is a cost function that associates a cost with any feasible path σ ∈ free . Often, the cost function c is the length of the path, ∥σ ∥, so that the optimal motion plan is the shortest collision-free path from x start to x goal . Most path planners consider X obs constant over time, with limitations in unstructured environments. A reactive behavior modifies the robot's motion at run-time to prevent collisions and accomplish the intended objective and is implemented via online path replanning. We consider two types of data structures embedded in X : a graph G := (V , E) and a tree T := (V T , E T ) ( Figure 1). V and E are the sets of nodes and connections (or edges) of G, respectively, while V T ⊂ V and E T ⊂ E are the sets of nodes and connections of T . Every node of T possesses a single parent and can have multiple children except the tree's root, which has no parent. Denote as: first-order connections: connections between nodes in T , i.e., E T . Once the tree is connected to the goal node, the path is obtained following the parents from the goal. second-order connections: connections to nodes that already have a parent in the tree and, therefore, an incoming first-order connection. The set of second-order connections is cl(E \ E T ). G, which extends T , knows about these connections, but T doesn't. This formulation handles multiple paths and interconnecting portions of the same tree. Both T and G will be exploited in this paper: T is used to build multiple new solutions, and G is queried to extract the best one.

A. REPLANNING ARCHITECTURE
This architecture handles trajectory execution, continuous collision checking, and replanning. It is not bound to a specific replanning algorithm. It offers the advantage of relieving the replanner of the task of collision checking along the path followed, thus reducing its computational load. Our replanning algorithm exploits this option (Sec. II-B). The architecture relies on three parallel threads (Alg. 1): Trajectory Execution Thread micro-interpolates the robot's current trajectory τ to send the new command to the robot controller at a high rate.  P ← S ∪ σ subpath ; 8: for σ j ∈ P do 9: free, x before , x after ← checkCollision(σ j ) 10: if free then 11: Define P as the set of available paths 2: Define Q 1 as the queue of the nodes of σ curr between x curr and the obstacle 3: Sort Q 1 by some criterion 4: for each node x n ∈ Q 1 do 5: Insert the nodes of each σ ∈ P into a queue Q 2 6: Sort Q 2 by some criterion 7: for each x j ∈ Q 2 do 8: Define the informed set on the best cost up to now 9: Get the subtree rooted in x n from the informed set 10: Grow the subtree in the informed set to reach x j 11: Update Q 1 if a solution was found 12: Get the best path from the graph Collision Checking Thread checks for the collisions along the path in execution. It considers σ subpath , which is the part of the current path σ curr from the current robot configuration x curr to the goal. This thread also checks for paths σ i ∈ S if MARS is running. These checks are parallelizable. This thread computes which is the last node of the path before the obstacle (x before ∈ w curr ) and the first one after (x after ∈ w curr ) (Fig. 2a). Path Replanning Thread invokes the replanning algorithm to find a path when the current one is obstructed or to optimize the current solution. Before that, a routine projects the robot state on σ curr to obtain x curr . If the replanner finds a new path, it assigns it to σ curr . The replanner gets the maximum computation time as an input, which may change depending on the situation, e.g., it can be shorter when the path is obstructed and longer otherwise. If the replanner fails and the robot's distance from the obstacle is critical, a robot stop signal is triggered, and a contingency plan must be implemented.
This architecture allows the replanning algorithm to avoid collision checking of the current path (and other available paths σ i ∈ S). In this way, the replanner has more time to search for a solution or to improve the current one.

B. REPLANNING ALGORITHM AT A GLANCE
The section introduces MARS, a path replanner that exploits a set of pre-computed paths to find a new solution quickly, even in high-dimensional and complex scenarios. MARS computes a free path when the current one becomes infeasible and optimizes it during the execution. MARS follows an anytime approach so that it gets the first solution quickly and then tries to improve it over time.
Without loss of generality, we will consider that the cost of a path is represented by its length: where (x 1 , x 2 , .., x M ) are the nodes of path σ . This cost function is commonly used, but the method can be easily extended to other cost functions as well, provided that an admissible informed set is available. Consider Algorithm 2, where a high-level pseudo-code is reported, while the details of the implementations are in Section II-D. MARS uses a set of pre-computed paths to find a new solution. The first step consists of adding the valid part of the current path to the set of available paths S. Then, all the nodes of the current path between the robot and the obstacle are inserted into a queue Q 1 . Some criterion sorts this queue (e.g., the closest nodes to the robot are the first processed). The algorithm tries to connect each node x n ∈ Q 1 to the nodes of the other available paths, inserted into a queue Q 2 , and sorted by some criterion (e.g., the distance from the node x n ). The next step consists of finding a path connecting x n ∈ Q 1 to x j ∈ Q 2 . The algorithm first defines an admissible set with the cost of the best solution found up to now and then selects the subtree with root in x n contained in this set. It then invokes a sampling-based path planner to grow the subtree until it reaches x j (Fig. 2a). Since x j has a parent node in the tree, a second-order connection is used to connect the subtree to x j . Second-order connections are not visible on the tree but on the directed graph. MARS creates multiple solutions from the robot's configuration to the goal, and then it extracts the best one from the graph (Fig. 2b).

C. PROPERTIES OF THE REPLANNING ALGORITHM
Sampling-based path planners are probabilistically complete if the probability of finding a solution is equal to one with infinite samples. The planners are almost-surely asymptotically optimal if the solution cost converges to the optimal one as the number of samples goes to infinity. These properties are not provable for replanners without any assumptions on obstacle dynamics. It is easy to build counterexamples in which an obstacle obstructs any new given solution, preventing the robot from finding a path to the goal. Nonetheless, assuming a static scene from a specific instant of time onward, we can prove these properties for MARS.

1) PROBABILISTIC COMPLETENESS
MARS searches for a path from a generic start node x n ∈ Q 1 to a generic goal node x j ∈ Q 2 , which do not necessarily correspond to the current robot configuration and the goal. Note that the subpaths from the robot configuration to x n and from x j to the goal are always feasible. The problem, therefore, reduces to proving that the algorithm is complete in searching for a path from x n to x j . A sufficient condition for an RRT-like planner to be probabilistically complete is to draw samples with a probability greater than zero across the search space. When the current solution is obstructed, we superimpose that c i equals infinity (see (7) in Sec. II-D). The ellipsoid, therefore, comprises the entire search space T ← σ curr .tree() 3: for σ j in S do 4: T j ← σ j .tree()

5:
if T j ̸ = T then 6: T ← T ∪ T j 7: for σ j in S do 8: σ j .setTree(T) 9: if T ̸ ⊂ G then 10: G.add(T ) 11: return updated tree T and directed graph G sampled with non-zero probability. With an infinite number of samples, the algorithm can find a feasible solution if one exists.

2) ASYMPTOTIC OPTIMALITY
MARS is asymptotic optimal when it uses an asymptotically optimal planner, such as RRT*. When MARS tries to optimize the current path, it chooses pairs of nodes (x n , x j ) such that x n ∈ Q 1 and x j ∈ Q 2 . When x n corresponds to the current robot configuration and x j corresponds to the goal, the algorithm behaves like Informed RRT* [30] and is therefore asymptotically optimal. For all other pairs of nodes, MARS optimizes the connecting path between the two. VOLUME 11, 2023 FIGURE 2. Representation of the subtree rooted in x n and built to reach x j . The green path is the current path; the yellow path is another available path. The red circle represents the current robot configuration x curr .

D. DETAILED DESCRIPTION OF THE REPLANNING ALGORITHM
Algorithm 4 is the meta-code of MARS. First, MARS merges the current path tree with those of the available paths (Alg. 4a). MARS connects paths that may have been computed with independent trees and builds a graph to extract the best solution. The trees of the available paths need to be merged with that of the current path. Since all trees have the same root (paths start at the same node), this procedure is trivial. Ultimately, all paths share the same tree, which belongs to graph G. The solution path σ RP is initialized with the subpath from x curr to the goal. The set of available paths S is enriched with the valid portion of the subpath σ RP to form the set P.
All the nodes of σ RP [x curr , x before ] are inserted into the queue Q 1 . Then, the algorithm computes multiple paths connecting x n ∈ Q 1 to the other available paths using connectingToPaths and assigns the best solution to σ c2p ; now, the algorithm builds the candidate solution concatenating the subpath from x curr to x n with σ c2p . If the candidate solution has a lower cost than σ RP , it becomes σ RP . At this point, Q 1 is updated (e.g., Q 1 is emptied, and the nodes of the new solution not previously used are inserted into the queue). Note that the number of nodes in Q 1 is small, so the sorting function is not computationally demanding. In the end, the graph G is searched for a path better than σ RP .
The core of the replanner is connectToPaths (Alg. 4b), called on each x n ∈ Q 1 . This function takes as input x n ∈ Q 1 and tries to find paths connecting it to the nodes of the available paths P. As output, it returns the best path σ sol from x n to x goal found so far. To do this, first, it populates the queue Q 2 with the nodes of each path σ j ∈ P and sorts it based on some criteria (e.g., on the distance from x n ). Then, a path from x n to each x j ∈ Q 2 is computed by informedPlan; the path σ conn found is then concatenated with the subpath σ jg from x j to x goal if it results in a better solution than the best one found so far.
The graph search (Alg. 4c) takes a lazy approach to collision checking. The algorithm computes a map of paths sorted by cost; then, it scrolls through the paths contained in the map until it finds a valid one. During this validation phase, only connections that have not been checked during this replanning call are considered: connections that make up the available paths, those of the connecting paths found, and others previously checked are not re-checked. The map is computed using a depth-first search algorithm starting from the goal node. Specifically, let be x g the goal node, x s the start node, x i the node considered at the i-th step of the search, and c gi the cost of the branch from x g to x i followed in this iteration. The search along this branch stops when Both first-order and second-order connections are considered during the search in the graph G.
The most demanding part of Algorithm 4 is Sub-Algorithm 4b, which must be very efficient.
First, we speed up the search by purging the nodes x j ∈ Q 2 that do not improve the solution. Let be x n ∈ Q 1 and x j ∈ Q 2 the the root and goal nodes of the connecting path σ conn (Fig. 3c). The candidate solution σ sol = σ conn ∪ σ j [x j , x goal ] is better than the current best solution The lower bound of c(σ conn ) is the Euclidean distance from x n to x j . So, x j improves the current path if If (5) does not hold, x j is discarded by connectToPaths. Note that, if σ i [x n , x goal ] is infeasible, (4) and (5) always hold as the path has infinite cost. When a solution is found, (5) is updated with the new path's cost c(σ sol ), such that In this way, only nodes with a greater than zero probability of improving the current solution are considered.
To enhance the speed of Sub-Algorithm 4b, we also exploit the informed sampling [4] in informedPlan. When searching  for a path connecting x n to x j , the search space can be shrunk to the hyper-ellipsoid where c i = c(σ sol ) − c(σ j [x j , x goal ]). Equation (7) represents an admissible set, so the nodes outside the ellipsoid are discarded since they cannot improve the solution (Fig. 3). informedPlan checks if an existing path between x n and x j with a cost lower than c max exists. This search is in G T s , i.e., the subgraph that contains the first-order and the secondorder connections between nodes of T s or to x j . A ready-touse solution can exist thanks to previous replanning iterations. Otherwise, growInEllipsoid invokes growTree 1 that grows the subtree in the ellipsoid E ll to reach x j . If a solution is found, replacing the connection to x j with a second-order connection is the only caveat. growInEllipsoid adopts a lazy collision check. The connections already in the subtree are checked only when the path is found. The branches with a collision are hidden, and growth begins again. Figure 3 reports one iteration of connectToPaths. We select x j ∈ Q 2 as a valid node to try to connect to by exploiting σ i [x n , x goal ] and σ j [x j , x goal ] (the bold green and yellow lines). The ellipsoid E ll (grey dotted ellipse) is defined by (7), and the already existing subtree rooted in x n and contained in E ll 1 growTree is a generic sampling-based planner, and the approach is easily extendable to bi-directional ones. is selected (Fig. 3a). The connections of the subtree outside E ll are not considered (dotted green lines). Then, the subtree is grown in E ll using a path planner (Fig. 3b). Once x j is reached, a second-order connection is created between it and the subtree (light green line). The light blue path (Fig. 3c) is the σ conn found. If the cost of σ conn ∪ σ j [x j , x goal ] is less than the cost of σ i [x n , x goal ], it becomes the new candidate solution. Then, the procedure is repeated for each x j ∈ Q 2 that satisfies (6). Once the whole Q 2 has been considered, Q 1 is updated, and a new x n popped.

A. NUMERICAL SIMULATIONS
A simulation testing was conducted to compare MARS with some state-of-the-art path replanners. In particular, MARS was compared with DRRT, Anytime DRRT, MPRRT, and with [31], which we will refer to by the acronym DRRT* VOLUME 11, 2023 FIGURE 5. Normalized path length in each scenario. When a boxplot is below the red dashed line, the replanner produces a shorter path than the initial one. The success rate is listed under each replanner's name. Only replanners with a success rate greater than 5% are shown. for convenience. The tests were conducted using ROS and MoveIt! on a computer with a 2.80 GHz CPU. The code implemented can be found here [41]. Each of the algorithms has been integrated into the architecture of Algorithm 1.
They were tested in 6 scenarios, which differed in the size of the search space (i.e., degrees of freedom of the robot): Scenarios 1, 2, and 3 consist of a point robot moving in a 3-dimensional space. There are fixed obstacles of various sizes in each of the scenarios (Table 2); Scenarios 4, 5 and 6 provide a 6-, 12-, and 18-DoF anthropomorphic robot, respectively. Each of these scenarios shares the same fixed obstacles.
The test consists of 200 executions for each scenario, divided into 20 queries and 10 iterations per query. At each query corresponds a different pair of start and goal configurations. For each start-goal pair, the test is repeated 10 times. Initially, the robot path is calculated. In the case of MARS, the set of the other paths (two paths) is also computed at the beginning. The planner used is RRT-Connect [42], followed by an optimization procedure [43]. During execution, new obstacles randomly appear, obstructing the robot's path and forcing the replanner to find a new path. q ← sampleInEllipsoid(E ll ) 6: x new ← T .growTree(q) 7: if ||x new − x j || < min_dist then 8: if path from tree root to x j is valid then 9: create a second-order connection from x new to x j 10: ok ← True 11: else 12: hide the invalid branch temporarily from T 13: return ok 14: function informedPlan(x n , x j , c max , max_time) 15: (σ nj , c nj ) ← searchBetterPath(x n , x j , G Ts , c max , max_time) 19: if ¬ isEmpty(σ nj ) then 20: ok ← True 21: else 22: t MAX ← max_time − t 23: ok ← growInEllipsoid(T s , E ll , x j , t MAX ) 24: if ok then 25: (σ nj , c nj ) ← searchBetterPath(x n , x j , G Ts , c max ) 26: return σ nj from x n to x j , c nj , ok ▷ Main connectToPaths Code 27: σ sol ← σ i [x n , x goal ]; c sol ← c(σ sol ) 28: for σ j ∈ P , x j ∈ w j do 29: if x n − x j < c max then 36: t MAX ← max_time − t 37: σ conn , c conn , ok ← informedPlan(x n , x j , c max , t MAX ) 38: if ok then 39: c new ← c conn + c jg 40: if c new < c sol then 41: σ sol ← σ conn ∪ σ jg ; c sol ← c new 42: return a new path σ sol from x n to x goal thread are 500Hz, and 30Hz. The queue Q 1 of MARS is sorted on the distance along the path from the current robot configuration. The queue Q 2 is sorted on the distance from the processed node x n ∈ Q 1 . The algorithms are evaluated using the following metrics: return σ better from x s to x g , c better is the percentage of tests in which the robot reached the goal without collisions. Collision rate: denote N collisions , N tests and N obs as the number of obstacles the robot collided with, of tests performed and of random obstacles in the considered scenario, respectively. The collision rate indicates the number of obstacles the robot collided with when the replanner failed. The denominator is the number of obstacles during unsuccessful iterations. For robots with a built-in safety stop procedure that avoid collisions, this metric can be used as a proxy for the number of safety stops. Normalized path length: denote ∥σ real ∥ and ∥σ init ∥ as the length of the path traversed by the robot and the length of the path computed at the beginning of the iteration, respectively. The n.p.l. = ∥σ real ∥/∥σ init ∥ indicates how longer (due to obstacle avoidance) or shorter (due to path optimization) the path is compared to the initial one. Since σ real depends on σ init , its length is normalized to obtain a comparable measure across tests. Figure 4 shows screenshots of two tests of MARS in a 3 DoF and 6 DoF scenario. Table 3 shows the success rate S% and collision rate C% of the algorithms in each scenario. Figure 5 shows the boxplot of the normalized path length n.p.l for replanners that achieved at least a 5% success rate. All planners have a high success rate for scenario 1. As the complexity grows, MARS maintains a high success rate (S% ≥ 88.5%). In scenarios 5 and 6, MPRRT and DRRT* achieve a success rate of 9.0% and 4.0%, respectively, whereas DRRT and Anytime DRRT always fail. MPRRT performs better than DRRT and Anytime DRRT in scenario 6 because it plans on multiple parallel threads, and its collision rate remains lower than DRRT and Anytime DRRT.   Figure 5 shows the n.p.l for the algorithms with a S% ≥ 5%. When a boxplot is below the red dashed line, the replanner produces a shorter path than the one computed initially. Anytime DRRT, MPRRT, and MARS continuously try to improve the solution, even when it is unobstructed. DRRT and DRRT* replan only when the path is obstructed. DRRT ends as soon as it finds a feasible solution. DRRT* uses all remaining replanning time to improve the found solution.
In scenarios 1-2 MARS, DDRT* and MPRRT provide comparable n.p.l. (Figure 5a and 5b). The performance difference compared to MARS increases with the complexity of the problem. MARS outperforms the baselines in all those situations where the time to compute and optimize the initial path is critical; the algorithm, in these cases, will act as an online optimizer. Anytime DRRT seems equivalent to DRRT, probably because most of the replanning time is spent by DRRT on finding a feasible solution, and the remaining time devoted to its improvement is small.
In scenarios 5 and 6, MARS and MPRRT have comparable n.p.l. results. However, they are dramatically different in terms of success rate (93% and 88.5% of MARS compared to 7.5% and 9% of MPRRT).

B. REAL-WORLD EXPERIMENTS
MARS and the replanning architecture were tested in the collaborative robotic cell of Figure 6. Specifically, the setup consists of a 6-DoF UR10e manipulator mounted upside down and an Intel Realsense D435 to track the operator's position. A speed scaling module was active during the experiments to slow down the trajectory depending on the humanrobot distance for safety reasons (see Section III.F of [44], and [45] for more details about the scaling module). In the experiments, the robot moved from a start position to an end position. At the same time, a human obstructed its initially calculated path, forcing the algorithm to search for a new valid path and to optimize it over time. A demonstration video of the application is attached to this paper.

IV. CONCLUSIONS AND FUTURE WORKS
This paper proposed MARS, a sampling-based path replanning algorithm for complex, high-dimensional scenarios. The novelty of the algorithm is the exploitation of a set of precomputed paths to find and optimize over time a new path when the current one is obstructed by an unexpected obstacle. The algorithm determines which nodes of the available paths to connect to based on the cost of the current solution and gradually builds a directed graph to exploit the results of previous iterations. Informed sampling, subtrees reuse, and lazy collision checking allows for fewer calculations and finding better solutions more quickly. MARS proved superior to leading sampling-based replanning algorithms in the state-of-the-art in terms of both success rate and quality of solutions found. The superiority of MARS lies in the fact that it reduces the complexity of the search by trying to connect the current path to a node of the other available paths closer to the current path than the goal.
We also propose a multithread architecture for executing the robot trajectory with continuous replanning and collision checking, applicable to many replanning algorithms. The open-source C++ code of the algorithms and the architecture is available at [41].
Future developments include analysis of how the initial set of paths affects MARS. The trade-off between diversity and quality of initial paths affects the performance, e.g., optimizing the initial path may reduce diversity, with the risk that a single obstacle will block them all, thus reducing the chances of success. Furthermore, we will improve the graph search algorithm. Currently, a depth-first search algorithm is used to find all solutions with a cost less than the desired value and then extract the valid and less costly one. An alternative and more efficient approach could be taken by LPA* [6], which could avoid searching all solutions before checking the validity of the less expensive one.