1 Introduction

Recently, unmanned aerial vehicles (UAV) are being increasingly used in many fields to perform tasks which are too difficult or dangerous for human beings, such as fire fighting[1], disaster relief[2], search and rescue[3], and target tracking[4], as well as space-oriented applications[5]. For these applications, the UAV should have the ability to plan a non-collision path in its work space. The central problem of path planning is to generate a collision-free path from an initial point to a target point in an environment with obstacles and optimize the path according to some criterions such as energy, time, and safety[6].

Planning algorithms have been developed in last few decades. In [7], the wave front algorithm is used as the path planner on three-dimensional normal distributions transform (3D-NDT) maps. This algorithm is intuitive, easy to implement, but it will fail to find the optimal path in complex environments. On the other hand, A* (see [8–10]) are best-first heuristic search algorithms that find the least-cost path from a given start node to goal node. Unfortunately, as the work space becomes larger and larger, the time spent to find the optimal path increases exponentially[5]. Roadmap is usually applied to assist these heuristic search algorithms, which is often represented on configuration space. Various roadmap approaches have been used such as visibility lines[11], virtual force[9], rapidly-exploring random trees[12, 13] and probabilistic roadmap method (PRM)[14].

For UAV, the path planning will be carried out in 3D environments, which is a big challenge especially in complex environments. The method often used for high-dimensional configuration space is based on sampling-based approaches, including the PRM[15,16] and its variants. PRM has two phases: a learning phase and a query phase[15]. During the learning phase, a data structure called the roadmap is constructed for a given scene. The roadmap is an undirected graph R = (N, E). The N is a set of nodes, which are randomly sampled points from collision-free configuration space. Pairs of promising nodes are chosen in the graph and a local planner is used to try to connect such placements with an edge. If they can be connected, then the edge is added to E. A good graph R should cover the free space well. In the query phase, the path is usually found by a path query such as Dijkstra or A*.

The bottleneck of basic PRM is that it has trouble in finding paths through narrow passages. This is due to the random sampling nodes in the free space so that too few nodes are placed in narrow passages. One solution is to add configurations near obstacle boundaries[17, 18]. Though this approach increases the probability of sampling nodes in narrow passages, many nodes outside the narrow passages do not help in improving the connectivity of roadmaps. Hsu et al.[19, 20] proposed a hybrid strategy: The bridge test is applied in narrow passages to increase the sampling density and it also uses uniform sampling strategy in open free space. In [21], the author improved lazy probabilistic path planning by using a lazy significant edge algorithm to place new samples in areas that have not yet been navigated. In these methods, the obstacles of the environments are presented by geometrical models. But in practice, the practical experimental environment cannot be described by geometrical models. For complex 3D scene, the most popular representation is the 3D point cloud[22-27] or voxels[28, 29]. When using basic PRM or variants mentioned above in these 3D representations, the collision checking is a heavy computational burden because of the huge number of obstacles (points or voxels). To deal with this problem, cell decomposition is a good choice which can divide the free space into subspaces. This method is used by Berg and Overmars[30] to separate large open regions for steering the sampling toward the most interesting regions of the scene. Also, Zhang et al.[31] proposed a hybrid approach for complete motion planning that combines approximate cell decomposition with PRM. But in these methods, the collision checking for obtaining a collision-free local path for the roadmap construction is also a big problem in complex 3D environments.

In this paper, we propose an algorithm based on PRM method for UAV path planning in 3D complex environments. The environment can be represented by 3D point cloud captured by laser scanner. However, the raw data from laser scanner is prohibitively huge for any path planning algorithms to be implemented in real time. So we use octree algorithm to divide the work space into voxels, which is a 3D grid representation of the environment. In order to extract free space for UAV navigation, we record the center point and size of free voxels during octree generation process. The size of the smallest free voxel for path planning is set to be larger than the physical size of UAV to ensure that it can pass through the path safely. The final path is a sequence of connected free voxels. In order to deal with the narrow-passage problem, the environment is equally divided into several sub-regions called bounding boxes. Different from our previous paper[32], the nodes are sampled randomly from free voxels in each bounding box according to the volume ratio of the free and occupied voxels. The connectivity evaluation of free voxels is carried out in each bounding box. Then the local planner will search collision-free paths for the nodes in the bounding box. In order to make the roadmap cover the whole free space, an additional bounding box is added to overlap the neighboring bounding boxes for the connectivity evaluation of the local roadmaps in the adjacent region. Finally, we use A* method to search path from the start voxel to the goal voxel in the roadmap. We have implemented this algorithm in practical outdoor environments, which are represented by 3D point cloud.

