Control method and system of miniature four-rotor unmanned aerial vehicle

Abstract

The invention discloses a control method and system of a miniature four-rotor unmanned aerial vehicle. The method comprises the following steps: acquiring a sensor signal sent by the miniature four-rotor unmanned aerial vehicle; according to the attitude angle information in the sensor signal and the total moment output to the miniature quadrotor unmanned aerial vehicle at the previous moment, acquiring a state quantity and a disturbance signal which are output by a linear extended state observer LESO in the controller and are associated with the attitude angle signal of the miniature quadrotor unmanned aerial vehicle; according to the state quantity, the disturbance signal, expected attitude information corresponding to the reference trajectory, and position information and speed information of the miniature quadrotor unmanned aerial vehicle in the sensor signal, a control instruction is output to the miniature quadrotor unmanned aerial vehicle, so that the miniature quadrotor unmanned aerial vehicle realizes high-dynamic flight based on the reference trajectory; wherein the reference track comprises a dynamic window drilling track penetrating through the target window. According to the method, the problem that the miniature unmanned aerial vehicle carries out fine tasks under a high dynamic condition is solved, and multi-state-quantity rapid tracking, stable control and planning are realized.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) control, and in particular to a control method and system for a micro quad-rotor UAV. Background Art

[0002] Micro-UAVs are required to perform rapid maneuvering missions in complex environments, such as reconnaissance, surveillance, target guidance, and close-range attack missions in disaster relief and communications relay. These missions place high demands on planning and precise tracking control. During highly dynamic flight, low Reynolds number aerodynamics, high-maneuverability flight dynamics, and rotorcraft actuators all contribute to the highly nonlinear and unsteady characteristics of UAVs. Traditional control methods suffer from low control accuracy and poor disturbance immunity in these conditions.

[0003] Diverse combat missions and environmental uncertainties increase the difficulty for micro-UAVs to perform rapid maneuver operations. Micro-UAVs are inherently small in size and have low Reynolds numbers, making them susceptible to atmospheric disturbances. Their flight dynamics exhibit nonlinear characteristics. The underactuated nature of quadcopters makes it difficult for traditional control methods to effectively track both trajectory position and its higher-order state variables. Furthermore, the attitude control used in most current open-source flight controllers is based on Euler angles. While the Euler angle method has clear physical meaning, it requires a large number of transcendental function calculations and presents issues with the rotation order. The resulting singularities and gimbal lock issues make it unsuitable for high-maneuver flight control.

[0004] In addition, in complex environments, micro-UAVs are faced with the need to pass through narrow windows. How to generate a reasonable and safe dynamic window-drilling trajectory while achieving stable flight control is a technical problem that needs to be solved. Summary of the Invention

[0005] In view of the above-mentioned shortcomings and deficiencies of the prior art, the present invention provides a control method and system for a micro quad-rotor UAV, which solves the technical problems of singularity and universal lock in the prior art, while realizing precise tracking and control of multiple state quantities, having strong anti-interference ability, and being conducive to the high dynamic flight of the micro UAV.

[0006] In order to achieve the above objectives, the main technical solutions adopted by the present invention include:

[0007] In a first aspect, an embodiment of the present invention provides a control method for a micro quadrotor drone, comprising:

[0008] S100, obtaining a sensor signal sent by the micro quadrotor drone;

[0009] S200, according to the attitude angle information in the sensor signal and the total torque output to the micro quadrotor drone at the last moment, obtaining the state quantity and disturbance signal associated with the attitude angle signal of the micro quadrotor drone output by the linear extended state observer LESO in the controller;

[0010] S300, outputting a control instruction to the micro-quadrotor drone based on the state quantity and the disturbance signal, the desired attitude information corresponding to the reference trajectory, and the position information and speed information of the micro-quadrotor drone in the sensor signal, so that the micro-quadrotor drone achieves high dynamic flight based on the reference trajectory;

[0011] The reference trajectory is a dynamic window drilling trajectory that passes through the target window.

[0012] Optionally, the state quantity and disturbance signal associated with the attitude angle signal in S200 include: z1 is an estimated value of the attitude angle, z2 is an estimated value of the attitude angular velocity, and z3 is an estimated value of the disturbance;

[0013]

[0014] Among them, β1, β2, and β3 are the gains of LESO, y is the attitude angle information in the sensor signal, and u is the control quantity corresponding to the control torque M.

[0015] Optionally, the outputting of the control instruction to the micro quadrotor UAV in S300 includes outputting a control torque M at a current moment for causing the micro quadrotor UAV to reach a desired position and a desired yaw angle;

[0016]

[0017] in, The three-channel disturbance estimation of LESO for roll, pitch and yaw, is the disturbance compensation for the three-axis torque given by LESO; is the angular velocity in the body coordinate system, J is the inertial matrix of the UAV in the body coordinate system, R is the rotation matrix from the body coordinate system to the inertial coordinate system,

[0018]

[0019] R d is the desired rotation matrix, Ω d is the desired attitude angular velocity, e R is the attitude angle error, e Ω is the angular velocity error; k R 、k Ω are the control coefficients that need to be adjusted, ψ, θ, are the attitude angles of the current micro quadrotor drone, namely the yaw, pitch and roll angles.

[0020] Optionally, in the simulation stage, the guidance and control integration of the micro quadrotor UAV is realized based on the linear extended state observer-geometric tracking control theory, and a subsystem is established through the Matlab Function module of Simulink to encapsulate the implementation; and the rotation matrix R is used on the SE(3) group during the implementation process; and the attitude error function is: R d is the desired rotation matrix; I is the identity matrix;

