Four-rotor unmanned aerial vehicle suspension load trajectory tracking control method based on disturbance observer

Abstract

The invention relates to the field of automatic pose control of a four-rotor unmanned aerial vehicle suspension load system, and discloses a four-rotor unmanned aerial vehicle suspension load trajectory tracking control method based on a disturbance observer. Establishing a kinetic model of the four-rotor unmanned aerial vehicle suspension load system by using a Newton-Euler method; load swing acting force and external disturbance are regarded as composite disturbance, and a disturbance observer is designed to accurately estimate and compensate system disturbance; a fixed time sliding mode controller is designed for the position subsystem, and position tracking control of the quad-rotor unmanned aerial vehicle is completed; expected tracking values of a roll angle and a pitch angle are calculated according to an inverse solution formula, a fixed-time nonsingular terminal sliding mode controller is designed for the attitude subsystem, attitude tracking control of the quad-rotor unmanned aerial vehicle is completed, and load swing can be effectively inhibited while the system rapidly and stably tracks an expected trajectory.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of pose automatic control of four-rotor unmanned aerial vehicle suspension load system, and particularly relates to a four-rotor unmanned aerial vehicle suspension load trajectory tracking control method based on a disturbance observer. BACKGROUND

[0002] In recent years, four-rotor unmanned aerial vehicles have been widely used in agricultural monitoring, express delivery and safety inspection due to their low cost, high mobility and rapid vertical take-off and landing. In the air transportation task of four-rotor unmanned aerial vehicles, the cable suspension load method is often used to reduce the inertial impact and attitude interference caused by the movement of the load on the body. However, the four-rotor unmanned aerial vehicle suspension load system has the characteristics of strong coupling, nonlinearity and under-actuation. During the suspension transportation process, the inertia of the load will inevitably cause swinging, which will affect the flight stability and safety of the unmanned aerial vehicle. Therefore, the current research focuses on designing a high-performance control strategy to effectively suppress the load swing and improve the trajectory tracking performance and overall flight stability of the system.

[0003] Currently, the control problem of four-rotor suspension load system mainly includes two types: (1) The system is decoupled into position, attitude and swing angle subsystems, and controllers are designed to achieve trajectory tracking and load swing suppression. However, the swing angle variable of the load can only be indirectly controlled through the position control signal, which highly depends on the performance of the position controller, causing control loop delay and conflict with position control. (2) The suspension load is regarded as external disturbance of the unmanned aerial vehicle, and a robust controller is designed to suppress the disturbance and ensure the trajectory tracking performance of the system. This method has simple control structure, easy to debug and implement.

[0004] The fixed-time control method sets the time parameter as a fixed constant, so that the controller outputs the control signal within a fixed time. This control strategy ensures that the upper bound of the convergence time is not affected by the initial state of the system, which is suitable for situations with high stability requirements and improves the shortcomings of finite-time control. SUMMARY

[0005] In view of the improvement needs of the prior art, the present application provides a four-rotor unmanned aerial vehicle suspension load trajectory tracking control method based on a disturbance observer, which can quickly and accurately estimate the system disturbance, realize the rapid and stable tracking of the expected trajectory, and effectively suppress the load swing.

[0006] The technical scheme adopted by the present application is as follows, that is, a four-rotor unmanned aerial vehicle suspension load trajectory tracking control method based on a disturbance observer, comprising the following steps:

[0007] Step 1: Newton-Euler method is used to establish the dynamic model of the four-rotor unmanned aerial vehicle suspension load system;

[0008] Step two: The load swing force and external disturbance are regarded as a compound disturbance, and a disturbance observer is designed to accurately estimate the system disturbance.

[0009] Step three: A fixed-time sliding mode controller is designed for the position subsystem to complete the position tracking control of the quadrotor UAV.

[0010] Step four: According to the inverse formula, the expected tracking values of roll angle and pitch angle are calculated, and a fixed-time nonsingular terminal sliding mode controller is designed for the attitude subsystem to complete the attitude tracking control of the quadrotor UAV.

[0011] In step one, the quadrotor UAV load suspension system is composed of an "x" shaped quadrotor UAV and a load connected to the UAV by a rope. In flight, the rope will not significantly deform due to wind or other external factors. To describe the motion state of the system, an inertial coordinate system and a quadrotor body coordinate system are established. The position of the quadrotor in the inertial system is represented by , and the attitude is represented by . The Newton-Euler method is used to establish the dynamic model of the quadrotor UAV load suspension system:

[0012]

[0013] where represents the mass of the quadrotor, and represents the mass of the load, represents the gravitational acceleration, represents the rotational inertia of each axis of the quadrotor, and represent the air resistance coefficients. and represent the control inputs of the position loop and the attitude loop, respectively, where the total lift generated by the quadrotor propeller is derived through coordinate transformation and is represented as:

[0014]

[0015] The load position in the inertial coordinate system is defined as , and the swing angle of the suspended load is represented by , where the swing angle is the angle between the hanging rope and the plane of the UAV, the swing angle is the angle between the hanging rope and the plane of the UAV. The load position is determined by the position of the quadrotor and the swing angle of the load:

[0016] ​​

[0017] where l represents the length of the rope. The coupling motion between the quadrotor and the payload is analyzed by using the Lagrange equation. The Lagrange function of the payload is:

[0018]

[0019] where, is the total kinetic energy of the system, is the total potential energy of the system, and q is the generalized coordinate of the system.

[0020] The total kinetic energy T of the quadrotor unmanned aerial vehicle hanging payload system is:

[0021]

[0022] The total potential energy V of the system is:

[0023]

[0024] The Lagrange equation is applied:

[0025]

[0026] Let and be substituted into the equation, and the dynamics model of the swing angle , is obtained:

[0027]

[0028] According to the positional relationship between the unmanned aerial vehicle and the payload, the second-order derivative of the position of the unmanned aerial vehicle is obtained, and the relationship between the acceleration of the payload and the acceleration of the unmanned aerial vehicle is obtained. Finally, the coupling dynamics model of the quadrotor unmanned aerial vehicle hanging payload system is obtained, and the disturbance term is added to the model, and the following equation is obtained:

[0029]

[0030] where,

[0031] In step two, the swing force of the payload in the system and the unknown external disturbance are taken as the compound disturbance of the position subsystem . In order to design the observer, the quadrotor unmanned aerial vehicle hanging payload model can be simplified as:

[0032]

[0033] where, represents the total mass of the system, .

[0034] To design the observer, define the variables:

[0035]

[0036] where the auxiliary variables are chosen as:

[0037]

[0038] where are all positive constants, , .

[0039]

[0040] To make converge in fixed time, the disturbance estimation is designed as:

[0041]

[0042] The observation error is .

[0043] Similarly, the fixed-time disturbance observer for the attitude subsystem is designed as:

[0044]

[0045] where

[0046]

[0047] The observation error is .

[0048] In step three, the desired position input in three directions is , is the actual position in three directions, define the position tracking error vector as , the derivative of the position tracking error vector is , , , , the sliding variable , the fixed-time nonsingular terminal sliding surface for the position subsystem is designed as:

[0049]

[0050] where , , , , . The definitions are as follows:

[0051]

[0052] wherein, , , is a small positive constant.

[0053] The double power law is chosen as follows:

[0054]

[0055] wherein, , , , .

[0056] The control variable of the position subsystem is designed as:

[0057]

[0058] In the fourth step, the desired attitude input of the attitude controller is wherein Given the total lift of the system , the expected tracking values of the pitch angle and roll angle are calculated as:

[0059]

[0060] The actual attitude output is defined as , and the attitude tracking error vector is defined as , , , The derivative of the attitude tracking error vector is , and the sliding variable . A fixed-time nonsingular terminal sliding mode surface is designed for the attitude subsystem:

[0061]

[0062] In the formula, , , , , , The definitions are as follows:

[0063]

[0064] wherein, , , is an arbitrarily small positive constant.

[0065] The following double-power approaching law is selected:

[0066]

[0067] wherein, , , , .

[0068] The control quantity of the attitude subsystem is designed as:

[0069]

[0070] Compared with the prior art, the present application has the following advantages:

[0071] (1) The fixed-time disturbance observer is designed, which can accurately estimate and compensate the compound disturbance of the system composed of the load swing force and external disturbance in a fixed time, thereby improving the response speed and anti-interference ability of the system, and solving the problem of slow convergence of the traditional disturbance observer.

[0072] (2) The system stable time is not affected by the initial state and is independent of the system limit. The problem that the convergence time of the traditional control system is usually dependent on the initial state and the longer the convergence time, the slower the system response may be caused is solved, and it is ensured that the unmanned aerial vehicle can converge quickly in a fixed time, which is more suitable for occasions with high stability requirements.

[0073] (3) The fixed-time non-singular terminal sliding mode controller is designed. This controller avoids the singularity problem of the traditional sliding mode through the sliding surface with a switching term, and reduces the chattering. The problem that the traditional control algorithm is difficult to guarantee high-precision trajectory tracking when facing complex environment and unknown external disturbance is solved, and the flight stability of the quad-rotor unmanned aerial vehicle suspended load is improved. BRIEF DESCRIPTION OF DRAWINGS

[0074] Figure 1 It is a control block diagram of the quad-rotor suspended load system.

[0075] Figure 2 It is a structural schematic diagram of the quad-rotor suspended load system.

[0076] Figure 3 It is a three-dimensional trajectory tracking diagram of the quad-rotor unmanned aerial vehicle.

