Path planning and control of a quadrotor UAV based on an improved APF using parallel search

. Control and path planning are two essential and challenging issues in quadrotor unmanned aerial vehicle (UAV). In this paper, an approach for moving around the nearest obstacle is integrated into an arti ﬁ cial potential ﬁ eld (APF) to avoid the trap of local minimum of APF. The advantage of this approach is that it can help the UAV successfully escape from the local minimum without collision with any obstacles. Moreover, the UAV may encounter the problem of unreachable target when there are too many obstacles near its target. To address the problem, a parallel search algorithm is proposed, which requires UAV to simultaneously detect obstacles between current point and target point when it moves around the nearest obstacle to approach the target. Then, to achieve tracking of the planned path, the desired attitude states are calculated. Considering the external disturbance acting on the quadrotor, a nonlinear disturbance observer (NDO) is developed to guarantee observation error to exponentially converge to zero. Furthermore, a backstepping controller synthesized with the NDO is designed to eliminate tracking errors of attitude. Finally, comparative simulations are carried out to illustrate the e ﬀ ectiveness of the proposed path planning algorithm and controller.


Introduction
In recent years, UAV has been used in various applications, such as infrastructure management [1], logistics delivery [2], and estimation of aboveground biomass of mangrove ecosystems [3]. The implementation of UAV in all these applications requires it to follow a predefined path. In addition, to achieve path tracking, a good control system should be provided. Therefore, path planning with automatic obstacle avoidance and control are two essential tasks in UAV.
Research on UAV control has been extensively reported in the literature. In [4], a proportional-integral-derivative (PID) controller is designed to accomplish altitude and attitude tracking for a quadrotor helicopter. A fuzzy PID control method is proposed in [5], where the fuzzy rules are employed to automatically adjust the three parameters of PID controller. In [6,7], an active disturbance rejection control (ADRC) scheme is developed to achieve trajectory tracking of a quadrotor UAV. PID and ADRC are model-free control strategies, which have an advantage of a simple control structure. However, it is difficult to tune the parameters of these two controllers.
In [8], an adaptive sliding mode control (SMC) is investigated to stabilize a quadrotor system subject to unknown external disturbance. [9] presents a continuous SMC approach to follow the predefined trajectories in position and attitude channels for a four-rotor UAV. An adaptive finite-time attitude tracking algorithm is applied to a quadrotor in the presence of uncertainty and disturbance in [10]. In [11], a disturbance observer is integrated to H ∞ technique to realize hovering control of a quadrotor. The robustness of such a method has been verified by experiments. In [12], the differential flatness is used in tracking control of translational and rotational movements of an UAV system considering modeling uncertainty and disturbance. An adaptive linear quadratic control strategy is proposed in [13] to stabilize three attitude angles of a quadrotor. To achieve trajectory following of position and attitude subsystems, a nonlinear disturbance observer-(NDO-) based backstepping controller is proposed in [14], where the NDO is utilized to estimate external disturbance.
The bioinspired algorithms have been applied to path planning of UAV. In [15], a chaotic artificial bee colony (ABC) method is developed to design a path in complex environments. In [16], an ant colony optimization (ACO) algorithm is proposed to achieve trajectory planning for a UAV. [17] presents a particle swarm optimization (PSO) algorithm to address path planning of UAV. Genetic algorithm (GA), as a popular optimization algorithm, has been employed to plan a path in UAV [18]. Furthermore, a comparison of GA and PSO for real-time path planning of UAV is carried out in [19], where the results indicate that, under the consideration of statistical significance, the trajectories produced by GA are superior compared to that produced by PSO when using the same encoding. In [20,21], a grey wolf optimizer is used to search a feasible and effective path for a UAV. An improved fruit fly optimization algorithm is introduced in [22] to address the problem of path planning of multiple UAVs in 3D complicated environments with online changing tasks. In [23], a flower pollination algorithm based on neighborhood global learning is employed to complete route planning of a UAV. [24] offers an evolutionary algorithm based on a novel separate evolution strategy to plan an optimized path for a single UAV. Furthermore, a constrained differential evolution is put forward to achieve path planning of a UAV in [25].
Besides the aforementioned bioinspired intelligent algorithms, there are many effective strategies to solve the problem of path planning of UAV. [26] studies a distributed multiagent path planning algorithm for quadrotors in dynamic environments. An energy-based path planning framework is used to improve flight endurance for a solarpowered UAV in [27]. A multiobjective path planning is presented in [28] to design a feasible path for a UAV, where safety is considered in the proposed algorithm. [29] introduces a path planning system based on an elliptical tangent model to reduce computational burden for a quadrotor UAV in an unknown unstructured outdoor environment. In [30], a ground feature-oriented approach is investigated to generate a suitable path for UAV mapping. Two path planning algorithms are designed in [31], one of which is based on the exact penalty method and successive convex approximation, and the other adopts a heuristic approach. In addition, [32] presents an improved A-star algorithm to generate a path for target tracking of a UAV. In [33], a Voronoi diagram-based multiple UAV path planning method is proposed to cooperatively attack multiple targets in a static environment. An improved rapidly exploring random tree (RRT) algorithm is introduced in [34] to realize 3D path planning of a UAV.
As an efficient path planning algorithm, APF has been applied to some scenarios, such as mobile robots [35,36] and automated vehicles [37]. The traditional APF (TAPF) has two shortcomings, i.e., local minimum and unreachable goal. To address these problems, the repulsive potential function of TAPF is replaced by Gaussian function in [38]. However, UAV still might fall into a local minimum using the improved APF in [38] when obstacle is on the line connecting current position and target position. Moreover, when multiple obstacles are around target and repulsive gain is large, the resultant repulsive forces might be equal to attractive force, in which case UAV could not approach target. Motivated by the above analysis, a novel APF based on parallel search is proposed for path planning of UAV in this paper.
The main contributions of the paper are summarized as follows: (1) A parallel search algorithm is proposed to address local minimum and unreachable target with obstacles nearby in TAPF (2) Compared with existing results of path planning algorithms [34,39,40], a shorter path and less time consumption are obtained using the proposed algorithm (3) Compared with ADRC [6,7], better tracking performance is obtained by the proposed controller based on NDO with exponential convergence when following the planned path The remainder of this paper is organized as follows. In Section 2, TAPF is introduced and the problems of local minimum and unreachable goal of TAPF are analyzed. In Section 3, a novel APF based on parallel search is presented. Section 4 introduces the design of the observer and controller of the quadrotor. In Section 5, comparative simulations are conducted to verify the effectiveness of the proposed algorithm and controller. Section 6 concludes the work.