[0021] The total thrust and total torque are pre-given control inputs to track the desired trajectory position p of the center of mass position. d (t) and the desired yaw angle Ψ d (t);

[0022] The origin of the cross-shaped quadcopter's body coordinate system is located at the center of mass of the aircraft, and the body coordinate axis Located on the plane defined by the centers of the four rotors, the body coordinate axis It is perpendicular to the plane and upwards, and in the same direction as the total thrust.

[0023] Optionally, when the micro quad-rotor drone is drilling a window, the diameter of the window being drilled is smaller than the diameter of the quad-rotor drone;

[0024] Before S100, the method further includes:

[0025] A100, based on the differential flatness characteristics of the quadrotor drone, performs trajectory optimization and generation, and uses the MinimumSnap trajectory planning method to combine the known starting position, ending position, and window position to obtain the window drilling trajectory as a reference trajectory.

[0026] Optionally, the A100 includes:

[0027] A101. Divide the window drilling trajectory into an approaching segment and a crossing segment;

[0028] A102. Use an inverse planning strategy to construct a crossing segment trajectory that satisfies dynamic constraints. Based on the initial state parameters of the crossing segment, reversely design the approach segment trajectory from the specified starting point to the starting point of the crossing segment.

[0029] A103. Then, the Minimum Snap trajectory planning method is used, based on the differential flatness theory, to plan a reference window drilling trajectory that satisfies velocity continuity and acceleration continuity at the design waypoint.

[0030] Alternatively, the target window is defined as a rectangular plane Π perpendicular to the ground plane Σ. Based on the target window width W and the quadrotor diameter L, the minimum rolling maneuver angle β = arccos(W / L) required to be completed is estimated, and the auxiliary plane Θ is constructed to meet the following conditions:

[0031] (1) Contains the target crossing point C; (2) Is orthogonal to plane Π; (3) Forms an angle β with the ground plane Σ;

[0032] In the plane Θ, a set of unit vectors {e1, e2, e3} are used to establish an orthogonal coordinate system.

[0033] e1 axis: in plane Θ, perpendicular to the intersection of plane Θ and ground plane Σ and pointing downward;

[0034] e3 axis: parallel to the plane Θ, normal vector upward;

[0035] e2 axis: along the intersection of plane Θ and ground plane Σ, forming a right-handed coordinate system with e1 and e3;

[0036] Plan the crossing segment trajectory BCD on plane Θ; the crossing segment consists of a semicircular arc with BD as the diameter, define the crossing speed v0, and satisfy the following constraints:

[0037] Constraint 1: Kinematic characteristics at vertex C, velocity v C , acceleration a C , rotor plane normal vector n R :v C =v0·e2;a C =gtanβ·e1;n R =e3;

[0038] The total thrust f satisfies: is the roll angle. Point C is at the highest point of the trajectory. The normal vector of the rotor plane at point C is defined to be parallel to e3. The rotor thrust and gravity are balanced in the vertical direction. The acceleration of point C on the trajectory is obtained. The direction of the acceleration is the projection of the total thrust f on the horizontal plane.

[0039] Constraint 2: Boundary point B, D parameters:

[0040] According to the centripetal acceleration formula,

[0041] Point B: v B =-‖v C ‖e1,a B =‖a C ‖e2

[0042] Point D: v D =‖v C ‖e1,a B =-‖aC ‖e2

[0043] Point B is the starting point of the crossing segment on plane π, and the position of point B is inferred based on the position of point C; point D is the end point of the crossing segment, and is symmetrically distributed with point B relative to plane π.

[0044] Optionally, the window drilling trajectory of the reference trajectory of A103 includes:

[0045]

[0046] Among them, P AB (t) represents the trajectory close to segment AB, P BC (t) represents the trajectory of the ascending part BC in the crossing segment, P CD (t) represents the return trajectory after crossing the window; t is time, t A , t B , t C , t D Respectively represent the time of points A, B, C, and D on the trajectory; p AB 、p BC 、p CD are all polynomial coefficients;

[0047] Constraints include: position continuity, velocity continuity, acceleration continuity, initial and end state constraints, maximum velocity and maximum acceleration.

[0048] Alternatively, assuming there are M trajectories, the total cost function and expression of all trajectories are as follows:

[0049]

[0050] Solve for the minimum value of J and satisfy the continuity constraint, that is, the hard constraint A eq p m =b eq , its mathematical expression reasoning:

[0051] For two adjacent trajectories, ensuring the continuity of the fourth-order derivative at the junction and the smoothness of the zero-order and first-order derivatives constitutes the trajectory continuity constraint. The expression of its position continuity is as follows:

[0052]

[0053]

[0054] Apply differential constraints to the beginning and end of the mth trajectory:

[0055]

[0056] Combining the continuity constraint and the differential constraint, we get the following formula:

[0057]

[0058] Finally, a constrained QP problem is constructed:

[0059]

[0060]

[0061] In the MATLAB simulation environment, the quadprog() function is used to solve the polynomial coefficients, and finally the trajectory points are discretized;

[0062] k: k-order derivative; For all positive integers i greater than or equal to k, calculate And add them together, and the same goes for l.

[0063] The k-th derivative of the m-th trajectory.

[0064]

[0065] T m : The time corresponding to the intersection of the mth and m+1th trajectories. For convenience, relative time is used, that is, for any trajectory, their time is considered to be [0, T m ).

[0066] p m,i : The coefficient of the i-th term of the m-th trajectory polynomial.

[0067] T m ik or lk to the power of .

[0068] p m+1,l : The coefficient of the first term of the polynomial of the m+1th trajectory.

