Vector six-rotor aircraft full-state control method based on high-order kinetic model

Abstract

The invention discloses a vector six-rotor aircraft full-state control method based on a high-order dynamic model, and belongs to the field of vector rotor aircraft tracking control. The invention aims to solve the problems of poor angular acceleration estimation accuracy and low precision of the existing vector rotorcraft control method based on a jerk dynamics model. According to the invention, an angular acceleration observer based on a high-order dynamic model and a full-drive system theory is designed, effective estimation of an angular acceleration signal can be realized without depending on external sensor equipment, and high-precision and high-speed tracking of the position attitude of the six-degree-of-freedom vector six-rotor aircraft can be realized. The method is mainly applied to the pose tracking of the six-degree-of-freedom vector six-rotor aircraft.

Description

Technical Field

[0001] The invention belongs to the field of vector rotorcraft tracking control. Background Art

[0002] A vectored rotorcraft is an aerial flight system driven by a vectored thrust unit consisting of a servo and a motor-driven propeller. Compared to traditional underactuated rotorcraft, this flight system boasts decoupled control of position and attitude: it can maintain a specific attitude while maneuvering, and can also change attitude while remaining stationary, or hovering. This feature makes vectored rotorcraft superior in aerial operations such as pipeline structural strength testing, high-voltage power grid insulator insulation testing, and offshore wind turbine blade inspection.

[0003] However, the introduction of the servo unit significantly increases the model complexity of the vector rotorcraft. The inertia of the servo drive unit is much larger than that of the propeller motor, so it is not easy to track the position command directly sent to the servo. For this reason, researchers have developed a vector rotorcraft control method based on the jerk dynamics model in recent years. Compared with the traditional control method based on the acceleration dynamics model, which can only obtain the acceleration command at most, the control method based on the jerk dynamics model can obtain the acceleration control command (the third-order derivative of the position, namely the acceleration, also known as the jerk). Since the speed command of the servo is related to the jerk signal of the vector rotorcraft, the control method based on the jerk dynamics model can ensure the continuity of the speed command of the servo, thereby achieving the advantages of smooth control and high control accuracy.

[0004] However, a feedback controller based on the jerk model (a full-state controller) requires acceleration feedback. Unlike vector aircraft, where linear acceleration can be directly acquired using accelerometers and mature commercial acceleration sensor chips are available, acquiring angular acceleration is much more difficult due to the lack of mature commercial sensor chips. Commercially available triaxial angular accelerometers are too bulky for the small flying manipulator system studied in this paper, making them impractical.

[0005] However, existing angular acceleration estimation methods suffer from shortcomings such as poor estimation accuracy, low precision, sensitivity to noise and system parameter variations, and poor robustness. Furthermore, existing feedback controllers based on jerk signals (full-state controllers) generally suffer from slow response, poor control stability, and weak anti-interference capabilities. Therefore, these issues urgently need to be addressed. Summary of the Invention

[0006] Aiming at the problems of poor angular acceleration estimation accuracy and low precision in the existing vector rotorcraft control method based on jerk dynamics model, the present invention provides a full-state control method for a vector hexacopter based on a high-order dynamics model.

[0007] A full-state control method for a vector hexacopter based on a high-order dynamics model includes:

[0008] S1, the angular acceleration observer receives the actual velocity of the aircraft in the output state at time t The angular velocity ω t , and the rate of change of the control force output by the full-state controller at time t Medium control torque change rate Process and estimate the angular acceleration at time t t is an integer, and t≥1;

[0009] S2, the angular acceleration at time t The linear acceleration of the aircraft at time t collected by the acceleration measurement unit Splice and get the full-state acceleration reference value at time t T is transpose;

[0010] S3, the full-state controller is based on the full-state expected posture q at time t d,t , full-state acceleration reference value And the actual position q of the full state in the output state of the aircraft t , generates the control force spinor change rate at time t+1 By controlling the amount of distribution Distribute and generate the aircraft's servo and motor control signals, control the aircraft's servo and motor, change the aircraft's output state at time t, and achieve full state control of the aircraft.

