Arm two-degree-of-freedom deformation four-rotor flight control method and device

Abstract

The invention discloses an arm two-degree-of-freedom transformation quadrotor flight control method and device. The method comprises the following steps: S1, carrying out structural design on an arm two-degree-of-freedom transformation quadrotor; s2, deducing a kinetic model and a kinematic model of the two-degree-of-freedom transformation quadrotor of the arm according to the two-degree-of-freedom transformation quadrotor structure of the arm; s3, deducing a control distribution model of the two-degree-of-freedom deformation quadrotor of the arm according to the steps S1 and S2; s4, analyzing the singularity problem of a control distribution model according to the control distribution model of the two-degree-of-freedom deformation quadrotor of the arm derived in the step S3, and designing a special control distribution model for the singularity; and S5, designing a pose quaternion nonsingular terminal sliding mode flight controller according to the dynamical model, the kinematic model, the control distribution model and the special control distribution model of the arm two-degree-of-freedom deformation quadrotor. The method has good control and stabilization effects on the two-degree-of-freedom deformation four rotors of the arms.

Description

Technical Field

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

[0002] The control technology of multi-rotors is becoming more mature. In addition to performing normal tasks such as aerial photography and environmental monitoring, it can also be used in complex and narrow environments such as underground pipelines, natural caves, or disaster area search and rescue. However, in complex and narrow spaces, such as winding pipelines and complex jungles, the environment is changeable and full of various uncertainties, which can easily cause collision interference and crash for multi-rotors. Therefore, how to adapt to complex and narrow environments is a difficult problem. Conventional multi-rotors have a fixed structure and lack the deformation ability of arm rotors. Therefore, it is difficult to change its shape according to the external environment, and its adaptability in narrow environments is weak.

[0003] Conventional multi-rotors are underactuated systems, and the rotor thrust is always perpendicular to the fuselage surface, so vector thrust cannot be generated, which makes it impossible for traditional multi-rotors to decouple position and attitude. In addition, the conventional multi-rotor fuselage structure is fixed and lacks deformation ability, which makes it unadaptable to certain narrow environments. Therefore, enhancing the spatial deformation ability of multi-rotors to improve maneuverability and adaptability in complex environments is an important research issue.

[0004] At present, deformable multi-rotors can be divided into two categories: arm rotation deformation and rotor tilt deformation. The arm structure of the tilt-rotor multi-rotor is fixed, and it is impossible to reduce the physical size of the fuselage during flight. In addition, there may be redundancy in thrust when changing the attitude during flight, which greatly affects the movement efficiency. In addition, the arm-deformable multi-rotor lacks the tilting degree of freedom, cannot complete thrust vectoring, lacks the ability to move independently in spatial position and attitude, can only achieve deformation in a horizontal attitude, and has limited adaptability to space. In response to these problems, it is urgent to design a new type of aircraft that has both deformation and tilt. Summary of the invention

[0005] Purpose of the invention: One purpose of the present invention is to provide a flight control method for a quadrotor with two-degree-of-freedom deformation of the arm that can achieve good controllability and aerial tilt and deformation capability.

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

[0007] Technical solution: The invention discloses a method for controlling the flight of a quadrotor with a two-degree-of-freedom deformation of the arm, comprising the following steps:

[0008] S1. According to the characteristics of the two-degree-of-freedom deformation quadrotor of the machine arm, which can realize the decoupling of posture and independent tilting while being able to pass through horizontal or vertical narrow environments, the deformation mode of the two-degree-of-freedom deformation quadrotor of the machine arm is analyzed, and the structure of the two-degree-of-freedom deformation quadrotor of the machine arm is determined;

[0009] S2. Based on the arm's two-degree-of-freedom deformation quadrotor structure, derive the dynamic model and kinematic model of the arm's two-degree-of-freedom deformation quadrotor;

[0010] S3. Based on the dynamics model and kinematics model of the two-degree-of-freedom deformation quadrotor, a control allocation model of the two-degree-of-freedom deformation quadrotor is derived;

[0011] S4. According to the control allocation model of the two-degree-of-freedom deformation quadrotor, analyze the singularity problem of the control allocation model, and design a special control allocation model for the singularity;

[0012] S5. Design a posture quaternion non-singular terminal sliding mode flight controller based on the dynamic model, kinematic model, control allocation model and special control allocation model of the two-degree-of-freedom deformation quadrotor.

[0013] Furthermore, in step S1, the two-degree-of-freedom deformation quadrotor structure of the machine arm includes a machine body and a two-degree-of-freedom deformation machine arm, and the two-degree-of-freedom deformation machine arm structure is:

[0014]

[0015] Among them, α i is the yaw angle of the arm coordinate system of the i-th arm relative to the body coordinate system, i represents the i-th arm i=1,2,3,4; is the rotation matrix from the arm coordinate system to the body coordinate system of the i-th arm, is the rotation matrix from the coordinate system of the first joint of the i-th arm to the arm coordinate system, is the rotation matrix from the coordinate system of the second joint of the i-th arm to the first joint of the i-th arm, β i is the yaw rotation angle of the first joint of the i-th arm relative to the arm coordinate system, γ i is the roll rotation angle of the second-level joint of the i-th arm relative to the first-level joint coordinate system;

[0016] The first joint degrees of freedom of the first and third arms rotate in the same manner, and the first joint degrees of freedom of the second and fourth arms rotate in the same manner, and β1=β3=-β2=-β4 is satisfied.

[0017] The position vector of the second joint coordinate system relative to the body coordinate system for:

[0018]

[0019] in, is the position vector of the first joint coordinate system of the i-th arm relative to the arm coordinate system, l1 is the distance between the first joint coordinate system of the i-th arm and the arm coordinate system, is the position vector of the second joint coordinate system of the i-th arm relative to the first joint coordinate system, and l2 is the distance between the first joint coordinate system of the i-th arm and the arm coordinate system.

