Hanging flight anti-swing control method for quad-rotor unmanned aerial vehicle

Abstract

The invention discloses a suspension flight anti-swing control method for a quad-rotor unmanned aerial vehicle. Comprising the following steps: acquiring an expected position, a yaw angle, an actual position and an attitude angle of the quad-rotor unmanned aerial vehicle in real time, and acquiring a load swing angle in real time; compensating the expected position according to the load swing angle to obtain a compensated expected position; position control is carried out according to the actual position and the compensated expected position of the quadrotor unmanned aerial vehicle, a virtual control quantity is obtained, and the vertical virtual control quantity is expected vertical pulling force; performing attitude calculation according to the virtual control quantity to obtain an expected roll angle and a pitch angle; attitude control is carried out according to the expected attitude angle and the actual attitude angle, expected torque is obtained, then expected rotating speeds of the four rotor motors are obtained, and the four-rotor unmanned aerial vehicle is controlled according to the expected rotating speeds. The damping controller provided by the invention considers the coupling characteristic between the four rotors and the hanging load, so that the hanging system can effectively inhibit the load swing angle while accurately following the expected path.

Description

Technical Field

[0001] This invention belongs to the field of aircraft automatic control technology, specifically a method for reducing sway control during suspended flight of a quadcopter unmanned aerial vehicle. Background Technology

[0002] Quadcopter drones, with their vertical takeoff and landing and hovering capabilities, are widely used for sling-mounted payload transport and delivery: including emergency supplies replenishment in mountainous and island areas, rapid deployment of medicines and life-saving equipment in disaster relief, power line inspection and overhead line crossing rope dropping, delivery and replacement of high-altitude components such as bridges, wind turbines, and high-rise buildings, deployment of fire extinguishing bombs and demolition tools in firefighting scenarios, deployment of special equipment in police and security sites, dropping of lifebuoys and mooring ropes in maritime operations, delivery of small tools and sample retrieval in agricultural scenarios, and deployment and retrieval of sensors in scientific research and surveying.

[0003] The introduction of a load adds two degrees of freedom to the quadcopter's sling-borne flight system, creating a more strongly underactuated coupled system between the airframe and the load. Since attitude and position control can only indirectly affect the load's state, the system's control difficulty increases significantly. In this situation, external wind disturbances exacerbate the load's sway, and if not suppressed in time, it can lead to flight instability or even a crash. Therefore, load sway reduction control is crucial for stable quadcopter flight during sling-borne flight. Summary of the Invention

[0004] To address the aforementioned technical problems, this invention provides a method for controlling the sway reduction during suspended flight of a quadcopter UAV, thereby suppressing load sway while effectively tracking a preset trajectory.

[0005] To achieve the above objectives, the technical solution of the present invention is as follows:

[0006] I. A method for reducing sway control during suspended flight of a quadcopter UAV

[0007] The control method includes the following steps:

[0008] Step S1: Obtain the desired position, desired yaw angle, actual position and actual attitude angle of the quadcopter UAV at the current moment, and collect the load swing angle at the current moment; the attitude angle includes roll angle, pitch angle and yaw angle.

[0009] Specifically, in step S1, the load swing angle includes the swing angle of the load in the YOZ plane and the XOZ plane in the machine coordinate system.

[0010] Step S2: Compensate the quadcopter drone's desired position based on the load swing angle at the current moment to obtain the compensated desired position.

[0011] Specifically, step S2 includes:

[0012] First, based on the load swing angle, the acceleration compensation amount is obtained using the following formula:

[0013]

[0014] In the formula, , , These represent the acceleration compensation amounts for the quadcopter drone along the X, Y, and Z axes, respectively. , , All are non-negative real numbers; and These represent the load swing angles in the YOZ and XOZ planes, respectively; the superscript "·" indicates the first derivative.

[0015] Subsequently, the second integral result of the acceleration compensation is obtained and used as the position compensation;

[0016] Finally, the sum of the position compensation amount and the desired position is calculated to obtain the compensated desired position.

[0017] Step S3: Perform position control based on the actual position and the desired position after compensation of the quadcopter drone to obtain the virtual control quantity of the quadcopter drone, and use the virtual control quantity in the vertical direction as the desired vertical pull force.

[0018] Specifically, in step S3, position control is performed according to the following formula:

[0019]

[0020]

[0021]

[0022] In the formula, u x u y u z These represent the virtual control variables of a quadcopter UAV on the X, Y, and Z axes, respectively; m u For the mass of the quadcopter, m L σi represents the load mass; l represents the length of the suspension rope; sig is a sign power function, || represents the absolute value, the superscript "·" represents the first derivative, and the superscript "··" represents the second derivative; x ud y ud z ud These represent the compensated desired positions of the quadcopter UAV along the X, Y, and Z axes, respectively; a x a y a zThese are the first adjustable parameters corresponding to the X, Y, and Z axes in the position controller; b x b y b z These are the second adjustable parameters corresponding to the X, Y, and Z axes in the position controller; e x e y e z These represent the positional errors of the quadcopter drone along the X, Y, and Z axes, respectively; h 1x h 1y h 1z These are the third adjustable parameters corresponding to the X, Y, and Z axes in the position controller; h 2x h 2y h 2z b1, b2, and b3 are the fourth adjustable parameters corresponding to the X, Y, and Z axes in the position controller, respectively; b1, b2, and b3 are the second intermediate variables corresponding to the X, Y, and Z axes in the position controller, respectively; σ x σ y σ z These are position loop-assisted adaptive single-gain non-singular fast termination controllers for the X, Y, and Z axes, respectively; f dx f dy f dz These are the interferences along the X, Y, and Z axes, respectively.

