# Path Planning and Control of a Quadrotor UAV Based on an Improved APF Using Parallel Search

Authors: Tianpeng Huang; Deqing Huang; Na Qin; Yanan Li
Journal: International Journal of Aerospace Engineering
 (2021)

---

## Abstract

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 artificial potential field (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 effectiveness of the proposed path planning algorithm and controller.


---

## Body

## 1. 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 solar-powered 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 pathThe remainder of this paper is organized as follows. In Section2, 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.


## 2. 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.LetPcurxcur,ycur and Ptarxtar,ytar represent the current position and target position of the UAV, respectively. Then, the attractive potential field is given by
(1)UaPcur=12kadaPcur,Ptar2,where ka is a positive gain and daPcur,Ptar is the minimum distance between current position and the target position, in a 2D case defined as
(2)da=xcur−xtar2+ycur−ytar2.LetPobixobi,yobi denote position of the ith obstacle, where i∈N+. Then, the repulsive potential field of the ith obstacle is defined as
(3)UriPcur=12kri1driPcur,Pobi−1d02dri≤d0,0dri>d0,where kri and d0 are positive constants and driPcur,Pobi is the minimum distance between current position and the ith obstacle, in a 2D case defined as
(4)dri=xcur−xobi2+ycur−yobi2.It is worth noting thatd0 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)
(5)FaPcur=kadaPcur,Ptar.Meanwhile, the repulsive force of theith obstacle is obtained from (3):
(6)FriPcur=kri1dri−1d01dri2dri≤d0,0dri>d0.Therefore, the path planning for UAV based on the TAPF algorithm is shown in Algorithm1.Algorithm 1: Path planning algorithm based on TAPF
Input: the current position Pcurxcur,ycur of UAV, target position Ptarxtar,ytar, obstacles position;Output: The path to the target;Calculateda;Update path matrixM⟵Pcur;whileda>c (c is a small positive constant) doCalculate attractive forceFa, using (2) and (5);Calculate repulsive forceFri, using (4) and (6);Calculate the components of attractive force along thex and y directions, respectively;Calculate the components of repulsive force along thex and y directions, respectively;Calculate the next point on the pathPnext according to the resultant of attractive force and repulsive force;UpdateM⟵Pnext;end while


### 2.2. 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 Figures1 and 2, where Fa and Fr represent the attractive force and resultant repulsive force at the current position, respectively, and Fri,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.Figure 1
Scenario 1: the local minimum of TAPF with a single obstacle.Figure 2
Scenario 1: the local minimum of TAPF with multiple obstacles.


### 2.3. Unreachable Target

Another shortcoming of TAPF is that the goal might be inaccessible for UAV, when obstacles are near the target, as shown in Figures3 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.Figure 3
Scenario 1: the unreachable goal of TAPF.Figure 4
Scenario 2: the unreachable goal of TAPF.


## 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.LetPcurxcur,ycur and Ptarxtar,ytar represent the current position and target position of the UAV, respectively. Then, the attractive potential field is given by
(1)UaPcur=12kadaPcur,Ptar2,where ka is a positive gain and daPcur,Ptar is the minimum distance between current position and the target position, in a 2D case defined as
(2)da=xcur−xtar2+ycur−ytar2.LetPobixobi,yobi denote position of the ith obstacle, where i∈N+. Then, the repulsive potential field of the ith obstacle is defined as
(3)UriPcur=12kri1driPcur,Pobi−1d02dri≤d0,0dri>d0,where kri and d0 are positive constants and driPcur,Pobi is the minimum distance between current position and the ith obstacle, in a 2D case defined as
(4)dri=xcur−xobi2+ycur−yobi2.It is worth noting thatd0 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)
(5)FaPcur=kadaPcur,Ptar.Meanwhile, the repulsive force of theith obstacle is obtained from (3):
(6)FriPcur=kri1dri−1d01dri2dri≤d0,0dri>d0.Therefore, the path planning for UAV based on the TAPF algorithm is shown in Algorithm1.Algorithm 1: Path planning algorithm based on TAPF
Input: the current position Pcurxcur,ycur of UAV, target position Ptarxtar,ytar, obstacles position;Output: The path to the target;Calculateda;Update path matrixM⟵Pcur;whileda>c (c is a small positive constant) doCalculate attractive forceFa, using (2) and (5);Calculate repulsive forceFri, using (4) and (6);Calculate the components of attractive force along thex and y directions, respectively;Calculate the components of repulsive force along thex and y directions, respectively;Calculate the next point on the pathPnext according to the resultant of attractive force and repulsive force;UpdateM⟵Pnext;end while