[0069] A m : Matrix A m is the continuity constraint matrix in the optimization problem (rows = number of constraints, columns = coefficient dimensions). This constraint enforces continuity of position, velocity, acceleration, and other derivatives at intermediate waypoints along the mth trajectory. Its physical significance is to ensure that at these waypoints, the object's position is continuous (no jumps), its velocity is continuous (no sudden changes in velocity), its acceleration is continuous (forces are applied smoothly), and its higher-order derivatives (here, jerks) are continuous.

[0070] p m : The polynomial coefficient vector of the mth trajectory [p m,0 p m,1 …p m,N ] abbreviation.

[0071] d m : Constraints for the mth segment of the trajectory, specific values ​​of the physical state. For the intermediate points, the position constraint is the actual coordinates of the waypoint (derived from the mission requirements), and the velocity / acceleration constraint is 0 (indicating that the left and right derivatives are equal).

[0072] A M : The constraint matrix of the Mth (last) segment of the trajectory, which handles the boundary conditions of the end point (such as fixed position / velocity / acceleration). The same applies to the starting point A_0.

[0073] p M : The polynomial coefficient vector of the Mth segment trajectory [p m,0 p m,1 …p m,N ] is an abbreviation of , since there are a total of M trajectories, which are the polynomial coefficients of the end points.

[0074] d M : Constraints for the Mth (final) segment of the trajectory, specific values ​​of the physical state. Ensure that the position / velocity / acceleration constraints at the endpoint meet the mission requirements. The same applies to the starting point d_0.

[0075] The cost function of the mth trajectory,

[0076] in is the variable to be optimized; Q is a symmetric positive definite matrix that defines the cost weight of the objective function.

[0077] In a second aspect, the present invention further provides a control system for a micro quadrotor drone, comprising: a ground control station responsible for monitoring, controlling, and task managing the micro quadrotor drone, and a quadrotor drone for dynamic window drilling;

[0078] The ground station executes any of the methods described in the first aspect above to enable the quadrotor drone to achieve high dynamic flight based on a reference trajectory.

[0079] The beneficial effects of the present invention are:

[0080] The control method of the embodiment of the present invention can enable the drone to maintain a large roll angle and pass through a slit smaller than its own diameter, solving the defect in the prior art that quadcopter drones have difficulty maintaining a stable large-angle roll posture for a long time and cannot pass through slits smaller than their own diameter; ensuring the safety, reproducibility and adaptability of maneuvering tasks.

[0081] At the same time, through simulation experiments, the method of the embodiment of the present invention also avoids the need for a large number of transcendental function operations for Euler angles, and the problems caused by the rotation order, as well as the resulting singularity and universal lock problems. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 Schematic diagram of the drone model and coordinate system in this embodiment;

[0083] Figure 2 Schematic diagram of the overall structure of the LESO-GTC control system in this embodiment;

[0084] Figure 3 Schematic diagram of the window drilling task planning strategy based on the LESO-GTC control system in this embodiment;

[0085] Figure 4 For this embodiment Figure 3 Schematic diagram of the crossing section planning;

[0086] Figure 5 Schematic diagram of the simulink digital simulation test platform structure in this embodiment;

[0087] Figure 6 This is the simulink digital simulation effect diagram;

[0088] Figure 7 This is a schematic diagram of the simulink digital simulation results, such as position, velocity, and attitude angle signals;

[0089] Figure 8 A schematic diagram of a control method for a micro quadrotor drone provided by an embodiment of the present invention;

[0090] Figure 9 This is a schematic diagram of a micro quadrotor drone drilling a window according to an embodiment of the present invention. DETAILED DESCRIPTION

[0091] In order to better explain the present invention and facilitate understanding, the present invention is described in detail below through specific implementation methods in conjunction with the accompanying drawings.

[0092] The purpose of this embodiment of the present invention is to provide a method for precise quadrotor control and planning based on LESO-GTC (Linear Extended State Observer-Geometric Tracking Control), addressing the challenge of micro-UAVs performing precise tasks under highly dynamic conditions and enabling rapid tracking, stable control, and planning of multiple state variables. This method first proposes LESO-GTC, implementing multi-state tracking control of a micro-high-maneuverability UAV under various constraints. Then, using a window-drilling task as an example, it proposes a trajectory planning strategy for highly dynamic tasks, generating a feasible smooth trajectory as a reference input.

[0093] The control method of the embodiment of the present invention can realize precise control of the micro quadrotor drone, ensure the dynamic response and maneuverability of the micro drone, and is suitable for engineering applications.

[0094] The current classification standards for drones are divided into micro, light, small, medium and large according to performance indicators such as empty weight, take-off weight and flight speed;

[0095] Micro unmanned aerial vehicle: refers to an unmanned aerial vehicle with an empty weight of less than 0.25 kg, a maximum true flight altitude of no more than 50 meters, a maximum level flight speed of no more than 40 kilometers per hour, and radio transmission equipment that meets the technical requirements of micro-power short-range technology, and which can be manually intervened and controlled at any time throughout the entire process.

[0096] The solution of the embodiment of the present invention is applicable to micro unmanned aerial vehicles, of which the micro quad-rotor drone belongs to the micro unmanned aerial vehicle. The drone and quad-rotor drone mentioned below all refer to micro quad-rotor drones.

[0097] Example 1

[0098] This embodiment provides a control method for a micro quadrotor drone, and the executor of the method can be a ground control station that controls the flight of the micro quadrotor drone; the ground control station of this embodiment controls the quadrotor drone to perform dynamic window drilling based on the following steps, which solves the defect in the prior art that the quadrotor drone has difficulty maintaining a stable large-angle rolling posture for a long time and cannot pass through narrow gaps smaller than its own diameter; and ensures the safety, reproducibility and adaptability of maneuvering tasks.