[0023] Specifically, the second intermediate variables b1, b2, and b3 corresponding to the X-axis, Y-axis, and Z-axis in the position controller are set according to the following formulas:

[0024]

[0025] In the formula, b1, b2, and b3 are the second intermediate variables corresponding to the X-axis, Y-axis, and Z-axis in the position controller, respectively; and These represent the load swing angles in the YOZ and XOZ planes, respectively. , .

[0026] Step S4: Perform attitude calculation based on the virtual control variables of the quadcopter UAV to obtain the desired roll angle and desired pitch angle.

[0027] Step S5: Perform attitude control based on the desired attitude angle and the actual attitude angle of the quadcopter UAV to obtain the desired torque of the quadcopter UAV. The desired torque includes the roll torque about the X-axis, the pitch torque about the Y-axis, and the yaw torque about the Z-axis.

[0028] Specifically, in step S5, attitude control is performed according to the following formula:

[0029]

[0030]

[0031]

[0032] In the formula, τ φ τ θ τ ψ These represent the desired roll moment about the X-axis, the desired pitch moment about the Y-axis, and the desired yaw moment about the Z-axis for the quadcopter; J xx J yy J zz σ represents the moment of inertia of the quadrotor in the X, Y, and Z directions, respectively; φ σ θ σ ψ These are attitude loop-assisted adaptive single-gain nonsingular fast termination controllers corresponding to the X, Y, and Z axes, respectively; z 2φ z 2θ z 2ψ These represent the uncertainties in the compensation model corresponding to the X, Y, and Z axes, respectively; φ d θ d ψ d These are the desired roll angle, desired pitch angle, and desired yaw angle, respectively; a φ a θ a ψ These are the first adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle, respectively; b φ b θ b ψ These are the second adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle; h 1φ h 1θ h 1ψ These are the third adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle; h 2φ h 2θ h 2ψ These are the fourth adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle; e φ e θ e ψ These represent roll angle error, pitch angle error, and yaw angle error, respectively; sig indicates a sign power function, || indicates the absolute value, the superscript "·" indicates the first derivative, and the superscript "··" indicates the second derivative.

[0033] Step S6: Based on the desired vertical pull and desired torque of the quadcopter drone, obtain the desired rotational speed of the four rotor motors, and control the position and attitude of the quadcopter drone based on the desired rotational speed of the four rotor motors.

[0034] II. A Quadrotor UAV Sling-on Flight Swing Reduction Control System

[0035] The control system includes:

[0036] The data acquisition unit is used to acquire the desired position, desired yaw angle, actual position and actual attitude angle of the quadcopter UAV in real time, and to acquire the load swing angle in real time.

[0037] The anti-swing controller is used to compensate for the desired position of the quadcopter drone based on the load swing angle, and obtain the compensated desired position.

[0038] The position controller is used to perform position control based on the actual position and the compensated desired position of the quadcopter drone, thereby obtaining the virtual control quantity of the quadcopter drone.

[0039] The attitude calculation unit is used to calculate the attitude based on the virtual control variables of the quadcopter UAV, and obtain the desired roll angle and desired pitch angle.

[0040] An attitude controller is used to control the attitude of a quadcopter drone based on its desired attitude angle and actual attitude angle, thereby obtaining the desired torque of the quadcopter drone.

[0041] The control and distribution unit obtains the desired rotational speeds of the four rotor motors based on the desired vertical pull and desired torque of the quadcopter drone, and sends control commands to the quadcopter drone based on the desired rotational speeds of the four rotor motors.

[0042] The beneficial effects of this invention are:

[0043] 1. The single-gain sliding mode controller designed in this invention simplifies the adjustment process.

[0044] 2. The adaptive robust controller designed in this invention ensures the smoothness of the control input and reduces abrupt changes.

[0045] 3. This invention uses a sway reduction controller designed by Lyapunov to address the problem that the load angle cannot be directly controlled, effectively suppressing the sway angle and ensuring the flight safety of the quadcopter.

[0046] 4. The present invention takes into account the coupling characteristics between the quadcopter and the suspended load in the design of the anti-sway controller, so that the suspension system can accurately follow the expected path while effectively suppressing the load sway angle. Attached Figure Description

[0047] Figure 1 This is a schematic diagram of the quadcopter suspension system of the present invention;

[0048] Figure 2 This is a control architecture diagram of the present invention. Detailed Implementation

[0049] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0050] This invention provides a method for reducing sway control during suspended flight of a quadcopter unmanned aerial vehicle (UAV), comprising the following steps:

