Four-rotor unmanned aerial vehicle robust accurate formation control method based on virtual control network and active disturbance rejection controller

Abstract

The invention discloses a quad-rotor unmanned aerial vehicle robust accurate formation control method based on a virtual control network and an active disturbance rejection controller. The invention discloses a four-rotor unmanned aerial vehicle robust accurate formation control method based on a virtual control network and an active-disturbance-rejection controller. The method comprises the following steps: S1, estimating the state of a virtual pilot four-rotor unmanned aerial vehicle; s2, establishing a virtual control network; and S3, the trajectory of the virtual pilot quadrotor unmanned aerial vehicle is generated by trajectory planning, each follower quadrotor unmanned aerial vehicle performs trajectory tracking on the virtual pilot quadrotor unmanned aerial vehicle, and the active disturbance rejection controller of the follower quadrotor unmanned aerial vehicle is independently applied. The four-rotor unmanned aerial vehicle robust accurate formation control method based on the virtual control network and the active disturbance rejection controller disclosed by the invention has the beneficial effects that a distributed observer is developed for each follower to estimate the position coordinate of a virtual leader, and the communication cost is greatly reduced.

Description

Technical Field

[0001] The present invention belongs to the field of quad-rotor unmanned aerial vehicle formation control, and in particular relates to a robust and precise formation control method for quad-rotor unmanned aerial vehicles based on a virtual control network and an anti-disturbance controller. Background Art

[0002] The subject name is a method for controlling the flight of a swarm of unmanned aerial vehicles, a drone, a system and a medium for the invention patent application, and the publication number is CN110703803A. The technical solution discloses "multiple drones to perform tasks, a drone ground command and control system, wherein the drone ground command and control system includes a formation control platform, a track planning platform, a data communication platform, an auxiliary system control platform, and a control conversion platform. When the drone swarm performs a task, it coordinates all information and works together to complete various tasks; wherein, a communication signal network of different frequencies is constructed for the drone swarm, and drones of the same frequency constitute a drone group. The drone swarm formation control algorithm based on MAS implements distributed control on multiple drone groups to weaken the role of the pilot aircraft. The drone ground command and control system All communication signal networks of the drone group are monitored at all times. Each drone can be dynamically and randomly designated as a new pilot when necessary, and the real-time status information of all the groups in the drone group is obtained. Different initialization autonomy levels are assigned to different groups according to the frequency of their communication networks, and then the autonomy levels of different drone groups are adjusted according to the real-time status information of different groups; the drone ground command and control system obtains the real-time environmental information of the drone group, and calculates the complexity information of the drone group in the process of executing the task according to the real-time environmental information and the final instructions of the ground control, wherein the complexity information includes the complexity of the task route and the complexity of the environment and the status information of the drone; if the complexity value of the complexity information is greater than or equal to the preset threshold, the drone group automatically selects the autonomy level to execute the task.

[0003] Taking the technical solution disclosed in the above invention patent application as an example, in the prior art, leader-follower, virtual structure and their combination are common formation control strategies. However, when the physical leader is damaged, the entire formation will not be able to proceed, and when the drones in the group fail or new drones join the group, the virtual structure lacks fault tolerance and flexibility. In addition, the centralized communication architecture is difficult to implement in engineering.

[0004] See attached figure Figure 1 , the aerodynamic model of the quadrotor drone is represented by equations (1) and (2):

[0005]

[0006] Among them, F i(i=1, 2, 3, 4) is the lift of the i-th rotor, Q i (i=1, 2, 3, 4) is the horizontal pulling force of rotor i, ω i (i=1, 2, 3, 4) is the angular velocity of rotor i, K L is the rotor's comprehensive lift coefficient, K Q is the comprehensive drag coefficient of the rotor, U1 is the control amount of the roll angle ξ, U2 is the control amount of the pitch angle θ, U3 is the yaw angle, ψ and U4 are the vertical accelerations The control quantity, (x, y, z) is the three-dimensional position coordinate of the UAV in space, and are the corresponding speed and acceleration, m is the mass of the drone, g is the acceleration of gravity, f1(·), f2(·) and f3(·) are internal disturbances, w1(t), w2(t) and w3(t) are external disturbances, I x ,I y ,I z is the three-axis moment of inertia, and l is the distance from the rotation axis of the four rotors to the center of mass of the fuselage.

[0007] It is worth mentioning that for precise formation flight with a specific shape, a relative kinematics model is used instead of a swarm kinematics mathematical model. At the same time, a virtual leader is used as the formation leader to guide the formation flight with a real-time planned flight path.

[0008] XOY represents the northeast celestial coordinate system, X ri O ri Y ri Represents the heading coordinate system, with the origin located at the center of gravity of the following drone i, X ri The axis points to the horizontal velocity direction of follower i, Z ri The axis is perpendicular to the ground and upward, Y ri The axis is determined by referring to the right-hand rule. (X ri Y ri Z ri ) represents the position of the virtual guide in the heading coordinate system of the following UAV i.

[0009] See attached figure Figure 2 , the relative kinematic model is represented by formula (3):