[0011] Preferably, in step S1, the angular acceleration at time t is estimated The implementation is:

[0012] S11, according to the ω received by the angular acceleration observer t and The state equation of the angular acceleration observer at time t is obtained:

[0013]

[0014] in,

[0015]

[0016]

[0017] x t-1，1 and x t-1，2 are the first and second observations of the angular acceleration observer at time t-1, x t-1，1 and x t-1，2 The initial value of is any given value. and x t-1，1 and x t-1，2 The rate of change, x t，1 and x t，2 are the first and second observations of the angular acceleration observer at time t, respectively, x t，1 and x t，2 The initial value of is 0. and x t，1 and x t，2 The rate of change, Δt is the time interval, I3 is the 3×3 unit matrix, 03 is a 3×3 matrix with all elements equal to 0, L0 and L1 are the first and second constant diagonal matrices respectively;

[0018] B1(ω t ),Θ(ω t ), Ω(J) are intermediate variables, Θ(ω0) is Θ(ω t ), Δω is the change in angular acceleration, ω0 is the initial value of angular velocity, and J is the inertia tensor of the aircraft; j i,k is the kth element in the i-th row of J, i=1,2,3, k=1,2,3, ω t1 、ω t2 and ω t3 are the angular velocity components of the aircraft in the X, Y and Z directions respectively;

[0019] S12, according to the x in the state equation of the angular acceleration observer at time t t，1 and B1(ω t ), estimate the angular acceleration at time t

[0020] Preferably, in step S12

[0021] Preferably,

[0022] Among them, l 01 ,l 02 ,l 03 ,l 11 ,l 12 ,l 13 are the first to sixth adjustable parameters of the angular acceleration observer.

[0023] Preferably, in step S3, the control force rotation rate of change at time t+1 is generated. The implementation methods include:

[0024] S31, according to the full state expected posture q at time t d,t And the full-state actual pose q in the output state of the aircraft t , define the full-state zero-order error in,

[0025] q d,t =[p d,t ,Q d,t ] T , p d,t is the expected position vector at time t, Q d,t is the expected attitude quaternion at time t;

[0026] q t =[p t ,Q t ] T , p t is the actual position vector at time t, Q t is the actual attitude quaternion at time t;

[0027] e t =p d,t -p t , e t is the position error at time t;

[0028] Q e,t is the attitude error quaternion at time t, Q t The conjugate quaternion, q e0,t and q ev,t They are the attitude error quaternion Q e,t The real and imaginary parts of

[0029] S32, e t Derivative, get the first-order position error and the second-order position error

[0030] S33, according to Q d,t , and get the expected angular velocity ω in the ground coordinate system d,t ;

[0031] S34, according to Q t 、 ω t and ω d,t , define the angular velocity deviation ω e,t ;

[0032] S35, according to ω e,t , construct attitude error quaternion Q e,t The real part q e0,t The second derivative of and the imaginary part q ev,t The second derivative of

[0033] S36, according to and Define the intermediate variable q A ;

[0034] According to q e0,t and q ev,t , define the intermediate variable q B ;

[0035] According to ω t 、 ω d,t and ω e,t , define the intermediate variable Ω e / B ;

[0036]

[0037] in, ω e,t The first derivative of and are ω d,t The first and second derivatives of q ev,t The first derivative of ; I3 is the 3×3 unit matrix, R EB is the coordinate transformation matrix from the aircraft body coordinate system to the world coordinate system;

[0038] S37, according to q B , construct intermediate variables

[0039]

[0040] According to q A ,q B and Ω e / B , construct the intermediate variable u t ;

[0041]

[0042]

[0043] Among them, v t and j t are intermediate variables. For p t The second derivative of For p d,t The third-order derivative of , m is the total mass of the aircraft, J is the inertia tensor of the aircraft, p bc is the vector from the center of mass vector of the aircraft to the origin of the coordinate system where the propeller is located, is the gravity f g The first derivative of , M is the inertia matrix of the aircraft, is the first derivative of M;

[0044] according to and Construct the full-state zero-order error e 1,t The first derivative of and the second-order derivative Thus, we get the intermediate variable