## 2.2. 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 Figures1 and 2, where Fa and Fr represent the attractive force and resultant repulsive force at the current position, respectively, and Fri,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.Figure 1
Scenario 1: the local minimum of TAPF with a single obstacle.Figure 2
Scenario 1: the local minimum of TAPF with multiple obstacles.


## 2.3. Unreachable Target

Another shortcoming of TAPF is that the goal might be inaccessible for UAV, when obstacles are near the target, as shown in Figures3 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.Figure 3
Scenario 1: the unreachable goal of TAPF.Figure 4
Scenario 2: the unreachable goal of TAPF.


## 3. 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 Figure5, from which it is observed that dr3 is smallest. To avoid collision with other obstacles, the UAV makes a circular motion with radius dr3 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.Figure 5
UAV moves around the nearest obstacle.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 Figures6 and 7 are used to further illustrate this idea.Figure 6
Example 1: the parallel search strategy for UAV when the goal is unreachable.Figure 7
Example 2: the parallel search strategy for UAV when the goal is unreachable.In Figure6, L1 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 L1 will be scanned continuously to find the intersection points of the line L1 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 Figure7, where L2 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 L2 and the obstacle is greater than zero. In these circumstances, the UAV moves firstly along the line L¯ to the point AxA,yA. Then, the UAV moves to the point D around the first obstacle with radius RA, where
(7)RA=yob1−yA2+xob1−xA2.Finally, the UAV successfully reaches the destination along the feasible pathL3. 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 Algorithm2.Algorithm 2: A novel APF algorithm based on parallel search for path planning of UAV
Input: the current position Pcurxcur,ycur of UAV, target position Ptarxtar,ytar, obstacle position;Output: the path to the target;Calculateda;Update path matrixM⟵Pcurwhileda>cdoCalculate attractive forceFa, using (2) and (5);Calculate repulsive forceFri, using (4) and (6);Calculate the components of attractive force along thex and y directions, respectively;Calculate the components of repulsive force along thex and y directions, respectively;Calculate the next point on the pathPnext according to the resultant of attractive force and repulsive force;Update matrixM⟵Pnext;Calculated1=Mj−Mj−3;if local minimum thenFind the obstacle closest to the current position, then calculate the distanceR between the obstacle and current position, and moves around the obstacle with radius R;end ifif goal unreachable thenSearch for obstacles between the current position and the target position;if Figure 6thenThe UAV moves directly to target alongL1;elseThe UAV moves to the pointA along L¯, then calculates RA and moves around obstacle1 in Figure 7 with radius RA to the point D. Finally, the UAV reaches the target from D.end ifend ifEnd whileRemark 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 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.


## 4. 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.(8)ϕ¨=Jy−Jzθ̇φ̇−Jrθ̇Ω+LFϕJx+d¯ϕ,θ¨=Jz−Jxϕ̇φ̇+Jrϕ̇Ω+LFθJy+d¯θ,φ¨=Jx−Jyϕ̇θ̇+fFφJz+d¯φ,where ϕ,θ,φ are altitude, roll angle, pitch angle and yaw angle of quadrotor, respectively; Fχ,χ=ϕ,θ,φ are the control inputs of the system; L,f,Jr,Jn,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-, y- and 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 Jrθ̇Ω/Jx and Jrϕ̇Ω/Jy are ignored here.