[0010]

[0011] The rotation angle ψ i ′=90°-ψ i , ψ i is the heading angle of the following drone i, (x i ,y i , z i ) and (xL ,y L , z L ) represents the position of the virtual leader in the northeast celestial coordinate system of the follower drone i, (x ri ,y ri , z ri ) represents the position of the virtual leader in the heading coordinate system {W} of the wingman i.

[0012] It is worth mentioning that, as an example, for a group of N quadrotor drones, the information exchange between the drones can be modeled by a directed graph G = (V, E), where V = {v i |i=1,2,…,n} is a set of vertices, and E is a set of directed edges describing the information exchange between drones. If there is a direct communication connection from drone i to drone j, e ij ∈E. The set of neighboring drones of drone i can be described as N in the communication topology i = {v j |e ji ∈E, j≠i}, then the communication topology of the entire formation can be expressed by the adjacency matrix A = (a ij ) n×n Description, where

[0013] For any drone v i ,if Drone i For any pair of different UAVs, if there is a directed path for exchanging information, the graph G is called connected.

[0014] It is worth mentioning that due to security reasons or communication bandwidth limitations, the information flow between drones is often severely limited, and a drone may not be able to communicate with all other drones in a distributed formation. Unexpected events such as local communication interruption and damage to some drone platforms can deteriorate the controllability of the entire formation and lead to mission failure. Therefore, how to ensure the controllability of the formation when local communication links or part of the drone platform are destroyed is a pressing issue. Generally speaking, there are three methods: the first is to formulate strategies to produce robust performance in the event of temporary loss of communication links; the second method is to reconfigure or adaptively configure when such a failure occurs; the third method, that is, the technical solution disclosed in the present invention, designs a directed graph-based anti-communication and drone platform destruction topology to improve the topological robustness of drones when performing tasks in complex environments.

[0015] It is worth mentioning that in the active anti-disturbance control part, the second-order system is represented by equation (4):

[0016]

[0017] Among them, x, is the system state, y is the regulated output, u is the control input, b is its gain coefficient, w(t) represents the external disturbance, is a function of the system dynamics including internal uncertainties; let Where b0 is the estimated value of the true value b. X3 is regarded as the total disturbance of the system and is called the extended state; let Then the original second-order system can be transformed into an integral chain system containing total disturbance, which is represented by equation (5):

[0018]

[0019] The structure of NLADRC / LADRC / ADRC is described as follows: ADRC usually contains a tracking differentiator (TD), a state observer (ESO) and a feedback control law. The tracking differentiator is used to arrange the conversion process and extract the difference of the input signal. A linear tracking differentiator is characterized by equation (6):

[0020]

[0021] Where V0 is the input, V i (i=1, 2) is the output, r0 is the adjustable speed factor, and fh is a function that ensures fast convergence from V1 to V0.

[0022] Among them, the state observer is used to estimate the state and total disturbance of the system, which is the core and essence of ADRC. A third-order state observer is represented by equation (7):

[0023]

[0024] The input of the state observer is the regulation output y and the control input u; the output of the state observer is z i (i = 1, 2, 3), respectively providing estimates of the system states x1, x2 and the total disturbance x3; β 0i (i=1, 2, 3) is the observer gain, e is the observer error "z1-y", is a nonlinear function, defined by equation (8):

[0025]

[0026] Among them, α i and δ are important parameters that are determined in advance. This nonlinear function has the characteristics of “small error and large gain” in the NLADRC framework; when α i <1, the error is large and the gain is small. i = 1, this nonlinear fal(e, α i,δ) function becomes a linear function, and the observer gain can be determined as β 01 =3ω o , where ω o is the observer bandwidth.

[0027] Among them, the feedback control law is used to suppress the residual and achieve the desired control target. The feedback control law is represented by formula (9):

[0028]

[0029] Among them, the form of U0 can use the following combination where k i (i=1,2) is the control gain. i ′=1, this nonlinear fal(e,α′ i ,δ) function becomes a linear function, k p , k d It can be determined as k p =2ω c , where ω c is the controller bandwidth.

[0030] See attached figure Figure 3 , showing the framework of the ADRC algorithm.

[0031] The following describes the SADRC principle: LADRC and NLADRC have their own advantages and disadvantages, and the goal of SADRC is to combine the advantages of LADRC and NLADRC. The present invention adopts a linear control law, and SADRC represents the switching between LESO and NLESO based on the state error e. The SADRC scheme is as follows:

[0032] Initial operation stage: If there may be an initial state error between the device and the state observer, NLESO is used during the transition time T (manually set) to avoid the "peak phenomenon", otherwise the "initial operation stage" step is omitted and the normal operation stage is directly entered;

[0033] Normal operation stage: See the attached figure Figure 4 , after the initial operation phase, the controller automatically switches between LESO and NLESO according to the error e. A specific δ′>δ is selected. When the error e<δ′, NLESO starts to work; otherwise, LESO starts to work. This δ′ is the general boundary of the performance of LESO and NLESO, that is, when e<δ′, NLESO is better than LESO, and LESO is better than NLESO. The SADRC scheme combines the advantages of LADRC and NLADRC. Summary of the invention

