Gain adaptive folding variant unmanned aerial vehicle control system and control method based on folding angle detection

Abstract

The invention discloses a gain adaptive folding variant unmanned aerial vehicle control system and control method based on folding angle detection, and relates to the field of flight control. The problem that the model of the folding type variant unmanned aerial vehicle changes irregularly in the process of changing the wing folding angle in the air is solved, and the safety and smoothness of wing folding angle change control of the folding type variant unmanned aerial vehicle are improved. According to the method, the wing folding angle and the control surface dynamic pressure are firstly detected, the real-time rotational inertia and the real-time control surface dynamic pressure of the aircraft are calculated, then the gain of the output quantity of the controller is adjusted according to the rotational inertia and the control surface dynamic pressure, and the output quantity is compensated and corrected. According to the method, the problem that the model changes irregularly in the process of changing the wing folding angle of the folding type variant unmanned aerial vehicle in the air is well solved, and the safety and smoothness of wing folding angle change control of the folding type variant unmanned aerial vehicle are improved.

Description

Technical Field

[0001] The present invention relates to the field of flight control. Background Art

[0002] A foldable drone is an aircraft with wings that can be deployed and folded. When deployed, the wings have a longer span, resulting in lower fuel consumption. When folded, the wings have a shorter span, allowing for a higher maximum speed. By deploying and folding the wings, this type of aircraft can adapt to a wider range of mission requirements. However, during the wing deployment and folding process, the aircraft's moment of inertia and aerodynamic shape change, resulting in different roll accelerations for control surfaces with the same deflection angle. This can cause large control errors or even uncontrollable aircraft.

[0003] Traditional control systems and control methods use multiple sets of control modes and parameters, and divide the wing folding angle into multiple segments. When the wing folding angle reaches a certain value, it directly switches to the corresponding control mode and parameters. However, the control of multiple modes and parameters is discontinuous, and problems are prone to occur during switching, and it lacks good versatility and robustness.

[0004] Therefore, designing a control system and control method that can adaptively adjust control parameters according to the wing folding angle and aircraft state to improve the stable control of the entire process of folding variant drones has always been a technical problem to be solved by technical personnel in this field. Summary of the Invention

[0005] In response to the above problems, the present invention proposes a gain-adaptive folding variant UAV control system and control method based on folding angle detection, which solves the problem of irregular model changes in the process of changing the folding angle of the wings of the folding variant UAV in the air, and improves the safety and smoothness of the control of the folding angle change of the wings of the folding variant UAV.

[0006] The technical solution of the present invention is: comprising the following steps:

[0007] Step 1: Obtain the wing folding angle θ and the rudder surface dynamic pressure p through the moment of inertia and rudder effect detection subsystem;

[0008] Step 2: Based on the wing folding angle θ and the dynamic pressure p of the control surface obtained in step 1, as well as the length of the folded section wing center of gravity from the rotation axis x1, x'1, x2, and the fuselage width x B The moment of inertia I of the straight line passing through the center of gravity of the folding section of the wing and parallel to the line connecting the nose and the tail of the aircraft k，1 , I k，2 , the moment of inertia of the fuselage around the pitch axis I pitch，B , the moment of inertia of the fuselage around the roll axis I roll，B , the moment of inertia of the fuselage around the yaw axis Iyaw，B , the mass of the folded wing m1, m2, the area of the control surface S, and the distance l from the control surface to the center of gravity of the aircraft when the wing is unfolded horizontally k The moment of inertia of the aircraft is calculated by the software part of the moment of inertia and rudder effect detection subsystem. k 、Aircraft control rudder arm L k and the dynamic pressure p of the control surface, where k is one of the body axis numbers pitch, roll, yaw, i.e. one of pitch, roll and yaw;

[0009] Step 3: Measure the angular velocity ω obtained from the gyroscope b And the input given angular velocity ω r , the uncompensated control quantity u is output through the angular velocity control algorithm 1k , where k is one of the body axis labels pitch, roll, yaw;

[0010] Step 4: The moment of inertia I of the aircraft obtained in step 2 k 、Aircraft control rudder arm L k and the dynamic pressure p of the control surface, and perform gain compensation on the output obtained in step 3 to obtain the appropriate control quantity u k , to achieve precise control of the aircraft; where k is one of the body axis labels pitch, roll, yaw; the control quantity u k Divided into u pitch ,u roll ,u yaw , are the final control amounts of pitch, roll and yaw after compensation.

