Review Article | Open Access
Survey of Robot 3D Path Planning Algorithms
Robot 3D (three-dimension) path planning targets for finding an optimal and collision-free path in a 3D workspace while taking into account kinematic constraints (including geometric, physical, and temporal constraints). The purpose of path planning, unlike motion planning which must be taken into consideration of dynamics, is to find a kinematically optimal path with the least time as well as model the environment completely. We discuss the fundamentals of these most successful robot 3D path planning algorithms which have been developed in recent years and concentrate on universally applicable algorithms which can be implemented in aerial robots, ground robots, and underwater robots. This paper classifies all the methods into five categories based on their exploring mechanisms and proposes a category, called multifusion based algorithms. For all these algorithms, they are analyzed from a time efficiency and implementable area perspective. Furthermore a comprehensive applicable analysis for each kind of method is presented after considering their merits and weaknesses.
Advances in commercial grade technology and advanced research make it possible for robots to appear in everyday life. Modern robots of different varieties include industrial robots, service robots, have seen bright future. For robots, such as Google Self-Driving Car  and iRobot Vacuum Cleaning robot , one of the most basic and important abilities is path planning, that is, autonomous routing. This requires both time efficiency to react to any emergency situation and safety consideration for implementation.
Path planning targets for moving robots from their initial locations to the goal location by their own actuators and strategies, and during the process, robots must always be able to avoid obstacles to maintain safety. Robots such as underwater robots [3, 4], wall-climbing robot , and micro air vehicles [6–8] have already been tested purposely with different kinds of methods. These methods can be basically perceived from [9–12], where algorithms were already detailed synthesized. These works contributes a lot; however they are analyzed in a general view without comparison and analyzed in specific perspectives as well as covering the latest works. Thus, we need to provide a comprehensive analysis on 3D path planning methods.
By reviewing the latest works, it is not hard to see that most works concentrate only on two dimensions (2D), thus limiting the behaviors of the robots only to surface or each iteration considering the height as a constant to achieve a 2.5-dimensional (2.5D) method. Choset  summarized a lot of his own and others’ works; however he mainly concentrates on 2D environment and therefore algorithms like bioinspired algorithms were paid no attention. LaValle et al.  focused mainly on sampling based algorithms. None of these researchers had analyzed all the algorithms in 3D planning area. Surveys such as  almost all analyze the path planning problems under 2D condition. Reference  only focuses on micro air vehicle’s path planning problems, and algorithms like mathematic optimal algorithms were ignored. Lately, authors in  did a thorough survey on coverage path planning, which is classified by distinguishing decomposition methods. They provided a basic understanding of 3D path planning methods and then narrowed their discussion to sampling based algorithm to explain the 3D planning idea.
Facing the more and more challenging environment that robots faced, where environments tend to be unstructured and full of uncertain factors, 3D path planning algorithms are urgently needed nowadays. Several typical kinds of 3D environments are presented in Figure 1, including forest, urban, and underwater environments. When planning in these complex situations, a simple 2D algorithm will not be qualified; thus 3D path planning algorithms are needed.
(a) Forest 
(b) Urban 
(c) Underwater 
Path planning in 3D environment shows great prospect, but unlike 2D path planning, the difficulties increase exponentially with kinematic constraints. In order to plan a collision-free path through the cluster environment, the problem of how to model the environment while taking the kinematic constraints into consideration needs to be solved. From the optimization point of view, finding a 3D optimal path planning problem is NP-hard; thus there exist no common solutions.
3D path planning algorithms include visibility graph  which works by connecting visible vertexes of polyhedron, random-exploring algorithms such as rapidly exploring random tree , Probabilistic Road Map , optimal search algorithms (such as Dijkstra’s algorithm , , and ), and bioinspired planning algorithms. What must be emphasized is that this paper only pays attention to broadly applicable methods proposed. Algorithms such as manifold based algorithms , which generate smooth path, have been ignored due to reason that it is only applicable to rigid body robots without aerodynamical or hydromechanical influences. Synthesizing all these methods, a two-step procedure of 3D path planning is summarized as follows.
Step 2. Then path planning algorithm is employed to find the best path according to the cost function, with the ability to achieve both time efficiency and cost minimum.
Steps above are not absolute procedures we must obey, and Step can be divided into two parts, or Step integrates with Step . According to Step , this paper discusses the efficiency of each algorithm and particularly concentrates on time complexity and applicability. Path planning in 3D environments may face much more uncertainties; thus all this should be taken into consideration to achieve the best path. Local minimal and global optimal are two contradictive points; this paper will analyze these two properties of the algorithms. This paper mainly answered several important questions: (a) what is the taxonomy of these 3D path planning algorithms? (b) How do these algorithms work in order to find the path? (c) Why is it suitable or not suitable for such environment?
This paper is an extension of , with more comprehensive explanation on each kind of algorithms.
The main contributions of this paper are listed below.
(1) The first contribution is proposing a taxonomy method for 3D path planning algorithms and puts forward a new category called multifusion based algorithms.
(2) The second contribution of this paper is providing a comprehensive analysis of 3D path planning algorithm which contains almost all of the current most successful methods.
The following sections are arranged as follows. Section 2 discusses some controversial points which need to be declared and gives definitions to these problems for further discussion. Section 3 explains the taxonomy of 3D path planning algorithms and gives a detailed analysis of the taxonomy’s reason and also lists elements of each category. This section includes the basic concept of each kind of algorithms. A series of sampling based algorithms are discussed particularly, and each element is put forward by comparison in Section 4. Node based optimal algorithms’ common properties are analyzed in Section 5, and this kind of algorithms shares the same merits. In Section 6, this paper discusses a special kind of planning algorithms, that is, mathematic model based algorithms, and confirms it to be path planning algorithm. Section 7 concentrates on the working mechanism of bioinspired algorithms, and three typical algorithms are analyzed elaborately. Multifusion based algorithms are discussed in Section 8, which are commonly ignored. Some typical researches are listed to prove this category. The last section outlines a conclusion and some directions for further research.
2. Preliminary Materials
This section discusses some controversial or ignored points which needed to be declared for further discussion in the following sections, namely, the difference between path planning and optimal path planning and definition of path planning and trajectory planning.
2.1. Problem Statement
Robots have the ability to operate without (or just a little guiding from) human. Ranging from underwater robots to aerial robots, when facing outdoor or indoor complex situations, they need a path planner to determine their next step movement. For path planning, the definition varies according to [8–10, 57]; this paper presents a more canonical definition based on .
Robots are assumed to operate in a three-dimension () space, sometimes called the workspace . This workspace will often contain obstacles; let be the th obstacle. The free workspace without threat of obstacles is the set of points ; it is the space where the robots should always stay. For robots the initial point is an element of , and the goal region is also an element of . Thus path planning problem is defined by a triplet , and the following definitions are given.
Definition 1 (path planning). Given a function of bounded variation, where and , if there exists a process that can achieve , for all , the process must be a continuous process without break; then is called path planning.
Definition 2 (optimal path planning). Given a path planning problem () and a cost function ( denote the set of all paths), if Definition 1 is fulfilled to find a path and , then is the optimal path and is optimal path planning.
2.2. Path Planning and Trajectory Planning
Path planning and trajectory planning problems are two distinct parts of robotics, but they are intimately related. There exist several works [8, 9, 11] concerned about this problem. Synthesizing all the corresponding knowledge together, this paper supports the following definitions for further discussion.
Definition 3 (path planning (augmentation)). Find a continuous curve (no need to be smooth) in the configuration space that begins from the start node to the goal end node , and the curve should obey the criteria: (a) being without time continuous consideration, (b) including stops in defined position, and (c) being made up of a number of segments, and each can be a trajectory.
Definition 4 (trajectory planning). Trajectory planning usually refers to the problem of taking the solution from a robot path planning algorithm and determining how to move along the path. Trajectory is a set of states that are associated with time; it can be described mathematically as a polynomial , and velocities and accelerations can be computed by taking derivatives with respect to time. It considers the kinodynamic constraints, which can be an element of path.
3. 3D Path Planning Algorithm Taxonomy
Algorithms of 3D path planning have been arising since last century; methods have different characteristics and can be applied to different robots and environments. For researchers and engineers, being stunned to swim in the algorithm sea is a common scene to start in this field. This paper reviews almost all the representative works and books and sorts out almost all the successful 3D path planning methods such as rapidly exploring random trees (RRT), Probabilistic Road Maps (PRM), Artificial Potential Field , and Mixed-Integer Programming . The taxonomy that proposed classifying current approaches of 3D path planning is illustrated in Figure 2.
This paper divides 3D path planning algorithms into five categories; the categories are distinguished from each other by their unique properties, such as sampling based algorithms explored by applying Monte Carlo sampling (or likely approaches) to achieve visible connection. From Sections 4–8 an exhaustive discussion of each category is presented.
3.1. Sampling Based Algorithms: Elements and Analysis
This kind of methods needs some preknown information of the whole workspace, that is, a mathematic representation to describe the workspace. This kind usually samples the environment as a set of nodes, or cells, or in other forms. Then map the environment or just search randomly to achieve a feasible path. The elements of sampling based algorithms are illustrated in Figure 3.
For ETC, it means the improved or similar versions of corresponding algorithms; DDRRT is Dynamic Domain RRT algorithm. There is no doubt that rapidly exploring random trees (RRT) and Probabilistic Road Maps (PRM) are sampling based algorithms. For 3D Voronoi, it forms a 3D obstacle free network based on preknown knowledge of the whole environment, which contains obstacle and free regions. To achieve this network, Voronoi algorithm still works in an optimization way to follow the varying edges to ensure equal distance . Artificial Potential Field algorithms are sorted as sampling based algorithm due to the fact that it needs the whole workspace sampling information to escape from local minima.
It is shown in the second level of Figure 3 that this paper divides the sampling based algorithms into two more detailed parts: active and passive. Active means algorithm such as rapidly exploring random trees which can achieve the best feasible path to the goal all by its own processing procedure. Passive means algorithms such as Probabilistic Road Maps (PRM) only generate a road net from start to the goal, thus a combination of search algorithms to pick up the best feasible path in the net map where many feasible paths exist. This paper classifies the algorithms, which cannot independently find the best path or any single navigation path, as passive.
3.2. Node Based Optimal Algorithms: Elements and Analysis
Node based optimal algorithms explore through the decomposed graph . Analyzing from the point of the search mechanism, node based optimal algorithms share the same property that they explore among a set of nodes (cell) in the map, where information sensing and processing procedures are already executed. This kind of methods can always find an optimal path according to the certain decomposition. Figure 4 illustrates the typical elements of node based optimal algorithms.
Where  represents Lifelong Planning algorithm which is a repeating version of (i.e., having the ability to handle dynamic threats), -Lite  was proposed based on , which deals with dynamic threat situations. Dijkstra’s algorithms, and , are traditionally classified as discrete optimal planning , or road map algorithms , or search algorithms , and so on. However, they do not collide with each other and just express the same thing where algorithms like this kind deal with discrete optimization based on grid decomposition.
3.3. Mathematic Model Based Algorithms: Elements and Analysis
Mathematic model based algorithms include linear algorithms and optimal control. These methods model the environment (kinematic constraints) as well as the system (dynamic) and then bound the cost function with all the kinematic and dynamic constraints bounds which are inequalities or equations to achieve an optimal solution. The elements of mathematic model based algorithms are presented in Figure 5.
Where flatness based method was first proposed by Chamseddine et al. , this method employs differential flatness to ensure control flatness along the reference path; it linearizes the nonlinear kinodynamic constraints to form a rather simple form. MILP is Mixed-Integer Linear Programming  which has a strong modeling capability to describe almost all the information. BIP is Binary Linear Programming ; it is a special case of linear programming where the variables have only 0-1 integer value.
3.4. Bioinspired Algorithms: Elements and Analysis
Bioinspired algorithms originate from mimicking biological behavior to solve problems. This kind of planning methods leaves out the process of constructing complex environment models to search a near optimal path based on stochastic approaches; it overcomes the weakness where general mathematic model based algorithms often fail (or drop into local minimum) in solving NP-hard problem with large number of variables and nonlinear objective functions . Figure 6 presents a set of typical current methods belonging to this category.
Where GA is Genetic Algorithm , it is the most famous population-based numerical optimization method; MA represents Memetic Algorithm ; it is a population-based heuristic exploring approach for combinatorial optimization problems; PSO is particle swarm optimization ; it is a population-based stochastic optimization algorithm; ACO is Ant Colony Optimization  that imitates the behavior of ant in finding the shortest path by using pheromone information; and SFLA is Shuffled Frog Leaping Algorithm  which is the combination of MA and PSO. For bioinspired algorithms, they are divided into Evolutionary Algorithm (EA) and Neural Network (NN) due to the fact that they are analyzed at different level, which will be discussed in detail in the following sections.
3.5. Multifusion Based Algorithms: Analysis
Fusion is a lately arising approach to improving the performance of 3D path planning algorithms; algorithms can benefit each other by this way. Usually, algorithms tend to fuse in a layer by layer way and aim to plan an optimal path (with better real time, or nonlocal optimal performance). For example, Artificial Potential Field algorithms usually tend to drop into local minima without navigation function or other tricks. Probabilistic Road Maps also cannot generate an optimal single path by itself. Thus this paper classifies this kind of algorithms, which are introduced by combining several algorithms together to achieve a better performance, as multifusion based algorithms. Section 8 will give a canonical illustration to this category.
4. Sampling Based Algorithms
In Section 3.1 a list of sampling based 3D path planning algorithms is already illustrated, and this part aims to give a detailed analysis of this kind of algorithms. In order to give a clear image of this kind of algorithms, this paper defines the RRT and its improved versions as RRT series, and PRM series contains PRM and its improved versions. This definition is also applicable to Voronoi and Artificial Potential Field.
4.1. RRT Series
4.1.1. Rapidly Exploring Random Trees
Rapidly exploring random tree (RRT) method is first proposed by LaValle . The method attempted to solve path planning problems under holonomic, nonholonomic, and kinodynamic constraints. RRT has the advantage of handling multi-DOF problems; thus it is widely used for PR2 and other robots . Authors in  proposed a fast local escaping version called Dynamic Domain RRT, which will be analyzed below. Karaman and Frazzoli  solved nonoptimal results problem of RRT by introducing RRG and a heuristic method .
RRT rapidly searches the configuration space to generate a path connecting the start node and the goal node. In each step a new node is sampled; if the extension from the sampled to the nearest node succeeds, a new node will be added. When this kind of method is applied to 3D environment, it normally assumes that there exists a 3D configuration space . The configuration space consists of two parts, a fixed obstacle region, , which must be avoided, and an obstacle free region, , where the robots must stay. Corresponding to the configuration space, a path state (or vertex) set includes all the sampling vertices which are generated by RRT exploration process. In order to implement the algorithm, the following steps should be obeyed (see in Figure 7).
Step 1. First add the initial state in as the first vertex. Then randomly choose a state in , and Figure 7 illustrates two states which are and .
Step 2. Select a nearest state to the newly generated state in based on a certain metric (mostly Euclidean metric) which is already designed; regard as the parent state of .
Step 3. is the state which shows the direction where the next step should go but may be beyond the robot’s reachability. Thus a control input factor is added, considering the kinodynamic constraints, in a cost function form. Then according to the constraints and cost function , we get the reachable state , judging whether it is in . If it locates in , then add it to the path set ; else ignore this state. It is illustrated in Figure 7; we delete and repeat the whole process.
Although RRT can find a path to the goal, it is still the problem that RRT explores based on Monte Carlo random sampling, which is always biasing explored region as it will increase with the time. The method will consume much time to find a way out when the environments are cluttered, let alone converge to the optimal.
4.1.2. Dynamic Domain RRT
Dynamic Domain RRT (DDRRT) aims to solve the shortcoming where RRT considers none of the obstacles region in the configuration space, especially local trap region. RRT does not consider the information of local environments and thus causes inappropriate sampling which may lead to low time efficiency. An intuitive comparison is given in Figure 8, where the red arrow means the guiding to possible next extending direction.
(a) It is hard for general RRT to deal with bug trap problem
(b) DDRRT tackles trap problem by limiting the extension near the margin
DDRRT and RRT differ in Step ; DDRRT introduces a -dimensional sphere (based on the dimension of the environment) of certain radius at the center to represent the reachable region. At each time if the distance of new sampling node to the nearest node is within radius , the sampling node will be chosen; otherwise it will be neglected. Then try to connect and ; if the extension is succeed, then set ; otherwise set , where is the boundary radius of dynamic domain.
DDRRT solves the Voronoi bias problem of general RRT; it can ensure fast exploration. However, it is the same as RRT where no post-smooth-processing is included; the path which is generated by DDRRT will never tend to be optimal.
4.1.3. Rapidly Exploring Random Graph (RRG)
RRT performs well in practice and can guarantee completeness, but it pays almost no attention to the quality of the results, and it is proved that RRT algorithms are not asymptotically optimal . To ensure asymptotic optimality, authors in  introduced rapidly exploring random graph (RRG). The work used a structure to represent the performance of the system, where is the set of states, is the initial states sets, is the transition relation, and is the labeling function which is used to map each state to the set of atomic propositions.
It is illustrated in Figure 9, where “current new node” denotes the new nodes generated at current steps. Unlike RRT, RRG tries to extend to every state returned by (line 7 of Algorithm 2), which is illustrated in Figure 9. In Figure 9(a), is the radius stated to choose the neighbor states around the center of “current new node”; then connects all these nodes to form a graph as illustrated in Figure 9(b) if the connection is collision-free (in Algorithm 2, from line 7 to line 12). The pseudocode is given in Algorithms 1 and 2.
(a) The near nodes returned
(b) connects all near nodes to form RRG
Line 7 shows the main difference between RRT and RRG, where RRG also connects to satisfaction vertices which lie in the circle region; thus it forms a graph. RRG holds the advantage of extending all the vertices returned by the process and then connects them to present a more complex map. Although it seems to be more complex in some way, it almost surely can converge to an optimal path. Reference  showed experimental results of RRG, and the comparison to PRM proves the advantage of RRG. Also, the conclusion suggested that RRG enables navigating multiple robots simultaneously. However, it generates a complex network like PRM and thus cannot find the optimal path by itself.
4.1.4. RRT-Star ()
 is the tree version of RRG which also preserves the asymptotic optimal property as RRG; it is proposed to tackle differential constraints. removes probable bad connection which works in a refining way; thus it optimizes the solutions to be less expensive than they used to be. Here a cost-function is defined to represent the cost of the unique path from to an arbitrary state and gives the equal to zero initially. differs RRG in postprocessing process, compared to Algorithm 2; pseudocode is given in Algorithm 3 with intuitive illustration Figure 10.
