A method for safe human-unmanned aerial vehicle (UAV) swarm motion planning based on dynamic game theory

Abstract

This invention discloses a human-unmanned aerial vehicle (UAV) swarm safe interactive motion planning method based on dynamic game theory. This method enables UAV swarms to follow a given human-defined trajectory in desired formation and autonomously avoid obstacles when human commands are unsafe. The method includes: Step A: Creating a UAV swarm model; Step B: Setting the desired formation, safety set, and communication relationships between UAVs; Step C: Designing the optimal response strategy for following UAVs, i.e., the Nash equilibrium strategy; Step D: Designing the safety control strategy for the leader UAV; Step E: Designing the formation tracking control strategy for the leader UAV; Step F: Implementing the human-unmanned aerial vehicle (UAV) swarm safe interactive motion planning using the controller determined in steps C, D, and E. This invention fully leverages the autonomy of the UAV swarm during the interaction process, enabling it to proactively detect and take measures to compensate for the shortcomings of human commands, achieving human-machine complementarity and safe interaction.

Description

Technical Field

[0001] This invention relates to a motion planning method, specifically a human-unmanned swarm safe interactive motion planning method based on dynamic game theory, belonging to the field of application technology of human-swarm interactive motion planning methods. Background Technology

[0002] While it is generally desirable for drone swarms to autonomously complete desired tasks, their inherent weaknesses in perception and decision-making inevitably necessitate collaboration with human operators within the same operational space. In such situations, the drone swarm is expected to execute human commands to assist in complex tasks; however, human negligence, limited visibility, or unforeseen emergencies can render these commands unsafe. Therefore, it is desirable for the drone swarm to recognize the insecurity of human commands and temporarily disobey them to ensure safety.

[0003] Control Barrier Functions (CBFs) are widely used to solve obstacle avoidance problems in robotic systems. Most existing work uses CBFs to construct a Quadratic Programming (QP) problem, planning a safe path by solving a constrained optimization problem at each discrete time step. However, the resulting optimization problem may be unsolvable when the system trajectory is too close to the safety set boundary, leading to obstacle avoidance failure. To address this issue, some works consider possible future gains / costs in the optimization objective and introduce CBFs into the obstacle avoidance problem within an optimal control framework. Most of these works introduce CBF-related terms into the optimization objective, making the objective non-smooth and exacerbating the difficulty of solving for the safe control strategy. Although some works use learning methods to approximate the corresponding safe control strategy, ensuring safety during online learning remains a crucial issue worthy of further investigation. Summary of the Invention

[0004] This invention addresses the shortcomings of existing technologies by providing a dynamic game-based method for safe human-unmanned aerial vehicle (UAV) swarm interaction motion planning. This method aims to mitigate the failure of existing technologies when replanning paths at discrete moments may fail when the system trajectory is too close to the safety set boundary. It fully leverages the autonomy of UAV swarms during the interaction process, enabling them to proactively detect and take measures to compensate for the deficiencies of human commands, thereby achieving human-machine complementarity and safe interaction.

[0005] To achieve the above-mentioned objectives, the present invention adopts the following technical solution:

[0006] The present invention provides a method for safe interactive motion planning of human-unmanned aerial vehicle swarms based on dynamic game theory, characterized by the following steps:

[0007] Step A: Create a drone swarm model;

[0008] Step A.1: Create a dynamic model of the UAV;

[0009] Let the drone swarm be a system consisting of N+1 drones, and let the position, velocity, and acceleration of each drone in space satisfy the second-order dynamic equations; use equation (1) to construct the dynamic model of the i-th drone at time t:

[0010]

[0011] In equation (1), x i (t) represents the augmented state consisting of the position and velocity of the i-th UAV at time t. x represents i The first derivative of (t), I3 is the offset dynamics matrix, 03 is the three-dimensional zero matrix, and I3 is the three-dimensional identity matrix. Given the input matrix, u i (t) represents the acceleration of the i-th UAV at time t; i = 0, 1, ..., N;

[0012] Step A.2: Construct a hierarchical structure for the drone swarm;