[0099] like Figure 8 As shown, the method of this embodiment may include the following steps:

[0100] S100, obtaining a sensor signal sent by the micro quadrotor drone;

[0101] S200, according to the attitude angle information in the sensor signal and the total torque output to the micro quadrotor drone at the last moment, obtaining the state quantity and disturbance signal associated with the attitude angle signal of the micro quadrotor drone output by the linear extended state observer LESO in the controller;

[0102] S300, outputting a control instruction to the micro-quadrotor drone based on the state quantity and the disturbance signal, the desired attitude information corresponding to the reference trajectory, and the position information and speed information of the micro-quadrotor drone in the sensor signal, so that the micro-quadrotor drone achieves high dynamic flight based on the reference trajectory;

[0103] The reference trajectory is a dynamic window drilling trajectory that passes through a designated window.

[0104] It should be noted that the diameter W of the window slit in this embodiment (eg Figure 9 (1) is smaller than the diameter L of the micro quadrotor drone (as shown in Figure 9 (2)), as shown in Figure 9 shown.

[0105] In the specific simulation experiment stage, in this embodiment, the guidance and control integration of the micro quadrotor UAV is designed based on the linear extended state observer-geometric tracking control theory, and a subsystem is established through the Matlab Function module of Simulink to encapsulate and implement it.

[0106] Example 2

[0107] To better understand the various steps in the above method, Figures 1 to 5 The following control variables, control torques, total torques and control commands are all M = [M1; M2; M3].

[0108] like Figure 2 As shown in the figure, the input of the LESO submodule is the control quantity M and the attitude angle signal (such as ψ, θ, ), the output of the LESO submodule is the state estimation (such as z1, z2, z3); ​​the input of the GTC submodule is the state estimation (such as z1, z2, z3) output by the LESO submodule, the sensor signals (such as p, v, R, Ω) and the reference trajectory position and its high-order derivatives and the desired yaw angle (such as p d , Ψ d ).

[0109] The sensor signals of this embodiment may include a series of parameters such as position, velocity, acceleration, attitude angle, attitude angular velocity, etc. In addition, the reference trajectory and its high-order derivatives may be pre-calculated using a trajectory planning algorithm, and the yaw angle may be defined as being constantly zero.

[0110] exist Figure 5 In the figure, INS is the inertial navigation system, FMS is the flight management system, and FCS is the flight control system. The digital simulation test platform of the micro high-dynamic UAV is used to simulate the above control method.

[0111] The following table gives the definition of parameter symbols used in the following formulas;

[0112]

[0113]

[0114] 1.1. The design process of the geometry tracking controller (LESO submodule) is described as follows:

[0115] Based on inertial coordinate system and body coordinate system like Figure 1 As shown, the origin of the cross-shaped quadcopter body coordinate system is located at the mass center of the aircraft, and the body coordinate axis Located on the plane defined by the centers of the four rotors, the body coordinate axis Vertically upward from the plane, and the total thrust f( Figure 1 The directions of f1, f2, and f3 in the figure represent the thrust of each rotor are the same.

[0116] According to the differential flatness property of the quadrotor UAV, the complex high-dimensional space trajectory planning problem is transformed into trajectory planning for the center of mass position and yaw angle, and the expected trajectory position p is selected. d (t) and the desired yaw angle Ψ d The desired direction of (t) is used as the LESO input. d (t) is the function of the expected position with respect to time, i.e., the expected trajectory position, t is time, and the subscript d represents the expected position.

[0117] Introducing the special Euclidean group SE(3), which is Semidirect product with the special orthogonal group SO(3):

[0118]

[0119] Assuming the inertial coordinate system The expression of the total thrust in the inertial reference frame is R and R in the following text BW Two symbols that represent the same meaning.

[0120]

[0121] Assume that the torque generated by each propeller in the quadrotor drone is proportional to its thrust, and its torque coefficient is c τf , then the total thrust f and total torque M can be expressed as

[0122]

[0123] The determinant of the above 4×4 matrix is ​​8c τf d 2 , so when d≠0 and c τf ≠0, the determinant is not 0, and the matrix is ​​invertible. For a given total thrust f and total torque M, the thrust f of each rotor is i (i=1,2,3,4) can be obtained from formula (1-3). Thus, the total thrust and total torque As a pre-given control input, the desired trajectory position p of the center of mass is tracked. d (t) and the desired yaw angle Ψ d (t). M1, M2, and M3 represent the rolling moment, pitching moment, and yaw moment, respectively.

[0124] The dynamic kinematic model of the quadrotor drone is described as follows:

[0125]

[0126] The superscript · indicates the derivative. The rotation matrix R is R BW The following formula (1-5):

[0127]

[0128] In formula (1-5), ψ, θ, They represent the actual yaw, pitch and roll attitude angles of the current micro quadrotor drone respectively.

[0129] Hat operator Λ: Defined as Map the vector to its antisymmetric matrix,

[0130] Right now

[0131]

[0132] The hat operator has the following properties:

[0133]

[0134] The translational dynamics of a quadrotor drone is determined by the total thrust Control, while the total thrust The direction along the body coordinate system By controlling the magnitude of the total thrust f and The desired direction tracks the given translation instruction such as the desired trajectory position p d (t). It should be noted that is the total thrust converted from the body coordinate system to the inertial coordinate system.

[0135] The corresponding expected posture: The desired posture is tracked and controlled by the given torque M.

[0136]

[0137] Ψ d is the desired yaw angle, g is the acceleration due to gravity. The subscript d indicates the desired; t d is an intermediate variable. In this embodiment, an intermediate coordinate system is introduced This coordinate system is obtained by rotating the ground coordinate system around the z-axis by the yaw angle, so,

[0138] The control command includes the three-dimensional desired trajectory position p d (t) and the desired yaw angle in one dimension

[0139] Ψ d (t), that is, to realize the control and tracking of three-dimensional position instructions and one-dimensional heading. The control goal is to make Where Proj[·] represents Normalized projection onto an orthogonal plane.

[0140] Define the position error e p , speed error e v and acceleration error e a as follows:

[0141]

[0142] p d is the expected position, v d Expected speed, a d Expected acceleration.

[0143] Define a posture error function ψ(·) in the SO(3) group:

[0144]

[0145] R d is the desired rotation matrix; I is the identity matrix; δR is the small change of R; η is the angular velocity matrix;

[0146] When R = R d Sometimes, there are Therefore, the attitude control target based on the rotation matrix is I3 is the 3*3 identity matrix, and the change of the rotation matrix is ​​expressed as When , the derivative expression of the error function of the rotation matrix is ​​as follows:

[0147]

[0148] Among them, the vee operator ∨: is the inverse of the hat operator, which converts the antisymmetric matrix into a vector, that is,

[0149] Then the attitude angle error e R and angular velocity error e Ω as follows:

[0150]

[0151] The expected angular velocity

[0152] For a given smooth tracking instruction (i.e. including p d (t),Ψ d (t) Control instruction)p d (t),Ψ d (t), body coordinate system The definition of is as follows;

[0153]

[0154] Among them, k p ,k v is a positive number, and the denominator is not 0, is the second derivative of the desired position with respect to time, that is, the desired acceleration.

[0155] Use a PD controller with feedforward to control the input (i.e., controller output f and M):

[0156]

[0157] where k p ,k v is a positive constant, It can be obtained by the differential flatness property, Ω is the attitude angle matrix, Ω d is the desired attitude angular velocity.

[0158] 1.2. The design process of the linear extended state observer (GTC submodule) is described as follows:

[0159] The LESO submodule is sensitive to the attitude angle. Therefore, a linear extended state observer (GTC) is introduced to enhance the anti-interference ability of the UAV for the attitude angle signal input from the sensor.

[0160] The attitude control loop of the quadrotor drone is a second-order system. Assuming that the added extended state x1=y, x3=f, y is a system output (in the attitude control system of the GTC submodule, it belongs to the attitude angle output), represents the derivative with respect to y.

[0161] List the state space equation in the following form, that is, the output y is determined by the current state x and the input control variable u:

[0162]

[0163] Where u is the control quantity, C=[1 0 0], is the differential of the acceleration and is physically bounded. b represents the control input gain, which is a partially known quantity (its approximate value is b0). From the above equation, the state space expression of the corresponding linear extended state observer (LESO) can be obtained as:

[0164]

[0165] z is the state estimate, is an estimate of the output.

[0166] When the observer gain L is appropriately selected, it is L = [β1β2β3] T In the case of , provide the state estimation of formula (1-16), z i ≈x i (i=1,2,3), z3 of the state observer is an approximation of f. Formula (1-16) can be rewritten as follows:

[0167]

[0168] β1 is the correction rate of the control position (y zero-order derivative) error z1, β2 is the correction rate of the control velocity (y first-order derivative) error z2, and β3 is the update rate of the control total disturbance z3.

[0169] z i ≈x i Represents the estimated value. In the attitude control system, z1 is the estimated value of the attitude angle, z2 is the estimated value of the attitude angular velocity, and z3 is the estimated value of the disturbance.

[0170] Introducing observer bandwidth ω o ,

[0171]

[0172] v o It is the only parameter to be adjusted in LESO and is given based on experience. For example, if the system noise is large, reduce ω o , the disturbance changes quickly, increasing ω o , ensuring that LESO is BIBO stable.

[0173] Use z i (i=1, 2, 3) replace the attitude angle, attitude angular velocity and disturbance input by the sensor respectively.

[0174] After the sensor's attitude angle signal and torque control signal M are input to the LESO, the estimated attitude angle z1, attitude angular velocity z2, and disturbance z3 are output. Active disturbance suppression is achieved by subtracting the observed disturbance z3. The torque control law in equation (1-14) then becomes:

[0175]

[0176] in is the disturbance estimation of LESO for the roll, pitch and yaw channels.

[0177] SO(3) is a special orthogonal group, SE(3) is a special Euclidean group, SO(3) and SE(3) are continuous in space and both belong to Lie groups, that is, groups with continuous smooth properties.

[0178] like Figure 2 The schematic diagram of the LESO-GTC control system is shown in Figure 2. LESO-GTC avoids the control singularity problem that occurs at critical points (the point where any axis rotates to 90 degrees) when using the Euler angle model by using a rotation matrix on the SE(3) group. It also avoids the problems of the quaternion method being sensitive to small measurement noise, non-intuitive parameter tuning, and disentanglement, making it suitable for large-angle and high-dynamic flight. The addition of the extended state observer improves the controller's anti-interference and anti-noise capabilities.

[0179] Some controller parameters of LESO-GTC are the ones used in the Simulink simulation experiment.

[0180]

[0181]

[0182] It should be noted here that M and f control the attitude and position of the drone at the same time. Simply put, f controls the total thrust output, and M controls the thrust distribution of the four rotors.

[0183] 1.3 Trajectory Planning Strategy for Window Drilling of Highly Dynamic Rotor UAV

[0184] The starting, ending, and intermediate window positions are known. Trajectory optimization and generation are performed based on the differential flatness characteristics of the quadrotor UAV. The Minimum Snap trajectory planning method is adopted, and a seventh-order polynomial is used to fit key nodes. The integral of the square of the fourth-order derivative (Snap) of the polynomial trajectory is used as the cost function. The optimal trajectory is generated by constraining the three-dimensional position, velocity, acceleration, and yaw angle at specific waypoints, while ensuring that the speed, acceleration, and input restrictions of the entire trajectory are met.

[0185] The following window drilling trajectory planning strategy is proposed:

[0186] First, as Figure 3As shown in the figure, the window drilling trajectory is divided into an approach segment and a crossing segment. The flight trajectory is divided into two stages: the approach segment (AB) and the crossing segment (BCD). Using an inverse planning strategy, a crossing segment trajectory that satisfies dynamic constraints is first constructed. Then, based on the initial state parameters of the crossing segment (including kinematic parameters such as position coordinates, velocity vector, and acceleration vector), the approach segment trajectory is inversely designed from the specified starting point to the starting point of the crossing segment.

[0187] Second, the planning method of the crossing section is as follows:

[0188] The target window is simplified to a rectangular plane Π perpendicular to the ground plane Σ. According to the safety window width W and the diameter L of the drone, the minimum rolling maneuver angle β required to be completed is estimated.

[0189] arccos(W / L). Construct the auxiliary plane Θ so that it satisfies the following conditions:

[0190] (1) Contains the target crossing point C (crossing point C in the inertial reference system coordinate p C );

[0191] (2) perpendicular to plane π;

[0192] (3) It forms an angle β with the ground plane Σ.

[0193] In the plane Θ, a set of unit vectors {e1, e2, e3} are used to establish an orthogonal coordinate system, such as Figure 3 As shown, where:

[0194] e1 axis: in plane Θ, perpendicular to the intersection of plane Θ and ground plane Σ and pointing downward;

[0195] e3 axis: parallel to the plane Θ, normal vector upward;

[0196] e2 axis: along the intersection of plane Θ and ground plane Σ, forming a right-handed coordinate system with e1 and e3.

[0197] Plan the crossing segment trajectory BCD on plane Θ. The crossing segment is designed as a semicircular arc with diameter BD. The crossing speed v0 is designed based on the UAV dynamic conditions and meets the following constraints:

[0198] (Constraint 1) Kinematic characteristics at vertex C, velocity v C , acceleration a C , rotor plane normal vector n R :

[0199] v C =v0·e2

[0200] a C =gtanβ·e1(1-20)

[0201] n R =e3

[0202] Total thrust meets: is the roll angle. Point C is at the highest point of the trajectory. The normal vector of the rotor plane here is designed to be parallel to e3, so that the rotor thrust and gravity are balanced in the vertical direction. The acceleration of point C on the trajectory can be calculated, and the direction is the projection of the total thrust f on the horizontal plane.

[0203] (Constraint 2) Boundary point B, D parameters:

[0204] According to the centripetal acceleration formula,

[0205] Point B: v B =-‖v C ‖e1,a B =‖a C ‖e2

[0206] Point D: v D =‖v C ‖e1,a B =-‖a C ‖e2

[0207] Point B is the starting point of the crossing segment on plane π. The arc radius can be obtained from the circumference theorem, and the position of point B can be deduced based on the position of point C. Point D is the end point of the crossing segment and is symmetrically distributed with point B relative to plane π. Figure 4 shown.

[0208] Third, finally, based on the Minimum Snap trajectory planning method, a reference trajectory ABCD is designed that satisfies differential flatness and has continuous velocity and acceleration.

[0209] Based on the differential flatness theory, in one-dimensional space, an N-order polynomial is used to fit the m-th trajectory, where the horizontal axis is time t and the vertical axis is position P m (t):

[0210]

[0211] The polynomial has N+1(p m,0 ,…,p m,N ) unknown coefficients, which are expressed as vector form [p m,0 p m,1 …p m,N ], and can be abbreviated as p m m refers to the mth trajectory, and N is a polynomial.

[0212] By taking the first-order derivative, second-order derivative, third-order derivative, and fourth-order derivative, we can get the velocity V, acceleration a, jerk, and jerk snap of the trajectory at any time:

[0213]

[0214] Construct a seventh-order polynomial trajectory connecting the approach segment and the crossing segment:

[0215]

[0216] Constraints include: position continuity, velocity continuity, acceleration continuity, initial and end state constraints, maximum velocity and maximum acceleration constraints.

[0217] The Minimum Snap trajectory optimization method is used to minimize the thrust change rate to save energy. The constructed minimization objective function expression is as follows:

[0218]

[0219] (T m -T m-1 ) N+N-7 Abbreviated as T N+N-7 , and assume that the time of each trajectory is known. By solving the m The cost function can be used to solve the coefficient p of the optimal trajectory m .

[0220] The time T for each trajectory segment is allocated using the trapezoidal allocation method. Assume that each segment has the following scenario: acceleration from zero speed to maximum speed, maintaining speed for a period of time, and then decelerating to the endpoint, where the speed returns to zero. Both acceleration and deceleration use a predetermined maximum acceleration. Finally, the expected average speed is used to obtain the duration of each segment.

[0221] Minimum Snap uses a 7th-degree polynomial, meaning N = 7, with a total of 8 unknowns. Its Q matrix is ​​as follows:

[0222]

[0223] Assuming there are M trajectories, the total cost function and expression of all trajectories are as follows:

[0224]

[0225] Solve for the minimum value of J and satisfy the continuity constraint (hard constraint) A eq p m =b eq , its mathematical expression is as follows:

[0226] For two adjacent trajectories, ensuring the continuity of the fourth-order derivative at the junction and the smoothness of the zero-order and first-order derivatives constitutes the trajectory continuity constraint, and its mathematical expression is as follows:

[0227]

[0228] Apply differential constraints to the beginning and end of the mth trajectory:

[0229]

[0230] Combining the continuity constraint and the differential constraint, we can get the following formula:

[0231]

[0232] Finally, a constrained QP problem is constructed:

[0233]

[0234] In the above formula, P m With p m There is a one-to-one correspondence, and P m With P m (t), It is the same symbol, P m is the vector of parameters, P m (t) corresponds to the trajectory. The superscript k represents the kth order derivative, T m Indicates the end time of the mth trajectory.

[0235] In the MATLAB simulation environment, the quadprog() function is used to solve the polynomial coefficients, and finally the trajectory points are discretized.

[0236] In this embodiment, based on the designed linear extended state observer-geometric tracking controller, a smooth reference trajectory and its high-order derivatives are input to directly output torque and total thrust, thereby realizing synchronous and precise tracking control of multiple state quantities of high dynamic trajectory under a finite feasible domain; at the same time, based on the trajectory planning method of differential flatness and Minimum Snap, the autonomous planning problem is solved for high maneuverability tasks such as drilling through narrow windows. It is understandable that in this application, the expected position and expected yaw angle in the window drilling trajectory are output through the simulated linear extended state observer-geometric tracking controller. Figure 6 and Figure 7 The simulink digital simulation effect diagram and simulink digital simulation results such as position, velocity, and attitude angle signals are shown.

[0237] The above scheme avoids the need for a large number of transcendental function calculations for Euler angles, and also avoids the problems caused by the rotation order, as well as the resulting singularity and universal lock problems; it achieves precise tracking and control of multiple state quantities, has strong anti-interference ability, and is conducive to the high-dynamic flight of micro-UAVs; it solves the problem that quadcopters are difficult to maintain a stable large-angle rolling posture for a long time in window drilling tasks, and ensures the safety, reproducibility, and adaptability of maneuvering tasks; the autonomous trajectory planning strategy for window drilling tasks can take into account the conditions of dynamic constraints, actuator constraints, and power limitation to generate an optimal trajectory with time constraints, which has reference significance for the further development of autonomous planning methods.

[0238] Example 3

[0239] In addition, an embodiment of the present invention also provides a control system for a micro quad-rotor drone, including: a ground control station responsible for monitoring, controlling and task managing the micro quad-rotor drone and a quad-rotor drone for dynamic window drilling; the ground control station executes the method described in Example 1 or Example 2 above to enable the quad-rotor drone to achieve high dynamic flight based on a reference trajectory.

[0240] The ground control station of this embodiment may be any electronic device, which may include a memory and a processor.

[0241] The memory stores a program or instruction, and when the processor executes the program or instruction, the steps of any of the above methods are implemented. The memory and the processor may be connected via a bus or other means. The processor may include one or more processing units, and the processor may be a central processing unit (CPU), a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA), or other chips.

[0242] An embodiment of the present invention further provides a readable storage medium having a program or instruction stored thereon, which, when executed by a processor, implements the steps of any of the above-described methods. The readable storage medium includes a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0243] The readable storage medium provided by the present disclosure implements the steps of the method of any of the above technical solutions when the program or instruction is executed by the processor, so the readable storage medium includes all the beneficial effects of any of the above technical solutions.