[0020] Furthermore, the dynamic model of the two-degree-of-freedom deformation quadrotor in step S2 is:

[0021]

[0022] Where m is the mass of the two-degree-of-freedom deformation quadrotor, v B is the linear velocity in the body coordinate system, v B The derivative of J = diag(J x ,J y ,J z ) is the inertia matrix of the two-degree-of-freedom deformation quadrotor, 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, ω B is the angular velocity in the body coordinate system, ω B The derivative of , q is the quaternion of the two-degree-of-freedom deformation quadrotor of the arm, q * is the conjugate quaternion of q, is the gravity in the world coordinate system, is the disturbance force, is the rotation matrix from the arm coordinate system to the body coordinate system of the i-th arm, is the rotation matrix from the coordinate system of the first joint of the i-th arm to the arm coordinate system, is the rotation matrix from the coordinate system of the second joint of the i-th arm to the coordinate system of the first joint of the i-th arm, is the lift of the i-th rotor in the second joint coordinate system, is the first joint coordinate system of the i-th arm Relative to the arm coordinate system The position vector of is the second joint coordinate system of the i-th arm Relative to the first joint coordinate system The position vector of k q is the rotor anti-torque coefficient, n i is the i-th rotor speed, is the interference torque.

[0023] Further, the dynamic model of the two-degree-of-freedom deformation quadrotor of the machine arm in step S2 includes a posture dynamic model and a kinematic model based on quaternion, and the posture dynamic model and kinematic model based on quaternion include an attitude kinematic and dynamic model based on quaternion, and a position dynamic and kinematic model based on quaternion;

[0024] Among them, the attitude kinematics and dynamics model based on quaternion is:

[0025]

[0026] Among them, q e =(q e0 ,q e1 ,q e2 ,q e3 ) T ,q e0 for q e The scalar part, q e1 for q e The second term, q e2 for q e The third term, q e3 for q e The fourth item, for q e The derivative of for q e The transpose of the vector part of is the angular velocity error of the machine system, f(ω B )=-J -1 ω B ×Jω B , is the angular velocity error, is the expected angular velocity of the machine system, d ω is the disturbance torque of attitude, u q is the input for posture dynamics.

[0027] The position dynamics and kinematics model based on quaternions is:

[0028]

[0029] Among them, v E is the linear velocity in the world coordinate system, p E is the position in the world coordinate system, For p E The derivative of B is the position of the machine system, is the acceleration in the world coordinate system, is the gravity in the world coordinate system, It is the disturbance force.

[0030] Furthermore, the control allocation model in step S3 is:

[0031]

[0032] in, is the static control allocation matrix, is the actuator matrix using intermediate actuator variables, is the rotation matrix from the arm coordinate system to the body coordinate system of the i-th arm, is the rotation matrix from the coordinate system of the first joint of the i-th arm to the arm coordinate system, is the rotation matrix from the coordinate system of the second joint of the i-th arm to the coordinate system of the first joint of the i-th arm, is the lift of the i-th rotor in the second joint coordinate system, is the first joint coordinate system of the i-th arm Relative to the arm coordinate system The position vector of is the second joint coordinate system of the i-th arm Relative to the first joint coordinate system The position vector of k m is the rotor anti-torque coefficient, n i is the i-th rotor speed;

[0033] The control output is solved as:

[0034]

[0035] in, for The pseudo-inverse calculation of is the thrust of the quadrotor with two degrees of freedom deformation of the arm in the body coordinate system, is the thrust torque;

[0036] The output of each executor is as follows

[0037]

[0038] in, for The square of the 2i-1th term of for The square of the 2ith term of for The square of the 2i+1th term of .

[0039] Furthermore, in step S4, the special control allocation model is designed as follows: When the aircraft passes through a horizontal narrow space, the special control allocation model is:

[0040]

[0041] According to the position control output, the thrust vector is calculated as:

[0042]

[0043] in, is the thrust in the world coordinate system under the X-axis singularity, are the X-axis and Z-axis components of the thrust in the world coordinate system

[0044] The normalized expected thrust vector is calculated as:

[0045]

[0046] in, is the normalized thrust vector in the world coordinate system;

[0047] The required rotation matrix obtained by the Rodriguez rotation method is:

[0048]

[0049] Among them, R d For I 3×3 for, It is a skew-symmetric matrix. It is expected that the rotation matrix can be converted into the expected quaternion Q d ;

[0050] When the aircraft passes through a vertically narrow space, the special control allocation model is:

[0051]

[0052] At this time, the thrust vector becomes:

[0053]

[0054] in, are the Y-axis and Z-axis components of the thrust in the world coordinate system;

[0055] The required rotation matrix is ​​obtained by the Rodriguez rotation method, that is

[0056]

[0057] in, is a skew-symmetric matrix; then, the desired rotation matrix is ​​converted to the desired quaternion q d .

[0058] Furthermore, a non-singular terminal sliding mode flight controller with quaternion position and attitude is designed, including:

[0059] The design of the attitude non-singular terminal sliding surface is:

[0060]

[0061] Among them, s q is the attitude non-singular terminal sliding surface, is the attitude error quaternion q e The vector part of for The derivative of q is the adjustable parameter of the attitude non-singular terminal sliding mode controller, q q is the adjustable parameter of the attitude non-singular terminal sliding mode controller, β q =diag(β q,1 ,β q,2 ,β q,3 ), βq,1, βq,2, βq,3 are the adjustable parameters of attitude non-singular terminal sliding mode controller;

[0062] The attitude non-singular terminal sliding mode control law is designed as:

[0063]

