Abstract
This paper introduces an original 3D path planning approach for Unmanned Aerial Vehicle (UAV) applications. More specifically, the core idea is to generate a smooth and collision-free path with respect to the vehicle dimension. Given a 3D grid representation of the environment, the Generalized Voronoi Graph (GVG) is first approximated using a filtered medial surface (FMS) algorithm on the corresponding navigable space. Based on an efficient pruning criterion, the produced FMS excludes GVG portions corresponding to narrow passages unfitting safe UAV navigation constraints, and thus it defines a set of guaranteed safe trajectories within the environment. Given a set of starting and destination coordinates, an adapted A-star algorithm is then applied to compute the shortest path on the FMS. Finally, an optimization process ensures the smoothness of the final path by fitting a set of 3D Bézier curves to the initial path. For a comparative study, the A-star algorithm is applied directly on the input environment representation and relevant comparative criteria are defined to assert the proposed approach using simulation results.
1. Introduction
This paper deals with the issue of 3D path planning for UAVs in a known environment with stationary obstacles. The problem is to define an efficient method for finding the safest path from a starting point to a target, taking into account the dimension of the UAV.
To deal with this problem, many methods are proposed in the literature. Sampling based algorithms [1], graph based search algorithms [2, 3], mathematic model based algorithms [4, 5], and bioinspired algorithms [6–8] are the fundamental families of path planning algorithms [9]. Most of the path planning solutions were originally developed for 2D problems and then extended to the third dimension, thus increasing the complexity of the approach and its computational cost.
Among the most known sampling based algorithms, we can cite the Rapidly Exploring Random Tree (RRT) [10] and the Probabilistic Road-Maps (PRM) [11] algorithms. These methods are initially designed for 2D path planning problems based on the sampling of the configuration space. The drawback of this kind of algorithms is that they return nonoptimal solutions [12]. To improve the quality of the solution, many variants of these sampling based algorithms have been proposed [12–16]. For aerial vehicles path planning problem, some adapted versions [17–20] are applied. Few methods based on Voronoi diagram have been used to deal with the problem of 3D path planning [21].
The A-star is the most popular graph based search algorithm. Developed by Hart et al. [22], it is based on Dijkstra’s algorithm [23] and has been widely implemented in robotics and video games. The conventional A-star is used to find the shortest (possible) path connecting edges of 8 neighbors in 2D grids and 26 neighbors in 3D grids. However, this algorithm cannot find the path in any neighbor connection angle. This implies a suboptimal solution represented as a path with continuous heading changes. With the aim of overcoming this limit and/or improving results in terms of time computation, memory requirement, and path length, some variants of the A-star algorithm have been developed [24–26]. For 3D applications, we can cite [2, 3, 27, 28]. Even though the solution was improved, the planned path is not unruffled enough and needs more refinements to make it smooth for aerial vehicles tracking.
In this paper, we propose a novel approach based on the skeletonization of the navigable space along with an adapted A-star path planner for aerial vehicles. The environment is represented as a discrete 3D occupancy grid supposedly given (its construction is out of scope here). Based on a pruning parameter, the skeletonization algorithm is able to discard noisy skeleton branches and thus exclude narrow passages with respect to the UAV dimensions. This constraint is of major importance but is rarely considered in the literature.
The key idea is to extract the filtered medial surface of the grid representation according to the dimension of the aerial vehicle. In the next step, the use of the A-star algorithm aims to find the shortest path on the FMS. Since the returned path is not smooth due to the continuous heading changes of the A-star path, a smoothing method is finally applied.
This may appear similar to the idea adopted in [29]. However, in this cited paper, the authors propose a 2D solution for a 3D path planning problem. The method is based on a 2D skeleton represented as a thin line. The 3rd dimension is not considered at any time.
The main contributions can be summarized in three essential points:(i)We introduce a 3D skeletonization algorithm for direct applications in aerial robotics context based on a 2D shape representation algorithm.(ii)In order to eliminate oscillations and tight turns in the generated path, we propose a new safety corridor based smoothing approach using Bézier curves.(iii)We perform a comparative study of the proposed approach by applying the A-star algorithm directly on an inflated occupancy grid and defining a set of quantitative criteria.
The paper is organized as follows. In Section 2, we present the two steps constituting the proposed solution. The first is the extraction of the FMS on the navigable space, and the second is the computation of the shortest path on the FMS. In Section 3, we introduce a new Bézier curve based method to smooth the initial planned path. In Section 4, our solution is compared to the A-star solution applied on the 3D grid representation. We finally conclude the paper and introduce the future work.
2. Path Planning on the Environments GVG
This section is dedicated to the problem of accurate and real-time path planning on the Generalized Voronoi Graph (GVG), i.e., the set of points in the environment at equal distance from their two closest obstacles. Given a 3D occupancy grid delimiting the navigable space from the obstacles , it is approximated here by the medial surface of , filtered with respect to the robot dimension.
2.1. Skeletonization and Filtering of the Navigable Space
Let be a subset of and be its complement. Initially defined by Blum [30], the real medial axis of (also referred to as skeleton) is the set of points at equal distance from their two closest neighbors in . As pointed out in [31], this definition is conceptually equivalent to the GVG of an environment and is thus used here as an accurate approximation. In the specific case of 3D shapes, it generates a set of surfaces and is thus also referred to as medial surface.
For each shape point , let denote the set of its closest points on , referred to as projection of . The corresponding Euclidean distance is denoted as . Accordingly, the skeleton can then be defined by
To efficiently approximate and filter the environment GVG, two important concerns have to be considered. First, the navigable space is expressed in this work in a discrete domain (the occupancy grid ). Applying (1) directly on would fail, since, in most cases, even when a given cell actually belongs to the environment GVG.
Therefore, the definition of the discrete medial axis has to be slightly modified. Although several solutions were proposed over the years, this work is based upon the Integer Medial Axis (IMA) [32]. Let denote the reduced projection of , corresponding to the first element in with respect to a lexical ordering. Given a digital shape and its complement , the discrete skeleton of can then be expressed aswhere is the set of direct neighbors of and is the midpoint of line segment . The first condition measures the (Euclidean) distance between the projections of and to remove skeleton points related to aliasing, while the second condition is used to produce a skeleton as thin as possible, since it only selects the best approximation to the real skeleton. Note that it is theoretically proven [32] that this definition efficiently approximates the medial axis of the corresponding shape in while removing skeleton points induced by discretization.
The second major concern is the question of boundary noise: Skeletons are known to be very sensitive to small shape deformations and tend to produce spurious branches. The IMA efficiently approximates Blum’s real medial axis but is not designed to filter noisy branches. This issue is addressed here using the delta medial axis (DMA) [33], a real-time pruning strategy able to identify and discard spurious skeleton branches in real time. The algorithm was initially designed for 2D shapes but can straightforwardly be extended to higher dimensions.
The core concepts of the algorithm are illustrated in Figure 1.
Given , , let denote the radius of the largest disk disjoint from and centered along :
The pruning criterion is then defined as the maximal value of among its direct neighbors :Note that the second condition is similar to the IMA. It contributes to generating a thin skeleton, and thus a better approximation to the GVG. The DMA is then defined as follows.
Definition 1 (delta medial axis). Let and . The delta medial axis of is the set of points with a maximal delta value of at least :
Intuitively, the DMA discards skeleton branches generated by shape deformations smaller than parameter . Based on the robot dimension, it can thus be tuned to retain only skeleton branches (thus GVG navigable paths) large enough to ensure safe navigation.
Applied on the input environment representation , the GVG is thus finally approximated and filtered for a given UAV size , by computing the corresponding FMS . Examples for different values are depicted in Figure 2.
(a)
(b)
In order to illustrate how the FMS extracted by skeletonization depends on the UAV size, we have considered the same environment representation for two different UAV sizes. As it can be seen in Figure 2, the FMS narrows when increases. This means that there are more candidate paths to reach the destination from an initial position when the UAV size is small.
2.2. Shortest Path Computation
The FMS defines a subspace from the navigable space that guarantees safe paths according to the dimension of the UAV. The A-star algorithm is then used to compute a 3D refined path on the FMS. It allows finding the path with the smallest cost from a set of candidate paths leading to a destination. The A-star is based on a cost function :where is the current node, denotes the moving cost from the starting point (node) to the current node , and denotes the moving cost from the current node to the target point (node) .
Given the coordinates of and , the aim here is to compute the shortest path on the FMS. The cost function is then the Euclidean distance in the 3D space. A tolerable distance has to be defined to consider two points in the FMS connected. In a 3D occupancy grid, the A-star algorithm uses searching restricted to 26-connectivity (giving the closest cells in 3D environment) or in every angle for some variants of the A-star. Here, the search forwards on the FMS based on . For a current node, neighbors are determined according to their distance to the current node. They will be examined if their distance to the current node is less than or equal to .
The generated path is a set of ordered waypoints defined on the FMS from to . We consider the two previous cases of UAV size (20 cm and 40 cm). Figure 3 shows the shortest path found for each case, with the vertical position . One can see that the planned path is suboptimized with oscillations, which requires a postprocessing step to refine it.
(a)
(b)
3. Smoothing the Planned Path
The generated path that the UAV is supposed to track must be elegant and unruffled without oscillations, but the path computed on the FMS is not necessarily smooth. To solve this problem, we propose to add an additional processing step based on the use of 3D Bézier curves.
3.1. Bézier Curves
Bézier curves are parametric curves frequently used in vehicle design, fonts representation, games, and 3D animations. Consider , points. The Bézier curve of degrees associated with these points is the curve defined by with :
are called the control points of the Bézier curve and define the control polygon. coefficients are the Bernstein polynomials of degree given as follows:
In a 3D environment, is the point . This means that the point has as coordinates
Because they are elegant and easy to use and can describe any shape, Bézier curves are appropriate to solve the smoothing problem of a set of ordered data. When it is about a path between two specific points and , one more reason to use Bézier curves is their endpoint interpolation property. This means that the first and last control points (denoted, resp., by and ) are the endpoints of the curve. To form a complex smooth path, cubic Bézier curves can be smoothly connected thanks to the endpoint tangent property. This property means that the cubic Bézier curve is tangent to in and . We will give more details on this point thereafter.
3.2. Bézier Curves Smoothing Strategy
The issue of smoothing of the obtained path is formulated as an optimization problem with constraints. The objective is to compute an approximated path to the real one which allows the UAV to make the least maneuvers. Note that there is a compromise between the feasibility of the path and its optimality. By smoothing, the path optimality in terms of safety and path length may degrade. The idea we propose here is to define a safety corridor around the original path as follows:where are the original data, are values of approximated data computed by interpolation, and is the Euclidean distance to the nearest obstacle in each original path point.
The smoothed path has to be in the safety corridor. For that, we have to determine (by solving the optimization problem) the control points defining the shape of the smoothed path formed by connected patches of Bézier curves. The endpoint interpolation property of Bézier curves ensures that the path starts in and ends in . The objective function to be minimized is the mean of the squared distance between real data and smoothed data. Suppose that there are waypoints in the original data. For smoothed data , we note that with uniform division, so and . The objective function is given by
The original data can be divided into several segments by giving the initial set of break points. Each segment will be approximated by a cubic Bézier curve (patch) by identifying the middle control points. If there are segments (i.e., break points), by solving the problem of optimization (see Figure 4), control points must be determined between and to define Bézier curves. It is important to ensure that the control points joining Bézier curves are not fixed and must be determined as well.
The -continuity between patches of Bézier curve can be translated to constraints of the optimization. Let , , , and denote the control points of the th Bézier curve. th and th cubic Bézier curves are continuous at the joining point if
We note that it is possible to add the constraints of UAV heading in the starting and target points, basing on the endpoint tangent property. But this is not considered in this work.
In the optimization process, we consider the case where no solution is found while respecting the constraint that the smoothed path must not be outside the safety corridor. In this case, we split the segment which leaves the safety corridor in two segments and we add their joining point to the break points list. This gives one more segment (consequently, one more Bézier curve to be defined) by replacing one segment with two new segments. We try again to find a solution of the optimization problem defining an approximated path in the safety corridor. The split step is repeated until this is done. Figure 5 shows the refined path after using the Bézier curves based approach. We can see that the path planned by the proposed method is optimal in terms of safety and smoothness.
(a)
(b)
Table 1 illustrates the results of the gap between original and smoothed paths. Intuitively, this gap can reach its maximum value where the safety corridor width is the maximum. We note that the smoothing method satisfies the condition on the maximum permitted smoothing gap. The smoothing gap mean does not exceed.
4. Results and Discussions
To provide a comparative study of the proposed approach, we apply the A-star directly on the 3D occupancy grid to find the shortest possible path. For that, we supply an inflated occupancy grid (OG) that takes into account the different UAV sizes considered before ( = 20 cm and = 40 cm). The results are shown in Figure 6 compared with the solutions depicted in Figure 5.
To efficiently analyze the results, we define a set of quantitative criteria for the comparative study. The comparison results are shown in Table 2.
The input data (see Table 2) of the search algorithm depends on the type of the approach applied and the UAV dimension. When directly using the A-star on the occupancy grid (without skeletonization), the input data is the navigable space in the inflated occupancy grid. Using the proposed approach, it is the FMS of the navigable space in the original grid. The observations drawn from Table 2 are as follows:(i)The proposed approach offers a major decrease in the volume of the input data in both cases (90.15% and 87.21% of decrease, resp.) for an increase of, respectively, 10.18% and 2.97% in the path length. The reduction in the research space is thanks to the skeletonization step. This is of great benefit when addressing real-time applications.(ii)For all cases, the smallest distance to obstacles is greater than the UAV size, which confirms that all planned paths are collision-free. The proposed approach gives more distance to obstacles supplying better path in terms of safety than the A-star algorithm applied directly on the OG. The evolution of the distance to obstacles along the path that the UAV is supposed to track is shown in Figure 7.
(a) Case 1
(b) Case 2
We perceive that the planned path provided by the proposed approach is not the optimum with regard to its length. However, it is more optimum with regard to the safety criterion than the path generated using the A-star directly on the occupancy grid.
The vehicle dynamics have been detailed in [34]. In this previous work, we have presented a generalized dynamical model of miniature multirotor vehicles. These UAVs are known for their high maneuverability. A control approach has been proposed and applied to track (user-designed) trajectories for an example of quadrotor, hexarotor, and octorotor UAVs.
As the smoothing step is performed to compute an approximate path requiring the least maneuvers of UAV without affecting its safety, we define two new quantitative parameters to better evaluate the proposed solution:(i): maximal change value of the altitude.(ii): maximal change value of the heading angle.
For the two UAV sizes considered, the following numbers are obtained:(i)In case 1 ( = 20 cm): cm, deg.(ii)In case 2 ( = 40 cm): cm, deg.
These parameter values confirm that there are no aggressive changes in altitude and heading angle between two waypoints in the obtained path.
We note that, for all simulated cases, we consider the particular case where starting and destination points are on the FMS. This is not necessarily the case. If one of the initial and target points or both of them are not on the FMS, the idea is to reach (from a starting point) the closest point on the FMS and/or give up the FMS at the closest point to the destination. These additional portions at the beginning and/or the end of the path can be generated using 3D Bézier curves. An optimization process can be applied to guarantee the smoothness of the path at the connecting points.
5. Conclusion and Future Works
In this paper, we present an efficient solution for 3D path planning problem in aerial robotics. It is based on the extraction of the FMS by a 3D skeletonization algorithm. Including a pruning parameter , the solution is given based on the UAV dimension discarding small branches. In the second step, we used the A-star algorithm to find the shortest path on the FMS. For smoothing, we proposed a method based on the use of Bézier curves.
In order to evaluate the performance of our approach, we compared the results to the A-star algorithm applied directly on the 3D occupancy grid. The quantitative criteria of comparison reveal that the proposed approach meets interesting performance criteria. Reducing interestingly the dimensionality of the searching space (by more than 87.21%) reduces memory requirement and is beneficial in terms of computational cost. It is also proved that this approach attaches greater importance to the path safety compared to the path length, so it provides the shortest safest path.
Currently, the proposed approach uses the conventional A-star for the planning of the shortest path. An improved version of the A-star is obviously required especially when the ratio UAV size/environment dimensionality is smaller. In future works, we intend to consider curvature constraints of the planned path in the cost function. Furthermore, we propose dealing with the dynamic constraints of the UAV in order to find feasible trajectories within its maneuverability ability.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.