Dubins Path-Oriented Rapidly Exploring Random Tree* for Three-Dimensional Path Planning of Unmanned Aerial Vehicles

Abstract

Unmanned aerial vehicles (UAVs) do not collide with obstacles, generate a path in real-time, and must fly to the target point. The sampling-based rapidly exploring random tree (RRT) algorithm has the advantages of fast computation and low computational complexity. It is suitable for real-time path generation, but the optimal path cannot be guaranteed. Further, the direction of the flight and the minimum radius of rotation have not been taken into account for the characteristics of the UAVs. This work proposes a Dubins path-oriented RRT* algorithm, which applies the Dubins path to the RRT algorithm to consider the direction of flight and the minimum radius of rotation and improves optimality and convergence. The proposed algorithm sets the sample node as the target point, orients toward the Dubins path, and then generates a tree. To verify the performance of the proposed algorithm, it is compared with existing RRT algorithms. As a result of performance analysis, the proposed algorithm improved the path length by 14.87% and the calculation time by 82.36%. Finally, the algorithm’s performance is verified by applying the proposed algorithm to a fixed-wing UAV and performing a numerical analysis of the generated path.

1. Introduction

With technological advancements, unmanned aerial vehicles (UAVs) are being used in various industries and recreational fields, with notable applications in disaster assessment, diagnosis, measurement, relief, etc. [1,2,3]. Ensuring safety and reliability should be prioritized to carry out smooth missions. That is, it is necessary to generate the shortest path to the destination without colliding with obstacles [4,5]. Typical path generation algorithms include grid-based, mathematical model-based, and sampling-based approaches [6].

Grid-based algorithms, such as Dijkstra, A*, and D*, are intuitive and search by dividing all sections evenly, reducing the probability of falling into the local maximum [7,8,9]. However, there is a disadvantage in that the calculation time increases exponentially depending on the search section [10]. Mathematical model-based algorithms, such as linear algorithms and optimal control, use constraints to achieve optimal solutions by modeling given surroundings and dynamic systems [6,11]. Although an optimal path can be generated through these methods, it takes a lot of time to model environmental conditions and calculate constraints. Sampling-based algorithms retrieve a node adjacent to a randomly selected point in a feasible space and expand the node if possible. This is repeated until the path to the destination is generated. The main advantages of sampling-based algorithms are low computational costs and applicability to high-dimensional problems. Typical sampling-based algorithms include probabilistic roadmaps (PRM) and rapidly exploring random tree (RRT). PRM is mainly used in fixed obstacle environments [12], while RRT is suitable for generating paths through tree extensions and also for use in environmental changes [10].

RRT has the advantage of quickly generating a path with a light computation burden but has limitations in that the generated path is not smooth and cannot guarantee an optimal path. RRT* holds the fundamentals of the RRT but guarantees gradual convergence to the optimal path through additional steps [13]. However, RRT* is not suitable for UAVs that operate in real-time due to expensive computational costs. An improved biased RRT is developed to aim at the target point with a certain probability in RRT [14,15,16]. Convergence increases by oriented toward a target point, but the convergence becomes poor when there are many obstacles.

This work applies the Dubins path to the RRT* to improve the convergence of the RRT for the path generation of fixed-wing UAVs and reduce the calculation time. The authors considered the flight direction and the minimum turning radius, which have not been considered in the RRT. Dubins path uses the Dubins curve proposed by Dubins to generate the shortest path considering the rotation radius limited to the position and direction at the given initial and final points [17]. As a result, the proposed algorithm reduces the cost of path generation computation and avoids obstacles by limiting unnecessary tree extensions. In addition, it aims at Dubins path to ensure optimality and considers the flight direction and the minimum turning radius. The simulation study is performed with a fixed-wing UAV model to confirm the characteristics of the algorithm. To verify the performance of the proposed algorithm, the simulation results are compared to RRT, RRT*, and biased RRT.

2. Previous Research

RRT, one of the sampling-based algorithms, is a method of finding a path by randomly generating several sample nodes without dividing a given space into a grid. It efficiently identifies the barrier-free space by checking whether the sample node or the line connecting the two-sample nodes collides with the obstacle. The sampling-based algorithm can be used in a high-dimensional space and has the advantage of being able to quickly generate a path due to a small amount of calculation. The basic idea is to build a tree that makes up nodes and connections. In the tree structure, all nodes are connected to one other node called a parent, and the starting point is the top parent of the tree. RRT calculates a tree connecting the starting point and the target point in a given space.

2.1. RRT Algorithm

RRT was proposed by LaValle [18], and Algorithm 1 and Figure 1 depict the pseudocode and the path generation process of the RRT, respectively.

Algorithm 1 RRT

1: procedure RRT(${\mathit{q}}_s,{\mathit{q}}_f$)   2:      $\mathbf{T}\leftarrow$ InitialSetting()   3:      while $norm({\mathit{q}}_f-{\mathit{q}}_{new})\ge R_{threshold}$ do   4:            ${\mathit{q}}_{rand}\leftarrow$ RandomSamling()   5:            ${\mathit{q}}_{nearest}\leftarrow$ FindNearest($\mathbf{T}$,${\mathit{q}}_{rand}$)   6:            ${\mathit{q}}_{new}\leftarrow$ Extend(${\mathit{q}}_{nearest},{\mathit{q}}_{rand},ϵ$)   7:            if CollisionCheck(${\mathit{q}}_{nearest},{\mathit{q}}_{new}$) then   8:                 $\mathbf{T}\leftarrow$${\mathit{q}}_{new}$   9:            end if 10:      end while 11:      ${\mathit{N}}_{\mathit{path}},t_{cal}\leftarrow$ SelectPath $\left(\mathbf{T}{\mathit{q}}_{\mathit{f}}\right)$ 12:      return ${\mathit{N}}_{\mathit{path}}$,$t_{cal}$ 13: end procedure