[0244] In the description of the present invention, it should be understood that the terms "first" and "second" are used for descriptive purposes only and should not be understood to indicate or imply relative importance or implicitly specify the number of the technical features indicated. Therefore, a feature specified as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, "plurality" means two or more, unless otherwise specifically defined.

[0245] In the present invention, unless otherwise expressly specified or limited, the terms "mounted," "connected," "connect," "fixed," etc. should be understood broadly. For example, they may refer to fixed connection, detachable connection, or integration; mechanical connection or electrical connection; direct connection or indirect connection through an intermediate medium; and internal communication between two components or interaction between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0246] In the present invention, unless otherwise expressly specified or limited, when a first feature is "above" or "below" a second feature, it may mean that the first and second features are in direct contact, or that the first and second features are in indirect contact through an intermediate medium. Furthermore, when a first feature is "above," "above," or "above" a second feature, it may mean that the first feature is directly above or obliquely above the second feature, or simply means that the first feature is at a higher level than the second feature. When a first feature is "below," "below," or "below" a second feature, it may mean that the first feature is directly below or obliquely below the second feature, or simply means that the first feature is at a lower level than the second feature.

[0247] In the description of this specification, the terms "one embodiment", "some embodiments", "embodiments", "examples", "specific examples" or "some examples" refer to the specific features, structures, materials or characteristics described in conjunction with the embodiment or example and included in at least one embodiment or example of the present invention. In this specification, the schematic expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0248] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may alter, modify, replace and modify the above embodiments within the scope of the present invention.

