A wireless speed measurement anti-interference quad-rotor unmanned aerial vehicle formation control method

Abstract

The application discloses a wireless speed measurement anti-interference quadrotor unmanned aerial vehicle formation control method, which comprises the following steps: constructing an unmanned cluster system composed of multiple quadrotor unmanned aerial vehicles in a three-dimensional space, and establishing a dynamic model with external disturbance and mass change for each quadrotor unmanned aerial vehicle by using Newton-Euler equation; designing a position loop geometric controller to realize trajectory tracking of each quadrotor unmanned aerial vehicle to a virtual leader and combining with an L1 adaptive control method to resist external disturbance of translational kinematics and mass change of the quadrotor unmanned aerial vehicle; introducing a high-order differentiator to eliminate the requirement of line speed measurement in the formation control of each quadrotor unmanned aerial vehicle, and estimating the first derivative and second derivative of an intermediate control variable u i by using the differentiator; adopting a rotation matrix to represent the attitude of the quadrotor unmanned aerial vehicle, designing a geometric attitude controller on Lie algebra space to realize attitude tracking of the virtual leader, and introducing an L1 adaptive control method to resist external disturbance of rotational kinematics.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of quad-rotor unmanned aerial vehicle formation control, and particularly relates to a wireless speed measurement anti-interference quad-rotor unmanned aerial vehicle formation control method. BACKGROUND

[0002] Quad-rotor unmanned aerial vehicles have been widely applied in military and civil fields such as reconnaissance and surveillance, attack on the ground, environmental monitoring, resource exploration, etc. due to their small size, light weight, strong adaptability and vertical take-off and landing. However, as a single platform, the quad-rotor unmanned aerial vehicle has defects such as limited reconnaissance search range, low work efficiency and poor fault guarantee, which makes it only suitable for performing low-difficulty and small-scope work tasks. In comparison, the quad-rotor unmanned aerial cluster system has stronger autonomy, more functions, better timeliness, stronger anti-destruction performance and higher economic efficiency. Therefore, how the quad-rotor unmanned aerial cluster system efficiently and safely performs corresponding tasks has attracted more and more research attention of technical personnel in the field.

[0003] Formation control is one of the key technologies for the quad-rotor unmanned aerial cluster system to generate, maintain and reconstruct formation. After years of research and development, three control structures, i.e. centralized, decentralized and distributed, have been formed. The classical control strategies based on the control structures include the leader-follower method, the virtual structure method, the behavior-based method and the consensus-based method, etc. Each control strategy has its advantages and disadvantages. Among them, the leader-follower strategy has been widely researched and applied due to its simple implementation and easy expansion. The virtual leader strategy derived from the leader-follower strategy can achieve more complex formation tasks without the need of additional sensors to obtain the motion information of the leader, and the motion information of the virtual leader can be flexibly adjusted according to actual needs, so it becomes a better strategy choice in formation control.

[0004] With the complexity, safety and efficiency requirements of the task performed by the UAV formation, the performance, stability and robustness of the formation control are required to be higher. Most of the formation control schemes assume that the linear velocity can be measured for system feedback. However, in practical applications, there is no sensor that can directly measure the linear velocity of the quadrotor, and some indirect measurement tools have a large estimation error. In a complex environment, the quadrotor UAV is easily affected by the time-varying external environmental disturbance, such as the external wind acting as an additional force and torque on the UAV dynamics, the disturbance will have a continuous impact on the stability of the controller, and even in some serious disturbance, the formation control will fail. During the task execution of the UAV swarm, the mass of the quadrotor UAV may change due to fuel consumption, load change and air pressure change, and damage or failure, and the controller design must consider the influence of mass change to ensure the safety and stability of flight. Based on the above problems, an anti-interference quadrotor UAV formation control method for wireless speed measurement is needed. SUMMARY

[0005] In view of the above technical problems, the present application provides an anti-interference quadrotor UAV formation control method for wireless speed measurement.

[0006] The technical scheme adopted by the present application to solve its technical problems is:

[0007] An anti-interference quadrotor UAV formation control method for wireless speed measurement, the method comprising the following steps:

[0008] S100: constructing an unmanned swarm system composed of multiple quadrotor UAVs in a three-dimensional space, and establishing a dynamic model with external disturbance and mass change for each quadrotor UAV by using Newton-Euler equation, and the control target based on the dynamic model is to design thrust and torque to make each quadrotor UAV meet the preset conditions;

[0009] S200: designing a position loop geometric controller to realize trajectory tracking of each quadrotor UAV to the virtual leader and combining with L1 adaptive control method to resist external disturbance of translational kinematics and mass change of the quadrotor UAV;

[0010] S300: introducing a high-order differentiator to eliminate the need for linear velocity measurement in the formation control of each quadrotor UAV, and using the differentiator to estimate the first and second derivatives of the intermediate control variable u i .

[0011] S400: using a rotation matrix to represent the attitude of the quadrotor UAV, designing a geometric attitude controller on the Lie algebra space to realize attitude tracking of the virtual leader, and introducing L1 adaptive control method to resist external disturbance of rotational kinematics.

[0012] Preferably, an inertial coordinate system I and a body coordinate system B are established, and the dynamic model in S100 is specifically as follows:

[0013]

[0014] wherein i represents the number of each quadrotor UAV in the unmanned cluster system, and respectively represent the position and linear velocity of each quadrotor UAV in the inertial coordinate system, R i ∈SO(3) is the attitude of each quadrotor UAV, also representing the rotation matrix from the body coordinate system to the inertial coordinate system, represents the angular velocity of each quadrotor UAV in the body coordinate system, respectively represent the first-order differential of p i , v i , R i , Ω i , the symbol "^" represents mapping the vector into the corresponding skew-symmetric matrix, g is the constant gravitational acceleration, e3=[0,0,1] T , m 0,i represents the standard mass of each quadrotor UAV, m Δ,i represents the variable mass of each quadrotor UAV, is the moment of inertia of each quadrotor UAV, and respectively represent the thrust and torque of each quadrotor UAV, d f,i and d τ,i are the external disturbances of the translational dynamics and rotational dynamics of each quadrotor UAV.

[0015] Preferably, the preset condition in S100 is specifically as follows:

[0016]

[0017] wherein j is the number of the quadrotor UAV adjacent to the UAV i, δ ij represents the relative position between the UAV i and the UAV j, determines the required formation mode, p0 is the time-varying trajectory of the virtual leader, is the first-order differential of p0, representing the time-varying speed of the virtual leader, that is, the reference linear speed of the formation.

[0018] Preferably, S200 comprises:

[0019] S210: defining the single-machine tracking error of each quadrotor UAV in the unmanned cluster system and the virtual leader as follows:

[0020]

[0021] wherein δi represents the desired relative position of the ith quadrotor UAV to the virtual leader, e x,i , e x,i represents the position tracking error and velocity tracking error of the ith quadrotor UAV to the virtual leader, respectively.

[0022] S220: According to the directed graph theory, the formation tracking error of each quadrotor UAV in the UAV swarm system can be given by the following formula:

[0023]

[0024] In the formula, ξ x,i , ξ v,i represents the formation position tracking error and formation velocity tracking error of the ith quadrotor UAV, respectively, N i is the dynamic neighborhood set of UAVs that can interact with the ith quadrotor UAV, a i0 is the communication topology value between the ith quadrotor UAV and the virtual leader, and if the quadrotor UAV i can communicate with the leader, the value of a i0 is 1, otherwise 0, a ij is an element in the adjacency matrix , the value of a ij is given by the following formula:

[0025]

[0026] S230: Based on the PD design, the intermediate auxiliary control variable u b,i = -f i R i e3 is as follows:

[0027]

[0028] In the formula, k x , k v is a positive gain control parameter, is the acceleration of the virtual leader.

[0029] S240: Define Then the position error dynamic equation of each quadrotor UAV is represented as:

[0030]

[0031] In the formula, 0 is a zero matrix, is an identity matrix;

[0032] S250: Design a position L1 adaptive control law u ad,i for each quadrotor UAV, wherein the state predictor is:

[0033]

[0034] wherein, represents the prediction error, is a user-selected diagonal Hurwitz matrix, which can make the prediction error quickly converge to 0 in exponential form;

[0035] In the time period of t∈[aT s ,(a+1)T s ], the piecewise constant adaptive estimation law is:

[0036]

[0037] wherein, a is a time index, T s is a time step,