[0034] In view of the status of the prior art, the present invention overcomes the above defects and provides a robust and precise formation control method for a quad-rotor UAV based on a virtual control network and an anti-disturbance controller.

[0035] The robust and precise formation control method of a quadrotor drone based on a virtual control network and an anti-disturbance control controller disclosed in the present invention has the main purpose of cleverly combining and making full use of the leader-follower and virtual structure methods, and proposing a virtual control network with a virtual leader; in order to overcome the defects of the centralized communication architecture, a distributed observer is proposed. In order to make the formation control method more robust to internal uncertainties and external disturbances, an outer loop position controller and an inner loop attitude controller are designed by adopting the anti-disturbance control technology.

[0036] The present invention adopts the following technical scheme, a method for robust and precise formation control of a quadrotor drone based on a virtual control network and an auto-disturbance rejection controller, comprising the following steps:

[0037] Step S1: estimating the state of the virtual pilot quadrotor drone;

[0038] Step S2: Establishing a virtual control network;

[0039] Step S3: The trajectory of the virtual pilot quadrotor UAV is generated by trajectory planning. Each follower quadrotor UAV tracks the trajectory of the virtual pilot quadrotor UAV and independently applies the anti-disturbance controller of the follower quadrotor UAV, so that each follower quadrotor UAV estimates the state of the virtual pilot quadrotor UAV by itself, so as to finally obtain the trajectory of each follower quadrotor UAV relative to the virtual pilot quadrotor UAV.

[0040] As a further preferred technical solution of the above technical solution, in step S1, let be the northeast celestial coordinate η L =[x L ,y L , z L ] T The position coordinates of the virtual navigator in the system, For U A V i The estimated position coordinates of the virtual lead; in the distributed communication framework, U A V i The virtual lead position coordinates are represented by formula (10):

[0041]

[0042] Among them, the adjustable variable λ>0, N L represents a group of drones that communicate directly with a virtual leader; the virtual leader state ηL Only in v i ∈N L When UAV j communicates directly with UAV i, a is obtained. ij =1, otherwise take a ij =0.

[0043] As a further preferred technical solution of the above technical solution, in step S1: the relative kinematic model of the quadrotor drone is represented by formula (16):

[0044]

[0045] The rotation angle ψ i ′=90°-ψ i , ψ i is the heading angle of the following drone i, (x i ,y i , z i )and denote the positions of the follower drone i and the estimated virtual leader in the northeast celestial coordinate system, (x ri ,y ri , z ri ) represents the position of the virtual leader in the heading coordinate system {W} of the wingman i; is the estimated state under the distributed communication topology.

[0046] As a further preferred technical solution of the above technical solution, step S2 is specifically implemented as the following steps: the virtual control points are set with different priorities. According to the order of priority, the virtual control points with higher priorities give priority to selecting the nearest UAV, and then the virtual control points with lower priorities are reconsidered, and the virtual control points with lower priorities select the nearest UAV.

[0047] As a further preferred technical solution of the above technical solution, step S2 is specifically implemented as the following steps: if the virtual control points are not set with different priorities, the relative distances between each drone and all virtual control points are calculated one by one, and the minimum sum distance is taken to determine the corresponding relationship between the drone and the virtual control point.

[0048] As a further preferred technical solution of the above technical solution, the "trajectory tracking" in step S3 is specifically implemented as the following steps: trajectory tracking adopts a cascade control method, including outer loop position control and inner loop attitude control.

[0049] As a further preferred technical solution of the above technical solution, "outer loop position control" is specifically implemented as follows:

[0050] The aerodynamic model of the quadrotor drone in step S7 is represented by equations (17) and (18):

[0051]

[0052] Among them, U x , U y , U z is the virtual control value, given by U x , U y , U z Calculate the inner loop attitude control command; f x , f y , f z is regarded as the total disturbance, w x , w y , w z are various internal and external disturbances; for the outer loop position control, let [x c y c z c ] T is the position command of the follower, [xyz] T is the real-time feedback of the follower's position coordinates, rewriting equation (16) into equation (19);

[0053]

[0054] Among them, (x r ,y r , z r ) represents the position of the virtual leader in the heading coordinate system {W} of the wingman, (x L ,y L , z L ) is the position coordinate of the virtual leader estimated by the following UAV; Based on formula (19), formula (20) is obtained:

[0055]

[0056] in, is the expected position coordinate of the virtual leader of the follower drone in the heading coordinate system;

[0057] Based on equations (17) to (20), a LADRC-based position controller is designed and obtained, which is represented by equations (21) to (23):

[0058]

[0059]

[0060] Among them, r x0 , r y0 , r z0 is a parameter that affects the speed of tracking the input signal, β xi , βyi , β zi , i = 1, 2, 3 are the ESO gains that need to be adjusted, k xi， k yi， k zi , i = 1, 2 is the gain of the control law that needs to be adjusted;

[0061] The “inner loop attitude control” is implemented as follows:

[0062] Known ψ c , the inner loop attitude angle control command (φ) is obtained by formula (24) c ,θ c , ψ c ):

[0063]

[0064] The inner loop attitude angle control command (φ c ,θ c , ψ c ) and the control quantity U4, the attitude loop controller is designed, which is represented by formula (25):

[0065]

[0066] Among them, V i (i=1, 2, 3) is the virtual control value, then we get formula (26):

[0067]

[0068] The SADRC controller is designed by equation (26), and is represented by equations (27) to (29):

[0069]

[0070] The robust and precise formation control method of a quadrotor drone based on a virtual control network and an anti-disturbance controller disclosed in the present invention has the following beneficial effects:

[0071] First, a distributed observer is developed for each follower to estimate the location coordinates of the virtual leader, which greatly reduces the communication cost;

[0072] Second, the concept of virtual control network is proposed and applied, which is more flexible and fault-tolerant than the traditional virtual structure method;

[0073] Thirdly, a distributed robust and precise formation controller is designed by adopting an anti-disturbance control system, which has high performance and is easy to implement in engineering. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1It is an aerodynamic model of a quadrotor drone.

[0075] Figure 2 It is a diagram of the relative positions between leaders and followers.

[0076] Figure 3 It is the framework diagram of the ADRC algorithm.

[0077] Figure 4 is a schematic diagram of the SADRC scheme.

[0078] Figure 5 is a schematic diagram of the virtual control network of the intended formation.

[0079] Figure 6 This is the overall plan diagram. DETAILED DESCRIPTION

[0080] The present invention discloses a robust and precise formation control method for a quadrotor unmanned aerial vehicle based on a virtual control network and an anti-disturbance controller. The specific implementation method of the present invention is further described below in conjunction with a preferred embodiment (Example 1).

[0081] Those skilled in the art should note that the “follower”, “follower”, etc. that may be involved in various embodiments of the present invention are the same concept and are no longer distinguished.

[0082] Those skilled in the art should note that the “virtual navigator”, “virtual team leader”, “virtual leader”, “virtual leader”, “virtual pilot quadrotor drone”, “navigator drone”, “virtual pilot drone”, etc. that may be involved in the various embodiments of the present invention are the same concept and are no longer distinguished.

[0083] Those skilled in the art should note that the “follower”, “follower quad-rotor drone”, etc. that may be involved in various embodiments of the present invention are the same concept and are no longer distinguished.

[0084] Those skilled in the art should note that the “NLADRC” that may be involved in various embodiments of the present invention is defined as: nonlinear active disturbance rejection control.

[0085] Those skilled in the art should note that the “LADRC” that may be involved in various embodiments of the present invention is defined as: linear active disturbance rejection control.

[0086] Those skilled in the art should note that the “ADRC” that may be involved in various embodiments of the present invention is defined as: Active Disturbance Rejection Control.

[0087] Those skilled in the art should note that the “ESO” that may be involved in various embodiments of the present invention is defined as: a state observer.

[0088] Those skilled in the art should note that various embodiments of the present invention may involve “SADRC”, which we define as: switching between LESO and NLESO based on the state error e.

[0089] Those skilled in the art should note that the “LESO” that may be involved in various embodiments of the present invention is defined as: a state observer.

[0090] Those skilled in the art should note that the various embodiments of the present invention may involve “NLESO”, which we define as: nonlinear extended state observer.

[0091] Example 1.

[0092] Preferably, this embodiment discloses a robust and precise formation control method for a quadrotor drone based on a virtual control network and an anti-disturbance control system.

[0093] Specifically, this embodiment discloses a method for robust and precise formation control of a quadrotor drone based on a virtual control network and an anti-disturbance rejection controller, comprising the following steps:

[0094] Step S1: estimating the state of the virtual pilot quadrotor drone;

[0095] Step S2: Establishing a virtual control network;

[0096] Step S3: The trajectory of the virtual pilot quadrotor UAV is generated by trajectory planning. Each follower quadrotor UAV tracks the trajectory of the virtual pilot quadrotor UAV and independently applies the anti-disturbance controller of the follower quadrotor UAV, so that each follower quadrotor UAV estimates the state of the virtual pilot quadrotor UAV by itself, so as to finally obtain the trajectory of each follower quadrotor UAV relative to the virtual pilot quadrotor UAV.

[0097] The technical highlights of step S1 (estimating the state of the virtual navigator) are described below.

[0098] Using a virtual leader instead of a physical leader can effectively avoid the problem of the entire formation being unable to proceed when the physical leader is damaged. It is difficult for each drone in the formation to communicate globally with other drones in the formation. Only a subset of drones in the formation can directly access the virtual leader's information, and drones that are not in this subset can only access it from "neighboring" drones. A drone may receive information about the virtual leader from different "neighboring" drones, so each drone needs to obtain consistent virtual leader status information based on local information fusion.

[0099] Let η L =[x L ,y L , zL ] T is the position coordinate of the virtual navigator in the northeast celestial coordinate system, For U A V i The estimated position coordinates of the virtual lead; in the distributed communication framework, U A V i The virtual lead position coordinates are represented by formula (10):