Claims

1. A control method for a micro quad-rotor drone, characterized in that: include: S100, obtaining a sensor signal sent by the micro quadrotor drone; S200, according to the attitude angle information in the sensor signal and the total torque output to the micro quadrotor drone at the last moment, obtaining the state quantity and disturbance signal associated with the attitude angle signal of the micro quadrotor drone output by the linear extended state observer LESO in the controller; S300, outputting a control instruction to the micro-quadrotor drone based on the state quantity and the disturbance signal, the desired attitude information corresponding to the reference trajectory, and the position information and speed information of the micro-quadrotor drone in the sensor signal, so that the micro-quadrotor drone achieves high dynamic flight based on the reference trajectory; The reference trajectory is a dynamic window drilling trajectory that passes through the target window.

2. The method according to claim 1, characterized in that The state quantity and disturbance signal associated with the attitude angle signal in S200 include: z1 is the estimated value of the attitude angle, z2 is the estimated value of the attitude angular velocity, and z3 is the estimated value of the disturbance; Among them, β1, β2, and β3 are the gains of LESO, y is the attitude angle information in the sensor signal, and u is the control quantity corresponding to the control torque M.

3. The method according to claim 1, characterized in that Outputting a control instruction to the micro quadrotor drone in S300 includes outputting a control torque M at a current moment for causing the micro quadrotor drone to reach a desired position and a desired yaw angle; in, The three-channel disturbance estimation of LESO for roll, pitch and yaw, z 3θ z 3ψ is the disturbance compensation for the three-axis torque given by LESO; is the angular velocity in the body coordinate system, J is the inertial matrix of the UAV in the body coordinate system, R is the rotation matrix from the body coordinate system to the inertial coordinate system, R d is the desired rotation matrix, Ω d is the desired attitude angular velocity, e R is the attitude angle error, e Ω is the angular velocity error; k R 、k Ω are the control coefficients that need to be adjusted, ψ, θ, are the attitude angles of the current micro quadrotor drone, namely the yaw, pitch and roll angles.