[0045] S38, according to u t and E t , and the rate of change of the control force screw is obtained

[0046] Preferably, in step S33,

[0047] It's Q d,t The conjugate quaternion of d / d,t is the expected angular velocity in the desired attitude coordinate system at time t.

[0048] Preferably, in step S34,

[0049] Among them, R EB It is the coordinate transformation matrix from the aircraft body coordinate system to the world coordinate system.

[0050] Preferably, in step S35,

[0051]

[0052] Where T is the transpose, q ev,t The first derivative of and are ω e,t The first and second derivatives of q e0,t The first derivative of .

[0053] Preferably, in step S37,

[0054] Preferably, in step S38,

[0055] Wherein, A=[A0 A1 A2], A is the adjustment coefficient matrix of the full-state controller, and A0, A1, and A2 represent the first to third adjustment coefficients of the full-state controller, respectively.

[0056] Beneficial effects of the present invention:

[0057] This invention proposes a full-state control method for a vector hexacopter based on a high-order dynamics model. The main advantages of this invention include:

[0058] (1) An angular acceleration observer based on a high-order dynamics model and full-drive system theory is proposed. This observer can effectively estimate the angular acceleration signal without relying on external sensor devices and provide an accurate data basis for the subsequent control signal generation process.

[0059] (2) A full-state controller based on a high-order dynamic model is proposed, which can achieve high-precision and high-speed tracking of the position and attitude of a six-degree-of-freedom vector hexacopter. The full-state controller further designs a high-level force and torque control based on the angular acceleration signal obtained by the angular acceleration observer and the linear acceleration signal measured by the angular acceleration measurement unit, thereby achieving high-precision posture control.

[0060] The present invention can effectively obtain the estimated value of angular acceleration through the angular acceleration observer, and further calculate the control force rotation rate of the full-state controller output. By distributing control quantities and controlling the aircraft's servos and motors, it is possible to achieve high-precision and high-speed tracking of the position and attitude of a six-degree-of-freedom vector six-rotor aircraft. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 Schematic diagram of the principle of the full-state control method of a vector hexacopter based on a high-order dynamics model according to the present invention;

[0062] Figure 2 This is a physical picture of a six-degree-of-freedom vector hexacopter. DETAILED DESCRIPTION

[0063] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0064] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other.

[0065] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, but they are not intended to limit the present invention.

[0066] Source of concept: The present invention aims to address the shortcomings of traditional vector rotorcraft control methods based on jerk dynamics models, such as poor accuracy and low precision of angular acceleration estimation, and the feedback controller (full-state controller) based on jerk signals generally has the disadvantages of slow response speed, poor control stability, and weak anti-interference ability. Therefore, the present invention provides a full-state control method for a vector six-rotor aircraft based on a high-order dynamics model, and the present invention mainly makes technical improvements in two aspects: first, an angular acceleration observer, and second, a full-state controller.

[0067] In the existing technology, there is an acceleration-level dynamic modeling of the vector rotorcraft. After ignoring some disturbance terms such as the centrifugal force generated by the motion of the system's center of mass and the inertial force and Coriolis force caused by the tilt of the servo mechanism, the acceleration-level dynamic model of the vector rotorcraft can be obtained.

[0068]

[0069] Among them, M is the inertia matrix of the vector rotorcraft, m is the total mass of the vector rotorcraft, p bc is the vector from the center of mass vector to the origin of the coordinate system where the propeller is located, R EB is the coordinate transformation matrix from the aircraft body coordinate system to the world coordinate system, J is the inertia tensor, p is the center of mass vector, and f g represents gravity, f dis and τ dis is the total external disturbance force and torque, is the control force screw at the current moment t, ω t is the angular velocity of the aircraft in the current machine system at time t.

[0070] Based on the acceleration-level dynamics model, the jerk-level dynamics model (high-order dynamics model) of the vector rotorcraft can be obtained by differentiating the model with respect to time as follows:

[0071]