[0013] The 0th drone in the drone swarm is designated as the leader and interacts directly with the operator; the remaining 1st, ..., Nth drones are all followers and respond autonomously based on the observed behavior of the leader; the leader drone and the follower drones are in a hierarchical relationship, with the leader at the top and the followers at the bottom.

[0014] Step B: Define the desired formation, safety set, and communication relationships between drones;

[0015] Step B.1: Set the desired formation;

[0016] Using the offset p of the k-th follower drone relative to the leader drone's state k This represents the desired formation of the k-th follower drone; k = 1, ..., N;

[0017] Step B.2: Use motion capture equipment to identify the location of pedestrians or dynamic obstacles in the movement space in order to set up a safety set;

[0018] Based on the pedestrian positions obtained by the motion capture device, a quadratic safety set C = {x} is defined. i (t), i = 0, 1, ..., N | h(x)i (t))>0}, where h(x) i (t) represents the collision function, and Represents the pedestrian position s at time t. h (t) is the augmented vector obtained by padding with zeros, r h For the safety radius;

[0019] Step B.3: Set up communication relationships between drones;

[0020] Each drone is treated as a node, and the communication relationships between drones are modeled using a directed graph G≡(V,ε,E) containing nodes, edges, and edge weights, where V={0,1,…,N} is a finite set of non-empty nodes. Let E be the edge set, (i,j) be the edge from node i to node j, and E = [e i,j Let be an (N+1)×(N+1) dimensional adjacency matrix describing the edge weights; where e i,j This represents the weight of the edge (j,i) from node j to node i;

[0021] Step C: Design the optimal response strategy for the drone under the objective function, i.e., the Nash equilibrium strategy;

[0022] Step C.1: Based on the status of neighboring drones and the desired formation, use equation (2) to design the local consistency error δ of the k-th follower drone at time t. k (t):

[0023]

[0024] In equation (2), Let represent the new state of the k-th follower drone at time t, and x k (t) represents the augmented state at time t, consisting of the position and velocity of the k-th follower drone. Let represent the new state of the j-th UAV at time t, and x j (t) represents the augmented state at time t, consisting of the position and velocity of the j-th UAV. j Let p0 represent the offset of the j-th drone. When j = 0, let p0 represent the 6×1 dimensional zero vector; N k Let N represent the set of neighbor nodes of the k-th follower drone, and N k = {j∈V:(j,k)∈ε}; (j,k) is the edge from node j to node k;

[0025] Step C.2: Use equation (3) to design the objective function V of the k-th follower UAV.k (δ k (t)), thus making the k-th follower drone in the objective function V k (δ k A game is formed under (t));

[0026]

[0027] In equation (3), Q k ∈R 6×6 and L k ∈R 3×3 Both are positive definite parameter matrices in the objective function of the k-th follower drone, R. 6×6 For a 6×6 dimensional real space, R 3×3 For a 3×3 real space, δ k (τ) represents the local consistency error of k following drones at time τ, u k (τ) represents the acceleration of the k-th follower drone at time τ. Indicate u k The transpose of (τ), δ k Transpose of (τ);

[0028] Step C.3: Based on optimal control theory and dynamic programming principle, the nonlinear partial differential equation satisfied by the Nash equilibrium strategy of the k-th following UAV is derived, as shown in equation (4);

[0029]

[0030] In equation (4), δ k (t) T δ k The transpose of (t), Let represent the Nash equilibrium strategy of the k-th follower drone at time t, and B T This represents the transpose of B. L represents k The reverse, Let represent the optimal value function of the k-th follower drone at time t, and inf represents the minimum operator. Describing the optimal value function Relative to δ k The gradient of (t), express transpose, d k Let represent the in-degree of the k-th follower drone, and

[0031] Step C.4: Use equation (5) to estimate the Nash equilibrium strategy of the k-th follower drone at time t:

[0032]

[0033] In equation (5), P k ∈R 6×6 Let be the positive definite matrix of the k-th follower drone, and satisfy the following condition:

[0034] Step D: Based on the safety set C, design the safety control strategy for the leader UAV at time t using equation (6).

[0035]

[0036] In equation (6), c b Y(x) represents a positive constant. i (t) is the control barrier function, which is obtained from equation (7). Represents the function Y(x) i (t) for x i The gradient of (t);

[0037]

[0038] In equation (7), h(0) represents the collision function h(x) i (t)) at x i The values ​​of (t) when it is always equal to 0, and