[0064] Among them, u q is the output of the attitude non-singular terminal sliding mode control law, for The inverse, for The derivative of q is the control parameter of attitude non-singular terminal sliding mode control, δ q is the disturbance control parameter of the attitude non-singular terminal sliding mode control law.

[0065] The design of the position non-singular terminal sliding surface is:

[0066]

[0067] in, for The derivative of p is the adjustable parameter of the position non-singular terminal sliding surface, q p is the adjustable parameter of the position non-singular terminal sliding surface;

[0068] Among them, β p =diag(β x ,β y ,β z ), β x ,βy ,β z is an adjustable parameter;

[0069] The position non-singular terminal sliding mode control law is designed as:

[0070]

[0071] Among them, u p is the output of the position non-singular terminal sliding mode control law, for The second derivative of p is the control parameter of the position non-singular terminal sliding mode control law, δ p is the disturbance control parameter of the position non-singular terminal sliding mode control law, is the gravity in the world coordinate system.

[0072] The invention provides a quadrotor flight control device with two-degree-of-freedom deformation of the aircraft arm, comprising:

[0073] The position non-singular terminal sliding mode control unit is based on the expected position value P of the quadrotor with two degrees of freedom deformation of the arm. d , and position P E With speed v E , the position control quantity F is obtained by the position control of the two-degree-of-freedom deformation quadrotor of the machine arm d , and then the expected quaternion q of the aerial robot is obtained based on the expected posture transformation of the first joint d ;

[0074] The attitude quaternion non-singular terminal sliding mode control unit is based on the expected quaternion q d As well as the actual attitude quaternion q and angular velocity ω of the two-degree-of-freedom deformation quadrotor B , the attitude control value M is obtained by the attitude control of the two-degree-of-freedom deformation quadrotor of the machine arm d .

[0075] Control distribution unit, arm two-degree-of-freedom deformation quadrotor position control quantity F d and attitude control amount M d The final rotor speed n is obtained by controlling the distributor i The roll rotation angle γ of the second joint of the i-th arm relative to the first joint coordinate system i , the yaw rotation angle β of the i-th arm i By active input.

[0076] 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 deformation quadrotor flight control method when executing the program stored in the memory.

[0077] The present invention discloses 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 deformation quadcopter flight control method are implemented.

[0078] Beneficial effects: Compared with the prior art, the significant technical effects of the present invention are:

[0079] (1) By dynamically changing the structure of the arms, the decoupled independent control of the quadrotor position and attitude is achieved, allowing the drone to "shrink" to pass through narrow horizontal or vertical environments, thus having stronger environmental adaptability.

[0080] (2) The UAV can accurately adjust its fuselage attitude while in a fixed-point hovering state, thereby achieving free changes in the attitude angle of the mission payload;

[0081] (3) Quaternion dynamics modeling and kinematics modeling were performed on the two-degree-of-freedom deformation quadrotor, avoiding the singularity problem caused by Euler angle modeling, which provided a good reference for the research on controllers;

[0082] (4) According to the characteristics of the quadrotor multi-actuator with two degrees of freedom of arm deformation, a control allocation model for the quadrotor with two degrees of freedom of arm deformation is designed, and a special control allocation model is realized in the singular state;

[0083] (5) In order to solve the problems of nonlinearity and external disturbance of the two-degree-of-freedom deformable quadrotor, a non-singular terminal sliding mode attitude controller is designed. It can not only overcome the influence of the system non-rigidity and parameter uncertainty of the two-degree-of-freedom deformable quadrotor itself on the controller, but also play a good role in suppressing the unknown external disturbances to the aircraft body during flight, thereby improving the robustness and reliability of the attitude controller of the two-degree-of-freedom deformable quadrotor. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0085] Figure 2 It is a schematic diagram of a two-degree-of-freedom deformation quadrotor targeted by the present invention;

[0086] Figure 3This is a structural diagram of a quadrotor arm with two degrees of freedom deformation targeted by the present invention;

[0087] Figure 4 A schematic diagram of a two-degree-of-freedom deformation quadrotor of the machine arm of the method of the present invention passing through a horizontal narrow environment;

[0088] Figure 5 A schematic diagram of a two-degree-of-freedom deformation quadrotor of the machine arm of the method of the present invention passing through a vertical narrow environment;

[0089] Figure 6 A schematic diagram of the control system implementation of the method of the present invention;

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

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

[0092] Fig. 9 It is a position change simulation curve diagram of the method of the present invention under independent position control in embodiment 1;

[0093] Fig.10 It is a posture simulation curve diagram of the method of the present invention under horizontal posture and arm deformation in embodiment 1;

[0094] Fig.11 It is a posture simulation curve diagram of the method of the present invention under vertical posture and arm deformation in embodiment 1. DETAILED DESCRIPTION

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

[0096] like Figure 1 As shown, the flight control method of the quadrotor with two-degree-of-freedom deformation of the arm of the present invention mainly includes the following steps:

[0097] S1. According to the characteristics of the two-degree-of-freedom deformation quadrotor of the machine arm, which can realize the decoupling of posture and independent tilting while being able to pass through horizontal or vertical narrow environments, the deformation mode of the two-degree-of-freedom deformation quadrotor of the machine arm is analyzed, and the structure of the two-degree-of-freedom deformation quadrotor of the machine arm is designed;

