Two-degree-of-freedom thrust vectoring twin-rotor flight control method and device

Abstract

The application discloses a two-degree-of-freedom thrust vector dual-rotor flight control method and device, which comprises the following steps: analyzing and designing a two-degree-of-freedom thrust vector dual-rotor structure; deriving a dynamics model of the two-degree-of-freedom thrust vector dual-rotor according to the two-degree-of-freedom thrust vector dual-rotor structure; deriving a control distribution equation according to the dynamics model of the two-degree-of-freedom thrust vector dual-rotor, which decomposes the dynamics model into a combination form of matrices, and solving the rotation angle of a joint and the rotation speed of a rotor through the control distribution equation; and designing a position and pose cascade PID-ADRC controller, including a position cascade PID-ADRC controller and a pose cascade PID-ADRC controller, according to the dynamics model of the two-degree-of-freedom thrust vector dual-rotor. The two-degree-of-freedom thrust vector dual-rotor has good control and stabilization.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of aerial vehicles and robot control, in particular to a two-degree-of-freedom thrust vector dual-rotor flight control method and device. BACKGROUND

[0002] The rotors of traditional multi-rotor aircraft are driven by motors fixed on the body, which can only generate thrust perpendicular to the plane of the fuselage, and cannot generate vector thrust. This design limits the maneuverability and adaptability of multi-rotor aircraft to diverse environments, such as narrow environments or tasks that require independent control of position and pose. In order to solve these limitations, people are exploring innovative technologies such as thrust vectoring and advanced control strategies to enhance the capabilities of multi-rotor drones, making them better suited for a wider range of applications. In order to solve the shortcomings of traditional multi-rotor aircraft, researchers have studied thrust vectoring multi-rotor aircraft with various configurations, such as adding degrees of freedom to the rotors or statically changing the thrust direction to achieve independent position and attitude control of the aircraft, thereby achieving various flight modes such as changing attitude while maintaining position, changing position while maintaining attitude, and changing both attitude and position.

[0003] In terms of energy utilization, multi-rotor aircraft also has certain shortcomings. The number of installed propeller motors of multi-rotor drones is inversely proportional to energy utilization, i.e. when the number of motors increases, power consumption increases, and endurance time must decrease. Although quad-rotor aircrafts are small, lightweight, and have high redundancy, the size of the battery carried is also restricted. The power structure and control method of quad-rotor aircrafts determine that although the control method is simple, the energy utilization efficiency is low and the endurance time is short. This leads to a mutual restriction between platform endurance time and carried computing units in some size-limited situations, and a good balance cannot be achieved. In terms of energy utilization, dual-rotor aircrafts are typically composed of a pair of counter-rotating rotors, which can be arranged horizontally or vertically to provide the required lift for hovering. The two rotors can be mounted on a rotating shaft and controlled by a servo motor to change their tilt angle. The torque input around the rotors is generated by tilting the two rotors and the differential thrust generated by different rotor speeds. Compared to traditional multi-rotor aircrafts, dual-rotor aircrafts 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, while the vector thrust enables stronger maneuverability. Under the same power consumption, the take-off weight of dual-rotor aircrafts is greater than that of quad-rotor aircrafts, and the endurance time is longer, allowing them to carry more powerful computing platforms. They are smaller in size and lighter in weight, making them more suitable for tasks such as pipeline maintenance and exploration of complex environments than traditional multi-rotor drones.

[0004] However, although the dual-rotor has the advantages of high energy utilization rate and small volume, it is still a pose-coupled aircraft. Like the traditional multi-rotor, although it has a single degree of freedom of vector thrust in the pitch channel, the single vector thrust needs to control the pitch and position at the same time. Like the multi-rotor, although the control under the pose coupling can be realized through the control strategy, the independence is lacked, and the independent control of the position and the attitude cannot be realized. SUMMARY

[0005] The purpose of the present application is to provide a two-degree-of-freedom thrust vector dual-rotor flight control method which can realize good control ability and aerial pitch attitude change ability.

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

[0007] Technical scheme: The two-degree-of-freedom thrust vector dual-rotor flight control method comprises the following steps:

[0008] S1. According to the characteristics of the two-degree-of-freedom pitch attitude change thrust vector dual-rotor which has the ability of pose decoupling independent attitude change and can take off and work on a non-horizontal plane, the two-degree-of-freedom thrust vector dual-rotor structure is analyzed and designed;

[0009] S2. According to the two-degree-of-freedom thrust vector dual-rotor structure, the dynamics model of the two-degree-of-freedom thrust vector dual-rotor is derived, wherein the dynamics model comprises a three-axis combined external force equation in the form of a quaternion and a three-axis combined external torque equation;

[0010] S3. According to the dynamics model of the two-degree-of-freedom thrust vector dual-rotor, 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 through the control allocation equation;

