A Method and System for Attitude Suppression and Vibration Control of Rotary-Wing UAVs Based on Additive Decomposition Notch Waves

Abstract

This invention relates to the field of attitude vibration reduction control for quadrotor aircraft, specifically disclosing a method and system for attitude vibration suppression control of a rotary-wing UAV based on additive decomposition notch filtering. The method includes: Step S1, based on the basic dynamic model of the quadrotor aircraft, introducing a blade eccentricity model with an unbalanced mass model to obtain a vibration suppression dynamic model for the quadrotor aircraft; Step S2, constructing an output vector based on the designed output matrix and the state variables in the vibration suppression dynamic model; Step S3, calculating the dynamic first derivative of the output vector along the vibration suppression dynamic model; Step S4, designing a traditional additive decomposition dynamic inverse controller based on the first derivative dynamics, and improving the filter in the traditional additive decomposition dynamic inverse controller using a notch filter to obtain an additive decomposition notch dynamic inverse controller; Step S5, using the additive decomposition notch dynamic inverse controller to perform attitude vibration suppression control on the rotary-wing UAV.

Description

Technical Field

[0001] This invention relates to the field of attitude damping and control technology for quadcopter aircraft, specifically to a method and system for attitude damping and control of rotary-wing unmanned aerial vehicles based on additive decomposition notch wave. Background Technology

[0002] Multirotor aircraft inevitably experience vibrations during flight. Generally, a multirotor is an assembly of multiple parts, typically connected by bolts. However, during flight, various uncontrollable stresses can loosen these bolts, causing low-frequency vibrations. Furthermore, atmospheric turbulence and unstable airflow create non-uniform flow fields. Multirotor aircraft operating in such non-uniform flow fields are subject to various uncertain disturbances and torques. In addition, the multirotor frame houses numerous modules such as sensors, flight control boards, and GPS. Improper arrangement of these modules can easily cause a shift in the frame's center of gravity. This shift introduces additional interference forces and may also cause vibrations. More seriously, the sensor output signals may contain this vibration noise, leading to decreased measurement accuracy. These noisy signals are transmitted to the drive unit (composed of propellers and motors) via the designed control signals, further amplifying the vibration amplitude. Moreover, due to manufacturing inaccuracies or installation errors, the propeller's center of gravity may not be collinear with the motor shaft. Therefore, multirotor aircraft also experience disturbance torque due to the unbalanced operation of the propellers. Because the propellers rotate at high speeds, the resulting centrifugal force varies periodically, with its period often proportional to the propeller's rotation period. The additional torque caused by this centrifugal force disturbance further affects attitude dynamics, ultimately leading to vibration. Therefore, suppressing the disturbance torque caused by propellers with unbalanced mass during operation is a crucial problem that needs to be solved. Summary of the Invention

[0003] This invention provides a method and system for attitude and vibration suppression control of a rotary-wing unmanned aerial vehicle based on additive decomposition notch filtering. The method includes:

[0004] Step S1: Based on the basic dynamic model of the quadrotor, introduce the blade eccentricity model of the unbalanced mass model to obtain the vibration damping dynamic model of the quadrotor.

[0005] Step S2: Construct the output vector based on the designed output matrix and the state variables in the vibration damping dynamics model;

[0006] Step S3: Calculate the first derivative dynamics of the output vector along the vibration suppression dynamic model;

[0007] Step S4: Based on the first derivative, dynamically design a traditional additive decomposition dynamic inverse controller, and use a quality factor of... and dominant frequency Notch filter The filter in the traditional additive decomposition dynamic inverse controller is improved to obtain an additive decomposition notch filter dynamic inverse controller, wherein, The Laplace transform symbol;

[0008] Step S5: Use the additive decomposition notch dynamic inverse controller to control the attitude vibration of the rotary-wing UAV.

[0009] Optionally, in step S1, the basic dynamic model specifically refers to:

[0010] ;

[0011] in, The coordinates are along the z-axis. for The derivative; The velocity along the z-axis, for The derivative of g, where g is the acceleration due to gravity. For total mass, For the actual total thrust, Let be the attitude angle vector, where For the roll attitude angle, The pitch angle, Where yaw is the attitude angle, and T is the transpose. for The derivative, For the transformation matrix, It is the angular velocity vector. The angular velocity of the rolling attitude. The pitch attitude angular velocity, The yaw attitude angular velocity, for The derivative, For predictive moment of inertia matrix, Represents a diagonal matrix. Let x be the moment of inertia along the x-axis. Let be the moment of inertia along the y-axis. Let z be the moment of inertia along the z-axis. for The inverse matrix, This represents the true attitude torque vector. For the actual rolling torque, For the actual pitch moment, For the actual yaw moment, This is the gyroscopic torque.

[0012] Optionally, in step S1, the blade eccentricity model of the unbalanced mass model is specifically as follows:

[0013] When the masses of the two blades of a propeller are not equal, an extra mass appears on one side. At this time, the propeller generates centripetal force, and an eccentric model of the propeller blades is constructed based on the centripetal force. :