[0039] Step E: Design the formation tracking control strategy for the leader drone;

[0040] Step E.1: Give an augmented system state based on the state evolution dynamics of all UAVs;

[0041] Based on the tracking error of the leader drone and the local consistency error of the follower drone, the global error augmentation vector at time t is defined as δ(t) = [δ0(t)δ1(t)…δ N (t)] T ,in, This represents the tracking error of the leader drone at time t. x represents the new state of the leader drone at time t. c (t) represents the expected reference trajectory given by the human operator at time t;

[0042] Step E.2: Under the condition that all follower drones adopt Nash equilibrium strategy, use Equation (8) to design the objective function V(δ(t)) of the leader drone, so that the leader drone forms a Stackelberg game under the objective function V(δ(t));

[0043]

[0044] In equation (8), Q∈R 6(N+1)×6(N+1) and L∈R 3×3 Let R represent the two positive definite parameter matrices in the objective function of the leader drone. 6(N+1)×6(N+1) Let u represent a 6(N+1)×6(N+1) dimensional real space. 0,f (τ) represents the formation tracking control strategy of the leader drone at time τ, and δ(τ) represents the global error augmentation vector at time τ. T Indicates the transpose of δ(τ);

[0045] Step E.3: Based on optimal control theory and dynamic programming principles, derive the nonlinear partial differential equation satisfied by the Stackelberg equilibrium strategy of the leader UAV under the objective function V(δ(t)), as shown in equation (9):

[0046]

[0047] In equation (9), This represents the optimal formation tracking control strategy for the leader drone at time τ, and V * (δ(t)) represents the optimal value function of the leader drone at time t, and V represents the optimal value function * The gradient of (δ(t)) with respect to δ(t), Denotes the augmented input matrix, and e N,0 L represents the weight of the edge (0, N). -1 Denotes the inverse of matrix L;

[0048] Step E.4: Construct an evaluator network and an actuator network composed of multinomial neural networks, and then use equations (10) and (11) to estimate the optimal value function V of the leader drone, respectively. * (δ(t)) and Stackelberg equilibrium strategy

[0049]

[0050]

[0051] In equations (10) and (11), W1(t)∈R H Let W2(t) ∈ R be the weight vector of the evaluator network at time t. H Let φ(δ(t)) ∈ R be the weight vector of the actuator network at time t. H The basis functions are polynomials, and H represents the number of hidden layers in the network;

[0052] The update rate of the weight vector W1(t) of the evaluator network at time t is calculated using equations (12) and (13), respectively. and the update rate of the weight vector W2(t) of the actuator network at time t.

[0053]

[0054]

[0055] In equations (12) and (13), α1>0 and α2>0 are two learning rate parameters, F1>0 is a constant parameter matrix, F2>0 is a constant parameter, Δφ(t) represents the difference between the polynomial basis functions at time t and at time tT, and Δφ(t)=φ(δ(t))-φ(δ(tT)), Δφ(t) T This represents the transpose of Δφ(t). This represents the gradient of the polynomial basis function φ(δ(t)) with respect to δ(t);

[0056] Step F: Apply control strategies to the leader drone And apply control strategies to each follower drone. This enables safe interactive motion planning between human and unmanned aerial vehicle swarms.

[0057] The present invention provides an electronic device, comprising a memory and a processor, wherein the memory is used to store a program that supports the processor in executing the human-unmanned swarm safe interactive motion planning method, and the processor is configured to execute the program stored in the memory.

[0058] The present invention discloses a computer-readable storage medium on which a computer program is stored, wherein the computer program, when executed by a processor, performs the steps of the human-unmanned swarm safe interactive motion planning method.

[0059] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0060] 1. This invention considers the individual infinite time interval optimality and inter-individual interaction of UAVs. It uses dynamic programming and optimal control techniques to model the human-UAV swarm safety interaction problem as a Stackelberg-Nash game problem, overcoming the shortcomings of existing technologies that fail to avoid obstacles because they only consider discrete time-time targets.

