Research on Trajectory Tracking Control of Inspection UAV Based on Real-Time Sensor Data

Abstract

In power inspection, uncertainties, such as wind gusts in the working environment, affect the trajectory of the inspection UAV (unmanned aerial vehicle), and a sliding mode adaptive robust control algorithm is proposed in this paper to solve this problem. For the nonlinear and under-driven characteristics of the inspection UAV system, a double closed-loop control system which includes a position loop and attitude loop is designed. Lyapunov stability analysis is used to determine whether the designed system could finally achieve asymptotic stability. Sliding-mode PID control and a backstepping control algorithm are applied to analyze the superiority of the control algorithm proposed in this paper. A PX4 based experimental platform system is built and experimental tests were carried out under outdoor environment. The effectiveness and superiority of the control algorithm are proposed in this paper. The experimental results show that the sliding mode PID control can achieve good accuracy with smaller computing costs. For nonlinear interference, the sliding mode adaptive robust control strategy can achieve higher trajectory tracking accuracy.

1. Introduction

Since the second industrial revolution, electricity has played an indispensable role in human life. China has a vast territory, and overhead transmission lines cover most of it for power transmission. However, most of these transmission networks are built outdoors and are vulnerable to bird nesting, thunder, and lightning. Therefore, it is necessary to conduct power inspections on the transmission line network. At present, China mainly uses electric inspection drones for this project. Compared with manual inspections, inspection drones can improve inspection efficiency and enhance safety [1,2,3]. As shown in Figure 1, electric power workers are conducting electric inspection work by use inspection drone.

In the power inspection project, the inspection drone works according to the inspection trajectory established by the staff, as shown in Figure 2. However, due to the interference of factors such as wind gusts at work, the inspection UAV has deviations in the process of tracking the inspection trajectory. Therefore, it is very necessary to design a controller that can make the patrol unmanned and track the patrol track stably.

At present, researchers have proposed the following control algorithms for the trajectory tracking control of the inspection drone: PID control, adaptive control, backstepping control, active disturbance rejection control, sliding mode control, etc. [4,5,6,7]. The PID control structure is simple, but the process of adjusting the parameters is relatively cumbersome and the robustness is poor.

Literature [8,9] uses PID control algorithm to design the controller. The error is calculated by the data obtained by the position and attitude sensor and theoretical data, the PID parameters are adjusted to make the error approach zero, and the flight trajectory of the UAV can be tracked. However, there are two main disadvantages. On the one hand, the process of adjusting parameters is complicated, while on the other hand, the robustness is poor, and it is difficult to achieve high-precision trajectory tracking. Literature [10] proposed the use of back-stepping control to design the trajectory tracking controller, establishing the position and attitude error dynamic model of the UAV, using the backstepping method to design the stabilization control items, and using Lyapunov theory to prove the closed-loop system stability, the final trajectory tracking experiment proved the effectiveness of the control algorithm. Although the system is stable, after disturbance is added, the tracking effect becomes worse, and the anti-interference ability of the system is not strong. In [11], the extended state observer of the switching function is used to estimate the disturbance in trajectory tracking, and the calculated estimated value is combined with sliding mode control to finally achieve the trajectory tracking effect. However, chattering occurs in sliding mode control, and the tracking accuracy is affected. Literature [12] uses dual-loop control to achieve high-precision trajectory tracking. The inner loop adopts sliding mode control, while the outer loop adopts linear auto-disturbance rejection control to suppress external interference. Experiments have proven that this method has fast response speed, strong robustness, and can meet the needs of trajectory tracking control of inspection drones. In the literature [13,14,15], fuzzy control is used to design the controller, and the fuzzy system is used to gradually approximate the uncertain items and external interference in the attitude model, while the controller is designed in combination with back-stepping control, which improves the accuracy of trajectory tracking. In recent years, neural network control has begun to increase in popularity, and it has also been favored by scholars. In the literature [16,17,18], facing the uncertainty of the inspection UAV model, the neural network control is used to approximate, and then the external disturbance is suppressed by the observer. Finally, combined with the backstep control, the trajectory tracking effect of the attitude and position of the inspection drone is well achieved.

This paper addresses the problem whereby the inspection UAV is disturbed by unknown factors, such as wind gusts in the trajectory tracking process, resulting in the inability to track the inspection trajectory with high accuracy. In this paper, a sliding mode adaptive robust control algorithm is used to design the controller. Compared with other control algorithms, the adaptive algorithm part can better suppress disturbances, while the sliding mode control part can enhance the stability of the system and compensate for the poor robustness of other control algorithms. A control group experiment was also conducted using the sliding mode PID control algorithm and the backstepping control algorithm, and the simulation results and experimental results proved that, in terms of control structure, the method in this paper is more concise, efficient, and has certain advantages compared with other control methods that require the design of an observer or compensator and the repeated adjustment of control parameters.

2. Inspection Drone Dynamic Model

2.1. Establishment of Coordinate System

In order to better study the motion state of inspection drone dynamic model, it is necessary to select the appropriate coordinate system. In this paper, the body axis system and inertial coordinate system are introduced to describe the rotation, position, and attitude of the aircraft. The body axis system is A($A_x$, $A_y$, $A_z$). The inertial coordinate system is B($B_x, B_y$, $B_z$). As shown in Figure 3, γ, θ, and $\psi$ are roll angle, pitch angle, and yaw angle, respectively. Through the rotation matrix of three angles, the transformation matrix from body axis system A to inertial coordinate system B can be deduced ${\mathit{R}}_{\mathit{A}\mathit{B}}$ [19].