The remainder of the paper is organized as follows: Section 2 introduces representation for complex 3D environment. Section 3 describes the proposed method for realtime path planning. The results of experiments are presented in Section 4. The conclusions and future works are given in Section 5.

2 Environment representation

3D-grid representation provides the most information, which will be applied for 3D path planning in this paper. For UAV path planning, the free space of the environment is the essential information. So the focus of our method is to find free space in the environment.

For speeding up the process of building 3D-grid structure, octree algorithm is used. Octree is a hierarchical data structure for spatial subdivision in 3D[33]. Each node in an octree represents the space contained in a cubic volume, usually called a voxel. This volume is recursively subdivided into eight sub-volumes until the sizes of voxels satisfy the expected resolution. In its most basic form, octree can be used to model a Boolean property[29]: Occupied or free. Without loss of generality, the sub-volumes, which contain 3D data, are applied to represent the environment as occupied voxels. We focus on the free voxels, which are also built during the octree generation process. As shown in Fig. 1, the occupied voxels will be sub-divided in the next step. We record positions and sizes of the free voxels in each step. In Fig. 1, the light voxels are free ones and the dark voxels are occupied. The free ones represent the free space in the environment. In order to describe the environment accurately, the researchers usually divide the scenes into high resolution voxels. Ryde mapped an office environment in 3D at 0.02m resolution[28]. In [29], the voxel size for maps is 0.04 m or 0.05 m. In this way, there are two problems: First, it suffers from significant computational burdens in detecting obstacles or planning path in complex environments. Second, the small free voxels cannot ensure the safety when used for UAV path planning (the size of our UAV is about 0.4 m). In our octree method, if the sub-volumes of one voxel are not large enough for UAV to pass through, this voxel will never be subdivided. So the resolution is dependent on the physical size of UAVs. Because the free voxels are extracted from different level of the octree algorithm, their sizes are not uniform.

Fig. 1
figure 1

The occupied voxels and free voxels are extracted from 3D data during octree building

Full size image

3 3D PRM algorithm

Our path planning strategy is first to sample nodes effectively according to the volume ratio of free and occupied voxels in sub-regions called bounding boxes. After the first phase, local planner is carried out in each sub-region to build the roadmap. Finally, the search approach is used to obtain a path from the roadmap.

3.1 Node selection

The node selection is the basic but important process of PRM. In basic algorithm, the random sampling strategy is applied for choosing nodes in the entire free space, which causes low success rate of sampling nodes in narrow passage region.

In order to deal with the narrow-passage problem, we divide the environment space into equal-sized sub-regions called bounding boxes, whose sizes are m times the resolution of free voxels. So the maximum number of voxels in a bounding box is n voxel = m 3 (m^2). There are three kinds of bounding boxes: empty box, full box and mixed box. In full boxes, there are only occupied voxels so that no voxel can be selected for roadmap construction. On the contrary, the voxels belonged to empty boxes are all free voxels, which can ensure the safety of UAV path planning. We can randomly choose any voxels as nodes of the roadmap. Though more nodes can make the roadmap cover the free space better, the computational burden is also increased in this situation. How to select nodes in empty boxes will be discussed in next section. Without loss of generality, the mixed boxes are the boundaries of obstacles that mean the narrow passages are also included in this kind of bounding boxes. In these boxes, we will increase the density of random nodes according to the ratio of occupied and free voxels. Because the sizes of free voxels are not uniform in our algorithm, the number is proportional to the volume ratio of the voxels. Then the number of selected nodes in one box is

$${n_{{\text{node}}}} = k.\frac{{\sum {v_{{\text{occupied}}}}}}{{\sum _{{\text{free}}}^{{v^i}}}}$$
(1)

where v occupied is the volume of one occupied voxel and v i free is the volume of the i-th free voxel, and k is a preset coefficient.

Fig. 2 shows the strategy to compute the number of selected nodes. There are 10 values for coefficient k, which are from 0.1n voxel to n voxel. If k = n voxel, the curve of number of selected nodes is so steep that the computational burden increases fast. So in application, we set k to 0.1n voxel. Then as shown in Fig. 2, when the occupied voxels is less than 0.625n voxel, we will choose quarter of free voxels as nodes. If the number of occupied voxels is bigger than 0.73n voxel, all the free voxels are selected. In the other situations, node selection is carried out according to (1).