[0014] ;

[0015] in, The distance between the additional mass and the motor shaft. Indicates time, Represents propeller i The initial turning angle, For propeller i angular velocity of rotation.

[0016] Optionally, in step S1, the vibration damping dynamics model of the quadcopter... Specifically:

[0017] ;

[0018] in, A The state matrix; B For control matrix; For the state variables of a quadcopter, For additive decomposition notch filter dynamic inverse controller, This represents the total disturbance.

[0019] Optionally, in step S2, the output vector is specifically:

[0020] ;

[0021] in, This is the output vector; is the output matrix; T is the transpose.

[0022] Optionally, in step S3, the first derivative dynamics of the vibration suppression dynamic model is specifically as follows:

[0023] ;

[0024] in, for The derivative, Given a diagonal matrix; This is an intermediate transition vector. .

[0025] Optionally, in step S4, the conventional additive decomposition dynamic inverse controller for:

[0026] ;

[0027] in, Positive design parameters; Let be the state variables of the quadcopter described in the Lagrange domain.

[0028] Optionally, in step S4, the additive decomposition notch filter dynamic inverse controller... u Specifically:

[0029] ;

[0030] Among them, vibration control component .

[0031] Optionally, a traditional additive decomposition dynamic inverse controller can be designed based on the first derivative, and a quality factor of 1 / 2 can be used. and dominant frequency Notch filter The filter in the traditional additive decomposition dynamic inverse controller is improved to obtain an additive decomposition notch dynamic inverse controller. The specific content is as follows:

[0032] ;

[0033] in, It is an identity matrix.

[0034] This invention also discloses a rotorcraft unmanned aerial vehicle (UAV) attitude vibration suppression control system based on additive decomposition notch filtering, the system comprising:

[0035] The vibration damping dynamics model module is used to introduce the blade eccentricity model of the unbalanced mass model into the basic dynamics model of the quadcopter to obtain the vibration damping dynamics model of the quadcopter.

[0036] The output vector construction module is used to construct the output vector based on the designed output matrix and the state variables in the vibration damping dynamics model.

[0037] A dynamic model differentiation model is used to calculate the first derivative dynamics of the output vector along the vibration suppression dynamic model;

[0038] The controller design module is used to dynamically design a traditional additive decomposition dynamic inverse controller based on the first derivative, and uses a quality factor of 1. and dominant frequency Notch filter The filter in the traditional additive decomposition dynamic inverse controller is improved to obtain an additive decomposition notch filter dynamic inverse controller, wherein, The Laplace transform symbol;

[0039] The attitude damping control module is used to control the attitude damping of a rotary-wing UAV using the additive decomposition notch dynamic inverse controller.

[0040] Compared with existing technologies, the advantages of this invention are as follows: First, it considers blade eccentricity as the main cause of attitude chattering in quadrotor aircraft and constructs a vibration suppression dynamic model that takes blade eccentricity into account. Second, it introduces a notch filter into the design process of a traditional additive decomposition dynamic inverse controller, which can effectively solve the attitude chattering problem of quadrotor aircraft in hovering state. Attached Figure Description

[0041] To more clearly illustrate the technical solution of the present invention, the drawings used in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0042] Figure 1 This is a flowchart illustrating the method steps of the attitude damping control method for rotary-wing unmanned aerial vehicles based on additive decomposition notch filtering according to an embodiment of the present invention.

[0043] Figure 2 This is a schematic diagram illustrating the mass eccentricity caused by blade damage in an embodiment of the present invention.

[0044] Figure 3 This is a simplified structural diagram of the X-type quadcopter in an embodiment of the present invention;

[0045] Figure 4 This is a structural framework diagram of the entire closed-loop system according to an embodiment of the present invention;

[0046] Figure 5 This is a schematic diagram of the attitude chattering test system according to an embodiment of the present invention;

[0047] Figure 6 This is a comparative analysis diagram of the vibration suppression effect of embodiments of the present invention. Detailed Implementation

[0048] 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.

[0049] Example 1:

[0050] A method for attitude damping control of rotary-wing UAVs based on additive decomposition notch filtering, such as Figure 1 As shown, the method includes:

[0051] Step S1: Based on the basic dynamic model of the quadrotor, introduce the blade eccentricity model of the unbalanced mass model to obtain the vibration damping dynamic model of the quadrotor.

[0052] This embodiment uses a general quadrotor dynamics model. Based on this, a blade eccentricity model including an unbalanced mass model is introduced to further quantify the mechanical response caused by the vibration of the blade eccentricity. Finally, the two models are integrated and deformed to form the final vibration damping dynamics model of the quadrotor for controller design.

[0053] The fundamental dynamic model is specifically as follows:

[0054] (1)

