3D trajectory planning based on the Rapidly-exploring Random Tree–Connect and artificial potential fields method for unmanned aerial vehicles

Abstract

This research proposes a multifaceted approach of three-dimensional trajectory planning based on the combination of Rapidly-exploring Random Tree–Connect algorithm and artificial potential field method to improve the path search ability and dynamic obstacles avoidance capability of unmanned aerial vehicles. Firstly, an improved method of the target gravity is developed by controlling the sampling range to reduce invalid sampling and speed up the convergence speed of the algorithm so as to lessen the restriction of low efficiency and random sampling of the Rapidly-exploring Random Tree–Connect algorithm. Moreover, the regulation factor is introduced into the artificial potential field method to deal with the problem of target unreachable in the trajectory planning. Then the improved Rapidly-exploring Random Tree–Connect algorithm is implemented to plan the global path in a complex environment. This step is carried out via selecting the local target point on the global path found in the global plan, dividing the complex environment into simple environment and utilizing the artificial potential field method to achieve the effect of avoiding unknown dynamic obstacles in the simple environment. Finally, cubic B-spline is employed to smoothing of the planned trajectory. The simulation results demonstrate that the combination of two improved algorithms improves the path search ability and dynamic barrier avoidance capability of the unmanned aerial vehicles.

Keywords

Trajectory planning UAVs RRT-Connect artificial potential field method cubic B-spline

Introduction

Unmanned aerial vehicles (UAVs) have several advantages such as the advancement of small volume, cost efficiency, low requirements for combat environment and precise battlefield survivability. Thus, UAVs plays an irreplaceable role in executing tasks in high-risk environments.

Path planning for UAVs is to find the optimal or suboptimal path under certain constraints. Trajectory planning is one of the key issues in UAVs mission planning.¹ Conventional path planning methods include, but are not limited to, genetic algorithm,² fuzzy rule method,³ artificial neural network,⁴ simulated annealing algorithm,⁵ ant colony optimization algorithm,⁶ and particle swarm optimization algorithm.⁷ Although these methods have certain advantages in solving general path planning problems, they have several deficiencies associated with applying to nonholonomic constraint planning problems. These methods need to model obstacles in a certain space,⁸ while Rapidly-exploring Random Tree (RRT) algorithm^9,10 tends to expand to open unexplored areas. As long as the time is long enough and the number of iterations is large enough, there will be no unexplored areas. RRT algorithm is widely used in path planning. RRT algorithm does not need to model the space and has fast search speed. It is suitable for solving the path planning problem of UAVs in high-dimensional space and complex environment. However, the search space based on RRT is blind and the node expansion link lacks memory. A number of studies suggested application of RRT-Connect algorithm^11
–13 to improve the search efficiency in the space. RRT-Connect algorithm is a motion planning algorithm based on sampling. It adds the guidance strategy of two tree two-way search on the basis of RRT algorithm, and adds the greedy strategy on the basis of growth mode, which substantially improves the search speed and efficiency of RRT-Connect algorithm compared with RRT algorithm. However, RRT-Connect algorithm has some drawbacks partly due to the randomness of RRT-Connect algorithm. For example, instability, that is, repeated planning for the same task will lead to different paths. There is no directionality and the convergence speed is slow.

The artificial potential field method can be used to solve the local path planning problem because of its simple principle and fast path planning speed.¹⁴ However, it is easy to fall into the local minimum during path planning, resulting in oscillation or flight termination.^15,16 In the potential field method, the potential function carries the information of obstacles and targets. If such information can be transmitted to RRT-Connect, the RRT-Connect algorithm would tend to move forward along the direction of the potential field when exploring the space. At the same time, the excellent exploration ability of RRT-Connect can just make up for the disadvantage that the potential field method is easy to fall into local minima. To the above end, the intent of this research was to develop an improved algorithm combining RRT-Connect algorithm and artificial potential field method for path planning. This framework was established based on hierarchical idea,¹⁷ which was the novelty of this research, to mitigate complexity of the environment. The path planning of UAVs is divided into two layers: upper and lower layers. The upper layer carries out global path planning and uses RRT connect algorithm to identify a global path. The lower layer carries out local path planning for dynamic obstacles in the environment on the global path planned by the upper layer, and this layer employs the artificial potential field method to optimize the local path.

The specific contributions of this research are:

Hierarchical planning method is proposed for path planning method of UAVs in complex three-dimensional space.

RRT-Connect algorithm and artificial potential field method are combined to plan the path in complex environment so as to improve the path search ability and dynamic obstacle avoidance ability of UAVs.

RRT-Connect algorithm and artificial potential field are also improved to achieve appropriate planning effects.

The overall structure of this article is as follows: In introduction, the relevant algorithms, research novelty, main contributions, and the overall structure of the article is introduced. The first section describes the related work. The second section proposes the problem and establishes following models. The third section introduces the method used in global path planning and its improvement. The fourth section introduces the methods used in local path planning and the improvement of this method. In the fifth section, the two improved algorithms are combined with the idea of hierarchical programming. The sixth section gives the mathematical basis for the convergence analysis of the combined algorithm. The experimental results are given in the seventh section. Finally, the conclusion and discussion are summarized in the eighth section.

Related work

In the past decade, many researchers have explored trajectory planning. The main related works in this field are listed below.