[0077] Figure 4 It is a position tracking curve diagram of the quad-rotor unmanned aerial vehicle.

[0078] Figure 5 It is an attitude tracking curve diagram of the quad-rotor unmanned aerial vehicle.

[0079] Figure 6 It is a load swing angle curve diagram. Detailed Implementation

[0080] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Various substitutions and modifications made based on ordinary technical knowledge and common practices in the art without departing from the above-described technical concept of the present invention should be included within the protection scope of the present invention.

[0081] This invention provides a method for tracking and controlling the suspended load trajectory of a quadrotor unmanned aerial vehicle based on a disturbance observer, such as... Figure 1 As shown, it includes the following steps:

[0082] Step 1: Establish a dynamic model of the load suspension system of the quadrotor UAV using the Newton-Euler method and the Lagrange equation method;

[0083] like Figure 2 As shown, the quadcopter drone's load-bearing system consists of an "X"-shaped quadcopter drone and a load connected to the drone by a rope. During flight, the rope will not undergo significant deformation due to wind or other external factors. To describe the system's motion, an inertial coordinate system is established. and quadcopter body coordinate system The position of the quadrotor in the inertial frame is determined by... This indicates that its posture is... express.

[0084] The load position is defined in the inertial coordinate system as The swing angle of the suspended load is determined by It means that among them The swing angle is for suspending ropes and drones Angle between planes The swing angle is for suspending ropes and drones The angle between the planes. The load position is determined by the quadcopter position and the load swing angle:

[0085]

[0086] Where l represents the rope length. A dynamic model of the suspended load and swing angle of the quadrotor UAV is established using the Newton-Euler method and the Lagrange equations. , The dynamic model:

[0087]

[0088] In the formula,

[0089]

[0090] where, denotes the mass of the quadrotor, denotes the mass of the payload, denotes the gravitational acceleration, denotes the moment of inertia of the quadrotor about each axis, and denotes the air drag coefficient. and denote the control inputs of the position loop and the attitude loop, respectively.

[0091] Step 2: The payload swing force and external disturbances are considered as a composite disturbance, and a disturbance observer is designed to accurately estimate the system disturbance.

[0092] The quadrotor UAV suspended payload model can be simplified as:

[0093]

[0094] where, denotes the total mass of the system, .

[0095] Disturbance estimation is designed as:

[0096]

[0097] where, ,

[0098]

[0099] where, are all normal numbers, , .

[0100] Disturbance estimation is designed as:

[0101]

[0102] where,

[0103]

[0104] Step 3: Design a fixed-time sliding mode controller for the position subsystem to complete the position tracking control of the quadrotor UAV;

[0105] The expected position input in three directions is , is the actual position in three directions, and the position tracking error vector is defined as , and the derivative of the position tracking error vector is , , , , sliding variable The fixed-time nonsingular terminal sliding surface is designed as follows:

[0106]

[0107] where , , , , . The definitions are as follows:

[0108]

[0109] where , , is a small positive constant.

[0110] The double power reaching law is chosen as follows:

[0111]

[0112] where , , , .

[0113] The control variable of the position subsystem is obtained as

[0114]

[0115] Step 4: According to the inverse formula, the expected tracking values of the pitch angle and roll angle are calculated, a fixed-time nonsingular terminal sliding mode controller is designed for the attitude subsystem, and the attitude tracking control of the quadrotor UAV is completed.

[0116] The expected attitude input of the attitude controller is where is given, and the total lift of the system is The expected tracking values of the pitch angle and roll angle are calculated as

[0117]

[0118] The actual attitude output is defined as The attitude tracking error vector is defined as , , , The derivative of the attitude tracking error vector is , sliding variable The control variables for the attitude subsystem are designed as follows:

[0119]

[0120] in, , , , , , , The definition is as follows:

[0121]

[0122] in, , , It is an arbitrarily small positive number.

[0123] To verify the effectiveness and impact of the present invention, the following simulation experiments and data analysis were conducted:

[0124] This embodiment uses the Matlab / Simulink platform for simulation verification. The model parameters of the quadcopter UAV's suspended load system are as follows:

[0125]

[0126] Set the initial simulation state of the system as follows: , The desired simulation trajectory is set as follows: Desired yaw angle External disturbances are set as , The simulation time was 25 seconds. The corresponding simulation results are shown below.

[0127] Figure 3 This is the 3D trajectory tracking effect of the unmanned aerial vehicle system. Figure 4 and Figure 5 These are the drone's position tracking curve and attitude angle tracking curve, respectively. As can be seen from the figures, the controller designed in this invention can achieve rapid tracking of the desired trajectory in a short time, and the attitude angle changes are relatively smooth with minimal fluctuations. Figure 6 As shown in the load swing angle curve, it can be seen that the quadcopter UAV has a small change in attitude angle during movement, which reduces the load swing amplitude and has the effect of suppressing load swing.

