Two-degree-of-freedom thrust vector dual-rotor flight control method and device

Abstract

The invention discloses a two-degree-of-freedom thrust vector dual-rotor flight control method and device. The method comprises the following steps: analyzing and designing a two-degree-of-freedom thrust vector dual-rotor structure; according to the two-degree-of-freedom thrust vector dual-rotor structure, deriving a two-degree-of-freedom thrust vector dual-rotor dynamic model; a control distribution equation is deduced according to the kinetic model of the two-degree-of-freedom thrust vector double rotors, the kinetic model is decomposed into a matrix combination form through the control distribution equation, and the rotation angle of a joint and the rotation speed of the rotors are solved through the control distribution equation; according to the dynamic model of the two-degree-of-freedom thrust vector dual-rotor, a pose cascade PID-ADRC controller is designed, and the pose cascade PID-ADRC controller comprises a position cascade PID-ADRC controller and an attitude cascade PID-ADRC controller. The method has good control and stabilization effects on the two-degree-of-freedom thrust vector double rotors.

Description

Technical Field

[0001] The present invention belongs to the field of aviation aircraft and robot control, and in particular relates to a two-degree-of-freedom thrust vector twin-rotor flight control method and device. Background Art

[0002] The rotors of traditional multi-rotor aircraft are driven by motors fixed to the fuselage, which can only generate thrust perpendicular to the plane of the fuselage, and cannot generate vector thrust. This design limits the maneuverability of multi-rotor aircraft and their adaptability to diverse environments, such as narrow environments or tasks that require independent control of posture decoupling. To address these limitations, people are exploring innovative technologies such as thrust vectoring and advanced control strategies to enhance the capabilities of multi-rotor drones so that they can better meet the needs of a wider range of applications. In order to address the shortcomings of traditional multi-rotor aircraft, researchers have studied thrust vectoring multi-rotor aircraft with various configurations, such as increasing the degree of freedom of the rotors or statically changing the thrust direction to achieve independent position and attitude control of the aircraft, thereby achieving multiple flight modes, such as maintaining the position while changing the attitude, maintaining the attitude while changing the position, and changing the attitude and position at the same time.

[0003] In terms of energy utilization, multi-rotors also have certain shortcomings. The number of motors installed on the blades of a multi-rotor drone is inversely proportional to the energy utilization rate, that is, when the number of motors increases, the power consumption increases, and the flight time will inevitably decrease. Although quad-rotor aircraft are small, light, and highly redundant, the size of the battery they carry is restricted. The power structure and control method of the quad-rotor determine that although the control method is simple, the energy utilization efficiency is low and the flight time is short. As a result, in some situations where the size is limited, the platform's flight time and the computing unit carried are mutually restricted, and a good balance cannot be achieved. In terms of energy utilization, a dual-rotor aircraft is usually composed of a pair of counter-rotating rotors, which can be arranged horizontally or longitudinally to provide the lift required for hovering. The two rotors of a dual-rotor can be installed on a rotating shaft, and their tilt angle is controlled by a servo motor. The torque input around the rotor is generated by tilting the two rotors and the differential thrust generated by different rotor speeds. Compared with traditional multi-rotors, dual-rotors have only two rotors and a more compact structure. The reduction in the number of rotors reduces the mutual disturbance of airflow between the rotors. At the same time, the vector thrust makes it more maneuverable. At the same power consumption, the take-off weight of the dual-rotor aircraft is much higher than that of the quad-rotor aircraft, and the endurance is longer, and it can carry a more powerful computing platform. It is smaller in size and lighter in weight, and is more suitable for tasks such as pipeline maintenance and complex environment exploration than traditional multi-rotor drones.

[0004] However, although the twin-rotor has the advantages of high energy utilization, small size and easier to pass through narrow spaces, it is still a posture-coupled aircraft. Like the traditional multi-rotor, although it has a single degree of freedom vector thrust in the pitch channel, its single vector thrust needs to control the pitch and position at the same time. Like the multi-rotor, although the control strategy can achieve posture-coupled control, it lacks independence and cannot achieve independent control of position and attitude. Summary of the invention

[0005] Purpose of the invention: One purpose of the present invention is to provide a two-degree-of-freedom thrust vectoring twin-rotor flight control method that can achieve good controllability and aerial pitch change capability.

[0006] Another object of the present invention is to provide a device used in the control method.

[0007] Technical solution: The two-degree-of-freedom thrust vectoring dual-rotor flight control method described in the present invention comprises the following steps:

[0008] S1. Based on the characteristics of the two-degree-of-freedom pitch-changeable thrust vector twin rotor, which has the ability of posture decoupling and independent posture change, and can take off and land and operate on non-horizontal surfaces, the two-degree-of-freedom thrust vector twin rotor structure is analyzed and designed;

[0009] S2. According to the two-degree-of-freedom thrust vectoring twin-rotor structure, a dynamic model of the two-degree-of-freedom thrust vectoring twin-rotor is derived, wherein the dynamic model includes a three-axis resultant external force equation and a three-axis resultant external moment equation in the form of quaternion;

[0010] S3. According to the dynamic model of the two-degree-of-freedom thrust vector twin rotor, a control allocation equation is derived. The control allocation equation decomposes the dynamic model into a combination of matrices, and the rotation angle of the joint and the rotor speed are solved by the control allocation equation;

[0011] S4. According to the dynamic model of the two-degree-of-freedom thrust vectoring twin-rotor, a position cascade PID-ADRC controller is designed, including a position cascade PID-ADRC controller and an attitude cascade PID-ADRC controller. The position cascade PID-ADRC controller includes a position outer-loop error controller and a velocity inner-loop PID-ADRC controller. The attitude cascade PID-ADRC controller includes an attitude outer-loop error controller and an attitude angular velocity inner-loop PID-ADRC controller.