[0100]

[0101] Where λ>0, N L represents a group of drones that communicate directly with a virtual leader; the virtual leader state η L Only in v i ∈N L When UAV j communicates directly with UAV i, a is obtained. ij =1, otherwise take a ij =0.

[0102] The asymptotic convergence condition of equation (10) is described below.

[0103] If and only if from the virtual lead to v i When there is a reachable communication topology, equation (10) can be

[0104] Consider the virtual leader as an additional drone in the formation, i.e. The communication status between other (physical) drones and the virtual leader is represented by equation (11):

[0105]

[0106] Construct a column vector σ=[σ1,…,σ i , …, σ n ] T ; Combined with formula (12):

[0107]

[0108] Rewrite equation (10) as equation (13):

[0109]

[0110] After transformation, formula (11) is rewritten as formula (14);

[0111]

[0112] According to formula (14), we get formula (15):

[0113]

[0114] Among them, when a ij =1, indicating U A V j with U A V j There is direct communication between them; if there is an accessible communication topology from the virtual navigator U A V i , there must be a sequence [i, j, ..., k, n+1] such that a ij =…=a k(n+1) =1, then

[0115] It is worth mentioning that formula (10) can make the estimated state under the distributed communication topology Asymptotically converges to the actual state η L The speed of asymptotic convergence is determined by the variable λ. Therefore, as long as a suitable λ is selected, the estimated state As the actual state η L Then, the relative kinematic model (3) is rewritten as (16):

[0116]

[0117] The rotation angle ψ i ′=90°-ψ i , ψ i is the heading angle of the following drone i, (x i ,y i , z i )and denote the positions of the follower drone i and the estimated virtual leader in the northeast celestial coordinate system, (x ri ,y ri , z ri ) represents the position of the virtual leader in the heading coordinate system {W} of the wingman i; is the estimated state under the distributed communication topology.

[0118] The technical key points of step S2 (establishing a virtual control network) are described below.

[0119] It should be noted that the traditional virtual structure method can easily set the formation structure of the drone swarm, and due to the formation error feedback mechanism, high-precision formation maintenance can be achieved. However, the traditional virtual structure method maps drones one by one to virtual control points. When a drone in the swarm fails or a new drone joins the swarm, the virtual structure will change, lacking fault tolerance and flexibility. Therefore, this embodiment adopts a virtual control network and no longer sets a one-to-one correspondence between drones and virtual control points. The virtual control points are set with different priorities. According to the order of priority, the virtual control points with higher priorities give priority to the nearest drone, and then reconsider the virtual control points with lower priorities and fill them in order.

[0120] Setting priorities for virtual control points has at least two benefits. First, when the number of virtual control points is greater than the number of drones, priority should be given to ensuring that drones are deployed at more important virtual points. For example, when using drones in a circular formation to dynamically capture a target, priority should be given to the four directions of the "ring"; when flying in a V-shaped formation, priority should be given to ensuring that drones are configured at points near the vertex. Second, it can greatly reduce the amount of calculation. For example, assuming there are ten virtual control points and ten drones, and the drones are configured based on the control priority points, then the number of calculations required is However, if no priority is set, the relative distance between each drone and all virtual control points is calculated one by one.

[0121] Then take the minimum sum distance to determine the correspondence between the drone and the virtual control point. The calculation process is as follows: First, the distance between each drone and each virtual control point needs 10*10=100 times; second, there is There are different distance combinations between the drone and the control point. The sum of the total distances corresponding to each solution needs to be solved again 3628800 times; finally, the corresponding relationship between the drone and the virtual control point is determined based on the minimum sum distance. The total number of calculations is 100+3628800+3628800=7257700, which shows that the amount of calculation is very huge, which is undoubtedly a huge waste of computing resources for drones.

[0122] The method of establishing a virtual control network is illustrated by taking a V-shaped formation as an example. Taking 9 virtual control points and 7 drones as an example, the virtual control network of the expected formation is as follows: Figure 5As shown. The black triangle is the virtual leader and the other circles represent followers. Set the priorities of the virtual control points to p1, p2, p3, p4, p5, p6, p7, p8 and p9, and arrange them in descending order. Give priority to virtual control point p1, and calculate the distance between control point p1 and the 7 drones, and then select the drone with the shortest distance to take over control point p1. Subsequently, according to the principle of shortest distance, consider virtual control points p2, p2, …, p7. When a new drone joins the formation, the corresponding control point p8 can be selected; when two new drones join the formation, the corresponding control points p8 and p9 can be selected in turn; if more new drones are added, the virtual control network can be expanded.

[0123] It can be seen that the virtual control network method is very flexible and has a certain fault tolerance. On the contrary, for the traditional virtual structure method, when the UAV is damaged or a new UAV joins the formation, the virtual structure will change and it will be difficult to continue the mission.