[0072] In the formula, (*) (3) represents the third derivative of the variable (*), is the gravity in the body coordinate system C B The rate of change in is the derivative of the inertia matrix, and is the total external disturbance force and torque change rate. is the output of the controller (rotor change rate), it is worth noting that Right now Control force change rate and control torque change rate It consists of two parts, namely Can be obtained from Directly extracted from.

[0073] The specific implementation methods are as follows:

[0074] Specific implementation method 1. Combination Figure 1 As shown, the full-state control method of a vector hexacopter based on a high-order dynamics model described in this embodiment includes:

[0075] S1, the angular acceleration observer receives the actual velocity of the aircraft in the output state at time t The angular velocity ω t , and the rate of change of the control force output by the full-state controller at time t Medium control torque change rate Process and estimate the angular acceleration at time t t is an integer, and t≥1;

[0076] S2, the angular acceleration at time t The linear acceleration of the aircraft at time t collected by the acceleration measurement unit Splice and get the full-state acceleration reference value at time t T is transpose;

[0077] S3, the full-state controller is based on the full-state expected posture q at time t d,t , full-state acceleration reference value And the actual position q of the full state in the output state of the aircraft t , generates the control force spinor change rate at time t+1 By controlling the amount of distribution Distribute and generate the aircraft's servo and motor control signals, control the aircraft's servo and motor, change the aircraft's output state at time t, and achieve full state control of the aircraft.

[0078] In specific applications, the control quantity allocation method is used to When performing allocation, the control amount allocation method used can be implemented through existing technologies, specifically algorithms such as the pseudo-inverse method or the null space projection method. and They are the control force change rate and control torque change rate at time t+1 respectively.

[0079] See also Figure 1, the six-degree-of-freedom vector six-rotor aircraft includes the full-state actual posture q t =[p t ,Q t ] T and the actual speed of the full state The main function of the angular acceleration observer is to use the full-state actual speed returned by the vector rotor body. The angular velocity ω t , and the rate of change of the control torque output by the controller These variables serve as input to the system, and the estimated angular acceleration is calculated based on them. It can accurately and robustly estimate the angular acceleration of the aircraft while being insensitive to errors in nominal parameters such as the system's moment of inertia.

[0080] The theoretical derivation process of angular acceleration observer design is as follows:

[0081] Here, ω t The inertia tensor J of the aircraft is specifically expanded in component form as follows:

[0082]

[0083] According to Euler's law in classical mechanics, we can get:

[0084]

[0085] Taking the derivative of both ends of the above equation we can get:

[0086]

[0087] The present invention will t Substitute the specific components of and J into the above formula From the expression of , we can get the following second-order full measurement system:

[0088]

[0089] Among them and are two intermediate calculation variables. When the angular velocity ω0 at the initial time 0 is known, the control time interval is Δt, and the angular velocity difference between the two control intervals (current time minus previous time) is Δω. The specific expression can be expressed as follows:

[0090]

[0091] Among them, Θ(ω t ) and Ω(J) are intermediate variables in the calculation.

[0092] Then, the present invention defines two observation variables according to the above formula as follows:

[0093]

[0094] It is worth noting that the two observed variables given here are only used to define two intermediate variables. t，1 and x t，2 The state space equation of the angular acceleration observer is designed, thereby constructing an angular acceleration observer.

[0095] Therefore, based on the above derivation process, the angular acceleration at time t estimated by the angular acceleration observer in step S1 is given as The implementation is:

[0096] S11, according to the ω received by the angular acceleration observer t and The state equation of the angular acceleration observer at time t is obtained:

[0097]

[0098] in,

[0099]

[0100]

[0101] x t-1，1 and x t-1，2 are the first and second observations of the angular acceleration observer at time t-1, x t-1，1 and x t-1，2 The initial value of is any given value, usually 0, and x t-1，1 and x t-1，2 The rate of change, x t，1 and x t，2 are the first and second observations of the angular acceleration observer at time t, respectively, x t，1 and x t，2 The initial value of is 0. and x t，1 and x t，2 The rate of change, Δt is the time interval, I3 is the 3×3 unit matrix, 03 is a 3×3 matrix with all elements equal to 0, L0 and L1 are the first and second constant diagonal matrices respectively;