[0012] Further, in step S1, the two-degree-of-freedom thrust vectoring dual-rotor structure includes a fuselage and a two-degree-of-freedom thrust vectoring rotor, and the two-degree-of-freedom thrust vectoring rotor structure is as follows:

[0013]

[0014] Among them, α i,1 is the rotation angle of the first-level servo joint of the i-th arm, i represents the i-th arm i=1,2; α i,2 is the rotation angle of the second-level servo joint of the i-th arm; is the rotation matrix from the first-level servo joint coordinate system of the i-th arm to the body coordinate system, is the rotation matrix from the second-level servo joint coordinate system to the first-level servo joint coordinate system of the i-th arm;

[0015] The position vector of the rotor center relative to the body coordinate system For:

[0016]

[0017] in, is the position vector of the rotor center in the second-level servo joint coordinate system, L P is the distance between the rotor center and the coordinate system of the second-stage servo joint; is the position vector of the second-level servo joint coordinate system in the first-level servo joint coordinate system, L Z is the distance between the rotor center and the coordinate system of the second-stage servo joint; is the position vector of the first-level servo joint coordinate system in the body coordinate system, L Y is the distance between the first-level servo joint coordinate system and the body coordinate system.

[0018] Furthermore, the dynamic model of the two-degree-of-freedom thrust vectoring twin rotor in step S2 is:

[0019]

[0020] in, For The derivative of is the linear velocity in the body coordinate system, Defined as the net external force in the body coordinate system, m b is the mass of the aircraft, for The derivative of is the angular velocity in the body coordinate system, is the inertia matrix of the two-degree-of-freedom thrust vector twin rotor, J x is the x-axis moment of inertia, J y is the moment of inertia along the y axis, J z is the z-axis moment of inertia, for The inverse, It is defined as the total external torque in the body coordinate system, Q b The derivative ofb is the quaternion of the two-degree-of-freedom thrust vector twin rotor, q0 is the real part of the quaternion, q1,q2,q3 are three variables of the imaginary part of the quaternion, for The transpose of F q Q b The antisymmetric matrix of .

[0021] Furthermore, the control allocation equation in step S3 is:

[0022]

[0023] in,

[0024] is the control allocation matrix, Assign parameter matrix to control, Assign input matrix to control;

[0025] Among them, k f is the rotor lift coefficient, L Z is the distance between the rotor center and the second-stage servo joint coordinate system, L Y is the distance between the first-level servo joint coordinate system and the body coordinate system, α 11 is the rotation angle of the first-level servo joint of the first arm, α 21 is the rotation angle of the first-level servo joint of the second arm, α 12 is the rotation angle of the second-level servo joint of the first arm, α 22 is the rotation angle of the second servo joint of the second arm. n1 is the rotor speed of the first arm, n2 is the rotor speed of the second arm, is the control input matrix.

[0026] Furthermore, The kinetic model is used to obtain:

[0027]

[0028] in, is the rotation matrix from the first-level servo joint coordinate system of the first arm to the body coordinate system, is the rotation matrix from the first-level servo joint coordinate system of the second arm to the body coordinate system, is the rotation matrix from the second-level servo joint coordinate system of the first arm to the first-level servo joint coordinate system, is the rotation matrix from the second-level servo joint coordinate system of the second arm to the first-level servo joint coordinate system, F 1,T is the rotor thrust of the second arm, F 2,Tis the rotor thrust of the second arm, is the rotor thrust of the first arm in the body coordinate system, is the rotor thrust of the second arm in the body coordinate system, is the position vector of the first arm rotor center relative to the body coordinate system, is the position vector of the rotor center of the second arm relative to the body coordinate system;

[0029] Then α 11 , α 12 , α 21 , α 22 , n1, n2 are obtained by the following formula:

[0030]

[0031] in, Assigning input matrix to control The ith element of .

[0032] Furthermore, the position outer loop error controller is designed as:

[0033]

[0034] Among them, k p,i is the gain of the outer position loop; is the expected value of the position in the world coordinate system, is the current value of the position in the world coordinate system, is the expected value of the velocity in the world coordinate system;

[0035] The speed inner loop PID-ADRC controller is designed as:

[0036] The speed inner loop of the position cascade PID-ADRC controller can be expressed in state space form:

[0037]

[0038] Among them, f(v x ),f(v y ),f(v z ) is the disturbance of x, y, and z channels, u v,x ,u v,y ,u v,z is the control amount of x, y, and z channels; are the parameters of the x, y, and z channels, m b is the mass of the aircraft, is the derivative of the velocity of the x, y, and z channels;

[0039] The extended state observer of each channel of the speed inner loop of the position cascade PID-ADRC controller is written as:

[0040]

[0041] Among them, β 1 / v,i ,β 2 / v,i is the gain of the extended state observer of each channel; is the velocity of the aircraft in the world coordinate system Observed value of is the velocity disturbance value of the aircraft in the world coordinate system; is the speed of the aircraft in the world coordinate system; e v,i is the speed error, for The derivative of for The derivative of v,i are the parameters of the x, y, and z channels, i = x, y, z;

[0042] The error feedback control law of each channel of the speed inner loop of the position cascade PID-ADRC controller is written as:

[0043]

[0044] Among them, △v i is the error of each channel, is the expected value of the velocity in the world coordinate system, u 0,i is the uncompensated control quantity of each channel, k p / v,i is the error gain coefficient, k i / v,i is the error integral coefficient, k d / v,i is the error differential coefficient, u v,i is the control quantity of each channel; finally, the total thrust in the world coordinate system is The thrust control quantity in the body coordinate system is is the thrust, q b is the quaternion of the two-degree-of-freedom thrust vector twin rotor, Q b The conjugate quaternion of .