Fig. 2
figure 2

The strategy of node selection

Full size image

We compare our strategy with the random sampling method. Because the voxels usually block each other in 3D scene, we test the two methods in a 2D simulation environment in order to illustrate the difference clearly. The environment is composed of two big free regions and one narrow passage as shown in Fig. 3. We divided the free space into 2D grids by quadtree method. There are total 329 free grids, and 55 of them are in the passage. The size of bounding box is 4 times of the resolution of free grids. Then according to our strategy, 109 nodes are selected in this environment. For comparison purpose, we select 109, 155 and 220 nodes in the same environment in the random sampling method. Then 100 independent runs are carried out for each situation respectively. We take down the number of selected grids in the narrow passage and the results are shown in Fig.4. Each box shows the number between the minimum and maximum. The square is the average value. The text above the box shows the success rate of constructing roadmap to connect the two big free spaces using our local planner (see Section 3.2). When 220 free grids in random sampling method are chosen, the result is similar to our strategy.

Fig. 3
figure 3

Two examples for comparing our strategy for node selection with uniform sampling method. (a) 109 nodes are selected according to our strategy; (b) The same number of nodes is randomly sampled in the same environment

Full size image

Two examples with 109 nodes are shown in Fig. 3. In Fig. 3 (b), there are no nodes in the middle of passage so that the roadmap cannot connect the two big free spaces. Furthermore, most of the random nodes are the smallest free grids, because the number of smallest free grids is much bigger than the large ones. These smallest nodes near obstacle boundaries do not help in improving the connectivity of roadmap[19]. Comparing with the random sampling method, our strategy is much better (Fig.3 (a)).

Fig. 4
figure 4

The number of selected grids in narrow passage

Full size image

3.2 Local planner

In some PRM methods, the local planners select a set of neighbors for each new node that is added to the roadmap. In this paper, bounding boxes have divided the work space into several sub-regions. So unlike other PRM methods, we do not need to search the k-nearest neighbors but consider all the selected nodes in the same bounding box as the neighbors. In the bounding box, if the number of nodes is small, some nodes are not connected by using straight-line local planner. This will affect the roadmap construction. In Fig. 5, there are three nodes in the bounding box. When using a straight-line local planner, the edges between every two nodes collide with the obstacle. To deal with this problem, we propose a simple method as local planner, which can search a collision-free path for two nodes.

Fig. 5
figure 5

Local planner in a bounding box

Full size image

As mentioned in previous section, the size of free voxels extracted by the octree algorithm is larger than the physical size of the UAV to ensure that it can pass through safely. In order to create a non-collision path, the free voxels must be connected one by one. So the connectivity between voxels must be evaluated. In this paper, two voxels who share a same surface between them are considered as neighbor voxels. And if the positions of two voxels satisfy