[0011] S4. According to the dynamics model of the two-degree-of-freedom thrust vector dual-rotor, a pose 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 comprises a position outer loop error controller and a speed inner loop PID-ADRC controller, and the attitude cascade PID-ADRC controller comprises an attitude outer loop error controller and an attitude angular velocity inner loop PID-ADRC controller.

[0012] Further, the two-degree-of-freedom thrust vector dual-rotor structure in step S1 comprises a body and a two-degree-of-freedom thrust vector rotor, and the two-degree-of-freedom thrust vector rotor structure is in the form of:

[0013] ;

[0014] in, For the first The rotation angle of the first-stage servo joint of each arm Indicates the first One arm ; For the first The rotation angle of the second-stage servo joint of each arm; For the first The rotation matrix from the first-level servo joint coordinate system to the body coordinate system of each robotic arm. For the first The rotation matrix from the second-level servo joint coordinate system to the first-level servo joint coordinate system of each arm;

[0015] The position vector of the rotor center relative to the airframe coordinate system for:

[0016] ;

[0017] in, This is the position vector of the rotor center in the second-stage servo joint coordinate system. This is the distance between the rotor center and the coordinate system of the second-stage servo joint; This is the position vector of the second-level servo joint coordinate system in the first-level servo joint coordinate system. This represents the distance between the first-level and second-level servo joint coordinate systems. This represents the position vector of the first-level servo joint coordinate system in the body coordinate system. This represents 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 vector twin rotor in step S2 is as follows:

[0019] ;

[0020] in, for The derivative, The linear velocity in the body coordinate system. Defined as the net external force in the body coordinate system. For the mass of the aircraft, for The derivative, Angular velocity in the body coordinate system The inertial matrix of a two-degree-of-freedom thrust vectoring twin rotor is... Let x be the moment of inertia along the x-axis. Let y be the moment of inertia. Let z be the moment of inertia along the z-axis. for The reverse, Defined as the net external torque in the body coordinate system. for The derivative of The current quaternion for the two-degree-of-freedom thrust vector dual rotor. , It is the real part of the quaternion. , These are the three variables of the imaginary part of a quaternion. for transpose, for An antisymmetric matrix.

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

[0022] ;

[0023] in,

[0024] To control the allocation matrix, To control the allocation of parameter matrices, To control the allocation of input matrices;

[0025] in, This is the rotor lift coefficient. This represents the distance between the rotor center and the coordinate system of the second-stage servo joint. This represents the distance between the first-level servo joint coordinate system and the body coordinate system. This refers to the rotation angle of the first-stage servo joint of the first arm. This refers to the rotation angle of the first-stage servo joint of the second arm. This refers to the rotation angle of the second-stage servo joint of the first arm. This refers to the rotation angle of the second-stage servo joint of the second arm. It is the rotor speed of the first arm. It is the rotor speed of the second arm. To control the input matrix.

[0026] Furthermore, The following is obtained through the dynamic model:

[0027] ;

[0028] in, This is the rotation matrix from the first-level servo joint coordinate system of the first arm to the body coordinate system. This is the rotation matrix from the first-level servo joint coordinate system of the second arm to the body coordinate system. R2 1 is the rotation matrix from the second level servo joint coordinate system to the first level servo joint coordinate system of the first manipulator, R2 2 is the rotation matrix from the second level servo joint coordinate system to the first level servo joint coordinate system of the second manipulator, F2 is the rotor thrust of the second manipulator, F2 is the rotor thrust of the second manipulator, F1 is the rotor thrust of the first manipulator in the body coordinate system, F2 is the rotor thrust of the second manipulator in the body coordinate system, R1 is the position vector of the rotor center of the first manipulator relative to the body coordinate system, R2 is the position vector of the rotor center of the second manipulator relative to the body coordinate system;

[0029] Then , , , , , is obtained by the following formula:

[0030] ;

[0031] wherein, is the first element of the control allocation input matrix .

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

[0033] ;

[0034] wherein, is the gain of the position outer loop; is the position desired value in the world coordinate system, is the position current value in the world coordinate system, is the speed desired value 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 the state space form as:

[0037] ;

[0038] wherein, is the disturbance of , , channel, is the disturbance of , ​, Control amount of the channel; For , , Parameter of the channel, For aircraft mass, For , , Derivative of the channel speed;

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

[0040] ;

[0041] Wherein, Gain of the extended state observer of each channel; The observed value of the aircraft speed value in the world coordinate system ; The aircraft speed disturbance value in the world coordinate system; The speed of the aircraft in the world coordinate system; The error of the speed, The derivative of , The derivative of , The derivative of , , Parameter of the channel, ;

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

[0043] ;

[0044] Wherein, Error of each channel, The speed expected value in the world coordinate system, Uncompensated control amount of each channel, Error gain coefficient, Error integral coefficient, Error differential coefficient, Control amount of each channel; finally, the total thrust in the world coordinate system is ; the thrust control amount in the body coordinate system is , The position thrust control amount of the two-degree-of-freedom thrust vector dual-rotor, The current quaternion of the two-degree-of-freedom thrust vector dual-rotor, TheConjugate quaternion of .