TAPF Applied to Path Planning of UAV
2.1. TAPF. APF is a virtual potential field in space, which consists of attractive potential field generated by target position and repulsive potential field generated by obstacle. The UAV automatically plans a path to destination under the influence of attractive potential field and repulsive potential field.
Let P cur ðx cur , y cur Þ and P tar ðx tar , y tar Þ represent the current position and target position of the UAV, respectively. Then, the attractive potential field is given by where k a is a positive gain and d a ðP cur , P tar Þ is the minimum distance between current position and the target position, in a 2D case defined as International Journal of Aerospace Engineering Input: the current position P cur ðx cur , y cur Þ of UAV, target position P tar ðx tar , y tar Þ, obstacles position; Output: The path to the target; Calculate d a ; Update path matrix M ⟵ P cur ; whiled a > c (c is a small positive constant) do Calculate attractive force F a , using (2) and (5); Calculate repulsive force F ri , using (4) and (6); Calculate the components of attractive force along the x and y directions, respectively; Calculate the components of repulsive force along the x and y directions, respectively; Calculate the next point on the path P next according to the resultant of attractive force and repulsive force; Update M ⟵ P next ; end while

International Journal of Aerospace Engineering
Let P obi ðx ob i , y ob i Þ denote position of the ith obstacle, where i ∈ N + . Then, the repulsive potential field of the ith obstacle is defined as where k ri and d 0 are positive constants and d ri ðP cur , P obi Þ is the minimum distance between current position and the ith obstacle, in a 2D case defined as ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ffi It is worth noting that d 0 in (3) shows the influence range of the repulsive potential field of obstacle. Obviously, the attractive potential field is global, while the repulsive potential field is local. Furthermore, the attractive force from the target is obtained by (1) Meanwhile, the repulsive force of the ith obstacle is obtained from (3): Therefore, the path planning for UAV based on the TAPF algorithm is shown in Algorithm 1.  Obs8

Local Minimum.
The local minimum is an inherent disadvantage of TAPF. When the attractive force and repulsive force reach a balance, the UAV would encounter a trap of a local minimum, which means that the UAV stops moving towards target, as shown in Figures 1 and 2, where F a and F r represent the attractive force and resultant repulsive force at the current position, respectively, and F ri , i = 1, 2, represents repulsive force of the ith obstacle. It is evident that a single obstacle or multiple obstacles may cause UAV to fall into a local minimum when a balance of the attractive force and repulsive force is reached.