[0098] The schematic diagram of the two-degree-of-freedom deformation quadrotor is as follows Figure 2 As shown, the aerial robot consists of a body and a two-degree-of-freedom deformable arm. The arm structure is as follows Figure 3As shown: (a) The yaw angle of the arm coordinate system of the i-th arm relative to the body coordinate system is defined as α i , Where i represents the i-th arm i=1,2,3,4; (b) The first joint degrees of freedom of arms 1 and 3 rotate in the same way, and the first joint degrees of freedom of arms 2 and 4 rotate in the same way. And β1=β3=-β2=-β4; (c) The yaw rotation angle of the first joint of the i-th arm relative to the arm coordinate system is β i , the roll rotation angle of the second joint of the i-th arm relative to the first joint coordinate system is γ i .definition is the rotation matrix from the body coordinate system to the world coordinate system, is the rotation matrix from the arm coordinate system to the body coordinate system of the i-th arm, is the rotation matrix from the coordinate system of the first joint of the i-th arm to the arm coordinate system of the i-th arm, is the rotation matrix from the coordinate system of the second joint of the i-th arm to the coordinate system of the first joint of the i-th arm.

[0099]

[0100] The position vector of the second joint coordinate system relative to the body coordinate system for

[0101]

[0102] in, is the position vector of the first joint coordinate system of the i-th arm relative to the arm coordinate system, l1 is the distance between the first joint coordinate system of the i-th arm and the arm coordinate system, is the position vector of the second joint coordinate system of the i-th arm relative to the first joint coordinate system, and l2 is the distance between the first joint coordinate system of the i-th arm and the arm coordinate system.

[0103] S2. According to the two-degree-of-freedom deformation quadrotor designed in step S1, a dynamic model and a kinematic model of the two-degree-of-freedom deformation quadrotor are derived;

[0104] The dynamic model of the quadrotor with two degrees of freedom deformation of the arm is:

[0105]

[0106] Among them, F E is the total external force in the world coordinate system, M B is the total external torque in the body coordinate system, v B is the linear velocity in the body coordinate system, v BThe derivative of B is the angular velocity in the body coordinate system, ω B The derivative of , m is the mass of the two-degree-of-freedom deformation quadrotor of the arm, q is the quaternion of the two-degree-of-freedom deformation quadrotor of the arm, q * is the conjugate quaternion of q. J = diag(J x ,J y ,J z ) is the inertia matrix of the two-degree-of-freedom deformation quadrotor, 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.

[0107] F E It consists of three parts: thrust, gravity and external interference, expressed as:

[0108]

[0109] in, is the disturbance force, is the gravity in the world coordinate system, g is the gravitational acceleration, is the thrust of the two-degree-of-freedom deformation quadrotor QTDA in the world coordinate system, expressed as:

[0110]

[0111] in, is the thrust of the i-th rotor in the body coordinate system, is the lift of the i-th rotor in the second joint coordinate system, k f is the rotor lift coefficient, n i is the i-th rotor speed. The total external torque M in the body coordinate system B Expressed as

[0112]

[0113] in, is the thrust torque, is the counter torque, is the interference torque.

[0114] It is expressed in the body coordinate system as:

[0115]

[0116] in, is the external torque of the i-th rotor in the body coordinate system.

[0117] It is expressed in the body coordinate system as:

[0118]

[0119] in,

[0120]

[0121] Among them, k m is the rotor anti-torque coefficient, Q i is the anti-torque moment of the i-th rotor, is the anti-torque torque vector of the i-th rotor in the second joint coordinate system.

[0122] In summary, the kinetic model is:

[0123]

[0124] The kinematic model of the two-degree-of-freedom deformation quadrotor includes attitude kinematics and position kinematics, as follows:

[0125] The attitude kinematics model based on quaternion is:

[0126]

[0127] in, for The derivative of is the vector part of q, for The transpose of q0 is the scalar part of q, ω B is the angular velocity of the machine system, is the vector part of the quaternion, and ||q||=1.

[0128] F q for

[0129]

[0130] Among them, I3 is a three-dimensional unit vector.

[0131] Attitude error quaternion q e is the unit quaternion of the body coordinate system relative to the desired coordinate system, expressed as:

[0132]

[0133] in, is the conjugate of the desired quaternion. is quaternion multiplication. Therefore, q e =(q e0 ,q e1 ,q e2 ,qe3 ) T , where q e0 for q e The scalar part, q e1 for q e The second term, q e2 for q e The third term, q e3 for q e The fourth term of . The attitude kinematics equation based on quaternion is:

[0134]

[0135] in, for q e The derivative of for q e The transpose of the vector part of is the angular velocity error of the machine system. for

[0136]

[0137] The attitude dynamics and kinematics model based on quaternion is obtained as follows:

[0138]

[0139] in, ω B The derivative of q is the input of attitude kinematics, f(ω B )=-J -1 ω B ×Jω B , is the angular velocity error. is the expected angular velocity of the machine system. ω is the disturbance torque of the attitude.

[0140] The position dynamics and kinematics model based on quaternion is obtained as follows:

[0141]

[0142] Among them, v E is the linear velocity in the world coordinate system, p E is the position in the world coordinate system, For p E The derivative of B is the position of the machine system, is the acceleration in the world coordinate system, is the gravity in the world coordinate system, It is the disturbance force.

[0143] S3. According to the two-degree-of-freedom deformation quadrotor dynamics of the arm established in step S2, a control allocation model is derived.

[0144] The actuators of the two-degree-of-freedom deformation quadrotor include four rotors and eight servos. The force and torque required for each channel can be decoupled from the system by changing the corresponding control output. The dynamic model is re-expressed as:

[0145]

[0146] in, is the thrust of the two-degree-of-freedom deformation quadrotor in the body coordinate system, A is the control allocation matrix, and N is the actuator matrix, which can be expressed as:

[0147]

[0148] in, is the square of the first rotor speed, is the square of the second rotor speed, is the square of the third rotor speed, is the square of the fourth rotor speed.