[0045] Further, the attitude outer-loop error controller is designed as

[0046] ;

[0047] where is the desired quaternion, is the inverse of the desired quaternion, is the current quaternion of the 2-DOF thrust vectoring quadrotor, is the error quaternion, the desired body angular velocity is generated by the vector part and the scalar part of the quaternion error, denoted as

[0048] ;

[0049] where is the error quaternion coefficient, is the desired body angular velocity;

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

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

[0052] ;

[0053] where is the disturbance of the attitude angular velocity inner-loop , , channel, is the control output of the attitude angular velocity inner-loop , , channel, , is the moment of inertia of the attitude angular velocity inner-loop , , channel, is the derivative of the , , channel angular velocity;

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

[0055] ;

[0056] where error of angular velocity, current value of angular velocity, observation value of each channel of inner loop of angular velocity, derivative of observation value of each channel of inner loop of angular velocity, perturbation observation value of each channel of inner loop of angular velocity, derivative of perturbation observation value of each channel of inner loop of angular velocity, gain of extended state observer of inner loop of angular velocity;

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

[0058] ;

[0059] wherein, error of each channel of inner loop of angular velocity, expected body angular velocity, uncompensated control amount of each channel, error gain coefficient of each channel of inner loop of angular velocity, error integral coefficient of each channel of inner loop of angular velocity, error differential coefficient of each channel of inner loop of angular velocity, control output of each channel of inner loop of attitude angular velocity, finally, attitude thrust torque control amount .

[0060] The two-degree-of-freedom thrust vector dual-rotor flight control device comprises:

[0061] The position cascade PID-ADRC controller obtains the position thrust control amount of the two-degree-of-freedom deformation quad-rotor according to the position expected value of the two-degree-of-freedom thrust vector dual-rotor , the position and the speed of the arm. ;

[0062] The attitude cascade PID-ADRC controller obtains the attitude thrust torque control amount of the two-degree-of-freedom thrust vector dual-rotor according to the expected quaternion and the current quaternion and the current value of angular velocity of the two-degree-of-freedom thrust vector dual-rotor. ;

[0063] The control distribution unit obtains the position thrust control amount of the two-degree-of-freedom thrust vector dual-rotor and the attitude thrust torque control amount The rotor rotation speed of the first arm is obtained by controlling the distribution The rotor rotation speed of the second arm The rotation angle of the first-stage servo joint of the first arm The rotation angle of the first-stage servo joint of the second arm The rotation angle of the second-stage servo joint of the first arm The rotation angle of the second-stage servo joint of the second arm .

[0064] The control device comprises a processor, a communication interface, a memory and a communication bus; the processor, the communication interface and the memory are in communication with each other through the communication bus; the memory is used for storing a computer program; and the processor is used for executing the program stored in the memory to realize the steps of the two-degree-of-freedom thrust vector dual-rotor flight control method.

[0065] The storage medium stores a computer program, and the computer program is executed by at least one processor to realize the steps of the two-degree-of-freedom thrust vector dual-rotor flight control method.

[0066] Advantages: Compared with the prior art, the significant technical effects of the application are: (1) by dynamically adjusting the thrust vector direction of the rotor of the arm, the decoupled independent control of the position and attitude of the dual-rotor is realized, the attitude of the fuselage can be accurately adjusted in the dual-rotor fixed-point hovering state, the attitude angle of the task load can be freely changed, and stronger flight maneuverability is achieved; (2) the two-degree-of-freedom thrust vector dual-rotor can take off and land on a non-horizontal plane, can carry mechanical claws, suction cups and other tools to adhere to the non-horizontal plane for work, and can carry weapons to attack and aim at a non-horizontal angle; (3) the four-element number dynamics modeling of the two-degree-of-freedom thrust vector dual-rotor avoids the singularity problem caused by Euler angle modeling, and provides a good reference for the research of the controller; (4) the control distribution model of the two-degree-of-freedom thrust vector dual-rotor is designed according to the characteristics of the multi-actuator of the two-degree-of-freedom thrust vector dual-rotor, and the power distribution of the multi-actuator is realized; (5) the PID-ADRC controller is designed for the nonlinearity and external disturbance of the two-degree-of-freedom thrust vector dual-rotor, which can not only overcome the influence of the system non-rigidity and parameter uncertainty of the two-degree-of-freedom deformation quad-rotor on the controller, but also can well suppress unknown external disturbances on the aircraft during flight. In addition, PID-ADRC has good feasibility and small calculation amount in engineering, and the anti-interference and feasibility of the two-degree-of-freedom thrust vector dual-rotor attitude controller are improved. BRIEF DESCRIPTION OF DRAWINGS

[0067] Figure 1 This is a flowchart of the method of the present invention;

