A two-degree-of-freedom deformation quadrotor flight control method and device

Abstract

The application discloses a kind of machine arm two degrees of freedom deformation quadcopter flight control method and device, comprising: S1, machine arm two degrees of freedom deformation quadcopter is designed in structure;S2, according to machine arm two degrees of freedom deformation quadcopter structure, derive the dynamics model and kinematics model of machine arm two degrees of freedom deformation quadcopter;S3, according to step S1, S2 derive the control allocation model of machine arm two degrees of freedom deformation quadcopter;S4, according to the control allocation model of machine arm two degrees of freedom deformation quadcopter in step S3 deduced, singularity problem is analyzed to control allocation model, and special control allocation model is designed to singularity;S5, according to the dynamics model, kinematics model, control allocation model and special control allocation model of machine arm two degrees of freedom deformation quadcopter, design pose quaternion nonsingular terminal sliding mode flight controller.The application has good control and stabilizing effect to machine arm two degrees of freedom deformation quadcopter.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of aerial vehicles and robot control, in particular to a method and device for controlling a four-rotor aircraft with two degrees of freedom of arm deformation. BACKGROUND

[0002] The control technology of multi-rotor aircrafts is mature. It can not only perform normal tasks such as aerial photography and environmental monitoring, but also be used for complex and narrow environment work such as underground pipeline, natural cave or disaster rescue. However, in complex and narrow space, such as winding pipeline and complex jungle, the environment is changeable and full of uncertain factors, which is easy to cause collision interference and lead to crash for multi-rotor aircrafts. Therefore, how to adapt to complex and narrow environment is a difficult problem. The conventional multi-rotor aircraft has a fixed structure, which lacks the deformation ability of arm rotors. Therefore, it is difficult to change its shape according to the external environment, and its adaptability in narrow environment is weak.

[0003] The conventional multi-rotor aircraft belongs to an under-actuated system, and the rotor thrust is always perpendicular to the aircraft surface, so it cannot generate vector thrust, which will lead to the position and attitude decoupling of the traditional multi-rotor aircraft. In addition, the conventional multi-rotor aircraft has a fixed body structure and lacks deformation ability, which does not have adaptability in some narrow environments. Therefore, it is an important research problem to enhance the spatial deformation ability of multi-rotor aircrafts to improve the maneuverability and adaptability in complex environments.

[0004] At present, the deformation multi-rotor aircraft can be divided into two categories: arm rotation deformation and rotor tilt deformation. The arm structure of tilt multi-rotor aircraft is fixed, which cannot reduce the physical size of the aircraft body during flight, and the thrust may be redundant when changing the attitude of flight, which greatly affects the motion efficiency. In addition, the arm deformation multi-rotor aircraft lacks tilt freedom, cannot complete thrust vectorization, lacks spatial position and attitude independent motion ability, and can only realize deformation under horizontal attitude, which has limited adaptability to space. In view of these problems, it is urgent to design a new type of aircraft with deformation and tilt. SUMMARY

[0005] The present application belongs to the field of aerial vehicles and robot control, in particular to a method and device for controlling a four-rotor aircraft with two degrees of freedom of arm deformation.

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

[0007] Technical scheme: The present application provides a method for controlling a four-rotor aircraft with two degrees of freedom of arm deformation, which comprises the following steps:

[0008] S1, according to the characteristics of the two-degree-of-freedom morphing quadrotor, the two-degree-of-freedom morphing quadrotor structure is determined by analyzing the morphing mode of the two-degree-of-freedom morphing quadrotor;

[0009] S2, according to the two-degree-of-freedom morphing quadrotor structure, the dynamics model and the kinematics model of the two-degree-of-freedom morphing quadrotor are derived;

[0010] S3, according to the dynamics model and the kinematics model of the two-degree-of-freedom morphing quadrotor, the control allocation model of the two-degree-of-freedom morphing quadrotor is derived;

[0011] S4, according to the control allocation model of the two-degree-of-freedom morphing quadrotor, the singularity problem of the control allocation model is analyzed, and a special control allocation model is designed for singularity;

[0012] S5, according to the dynamics model, the kinematics model, the control allocation model and the special control allocation model of the two-degree-of-freedom morphing quadrotor, a position quaternion nonsingular terminal sliding mode flight controller is designed.

[0013] Further, the two-degree-of-freedom morphing quadrotor structure in step S1 includes a body and a two-degree-of-freedom morphing arm, and the structure of the two-degree-of-freedom morphing arm is as follows:

[0014]

[0015] Wherein, α i is the yaw angle of the i-th arm coordinate system relative to the body coordinate system, i represents the i-th arm i=1, 2, 3, 4; is the rotation matrix of the i-th arm coordinate system to the body coordinate system, is the rotation matrix of the first joint coordinate system of the i-th arm to the arm coordinate system, is the rotation matrix of the second joint coordinate system of the i-th arm to the first joint coordinate system 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 joint of the i-th arm relative to the first joint coordinate system;

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

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

[0018]

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