$$\left\{ {\begin{array}{*{20}{l}} {|{x_a} - {x_b} \leqslant \frac{{\left( {{S_a} + {S_b}} \right)}}{2}} \\ {|{y_a} - {y_b} \leqslant \frac{{\left( {{S_a} + {S_b}} \right)}}{2}} \\ {|{z_a} - {x_b} \leqslant \frac{{\left( {{S_a} + {S_b}} \right)}}{2}} \end{array}} \right.$$

where S a and S b are sizes of the two voxels, the voxels are connected in x axis as shown in Fig. 6. In this way, the shared area of two voxels can ensure the UAVs to pass through safely.

Fig. 6
figure 6

The connectivity of two free voxels is evaluated by their position coordinates

Full size image

Based on the connectivity evaluation, we search a local path between the selected nodes by using A* method as shown in Fig. 5. All the local paths are composed of connected free voxels, which can ensure the safety of UAV. But only having the local paths in the bounding box is not enough for roadmap construction. To plan a complete path from start position to goal position, the connection status of local path in different neighbor bounding boxes is necessary. This problem will be solved by inserting another bounding box between the neighbor ones. As shown in Fig. 7, A and B are two neighbor bounding boxes, a and b are two free voxels belonging to the different boxes, respectively. So after the process of evaluation mentioned above, there is no connectivity between voxel a and b, because they are evaluated in different bounding boxes. But when there is another bounding box C inserted between A and B, the connectivity of a and b can be evaluated in C.

Fig. 7
figure 7

Another bounding box C inserted between two neighbor ones (A and B) is used to evaluate connectivity of two free voxels (a and b) in different bounding boxes

Full size image

The first condition for connecting the local paths in box A and B is that nodes a and b are belonged to box C. Because half part of A is in box C, when selecting a node randomly in box A, the probability of the node belonging to C is½. Suppose A is an empty box and ω • n voxel nodes are selected in box A. In order to ensure at least one node is placed in box C, the probability need to satisfy following condition

$$\frac{{\omega \cdot {n_{{\text{voxel}}}}}}{2} = \frac{{\omega \cdot {m^3}}}{2} \geqslant 1$$
(2)

We can get \(\omega \geqslant \frac{2}{{{m^3}}}\). Considering the computational burden, we select \(\omega = \frac{2}{{{m^3}}}\) nodes in empty box. In this way, two nodes in one box can suffice for connecting the local path. But it is not reliable for roadmap construction. So in practice, we set ω to \(\frac{2}{{{2^3}}}\), which means one in four free voxels are selected in empty bounding box. This strategy is also used for mixed boxes (Fig. 2).

3.3 Query phase

Suppose the roadmap is built and the start and goal free voxels are given. A* method is used to assist PRM for path planning during the query phase. A* is widely used for finding path. Here, we use A* to search the shortest path from the start node to the goal node in the roadmap. In this process, we consider the straight-line distance and the number of free voxels connecting the two nodes in the roadmap as the searching cost. The specific steps are as follows:

  1. 1.

    \(D\left[ {{N_i}} \right] = {\text{Arcs}}\left[ {{N_s},{N_i}} \right],\left( {i = 1, \cdots n} \right)//{N_s}\,{\text{is}}\,{\text{the}}\,{\text{start}}\,{\text{node}}.\)

  2. 2.

    \(P\left[ {{N_i}} \right] = \left\{ {\begin{array}{*{20}{l}} {{N_i},{\text{Arcs}}\left[ {{N_s},{N_i}} \right] = \infty } \\ {{N_s},{\text{Arcs}}\left[ {{N_s},{N_i}} \right] = \infty } \end{array}//P\,{\text{stores}}\,{\text{the}}\,{\text{previous}}\,{\text{node}}\,{\text{of}}\,{N_i}\,{\text{in}}\,{\text{path}}.} \right.\)

  3. 3.

    \(W\left[ {{N_i}} \right] = D\left[ {{N_i}} \right] + H\left( {{N_i},{N_g}} \right)//{N_g}\,{\text{is}}\,{\text{the}}\,{\text{goal}}\,{\text{node}}.\,//H\left( {{N_i},{N_g}} \right)\,{\text{is}}\,{\text{the}}\,{\text{heuristic}}\,{\text{function}}.\)

  4. 4.

    Find N k that satisfies W[N i ] = min(W)

  5. 5.

    S.push_back(N k )

  6. 6.

    While N k ! = N g

  7. 7.

    For j=1 to n

  8. 8.

    If D[Nj] > D[N k ]+Arcs[N k , N j ]

  9. 9.

    D[Nj] = D[N k ]+Arcs[N k ,Nj];

  10. 10.

    W[Nj] = D[N j ]+H(Nj,Ng);

  11. 11.

    P[Nj] = N k ;

  12. 12.

    End

  13. 13.

    End

  14. 14.

    Find N k that satisfies W[Ni] = min(W)

  15. 15.

    S.push_back(N k ) //S stores the nodes in the path

  16. 16.

    End

Arcs[N i , N j ] describes the connection between node Ni and N j . The value is

$${\text{Arce[}}{N_i},{N_j}] = \left[ {\begin{array}{*{20}{l}} {\infty \quad \overline {{N_l}{N_j}} \in E,} \\ {D\left( {{N_i},{N_j}} \right) + N\left( {{N_i},{N_j}} \right),\quad \overline {{N_l}{N_j}} \notin E} \end{array}} \right]$$
(3)

where D(N i , N j ) describes the distance between N i and N j , and N(N i , N j ) is the number of free voxels connecting N i and N j based on voxel connectivity evaluation by local planner. By using A* searching algorithm, the path is stored in vector P by retrieval from goal node.

4 Experiment results

This section is composed of two parts. First, we implement our method and test its performance with different sizes of bounding box in outdoor environments. According to the results, we will determine the value of m. In the second part, a larger and more complex environment will be used to test the validity of the method. The raw data of all the environments is 3D point cloud. For convenience, the data is collected by a 3D laser range finder. The 3D laser range finder is composed of a 2D laser scanner (LMS 291) and a platform with tilting mechanism. Although, the data is not obtained by UAV, it is very similar to the data collected by UAV with laser range finder in [27]. The size of our UAV is 0.4 m. During the octree generation process, the resolution of occupied voxel is set to be 0.2m and the resolution of free voxel is 0.5 m. The computations are all performed on a PC with Intel(R) Core(TM) i5-2520M CPU and 4GB memory, which is an off-board computer.

4.1 Experiments for m selection

The experiments are carried out in three outdoor environments as shown in Fig. 8. The first environment is simple, which has a path in the middle and two rows of bushes on each side of the path (Fig. 8(a)). In the second one, the sparse trees make the environment more challenging for UAV path planning (Fig. 8(b)). The last environment is more complex, which is composed of dense trees and bushes (Fig.8 (c)). Some parameters for the environments can be found in Table 1.

Fig. 8
figure 8

Path planning in three outdoor environments. (a) – (c) Three outdoor environments; (d) – (f) The 3D point clouds for the three environments; (g) – (i) The time statistics of each step for different m; (j)–(l) The results of path planning

Full size image
Table 1 Parameters for the experimental environments
Full size table

The size of bounding box is decided by the value of m. If m is set to 4, the size of bounding box is 4×0.5 = 2 m. We run path planning with different values of m (from 2 to 10) in the three environments. For each value, the method is carried out 100 times. The success rates of finding a path from the start voxel to the goal voxel are depicted in Fig. 9. And the average time for each step is shown in Fig.8 (g)– (i). The numbers above the bars are the numbers of selected free voxels. The figures in the last row show the results of path planning (Fig. 8 (j)–(l)).

Fig. 9
figure 9

The success rates for the different values of m in the three environments

Full size image

When m = 2, the size of bounding box is so small that a few free voxels are selected in one bounding box. As mentioned in Section 3.2, if the number of nodes is small in bounding box, it is not reliable for roadmap construction. So the success rates are not 100 % for all the three environments. On the other hand, when the value of m is bigger, the rates are much lower. This is because the nodes in the roadmap are too less (see the number above the bars in Fig. 8 (g)–(i)). In fact, the n node obtained in Fig.2 is a multiple of the volume of smallest free voxels. When using octree method to divide the free space, the volumes of the free voxels are not consistent. So the number of nodes is usually smaller than n node. The bigger the value of m, the more possible that the bounding box includes big free voxels. So the number of nodes is less. In Fig.9, when m is set to 3 and 4, the success rates are 100 %. But the total time for path planning is different in Fig. 8 (g) – (i). In summary, the m should be set to 4.

4.2 Larger environment

Moreover, we also test the proposed method in a bigger outdoor environment. As shown in Fig. 10 (a), this is a wooded environment, which is composed of results of 10 laser scans. The size of the scene is 47 m×32 m×10m, and the number of the raw laser points is 444 030. In this experiment, the free space is divided into 8169 free voxels. When m is set to 4, we choose 1 194 free voxels as nodes to build the roadmap. The experiment is repeated 100 times. The final result of path planning is shown in Fig. 10(b) and Fig. 10 (c). The experiment indicates that the proposed algorithm can find a safe path for UAV in complex environment. The average computation time in each step is shown in Table 2.

Fig. 10
figure 10

Path planning in a large wooded environment. (a) A complex outdoor scene is composed of 3D points; (b) An overhead view of the path planning result; (c) A side view of the path

Full size image
Table 2 The average computation time in each step
Full size table

5 Conclusions

In this paper, we propose a 3D path planning method for UAV navigation in complex environments. In this study, we use octree algorithm to divide the work space into voxels that is a 3D grid representation of the environment. The free voxels, which have enough space tolerance to satisfy the safety needs of the UAV, are sorted to represent the free space of the environment. In order to obtain the connectivity of free voxels effectively, the environment is divided into several regions called bounding boxes, and the evaluation of free voxels connectivity is carried out in each bounding box instead of in the whole space. The basic PRM uses random sampling strategy to choose nodes in the entire free space, which causes the narrow-passage problem that may lead to failure of the path planning algorithm. We improve the PRM by random sampling in bounding boxes to ensure a more reasonable distribution in the 3D space. Finally, based on the voxel connectivity, the selected nodes compose a roadmap, which is applied for path searching by A* algorithm for feasible path. The experiments of path planning are carried out in real outdoor environments. The result shows the algorithm can create a non-collision path for UAV.