Informed Anytime Fast Marching Tree for Asymptotically Optimal Motion Planning

In many applications, it is necessary for motion planning planners to get high-quality solutions in high-dimensional complex problems. In this article, we propose an anytime asymptotically optimal sampling-based algorithm, namely, informed anytime fast marching tree (IAFMT*), designed for solving motion planning problems. Employing a hybrid incremental search and a dynamic optimal search, the IAFMT* fast finds a feasible solution; if time permits, it can efficiently improve the solution toward the optimal solution. This article also presents the theoretical analysis of probabilistic completeness, asymptotic optimality, and computational complexity on the proposed algorithm. Its ability to converge to a high-quality solution with efficiency, stability, and self-adaptability has been tested by challenging simulations and a humanoid mobile robot.

Informed Anytime Fast Marching Tree for widespread methods available in practical applications. There are natural advantages for sampling-based methods to solve high-dimensional problems as they avoid the explicit construction of the C-space by random sampling [14], [15].
Efforts to find high-quality solutions, whose quality can be measured according to length, clearance, smoothness, and energy, lead to asymptotically optimal (AO) methods. A highquality solution is a path whose cost is lower than the given cost threshold or close to the cost of the optimal solution. The optimal solution is the path with the lowest cost in a C-space. Methods with asymptotic optimality, such as RRT * and PRM * [16], are able to asymptotically converge to the optimal solution as the number of samples goes to infinity. Whether the optimal solution can be converged is usually concerned in theory, while in practical applications, we focus on obtaining high-quality solutions, not the optimal solution, because it is difficult for motion planning methods to converge to the optimal solution in limited runtime and computational resources.
For high-dimensional complex problems, sampling-based AO methods may not consider the efficiency and stability to achieve high-quality solutions [17], [18]. The informed sampling technique is introduced to AO methods, such as informed RRT * [19] and batch informed trees (BIT * ) [20], to reduce computational burden by defining the sampling region as a hyperellipsoid for finding a high-quality solution. Fast marching trees (FMT * ) [21] performs a "lazy" dynamic programming recursion on a batch of random samples to generate a low-cost tree as its roadmap. It is capable of solving high-dimensional complex problems efficiently, especially in scenarios where collision-checking is expensive, if the number of samples is reasonable. However, when the number of samples is small, FMT * is difficult to obtain high-quality solutions, sometimes it even fails to converge, as shown in Fig. 1(a) and (b). FMT * will lose the advantage of computational efficiency if the number of samples is large, as presented in Fig. 1(c).
For different problems, it is hard for motion planning methods without self-adaptability to fast converge to high-quality solutions, which means that the reasonable parameter values of a method may be completely different when solving simple problems and complex problems, respectively. In short, methods with self-adaptability can automatically tune parameters as requirements change. For example, the performance of FMT * and its variants [22], [23] heavily depends on the number of samples, which leads to lack of self-adaptability for the methods. The reasonable number of samples may be several thousand in 2-D environments, while that could be tens of thousands in 3-D environments, as shown in Fig. 1(e), even in the same environment, different number of samples are needed to solve problems with different levels of difficulty, as shown in Fig. 1(d) and (f). It is not easy to determine the reasonable number of samples for an unfamiliar problem. Some AO methods have anytime performance leading to self-adaptability, named anytime AO methods, such as RRT * , PRM * , and their variants [24]- [26]. Anytime methods provide any solution as quickly as possible and yield higher quality solutions if time permits [27], [28]. Anytime AO methods fast search for an initial solution and asymptotically improve the solution toward the optimal solution. Motion planning using lower bounds (MPLB) [29] is a quasi-anytime algorithm by solving a series of independent problems and better solutions are only returned after a subproblem is solved.
This article presents an anytime AO algorithm, namely, informed anytime FMT * (IAFMT * ). IAFMT * extends FMT * to an anytime algorithm by designing a hybrid incremental search and a dynamic optimal search, in addition, the informed sampling technique is employed to refine efficiency. The hybrid incremental search integrates the batch sampling search, like FMT * , and the single sampling search, like RRT * , to build a low-cost spanning tree, which balances the efficiency of the batch sampling search and the flexibility of the single sampling search. As we know, the "nonlazy" search tends to find a highquality solution by taking lots of computational resources, while the "lazy" search efficiently obtains a feasible solution. The proposed dynamic optimal search considers a tradeoff between the "nonlazy" and "lazy" searches to fast improve a spanning tree and it achieves a high-quality solution. Additionally, this article gives the theoretical analysis on the IAFMT * in depth, such as the analysis of probabilistic completeness, asymptotic optimality, and computational complexity. This article also comprehensively evaluates the performance of the proposed algorithm by a series of 2-D, 3-D simulations, and real-world experimental tests. The experimental results show that the proposed algorithm, IAFMT * , has the ability to converge to a high-quality solution with efficiency, stability, and self-adaptability when compared with the state-of-the-art algorithms, namely, PRM * , informed RRT * , and FMT * .
Our contribution can be summarized as follows. 1) This article presents an anytime sampling-based AO algorithm, namely, IAFMT * , which is used to solve highdimensional complex motion planning problems and get high-quality solutions. 2) We propose a strategy that balances the benefits of the batch and single sampling searches, the "nonlazy" and "lazy" searches, which makes IAFMT * efficient, stable, and self-adaptive. 3) In order to support the proposed algorithm in theory, we provide the analysis of probabilistic completeness, asymptotic optimality, and computational complexity. 4) This article comprehensively evaluates the performance of IAFMT * for solving high-dimensional complex problems by simulations and a humanoid mobile robot. This article is organized as follows. In Section II, the problem is formally defined. Section III introduces the proposed algorithm, IAFMT * . The theoretical analysis of IAFMT * is provided in Section IV. Next, Section V presents the results of the simulation and experiments. Finally, Section VI concludes this article and gives future research directions.