[0020] Further, the dynamic model of the manipulator two-degree-of-freedom morphing quadrotor in step S2 is:

[0021]

[0022] wherein, m is the mass of the manipulator two-degree-of-freedom morphing quadrotor, v B is the linear velocity in the body coordinate system, is the derivative of v B , J = diag(J x , J y , J z ) is the inertia matrix of the manipulator two-degree-of-freedom morphing quadrotor, J x is the x-axis rotational inertia, J y is the y-axis rotational inertia, and J z is the z-axis rotational inertia, ω B is the angular velocity in the body coordinate system, is the derivative of ω B , q is the quaternion of the manipulator two-degree-of-freedom morphing quadrotor, q * is the conjugate quaternion of q, is the gravity in the world coordinate system, is the disturbance force, is the rotation matrix of the manipulator coordinate system of the i-th manipulator arm to the body coordinate system, is the rotation matrix of the coordinate system of the first joint of the i-th manipulator arm to the manipulator coordinate system, is the rotation matrix of the coordinate system of the second joint of the i-th manipulator arm to the coordinate system of the first joint of the i-th manipulator arm, is the lift of the i-th rotor in the second joint coordinate system, is the position vector of the first joint coordinate system of the i-th manipulator arm relative to the manipulator coordinate system , is the position vector of the second joint coordinate system of the i-th manipulator arm relative to the first joint coordinate system , k q is the rotor counter-torque coefficient, n i is the rotational speed of the i-th rotor, is the disturbance torque.

[0023] Further, the dynamic model of the two-degree-of-freedom deformation quadrotor in step S2 includes a quaternion-based pose dynamic model and a kinematic model, the quaternion-based pose dynamic model and the kinematic model including a quaternion-based attitude kinematics and dynamics model, and a quaternion-based position dynamics and kinematics model;

[0024] The quaternion-based attitude kinematics and dynamics model is:

[0025]

[0026] where q e = (q e0 , q e1 , q e2 , q e3 ) T is the scalar part of q e0 , q e is the second term of q e1 , q e is the third term of q e2 , q e is the fourth term of q e3 , q e is the derivative of q e , is the transpose of the vector part of q e , is the angular velocity error in the body frame, f(ω B ) = -J -1 ω B x Jω B , is the angular velocity error, is the angular velocity expectation in the body frame, d ω is the disturbance torque of the attitude, u q is the input of the attitude dynamics.

[0027] The quaternion-based position dynamics and kinematics model is:

[0028]

[0029] where v E is the linear velocity in the world frame, p E is the position in the world frame, is the derivative of p E , p B is the position in the body frame, is the acceleration in the world frame, ​is the gravity in the world coordinate system, is the disturbance force.

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

[0031]

[0032] wherein, is the static control allocation matrix, is the actuator matrix using the intermediate actuator variables, is the rotation matrix of the i-th manipulator coordinate system to the body coordinate system, is the rotation matrix of the first joint coordinate system of the i-th manipulator to the manipulator coordinate system, is the rotation matrix of the second joint coordinate system of the i-th manipulator to the first joint coordinate system of the i-th manipulator, is the lift force of the i-th rotor in the second joint coordinate system, is the position vector of the first joint coordinate system of the i-th manipulator relative to the manipulator coordinate system , is the position vector of the second joint coordinate system of the i-th manipulator relative to the first joint coordinate system of the i-th manipulator, k m is the rotor counter-torque coefficient, n i is the i-th rotor speed;

[0033] The control output is solved as:

[0034]

[0035] wherein, is the pseudo-inverse calculation of , is the thrust of the manipulator two-degree-of-freedom deformation quadrotor in the body coordinate system, is the thrust torque;

[0036] The output results of each actuator are as follows

[0037]

[0038] wherein, is the square of the 2i-1 term of , is the square of the 2i term of , is the square of the 2i+1 term of .

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

[0040]

[0041] According to the position control output, the calculation formula of the thrust vector is:

[0042]

[0043] wherein, is the thrust in the X-axis singular world coordinate system, is the X-axis and Z-axis components of the thrust in the world coordinate system

[0044] The normalized expected thrust vector calculation formula is:

[0045]

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

[0047] The required rotation matrix is obtained by the Rodrigues rotation method, that is:

[0048]

[0049] wherein, R d is, I 3×3 is, is a skew-symmetric matrix, and the expected rotation matrix can be converted into the expected quaternion Q d .

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

[0051]

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

[0053]

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

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

[0056]

[0057] wherein, is a skew-symmetric matrix; then, the expected rotation matrix is converted into the expected quaternion q d .

[0058] Furthermore, a pose quaternion nonsingular terminal sliding mode flight controller is designed, including:

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

[0060]

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

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

[0063]

[0064] Among them, u q For the attitude non-singular terminal sliding mode control law output, for The reverse, for The derivative of η q δ is the control parameter for attitude nonsingular terminal sliding mode control. q For the attitude non-singular terminal sliding mode control law, the disturbance control parameters are denoted as .

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

[0066]

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