### 4.2. Assumptions

To make the subsequent analysis rigorous, the following assumption is given.Assumption 1.
It is assumed that the disturbances change slowly, i.e.,ḋϕ=ḋθ=ḋφ=0.Assumption 2.
In the design of controller for the quadrotor, to avoid any singularity, we set−π/2<ϕ<π/2 and −π/2<θ<π/2.


### 4.3. Observer Design

Some of the involved disturbance components in (8) are redefined as dϕ=Jxd¯ϕ, dθ=Jyd¯θ, and dφ=Jzd¯φ. To compensate for external disturbance, a NDO with exponential convergence is proposed. Define observation error as
(9)d~χ=dχ−d^χ,where d^χ is the estimate of dχ with χ=ϕ,θ,φ. Considering Assumption 1, the derivative of observation error d~χ in (9) in attitude channel is obtained by
(10)d~̇χ=−d^̇χ.Then, the NDO in attitude channel is designed as(11)ẋχ=PχWj−Yj−Pχd^χ,d^χ=xχ+PχJnχ̇,where j=1,2,3, n=x,y,z, W1=Jz−Jyθ̇φ̇, Y1=LU2, W2=Jx−Jzϕ̇φ̇, Y2=LU3, W3=Jy−Jxϕ̇θ̇, Y3=fU4, xχ is an auxiliary variable, and Pχ is a positive gain.Theorem 2.
If Assumption1 holds and the observer is designed as (11), then the observation error d~z in (9) will exponentially converge to zero.Proof.
See Appendix.☐Remark 3.
Note thatd^χ is not the direct estimates of d¯χ in (8) from (9). If we define d˘χ=d^χ/ε,ε=Jx,Jy,Jz, then d˘i denote the estimates of d¯χ in (8).


### 4.4. 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(12)eχ=χd−χ,where χd is the desired altitude signal. Then, the attitude controller is designed as
(13)Fχ=Rj+Gjcχ1ėχ+χ¨d+eχ+cχ2eχ1+Qj,where R1=Jz−Jy/Lθ̇φ̇, G1=Jx/L, Q1=d^ϕ/L, R2=Jx−Jz/Lϕ̇φ̇, G2=Jy/L, Q3=d^θ/L, R3=Jy−Jx/Lϕ̇θ̇, G3=Jz/f, Q3=d^φ/f, cχ1,cχ2>λχ/2 with λχ being a positive constant, and
(14)eχ1=−χ̇+cχ1eχ+χ̇d.Theorem 4.
Under Assumption1 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.☐


## 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.(8)ϕ¨=Jy−Jzθ̇φ̇−Jrθ̇Ω+LFϕJx+d¯ϕ,θ¨=Jz−Jxϕ̇φ̇+Jrϕ̇Ω+LFθJy+d¯θ,φ¨=Jx−Jyϕ̇θ̇+fFφJz+d¯φ,where ϕ,θ,φ are altitude, roll angle, pitch angle and yaw angle of quadrotor, respectively; Fχ,χ=ϕ,θ,φ are the control inputs of the system; L,f,Jr,Jn,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-, y- and 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 Jrθ̇Ω/Jx and Jrϕ̇Ω/Jy are ignored here.


## 4.2. Assumptions

To make the subsequent analysis rigorous, the following assumption is given.Assumption 1.
It is assumed that the disturbances change slowly, i.e.,ḋϕ=ḋθ=ḋφ=0.Assumption 2.
In the design of controller for the quadrotor, to avoid any singularity, we set−π/2<ϕ<π/2 and −π/2<θ<π/2.


## 4.3. Observer Design