[0011] In step 1, the wing folding angle θ and the control surface dynamic pressure p are detected by the hardware component of the moment of inertia and rudder effect detection subsystem, which includes a folding angle sensor arranged at the rotation axis between the inner wing and the fuselage, a dynamic pressure probe arranged in front of the control surface rotation axis, and a corresponding differential pressure and airspeed sensor; the dynamic pressure probe and the differential pressure and airspeed sensor are connected by an air duct;

[0012] By placing an angle sensor at the rotating shaft between the inner wing and the fuselage, the angle θ between the inner wing and the fuselage can be obtained. Moreover, since the outer wing is always parallel to the fuselage, the angle between the inner and outer wings is also θ. A dynamic pressure probe is placed in front of the rudder shaft to obtain the dynamic pressure p of the rudder surface.

[0013] The input of the software part of the moment of inertia and rudder effect detection subsystem in step 2 is the wing folding angle θ, the length of the folded section wing center of gravity from the rotation axis x1, x'1, x2, and the fuselage width x BThe moment of inertia I of the straight line passing through the center of gravity of the folding section of the wing and parallel to the line connecting the nose and the tail of the aircraft k，1 , I k，2 、Fleet moment of inertia I k，B , the folded wing masses m1, m2, the dynamic pressure on the control surface p, the control surface area S, and the distance l from the control surface to the center of gravity of the aircraft when the wing is unfolded horizontally k , the output is the moment of inertia of the aircraft I k and the aircraft control surface lever arm L k ;

[0014] The moment of inertia I of the folding section wing passing through the center of gravity of the folding section wing and parallel to the line connecting the nose and the tail of the aircraft is k，1 , I k，2 Divided into the moment of inertia around the pitch axis I pitch，1 , I pitch，2 , the moment of inertia around the roll axis I roll，1 , I roll，2 , the moment of inertia about the yaw axis I yaw，1 , I yaw，2 ;

[0015] The distance l from the control surface to the center of gravity of the aircraft when the wings are unfolded horizontally k Divided into the distance l along the x-axis pitch and the distance l along the z axis rool , while l yaw =l pitch ;

[0016] The moment of inertia I k Divided into the moment of inertia around the pitch axis I pitch , the moment of inertia around the roll axis I roll , the moment of inertia about the yaw axis I yaw ;

[0017] The aircraft control rudder arm L k The force arm L is divided into the component of the rudder aerodynamic force that causes the aircraft to produce pitch acceleration pitch , the force arm L of the component of the aerodynamic force of the control surface that causes the aircraft to produce rolling acceleration roll ;

[0018] According to the wing folding angle θ, calculate the aircraft's moment of inertia I k , the moment of inertia of the aircraft I k The calculation method is as follows:

[0019] I pitch =I pitch，B +I pitch，1 +I pitch，2

[0020]

[0021] I yaw =I yaw,B +I yaw，1 +I yaw，2 ;

[0022] Where d1 is the distance from the center of gravity of the inner folding section wing to the hinge between the inner folding section wing and the fuselage, and d2 is the distance from the center of gravity of the outer folding section wing to the hinge between the outer folding section wing and the inner folding section wing. The calculation method of d1 and d2 is as follows:

[0023]

[0024] in

[0025] According to the wing folding angle θ, calculate the aircraft control surface force arm L k , L k The specific calculation method is as follows:

[0026] L pitch =l pitch