[0068] Figure 2 This is a schematic diagram of a non-horizontal application scenario of a two-degree-of-freedom thrust vector dual-rotor, which is the subject of this invention; wherein (a) is a schematic diagram of a two-degree-of-freedom thrust vector dual-rotor inspecting a pipeline, (b) is a schematic diagram of a two-degree-of-freedom thrust vector dual-rotor taking off and landing on a non-horizontal surface, (c) is a schematic diagram of a two-degree-of-freedom thrust vector dual-rotor traversing a narrow space, (d) is a schematic diagram of a two-degree-of-freedom thrust vector dual-rotor carrying a mechanical claw attaching to a tower, and (e) is a schematic diagram of a two-degree-of-freedom thrust vector dual-rotor carrying a firearm to strike a target;

[0069] Figure 3 This is a schematic diagram of a two-degree-of-freedom thrust vectoring dual rotor, which is the subject of this invention.

[0070] Figure 4 This is a structural diagram of the two-degree-of-freedom thrust vectoring twin-rotor arm for which this invention is based;

[0071] Figure 5 This is a schematic diagram illustrating the implementation principle of the two-degree-of-freedom thrust vector dual-rotor control system of the present invention.

[0072] Figure 6 The following is a simulation curve of the quaternion variation under independent attitude control according to Example 1; (a) is the quaternion variation curve. The curve of change, (b) is the quaternion in The curve of change, (c) is the quaternion in The change curve of , (d) is the quaternion The curve of change;

[0073] Figure 7 This is a simulation curve of the Euler angle variation under independent attitude control using the method of the present invention in Example 1.

[0074] Figure 8 The following are simulation curves showing the change of disturbance torque under independent attitude control using the method of the present invention in Example 1; where (a) is the actual and estimated disturbance torque change curves along the X-axis; (b) is the actual and estimated disturbance torque change curves along the Y-axis; and (c) is the actual and estimated disturbance torque change curves along the Z-axis.

[0075] Figure 9 The figure shows a three-dimensional position change simulation curve of the method of the present invention under independent position control in Example 1. Detailed Implementation

[0076] The present invention will now be described in detail with reference to 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 should not be construed as limiting the present invention.

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

[0078] S1. Based on the characteristics of a two-degree-of-freedom pitch-and-thrust-vectoring twin-rotor, which possesses independent attitude-changing capabilities and can take off and land on non-horizontal surfaces, a two-degree-of-freedom thrust-vectoring 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, carry mechanical claws, suction cups and other tools to attach to non-horizontal surfaces for operation, change its attitude to pass through narrow spaces, and carry weapons to perform non-horizontal angle strikes and aiming analysis.

[0079] A schematic diagram of a two-degree-of-freedom thrust vectoring dual rotor is shown below. Figure 3 As shown, the aerial robot consists of a body and a two-degree-of-freedom thrust vectoring arm, with the arm structure as follows: Figure 4 As shown, For the first The thrust of the rotor of each arm, the first The coordinate system of the second-level servo joint of each robotic arm is as follows: , No. The coordinate system of the first-stage servo joint of the robotic arm is as follows: (a) Definition For the first The rotation angle of the first-stage servo joint of each arm Indicates the first One arm (b) For the first The rotation angle of the second-level servo joint of each arm; (c) Definition For the first The rotation matrix from the first-level servo joint coordinate system to the body coordinate system of each robotic arm. For the first The rotation matrix from the second-level servo joint coordinate system to the first-level servo joint coordinate system of each arm.

[0080] (1);

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

[0082] (2);

[0083] where, is the position vector of the rotor center in the second stage servo joint coordinate system, is the distance from the rotor center to the second stage servo joint coordinate system; is the position vector of the second stage servo joint coordinate system in the first stage servo joint coordinate system, is the distance from the first stage to the second stage servo joint coordinate system; is the position vector of the first stage servo joint coordinate system in the body coordinate system, is the distance from the first stage servo joint coordinate system to the body coordinate system.

[0084] S2, according to the two-degree-of-freedom thrust vectoring double-rotor designed in step S1, deriving the dynamics model of the two-degree-of-freedom thrust vectoring double-rotor; wherein the dynamics model consists of three-axis combined external force equation and three-axis combined external torque equation in quaternion form;

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

[0086] (3);

[0087] where, is defined as the combined external force in the body coordinate system, is defined as the combined external torque in the body coordinate system, is the mass of the aircraft, is the linear velocity of the aircraft in the body coordinate system, is the derivative of is the inertia matrix of the two-degree-of-freedom thrust vectoring double-rotor, is the x-axis rotational inertia, is the y-axis rotational inertia, is the z-axis rotational inertia, is the angular velocity in the body coordinate system, is the derivative of is the current quaternion of the two-degree-of-freedom thrust vectoring double-rotor, is the derivative of is the skew-symmetric matrix of , , is the transpose of , , , is the transpose of . is the real part of the quaternion, is the three variables of the imaginary part of the quaternion. There are

[0088] (4);