RRT aims to extend the tree through an obstacle check between the tree $\mathbf{T}$ starting from the start point ${\mathit{q}}_s$ and the randomly generated node ${\mathit{q}}_{rand}$ in a given space, generating a path to reach the target point ${\mathit{q}}_f$. A sample node ${\mathit{q}}_{rand}$ is generated, the distance between ${\mathit{q}}_{rand}$ and all nodes constituting the tree is measured, and the nearest node ${\mathit{q}}_{nearest}$ is set. Then, it creates ${\mathit{q}}_{new}$ at a point $\epsilon$ distance away from ${\mathit{q}}_{nearest}$ in the ${\mathit{q}}_{rand}$ direction. When ${\mathit{q}}_{new}$ is created, an obstacle check is performed between ${\mathit{q}}_{new}$ and ${\mathit{q}}_{nearest}$. If there is no obstacle between the two nodes, ${\mathit{q}}_{new}$ is added to expand the tree. If there is an obstacle, the process of creating ${\mathit{q}}_{rand}$ is repeated. It repeats until a certain number of nodes are satisfied, such as generating ${\mathit{q}}_{rand}$ or ${\mathit{q}}_{new}$ or until the tree reaches a certain radius of ${\mathit{q}}_f$. When the tree is expanded, it connects ${\mathit{q}}_f$ and the tree and starts with ${\mathit{q}}_f$ and performs the process of finding the parent of the previously created node. This is repeated until ${\mathit{q}}_s$ is reached to generate a path.

2.2. RRT* Algorithm

RRT* is an algorithm that improves optimality by adding a process of re-selecting parent nodes and re-connecting trees to the RRT [19]. The pseudocode and the path generation process of the RRT* are shown in Algorithm 2 and Figure 2, respectively.

Algorithm 2 RRT*

1: procedure RRT*(${\mathit{q}}_s,{\mathit{q}}_f$)   2:      $\mathbf{T}\leftarrow$ InitialSetting()   3:      while $norm({\mathit{q}}_f-{\mathit{q}}_{new})\ge R_{threshold}$ do   4:            ${\mathit{q}}_{rand}\leftarrow$ RandomSamling()   5:            ${\mathit{q}}_{nearest}\leftarrow$ FindNearest($\mathbf{T}$,${\mathit{q}}_{rand}$)   6:            ${\mathit{q}}_{new}\leftarrow$ Extend(${\mathit{q}}_{nearest},q_{rand},ϵ$)   7:            if CollisionCheck(${\mathit{q}}_{nearest},q_{new}$) then   8:                 $\mathbf{T}\leftarrow$${\mathit{q}}_{new}$   9:                 ${\mathbf{Q}}_{\mathbf{near}}$,${\mathit{q}}_{min}\leftarrow$ BestParent($\mathbf{T}$,${\mathit{q}}_{new},R^{*}$) 10:                 $\mathbf{T}\leftarrow$ Rewiring($\mathbf{T},{\mathbf{Q}}_{\mathbf{near}}$,${\mathit{q}}_{new},q_{min},R^{*}$) 11:            end if 12:      end while 13:      ${\mathit{N}}_{\mathit{path}},t_{cal}\leftarrow$ SelectPath($\mathbf{T}$,${\mathit{q}}_f$) 14:      return ${\mathit{N}}_{\mathit{path}},t_{cal}$ 15: end procedure

RRT* finds the node closest to ${\mathit{q}}_{rand}$, sets it to ${\mathit{q}}_{nearest}$, and creates ${\mathit{q}}_{new}$ at points $\epsilon$ distance away in the ${\mathit{q}}_{rand}$ direction. When ${\mathit{q}}_{new}$ is created, as shown in Figure 2a, nodes that are within a certain radius around ${\mathit{q}}_{new}$ and do not get hit by obstacles are set as ${\mathit{q}}_{near}$, and the node with the minimum path cost to ${\mathit{q}}_{new}$ among ${\mathit{q}}_{near}$ is found. If it finds a ${\mathit{q}}_{near}$ with a lower path cost than the previously connected ${\mathit{q}}_{nearest}$, it disconnects from ${\mathit{q}}_{nearest}$, sets the node a with low path cost as the parent node ${\mathit{q}}_{near}$ of ${\mathit{q}}_{new}$, and re-connects the tree. The path cost is calculated by using ${\mathit{q}}_{new}$ as a parent node for ${\mathit{q}}_{near}$ within a certain radius around ${\mathit{q}}_{new}$. If the path cost is lower when connected through ${\mathit{q}}_{new}$ than the existing path cost of ${\mathit{q}}_{near}$, the tree that was previously connected to ${\mathit{q}}_{near}$ disconnects and re-connects ${\mathit{q}}_{new}$ and the tree, as shown in Figure 2b. RRT* complements optimality, which is a disadvantage of the RRT, through the best parent process of re-connecting ${\mathit{q}}_{nearest}$ and the re-wiring process of reducing the path cost of ${\mathit{q}}_{near}$.