[0055] in, The coordinates are along the z-axis. for The derivative; The velocity along the z-axis, for The derivative of g, where g is the acceleration due to gravity. For total mass, For the actual total thrust, Let be the attitude angle vector, where For the roll attitude angle, The pitch angle, Where yaw is the attitude angle, and T is the transpose. for The derivative, For the transformation matrix, It is the angular velocity vector. The angular velocity of the rolling attitude. The pitch attitude angular velocity, The yaw attitude angular velocity, for The derivative, For predictive moment of inertia matrix, Represents a diagonal matrix. Let x be the moment of inertia along the x-axis. Let be the moment of inertia along the y-axis. Let z be the moment of inertia along the z-axis. for The inverse matrix, This represents the true attitude torque vector. For the actual rolling torque, For the actual pitch moment, For the actual yaw moment, This is the gyroscopic torque.

[0056] Transformation matrix W Defined as:

[0057] (2)

[0058] Gyro torque Defined as:

[0059] (3)

[0060] in, Represents the unit vector along the z-axis. This represents the total moment of inertia of the motor rotor and propeller. For propeller i angular velocity of rotation;

[0061] Due to manufacturing limitations or installation errors, the center of mass of the propeller is often not collinear with the motor shaft. Therefore, the propeller experiences unbalanced rotation, causing severe vibrations when the quadcopter hovers. Figure 2 As shown, the edge of the right propeller is damaged, therefore the mass of the left propeller is greater than the mass of the right propeller.

[0062] For ease of analysis, let's assume that this damage would add an extra mass to the left side of the propeller. And its distance from the motor shaft is The added mass causes the masses of the left and right blades of the propeller to no longer be equal, resulting in an imbalance mass. Force analysis shows that this imbalance mass generates a centripetal force. This centripetal force produces a high-frequency vibration, which in turn affects the measurement accuracy of the airborne gyroscope. Figure 3 A simplified structural diagram of an X-type quadcopter is given, in which each blade is assumed to have a mass of... , Based on the first The centripetal force generated by an unbalanced propeller during rotation is used to construct the following propeller blade eccentricity model:

[0063] (4)

[0064] in, The distance between the additional mass and the motor shaft. Indicates time, Represents propeller i The initial turning angle, For propeller i rotational angular velocity, t This is a time variable. Ultimately, the generated total centripetal force will produce a control allocation matrix related to the vibration:

[0065] (5)

[0066] in:

[0067] , , , ; Indicates the initial rotation angle of the blade; Indicates the radius of the rack. Represents propeller i Distance to the base, such as Figure 2 As shown.

[0068] True total thrust and the true attitude torque vector Combined into a total control vector It can be defined as:

[0069] (6)

[0070] in, The original control allocation matrix of a quadcopter can be defined in the following form:

[0071] (7)

[0072] in, Indicates the thrust coefficient; The torque coefficient is shown in the diagram below. Figure 4 As shown, the height channel uses classic (proportional-integral-derivative) PID technology to design the actual total thrust. The design process is omitted. The following mainly introduces how to apply additive decomposition notch filtering technology to the design process of attitude damping controllers for real quadcopters.

[0073] Next, the blade eccentricity model (4) is regarded as a disturbance and further integrated into the basic dynamic model (1) of the quadcopter, thus forming the vibration damping dynamic model of the quadcopter. The specific construction process is divided into the following steps.

[0074] Taking the roll channel as an example (the modeling process for the pitch and yaw channels is similar), we study its linear dynamic model under hovering conditions:

[0075] (8)

[0076] in, This represents the disturbance torque component generated by the blade eccentricity model in the roll channel. This represents the dominant frequency. For clarity, let:

[0077] (9)

[0078] The linear dynamic model (8) can be further rewritten as:

[0079] (10)

[0080] Using the same approach, the dynamic equations for the pitch and yaw channels can be described as follows:

[0081] (11)

[0082] ;

[0083] in, , These represent the disturbance torque components generated by the blade eccentricity model in the pitch and yaw channels, respectively.

[0084] Integrating the linearized dynamic subsystems (10) and (11) yields:

[0085] (12)

[0086] in , , ,symbol Let represent a diagonal matrix. Next, to represent the uncertain components in the true inertia matrix, it can be expressed in the following form:

[0087] (13)

[0088] in The inertia matrix represents the precise measurement. This represents the remaining uncertain inertia matrix. Let (13) represent the identity matrix. Substituting (13) into (12) yields...

[0089] (14)

[0090] Next, the pole placement technique is used to transform the unstable state matrix. Transform into a stable state matrix The role of node configuration technology is to find a gain matrix. The following equation applies to the relationship between the stable state matrix and the unstable state matrix:

[0091] (15)

[0092] Introducing state vectors in (15)(15) This can then be further expressed as

[0093] (16)

[0094] Substituting (16) into (14) and (14) yields the vibration damping dynamics model for the multi-rotor unmanned aerial vehicle:

[0095] (17)