[0149] There is β in A i and γ i variables, but they can be adjusted to transform into active or passive control quantities. Therefore, the yaw rotation angle β of the first joint of the i-th arm i Considered as the active actuator variable, the second joint roll rotation angle γ i is regarded as a passive actuator variable and is regulated by the position and attitude control output. i and γ i Therefore, the intermediate actuator variable method can be used to separate the passive actuator variables from the dynamic allocation matrix, so that according to γ i The speed of the i-th rotor n i This breaks down into:

[0150]

[0151] in, is the velocity decomposition of the i-th rotor, is the square of the rotation speed of the i-th rotor.

[0152] The executor matrix using intermediate executor variables can be reformulated as:

[0153]

[0154] The control allocation equation is rewritten as:

[0155]

[0156] in, is the static control allocation matrix, expressed as:

[0157]

[0158] Where l1 is the distance between the first joint coordinate system of the i-th arm and the arm coordinate system, l2 is the distance between the first joint coordinate system of the i-th arm and the arm coordinate system, k m is the rotor anti-torque coefficient.

[0159] Next, The pseudo-inverse of The control output is solved as:

[0160]

[0161] The output of each executor is as follows

[0162]

[0163] in, for The square of the 2i-1th term of for The square of the 2ith term of for The square of the 2i+1th term of .

[0164] S4. Design a special control allocation model according to the control allocation equation 20 established in step S3.

[0165] When the aircraft passes through a horizontal narrow space such as Figure 4 When the horizontal length is the smallest at this time for The first row and the kth column of . It can be seen that The first row of does not have full rank, so The X-axis degree of freedom is lost. The X-axis thrust under this system can be obtained The Y-axis thrust of the machine system Since the cosine terms are all 1, the maximum thrust can be obtained. Therefore, there is no thrust that cancels each other out, and the flight efficiency is the highest. However, since the degree of freedom of the X-axis is lost, the control allocation matrix needs to be specially processed. The rewritten control allocation matrix is:

[0166]

[0167] in, Control assignment matrix for the X-axis singularity.

[0168] At this time, the control strategy of the X-axis position becomes the pitch attitude coupling control. At this time, part of the expected quaternion comes from the output command of the X-axis position, so it is necessary to add the conversion equation of position and attitude. According to the position control output, the calculation formula of the thrust vector is

[0169]

[0170] in, is the thrust in the world coordinate system under the X-axis singularity, are the X-axis and Z-axis components of the thrust in the world coordinate system.

[0171] In order to make the thrust vector consistent with the desired thrust vector, the aircraft's attitude needs to be adjusted so that the aircraft's Z axis is aligned with the Therefore, the normalized expected thrust vector calculation formula is:

[0172]

[0173] in, is the normalized thrust vector in the world coordinate system.

[0174] Current fuselage vector The rotation axis of can be obtained by

[0175]

[0176] Where Θ is the vector around the fuselage The angle of the rotation axis.

[0177] Desired thrust vector Available The required rotation angle of the thrust vector is The desired rotation matrix can be obtained by the Rodriguez rotation method, that is

[0178]

[0179] Among them, R d is the desired rotation matrix, I 3×3 is the three-dimensional identity matrix, is a skew-symmetric matrix. In addition, the desired rotation matrix can be converted to the desired quaternion q d .

[0180] When the aircraft passes through a vertically narrow space such as Figure 5 When , the optimal mode of the aircraft is At this time there is for The 2nd row and kth column of . It can be seen that The second row of is not full rank, so The Y-axis degrees of freedom are lost. Therefore, special treatment of the control assignment matrix is ​​required. The rewritten Y-axis singular control assignment matrix is for:

[0181]

[0182] The control strategy for the Y position is now coupled to the roll attitude. Part of the required quaternion now comes from the output command for the Y position, so the transformation equations for position and attitude need to be added. At this point, the thrust vector under the Y singular becomes:

[0183]

[0184] in, are the Y-axis and Z-axis components of the thrust in the world coordinate system.

[0185] Expected thrust vectoring available The rotation angle between the current fuselage Z-axis vector and the required thrust vector is The required rotation matrix can be obtained by the Rodriguez rotation method, that is

[0186]

[0187] in, is a skew-symmetric matrix. Then, the desired rotation matrix can be converted to the desired quaternion q d .

[0188] S5. According to the dynamic model, kinematic model and control allocation model of the two-degree-of-freedom deformation quadrotor established in step S3, a posture quaternion non-singular terminal sliding mode flight controller is designed.

[0189] The design of the attitude non-singular terminal sliding surface is:

[0190]

[0191] Among them, s q is the attitude non-singular terminal sliding surface, is the attitude error quaternion q e The vector part of for The derivative of q is the adjustable parameter of the attitude non-singular terminal sliding mode controller, q q is the adjustable parameter of the attitude non-singular terminal sliding mode controller, β q =diag(β q,1 ,β q,2 ,βq,3 ),β q,i >0,β q,1 , β q,2 , β q,3 is the adjustable parameter of attitude quaternion non-singular terminal sliding mode controller,

[0192] The attitude non-singular terminal sliding mode control law is designed as:

[0193]

[0194] Among them, u q is the output of the attitude non-singular terminal sliding mode control law, for The inverse, for The derivative of q is the control parameter of attitude non-singular terminal sliding mode control, δ q is the disturbance control parameter of the attitude non-singular terminal sliding mode control law.

[0195] The Lyapunov function is chosen as:

[0196]

[0197] Then there are:

[0198]

[0199] in, V q The derivative of For q The derivative of is the angular velocity error, for The derivative of is the expected angular velocity of the machine system, f(ω B )=-J -1 ω B ×Jω B For q is the output of the attitude non-singular terminal sliding mode control law, d ω is the disturbance torque of the attitude. for

[0200]

[0201] and

[0202]