4. The method according to claim 1, wherein In the simulation stage, the guidance and control integration of the micro quadrotor UAV is realized based on the linear extended state observer-geometric tracking control theory. A subsystem is established through the MatlabFunction module of Simulink to encapsulate the implementation; and the rotation matrix R is used on the SE(3) group during the implementation process; and the attitude error function is: R d is the desired rotation matrix; I is the identity matrix; The total thrust and total torque are pre-given control inputs to track the desired trajectory position p of the center of mass position. d (t) and the desired yaw angle Ψ d (t); The origin of the cross-shaped quadcopter's body coordinate system is located at the center of mass of the aircraft, and the body coordinate axis Located on the plane defined by the centers of the four rotors, the body coordinate axis It is perpendicular to the plane and upwards, and in the same direction as the total thrust.

5. The method according to claim 1, wherein When the micro quadcopter drone drills a window, the window diameter is smaller than the diameter of the quadcopter drone; Before S100, the method further includes: A100, based on the differential flatness characteristics of the quadrotor drone, performs trajectory optimization and generation, and uses the MinimumSnap trajectory planning method to combine the known starting position, ending position, and window position to obtain the window drilling trajectory as a reference trajectory.

6. The method according to claim 5, characterized in that The A100 includes: A101. Divide the window drilling trajectory into an approaching segment and a crossing segment; A102. Use an inverse planning strategy to construct a crossing segment trajectory that satisfies dynamic constraints. Based on the initial state parameters of the crossing segment, reversely design the approach segment trajectory from the specified starting point to the starting point of the crossing segment. A103. Then, the Minimum Snap trajectory planning method is used, based on the differential flatness theory, to plan a reference window drilling trajectory that satisfies velocity continuity and acceleration continuity at the design waypoint.