[0045] Furthermore, the attitude outer loop error controller is designed as:

[0046]

[0047] Among them, q b,d is the expected quaternion, is the inverse of the desired quaternion, q b is the current quaternion, q eis the error quaternion, and the expected body angular velocity is the vector part of the quaternion error and the scalar part q e Produced, expressed as:

[0048]

[0049] Among them, k ω is the error quaternion coefficient, ω d is the desired body angular velocity;

[0050] The attitude angular velocity inner loop PID-ADRC controller is designed as follows:

[0051] The attitude angular velocity inner loop of the attitude cascade PID-ADRC controller can be expressed in state space form:

[0052]

[0053] Among them, f(ω x ),f(ω y ),f(ω z ) is the disturbance of the inner loop x, y, z channels of the attitude angular velocity, u ω,x ,u ω,y ,u ω,z It is the control output of the inner loop x, y, and z channels of the attitude angular velocity. J xx ,J yy ,J zz is the moment of inertia of the inner loop x, y, z channels of the attitude angular velocity, is the derivative of the angular velocity of the x, y, and z channels;

[0054] The attitude angular velocity inner loop expansion state observer of the attitude cascade PID-ADRC controller is written as:

[0055]

[0056] Among them, e ω,i is the error in angular velocity, ω i is the current value of angular velocity, is the observed value of each channel of the angular velocity inner loop, is the derivative of the observed value of each channel of the angular velocity inner loop, is the disturbance observation value of each channel of the angular velocity inner loop, is the derivative of the disturbance observation value of each channel of the angular velocity inner loop, β 1 / ω,i ,β 2 / ω,i is the gain of the extended state observer of the angular velocity inner loop;

[0057] The error feedback control law of each channel of the attitude angular velocity inner loop of the attitude cascade PID-ADRC controller is written as:

[0058]

[0059] Among them, △ω i is the error of each channel of the angular velocity inner loop, ω d,i is the desired body angular velocity, u 0,i , i = x, y, z are the parameters of each channel of angular velocity, k p / ω,i is the error gain coefficient of each channel of the angular velocity inner loop, k i / ω,i is the error integral coefficient of each channel of the angular velocity inner loop, k d / ω,i is the error differential coefficient of each channel of the angular velocity inner loop, △ω i is the error of each channel of the angular velocity inner loop, u ω,i is the control quantity of each channel of the angular velocity inner loop, i = x, y, z, and finally, the attitude thrust torque control quantity

[0060] The two-degree-of-freedom thrust vectoring dual-rotor flight control device of the present invention comprises:

[0061] Position cascade PID-ADRC controller, based on the position expected value of the two-degree-of-freedom thrust vector twin rotor and location and speed The position thrust control value is obtained by controlling the position of the two-degree-of-freedom deformation quadrotor

[0062] Attitude cascade PID-ADRC controller, according to the expected quaternion q b,d And the actual attitude quaternion q of the two-degree-of-freedom thrust vector twin rotor b and angular velocity ω i , the attitude thrust torque control quantity is obtained for the attitude control of the two-degree-of-freedom thrust vector twin rotor

[0063] Control distribution unit, two-degree-of-freedom thrust vectoring dual rotor position thrust control and attitude control Through control distribution, we finally get the rotor speed n1 of the first arm, the rotor speed n2 of the second arm, and the rotation angle α of the first-stage servo joint of the first arm. 11 , the rotation angle α of the first-stage servo joint of the second arm 21 , the rotation angle α of the second-level servo joint of the first arm 12 , the rotation angle α of the second servo joint of the second arm 22 .

[0064] A control device described in the present invention includes a processor, a communication interface, a memory and a communication bus; wherein the processor, the communication interface and the memory communicate with each other through the communication bus; the memory is used to store computer programs; the processor is used to implement the steps of the two-degree-of-freedom thrust vector dual-rotor attitude flight control method when executing the program stored in the memory.

[0065] The present invention provides a storage medium having a computer program stored thereon, and when the computer program is executed by at least one processor, the steps of the two-degree-of-freedom thrust vectoring twin-rotor flight control method as described above are implemented.

[0066] Beneficial effects: Compared with the prior art, the significant technical effects of the present invention are: (1) by dynamically adjusting the thrust vector direction of the arm rotor, the decoupled independent control of the dual rotor position and attitude is achieved, and the fuselage attitude can be accurately adjusted in the dual rotor fixed-point hovering state, thereby realizing the free change of the mission load attitude angle, thereby having stronger flight maneuverability; (2) The two-degree-of-freedom thrust vector dual rotor can enable the dual rotor to take off and land on a non-horizontal surface, can carry mechanical claws, suction cups and other tools to attach to a non-horizontal surface for operation, and can carry weapons for attack and aiming at non-horizontal angles; (3) The two-degree-of-freedom thrust vector dual rotor is modeled by quaternion dynamics, which avoids the singularity problem caused by Euler angle modeling and has no effect on the controller. The research provides a good reference significance; (4) According to the characteristics of two-degree-of-freedom thrust vectoring dual-rotor multi-actuator, a control distribution model of two-degree-of-freedom thrust vectoring dual-rotor is designed to realize the power distribution of multiple actuators; (5) In view of the nonlinearity and external disturbance problems of two-degree-of-freedom thrust vectoring dual-rotor, the designed PID-ADRC controller can not only overcome the system non-rigidity and parameter uncertainty of the two-degree-of-freedom deformation quadcopter itself, but also play a good role in suppressing the unknown external disturbances suffered by the aircraft during flight. In addition, PID-ADRC has good feasibility and small calculation amount in engineering, which improves the anti-disturbance and feasibility of the two-degree-of-freedom thrust vectoring dual-rotor attitude controller. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 is a flow chart of the method of the present invention;