Some of the involved disturbance components in (8) are redefined as dϕ=Jxd¯ϕ, dθ=Jyd¯θ, and dφ=Jzd¯φ. To compensate for external disturbance, a NDO with exponential convergence is proposed. Define observation error as
(9)d~χ=dχ−d^χ,where d^χ is the estimate of dχ with χ=ϕ,θ,φ. Considering Assumption 1, the derivative of observation error d~χ in (9) in attitude channel is obtained by
(10)d~̇χ=−d^̇χ.Then, the NDO in attitude channel is designed as(11)ẋχ=PχWj−Yj−Pχd^χ,d^χ=xχ+PχJnχ̇,where j=1,2,3, n=x,y,z, W1=Jz−Jyθ̇φ̇, Y1=LU2, W2=Jx−Jzϕ̇φ̇, Y2=LU3, W3=Jy−Jxϕ̇θ̇, Y3=fU4, xχ is an auxiliary variable, and Pχ is a positive gain.Theorem 2.
If Assumption1 holds and the observer is designed as (11), then the observation error d~z in (9) will exponentially converge to zero.Proof.
See Appendix.☐Remark 3.
Note thatd^χ is not the direct estimates of d¯χ in (8) from (9). If we define d˘χ=d^χ/ε,ε=Jx,Jy,Jz, then d˘i denote the estimates of d¯χ in (8).


## 4.4. 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(12)eχ=χd−χ,where χd is the desired altitude signal. Then, the attitude controller is designed as
(13)Fχ=Rj+Gjcχ1ėχ+χ¨d+eχ+cχ2eχ1+Qj,where R1=Jz−Jy/Lθ̇φ̇, G1=Jx/L, Q1=d^ϕ/L, R2=Jx−Jz/Lϕ̇φ̇, G2=Jy/L, Q3=d^θ/L, R3=Jy−Jx/Lϕ̇θ̇, G3=Jz/f, Q3=d^φ/f, cχ1,cχ2>λχ/2 with λχ being a positive constant, and
(14)eχ1=−χ̇+cχ1eχ+χ̇d.Theorem 4.
Under Assumption1 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.☐


## 5. Simulation

### 5.1. Comparison with TAPF

The simulation parameters including the start position, target position, and obstacle positions in Figures8–10 are listed in 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.Figure 8
Scenario 1: the result of path planning of the proposed algorithm.Figure 9
Scenario 2: the result of path planning of the proposed algorithm.Figure 10
Scenario 3: the result of path planning of the proposed algorithm in a complex environment.Table 1
The simulation parameters in Figures8–10.
ParameterFigure8Figure9Figure10Start position(0,0)(0,0)(0,0)Target position(24,24)(24,24)(32,32)Obstacle1 position(16,16)(13,16)(10,9)Obstacle2 position(22,24)(16,13)(11,16)Obstacle3 position(23,25)(20.5,23.5)(18,14)Obstacle4 position(24.7,25.2)(23,25)(17,24)Obstacle5 position(25.5,24)(24,26.5)(24,22)Obstacle6 position(25,22.5)(25.2,25.2)(28,30)Obstacle7 position×(26.5,24)(29,28)Obstacle8 position×(25.3,22.5)(31,28)Obstacle9 position×(24,21)×Obstacle10 position×(23,23)×


### 5.2. Comparison with BUG1, BUG2, and RRT