[0203] Among them, δ q1 is the first term of the disturbance control parameter of the attitude non-singular terminal sliding mode control law, δq2 is the second term of the disturbance control parameter of the attitude non-singular terminal sliding mode control law, δ q3 It is the third term of the disturbance control parameter of the attitude non-singular terminal sliding mode control law.

[0204] Then there is

[0205]

[0206] because is a diagonal matrix whose diagonal elements are all positive. Therefore,

[0207]

[0208] choose as the ideal position and velocity in the world coordinate system. The position error is expressed as

[0209]

[0210] in, is the position error in the world coordinate system, p E is the position in the world coordinate system, is the ideal position in the world coordinate system, is the velocity error in the world coordinate system, v E is the velocity in the world coordinate system, is the ideal velocity in the world coordinate system.

[0211] The design of the position non-singular terminal sliding surface is:

[0212]

[0213] in, for The derivative of p is the adjustable parameter of the position non-singular terminal sliding surface, q p is an adjustable parameter of the position non-singular terminal sliding surface.

[0214] Among them, β p =diag(β x ,β y ,β z ),β x >0,β y >0,β z >0,β x ,β y ,β z It is an adjustable parameter.

[0215] The position non-singular terminal sliding mode control law is designed as:

[0216]

[0217] Among them, u p is the output of the position non-singular terminal sliding mode control law, for The second derivative of p is the control parameter of the position non-singular terminal sliding mode control law, δ p is the disturbance control parameter of the position non-singular terminal sliding mode control law, is the gravity in the world coordinate system.

[0218] The Lyapunov function is chosen as:

[0219]

[0220] Then there is

[0221]

[0222] in, V p The derivative of is the position non-singular terminal sliding surface s p The transpose of For p The derivative of The derivative of for The second-order derivative of for The second-order derivative of is the position p in the world coordinate system E The second derivative of .

[0223]

[0224] Among them, δ x is the first term in the disturbance control parameter of the position non-singular terminal sliding mode control law, δ y is the second term in the disturbance control parameter of the position non-singular terminal sliding mode control law, δ z is the third term in the disturbance control parameter of the position non-singular terminal sliding mode control law.

[0225] Then,

[0226]

[0227] because is a diagonal matrix with every diagonal element being positive. So

[0228]

[0229] Therefore, the stability of the quaternion non-singular terminal sliding mode attitude controller is demonstrated.

[0230] According to the quaternion non-singular terminal sliding mode attitude controller designed in the above step S5, a control system implementation schematic diagram is established, as shown in FIG. Figure 6 As shown, the arm two-degree-of-freedom deformation quadrotor includes position non-singular terminal sliding mode control, attitude quaternion non-singular terminal sliding mode control, and control allocation; wherein:

[0231] Position non-singular terminal sliding mode control, according to the expected position value P of the quadrotor with two degrees of freedom deformation of the arm d , and position P E With speed v E , the position control quantity F is obtained by the position control of the two-degree-of-freedom deformation quadrotor of the machine arm d , and then the expected quaternion q of the aerial robot is obtained based on the expected posture transformation of the first joint d ;

[0232] Attitude quaternion non-singular terminal sliding mode control, according to the expected quaternion q d As well as the actual attitude quaternion q and angular velocity ω of the two-degree-of-freedom deformation quadrotor B , the attitude control value M is obtained by the attitude control of the two-degree-of-freedom deformation quadrotor of the machine arm d .

[0233] Control distribution, arm two-degree-of-freedom deformation quadrotor position control quantity F d and attitude control amount M d The final rotor speed n is obtained by controlling the distributor i The roll rotation angle γ of the second joint of the i-th arm relative to the first joint coordinate system i , the yaw rotation angle β of the i-th arm i By active input.

[0234] 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 deformation of the arm four-rotor flight control method when executing the program stored in the memory, and achieve the same technical effect as the above-mentioned method.

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