2.3. Biased RRT Algorithm

The biased RRT is an improved algorithm that supplements the node generation method in the RRT and improves the randomly sampled nodes to a target point with a certain probability [14]. If the probability of aiming for the target point is high, convergence is increased in the process of expanding the tree. However, the map cannot be searched as a whole, so it takes more time to get out of the obstacle than before. There is a deviation in the path generation time depending on the complexity and directionality probability of the obstacle. Because the operating environment is not the same every time, it is generally known that using a directivity probability of 10% is the most effective regardless of the density of obstacles [9]. The pseudocode and the path generation process of the biased RRT are shown in Algorithm 3 and Figure 3, respectively.

Algorithm 3 Biased RRT

1: procedure BRRT(${\mathit{q}}_s,{\mathit{q}}_f$)   2:      $\mathbf{T}\leftarrow$ InitialSetting()   3:      while $norm({\mathit{q}}_f-{\mathit{q}}_{new})\ge R_{threshold}$ do   4:            ${\mathit{q}}_{rand}\leftarrow$ RandomSamling($k,{\mathit{q}}_{\mathit{f}}$)   5:            ${\mathit{q}}_{nearest}\leftarrow$ FindNearest($\mathbf{T}$,${\mathit{q}}_{rand}$)   6:            ${\mathit{q}}_{new}\leftarrow$ Extend(${\mathit{q}}_{nearest},q_{rand},ϵ$)   7:            if CollisionCheck(${\mathit{q}}_{nearest},q_{new}$) then   8:                 $\mathbf{T}$←${\mathit{q}}_{new}$   9:           end if 10:     end while 11:     ${\mathit{N}}_{\mathit{path}},t_{cal}\leftarrow$ SelectPath($\mathbf{T}$,${\mathit{q}}_{\mathit{f}}$) 12:     return ${\mathit{N}}_{\mathit{path}},t_{cal}$ 13: end procedure

In the biased RRT, only the process of generating ${\mathit{q}}_{rand}$ from the RRT has been changed, and the rest of the process is the same as in the RRT. In the RRT, it extends the tree toward ${\mathit{q}}_f$ by replacing ${\mathit{q}}_{rand}$, which is randomly sampled, with ${\mathit{q}}_f$ according to a certain probability. The directivity probability that directs ${\mathit{q}}_f$ to a certain probability is called k, and the tree expands toward ${\mathit{q}}_f$ for each k probability so that it has convergence. When ${\mathit{q}}_{new}^{{}^{\prime }}$ generated by ${\mathit{q}}_f$ collides with an obstacle to solve these problems, ${\mathit{q}}_{rand}$ is used to generate ${\mathit{q}}_{new}$ in the direction without obstacles and extend the tree.

3. Dubins Path-Oriented RRT* Algorithm

The pseudocode of the Dubins path-oriented RRT* proposed is shown in Algorithm 4, and the path generation process of the algorithm is shown in Figure 4 and Figure 5. The Dubins path-oriented RRT* complements the optimality and convergence of the RRT and utilizes the Dubins path to consider the flight direction and minimum radius of rotation of the UAV. The proposed algorithm has two methods for complementing optimality. The first method generates the Dubins path in the position and direction of the initial and final points. It is oriented toward the generated Dubins path and generates a path. The second method, similar to the RRT*, performs the process of re-selecting the parent node and re-constructing the tree. The method for compensating convergence has a 100% probability of replacing ${\mathit{q}}_{rand}$ with ${\mathit{q}}_f$, so convergence to the target is further obtained than in the biased RRT. However, when the percentage of the obstacle space is increased, ${\mathit{q}}_{new}$ is not generated properly like the biased RRT, resulting in the result of being blocked by obstacles and reduced convergence.

Algorithm 4 DRRT*