7. The method according to claim 6, characterized in that The target window is defined as a rectangular plane Π perpendicular to the ground plane Σ. Based on the target window width W and the quadrotor diameter L, the minimum rolling maneuver angle β required to be completed is estimated to be arccos(W / L). The auxiliary plane Θ is constructed to meet the following conditions: (1) Contains the target crossing point C; (2) Is orthogonal to plane Π; (3) Forms an angle β with the ground plane Σ; In the plane Θ, a set of unit vectors {e1, e2, e3} are used to establish an orthogonal coordinate system. e1 axis: in plane Θ, perpendicular to the intersection of plane Θ and ground plane Σ and pointing downward; e3 axis: parallel to the plane Θ, normal vector upward; e2 axis: along the intersection of plane Θ and ground plane Σ, forming a right-handed coordinate system with e1 and e3; Plan the crossing segment trajectory BCD on plane Θ; the crossing segment consists of a semicircular arc with BD as the diameter, define the crossing speed v0, and satisfy the following constraints: Constraint 1: Kinematic characteristics at vertex C, velocity v C , acceleration a C , rotor plane normal vector n R : v C =v0·e2 a C =gtanβ·e1 n R =e3 The total thrust f satisfies: is the roll angle. Point C is at the highest point of the trajectory. The normal vector of the rotor plane at point C is defined to be parallel to e3. The rotor thrust and gravity are balanced in the vertical direction. The acceleration of point C on the trajectory is obtained. The direction of the acceleration is the projection of the total thrust f on the horizontal plane. Constraint 2: Boundary point B, D parameters: According to the centripetal acceleration formula, Point B: v B =-‖v C ‖e1,a B =‖a C ‖e2 Point D: v D = ‖v C ‖e1,a B = -‖a C ‖e2 Point B is the starting point of the crossing segment on plane π, and the position of point B is inferred based on the position of point C; point D is the end point of the crossing segment, and is symmetrically distributed with point B relative to plane π.

8. The method according to claim 7, characterized in that The window drilling trajectory of the reference trajectory of A103 includes: Among them, P AB (t) represents the trajectory close to segment AB, P BC (t) represents the trajectory of the ascending part BC in the crossing segment, P CD (t) represents the return trajectory after crossing the window; t is time, t A , t B , t C , t D Represent the moments of points A, B, C, and D on the trajectory respectively; p AB 、p BC 、p CD are all polynomial coefficients; Constraints include: position continuity, velocity continuity, acceleration continuity, initial and end state constraints, maximum velocity and maximum acceleration constraints.

9. A control system for a micro quad-rotor drone, characterized in that: include: a ground control station for monitoring, control, and mission management of the micro-quadrotor drone and a quadrotor drone for dynamic window drilling; The ground control station executes the method described in any one of claims 1 to 8 to enable the quadrotor drone to achieve high dynamic flight based on a reference trajectory.

Images