$${\mathit{R}}_{\mathit{A}\mathit{B}} ={\mathit{R}}_{\mathit{\gamma }}{\mathit{R}}_{\mathit{\theta }}{\mathit{R}}_{\mathit{\psi }}=\left[\begin{array}{ccc}C\theta C\psi & \mathrm{S}\theta \mathrm{S}\gamma C\psi -\mathrm{S}\psi C\gamma & C\psi C\gamma \mathrm{S}\theta +\mathrm{S}\gamma \mathrm{S}\psi \\ C\theta \mathrm{S}\psi & \mathrm{S}\theta \mathrm{S}\gamma \mathrm{S}\psi +C\psi C\gamma & \mathrm{S}\theta \mathrm{S}\psi C\gamma -\mathrm{S}\gamma C\psi \\ -\mathrm{S}\theta & C\theta \mathrm{S}\gamma & C\theta C\gamma \end{array}\right]$$

(1)

2.2. Establishment of Coordinate System

In order to facilitate the establishment of the dynamic model, the following assumptions are proposed:

• The four-rotor aircraft is regarded as a rigid body, and its body structure is completely uniform and symmetrical.
• The origin O of the airframe coordinate system coincides with the origin O of the inertial coordinate system to ensure that the inertial matrix is a diagonal matrix in the rigid body coordinate system [20].

Because the main motion forms of quadrotor aircraft are divided into two types, one is translation motion relative to inertial frame B, while the other is rotation relative to body axis system A. Translation has 3 degrees of freedom, rotation has 3 degrees of freedom, a total of 6 degrees of freedom. Therefore, according to the total degrees of freedom, the generalized coordinates can be determined as q = ${\left(\xi ,\eta \right)}^{\mathrm{T}}$, where $\mathit{\xi }$ = [x,y,z]^T represents location information; $\mathit{\eta }$ = [$\gamma , \theta$,$\psi$]^T indicates attitude information. The mass of the aircraft is m, the acceleration of gravity is g, and the distance from the end of the rotor to the center of gravity of the aircraft is l. The inertia matrix of quadrotor aircraft in the body axis system is as follows:

$$\mathit{I}=\left[\begin{array}{ccc}{I}_x& 0& 0\\ 0& I_y& 0\\ 0& 0& I_z\end{array}\right]$$

where $I_x,I_y,I_z$ represent the moment of inertia of the x axis, y axis, and z axis in a coordinate system, which can be obtained by testing [21].

The expression equation of position part of quadrotor aircraft is as follows:

$$m\ddot{\mathit{\xi }}=u_1{\mathit{R}}_{\mathit{A}\mathit{B}}{\mathit{e}}_{\mathbf{3}}-\mathrm{mg}{\mathit{e}}_{\mathit{3}}+{\mathit{d}}_{\mathit{s}}$$

(2)