[0096] in, This represents the total disturbance, which includes both the disturbance component from the blade eccentricity model and the disturbance component from the uncertain inertia. This indicates the need for further design of an additive decomposition notch filter dynamic inverse controller, in which... For auxiliary control signals of the roll channel, For pitch channel auxiliary control signals, This serves as an auxiliary control signal for the yaw channel. Clearly, the gain matrix can be accurately obtained through pole placement techniques. The specific value, on the other hand It is an inertial matrix that can be accurately measured. Therefore, all that is needed afterward is to complete... The design work allows us to deduce the true attitude torque vector. The specific expression is used to control the attitude of the drone.

[0097] Step S2: Construct the output vector based on the designed output matrix and the state variables in the vibration damping dynamics model.

[0098] Output redefinition technique indicates the steady-state matrix The following equation must be satisfied:

[0099] (18)

[0100] Among them, the output matrix , Each represents a constant parameter vector that needs to be designed; Represents a known diagonal matrix ; , and yes The feature values ​​in the vector are then used. Next, the output vector can be defined as follows:

[0101] (19)

[0102] Step S3: Calculate the first derivative dynamics of the output vector along the vibration suppression dynamic model.

[0103] First, taking the derivative of equation (19) yields the first-order derivative dynamics of the output vector:

[0104] (20)

[0105] Then, substituting the vibration damping dynamics model (17) into (20) yields:

[0106] ;(twenty one)

[0107] Next, using equation (18), (21) can be transformed into the following form:

[0108] ;(twenty two)

[0109] Finally, substituting (19) into (22) yields the first derivative dynamics of the output vector along the damping dynamics model (17):

[0110] ;(twenty three)

[0111] in .

[0112] Step S4: Based on the first derivative, dynamically design a traditional additive decomposition dynamic inverse controller, and use a quality factor of... and dominant frequency Notch filter The filter in the traditional additive decomposition dynamic inverse controller is improved to obtain an additive decomposition notch filter dynamic inverse controller, wherein, This is the Laplace transform symbol.

[0113] First, for the first derivative dynamics (23), the following master system dynamics can be designed:

[0114] ;(twenty four)

[0115] in, Represents the state variables of the main system. express The derivative of the first derivative. Then, subtracting the main system (24) from the first derivative dynamic (23) yields:

[0116] (25)

[0117] Next, assume the state variables in the auxiliary system Therefore, equation (25) can be expressed as the dynamics of the auxiliary system:

[0118] (26)

[0119] make Then, output vector y Laplace transform It can also be represented as:

[0120] (27)

[0121] in, s The Laplace transform symbol, yes Laplace transform, yes Laplace transform.

[0122] Next, performing a Laplace transform on the main system dynamics (24) yields the following results: and attitude torque auxiliary control signal Transfer function between :

[0123] (28)

[0124] in, It is a third-order identity matrix. Representing the Laplace transform symbol. Using the transfer function. , It can be transformed into the following form:

[0125] (29)

[0126] in, It is an additive decomposition notch filter dynamic inverse controller u Laplace transform.

[0127] Substituting (29) into (27), the Laplace transform of the output vector can also be expressed as:

[0128] (30)

[0129] Next, a general form of an additive decomposition dynamic inverse controller is given:

[0130] (31)

[0131] The superscript "-1" indicates The reverse, This refers to the coupling filter, which needs to be designed specifically for the different types of disturbances encountered by the quadcopter during mission execution. (30) Using (31) After replacement, we can get:

[0132] (32)

[0133] Taking the Laplace transform of the output vector yields:

[0134] (33)

[0135] Substituting (33) into (32) yields: Another way to express it:

[0136] (34)

[0137] Substituting (34) into (31) yields:

[0138] (35)

[0139] When only slow time-varying disturbances exist, a positive design parameter can be selected as follows: low-pass filter . (35) use After the replacement, a traditional additive state decomposition dynamic inverse controller of the following type can be designed:

[0140] (36)

[0141] However, in practical applications, quadcopters not only have low-frequency disturbances, but also specific dominant frequencies related to the propeller angular velocity. The vibration components. Therefore, a notch filter is used to improve the filter in the traditional additive decomposition dynamic inverse controller, that is, the present invention adopts the following type of coupled notch filter:

[0142] (37)

[0143] in, It is a notch filter. This notch filter has two design parameters, namely... Indicates the quality factor. Indicates the dominant frequency. When the general filter in (35) Coupled Filter After replacement, the additive state decomposition notch dynamic inverse controller can be obtained:

[0144] (38)

[0145] Vibration control component .

[0146] It is worth noting that the decision to introduce a vibration control component can be made based on the characteristics of the external disturbance (vibration and noise). If the relevant sensors or detection equipment do not capture vibrations of a specific frequency, then in this case, it is possible to... Set to zero, that is It doesn't work.

[0147] Step S5: Use the additive state decomposition notch dynamic inverse controller to perform attitude and vibration suppression control on the rotary-wing UAV. In this embodiment, simulation and real-machine experiments are used to demonstrate that the proposed control algorithm has stronger vibration suppression capabilities compared to the traditional additive state decomposition dynamic inverse controller.