[0061] 2. This invention decouples the learning process used to achieve formation tracking from the security issue, and designs a security control strategy based on a control barrier function and a learning-based formation tracking control strategy, which ensures both the existence of the optimal path and that security constraints are not violated when implementing cluster tasks.

[0062] 3. The safety control strategy designed in this invention has excellent scalability and can be combined with other nominal controllers to achieve different tasks. Attached Figure Description

[0063] Figure 1 This is a flowchart illustrating the human-unmanned swarm safe interactive motion planning method based on dynamic game theory of the present invention.

[0064] Figure 2 This is a diagram showing the information transmission relationship between the drone swarm, human operators, and obstacle locations in this invention.

[0065] Figure 3 This is a diagram showing the experimental results of the present invention. Detailed Implementation

[0066] In this embodiment, a human-UAV swarm safe interactive motion planning method based on dynamic game theory considers the optimality of individuals over infinite time intervals and the interaction between individuals. It overcomes the shortcomings of existing technologies that fail to avoid obstacles due to only considering discrete-time targets. Furthermore, it decouples the learning process used to achieve formation tracking from the safety issue, designing a safety control strategy based on a control obstacle function and a learning-based formation tracking control strategy respectively, ensuring that the learning process does not violate safety constraints. In addition, the designed safety control strategy has excellent scalability and can be combined with other nominal controllers to achieve different tasks. Specifically, this human-UAV swarm safe interactive motion planning method is used to enable a UAV swarm to follow a given trajectory in desired formation and autonomously avoid obstacles when human instructions are unsafe. Figure 1 As shown, it specifically includes:

[0067] Step A: Create a drone swarm model;

[0068] Step A.1: Create a dynamic model of the UAV;

[0069] Let the drone swarm be a system consisting of N+1 drones, and the position, velocity, and acceleration of the i-th drone in space all satisfy the second-order dynamic equations: Among them, s i (t) represents the spatial position of the i-th drone at time t, v i (t) represents the angular velocity of the i-th UAV at time t, a i (t) represents the acceleration of the i-th drone at time t. and They represent s respectively i (t) and v i The first derivative of (t), i = 0, 1, ..., N; using x i (t)=[x i (t) T v i (t) T ] T The augmented state is composed of the position and velocity of the i-th UAV at time t. Based on this, the dynamic model of the i-th UAV at time t is constructed using equation (1):

[0070]

[0071] In equation (1), x represents i The first derivative of (t), I3 is the offset dynamics matrix, 03 is the three-dimensional zero matrix, and I3 is the three-dimensional identity matrix. Given the input matrix, u i (t) represents the acceleration of the i-th UAV at time t, and u i (t)=a i (t); i = 0, 1, ..., N;

[0072] Step A.2: Construct a hierarchical structure for the drone swarm;

[0073] The 0th drone in the drone swarm is designated as the leader, interacting directly with the operator. The remaining drones, numbered 1 through N, are followers, autonomously responding based on observed leader behavior. A hierarchical relationship exists between the leader and followers, with the leader at the top and the followers at the bottom. The leader is responsible for guiding the swarm in a desired formation, following a human-given trajectory while ensuring all drones remain within a safe zone. Follower drones are rational individuals, always responding optimally to the current behavior of the leader drone.

[0074] Step B: Define the desired formation, safety set, and communication relationships between drones;

[0075] Step B.1: Set the desired formation;

[0076] Using the offset p of the k-th follower drone relative to the leader drone's statek This represents the desired formation of the k-th follower drone; k = 1, ..., N;

[0077] Step B.2: Use motion capture equipment to identify the location of pedestrians or dynamic obstacles in the movement space in order to set up a safety set;

[0078] Based on the pedestrian positions obtained by the motion capture device, a quadratic safety set C = {x} is defined. i (t), i = 0, 1, ..., N | h(x) i (t))>0}, where h(x) i (t) represents the collision function, and Represents the pedestrian position s at time t. h (t) is the augmented vector obtained by padding with zeros, r h For the safety radius;

[0079] Step B.3: Set up communication relationships between drones;