1: procedure DRRT*(${\mathit{q}}_s,{\mathit{q}}_f$,dubins path)   2:      $\mathbf{T}\leftarrow$ InitialSetting()   3:      while $norm({\mathit{q}}_f-{\mathit{q}}_{new})\ge R_{threshold}$ do   4:            ${\mathit{q}}_{rand}\leftarrow$ RandomSamling()   5:            ${\mathit{q}}_{nearest}^{*}\leftarrow$ FindNearest($\mathbf{T}$,${\mathit{q}}_f$)   6:            ${\mathit{q}}_{new}^{*}\leftarrow$ Extend(${\mathit{q}}_{nearest}^{*},{\mathit{q}}_f,ϵ$)   7:            ${\mathit{q}}^{*}\leftarrow$ ColsestPoint(${\mathit{q}}_{new}^{*}$, ${\mathit{q}}_s,{\mathit{q}}_f$,dubins path)   8:            ${\mathit{q}}_{\mathit{nearest}}^{{}^{\prime }}\leftarrow$ FindNearest($\mathbf{T}$,${\mathit{q}}^{*}$)   9:            ${\mathit{q}}_{new}^{{}^{\prime }}\leftarrow$ Extend(${\mathit{q}}_{nearest}^{*},{\mathit{q}}^{*},ϵ$) 10:            if CollisionCheck(${\mathit{q}}_{nearest}^{{}^{\prime }},{\mathit{q}}_{new}^{{}^{\prime }}$) then 11:                $\mathbf{T}\leftarrow$${\mathit{q}}_{new}^{{}^{\prime }}$ 12:                ${\mathbf{Q}}_{\mathbf{near}}$,${\mathit{q}}_{min}\leftarrow$ BestParent($\mathbf{T}$,${\mathit{q}}_{new}^{{}^{\prime }},R^{*}$) 13:                $\mathbf{T}\leftarrow$ Rewiring($\mathbf{T},{\mathbf{Q}}_{\mathbf{near}}$, ${\mathit{q}}_{new}^{{}^{\prime }},{\mathit{q}}_{min},R^{*}$) 14:            else 15:                 ${\mathit{q}}_{nearest}\leftarrow$ FindNearest($\mathbf{T}$,${\mathit{q}}_{rand}$) 16:                 ${\mathit{q}}_{new}\leftarrow$ Extend(${\mathit{q}}_{nearest},{\mathit{q}}_{rand},ϵ$) 17:                 if CollisionCheck(${\mathit{q}}_{nearest},{\mathit{q}}_{new}$) then 18:                    $\mathbf{T}\leftarrow$${\mathit{q}}_{new}$ 19:                    ${\mathbf{Q}}_{\mathbf{near}}$, ${\mathit{q}}_{min}\leftarrow$ BestParent($\mathbf{T}$,${\mathit{q}}_{new},R^{*}$) 20:                    $\mathbf{T}\leftarrow$ Rewiring(${\mathbf{T},\mathbf{Q}}_{\mathbf{near}}$, ${\mathit{q}}_{new},{\mathit{q}}_{min},R^{*}$) 21:             end if 22:          end if 23:      end while 24:      ${\mathit{N}}_{\mathit{path}},t_{cal}\leftarrow$ SelectPath($\mathbf{T}$,${\mathit{q}}_f$) 25:      return ${\mathit{N}}_{\mathit{path}},t_{cal}$ 26: end procedure

3.1. Dubins Path Generation

Path generation is realized based on the Dubins path. A route is created by considering constant altitude, cruising speed v, and the UAV with maximum rotational curvature $c_{\max }$. Given the initial point ${\mathit{P}}_s$ and final point ${\mathit{P}}_f$, the shortest path includes a combination of three path segments: a straight line segment (S) and an arc segment with a minimum radius (R or L). The four cases, LSL, LSR, RSR, and RSL, of the Dubins path are composed of two curved segments and a straight segment [20]. The shortest path is selected by comparing the four generated paths. The geometry of the Dubins path is depicted in Figure 6.

The positions and directions of the initial and final points in a 3D space are defined as

$$\begin{array}{cc}\hfill {\mathit{q}}_s& ={\left[x_{sx}{x}_{sy}{x}_{sz}\right]}^T\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill {\mathit{q}}_f& ={\left[x_{fx}{x}_{fy}{x}_{fz}\right]}^T\hfill \end{array}$$

(2)

$$\begin{array}{cc}\hfill {\mathit{v}}_s& ={\left[v_{sx}{v}_{sy}{v}_{sz}\right]}^T\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill {\mathit{v}}_f& ={\left[v_{fx}{v}_{fy}{v}_{fz}\right]}^T\hfill \end{array}$$

(4)

In the earth-fixed cartesian coordinate frame, the path is expressed as

$$\begin{array}{cc}\hfill \frac{dx\left(s\right)}{ds}& =\cos \gamma \left(s\right)\cos \psi \left(s\right)\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill \frac{dy\left(s\right)}{ds}& =\cos \gamma \left(s\right)\sin \psi \left(s\right)\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill \frac{dz\left(s\right)}{ds}& =\sin \gamma \left(s\right)\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill \frac{d\psi \left(s\right)}{ds}& =\eta \hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill \frac{d\gamma \left(s\right)}{ds}& =\mu \hfill \end{array}$$

(9)

where s represents the curvilinear abscissa on the path, $\psi$ is the heading angle, and $\gamma$ is the flight path angle. As the vehicle is constrained by its minimum turn radius r, the path should have a maximum curvature limit $c_{\max }$. The curvature $c\left(s\right)$ is given by

$$\begin{array}{c}\hfill c\left(s\right)=\sqrt{{\eta }^2\left(s\right){\cos }^2\gamma \left(s\right)+{\mu }^2\left(s\right)}\end{array}$$

(10)

and the objective is to minimize the path length while satisfying the following constraint:

$$\begin{array}{c}\hfill -c_{\max }\le c\left(s\right)\le c_{\max }\end{array}$$

(11)

Common to both planes of the initial and final curved paths of the minimum radius is a myriad of different planes, as shown in Figure 7.

The straight-line path segment between these two curves is the intersecting line between the initial and final maneuver planes. Suppose $\mathit{X}\left(X_x,X_y,X_z\right)$ is the vector common to both planes as follows [17]:

$$\begin{array}{c}\hfill \mathit{X}={\mathit{p}}_f-{\mathit{p}}_s=\left({\mathit{q}}_f+r{\mathit{w}}_f-r{\mathit{y}}_f\right)-\left({\mathit{q}}_s+r{\mathit{w}}_s-r{\mathit{y}}_s\right)\end{array}$$

(12)

Adding the first and second rotation angles ${\theta }_1$ and ${\theta }_2$ yields the following equation:

$$\begin{array}{c}\hfill \mathit{X}=\left({\mathit{q}}_f-{\mathit{q}}_s\right)\mp r\left(\mathit{X}+{\mathit{v}}_s\right)\tan \frac{{\theta }_s}{2}\mp r\left(\mathit{X}+{\mathit{v}}_f\right)\tan \frac{{\theta }_f}{2}\end{array}$$