[0027] L roll =l rool -(x1+x'1)(1+cos(θ))

[0028] L yaw =l rool ;

[0029] The input of the angular velocity control algorithm in step 3 is the measured angular velocity ω obtained by the gyroscope b And the given target angular velocity ω r , the output is the uncompensated control quantity u k , which is divided into the uncompensated pitch control quantity u pitch , the uncompensated control amount u roll , yaw uncompensated control quantity u yaw ;

[0030] The angular velocity control algorithm allows parameters to vary within a certain range. If the moment of inertia and rudder effect detection subsystem fails or measures inaccurately, as long as it can still output approximate measurement parameters in response to changes in flight status, the final wing folding control effect will not be significantly affected.

[0031] The angular velocity control algorithm may be an active disturbance rejection control algorithm, which includes an extended state observer, a tracking differentiator, state feedback, and total disturbance compensation. The steps of the active disturbance rejection control algorithm are as follows:

[0032] Step 3.1, Extended State Observer Update: According to the angular velocity ω measured by the gyroscope b The control quantity u output by the controller at the previous moment k And the moment of inertia I of the aircraft k 、Aircraft control rudder arm L k and the dynamic pressure p of the control surface, and substitute into the expanded observer to update the state quantity z jk , where j is the order of the state quantity corresponding to each body axis k, where k is one of the body axis labels pitch, roll, yaw;

[0033] Step 3.2, arrange the wing folding process: use the tracking differentiator to input the given angular velocity ω r Perform preprocessing to plan a uniform acceleration and deceleration process for the step change of the given angular velocity, and output the preprocessed given angular velocity and angular acceleration v 1k , v 2k , which is divided into angular velocity and angular acceleration v around the pitch axis 1pitch , v 2pitch , angular velocity and angular acceleration v around the roll axis 1roll , v 2roll , angular velocity and angular acceleration v around the yaw axis 1yaw , v 2yaw ;

[0034] Step 3.3, calculate state feedback: output state quantity z according to the extended state observer described in step 3.1 1k , z 2k , with the given angular velocity and given angular acceleration v described in step 3.2 1k , v 2k , substituting into the state feedback law, we can get the ideal control quantity u designed for the pure integral model 1k , which is divided into the ideal control quantity u for pitch 1pitch , the ideal control quantity u of roll 1roll , the ideal control quantity u of yaw 1yaw ;

[0035] Step 3.4: Total disturbance compensation: The estimated value z of the equivalent total disturbance output by the extended state observer described in step 3.1 is 3k , which is divided into an estimate of the equivalent total disturbance z around the pitch axis 3pitch , an estimate of the equivalent total disturbance around the roll axis z 3roll , an estimate of the equivalent total disturbance z around the yaw axis 3yaw ;

[0036] Then, the ideal control quantity described in step 3.3 is subjected to disturbance compensation to make the actual dynamics of the aircraft approach the ideal pure integral model u 1k, which is divided into the uncompensated pitch control amount u 1pitch , uncompensated roll control amount u 1roll , uncompensated yaw control value u 1yaw .

[0037] The gain compensation method described in step 4 is as follows:

[0038]

[0039] in:

[0040]

[0041] where u pitch ,u roll ,u yaw are the final control amounts of pitch, roll and yaw after compensation, u 1pitch ,u 1roll ,u 1yaw are the uncompensated control quantities;

[0042] Then distribute the obtained control quantity to obtain the control signal servo of the left servo L , and the control signal of the right servo R :

[0043]

[0044] Among them servo trimL , servo trimR They are the neutral point zero values of the left and right servos respectively, which are obtained by on-site adjustment during aircraft assembly.

[0045] The present invention first detects the wing folding angle and control surface dynamic pressure, calculates the aircraft's real-time moment of inertia and control surface dynamic pressure, and then adjusts the controller output gain based on the moment of inertia and control surface dynamic pressure to compensate for the output. This invention effectively solves the problem of irregular model changes during mid-flight wing folding angle changes in foldable UAVs, improving the safety and smoothness of folding angle control for foldable UAVs.

[0046] Compared to existing technologies, the present invention can better control the attitude angle and angular velocity of a foldable UAV during the wing folding process, avoiding overshoot or undershoot. It also ensures that if the input angular acceleration requirements are the same, the actual angular acceleration generated by the aircraft's control surfaces is the same. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 This is a block diagram of the gain adaptive control method for folding angle detection;

[0048] Figure 2 It is a schematic diagram of the coordinate axes of the aircraft body;

[0049] Figure 3 It is a schematic diagram of aircraft size parameters;

[0050] Figure 4 This is a schematic diagram of the side installation method of a single dynamic pressure probe;

[0051] Figure 5 This is the block diagram of the one-dimensional second-order active disturbance rejection control algorithm;

[0052] Figure numerals: 1 left servo, 2 right servo, 3 left inner wing, 4 right inner wing, 5 left outer wing, 6 right outer wing, 7 left rudder, 8 right rudder, 9 fuselage, 10 wing folding mechanism; a, moment of inertia and rudder effect detection subsystem; a1, moment of inertia and rudder effect detection hardware part; a2, moment of inertia and rudder effect detection software part; b, angular velocity control algorithm; c, control quantity gain adjustment. DETAILED DESCRIPTION

[0053] In order to clearly illustrate the technical features of this patent, this patent is described in detail below through specific implementation methods and in combination with its accompanying drawings.

[0054] The present invention proposes a gain-adaptive foldable variant UAV control system and control method based on folding angle detection. The foldable variant UAV mainly consists of 1 left servo, 2 right servo, 3 left inner wing, 4 right inner wing, 5 left outer wing, 6 right outer wing, 7 left rudder, 8 right rudder, 9 fuselage, and 10 wing folding mechanism.

[0055] like Figure 1 As shown, first, an angle sensor is arranged at the wing folding mechanism to obtain the wing folding angle, and a dynamic pressure detection sensor is arranged in front of the rudder surface rotation shaft to obtain rudder surface dynamic pressure data; based on the wing folding angle data obtained by the angle sensor and the rudder surface dynamic pressure data obtained by the dynamic pressure detection sensor, the aircraft's moment of inertia and the aircraft's rudder surface control arm are calculated; based on the measured angular velocity obtained by the gyroscope and the input given angular velocity, a control variable is output through a control algorithm; based on the aircraft's moment of inertia and the aircraft's rudder surface control arm, a gain compensation is performed on the output variable to obtain an appropriate rudder deflection, thereby achieving precise control of the aircraft;

[0056] In order to facilitate the subsequent algorithm description, the body coordinate axis is defined as needed, as follows Figure 2 shown.

[0057] The following is a specific implementation step.

[0058] Step 1: Obtain the wing folding angle and rudder surface dynamic pressure data through the hardware part of the moment of inertia and rudder effect detection subsystem;

[0059] like Figure 3 As shown in the figure, the foldable variant UAV can keep the outer wing parallel to the fuselage during the wing folding process. It only needs to arrange an angle sensor at the rotation axis of the inner wing and the fuselage to obtain the angle θ between the inner wing and the fuselage. Since the outer wing is always parallel to the fuselage, the angle between the inner wing and the outer wing can be obtained as θ.

[0060] like Figure 4 The dynamic pressure probe in front of the rudder shaft is shown, and the dynamic pressure of the rudder surface is obtained as p.

[0061] The remaining input parameters are the length of the folded section wing center of gravity from the axis of rotation x1, x'1, x2, the fuselage width x B The moment of inertia I of the straight line passing through the center of gravity of the folding section of the wing and parallel to the line connecting the nose and the tail of the aircraft k，1 , I k，2 (divided into the moment of inertia around the pitch axis I pitch1 , I pitch，2 , the moment of inertia around the roll axis I roll，1 , I roll，2 , the moment of inertia about the yaw axis I yaw，1 , I yaw，2 ), the moment of inertia of the fuselage around the pitch axis I pitch，B , the moment of inertia of the fuselage around the roll axis I roll，B , the moment of inertia of the fuselage around the yaw axis I yaw，B , the folded wing mass m1, m2, the control surface area S, and the distance l from the control surface to the center of gravity of the aircraft when the wing is unfolded horizontally k (divided into the distance l along the x-axis pitch and the distance l along the z axis rool , while l yaw =l pitch ) The foldable variant drone can be measured before the control system is used. The length of the folding section wing center of gravity from the rotation axis is x1, x'1, x2, and the fuselage width is x B , the distance from the control surface to the center of gravity of the aircraft when the wings are unfolded horizontally k (divided into the distance l along the x-axis pitch and the distance l along the z axis rool , while l yaw =l pitch ) is shown in the measurement diagram Figure 3 shown.

[0062] Step 2: Calculate the average rudder surface dynamic pressure through the software part of the moment of inertia and rudder efficiency detection subsystem;

[0063] According to the wing folding angle θ, calculate the aircraft's moment of inertia I k (divided into the moment of inertia around the pitch axis I pitch , the moment of inertia around the roll axis I roll , the moment of inertia about the yaw axis I yaw ), the moment of inertia of the aircraft I k The calculation method is as follows:

[0064] I pitch =I pitch，B +I pitch，1 +I pitch，2

[0065]

[0066] I yaw =I yaw，B +I yaw，1 +I yaw，2 ;

[0067] where d i is the distance from the center of gravity of the folded wing to the straight line connecting the nose and tail, d i The calculation method is as follows:

[0068]

[0069] in

[0070] According to the wing folding angle θ, calculate the aircraft control surface force arm L k (divided into the force arm L of the component of the rudder aerodynamic force that causes the aircraft to produce pitch acceleration pitch , the force arm L of the component of the aerodynamic force of the control surface that causes the aircraft to produce rolling acceleration roll ), L k The specific calculation method is as follows:

[0071] L pitch =l pitch

[0072] L roll =l rool -(x1+x'1)(1+cos(θ))

[0073] L yaw =l rool .

[0074] Step 3: Use the active disturbance rejection control algorithm to obtain the uncompensated control quantity;

[0075] As Figure 5 Taking the one-dimensional second-order active disturbance rejection control algorithm block diagram shown in the figure as an example, a specific angular velocity control algorithm is designed.d is the reference input signal, ν1 is the reference signal processed by the tracking differentiator, e is the state error, u0 is the ideal control quantity, u is the actual control quantity after disturbance compensation, b0 is the gain coefficient estimate of the control quantity, w is the external disturbance, y is the controlled quantity, is the measured filtered value of the controlled quantity, which is obtained by the measurement filtering system by default. z1 and z2 are the observation quantities corresponding to the state quantities. z3 is the expanded state observation quantity, which is used to track and estimate other internal and external disturbance forces at the input end except the control force.

[0076] According to the model, the expanded state observer on the three body axes is designed. Before that, the fal function used is explained below:

[0077] fal is a nonlinear function, and its specific expression is:

[0078]

[0079] a is the nonlinear factor, and δ is the filter factor. The fal function is constructed by segmenting a proportional function and an exponential function. The power a of the exponential function is generally between 0 and 1. The line x = δ truncates the exponential function, retaining the portion where x > δ. The function graph is then symmetrical about the origin to form the fal function. The linear interval width range is ±δ.

[0080] Therefore, the process of calculating the control quantity output by the active disturbance rejection control algorithm is:

[0081] Step 3.1: First, construct a three-axis extended state observer and update the extended state observer: according to the angular velocity measured by the gyroscope, the control variable output by the controller at the previous moment, and the average rudder surface dynamic pressure, substitute them into the expanded observer model to update the state;

[0082] The x-axis is:

[0083]

[0084] where e x is the angular velocity observer error of the x-axis, z 1x , z 2x are the x-axis angular velocity ω x and its rate of change The corresponding observation, z 3x is the observed value of the equivalent total disturbance on the x-axis, β 01x , β 02x , β 03x is the x-axis observer gain coefficient, f 1x is the second-order coefficient of the x-axis angular velocity observer, p is the dynamic pressure of the rudder surface, b 1x is the pitch control coefficient.

[0085] The y-axis is:

[0086]

[0087] where e y is the angular velocity observer error of the y-axis, z 1y , z 2y are the y-axis angular velocity ω y and its rate of change The corresponding observation, z 3y is the observed value of the equivalent total disturbance on the y-axis, β 01y , β 02y , β 03y is the y-axis observer gain coefficient, f 1y is the second-order coefficient of the y-axis angular velocity observer, p is the dynamic pressure of the control surface, b 1y is the yaw control coefficient.

[0088] The z-axis is:

[0089]

[0090] where e z is the angular velocity observer error of the z-axis, z 1z , z 2z are the z-axis angular velocity ω z and its rate of change The corresponding observation, z 3z is the observed value of the equivalent total disturbance on the z axis, β 01z ,β 02z , β 03z is the z-axis observer gain coefficient, f 1z is the second-order coefficient of the z-axis angular velocity observer, p is the dynamic pressure of the rudder surface, b 1z is the roll control coefficient.

[0091] Step 3.2, arrange the wing folding process: use the tracking differentiator to input the given angular velocity ω r Perform preprocessing to plan a uniform acceleration and deceleration process for the step change of the given angular velocity, and output the preprocessed given angular velocity and angular acceleration;

[0092] According to the maneuverability of the aircraft, tracking differentiators on the three body axes are designed. The differential tracker constructed by the fhan function is used as the feedforward link to preprocess the input signal and extract the processed differential signal in order to design the second-order state feedback link and obtain better input response dynamics.

[0093]

[0094] where ω rx ,ω ry ,ωrz is the three-axis angular velocity of the given input, v 1x , v 1y , v 1z and v 2z , ν 2y , v 2z is the target angular velocity ω rx ,ω ry ,ω rz The command signal after pre-planning and the rate of change of the command signal, r tdx , r tdy , r tdz and h tdx , h tdy , h tdz The parameters of the fastest integrated function fhan in the tracking differentiator are as follows. The fastest integrated function fhan(x1, x2, r, h) defines a two-input nonlinear function, and its operation process is as follows:

[0095]

[0096] Where d, d0, y, a0, and a are temporary variables in the calculation process, r and h are function parameters, fh is the function output value, and x1 and x2 are input values. sign(x) is the sign function, and the expression is:

[0097]

[0098] Step 3.2. Calculate the state feedback: Substitute the angular velocity and angular acceleration output by the extended state observer described in step 3.1 and the preprocessed given angular velocity and given angular acceleration described in step 3.2 into the state feedback rate to obtain the ideal control variable designed for the pure integral model;

[0099] Use the fhan function to design the nonlinear feedback of the three body axes and obtain the ideal control quantity u of the three axes 0x ,u oy ,u 0z :

[0100]

[0101] where c x , c y , c z is the damping coefficient of state feedback, r fbx r fby , r fbz and h fbx , h fby , h fbz is the parameter of the fastest synthesis function fhan in state feedback.

[0102] Step 3.4, total disturbance compensation: Based on the estimated equivalent total disturbance output by the extended state observer described in step 3.1, disturbance compensation is performed on the ideal control variable described in step 3.3, so that the actual dynamics of the aircraft approach the ideal pure integral model;

[0103] Compensate the total disturbance of the three axes and obtain the control amount u of the x-axis, y-axis and z-axis without gain compensation 1x 、u 1y and u 1z :

[0104]

[0105] where z 2x is the rate of change of angular velocity along the x-axis The corresponding observation, z 3x is the observed value of the equivalent total disturbance on the x-axis, f 1x is the second-order coefficient of the x-axis angular velocity observer, b 1x is the pitch control coefficient; z 2y is the rate of change of y-axis angular velocity The corresponding observation, z 3y is the observed value of the equivalent total disturbance on the y-axis, f 1y is the second-order coefficient of the y-axis angular velocity observer, b 1y is the yaw control coefficient; z 2z is the rate of change of angular velocity along the z axis The corresponding observation, z 3z is the observed value of the equivalent total disturbance on the z axis, f 1z is the second-order coefficient of the z-axis angular velocity observer, b 1z is the rolling control coefficient;

[0106] Step 4: Perform gain compensation on the control amount and distribute the control amount;

[0107] The gain compensation method is as follows:

[0108]

[0109] in:

[0110]

[0111] where u x ,u y ,u z are the final pitch, yaw and roll control values after compensation, u 1x ,u 1y ,u 1z are the uncompensated pitch, yaw, and roll control quantities, respectively.

[0112] Then distribute the obtained control quantity to obtain the control signal servo of the left servo 2 L , and the control signal of right servo 3 R :

[0113]

[0114] Among them servo trimL , servo trimR They are the neutral point zero values of the left and right servos respectively, which are obtained by on-site adjustment during aircraft assembly.

[0115] There are many specific implementation ways of the present invention. The above is only the preferred implementation method of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements can be made without departing from the principles of the present invention. These improvements should also be considered as the scope of protection of the present invention.

Claims

1. A gain-adaptive foldable variant UAV control method based on folding angle detection, characterized in that: It is divided into the following steps: Step 1: Obtain the wing folding angle θ and the rudder surface dynamic pressure p through the moment of inertia and rudder effect detection subsystem; Step 2: Based on the wing folding angle θ and the dynamic pressure p of the control surface obtained in step 1, as well as the length of the folded section wing center of gravity from the rotation axis x1, x'1, x2, and the fuselage width x B The moment of inertia I of the straight line passing through the center of gravity of the folding section of the wing and parallel to the line connecting the nose and the tail of the aircraft k，1 ,I k，2 , the moment of inertia of the fuselage around the pitch axis I pitch，B , the moment of inertia of the fuselage around the roll axis I roll，B , the moment of inertia of the fuselage around the yaw axis I yaw，B , the folded wing mass m1, m2, the control surface area S, and the distance l from the control surface to the center of gravity of the aircraft when the wing is unfolded horizontally k The moment of inertia of the aircraft is calculated by the software part of the moment of inertia and rudder effect detection subsystem. k 、Aircraft control rudder arm L k and the dynamic pressure p of the control surface, where k is one of the body axis numbers pitch, roll, yaw, i.e. one of pitch, roll and yaw; Step 3: Measure the angular velocity ω obtained from the gyroscope b And the input given angular velocity ω r , the uncompensated control quantity u is output through the angular velocity control algorithm 1k , where k is one of the body axis labels pitch, roll, yaw; Step 4: The moment of inertia I of the aircraft obtained in step 2 k 、Aircraft control rudder arm L k and the dynamic pressure p of the control surface, and perform gain compensation on the output obtained in step 3 to obtain the appropriate control quantity u k , to achieve precise control of the aircraft; where k is one of the body axis labels pitch, roll, yaw; the control quantity u k Divided into u pitch ,u roll ,u yaw , are the final control amounts of pitch, roll and yaw after compensation.

2. The method for controlling a foldable variant UAV with adaptive gain based on folding angle detection according to claim 1, characterized in that: In step 1, the wing folding angle θ and the control surface dynamic pressure p are detected by the hardware component of the moment of inertia and rudder effect detection subsystem, which includes a folding angle sensor arranged at the rotation axis between the inner wing and the fuselage, a dynamic pressure probe arranged in front of the control surface rotation axis, and a corresponding differential pressure and airspeed sensor; the dynamic pressure probe and the differential pressure and airspeed sensor are connected by an air duct; By placing an angle sensor at the rotating shaft between the inner wing and the fuselage, the angle θ between the inner wing and the fuselage can be obtained. Moreover, since the outer wing is always parallel to the fuselage, the angle between the inner and outer wings is also θ. A dynamic pressure probe is placed in front of the rudder shaft to obtain the dynamic pressure p of the rudder surface.

3. The method for controlling a foldable variant UAV with adaptive gain based on folding angle detection according to claim 1, characterized in that: The input of the software part of the moment of inertia and rudder effect detection subsystem in step 2 is the wing folding angle θ, the length of the folded section wing center of gravity from the rotation axis x1, x'1, x2, and the fuselage width x B The moment of inertia I of the straight line passing through the center of gravity of the folding section of the wing and parallel to the line connecting the nose and the tail of the aircraft k，1 , I k，2 、Fleet moment of inertia I k，B , the mass of the folded wing m1, m2, the dynamic pressure p on the control surface, the area S of the control surface, and the distance l from the control surface to the center of gravity of the aircraft when the wing is unfolded horizontally k , the output is the moment of inertia of the aircraft I k and the aircraft control surface lever arm L k ; The moment of inertia I of the folding section wing passing through the center of gravity of the folding section wing and parallel to the line connecting the nose and the tail of the aircraft is k,1 ,I k,2 Divided into the moment of inertia around the pitch axis I pitch,1 ,I pitch,2 , the moment of inertia around the roll axis I roll，1 , I roll，2 , the moment of inertia around the yaw axis I yaw，1， I yaw，2 ; The distance l from the control surface to the center of gravity of the aircraft when the wings are unfolded horizontally k Divided into the distance l along the x-axis pitch and the distance l along the z axis rool , while l yaw =l pitch ; The moment of inertia I k Divided into the moment of inertia around the pitch axis I pitch , the moment of inertia around the roll axis I roll , the moment of inertia about the yaw axis I yaw ; The aircraft control rudder arm L k The force arm L is divided into the component of the rudder aerodynamic force that causes the aircraft to produce pitch acceleration pitch , the force arm L of the component of the aerodynamic force of the control surface that causes the aircraft to produce rolling acceleration roll ; According to the wing folding angle θ, calculate the aircraft's moment of inertia I k , the moment of inertia of the aircraft I k The calculation method is as follows: I pitch =I pitch,B +I pitch,1 +I pitch,2 I yaw =I gaw，B +I yaw，1 +I yaw，2 ; Where d1 is the distance from the center of gravity of the inner folding section wing to the hinge between the inner folding section wing and the fuselage, and d2 is the distance from the center of gravity of the outer folding section wing to the hinge between the outer folding section wing and the inner folding section wing. The calculation method of d1 and d2 is as follows: in According to the wing folding angle θ, calculate the aircraft control surface force arm L k , L k The specific calculation method is as follows: L pitch ＝l pitch L roll =l rool -(x1+x'1)(1+cos(θ)) L yaw ＝l rool。 4. The method for controlling a foldable variant UAV with adaptive gain based on folding angle detection according to claim 3, characterized in that: The input of the angular velocity control algorithm in step 3 is the measured angular velocity ω obtained by the gyroscope b And the given target angular velocity ω r , the output is the uncompensated control quantity u k , which is divided into the uncompensated pitch control quantity u pitch , the uncompensated control amount u roll , yaw uncompensated control quantity u yaw ; The angular velocity control algorithm allows parameters to vary within a certain range. If the moment of inertia and rudder effect detection subsystem fails or measures inaccurately, as long as it can still output approximate measurement parameters in response to changes in flight status, the final wing folding control effect will not be significantly affected.

5. The method for controlling a foldable variant UAV with adaptive gain based on folding angle detection according to claim 4, characterized in that: The angular velocity control algorithm may be an active disturbance rejection control algorithm, which includes an extended state observer, a tracking differentiator, state feedback, and total disturbance compensation. The steps of the active disturbance rejection control algorithm are as follows: Step 3.1, Extended State Observer Update: According to the angular velocity ω measured by the gyroscope b The control quantity u output by the controller at the previous moment k And the moment of inertia I of the aircraft k 、Aircraft control rudder arm L k and the dynamic pressure p of the control surface, and substitute into the expanded observer to update the state quantity z jk , where j is the order of the state quantity corresponding to each body axis k, where k is one of the body axis labels pitch, roll, yaw; Step 3.2, arrange the wing folding process: use the tracking differentiator to input the given angular velocity ω r Perform preprocessing to plan a uniform acceleration and deceleration process for the step change of the given angular velocity, and output the preprocessed given angular velocity and angular acceleration v 1k ,v 2k , which is divided into angular velocity and angular acceleration v around the pitch axis 1pitch ,v 2pitch , angular velocity and angular acceleration v around the roll axis 1roll ,v 2roll , angular velocity and angular acceleration v around the yaw axis 1yaw ,v 2yaw ; Step 3.3, calculate state feedback: output state quantity z according to the extended state observer described in step 3.1 1k ,z 2k , with the given angular velocity and given angular acceleration v described in step 3.2 1k ,v 2k , substituting into the state feedback law, we can get the ideal control quantity u designed for the pure integral model 1k , which is divided into the ideal control quantity u for pitch 1pitch , the ideal control quantity u of roll 1roll , the ideal control quantity u of yaw 1yaw ; Step 3.4: Total disturbance compensation: The estimated value z of the equivalent total disturbance output by the extended state observer described in step 3.1 is 3k , which is divided into an estimate of the equivalent total disturbance z around the pitch axis 3pitch , an estimate of the equivalent total disturbance around the roll axis z 3roll , an estimate of the equivalent total disturbance z around the yaw axis 3yaw ; Then, the ideal control quantity described in step 3.3 is subjected to disturbance compensation to make the actual dynamics of the aircraft approach the ideal pure integral model u 1k , which is divided into the uncompensated pitch control amount u 1pitch , uncompensated roll control amount u 1roll , uncompensated yaw control value u 1paw .

6. The method for controlling a foldable variant UAV with adaptive gain based on folding angle detection according to claim 5, characterized in that: The gain compensation method described in step 4 is as follows: in: where u pitch ,u roll ,u yaw are the final control amounts of pitch, roll and yaw after compensation, u 1pitch ,u 1roll ,u 1yaw are the uncompensated control quantities; Then distribute the obtained control quantity to obtain the control signal servo of the left servo L , and the control signal of the right servo R : Among them servo trimL servo trimR They are the neutral point zero values of the left and right servos respectively, which are obtained by on-site adjustment during aircraft assembly.

Images