[0051] Step S1: Obtain the desired position, desired yaw angle, actual position, and actual attitude angle of the quadcopter drone at the current moment, and use an angle sensor to collect the load swing angle at the current moment in real time.

[0052] The attitude angles include roll angle, pitch angle, and yaw angle.

[0053] The load swing angle includes the swing angle of the load in the YOZ plane and the XOZ plane in the body coordinate system.

[0054] Step S2: Compensate the quadcopter drone's desired position based on the load swing angle at the current moment to obtain the compensated desired position.

[0055] Specifically, the following steps are included:

[0056] Based on the load swing angle, the acceleration compensation amount can be obtained using the following formula:

[0057]

[0058] In the formula, , , These represent the acceleration compensation amounts for the quadcopter drone along the X, Y, and Z axes, respectively. , , All are non-negative real numbers (unit: m / s); and These represent the load swing angles in the YOZ and XOZ planes, respectively; the superscript "·" indicates the first derivative.

[0059] Based on the acceleration compensation amount, the second integral result is calculated using the following formula, and the second integral result of the acceleration compensation amount is used as the position compensation amount:

[0060]

[0061] In the formula, x xa y xa z xa These represent the position compensation amounts of the quadcopter drone on the X, Y, and Z axes, respectively.

[0062] The sum of the position compensation amount and the desired position is calculated using the following formula to obtain the compensated desired position:

[0063]

[0064] In the formula, x ud y ud z ud These represent the compensated desired positions of the quadcopter drone on the X, Y, and Z axes, respectively.

[0065] Step S3: Perform position control based on the actual position and the desired position after compensation of the quadcopter drone to obtain the virtual control quantities of the quadcopter drone in the vertical and horizontal directions, and use the virtual control quantity in the vertical direction as the desired vertical pull.

[0066] In step S3, position control is performed according to the following formula:

[0067]

[0068]

[0069]

[0070] In the formula, u x u y u z These represent the virtual control variables of a quadcopter UAV on the X, Y, and Z axes, respectively; m u For the mass of the quadcopter, m L Let be the load mass; l be the length of the suspension rope; sig be a sign power function, sig α (x)=|x| α sign(x), || denotes the absolute value, the superscript "·" indicates the first derivative, and the superscript "··" indicates the second derivative; x ud y ud z ud These represent the compensated desired positions of the quadcopter UAV along the X, Y, and Z axes, respectively; a x a y a z These are the first adjustable parameters corresponding to the X, Y, and Z axes in the position controller; b x b y b z These are the second adjustable parameters corresponding to the X, Y, and Z axes in the position controller; e x e y e z These represent the positional errors of the quadcopter drone along the X, Y, and Z axes, respectively; h 1x h 1y h 1z These are the third adjustable parameters corresponding to the X, Y, and Z axes in the position controller; h 2xh 2y h 2z b1, b2, and b3 are the fourth adjustable parameters corresponding to the X, Y, and Z axes in the position controller, respectively; b1, b2, and b3 are the second intermediate variables corresponding to the X, Y, and Z axes in the position controller, respectively; σ x σ y σ z These are position loop-assisted adaptive single-gain non-singular fast termination controllers for the X, Y, and Z axes, respectively; f dx f dy f dz These are the interferences along the X, Y, and Z axes, respectively.

[0071] In the position controller, the second intermediate variables b1, b2, and b3 corresponding to the X-axis, Y-axis, and Z-axis are set according to the following formulas:

[0072]

[0073] In the formula, b1, b2, and b3 are the second intermediate variables corresponding to the X-axis, Y-axis, and Z-axis in the position controller, respectively; and These represent the load swing angles in the YOZ and XOZ planes, respectively. , .

[0074] Step S4: Perform attitude calculation based on the virtual control variables of the quadcopter UAV to obtain the desired roll angle and desired pitch angle.

[0075] The attitude calculation process can be as follows: using inverse trigonometric functions, based on the virtual control variables u of the quadcopter UAV on the X, Y, and Z axes. x u y u z The desired roll angle φ is calculated. d Desired pitch angle θ d .

[0076] Step S5: Perform attitude control based on the desired attitude angles (desired yaw angle, desired roll angle, desired pitch angle) and actual attitude angles (actual yaw angle, actual roll angle, actual pitch angle) of the quadcopter UAV to obtain the desired torque of the quadcopter UAV.

[0077] In step S5, the desired torque includes the roll torque about the X-axis, the pitch torque about the Y-axis, and the yaw torque about the Z-axis.

[0078] In step S5, attitude control is performed according to the following formula:

[0079]

[0080]

[0081]