[0080] Each drone is treated as a node, and the communication relationships between drones are modeled using a directed graph G≡(V,ε,E) containing nodes, edges, and edge weights, where V={0,1,…,N} is a finite set of non-empty nodes. Let E be the edge set, (i,j) be the edge from node i to node j, and E = [e i,j Let be an (N+1)×(N+1) dimensional adjacency matrix describing the edge weights; where e i,j Let e ​​represent the weight of the edge (j,i) from node j to node i; if a directed edge exists from node j to node i, then e i,j >0; otherwise e i,j =0; The directed graph between the drones set up in the experiment is as follows: Figure 2 As shown;

[0081] Step C: Design the optimal response strategy for the drone under the objective function, i.e., the Nash equilibrium strategy;

[0082] Step C.1: Based on the neighboring drone status and the desired formation, adjust the offset p. k The formation problem is transformed into a uniform control problem. To this end, the local uniformity error δ of the k-th follower UAV at time t is designed using equation (2). k (t):

[0083]

[0084] In equation (2), Let represent the new state of the k-th follower drone at time t, and x k(t) represents the augmented state at time t, consisting of the position and velocity of the k-th follower drone. Let represent the new state of the j-th UAV at time t, and x j (t) represents the augmented state at time t, consisting of the position and velocity of the j-th UAV. j Let p0 represent the offset of the j-th drone. When j = 0, let p0 represent the 6×1 dimensional zero vector; N k Let N represent the set of neighbor nodes of the k-th follower drone, and N k ={j∈V:(j,k)∈ε}; (j,k) is the edge from node j to node k; it can be seen from equation (2) that δ k (t) describes the new state of the k-th drone at time t. The new state of drones with their neighbors In the border right e k,j The weighted sum under the following conditions;

[0085] Step C.2: Use equation (3) to design the objective function V of the k-th follower UAV. k (δ k (t)), thus making the k-th follower drone in the objective function V k (δ k A game is formed under (t));

[0086]

[0087] In equation (3), Q k ∈R 6×6 and L k ∈R 3×3 Both are positive definite parameter matrices in the objective function of the k-th follower drone, R. 6×6 For a 6×6 dimensional real space, R 3×3 For a 3×3 real space, δ k (τ) represents the local consistency error of k following drones at time τ, u k (τ) represents the acceleration of the k-th follower drone at time τ. Indicate u k The transpose of (τ), δ k The transpose of (τ).

[0088] Note that the objective function V k (δ k(t) is an infinite time integral function, which means that when designing the follower drone control strategy, not only the target at the current time t is considered, but also the target at future time. This helps to plan the safe path in advance and solves the problem that the optimization problem is unsolvable when only considering the target at discrete time, that is, the safe path does not exist.

[0089] Although in the objective function V k (δ k In (t)), V k (δ k (t) only with δ k (t) is explicitly correlated, but due to δ k The evolution of (t) is influenced by the strategy u of the k-th follower drone. k (t) and its neighboring drone strategies u j (t),j∈N k The influence of V k (δ k (t) is also implicitly affected by u k (t) and u j (t),j∈N k The impact of this. Therefore, the follower drone in the objective function V k (δ k Under the condition (t)), a game is formed, and the optimal response strategy is the Nash equilibrium strategy.

[0090] Step C.3: Based on optimal control theory and dynamic programming principle, the nonlinear partial differential equation satisfied by the Nash equilibrium strategy of the k-th following UAV is derived, as shown in equation (4);

[0091]

[0092] In equation (4), δ k (t) T δ k The transpose of (t), Let represent the Nash equilibrium strategy of the k-th follower drone at time t, and B T This represents the transpose of B. L represents k The reverse, Let represent the optimal value function of the k-th follower drone at time t, and inf represents the minimum operator. Describing the optimal value function Relative to δ k The gradient of (t), express transpose, d k Let represent the in-degree of the k-th follower drone, and

[0093] Step C.4: Based on the assumption of weak coupling in robot communication, using Approximately represents the optimal value function, where P k It is a positive definite matrix and satisfies Equation (5) is used to estimate the Nash equilibrium policy of the k-th follower drone at time t:

[0094]

[0095] Step D: Based on the safety set C, design the safety control strategy for the leader UAV at time t using equation (6).

[0096]

[0097] In equation (6), c b Y(x) represents a positive constant. i (t) is the control barrier function, which is obtained from equation (7). Represents the function Y(x) i (t) for x i The gradient of (t);

[0098]

[0099] In equation (7), h(0) represents the collision function h(x) i (t)) at x i The values ​​of (t) when it is always equal to 0, and c b The value of c is crucial; when c b When the value is small, the drone's avoidance speed is low when approaching obstacles, which may lead to obstacle avoidance failure; while when c b When the value is large, the drone will have a high avoidance speed when approaching an obstacle, which may make it difficult for the drone swarm to respond quickly to human commands after avoiding obstacles ahead.

[0100] Step E: Design the formation tracking control strategy for the leader drone;

[0101] Step E.1: Give an augmented system state based on the state evolution dynamics of all UAVs;

[0102] Based on the tracking error of the leader drone and the local consistency error of the follower drone, the global error augmentation vector at time t is defined as δ(t) = [δ0(t)δ1(t)…δ N (t)] T ,in, This represents the tracking error of the leader drone at time t. x represents the new state of the leader drone at time t. c (t) represents the expected reference trajectory given by the human operator;

[0103] Step E.2: Under the condition that all follower drones adopt the Nash equilibrium strategy, use Equation (8) to design the objective function V(δ(t)) of the leader drone, so that the leader drone forms a Stackelberg game under the objective function V(δ(t));

[0104]

[0105] In equation (8), Q∈R 6(N+1)×6(N+1) and L∈R 3×3 Let R represent the two positive definite parameter matrices in the objective function of the leader drone. 6(N+1)×6(N+1) Let u represent a 6(N+1)×6(N+1) dimensional real space. 0,f (τ) represents the formation tracking control strategy of the leader drone at time τ, and δ(τ) represents the global error augmentation vector at time τ. T Indicates the transpose of δ(τ);

[0106] Note that the evolution of the global error augmentation vector δ(t) is influenced by the control strategies of all follower drones. Assuming all follower drones are rational individuals—that is, they always respond optimally to the behavior of the current leader drone—in the following analysis, each follower drone adopts its Nash equilibrium strategy. According to the definition of Stackelberg equilibrium strategy, the leader drone control strategy obtained by optimizing the objective function V(δ(t)) of the leader drone is the Stackelberg equilibrium strategy.

[0107] Step E.3: Based on optimal control theory and dynamic programming principles, derive the nonlinear partial differential equation satisfied by the Stackelberg equilibrium strategy of the leader UAV under the objective function V(δ(t)), as shown in equation (9):

[0108]

[0109] In equation (9), This represents the optimal formation tracking control strategy for the leader drone at time τ, and V * (δ(t)) represents the optimal value function of the leader drone at time t, and V represents the optimal value function * The gradient of (δ(t)) with respect to δ, Denotes the augmented input matrix, and e N,0 L represents the weight of the edge (0, N). -1 Denotes the inverse of matrix L;

[0110] Step E.4: Construct an evaluator network and an actuator network composed of multinomial neural networks, and then use equations (10) and (11) to estimate the optimal value function V of the leader drone, respectively. * (δ(t)) and Stackelberg equilibrium strategy

[0111] V * (δt))≈W1t) T φ(δ(t)) (10)

[0112]

[0113] In equations (10) and (11), W1(t)∈R H Let W1t be the weight vector of the evaluator network at time t. T W1(t) represents the transpose of W2(t); W2(t)∈R H Let φ(δ(t)) ∈ R be the weight vector of the actuator network at time t. H Here, H represents the number of hidden layers in the network; in experiments, a polynomial basis function φ(δ(t)) can be set up, consisting of the quadratic form and cross term of each component in δ(t).

[0114] The optimal value function V of the leader drone estimated in equations (10) and (11) * (δ(t)) and Stackelberg equilibrium strategy Substituting into equation (9) yields the nonlinear partial differential equation, which produces an estimated residual E(t), and Minimize using gradient descent The update rates of W1(t) and W2(t) can be obtained. The update rates of the evaluator network's weight vector W1(t) at time t are calculated using equations (12) and (13), respectively. and the update rate of the weight vector W2(t) of the actuator network at time t.

[0115]

[0116] In equations (12) and (13), α1>0 and α2>0 are two learning rate parameters, F1>0 is a constant parameter matrix, F2>0 is a constant parameter, Δφ(t) represents the difference between the polynomial basis functions at time t and at time tT, and Δφ(t)=φ(δ)(t))-φ(δ(tT)), Δφ(t) T This represents the transpose of Δφ(t). This represents the gradient of the polynomial basis function φ(δ(t)) with respect to δ(t);

