Obstacle Avoidance Control of Unmanned Aerial Vehicle with Motor Loss-of-Effectiveness Fault Based on Improved Artificial Potential Field

Abstract

This paper presents an obstacle avoidance control strategy for an underactuated quadrotor unmanned aerial vehicle with motor loss-of-effectiveness fault and disturbance. The control system is divided into two parts: the obstacle avoidance loop and the tracking loop. By introducing the height factor in the artificial potential field function, an improved obstacle avoidance strategy is designed in the obstacle avoidance loop. Compared with the existing literature, the proposed obstacle avoidance strategy can avoid falling into the trap of the local optimum when a UAV encounters obstacles. At the same time, considering the sudden motor loss-of-effectiveness fault of UAV, adaptive technology is used to estimate the fault parameters online to restrain the effects of motor loss-of-effectiveness fault in the tracking loop. The stability of the closed-loop UAV system is guaranteed by stabilizing each of the subsystems through backstepping technology. Simulations are conducted to demonstrate the effectiveness of the designed obstacle avoidance control strategy.

1. Introduction

With the vigorous development of the unmanned aerial vehicle (UAV) industry, the popularity of UAVs in military and civil fields has further increased recently [1]. Specific applications for UAVs are traffic management, wireless networks, and mobile communications [2,3]. Moreover, the rise of Unmanned robots and related products has also aroused extensive attention in the academic community [4,5,6,7], which further increases the academic community’s enthusiasm for UAV research.

When UAVs perform flight tasks, autonomous obstacle avoidance capability should be possessed due to the complexity of the flight environment and the diversity of obstacles in their path. In the literature, various obstacle avoidance schemes are proposed. The geometric method relies on the analysis of geometric properties to ensure that the minimum distance defined between the robot and the obstacle is not broken. However, uniform geometric methods for dealing with different types of obstacles are difficult to find [8,9]. The force field method uses the concept of repulsion or attraction field to repel or attract a robot from an obstacle to a target. However, there is a local minimum problem [10,11,12,13,14]. The optimization-based method relies on the calculation of circumvention trajectories based on geographic information, and its goal is to provide optimal circumvention trajectories. However, this process can involve a lot of computation [15,16]. The perception and avoidance method mainly focuses on reducing the required computing power and short response time. However, it requires high-precision sensors and has a high cost [17,18]. Among them, the force field method is also called the artificial potential field (APF) method. This method makes the UAV a particle in multiple force fields so as to manipulate the attraction and repulsion to avoid obstacles. It solves the local minimum problem of APF by introducing a rotational potential field in [10], so that UAVs can get rid of the common local minimum and oscillation. A method of moving around the nearest obstacle is integrated into the APF to avoid APF falling into local minima in [11]. A method was proposed to find optimal paths in an environment with static and dynamic obstacles in [12]. A 3D collision avoidance strategy was proposed to solve obstacle avoidance in a dynamic environment in [13]. An obstacle avoidance method based on an Artificial Potential Field with the virtual core concept is proposed for obstacle avoidance in [14]. The above studies have proposed some solutions to the local minimum problem, such as introducing a rotational potential field [10,13] and moving around the nearest obstacle method [11]. However, all the above methods have certain limitations. For instance, the rotational potential field can only make the UAV pass through a narrow channel and may not work for the dense obstacle. The virtual obstacle method fills the local minimum point with virtual obstacles, which may increase the tracking error of the UAV. Therefore, it is urgent to propose a new obstacle avoidance method that can overcome the above limitations, which is one of the motivations for this paper.

On the other hand, the fault of the UAV is also an important factor leading to mission failure. A quadrotor UAV is more likely to encounter problems such as motor loss-of-effectiveness faults [19] due to the characteristics of reduced device size, reduced switching energy, and accelerated equipment operation rate. Once a motor failure occurs, the obstacle avoidance strategy might fail in the obstacle avoidance loop. To solve these problems, fault-tolerant control emerges as the circumstances require [20,21,22]. H. Ma et al. proposed an observer-based adaptive controller to estimate and compensate for faults without adding sensors or actuators to increase hardware redundancy [20]. S. Sun et al. designed a sensor-based fault-tolerant controller for a four-rotor UAV with a flight speed greater than 8 m/s and two rotor failures [21]. Aiming at the problem of fault-tolerant cooperative control for multi-UAVs with actuator faults and input saturation, Z. Yu et al. constructed a distributed fault-tolerant control scheme by using dynamic surface control [22]. However, when the UAV encounters obstacles, the above-mentioned fault-tolerant methods might fail. Therefore, an obstacle avoidance strategy is not only capable of obstacle avoidance but also fault tolerance. This is the other of the motivations for this paper.

Motivated by the above observations, this paper addresses obstacle avoidance control based on an improved APF of a UAV subject to motor loss-of-effectiveness fault. A new strategy is presented for obstacle avoidance control with the capability of restraining the effects of motor loss-of-effectiveness fault. Specifically, an adaptive motor fault estimator is used to estimate the parameters of the fault, and improved artificial potential functions are constructed to avoid obstacles. Then, obstacle avoidance control laws are designed with the aid of an adaptive motor fault estimator, improved artificial potential functions, and the backstepping technique. Finally, the stability of a closed-loop system is analyzed based on input-to-state stability. The effectiveness of our proposed strategy is demonstrated in the simulation.