Unreachable Target.
Another shortcoming of TAPF is that the goal might be inaccessible for UAV, when obstacles are near the target, as shown in Figures 3 and 4, where Obsi , i = 1, ⋯, 8, denotes the ith obstacle. In fact, the attraction of the goal to UAV is gradually decreasing, as the UAV approaches destination from (5), while the repulsive force of obstacles to UAV is gradually increasing. Therefore, the UAV fails to plan a path to destination using TAPF.

Novel APF Based on Parallel Search
To address the problems of the local minimum and unreachable goal of TAPF, a novel parallel search-based APF algorithm is proposed to achieve path planning of a UAV. Such an algorithm consists of two key ideas. The first idea is that UAV moves around the nearest obstacle when it encounters a local minimum, as shown in Figure 5, from which it is observed that d r3 is smallest. To avoid collision with other obstacles, the UAV makes a circular motion with radius d r3 around the third obstacle. Then, the balance of the attractive force and repulsive force will be broken once the UAV moves a step around the third obstacle, which would guide the UAV to escape from the local minimum under the APF framework.
The second idea of the proposed algorithm is that when the goal is unreachable for the UAV, the movement around the nearest obstacle and the detection of obstacle between the current position and target position will be performed in parallel. Two examples in Figures 6 and 7 are used to further illustrate this idea.
In Figure 6, L 1 represents a line that goes through the current position of UAV and target position. When the target is unreachable for UAV, the points on the line L 1 will be scanned continuously to find the intersection points of the line L 1 and the obstacle. If the number of intersection points is equal to zero, it means the UAV can move directly to the target.
In the following, we will explain the scenario in Figure 7, where L 2 represents a line that goes through the current position of UAV and target position and L denotes a line connecting the current position of UAV and the obstacle. It is obvious that the number of intersection points of L 2 and the obstacle is greater than zero. In these circumstances, the UAV moves firstly along the line L to the point Aðx A , y A Þ. Then, the UAV moves to the point D around the first obstacle with radius R A , where Finally, the UAV successfully reaches the destination along the feasible path L 3 . It is worth noting that the detection of the obstacle between the current position and target position will also be performed simultaneously when UAV is at point B or C.
In summary, the flow of the proposed algorithm is shown in Algorithm 2.
Remark 1. In simulation, it is easy to judge the local minimum and the unreachable goal, since the obstacle position and the target position are known. However, the obstacle position may be unknown in real experiments. Therefore, the UAV firstly moves one step around the nearest obstacle within the scanning range of sensor of UAV when it cannot continue moving towards the target. If the current position   International Journal of Aerospace Engineering is a local minimum, the balance of forces will be broken and the UAV will escape this trap. Otherwise, the UAV moves to an unreachable target.

Controller Design Based on NDO
4.1. Mathematical Model of Quadrotor UAV. To track the planned path in 2D space, only the attitude angles need to be regulated. Therefore, attitude dynamics of the quadrotor subject to external disturbances are introduced here.
where ½ϕ, θ, φ are altitude, roll angle, pitch angle and yaw angle of quadrotor, respectively; F χ , χ = ϕ, θ, φ are the control inputs of the system; L, f , J r , J n , n = x, y, z denote the distance from rotor center to mass center, force to moment factor, inertia of each propeller, inertias of the quadrotor around x-, yand z-axes, respectively; and Ω is the difference in angular speed of the rotors on the diagonal of the quadrotor. The terms d ϕ , d θ , and d φ denote the effect of wind on the translational and rotational subsystems of the quadrotor in the form of external disturbances. Compared with brushless motor, the propeller of quadrotor is very light; therefore, the terms J r _ θΩ/J x and J r _ ϕΩ/J y are ignored here.

Assumptions.
To make the subsequent analysis rigorous, the following assumption is given.

Observer Design. Some of the involved disturbance components in (8) are redefined as
To compensate for external disturbance, a NDO with exponential convergence is proposed. Define observation error asd whered χ is the estimate of d χ with χ = ϕ, θ, φ. Considering Assumption 1, the derivative of observation errord χ in (9) in attitude channel is obtained by Input: the current position P cur ðx cur , y cur Þ of UAV, target position P tar ðx tar , y tar Þ, obstacle position; Output: the path to the target; Calculate d a ; Update path matrix M ⟵ P cur whiled a > cdo Calculate attractive force F a , using (2) and (5); Calculate repulsive force F ri , using (4) and (6) The UAV moves directly to target along L 1 ; else The UAV moves to the point A along L, then calculates R A and moves around obstacle1 in Figure 7 with radius R A to the point D. Finally, the UAV reaches the target from D.

end if end if End while
Algorithm 2: A novel APF algorithm based on parallel search for path planning of UAV 6 International Journal of Aerospace Engineering where j = 1, 2, 3, n = x, y, z, x χ is an auxiliary variable, and P χ is a positive gain. Theorem 2. If Assumption 1 holds and the observer is designed as (11), then the observation errord z in (9) will exponentially converge to zero.