Considering the characteristics of the proposed algorithm, BUG1, BUG2, and RRT, and to fairly compare their abilities of path planning, the obstacles of different shapes are placed around the line connecting the start point and the target point.The simulation parameters including the start position, target position, and obstacle positions in Figures11–14 are shown in Table 2. Furthermore, the comparisons with the three path planning algorithms, namely, BUG1, BUG2, 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, BUG1, BUG2, 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 BUG1, BUG2, 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 BUG1 and BUG2. 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.Figure 11
Scenario 1: the result of path planning of the four algorithms with an obstacle.Figure 12
Scenario 2: the result of path planning of the four algorithms with an obstacle.Figure 13
Scenario 1: the result of path planning of the four algorithms with multiple obstacles.Figure 14
Scenario 2: the result of path planning of the four algorithms with multiple obstacles.Table 2
The simulation parameters in Figures11–14.
ParameterFigure11Figure12Figure13Figure14Start position(0,0)(0,0)(0,0)(0,0)Target position(24,24)(24,24)(36,36)(36,36)Obstacle1 position(16,15)(12,13)(9,9)(9,10)Obstacle2 position××(14,14)(14,16)Obstacle3 position××(19,18)(20,10)Obstacle4 position××(24,23)(20,20)Obstacle5 position××(31,30)(29,17)Obstacle6 position×××(28,28)Obstacle7 position×××(35,34)Table 3
Time consumption of the four algorithms in Figures11–14.
AlgorithmFigure11Figure12Figure13Figure14Proposed0.0567 s0.0554 s0.0748 s0.0947 sBUG10.0829 s0.0736 s0.1292 s0.1419 sBUG20.0701 s0.0714 s0.1210 s0.1260 sRRT0.7150 s0.2022 s1.1637 s0.5662 STable 4
Path length of the four algorithms in Figures11–14.
AlgorithmFigure11Figure12Figure13Figure14Proposed35.8800 m35.5200 m56.1371 m56.6288 mBUG137.1662 m38.5924 m62.7243 m65.8486 mBUG236.3676 m37.3622 m64.2180 m63.2446 mRRT56.2966 m44.4007 m66.2756 m91.0735 mOverall, compared with BUG1, BUG2, 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.


### 5.3. Trajectory Tracking

The physical parameters of quadrotor are set as follows:L=0.4 m, Jx=0.16 kgm2, Jy=0.16 kgm2, Jz=0.32 kgm2, and f=0.05 m. The initial attitude of the quadrotor is set as 0rad. Furthermore, to follow the planned path, the desired attitude angles need to be addressed. Figure 11 is used as an example here, whose desired attitude states are calculated as follows: dϕ=0, dθ1=1, dθ2=0, dφ1=0.785, dφ2=1.415, dφ3=2.597, dφ4=1.537, dφ5=1.865, dφ6=1.617, dφ7=1.514, dφ8=1.399, dφ9=1.288, dφ10=1.181, dφ11=1.011, dφ12=0.985, dφ13=0.813, dφ14=0.78, dφ15=0.737, dφ16=0.669, dφ17=0.607, dφ18=0.552, dφ19=0.506, and dφ20=0.47. 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 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
(15)dϕ=0.4t−0.2,0≤t<6,2.2,6≤t<14,−0.1t+3.6,14≤t<20,−0.t+5.6,20≤t<33,(16)dθ=cosπ6t+0.4,(17)dφ=1,0≤t<5or10≤t<15⋯,0,5≤t<10or15≤t≤20⋯.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 Figure15, 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.Figure 15
Estimations of external disturbances.The corresponding tracking results for the desired signals are presented in Figure16. 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.Figure 16
Tracking results for reference inputs.


## 5.1. Comparison with TAPF

The simulation parameters including the start position, target position, and obstacle positions in Figures8–10 are listed in 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.Figure 8
Scenario 1: the result of path planning of the proposed algorithm.Figure 9
Scenario 2: the result of path planning of the proposed algorithm.Figure 10
Scenario 3: the result of path planning of the proposed algorithm in a complex environment.Table 1
The simulation parameters in Figures8–10.
ParameterFigure8Figure9Figure10Start position(0,0)(0,0)(0,0)Target position(24,24)(24,24)(32,32)Obstacle1 position(16,16)(13,16)(10,9)Obstacle2 position(22,24)(16,13)(11,16)Obstacle3 position(23,25)(20.5,23.5)(18,14)Obstacle4 position(24.7,25.2)(23,25)(17,24)Obstacle5 position(25.5,24)(24,26.5)(24,22)Obstacle6 position(25,22.5)(25.2,25.2)(28,30)Obstacle7 position×(26.5,24)(29,28)Obstacle8 position×(25.3,22.5)(31,28)Obstacle9 position×(24,21)×Obstacle10 position×(23,23)×