[0068] Figure 2Schematic diagrams of non-horizontal application scenarios of the two-degree-of-freedom thrust vectoring twin rotors targeted by the present invention; wherein (a) is a schematic diagram of the two-degree-of-freedom thrust vectoring twin rotors detecting pipelines, (b) is a schematic diagram of the two-degree-of-freedom thrust vectoring twin rotors taking off and landing on a non-horizontal plane, (c) is a schematic diagram of the two-degree-of-freedom thrust vectoring twin rotors passing through a narrow space, (d) is a schematic diagram of the two-degree-of-freedom thrust vectoring twin rotors carrying mechanical claws attached to a pole tower, and (e) is a schematic diagram of the two-degree-of-freedom thrust vectoring twin rotors carrying firearms to strike a target;

[0069] Figure 3 A schematic diagram of a two-degree-of-freedom thrust vectoring twin rotor targeted by the present invention;

[0070] Figure 4 The present invention is directed to a two-degree-of-freedom thrust vectoring twin-rotor arm structure diagram;

[0071] Figure 5 It is a schematic diagram of the implementation principle of the two-degree-of-freedom thrust vectoring dual-rotor control system of the method of the present invention;

[0072] Figure 6 The simulation curve diagrams of quaternion changes of the method of the present invention under independent posture control in Example 1 are shown; (a) is the change curve of q0 in the quaternion, (b) is the change curve of q1 in the quaternion, (c) is the change curve of q2 in the quaternion, and (d) is the change curve of q3 in the quaternion;

[0073] Figure 7 It is a simulation curve diagram of Euler angle change under independent posture control of the method of the present invention in embodiment 1;

[0074] Figure 8 The simulation curve diagram of the disturbance torque change of the method of the present invention under independent attitude control in the embodiment 1 is shown in FIG. 1 ; wherein (a) is the actual disturbance torque and estimated disturbance torque change curve of the X axis. (b) is the actual disturbance torque and estimated disturbance torque change curve of the Y axis. (c) is the actual disturbance torque and estimated disturbance torque change curve of the Z axis.

[0075] Fig. 9 It is a simulation curve diagram of three-dimensional position change under independent position control of the method of the present invention in embodiment 1. DETAILED DESCRIPTION

[0076] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. Examples of the embodiments are shown in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be interpreted as limiting the present invention.

[0077] like Figure 1 As shown, the two-degree-of-freedom thrust vectoring dual-rotor flight control method described in the present invention mainly includes the following steps:

[0078] S1. Based on the characteristics of the two-degree-of-freedom pitch-changeable thrust vector twin rotor, which has the ability to change its posture independently and can take off and land and operate on non-horizontal surfaces, a two-degree-of-freedom thrust vector twin rotor structure is designed, such as Figure 2 As shown in (a) to (e), it can take off and land on non-horizontal surfaces, can carry tools such as mechanical claws and suction cups to attach to non-horizontal surfaces for operations, can change its posture to pass through narrow spaces, and can carry weapons for non-horizontal angle strikes and aiming analysis.

[0079] Schematic diagram of two-degree-of-freedom thrust vectoring twin rotor Figure 3 As shown, the aerial robot consists of a body and a two-degree-of-freedom thrust vectoring arm. The arm structure is as follows Figure 4 As shown, is the thrust of the rotor of the i-th arm, and the coordinate system of the second-stage servo joint of the i-th arm is {X i,S2 ,Y i,S2 ,Z i,S2}, the coordinate system of the first-level servo joint of the i-th arm is {X i ,S1 ,Y i,S1 ,Z i,S1}. (a) Define α i,1 is the rotation angle of the first-level servo joint of the i-th arm, i represents the i-th arm i=1,2; (b) α i,2 is the rotation angle of the second-level servo joint of the i-th arm; (c) Definition is the rotation matrix from the first-level servo joint coordinate system of the i-th arm to the body coordinate system, is the rotation matrix from the second-level servo joint coordinate system of the i-th arm to the first-level servo joint coordinate system.

[0080]

[0081] The position vector of the rotor center relative to the body coordinate system for

[0082]

[0083] in, is the position vector of the rotor center in the second-level servo joint coordinate system, L P is the distance between the rotor center and the coordinate system of the second-stage servo joint; is the position vector of the second-level servo joint coordinate system in the first-level servo joint coordinate system, L Z is the distance between the first-stage and second-stage servo joint coordinate systems; is the position vector of the first-level servo joint coordinate system in the body coordinate system, L Yis the distance between the first-level servo joint coordinate system and the body coordinate system.

[0084] S2. According to the two-degree-of-freedom thrust vectoring twin rotor designed in step S1, a dynamic model of the two-degree-of-freedom thrust vectoring twin rotor is derived; wherein the dynamic model is composed of a three-axis combined external force equation and a three-axis combined external moment equation in the form of quaternions;

[0085] The dynamic model of the two-degree-of-freedom thrust vectoring twin rotor is:

[0086]

[0087] in, Defined as the resultant external force in the body coordinate system, Defined as the total external moment in the body coordinate system, m b is the mass of the aircraft, is the linear velocity of the aircraft in the body coordinate system, for The derivative of is the inertia matrix of the two-degree-of-freedom thrust vector twin rotor, J x is the x-axis moment of inertia, J y is the moment of inertia along the y axis, J z is the z-axis moment of inertia, is the angular velocity in the body coordinate system, For The derivative of b is the quaternion of the two-degree-of-freedom thrust vector twin rotor, Q b The derivative of q Q b The antisymmetric matrix of for q0 is the real part of the quaternion, and q1, q2, q3 are the three variables of the imaginary part of the quaternion.

[0088] have

[0089]