[0102] B1(ω t ),Θ(ω t ), Ω(J) are intermediate variables, Θ(ω0) is Θ(ω t) is obtained by substituting the angular velocity ω0 at the initial moment into Θ(ω t ) expression, Δω is the change in angular acceleration, ω0 is the initial value of angular velocity, and J is the inertia tensor of the aircraft; j i,k is the kth element in the i-th row of J, i=1,2,3, k=1,2,3, ω t1 、ω t2 and ω t3 are the angular velocity components of the aircraft in the X, Y and Z directions respectively; [] × Indicates the antisymmetric matrix operation on the vector;

[0103] l 01 ,l 02 ,l 03 ,l 11 ,l 12 ,l 13 are the first to sixth adjustable parameters of the angular acceleration observer.

[0104] S12, according to the x in the state equation of the angular acceleration observer at time t t，1 and B1(ω t ), estimate the angular acceleration at time t

[0105] In this preferred embodiment, the angular acceleration at time t is estimated. In the specific process, the influence of the torque change rate on the angular acceleration of the aircraft is determined based on the accurate attitude dynamics model and the derivative equation of the model equation (referred to as the jerk dynamics model or high-order dynamics model in the present invention), and is taken into account in the observer algorithm. At the same time, due to the use of a high-order system model, namely jerk dynamics modeling, the observer is insensitive to constant interference such as nominal inertia matrix error, external constant torque disturbance, etc.

[0106] In the design of the full-state controller, the present invention is based on the external full-state expected posture q d,t =[p d,t ,Q d,t ] T , get the first and second order derivatives of the position error, and output the actual position q of the whole state according to the system state t =[p t ,Q t ] T , Actual speed in all states and the angular acceleration observer and the inertial measurement unit The combined full-state acceleration reference value The controller output is obtained through a series of calculations

[0107] Furthermore, in step S3, the control force rotation rate of change at time t+1 is generated: The implementation methods include:

[0108] S31, according to the full state expected posture q at time t d,t And the full-state actual pose q in the output state of the aircraft t , define the full-state zero-order error in,

[0109] q d,t =[p d,t ,Q d,t ] T , p d,t is the expected position vector at time t, Q d,t is the expected attitude quaternion at time t;

[0110] q t =[p t ,Q t ] T , p t is the actual position vector at time t, Q t is the actual attitude quaternion at time t;

[0111] e t =p d,t -p t , e t is the position error at time t;

[0112] Q e,t is the attitude error quaternion at time t, Q t The conjugate quaternion, q e0,t and q ev,t They are the attitude error quaternion Q e,t The real and imaginary parts of

[0113] S32, e t Derivative, get the first-order position error and the second-order position error

[0114] S33, according to Q d,t , and get the expected angular velocity ω in the ground coordinate system d,t , specifically,

[0115]

[0116] It's Q d,t The conjugate quaternion ofd / d,t is the expected angular velocity in the desired attitude coordinate system at time t;

[0117] S34, according to Q t 、 ω t and ω d,t , define the angular velocity deviation ω e,t ;

[0118] Specifically,

[0119] R EB is the coordinate transformation matrix from the aircraft body coordinate system to the world coordinate system;

[0120] S35, according to ω e,t , construct attitude error quaternion Q e,t The real part q e0,t The second derivative of and the imaginary part q ev,t The second derivative of

[0121] Specifically,

[0122]

[0123] Where T is the transpose, q ev,t The first derivative of and are ω e,t The first and second derivatives of q e0,t The first derivative of ;

[0124] S36, according to and Define the intermediate variable q A ;

[0125] According to q e0,t and q ev,t , define the intermediate variable q B ;

[0126] According to ω t 、 ω d,t and ω e,t , define the intermediate variable Ω e / B ;

[0127]

[0128] in, ω e,t The first derivative of and are ω d,t The first and second derivatives of q ev,t The first derivative of ; I3 is the 3×3 unit matrix, R EB is the coordinate transformation matrix from the aircraft body coordinate system to the world coordinate system;

[0129] S37, according to q B , construct intermediate variables