[0038] The position loop L1 adaptive control law is:

[0039]

[0040] wherein, is a low-pass filter, w p is a filter cutoff frequency;

[0041] S260: The complete controller of the position loop is:

[0042] u i =u b,i +u ad,i (11)

[0043] S270: The static thrust input of each quadrotor UAV is as follows:

[0044] f i =-u i ·R i e3 (12)

[0045] wherein the symbol "·" represents an inner product.

[0046] Preferably, S300 comprises:

[0047] S310: The linear velocity and linear acceleration of each quadrotor UAV are estimated by a high-order differentiator:

[0048]

[0049] wherein, is an estimation error, k1>0, k2>0, λ>0 are all normal number gains, and t≤T cβ = 0 when t = 0, and β = 1 otherwise, T c is an arbitrary positive constant;

[0050] S320: estimating the first and second derivatives of the intermediate control variable ui of each quadrotor UAV using a higher-order differentiator:

[0051]

[0052] wherein, is an estimation error, and k3>0, k4>0 are positive constant gains.

[0053] Preferably, S400 comprises:

[0054] S410: given any desired yaw angle γ, obtaining the desired geometric attitude R d,i = [b 1d,i ,b 2d,i ,b 3d,i ] T is expressed as follows:

[0055]

[0056] wherein is not parallel to b 3d,i , and the corresponding desired angular velocity is wherein the symbol "V" represents mapping of a skew-symmetric matrix into a corresponding vector;

[0057] S420: defining a structure error function on the nonlinear space SO3 as tr(·) represents the trace of a matrix, and I represents a three-dimensional unit matrix, according to which the attitude tracking error e R,i is defined as:

[0058]

[0059] wherein the symbol "V" represents mapping of a skew-symmetric matrix into a corresponding vector;

[0060] The angular velocity tracking error e Ω,i is defined as:

[0061] e Ω,i = Ω i - R T i R d,i Ω d,i (17)

[0062] S430: rewriting the angular velocity dynamic equation in equation (1) into the following form:

[0063]

[0064] where τ b,i and τ ad,i are the attitude geometric control law and the attitude L1 adaptive control law to be designed later, respectively;

[0065] S440: Design the basic attitude geometric control torque for each quadrotor UAV:

[0066]

[0067] where K R ,K Ω are positive gain control parameters, is the first order differential of Ω d,i ;

[0068] S450: Design the attitude L1 adaptive control law τ ad,i for each quadrotor UAV, where the state predictor is:

[0069]

[0070] where e is the prediction error, is a user-selected diagonal Hurwitz matrix, which can make the prediction error e converge to 0 exponentially and rapidly;

[0071] In the time period t∈[aT s ,(a+1)T s ], the piecewise constant adaptive estimation law is:

[0072]

[0073] where Φ=A ps -1 (exp(A ps T s )-I),

[0074] The attitude loop L1 adaptive control law is:

[0075]

[0076] where L is a low-pass filter, and w a is the filter cutoff frequency;

[0077] S460: The torque of each quadrotor UAV is:

[0078] τ i =τ b,i +τ ad,i (23)

[0079] The aforementioned method for anti-interference quadrotor UAV formation control using wireless velocity measurement introduces differentiator technology to estimate the higher-order derivative values ​​of unmeasurable linear velocity and intermediate auxiliary control thrust, thereby improving the robustness of the UAV swarm system and reducing the complexity of numerical calculations. The position and attitude subsystems are designed with controllers based on geometric tracking control laws plus L1 adaptive control laws, enabling the UAV swarm system to resist the effects of mismatched and matched disturbances. This addresses the impact of external time-varying disturbances and UAV mass variations on system stability and safety. The proposed controller has low computational complexity, easily adjustable parameters, and can resist disturbances of varying magnitudes using only the same set of parameters. It also exhibits fast convergence speed and can quickly form the required formation. Attached Figure Description

[0080] Figure 1 This is a flowchart of a quadcopter UAV formation control method for wireless speed measurement and anti-interference according to the present invention;

[0081] Figure 2 This is a schematic diagram of a wireless speed measurement anti-interference quadrotor UAV formation control method according to the present invention;

[0082] Figure 3 This is a schematic diagram of the virtual navigator-follower relationship topology according to an embodiment of the present invention;