[0090] Among them, I3 is the three-dimensional unit matrix, Expressed as The antisymmetric matrix of Defined as the resultant external force in the body coordinate system, Defined as the total external torque in the body coordinate system. The total external force includes thrust gravity and other internal and external interference forces δF B The gravity calculation method of the body coordinate system is Q b The conjugate quaternion of . The total external torque includes the rotor torque Reaction torque and other internal and external interference torques δM B .

[0091] Expressed as

[0092]

[0093] S3. According to the two-degree-of-freedom thrust vector dual-rotor dynamics model established in step S2, a control allocation equation is derived; the control allocation equation decomposes the dynamics model into a combination of matrices, and the rotation angle of the joint and the rotor speed are solved by the control allocation equation.

[0094] The control allocation equation is:

[0095]

[0096] in,

[0097] is the control allocation matrix, Assign parameter matrix to control, Assign input matrix to control.

[0098]

[0099] Among them, k f is the rotor lift coefficient, L Z is the distance between the rotor center and the second-stage servo joint coordinate system, L Y is the distance between the first-level servo joint coordinate system and the body coordinate system. 11 is the rotation angle of the first-level servo joint of the first arm, α 21 is the rotation angle of the first-level servo joint of the second arm, α 12 is the rotation angle of the second-level servo joint of the first arm, α 22 is the rotation angle of the second servo joint of the second arm. n1 is the rotor speed of the first arm, and n2 is the rotor speed of the second arm. is the control input matrix. is the rotation matrix from the first-level servo joint coordinate system of the first arm to the body coordinate system, is the rotation matrix from the first-level servo joint coordinate system of the second arm to the body coordinate system, is the rotation matrix from the second-level servo joint coordinate system of the first arm to the first-level servo joint coordinate system, is the rotation matrix from the second-level servo joint coordinate system to the first-level servo joint coordinate system of the second arm.1,T is the rotor thrust of the second arm, F 2,T is the rotor thrust of the second arm. is the rotor thrust of the first arm in the body coordinate system, is the rotor thrust of the second arm in the body coordinate system, is the position vector of the first arm rotor center relative to the body coordinate system, It is the position vector of the rotor center of the second arm relative to the body coordinate system.

[0100] Then α 11 , α 12 , α 21 , α 22 , n1, n2 can be obtained by the following formula:

[0101]

[0102] in, Assigning input matrix to control The ith element of .

[0103] S4. According to the dynamic model established in step S2, a position cascade PID-ADRC controller is designed, wherein the position cascade PID-ADRC controller includes a position outer loop error controller and a velocity inner loop PID-ADRC controller. The attitude cascade PID-ADRC controller includes an attitude outer loop error controller and an attitude angular velocity inner loop PID-ADRC controller.

[0104] The position outer loop error controller is designed as:

[0105]

[0106] Among them, k p,i is the gain of the outer position loop. is the expected value of the position in the world coordinate system, is the current value of the position in the world coordinate system, is the expected value of the velocity in the world coordinate system.

[0107] The speed inner loop PID-ADRC controller is designed as:

[0108] The speed inner loop of the position cascade PID-ADRC controller can be expressed in state space form as:

[0109]

[0110] Among them, f(v x ),f(v y ),f(v z) are the perturbations of x, y, and z channels respectively, u v,x ,u v,y ,u v,z They are the control values ​​for the x, y, and z channels respectively. are the parameters of x, y, and z channels respectively, m b is the mass of the aircraft, are the derivatives of the x, y, and z channel velocities, respectively.

[0111] The expanded state observer of each channel of the speed inner loop of the position cascade PID-ADRC controller is written as:

[0112]

[0113] Among them, β 1 / v,i ,β 2 / v,i , i = x, y, z is the gain of the extended state observer of each channel, is the velocity of the aircraft in the world coordinate system The observed value of is the velocity disturbance value of the aircraft in the world coordinate system, is the speed of the aircraft in the world coordinate system, e v,i is the speed error, for The derivative of for The derivative of v,i ,i=x,y,z are the parameters of x, y, and z channels.

[0114] The PID error feedback control law of each channel of the speed inner loop of the position cascade PID-ADRC controller is written as:

[0115]

[0116] in, is the expected value of the velocity in the world coordinate system, is the velocity of the aircraft in the world coordinate system The observed value, u 0,i is the uncompensated control quantity of each channel, u v,i , i=x, y, z are the control quantities after compensation of each channel, b v,i , i=x,y,z are the parameters of xyz channel, △v i is the error of each channel, k p / v,i is the error gain coefficient, k i / v,i is the error integral coefficient, k d / v,i is the error differential coefficient; finally, the total thrust in the world coordinate system is The thrust control quantity in the body coordinate system is

[0117] The attitude outer loop error controller is designed as:

[0118]

[0119] Among them, q b,d is the expected quaternion, is the inverse of the desired quaternion, q b is the current quaternion. e is the error quaternion. The desired body angular velocity is given by the vector part of the quaternion error and the scalar part q e Produced, expressed as:

[0120]

[0121] Among them, k ω is the error quaternion coefficient, ω d is the desired body angular velocity.

[0122] The attitude angular velocity inner loop PID-ADRC controller is designed as follows:

[0123] The attitude angular velocity inner loop of the attitude cascade PID-ADRC controller can be expressed in state space form:

[0124]

[0125] Among them, f(ω x ),f(ω y ),f(ω z ) is the disturbance of each channel of the inner loop of attitude angular velocity, u ω,x ,u ω,y ,u ω,z It is the control output of each channel of the inner loop of attitude angular velocity. J xx ,J yy ,J zz is the moment of inertia of each channel in the inner loop of attitude angular velocity. is the derivative of the angular velocity of the xyz channel.