(13)

where $\cos {\theta }_s={\mathit{v}}_s\mathit{v}$ and $\cos {\theta }_f={\mathit{v}}_f\mathit{X}$, and $\mathit{X}$ are unit vectors of $\mathit{X}$. Through simplification, one obtains the following equation:

$$\begin{array}{cc}\hfill \left(x_{sx}-x_{fx}\right)& =X_x\pm r{x}_x\left[\tan \frac{{\theta }_s}{2}+\tan \frac{{\theta }_f}{2}\right]\pm r\left[v_{fx}\tan \frac{{\theta }_f}{2}+v_{sx}\tan \frac{{\theta }_s}{2}\right]\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill \left(x_{sy}-x_{fy}\right)& =X_y\pm r{x}_y\left[\tan \frac{{\theta }_s}{2}+\tan \frac{{\theta }_f}{2}\right]\pm r\left[v_{fy}\tan \frac{{\theta }_f}{2}+v_{sy}\tan \frac{{\theta }_s}{2}\right]\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill \left(x_{sz}-x_{fz}\right)& =X_z\pm r{x}_z\left[\tan \frac{{\theta }_s}{2}+\tan \frac{{\theta }_f}{2}\right]\pm r\left[v_{fz}\tan \frac{{\theta }_f}{2}+v_{sz}\tan \frac{{\theta }_s}{2}\right]\hfill \end{array}$$

(16)

In the case of a system of multivariable equations, the solution of the system of equations can be determined through the Gauss–Newton method, among other methods. In this manner, a common plane $\mathit{X}$ is obtained.

3.2. Optimization Improvement Method

The proposed algorithm has two methods to complement optimality. The first method considers the constraint condition of the minimum turning radius when the positions and directions of the initial point and the final point are given and generates a path using the Dubins path that generates the shortest path.

• As shown in Figure 4a, ${\mathit{q}}_{nearest}^{*}$ is selected by searching for the node closest to ${\mathit{q}}_f$ and ${\mathit{q}}_{new}^{*}$ is created at a certain distance $\epsilon$ from ${\mathit{q}}_{nearest}^{*}$ in the ${\mathit{q}}_f$ direction (lines 4 and 5).
• ${\mathit{q}}^{*}$ means that ${\mathit{q}}_{new}^{*}$ is the closest point to the Dubins path. ${\mathit{q}}^{*}$ is the point where the paths of ${\mathit{q}}^{*}$ new and the Dubins path are vertical (line 6).
• As shown in Figure 4b, one selects ${\mathit{q}}_{nearest}^{{}^{\prime }}$ by searching for the node closest to ${\mathit{q}}^{*}$ and creates ${\mathit{q}}_{new}^{{}^{\prime }}$ at a position $\epsilon$ away from ${\mathit{q}}_{nearest}^{{}^{\prime }}$ by a certain distance $\epsilon$ in the ${\mathit{q}}^{*}$ direction (lines 7 and 8).

The second method re-selects the parent node and re-connects the tree, similar to the RRT*.

• As shown in Figure 2a, ${\mathit{q}}_{new}^{{}^{\prime }}$ or ${\mathit{q}}_{new}$ is re-elected as the parent node among ${\mathit{q}}_{near}$ with the lowest path cost (lines 11 and 18).
• If the path cost when connecting with ${\mathit{q}}_{new}^{{}^{\prime }}$ or ${\mathit{q}}_{new}$ is lower than the existing path cost of ${\mathit{q}}_{near}$ within a certain radius, as shown in Figure 2b, the tree is re-constructed (lines 12 and 19).

3.3. Convergence Improvement Method

The proposed algorithm complemented the convergence by improving the biased RRT. The biased RRT is stochastically oriented to ${\mathit{q}}_f$, which causes unnecessary computation. Therefore, one always sets ${\mathit{q}}_f$ to the sample node to improve convergence. However, if there are obstacles, there is a risk of falling into the local minima. To reduce this risk, sample nodes are selectively used.

• When selecting ${\mathit{q}}_{nearest}^{*}$, one proceeds with the sample node using ${\mathit{q}}_f$ and generates ${\mathit{q}}_{new}^{*}$ in the ${\mathit{q}}_f$ direction (lines 4 and 5).
• As shown in Figure 5, when ${\mathit{q}}_{new}^{{}^{\prime }}$ collides with an obstacle, ${\mathit{q}}_{nearest}$ is selected using ${\mathit{q}}_{rand}$ and ${\mathit{q}}_{new}$ is created (lines 3 and 13–15).

4. Path Tracking and Control

4.1. Path Tracking

A nonlinear UAV path tracking algorithm similar to the line-of-sight guidance algorithm is adopted. Equations of motion that do not take into account the dynamic characteristics of the UAV are expressed as

$$\begin{array}{cc}\hfill \dot{x}& =v\cos \psi \cos \gamma \hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill \dot{y}& =v\sin \psi \cos \gamma \hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill \dot{z}& =v\sin \gamma \hfill \end{array}$$

(19)

where v is assumed to be constant, and $\psi$ and $\gamma$ represent the control inputs. The path to be tracked is represented by the following equations:

$$\begin{array}{cc}\hfill {\displaystyle \frac{d{x}_r\left(s\right)}{ds}}& =\cos {\psi }_r\left(s\right)\cos {\gamma }_r\left(s\right)\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill {\displaystyle \frac{d{y}_r\left(s\right)}{ds}}& =\sin {\psi }_r\left(s\right)\cos {\gamma }_r\left(s\right)\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill {\displaystyle \frac{d{z}_r\left(s\right)}{ds}}& =\sin {\gamma }_r\left(s\right)\hfill \end{array}$$

(22)

At each time instant, $s^{*}$ represents the path point closest to UAV in abscissa. This point ensures that the vector in the direction of the path from the UAV and the vector tangent to the path are perpendicular. Thus,

$$\begin{array}{c}\hfill \left(x_r\left(s^{*}\right)-x\right){\left.\frac{d{x}_r\left(s\right)}{ds}\right|}_{s=s^{*}}+\left(y_r\left(s^{*}\right)-y\right){\left.\frac{d{y}_r\left(s\right)}{ds}\right|}_{s=s^{*}}+\left(z_r\left(s^{*}\right)-z\right){\left.\frac{d{z}_r\left(s\right)}{ds}\right|}_{s=s^{*}}=0\end{array}$$

(23)

where

$$\begin{array}{cc}\hfill e_x& =x_r\left(s^{*}\right)-x\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill e_y& =y_r\left(s^{*}\right)-y\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill e_z& =z_r\left(s^{*}\right)-z\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill e& =\sqrt{e_x^2+e_y^2+e_z^2}\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill {\psi }_r^{*}& ={\psi }_r\left(s^{*}\right)\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill {\gamma }_r^{*}& ={\gamma }_r\left(s^{*}\right)\hfill \end{array}$$

(29)

This expression can be rewritten as

$$\begin{array}{c}\hfill e_x\cos {\psi }_r^{*}\cos {\gamma }_r^{*}+e_y\sin {\psi }_r^{*}\cos {\gamma }_r^{*}+e_z\sin {\gamma }_r^{*}=0\end{array}$$

(30)

Consider the following straight line $\Omega$ tangent to the path at $\left(x_r,y_r,z_r\right)$

$$\begin{array}{c}\hfill \Omega =\left[\begin{array}{c}{x}_l\left(s\right)\\ y_l\left(s\right)\\ z_l\left(s\right)\end{array}\right]=\left[\begin{array}{c}{x}_r\left(s^{*}\right)+s\cos {\psi }_r^{*}\cos {\gamma }_r^{*}\\ y_r\left(s^{*}\right)+s\sin {\psi }_r^{*}\cos {\gamma }_r^{*}\\ z_r\left(s^{*}\right)+s\sin {\gamma }_r^{*}\end{array}\right]\end{array}$$

(31)

where $K_p>0$ represents the prediction distance of the existing line-of-sight derivation algorithm [21], and $s=1/K_p$. If $K_p\to 0$, it follows the direction of the tangent while maintaining a constant cross-track error. If $K_p\to \infty$, it moves toward the path point closest to the UAV. At each time instant, $\psi$ and $\gamma$ are chosen so as to satisfy the following:

$$\begin{array}{cc}\hfill \cos {\psi }_d& =\frac{{\tilde{e}}_x}{\sqrt{{\tilde{e}}_x^2+{\tilde{e}}_y^2}}\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill \sin {\psi }_d& =\frac{{\tilde{e}}_y}{\sqrt{{\tilde{e}}_x^2+{\tilde{e}}_y^2}}\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill \cos {\gamma }_d& =\frac{\sqrt{{\tilde{e}}_x^2+{\tilde{e}}_y^2}}{\tilde{e}}\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill \sin {\gamma }_d& =\frac{{\tilde{e}}_z}{\tilde{e}}\hfill \end{array}$$

(35)

where

$$\begin{array}{cc}\hfill {\tilde{e}}_x& ={\left.x_l\left(s\right)\right|}_{s=1/K_p}-x_u=e_x+\frac{1}{K_p}\cos {\psi }_r^{*}\cos {\gamma }_r^{*}\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill {\tilde{e}}_y& ={\left.y_l\left(s\right)\right|}_{s=1/K_p}-y_u=e_y+\frac{1}{K_p}\sin {\psi }_r^{*}\cos {\gamma }_r^{*}\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill {\tilde{e}}_z& ={\left.z_l\left(s\right)\right|}_{s=1/K_p}-z_u=e_z+\frac{1}{K_p}\sin {\gamma }_r^{*}\hfill \end{array}$$

(38)

$$\begin{array}{cc}\hfill \tilde{e}& =\sqrt{{\tilde{e}}_x^2+{\tilde{e}}_y^2+{\tilde{e}}_z^2}=\sqrt{e^2+\frac{1}{K_p^2}}\hfill \end{array}$$

(39)

The bank angle command generation can be realized from the equation for the centrifugal force as follows:

$$\begin{array}{cc}\hfill F_L\sin \varphi & =ma\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill F_L\cos \varphi & =mg\hfill \end{array}$$

(41)

where $F_L$ is the total lift force, m is the mass, g is the gravitational acceleration, and $\varphi$ is the bank angle. Accordingly,

$$\begin{array}{c}\hfill \tan \varphi =\frac{a}{g}\to \tan {\varphi }_d=\frac{a_{cmd}}{g}\end{array}$$

(42)

where ${\varphi }_d$ is the bank angle command, and $a_{cmd}$ is the lateral acceleration [22], which is defined as