Hebecker et al.¹⁸ discretized the field of view of UAV sensor into grid map, and then used wavefront algorithm to realize local three-dimensional path planning based on the distribution of obstacles in grid map. Li et al.¹⁹ proposed an adaptive particle swarm optimization algorithm with different learning strategies for finding the optimal path of mobile robot in complex environment. The search strategy is adjusted at different stages of the search process, which improves the search ability of particle swarm optimization algorithm. Radmanesh et al.²⁰ proposed that the algorithm of UAV autonomous track needs to obtain accurate information about the environmental state. Therefore, a UAV trajectory optimization algorithm based on wolf swarm algorithm is designed, an efficient Bayesian form is used, and the unit weighting concept based on distance value function is used to achieve good results. Steiner et al.²¹ proposed a reactive obstacle avoidance path planning method for UAV based on open sector, which designed a series of avoidance rules according to the two-dimensional scanning information of airborne lidar and the short-term memory information of UAV’s past maneuver behavior. Wei Nianwei et al.²² designed DP algorithm to extract the key nodes of the initial trajectory generated by PRM algorithm, and then smoothed it with clothoid curve to achieve the purpose of path planning. Lindqvist et al.²³ used the nonlinear model predictive control method to directly generate the control input to avoid maneuver. Wu et al.²⁴ combined the potential field class path planning method with the receding horizon control (RHC) strategy to online optimize the parameters of the potential field class method through the RHC strategy to deal with the complex and changeable obstacle environment. Hu Qiang et al.²⁵ combined the dubins curve with A* algorithm and considered the angle constraint of the autonomous underwater vehicle (AUV) to apply it to path planning. Li Shichang¹⁷ proposed a path planning algorithm based on A* and artificial potential field method according to the flight characteristics of four rotor UAV and the demand of avoiding obstacles in real time. We take inspiration from Li Shichang¹⁷ and introduce the idea of hierarchical planning for trajectory planning in complex environment, and realize the global path planning based on RRT connect algorithm. We also improve the force field function for the local minimum problem of the artificial potential field method, and realize the local path planning based on the artificial potential field method.

Problem and modeling

The following models are established for the kinematics of UAVs, the constraints of UAVs and dynamic obstacles, and the constraints of UAVs and the local target points.

Kinematics constraints of UAVs

During the flight, the kinematics equation of a UAV can be expressed as