[0126] The attitude angular velocity inner loop expansion state observer of the attitude cascade PID-ADRC controller is written as:

[0127]

[0128] Among them, e ω,i is the error in angular velocity, β 1 / ω,i ,β 2 / ω,i is the gain of the extended state observer of the angular velocity inner loop, is the observed value of each channel of the angular velocity inner loop, is the disturbance observation value of each channel of the angular velocity inner loop, is the derivative of the observed value of each channel of the angular velocity inner loop, is the derivative of the disturbance observation value of each channel of the angular velocity inner loop, ω i is the current value of angular velocity. ω,i ,i=x,y,z are the parameters of each angular velocity channel.

[0129] The PID error feedback control law of each channel of the attitude angular velocity inner loop of the attitude cascade PID-ADRC controller is written as:

[0130]

[0131] Among them, ω d,i is the desired body angular velocity, is the observed value of each channel of the angular velocity inner loop, is the disturbance observation value of each channel of the angular velocity inner loop, u 0,i , i = x, y, z are the parameters of each channel of angular velocity, u ω,i , i = x, y, z is the control quantity of each channel of the angular velocity inner loop, b ω,i , i=x, y, z are the parameters of each channel of angular velocity, △ω i is the error of each channel of the angular velocity inner loop, k p / ω,i is the error gain coefficient of each channel of the angular velocity inner loop, k i / ω,i is the error integral coefficient of each channel of the angular velocity inner loop, k d / ω,i is the error differential coefficient of each channel of the angular velocity inner loop. Finally, the attitude thrust torque control quantity

[0132] According to the attitude PID-ADRC flight controller designed in step S4 above, a control system implementation schematic is established, as shown in Figure 5 As shown, the two-degree-of-freedom thrust vectoring dual rotor includes a position cascade PID-ADRC controller, an attitude cascade PID-ADRC controller, and a control distribution unit; wherein:

[0133] Position cascade PID-ADRC controller, based on the position expected value of the two-degree-of-freedom thrust vector twin rotor and location and speed The position thrust control value is obtained by controlling the position of the two-degree-of-freedom deformation quadrotor

[0134] Attitude cascade PID-ADRC controller, according to the expected quaternion q b,d And the actual attitude quaternion q of the two-degree-of-freedom thrust vector twin rotor b and angular velocity ω i, the attitude thrust torque control quantity is obtained for the attitude control of the two-degree-of-freedom thrust vector twin rotor

[0135] Control distribution unit, two-degree-of-freedom thrust vectoring dual rotor position thrust control and attitude control Through control distribution, we finally get the rotor speed n1 of the first arm, the rotor speed n2 of the second arm, and the rotation angle α of the first-stage servo joint of the first arm. 11 , the rotation angle α of the first-stage servo joint of the second arm 21 , the rotation angle α of the second-level servo joint of the first arm 12 , the rotation angle α of the second servo joint of the second arm 22 .

[0136] In one embodiment of the present invention, a control device includes a processor, a communication interface, a memory and a communication bus; wherein the processor, the communication interface and the memory communicate with each other through the communication bus; the memory is used to store computer programs; the processor is used to implement the above-mentioned two-degree-of-freedom thrust vectoring twin-rotor flight control method when executing the program stored in the memory, and achieve the same technical effect as the above-mentioned method.

[0137] In one embodiment of the present invention, a storage medium stores a computer program, which, when executed by at least one processor, implements the steps of the two-degree-of-freedom thrust vectoring twin-rotor flight control method and achieves the same technical effect as the above method.

[0138] Embodiments of the present invention are described below.