[0148] The entire experimental platform consists of two parts: software-in-the-loop simulation and flight experiments. In the controller design phase, a notch filter controller model and a complete quadcopter dynamics model were constructed using Simulink software. Then, software-in-the-loop simulation was performed to initially adjust the control parameters. Next, the controller in Simulink was converted to C++. Finally, flight experiments were conducted to verify the vibration suppression capability of the proposed control algorithm. Figure 5 As shown, this experiment uses an X-type quadcopter as the controlled object, with a laser mounted above it. The laser beam is projected onto a target. When the quadcopter is hovering, even a slight vibration in its attitude will cause a noticeable displacement of the red dot on the target, thus helping to quantify the intensity of the vibration. The inherent parameters of the quadcopter involved in the experiment can be obtained from Table 1.

[0149] Table 1

[0150]

[0151] Disassembling the additive decomposition notch filter dynamic inverse controller shown in (38) yields:

[0152] (39)

[0153] in For auxiliary control signals of the roll channel, For pitch channel auxiliary control signals, Design parameters for auxiliary control signals for the yaw channel , , , , and , , , , , .

[0154] One of the key parameters affecting vibration suppression effectiveness is the quality factor q. Therefore, this study focuses on analyzing the impact of different values ​​of q on vibration suppression performance under the action of an additive decomposition notch filter dynamic inverse controller. Figure 6 It was found that the smaller the value of q, the smoother the roll velocity curve, meaning a more significant effect on vibration and noise suppression. However, as q gradually decreases, it also affects the convergence characteristics of the closed-loop system. For example, when q = 1.25, the roll velocity curve still has a steady-state error of 5.294 ohms from the equilibrium point after approximately 10 seconds of simulation. This is because an excessively small q value severely impacts the stability of the closed-loop system. Therefore, considering both vibration and noise suppression effectiveness and state convergence speed, the final quality factor q was set to 2.5.

[0155] Example 2

[0156] A rotary-wing UAV attitude vibration suppression control system based on additive decomposition notch filtering, such as Figure 1 As shown, the system includes:

[0157] The vibration damping dynamics model module is used to obtain the vibration damping dynamics model of the quadrotor by introducing the blade eccentricity model of the unbalanced mass model into the basic dynamics model of the quadrotor.

[0158] This embodiment uses a general quadrotor dynamics model. Based on this, a blade eccentricity model including an unbalanced mass model is introduced to further quantify the mechanical response caused by the vibration of the blade eccentricity. Finally, the two models are integrated and deformed to form the final vibration damping dynamics model of the quadrotor for controller design.

[0159] The fundamental dynamic model is specifically as follows:

[0160] (40)

[0161] in, The coordinates are along the z-axis. for The derivative; The velocity along the z-axis, for The derivative of g, where g is the acceleration due to gravity. For total mass, For the actual total thrust, Let be the attitude angle vector, where For the roll attitude angle, The pitch angle, Where yaw is the attitude angle, and T is the transpose. for The derivative, For the transformation matrix, It is the angular velocity vector. The angular velocity of the rolling attitude. The pitch attitude angular velocity, The yaw attitude angular velocity, for The derivative, For predictive moment of inertia matrix, Represents a diagonal matrix. Let x be the moment of inertia along the x-axis. Let be the moment of inertia along the y-axis. Let z be the moment of inertia along the z-axis. for The inverse matrix, This represents the true attitude torque vector. For the actual rolling torque, For the actual pitch moment, For the actual yaw moment, This is the gyroscopic torque.

[0162] Transformation matrix W Defined as:

[0163] (41)

[0164] Gyro torque Defined as:

[0165] (42)

[0166] in, Represents the unit vector along the z-axis. This represents the total moment of inertia of the motor rotor and propeller. For propeller i angular velocity of rotation;

[0167] Due to manufacturing limitations or installation errors, the center of mass of the propeller is often not collinear with the motor shaft. Therefore, the propeller experiences unbalanced rotation, causing severe vibrations when the quadcopter hovers. Figure 2 As shown, the edge of the right propeller is damaged, therefore the mass of the left propeller is greater than the mass of the right propeller.

[0168] For ease of analysis, let's assume that this damage would add an extra mass to the left side of the propeller. And its distance from the motor shaft is The added mass causes the masses of the left and right blades of the propeller to no longer be equal, resulting in an imbalance mass. Force analysis shows that this imbalance mass generates a centripetal force. This centripetal force produces a high-frequency vibration, which in turn affects the measurement accuracy of the airborne gyroscope. Figure 3 A simplified structural diagram of an X-type quadcopter is given, in which each blade is assumed to have a mass of... , Based on the first The centripetal force generated by an unbalanced propeller during rotation is used to construct the following propeller blade eccentricity model:

[0169] (43)

[0170] in, The distance between the additional mass and the motor shaft. Indicates time, Represents propeller i The initial turning angle, For propeller i rotational angular velocity, t This is a time variable. Ultimately, the generated total centripetal force will produce a control allocation matrix related to the vibration:

[0171] (44)

[0172] in:

[0173] , , , ; Indicates the initial rotation angle of the blade; Indicates the radius of the rack. Represents propeller i Distance to the base, such as Figure 2 As shown.

[0174] True total thrust and the true attitude torque vector Combined into a total control vector It can be defined as:

[0175] (45)

[0176] in, The original control allocation matrix of a quadcopter can be defined in the following form:

[0177] (46)

[0178] in, Indicates the thrust coefficient; The torque coefficient is shown in the diagram below. Figure 4As shown, the height channel uses classic (proportional-integral-derivative) PID technology to design the actual total thrust. The design process is omitted. The following mainly introduces how to apply additive decomposition notch filtering technology to the design process of attitude damping controllers for real quadcopters.

[0179] Next, the blade eccentricity model (43) is regarded as a disturbance and further integrated into the basic dynamic model of the quadcopter, thus forming the vibration damping dynamic model of the quadcopter. The specific construction process is divided into the following steps.

[0180] Taking the roll channel as an example (the modeling process for the pitch and yaw channels is similar), we study its linear dynamic model under hovering conditions:

[0181] (47)

[0182] in, This represents the disturbance torque component generated by the blade eccentricity model in the roll channel. This represents the dominant frequency. For clarity, let:

[0183] (48)

[0184] The linear dynamic model (47) can be further rewritten as:

[0185] (49)

[0186] Using the same approach, the dynamic equations for the pitch and yaw channels can be described as follows:

[0187] (50)

[0188] ;

[0189] in, , These represent the disturbance torque components generated by the blade eccentricity model in the pitch and yaw channels, respectively.

[0190] Integrating the linearized dynamics (49) and (50) yields:

[0191] (51)

[0192] in , , ,symbol Let represent a diagonal matrix. Next, to represent the uncertain components in the true inertia matrix, it can be expressed in the following form:

[0193] (52)

[0194] in The inertia matrix represents the precise measurement. This represents the remaining uncertain inertia matrix. Let (52) represent the identity matrix. Substituting (52) into (51) yields...

[0195] (53)

[0196] Next, the pole placement technique is used to transform the unstable state matrix. Transform into a stable state matrix The role of node configuration technology is to find a gain matrix. The following equation applies to the relationship between the stable state matrix and the unstable state matrix:

[0197] (54)

[0198] Introduce state vectors in (54) This can then be further expressed as

[0199] (55)

[0200] Substituting (55) into (53) yields the vibration damping dynamics model for the multi-rotor unmanned aerial vehicle:

[0201] (56)

[0202] in, This represents the total disturbance, which includes both the disturbance component from the blade eccentricity model and the disturbance component from the uncertain inertia. This indicates the need for further design of an additive decomposition notch filter dynamic inverse controller, in which... For auxiliary control signals of the roll channel, For pitch channel auxiliary control signals, This serves as an auxiliary control signal for the yaw channel. Clearly, the gain matrix can be accurately obtained through pole placement techniques. The specific value, on the other hand It is an inertial matrix that can be accurately measured. Therefore, all that is needed afterward is to complete... The design work allows us to deduce the true attitude torque vector. The specific expression is used to control the attitude of the drone.

[0203] The output vector construction module is used to construct the output vector based on the designed output matrix and the state variables in the vibration damping dynamics model.

[0204] Output redefinition technique indicates the steady-state matrix The following equation must be satisfied:

[0205] (57)

[0206] Among them, the output matrix , Each represents a constant parameter vector that needs to be designed; Represents a known diagonal matrix ; , and yes The feature values ​​in the vector are then used. Next, the output vector can be defined as follows:

[0207] (58)

[0208] The dynamic model derivative model is used to calculate the first derivative dynamics of the output vector along the vibration suppression dynamic model.

[0209] First, taking the derivative of equation (58) yields the first-order derivative dynamics of the output vector:

[0210] (59)

[0211] Then, substituting the vibration damping dynamics model (56) into (59) yields:

[0212] (60)

[0213] Next, using equation (57), (60) can be transformed into the following form:

[0214] (61)

[0215] Finally, substituting (58) into (61) yields the first derivative dynamics of the output vector along the damping dynamics model (56):

[0216] (62)

[0217] in .

[0218] The controller design module is used to dynamically design a traditional additive decomposition dynamic inverse controller based on the first derivative, and uses a quality factor of 1. and dominant frequency Notch filter The filter in the traditional additive decomposition dynamic inverse controller is improved to obtain an additive decomposition notch filter dynamic inverse controller, wherein, This is the Laplace transform symbol.

[0219] First, for the first derivative dynamics (62), the following main system dynamics can be designed:

[0220] (63)

[0221] in, Represents the state variables of the main system. express The derivative of the first derivative. Then, subtracting the main system (63) from the first derivative dynamic (62) yields:

[0222] (64)

[0223] Next, assume the state variables in the auxiliary system Therefore, equation (64) can be expressed as the dynamics of the auxiliary system:

