# Filtered Medial Surface Based Approach for 3D Collision-Free Path Planning Problem

Authors: Karima Benzaid; Romain Marie; Noura Mansouri; Ouiddad Labbani-Igbida
Journal: Journal of Robotics
 (2018)

---

## 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 adaptedA-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, theA-star algorithm is applied directly on the input environment representation and relevant comparative criteria are defined to assert the proposed approach using simulation results.


---

## Body

## 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].TheA-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 conventionalA-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 theA-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 adaptedA-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 thefiltered medial surface of the grid representation according to the dimension of the aerial vehicle. In the next step, the use of theA-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 theA-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 theA-star algorithm directly on an inflated occupancy grid and defining a set of quantitative criteria.The paper is organized as follows. In Section2, 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 theA-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 gridF∪O delimiting the navigable space F from the obstacles O, it is approximated here by the medial surface of F, filtered with respect to the robot dimension.


### 2.1. Skeletonization and Filtering of the Navigable Space

LetXR be a subset of Rn and XRc be its complement. Initially defined by Blum [30], the real medial axis SR of XR (also referred to as skeleton) is the set of points x∈XR at equal distance from their two closest neighbors in XRc. 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 pointx∈XR, let Px=argminy∈XRcd(x,y) denote the set of its closest points on XRc, referred to as projection of x. The corresponding Euclidean distance is denoted as Dx=d(x,Px). Accordingly, the skeleton SR(XR) can then be defined by(1)SRXR=x∈XR,Px≥2.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 gridF∪O). Applying (1) directly on F would fail, since, in most cases, |Px|=1 even when a given cell x 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 Px∗ denote the reduced projection of x, corresponding to the first element in Px with respect to a lexical ordering. Given a digital shape XZ⊂Zn and its complement XZc, the discrete skeleton SZ of XZ can then be expressed as(2)SZXZ=x∈XZ,∃y∈Nx∣dPx∗,Py∗>1,dm,Px∗≥dm,Py∗,where Nx={y∈XZ,d(x,y)=1} is the set of direct neighbors of x and m is the midpoint of line segment [x,y]. The first condition measures the (Euclidean) distance between the projections of x and y 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 Rn 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 Figure1.Figure 1
Illustrative representation of the concepts involved in DMA computation.Givenx∈XZ, y∈Nx, let rx,y denote the radius of the largest disk disjoint from Xc and centered along [Px,Py]:(3)rx,y=sup⁡Dz∣z∈Px,Py.The pruning criterionδ∗(x) is then defined as the maximal value of rx,y among its direct neighbors y∈Nx:(4)δ∗x=sup⁡rx,y∣y∈Nx,dm,Px≥dm,Py.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δ∈R+∗ and XZ⊂Zn. The delta medial axis MAδXZ of XZ is the set of points x∈XZ with a maximal delta value δ∗x of at least δ:(5)MAδXZ=x∈XZ∣δMx≥δ.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 representationF∪O, the GVG is thus finally approximated and filtered for a given UAV size δ, by computing the corresponding FMS MAδ(F). Examples for different δ values are depicted in Figure 2.Figure 2
Two FMS of the environment based on different UAV sizes ((a)δ = 20 cm, (b) δ = 40 cm).
(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 Figure2, 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. TheA-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. TheA-star is based on a cost function F(n):(6)Fn=Gn+Hn,where n is the current node, G(n) denotes the moving cost from thestarting point (node)PS to the current node n, and H(n) denotes the moving cost from the current node n to thetarget point (node)PT.Given the coordinates ofPS and PT, 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 Dt has to be defined to consider two points in the FMS connected. In a 3D occupancy grid, theA-star algorithm uses searching restricted to 26-connectivity (giving the closest cells in 3D environment) or in every angle for some variants of theA-star. Here, the search forwards on the FMS based on Dt. 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 Dt.The generated path is a set of ordered waypoints defined on the FMS fromPS to PT. 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 z. One can see that the planned path is suboptimized with oscillations, which requires a postprocessing step to refine it.Figure 3
Shortest path computed on the FMS based on different UAV sizes (20 cm (a), 40 cm (b)) with the altitude (right column) plotted for each case.
(a) (b)


## 2.1. Skeletonization and Filtering of the Navigable Space

LetXR be a subset of Rn and XRc be its complement. Initially defined by Blum [30], the real medial axis SR of XR (also referred to as skeleton) is the set of points x∈XR at equal distance from their two closest neighbors in XRc. 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 pointx∈XR, let Px=argminy∈XRcd(x,y) denote the set of its closest points on XRc, referred to as projection of x. The corresponding Euclidean distance is denoted as Dx=d(x,Px). Accordingly, the skeleton SR(XR) can then be defined by(1)SRXR=x∈XR,Px≥2.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 gridF∪O). Applying (1) directly on F would fail, since, in most cases, |Px|=1 even when a given cell x 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 Px∗ denote the reduced projection of x, corresponding to the first element in Px with respect to a lexical ordering. Given a digital shape XZ⊂Zn and its complement XZc, the discrete skeleton SZ of XZ can then be expressed as(2)SZXZ=x∈XZ,∃y∈Nx∣dPx∗,Py∗>1,dm,Px∗≥dm,Py∗,where Nx={y∈XZ,d(x,y)=1} is the set of direct neighbors of x and m is the midpoint of line segment [x,y]. The first condition measures the (Euclidean) distance between the projections of x and y 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 Rn 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 Figure1.Figure 1
Illustrative representation of the concepts involved in DMA computation.Givenx∈XZ, y∈Nx, let rx,y denote the radius of the largest disk disjoint from Xc and centered along [Px,Py]:(3)rx,y=sup⁡Dz∣z∈Px,Py.The pruning criterionδ∗(x) is then defined as the maximal value of rx,y among its direct neighbors y∈Nx:(4)δ∗x=sup⁡rx,y∣y∈Nx,dm,Px≥dm,Py.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δ∈R+∗ and XZ⊂Zn. The delta medial axis MAδXZ of XZ is the set of points x∈XZ with a maximal delta value δ∗x of at least δ:(5)MAδXZ=x∈XZ∣δMx≥δ.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 representationF∪O, the GVG is thus finally approximated and filtered for a given UAV size δ, by computing the corresponding FMS MAδ(F). Examples for different δ values are depicted in Figure 2.Figure 2
Two FMS of the environment based on different UAV sizes ((a)δ = 20 cm, (b) δ = 40 cm).
(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 Figure2, 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. TheA-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. TheA-star is based on a cost function F(n):(6)Fn=Gn+Hn,where n is the current node, G(n) denotes the moving cost from thestarting point (node)PS to the current node n, and H(n) denotes the moving cost from the current node n to thetarget point (node)PT.Given the coordinates ofPS and PT, 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 Dt has to be defined to consider two points in the FMS connected. In a 3D occupancy grid, theA-star algorithm uses searching restricted to 26-connectivity (giving the closest cells in 3D environment) or in every angle for some variants of theA-star. Here, the search forwards on the FMS based on Dt. 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 Dt.The generated path is a set of ordered waypoints defined on the FMS fromPS to PT. 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 z. One can see that the planned path is suboptimized with oscillations, which requires a postprocessing step to refine it.Figure 3
Shortest path computed on the FMS based on different UAV sizes (20 cm (a), 40 cm (b)) with the altitude (right column) plotted for each case.
(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. ConsiderP0,P1,…,Pn(n≥1), n+1 points. The Bézier curve of n degrees associated with these points is the curve C defined by M(t) with t∈0,1:(7)Mt=B0tP0+B1tP1+⋯+BntPnP i   ( i = 1 : n ) are called the control points of the Bézier curve and define the control polygon. Bi(0≤i≤n) coefficients are theBernstein polynomials of degree n given as follows:(8)Bi,nt=Cniti1-tn-i,i=0,…,nwithCni=n!i!n-i!.In a 3D environment,Pi is the point (xi,yi,zi). This means that the point M(t) has as coordinates (9)xt=∑i=0nBitxi,yt=∑i=0nBityi,zt=∑i=0nBitzi.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 pointsPS and PT, one more reason to use Bézier curves is theirendpoint interpolation property. This means that the first and last control points (denoted, resp., by PS and PT) are the endpoints of the curve. To form a complex smooth path, cubic Bézier curves can be smoothly connected thanks to theendpoint tangent property. This property means that the cubic Bézier curve C is tangent to P0,P1P2,P3 in P0P3 and M˙(0)=3(P1-P0)M˙(1)=3(P3-P2). 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:(10)Qj-ptj≤Dj-δ,where Qj are the original data, p(tj) are values of approximated data computed by interpolation, and Dj 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 inPS and ends in PT. The objective function to be minimized is the mean of the squared distance between real data and smoothed data. Suppose that there are m waypoints Qj(j=1:m) in the original data. For smoothed data p(tj), we note that 0≤tj≤1 with uniform division, so Q1=p(0)=PS and Qm=p(1)=PT. The objective function is given by(11)minptj⁡1m-2∑j=2m-1Qj-ptj2.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 arel segments (i.e., l-1 break points), by solving the problem of optimization (see Figure 4), 3l-1 control points must be determined between PS and PT to define l 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.Figure 4
Smoothing strategy illustration for thekth segment.TheC1-continuity between patches of Bézier curve can be translated to constraints of the optimization. Let P0k, P1k, P2k, and P3k denote the control points of the kth Bézier curve. kth and (k+1)th cubic Bézier curves are C1 continuous at the joining point if (12)P3k=P0k+1,P3k-P2k=P1k+1-P0k+1.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. Figure5 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.Figure 5
Final path for different UAV sizes (20 cm (a), 40 cm (b)).
(a) (b)Table1 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.Table 1
Smoothing results.
case 1 δ = 20 cm case 2 δ = 40 cm Maximum allowed smoothing gap (cm) 42.59 32.58 Maximum smoothing gap (cm) 8.13 8.98 Smoothing gap mean (cm) 2.70 2.55


## 3.1. Bézier Curves

Bézier curves are parametric curves frequently used in vehicle design, fonts representation, games, and 3D animations. ConsiderP0,P1,…,Pn(n≥1), n+1 points. The Bézier curve of n degrees associated with these points is the curve C defined by M(t) with t∈0,1:(7)Mt=B0tP0+B1tP1+⋯+BntPnP i   ( i = 1 : n ) are called the control points of the Bézier curve and define the control polygon. Bi(0≤i≤n) coefficients are theBernstein polynomials of degree n given as follows:(8)Bi,nt=Cniti1-tn-i,i=0,…,nwithCni=n!i!n-i!.In a 3D environment,Pi is the point (xi,yi,zi). This means that the point M(t) has as coordinates (9)xt=∑i=0nBitxi,yt=∑i=0nBityi,zt=∑i=0nBitzi.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 pointsPS and PT, one more reason to use Bézier curves is theirendpoint interpolation property. This means that the first and last control points (denoted, resp., by PS and PT) are the endpoints of the curve. To form a complex smooth path, cubic Bézier curves can be smoothly connected thanks to theendpoint tangent property. This property means that the cubic Bézier curve C is tangent to P0,P1P2,P3 in P0P3 and M˙(0)=3(P1-P0)M˙(1)=3(P3-P2). 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:(10)Qj-ptj≤Dj-δ,where Qj are the original data, p(tj) are values of approximated data computed by interpolation, and Dj 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 inPS and ends in PT. The objective function to be minimized is the mean of the squared distance between real data and smoothed data. Suppose that there are m waypoints Qj(j=1:m) in the original data. For smoothed data p(tj), we note that 0≤tj≤1 with uniform division, so Q1=p(0)=PS and Qm=p(1)=PT. The objective function is given by(11)minptj⁡1m-2∑j=2m-1Qj-ptj2.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 arel segments (i.e., l-1 break points), by solving the problem of optimization (see Figure 4), 3l-1 control points must be determined between PS and PT to define l 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.Figure 4
Smoothing strategy illustration for thekth segment.TheC1-continuity between patches of Bézier curve can be translated to constraints of the optimization. Let P0k, P1k, P2k, and P3k denote the control points of the kth Bézier curve. kth and (k+1)th cubic Bézier curves are C1 continuous at the joining point if (12)P3k=P0k+1,P3k-P2k=P1k+1-P0k+1.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. Figure5 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.Figure 5
Final path for different UAV sizes (20 cm (a), 40 cm (b)).
(a) (b)Table1 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.Table 1
Smoothing results.
case 1 δ = 20 cm case 2 δ = 40 cm Maximum allowed smoothing gap (cm) 42.59 32.58 Maximum smoothing gap (cm) 8.13 8.98 Smoothing gap mean (cm) 2.70 2.55


## 4. Results and Discussions

To provide a comparative study of the proposed approach, we apply theA-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.Figure 6
Comparison between the proposed approach based on different UAV sizes (20 cm (cyan line), 40 cm (magenta line)) and the A-star applied on the occupancy grid taking into account different UAV sizes (20 cm (orange line), 40 cm (black line)).To efficiently analyze the results, we define a set of quantitative criteria for the comparative study. The comparison results are shown in Table2.Table 2
Quantitative comparison between theA-star applied on the OG and the proposed approach based on two different UAV sizes (δ = 20 cm (case 1), δ = 40 cm (case 2)).
A-star on the OG Proposed approach case 1 case 2 case 1 case 2 Input data number 236644 61841 23324 7913 Path length (m) 9.03 11.08 9.95 11.41 Number of path points 155 209 126 142 Smallest dist. to obstacles (cm) 25.63 42.86 31.13 46.28 Mean of dist. to obstacles (cm) 40.18 50.51 48.42 56.88The input data (see Table2) of the search algorithm depends on the type of the approach applied and the UAV dimension. When directly using theA-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 theA-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.Figure 7
Evolution of the distance to obstacles ((a)δ = 20 cm, (b) δ = 40 cm).
(a)
Case 1 (b)
Case 2We 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 theA-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)
ΔZmax: maximal change value of the altitude.(ii)
Δψmax: maximal change value of the heading angle.For the two UAV sizes considered, the following numbers are obtained:(i)
In case 1 (δ =20 cm): ΔZmax=08.78 cm, Δψmax=28.43 deg.(ii)
In case 2 (δ =40 cm): ΔZmax=05.41 cm, Δψmax=23.84 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 theA-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 theA-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 conventionalA-star for the planning of the shortest path. An improved version of theA-star is obviously required especially when the ratioUAV 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.



---

*Source: 4676720-2018-06-03.xml*