where ${\mathit{e}}_{\mathit{3}}$=${\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^{\mathrm{T}}, {\mathit{d}}_{\mathit{s}}$ is the disturbing force of the moving part of the position, $u_1$ is the lift power of a four rotor aircraft, ${\mathit{R}}_{\mathit{A}\mathit{B}}$ is a transformation matrix, and ${\mathit{d}}_{\mathit{s}}$= ${\left[\begin{array}{ccc}{d}_x& d_y& d_z\end{array}\right]}^{\mathrm{T}}$ is the interference force in the translational motion.

The equations for the rotational part of the quadrotor aircraft are as follows:

$$\mathit{J}\ddot{\mathit{\eta }}+C\dot{\mathit{\eta }}=\mathit{\tau }+{\mathit{d}}_{\mathit{M}}$$

(3)

where τ is the moment of rolling, pitching and yawing, τ = ${\left[\begin{array}{ccc}{\tau }_{\gamma }& {\tau }_{\theta }& {\tau }_{\psi }\end{array}\right]}^{\mathrm{T}}$. ${\mathit{d}}_{\mathit{M}}$ = ${\left[\begin{array}{ccc}{d}_{\gamma }& d_{\theta }& d_{\psi }\end{array}\right]}^{\mathrm{T}}$; is the disturbing torque in the rotational motion; J is the representation of the moment of inertia I of the rigid body in B coordinate system.

$$\mathit{J}=\left[\begin{array}{ccc}{I}_x& 0& -I_x\mathrm{S}\theta \\ 0& I_y{C\gamma }^2+I_z{\mathrm{S}\gamma }^2& \left(I_y-I_z\right)\mathrm{S}\gamma C\gamma C\theta \\ -I_x\mathrm{S}\theta & \left(I_y-I_z\right)\mathrm{S}\gamma C\gamma C\theta & I_x\mathrm{S}{\theta }^2+I_y{\mathrm{S}\gamma }^{2}C{\theta }^2+I_z{C\gamma }^{2}C{\theta }^2\end{array}\right]$$

(4)

The coefficient C is the Coriolis force and centrifugal term [22], which is expressed in the following form [23].

$$\mathit{C}=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$$

(5)

$$\begin{array}{l}{C}_{11}=0\\ C_{12}=-I_x\dot{\psi }C\theta +\left(I_y-I_z\right)(\dot{\theta }\mathrm{S}\gamma C\gamma +\psi {\mathrm{S}\gamma }^{2}C\theta -\dot{\psi }{C\gamma }^{2}C\theta )\\ C_{13}=\left(I_z-I_y\right)\dot{\psi }\mathrm{S}\gamma C\gamma C{\theta }^2\\ C_{21}=-C_{12}\\ C_{22}=\left(I_z-I_y\right)\dot{\gamma }\mathrm{S}\gamma C\gamma \\ C_{23}=-I_x\dot{\psi }C\theta \mathrm{S}\theta +I_y\dot{\psi }C\theta \mathrm{S}\theta {\mathrm{S}\gamma }^2+I_z\dot{\psi }C\theta \mathrm{S}\theta {C\gamma }^2\\ C_{31}=\left(I_y-I_z\right)\dot{\psi }\mathrm{S}\gamma C\gamma -I_x\dot{\theta }C\theta \\ C_{32}=\left(I_z-I_y\right)(\dot{\theta }\mathrm{S}\gamma C\gamma \mathrm{S}\theta +{\dot{\gamma }\mathrm{S}\gamma }^{2}C\theta )+\left(I_y-I_z\right){\dot{\gamma }C\gamma }^{2}C\theta +I_x\dot{\psi }C\theta \mathrm{S}\theta -I_y\dot{\psi }{\mathrm{S}\gamma }^{2}C\theta S\theta -I_z\dot{\psi }C{\gamma }^{2}C\theta \mathrm{S}\theta \\ C_{33}=\left(I_y-I_z\right)\dot{\gamma }\mathrm{S}\gamma C\gamma C{\theta }^2-I_y\dot{\theta }{\mathrm{S}\gamma }^{2}C\theta \mathrm{S}\theta -I_z\dot{\theta }{C\gamma }^{2}C\theta \mathrm{S}\theta +I_x\dot{\theta }C\theta \mathrm{S}\theta \end{array}$$

3. Control System Design

3.1. Controller Design Structure

In this paper, a sliding mode adaptive robust trajectory tracking control strategy for quadrotor aircraft is proposed. The system is divided into two dynamic subsystems: position subsystem and attitude subsystem. The position subsystem is taken as the outer loop subsystem, and the attitude part is taken as the inner loop subsystem. The overall design scheme is shown in Figure 4.

3.2. Position Controller Design

The main design idea of the position controller is to compare the ideal position trajectory with the actual position trajectory, and finally make the error of the two approaches to zero.

Thus, the desired tracking displacement trajectory can be set as ${\mathit{\xi }}_{\mathit{c}}$, The displacement obtained by actual tracking is recorded as $\mathit{\xi }$. Set the error to ${\mathit{e}}_{\mathit{s}}$, where ${\mathit{e}}_{\mathit{s}}$= $\mathit{\xi }-{\mathit{\xi }}_{\mathit{c}}$. According to Formula (2), $\ddot{\mathit{\xi }}$ can be expressed separately. Then, find the two derivatives on both sides of the error expression as follows:

$$\ddot{{\mathit{e}}_{\mathit{s}}}=\frac{u_{\mathit{s}}+{\mathit{d}}_{\mathit{s}}}{m}-g{\mathit{e}}_{\mathit{3}}-\ddot{{\mathit{\xi }}_{\mathit{c}}}$$

(6)

In the formula, ${\mathit{u}}_{\mathit{s}}$= $u_1{\mathit{R}}_{\mathit{A}\mathit{B}}{\mathit{e}}_{\mathit{3}}$ is the control input that needs to be designed,${\mathit{u}}_{\mathit{s}}$ is understood as a virtual control law, as an intermediate quantity of coordinate transfomation.

According to the sliding mode adaptive robust control method, the sliding mode function is designed first. The sliding mode function can be set as follows:

$${\mathit{S}}_{\mathbf{1}}=\dot{{\mathit{e}}_{\mathit{s}}}+{\lambda }_1{\mathit{e}}_{\mathit{s}},{\lambda }_1>0$$

(7)

The virtual control law ${\mathit{u}}_{\mathit{s}}$ of the position subsystem is designed as follows:

$${\mathit{u}}_{\mathit{s}}=\widehat{m}{\overline{\mathit{u}}}_{\mathit{s}}-\widehat{{\mathit{d}}_{\mathit{s}}}$$

(8)

${\overline{u}}_{\mathit{s}}$ is expressed as:

$${\overline{\mathit{u}}}_{\mathit{s}}=g{\mathit{e}}_{\mathit{3}}+\ddot{{\mathit{\xi }}_{\mathit{c}}}-{\lambda }_1\dot{{\mathit{e}}_{\mathit{s}}}-c_1{\mathit{S}}_{\mathbf{1}}$$

(9)

$c_1$ is the control coefficient, where $c_1>0$, $\widehat{m}$ is an estimator of quality, $\widehat{{\mathit{d}}_{\mathit{s}}}$ is an etimate of the disturbance force of the displacement part.

Calculating the first derivative of sliding mode function:

$$\dot{{\mathit{S}}_{\mathbf{1}}}=\ddot{{\mathit{e}}_{\mathit{s}}}+{\lambda }_1\dot{{\mathit{e}}_{\mathit{s}}}$$

(10)

$$\dot{{\mathit{S}}_{\mathbf{1}}}=\frac{\widehat{m}{\overline{\mathit{u}}}_{\mathit{s}}-\widehat{{\mathit{d}}_{\mathit{s}}}+{\mathit{d}}_{\mathit{s}}}{m}-g{\mathit{e}}_{\mathit{3}}-\ddot{{\mathit{\xi }}_{\mathit{c}}}+{\lambda }_1\dot{{\mathit{e}}_{\mathit{s}}}$$

(11)

Further sorting out:

$$\dot{{\mathit{S}}_{\mathbf{1}}}=\frac{\widehat{m}{\overline{\mathit{u}}}_{\mathit{s}}-\widehat{{\mathit{d}}_{\mathit{s}}} + {\mathit{d}}_{\mathit{s}}}{m}-{\overline{\mathit{u}}}_{\mathit{s}}-c_1{\mathit{S}}_{\mathbf{1}}$$

(12)

Compared with other articles, in the position motion part, the main purpose of this paper is to reduce the influence of the disturbance torque on the motion trajectory, so the disturbance torque is designed as the adaptive law to better track the position trajectory. Simultaneously, in order to reflect the superiority of the control method, considering the change of the mass caused by the load of the aircraft, which will affect the lift dynamics and stability of the quadrotor aircraft, the mass adaptive law is also designed.

The adaptive law is defined as follows:

$$\dot{\widehat{{\mathit{d}}_{\mathit{s}}}}={\gamma }_1{\mathit{S}}_{\mathbf{1}}$$

(13)

$$\dot{\widehat{\mathit{m}}}=-{\gamma }_2{\mathit{S}}_{\mathbf{1}}{}^{\mathrm{T}}{\overline{\mathit{u}}}_{\mathit{s}}$$

(14)

${\gamma }_1$ and ${\gamma }_2$ are control coefficients.

The disturbance force error of displacement part is defined as ${\mathit{e}}_{{\mathit{d}}_{\mathit{s}}}$=${\mathit{d}}_{\mathit{s}}-\widehat{{\mathit{d}}_{\mathit{s}}}$. The quality error is $e_m$ = m$-\widehat{m}$.

Lyapunov function is defined as follows:

$$V_1=\frac{1}{2}{\mathit{S}}_{\mathbf{1}}{}^{\mathrm{T}}{\mathit{S}}_{\mathbf{1}}+\frac{1}{2m{\gamma }_1}{\mathit{e}}_{{\mathit{d}}_{\mathit{s}}}{}^{\mathrm{T}}{\mathit{e}}_{{\mathit{d}}_{\mathit{s}}}+\frac{1}{2m{\gamma }_2}{e}_m{}^2$$

(15)

The derivation of Lyapunov function is obtained:

$$\dot{V_1}={\mathit{S}}_{\mathbf{1}}{}^{\mathrm{T}}\left(\frac{\widehat{m}{\overline{u}}_{\mathit{s}}-\widehat{{\mathit{d}}_{\mathit{s}}} +{\mathit{d}}_{\mathit{s}}}{m}-{\overline{\mathit{u}}}_{\mathit{s}}-c_1{\mathit{S}}_{\mathbf{1}}\right)+\frac{1}{m{\gamma }_1}{\mathit{e}}_{{\mathit{d}}_{\mathit{s}}}{}^{\mathrm{T}}{\dot{e}}_{{\mathit{d}}_{\mathit{s}}}+\frac{1}{m{\gamma }_2}{e}_m{\dot{e}}_m$$

(16)

The interference force and mass of the position part are defined as slow time-varying signals, so the mass error ${\dot{e}}_m$ is related to $\dot{\widehat{m}}$ in adaptive control law ${\dot{e}}_m=-\dot{\widehat{m}}$. By analogy, the position disturbance force error and $\dot{\widehat{{\mathit{d}}_{\mathit{s}}}}$ in adaptive control law also exists as ${\dot{e}}_{{\mathit{d}}_{\mathit{s}}}$ = $-\dot{\widehat{{\mathit{d}}_{\mathit{s}}}}$. The mass error of position interference force error is expressed by this relation, and then it is brought into the Lyapunov derivative function

$$\dot{V_1}=-c_1{\mathit{S}}_{\mathbf{1}}{}^{\mathrm{T}}{\mathit{S}}_{\mathbf{1}}-\frac{{\mathit{e}}_{{\mathit{d}}_{\mathit{s}}}{}^{\mathrm{T}}}{m{\gamma }_1}(\dot{\widehat{{\mathit{d}}_{\mathit{s}}}}-{\gamma }_1{\mathit{S}}_{\mathbf{1}})-\frac{{\mathit{e}}_m}{m{\gamma }_2}(\dot{\widehat{m}}+{\gamma }_2{\mathit{S}}_{\mathbf{1}}{}^{\mathrm{T}}{\overline{\mathit{u}}}_{\mathit{s}})$$

(17)

According to the above relations, $\dot{V_1}$ is approximately equal to $-c_1{\mathit{S}}_{\mathbf{1}}{}^{\mathrm{T}}{\mathit{S}}_{\mathbf{1}}$. According to Lyapunov’s law [24], in order to ensure the system is in a stable state, it is necessary for $\dot{V_1}=-c_1{\mathit{S}}_{\mathbf{1}}{}^{\mathrm{T}}{\mathit{S}}_{\mathbf{1}}\le$ 0. The analysis shows that when the sliding mode function is not zero, the value of Lyapunov derivative function is always less than zero. According to the property of derivative function, the Lyapunov function decreases monotonically, and the sliding mode function, disturbance force error, and mass error decrease gradually. When the sliding mode function ${\mathit{S}}_{\mathbf{1}}$ is equal to zero, the Liapunov derivative is equal to zero. At this time, the concept of LaSalle invariant set principle is introduced to analyze. According to the principle of LaSalle invariant set [25], it can be considered that the sliding mode function converges to zero gradually. When t$\to \infty$,${\mathit{S}}_{\mathbf{1}}\to 0$. Further, the sliding mode function ${\mathit{S}}_{\mathbf{1}}$ is equal to zero, because the Lyapunov derivative function $\dot{V_1}$= 0, so the Lyapunov function is a constant, and ${\mathit{e}}_{{\mathit{d}}_{\mathit{s}}}$ and $e_m$ are invariant bounded quantities.

In this case, the sliding mode function will continue to cross at zero, causing the two error quantities to constantly change within a bounded interval, and there is no guarantee that the two error quantities will approach zero. In order to avoid the excessive amount of quality error, the input quantity $u_1$ needs to be continuously increased, so when the maximum and minimum values of the quality are known, the adaptive law of quality is corrected by a mapping adaptive algorithm [26].

$$\begin{gathered} \dot{\widehat{m}}=\mathrm{Pro}{j}_{\widehat{m}}\left(-{\gamma }_2{\mathit{S}}_{\mathbf{1}}{}^{\mathrm{T}}{\overline{\mathit{u}}}_{\mathit{s}}\right) \\ \mathrm{Pro}{j}_{\widehat{m}}(.)=\{\begin{array}{c}0, \mathrm{if} \widehat{m}\ge m_{max}\mathrm{and}.>0\\ 0, \mathrm{if} \widehat{m}\le -m_{max}\mathrm{and}.<0, \\ \mathrm{other}\end{array} \end{gathered}$$

(18)

When $\widehat{m}$ is greater than the maximum value of the limit range and is in an increasing state, at this time, $\widehat{m}$ remains unchanged, $\widehat{m}$ is equal to 0. When $\widehat{m}$ is lower than the minimum value of the limit range and is in a state of decreasing, at this time, $\widehat{m}$ remains unchanged, and $\widehat{m}$ is equal to 0. The mapping adaptive algorithm can not only ensure that $\widehat{m}$ is equal to 0, but also ensures that $\dot{V_1}\le$0.

In the case that the virtual control law ${\mathit{u}}_{\mathit{s}}$ can be calculated, the actual lifting power $u_1$ of the quadrotor and the sum ${\mathit{\eta }}_{\mathit{s}}$ of the three attitude angle signals of the attitude subsystem must be calculated. Therefore, the virtual control input quantity ${\mathit{u}}_{\mathit{s}}$ can be expressed as: ${\left[\begin{array}{ccc}{u}_x& u_y& u_z\end{array}\right]}^{\mathrm{T}}$, and the intermediate quantity ${\mathit{\eta }}_{\mathit{s}}$ is expressed as the following form: ${\left[\begin{array}{ccc}{\gamma }_s& {\theta }_s& {\psi }_s\end{array}\right]}^{\mathrm{T}}$.

Because ${\mathit{u}}_{\mathit{s}}$=$u_1{\mathit{R}}_{\mathit{A}\mathit{B}}{\mathit{e}}_{\mathit{3}}$, the matrix operation on ${\mathit{u}}_{\mathit{s}}$ is expressed as the following form, and the power input expression in the x, y, and z directions can be obtained by sorting.

$$\left[\begin{array}{c}{u}_x\\ u_y\\ u_z\end{array}\right]=u_1{\mathit{R}}_{\mathit{A}\mathit{B}}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$$

(19)

$$u_x=u_1\left(\cos \psi \cos \gamma \sin \theta +\sin \gamma \sin \psi \right)$$

(20)

$$u_y=u_1\left(\sin \theta \sin \psi \cos \gamma -\sin \gamma \cos \psi \right)$$

(21)

$$u_z=u_1\left(\cos \theta \cos \gamma \right)$$

(22)

The $u_1$= $\frac{u_z}{\cos \theta \cos \gamma }$ are substituted into expressions (21) and (22) and transformed into state space expression.

$$u_x=u_z\left[\begin{array}{cc}\cos \psi & \sin \psi \end{array}\right]\left[\begin{array}{c}\tan \theta \\ \tan \gamma \sec \theta \end{array}\right]$$

(23)

$$u_y=u_z\left[\begin{array}{cc}\cos \psi & -\sin \psi \end{array}\right]\left[\begin{array}{c}\tan \theta \\ \tan \gamma \sec \theta \end{array}\right]$$

(24)

Adding the intermediate amount ${\mathit{\eta }}_{\mathit{s}}$ leads to:

$$\left[\begin{array}{c}{u}_x\\ u_y\end{array}\right]=u_z\left[\begin{array}{c}\begin{array}{cc}\cos {\psi }_s& \sin {\psi }_s\end{array}\\ \begin{array}{cc}\cos {\psi }_s& -\sin {\psi }_s\end{array}\end{array}\right]\left[\begin{array}{c}\tan {\theta }_s\\ \tan {\gamma }_s\sec {\theta }_s\end{array}\right]$$

(25)

$$u_x\cos {\psi }_s+u_y\sin {\psi }_s=u_z\tan {\theta }_s$$

(26)

$$u_x\sin {\psi }_s-u_y\cos {\psi }_s=u_z\frac{\tan {\gamma }_s}{\cos {\theta }_s}$$

(27)

The pitch angle and roll angle can be expressed as follows:

$${\theta }_s=\arctan \frac{u_x\cos {\psi }_s+u_y\sin {\psi }_s}{u_z}$$

(28)

$${\gamma }_s=\arctan \left(\cos {\theta }_s\frac{u_x\sin {\psi }_s-u_y\cos {\psi }_s}{u_z}\right)$$

(29)

The pitch angle signal ${\theta }_s$ and the roll angle signal ${\gamma }_s$ are used to track the reference position. They are generated by the virtual control input of the outer ring part and passed to the attitude subsystem of the inner ring part. The error generated by the outer ring part is processed by the inner ring part eliminate. The yaw angle ${\psi }_s$ is used as a given control signal and is generated by a signal generator. The input value of the signal generator can be set to track any yaw angle. The actual position controller input design is $u_1$=$u_z/\cos \theta \cos \gamma$, and $u_1$ is the actual control input.

3.3. Design of Attitude Tracking Controller

The attitude control part is the inner loop subsystem, which realizes attitude control through the inner loop control law, while tracking the pitch angle signal quantity ${\theta }_s$ and roll angle signal quantity ${\gamma }_s$ generated by the outer loop control. In the control of the attitude subsystem, the main tracking quantity is ${\mathit{\eta }}_{\mathit{s}}$. According to the expression of the posture part equation derived above, the controller of the attitude part is mainly designed for the control quantity input torque τ. At the same time, the uncertainty in the quadrotor model and the external non-structural disturbance torque in the rotational motion are considered. Therefore, the expression of the attitude part of the quadrotor can be written as follows.

$${\mathit{J}}_{\mathit{s}}\ddot{\mathit{\eta }}=-{\mathit{C}}_{\mathit{s}}\dot{\mathit{\eta }}+\mathit{\tau }+{\mathit{d}}_M-{\mathit{J}}_{\mathit{\Delta }}\ddot{\mathit{\eta }}-{\mathit{C}}_{\mathit{\Delta }}\dot{\mathit{\eta }}$$

(30)

where J =${\mathit{J}}_{\mathit{s}}$+ ${\mathit{J}}_{\mathit{\Delta }}$, C =${\mathit{C}}_{\mathit{s}}$+${\mathit{C}}_{\mathit{\Delta }}$. The interference force ${\mathit{d}}_{\mathbf{1}}$ in the x direction can be expressed as an expression related to the interference torque. Further, consider that ${\mathit{d}}_{\mathbf{1}}$ is a bounded quantity.

So, the dynamic model of the attitude subsystem can be expressed as:

$${\mathit{J}}_{\mathit{s}}\ddot{\mathit{\eta }}+{\mathit{C}}_{\mathit{s}}\dot{\mathit{\eta }}=\mathit{\tau }+{\mathit{d}}_{\mathbf{1}}$$

(31)

The tracking error signal of the attitude subsystem is ${\mathit{e}}_{\mathit{\eta }}$=$\mathit{\eta }-{\mathit{\eta }}_{\mathit{s}}$, and the derivative is further expressed as $\dot{{\mathit{e}}_{\mathit{r}}}$=$\dot{{\mathit{\eta }}_{\mathit{s}}}-{\lambda }_2{\mathit{e}}_{\mathit{\eta }}$, ${\mathit{\eta }}_{\mathit{s}}$=${\left[\begin{array}{ccc}{\gamma }_s& {\theta }_s& {\psi }_s\end{array}\right]}^{\mathrm{T}}$.

Similar to the design idea of the position controller, first define the sliding mode function. The sliding mode function ${\mathit{S}}_{\mathbf{2}}$ is set to the following form:

$${\mathit{S}}_{\mathbf{2}}=\dot{\mathit{\eta }}-\dot{{\mathit{e}}_{\mathit{r}}}=\dot{{\mathit{e}}_{\mathit{\eta }}}+{\lambda }_2{\mathit{e}}_{\mathit{\eta }}$$

(32)

${\lambda }_2$ is the control parameter.${\lambda }_2$ > 0.

According to the defined sliding mode function relationship, the attitude error subsystem can be transformed into the following form:

$${\mathit{J}}_{\mathit{s}}\dot{{\mathit{S}}_{\mathbf{2}}}=\mathit{\tau }+{\mathit{d}}_{\mathbf{1}}-{\mathit{C}}_{\mathit{s}}\dot{\mathit{\eta }}-{\mathit{J}}_{\mathit{s}}\ddot{{\mathit{e}}_{\mathit{r}}}$$

(33)

Therefore, the control quantity input torque can be expressed as the following form:

$$\mathit{\tau } ={\mathit{J}}_{\mathit{s}}\ddot{{\mathit{e}}_{\mathit{r}}}+{\mathit{C}}_{\mathit{s}}\dot{\mathit{\eta }}-c_2{\mathit{S}}_{\mathbf{2}}-\epsilon sgn\left({\mathit{S}}_{\mathbf{2}}\right)$$

(34)

In Formula (34): ε and $c_2$ are control parameters, $\epsilon$>$d_{\gamma }$,$c_2$ > 0.$sgn$ is a symbolic function.

It can be seen from the expression of the exponential reaching law that when the exponential approaching speed is gradually reduced from a larger value to zero, the reaching time becomes shorter and the speed of the target point approaching the switching plane is very small. Because it is a process of gradual approximation, it is difficult to ensure that the moving point reaches the switching plane in a certain time, so an isokinetic approach term $-\epsilon sgn\left({\mathit{S}}_{\mathbf{2}}\right)$ is added to the above-mentioned basis [18,19].

The nominal values of the defined model are:

$${\mathit{J}}_{\mathit{S}}=\left[\begin{array}{ccc}{I}_x& 0& 0\\ 0& I_y+I_Z& 0\\ 0& 0& I_x+I_y+I_Z\end{array}\right]{\mathit{C}}_{\mathit{s}} =0.9{C}$$

(35)

Furthermore, the Lyapunov function is constructed as follows:

$$V_2 =\frac{ 1}{ 2}{\mathit{S}}_{\mathbf{2}}{}^{\mathrm{T}}{\mathit{J}}_{\mathit{S}}{\mathit{S}}_{\mathbf{2}}$$

(36)

The derivative function of $V_2$ can be obtained as follows:

$$\dot{V_2}={\mathit{S}}_{\mathbf{2}}{}^{\mathrm{T}}{\mathit{J}}_{\mathit{S}}\dot{{\mathit{S}}_{\mathbf{2}}}={\mathit{S}}_{\mathbf{2}}{}^{\mathrm{T}}(\mathit{\tau }+{\mathit{d}}_{\mathbf{1}}-{\mathit{C}}_{\mathit{s}}\dot{\mathit{\eta }}-{\mathit{J}}_{\mathit{s}}\ddot{{\mathit{e}}_{\mathit{r}}})$$

(37)

After the control quantity τ is brought in, the following results can be obtained:

$$\dot{V_2}=-2c_2{\mathit{J}}_{\mathit{S}}{}^{-1}{V}_2$$

(38)

It can be seen that Lyapunov derivative function $\dot{V_2}$ is a negative number, so the characteristic of Lyapunov function is monotonic decreasing. According to Lyapunov stability law, the attitude subsystem can be considered as stable, and the error is convergent.

For the whole system, the Lyapunov function is defined as:

$$V=V_1+V_2$$

(39)

Put $V_1$ and $V_2$ in it.

$$\dot{V}\text{≤}-c_1{\mathit{S}}_{\mathbf{1}}{}^{\mathrm{T}}{\mathit{S}}_{\mathbf{1}}-c_2{}^{’}{V}_2\le 0$$

(40)

According to the Lyapunov stability law, the whole system can be considered to be in a state of asymptotic stability.

The position controller obtains the pitch signal ${\theta }_s$ and roll signal ${\gamma }_s$. From the expression of input torque in formula (34), it can be seen that the first derivative and the second derivative of the two signals need to be calculated respectively. In this case, we can use the form of third-order differentiator to realize the control of ${\theta }_s$ and ${\gamma }_s$ first derivative and second derivative.

4. Simulation Research

Simulation

First of all, given the theoretical inspection trajectory parameter ${\mathit{\xi }}_{\mathit{c}}$ of the inspection drone, the theoretical trajectory is: x = cos(1/2t); y = sin(1/2t); z = t. Due to the under-driving characteristics of the inspection drone, the pitch angle and roll angle are transmitted to the inner loop control system as intermediate command signals, so only the yaw angle needs to be defined at this time. Set the constant of yaw angle to ${\psi }_s$= π/6. The distance from the end of the rotor to the center of gravity of the aircraft is 0.5 m. The parameters in the inertia matrix I are set as follows:$I_x$= 0.121; $I_y$= 0.121; $I_z$ = 0.151.

Interference force in displacement motion ${\mathit{d}}_{\mathit{s}}$ is:

$$\left[\begin{array}{c}\sin \left(\pi t\right)\\ \cos \left(\pi t\right)\\ \cos \left(\pi t\right)\end{array}\right]$$

Disturbing torque in rotating motion ${\mathit{d}}_{\mathit{M}}$ is taken as:

$$\left[\begin{array}{c}\sin \left(0.5\pi t\right)+0.5\\ \cos \left(0.5\pi t\right)+0.5\\ \sin \left(0.5\pi t\right)+0.6\end{array}\right]$$

Set the simulation time to 30 s and change once in 10 s. In 0–10 s, m is 1 kg; in 10–20 s, m is 3 kg; in 20–30 s, m is 5 kg. In the position tracking controller, $c_1$ = 6, ${\lambda }_1$= diag [2, 2, 2], ${\gamma }_1=0.80,{\gamma }_2=0.30$. In the attitude controller, $c_2$= 8, $\epsilon$ = 0.30, ${\lambda }_2$= diag [60, 60, 60]. In the switching control process, the saturation function is used instead of the switching function, and the boundary layer thickness is taken as 0.30.

The simulation results are shown in the figure.

It can be seen from Figure 5 that the inspection drone can track the inspection trajectory well. The blue is the theoretically designed inspection trajectory, and the red is the actual tracking trajectory under the disturbance of the disturbance. It can be seen from the figure The two trajectories are close to the same, which shows the effectiveness of the control algorithm proposed in this paper. Figure 6a–c present the tracking errors in the x, y, and z directions of the inspection drone, respectively. It can be seen that after the quality is changed, the error changes in different time periods are different, and the error is very small and within a reasonable range. Figure 7a–c are the trajectory tracking curves of the roll angle, pitch angle, and yaw angle of the inspection drone. It can be seen that the actual roll angle, pitch angle, and the yaw angle and the ideal roll angle, pitch angle, and yaw angle curve basically tend to be the same. Figure 8 shows the change in the lifting power of the inspection drone, i.e., the input quantity $u_1$. From the figure, it can be seen that the lift changes with the change of mass, and the value is close to the gravity of the inspection drone. Figure 9 shows the quality self-adaptive estimation result. The quality estimation value and the actual quality value curve are roughly the same, and the error is within the allowable range. Figure 10 shows a comparison diagram of the disturbance force design value and estimated value of the position movement part. It can be clearly seen from the figure that the disturbance force error in the x-direction and y-direction basically approaches zero, achieving the expected goal. Compared with the designed interference force, the z direction is significantly reduced, and the disturbance is suppressed. The effectiveness and robustness of this control algorithm can also be seen from the before and after changes of the interference force. In practical experiments, the gust of wind can be regarded as a disturbance, and the disturbance can be well suppressed by the adaptive law.

Through the above simulation analysis and research, it can be seen that the controller designed by the sliding mode adaptive robust control method can accurately and stably track the given patrol, taking into account the dynamic characteristics of the patrol drone, external disturbance, and system uncertainty. Check the trajectory, obtain good control performance, and verify the effectiveness of the control algorithm.

5. Experimental Design

5.1. Sliding Mode PID Control Group Experiment

The first group of control experiments uses the sliding mode PID control algorithm to design the inspection drone controller. The control structure diagram is shown in Figure 11.

The system is divided into two parts, one is a full drive subsystem, the other is the under drive subsystem. PID control is used in the full drive subsystem and sliding mode control is used in the underactuated subsystem. When other conditions remain unchanged, the mass is 1 kg for the control experiment. The simulation results are shown in the figure.

From the simulation results of the sliding mode PID control algorithm, it can be seen from Figure 12 that the overall trajectory tracking has a good effect. The specific position tracking amount is shown in Figure 13. It is not difficult to see from the figure that the error in the x direction is between −0.03 and 0.03; the error in the y direction is between −0.02 and 0.02; the error in the z direction is between −0.026 and 0.026. Although the error is oscillating, it is higher in terms of accuracy than the control algorithm proposed in this paper. Figure 14 presents the attitude tracking curves. It can be seen that the overall trajectory tracking accuracy has a good effect, which is equivalent to the control effect in this article.

5.2. Backstepping Control Group Experiment

The second group of control group experiments adopts backstepping control to design the controller. The control structure diagram is shown in Figure 15. The basic principle of backstepping is to decompose a complex nonlinear system into subsystems that do not exceed the order of the system, and then design partial Lyapunov functions and intermediate virtual control variables for each subsystem, and “backward” to the entire system. They are integrated to complete the design of the entire control law. Therefore, the overall system of the inspection UAV can also be divided into two parts, namely the position subsystem and the attitude subsystem. Firstly, the theoretical inspection trajectory of the inspection drone is given. At the same time, the height control uses the error amount between the actual height and the theoretical height as input, and the lift $u_1$ in the z direction is obtained through the calculation of the controller. Similarly, in the translation part, the theoretical values and actual errors in the x and y directions are used as input, and the lift $u_1$ in the z direction is brought into the translation controller to calculate the lift $u_x$ and $u_y$ in the two directions. After $u_x$ and $u_y$ undergo backstepping control operations, the theoretical roll angle and theoretical pitch angle of the system are obtained. The second is the design of the attitude subsystem, which is similar to the design of the position controller. The theoretical value of the three angles of the attitude part and the actual value measured by the attitude sensor are calculated to obtain the error, and the error is used as the input. It should be noted that the yaw angle is still set at this time. The three angle errors are input into the attitude controller for calculation, and the input torques $u_2$, $u_{\mathit{3}}$, and $u_4$ of the three angles are obtained. Finally, the input lift and torque obtained by the two subsystems are used to control the work of the inspection drone. The simulation results are shown in the figure.

It can be seen from Figure 16 that due to the addition of disturbance, the overall trajectory tracking effect is not ideal, and the disturbance is relatively serious. Looking at the error change curves in the three directions, the overall error curve is in an oscillating state, and the oscillating range is relatively large, which shows that the control algorithm is greatly affected in terms of accuracy. It can be seen from Figure 17 and Figure 18 that the tracking effect of the three angles is affected by the disturbance and the angle tracking cannot be performed well.

6. Experimental Study

In order to better verify the effectiveness of the proposed control algorithm, this paper adds an actual experimental platform for verification. The hardware system part of the experimental platform is the PIX series flight controller with the model of the main control chip STM32F427. The MPU6050 sensor is selected as the attitude acquisition component. The GPS module with the model number M8N and the MS5611-01BA air pressure sensor are selected for the measurement of position parameters. The experiment mainly uses wind as interference to measure the flight status and trajectory tracking of the UAV to verify the superiority of the control strategy proposed in this paper. The patrol track of the experimental design is shown in Figure 19.

According to Figure 20, it can be seen that after designing the predetermined trajectory, the inspection drone can roughly track the predetermined trajectory, and although there is an error in the tracking result, considering the influence of wind gusts and disturbances during the experiment, the tracking result is also within the controllable range. The experimental data from Figure 21, Figure 22, Figure 23, Figure 24, Figure 25 and Figure 26 can clearly show the tracking results of each index, and the actual value curve and the theoretical value basically tend to be the same, which shows the effectiveness of the control algorithm, and the error range of each index is also within the plan.

In order to better compare the three control algorithms during the experiments, different algorithms are used for the control experiments. The parameters of the experimental equipment are kept constant and only the control algorithms are changed. The experimental results of the three control algorithms are shown in

Table 1. Control algorithm error tracking result analysis table.

Control Method	x-Direction Tracking Error Range	y-Direction Tracking Error Range	z-Direction Tracking Error Range
Sliding Mode Adaptive Robustness	−0.21–0.16	−0.31–0.58	−0.63–1.21
Sliding Mode PID	−0.23–0.36	−0.86–0.79	−0.59–1.32
backstepping control	−0.56–1.21	−1.52–1.63	−1.63–1.82

.

Combining the experimental data in Table 1 and

Table 2. Attitude trajectory tracking error.

Control Method	Roll Angle	Pitch Angle	Yaw Angle
Sliding Mode Adaptive Robustness	−0.5–0.75	−0.04–0.58	−0.21–0.36
Sliding Mode PID	−0.86–0.95	−0.5–0.36	−0.31–0.48
backstepping control	−0.98–1.025	−0.93–0.86	−1.26–1.10

, it can be seen that the sliding mode adaptive robust control algorithm is faster in terms of stabilization time than the sliding mode PID control algorithm and the backstepping control algorithm. Although the two algorithms used as the control group can finally track the motion trajectory of the quadrotor aircraft, in terms of tracking error, the tracking error range of the two algorithms is larger than that of the sliding mode adaptive robust control algorithm. Although the sliding mode PID algorithm can achieve good results in respect of stability time and tracking accuracy, it is very cumbersome to adjust the PID parameters, and the overshoot phenomenon is serious when the parameters are not selected properly. It can be concluded that the sliding mode adaptive robust control algorithm has good stability, higher precision, and obvious advantages by comparing the experimental results of the other two control algorithms under the condition of considering the disturbance.

7. Discussion

Although the simulation results can basically achieve the expected goal, the error in the z-direction is significantly different compared to the error variation range in the x and y directions. Moreover, the final obtained disturbance moment in z-direction is also significantly different compared to the other two directions, which shows that the accuracy and the suppression of disturbance in z-direction still need to be improved. Further analysis of the causes of this situation may be as follows.

• The inspection drone is an underdriven system where the control volume is less than the output volume.
• Jitter vibration will be generated near the transition to the slide surface during the control process.
• The actual altitude always has a constant small deviation from the ideal altitude after the trajectory tracking process is stabilized.
• The inner loop of attitude control and the outer loop of position control cannot be coordinated immediately during the continuous steering process.

8. Conclusions

The results show that the sliding mode adaptive robust control strategy used in this paper can support the inspection UAV to track the inspection trajectory well. In the presence of interference, the stability and accuracy of the inspection UAV are guaranteed. Simulation results and experimental results prove the effectiveness of the control method. The simulation experiments of the control group prove the advantages of the control strategy in this study. At the same time, in the process of comparison with the sliding mode PID control and the backstepping control, it can be clearly seen that the sliding mode PID can achieve reasonable results in terms of control accuracy, and the backstepping control, although designed with system stability in mind, struggles to achieve the expected results in terms of control accuracy. In the control process of the actual experiment, from the data measured by the sensor and the theoretical data curve calculated on the ground station, it can be seen that the control algorithm used can reach the actual demand, and the inspection drone can follow the control point well for inspection. In the future, we will continue to combine the algorithm from the anti-interference and the inspection drone in order to better apply to the actual project.