[0083] Figure 4 This is a schematic diagram of the formation process trajectory of the quadcopter unmanned swarm system in a simulation according to an embodiment of the present invention;

[0084] Figure 5 This is a schematic diagram of the position error of the quadcopter unmanned swarm system in simulation according to an embodiment of the present invention;

[0085] Figure 6 This is a schematic diagram of the attitude error of the quadcopter unmanned swarm system in simulation according to an embodiment of the present invention;

[0086] Figure 7 This refers to the differentiator position estimation error in the simulation of the quadcopter unmanned swarm system according to an embodiment of the present invention.

[0087] Figure 8 The error in the thrust, the intermediate control variable of the differentiator in the simulation of the quadcopter unmanned swarm system of this invention;

[0088] Figure 9 This refers to the estimation error of time-varying disturbances and mass changes in the simulation of the quadcopter drone swarm system according to the embodiments of the present invention. Detailed Implementation

[0089] To enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings.

[0090] In one embodiment, as shown in Figure 1 and 2 A wireless speed measurement anti-jamming quadrotor unmanned aerial vehicle formation control method, the method comprising the following steps:

[0091] S100: Construct an unmanned cluster system composed of multiple quadrotor unmanned aerial vehicles in three-dimensional space, and establish a dynamic model with external disturbance and mass change for each quadrotor unmanned aerial vehicle using Newton-Euler equation. The control objective based on the dynamic model is to design thrust and torque to make each quadrotor unmanned aerial vehicle meet the preset conditions.

[0092] In one embodiment, an inertial coordinate system I and a body coordinate system B are established, and the dynamic model in S100 is specifically:

[0093]

[0094] In the formula, i represents the number of quadrotor unmanned aerial vehicles in the unmanned cluster system, and respectively represent the position and linear velocity of each quadrotor unmanned aerial vehicle in the inertial coordinate, R i ∈SO(3) is the attitude of each quadrotor unmanned aerial vehicle, also representing the rotation matrix from the body coordinate system to the inertial coordinate system, represents the angular velocity of each quadrotor unmanned aerial vehicle in the body coordinate system, respectively represent the first-order differential of p i , v i , R i , Ω i , the symbol "^" represents mapping the vector into the corresponding skew-symmetric matrix, g is the constant gravitational acceleration, e3=[0,0,1] T , m 0,i represents the standard mass of each quadrotor unmanned aerial vehicle, m Δ,i represents the changing mass of each quadrotor unmanned aerial vehicle, is the moment of inertia of each quadrotor unmanned aerial vehicle, and respectively represent the thrust and torque of each quadrotor unmanned aerial vehicle, d f,i and d τ,i are the external disturbances of the translational dynamics and rotational dynamics of each quadrotor unmanned aerial vehicle.

[0095] Specifically, the directed interaction topology between the quadrotor unmanned cluster system is as shown in Figure 3As shown, the actual quadrotor UAVs form a square formation at four corners, and from the top left corner, the UAVs are in the order of UAV1, UAV2, UAV3, UAV4 in a counterclockwise direction, and the middle is the virtual leader UAV. 1-4 are the directed interaction topologies among the actual quadrotor UAVs, and 5-7 are the directed interaction topologies between the virtual leader and the actual quadrotor UAVs. Compared with the undirected interaction topology, the directed interaction topology only consumes half of the communication and energy resources, and is more suitable for the quadrotor UAVs with limited communication and energy in practical applications.

[0096] Based on Figure 3 The control objective of the directed interaction topology map unmanned cluster system is to design thrust f i and torque τ i to make each quadrotor UAV meet the preset condition.

[0097] In one embodiment, the preset condition in S100 is specifically:

[0098]

[0099] In the formula, j is the number of the quadrotor UAV, adjacent to the UAV i, δ ij represents the relative position between the UAV i and j, determines the required formation mode, p0 is the time-varying trajectory of the virtual leader, is the first-order differential of p0, and represents the time-varying speed of the virtual leader, that is, the reference linear speed of the formation shape.

[0100] S200: Design a position loop geometric controller to realize the trajectory tracking of each quadrotor UAV to the virtual leader and combine the L1 adaptive control method to resist the external disturbance of the translational kinematics and the mass change of the quadrotor UAV.