[0124] It is worth mentioning that for multi-rotor drones, there are no strict requirements between the positions of drones, but fixed-wing drones need to consider the aerodynamic coupling effect. The communication topology between each virtual control point is designed to be anti-destruction topology, and if one or more drones are damaged, it can be redesigned according to the "formation reconstruction strategy".

[0125] Among them, the following explains the technical points of step S3 (the trajectory of the virtual pilot quadrotor UAV is generated by trajectory planning, each follower quadrotor UAV tracks the trajectory of the virtual pilot quadrotor UAV, and independently applies the self-disturbance rejection controller of the follower quadrotor UAV, so that each follower quadrotor UAV estimates the state of the virtual pilot quadrotor UAV by itself, so as to finally obtain the trajectory of each follower quadrotor UAV relative to the virtual pilot quadrotor UAV).

[0126] The essence of formation control in the leader-follower method is that the followers track the trajectory of the leader. The trajectory of the virtual leader is generated through trajectory planning, and each follower independently designs the virtual leader state estimated by the controller according to its trajectory. In this way, the multi-UAV formation control problem is simplified to a dual-UAV trajectory tracking problem, so that the UAV swarm has distributed characteristics.

[0127] Based on the dynamic model of the quadcopter, the trajectory tracking (control) adopts a cascade control method, including outer loop position control and inner loop attitude control. The overall scheme of the control system (the entire multi-loop control system of the UAV close-range formation model) is as follows: Figure 6As shown. The outer loop position controller adopts LADRC, which reduces the difficulty of parameter tuning. The linear tracking differentiator (TD) is used to arrange the transition process to track the motion trajectory of the virtual leader, and the linear state observer (ESO) is used to estimate wind disturbances and other uncertainties (such as load changes) and compensate for them. Since the attitude inner loop is highly coupled and sensitive to disturbances, the attitude angle control requires fast response speed, high tracking accuracy, and strong anti-interference ability, so SADRC is adopted. At the same time, since the outer loop position controller solves the angle control command, there is no jump, so the conversion process is cancelled to reduce the response delay. The design and stability analysis of the controller are described as follows.

[0128] 1) Outer loop position controller / formation controller: The aerodynamic model of the quadrotor drone represented by equations (1) and (2) in the background technology is an ideal model. In reality, there are various internal and external disturbances, such as wind disturbances and parameter disturbances. The new aerodynamic model of the quadrotor drone is represented by equations (17) and (18):

[0129]

[0130] Among them, U x , U y , U z is the virtual control value, given by U x , U y , U z Calculate the inner loop attitude control command; f x , f y , f z is regarded as the total disturbance, w x , w y , w z are various internal and external disturbances; for the outer loop position control, let [x c y c z c ] T is the position command of the follower, [xyz] T is the real-time feedback of the follower's position coordinates, rewriting equation (16) into equation (19);

[0131]

[0132] Among them, (x r ,y r , z r ) represents the actual position coordinates of the virtual leader in the heading coordinate system of the following drone, (x L ,y L , z L ) is the position coordinate of the virtual leader estimated by the following UAV; Based on formula (19), formula (20) is obtained:

[0133]

[0134] in, is the expected position coordinate of the virtual leader in the heading coordinate system of the follower drone.

[0135] Based on equations (17) to (20), a LADRC-based position controller is designed and obtained, which is represented by equations (21) to (23).

[0136]

[0137] Among them, r x0 , r y0 , r z0 is a parameter that affects the speed of tracking the input signal, β xi , β yi , β zi , i = 1, 2, 3 are the ESO gains that need to be adjusted, k xi , k yi , k zi , i=1, 2 is the gain of the control law that needs to be adjusted.

[0138] 2) Design of internal attitude loop controller: First, the inner loop attitude angle control command (φ) needs to be calculated from the outer loop position controller. c ,θ c , ψ c ) and the control quantity U4. However, through these three equations (U x , U y , U z ) to solve a set of fixed solutions. Since there is a large degree of coupling between the x-axis motion and the pitch angle and the y-axis motion and the roll angle, and the control quantity U4 is a necessary command, it is impossible to solve a set of fixed solutions by knowing ψ c In the case of , the inner loop attitude angle control command (φ) is obtained by equation (24) c ,θ c , ψ c ):

[0139]

[0140] The inner loop attitude angle control command (φ c ,θ c , ψ c ) and the control quantity U4, the attitude loop controller is designed, which is represented by formula (25):

[0141]

[0142] Among them, V i(i = 1, 2, 3) is the virtual control value, then the formula (26) is obtained:

[0143]

[0144] Design the SADRC controller from formula (26), characterized by formulas (27) to (29):

[0145]

[0146]

[0147] To simply describe the switching mechanism between the nonlinear state observer (ESO) and the linear state observer (ESO), it is characterized by formula (30):

[0148]

[0149] Among them, α i , δ i and γ i are important parameters that need to be determined in advance. The determination rules of α i , δ i and γ i are described as follows respectively.

[0150] Specifically, delta_i is the linear interval, and its value cannot be too small, otherwise the nonlinear gain will be too large; nor can it be too large, otherwise the advantage of the nonlinear gain cannot be reflected.