[0130]

[0131] According to q A ,q B and Ω e / B , construct the intermediate variable u t ;

[0132]

[0133]

[0134] Among them, v t and j t are intermediate variables. For p t The second derivative of For p d,t The third-order derivative of , m is the total mass of the aircraft, J is the inertia tensor of the aircraft, p bc is the vector from the center of mass vector of the aircraft to the origin of the coordinate system where the propeller is located, is the gravity f g The first derivative of , M is the inertia matrix of the aircraft, is the first derivative of M;

[0135] according to and Construct the full-state zero-order error e 1,t The first derivative of and the second-order derivative Thus, we get the intermediate variable in,

[0136] S38, according to u t and E t , and the rate of change of the control force screw is obtained

[0137] Wherein, A=[A0 A1 A2], A is the adjustment coefficient matrix of the full-state controller, and A0, A1, and A2 represent the first to third adjustment coefficients of the full-state controller, respectively.

[0138] In this preferred embodiment, the control force rotation rate is obtained In the process of the proposed method, the position and attitude errors and the first and second order derivatives of the errors are taken into account, especially the dynamics of the position and attitude are simultaneously considered. In particular, the position and attitude dynamics are accurately modeled and the derivatives on both sides of the dynamic equation are solved (called the jerk dynamic model in this invention). The attitude representation method based on quaternions is used to achieve an accurate and non-singular attitude error representation, so that the obtained Better, the implemented control method has fast response speed, high control accuracy and good robustness.

[0139] Although the present invention is described herein with reference to specific embodiments, it should be understood that these embodiments are merely illustrative of the principles and applications of the invention. It should be understood that many modifications may be made to the illustrative embodiments, and that other arrangements may be devised, without departing from the spirit and scope of the invention as defined by the appended claims. It should be understood that the various dependent claims and features described herein may be combined in ways other than those described in the original claims. It should also be understood that features described in conjunction with individual embodiments may be employed in conjunction with other described embodiments.

Claims

1. A full-state control method for a vector hexacopter based on a high-order dynamics model, characterized in that: The method includes: S1, the angular acceleration observer receives the actual velocity of the aircraft in the output state at time t The angular velocity ω t , and the rate of change of the control force output by the full-state controller at time t Medium control torque change rate Process and estimate the angular acceleration at time t t is an integer, and t≥1; S2, the angular acceleration at time t The linear acceleration of the aircraft at time t collected by the acceleration measurement unit Splice and get the full-state acceleration reference value at time t T is transpose; S3, the full-state controller is based on the full-state expected posture q at time t d,t , full-state acceleration reference value And the actual position q of the full state in the output state of the aircraft t , generates the control force spinor change rate at time t+1 Through the control quantity allocation method Distribute and generate the aircraft's servo and motor control signals, control the aircraft's servo and motor, change the aircraft's output state at time t, and achieve full state control of the aircraft.

2. The full-state control method for a vector hexacopter based on a high-order dynamics model according to claim 1, characterized in that: In step S1, the angular acceleration at time t is estimated The implementation is: S11, according to the ω received by the angular acceleration observer t and The state equation of the angular acceleration observer at time t is obtained: in, x t-1，1 and x t-1，2 are the first and second observations of the angular acceleration observer at time t-1, x t-1，1 and x t-1，2 The initial value of is any given value. and x t-1，1 and x t-1，2 The rate of change, x t，1 and x t，2 are the first and second observations of the angular acceleration observer at time t, respectively, x t，1 and x t，2 The initial value of is 0. and x t，1 and x t，2 The rate of change, Δt is the time interval, I3 is the 3×3 unit matrix, 03 is a 3×3 matrix with all elements equal to 0, L0 and L1 are the first and second constant diagonal matrices respectively; B1(ω t ),Θ(ω t ), Ω(J) are intermediate variables, Θ(ω0) is Θ(ω t ), Δω is the change in angular acceleration, ω0 is the initial value of angular velocity, and J is the inertia tensor of the aircraft; j i,k is the kth element in the i-th row of J, i=1,2,3, k=1,2,3, ω t1 、ω t2 and ω t3 are the angular velocity components of the aircraft in the X, Y and Z directions respectively; S12, according to the x in the state equation of the angular acceleration observer at time t t，1 and B1(ω t ), estimate the angular acceleration at time t 3. The full-state control method for a vector hexacopter based on a high-order dynamics model according to claim 2, characterized in that: In step S12 4. The full-state control method for a vector hexacopter based on a high-order dynamics model according to claim 2, characterized in that: Among them, l 01 ,l 02 ,l 03 ,l 11 ,l 12 ,l 13 are the first to sixth adjustable parameters of the angular acceleration observer.