[0082] In the formula, τ φ τ θ τ ψ These represent the desired roll moment about the X-axis, the desired pitch moment about the Y-axis, and the desired yaw moment about the Z-axis for the quadcopter; J xx J yy J zz σ represents the moment of inertia of the quadrotor in the X, Y, and Z directions, respectively; φ σ θ σ ψ These are attitude loop-assisted adaptive single-gain nonsingular fast termination controllers corresponding to the X, Y, and Z axes, respectively; z 2φ z 2θ z 2ψ These represent the uncertainties in the compensation model corresponding to the X, Y, and Z axes, respectively; φ d θ d ψ d These are the desired roll angle, desired pitch angle, and desired yaw angle, respectively; a φ a θ a ψ These are the first adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle, respectively; b φ b θ b ψ These are the second adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle; h 1φ h 1θ h 1ψ These are the third adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle; h 2φ h 2θ h 2ψ These are the fourth adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle; e φ e θ e ψ These represent roll angle error, pitch angle error, and yaw angle error, respectively; sig indicates a sign power function, || indicates the absolute value, the superscript "·" indicates the first derivative, and the superscript "··" indicates the second derivative.

[0083] Step S6: Based on the desired vertical pull and desired torque of the quadcopter drone, construct a control allocation matrix, obtain the desired rotational speed of the four rotor motors based on the control allocation matrix, and control the position and attitude of the quadcopter drone based on the desired rotational speed of the four rotor motors.

[0084] The present invention also provides a control system for reducing sway during sling flight of a quadcopter unmanned aerial vehicle, comprising:

[0085] The data acquisition unit is used to acquire the desired position, desired yaw angle, actual position and actual attitude angle of the quadcopter UAV in real time, and to acquire the load swing angle in real time using an angle sensor.

[0086] The anti-swing controller is used to compensate for the desired position of the quadcopter drone based on the load swing angle, and obtain the compensated desired position.

[0087] The position controller is used to perform position control based on the actual position and the compensated desired position of the quadcopter UAV, and obtain the virtual control quantities of the quadcopter UAV in the vertical and horizontal directions.

[0088] The attitude calculation unit is used to calculate the attitude based on the virtual control variables of the quadcopter UAV, and obtain the desired roll angle and desired pitch angle.

[0089] An attitude controller is used to control the attitude of a quadcopter UAV based on its desired attitude angles (desired yaw angle, desired roll angle, desired pitch angle) and actual attitude angles (actual yaw angle, actual roll angle, actual pitch angle) to obtain the desired torque of the quadcopter UAV.

[0090] The control allocation unit constructs a control allocation matrix based on the desired vertical pull and desired torque of the quadcopter drone, obtains the desired rotational speeds of the four rotor motors based on the control allocation matrix, and sends control commands to the quadcopter drone based on the desired rotational speeds of the four rotor motors.

[0091] Specific embodiments of the present invention are as follows:

[0092] Example

[0093] This embodiment provides a method for reducing sway control during quadrotor-mounted flight, wherein the quadrotor-mounted system is as follows: Figure 1 As shown, it includes a quadcopter, ropes, and loads suspended by the ropes.

[0094] like Figure 2 As shown, the control system includes a position controller (specifically a non-singular terminal sliding mode controller), an attitude controller (specifically a non-singular terminal sliding mode controller), an extended state observer (specifically an observer of system errors), and a yaw reduction controller (specifically a controller based on Lyapunov functions).

[0095] The control method specifically includes the following steps:

[0096] Step S1: Establish the dynamic model of the quadrotor using the Newton-Euler method:

[0097] First, define an inertial coordinate system {E}, a body coordinate system {B}, and a load coordinate system {L}, with their positive directions being the directions of the coordinate axes. The origins of the body and load coordinate systems are located at the center of mass of the quadrotor. Define the position vector of the quadrotor in the inertial coordinate system. Where x, y, and z are the positions of the quadrotor on the X, Y, and Z axes, respectively, and the velocity vector is... ,in , , These represent the flight speeds of the quadcopter along the X, Y, and Z axes, respectively. The quadcopter's attitude angles... ,in , , These represent the roll, pitch, and yaw angles generated by the quadrotor around the X, Y, and Z axes of the airframe coordinate system, respectively. The angular velocity of the quadrotor... , where p, q, and r are the angular velocities of the quadrotor around the X-axis, Y-axis, and Z-axis of the body coordinate system, respectively.

[0098] Secondly, we make the following assumptions:

[0099] (1) The load is a point mass, and only the change in position is considered;

[0100] (2) The center of mass of the quadcopter and the center of mass of the load are located at their respective geometric centers;

[0101] (3) The angle between the load and the vertical direction is {-π / 2, π / 2};

[0102] (4) The quadcopter and the load are connected by a massless rope of constant length, with the connection point at the center of mass of the drone and the load, respectively.

[0103] (5) External interference It is bounded and differentiable.

[0104] Subsequently, based on the Newton-Euler equations, the dynamic equations of the quadcopter suspension system were established. The dynamic equations are expressed by the following formula:

[0105]

[0106] In the formula, m is the mass of the quadcopter. This is the thrust vector of the quadrotor in the airframe coordinate system. This represents the gravitational force vector acting on the quadrotor in the inertial coordinate system. For interference along the XYZ axes, Here is the inertia matrix of the quadcopter. U1 represents the torque generated by the quadrotor around the XYZ axes in the body coordinate system, U2, U3, and U4 represent the pitching moment, roll moment, and yaw moment of the quadrotor around the X-axis, respectively, in the body coordinate system. The superscript B indicates the body coordinate system, and the superscript E indicates the inertial coordinate system.