[0089] wherein, is a three-dimensional identity matrix, is defined as the antisymmetric matrix of , is defined as the resultant external force in the body coordinate system, is defined as the resultant external moment in the body coordinate system. The resultant external force includes the thrust force , the gravity and other internal and external disturbance forces . The gravity in the body coordinate system is calculated as , is the conjugate quaternion of . The resultant external moment includes the rotor moment , the counter-torque moment and other internal and external disturbance moments . It is expressed as

[0090] (5);

[0091] S3, according to the two-degree-of-freedom thrust vector dual-rotor dynamic model established in step S2, a control allocation equation is derived; the control allocation equation decomposes the dynamic model into a combination of matrices, and the rotation angles of the joints and the rotor speeds are solved through the control allocation equation.

[0092] The control allocation equation is:

[0093] (6);

[0094] wherein,

[0095] is a control allocation matrix, is a control allocation parameter matrix, is a control allocation input matrix.

[0096] .

[0097] wherein, is a rotor lift coefficient, is the distance between the rotor center and the second-stage servo joint coordinate system, is the distance between the first-stage servo joint coordinate system and the body coordinate system. is the rotation angle of the first-stage servo joint of the first arm, is the rotation angle of the first-stage servo joint of the second arm, is the rotation angle of the second-stage servo joint of the first arm, is the rotation angle of the second-stage servo joint of the second arm. is the rotor speed of the first arm, is the rotor speed of the 2nd manipulator. is the control input matrix. is the rotation matrix of the first level servo joint coordinate system of the 1st manipulator to the body coordinate system, is the rotation matrix of the first level servo joint coordinate system of the 2nd manipulator to the body coordinate system, is the rotation matrix of the second level servo joint coordinate system of the 1st manipulator to the first level servo joint coordinate system, is the rotation matrix of the second level servo joint coordinate system of the 2nd manipulator to the first level servo joint coordinate system. is the rotor thrust of the 2nd manipulator, is the rotor thrust of the 2nd manipulator. is the rotor thrust of the 1st manipulator in the body coordinate system, is the rotor thrust of the 2nd manipulator in the body coordinate system, is the position vector of the rotor center of the 1st manipulator relative to the body coordinate system, is the position vector of the rotor center of the 2nd manipulator relative to the body coordinate system.

[0098] Then , , , , , can be obtained by the following formula:

[0099] (8);

[0100] wherein, is the element of the control distribution input matrix .

[0101] S4, according to the dynamic model established in step S2, a pose cascade PID-ADRC controller is designed, the position cascade PID-ADRC controller includes a position outer loop error controller and a speed 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.

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

[0103] (9);

[0104] wherein, is the gain of the position outer loop. is the position expected value in the world coordinate system, is the current position value in the world coordinate system, ​It is the expected velocity value in the world coordinate system.

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

[0106] The speed inner loop of the position cascaded PID-ADRC controller can be represented in state-space form as follows:

[0107] (10);

[0108] in, These are the perturbations for the x, y, and z channels, respectively. These are the control values ​​for the x, y, and z channels, respectively. These are the parameters for the x, y, and z channels, respectively. For the mass of the aircraft, These are the derivatives of the velocities of the x, y, and z channels, respectively.

[0109] The extended state observer for each channel of the speed inner loop of the position cascaded PID-ADRC controller is written as follows:

[0110] (11);

[0111] in, For the gain of the extended state observer of each channel, The velocity value of the aircraft in the world coordinate system The observed values, It is the velocity disturbance value of the aircraft in the world coordinate system. The velocity of the aircraft in the world coordinate system. For speed error, for The derivative of for The derivative, These are the parameters for the x, y, and z channels.

[0112] The PID error feedback control law for each channel of the speed inner loop of the position cascaded PID-ADRC controller is written as follows:

[0113] (12);

[0114] in, It is the expected velocity value in the world coordinate system. The velocity value of the aircraft in the world coordinate system The observed values, For the uncompensated control quantities of each channel, The control quantity after compensation for each channel. These are the parameters for the xyz channels. For the error of each channel, is the error gain coefficient, is the error integral coefficient, is the error derivative coefficient; finally, the total thrust in the world coordinate system is . The thrust control quantity in the body coordinate system is .

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

[0116] (13);

[0117] where, is the desired quaternion, is the inverse of the desired quaternion, is the current quaternion of the two-degree-of-freedom thrust vector dual-rotor. is the error quaternion. The desired body angular velocity is generated by the vector part and the scalar part of the quaternion error, expressed as:

[0118] (14);

[0119] where, is the error quaternion coefficient, is the desired body angular velocity.

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

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

[0122] (15);

[0123] where, is the disturbance of each channel of the attitude angular velocity inner loop, is the control output of each channel of the attitude angular velocity inner loop. , is the moment of inertia of each channel of the attitude angular velocity inner loop. is the derivative of the xyz channel angular velocity.

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

[0125] (16);