5. The full-state control method for a vector hexacopter based on a high-order dynamics model according to claim 1, characterized in that: In step S3, the control force rotation rate of change at time t+1 is generated The implementation methods include: S31, according to the full state expected posture q at time t d,t And the full-state actual pose q in the output state of the aircraft t , define the full-state zero-order error in, q d,t =[p d,t ,Q d,t ] T , p d,t is the expected position vector at time t, Q d,t is the expected attitude quaternion at time t; q t =[p t ,Q t ] T , p t is the actual position vector at time t, Q t is the actual attitude quaternion at time t; e t =p d,t -p t , e t is the position error at time t; Q e,t is the attitude error quaternion at time t, Q t The conjugate quaternion, q e0,t and q ev,t They are the attitude error quaternion Q e,t The real and imaginary parts of S32, e t Derivative, get the first-order position error and the second-order position error S33, according to Q d,t , and get the expected angular velocity ω in the ground coordinate system d,t ; S34, according to Q t 、 ω t and ω d,t , define the angular velocity deviation ω e,t ; S35, according to ω e,t , construct attitude error quaternion Q e,t The real part q e0,t The second derivative of and the imaginary part q ev,t The second derivative of S36, according to and Define the intermediate variable q A ; According to q e0,t and q ev,t , define the intermediate variable q B ; According to ω t 、 ω d,t and ω e,t , define the intermediate variable Ω e / B ; in, ω e,t The first derivative of and are ω d,t The first and second derivatives of q ev,t The first derivative of ; I3 is the 3×3 unit matrix, R EB is the coordinate transformation matrix from the aircraft body coordinate system to the world coordinate system; S37, according to q B , construct intermediate variables According to q A ,q B and Ω e / B , construct the intermediate variable u t ; Among them, v t and j t are intermediate variables. For p t The second derivative of For p d,t The third-order derivative of , m is the total mass of the aircraft, J is the inertia tensor of the aircraft, p bc is the vector from the center of mass vector of the aircraft to the origin of the coordinate system where the propeller is located, is the gravity f g The first derivative of , M is the inertia matrix of the aircraft, is the first derivative of M; according to and Construct the full-state zero-order error e 1,t The first derivative of and the second-order derivative Thus, we get the intermediate variable S38, according to u t and E t , and the rate of change of the control force screw is obtained 6. The full-state control method for a vector hexacopter based on a high-order dynamics model according to claim 5, characterized in that: In step S33, It's Q d,t The conjugate quaternion of d / d,t is the expected angular velocity in the desired attitude coordinate system at time t.

7. The full-state control method for a vector hexacopter based on a high-order dynamics model according to claim 5, characterized in that: In step S34, Among them, R EB It is the coordinate transformation matrix from the aircraft body coordinate system to the world coordinate system.

8. The full-state control method for a vector hexacopter based on a high-order dynamics model according to claim 5, characterized in that: In step S35, Where T is the transpose, q ev,t The first derivative of and are ω e,t The first and second derivatives of q e0,t The first derivative of .

9. The full-state control method for a vector hexacopter based on a high-order dynamics model according to claim 5, characterized in that: In step S37, 10. The full-state control method for a vector hexacopter based on a high-order dynamics model according to claim 5, characterized in that: In step S38, Wherein, A=[A0 A1 A2], A is the adjustment coefficient matrix of the full-state controller, and A0, A1, and A2 represent the first to third adjustment coefficients of the full-state controller, respectively.

Images