[0224] (65)

[0225] make Then, output vector y Laplace transform It can also be represented as:

[0226] (66)

[0227] in, s The Laplace transform symbol, yes Laplace transform, yes Laplace transform.

[0228] Next, performing a Laplace transform on the main system dynamics (63) yields the following results: and attitude torque auxiliary control signal Transfer function between :

[0229] (67)

[0230] in, It is a third-order identity matrix. Representing the Laplace transform symbol. Using the transfer function. , It can be transformed into the following form:

[0231] (68)

[0232] in, It is an additive decomposition notch filter dynamic inverse controller u Laplace transform.

[0233] Substituting (68) into (66), the Laplace transform of the output vector can also be expressed as:

[0234] (69)

[0235] Next, a general form of an additive decomposition dynamic inverse controller is given:

[0236] (70)

[0237] The superscript "-1" indicates The reverse, This refers to the coupling filter, which needs to be designed specifically for the different types of disturbances encountered by the quadcopter during mission execution. (69) Use (70) After replacement, we can get:

[0238] (71)

[0239] Taking the Laplace transform of the output vector yields:

[0240] (72)

[0241] Substituting (72) into (71) yields: Another way to express it:

[0242] (73)

[0243] Substituting (73) into (70) yields:

[0244] (74)

[0245] When only slow time-varying disturbances exist, a positive design parameter can be selected as follows: low-pass filter . (74) use After the replacement, a traditional additive state decomposition dynamic inverse controller of the following type can be designed:

[0246] (75)

[0247] However, in practical applications, quadcopters not only have low-frequency disturbances, but also specific dominant frequencies related to the propeller angular velocity. The vibration components. Therefore, a notch filter is used to improve the filter in the traditional additive decomposition dynamic inverse controller, that is, the present invention adopts the following type of coupled notch filter:

[0248] (76)

[0249] in, It is a notch filter. This notch filter has two design parameters, namely... Indicates the quality factor. Indicates the dominant frequency. When the general filter in (74)... Coupled Filter After replacement, the additive state decomposition notch dynamic inverse controller can be obtained:

[0250] (77)

[0251] Vibration control component .

[0252] It is worth noting that the decision to introduce a vibration control component can be made based on the characteristics of the external disturbance (vibration and noise). If the relevant sensors or detection equipment do not capture vibrations of a specific frequency, then in this case, it is possible to... Set to zero, that is It doesn't work.

[0253] The attitude damping control module is used to control the attitude damping of a rotary-wing UAV using the additive decomposition notch dynamic inverse controller.

[0254] In this embodiment, simulation and real machine experiments are used to demonstrate that the proposed control algorithm has a stronger vibration suppression capability compared with the traditional additive state decomposition dynamic inverse controller.

[0255] The entire experimental platform consists of two parts: software-in-the-loop simulation and flight experiments. In the controller design phase, a notch filter controller model and a complete quadcopter dynamics model were constructed using Simulink software. Then, software-in-the-loop simulation was performed to initially adjust the control parameters. Next, the controller in Simulink was converted to C++. Finally, flight experiments were conducted to verify the vibration suppression capability of the proposed control algorithm. Figure 5As shown, this experiment uses an X-type quadcopter as the controlled object, with a laser mounted above it. The laser beam is projected onto a target. When the quadcopter is hovering, even a slight vibration in its attitude will cause a noticeable displacement of the red dot on the target, thus helping to quantify the intensity of the vibration. The inherent parameters of the quadcopter involved in the experiment can be obtained from Table 2.

[0256] Table 2

[0257]

[0258] Disassembling the additive decomposition notch dynamic inverse controller shown in (77) yields:

[0259] (78)

[0260] in For auxiliary control signals of the roll channel, For pitch channel auxiliary control signals, Design parameters for auxiliary control signals for the yaw channel , , , , and , , , , , .

[0261] One of the key parameters affecting vibration suppression effectiveness is the quality factor q. Therefore, this study focuses on analyzing the impact of different values ​​of q on vibration suppression performance under the action of an additive decomposition notch filter dynamic inverse controller. Figure 6 It was found that the smaller the value of q, the smoother the roll velocity curve, meaning a more significant effect on vibration and noise suppression. However, as q gradually decreases, it also affects the convergence characteristics of the closed-loop system. For example, when q = 1.25, the roll velocity curve still has a steady-state error of 5.294 ohms from the equilibrium point after approximately 10 seconds of simulation. This is because an excessively small q value severely impacts the stability of the closed-loop system. Therefore, considering both vibration and noise suppression effectiveness and state convergence speed, the final quality factor q was set to 2.5.