$$\begin{array}{c}\hfill a_{cmd}=\frac{2V^2}{L}\sin \eta \end{array}$$

(43)

where L is the look-ahead distance, and $\eta$ is the angle between the velocity vector V and line L.

4.2. Constraint Sliding Mode Controller

A sliding mode controller involving a robust control scheme that can ensure control performance and stability even in the presence of uncertainty is used [23,24]. The designed controller imposes a constraint on the angular velocity by adding a saturation function to the existing sliding mode controller.

The proposed controller is named the constraint sliding mode controller (CSMC), with the sliding surface defined as

$$\mathit{s}=\mathit{\omega }+A\underset{\xi }{sat}\left({\mathit{q}}_e\right)$$

(44)

where $\omega$ represents the angular velocities of the body frame, ${\mathit{q}}_e$ is the error quaternion, and $A=\mathrm{diag}\left(a_1,a_2,a_3\right)$. Here, $a_i>0$, and $\xi =c_{\max }/a_i$.

Since the sliding surface designed in Equation (44) is $s=0$, the state variable converges to the target point in the same way as the operating characteristics of the existing sliding mode. Note that there is a point where the equilibrium point changes in the sliding phase. The conditions for the saturation function are defined as

$$\begin{array}{c}\hfill sat\left(q_i\right)=\left\{\begin{array}{cc}\xi ,\hfill & \mathrm{if}\xi <q_i\hfill \\ q_i,\hfill & \mathrm{if}-\xi \le q_i\le \xi \hfill \\ -\xi ,\hfill & \mathrm{if}-\xi >q_i\hfill \end{array}\right.\end{array}$$

(45)

which represents a saturation function that allows a smaller value to be selected through a variable between the values of each component $q_i$ of the posture error vector and the threshold variable $\xi$.

The reaching law for satisfying Equation (44) is defined as [25]

$$\begin{array}{c}\hfill \dot{\mathit{s}}=-c\mathrm{sgn}\left(\mathit{s}\right){\left|\mathit{s}\right|}^{\beta }\end{array}$$

(46)

While deriving the control input, the three-axis control input for the UAV posture considering the angular velocity limitation can be obtained by differentiating the equation with respect to time and arranging the resulting equations and equations of motion of the UAV as follows:

$$\begin{array}{c}\hfill \mathit{u}=l^{-1}\left[-f-C\mathrm{sgn}\left(\mathit{s}\right){\left|\mathit{s}\right|}^{\beta }+\mathit{\omega }\times \mathit{J}\mathit{\omega }-a\mathit{J}\frac{d}{dt}\left\{\underset{\xi }{sat}\left({\mathit{q}}_e\right)\right\}\right]\end{array}$$

(47)

To validate the stability of the designed controller, the Lyapunov candidate function is defined as

$$\begin{array}{c}\hfill V=\frac{1}{2}{\mathit{s}}^T\mathit{J}\mathit{s}\end{array}$$

(48)

and the equation is differentiated with respect to time to ensure stability as follows:

$$\begin{array}{cc}\hfill \dot{V}={\mathit{s}}^T\mathit{J}\mathit{s}& ={\mathit{s}}^T\mathit{J}\left(\dot{\mathit{\omega }}+a\frac{d}{dt}\left(sat\left(q_e\right)\right)\right)\hfill \\ \hfill & ={\mathit{s}}^T\left(-\mathit{\omega }\times \mathit{J}\mathit{\omega }+\tau +a\mathit{J}\frac{d}{dt}\left(sat\left(q_e\right)\right)\right)\hfill \\ \hfill & ={\mathit{s}}^T\left(-c\mathit{J}\mathrm{sgn}\left(\mathit{s}\right){\left|\mathit{s}\right|}^{\beta }\right)\hfill \\ \hfill & <0\hfill \end{array}$$

This expression is negative definite, and thus, it can be concluded that the control system is Lyapunov stable.

5. Simulation Study

The simulation is compared with the RRT, RRT*, and biased RRT to verify the performance of the proposed algorithm. Each algorithm ends the simulation when it creates a tree within a certain radius of $q_f$ or ends when the number of trees is 1000. Note that the proposed algorithm applies the Dubins path to generate a path that takes into account the direction of flight and the minimum turning radius. To compare algorithms under the same conditions as the proposed algorithm, torus structures are set as obstacles at the initial and final points, as shown in Figure 7, and paths are generated considering flight direction and minimum radius of rotation. For the reasonable analysis, it was simulated 100 times. For each algorithm, $\epsilon$ is 30 m, and the simulation termination condition $R_{threshold}$ is 50 m. The $R^{*}$ of the RRT* and the proposed algorithm is $\epsilon \times 8$ m, and k of the biased RRT is set to 10%. Since short and fast path generation is important, the simulation analysis is made with respect to the number of tree generations, path length, and calculation time. In this work, the concept of average precision is used to quantitatively compare the performance of two different algorithms for performance analysis [13].

In the 3D space, one proceeded with the simulation in three cases, as shown in Figure 8.

The states of the position and direction of each obstacle case are shown in

Table 1. Way-points for simulation.

No Obs				Center Obs/Scattered Obs
$q_s$	$q_f$	$v_s$	$v_f$	$q_s$	$q_f$	$v_s$	$v_f$
$\left[\begin{array}{c}500\\ 500\\ 0\end{array}\right]$m	$\left[\begin{array}{c}1000\\ 1000\\ 100\end{array}\right]$m	$\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]$	$\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]$	$\left[\begin{array}{c}100\\ 100\\ 0\end{array}\right]$m	$\left[\begin{array}{c}1000\\ 1000\\ 100\end{array}\right]$m	$\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]$	$\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]$