[0236] The designed two-degree-of-freedom deformable quadrotor (such as Figure 2The specific parameters of the mathematical model of the two-degree-of-freedom deformation quadrotor of the machine arm are shown in Table 1:

[0237] Table 1 System simulation parameters

[0238]

[0239] Case 1: Simulation of independent attitude control of quadrotor with two-degree-of-freedom deformation of the arm;

[0240] In the simulation experiment, the initial roll angle of the two-degree-of-freedom deformation quadrotor is set to φ = 0°, the expected roll angle at the first second is φ = 90°, the expected roll angle at the fifth second is φ = 0°, and the expected roll angle at the tenth second is φ = -90°. The simulation results are shown in Figure 7 As shown in (a) to (d) and Figure 8 As shown in (a) to (c), the simulation results show that the NTSM controller responds better to PID and SMC, and the present invention can track the target value well. At the same time, the quaternion method can avoid the singularity problem of Euler angles and can track the desired posture quickly and robustly.

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

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

[0243] The expected position of the two-degree-of-freedom deformation quadrotor designed in the simulation experiment is

[0244]

[0245] The body position response curve is as follows Fig. 9 As shown in (a) to (c), the simulation results show that the NTSM controller position response PID and SMC are better, the present invention can track the target value well, and can quickly and robustly track the desired position.

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

[0247] Case 3: Motion simulation of the quadrotor with two degrees of freedom deformation of the arm in horizontal attitude and independent position of the arm deformation;

[0248] In the simulation experiment, the initial arm state of the two-degree-of-freedom deformation quadrotor is designed to be β1=β3=45°, β2=β4=-45°. The expected position of the Y axis is set to 1m at the 2nd second, the expected position of the X axis is set to 1m at the 12th second, and the arm state is set to β1=β3=-45°, β2=β4=45° at the 10th second. The simulation results are shown in Fig.10 As shown in (a) to (d), the simulation results show that the NTSM controller still has a good control effect when the horizontal attitude arm is deformed.

[0249] The simulation results show that the present invention can maintain good posture control capability in a horizontal posture and with the arm deformed.

[0250] Case 4: Arm two-degree-of-freedom deformation quadrotor in vertical attitude, independent position motion simulation of arm deformation;

[0251] In the simulation experiment, the initial arm state of the two-degree-of-freedom deformation quadrotor is designed to be β1=β3=45°, β2=β4=-45°. At the 20th second, the arm state is set to β1=β3=-45°, β2=β4=45°; at the 2nd second, the expected pitch angle is set to 90°, at the 15th second, the expected pitch angle is set to 0°, at the 20th second, the expected roll angle is set to 0°, and at the 35th second, the expected roll angle is set to 0°; at the 5th second, the expected position of the Y axis is set to 1m, and at the 25th second, the expected position of the X axis is set to 1m; the simulation results are shown in Figure 2. Fig.11 As shown in (a) to (d), the simulation results show that the NTSM controller still has a good control effect under the deformation of the vertical attitude arm.

[0252] Simulation results show that the present invention can maintain good posture control capability in a vertical posture and when the machine arm is deformed.

Claims

1. A flight control method for a quadrotor with two-degree-of-freedom deformation of the arm, characterized in that: The following steps are involved: S1. According to the characteristics of the two-degree-of-freedom deformation quadrotor of the machine arm, which can realize the decoupling of posture and independent tilting while being able to pass through horizontal or vertical narrow environments, the deformation mode of the two-degree-of-freedom deformation quadrotor of the machine arm is analyzed, and the structure of the two-degree-of-freedom deformation quadrotor of the machine arm is determined; S2. Based on the arm's two-degree-of-freedom deformation quadrotor structure, derive the dynamic model and kinematic model of the arm's two-degree-of-freedom deformation quadrotor; S3. Based on the dynamics model and kinematics model of the two-degree-of-freedom deformation quadrotor, a control allocation model of the two-degree-of-freedom deformation quadrotor is derived; S4. According to the control allocation model of the two-degree-of-freedom deformation quadrotor, analyze the singularity problem of the control allocation model, and design a special control allocation model for the singularity; S5. Design a posture quaternion non-singular terminal sliding mode flight controller based on the dynamic model, kinematic model, control allocation model and special control allocation model of the two-degree-of-freedom deformation quadrotor.

2. The flight control method of a quadrotor with two-degree-of-freedom deformation of the arm according to claim 1, characterized in that: In step S1, the two-degree-of-freedom deformation quadrotor structure includes a body and a two-degree-of-freedom deformation arm. The two-degree-of-freedom deformation arm structure is as follows: Among them, α i is the yaw angle of the arm coordinate system of the i-th arm relative to the body coordinate system, i represents the i-th arm i=1,2,3,4; is the rotation matrix from the arm coordinate system to the body coordinate system of the i-th arm, is the rotation matrix from the coordinate system of the first joint of the i-th arm to the arm coordinate system, is the rotation matrix from the coordinate system of the second joint of the i-th arm to the first joint of the i-th arm, β i is the yaw rotation angle of the first joint of the i-th arm relative to the arm coordinate system, γ i is the roll rotation angle of the second-level joint of the i-th arm relative to the first-level joint coordinate system; The first joint degrees of freedom of the first and third arms rotate in the same manner, and the first joint degrees of freedom of the second and fourth arms rotate in the same manner, and β1=β3=-β2=-β4 is satisfied. The position vector of the second joint coordinate system relative to the body coordinate system for: in, is the position vector of the first joint coordinate system of the i-th arm relative to the arm coordinate system, l1 is the distance between the first joint coordinate system of the i-th arm and the arm coordinate system, is the position vector of the second joint coordinate system of the i-th arm relative to the first joint coordinate system, and l2 is the distance between the first joint coordinate system of the i-th arm and the arm coordinate system.

3. The flight control method of a quadrotor with two-degree-of-freedom deformation of the arm according to claim 1, characterized in that: The dynamic model of the two-degree-of-freedom deformation quadrotor in step S2 is: Where m is the mass of the two-degree-of-freedom deformation quadrotor, v B is the linear velocity in the body coordinate system, v B The derivative of J = diag(J x ,J y ,J z ) is the inertia matrix of the two-degree-of-freedom deformation quadrotor, 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, ω B is the angular velocity in the body coordinate system, ω B The derivative of , q is the quaternion of the two-degree-of-freedom deformation quadrotor of the arm, q * is the conjugate quaternion of q, is the gravity in the world coordinate system, is the disturbance force, is the rotation matrix from the arm coordinate system to the body coordinate system of the i-th arm, is the rotation matrix from the coordinate system of the first joint of the i-th arm to the arm coordinate system, is the rotation matrix from the coordinate system of the second joint of the i-th arm to the coordinate system of the first joint of the i-th arm, is the lift of the i-th rotor in the second joint coordinate system, is the first joint coordinate system of the i-th arm Relative to the arm coordinate system The position vector of is the second joint coordinate system of the i-th arm Relative to the first joint coordinate system F N1,i The position vector of k q is the rotor anti-torque coefficient, n i is the i-th rotor speed, is the interference torque.

4. The flight control method of a quadrotor with two-degree-of-freedom deformation of the arm according to claim 1, characterized in that: In step S2, the kinematic and dynamic models of the two-degree-of-freedom deformation quadrotor of the machine arm include a quaternion-based position dynamics model and a kinematic model, and the quaternion-based position dynamics model and kinematic model include a quaternion-based attitude kinematics and dynamics model, and a quaternion-based position dynamics and kinematics model; Among them, the attitude kinematics and dynamics model based on quaternion is: Among them, q e =(q e0 ,q e1 ,q e2 ,q e3 ) T , qe0 is the scalar part of qe, qe1 is the second term of qe, qe2 is the third term of qe, q e3 Q e The fourth item, Q e The derivative of Q e The transpose of the vector part of is the angular velocity error of the machine system, f(ω B )=-J -1 ω B ×Jω B , is the angular velocity error, is the expected angular velocity of the machine system, d ω is the disturbance torque of attitude, u q is the input for posture dynamics. The position dynamics and kinematics model based on quaternions is: Among them, v E is the linear velocity in the world coordinate system, p E is the position in the world coordinate system, For p E The derivative of B is the position of the machine system, is the acceleration in the world coordinate system, is the gravity in the world coordinate system, It is the disturbance force.

5. The flight control method of a quadrotor with two-degree-of-freedom deformation of the arm according to claim 1, characterized in that: The control allocation model in step S3 is: in, is the static control allocation matrix, is the actuator matrix using intermediate actuator variables, is the rotation matrix from the arm coordinate system to the body coordinate system of the i-th arm, is the rotation matrix from the coordinate system of the first joint of the i-th arm to the arm coordinate system, is the rotation matrix from the coordinate system of the second joint of the i-th arm to the coordinate system of the first joint of the i-th arm, is the lift of the i-th rotor in the second joint coordinate system, is the first joint coordinate system of the i-th arm Relative to the arm coordinate system The position vector of is the second joint coordinate system of the i-th arm Relative to the first joint coordinate system The position vector of k m is the rotor anti-torque coefficient, n i is the i-th rotor speed; The control output is solved as: in, for The pseudo-inverse calculation of is the thrust of the quadrotor with two degrees of freedom deformation of the arm in the body coordinate system, is the thrust torque; The output of each executor is as follows in, for The square of the 2i-1th term of for The square of the 2ith term of for The square of the 2i+1th term of .

6. The flight control method of a quadrotor with two-degree-of-freedom deformation of the arm according to claim 1, characterized in that: The special control allocation model is designed in step S4 as follows: When the aircraft passes through a horizontal narrow space, the special control allocation model is: According to the position control output, the thrust vector is calculated as: in, is the thrust in the world coordinate system under the X-axis singularity, are the X-axis and Z-axis components of the thrust in the world coordinate system The normalized expected thrust vector is calculated as: in, is the normalized thrust vector in the world coordinate system; The required rotation matrix obtained by the Rodriguez rotation method is: Among them, R d For I 3×3 for, × is a skew-symmetric matrix, and the expected rotation matrix can be converted to the expected quaternion q d ; When the aircraft passes through a vertically narrow space, the special control allocation model is: At this time, the thrust vector becomes: in, are the Y-axis and Z-axis components of the thrust in the world coordinate system; The required rotation matrix is ​​obtained by the Rodriguez rotation method, that is in, × is a skew-symmetric matrix; then, the desired rotation matrix is ​​converted to the desired quaternion q d .

7. The flight control method of a quadrotor with two-degree-of-freedom deformation of the arm according to claim 1, characterized in that: Design a non-singular terminal sliding mode flight controller for attitude quaternion, including: The design of the attitude non-singular terminal sliding surface is: Among them, s q is the attitude non-singular terminal sliding surface, is the attitude error quaternion q e The vector part of for The derivative of q is the adjustable parameter of the attitude non-singular terminal sliding mode controller, q q is the adjustable parameter of the attitude non-singular terminal sliding mode controller, β q =diag(β q,1 ,β q,2 ,β q,3 ), βq,1, βq,2, βq,3 are the adjustable parameters of attitude non-singular terminal sliding mode controller; The attitude non-singular terminal sliding mode control law is designed as: Among them, u q is the output of the attitude non-singular terminal sliding mode control law, F qe The inverse, F qe The derivative of q is the control parameter of attitude non-singular terminal sliding mode control, δ q is the disturbance control parameter of the attitude non-singular terminal sliding mode control law. The design of the position non-singular terminal sliding surface is: in, for The derivative of p is the adjustable parameter of the position non-singular terminal sliding surface, q p is the adjustable parameter of the position non-singular terminal sliding surface; Among them, β p =diag(β x ,β y ,β z ), β x ,β y ,β z is an adjustable parameter; The position non-singular terminal sliding mode control law is designed as: Among them, u p is the output of the position non-singular terminal sliding mode control law, for The second derivative of p is the control parameter of the position non-singular terminal sliding mode control law, δ p is the disturbance control parameter of the position non-singular terminal sliding mode control law, is the gravity in the world coordinate system.

8. A quadrotor flight control device with two-degree-of-freedom deformation of the arm, characterized in that: include: The position non-singular terminal sliding mode control unit is based on the expected position value P of the quadrotor with two degrees of freedom deformation of the arm. d , and position P E With speed v E , the position control quantity F is obtained by the position control of the two-degree-of-freedom deformation quadrotor of the machine arm d , and then the expected quaternion q of the aerial robot is obtained based on the expected posture transformation of the first joint d ; The attitude quaternion non-singular terminal sliding mode control unit is based on the expected quaternion q d As well as the actual attitude quaternion q and angular velocity ω of the two-degree-of-freedom deformation quadrotor B , the attitude control value M is obtained by the attitude control of the two-degree-of-freedom deformation quadrotor of the machine arm d . Control distribution unit, arm two-degree-of-freedom deformation quadrotor position control quantity F d and attitude control amount M d The final rotor speed n is obtained by controlling the distributor i The roll rotation angle γ of the second joint of the i-th arm relative to the first joint coordinate system i , the yaw rotation angle β of the i-th arm i By active input.

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 arm two-degree-of-freedom deformation quadrotor position 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 deformation quadrotor flight control method as described in any one of claims 1 to 7.

Images