[0262] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made to the technical solutions of the present invention by those skilled in the art without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A method for attitude and vibration suppression control of a rotary-wing UAV based on additive decomposition notch filtering, characterized in that, The method includes: Step S1: Based on the basic dynamic model of the quadrotor, introduce the blade eccentricity model of the unbalanced mass model to obtain the vibration damping dynamic model of the quadrotor. Step S2: Construct the output vector based on the designed output matrix and the state variables in the vibration damping dynamics model; Step S3: Calculate the first derivative dynamics of the output vector along the vibration suppression dynamic model; Step S4: Based on the first derivative, dynamically design a traditional additive decomposition dynamic inverse controller, and use a quality factor of... and dominant frequency Notch filter The filter in the traditional additive decomposition dynamic inverse controller is improved to obtain an additive decomposition notch filter dynamic inverse controller, wherein, The symbol for the Laplace transform; Step S5: Use the additive decomposition notch dynamic inverse controller to control the attitude vibration suppression of the rotary-wing UAV; In step S1, the basic dynamic model is specifically as follows: ； in, The coordinates are along the z-axis. for The derivative; The velocity along the z-axis, for The derivative of g, where g is the acceleration due to gravity. For total mass, For the actual total thrust, Let be the attitude angle vector, where For the roll attitude angle, The pitch angle, Where yaw is the attitude angle, and T is the transpose. for The derivative, For the transformation matrix, It is the angular velocity vector. The angular velocity of the rolling attitude. The pitch attitude angular velocity, The yaw attitude angular velocity, for The derivative, For predictive moment of inertia matrix, Represents a diagonal matrix. Let x be the moment of inertia along the x-axis. Let be the moment of inertia along the y-axis. Let z be the moment of inertia along the z-axis. for The inverse matrix, This represents the true attitude torque vector. For the actual rolling torque, For the actual pitch moment, For the actual yaw moment, This refers to the gyroscopic torque. In step S1, the blade eccentricity model of the unbalanced mass model is specifically as follows: When the masses of the two blades of a propeller are not equal, an extra mass appears on one side. At this time, the propeller generates centripetal force, and an eccentric model of the propeller blades is constructed based on the centripetal force. : ； in, The distance between the additional mass and the motor shaft. Indicates time, Indicates propeller i The initial turning angle, For propeller i angular velocity of rotation.

2. The attitude and vibration suppression control method for rotary-wing UAVs based on additive decomposition notch filtering according to claim 1, characterized in that, In step S1, the vibration damping dynamics model of the quadcopter aircraft Specifically: ； in, A The state matrix; B For control matrix; For the state variables of a quadcopter, For additive decomposition notch filter dynamic inverse controller, This represents the total disturbance.

3. The attitude and vibration suppression control method for rotary-wing UAVs based on additive decomposition notch filtering according to claim 2, characterized in that, In step S2, the output vector is specifically: ； in, This is the output vector; is the output matrix; T is the transpose.

4. The attitude and vibration suppression control method for rotary-wing UAVs based on additive decomposition notch filtering according to claim 3, characterized in that, In step S3, the first derivative dynamics of the vibration suppression dynamic model are specifically as follows: ； in, for The derivative, Given a diagonal matrix; This is an intermediate transition vector. .

5. The attitude and vibration suppression control method for rotary-wing UAVs based on additive decomposition notch filtering according to claim 4, characterized in that, In step S4, the traditional additive decomposition dynamic inverse controller for: ； in, Positive design parameters; Let be the state variables of the quadcopter described in the Lagrange domain.

6. The attitude and vibration suppression control method for rotary-wing UAVs based on additive decomposition notch filtering according to claim 5, characterized in that, In step S4, the additive decomposition notch dynamic inverse controller u Specifically: ； Among them, vibration control component .

7. The attitude and vibration suppression control method for rotary-wing UAVs based on additive decomposition notch filtering according to claim 6, characterized in that, Based on the first derivative, a traditional additive decomposition dynamic inverse controller is designed, and a quality factor of is used. and dominant frequency Notch filter The filter in the traditional additive decomposition dynamic inverse controller is improved to obtain an additive decomposition notch dynamic inverse controller. The specific content is as follows: ； in, It is an identity matrix.

8. A rotary-wing unmanned aerial vehicle (UAV) attitude and vibration suppression control system based on additive decomposition notch filtering, the system being used to implement the method described in any one of claims 1-7, characterized in that the system... include: The vibration damping dynamics model module is used to introduce the blade eccentricity model of the unbalanced mass model into the basic dynamics model of the quadcopter to obtain the vibration damping dynamics model of the quadcopter. The output vector construction module is used to construct the output vector based on the designed output matrix and the state variables in the vibration damping dynamics model. A dynamic model differentiation model is used to calculate the first derivative dynamics of the output vector along the vibration suppression dynamic model; The controller design module is used to dynamically design a traditional additive decomposition dynamic inverse controller based on the first derivative, and uses a quality factor of 1. and dominant frequency Notch filter The filter in the traditional additive decomposition dynamic inverse controller is improved to obtain an additive decomposition notch filter dynamic inverse controller, wherein, The symbol for the Laplace transform; The attitude damping control module is used to control the attitude damping of a rotary-wing UAV using the additive decomposition notch dynamic inverse controller.

Images