[0107] Since the pitch and roll angles of a quadcopter are relatively small in actual flight, a small-angle approximation is made. Therefore, the rate of change of pitch angular velocity can be simplified using the following formula:

[0108]

[0109] In the formula, For Euler angular rate, ω represents the angular velocity of the quadcopter in the body coordinate system.

[0110] Based on the dynamic equations and the conversion formula between Euler angular rate and angular velocity, the flight dynamics model of the quadcopter is obtained. The flight dynamics model of the quadcopter is expressed by the following formula:

[0111]

[0112] In the formula, Let X be the acceleration of the quadcopter in the X direction. Let be the acceleration of the quadcopter in the Y direction. Let Z be the acceleration of the quadcopter in the Z direction. For roll acceleration, For pitch acceleration, Yaw acceleration; , U1 represents the total thrust of the quadcopter in the airframe coordinate system, while U2, U3, and U4 represent the roll moment, pitch moment, and yaw moment of the quadcopter about the X-axis, Y-axis, and Z-axis, respectively, in the airframe coordinate system. , , These are the moments of inertia of the quadcopter in the X, Y, and Z directions, respectively.

[0113] The inputs to the control system are U1, U2, U3, and U4, which are obtained using the following formulas:

[0114]

[0115] In the formula, This is the rotor thrust coefficient. It is the rotor torque coefficient. d is the rotational speed of the i-th motor, and d is the distance from the rotor shaft to the center of the UAV body.

[0116] Step S2: Establish the suspension dynamics model using the Lagrange equations:

[0117] Quadrotor suspension system such as Figure 1 As shown. First, calculate the position vector between the load and the quadcopter; their relationship is expressed as:

[0118]

[0119] In the formula, Let be the position vector of the quadrotor in the inertial coordinate system. Let be the position vector of the load in the inertial coordinate system. The length of the rope, This is the rotation matrix from the body coordinate system to the load coordinate system. and These represent the swing angles of the load in the YOZ and XOZ planes, respectively. and These are the rotation matrices of the load around the X-axis and y-axis, respectively.

[0120] Wherein, the rotation matrices of the load about the X and y axes and They are represented as follows:

[0121] ,

[0122] In the formula, , .

[0123] By taking the second derivative with respect to each input of the control system, we can obtain the acceleration of the load in the X, Y, and Z directions:

[0124]

[0125] In the formula, , , These represent the accelerations of the load in the X, Y, and Z directions, respectively. , , , respectively, represent the accelerations of the quadcopter in the X, Y, and Z directions; l is the length of the suspension rope; the superscript "·" indicates the first derivative, and the superscript "··" indicates the second derivative.

[0126] Then, the Lagrange equation is established, as shown in the following formula:

[0127]

[0128] In the formula, Let be the Lagrangian function, T be the total kinetic energy of the quadrotor suspension system, V be the total potential energy of the quadrotor suspension system, q be the generalized coordinate system of the quadrotor suspension system, and Q be the generalized force acting on the quadrotor suspension system.

[0129] In the Lagrange equations, the parameters are set according to the following formulas:

[0130]

[0131]

[0132]

[0133]

[0134] In the formula, For the mass of a quadcopter, For load mass, ω x ω y ω z ωp, ωq, and ωr, respectively, represent the angular velocities of the quadrotor around the X, Y, and Z axes of the body coordinate system, corresponding to p, q, and r as mentioned above; g is the acceleration due to gravity. , , These represent the interferences of the quadcopter along the X, Y, and Z axes, respectively. , These represent the interferences of the load in the YOZ and XOZ planes, respectively; the superscript "·" indicates the first derivative.

[0135] Solve for the rotation matrices of the load about the X and y axes, and simplify the flight dynamics model of the quadrotor to obtain the quadrotor sling-mounted dynamics model. The quadrotor sling-mounted dynamics model is expressed as:

[0136]

[0137] In the formula, , , , , , All are intermediate variables, set according to the following formulas:

[0138]

[0139] In the formula, , ; and These represent the swing angles of the load in the YOZ and XOZ planes, respectively. The superscript "·" indicates the first derivative, and the superscript "··" indicates the second derivative.

[0140] As can be seen from the above dynamic model, the oscillation angle of the load is closely related to the quadrotor, but the quadrotor's actuator cannot directly reduce the oscillation of the load. To suppress the oscillation of the load, this invention designs a position controller, an attitude controller, an extended state observer, and an oscillation reduction controller to ensure flight safety.

[0141] Step S3: Design an attitude loop extension observer to estimate the attitude loop error:

[0142] To estimate attitude loop disturbances, an extended state observer is designed for the attitude loop as follows, taking the roll angle φ channel as an example:

[0143]

[0144] In the formula, , This is the observer gain, and all its values ​​are positive constants. For roll angle, For the desired roll angle, This represents the uncertainty in the compensation model corresponding to the roll angle.