II. PROBLEM FORMULATION
Let X ⊆ R d and X free ⊂ X be a d-dimensional C-space and the obstacle-free space, respectively. Let x init ∈ X free denote the initial node and X goal ⊂ X free be the set of goal nodes. A feasible path to the motion planning problem can be denoted by 1], which means that the initial node x init connects to any node x goal ∈ X goal through free space. Let Σ be the set of all feasible paths and J(σ) be the path cost according to Euclidean metric in X .
The optimal motion planning problem can be described that, given a path cost function J : Σ → R ≥ 0, finding the optimal solution σ * satisfies (1) This article will interchangeably refer to points in X as nodes or samples.

Algorithm 1: IAFMT * .
Require: Query (x init , x goal ), Search radius r n , Sample count n 0 , Available time t, given cost threshold J given if Cost T (x goal ) ≤ J given then 10 return σ HQ ← σ 11 return σ

III. INFORMED ANYTIME FAST MARCHING TREE
We now present the IAFMT * described in the pseudocode from Algorithms 1-5.

A. Algorithm Overview
It is necessary to introduce some notions and functions before describing IAFMT * . V i is the set of nodes including x init , x goal , and batch random samples n generated by SampleFree(n). T = (V, E) denotes a spanning tree, where V and E are the treenode and tree-edge sets, respectively. V u is the unvisited node set where the nodes are not added to the tree. V op contains the open nodes that may be expanded on the tree. V c is the closed node set where there are no unvisited nodes near each tree node. Let Near(V space , z, r n ), i.e., N z , be a function which returns the node set that satisfies where V space is the set of all nodes in the current C-space, z is an open node selected for expansion, and r n denotes the search radius. The search radius r n [21] is where η > 0 is a small constant, μ(·) denotes the Lebesgue measure, and ζ d is the volume of the unit ball in the d-dimensional Euclidean space. Cost T (x) returns the shortest path cost from x init to x on the tree, i.e., the low cost value for x. We establish the IAFMT * algorithm in Algorithm 1. The IAFMT * algorithm first determines V i by random uniform sampling, then it initializes a tree and other parameters by the function Initialize(V i , V u , E). HybridIncrementalSearch(T, z) and DynamicOptimalSearch(V i−1 , T ), which are different from FMT * , are introduced to search feasible paths and highquality paths. HybridIncrementalSearch(T, z) first uses the batch sampling search to fast expand a low-cost spanning tree and try to find a feasible path σ. And if a feasible path σ is not found, the function will perform the single sampling search to employs the informed sampling technique to substantially reduce the search region where there exists better paths and it asymptotically improves the tree T to achieve a high-quality path by integrating the "nonlazy" and "lazy" searches. The IAFMT * algorithm returns σ HQ when the cost of the current feasible path is less than or equal to the given cost threshold J given . σ HQ is equal to σ * when J given ≤ J(σ * ) and available time t is enough.
There are the following three contributions in IAFMT * : 1) IAFMT * extends FMT * to an anytime AO algorithm which can fast find a high-quality path adaptively in a given runtime; 2) the hybrid incremental search introduces the incremental single sampling search to the "lazy" batch sampling search of FMT * in order to have the ability to quickly get feasible paths as facing different motion planning problems; and 3) the dynamic optimal search considers a tradeoff between the "nonlazy" and "lazy" searches to anytime asymptotically improve a spanning tree, which provides low-cost feasible paths and a high-quality path, even the optimal solution, if conditions permit.