. The performance of the proposed algorithm is verified using the fixed-wing UAV with a minimum turning radius r of 100 m as a sliding mode controller considering the guidance algorithm and angular velocity limit [26].

5.1. Simulation Analysis (100 Times)

Figure 9, Figure 10 and Figure 11 show each algorithm’s result performed 100 times for the three cases. Here, (a,e,i,m) is the path generation result, (b,f,j,n) is the total number of nodes, (c,g,k,o) is the path length, and (d,h,l,p) is the calculation time.

The existing algorithms encountered a local minima with about 51% no obstacles, about 53% for center obstacles, and about 49% for scattered obstacles. Further, the minimum radius of rotation has not been taken into account. On the other hand, the proposed algorithm considered the flight direction and the minimum rotation radius yielding a local minima-free path. However, there is a risk of falling into the local minima in the case of a space where it is unable to turn correctly because the path suggested is generated by considering the minimum dynamic turning radius of UAVs.

Figure 12 represents the summary of the results. In all the cases, the proposed algorithms generated the overwhelmingly fewest nodes and generated paths. Further, the proposed algorithm for all obstacle cases showed the least computational burden. The proposed algorithm reduced the calculation time with respect to other algorithms in the no obstacles case by about 99.6%. In the case of the center obstacle, the calculation time of the proposed algorithm with respect to the RRT, RRT*, and biased RRT were reduced by 75.2%, 97.2%, and 72.6%, respectively. In the case of the scattered obstacles, the calculation time of the proposed algorithm with respect to the RRT, RRT*, and biased RRT were reduced by 27.6%, 91.8%, and 26.1%, respectively.

It is important to note that the proposed algorithm produced the shortest path in all obstacle cases. In the case of no obstacle, the path length using the proposed algorithm with respect to the RRT, RRT*, and biased RRT were reduced by 23.8%, 5%, and 21.7%, respectively. In the case of the center obstacle, the path length using the proposed algorithm with respect to the RRT, RRT*, and biased RRT were reduced by 19.6%, 0.5%, and 15.9%, respectively. In the case of the scattered obstacles, the path length using the proposed algorithm with respect to the RRT, RRT*, and biased RRT were reduced by 21.4%, 2%, and 18%, respectively.

It was confirmed that the proposed algorithm improved the performance effectively in both path length and calculation time compared to the three widely used algorithms, and the path was generated in consideration of the flight direction and minimum radius of rotation.

5.2. Simulation of UAV Path Tracking

Figure 13 shows the case of no obstacles. In the absence of obstacles, the path with the fastest computational time out of 100 simulations is generated in Figure 13a, and the generated path was tracked using a fixed-wing UAV. In the case of a position error, as shown in Figure 13b, the tracking performance is satisfactory, except in the beginning. An error occurs when the path is initiated in a circle; this error is likely a natural error caused by a sudden change in the attitude command due to the change in maneuvering. Nevertheless, the maximum position error is less than 5 m, and the straight line segment corresponds to an error of approximately ${10}^{-2}$. Figure 13c shows the attitude command value generated through the guidance algorithm and the actual attitude value of the UAV. As shown in Figure 13d, an error occurs during the circular maneuver, similar to the position error. This error is also likely a natural error due to the sudden change in the posture command due to the change in maneuvering, and an error of approximately ${10}^{-7}$ is observed in the straight line segment. Figure 13e shows the angular velocity of the UAV while satisfying the maximum angular velocity value range set by the CSMC, in accordance with the angular velocity limitation.

Figure 14 shows the case of the center obstacle. In the case of a center obstacle, the path with the fastest computational time out of 100 simulations was generated in Figure 14a, and the generated path was tracked using a fixed-wing UAV.

Figure 15 shows the case of scattered obstacles. In the case of scattered obstacles, the path with the fastest computational time out of 100 simulations was generated in Figure 15a, and the generated path was tracked using a fixed-wing UAV.

It is found that the result where obstacles exist is similar to the case where there is no obstacle. The maximum position error is within 5 m, and it showed an error of ${10}^{-2}$ when in a straight line segment. The maximum attitude error is within 23${}^{\circ }$, and it showed an error of ${10}^{-7}$ when in a straight line segment.

6. Conclusions

In this study, the authors solved the problem of generating a path to avoid obstacles to the target point without collision. This work proposes the Dubins path-oriented RRT* algorithm, which has improved optimality and convergence over the conventional RRT algorithm and considers the flight direction and minimum turning radius according to the characteristics of the fixed-wing UAV. In order to improve optimality, the Dubins path was oriented, and the RRT* algorithm concept was added. In order to improve convergence, the target point was set as a sample node, and when there were obstacles, the randomly generated sample node was selectively set to solve the problem of convergence degradation. The performance of the proposed algorithm was verified by comparing 100 simulations of each algorithm under the same conditions. In the three obstacle cases, the proposed algorithm has improved path length and calculation time compared to the existing algorithm. Finally, using the guidance algorithm and the controller, one validated whether the path generated by the proposed algorithm followed the fixed-wing UAV. Consequently, the technology proposed in this paper is considered suitable for smooth missions with the safety and reliability of unmanned aerial vehicles and is suitable for actual applications. Future works will include experimental validation of the proposed algorithm.