[0139] With the designed two-degree-of-freedom thrust vector twin rotor (such as Figure 3 The specific parameters of the mathematical model of the two-degree-of-freedom thrust vectoring twin rotor are shown in Table 1:

[0140] Table 1 System simulation parameters

[0141]

[0142]

[0143] Case 1: 2-DOF thrust vectoring dual-rotor independent attitude control simulation;

[0144] In the simulation experiment, the initial pitch angle of the two-degree-of-freedom deformation quadrotor is set to θ = 0°, the expected pitch angle is set to θ = 90° in the 2nd second, and the expected pitch angle is set to θ = -90° in the 40th second. The added disturbance noise is

[0145]

[0146] in, is the disturbance torque of the XYZ axis. The quaternion simulation results are as follows Figure 6 As shown in (a) to (d), (a) is the change curve of q0 in the quaternion, (b) is the change curve of q1 in the quaternion, (c) is the change curve of q2 in the quaternion, and (d) is the change curve of q3 in the quaternion. In the process of pitch from 0° to ±90°, the quaternion changes continuously without singularity problems. The simulation results of Euler angles are shown in Figure 7 As shown in the figure, when the pitch changes from 0° to ±90°, singularity problems occur in the roll angle and heading angle. The simulation results of the interference torque are shown in Figure 8 As shown in (a) to (c). Figure 8 (a) is the change curve of the actual disturbance torque and the estimated disturbance torque of the X-axis. Figure 8 (b) is the change curve of the actual disturbance torque and the estimated disturbance torque of the Y axis. Figure 8 (c) is the actual disturbance torque and estimated disturbance torque change curve of the Z axis. The simulation results show that the PID-ADRC control effect is good, the present invention can track the target value well, and ADRC can observe the disturbance torque. In addition, the quaternion method can avoid the singularity problem of Euler angles and can quickly and robustly track the desired posture.

[0147] The simulation results show that the present invention can effectively improve the response stability of the posture when the posture changes.

[0148] Case 2: Simulation of independent position control of quadrotor with two-degree-of-freedom deformation of the arm;

[0149] The expected position of the two-degree-of-freedom deformable quadrotor designed in the simulation experiment is:

[0150]

[0151] in, is the expected position of the aircraft on the XYZ axis in the world coordinate system. The body position response curve is as follows: Fig. 9 As shown, the simulation results show that PID-ADRC can track the desired position target well, and the present invention can track the target value well and can track the desired position quickly and robustly.

[0152] The simulation results show that the present invention can effectively improve the response speed of the position when the position changes.

Claims

1. A two-degree-of-freedom thrust vectoring twin-rotor flight control method, characterized in that: The following steps are involved: S1. Based on the characteristics of the two-degree-of-freedom pitch-changeable thrust vector twin rotor, which has the ability of posture decoupling and independent posture change, and can take off and land and operate on non-horizontal surfaces, the two-degree-of-freedom thrust vector twin rotor structure is analyzed and designed; S2. According to the two-degree-of-freedom thrust vectoring twin-rotor structure, a dynamic model of the two-degree-of-freedom thrust vectoring twin-rotor is derived, wherein the dynamic model includes a three-axis resultant external force equation and a three-axis resultant external moment equation in the form of quaternion; S3. According to the dynamic model of the two-degree-of-freedom thrust vector twin rotor, a control allocation equation is derived. The control allocation equation decomposes the dynamic model into a combination of matrices, and the rotation angle of the joint and the rotor speed are solved by the control allocation equation; S4. According to the dynamic model of the two-degree-of-freedom thrust vectoring twin-rotor, a position cascade PID-ADRC controller is designed, including a position cascade PID-ADRC controller and an attitude cascade PID-ADRC controller. The position cascade PID-ADRC controller includes a position outer-loop error controller and a velocity inner-loop PID-ADRC controller. The attitude cascade PID-ADRC controller includes an attitude outer-loop error controller and an attitude angular velocity inner-loop PID-ADRC controller.

2. A two-degree-of-freedom thrust vectoring twin-rotor flight control method according to claim 1, characterized in that: In step S1, the two-degree-of-freedom thrust vectoring dual-rotor structure includes a fuselage and a two-degree-of-freedom thrust vectoring rotor, and the two-degree-of-freedom thrust vectoring rotor structure is as follows: Among them, α i,1 is the rotation angle of the first-level servo joint of the i-th arm, i represents the i-th arm i=1,2; α i,2 is the rotation angle of the second-level servo joint of the i-th arm; is the rotation matrix from the first-level servo joint coordinate system of the i-th arm to the body coordinate system, is the rotation matrix from the second-level servo joint coordinate system to the first-level servo joint coordinate system of the i-th arm; The position vector of the rotor center relative to the body coordinate system for: in, is the position vector of the rotor center in the second-level servo joint coordinate system, L P is the distance between the rotor center and the coordinate system of the second-stage servo joint; is the position vector of the second-level servo joint coordinate system in the first-level servo joint coordinate system, L Z is the distance between the rotor center and the coordinate system of the second-stage servo joint; is the position vector of the first-level servo joint coordinate system in the body coordinate system, L Y is the distance between the first-level servo joint coordinate system and the body coordinate system.

3. The two-degree-of-freedom thrust vectoring twin-rotor flight control method according to claim 1, characterized in that: The dynamic model of the two-degree-of-freedom thrust vectoring twin rotor in step S2 is: in, for The derivative of is the linear velocity in the body coordinate system, Defined as the net external force in the body coordinate system, m b is the mass of the aircraft, for The derivative of is the angular velocity in the body coordinate system, is the inertia matrix of the two-degree-of-freedom thrust vector twin rotor, J x is the x-axis moment of inertia, J y is the moment of inertia along the y axis, J z is the moment of inertia about the z axis, for The inverse, It is defined as the total external torque in the body coordinate system, for q b The derivative of q b is the quaternion of the two-degree-of-freedom thrust vector twin rotor, q0 is the real part of the quaternion, q1,q2,q3 are three variables of the imaginary part of the quaternion, for The transpose of F q for q b The antisymmetric matrix of .

4. The two-degree-of-freedom thrust vectoring twin-rotor flight control method according to claim 1, characterized in that: The control allocation equation in step S3 is: in, is the control allocation matrix, Assign parameter matrix to control, Assign input matrix to control; Among them, k f is the rotor lift coefficient, L Z is the distance between the rotor center and the second-stage servo joint coordinate system, L Y is the distance between the first-level servo joint coordinate system and the body coordinate system, α 11 is the rotation angle of the first-level servo joint of the first arm, α 21 is the rotation angle of the first-level servo joint of the second arm, α 12 is the rotation angle of the second-level servo joint of the first arm, α 22 is the rotation angle of the second servo joint of the second arm. n1 is the rotor speed of the first arm, n2 is the rotor speed of the second arm, is the control input matrix.

5. The two-degree-of-freedom thrust vectoring twin-rotor flight control method according to claim 4, characterized in that: The kinetic model is used to obtain: in, is the rotation matrix from the first-level servo joint coordinate system of the first arm to the body coordinate system, is the rotation matrix from the first-level servo joint coordinate system of the second arm to the body coordinate system, is the rotation matrix from the second-level servo joint coordinate system of the first arm to the first-level servo joint coordinate system, is the rotation matrix from the second-level servo joint coordinate system of the second arm to the first-level servo joint coordinate system, F 1,T is the rotor thrust of the second arm, F 2,T is the rotor thrust of the second arm, is the rotor thrust of the first arm in the body coordinate system, is the rotor thrust of the second arm in the body coordinate system, is the position vector of the first arm rotor center relative to the body coordinate system, is the position vector of the rotor center of the second arm relative to the body coordinate system; Then α 11 , α 12 , α 21 , α 22 , n1, n2 are obtained by the following formula: in, Assign the i-th element of the input matrix N to the control.

6. The two-degree-of-freedom thrust vectoring twin-rotor flight control method according to claim 1, characterized in that: The position outer loop error controller is designed as: Among them, k p,i is the gain of the outer position loop; is the expected value of the position in the world coordinate system, is the current value of the position in the world coordinate system, is the expected value of the velocity in the world coordinate system; The speed inner loop PID-ADRC controller is designed as: The speed inner loop of the position cascade PID-ADRC controller can be expressed in state space form: Among them, f(v x ),f(v y ),f(v z ) is the disturbance of x, y, and z channels, u v,x ,u v,y ,u v,z is the control amount of x, y, and z channels; are the parameters of the x, y, and z channels, m b is the mass of the aircraft, is the derivative of the velocity of the x, y, and z channels; The extended state observer of each channel of the speed inner loop of the position cascade PID-ADRC controller is written as: Among them, β 1 / v,i ,β 2 / v,i is the gain of the extended state observer of each channel; is the velocity of the aircraft in the world coordinate system Observed value of is the velocity disturbance value of the aircraft in the world coordinate system; is the speed of the aircraft in the world coordinate system; e v,i is the speed error, for The derivative of for The derivative of v,i are the parameters of the x, y, and z channels, i = x, y, z; The error feedback control law of each channel of the speed inner loop of the position cascade PID-ADRC controller is written as: Among them, △v i is the error of each channel, is the expected value of the velocity in the world coordinate system, u 0,i is the uncompensated control quantity of each channel, k p / v,i is the error gain coefficient, k i / v,i is the error integral coefficient, k d / v,i is the error differential coefficient, u v,i is the control quantity of each channel; finally, the total thrust in the world coordinate system is The thrust control quantity in the body coordinate system is is the thrust, q b is the quaternion of the two-degree-of-freedom thrust vector twin rotor, Q b The conjugate quaternion of .

7. The two-degree-of-freedom thrust vectoring twin-rotor flight control method according to claim 1, characterized in that: The attitude outer loop error controller is designed as: Among them, q b,d is the expected quaternion, is the inverse of the desired quaternion, q b is the current quaternion, q e is the error quaternion, and the expected body angular velocity is the vector part of the quaternion error and the scalar part q e Produced, expressed as: Among them, k ω is the error quaternion coefficient, ω d is the desired body angular velocity; The attitude angular velocity inner loop PID-ADRC controller is designed as follows: The attitude angular velocity inner loop of the attitude cascade PID-ADRC controller can be expressed in state space form: Among them, f(ω x ),f(ω y ),f(ω z ) is the disturbance of the inner loop x, y, z channels of the attitude angular velocity, u ω,x ,u ω,y ,u ω,z It is the control output of the inner loop x, y, and z channels of the attitude angular velocity. J xx ,J yy ,J zz is the moment of inertia of the inner loop x, y, z channels of the attitude angular velocity, is the derivative of the angular velocity of the x, y, and z channels; The attitude angular velocity inner loop expansion state observer of the attitude cascade PID-ADRC controller is written as: Among them, e ω,i is the error in angular velocity, ω i is the current value of angular velocity, is the observed value of each channel of the angular velocity inner loop, is the derivative of the observed value of each channel of the angular velocity inner loop, is the disturbance observation value of each channel of the angular velocity inner loop, is the derivative of the disturbance observation value of each channel of the angular velocity inner loop, β 1 / ω,i ,β 2 / ω,i is the gain of the extended state observer of the angular velocity inner loop; The error feedback control law of each channel of the attitude angular velocity inner loop of the attitude cascade PID-ADRC controller is written as: Among them, △ω i is the error of each channel of the angular velocity inner loop, ω d,i is the desired body angular velocity, u 0,i , i = x, y, z are the parameters of each channel of angular velocity, k p / ω,i is the error gain coefficient of each channel of the angular velocity inner loop, k i / ω,i is the error integral coefficient of each channel of the angular velocity inner loop, k d / ω,i is the error differential coefficient of each channel of the angular velocity inner loop, △ω i is the error of each channel of the angular velocity inner loop, u ω,i is the control quantity of each channel of the angular velocity inner loop, i = x, y, z, and finally, the attitude thrust torque control quantity 8. A two-degree-of-freedom thrust vectoring twin-rotor flight control device, characterized in that: include: Position cascade PID-ADRC controller, based on the position expected value of the two-degree-of-freedom thrust vector twin rotor and location and speed The position thrust control value is obtained by controlling the position of the two-degree-of-freedom deformation quadrotor Attitude cascade PID-ADRC controller, according to the expected quaternion q b,d And the actual attitude quaternion q of the two-degree-of-freedom thrust vector twin rotor b and angular velocity ω i , the attitude thrust torque control quantity is obtained for the attitude control of the two-degree-of-freedom thrust vector twin rotor Control distribution unit, two-degree-of-freedom thrust vectoring dual rotor position thrust control and attitude control Through control distribution, we finally get the rotor speed n1 of the first arm, the rotor speed n2 of the second arm, and the rotation angle α of the first-stage servo joint of the first arm. 11 , the rotation angle α of the first-stage servo joint of the second arm 21 , the rotation angle α of the second-level servo joint of the first arm 12 , the rotation angle α of the second servo joint of the second arm 22 .

9. A control device, characterized in that: It includes a processor, a communication interface, a memory and a communication bus; wherein the processor, the communication interface and the memory communicate with each other through the communication bus; the memory is used to store computer programs; the processor is used to implement the steps of the two-degree-of-freedom thrust vectoring twin-rotor attitude flight control method described in any one of claims 1-7 when executing the program stored in the memory.

10. A storage medium, characterized in that: The storage medium stores a computer program, which, when executed by at least one processor, implements the steps of the two-degree-of-freedom thrust vectoring twin-rotor flight control method as described in any one of claims 1-7.

Images