## 5.2. Comparison with BUG1, BUG2, and RRT

Considering the characteristics of the proposed algorithm, BUG1, BUG2, and RRT, and to fairly compare their abilities of path planning, the obstacles of different shapes are placed around the line connecting the start point and the target point.The simulation parameters including the start position, target position, and obstacle positions in Figures11–14 are shown in Table 2. Furthermore, the comparisons with the three path planning algorithms, namely, BUG1, BUG2, 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, BUG1, BUG2, 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 BUG1, BUG2, 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 BUG1 and BUG2. 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.Figure 11
Scenario 1: the result of path planning of the four algorithms with an obstacle.Figure 12
Scenario 2: the result of path planning of the four algorithms with an obstacle.Figure 13
Scenario 1: the result of path planning of the four algorithms with multiple obstacles.Figure 14
Scenario 2: the result of path planning of the four algorithms with multiple obstacles.Table 2
The simulation parameters in Figures11–14.
ParameterFigure11Figure12Figure13Figure14Start position(0,0)(0,0)(0,0)(0,0)Target position(24,24)(24,24)(36,36)(36,36)Obstacle1 position(16,15)(12,13)(9,9)(9,10)Obstacle2 position××(14,14)(14,16)Obstacle3 position××(19,18)(20,10)Obstacle4 position××(24,23)(20,20)Obstacle5 position××(31,30)(29,17)Obstacle6 position×××(28,28)Obstacle7 position×××(35,34)Table 3
Time consumption of the four algorithms in Figures11–14.
AlgorithmFigure11Figure12Figure13Figure14Proposed0.0567 s0.0554 s0.0748 s0.0947 sBUG10.0829 s0.0736 s0.1292 s0.1419 sBUG20.0701 s0.0714 s0.1210 s0.1260 sRRT0.7150 s0.2022 s1.1637 s0.5662 STable 4
Path length of the four algorithms in Figures11–14.
AlgorithmFigure11Figure12Figure13Figure14Proposed35.8800 m35.5200 m56.1371 m56.6288 mBUG137.1662 m38.5924 m62.7243 m65.8486 mBUG236.3676 m37.3622 m64.2180 m63.2446 mRRT56.2966 m44.4007 m66.2756 m91.0735 mOverall, compared with BUG1, BUG2, 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.


## 5.3. Trajectory Tracking

The physical parameters of quadrotor are set as follows:L=0.4 m, Jx=0.16 kgm2, Jy=0.16 kgm2, Jz=0.32 kgm2, and f=0.05 m. The initial attitude of the quadrotor is set as 0rad. Furthermore, to follow the planned path, the desired attitude angles need to be addressed. Figure 11 is used as an example here, whose desired attitude states are calculated as follows: dϕ=0, dθ1=1, dθ2=0, dφ1=0.785, dφ2=1.415, dφ3=2.597, dφ4=1.537, dφ5=1.865, dφ6=1.617, dφ7=1.514, dφ8=1.399, dφ9=1.288, dφ10=1.181, dφ11=1.011, dφ12=0.985, dφ13=0.813, dφ14=0.78, dφ15=0.737, dφ16=0.669, dφ17=0.607, dφ18=0.552, dφ19=0.506, and dφ20=0.47. 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 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
(15)dϕ=0.4t−0.2,0≤t<6,2.2,6≤t<14,−0.1t+3.6,14≤t<20,−0.t+5.6,20≤t<33,(16)dθ=cosπ6t+0.4,(17)dφ=1,0≤t<5or10≤t<15⋯,0,5≤t<10or15≤t≤20⋯.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 Figure15, 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.Figure 15
Estimations of external disturbances.The corresponding tracking results for the desired signals are presented in Figure16. 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.Figure 16
Tracking results for reference inputs.


## 6. 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 are performed to verify the effectiveness of the proposed path planning algorithm and the proposed controller.



---

*Source: 5524841-2021-07-15.xml*