[0101] In one embodiment, S200 includes:

[0102] S210: Define the single-machine tracking error of each quadrotor UAV in the unmanned cluster system to the virtual leader as follows:

[0103]

[0104] In the formula, δ i represents the expected relative position of the i-th quadrotor UAV to the virtual leader, e x,i , e x,i respectively represent the position tracking error and the speed tracking error of the i-th quadrotor UAV relative to the virtual leader;

[0105] S220: According to the directed graph theory, the formation tracking error of each quadrotor UAV in the unmanned cluster system can be given by the following formula:

[0106]

[0107] where, ξ x,i ,ξ v,i denote the formation position tracking error and formation velocity tracking error of the ith quadrotor UAV, N i is the dynamic neighborhood set of UAVs that can interact with the ith quadrotor UAV, a i0 is the communication topology value between the ith quadrotor UAV and the virtual leader, if the quadrotor UAV i can communicate with the leader, then the value of a i0 is 1, otherwise 0, a ij is the element in the adjacency matrix , the value of a ij is given by:

[0108]

[0109] S230: Design the intermediate auxiliary control variable u b,i = -f i R i e3 as follows:

[0110]

[0111] where, k x ,k v are positive gain control parameters, is the acceleration of the virtual leader.

[0112] S240: Define Then the position error dynamic equation of each quadrotor UAV is expressed as:

[0113]

[0114] where is a zero matrix, is an identity matrix;

[0115] S250: Design the position Li adaptive control law u ad,i for each quadrotor UAV, where the state predictor is:

[0116]

[0117] where, denotes the prediction error, is a diagonal Hurwitz matrix selected by the user, which can make the prediction error exponentially converge to 0 in a fast manner;