[0068] Where, β p =diag(β) x ,β y ,β z ), β x ,βy ,β z These are adjustable parameters;

[0069] The sliding mode control law for non-singular terminals is designed as follows:

[0070]

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

[0072] The present invention provides a two-degree-of-freedom deformable quadrotor flight control device for an arm, comprising:

[0073] The position non-singular terminal sliding mode control unit, based on the desired position value P of the two-DOF deformable quadrotor arm. d and position P E With velocity v E The position control quantity F is obtained from the position control of the two-degree-of-freedom deformable quadrotor arm. d Then, based on the expected pose transformation of the first joint, the expected quaternion q of the aerial robot is obtained. d ;

[0074] Attitude quaternion nonsingular terminal sliding mode control unit, based on the desired quaternion q d And the actual attitude quaternion q and angular velocity ω of the two-degree-of-freedom deformable quadrotor arm. B The attitude control quantity M is obtained for the attitude control of the two-degree-of-freedom deformable quadrotor arm. d .

[0075] Control distribution unit, position control quantity F of the two-degree-of-freedom deformable quadrotor arm d With attitude control quantity 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 Input by user.

[0076] The control device comprises a processor, a communication interface, a memory and a communication bus; wherein the processor, the communication interface and the memory complete mutual communication through the communication bus; the memory is used for storing a computer program; and the processor is used for executing the program stored on the memory to realize the steps of the flight control method of the two-degree-of-freedom morphing four-rotor of a robot arm.

[0077] The storage medium comprises a computer program stored thereon, and the computer program is executed by at least one processor to realize the steps of the flight control method of the two-degree-of-freedom morphing four-rotor of a robot arm.

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

[0079] (1) by dynamically changing the structure of the robot arm, the position and attitude of the four-rotor are decoupled and independently controlled, so that the unmanned aerial vehicle can adopt a "shrinking body" mode to pass through horizontal or vertical narrow environments, thereby having stronger environmental adaptability

[0080] (2) the attitude of the body can be accurately adjusted in the state of the unmanned aerial vehicle hovering at a fixed point, so that the attitude angle of the task load is freely changed;

[0081] (3) the quaternion dynamics modeling and kinematics modeling of the two-degree-of-freedom morphing four-rotor of the robot arm are performed, the singularity problem caused by the Euler angle modeling is avoided, and the controller research provides a good reference;

[0082] (4) according to the characteristics of the multi-actuator of the two-degree-of-freedom morphing four-rotor of the robot arm, a control distribution model of the two-degree-of-freedom morphing four-rotor of the robot arm is designed, and a special control distribution model in a singular state is realized;

[0083] (5) in view of the problems of the two-degree-of-freedom morphing four-rotor of the robot arm, such as nonlinearity and existence of external disturbance, a non-singular terminal sliding mode attitude controller is designed, which not only can overcome the influence of the system non-rigidity and parameter uncertainty of the two-degree-of-freedom morphing four-rotor of the robot arm on the controller, but also can well suppress unknown external disturbances on the body during flight, thereby improving the robustness and reliability of the attitude controller of the two-degree-of-freedom morphing four-rotor of the robot arm. BRIEF DESCRIPTION OF DRAWINGS

[0084] Figure 1 The method flowchart of the present application;

[0085] Figure 2 The schematic diagram of the two-degree-of-freedom morphing four-rotor of the robot arm targeted by the present application;

[0086] Figure 3Structure diagram of a two-degree-of-freedom deformation four-rotor arm for the method of the present application;

[0087] Figure 4 Schematic diagram of a two-degree-of-freedom deformation four-rotor arm of the method of the present application crossing a horizontal narrow environment;

[0088] Figure 5 Schematic diagram of a two-degree-of-freedom deformation four-rotor arm of the method of the present application crossing a vertical narrow environment;

[0089] Figure 6 Principle diagram of a control system for the method of the present application;

[0090] Figure 7 Quaternion change simulation curve diagram of the method of the present application under independent attitude control in the case of example 1;

[0091] Figure 8 Euler angle change simulation curve diagram of the method of the present application under independent attitude control in the case of example 1;

[0092] Figure 9 Position change simulation curve diagram of the method of the present application under independent position control in the case of example 1;

[0093] Figure 10 Pose simulation curve diagram of the method of the present application under horizontal attitude and arm deformation in the case of example 1;

[0094] Figure 11 Pose simulation curve diagram of the method of the present application under vertical attitude and arm deformation in the case of example 1. DETAILED DESCRIPTION

[0095] The present application will be described in detail below with reference to the accompanying drawings and specific embodiments. The examples of the embodiments are shown in the drawings. The embodiments described below by reference to the drawings are exemplary and are only used to explain the present application, and cannot be explained as a limitation of the present application.

[0096] As shown in Figure 1 , the flight control method of the two-degree-of-freedom deformation four-rotor arm of the present application mainly includes the following steps:

[0097] S1, according to the characteristics that the two-degree-of-freedom deformation four-rotor arm can realize pose decoupling independent tilting and can cross horizontal or vertical narrow environment, analyzing the deformation mode of the two-degree-of-freedom deformation four-rotor arm, and designing the structure of the two-degree-of-freedom deformation four-rotor arm;

[0098] The schematic diagram of the two-degree-of-freedom deformation four-rotor arm is shown in Figure 2 , the aerial robot is composed of a body and a two-degree-of-freedom deformation arm, and the arm structure is as shown in Figure 3The yaw angle of the i-th arm coordinate system relative to the body coordinate system is defined as α i , wherein i represents the i-th arm i = 1, 2, 3, 4; (b) the first joint freedom of the 1st and 3rd arms rotates in the same way, and the first joint freedom of the 2nd and 4th arms rotates in the same way. And it satisfies β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 . The rotation matrix of the body coordinate system to the world coordinate system is defined as R , the rotation matrix of the i-th arm coordinate system to the body coordinate system is R , the rotation matrix of the first joint coordinate system of the i-th arm to the arm coordinate system of the i-th arm is R , and the rotation matrix of the second joint coordinate system of the i-th arm to the first joint coordinate system of the i-th arm is R . Wherein

[0099]

[0100] The position vector of the second joint coordinate system relative to the body coordinate system is defined as p

[0101]

[0102] wherein, is the position vector of the first joint coordinate system of the i-th arm relative to the arm coordinate system, and l1 is the distance of the first joint coordinate system of the i-th arm relative to 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 of the first joint coordinate system of the i-th arm relative to the arm coordinate system.

[0103] S2, according to the arm two-degree-of-freedom deformation quadrotor designed in step S1, deriving the dynamics model and kinematics model of the arm two-degree-of-freedom deformation quadrotor;

[0104] The dynamics model of the arm two-degree-of-freedom deformation quadrotor is:

[0105]