[0151] gama_i is obtained by experiment or theoretical derivation. Its essence is the critical point of the performance of LESO and NLeso: when the error e_i < gama_i, take NLESO; when the error e_i > gama_i, take LESO. This can give full play to the advantages of LESO and NLeso and make the overall performance optimal.

[0152] Specifically, the physical meaning and determination rule of α i are as follows:

[0153] Physical meaning: α i is the power parameter in the fal function (nonlinear function), which determines the nonlinear degree of the fal function. By adjusting the value of α i , the gain of the fal function at different error magnitudes can be changed, so as to achieve the control effect of "large gain for small error, small gain for large error".

[0154] Determination rule: The value of α i is usually determined according to the actual application scenario and the desired control performance. Generally speaking, a smaller α iThe value will make the fal function have a smaller gain when the error is large and a larger gain when the error is small, which helps to speed up the convergence and reduce overshoot. In practical applications, the appropriate α can be determined by simulation or experiment. i value.

[0155] Specifically, β i The physical meaning and determination rules of:

[0156] Physical meaning: β i It is the gain parameter in ESO (Extended State Observer) or NLESO (Nonlinear Extended State Observer), which is used to adjust the observer's estimation speed of the system state and disturbance.

[0157] Determine the rule: β i The value of usually needs to be determined based on the dynamic characteristics and noise level of the system. i The value of β can speed up the response of the observer, but it may also lead to excessive sensitivity to noise. Therefore, in practical applications, it is necessary to find a suitable β through debugging and experiments. i value to strike a balance between fast response and noise suppression.

[0158] When the virtual control value V i (i=1,2,3) is determined, the actual control value U i (i=1, 2, 3) is determined by formula (31):

[0159]

[0160] 3) Control system stability analysis: In order to ensure the stability of the multi-loop control system, it is necessary to analyze the stability of the entire error feedback closed-loop control subsystem. Before conducting a formal analysis, two existing technologies are described: LADRC for single-input single-output (SISO) systems and SADRC for multiple-input multiple-output (MIMO) systems.

[0161] Consider a general second-order SISO nonlinear time-varying dynamic system, such as Equation (17) and Equation (18), and apply LADRC to control the object. Those skilled in the art will know that: Assuming f is differentiable and setting If h is bounded, then there exists a constant σ>0, a finite T>0, ω o >0,ω c > 0, so for some positive integer j and

[0162] Considering a general second-order MIMO nonlinear time-varying dynamic system like Equation (26), and applying SADRC to control the object, those skilled in the art will know that: assuming f is differentiable, and and h are bounded. If the gain vector K and the observer gain vector L are chosen so that A c l is Hurwitz, then δ cl (t) exponentially converges to a bounded sphere, where λ m ax(P cl ) is P cl The maximum eigenvalue, P cl It is equation A cl TP cl +P cl A cl =-I (mn+m(n+1))×(mn+m(n+1)) The solution, h max is the absolute maximum value of h(t).

[0163] On this basis, the stability analysis of the entire error feedback closed-loop subsystem is described below. The Lyapunov function is constructed for the multi-loop control system of the quadrotor drone, which is represented by equation (32):

[0164]

[0165] Where x represents the state variable of the multi-loop control system, e1 and e2 are the tracking errors of the position loop and attitude loop. Therefore, the entire multi-loop control system is asymptotically stable as long as Next, demonstrate this by incrementally adding each loop from the inner loop to the outer loop.

[0166] Stability analysis of attitude loop. The tracking error of the attitude loop is in is the control instruction, ξ2=[φ θ ψ] T is the actual attitude angle. For this error, the Lyapunov function V(ξ2) is constructed as follows, represented by equation (33):

[0167]

[0168] Those skilled in the art will know that by adjusting the appropriate gain vector K and the observer gain vector L, V(ξ2) < 0 can be ensured. This means that the tracking error e2 converges asymptotically to zero and ξ2 converges asymptotically to

[0169] Position loop stability analysis. Similarly, the tracking error of the position loop is in is the control instruction, ξ1=[xyz] T is the actual position coordinate. For this error, the Lyapunov function V(ξ1) is constructed as follows, represented by equation (34):

[0170]

[0171] According to the position control model (17) and equation (18), the stability of the outer position loop has no direct relationship with the attitude angle. Those skilled in the art will know that by adjusting the appropriate values ​​of the gain vector K′ and the observer gain vector L′, it is possible to ensure Therefore, we can draw a similar conclusion that by adjusting the appropriate values ​​of K, K′, L, L′ we can ensure This means that the derivative of the Lyapunov function of the multi-loop control system of the quadrotor drone is

[0172] In summary, the entire multi-loop control system of the UAV close-range formation model is asymptotically stable.

[0173] It is worth mentioning that the technical features such as the specific selection of the quadcopter involved in the patent application of this invention should be regarded as the prior art. The specific structure, working principle and possible control method and spatial layout method of these technical features can be selected according to the conventional selection in the field, and should not be regarded as the inventive point of the patent of this invention. The patent of this invention will not be further elaborated.