The contributions of this paper are as follows:

(1)

The obstacle avoidance strategy in the existing literature [23] might fall into the trap of the local optimum. To avoid it, the height factor is introduced in the APF function to generate a new path in this paper. Thus, the obstacle avoidance control strategy guarantees that the path can get rid of the local optimum by raising the UAV’s height so that the UAV can climb over obstacles.

(2)

The effectiveness of these controllers [10,11,12,13] is satisfactory in terms of trajectory tracking capability when the UAV encounters an obstacle. However, when a fault occurs, the original obstacle avoidance strategy may fail. Therefore, this paper proposes the combination of UAV obstacle avoidance and fault tolerance for realizing the autonomous obstacle avoidance of the UAV in the case of motor failure.

The structure of this paper is as follows: Section 2 introduces the problems to be solved and the UAV system model used in this paper. Section 3 describes the improved obstacle avoidance strategy. Section 4 introduces the design of the fault-tolerant controller in this paper and gives stability proof. Section 5 carries out the simulation experiment, which separately gives the simulation experiment and the comparison simulation experiment results.

Notation 1. 

Throughout this paper, $diag\left\{·\right\}$ denotes the diagonal matrix, and $∥X∥$ denotes the 2-norm of a vector X.

2. UAV Model and Problem Description

2.1. UAV Model

The quadrotor configuration frame scheme is shown in Figure 1.

In Figure 1, the thrust generated by the propellers driven by the four motors of the UAV is defined as $f_i,i=1,2,3,4$. The coordinate system $\mathcal{I}=\{x_{\mathcal{I}},y_{\mathcal{I}},z_{\mathcal{I}}\}$ is the ground coordinate system, where $z_{\mathcal{I}}$ is perpendicular to the ground and points to the sky. The coordinate system $\mathcal{B}=\{x_{\mathcal{B}},y_{\mathcal{B}},z_{\mathcal{B}}\}$ in the above figure is the UAV body coordinate system, and its coordinate origin is the centroid of the UAV.

The nonlinear dynamic model of the UAV considered in this paper is established as follows:

$$\dot{p}=v,$$

(1)

$$\dot{v}=m^{-1}Kv+\mathsf{\Omega }-e_{3}g,$$

(2)

$$\dot{\eta }=h_1\left(\eta \right)\omega +D,$$

(3)

$$\dot{\omega }=h_2\left(\omega \right)+Bf,$$

(4)