B. Hybrid Incremental Search
The hybrid incremental search aims to find an initial path efficiently and flexibly by integrating the batch sampling search and the single sampling search, as shown in Fig. 2(a)-(c). In Algorithm 2, based on the batch random samples n in Algorithm 1, the function ExpandTree(z) efficiently builds a spanning tree, where z consistently denotes the open node selected for expansion. Path(x goal , T ) returns the lowest cost feasible path from x init to x goal on the tree. However, the batch sampling search ExpandTree(z) cannot always obtain a path. The single sampling search InsertNode(T ) flexibly connects one new node to the tree in order to assist ExpandTree(z) in finding an initial path.
The ExpandTree(z), as shown in Algorithm 3, is introduced from FMT * [21] except for RewireConnection(x, Y near ) used in the dynamic optimal search in Algorithm 5. The lowest cost open node z is selected to search for its neighbor unvisited node set X near . For each x ∈ X near , its neighbor open node set Y near is obtained to determine the lowest cost node y min . If the connection between y min and x is valid, the x with y min as its parent is sent to the set V op which will be added to the tree T and V op once all x samples in X near have been considered. It is noteworthy that the x samples in V op , not in V op , will not be connected to the remaining samples in X near . Thereafter, the node z is removed from the open set V op and added to the closed set V c . RewireConnection(x, Y near ) is able to improve the spanning tree if a path σ exists; we will explain it later. The batch sampling search returns Failure when V op and σ are empty.
The InsertNode(T ) shown in Algorithm 4 determines a new node s from X free and activates the neighbor closed nodes W near . The new node is added to the existing tree in an optimal way. In addition, the new node can be regarded as a bridge between the Algorithm 4: InsertNode(T ).
1 s ← SampleFree (1); else then W near ← W near \{x min } 9 return T closed nodes and the unvisited nodes to help the ExpandTree(z) expand the tree again.

C. Dynamic Optimal Search
The proposed dynamic optimal search, balancing the features of the "lazy" and "nonlazy" searches, asymptotically improves the existing tree to find a better path until the optimal path is obtained, as shown in Fig. 2(d)-(f). We briefly list new functions and notions in Algorithm 5. Let Prune(V i−1 , Cost T (x goal )) be the function that prunes the nodes and edges in V i−1 by the informed technique determining a hyperelliptic region in C-space, where V i−1 only contains closed nodes and unvisited nodes. Better paths may exist in the hyperelliptic region that satisfies Cost ig (v) denotes the sum of the lowest cost from x init to v and from v to x goal , ignoring obstacles. The function InformedSampleFree(n, Cost T (x goal )) samples n nodes in the hyperelliptic region and the nodes not on the tree in V i are added to V u by UnconnectNodes(V i ).
Algorithm 5 shows the dynamic optimal search for IAFMT * . Prune(V i−1 , Cost T (x goal )) deletes the nodes and edges outside the hyperelliptic subregion given by the informed technique and InformedSampleFree(n, Cost T (x goal )) provides n new nodes in the subregion. V u includes the new nodes in the i'th iteration and the unvisited nodes in the (i−1)'th iteration after executing UnconnectSamples(V i ). It is noted that all the closed nodes in V i are reopened when the dynamic optimal search is initialized. The dynamic optimal search asymptotically improves the tree and tries to find a lower cost path at any time. The function RewireConnection(x, Y near ) in Algorithm 3 makes the ExpandTree(z) improve the searching tree. The ExpandTree(z) employs the "lazy" search, a neighbor unvisited node x is simply skipped if a connection between x and its neighbor open node intersects an obstacle, to connect the nodes in V u to the tree (Algorithm 3, lines 2-8) as the dynamic optimal search is executed. Thereafter, the "nonlazy" search is conducted by the RewireConnection(x, Y near ) to rewire every neighbor open node h, which connects the lowest cost node as its parent node.

A. Probabilistic Completeness
The proposed algorithm IAFMT * ensures probabilistic completeness which means that the probability of solving a problem goes to 1 as the number of samples approaches infinity.
Theorem 1: IAFMT * is a probabilistically complete algorithm. For a given motion planning problem, the probability of searching for a feasible path is as follows: Proof: The following three arguments are used to prove the Theorem 1: 1) a batch of nodes including x init and x goal are randomly sampled from X free ; 2) the batch sampling search starts to build a searching tree from x init and expands the tree with the lowest cost node in T as the selected node z; 3) the single sampling search helps the batch sampling search explore the area where the distance from any node to the tree nodes is longer than r n . Therefore, IAFMT * performs the hybrid incremental search to steadily grow a searching tree outward from x init . The probability of finding a feasible path approaches one as the number of samples n goes to infinity. So, it is stated that IAFMT * can guarantee probability completeness.