[0126] where, is the error of the angular velocity, is the gain of the extended state observer of the angular velocity inner loop, is the observation value of the angular velocity inner loop of each channel, is the disturbance observation value of the angular velocity inner loop of each channel, is the derivative of the observation value of the angular velocity inner loop of each channel, is the derivative of the disturbance observation value of the angular velocity inner loop of each channel, is the current value of the angular velocity. is the parameter of the angular velocity of each channel.

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

[0128] (17);

[0129] wherein, is the expected body angular velocity, is the observation value of the angular velocity inner loop of each channel, is the disturbance observation value of the angular velocity inner loop of each channel, is the uncompensated control amount of each channel, is the control amount of the angular velocity inner loop of each channel, is the parameter of the angular velocity of each channel, is the error of the angular velocity inner loop of each channel, is the error gain coefficient of the angular velocity inner loop of each channel, is the error integral coefficient of the angular velocity inner loop of each channel, is the error differential coefficient of the angular velocity inner loop of each channel. Finally, the attitude thrust torque control amount .

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

[0131] The position cascade PID-ADRC controller obtains the position thrust control amount according to the position expected value of the two-degree-of-freedom thrust vector dual-rotor, the position and the speed of the arm two-degree-of-freedom deformation quad-rotor.

[0132] The attitude cascade PID-ADRC controller obtains the attitude thrust control amount according to the expected quaternion and the current quaternion The attitude thrust torque control quantity is obtained by attitude control of a two-degree-of-freedom thrust vector twin rotor. .

[0133] Control and distribution unit, two-degree-of-freedom thrust vector control for dual rotor position and thrust. With attitude thrust torque control The rotor speed of the first arm is finally obtained through control allocation. The rotor speed of the second arm The rotation angle of the first-stage servo joint of the first arm The rotation angle of the first-stage servo joint of the second arm The rotation angle of the second-stage servo joint of the first arm The rotation angle of the second-stage servo joint of the second arm .

[0134] 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 execute the program stored in the memory to implement the above-described two-degree-of-freedom thrust vector dual-rotor flight control method and achieve the same technical effect as the above method.

[0135] In one embodiment of the present invention, a storage medium is provided, on which a computer program is stored. When the computer program is executed by at least one processor, it implements the steps of the two-degree-of-freedom thrust vector dual-rotor flight control method and achieves the same technical effect as the method described above.

[0136] The embodiments of the present invention are described below.

[0137] The designed two-degree-of-freedom thrust vectoring twin rotor (such as...) Figure 3 The object shown is a MATLAB / Simulink model, and its effectiveness of the control method of this invention is verified through simulation. The specific parameters of the mathematical model of the two-degree-of-freedom thrust vector dual rotor are shown in Table 1.

[0138] Table 1 System Simulation Parameters

[0139]

[0140] Case 1: Simulation of independent attitude control for a two-degree-of-freedom thrust vector dual rotor;

[0141] In the simulation experiment, the initial pitch angle of the two-degree-of-freedom deformable quadrotor arm was set. The desired pitch angle is set at the 2nd second. The expected pitch angle at 40 seconds The added disturbance noise is

[0142] (18);

[0143] in, The disturbance torques are for the XYZ axes. Quaternion simulation results are as follows: Figure 6 As shown in (a) to (d), (a) is a quaternion. The curve of change, (b) is the quaternion in The curve of change, (c) is the quaternion in The change curve of , (d) is the quaternion The variation curve shows that the quaternions change continuously from 0° to ±90° in pitch, without any singularity issues. The Euler angle simulation results are as follows... Figure 7 As shown, singularities appeared in the roll and yaw angles during the pitch progression from 0° to ±90°. The simulation results for the disturbance torque are as follows... Figure 8 As shown in (a) to (c). Figure 8 In the middle (a), the curves showing the actual and estimated disturbance torque changes along the X-axis are shown. Figure 8 In the middle (b), the curves showing the actual and estimated disturbance torque changes along the Y-axis are shown. Figure 8 In Figure (c), the curves showing the actual and estimated disturbance torque changes along the Z-axis are presented. Simulation results demonstrate that the PID-ADRC control is effective, and the present invention can track the target value well. Furthermore, ADRC can observe the disturbance torque, and the quaternion method avoids the singularity problem of Euler angles, enabling fast and robust tracking of the desired attitude.

[0144] Simulation results show that the present invention can effectively improve the stability of attitude response when attitude changes.

[0145] Case 2: Simulation of independent position control for a two-DOF deformable quadrotor arm;

[0146] The desired position of the two-DOF deformable quadrotor arm in the simulation experiment is:

[0147] (19);

[0148] in, Given the desired XYZ axis position of the aircraft in the world coordinate system, the aircraft position response curve is as follows: Figure 9 As shown in the simulation results, the PID-ADRC can track the target at the desired position well. The present invention can track the target value well and can track the desired position quickly and robustly.

[0149] Simulation results show that the present invention can effectively improve the position response speed when the position changes.