[0145] Step S4: Design an attitude controller to control the attitude angles of the quadcopter:

[0146] First, the non-singular terminal sliding surface is designed as follows:

[0147]

[0148] In the formula, a, b, h1, and h2 are the first adjustable parameter, the second adjustable parameter, the third adjustable parameter, and the fourth adjustable parameter, respectively, and h1 and h2 are both positive integers. and .

[0149] Taking the Φ channel as an example, the error signal is defined. ,in φ is the desired roll angle, and φ is the actual roll angle.

[0150] For the error signal e φ Differentiating and substituting into the quadcopter suspension dynamics model, we get:

[0151]

[0152] In the formula, , , These represent the actual roll angle, actual pitch angle, and actual yaw angle generated by the quadcopter around the X, Y, and Z axes of the airframe coordinate system, respectively. For the desired roll angle, J xx J yy J zzU1, U2, and U3 represent the moments of inertia of the quadcopter in the X, Y, and Z directions, respectively; U2 is the pitching moment of the quadcopter about the X-axis; the superscript "·" indicates the first derivative, and the superscript "··" indicates the second derivative.

[0153] Considering the uncertain attitude dynamics and the second-order attitude error signal, the control law is designed as follows:

[0154]

[0155] In the formula, τ φ J is the desired rolling torque of the quadcopter about the X-axis. xx Let be the moment of inertia of the quadcopter in the X direction. This indicates an auxiliary adaptive single-gain non-singular fast termination controller. Uncertainty in the compensation model, φ d For the desired roll angle, a φ b φ h 1φ h 2φ These are the first, second, third, and fourth adjustable parameters of the attitude controller corresponding to the roll angle, respectively. φ This refers to the roll angle error;

[0156] sig is a sign power function, || represents the absolute value, the superscript "·" represents the first derivative, and the superscript "··" represents the second derivative.

[0157] Among them, the auxiliary adaptive single-gain non-singular fast termination controller Set it according to the following formula:

[0158]

[0159] In the formula, k is the coefficient of the auxiliary adaptive single-gain non-singular fast terminal controller, s is the non-singular terminal sliding surface, and sig is the sign power function.

[0160] The coefficient k of the auxiliary adaptive single-gain non-singular fast terminal controller is set according to the following formula:

[0161]

[0162] In the formula, , These are the fourth and fifth adjustable parameters, respectively. and They determine the rate at which k increases or decreases, respectively, and || represents the absolute value.

[0163] Similarly, the control laws for pitch and yaw angles can be derived as follows:

[0164]

[0165]

[0166] In the formula, τ θ τ ψ These represent the desired pitching moment and the desired yaw moment of the quadcopter about the Y-axis, respectively; J yy J zz σ represents the moment of inertia of the quadrotor in the Y and Z directions, respectively; θ σ ψ These are attitude loop-assisted adaptive single-gain nonsingular fast termination controllers for the Y and Z axes, respectively; z 2θ z 2ψ These represent the uncertainties in the compensation model corresponding to the Y-axis and Z-axis, respectively; θ d ψ d These are the desired pitch angle and the desired yaw angle, respectively; a θ a ψ These are the first adjustable parameters of the attitude controller corresponding to the pitch and yaw angles, respectively; b θ b ψ These are the second adjustable parameters of the attitude controller corresponding to the pitch and yaw angles, respectively; h 1θ h 1ψ These are the third adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle; h 2θ h 2ψ These are the fourth adjustable parameters of the attitude controller corresponding to pitch and yaw angles, respectively; e θ e ψ These represent pitch angle error and yaw angle error, respectively; sig represents a sign power function, || represents the absolute value, the superscript "·" represents the first derivative, and the superscript "··" represents the second derivative.

[0167] S5: Design a position controller to ensure the quadcopter accurately tracks its trajectory.

[0168] First, the sliding surface of the position controller is the same as that of the attitude controller.

[0169]

[0170] In the formula, e is the error signal, and a, b, h1, and h2 are the first, second, third, and fourth adjustable parameters, respectively. , It is a positive integer. and .

[0171] Taking the X channel as an example, the error signal is defined. ,in The desired position for the quadcopter. This refers to the actual position of the quadcopter.

[0172] Differentiating the error signal and substituting it into the quadcopter suspension dynamics model, we get:

[0173]

[0174] In the formula, a1 and b1 are the first and second intermediate variables, respectively, and U1 is the total thrust of the quadrotor in the body coordinate system. For the mass of a quadcopter, Let f be the load mass, l be the length of the suspension rope, and f be the length of the load. dx Interference on the X-axis.

[0175] Considering uncertain dynamics and a second-order position error signal, the control law is designed as follows:

[0176]

[0177] In the formula, u x This refers to the virtual control variables of a quadcopter drone on the X-axis. For the mass of a quadcopter, Let l be the load mass, l be the length of the suspension rope, sig be the sign power function, and e be the load mass. x Let x be the position error of the quadcopter drone on the X-axis. ud For the desired position of the quadcopter drone on the X-axis, a x b x h 1x h 2x These are the first adjustable parameter, second adjustable parameter, third adjustable parameter, and fourth adjustable parameter corresponding to the X-axis in the position controller, respectively. b1 is the first intermediate variable corresponding to the X-axis in the position controller, and σ x For the position loop auxiliary adaptive single-gain nonsingular fast termination controller corresponding to the X-axis, f dx Interference on the X-axis.