B. Asymptotic Optimality
Let σ T : [0, 1] → X be a tracing path that traces the path σ. The cost of σ T is bounded as when σ T approaches σ, where ε is a given small constant. Theorem 2: IAFMT * is an AO algorithm. For an optimal motion planning problem as defined in Section II, IAFMT * makes a feasible path σ converge in probability to the optimal path σ * as n → ∞. Specifically, for any ε > 0 Proof: The proof is based on [21] that probability is bounded Finding a tracing path σ T by IAFMT * approximates σ * with Obviously, for any ε > 0, we can obtain the inequality As n is large enough, a tracing path σ T approaching σ T can be found and the following inequality can be deduced: where by [21]. Let σ T returned by IAFMT * approximate σ * , for any If ε ≤ 3, the inequality can be obtained which follows that Hence, If ε > 3, the above statement still holds due to the probability being monotone-decreasing about ε. Therefore, IAFMT * is an AO algorithm, as claimed.

C. Computational Complexity
The space complexity SC IAFMT * n as the amount of memory space occupied by IAFMT * in iteration n. The time complexity TC IAFMT * n is defined as the number of calls to the most timeconsuming IAFMT * function in iteration n.

O(log n) time is required to run HybridIncrementalSearch(·)
and DynamicOptimalSearch(·) once, respectively. Hence, IAFMT * takes O(n log n) time for n nodes.

1) Simulation Setup:
We provide numerical experiments to evaluate the performance of IAFMT * by comparing it with three state-of-the-art algorithms, namely, PRM * , informed RRT * , and FMT * . All algorithms are run on a 3.7-GHz Intel Core i7-8700K CPU with 16 GB of memory and tested in the OMPL [30] v1.4.0 benchmark. Four OMPL test scenarios are considered-two 2-D scenarios, "Bug Trap" and "Maze", in the SE(2) C-space as well as two 3-D scenarios, "Cubicles" and "Apartment", in the SE(3) C-space, as illustrated in Fig. 3. Setting runtime t given and cost threshold J given in each scenario, as shown in Table I, can assess the ability of converging to a high-quality solution in limited resources by test algorithms. The default OMPL settings are employed for PRM * and informed RRT * . We use the default value r n = 1.1 for FMT * and IAFMT * . n = 1000 is given as the initial number of samples for IAFMT * in all scenarios.
In order to compare the nonanytime AO algorithm FMT * with the other three anytime AO algorithms, we vary the given number of samples for FMT * in different scenarios. Sample counts are varied from 1000 to 10 000 points in "Bug Trap" and "Maze", from 1000 to 50 000 points in "Cubicles", from 1000 to 80 000 points in "Apartment". FMT * runs 20 times at  Table II, where t avg is the average runtime, J avg is the average path cost, and I-RRT * denotes informed RRT * . In terms of computational efficiency, i.e., t avg , the performance of FMT * and IAFMT * substantially outperform PRM * and informed RRT * , and that of IAFMT * is slightly better than FMT * in general. All algorithms can give planning paths with lower J avg than J given in each scenario, except J avg of PRM * is 1914.18 in "Cubicles" scenario, which means that PRM * cannot find low-cost paths. Let Sol in Table II be the success rate of obtaining the feasible path by the algorithms. And Opt is the success rate of getting the high-quality path satisfying the given cost threshold J given by the algorithms. The success rate of obtaining the feasible path and high-quality path by IAFMT * is obviously higher than the other algorithms in all scenarios. In the most challenging "Apartment" scenario, the success rate of obtaining the high-quality path by IAFMT * is 64%, 70%, and 20.9% higher than PRM * , informed RRT * , and FMT * , respectively, which demonstrates the advantages of IAFMT * in the capability of solving a high-quality solution. IAFMT * also shows self-adaptive ability as the success rates of achieving the feasible path and the high-quality path are 100% in all scenarios.
Figs. 4 and 5 use the box plots to visually show the statistical information for computation time and path cost given by each algorithm, where the red dashed lines denote J given and the small circles are data outliers. As presented in Fig. 4, data points of path cost generated by IAFMT * are all under the line of J given , and the points are closer to the given line except in "Maze" scenario as the low-cost paths are easy to be found by IAFMT. Hence, IAFMT * can save lots of computational resources while guaranteeing high-quality solutions. In addition, the data points of path cost by IAFMT * is denser in general, which indicates that the performance of obtaining high-quality paths is more stable than the other algorithms. The same features of data points are also shown in Fig. 5. The lower and denser data points of computation time given by IAFMT * show that IAFMT * has the  capability to converge to a high-quality solution rapidly and stably.