[0106] wherein 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, is v B ​derivative of ω B is the angular velocity in body frame, is ω B derivative, m is the mass of the 2-DOF morphing quadrotor, q is the quaternion of the 2-DOF morphing quadrotor, q * is the conjugate quaternion of q. J = diag(J x ,J y ,J z is the inertia matrix of the 2-DOF morphing quadrotor, J x is the x-axis moment of inertia, J y is the y-axis moment of inertia, J z is the z-axis moment of inertia.

[0107] F E is composed of three parts: thrust, gravity and external disturbance, and is expressed as:

[0108]

[0109] where, is the disturbance force, is the gravity in world frame, g is the gravity acceleration, is the thrust of the 2-DOF morphing quadrotor QTDA in world frame, and is expressed as:

[0110]

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

[0112]

[0113] where, is the thrust torque, is the counter torque, is the disturbance torque.

[0114] is expressed in body frame as:

[0115]

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

[0117] In the body frame, it is expressed as:

[0118]

[0119] where,

[0120]

[0121] where, k m is the rotor counter torque coefficient, Q i is the counter torque of the i-th rotor, is the counter torque vector of the i-th rotor in the second joint frame.

[0122] In summary, the dynamics model is:

[0123]

[0124] The kinematics model of the two-degree-of-freedom morphing four-rotor UAV includes attitude kinematics and position kinematics, as follows:

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

[0126]

[0127] where, is the derivative of , q is the vector part of q, is the transpose of , q0 is the scalar part of q, ω B is the angular velocity in the body frame, is the vector part of quaternion, and ||q|| = 1.

[0128] F q is the

[0129]

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

[0131] The attitude error quaternion q e is the unit quaternion of the attitude of the body frame relative to the desired frame, expressed as:

[0132]

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

[0134]

[0135] where is the derivative of q e , is the transpose of the vector part of q e , is the angular velocity error in the body frame. is

[0136]

[0137] The integrated quaternion-based attitude dynamics and kinematics model is:

[0138]

[0139] where is the derivative of ω B , u q is the input of attitude kinematics, f(ω B ) = -J -1 ω B x Jω B , is the angular velocity error. is the angular velocity expectation in the body frame. d ω is the disturbance torque of attitude.

[0140] The integrated quaternion-based position dynamics and kinematics model is:

[0141]

[0142] where v E is the linear velocity in the world frame, p E is the position in the world frame, is the derivative of p E , p B is the position in the body frame, is the acceleration in the world frame, is the gravity in the world frame, is the disturbance force.

[0143] S3, derive the control allocation model according to the dynamics of the two-DOF morphing quadrotor.

[0144] The actuators of the two-DOF morphing quadrotor consist of four rotors and eight servo motors. The required force and moment of each channel can be decoupled from the system by changing the corresponding control output. The dynamics model is re-expressed as:

[0145]

[0146] where, is the thrust of the two-DOF morphing quadrotor in the body frame, A is the control allocation matrix, N is the actuator matrix, which is expressed as:

[0147]

[0148] where, 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 are β i and γ i variables in A, but they can be adjusted so that they are transformed into active or passive control variables. Therefore, the first joint yaw rotation angle β i of the i-th arm is regarded as an active actuator variable, and the second joint roll rotation angle γ i is regarded as a passive actuator variable, which is adjusted by the position and attitude control output. Since the control allocation matrix A is related to β i and γ i , the passive actuator variable can be separated from the dynamic allocation matrix using the intermediate actuator variable method, so that the speed n i of the i-th rotor is decomposed according to γ i :

[0150]

[0151] where, is the speed decomposition of the i-th rotor, is the square of the speed of the i-th rotor.

[0152] The actuator matrix using the intermediate actuator variable can be re-expressed as:

[0153]

[0154] The control allocation equation is rewritten as:

[0155]

[0156] in, It is a static control allocation matrix, represented 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, and k m This is the rotor anti-torque coefficient.

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

[0160]

[0161] The output of each actuator is as follows:

[0162]

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

[0164] S4. Based on the control allocation equation 20 established in step S3, design a special control allocation model.

[0165] When the aircraft passes through a narrow horizontal space, such as Figure 4 When the horizontal length is the minimum, it is at this time for The first row and k column. It can be seen that... The first row is not full rank, therefore The X-axis degree of freedom is lost. The X-axis thrust of the system at this time can be obtained. Y-axis thrust under the machine system Since all cosine terms are 1, maximum thrust can be obtained. Therefore, there are no mutually canceling thrusts, resulting in the highest flight efficiency. However, due to the loss of the X-axis degree of freedom, the control allocation matrix needs special processing. The rewritten control allocation matrix is ​​as follows:

[0166]

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

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

[0169]

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

[0171] In order to keep the thrust vector consistent with the desired thrust vector, the attitude of the aircraft needs to be adjusted so that the Z-axis of the aircraft is consistent with . Therefore, the calculation formula of the normalized expected thrust vector is:

[0172]

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

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

[0175]

[0176] where, Θ is the angle of rotation around the rotation axis of the fuselage vector .

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

[0178]

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

[0180] When the aircraft passes through a vertical narrow space such as Figure 5 , the optimal way for the aircraft is At this time, there is as the 2nd row kth column of It can be seen that, the 2nd row of is not full rank, so the Y-axis degree of freedom will be lost. Therefore, special treatment is needed for the control allocation matrix. The control allocation matrix with rewritten Y-axis singularity is:

[0181]

[0182] Now, the control strategy of Y-axis position is coupled with the roll attitude. The quaternion needed by the part now comes from the output command of Y-axis position, so the transformation equation of position and attitude needs to be added. At this time, the thrust vector under Y-singularity becomes

[0183]

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

[0185] The desired thrust vector can be obtained by The rotation angle of the current body Z-axis vector and the desired thrust vector is The required rotation matrix can be obtained by the Rodrigues rotation method, that is,

[0186]

[0187] where, is a skew-symmetric matrix. Then, the desired rotation matrix can be converted into 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 pose quaternion non-singular terminal sliding mode flight controller is designed.

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

[0190]

[0191] where, s q is the attitude non-singular terminal sliding surface, is the vector part of the attitude error quaternion q e , is the derivative of is the derivative of , p 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 attitude quaternion nonsingular terminal sliding mode controller adjustable parameter,

[0192] The attitude nonsingular terminal sliding mode control law is designed as:

[0193]

[0194] where u q is the attitude nonsingular terminal sliding mode control law output, is the inverse of , is the derivative of , η q is the attitude nonsingular terminal sliding mode control parameter, δ q is the attitude nonsingular terminal sliding mode control law disturbance control parameter.

[0195] The Lyapunov function is chosen as:

[0196]

[0197] Then we have:

[0198]

[0199] where, is the derivative of V q , is the derivative of s q , is the angular velocity error, is the derivative of , is the angular velocity expectation under the body frame, f(ω B ) = -J -1 ω B x Jω B , u q is the attitude nonsingular terminal sliding mode control law output, d ω is the attitude disturbance moment. is the derivative of

[0200]

[0201] and

[0202]

[0203] where, δ q1 is the first term of the attitude nonsingular terminal sliding mode control law disturbance control parameter, δq2 is the second term of the disturbance control parameter of the attitude nonsingular terminal sliding mode control law, δ q3 is the third term of the disturbance control parameter of the attitude nonsingular terminal sliding mode control law.

[0204] Then there are

[0205]

[0206] Since is a diagonal matrix with all positive diagonal elements. Therefore,

[0207]

[0208] Select as the desired position and velocity in the world coordinate system. The position error is denoted as

[0209]

[0210] where is the position error in the world coordinate system, p E is the position in the world coordinate system, is the desired 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 desired velocity in the world coordinate system.

[0211] The design of the position nonsingular terminal sliding mode surface is:

[0212]

[0213] where is the derivative of , p p is the adjustable parameter of the position nonsingular terminal sliding mode surface, q p is the adjustable parameter of the position nonsingular terminal sliding mode surface.

[0214] where, β p = diag(β x , β y , β z ), β x > 0, β y > 0, β z > 0, β x , β y , β z are adjustable parameters.

[0215] The design of the position nonsingular terminal sliding mode control law is:

[0216]

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

[0218] The Lyapunov function is chosen as:

[0219]

[0220] Then there is

[0221]

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

[0223]

[0224] Where, δ x δ is the first term in the disturbance control parameters of the position nonsingular terminal sliding mode control law. y δ is the second term in the disturbance control parameters of the position nonsingular terminal sliding mode control law. z This is the third term in the disturbance control parameters of the position non-singular terminal sliding mode control law.

[0225] Then,

[0226]

[0227] because It is a diagonal matrix where every diagonal element is positive. Therefore

[0228]

[0229] Therefore, the stability of the quaternion nonsingular terminal sliding mode attitude controller is proved.

[0230] According to the quaternion nonsingular terminal sliding mode attitude controller designed in the above step S5, a control system implementation schematic diagram is established, as shown in the figure. Figure 6 The two-degree-of-freedom morphing quadrotor arm includes position nonsingular terminal sliding mode control, attitude quaternion nonsingular terminal sliding mode control, and control distribution.

[0231] The position nonsingular terminal sliding mode control is obtained according to the position expected value P d of the two-degree-of-freedom morphing quadrotor arm, the position P E and the speed v E of the two-degree-of-freedom morphing quadrotor arm. d The position control amount F d of the two-degree-of-freedom morphing quadrotor arm is obtained. d The attitude quaternion nonsingular terminal sliding mode control is obtained according to the expected quaternion q B and the actual attitude quaternion q and the angular velocity ω d of the two-degree-of-freedom morphing quadrotor arm. d The attitude control amount M d of the two-degree-of-freedom morphing quadrotor arm is obtained.

[0233] The control distribution is obtained by the control distributor of the position control amount F i and the attitude control amount M i of the two-degree-of-freedom morphing quadrotor arm. i The final rotor speed n and the roll rotation angle γ of the second joint of the i-th arm relative to the first joint coordinate system

[0001] and the yaw rotation angle β of the i-th arm are obtained by the active input.

[0234] In an embodiment of the present application, a control device includes a processor, a communication interface, a memory and a communication bus; wherein the processor, the communication interface, the memory complete the communication among each other through the communication bus; the memory is used to store a computer program; the processor is used to execute the program stored on the memory to realize the above-mentioned two-degree-of-freedom morphing quadrotor arm flight control method and achieve the technical effects consistent with the above-mentioned method.

[0235] The embodiments of the present application are described below.

[0236] The designed two-degree-of-freedom morphing quadrotor arm (such as Figure 2The object is shown in the MATLAB / Simulink environment, and simulation verification is performed to verify the effectiveness of the control method. The specific parameters of the mathematical model of the two-degree-of-freedom arm deformation quadrotor are shown in Table 1:

[0237] Table 1 System simulation parameters

[0238]

[0239] Case 1: Independent attitude control simulation of the two-degree-of-freedom arm deformation quadrotor;

[0240] In the simulation experiment, the initial roll angle of the two-degree-of-freedom arm deformation quadrotor is set to φ = 0°, the desired roll angle at the 1st second is φ = 90°, the desired roll angle at the 5th second is φ = 0°, and the desired roll angle at the 10th second is φ = -90°. The simulation results are shown in Figure 7 (a)-(d) and shown in Figure 8 (a)-(c). The simulation results show that the NTSM controller responds to PID and SMC better, the application can track the target value well, and the quaternion method can avoid the singularity problem of Euler angles and can quickly and robustly track the desired attitude.

[0241] The simulation results show that the application can effectively improve the response speed of the attitude when the attitude changes.

[0242] Case 2: Independent position control simulation of the two-degree-of-freedom arm deformation quadrotor;

[0243] In the simulation experiment, the desired position of the two-degree-of-freedom arm deformation quadrotor is designed as

[0244]

[0245] The body position response curve is shown in Figure 9 (a)-(c), and the simulation results show that the NTSM controller responds to PID and SMC better, the application can track the target value well, and can quickly and robustly track the desired position.

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

[0247] Case 3: Independent position motion simulation of the two-degree-of-freedom arm deformation quadrotor in horizontal attitude and arm deformation;

[0248] In the simulation experiment, the initial arm state of the two-degree-of-freedom arm deformation quadrotor is designed as β1= β3= 45°, β2= β4= -45°, the desired position of the Y axis is set to 1m at the 2nd second, the desired 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 inFigure 10 The simulation results shown in (a)-(d) show that the NTSM controller still has good control effect in the horizontal posture arm deformation.

[0249] The simulation results show that the application can maintain good posture control ability under the vertical posture and arm deformation.

[0250] Case 4: Independent position motion simulation of four-rotor in vertical posture and arm deformation of two-degree-of-freedom arm;

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

[0252] The simulation results show that the application can maintain good posture control ability under the vertical posture and arm deformation.

Claims

1. A method for controlling a two-degree-of-freedom morphing quadrotor flight, characterized in that, Comprise the following steps: S1, according to the characteristics of the two degree of freedom deformation quadrotor aircraft, which can realize pose decoupling independent tilt and can pass through horizontal or vertical narrow environment, analyze the deformation mode of the two degree of freedom deformation quadrotor aircraft, and determine the structure of the two degree of freedom deformation quadrotor aircraft; S2, according to the structure of the two degree of freedom deformation quadrotor aircraft, derive the dynamics model and kinematics model of the two degree of freedom deformation quadrotor aircraft; S3, according to the dynamics model and kinematics model of the two degree of freedom deformation quadrotor aircraft, derive the control allocation model of the two degree of freedom deformation quadrotor aircraft; S4, according to the control allocation model of the two degree of freedom deformation quadrotor aircraft, analyze the singularity problem of the control allocation model, and design a special control allocation model for singularity; S5, according to the dynamics model, kinematics model, control allocation model and special control allocation model of the two degree of freedom deformation quadrotor aircraft, design a pose quaternion nonsingular terminal sliding mode flight controller; including: The design of attitude nonsingular terminal sliding mode surface is: ， wherein, is a non-singular terminal sliding mode surface for attitude, is a vector part of attitude error quaternion is a derivative of is a non-singular terminal sliding mode controller tunable parameter for attitude, is a non-singular terminal sliding mode controller tunable parameter for attitude, , , , is a non-singular terminal sliding mode controller tunable parameter for attitude;​​ The design of attitude nonsingular terminal sliding mode control law is: ， in, For the attitude non-singular terminal sliding mode control law output, The inertial matrix of the two-degree-of-freedom deformable quadrotor arm is given. 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, for The derivative, , for The scalar part, for The second item, for The third item, for The fourth item, It is a three-dimensional unit vector. , for The reverse, Let be the angular velocity in the machine system. For the angular velocity error under the machine system, For attitude non-singular terminal sliding mode control, the control parameters are... The disturbance control parameters are for the attitude non-singular terminal sliding mode control law; The design of position nonsingular terminal sliding mode surface is: ， wherein, is a position non-singular terminal sliding surface, is derivative of is a position error in the world coordinate system, is an adjustable parameter of the position non-singular terminal sliding surface, is an adjustable parameter of the position non-singular terminal sliding surface; wherein , is an adjustable parameter; The design of position nonsingular terminal sliding mode control law is: ， wherein, is the output of the position non-singular terminal sliding mode control law, is the mass of the two-degree-of-freedom morphing quadrotor, is the second derivative of is the desired position in the world coordinate frame, is the control parameter of the position non-singular terminal sliding mode control law, is the disturbance control parameter of the position non-singular terminal sliding mode control law, is the gravity in the world coordinate frame.​ 2. The method of claim 1, wherein the method further comprises: The structure of the two degree of freedom deformation quadrotor aircraft in step S1 includes the body and the two degree of freedom deformation arm, and the structure of the two degree of freedom deformation arm is: ， in, For the first The yaw angle of the arm coordinate system relative to the body coordinate system. Indicates the first One arm ; For the first The rotation matrix from the arm coordinate system to the body coordinate system for each arm. For the first The rotation matrix from the coordinate system of the first joint of the robotic arm to the coordinate system of the robotic arm itself. For the first The coordinate system of the second joint of the machine arm to the first Rotation matrix of the first joint of each arm For the first The yaw rotation angle of the first joint of each arm relative to the arm coordinate system For the first The roll angle of the second-stage joint of each arm relative to the coordinate system of the first-stage joint; The first and third robot arms have the same first joint freedom degree rotation mode, the second and fourth robot arms have the same first joint freedom degree rotation mode, and the following conditions are satisfied ; Position vector of the second joint coordinate system with respect to the body coordinate system Position vector is: ， in, For the first The position vector of the first joint coordinate system of each arm relative to the arm coordinate system. For the first The distance between the first joint coordinate system of each arm and the arm coordinate system. For the first The position vector of the second joint coordinate system of each arm relative to the first joint coordinate system. For the first The distance between the first joint coordinate system of each arm and the arm coordinate system.

3. The method of claim 1, wherein, The dynamics model of the two degree of freedom deformation quadrotor aircraft in step S2 is: ， in, The linear velocity in the body coordinate system. for The derivative, for The derivative, For the quaternion of the two-degree-of-freedom deformable quadrotor arm, [the quaternion is missing here]. for The conjugate quaternion, For gravity in the world coordinate system, For disturbance force, For the first The rotation matrix from the arm coordinate system to the body coordinate system for each arm. For the first The rotation matrix from the coordinate system of the first joint of the robotic arm to the coordinate system of the robotic arm itself. For the first The coordinate system of the second joint of the machine arm to the first Rotation matrix of the coordinate system of the first joint of each arm It is the second joint coordinate system The lift of each rotor, For the first The first joint coordinate system of the machine arm Relative to the arm coordinate system position vector, For the first The second joint coordinate system of the machine arm Relative to the first joint coordinate system position vector, , This is the rotor anti-torque coefficient. For the first One rotor speed, This is the disturbance torque.

4. The method of claim 1, wherein, The kinematics and dynamics model of the two degree of freedom deformation quadrotor aircraft in step S2 includes the pose dynamics model and kinematics model based on quaternion, which includes the attitude kinematics and dynamics model based on quaternion, and the position dynamics and kinematics model based on quaternion; The attitude kinematics and dynamics model based on quaternion is: ， wherein , is the derivative of , is the disturbance torque of the attitude, is the input of the attitude dynamics; The position dynamics and kinematics model based on quaternion is: ， in, The linear velocity in the world coordinate system. This refers to the position in the world coordinate system. for The derivative, Position within the machine system, For the quaternion of the two-degree-of-freedom deformable quadrotor arm, [the quaternion is missing here]. for The conjugate quaternion, Acceleration in the world coordinate system For gravity in the world coordinate system, It is a disturbance force.

5. The method of claim 1, wherein, The control allocation model in step S3 is: ， in, It is a static control allocation matrix. For the executor matrix that uses intermediate executor variables, , For the first The rotation matrix from the arm coordinate system to the body coordinate system for each arm. For the first The rotation matrix from the coordinate system of the first joint of the robotic arm to the coordinate system of the robotic arm itself. For the first The coordinate system of the second joint of the machine arm to the first Rotation matrix of the coordinate system of the first joint of each arm It is the second joint coordinate system The lift of each rotor, For the first The first joint coordinate system of the machine arm Relative to the arm coordinate system position vector, For the first The second joint coordinate system of the machine arm Relative to the first joint coordinate system position vector, It is the second joint coordinate system The lift of each rotor, , This is the rotor anti-torque coefficient. For the first One rotor speed, For the first The roll angle of the second joint of each arm relative to the coordinate system of the first joint. Indicates the first One arm ; The control output solution is: ， wherein, is the pseudo-inverse calculation, is the thrust of the two-degree-of-freedom morphing quadrotor in the body coordinate system, is the thrust moment; The output results of each actuator are as follows ， wherein is the square of the is the square of the is the square of the 6. The two-degree-of-freedom morphing quadrotor flight control method of claim 1, wherein, The special control allocation model designed in step S4 is as follows: When the aircraft passes through horizontal narrow space, the special control allocation model is: ， wherein, is the rotor lift coefficient, is the distance of the first joint coordinate system of the first arm relative to the arm coordinate system, is the distance of the first joint coordinate system of the second arm relative to the arm coordinate system, is the rotor counter torque coefficient; According to the position control output, the calculation formula of the thrust vector is: ， wherein, is the thrust in the world coordinate system for the X-axis singularity, is the X-axis and Z-axis components of the thrust in the world coordinate system; The calculation formula of the normalized expected thrust vector is: ， wherein, is the normalized thrust vector in the world coordinate system; The required rotation matrix is obtained by the Rodriguez rotation method: ， wherein is a desired rotation matrix, is a three-dimensional identity matrix, is a skew-symmetric matrix, the desired rotation matrix can be converted into a desired quaternion , is an angle of rotation about the body vector is an angle of rotation about the body vector is an angle of rotation about the desired thrust vector; When the aircraft passes through vertical narrow space, the special control allocation model is: ， At this time, the thrust vector becomes: ， wherein, Pxy, Pxzare the Y 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 ， wherein is a skew-symmetric matrix; then, the desired rotation matrix is converted to the desired quaternion .

7. A control device for the two-degree-of-freedom morphing quadrotor flight control method of any one of claims 1-6, characterized in that, Comprise: The position non-singular terminal sliding mode control unit obtains a position control amount according to position control of the two-degree-of-freedom morphing quadrotor , and position and velocity of the two-degree-of-freedom morphing quadrotor ; and obtains an expected quaternion of the aerial robot according to the expected pose of the first joint ; The attitude quaternion nonsingular terminal sliding mode control unit obtains the attitude control amount according to the expected quaternion and the actual attitude quaternion and angular velocity of the two-degree-of-freedom morphing quadrotor and the actual attitude quaternion and angular velocity of the two-degree-of-freedom morphing quadrotor of the two-degree-of-freedom morphing quadrotor ; Control distribution unit, position control of the two-DOF deformable quadrotor arm With attitude control quantity The final rotor speed is obtained by controlling the distributor. With the The roll rotation angle of the second joint of the machine arm relative to the coordinate system of the first joint. , No. Yaw rotation angle of each arm Input by user.

8. A control device, characterized by Comprise a processor, a communication interface, a memory and a communication bus; wherein the processor, the communication interface, the memory complete mutual communication through the communication bus; the memory is used for storing computer programs; the processor is used for executing the programs stored on the memory, realizing the steps of the flight control method of the two degree of freedom deformation quadrotor aircraft in any one of claims 1-6.

9. A storage medium, characterized by The storage medium stores computer programs, and the computer programs are executed by at least one processor to realize the steps of the flight control method of the two degree of freedom deformation quadrotor aircraft in any one of claims 1-6.

Images