Claims

1. A two-degree-of-freedom thrust vectoring dual-rotor flight control method, characterized in that, Includes the following steps: S1. Based on the characteristics of the two-degree-of-freedom pitch-and-thrust vector twin rotor, which has the ability to independently change attitude through attitude decoupling and can take off and land and operate on non-horizontal surfaces, analyze and design the structure of the two-degree-of-freedom thrust vector twin rotor. S2. Based on the two-degree-of-freedom thrust vector dual rotor structure, derive the dynamic model of the two-degree-of-freedom thrust vector dual rotor, where the dynamic model includes the three-axis resultant external force equation and the three-axis resultant external moment equation in quaternion form; S3. Based on the dynamic model of the two-degree-of-freedom thrust vector dual rotor, the control distribution equation is derived. The control distribution equation decomposes the dynamic model into a combination of matrices, and the joint rotation angle and rotor speed are solved by the control distribution equation. S4. Based on the dynamic model of the two-degree-of-freedom thrust vector dual rotor, design an attitude cascade PID-ADRC controller, 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. The two-degree-of-freedom thrust vector dual-rotor flight control method according to claim 1, characterized in that, The two-degree-of-freedom thrust vectoring dual-rotor structure in step S1 includes a fuselage and a two-degree-of-freedom thrust vectoring rotor. The two-degree-of-freedom thrust vectoring rotor structure is structured as follows: ； in, For the first The rotation angle of the first-stage servo joint of each arm Indicates the first One arm ; For the first The rotation angle of the second-stage servo joint of each arm; For the first The rotation matrix from the first-level servo joint coordinate system to the body coordinate system of each robotic arm. For the first The rotation matrix from the second-level servo joint coordinate system to the first-level servo joint coordinate system of each arm; The position vector of the rotor center relative to the airframe coordinate system for: ； in, This is the position vector of the rotor center in the second-stage servo joint coordinate system. This is the distance between the rotor center and the coordinate system of the second-stage servo joint; This is the position vector of the second-level servo joint coordinate system in the first-level servo joint coordinate system. This represents the distance between the first-level and second-level servo joint coordinate systems. This represents the position vector of the first-level servo joint coordinate system in the body coordinate system. This represents the distance between the first-level servo joint coordinate system and the body coordinate system.

3. The two-degree-of-freedom thrust vector dual-rotor flight control method according to claim 1, characterized in that, The dynamic model of the two-degree-of-freedom thrust vector twin rotor in step S2 is as follows: ； in, for The derivative of The linear velocity in the body coordinate system. Defined as the net external force in the body coordinate system. For the mass of the aircraft, for The derivative of Angular velocity in the body coordinate system The inertial matrix of a two-degree-of-freedom thrust vectoring twin rotor is... Let x be the moment of inertia along the x-axis. Let y be the moment of inertia. Let z be the moment of inertia along the z-axis. for The reverse, Defined as the net external torque in the body coordinate system. for The derivative of The current quaternion for the two-degree-of-freedom thrust vector dual rotor. , It is the real part of the quaternion. , These are the three variables of the imaginary part of a quaternion. for transpose, for An antisymmetric matrix.

4. The two-degree-of-freedom thrust vector dual-rotor flight control method according to claim 1, characterized in that, The control allocation equation in step S3 is: ； in, To control the allocation matrix, To control the allocation of parameter matrices, To control the allocation of input matrices; in, This is the rotor lift coefficient. This represents the distance between the rotor center and the coordinate system of the second-stage servo joint. This represents the distance between the first-level servo joint coordinate system and the body coordinate system. This refers to the rotation angle of the first-stage servo joint of the first arm. This refers to the rotation angle of the first-stage servo joint of the second arm. This refers to the rotation angle of the second-stage servo joint of the first arm. This refers to the rotation angle of the second-stage servo joint of the second arm. It is the rotor speed of the first arm. It is the rotor speed of the second arm. To control the input matrix.

5. The two-degree-of-freedom thrust vector dual-rotor flight control method according to claim 4, characterized in that, The following is obtained through the dynamic model: ； in, This is the rotation matrix from the first-level servo joint coordinate system of the first arm to the body coordinate system. This is the rotation matrix from the first-level servo joint coordinate system of the second arm to the body coordinate system. This is the rotation matrix from the second-level servo joint coordinate system to the first-level servo joint coordinate system of the first arm. This is the rotation matrix from the second-level servo joint coordinate system to the first-level servo joint coordinate system of the second arm. For the rotor thrust of the second arm, For the rotor thrust of the second arm, The rotor thrust of the first arm in the body coordinate system. The rotor thrust of the second arm in the body coordinate system. This is the position vector of the rotor center of the first arm relative to the fuselage coordinate system. This is the position vector of the rotor center of the second arm relative to the fuselage coordinate system; Then , , , , , It can be obtained using the following formula: ； in, To control the allocation of input matrices The Each element.