[0178] in, It is an auxiliary adaptive single-gain non-singular fast termination controller, configured as follows:

[0179]

[0180] In the formula, k is the coefficient of the auxiliary adaptive single-gain non-singular fast terminal controller, and s is the non-singular terminal sliding surface.

[0181] The coefficient k of the auxiliary adaptive single-gain nonsingular fast terminal controller is set as follows:

[0182]

[0183] in, , These are the fourth and fifth adjustable parameters, respectively. and They determine the rate at which k increases or decreases, respectively.

[0184] Similarly, the control laws for the y-axis and z-axis can be obtained as follows:

[0185]

[0186]

[0187] In the formula, u y u z These are the virtual control variables of the quadcopter UAV on the Y and Z axes, respectively; m u For the mass of the quadcopter, m L Let be the load mass; l be the length of the suspension rope; sig be a sign power function, sig α (x)=|x| α ×sign(x), || represents the absolute value, the superscript "·" represents the first derivative, and the superscript "··" represents the second derivative; y ud z ud These represent the compensated desired positions of the quadcopter UAV on the Y and Z axes, respectively; a y a z These are the first adjustable parameters corresponding to the Y-axis and Z-axis in the position controller; b y b z These are the second adjustable parameters corresponding to the Y and Z axes in the position controller; e y e z These represent the positional errors of the quadcopter drone along the Y and Z axes, respectively; h 1y h 1z These are the third adjustable parameters corresponding to the Y-axis and Z-axis in the position controller; h 2y h 2z b1 and b2 are the fourth adjustable parameters corresponding to the Y-axis and Z-axis in the position controller, respectively; b3 and b4 are the second intermediate variables corresponding to the Y-axis and Z-axis in the position controller, respectively; σ y σ z These are position loop-assisted adaptive single-gain non-singular fast termination controllers for the Y and Z axes, respectively; f dy f dz These are the interferences on the Y-axis and Z-axis, respectively.

[0188] Step S6: Design a sway control to suppress load sway:

[0189] First, design the Lyapunov function:

[0190]

[0191] In the formula, l is the length of the suspension rope. and θ represents the swing angle of the load in the YOZ plane and the XOZ plane, respectively; g is the acceleration due to gravity.

[0192] Taking the time derivative of the Lyapunov function, we obtain the following:

[0193]

[0194] Substituting the derivative of the Lyapunov function into the quadcopter suspension dynamics model, we get:

[0195]

[0196] According to Lyapunov's second method, for the system to be stable, the following conditions must be met: Negative definiteness, therefore let:

[0197]

[0198] In the formula, , , These represent the acceleration compensation amounts for the quadcopter UAV in the X, Y, and Z directions, respectively. and These represent the swing angles of the load in the YOZ plane and the XOZ plane, respectively. , , All are adjustable parameters and are non-negative real numbers.

[0199] The embodiments described above are merely preferred embodiments to illustrate the present invention and are not intended to limit the invention in any way. Those skilled in the art can modify, substitute equivalent elements, or improve the embodiments within the spirit and principles of the present invention. Any modifications, equivalent changes, and improvements made thereto should be considered to fall within the protection scope defined by the claims of the present invention.

Claims

1. A method for reducing sway control during suspended flight of a quadcopter unmanned aerial vehicle (UAV), characterized in that, Includes the following steps: Step S1: Obtain the desired position, desired yaw angle, actual position, and actual attitude angle of the quadcopter UAV at the current moment, and collect the load swing angle at the current moment; the attitude angle includes roll angle, pitch angle, and yaw angle; Step S2: Compensate the quadcopter drone's desired position based on the load swing angle at the current moment to obtain the compensated desired position; Step S3: Perform position control based on the actual position and the expected position after compensation of the quadcopter UAV to obtain the virtual control quantity, and use the virtual control quantity in the vertical direction as the expected vertical tension. Step S4: Perform attitude calculation based on the virtual control variables of the quadcopter UAV to obtain the desired roll angle and desired pitch angle; Step S5: Perform attitude control based on the desired attitude angle and the actual attitude angle of the quadcopter UAV to obtain the desired torque; Step S6: Based on the desired vertical pull and desired torque of the quadcopter drone, obtain the desired rotational speed of the four rotor motors, and control the position and attitude of the quadcopter drone based on the desired rotational speed of the four rotor motors.

2. The method for reducing sway control during suspended flight of a quadcopter UAV according to claim 1, characterized in that: In step S1, the load swing angle includes the swing angle of the load in the YOZ plane and the XOZ plane in the body coordinate system.