Claims

1. A method for tracking and controlling the suspended load trajectory of a quadrotor unmanned aerial vehicle based on a disturbance observer, characterized in that, Includes the following steps: S1. A dynamic model of the load suspension system of a quadcopter UAV is established using the Newton-Euler method. S2. Treat the load swing force and external disturbance as a composite disturbance, and design a disturbance observer to accurately estimate the system disturbance. S3 is designed as a fixed-time sliding mode controller for the position subsystem to complete the position tracking control of the quadcopter UAV. S4. Calculate the desired tracking values ​​of roll angle and pitch angle based on the inverse kinematics formula, design a fixed-time non-singular terminal sliding mode controller for the attitude subsystem, and complete the attitude tracking control of the quadcopter UAV.

2. The quadrotor UAV suspended load trajectory tracking control method based on a disturbance observer according to claim 1, characterized in that, In step S1, the quadcopter drone load suspension system consists of an "X"-shaped quadcopter drone and a load connected to the drone by a rope. During flight, the rope will not undergo significant deformation due to wind or other external factors. To describe the system's motion, an inertial coordinate system is established. and quadcopter body coordinate system The position of the quadrotor in the inertial frame is determined by... This indicates that its posture is... The dynamic model of the suspended load system of the quadrotor UAV is established using the Newton-Euler method: in, Indicates the mass of the quadcopter, indicating The quality of the load, Represents gravitational acceleration. These represent the moments of inertia of each axis of the quadcopter. and This represents the air drag coefficient. and These represent the control inputs for the position loop and attitude loop, respectively.

3. The quadrotor UAV suspended load trajectory tracking control method based on a disturbance observer according to claim 1, characterized in that, The load position is defined in the inertial coordinate system as The swing angle of the suspended load is determined by It means that among them The swing angle is for suspending ropes and drones Angle between planes The swing angle is for suspending ropes and drones The angle between the planes. The load position is determined by the quadcopter position and the load swing angle: Where l represents the rope length. The coupled motion between the quadrotor and the load is analyzed using the Lagrange equations. The Lagrange function of the load is: in, The total kinetic energy of the system, Let q be the total potential energy of the system, and q be the generalized coordinate of the system. The total kinetic energy T of the quadcopter drone's suspended load system is: The total potential energy V of the system is: Applying the Lagrange equation: make and Substituting the values ​​into the equations, we obtain the swing angle. , The dynamic model:

4. The quadrotor UAV suspended load trajectory tracking control method based on a disturbance observer according to claim 3, characterized in that, Based on the positional relationship between the UAV and the load, the second derivative with respect to the UAV's position is taken to obtain the relationship between the load's acceleration and the UAV's acceleration. This leads to the coupled dynamics model of the quadcopter UAV's suspended load system. Adding disturbance terms to the model yields: In the formula, 5. The method for tracking and controlling the suspended load trajectory of a quadrotor UAV based on a disturbance observer according to claim 1, characterized in that, In step S2, the load swing force and the unknown external disturbance in the system are treated as a combined disturbance of the position subsystem. To design the observer, the suspended load model of the quadcopter UAV can be simplified as follows: In the formula, Indicates the total mass of the system. . To design the observer, define the following variables: In the formula, auxiliary variables The expression to be selected is: in, All are positive numbers. , . In order to It can converge within a fixed time, and the perturbation estimation Designed as follows: The observation error is . Similarly, the fixed-time perturbation observer for the attitude subsystem is designed as follows: In the formula, The observation error is .

6. The quadrotor UAV suspended load trajectory tracking control method based on a disturbance observer according to claim 1, characterized in that, In step S3, the desired positions in the three directions are input as follows: , These are the actual positions in three directions, and the position tracking error vector is defined as follows: The derivative of the position tracking error vector is , , , Sliding mode variables The following fixed-time nonsingular terminal sliding surface is designed for the position subsystem: In the formula, , , , , , The definition is as follows: in, , , It is a small positive number. Choose the following double-power-degree approach law: in, , , , . Based on the above formula, the control quantity of the position subsystem is designed as follows:

7. The quadrotor UAV suspended load trajectory tracking control method based on a disturbance observer according to claim 1, characterized in that, In step S4, the desired attitude input for the attitude controller is: ,in Given the total lift of the system The formulas for calculating the expected tracking values ​​of pitch and roll angles are as follows: Define the actual attitude output as Define the attitude tracking error vector as , , , The derivative of the attitude tracking error vector is Sliding mode variables . Design a fixed-time nonsingular terminal sliding surface for the attitude subsystem: In the formula, , , , , , The definition is as follows: in, , , It is an arbitrarily small positive number. Choose the following double-power-degree approach law: in, , , , . Based on the above equation, the control variables of the attitude subsystem are designed as follows:

Images