[0117] Step F: Apply control strategies to the leader drone And apply control strategies to each follower drone. This enables safe interactive motion planning between human and unmanned aerial vehicle swarms.

[0118] This invention designs a safety control strategy based on a control barrier function. and integrate it with formation tracking control strategy The combined effect of these factors on the leader drone ensures that the drone swarm can temporarily disobey human commands when they are unsafe, and then resume human commands once safety is guaranteed. Experimental results are as follows... Figure 3 As shown. In addition, the designed security control strategy... It has good scalability and can work with other nominal control strategies to achieve a variety of cluster tasks.

[0119] In this embodiment, an electronic device includes a memory and a processor. The memory is used to store a program that supports the processor in executing the above-described human-unmanned swarm safe interactive motion planning method. The processor is configured to execute the program stored in the memory.

[0120] In this embodiment, a computer-readable storage medium stores a computer program that, when executed by a processor, performs the steps of the above-described human-unmanned swarm safe interactive motion planning method.

Claims

1. A method for safe interactive motion planning of human-UAV swarm based on dynamic game, characterized in that, is performed as follows: Step A: creating a UAV swarm model; Step A.1: creating a UAV dynamics model; Let the UAV cluster be a system composed of a plurality of UAVs, and the position, velocity, and acceleration of each UAV in space satisfy a second-order dynamic equation; a dynamic model of the i-th UAV at time t is constructed using equation (1): (1) In formula (1), denotes the augmented state consisting of the position and velocity of the i-th drone t, denotes the first derivative of is the bias dynamics matrix, is a three-dimensional zero matrix, is a three-dimensional identity matrix, is the input matrix, is the acceleration of the i-th drone at time t; ;​​ Step A.2: constructing a hierarchical structure of the UAV swarm; Designate the 0th drone in the drone swarm as the leader drone, and have it interact directly with the operator; the remaining drones... All drones are follower drones and autonomously respond based on the observed behavior of the leader drone; the leader drone and the follower drones are in a hierarchical relationship, with the leader drone at the upper level and the follower drones at the lower level; Step B: setting a desired formation, a safety set, and a communication relationship among UAVs; Step B.1: setting a desired formation; an offset of a second follower drone relative to a state of a leader drone an offset of a second follower drone relative to a state of a leader drone an offset of a second follower drone relative to a state of a leader drone an offset of a second follower drone relative to a state of a leader drone an offset of a second follower drone relative to a state of a leader drone Step B.2: identifying the position of a pedestrian or a dynamic obstacle in the motion space by using a motion capture device to set a safety set; Setting a safety set of quadratic form based on the pedestrian position obtained by the motion capture device wherein, represents a collision function, and ; represents the pedestrian position at time t an augmented vector obtained by supplementing 0, is a safety radius; Step B.3: setting a communication relationship among UAVs; each drone is considered as a node, thereby modeling the communication relationship between drones by a directed graph comprising nodes, edges, weights of edges modeling the communication relationship between drones, wherein is a finite non-empty set of nodes, is a set of edges, is an edge from node to node , and is a dimensional adjacency matrix describing the weights of edges; wherein denotes the weight of the edge from node j to node i . Step C: designing an optimal response strategy of a follower UAV under a target function, i.e., a Nash equilibrium strategy; Step C.1: Design the local consensus error of the kth follower UAV at time t according to the neighbor UAV states and the desired formation shape using equation (2) : (2) In equation (2), Let represent the new state of the k-th follower drone at time t, and , Indicates by the first The augmented state at time t, composed of the positions and velocities of the follower drones. Let represent the new state of the j-th UAV at time t, and , Indicates by the first The augmented state at time t, composed of the position and velocity of each drone. Indicates the first The offset of each drone, when j=0, let Represents a 6×1 dimensional zero vector; Indicates the first The set of neighboring nodes of a follower drone, and ; For the node j To the node k The edge; Step C.2: Design the objective function of the kth follower UAV with formula (3) such that the kth follower UAV forms a game with the objective function below. (3) In equation (3), and Both are two positive definite parameter matrices in the objective function of the k-th follower drone. for Dimensional Space for Dimensional Space Indicates k following drones in Local consistency error at time, Indicates the k-th follower drone in acceleration at any moment express transpose, express Transpose of; Step C.3: deriving a nonlinear partial differential equation satisfied by the Nash equilibrium strategy of the kth follower UAV according to optimal control theory and the dynamic programming principle, as shown in equation (4); (4) In equation (4), express transpose, Let represent the Nash equilibrium strategy of the k-th follower drone at time t, and , express transpose, express The reverse, Let represent the optimal value function of the k-th follower drone at time t, and , This represents the operator for finding the minimum value. Describing the optimal value function Compared to gradient, express transpose, Let represent the in-degree of the k-th follower drone, and ; Step C.4: estimating the Nash equilibrium strategy of the kth follower UAV at time t by using equation (5): (5) In formula (5), is a positive definite matrix for the kth follower drone and satisfies ; Step D: Safe set based , the safe control strategy of the leading UAV at time t is designed by using formula (6) ; (6) In formula (6), denotes a normal number, is a control barrier function and is obtained from formula (7), denotes a function of the gradient of ; (7) In formula (7), represents a collision function In is equal to 0, and ; Step E: designing a formation tracking control strategy of a leader UAV; Step E.1: giving an augmented system state according to the state evolution dynamics of all UAVs; According to the tracking error of the leader drone and the local consensus error of the follower drone, a global error augmented vector at time t is defined wherein, denotes the tracking error of the leader drone at time t, denotes the new state of the leader drone at time t, denotes the desired reference trajectory given by the human operator at time t; Step E.2: Design the leader's objective function using equation (8) under the condition that the follower UAVs all take Nash equilibrium strategy so that the leader UAV forms a Stackelberg game under the objective function ​ (8) In equation (8), and Let represent the two positive definite parameter matrices in the objective function of the leader drone. express Dimensional Space Indicating that the leader's drone is in Real-time formation tracking and control strategy, express The global error augmentation vector at time step [time]. express Transpose of; Step E.3: Derive the nonlinear partial differential equation that the leader drone's Stackelberg equilibrium strategy satisfies under the objective function (9) according to the optimal control theory and the dynamic programming principle. (9) In formula (9), represents the optimal formation tracking control strategy of the leader UAV at time , represents the optimal value function of the leader UAV at time , , represents the gradient of the optimal value function with respect to , represents the augmented input matrix, and , represents the weight of the edge , represents the inverse of the matrix ; Step E.4: Constructing the evaluator network and the actuator network consisting of polynomial neural networks to estimate the leader's optimal value function using equations (10) and (11), respectively and the Stackelberg equilibrium strategy ; (10) (11) in formula (10) and formula (11), is the weight vector of the evaluator network at time t, is the weight vector of the actuator network at time t, is a polynomial basis function, denotes the number of hidden layers in the network; The weight vector of the evaluator network at time t is calculated using equations (12) and (13), respectively. update rate and the weight vector of the actuator network at time t update rate : (12) (13) in formula (12) and formula (13), and are two learning rate parameters, is a constant parameter matrix, is a constant parameter, denotes the difference between the polynomial basis function at time t and the polynomial basis function at time t-T, and , denotes the transpose of denotes the gradient of the polynomial basis function with respect to . Step F: Apply control policy to leader drone and apply control policy to each follower drone to achieve human-drone swarm safe interaction motion planning.

2. An electronic device comprising a memory and a processor, characterized in that The memory is configured to store a program supporting the processor to execute the human-UAV swarm safety interaction motion planning method of claim 1, and the processor is configured to execute the program stored in the memory.

3. A computer-readable storage medium having stored thereon a computer program, characterized in that The computer program is configured to execute the steps of the human-UAV swarm safety interaction motion planning method of claim 1 when executed by the processor.

Images