3. The method for reducing sway control during suspended flight of a quadcopter UAV according to claim 1, characterized in that: Step S2 includes: Based on the load swing angle, the acceleration compensation amount can be obtained using the following formula: In the formula, , , These represent the acceleration compensation amounts for the quadcopter drone along the X, Y, and Z axes, respectively. , , All are non-negative real numbers; and These represent the load swing angles in the YOZ and XOZ planes, respectively; the superscript "·" indicates the first derivative. The second integral of the acceleration compensation amount is used as the position compensation amount; Calculate the sum of the position compensation amount and the desired position to obtain the compensated desired position.

4. The method for reducing sway control during suspended flight of a quadcopter UAV according to claim 1, characterized in that: In step S3, position control is performed according to the following formula: In the formula, u x u y u z These represent the virtual control variables of a quadcopter UAV on the X, Y, and Z axes, respectively; m u For the mass of the quadcopter, m L σi represents the load mass; l represents the length of the suspension rope; sig is a sign power function, || represents the absolute value, the superscript "·" represents the first derivative, and the superscript "··" represents the second derivative; x ud y ud z ud These represent the compensated desired positions of the quadcopter UAV along the X, Y, and Z axes, respectively; a x a y a z These are the first adjustable parameters corresponding to the X, Y, and Z axes in the position controller; b x b y b z These are the second adjustable parameters corresponding to the X, Y, and Z axes in the position controller; e x e y e z These represent the positional errors of the quadcopter drone along the X, Y, and Z axes, respectively; h 1x h 1y h 1z These are the third adjustable parameters corresponding to the X, Y, and Z axes in the position controller; h 2x h 2y h 2z b1, b2, and b3 are the fourth adjustable parameters corresponding to the X, Y, and Z axes in the position controller, respectively; b1, b2, and b3 are the second intermediate variables corresponding to the X, Y, and Z axes in the position controller, respectively; σ x σ y σ z These are position loop-assisted adaptive single-gain non-singular fast termination controllers for the X, Y, and Z axes, respectively; f dx f dy f dz These are the interferences along the X, Y, and Z axes, respectively.

5. The method for reducing sway control during suspended flight of a quadcopter UAV according to claim 4, characterized in that: In the position controller, the second intermediate variables b1, b2, and b3 corresponding to the X-axis, Y-axis, and Z-axis are set according to the following formulas: In the formula, b1, b2, and b3 are the second intermediate variables corresponding to the X-axis, Y-axis, and Z-axis in the position controller, respectively; and These represent the load swing angles in the YOZ and XOZ planes, respectively. , .

6. The method for reducing sway control during suspended flight of a quadcopter UAV according to claim 1, characterized in that: In step S5, the desired torque includes the roll torque about the X-axis, the pitch torque about the Y-axis, and the yaw torque about the Z-axis.

7. The method for reducing sway control during suspended flight of a quadcopter UAV according to claim 1, characterized in that: In step S5, attitude control is performed according to the following formula: In the formula, τ φ τ θ τ ψ These represent the desired roll moment about the X-axis, the desired pitch moment about the Y-axis, and the desired yaw moment about the Z-axis for the quadcopter; J xx J yy J zz σ represents the moment of inertia of the quadrotor in the X, Y, and Z directions, respectively; φ σ θ σ ψ These are attitude loop-assisted adaptive single-gain nonsingular fast termination controllers corresponding to the X, Y, and Z axes, respectively; z 2φ z 2θ z 2ψ These represent the uncertainties in the compensation model corresponding to the X, Y, and Z axes, respectively; φ d θ d ψ d These are the desired roll angle, desired pitch angle, and desired yaw angle, respectively; a φ a θ a ψ These are the first adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle, respectively; b φ b θ b ψ These are the second adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle; h 1φ h 1θ h 1ψ These are the third adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle; h 2φ h 2θ h 2ψ These are the fourth adjustable parameters of the attitude controller corresponding to roll angle, pitch angle, and yaw angle; e φ e θ e ψ These represent roll angle error, pitch angle error, and yaw angle error, respectively; sig indicates a sign power function, || indicates the absolute value, the superscript "·" indicates the first derivative, and the superscript "··" indicates the second derivative.

8. A control system for reducing sway during suspended flight of a quadcopter unmanned aerial vehicle (UAV), characterized in that, include: The data acquisition unit is used to acquire the desired position, desired yaw angle, actual position and actual attitude angle of the quadcopter UAV in real time, and to acquire the load swing angle in real time. The anti-sway controller is used to compensate for the desired position of the quadcopter drone based on the load sway angle, and obtain the compensated desired position. The position controller is used to perform position control based on the actual position and the compensated desired position of the quadcopter drone, thereby obtaining the virtual control quantity of the quadcopter drone. The attitude calculation unit is used to calculate the attitude based on the virtual control variables of the quadcopter UAV, and obtain the desired roll angle and desired pitch angle. An attitude controller is used to control the attitude of a quadcopter drone based on its desired attitude angle and actual attitude angle, thereby obtaining the desired torque of the quadcopter drone. The control and distribution unit obtains the desired rotational speeds of the four rotor motors based on the desired vertical pull and desired torque of the quadcopter drone, and sends control commands to the quadcopter drone based on the desired rotational speeds of the four rotor motors.

Images