\{\begin{cases} x_{t} = x_{t - 1} + Δ T v_{t} cos θ_{t} cos ψ_{t} \\ y_{t} = y_{t - 1} + Δ T v_{t} cos θ sin ψ_{t} \\ z_{t} = z_{t - 1} + Δ T v_{t} sin θ \end{cases}

where v_t is the movement speed of the UAV at $t - 1 \sim t$ ; $Δ T$ is the time interval at $t - 1 \sim t$ ; $θ_{t}$ is the pitch angle of the UAV’s movement direction at $t - 1 \sim t$ ; $ψ_{t}$ is the yaw angle of the UAV’s movement direction at $t - 1 \sim t$ .

Let G_t be the position coordinate of the UAV at t; $G_{t - 1}$ be the position coordinate of the UAV at $t - 1$

G_{t} = {[x_{t}, y_{t}, z_{t}]}^{T}

G_{t - 1} = {[x_{t - 1}, y_{t - 1}, z_{t - 1}]}^{T}

\{\begin{cases} v_{t} \leq v_{max} \\ | θ_{t} - θ_{t - 1} | \leq θ_{max} \\ | ψ_{t} - ψ_{t - 1} | \leq ψ_{max} \end{cases}

where $v_{max}$ is the maximum flight speed of the UAV; $θ_{max}$ is the maximum pitch angle; $ψ_{max}$ is the maximum yaw angle.

Distance constraints between UAVs and dynamic obstacles

To safely fly to the target location, the UAV needs to maintain a proper safe distance from dynamic obstacles. Suppose that at time t, the coordinate point of the UAV is $X (t)$ , and the coordinate point of the kth dynamic obstacle is $X_{k} (t)$ , then the coordinates between the UAV and the dynamic obstacle must satisfy the following relationship

|| X (t) - X_{k} (t) || \geq d_{min}, k \in {1, \dots, n}

where $|| X (t) - X_{k} (t) ||$ is the Euclidean distance between the UAV and the kth dynamic obstacle, and $d_{min}$ is the safety distance.

Distance constraints between UAVs and local target point

In the local trajectory planning of the UAV, the distance between the UAV and the local target point needs to be considered. The local target points selected in this research are all nodes of the optimal path found in the global plan. When the UAV is between the first node and the second node, the second node is selected as the local target point; when the UAV is between the second node and the third node, the third node is selected as the local target point; in the same way, select the fourth node, the fifth node,…, the last node as the local target point respectively. Since the local target point is determined by the current position of the UAV, the distance between the UAV and the local target point is assumed to be $d_{i j}$ , where $i = 1, 2, \dots, n - 1, j = i + 1$ , n is the number of nodes in the global path. For example, when, $i = 3, j = 4$ , the fourth node of the global path is selected as the local target point, and the distance between the UAV and the local target point is $d_{34}$ . Therefore, the distance constraint between the UAV and the local target point should satisfy

| | X_{j} - X_{i} | | \geq d_{i j}

where $| | X_{j} - X_{i} | |$ represents the Euclidean distance from the j node to i node.

When the UAV flies near the local target position, the selection of the local target point is easy to be confused thereby reset the constraint distance between the UAV and the local target point. Take the current local target point as the center and make a circle with a radius of r, where r is infinitesimal. When the UAV is in the circle, the current local target point is selected as the target point, otherwise the next node is selected as the local target point.

Global path planning based on RRT-Connect algorithm

Basic RRT-Connect algorithm principle

The RRT-Connect algorithm²⁶ first initializes two trees T ₁ and T ₂. T ₁ and T ₂ respectively expand toward each other until the two trees meet, even if they successfully find a trajectory; but if they do not meet after reaching the set number of N iterations, it is judged that the path planning has failed. The algorithm flow chart is shown in Figure 1.

Figure 1.

The flowchart of RRT-Connect algorithm.

Improved RRT-Connect algorithm design

The basic RRT-Connect algorithm is improved²⁷ for reducing excessive exploration of useless space and reduce planning time.

In the selection of random tree nodes $b_{r a n d}$ , the attraction of target points is introduced, and the range of random tree node selection is controlled within the rectangular range of the UAV position and the target position as diagonal points, in order to prevent the obstacle range between the two from exceeding the rectangular range, making it impossible to plan a suitable trajectory, and an expandable margin r is set for the range

b_{r a n d} = r a n d (b_{g o a l} - b_{n e a r e s t} + r)

In order to obtain the optimal trajectory, the idea of target attraction is introduced in the selection of $b_{n e w}$ , and the selection of $b_{n e w}$ is determined as

b_{n e w} = b_{n e a r e s t} + λ_{1} \frac{b_{r a n d} - b_{n e a r e s t}}{| b_{r a n d} - b_{n e a r e s t} |} + λ_{2} \frac{b_{g o a l} - b_{n e a r e s t}}{| b_{g o a l} - b_{n e a r e s t} |}

where $λ_{1}$ is the step length of the random tree expansion in the direction of any point, $λ_{2}$ is the step length of the random tree expansion in the direction of the target point, ${|b}_{g o a l} {-b}_{n e a r e s t} |$ is the Euclidean distance between the target point $b_{g o a l}$ and the growth point $b_{n e a r e s t}$ (the closest point in the tree), when the tree grows, $b_{g o a l} = b_{i n i t}$ .

After adding the target gravitational function in the basic RRT algorithm, the selection of $b_{new}$ is inclined to $b_{g o a l}$ . When $λ_{1}$ is larger, the randomness of $b_{new}$ is stronger, when $λ_{2}$ is larger, the deviation of $b_{new}$ to the target is stronger. The construction process is shown in Figure 2.

Figure 2.

The principle of improve RRT-Connect.

When the distance between $b_{n e w}$ and $b_{n e w}^{'}$ of the two trees $T_{1}, T_{2}$ is less than the minimum growth step length $min {λ_{1}, λ_{2}}$ , the exploration of the new node is ended, and the shortest path is recorded as the desired trajectory.

Since the UAV has a maximum speed $v_{max}$ constraint, there are corresponding constraints on the step length of the random tree expansion

max {λ_{1}, λ_{2}} < v_{max} t

where t is the time it takes for the UAV to move from the current node to the next node. Since the UAV’s trajectory planning has the constraints of the maximum pitch angle $θ_{max}$ and the maximum yaw angle $ψ_{max}$ , if the selected angle between $b_{n e w}$ and $b_{n e a r e s t}$ and $b_{n e a r e s t}$ and the previous node is greater than the maximum yaw angle $ψ_{max}$ in the horizontal plane or greater than the maximum pitch angle $θ_{max}$ in the vertical plane, the $b_{r a n d}$ is reselected.

Local path planning based on artificial potential field method

The basic principle of traditional artificial potential field method

The traditional artificial potential field (TAPF) assumes that the UAV moves under a virtual force field; obstacles repel the UAV, and the target point generates gravitation on the UAV, and the combined force of the gravitational and repulsive forces guides the UAV forward and avoids obstacles in the environment.²⁸

Gravitational potential field function

|| {\vec{U}}_{a t t} ||= \frac{1}{2} K_{a t t} d^{2} (P, P_{g o a l})

In the formula, $K_{a t t}$ represents the gravitational gain constant, $d (P, P_{g o a l})$ represents the distance between the current position of the UAV and the target point.

Assuming that the UAV is moving in a three-dimensional space, let $P = (x, y, z)$ be the coordinates of the current position, $P_{g o a l} = (x_{g o a l}, y_{g o a l}, z_{g o a l})$ be the coordinates of the target point. Gravity is the negative gradient of the gravitational potential field

|| {\vec{F}}_{a t t} {||=K}_{a t t} d (P, P_{g o a l})

The gravitational direction angle $θ_{a t t}$ is the angle between the straight line pointing from the current position of the UAV to the target point and the horizontal straight line to the right, as shown in Figure 3.

Figure 3.

The UAV under the artificial potential field method is being sought.

Repulsion potential field function

|| {\vec{U}}_{r e p^{k}} ||= \{\begin{cases} \frac{1}{2} K_{r e p} (\frac{1}{d (P, P_{o^{k}})} - \frac{1}{d_{o}})^{2}, d (P, P_{o^{k}}) \leq d_{o} \\ 0, d (P, P_{o^{k}}) > d_{o} \end{cases}

In the formula, $K_{r e p}$ represents the repulsion gain constant; k represents the kth obstacle; d_o represents the effective range radius of the obstacle; $d (P, P_{o^{k}})$ represents the distance between the drone and the kth obstacle, and $P_{o^{k}} = (x_{o^{k}}, y_{o^{k}})$ represents the current obstacle coordinates. The repulsion is the negative gradient of the repulsion potential field

|| {\vec{F}}_{r e p^{k}} ||=-|| \nabla {\vec{U}}_{r e p^{k}} ||= \{\begin{cases} K_{r e p} (\frac{1}{d (P, P_{o^{k}})} - \frac{1}{d_{o}}) \frac{1}{d^{2} (P, P_{o^{k}})}, d (P, P_{o^{k}}) \leq d_{o} \\ 0, d (P, P_{o^{k}}) > d_{o} \end{cases}

The repulsive force direction angle $θ_{r e p}$ is the angle between the straight line pointing from the obstacle point to the current position of the UAV and the horizontal right straight line, as shown in Figure 3.

Resultant force field

| | \vec{U} | | = | | {\vec{U}}_{a t t} + {\vec{U}}_{r e p^{k}} | |

|| \vec{F} ||=-|| \nabla \vec{U} ||

The direction angle $θ$ of the resultant force is shown in Figure 3.

Improved artificial potential field method

In order to solve the problem of unreachable target, the repulsion potential field function is improved^29,30

|| {\vec{U}}_{r e p^{k}} ||= \{\begin{cases} \frac{1}{2} K_{r e p} (\frac{1}{d (P, P_{o^{k}})} - \frac{1}{d_{o}})^{2} d^{n} (P, P_{g o a l}), d (P, P_{o^{k}}) \leq d_{o} \\ 0, d (P, P_{o^{k}}) > d_{o} \end{cases}

This repulsive force field function introduces an adjustment factor. When the target and obstacles are close to each other, it can avoid the situation that the repulsive force is too large to bypass the target. This article will use an improved repulsive force field function.³¹ The repulsion function is as follows

|| {\vec{F}}_{r e p^{k}} ||=-|| \nabla {\vec{U}}_{r e p^{k}} ||= \{\begin{cases} | | {\vec{F}}_{r e p^{k}}^{[1]} + {\vec{F}}_{r e p^{k}}^{[2]} | |, d (P, P_{o^{k}}) \leq d_{o} \\ 0, d (P, P_{o^{k}}) > d_{o} \end{cases}

|| {\vec{F}}_{r e p^{k}}^{[1]} ||=- \frac{n K_{r e p}}{2} {(\frac{1}{d (P, P_{o^{k}})} - \frac{1}{d_{o}})}^{2} d^{n - 1} (P, P_{g o a l})

|| {\vec{F}}_{r e p^{k}}^{[2]} ||= K_{r e p} (\frac{1}{d (P, P_{o^{k}})} - \frac{1}{d_{o}}) \frac{d^{n} (P, P_{g o a l})}{d^{2} (P, P_{o^{k}})}

It can be found that there are $d (P, P_{g o a l})$ and $d (P, P_{o^{k}})$ two variables in the repulsion potential field, so the repulsion is composed of two components ${\vec{F}}_{r e p^{k}}^{[1]}$ and ${\vec{F}}_{r e p^{k}}^{[2]}$ . The angles of the two components are different. As shown in Figure 4, the direction angle $θ_{r e p^{k}}^{[1]}$ of the component ${\vec{F}}_{r e p^{k}}^{[1]}$ is the angle between the straight line from the drone pointing to the target point and the straight line horizontal to the right, while the direction angle $θ_{r e p^{k}}^{[2]}$ of the component ${\vec{F}}_{r e p^{k}}^{[2]}$ is the angle between the straight line pointing from the obstacle to the UAV and the horizontal straight line to the right.

Figure 4.

The UAV under improving artificial potential field methods is being sought.

The combined repulsion force is the sum of the repulsion forces of all obstacles in the environment

|| {\vec{F}}_{rep} ||=|| \sum_{k = 1}^{n} {\vec{F}}_{{rep}^{k}} ||

where n is the number of obstacles in the environment. As shown in Figure 4, the resultant force received by the UAV in the entire potential field is $\vec{F}$ .

Hierarchical planning approach

As far as the function of UAV avoiding dynamic obstacles in complex environments,³² the artificial potential field method is widely used.^33,34 However, considering advantages of artificial potential field for local path planning in a simple environment, this article adopts the hierarchical idea for path planning.

The upper layer uses the RRT-Connect algorithm to plan the global path; then on the found global path, the artificial potential field method is used to plan the lower layer local path.

After finding a better global path with the RRT-Connect algorithm, the path node is regarded as the center coordinate, and a safe area for drone flight is established around it. As shown in Figure 5, the dots in the figure are the planned trajectory nodes, the dotted line is a circle drawn with the path node as the center and the safety distance $d_{min}$ as the radius. $λ$ is half of the minimum step $λ = min {λ_{1}, λ_{2}} / 2$ , thus $r = \sqrt{d_{min}^{2} - λ^{2}}$ . Once the dynamic obstacle appears in the red line area in Figure 5, an artificial potential field is established for the dynamic obstacle. Then according to the assumptions in Distance constraints between UAVs and local target point, the appropriate local target points are determined, and the local path planning is carried out.

Figure 5.

The flight safety zone of the UAV.

However, the generated trajectory at this time is a polyline. Ultimately, the last step is to perform cubic B-spline fitting smoothing³⁵ processing on the trajectory

P (t) = \sum_{i = 0}^{n} P_{i} F_{i, k} (t)

F_{i, k} (t) = \frac{1}{k!} \sum_{m = 0}^{k - i} {(- 1)}^{m} (\begin{array}{l} m \\ k + 1 \end{array}) {(t + k - m)}^{k}

where P is the current trajectory point $i = 1, 2, 3$ and $F_{i, k} (t)$ is the k-order B-spline basis function, where $k = 3$ is the factorial. By substituting the trajectory points $P_{0}, P_{1}, P_{2}, P_{3}$ , a section of cubic B-spline curve approximated by the four points can be obtained.

The flow chart of the implementation steps of the trajectory planning method is shown in Figure 6. In the figure, the included angle a is the included angle between $b_{n e w}$ and $b_{n e a r e s t}$ , and the included angle $a'$ is the included angle between $b_{n e w}$ and the previous node of $b_{n e a r e s t}$ .

Figure 6.

Trail planning flowchart.

The pseudo code of the whole algorithm is shown in Table 1.

Table 1.

The pseudo code of the algorithm.

1:	$T_{1} \leftarrow \{b_{t r i t}\}, E_{1} \leftarrow ϕ; T_{1} \leftarrow (T_{1}, E_{1})$
2:	$T_{2} \leftarrow \{b_{g o a l}\}, E_{2} \leftarrow ϕ; T_{2} \leftarrow (T_{2}, E_{2}); i = 0$
3:	$w h i l e i < N$
4:	$b_{r a n d} \leftarrow S a m p l e (i); b_{r a n d} = r a n d (b_{g o a l} - b_{n e a r e s t} + r); i = i + 1$
5:	$b_{n e a r e s t} \leftarrow N e a r e s t (T_{1}, b_{r a n d})$
6:	$b_{n e w} \leftarrow S t e e r (b_{n e a r e s t}, b_{r a n d}, b_{g o a l})$
7:	$i f a n g l e S a t i s f y (θ_{max}, ψ_{max})$
8:	$r e t u r n (T_{1}, T_{2})$
9:	$e l s e$
10:	$i f o b s t a c l e F r e e (b_{n e a r e s t}, b_{n e w})$
11:	$r e t u r n (T_{1}, T_{2})$
12:	$e l s e$
13:	$T_{1} \leftarrow T_{1} \cup \{b_{n e w}\}$
14:	$E_{1} \leftarrow E_{1} \cup \{(b_{n e a r e s t}, b_{n e w})\}$
15:	$b_{n e a r e s t}^{'} \leftarrow N e a r e s t (T_{2}, b_{r a n d})$
16:	$b_{n e w}^{'} \leftarrow S t e e r (b_{n e a r e s t}^{'}, b_{n e w}, b_{t r i t})$
17:	$i f a n g l e S a t i s f y (θ_{max}^{'}, ψ_{max})$
18:	$r e t u r n (T_{1}, T_{2})$
19:	$e l s e$
20:	$i f o b s t a c l e F r e e (b_{n e a r e s t}^{'}, b_{n e w}^{'})$
21:	$r e t u r n (T_{1}, T_{2})$
22:	$e l s e$
23:	$T_{2} \leftarrow T_{2} \cup \{b_{n e w}^{'}\}$
24:	$E_{2} \leftarrow E_{2} \cup \{(b_{n e a r e s t}^{'}, b_{n e w}^{'})\}$
25:	$i f D i s t a n c e (b_{n e w}, b_{n e w}^{'})$
26:	$p a t h \leftarrow \{(k_{1}, k_{2}, \dots, k_{n})\}$
27:	$e l s e$
28:	$r e t u r n (T_{1}, T_{2})$
29:	$k_{n}^{'} \leftarrow N e a r e s t (k_{1}, k_{2}, \dots, k_{n})$
30:	$i f d y n a m i c O b s t a c l e (A)$
31:	$r e t u r n (k_{n}^{'})$
32:	$e l s e$
33:	$s m o o t h (p a t h)$
34:	$e n d$

Theoretical analysis of combination algorithm

In any state space, if an algorithm can converge to the feasible solution, it is said that the algorithm has probability completeness, and if it can find the optimal solution in finite iterations, it is said that the algorithm has asymptotic optimality. This section provides the mathematical basis for the convergence proof of the proposed combination algorithm.^36,37

Probabilistic completeness

The definition of probabilistic completeness is that if an algorithm has probabilistic completeness, there is

\underset{n \to \infty}{lim inf} P ({\exists b_{g o a l} \in T_{n}^{A L G} \cap X_{g o a l} s u c h t h a t b_{t r i t} i s c o n n e c t e d t o b_{g o a l} i n G_{n}^{A L G}}) = 1

for any path planning problem $(X_{f r e e}, b_{t r i t}, X_{g o a l})$ with feasible solution. Where $X_{f r e e} = X \ X_{o b s}$ is the barrier free area; $X_{o b s}$ is the obstacle area; $X_{g o a l}$ is the target area; $b_{t r i t}$ is the initial position and an element in $X_{f r e e}$ ; $b_{g o a l}$ is the target location.

Through the analysis of the combined algorithm, it can be obtained that for any path planning problem $(X_{f r e e}, b_{t r i t}, X_{g o a l})$ with feasible solution, there is always a constant $a > 0, n_{0} \in N$

P ({T_{n} \cap X_{g o a l} \neq ϕ}) > 1 - e^{- a n}, \forall n > n_{0} .

In other words, when $n_{0} \in N$ is large enough, $\forall n > n_{0}$ . Can make $\exists b_{g o a l} \in T_{n} \cap X_{g o a l}$ , which means $b_{t r i t}$ can be connected to $b_{g o a l}$ .

Asymptotic optimality

Let $c : \sum X_{f r e e} \to R \geq 0$ be a function, called cost function, which assigns a nonnegative cost to all collision free paths. The optimality problem of path planning is to find a feasible path with the lowest cost.

The definition of asymptotic optimality is that if an algorithm has asymptotic optimality, there is an optimal solution for any path planning problem $(X_{f r e e}, b_{t r i t}, X_{g o a l})$ , and its cost function $c : \sum X_{f r e e} \to R \geq 0$ will be equal to a finite path cost, that is

P ({\underset{n \to \infty}{lim sup} Y_{n} = c^{*}}) = 1

$σ^{*}$ represents an optimal path. For $n \in N$ , construct a cuboid with sequence ${B_{n}}_{n \in N}$ to cover $σ_{n}$ , which is recorded as $B_{n} = {B_{n, 1}, B_{n, 2}, \dots, B_{n, M_{n}}} : = cov e r i n g Cuboid (σ_{n}, r_{n})$ , where r_n is the diagonal of the cuboid whose current position and target position are the diagonal points respectively

r_{n} = | | b_{n e w} - b_{g o a l} {||}_{m}

For $m \in {1, 2, \dots, M_{n}}$ , $A_{n, m}$ event is defined as the existence of two nodes $b_{i}, b_{i^{'}} \in T_{n}$ , where $b_{i} \in B_{n, m}, b_{i^{'}} \in B_{n, m + 1}$ and $Y_{i^{'}} < Y_{i}$ . In this case, b_i and $b_{i^{'}}$ will be connected to the boundary of feasible path graph G_n .

Fast convergence to the optimal path

Given a random node $b_{r a n d}$ and the minimum path cost functions $σ_{a} [0, s_{a}] : = \{σ_{a} (0) = b_{t r i t}^{a}, σ_{a} (s_{a}) = b_{r a n d}\}$ and $σ_{b} [0, s_{b}] : = \{σ_{b} (0) = b_{t r i t}^{b}, σ_{b} (s_{b}) = b_{r a n d}\}$ . The sample insertion process of the combined algorithm can be summarized as ${b_{r a n d} \in T_{a} : c (σ_{a}) \leq c (σ_{b})}$ or ${b_{r a n d} \in T_{b} : c (σ_{b}) < c (σ_{a})}$ , that is, the random node $b_{r a n d}$ is always inserted into the nearest random tree. This measure ensures that the random node $b_{r a n d}$ is always inserted into the area with high density of adjacent nodes. The combined algorithm inserts nodes into the adjacent areas with high density, so that the reorganized search tree plays the greatest role in each cycle. At the same time, the combined algorithm grows two random trees at the same time, so that the convergence speed of the combined algorithm is much faster than that of RRT.

Simulation results and analysis

Hierarchical planning experiment

The processor of the hardware platform used in the simulation experiment is Intel(R) Core(TM) i7-6700HQ CPU @ 2.60 GHz, the memory is 8 GB; the analysis software is Matlab 2020b.

In the environment of $100 \times 100 \times 100$ as shown in Figure 7, there are nine sphere obstacles distributed, assuming that the coordinates of the starting point of the drone flight is $(5, 5, 5)$ and the coordinates of the target point is $(95, 95, 95)$ , respectively, the basic RRT-Connect algorithm and the improved RRT-Connect algorithm are used for path planning. The experimental results are as follows:

Figure 7.

Effect diagram comparison. (a) Effect diagram of basic RRT-Connect algorithm planning and (b) effect diagram of improved RRT-Connect algorithm planning.

Comparing the graph in Figure 7, it can be clearly seen that the improved RRT-Connect algorithm reduces the useless search in the blank area compared to the original algorithm. The red line segment in Figure 7 is the path of useless search.

Due to the randomness of the RRT-Connect algorithm, 50 simulation experiments were carried out in this environment. Compare the average running time of the original RRT-Connect algorithm and the improved RRT-Connect algorithm, the average number of nodes explored each time, and the average number of path nodes. The results are shown in Table 2.

Table 2.

The algorithm compares the indicators.^a

Algorithm	ART (s)	ANNEET	ANPN
Basic algorithm	0.01206	202.12	17.70
Improve algorithm	0.01003	132.56	15.37

^a ART is average running time, ANNEET is average number of nodes explored each time, and ANPN is average number of path nodes in the table, respectively.

According to the results in Table 2, the improved algorithm reduces the average number of exploration nodes by about 52%, effectively reduces invalid sampling, and shortens the average running time by about 20%, speeds up the search speed.

Our results indicated that UAV can reach the desired target location when the RRT-Connect algorithm has employed for planning a global path. Moreover, we proposed the artificial potential field technique for aiming at the dynamic obstacles encountered during the flight of the UAV. Each pair of adjacent nodes in the global path is locally planned to ensure the safety of the UAV flight.

Carry out 50 simulation experiments in the environment of $100 \times 100 \times 100$ , choose the best path in it, and use the nodes of this path as the local target points to plan the local path. There are 12 nodes in this path. The coordinates of the first four nodes are listed here as shown in Table 3.

Table 3.

The local path node.

Global path	First node	Second node	Third node	Fourth node
Node coordinates	(5.0571, 16.9569, 6.96017)	(12.247, 20.8125, 19.3427)	(13.6182, 24.2819, 32.2845)	(21.9802, 25.6271, 41.9848)

The starting point coordinates is $(5, 5, 5)$ , the 12 nodes on the global path are respectively used as local target points, and the artificial potential field method is used for local path planning. Figure 8 below is the simulation result of selecting the first four nodes as the local target points:

Figure 8.

Simulation results for the first four nodes. (a) First node, (b) second node, (c) third node, and (d) fourth node.

Figure 8‘s $a,b,c,d$ is a simple environment divided from the complex environment of $100 \times 100 \times 100$ . It can be seen intuitively from the figure that the artificial potential field method can accurately avoid dynamic obstacles in a simple three-dimensional environment.

Figure 9 shows the trajectory planning simulation through combining the two algorithms. The blue line is the trajectory planned by the combination of RRT-Connect and the artificial potential field; the red line is the trajectory smoothed by the cubic B-spline fitting of the combined algorithm; the dots are the dynamic obstacles. The Figure 9 illustrate that the path planned by the combined algorithm is a polyline and is not smooth; after cubic B-spline fitting, the trajectory is very smooth, and it has a high degree of fit with the original trajectory.

Figure 9.

The results of trajectory planning simulation. (a) 3D trajectory and (b) trajectory on X–Y plane.

Figure 10 shows the cost of each iteration of the best path, where the cost function is set to the time of each iteration. It can be seen from the figure that for this specific scenario, after thousands of iterations, the time cost of the best path of the combined algorithm decreases significantly, and finally converges to the best cost, which is about 15. According to the time cost convergence of the best path in the figure, it can be concluded that the combined algorithm improves the search speed of the path.

Figure 10.

The cost of the best paths in the combined algorithm.

Figure 11 shows the curvature of the track. It can be seen from the Figure 10 that the curvature value of the whole track is continuous, and when encountering obstacles, the UAV will bypass in order to avoid obstacles. At this time, the curvature of the track will suddenly change, as shown in the sharp point in the figure. Therefore, through the continuity and change of the curvature value of the track, it can be concluded that the proposed method can effectively avoid static and dynamic obstacles. At present, the research on avoiding static obstacles has been very mature, so avoiding dynamic obstacles is the focus and difficulty of the current research. It can be clearly seen in Figure 11 that the curvature of avoiding dynamic obstacles and avoiding static obstacles is continuous and changes reasonably. Therefore, the ability to avoid dynamic obstacles is proved.

Figure 11.

Curvature of track.

Table 4 shows the curvature of the cusps in the curvature graph. The curvature of cusps 2, 3, and 6 in the table is larger than that of other cusps. Because the corresponding path avoids dynamic obstacles and the dynamic obstacles are very close to the global planning path, the turning radius of UAV is small, so the curvature of planning path is large. The curvatures of cusps 1, 4, 5, 7, 8, 9, and 10 in the table are small. The obstacles avoided at the path corresponding to these curvatures are static obstacles or dynamic obstacles at a certain distance from the global path, which makes the turning radius of UAV larger and the curvature of the planned path smaller. In the experimental design, the volume of static obstacles is much larger than that of dynamic obstacles, so the turning radius of UAV when avoiding static obstacles is larger than that when avoiding dynamic obstacles. The path without avoiding obstacles has a curvature of 0 and moves along a straight line.

Table 4.

Curvature value of track.

Number	1	2	3	4	5
Curvature	0.043	0.42	0.81	0.14	0.09
Number	6	7	8	9	10
Curvature	0.25	0.17	0.12	0.15	0.20

Comparative experiment

This research also scrutinized difference between combined algorithm and the improved RRT-Connect algorithm to reveal the effectiveness of the algorithm. Due to the randomness of RRT-Connect algorithm, under the same obstacle environment, take $(5, 5, 5)$ as the starting position and $(95, 95, 95)$ as the target position, conduct 50 simulation experiments, and select the best four simulation results, as shown in Figure 12.

Figure 12.

Optimal four times simulation results. (a) Simulation result 1, (b) simulation result 2, (c) simulation result 3, and (d) simulation result 4.

The simulation results of these four times (Figure 12) are compared with the simulation results of the combined algorithm (Figure 9). The careful comparison showed that it can be seen the path planned with the combined algorithm was more reasonable and optimized.

Table 5 show the path length, algorithm calculation time and response time of the algorithm when the environment changes (dynamic obstacles) and the average cost of the path of these four simulation results and combined algorithm simulation results.

Table 5.

The performance of the combined algorithm is compared with the improved RRT-Connect algorithm.^a

Algorithm		PL (m)	ACT (s)	RT (s)	PC (−)
Improved RRT-Connect algorithm	a	170.10	0.05256	0.05256	297.3
	b	183.89	0.05301	0.05301	316.3
	c	202.72	0.05506	0.05506	312.8
	d	179.95	0.05269	0.05269	321.1
Combined algorithm		163.37	0.04943	0.02458	128.2

^a PL is the path length, ACT is the algorithm computation time, RT is the algorithm’s reaction time, and PC is the average cost of the path.

The path planning time of the combined algorithm includes global path planning time, local path planning time, and path smoothing time. The simulation results showed that the shortest calculation time of the four paths planned by the improved RRT-Connect algorithm was 0.05256s. The calculation time of the combined algorithm was shortened by about 6%, and from the average cost value of the path in the table, it can be seen that the cost value of the improved RRT-Connect algorithm is more than twice that of the combined algorithm. Therefore, our findings suggested that the combined algorithm improved the speed of path planning.

In terms of the length of the planned path, the length of the four paths planned by the improved RRT-Connect algorithm was 170.10, 183.89, 202.72, and 179.95 m, respectively. However, these four paths were not the best in comparison with the path length planned by the combined algorithm. Moreover, the path planned by the combined algorithm was more appropriate compared to the improved RRT-Connect algorithm.

The ability to avoid dynamic obstacles is the ability of the algorithm to adjust the path in time according to the changes of the surrounding environment. In the main, the shorter the calculation time of the algorithm could result in the stronger the obstacle avoidance ability. When the surrounding environment changes, the improved RRT-Connect algorithm needs to be re-planed the path according to the global environment, so the response time of the algorithm to the environment change is the time of path planning. For the combined algorithm, since the actual path planning is carried out by the artificial potential field method, the response time to environmental changes is the time of path planning by the artificial potential field method. The time of local path planning by the artificial potential field method is very short, only 0.02458s as shown in Table 5 Therefore, the combined algorithm can act precisely and smartly avoiding dynamic obstacles in comparison with is more precise than the improved RRT-Connect algorithm.

The findings of this research demonstrated that, the proposed combined algorithm yielded promising in the speed, length, and real-time of path planning. The combined algorithm can carry out efficiently global path planning and avoid dynamic obstacles. Therefore, this algorithm can be used in Multirotor UAV path planning.

Conclusion

The improved RRT-Connect algorithm proposed in this article can quickly expand toward the target point when trajectory planning in a space with no obstacles. When trajectory planning in a space with obstacles, by controlling the sampling range, after introducing the target gravity, it can also expand to the target point faster. The improved artificial potential field method proposed in this article can effectively avoid dynamic obstacles in the simple environment after dividing the complex environment of $100 \times 100 \times 100$ . In the end, the combination of the two improved algorithms realizes that the UAV has strong path search capabilities and dynamic obstacle avoidance capabilities at the same time.

This article mainly studies the avoidance of UAVs from dynamic and static obstacles in the environment and the reasonable route planning. Because UAVs has the ability of three-dimensional space movement, mobile robots with three-dimensional space movement ability, such as unmanned airship and unmanned underwater robot, can use this structure to deal with the problem of path planning. Robots with two-dimensional space motion ability, such as unmanned ship and unmanned vehicle, can be used after dimensionality reduction of the algorithm and model in this article.

Footnotes

Acknowledgments

The authors would like to thank Dr. Amir Reza Shahtahmassebi and the anonymous reviewers for their helpful suggestions.

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work is supported by Sichuan Science and Technology Program (No. 2020YJ0368), Zigong Science and Technology Program (No. 2019YYJC03), Graduate innovation fund project of Sichuan University of Science & Engineering (No. y2021066), Opening Project of Sichuan Province University Key Laboratory of Bridge Non-destruction Detecting and Engineering Computing (No. 2021QZJ01).

ORCID iDs

Lijia Cao

Lin Wang

Yang Liu

References

Gao

Wang

. Mixed population RRT algorithm for UAV path planning. Syst Eng Electr Technol 2020; 42(01): 101–107.

Chen

Hou

. Research of UAV track planning and dynamic obstacle avoidance algorithm. In: 2020 Chinese Automation Congress (CAC), Shanghai, China, 6–8 November 2020, pp. 301–306.

Kang

Zeng

Yao

, et al. RRT* algorithm under holonomic conditions for UAVs. In: IEEE international symposium on dependable, autonomic and secure computing (DASC), Fukuoka, Japan, 5–8 August 2019, pp. 716–720.

Cui

Wei

Liu

, et al. UAV motion strategies in uncertain dynamic environments: a path planning method based on Q-learning strategy. Appl Sci 2018; 8(11): 2169.

Xiao

Tan

Wang

. A simulated annealing algorithm and grid map-based UAV coverage path planning method for 3D reconstruction. Electronics 2021; 10(7): 853.

Guan

Gao

Bai

. Double-ant colony based UAV path planning algorithm. In: Proceedings of the 2019 11th international conference on machine learning and computing (ICMLC ‘19), 2019, pp. 258–262.

Jamshidi

Nekoukar

Refan

. Analysis of parallel genetic algorithm and parallel particle swarm optimization algorithm UAV path planning on controller area network. J Contr Auto Electr Syst 2020; 31(1): 129–140.

Pei

Yang

. Path planning algorithm for mobile robot based on improved RRT*. Comput Eng 2019; 45(05): 285–290.

Zhang

. A robot arm motion planning algorithm guided by cost function. Mech Sci Technol 2020; 39(01): 62–67.

10.

Taheri

Ferdowsi

Danesh

. Fuzzy greedy RRT path planning algorithm in a complex configuration space. Int J Contr Auto Syst 2018; 16(6): 3026–3035.

11.

Chen

. An improved path planning algorithm for UAV based on RRT. In: 2021 4th international conference on advanced electronic materials, computers and software engineering (AEMCSE), Changsha, China, 6–28 March 2021, pp. 895–898.

12.

Chen

Mantegh

, et al. Fuzzy kinodynamic RRT: a dynamic path planning and obstacle avoidance method. In: 2020 international conference on unmanned aircraft systems (ICUAS), Athens, Greece, 1–4 September 2020, pp. 188–195.

13.

Zhang

Yao

. An improved path planning algorithm for unmanned aerial vehicle based on RRT-Connect. In: 2018 37th Chinese control conference (CCC), Wuhan, China, 25–27 July 2018, pp. 4854–4858.

14.

Oussama

. Real-time obstacle avoidance for manipulators and mobile robots. Int J Robot Res 1986; 5(1): 90–98.

15.

Mabrouk

Mcinnes

. Solving the potential field local minimum problem using internal agent states. Robot Auto Syst 2018; 56(12): 1050–1060.

16.

Zhang

Song

. Study on the local minima problem of path planning using potential field method in unknown environments. J Auto 2010; 36(08): 1122–1130.

17.

. Research on path planning algorithm of quadrotor based on A* and artificial potential field. Harbin: Harbin Institute of Technology, 2017.

18.

Tanja

Robert

Frank

. Model-based local path planning for UAVs. J Intell Robot Syst 2015; 78(1): 127–142.

19.

Guangsheng

Wusheng

. Path planning for mobile robot using self-adaptive learning particle swarm optimization. Sci China (Information Sciences) 2018; 61(05): 267–284.

20.

Mohammadreza

Manish

Mohammad

. Grey wolf optimization based sense and avoid algorithm in a Bayesian framework for multiple UAV path planning in an uncertain environment. Aerospace Sci Technol 2018; 77: 168–179.

21.

Steiner

Bourne

, et al. Open-sector rapid-reactive collision avoidance: application in aerial robot navigation through outdoor unstructured environments. Robot Auto Syst 2019; 112: 211–220.

22.

Wei

Jiang

Liu

, et al. Method of PRM path optimization based on key nodes and smooth processing. Comput Eng Appl 2020; 56(16): 118–123.

23.

Bjorn

Sina

Ali

AAM

, et al. Nonlinear MPC for collision avoidance and control of UAVs with dynamic obstacles. IEEE Robot Auto Lett 2020; 5(4): 6001–6008.

24.

Wang

Zhang

, et al. On obstacle avoidance path planning in unknown 3D environments: a fluid-based framework. ISA Trans 2020; 111: 249–264.

25.

Gao

Liu

, et al. AUV path planning method based on improved A* search and dubins curve. Comput Measure Contr 2016; 24(08): 259–262.

26.

Kuffner

Lavalle

. RRT-Connect: an efficient approach to single-query path planning. In: IEEE international conference on robotics and automation, San Francisco, CA, USA, 24–28 April 2000, pp. 995–1001.

27.

Zhang

Zhen

Chen

. Collaborative path planning based on improved RRT-connect algorithm. Electronics Optics & Control, pp. 1–6. Available at: http://gffiyc3066c973e9140a1hwof5ofbw6xq565o5.fffb.suse.cwkeji.cn:999/kcms/detail/41.1227.TN.20210602.0846.002.html (accessed 13 August 2021).

28.

Park

Lee

Kim

. Trajectory planning with and artificial potential field-based real-time inverse kinematics. Int J Contr Auto Syst 2020; 8(18): 2095–2107.

29.

Priyadarshi

Himanshu

Dayal

. Path planning of humanoids based on artificial potential field method in unknown environments. Expert Syst 2019; 36(2): 1–12.

30.

Yao

Zheng

, et al. Path planning method with improved artificial potential field—a reinforcement learning perspective. IEEE Access 2020; 8: 135513–135523.

31.

Han

. UAV path planning based on improved artificial potential field. Syst Eng Electr, pp. 1–9. Available at: http://gffiyc3066c973e9140a1hwof5ofbw6xq565o5.fffb.suse.cwkeji.cn:999/kcms/detail/11.2422.TN.20210531.1117.014.html (accessed 13 August 2021).

32.

. UAV 3D environment obstacle avoidance trajectory planning based on improved artificial potential field method. J Phys Conf Ser 2021; 1885(2): 22020.

33.

Shu

. Cooperative task planning method of multi-UAV in dynamic environment. Nanjing: Nanjing University of Posts and Telecommunications, 2019.

34.

Bao

, et al. Obstacle avoidance path planning of unmanned aerial vehicle in 3D dynamic environment based on improved artificial potential field method. J Shanghai Dianji Univer 2020; 5(23): 279–285.

35.

Z-H

Chen

. Non-uniform cubic B-spline curves interpolation for UAV 3-D track smoothing. Contr Eng 2020; 27(07): 1259–1266.

36.

Karaman

Frazzoli

. Incremental sampling-based algorithms for optimal motion planning. Robot Sci Syst VI 2010 pp. 1–20. Available at: https://dx-doi-org.web.bisu.edu.cn/10.15607/RSS.2010.VI.034.

37.

Qureshi

Ayaz

. Intelligent bidirectional rapidly-exploring random trees for optimal motion planning in complex cluttered environments. Robot Auto Syst 2015; 68: 1–11.