(a) Pruning process of
(b) The final graph
Let us take the same example case as illustrated in Figure 9(a). first finds the state as well as the neighbor states and then adds the state to the tree as well as the minimal cost connection if it exists (from line 4 to line 12). Further, tries to eliminate the connections which have a larger cost by connection via state (from line 14 to line 21). Because of the pruning and reconnecting, the tree turns to be more compact and dense as illustrated in Figure 10(b). Consequently the overall minimal cost can be obtained. However, the whole time consumption increases, and also it cannot work to generate multipath. Choudhury et al.  assumed that if the nearest parent already has children nearby; then the second best parent will be chosen. This method solves the problem of general RRT and by supporting an on-line fast replanning method.
4.2. PRM Series
Unlike RRT, Probabilistic Road Map (PRM) considers different choices for the set of states to which connections are attempted. PRM  is the first popular multiple-query method for building a road map by using sampling approach. When applied to 3D space, it first defines the configuration space and an obstacle free space . The method samples a random state in . Then it tries to connect to nearest neighbors (-PRM) or connect to states within a region or connect to states under computational burden which is a combination of the above two, that is, . It should be declared that each connection is collision-free. After the road map is absolutely formed, a node based search algorithm (Dijkstra, , , etc.) is used to find the least cost path from initial state to the goal state. The pseudocode of PRM is illustrated in Algorithm 4.
In Algorithm 4, PRM adds all the connections which are without collision (from line 4 to line 9). Authors in  first implemented PRM into 3D environment and showed fast exploration performance. However, with the expanding of the exploring graph, the expense on collision checking increases. Amato et al.  put it forward by proposing an obstacle-based nodes generation strategy; at each step the road map candidate points are selected on the obstacle surface. This trick reduces the total processing time by ignoring the useless points in obstacle region. Hsu et al.  solved the “dynamic threat weak” problem of PRM by introducing a concept of “milestone,” that is, which creates a real time graph connecting the initial point and the goal point. It was implemented in ground robots and proved of having quick convergence ability. Yan et al.  proposed an octree to build 3D grid-structure which weakens the effect of randomness, thus guaranteeing fast searching ability. The work gave a great thought of random sampling in bounding box array. Reference  introduced to guarantee asymptotical optimality. Authors in  concentrated on the problem that typical PRM cannot tackle dynamic threats; they imported the idea of potential to avoid this, which is called Reactive Deformation Road Maps (RDR).
PRM has the bottleneck of not being able to determine an efficient near vertex choosing and connection principle. Based on the difference of vertex chosen criteria, this paper combines the idea of [10, 57] and divides PRM into three parts in Table 1.
Although has the advantage of adopting enough samples to ensure smoothness by adjusting parameter , the extension will be biased if a fixed direction tends to be much more dense. -PRM can keep out the shortcoming of -PRM, but an unreasonable radius may result in an excessive computational burden. likes an adaptable -PRM; it can guarantee all direction connection and also limit the computational burden to a certain degree.
Shamos and Hoey  introduced the Voronoi diagram into the field of computational geometry; it is firstly used for finite points in the Euclidean plane and now widely used with a series of improved forms in the field of path planning. Voronoi diagram generates topological connection; the distances from the edges to the nearby obstacles are the same.
3D Voronoi first selects an initial site; this site’s coordinates hold the property that the minimal distance to the obstacles nearby is the same. Then it calculates new Voronoi sites based on calculation of the Voronoi channel  and defines the Voronoi net bounds. The whole process stops when all the sites are recorded and all the channels are obtained. The calculation of the Voronoi channel is used to choose a Voronoi site, and the equation of Voronoi channel for the triplet of objects is as follows:where is the minimal distance from a give point (or robot as a mass point with 3D coordinate) to the surface of th obstacle in 3D space. Equation (1) extends the characteristics of general Voronoi, if there exists a small shift along the channel and the point should satisfy the constraint equation,With respect to (1), linearization of this system should obeyThis ensures finding the direction of the displacement ; thus we can get a new point along the vector.
An estimation function (4) is used to improve the computer-based accuracy, that is, controlling the deviation of the trajectory from the Voronoi channel:Let ensure that the point is on the Voronoi channel. If ( is desired value), then it returns to the Voronoi channel by applying gradient decay procedure. However, in order to ensure fast convergence, the preliminary sampling of each state is very important; thus this kind of algorithm is classified as sampling based (even though it introduced information adjustment feedback to correct the initial sampling).
Voronoi generates a global graph or local graph, but almost the same as RRG and PRM; it also cannot generate the shortest path at the same time. Thus it seeks help from Dijkstra’s algorithm, , , and so forth. This paper defines three steps to Voronoi path generation methods: (a) sampling the environment or just seeking help from other environment construction methods, (b) generating a 3D Voronoi graph, and (c) employing a search algorithm to find the minimal cost path globally.
Luchnikov et al.  first proposed an elaborate 3D Voronoi diagram construction method, which solves 3D complex system path planning problem. Reference [35–37] improved it to a further stage by proposing a radial edge like data structure which is capable of dealing with topological characteristics of Euclidean Voronoi diagram of spheres. The topological characteristics are region, face, edge, vertex, oop, and partial edge, which contain the geometric information and adjacency relationship. When implemented in reality, Lifeng and Shuqing  introduced the method that uses geographical information system to generate the environment nodes and combined with Dijkstra’s algorithm to find the shortest path. Sharifi et al.  assigned Voronoi region to a group of UAV to solve the problem of coverage planning for an environment. Voronoi was combined with potential field method in [39, 40] which is efficient to guarantee fast convergence. Voronoi channel is built mainly based on static obstacles; authors in  tackle dynamic threats by adding a bound to the Voronoi channels, and the idea can be introduced to 3D environment.
4.4. Artificial Potential Algorithms
Since first proposed by Khatib , potential field methods have been widely researched and implemented universally because of its low computational complexity. Potential field methods are based on the idea of assigning a potential function (relationship between obstacle free and obstacle space) to free space and simulating the vehicle as a particle reacting to force due to the potential field. It computes the goal attraction and obstacle repulsion simultaneously and guides the robot along the total force gradient. The potential field can be represented aswhere, in formula (5), is the total potential at state , is the attractive potential at state , and denotes the repulsive potential at state . Given , that is, the virtual attractive force (from all neighbor obstacles, also please keep in mind the relation between the gradient of potential field and virtual force can be negative or positive for your consideration), then the force field at state can be represented asFor the attractive potential,where is the goal node, is the error vector with respect to the goal node, and is the attractive force at . Hence, attractive force increases with the distance between current node and goal node. For the repulsive potential,where is the distance from to obstacle region which has been partitioned in convex components, the distance can be defined flexibly, is a polar value, is the th obstacle component repulsive force to , and is the safe margin of th convex component which is defined by certain obstacle. For field implementation, APF tends to be more complicated as the bounder of the obstacles is not constant; thus the time efficiency is constructed based mainly on initial guess; thus we classify it as sampling based algorithm.
However, such methods are incomplete because they are prone to drop into local minima area. Many studies have been done to help potential field algorithms to overcome the local minima by generating navigation functions or computing the potential with constraints. Reference  proposed a harmonic potential method to solve the local minima problem by using Laplace’s Equation to constrain the generation of a potential function; Rimon and Koditschek  proposed a Morse function with a single minimum at the desired destination strategy to form a strong stable robot navigation method to jump out of the local minima, and this is the first formally proposed navigation method. Authors in  imported the idea of Hamilton-Jacobi-Bellman to form a HJB function:where is the control and is the potential factor. This function generates a unique global minimum value; thus it is able to yield a global feasible path that jumps out of local minima, and [45, 46] put it to a higher stage. Ge and Cui  solved the goals nonreachable with obstacle nearby (GNRON) problem by introducing a new repulsive:where is the minimal distance between the robot and the goal . ensures the total potential arrives at its global minimum, that is, , if and only if .
Sampling based algorithms exploring depends much on initial guess, and this guess repeats every step. The algorithms may take advantage of nearby collision detection or potential adjustment. Although this leads to loss of completeness and inexplicit construction of the environment sometimes, this can somehow reduce the dependence on environment model and thus enable them to be implemented in various environments. We summarize all the advantage and weakness of this kind of algorithms and analyze each subcategory in detail as illustrated in Table 2.
Except for algorithms analyzed in Table 2, algorithms such as visibility graphs  and corridor map  also belong to sampling based algorithms. Visibility graph method likes a simple version of obstacle-based PRM which is proposed by Amato et al. . Corridor map method likes octree-structure PRM introduced by Yan et al. . They both defined a cell decomposition method to construct the workspace. However, such as PRM and Voronoi, these methods need a node search algorithm to achieve the best path.
5. Node Based Optimal Algorithms
Node based optimal algorithms are classified by the reason that they deal with nodes’ and arcs’ weight information (but not limited to these, or sometimes called grid); they calculate the cost by exploring through the nodes, thus to find the optimal path. This kind of algorithms is also called network algorithms  which means they search through the generated network, and it means the same with node based algorithms.
5.1. Dijkstra’s Algorithm
Named after Dijkstra , Dijkstra’s algorithm targets for finding the shortest path in a graph where edges’/arcs’ weights are already known. Dijkstra’s algorithm is a special form of dynamic programming and it is also a breath first search method. It finds shortest path which depends purely on local path cost. When applying 3D space, a 3D weighted graph must be built first; then it searches the whole graph to find the minimum cost path. A generalization of Dijkstra’s algorithm is shown below with pseudocode in Algorithm 5.
Authors in  proposed an improved Dijkstra’s algorithm by adding a center constraint; it works well in tubular objects, which is first proposed by . The work  showed that 3D GIS environment combined method may be a practical way to implement in real outside world, and experimental results are provided to prove that Dijkstra’s algorithm acts well enough. But Dijkstra’s algorithm relies much on the priority queue data structure type, which influences the total exploring time.
5.2. A-Star ()
A-star ()  is an extension of Dijkstra’s algorithm, which reduces the total number of states by introducing a heuristic estimation of the cost from the current state to the goal state. The heuristic function can be designed to obtain the constraints, while the estimation must never overestimate the actual cost to get the nearest goal node. By applying this guiding like heuristic, can converge very fast and ensures optimality as well.
is proposed by introducing an evaluation function (13), which consists of postcalculation toward the initial state and heuristic estimation toward the goal,where is the cost from initial state to current state , which is the same as in Algorithm 5. is the heuristic estimation of the cost of an optimal path from current state to goal state. The estimation of each state tends to be close to the real cost; thus has a faster speed to converge based on comparison of the cost of neighbors. Compared with Dijkstra’s algorithm, obtains a faster speed to converge.
For 3D environment, has been widely implemented. Amato et al.  constructed the feasible road map by applying PRM and then adopted to execute best route exploration. Authors in  implemented with UAV platform with an octree based PRM. Niu and Zhuo  introduced “cell” and “region” conception to enhance the environment understanding of , thus enabling flexible representation of 3D environments. Koenig and Likhachev proposed an environment-representation varying adaptive , that is, Lifelong Planning (LPA) . can adapt to environment changes by using previous information as well as iterative replanning. Williams and Ragno  propounded a conflict-direct ; it accelerates the exploring process by eliminating subspaces around each state that are inconsistent. It is proposed in  that can choose any state to be parent state, thus resulting in a more flat turning angle, named . The algorithm has the ability to be able to obtain system constraints; thus it can find shorter and more realistic path. De Filippis et al.  implemented both and in 3D environment, and an experimental comparison is given to prove that reduces the searching compared to . Although acts well compared to , but when applied to 3D environment, it consumes much time to check unexpected neighbors. check method, called lazy , is proposed to avoid unnecessary check of unexpected neighbors. Authors in  introduced a method to reuse information from previous explorations and update information through the affected and relevant portions of the exploring space. This may cause extra computational consumption, but it can tackle dynamical threat and converge fast.
5.3. D-Star ()
D-star () [23, 52], short for dynamic , is famous for its wide use in the DARPA unmanned ground vehicle programs. is a sensor based algorithm that deals with dynamic obstacles by real time changing its edge’s weights to form a temporal map and then moves the robot from its current location to the goal location in the shortest unblocked path.
, As well as , evaluates the cost by considering the postcalculation and forward estimation. maintains a list of states which is used to propagate information about changes of the arcs cost function. The evaluation function is represented as
However, differing from , is not necessarily the shortest path length to the goal compared to ; also computation of assumes that the robot can pass through obstacles. At each time it updates a minimum heuristic function when it encounters new obstacles and the whole graph, thus enabling efficient searching in dynamic environments.
Smirnov  considered the worst travel distance case of and restrained its lower bound as steps and upper bound as . The bound is used to solve the problem that uses unrealistic distance in its graph, and there is a large gap between the lower bounds and upper bounds. Based on , Tovey et al.  put it forward by adding more tighter bounds for . Koenig and Likhachev  extended to the case where the goal changes between replanning episodes; it is like a simplified version of , called Lite, but it is proposed based on .
Table 3 provides a straightforward summarization of node based optimal algorithms. As the name implies, node based optimal algorithms deal with node and arc information. However, as the nodes and arcs provide an incomplete structure of the configuration space, this kind of method can only achieve the best result limited by the representation of environment. This kind of algorithms is single search methods. It cannot generate multipath for fleet. For real time implementation, Dijkstra’s algorithm’s time complexity is ( is the number of the nodes); and reduce the complexity to a lower degree, which enables on-line implementation.
6. Mathematic Model Based Algorithms
Node based optimal algorithms use grids to represent configuration space; meanwhile this kind of methods assumes the robot as a point and only considers the acceleration and velocity constraints. Thus it is not complete to represent the environments as well as consider system dynamics. Mathematic model based algorithms optimize by describing kinodynamic constraints in combination of polynomial forms. They can model the environment as a time variant system (sometimes even a model and time variant system), which is synchronization to the current location of the robot.
Due to the fact that this kind of algorithms can handle dynamics constraints to achieve cost optimum, some researchers regard mathematic optimization methods as trajectory planning method. However, mathematic model based algorithms are proposed to solve the problem of finding a path or trajectory locally or globally [45, 105]. In order to distinguish from trajectory planning methods, we provide a stronger definition compared to Definition 4.
Definition 5. Trajectory planning assumes output to be time continuous and must be able to ensure control limitation, which is still a part of the whole path.
According to Definition 5 above, local mathematic optimization methods also partly belong to path planning field. Authors in  transformed kinodynamic constraints into a bound of linear or nonlinear constraints and then used mathematic programming methods to find the bounded optimal path. Miller et al.  modeled the path planning problem in an optimal control form, which combines cost criterion and Hamiltonian function to form a boundary value problem (BVP) to find a realistic optimal path globally. Chamseddine et al.  introduced flatness based method to linearize the nonlinear bounds into polynomial form, and a bang-bang control is employed to generate a global optimal path. Other mathematic optimization methods such as level set method and support vector machine method also belong to this kind.
6.1. Linear Algorithms
For linear algorithm form based path planning problem, this kind of algorithm tends to have the following form: subject towhere is a cost function that takes into kinodynamic constraints as as well as the expected properties, such as minimum distance, energy, and threat. represents the control factor, is the kinodynamic constraint which acts as penalty function, and is the exploring step reachable region function to ensure a strong convergence ability to the goal. Equation (16) is the initial condition, (17) is the final condition, (18) is algebraic path constraints, and (19) is the control constraints.
Linear algorithms have the ability to describe the environment completely; meanwhile they can model the kinematics and dynamics constraints. Furthermore, linear algorithms can handle control disturbance or model uncertainty. Mixed-Integer Linear Programming (MILP) methods combine binary and integer logical constraints and are most commonly used because they closely represent the environment and system. Reference [3, 5, 106] implemented MILP, respectively, in aerial, underwater, and ground robots. Bhattacharya  directly supported a free MATLAB toolbox, which is called OPTRAGEN, to solve the MILP problem. Masehian and Habibi  used 0-1 binary integer to represent the path length operator and then solved the path planning problem using binary integer programming.
6.2. Optimal Control
Path planning problem can be posed in an optimal control form, where optimal control can find the state and control oriented path based on a set of differential equations . Optimal control can be interpreted as an extension of the linear algorithms to an infinite number of variables’ condition, with the ability to model uncertainty as linear chance constraints much more easier.
A basic optimization problem is depicted in a flowchart form in Figure 11, where the initial state (or current state) and goal state are included in the constraints to ensure completeness.
For optimal control case, a general system model is considered in time continuous form,where represent the evolving state and is the control parameters. The minimizing criterion is With the necessary constraints,
Then cost function (an recommended performance index) can be derived by including all constraints above:
Then we can achieve the Hamiltonian:where (22) is the initial condition and (23) is the final condition. Hamiltonian is used to solve the optimal problem based on the maximum principle and then follow the typical optimal solving procedure to generate an optimal path globally. This paper only explains in a general form; each equation can be extended to contain much more constraints.
Tisdale et al.  implemented discrete receding-horizon control (RHC) in UAV, which is a variant of optimal control. Chen and Schwartz  described the optimal control problem in detail and proposed a free MATLAB toolbox RIOTS_95, and in  they further implemented it to solve model predictive problem.
This kind of methods maintains a complete form to describe the states and environmental variables, and when applied in 3D cluttered environments, these methods can adapt themselves by employing much more constraints according to the environments. These algorithms tend to have a much more complex formulation, that is, a heavy computational burden. There exists a method that can solve the weakness, that is, discrete decisions in an optimization procedure. The method allows problems to be solved on-line flexibly by devising the environment [58–60]. For example, Bellingham et al.  only used MILP to justify the local states and thus increase the whole exploring process a lot. Based on the analysis above, this paper provides a summarization in Table 4.
7. Bioinspired Algorithms
Path planning is attributed to the top layer of a robot control process, which enables robots to work without (or with little) help from man. The motivation of planning a path in real cluttered environments is that the robots should obtain the ability to accomplish the mission by itself without supervision. For bioinspired algorithms, they originate from imitating the way how humans or other natural creatures behave or think, and they form a family of a series of algorithms which can solve NP-hard problems to generate a near optimal path.
There are two subcategories of bioinspired algorithms: one is Evolutionary Algorithm, which stems from analyzing the behavior of a certain species; another is Neural Network algorithm which follows the way how inner neuron processes the information. They belong to different level accordingly, and this paper discusses them, respectively. For Evolutionary Algorithms, they work almost with the same mechanism; thus this paper mainly analyzes two relative popular algorithms: Genetic Algorithm and Ant Colony Optimization Algorithm.
7.1. Evolutionary Algorithm
Evolutionary Algorithm is an umbrella name which includes Genetic Algorithm (GA), Memetic Algorithm (MA), Particle Swarm Optimization (PSO), Ant Colony Optimization (ACO), and Shuffled Frog Leaping Algorithm (SFLA). EA was propounded to solve the problem where traditional linear programming and dynamic programming often fail to solve NP-hard problems with large number of variables. EA is a stochastic search approach that imitates the metaphor of natural biological evolution and social behavior. The firstly proposed and now widely implemented Evolutionary method is GA; then inspired by different natural process four other methods developed.
Figure 12 illustrates a typical flowchart of Evolutionary Algorithm, which is proposed by Dong and Juris . Evolutionary Algorithms start by selecting randomly feasible solutions as the first generation. Then it takes the environment, robot’s capacity (dynamic ability), goal, and other constraints into consideration, to evaluate the fitness of each individual. In the next step, a set of individuals is selected as parents for the next generation according to their fitness. The last step is a mutation and crossover step. The whole process is performed in an iterative way and stops the process when a preset goal is achieved. The best fitness individuals are decoded as the optimal path nodes.
7.1.1. Genetic Algorithms
Holland  firstly introduced GA, and now it is the most popular population-based optimization method. The basic version of GA defines a cost function to evaluate the potential solutions. Then a partly random crossover operator takes two parents from the population set and recombines them in some way. The mutation operator tries to modify the solutions and aims to achieve a valid solution in order to escape local optimality.
GA holds the process that all individuals can exchange information; by doing this way, this nest generation can converge very fast by using this information. However, if the population becomes too similar and loses population diversity, it often leads to premature convergence. If there exists too much population diversity, it may result in a heavy computational burden to investigate the poor solutions. GA needs to repeatedly evaluate the fitness of the current generation, which also causes a high computational burden.
Fonlupt et al.  proposed a cooperating GA to solve the premature convergence problem of typical GA. The method holds the concept that when a certain GA is stuck in local minimum, then another GA might provide a feasible path point to allow it to search again. Hacioglu and Ozkol  and Hacioĝlu and Özkol  solved the problem by introducing an idea, that is, periodically applying a vibrational mutation operator to the whole population. Therefore, it becomes possible to escape from local optimums and then to obtain a global optimal path. To avoid time consumption with a high burden, authors in  proposed a combination of GA and Voronoi method. The Voronoi diagrams are constructed by using fuzzy -means clustering method to generate the first generation, which accelerates the convergence speed. RRT obtains the merits of random exploring, which can enable escaping of local optimum. Authors in  employed RRT to generate the first generation of chromosomes to achieve global optimality.
7.1.2. Ant Colony Optimization
Animals such as ants could manage to establish shortest path from their colony to the feeding source and back home by group cooperation; researchers mimic the behavior and proposed Ant Colony Optimization (ACO) method. ACO introduces two basic concepts, which are “intensity of trail” and “visibility” to form the transition probability which at last decides which way to go, thus to formulate the shortest path.
“Intensity of trail” on edge at time is expressed by the following formula:where represents the information about how many ants in the past have chosen the same edge and is a weight to represent how much information is left between and :where is the sum of all ants’ “pheromone” laid on the edge between time and , is the th ant “pheromone” laid, is a constant, and is the th ant tour length.
“Visibility” on edge can be described aswhere is the Euclidean distance between and and determines the degree of how close the state is to state .
ACO aims to find the best path by evaluating the pheromone density in each step. The process runs flexibly in dynamic environments only needing to change the representation of “intensity of trail” of a certain edge. The algorithm is proved to be able to deal with multiobjective path planning problems, and it is also able to tackle continuous planning problems . ACO is now widely implemented in 3D environment [7, 73] for path planning. However, it must be emphasized that the basic ACO is not applicable to handle vast size of pheromone matrix in practical time with computer memory limitation. Thus it is validated in simulation, but not practical in real time planning situations in most cases.
Authors in  proposed a differential evolution (DE) ACO method by applying differential evolution to optimize the pheromone tail. DE is a simple population-based algorithm; it tends to be more efficiency; thus the work takes advantage of DE crossover operation to achieve faster convergence. Method proposed in  started with relative coordinates, which can avoid rotation transformation for UAV, to reduce time consumption for generating an optimal path. Saber and Alshareef  came up with using to increase the local searching ability and introduced probabilistic nearest neighbor method to estimate the pheromone intensity; it is proved to be effective.
The other three Evolutionary Algorithms, Memetic Algorithm , Particle Swarm Optimization , and Shuffled Frog Leaping Algorithm , all almost share the same exploring process, and their shortcomings and advantages are also almost the same. Although compared to GA and ACO they have a lower degree of implementation, still a lot of works [4, 111, 112] have been done with these methods.
7.2. Neural Network
Another subcategory of bioinspired path planning method is Neural Network (NN). NN was introduced first introduced by Glasius et al.  to avoid obstacle as well as navigation and then became popular and implemented to path planning in various areas [114–116]. NN approach aims to generate a dynamic landscape in a neural-like form. It shares some merits as Artificial Potential Field method; the unsearched areas attract the robot in the entire space globally. A typical shunting equation is used to express the dynamics of a robot in neuron network, which is represented in the following form:where denotes the neuron activity of the th neuron. Parameters , , and are nonnegative constants representing the passive decay rate and the upper and lower bounds of the neural activity, respectively. and are the excitatory and inhibitory inputs. is the external input to the th neuron, and it is defined as if it is a safe unexplored area; if it is an obstacle area; means other cases. Here is a very large positive constant.
For NN, in each step it first chooses the maximal neural activity among the neighbor neurons; then the next location is determined by this maximal neural activity. Equation (29) guarantees that the positive neural activity can propagate to all the free unexplored space with the maximal neural activity, but negative activity only stays locally. Although NN attracted so much attention, it shares the weakness as other bioinspired algorithms that it cannot form canonical rules and model; thus the reliability and time consumption are not guaranteed. Even though Hopfield model is introduced to solve the weakness , this is only applicable to certain problems.
Kassim and Kumar  propounded a new “wave expansion neural network (WENN)” which introduces grid potential for path planning; the method was implemented to 3D workspace and proved to have time efficiency. Recently Kroumov et al. [81, 82] improved the method by combination with potential field methods; this method accelerates the speed of basic NN. But according to , it clearly can be seen that when extended to 3D environments, the number of neighbor neurons will increase to , which means the computation complexity will explode simultaneously. Thus it is applicable to implement real time.
Bioinspired algorithms stem from mimicking the natural behavior; they specify a set of optimizing rules as well as the model of processing the information. Then the whole process executes based on the rules and models by iterative optimization. Thus the final results rely much on the rules and model proposed, and the time consumption cannot be guaranteed. Often if the environment becomes more complex, they need a large computational resource to find a solution to meet the expectation (Table 5).
EA is named as a kind of population-based algorithm; all of its subcategories share almost the same procedures: reproduction, mutation, recombination, and selection. Although this kind of algorithms tends to be time consuming and premature convergence, it can deal with multiobjectives problems and solve NP-hard problems. GA is the most popular Evolutionary Algorithm, and PSO is proved to act much faster than GA, but PSO is sensitive to parameters. PSO can be solved by hybrid approach  but also holds the same shortcoming as GA. ACO can find optimal path without premature convergence, but it is impossible for the basic ACO to deal with real complex environment. SFLA operates the local exploration by using PSO-like method, but it shuffles virtual frogs periodically with ensuring global exploration. The PSO-like method relies strongly on suitable parameters . MA is sometimes called hybrid GA, which differs from GA in that MA shares chunk of the gene rather than a neat crossover in chromosomes. Since MA coupled with a learning procedure to perform local refinements, it still has a more heavy computational burden. NN is not sensitive to environment changes, but it relies too much on its model.
8. Multifusion Based Algorithms
Multifusion based algorithms manage with problems where usually a single approach proposed cannot work to find an optimal path individually. When faced with unknown environments, with whether dynamic threat or static threat, not all traditional single planning approaches can fulfill the task of obtaining cost minimum or fast convergence or computational efficiency. For example, PRM cannot generate an optimal path itself, which is the same as Voronoi. Potential field methods often jump into local minimum; node based optimal algorithms need preknown environmental skeleton information. Mathematic model based algorithms tend to be time consuming and unable to solve NP-hard problem with varying environments; bioinspired algorithms vary their performance with the model diagram and have heavy time complexity. Thus researchers try to introduce a combination of different approaches to form a fast searching and global optimal algorithm.
Yan et al.  used 3D grid to represent the environment and 3D PRM to form a road map in obstacle free space; at last optimal search algorithm combined to achieve an optimal path. Masehian and Amin-Naseri  introduced a visibility graph, Voronoi diagram, and potential field (VVP) integrated algorithm. If the extension of VVP algorithm to 3D space is investigated, it shows effective tradeoff between the shortest and safest path. Schøler et al.  combined visibility graph and Dijkstra’s algorithm (or Geodesics) to find an optimal solution for path planning problem in 3D. Jaishankar and Pralhad  planned outdoor environment path by citing GIS-MCDA approach which first synthesizes a variety of information to generate a “combined gray level image”; then an optimal searching algorithm as , , or EA is used to achieve an optimal path. A hybrid of mathematic model based algorithms with EA  is proposed to solve NP-hard problem, where general EA tend to be premature. A lot of works adopt this idea; this paper gives a pioneering work of coming up with a taxonomy of this kind of methods.
Based on the principle of each algorithm, this paper classifies all of these multifusion based algorithms into two categories: (a) Embedded Multifusion Algorithms (EMA) and (b) Ranked Multifusion Algorithms (RMA). EMA combines several algorithms’ advantages together; these algorithms work in simultaneous model, thus ensuring a tightly coupled approach to achieve better performance. RMA means each algorithm works in separate level, where they work rankly. Table 6 illustrates several typical algorithms of each category.
9. Analysis and Conclusion
This paper analyzes a certain aspect of providing a comprehensive knowledge of 3D path planning, including canonical definition and basic knowledge of each kind of algorithms as well as applicable area. By analyzing all these algorithms which have already proved to be successful, this paper classifies all the approaches into five categories: sampling based algorithms, node based optimal algorithms, mathematic model based algorithms, bioinspired algorithms, and multifusion based algorithms. This paper first lists the elements of each category and then supports a critical discussion of each algorithm. During each discussion process, this paper answers four important questions, respectively: (a) what is the taxonomy of these 3D path planning algorithms? (b) How do these algorithms work in order to find the path? (c) Why is it suitable or not suitable for such environment?
According to the analysis we supported, several conclusions can be drawn as illustrated in Table 7, with the details of time complexity, static (S) or dynamic (D) environmental applicability, and real time applicability.