A safe formation control method against saturation
By designing an anti-saturation safe formation control method, the problem of poor safety performance in UAV formation control is solved, and the stability and safety are improved under obstacle and input saturation conditions, ensuring the robustness and practicality of formation control.
Patent Information
- Application Number
- CN202410134868.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-01-31
- Publication Date
- 2024-11-05
- Estimated Expiration
- 2044-01-31
AI Technical Summary
Existing drone formation control methods have poor safety performance when encountering obstacles and fail to effectively handle input saturation issues, resulting in unstable control performance.
The design of a saturation-resistant safe formation control method includes establishing an affine transformation formation model, a finite-time disturbance observer, an anti-saturation auxiliary system, a nominal anti-saturation formation controller, and a safety controller. These steps improve the system's disturbance resistance and anti-saturation characteristics, ensuring system stability and safety.
While maintaining formation stability, the system's anti-disturbance capability and anti-saturation characteristics were improved, obstacle avoidance and inter-machine collision avoidance issues were resolved, and the safety performance and robustness of the control method were guaranteed.
Smart Images

Figure CN117970941B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the field of advanced control technology, and in particular relates to an anti-saturation safety formation control method. Background Art
[0002] Multi-UAV systems have been widely used in environmental monitoring, target detection, transportation and other fields. The premise for the realization of these applications is the formation control problem. In the past twenty years, many UAV formation control methods have been studied. For example, in document 1 (Layered affine formation control of networked uncertain systems: A fully distributed approach over directed graphs [J]. IEEE transactions on cybernetics, 2020, 51 (12): 6119-6130.), the affine transformation formation system is divided into two layers, and a new two-layer hybrid affine formation control system in networked Euler-Lagrangian dynamics under a directed graph structure is proposed. However, document 1 does not consider how the system maintains safety when encountering obstacles, and does not consider the problem of input saturation. In reference 2 (Continuous-time Gaussian Process Trajectory Generation for Multi-robot Formation via Probabilistic Inference [C] / / 2021 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2021: 9247-9253.), a GPMP-based formation planning method is proposed for corridor environments with varying widths. Reference 3 (Robust and efficient trajectory planning for formation flight in dense environments [J]. IEEE Transactions on Robotics, 2023.) adopts a hierarchical planning method to propose a formation planning method in a complex environment. Although references 2 and 3 have achieved remarkable results, they do not consider the stability proof of the system, and the existence of disturbances is not considered, so it is still difficult to ensure safety. At the same time, some existing studies are based on behavior-based formation control, which decomposes the system behavior into simple behaviors to achieve the requirements of formation and obstacle avoidance, but the converted behaviors have no transition between sub-behaviors, which often leads to unstable control effects and cannot be quantitatively described mathematically. It is difficult to analyze the stability and convergence of the formation.
[0003] In summary, the safety performance of existing control methods is still poor, and it is necessary to propose a new safe formation control method. Summary of the invention
[0004] The purpose of the present invention is to solve the problem of poor safety performance of existing control methods and to propose an anti-saturation safety formation control method.
[0005] The technical solution adopted by the present invention to solve the above technical problems is: a saturation-resistant safety formation control method, the method specifically comprising the following steps:
[0006] Step 1: Establish an affine transformation formation model of individuals in the UAV system;
[0007] Step 2: Establish a finite-time disturbance observer based on the affine transformation formation model in step 1, and use the finite-time disturbance observer to estimate the lumped disturbance in the system;
[0008] Step 3: Design an anti-saturation auxiliary system;
[0009] Step 4: Based on the results of step 2 and step 3, design a nominal anti-saturation formation controller;
[0010] Step 5: Design a safety controller based on the nominal anti-saturation formation controller to complete obstacle avoidance and inter-aircraft collision avoidance.
[0011] Furthermore, the affine transformation formation model of the individuals is:
[0012]
[0013] Among them, p f is the target position vector of the follower, Yes f The first derivative of f is the velocity vector of the follower, Yes f The first-order derivative of , d is the unknown lumped disturbance, u f is the control input vector of the follower.
[0014] Furthermore, the target position vector p of the follower f According to the current position vector of the navigator:
[0015]
[0016] in, It is the stress matrix constructed by the leader and the follower under the system working dimension. is the stress matrix constructed between followers under the system working dimension. The superscript -1 is the inverse of the matrix. l is the current position vector of the navigator.
[0017] Furthermore, the stress matrix and The calculation method of
[0018]
[0019] where, Ω ij is the element in the \(i\)-th row and \(j\)-th column of the intermediate variable matrix Ω, ω ij is the connection weight between the \(i\)-th individual and the \(j\)-th individual, Ω ll 、Ω lf 、Ω fl and Ω ff are sub-matrices in the intermediate variable matrix Ω, ε is the set of connection edges of the system connection topology, is the Kronecker product operation, is the set of neighbor individuals of the \(i\)-th individual in the system connection topology, I n is the identity matrix, and \(n\) is the dimension of the system operation.
[0020] Furthermore, the finite-time disturbance observer is:
[0021]
[0022] where, λ0, λ1 and L are the parameters of the finite-time disturbance observer, sign(·) is the sign function, diag(·) represents taking the diagonal matrix, |·| represents taking the absolute value, z0 is the intermediate variable, is the first derivative of z0, is the estimation result of the lumped disturbance d, is 's first derivative, and μ0, μ1 are intermediate transition variables.
[0023] Furthermore, the expression of the anti-saturation auxiliary system is:
[0024]
[0025] where, ψ is the auxiliary variable, is the first derivative of ψ, \(p\) and \(q\) are odd numbers, satisfying \(q < p < 2q\), constants \(a > 0\), \(b > 0\), \(c > 0\), ‖·‖ is the 2-norm, ψ ref is the reference quantity of the auxiliary variable, Δu f is the control input deviation caused by input saturation, and \(h'\) is the set anti-saturation parameter.
[0026] Furthermore, the nominal anti-saturation formation controller is:
[0027]
[0028] Among them, ‖·‖1 is the 1-norm, the superscript T is the transpose of the matrix, the superscript -1 is the inverse of the matrix, s is the designed sliding surface, and u eq is the control input that guides the state into the terminal sliding surface, v d is the desired formation speed, Yes d The first-order derivative of , B, k1, k2, K, η are constant parameters of the anti-saturation nominal control input, w1 and ε0 are intermediate variables, I is the identity matrix, e is the current state error, is the first derivative of e.
[0029] Furthermore, the designed sliding surface s is:
[0030]
[0031] Furthermore, the design method of the safety controller is as follows:
[0032] Step 51: Design the control obstacle function h(p,v) between the i-th individual and the obstacle:
[0033]
[0034] Among them, p i is the position of the ith individual, p o is the position of the obstacle closest to the i-th individual, v i is the velocity of the ith individual, v o is the speed of the obstacle closest to the i-th individual, d io is the Euclidean distance between the ith individual and the nearest obstacle, D s It is a pre-set safe distance;
[0035] Design the distributed control barrier function between the i-th individual and the obstacle:
[0036]
[0037] Among them, u i is the nominal input of the ith individual, Δv io is the relative speed between the ith individual and the nearest obstacle, b io is with d io Constraints associated with the control barrier function:
[0038]
[0039] Among them, γ is the parameter of the control barrier function, γ is a constant, Yes io The first derivative of ;
[0040] Step 52: Design the control barrier function between individuals
[0041]
[0042] Among them, p i is the position of the ith individual, p k is the position of the kth individual, v i is the velocity of the ith individual, v k is the velocity of the kth individual, d ik is the Euclidean distance between the i-th individual and the k-th individual;
[0043] Design the distributed control barrier function between individuals:
[0044]
[0045] Where Δv ik is the relative speed between the ith individual and the kth individual, b ik is with d ik Constraints associated with the control barrier function:
[0046]
[0047] Among them, γ is the parameter of the control barrier function, γ is a constant, Yes ik The first derivative of ;
[0048] Step 53: Design a safety controller based on the control barrier function and distributed control barrier function designed in step 51 and step 52:
[0049]
[0050] in, is the control input of the ith individual, let is the infinite norm of the control input of the ith individual, α i is the control input limit of the ith individual, is a real number and m is the dimension of the control input.
[0051] The beneficial effects of the present invention are:
[0052] The present invention proposes an anti-saturation safety formation control method. The control method includes four parts: finite-time disturbance observer design, anti-saturation auxiliary system design, nominal anti-saturation formation controller design and safety controller design. While maintaining the properties of the affine transformation formation system, the system's anti-disturbance and anti-saturation characteristics are improved, the system's collision avoidance problem is solved, the safety performance of the control method is guaranteed, and it is more robust and practical. BRIEF DESCRIPTION OF THE DRAWINGS
[0053] Figure 1 A flow chart of an anti-saturation safety formation control method of the present invention;
[0054] Figure 2 Schematic diagram of affine transformation formation. DETAILED DESCRIPTION
[0055] Specific implementation method 1. Combination Figure 1 The present embodiment describes a method for controlling a safe formation against saturation, the method specifically comprising the following steps:
[0056] Step 1: Establish an affine transformation formation model of individuals in the UAV system;
[0057] Step 2: Establish a finite-time disturbance observer based on the affine transformation formation model in step 1, and use the finite-time disturbance observer to estimate the lumped disturbance in the system;
[0058] Step 3: Design an anti-saturation auxiliary system;
[0059] Step 4: Based on the results of step 2 and step 3, design a nominal anti-saturation formation controller;
[0060] Step 5: Design a safety controller based on the nominal anti-saturation formation controller to complete obstacle avoidance and inter-aircraft collision avoidance.
[0061] The present invention uses a finite time disturbance observer to estimate the lumped disturbance in the system, and can estimate the unknown disturbance term within a finite time to achieve compensation for the disturbance. The present invention increases the auxiliary variables by designing an anti-saturation auxiliary system to maintain the global stability of the system when the actuator is continuously saturated.
[0062] Combining the finite-time disturbance observer and the anti-saturation auxiliary system, it can converge to the specified target position within a finite time and ensure the stability of the system. Combining the control obstacle function and the nominal anti-saturation formation controller to design a safety controller, construct a quadratic optimization problem, and achieve obstacle avoidance and collision avoidance while maintaining the formation.
[0063] Specific implementation method 2: Combination Figure 2This embodiment is different from the first embodiment in that the affine transformation formation model of the individuals is:
[0064]
[0065] Among them, p f is the target position vector of the follower, Yes f The first derivative of f is the velocity vector of the follower, Yes f The first-order derivative of , d is the unknown lumped disturbance, u f is the control input vector of the follower.
[0066] The other steps and parameters are the same as those in the first embodiment.
[0067] Specific implementation method 3: This implementation method is different from specific implementation method 1 or 2 in that the target position vector p of the follower f According to the current position vector of the navigator:
[0068]
[0069] in, It is the stress matrix constructed by the leader and the follower under the system working dimension. is the stress matrix constructed between followers under the system working dimension. The superscript -1 is the inverse of the matrix. l is the current position vector of the navigator.
[0070] The other steps and parameters are the same as those in the first or second embodiment.
[0071] Specific implementation method 4: This implementation method is different from specific implementation methods 1 to 3 in that the stress matrix and The calculation method is:
[0072]
[0073] Among them, Ω ij is the element in the i-th row and j-th column of the intermediate variable matrix Ω, ω ij is the connection weight between the i-th individual and the j-th individual, Ω ll ,Ω lf ,Ω fl and Ω ff is a submatrix in the intermediate variable matrix Ω, ε is the set of connection edges of the system connection topology, is the Kronecker product operation, is the set of neighbor individuals of the \(i\)-th individual in the system connection topology (a neighbor individual refers to an individual that has a connection edge with the \(i\)-th individual), \(I\) n is the identity matrix, and \(n\) is the dimension of the system operation.
[0074] Other steps and parameters are the same as those in any one of the specific embodiments 1 to 3.
[0075] In the present invention, \(\Omega\) ll is the stress matrix composed of the connection relationships and connection weights among all leaders, \(\Omega\) ff is the stress matrix composed of the connection relationships and connection weights among all followers. \(\Omega\) lf is the stress matrix composed of the connection relationships and connection weights from all leaders to all followers. \(\Omega\) fl is the stress matrix composed of the connection relationships and connection weights from all followers to all leaders. In the system represented by an undirected graph, \(\Omega\) fl and \(\Omega\) lf are transposes of each other. There are differences in the system represented by a directed graph. In the present invention, only the case of an undirected graph is considered.
[0076] Specific embodiment 5. The difference between this embodiment and any one of the specific embodiments 1 to 4 is that the finite-time disturbance observer is as follows:
[0077]
[0078] where \(\lambda_0\), \(\lambda_1\), and \(L\) are parameters of the finite-time disturbance observer, \(\lambda_0\), \(\lambda_1\), and \(L\) are all constants, \(\text{sign}(\cdot)\) is the sign function, \(\text{diag}(\cdot)\) represents taking the diagonal matrix, \(|\cdot|\) represents taking the absolute value, \(z_0\) is an intermediate variable, is the first derivative of \(z_0\), is the estimated result of the lumped disturbance \(d\), is 's first derivative, and \(\mu_0\), \(\mu_1\) are intermediate transition variables.
[0079] Other steps and parameters are the same as those in any one of the specific embodiments 1 to 4.
[0080] Specific embodiment 6. The difference between this embodiment and any one of the specific embodiments 1 to 5 is that the expression of the anti-saturation auxiliary system is as follows:
[0081]
[0082] where \(\psi\) is an auxiliary variable, is the first derivative of \(\psi\), \(p\) and \(q\) are odd numbers, satisfying \(q < p < 2q\), constants \(a>0\), \(b>0\), \(c>0\), \(\|\cdot\|\) is the 2-norm, \(\psi\) ref is the reference quantity of the auxiliary variable, used to switch the anti-saturation strategy, \(\Delta u\)f is the control input deviation caused by input saturation, and h′ is the set anti-saturation parameter.
[0083] The other steps and parameters are the same as those in Specific Implementations 1 to 5.
[0084] The segmented anti-saturation auxiliary system designed in this implementation mode can ensure that the auxiliary system converges to 0 in a finite time on the basis of ensuring system stability, and can also reduce oscillations that occur after system saturation.
[0085] Specific implementation method 7: This implementation method is different from any one of specific implementation methods 1 to 6 in that the nominal anti-saturation formation controller is:
[0086]
[0087] Among them, ‖·‖1 is the 1-norm, the superscript T is the transpose of the matrix, the superscript -1 is the inverse of the matrix, s is the designed sliding surface, and u eq is the control input that guides the state into the terminal sliding surface, v d is the desired formation speed, Yes d The first-order derivative of , B, k1, k2, K, η are constant parameters of the anti-saturation nominal control input, w1 and ε0 are intermediate variables, I is the identity matrix, e is the current state error, is the first derivative of e.
[0088] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.
[0089] Specific embodiment 8: This embodiment is different from any one of specific embodiments 1 to 7 in that the designed sliding surface s is:
[0090]
[0091] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.
[0092] Specific implementation method 9: This implementation method is different from any one of specific implementation methods 1 to 8 in that the design method of the safety controller is as follows:
[0093] Step 51: Design the control obstacle function h(p,v) between the i-th individual and the obstacle:
[0094]
[0095] Among them, p i is the position of the ith individual, p o is the position of the obstacle closest to the i-th individual, v i is the velocity of the ith individual, vo is the speed of the obstacle closest to the ith individual. For a static obstacle, v o =0,d io is the Euclidean distance between the ith individual and the nearest obstacle, D s It is a pre-set safe distance;
[0096] Design the distributed control barrier function between the i-th individual and the obstacle:
[0097]
[0098] Among them, u i is the nominal input of the ith individual (according to the control input vector u f Get, that is, u i is the vector u f The control input corresponding to the i-th follower in io is the relative speed between the ith individual and the nearest obstacle, b io is with d io Constraints associated with the control barrier function:
[0099]
[0100] Among them, γ is the parameter of the control barrier function, γ is a constant, Yes io The first derivative of ;
[0101] Step 52: Design the control barrier function between individuals
[0102]
[0103] Among them, p i is the position of the ith individual, p k is the position of the kth individual, v i is the velocity of the ith individual, v k is the velocity of the kth individual, d ik is the Euclidean distance between the i-th individual and the k-th individual;
[0104] Design the distributed control barrier function between individuals:
[0105]
[0106] Where Δv ik is the relative speed between the ith individual and the kth individual, b ik is with d ik Constraints associated with the control barrier function:
[0107]
[0108] Among them, γ is the parameter of the control barrier function, γ is a constant, Yes ik The first derivative of ;
[0109] Step 53: Design a safety controller based on the control barrier function and distributed control barrier function designed in step 51 and step 52:
[0110]
[0111] in, is the control input of the ith individual, let is the infinite norm of the control input of the ith individual, α i is the control input limit of the ith individual, is a real number and m is the dimension of the control input.
[0112] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.
[0113] The first two constraints of the follower safety controller designed in this embodiment are safety guarantee between obstacles and between machines, and the last one is the input constraint. The control input of the leader is obtained by setting in advance and does not need to be designed.
[0114] The above calculation examples of the present invention are only used to explain the calculation model and calculation process of the present invention in detail, and are not intended to limit the implementation methods of the present invention. For ordinary technicians in the relevant field, other different forms of changes or modifications can be made based on the above description. It is impossible to list all the implementation methods here. All obvious changes or modifications derived from the technical solution of the present invention are still within the scope of protection of the present invention.
Claims
1. A method for controlling a safe formation against saturation, characterized in that: The method specifically comprises the following steps: Step 1: Establish an affine transformation formation model of individuals in the UAV system; Step 2: Establish a finite-time disturbance observer based on the affine transformation formation model in step 1, and use the finite-time disturbance observer to estimate the lumped disturbance in the system; Step 3: Design an anti-saturation auxiliary system; The expression of the anti-saturation auxiliary system is: where ψ is an auxiliary variable, is the first derivative of ψ, p and q are odd numbers, satisfying q < p < 2q, constants a > 0, b > 0, c > 0, ‖·‖ is the 2-norm, ψ ref is the reference quantity of the auxiliary variable, Δu f is the control input deviation caused by input saturation, and h′ is the set anti-saturation parameter; Step 4: Based on the results of step 2 and step 3, design a nominal anti-saturation formation controller; Step 5: Design a safety controller based on the nominal anti-saturation formation controller to complete obstacle avoidance and inter-aircraft collision avoidance.
2. The anti-saturation safety formation control method according to claim 1, characterized in that: The affine transformation formation model of the individuals is: Among them, p f is the target position vector of the follower, Yes f The first derivative of f is the velocity vector of the follower, Yes f The first-order derivative of , d is the unknown lumped disturbance, u f is the control input vector of the follower.
3. The anti-saturation safety formation control method according to claim 2, characterized in that: The target position vector p of the follower f According to the current position vector of the navigator: in, It is the stress matrix constructed by the leader and the follower under the system working dimension. is the stress matrix constructed between followers under the system working dimension. The superscript -1 is the inverse of the matrix. l is the current position vector of the navigator.
4. The anti-saturation safety formation control method according to claim 3 is characterized in that: The stress matrix and The calculation method is: Among them, Ω ij is the element in the i-th row and j-th column of the intermediate variable matrix Ω, ω ij is the connection weight between the i-th individual and the j-th individual, Ω ll ,Ω lf ,Ω fl and Ω ff is a submatrix in the intermediate variable matrix Ω, ε is the set of connection edges of the system connection topology, is the Kronecker product operation, is the set of neighbor individuals of the ith individual in the system connection topology, I n is the identity matrix and n is the dimension in which the system operates.
5. The anti-saturation safety formation control method according to claim 4, characterized in that: The finite-time disturbance observer is: Among them, λ0, λ1 and L are the parameters of the finite-time disturbance observer, sign(·) is the sign function, diag(·) represents the diagonal matrix, |·| represents the absolute value, z0 is the intermediate variable, is the first derivative of z0, is the estimation result of the lumped disturbance d, yes The first-order derivative of , μ0 and μ1 are intermediate transition variables.
6. The anti-saturation safety formation control method according to claim 5, characterized in that: The nominal anti-saturation formation controller is: Among them, ‖·‖1 is the 1-norm, the superscript T is the transpose of the matrix, the superscript -1 is the inverse of the matrix, s is the designed sliding surface, and u eq is the control input that guides the state into the terminal sliding surface, v d is the desired formation speed, Yes d The first-order derivative of , B, k1, k2, K, η are constant parameters of the anti-saturation nominal control input, w1 and ε0 are intermediate variables, I is the identity matrix, e is the current state error, is the first derivative of e.
7. The anti-saturation safety formation control method according to claim 6, characterized in that: The designed sliding surface s is:
8. The anti-saturation safety formation control method according to claim 7, characterized in that: The design method of the safety controller is as follows: Step 51: Design the control obstacle function h(p,v) between the i-th individual and the obstacle: Among them, p i is the position of the ith individual, p o is the position of the obstacle closest to the i-th individual, v i is the velocity of the ith individual, v o is the speed of the obstacle closest to the i-th individual, d io is the Euclidean distance between the ith individual and the nearest obstacle, D s It is a pre-set safe distance; Design the distributed control barrier function between the i-th individual and the obstacle: Among them, u i is the nominal input of the ith individual, Δv io is the relative speed between the ith individual and the nearest obstacle, b io is with d io Constraints associated with the control barrier function: Among them, γ is the parameter of the control barrier function, γ is a constant, Yes io The first derivative of ; Step 52: Design the control barrier function between individuals Among them, p i is the position of the ith individual, p k is the position of the kth individual, v i is the velocity of the ith individual, v k is the velocity of the kth individual, d ik is the Euclidean distance between the i-th individual and the k-th individual; Design the distributed control barrier function between individuals: Where Δv ik is the relative speed between the ith individual and the kth individual, b ik is with d ik Constraints associated with the control barrier function: Among them, γ is the parameter of the control barrier function, γ is a constant, Yes ik The first derivative of ; Step 53: Design a safety controller based on the control barrier function and distributed control barrier function designed in step 51 and step 52: in, is the control input of the ith individual, let is the infinite norm of the control input of the ith individual, α i is the control input limit of the ith individual, is a real number and m is the dimension of the control input.