1) Experiment Setup:
A simulation and a pick-and-place experiment on NEU-Robot equipped with 6-DOF manipulators, as shown in Fig. 6, are carried out to evaluate the performance of IAFMT * in application. We also use the four algorithms, run on a 2.3-GHz Intel Core i7-3610QE CPU with 8 GB of memory, to plan the trajectory of the manipulators. For the end-effectors of the manipulators, position precision and orientation precision are 0.01 m and 0.1 rad. It is the same as before for the parameters of PRM * and informed RRT * . The search radius r n is also 1.1 for FMT * and IAFMT * . Each algorithm runs ten times to plan the trajectory for the manipulators. We do not set the cost threshold J given in all experiments so as to test the ability of the algorithms to explore the high-quality trajectory.
In the simulation of the right manipulator, as shown in Fig. 7(a), the given runtime t given is 5 s and 50 randomized  boxes are added to the environment as fixed obstacles. We give variable sample counts of FMT * from the order of 1000 to 5500 points and set initial sample counts of IAFMT * to 1000 points. In addition, initial sample count n is set from 1000 to 5000 in order to investigate the influence of variable n on the performance of IAFMT * . The pick-and-place process of the left manipulator consists of four phases, as shown in Fig. 7(b)-(e). The given runtime is set to 5 s in the pick and reset phases, it is 1 s in the hold and place phases, and the total planning time is limited in 12 s. The sample counts for FMT * are given from 1000 to 10 000 points and the initial sample counts for IAFMT * are always 1000 points.
2) Results and Discussion: The algorithms only need to plan one trajectory in the randomized simulated clutter scene, while they try to plan four different trajectories in succession in the real world, which means that the motion planning is failed if one of the four trajectories fails to plan. Experiment results in   Table III show that the planning success rate of informed RRT * is reduced by 40% with the increase of problem difficulty, and IAFMT * performs well in all experiments due to its self-adaptive ability. Besides, the low computational load of IAFMT * leads to high convergence rate even on the low-end computer. It is noted that FMT * shows good performance similar to IAFMT * , but FMT * is not a self-adaptive algorithm as the parameter, i.e., the sample count, is tuned manually according to experience.
The ability of algorithms to explore the high-quality trajectory is presented in Fig. 8(a) and (b). The visual statistical information for path cost shows that the average and median values given by IAFMT * are the smallest than the other algorithms and its data points are denser, which indicates that IAFMT * can stably plan the high-quality trajectories. Fig. 9 presents the performance of every algorithm in each phase of the pick-and-place experiment. All algorithms plan a similar trajectory in the hold and place phases, while IAFMT * performs well in the pick and reset phases due to its outstanding motion planning ability.
Furthermore, we change the parameter of IAFMT * in the simulation to investigate the influence of variable initial sample counts on the performance. The success rate of the motion planning, solved by IAFMT * with initial sample count n = 1000, is 80% and the planning success rate is 100% as n is from 2000 to 5000. As shown in Fig. 10, when n is set to 1000, IAFMT * can fast search for an initial path with a high cost and it iteratively improves the solution to yield a final path with a Fig. 10. Performance of IAFMT * tested by varying initial sample counts in the simulation. low cost until the computational time reaches the given runtime. With the increase of the initial sample counts, there are denser initial samples in the C-space, which leads to the increase in time of getting an initial path and the low cost of the initial path approaching the final-path cost. The experiment results show that IAFMT * with different initial sample counts can stably converge to a high-quality solution, which demonstrates that IAFMT * is highly self-adaptive.

VI. CONCLUSION
In this article, an anytime AO sampling-based algorithm, namely, IAFMT * was presented to solve motion planning problems, especially high-dimensional complex problems. This article also gave the theoretical analysis of probabilistic completeness, asymptotic optimality, and computational complexity on the proposed algorithm. The challenging simulation and experimental results verified that the proposed algorithm can converge to a high-quality solution with an efficient, stable, and self-adaptive performance.
In the future, we will introduce the GPU-based parallel computing technique into IAFMT * to achieve real-time motion planning and it is interesting to evaluate the performance of IAFMT * without knowledge of the geometry of obstacles and environments.