[0174] For those skilled in the art, it is still possible to modify the technical solutions described in the aforementioned embodiments, or to make equivalent substitutions for some of the technical features therein. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A robust and precise formation control method for a quadrotor drone based on a virtual control network and an anti-disturbance controller, characterized in that: The following steps are involved: Step S1: estimating the state of the virtual pilot quadrotor drone; Step S2: Establishing a virtual control network; Step S3: The trajectory of the virtual pilot quadrotor UAV is generated by trajectory planning. Each follower quadrotor UAV tracks the trajectory of the virtual pilot quadrotor UAV and independently applies the anti-disturbance controller of the follower quadrotor UAV, so that each follower quadrotor UAV estimates the state of the virtual pilot quadrotor UAV by itself, so as to finally obtain the trajectory of each follower quadrotor UAV relative to the virtual pilot quadrotor UAV.

2. The method for robust and precise formation control of a quadrotor drone based on a virtual control network and an anti-disturbance control controller according to claim 1 is characterized in that: In step S1, let n L =[x L ,y L , z L ] T is the position coordinate of the virtual navigator in the northeast celestial coordinate system, For U A V i Estimated virtual lead position coordinates; In the distributed communication framework, U A V i The virtual lead position coordinates are represented by formula (10): Where λ>0, N L represents a group of drones that communicate directly with a virtual leader; the virtual leader state η L Only in v i ∈N L When UAV j communicates directly with UAV i, a is obtained. ij =1, otherwise take a ij =0.

3. The method for robust and precise formation control of a quadrotor drone based on a virtual control network and an anti-disturbance controller according to claim 2 is characterized in that: In step S1: the relative kinematic model of the quadrotor drone is represented by equation (16): The rotation angle ψ i ′=90°-ψ i , ψ i is the heading angle of the following drone i, (x i ,y i , z i )and denote the positions of the follower drone i and the estimated virtual leader in the northeast celestial coordinate system, (x ri ,y ri , z ri ) represents the position of the virtual leader in the heading coordinate system {W} of the wingman i; is the estimated state under the distributed communication topology.

4. The method for robust and precise formation control of a quadrotor drone based on a virtual control network and an anti-disturbance controller according to claim 1 is characterized in that: Step S2 is specifically implemented as the following steps: virtual control points are set with different priorities, and according to the order of priorities, virtual control points with higher priorities give priority to selecting the nearest UAV, and then virtual control points with lower priorities are reconsidered, and the nearest UAV is selected by the virtual control points with lower priorities.

5. The method for robust and precise formation control of a quadrotor drone based on a virtual control network and an anti-disturbance controller according to claim 1 is characterized in that: Step S2 is specifically implemented as the following steps: If the virtual control points are not set with different priorities, the relative distances between each drone and all virtual control points are calculated one by one, and the minimum sum distance is taken to determine the corresponding relationship between the drone and the virtual control point.

6. The method for robust and precise formation control of a quadrotor drone based on a virtual control network and an anti-disturbance controller according to claim 1 is characterized in that: The "trajectory tracking" of step S3 is specifically implemented as follows: trajectory tracking adopts a cascade control method, including outer loop position control and inner loop attitude control.

7. The method for robust and precise formation control of a quadrotor drone based on a virtual control network and an anti-disturbance controller according to claim 6 is characterized in that: "Outer loop position control" is implemented as follows: The aerodynamic model of the quadrotor drone in step S7 is represented by equations (17) and (18): Among them, U x , U y , U z is the virtual control value, given by U x , U y , U z Calculate the inner loop attitude control command; f x , f y , f z is regarded as the total disturbance, w x , w y , w z are various internal and external disturbances; for the outer loop position control, let [x c y c z c ] T is the position command of the follower, |xyz] T is the real-time feedback of the position coordinates of the slave, rewriting equation (16) into equation (19); Among them, (x r ,y r , z r ) represents the position of the virtual leader in the heading coordinate system {W} of the wingman, (x L ,y L , z L ) is the position coordinate of the virtual leader estimated by the follower UAV; Based on formula (19), formula (20) is obtained: in, is the expected position coordinate of the virtual leader of the follower UAV in the heading coordinate system; Based on equations (17) to (20), a LADRC-based position controller is designed and obtained, which is represented by equations (21) to (23): Among them, r x0 , r y0 , r z0 is a parameter that affects the speed of tracking the input signal, β xi , β yi , β zi , i = 1, 2, 3 are the ESO gains that need to be adjusted, k xi , k yi , k zi , i = 1, 2 is the gain of the control law that needs to be adjusted; "Inner loop attitude control" is specifically implemented as follows: Known ψ c , the inner loop attitude angle control command (φ) is obtained by formula (24) c ,θ c , ψ c ): The inner loop attitude angle control command (φ c ,θ c , ψ c ) and the control quantity U4, the attitude loop controller is designed, which is represented by formula (25): Among them, V i (i=1, 2, 3) is the virtual control value, then we get formula (26): The SADRC controller is designed by equation (26), and is represented by equations (27) to (29):

Images