where $p={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^T$ represents the inertial position; $v={\left[\begin{array}{ccc}\dot{x}& \dot{y}& \dot{z}\end{array}\right]}^T$ represents inertial velocity; m represents UAV quality; $K=diag\{k_x,k_y,k_z\}$ are the aerodynamic parameters; $\mathsf{\Omega }={\left[\begin{array}{ccc}{\mathsf{\Omega }}_1& {\mathsf{\Omega }}_2& {\mathsf{\Omega }}_3\end{array}\right]}^T$ is the virtual control quantity; $e_3={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$; g is gravity acceleration; $\eta ={\left[\begin{array}{cccc}\varrho & \varphi & \theta & \psi \end{array}\right]}^T$ is the extended Euler angle, where $\varrho$ is the virtual position angle of the UAV, $\varphi$ is the roll angle of the UAV, $\theta$ is the pitch angle of the UAV, and $\psi$ is the yaw angle of the UAV; $\omega ={\left[\begin{array}{cccc}\dot{\varrho }& s& q& r\end{array}\right]}^T$ is the extended Euler angular velocity, of which $\dot{\varrho }$ is the virtual position angular velocity of the UAV, s is the roll angular velocity of the UAV, q is the pitch angular velocity of the UAV, r is the yaw angular velocity of the UAV; $D={\left[\begin{array}{cccc}{d}_1\left(t\right)& d_2\left(t\right)& d_3\left(t\right)& d_4\left(t\right)\end{array}\right]}^T$ is the external disturbance; $u={\displaystyle \sum _{i=1}^4}{f}_i$ represents the lift of the UAV, of which $i=1,2,3,4$; $h_1\left(\eta \right)$, $h_2\left(\omega \right)$ is the nonlinear term of the system; B is a constant matrix; $f={\left[\begin{array}{cccc}{f}_1& f_2& f_3& f_4\end{array}\right]}^T$ is the lift provided by four drone motors. Among them are the mathematical expressions of virtual control quantity $\mathsf{\Omega }$, and the system of nonlinear terms $h_1\left(\eta \right)$ and $h_2\left(\omega \right)$ are as follows: $\mathsf{\Omega }={\left[\begin{array}{ccc}\frac{(cos(\varphi )sin(\theta )cos(\psi )-sin(\varphi )sin(\psi \left)\right)}{m}u& \frac{(cos(\varphi )sin(\theta )sin(\psi )-sin(\varphi )cos(\psi \left)\right)}{m}u& \frac{cos\varphi cos\theta }{m}u\end{array}\right]}^T$;

$h_1\left(\eta \right)=diag\left\{1,R\right\}$ and R represents the matrix relating angular body rates to Euler angle rates and its specific form is widely available in the aerospace literature; $h_2\left(\omega \right)={\left[\begin{array}{cccc}0& \frac{J_y-J_z}{J_x}qr& \frac{J_z-J_x}{J_y}sr& \frac{J_x-J_y}{J_z}sq\end{array}\right]}^T$ and $J=diag\{J_x,J_y,J_z\}$ is the three-axis rotational inertia of the UAV; $B=ML,M=diag\{1,J^{-1}\},L=\left[\begin{array}{cccc}1& 1& 1& 1\\ -l& -l& l& l\\ -l& l& l& -l\\ -c& c& -c& c\end{array}\right]$, l is the distance between the motor and the center of the UAV and c is the torque coefficient.

2.2. Problem Description

The purpose of this paper is to achieve the following objectives:

(1)

Tracking Objective: The UAV is driven to achieve a fixed height, which tracks with relative distances as follows:

$${\displaystyle \underset{t\to \infty }{lim}\left(p\left(t\right)-p_{target}\left(t\right)\right)=0,}$$

where $p\left(t\right)$ and $p_{target}\left(t\right)$ denote the actual location and desired position of the UAV, respectively.

(2)

Obstacle Avoidance Objective: The UAV avoids obstacles such that

$$∥p\left(t\right)-p_{oi}\left(t\right)∥>d_{safe},$$

where $d_{safe}$ denotes the safe distance between the UAV and the obstacle, and $p_{oi}\left(t\right)$ denotes the position of ith obstacle.

(3)

Fault Tolerance Objective: When a motor loss-of-effectiveness fault occurs during flight, the UAV can accomplish the tracking objective and obstacle avoidance.

3. Main Results

This section first introduces the structure diagram.

3.1. Structure Diagram

Aiming at the above control objectives, the structure diagram of the control strategy in this paper is shown in Figure 2.

Next, this section introduces the design of the obstacle avoidance loop and the tracking loop, respectively.

3.2. Obstacle Avoidance Loop Design

The obstacle avoidance loop is designed to assist the UAV in finding a safe path to avoid obstacles. The input information of this loop is the real position $p\left(t\right)$, target position $p_{target}\left(t\right)$, and obstacle position $p_{oi}\left(t\right)$ of the UAV, and the output is the reference position $p_d$. The remainder of this part introduces the traditional APF and the improved APF proposed in this paper.

3.2.1. Traditional APF

APF was proposed by Khatib in the 1980s [23]. The basic idea of traditional APF is to assume that the robot moves under two forces in the virtual potential field. One is the attraction of the destination, which pulls the robot there. The other is the repulsion from the obstacle, which pushes the robot away from the obstacle. In the virtual potential field, a path is constructed along the direction of attraction and repulsion to connect the starting point and end point so as to realize the obstacle avoidance function. Traditional APF can be expressed as:

$$U_{\mathrm{attract}}=\frac{1}{2}{k}_{\sigma }{(p-p_{target})}^2,$$

(5)

$$F_{attract}=k_{\sigma }(p-p_{target}),$$

(6)

$$U_{repel}=\left\{\begin{array}{cc}\frac{1}{2}{k}_{\varsigma }{(\frac{1}{∥\overrightarrow{d}∥}-\frac{1}{d_{safe}})}^2,& ∥\overrightarrow{d}∥\le d_{safe};\\ 0,& ∥\overrightarrow{d}∥>d_{safe}.\end{array}\right.$$

(7)

$$F_{repel}=\left\{\begin{array}{cc}{k}_{\varsigma }(\frac{1}{d_{safe}}-\frac{1}{∥\overrightarrow{d}∥})\frac{\overrightarrow{d}}{∥\overrightarrow{d}∥},& ∥\overrightarrow{d}∥\le d_{safe};\\ 0,& ∥\overrightarrow{d}∥>d_{safe}.\end{array}\right.$$

(8)

$$F_{\mathrm{all}}=\sum _{i=1}^n{F}_{repel}+F_{attract}.$$

(9)

Among them, $U_{\mathrm{attract}}$ and $U_{repel}$ are the target attraction potential field and obstacle repulsion potential field, respectively. The current location of the robot is p, and the target location is $p_{target}$. $∥\overrightarrow{d}∥$ is the current distance between the robot and the obstacle, and $d_{safe}$ is the safe distance between the robot and the obstacle. The repulsive force of the obstacle to the robot and attraction of the target to the robot is denoted by $F_{repel}$ and $F_{attract}$, respectively. $F_{\mathrm{all}}$ is the resultant force of the potential field on the robot, where $k_{\varsigma }$ and $k_{\sigma }$ are the repulsion and attraction coefficients, respectively.

The robot moves under the action of resultant force $F_{\mathrm{all}}$, where the attraction of the target is proportional to the distance from the target. For repulsive force $F_{repel}$, when the distance between the robot and the obstacle is kept outside the safe distance, the obstacle does not produce a repulsive force on the robot. When the distance between the robot and the obstacle is less than the safe distance, the repulsive force of the obstacle increases with the decrease in the distance between the robot and the obstacle.

As an effective obstacle avoidance method, the traditional APF method has many advantages. However, in some cases, the traditional APF has some shortcomings. For example, when the robot is running, its attraction and repulsion are equal in the opposite direction, and the resultant force of the robot is zero; that is, $F_{\mathrm{all}}={\displaystyle \sum _{i=1}^n}{F}_{repel}+F_{attract}=0$. The traditional APF method will fall into the local optimal trap, which is the local optimal trap problem [24] of the traditional APF method.

3.2.2. Improved APF

This part introduces the design of an improved APF function, which includes the repulsion force field function and the gravity field function. A height factor will be introduced into the repulsion field of the obstacle to get rid of the local optimum trap. The target gravity of the gravitational field is designed using the backstepping technique. The obstacle avoidance loop outputs the reference position $p_d$ of the UAV. Next, the repulsion field and gravity field are designed, respectively.

A UAV needs to have real-time obstacle avoidance capability when performing follow-up tasks outdoors. In the APF obstacle avoidance method, the repulsion design of the repulsion field directly affects the obstacle avoidance of the UAV. Therefore, the repulsion design is particularly important.

To avoid static obstacles, the repulsion field function $R_i$ of the ith obstacle is designed as follows:

$$R_i=\left\{\begin{array}{cc}{\left(\frac{r{p}^2-{∥p_{ixy}∥}^2}{{∥p_{ixy}∥}^2-r{c}^2}\right)}^2,& rc<∥p_{ixy}∥<rp,Z_{xy}\le 2\ast rp,z\le z_{oi};\\ \begin{array}{c}{\left(\frac{r{p}^2-{∥p_{ixy}∥}^2}{{∥p_{ixy}∥}^2-r{c}^2}\right)}^2\\ +(1+Z_{xy}-2\ast rp)\ast {∥p_{iz}∥}^2,\end{array}& rc<∥p_{ixy}∥<rp,Z_{xy}>2\ast rp,z<z_{io}+rc;\\ 0^3,& other.\end{array}\right.$$

(10)

In the repulsion field function (10), the difference $p_{iz}$ between the height of the ith obstacle and the current height of the UAV is introduced; that is, the height factor. Therefore, the repulsive force field can provide repulsive force according to the height factor so that the UAV rises in height, climbs over the obstacles, and gets rid of the local minimum trap.

The repulsive force is derived from the partial derivative of the repulsive force field function (10).

$$F_{repel}=\left\{\begin{array}{cc}\frac{4(r{p}^2-r{c}^2)({∥p_{ixy}∥}^2-r{p}^2)}{{∥p_{ixy}∥}^2-r{c}^2}\ast p_{ixy},& \begin{array}{c}rc<∥p_{ixy}∥<rp,Z_{xy}\le 2\ast rp,z\le z_{oi};\hfill \end{array}\\ \begin{array}{c}\frac{4(r{p}^2-r{c}^2)({∥p_{ixy}∥}^2-r{p}^2)}{{∥p_{ixy}∥}^2-r{c}^2}\ast p_{ixy}\hfill \\ +2\ast (1+Z_{xy}-2\ast rp)\ast p_{iz},\hfill \end{array}& \begin{array}{c}rc<∥p_{ixy}∥<rp,Z_{xy}>2\ast rp,z<z_{oi}+rc;\hfill \end{array}\\ 0^3,& other,\end{array}\right.$$

(11)

where $p_{oi}={\left[\begin{array}{ccc}{x}_{oi}& y_{oi}& z_{oi}\end{array}\right]}^T$, $p_{ixy}={\left[\begin{array}{ccc}x& y& 0\end{array}\right]}^T-{\left[\begin{array}{ccc}{x}_{oi}& y_{oi}& 0\end{array}\right]}^T$; $p_{iz}={\left[\begin{array}{ccc}0& 0& z_{oi}\end{array}\right]}^T-{\left[\begin{array}{ccc}0& 0& z\end{array}\right]}^T$; $0^3={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T$, $x_{oi}$, $y_{oi}$, and $z_{oi}$ represent the coordinates and height of the ith obstacle, respectively. $r_p$ is the safe horizontal distance from the center of the obstacle when the UAV is flying. $r_c$ is the obstacle collision distance, and $Z_{xy}$ is the horizontal distance between the UAV and the moving ground target.

The obstacle avoidance algorithm of the repulsion field can be concluded as Algorithm 1.

Remark 1. 

The repulsive force field (10) can provide a repulsive force (11) according to the height factor so that the UAV rises in height, climbs over the obstacles, and gets rid of the local minimum trap. When the UAV is not in the local optimal trap, the obstacle avoidance strategy reduces to the existing strategies in the literature [23]. Therefore, the strategy proposed in this paper has a wider application.

Remark 2. 

A judgment condition that can give information about if the UAV is in the local optimal trap is given for the first time. According to the judgment conditions, the repulsion field function (10) designed in this paper introduces the error variable $Z_{xy}$. When the horizontal distance between the UAV and moving ground target is greater than twice the horizontal safety distance of obstacles, namely, when $Z_{xy}>2\ast rp$, the UAV is judged to fall into the local optimal trap.

In this paper, the gravity field is directly designed by the backstepping method to meet the tracking objective.

Algorithm 1 Improved APF (repulsion field)

Input: obstacle horizontal distance vector $p_{ixy}$, obstacle vertical distance vector $p_{iz}$, horizontal distance vector of target $Z_{xy}$, obstacle horizontal safe distance $r_p$ and obstacle collision distance $r_c$. Output: repulsive force $F_{repel}$.   1: Start the task.   2: if$Z_{xy}>2\ast rp$then   3:     The UAV is judged to fall into the local optimal trap.   4:     if $rc<∥p_{ixy}∥<rp,z<z_{oi}+rc$ then   5:         repulsive force $F_{repel}=\frac{4(r{p}^2-r{c}^2)({∥p_{ixy}∥}^2-r{p}^2)}{{∥p_{ixy}∥}^2-r{c}^2}\ast p_{ixy}+2\ast (1+Z_{xy}-2\ast rp)\ast p_{iz}$, so that the UAV rise.   6:     else   7:         UAV height z satisfies $z<z_{oi}+rc$. Repulsive force $F_{repel}=0^3$.   8: else   9:     The UAV is not judged to fall into the local optimal trap. 10:     if $rc<∥p_{ixy}∥<rp,z\le z_{oi}$ then 11:         repulsive force $F_{repel}=\frac{4(r{p}^2-r{c}^2)({∥p_{ixy}∥}^2-r{p}^2)}{{∥p_{ixy}∥}^2-r{c}^2}\ast p_{ixy}$, so that the UAV avoiding obstacles. 12:     else 13:         There are no obstacles in the path of UAV, repulsive force $F_{repel}=0^3$. 14: return repulsive force $F_{repel}$.

First, the speed error is defined as follows:

$$z_2=v-v_d,$$

(12)

among them, $v_d$ is the virtual speed control signal, which is expressed as:

$$v_d=-k_1{z}_1+{\dot{p}}_{target},$$

(13)

where $k_1=diag\left\{k_1^1,k_1^2,k_1^3\right\}$ and $k_1^i>0$. Target attraction is designed as follows:

$$F_{attract}=-k_2{z}_2-z_1-m^{-1}Kv+e_{3}g+{\dot{v}}_d,$$

(14)

where $k_2=diag\left\{k_2^1,k_2^2,k_2^3\right\}$ and $k_2^i>0$. Next, the main result is given in the design of the gravity field.

Theorem 1. 

Given the UAV system defined by (1) and (2), the attraction given in (14) ensures an asymptotically stable (AS) result of the tracking error $z_1$ in the sense that ${\displaystyle \underset{t\to \infty }{lim}{z}_1=0}$.

Proof. 

First, the derivation of position error $z_1$ and velocity error $z_2$ with respect to time t can be obtained:

$$\begin{array}{c}{\dot{z}}_1=-k_1{z}_1+z_2,\end{array}$$

(15)

$$\begin{array}{c}{\dot{z}}_2=m^{-1}Kv+F_{attract}-e_{3}g-{\dot{v}}_d.\end{array}$$

(16)

The Lyapunov function is designed as follows:

$$V_1=\frac{1}{2}{z}_1^T{z}_1+\frac{1}{2}{z}_2^T{z}_2.$$

(17)

Derivate (17) about time t and bring (15) and (16),

$$\begin{array}{c}{\dot{V}}_1=-k_1{z}_1^T{z}_1+z_1^T(z_1+m^{-1}Kv+F_{attract}-e_{3}g-{\dot{v}}_d).\end{array}$$

(18)

Bring (14) into (18),

$${\dot{V}}_1=-k_1{z}_1^T{z}_1-k_2{z}_2^T{z}_2.$$

(19)

Based on Equations (17) and (19), obviously, $z_1\in L_{\infty },z_2\in L_{\infty }$. There is also Formulas (15) and (16) available, ${\dot{z}}_1\in L_2,{\dot{z}}_2\in L_2$. At the same time, $V_1\left(t\right)$ is a semidefinite function and ${\dot{V}}_1\left(t\right)\le 0$ obviously $\underset{t\to \infty }{lim}{V}_1\left(t\right)=V_{\infty }$ exists and is bounded. For (19) on time $t\in \left[t_0,\infty \right]$ integral, ${\int }_{t_0}^{\infty }(-k_1{z}_1^T{z}_1-k_2{z}_2^T{z}_2)dt\le V_1\left(t_0\right)-V_{\infty }$. Therefore, $z_1$ and $z_2$ are square integrable, according to the Barbalt lemma, $\underset{t\to \infty }{lim}{z}_1={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T,\underset{t\to \infty }{lim}{z}_3={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T$. □

In view of the gravity and repulsion provided by the improved APF, the UAV is regarded as a particle in the potential field, and its acceleration is directly affected by the attractiveness of the repulsion. Therefore, the reference position $p_d$ of the UAV can be obtained by substituting the resultant force $F_{\mathrm{all}}={\displaystyle \sum _{i=1}^n}{F}_{repel}+F_{attract}$ as virtual control force $\mathsf{\Omega }$ into (1) and (2).

3.3. Tracking Loop Design

This part introduces the design of the tracking loop, in which the backstepping control technology is used to achieve the tracking objective, and adaptive technology is used to achieve the fault tolerance objective. Next, the UAV motor fault model is introduced.

3.3.1. UAV Motor Fault Model

The fault types of the UAV motor are as follows:

$$f_i^F={\rho }_i{f}_i,$$

(20)

where $f_{{}_i}^F,i=1,2,3,4$ represents the motor thrust with fault, and ${\rho }_i,i=1,2,3,4$ is UAV motor fault factor with ${\rho }_i\in (0,1]$. Bringing (20) into the UAV system model (1)–(4), the UAV motor fault model is as follows:

$$\dot{p}=v,$$

(21)

$$\dot{v}=m^{-1}Kv+\mathsf{\Omega }-e_{3}g,$$

(22)

$$\dot{\eta }=h_1\left(\eta \right)\omega +D,$$

(23)

$$\dot{\omega }=h_2\left(\omega \right)+B\rho f.$$

(24)

Next, the design of a fault-tolerant controller is introduced.

3.3.2. Adaptive Backstepping Fault-Tolerant Controller Design

The quadrotor UAV can be divided into a position control loop and an attitude control loop. In the position loop, the acceleration of a quadrotor UAV is only controlled by its force in the three-dimensional direction. Obviously, according to the body structure of the quadrotor UAV, its actuator cannot directly give the thrust in the three-dimensional direction of the quadrotor UAV, so these forces are virtual. At the same time, the thrust of the quadrotor UAV comes from its four motors, and its thrust can directly control the attitude angle and height of the UAV, so it is necessary to convert the expected position of the UAV into the expected attitude angle of the UAV.

To begin, the difference between the designed UAV reference position $p_d$ and the actual position p is written as follows:

$$z_3=p-p_d.$$

(25)

For the UAV position systems (21) and (22), the PD position controller is designed as follows:

$$\mathsf{\Omega }=K_P{z}_3\left(t\right)+K_D\frac{d{z}_3\left(t\right)}{dt}.$$

(26)

The expected pitch angle, expected roll angle, and expected lift of a quadrotor UAV are shown as follows:

$$\begin{array}{c}{\varphi }_d={sin}^{-1}\left(\frac{{\mathsf{\Omega }}_{1}sin\left({\psi }_d\right)-{\mathsf{\Omega }}_{2}cos\left({\psi }_d\right)}{u_d}\right),\hfill \\ {\theta }_d={tan}^{-1}\left(\frac{{\mathsf{\Omega }}_{1}cos\left({\psi }_d\right)+{\mathsf{\Omega }}_{2}sin\left({\psi }_d\right)}{{\mathsf{\Omega }}_3}\right),\hfill \\ u_d=\sqrt{{\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2+{\mathsf{\Omega }}_3^2},\hfill \end{array}$$

(27)

where ${\psi }_d$ is the expected yaw angle of the UAV, and its value is set in advance.

Based on the nonlinear adaptive backstepping technique, the following error variables are designed:

$$z_4=a_1(\eta -{\eta }_d)+a_2{\int }_0^t(\eta -{\eta }_d)d\tau ,$$

(28)

$$z_5=\omega -{\omega }_d,$$

(29)

where ${\eta }_d={\left[\begin{array}{cccc}{\varrho }_d& {\varphi }_d& {\theta }_d& {\psi }_d\end{array}\right]}^T$ is the expected values of the lift, roll angle, pitch angle, and yaw angle. $a_1=diag\left\{a_1^1,a_1^2,a_1^3,a_1^4\right\}$, $a_2=diag\left\{a_2^1,a_2^2,a_2^3,a_2^4\right\}$ are the gain coefficients and $a_1^i>0,a_2^i>0$, ${\omega }_d$ is the control variables of the virtual angular velocity, whose specific forms is as follows:

$${\omega }_d={\left(a_1{h}_1\left(\eta \right)\right)}^{-1}(a_1{\dot{\eta }}_d-a_{1}sgn\left(z_4^T\right)\widehat{\overline{D}}-a_2(\eta -{\eta }_d)-k_3{z}_4),$$

(30)

where $k_3$ is the virtual angular velocity signal gain, $k_3=diag\left\{k_3^1,k_3^2,k_3^3\right\}$ and $k_3>0$. The nonlinear adaptive backstepping controller is designed as follows:

$$f={\left(B\widehat{\rho }\right)}^{-1}(-k_4{z}_5-{\left(a_1{h}_1\left(\eta \right)\right)}^T{z}_4-h_2\left(\omega \right)+{\dot{\omega }}_d),$$

(31)

where $k_4$ is the control gain of the nonlinear adaptive backstepping controller, $k_3=diag\left\{k_3^1,k_3^2,k_3^3\right\}$ and $k_3>0$. $\widehat{\rho }$ is the estimated value of motor fault factor $\rho$. According to the projection gravity, the adaptive estimation law is designed as follows:

$$\dot{\widehat{{\rho }_i}}=\left\{\begin{array}{cc}{\gamma }_{1i}{z}_5^T{B}_i{f}_i,& 0<{\widehat{\rho }}_i\le 1;\\ 0,& other.\end{array}\right.$$

(32)

$\widehat{\overline{D}}$ is the estimated value of the disturbance upper bound $\overline{D}$, and the adaptive estimation law is designed as follows:

$${\dot{\widehat{\overline{D}}}}_j={\gamma }_{2j}\left|z_{4j}^T\right|,$$

(33)

where ${\gamma }_{1i}$ and ${\gamma }_{2j}$ are the gain coefficients to be designed.

Theorem 2. 

Considering the influence of motor fault form (20) on UAV, a nonlinear adaptive backstepping controller (31) and an adaptive estimation law of motor fault factor (32) are designed for the UAV attitude system (23) and (24) to realize the asymptotic stability of attitude error.

Proof. 

First, the Lyapunov function is designed as follows:

$$V_2=\frac{1}{2}{z}_4^T{z}_4+\frac{1}{2}{z}_5^T{z}_5.$$

(34)

The derivation of the above Lyapunov function with respect to time can be obtained:

$$\begin{array}{c}{\dot{V}}_2=-k_3{z}_4^T{z}_4+z_5^T({\left(a_1{h}_1\left(\eta \right)\right)}^T{z}_4+h_2\left(x_2\right)+B\rho f-{\dot{\omega }}_d).\end{array}$$

(35)

The estimation error of the motor fault factor is as follows:

$$\tilde{\rho }=\rho -\widehat{\rho }.$$

(36)

The estimation error of the disturbance upper bound is as follows:

$$\tilde{\overline{D}}=\overline{D}-\widehat{\overline{D}}.$$

(37)

Bring (36) and (37) into (35) and derive:

$$\begin{array}{c}{\dot{V}}_2\le -k_3{z}_4^T{z}_4+z_5^T({\left(a_1{h}_1\left(\eta \right)\right)}^T{z}_4+h_2\left(\omega \right)+B\widehat{\rho }f-{\dot{\omega }}_d)+z_5^{T}B\tilde{\rho }f+\left|z_4^T\right|a_1\tilde{\overline{D}}.\end{array}$$

(38)

The new Lyapunov function is designed as follows:

$$V_3=V_2+\sum _{i=1}^4\frac{{{\tilde{\rho }}_i}^2}{2{\gamma }_{1i}}+\sum _{j=1}^4\frac{{\tilde{\overline{D}}}_j^2}{2{\gamma }_{2j}}.$$

(39)

The derivation of Equation (39) with respect to time t can be obtained as follows:

$$\begin{array}{cc}\hfill {\dot{V}}_3& \le -k_3{z}_4^T{z}_4+z_5^T({\left(a_1{h}_1\left(\eta \right)\right)}^T{z}_4+h_2\left(\omega \right)+B\widehat{\rho }f-{\dot{\omega }}_d)\hfill \\ & +z_5^{T}B\tilde{\rho }f+\left|z_4^T\right|a_1\tilde{\overline{D}}+\sum _{i=1}^4\frac{{\dot{\tilde{\rho }}}_i{\tilde{\rho }}_i}{{\gamma }_i}+\sum _{j=1}^4\frac{{\tilde{\overline{D}}}_j\dot{{\tilde{\overline{D}}}_j}}{{\gamma }_{2j}}.\hfill \end{array}$$

(40)

By bringing Formulas (31) and (32) into Formula (40):

$${\dot{V}}_3\le -k_3{z}_4^T{z}_4-k_4{z}_5^T{z}_5.$$

(41)

Based on Equations (34) and (41), obviously, $z_4\in L_{\infty },z_5\in L_{\infty }$. There are also Formulas (28)–(30) available, ${\dot{z}}_4\in L_2,{\dot{z}}_5\in L_2$. At the same time, $V_3\left(t\right)$ is a semidefinite function and ${\dot{V}}_3\left(t\right)\le 0$, obviously $\underset{t\to \infty }{lim}{V}_3\left(t\right)=V_{\infty }$ exists and is bounded. For (41) on time $t\in \left[t_0,\infty \right]$ integral, ${\int }_{t_0}^{\infty }(-k_1{z}_1^T{z}_1-k_2{z}_2^T{z}_2)dt\le V_3\left(t_0\right)-V_{\infty }$. Therefore, $z_4$ and $z_5$ is square integrable, according to the Barbalt lemma, $\underset{t\to \infty }{lim}{z}_4={\left[\begin{array}{cccc}0& 0& 0& 0\end{array}\right]}^T,\underset{t\to \infty }{lim}{z}_5={\left[\begin{array}{cccc}0& 0& 0& 0\end{array}\right]}^T$. □

4. Simulation Results

In this chapter, the obstacle avoidance algorithm proposed in this paper is verified by a simulink simulation experiment. The UAV physical parameters selected in this paper are as follows: The quality of the UAV is $m=0.55$ kg; the distance from the motor to the UAV centroid is $l=0.11$ m; the torque coefficient is $c=1.1905\ast {10}^{-2}\mathrm{N}\ast \mathrm{m}{\mathrm{s}}^2/\mathrm{ra}{\mathrm{d}}^2$, the three-dimensional rotational inertia of the UAV is $J_x=1.9\ast {10}^{-3}\mathrm{N}\ast \mathrm{m}{\mathrm{s}}^2/\mathrm{ra}{\mathrm{d}}^2$, $J_y=1.9\ast {10}^{-3}\mathrm{N}\ast \mathrm{m}{\mathrm{s}}^2/\mathrm{ra}{\mathrm{d}}^2$, $J_z=2.4\ast {10}^{-3}\mathrm{N}\ast \mathrm{m}{\mathrm{s}}^2/\mathrm{ra}{\mathrm{d}}^2$; the trajectory of the moving ground target selected in this paper varies according to time as follows. Where the moving ground target moved slowly 10 s ago, and the expression is: $x\left(t\right)=0;$$y\left(t\right)=0.5\ast t$. After 10 s, the speed of the moving object on the ground becomes twice as fast as before, and the expression is: $x\left(t\right)=0$; $y\left(t\right)=1\ast t-5$.

The moving ground target moves along the y-axis; that is, the moving ground target moves uniformly along the y-axis at 0.5 m/s in the first 10 s, accelerates in the 10th second, and moves uniformly along the y-axis at 1 m/s after 10 s. Following the UAV according to the expected height of 9 m above the moving ground target tracking, three obstacles are established in the simulation map, and their coordinates and heights are shown in

Table 1. Obstacle information.

Obstacle	Height (m)	x-Axis (m)	y-Axis (m)	Horizontal Safety Distance ${\mathit{r}}_{\mathit{p}}$ (m)	Obstacle Collision Distance ${\mathit{r}}_{\mathit{c}}$ (m)
Obstacle 1	12	−1	2	2	1
Obstacle 2	12	1	2	2	1
Obstacle 3	12	0.5	15	2	1

:

In this paper, the UAV motor loss-of-effectiveness fault is as follows:

$$\rho =\left\{\begin{array}{cc}{\rho }_1=0.7,{\rho }_1=0.5,{\rho }_1=0.8,{\rho }_1=0.5,& t\in [5,20];\\ {\rho }_1=0.7,{\rho }_1=0.5,{\rho }_1=0.8,{\rho }_1=0.5,& t\in [30,35];\\ {\rho }_1=1,{\rho }_1=1,{\rho }_1=1,{\rho }_1=1,& other.\end{array}\right.$$

(42)

The external disturbances are as follows:

$$D=\left\{\begin{array}{cc}\left[\begin{array}{cccc}0.05\ast sin\left(t\right)& 0.05\ast sin\left(t\right)& 0.05\ast sin\left(t\right)& 0.05\ast sin\left(t\right)\end{array}\right],& t\in [5,20];\\ \left[\begin{array}{cccc}0& 0& 0& 0\end{array}\right],& other.\end{array}\right.$$

(43)

The simulation results are as follows:

The error $z_1=p-p_{target}$ between the actual position of the UAV and the target position is shown in Figure 3. By observing the above simulation results, it can be found that at a certain time, the UAV starts to rise, and with the gradual approximation of the UAV and the expected height, $z_{1z}$ gradually approaches zero. At the same time, the moving ground object began to move, and at $t=1$ s, the error in the y-direction gradually increased. In fact, the repulsion of the obstacle was opposite to the gravity of the target point in the same direction, which made the UAV fall into the local optimal trap and forced the UAV to stop following the task. However, as the horizontal distance between the UAV and the ground tracking object gradually widens until it reaches the decision threshold $Z_{xy}>2\ast rp$, the UAV judges itself that it is in the local optimal trap, and the vertical obstacle avoidance repulsion field starts. Around $t=9$ s, the UAV altitude rises until crossing obstacles, getting rid of the local optimal trap, then the altitude drops to a predetermined height and continues to complete the following task, $z_1=0^3$. When $t=10$ s, the moving ground object starts to accelerate. At the same time, the UAV also performs accelerated tracking. When an asymmetric single obstacle is encountered, obstacle avoidance can also be completed in the plane.

The error $z_3$ between the UAV obstacle avoidance position $p_d$ and the real position p is shown in Figure 4. Among them, the fault occurs in $t=10$ s. At the same time, it is also a key time for the UAV to perform vertical obstacle avoidance to get rid of most of the local traps. Observing the above simulation results, it can be seen that when the UAV obstacle avoidance occurs at the same time, the UAV can also quickly complete the obstacle avoidance task and continue to perform the following task. When $t=10$ s, the speed of the moving ground target increases to twice the original speed, and $z_3$ increases rapidly. When the UAV gets rid of the local minimum trap and starts to track the target, $z_3$ decreases rapidly. At $t=20$ s, in the face of asymmetric obstacles, because it is not necessary to get rid of the local optimal trap, the interference caused by the UAV motor fault is small. At $t=30$ s and $t=35$ s, by observing the above simulation results, the error caused by the UAV motor fault can be controlled within ${10}^{-3}$.

The attitude angle error $z_4$ of the UAV is shown in Figure 5. At $t=10$ s, the motor fault occurs when the UAV gets rid of the local optimal trap, and the speed of the moving ground target doubles, $z_4$, fluctuates. At t = 20 s, t = 30 s, t = 35 s, after the motor fault occurs, the UAV can still ensure the asymptotic stability of attitude angle error $z_4$ under the fault-tolerant control, and the maximum error fluctuation is controlled within ${10}^{-4}$ orders of magnitude. The simulation images without considering faults are shown in Figure 6, Figure 7 and Figure 8.

By observing the three groups of comparative simulation images, it can be found that when the motor fault occurs in the first UAV at $t=10$ s, the three groups of errors of the UAV have been unable to meet the expected requirements, and the tracking task of the UAV has failed.

Finally, 3D images of the UAV trapped in the local optimal trap and separated from this algorithm are given in Figure 9.

5. Conclusions

In this paper, an obstacle avoidance control strategy for an underactuated quadrotor unmanned aerial vehicle with a motor loss-of-effectiveness fault and disturbance method is presented. By introducing a height factor into the artificial potential field function, the height of the UAV is improved to get rid of the local optimum so that the UAV can overcome the obstacle. Considering the sudden motor loss-of-effectiveness fault of the UAV, adaptive technology is used to estimate the fault parameters online to restrain the effects of the motor loss-of-effectiveness fault. Simulations are conducted to demonstrate the effectiveness of the designed obstacle avoidance control strategy.