[0118] for t ∈ [aT s ,(a+1)Ts The piecewise constant adaptive estimation law is:

[0119]

[0120] where a is the time index, T s is the time step,

[0121] The position loop L1 adaptive control law is:

[0122]

[0123] where is a low-pass filter, w p is the filter cutoff frequency;

[0124] The full controller for the position loop is:

[0125] u i = u b,i + u ad,i (11)

[0126] The static thrust input for each quadrotor UAV is:

[0127] f i = - u i · R i e3 (12)

[0128] where the symbol "·" denotes the inner product.

[0129] S300: Introduce higher order differentiators to eliminate the need for linear velocity measurements in the formation control of each quadrotor UAV and to estimate the first and second derivatives of the intermediate control variable u i .

[0130] In one embodiment, S300 includes:

[0131] S310: Estimate the linear velocity and linear acceleration of each quadrotor UAV through higher order differentiators:

[0132]

[0133] where is the estimation error, k1 > 0, k2 > 0, and λ > 0 are constant gains, and β = 0 when t ≤ T c , and β = 1 otherwise, and T c > 0 is an arbitrary positive constant.

[0134] S320: Estimate the intermediate control variable u ifirst and second derivatives:

[0135]

[0136] wherein, are estimation errors, k3>0, k4>0 are normal number gains.

[0137] S400: A rotation matrix is used to represent the attitude of the quadrotor UAV, a geometric attitude controller is designed in Lie algebra space to realize attitude tracking of the virtual leader, and an L1 adaptive control method is introduced to resist external disturbances of rotational kinematics.

[0138] In one embodiment, S400 includes:

[0139] S410: Given any desired yaw angle γ, the desired geometric attitude R d,i = [b 1d,i , b 2d,i , b 3d,i ] T is expressed as follows:

[0140]

[0141] wherein is not parallel to b 3d,i , and the corresponding desired angular velocity is wherein the symbol "V" represents mapping of the skew-symmetric matrix into the corresponding vector;

[0142] S420: A structure error function is defined on the nonlinear space SO3 as tr(·) represents the trace of the matrix, I represents a three-dimensional unit matrix, and accordingly, the attitude tracking error e R,i is defined as:

[0143]

[0144] wherein the symbol "V" represents mapping of the skew-symmetric matrix into the corresponding vector;

[0145] The angular velocity tracking error e Ω,i is defined as:

[0146] e Ω,i = Ω i - R T i R d,i Ω d,i (17)

[0147] S430: The angular velocity dynamic equation in formula (1) is rewritten in the following form:

[0148]

[0149] where τ b,i and τ ad,i are the attitude geometric control law and the attitude L1 adaptive control law to be designed later, respectively;

[0150] S440: Design the basic attitude geometric control torque for each quadrotor UAV:

[0151]

[0152] where K R ,K Ω are positive gain control parameters, is the first order differential of Ω d,i

[0153] S450: Design the attitude L1 adaptive control law τ ad,i for each quadrotor UAV, where the state predictor is:

[0154]

[0155] where e is the prediction error, is a user-selected diagonal Hurwitz matrix that can make the prediction error e exponentially converge to 0 in a fast manner;

[0156] In the time period t∈[aT s ,(a+1)T s ], the piecewise constant adaptive estimation law is:

[0157]

[0158] where Φ=A ps -1 (exp(A ps T s )-I),

[0159] The attitude L1 adaptive control law is:

[0160]

[0161] where L is a low-pass filter, w a is the filter cutoff frequency;

[0162] S460: The torque of each quadrotor UAV is:

[0163] τ i =τ b,i +τ ad,i (23)​

[0164] In particular, the low-pass filter is used to prevent high frequency components from entering the control channel.

[0165] Here, in order to better illustrate the effectiveness of the method of the application for formation control of quadrotor unmanned aerial vehicles under wireless speed measurement, external disturbance and mass change, combined with Figures 4-9 The simulation is described here:

[0166] For convenience of description, the data appearing below are all assumed to use international units. In the simulation, we consider a group of four actual quadrotor unmanned aerial vehicles UAV1-UAV4 and a virtual leader Virtual leader to form an unmanned cluster system, which has a directed network topology structure as shown in Figure 3 . Among them: the mass of each quadrotor unmanned aerial vehicle model is taken as m i = 0.075, the moment of inertia J i = diag([5.8*10 e-5 , 7.2*10 e-5 , 10*10 e-5 ]), i = {1, 2, 3, 4}; the initial position of each quadrotor unmanned aerial vehicle is set as: p1 = [2, 5, 0] T , p2 = [-1 10] T , p3 = [3, -1, 0] T , p4 = [2.5, 2, 0] T ; the initial linear velocity and angular velocity are zero; the desired formation of the system is determined by the relative displacement of the actual quadrotor unmanned aerial vehicle and the virtual leader quadrotor unmanned aerial vehicle δ1 = [-2 2 0] T , δ2 = [-2 -2 0] T , δ3 = [2 -2 0] T , δ4 = [2 2 0] T , the reference linear velocity of the virtual leader is: The desired yaw angle is: Suppose that each unmanned aerial vehicle is subjected to a time-varying disturbance d f,i = [1.2sint, 1.2sint, 1.2sint] T , d τ,i = [1.5sint, 1.5sint, 1.5sint] T , the mass of each quadrotor unmanned aerial vehicle changes as m Δ,1 = 0.05, m Δ,2 = 0.06, m Δ,3 = 0.01, m Δ,4 = 0.075, the control parameters of the geometric controller are set as: k x = 8, k v = 1.2, kr = 100, k Ω = 1.1, the parameters of the L1 adaptive controller are set as w p = 10, w a = 6, Ts = 0.002, A ps = -diag(5, 5, 5), A as = -diag(10, 10, 10), the parameters of the differentiator are set as: k1 = 10, k2 = 10, k3 = 10, k4 = 1, λ = 0.005. The corresponding simulation results under these parameter configurations are shown in Figs. 16-20. Figures 4-9 Figure 4 Fig. 16 shows the three-dimensional trajectory of the whole formation. Figure 5 Fig. 17 shows the position formation error of the quad-rotor unmanned cluster system in simulation, it can be seen that the position formation error converges to the vicinity of zero quickly. Figure 6 Fig. 18 shows the attitude formation error of the quad-rotor unmanned cluster system in simulation, it can be seen that the attitude formation error converges to the vicinity of zero quickly. Figure 7 Figure 8 Figs. 19 and 20 respectively show the differentiator error It can be seen that the differentiator error can converge to zero quickly, which indicates that the differentiator can quickly and accurately estimate the linear velocity value and the high-order derivative value of the intermediate control thrust. Figure 9 Fig. 21 shows the estimation error of the time-varying disturbance and the mass change. The simulation results show that the formation control algorithm of the present application has good control performance.

[0167] The present application has the following beneficial effects:

[0168] (1) The differentiator technology is introduced to estimate the unmeasurable linear velocity and the high-order derivative value of the intermediate auxiliary control thrust, which improves the robustness of the unmanned cluster system and reduces the complexity of numerical calculation.

[0169] (2) The position subsystem and the attitude subsystem are respectively designed to have the controller of the geometric tracking control law plus the L1 adaptive control law, so that the unmanned cluster system can resist the influence of the non-matching and matching disturbances, i.e. the influence of the external time-varying disturbance and the mass change of the unmanned aerial vehicle on the stability and safety of the system is solved.

[0170] (3) The proposed controller has low calculation complexity, the parameters are easy to adjust, the same set of parameters can resist disturbances of different sizes, the convergence speed is fast, and the required formation shape can be quickly formed.

[0171] ​​The above describes in detail the wireless speed measurement anti-interference quad-rotor unmanned aerial vehicle formation control method provided by the present application. The principles and implementation manners of the present application are described by using specific examples in this paper, and the above description of the examples is only used to help understand the core idea of the present application. It should be pointed out that, for those skilled in the art, some improvements and modifications can be made to the present application without departing from the principles of the present application, and these improvements and modifications also fall within the protection scope of the claims of the present application.

Claims

1. A method for anti-interference quadrotor UAV formation control using wireless speed measurement, characterized in that, The method includes the following steps: S100: Construct an unmanned swarm system consisting of multiple quadrotor drones in three-dimensional space, and use the Newton-Euler equations to establish a dynamic model for each quadrotor drone with external disturbances and mass changes. The control objective based on the dynamic model is to design thrust and torque so that each quadrotor drone meets preset conditions. S200: Design a position loop geometry controller to enable each quadcopter UAV to track the trajectory of the virtual navigator and combine it with the L1 adaptive control method to resist external disturbances in translational kinematics and changes in the mass of the quadcopter UAV. S300: Introduces a high-order differentiator to eliminate the need for linear velocity measurement in the formation control of each quadcopter UAV, and uses the differentiator to estimate the intermediate control variable u. i The first and second derivatives; S400: The attitude of the quadcopter UAV is represented by a rotation matrix. A geometric attitude controller is designed in Lie algebra space to track the attitude of the virtual navigator and an L1 adaptive control method is introduced to resist external disturbances in rotational kinematics.

2. The method according to claim 1, characterized in that, Establish inertial coordinate system I and body coordinate system B. The dynamic model in S100 is as follows: In the formula, i represents the number of the quadcopter drone in the unmanned swarm system. and R represents the position and linear velocity of each quadcopter UAV in inertial coordinates. i ∈SO(3) represents the attitude of each quadcopter UAV, and also represents the rotation matrix from the body coordinate system to the inertial coordinate system. This represents the angular velocity of each quadcopter drone in the body coordinate system. They represent p respectively i v i R i Ω i The first derivative of , where the symbol "^" represents mapping the vector to the corresponding antisymmetric matrix, g is the constant gravitational acceleration, and e3 = [0,0,1] T m 0,i This represents the standard mass of each quadcopter drone, m. Δ,i This represents the change in mass for each quadcopter drone. The moment of inertia of each quadcopter drone, and d represents the thrust and torque of each quadcopter drone, respectively. f,i and d τ,i These are the external disturbances for the translational and rotational dynamics of each quadcopter UAV, respectively.

3. The method according to claim 2, characterized in that, The preset conditions in S100 are as follows: In the formula, j is the number of the quadcopter UAV, adjacent to UAV i, δ ij The relative positions of drones i and j determine the required formation pattern, and p0 is the time-varying trajectory of the virtual navigator. It is the first derivative of p0, representing the time-varying velocity of the virtual navigator, which is also the reference linear velocity of the formation.

4. The method according to claim 3, characterized in that, S200 includes: S210: Define the single-unit tracking error between each quadcopter drone and the virtual navigator in an unmanned swarm system as follows: In the formula, δ i Let e ​​represent the expected relative position of the i-th quadcopter drone and the virtual navigator. x,i e x,i Let $i$ represent the position tracking error and velocity tracking error of the i-th quadcopter UAV relative to the virtual navigator, respectively. S220: According to directed graph theory, the formation tracking error of each quadcopter drone in an unmanned swarm system can be given by the following formula: In the formula, ξ x,i ξ v,i Let N represent the formation position tracking error and formation speed tracking error of the i-th quadcopter UAV, respectively. i It is the dynamic neighborhood set of UAVs that can interact with the i-th quadcopter UAV, a i0 It is the communication topology value between the i-th quadcopter drone and the virtual navigator. If the quadcopter drone i can communicate with the navigator, then a i0 The value of a is 1, otherwise it is 0. ij It is an adjacency matrix The element in, a ij The value is given by the following formula: S230: Design of intermediate auxiliary control variable u based on PD b,i =-f i R i e3 is as follows: In the formula, k x ,k v It is a positive gain control parameter. Accelerating the virtual navigator; S240: Definition The dynamic equation for the position error of each quadcopter UAV is then expressed as: In the formula It is a zero matrix. It is the identity matrix; S250: Designs a positional L1 adaptive control law u for each quadcopter UAV. ad,i The state predictor is: In the formula, Indicates the prediction error. It is the diagonal Herwitz matrix selected by the user, which can reduce the prediction error. It converges rapidly to 0 in an exponential manner; In t∈[aT] s ,(a+1)T s The piecewise constant adaptive estimation law for the time period is: In the formula, a is the time sequence number, and T s It is the time step, Φ = A ps -1 (exp(A ps T s )-I), The adaptive control law for position loop L1 is: In the formula, It is a low-pass filter, w p It is the filter cutoff frequency; S260: The complete controller for the position loop is: in i =in b,i +in ad,i (11) S270: The static thrust input for each quadcopter UAV is as follows: f i =-u i ·R i e3 (12) The symbol "·" in the formula represents the inner product.

5. The method according to claim 1, characterized in that, The S300 includes: S310: Estimates the linear velocity and linear acceleration of each quadcopter drone using a high-order differentiator: In the formula, To estimate the error, k1 > 0, k2 > 0, and λ > 0 are all normal gain values, when t ≤ T. c When β = 0, otherwise β = 1, T c >0 is any positive constant; S320: The intermediate control variable u of each quadcopter UAV is estimated using a high-order differentiator. i First and second derivatives: In the formula, To estimate the error, k3 > 0 and k4 > 0 are both normal gain values.

6. The method according to claim 5, characterized in that, The S400 includes: S410: Given an arbitrary desired yaw angle γ, obtain the desired geometric attitude R. d,i =[b 1d,i ,b 2d,i ,b 3d,i ] T It is expressed as follows: In the formula Not parallel to b 3d,i The corresponding expected angular velocity is In the formula, the symbol "∨" indicates that the antisymmetric matrix is ​​mapped to the corresponding vector; S420: The structural error function is defined on the nonlinear space SO3 as follows: tr(·) denotes the trace of the matrix, I denotes the three-dimensional identity matrix, and thus, the attitude tracking error e R,i Defined as: In the formula, the symbol "∨" indicates that the antisymmetric matrix is ​​mapped to the corresponding vector; Angular velocity tracking error e Ω,i Defined as: e Ω,i =Oh i -R T i R d,i Oh d,i (17) S430: Rewrite the dynamic equation of angular velocity in equation (1) as follows: In the formula, τ b,i and τ ad,i These are the attitude geometry control law and the attitude L1 adaptive control law that need to be designed later. S440: Designs the basic attitude geometry control torque for each quadcopter UAV. In the formula K R ,K Ω This is a positive gain control parameter. It is Ω d,i The first derivative; S450: Designing an attitude L1 adaptive control law τ for each quadcopter UAV ad,i The state predictor is: In the formula It is the prediction error. It is the diagonal Herwitz matrix selected by the user, which can reduce the prediction error. It converges rapidly to 0 in an exponential manner; In t∈[aT] s ,(a+1)T s The piecewise constant adaptive estimation law for the time period is: In the formula, The attitude loop L1 adaptive control law is: In the formula, It is a low-pass filter, w a It is the filter cutoff frequency; S460: The torque of each quadcopter drone is: t i =t b,i +t ad,i (23).

Images