6. The two-degree-of-freedom thrust vector dual-rotor flight control method according to claim 1, characterized in that, The position outer loop error controller is designed as follows: ； in, The gain of the outer loop; It is the expected value of the position in the world coordinate system. It is the current value of the position in the world coordinate system. It is the expected velocity value in the world coordinate system; The speed inner loop PID-ADRC controller is designed as follows: The speed inner loop of the position cascaded PID-ADRC controller can be represented in state-space form: ； in, for , , Disturbance in the channel. for , , Channel control parameters; for , , Channel parameters, For the mass of the aircraft, for , , The derivative of the channel velocity; The extended state observer for each channel of the speed inner loop of the position cascaded PID-ADRC controller is written as follows: ； in, The gain of the extended state observer for each channel; The velocity value of the aircraft in the world coordinate system Observed values; It is the velocity disturbance value of the aircraft in the world coordinate system; The velocity of the aircraft in the world coordinate system; For speed error, for The derivative of for The derivative of for , , Channel parameters, ; The error feedback control law for each channel of the speed inner loop of the position cascaded PID-ADRC controller is written as: ； in, For the error of each channel, It is the expected velocity value in the world coordinate system. For the uncompensated control quantities of each channel, This is the error gain coefficient. For the error integral coefficient, These are the error differential coefficients. For each channel's control quantity; ultimately, the total thrust in the world coordinate system is The thrust control quantity in the body coordinate system is , This refers to the two-degree-of-freedom thrust vector control quantity for the dual rotor position thrust. The current quaternion for the two-degree-of-freedom thrust vector dual rotor. for The conjugate quaternion.

7. The two-degree-of-freedom thrust vector dual-rotor flight control method according to claim 1, characterized in that, The attitude outer loop error controller is designed as follows: ； in, For the expected quaternion, To find the inverse of the expected quaternion, The current quaternion for the two-degree-of-freedom thrust vector dual rotor. As an error quaternion, the desired body angular velocity is determined by the vector part of the quaternion error. and scalar part The generation is represented as: ； in, For the error quaternion coefficients, The desired angular velocity of the organism; The attitude angular velocity inner loop PID-ADRC controller is designed as follows: The attitude angular velocity inner loop of the attitude cascaded PID-ADRC controller can be represented in state-space form: ； in, Inner loop of attitude angular velocity , , Disturbance in the channel. Inner loop of attitude angular velocity , , Channel control output, , Inner loop of attitude angular velocity , , The rotational inertia of the channel, for , , The derivative of the channel angular velocity; The attitude angular velocity inner loop expansion state observer of the attitude cascade PID-ADRC controller is written as follows: ； in, The error is the angular velocity. This is the current value of the angular velocity. These are the observed values ​​for each channel of the inner loop of angular velocity. The derivatives of the observed values ​​for each channel of the inner loop of angular velocity are given. These are the disturbance observation values ​​for each channel of the inner loop of angular velocity. Let be the derivative of the disturbance observation values ​​for each channel of the inner loop of angular velocity. The gain of the extended state observer for the inner loop of angular velocity; The error feedback control law for each channel of the attitude angular velocity inner loop of the attitude cascade PID-ADRC controller is written as follows: ； in, The error of each channel in the inner loop of angular velocity. For the desired angular velocity of the body, For the uncompensated control quantities of each channel, This represents the error gain coefficient for each channel of the inner loop of angular velocity. For each channel of the inner loop of angular velocity, the error integral coefficient is... These are the error differential coefficients for each channel of the inner loop of angular velocity. This refers to the control outputs of each channel within the attitude angular velocity inner loop. Ultimately, the attitude thrust torque control quantity .

8. A two-degree-of-freedom thrust vectoring dual-rotor flight control device, characterized in that, include: Position cascaded PID-ADRC controller, based on the desired position of the two-degree-of-freedom thrust vector dual rotor. and location With speed The position thrust control amount is obtained by controlling the position of the two-degree-of-freedom deformable quadrotor arm. ; Attitude cascaded PID-ADRC controller, based on desired quaternion And the current quaternion of the two-degree-of-freedom thrust vectoring dual rotor. and current value of angular velocity The attitude thrust torque control quantity is obtained by attitude control of a two-degree-of-freedom thrust vector twin rotor. ; Control and distribution unit, two-degree-of-freedom thrust vector control for dual rotor position and thrust. With attitude thrust torque control The rotor speed of the first arm is finally obtained through control allocation. The rotor speed of the second arm The rotation angle of the first-stage servo joint of the first arm The rotation angle of the first-stage servo joint of the second arm The rotation angle of the second-stage servo joint of the first arm The rotation angle of the second-stage servo joint of the second arm .

9. A control device, characterized in that, The system 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; and the processor, when executing the program stored in the memory, implements the steps of the two-degree-of-freedom thrust vector dual-rotor attitude flight control method according to any one of claims 1-7.

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 vector dual-rotor flight control method as described in any one of claims 1-7.

Images