Controller Design.
To address the problem of tracking control in the attitude channel, a backstepping scheme is proposed. We define the tracking error of attitude as  Start position (0,0) (0,0) (0,0) Target position (     International Journal of Aerospace Engineering where χ d is the desired altitude signal. Then, the attitude controller is designed as where , c χ2 > λ χ /2 with λ χ being a positive constant, and Theorem 4. Under Assumption 1 and Assumption 2, for the altitude dynamics in (8), if the control input F χ is designed as (13), then the tracking error for desired attitude is guaranteed to converge to zero exponentially, i.e., e χ ⟶ 0 as t ⟶ ∞.
Proof. See Appendix. ☐  Table 1. To demonstrate the effectiveness of the proposed path planning framework to deal with the traps of local minimum and unreachable goal of TAPF in Figures 1-4, simulations are carried out, as shown in Figures 8 and 9, where we can observe that the UAV can plan a feasible path to the destination with obstacle avoidance. Furthermore, the proposed algorithm is verified in complex environments with multiple obstacles in Figure 10.           Table 2. Furthermore, the comparisons with the three path planning algorithms, namely, BUG 1 , BUG 2 , and RRT, are presented in Figures 11-14. Figures 11 and 12 show the results of path planning of UAV with a single obstacle, while the results of path planning of UAV with multiple obstacles are shown in Figures 13 and 14. From the results, the feasible path to the target with obstacle avoidance can be obtained when the proposed algorithm, BUG 1 , BUG 2 , and RRT are applied to path planning of the UAV, respectively. Table 3 shows the time consumption of the four algorithms. It is obvious that in either the environment with a single obstacle or with multiple obstacles, the time consumption of the proposed algorithm is the least. The path length in Figures 11-14 are listed in Table 4, from which we find that compared with paths generated by BUG 1 , BUG 2 , and RRT, a shorter path is obtained using the proposed algorithm. For the environment with a single obstacle, the shape of obstacle has a greater effect on the length of path provided by BUG 1 and BUG 2 . Meanwhile, RRT has the worst performance in terms of time consumption and path length. Also, the UAV cannot reach the target accurately when RRT is applied to the UAV.

Simulation
Overall, compared with BUG 1 , BUG 2 , and RRT, the proposed algorithm has advantages in time consumption and path length, which means that less time and energy are required to reach the target for the UAV. It should be noted that (1) the yaw angle is used to adjust the forward direction of the quadrotor, and the pitch angle is used to control the forward speed of the quadrotor, while the roll angle is required to maintain at 0rad and (2) when the yaw angle is trying to maintain one of the above states, a desired pitch

10
International Journal of Aerospace Engineering angle of 0:1rad will be tracked and when the yaw angle is switched between the two states, the desired pitch angle is set as 0rad. In addition, the disturbances in attitude channels are given as To verify the effectiveness of the proposed NDO, the time-varying disturbance with different frequencies are injected into the quadrotor system. The estimations of external disturbances are shown in Figure 15, where we can see that the disturbances (15), (16), (17) can be estimated, even if the derivatives of the disturbances are assumed to be 0 in the design of the disturbance observer.
However, the disturbance estimations of the pitch and yaw channels have small fluctuations.
The corresponding tracking results for the desired signals are presented in Figure 16. Obviously, tracking and frequent switching of so many states raise a challenging problem for the quadrotor controller, especially in the pitch and yaw channels. However, compared to ADRC, the proposed controller has better tracking performance. In addition, in the roll channel, oscillation is generated in the initial stage, and a large spike is produced at the 20th second when ADRC is applied to the quadrotor, while a smoother tracking performance is provided by the proposed controller.

Conclusion
In this paper, a novel APF algorithm based on parallel search is proposed for path planning of a UAV. An algorithm for moving around the nearest obstacle is synthesized into APF to avoid the trap of local minimum. A parallel search algorithm is presented to address the problem of an unreachable target. Furthermore, to achieve tracking of planned trajectory of quadrotor UAV subject to external disturbance, a backstepping control strategy with a NDO is designed. The comparative simulations  indicates that the observation errord χ will exponentially converge to zero as t ⟶ ∞, i.e.,d χ will exponentially converge to d χ as t ⟶ ∞ under Assumption 1. This completes the proof. ☐

B. Proof of Theorem 4
Proof. The whole proof is divided into two steps.

Data Availability
The data used to support this study are included within the article.

Conflicts of Interest
The authors declare that they have no conflicts of interest.

12
International Journal of Aerospace Engineering