Distance-based trajectory optimization method for distributed formation of intelligent agents

Abstract

The application discloses a kind of multi-agent distributed formation trajectory optimization method based on distance, based on agent dynamics, smoothness and safety, design a kind of considering time's decentralized trajectory planning, put this trajectory representation into multi-agent scene, improve the practicability and safety of formation movement.First, the system mathematical model is established, based on negative gradient control algorithm, a kind of formation control law is proposed.Then a cost function based on graph theory is designed, which quantifies the similarity of the distance weight of each edge in the three-dimensional formation of multiple agents.Finally, combining the advantages of the above formation control method and trajectory optimization, a trajectory optimization method suitable for multi-agent formation is proposed.The application can ensure that the multi-agent completes the task of moving according to the optimized trajectory while maintaining the formation, and the algorithm is simple and easy to implement.

Description

Technical Field

[0001] The present invention relates to a distance-based intelligent agent distributed formation trajectory optimization method, belonging to the technical field of intelligent agent cluster formation control. Background Art

[0002] Distributed formation control involves a multi-agent formation cluster. Faced with diverse environments and tasks, the entire cluster can spontaneously organize and control itself, ensuring collision-free interactions between agents. Within certain rules or frameworks, smooth transitions between formations are achieved. Distributed collaborative formation control primarily addresses three problems: First, the reconstruction of agent formations in randomly distributed environments—that is, the formation of a formation when no formation exists, or the maintenance of a formation structure when the formation is deformed. Second, the switching between formations—adjusting formations in different scenarios. Third, the collision problem between formations during formation reconstruction or transformation. These three problems are addressed by designing a distributed formation control algorithm.

[0003] According to the different control methods, multi-agent formation control can be mainly divided into two categories: relative position-based formation control and relative distance-based formation control. Relative position-based formation means that the desired formation is determined by the desired relative position vector, while relative distance-based formation means that the desired formation is determined by the desired relative distance. The former desired formation has only a single equilibrium point and can be globally stabilized by a linear feedback control strategy. However, this type of control method cannot avoid mutual collisions between autonomous agents. Unlike the former, distance-based formation control can avoid collision problems between autonomous agents. For multi-agents, navigation in a prescribed formation is widely used in various situations. However, the relevant planning strategies usually lack the ability to avoid obstacles in cluttered environments. Summary of the Invention

[0004] To address the aforementioned technical issues, this paper proposes a multi-agent trajectory planning method for maintaining formation in complex environments. Based on agent dynamics, smoothness, and safety, a time-sensitive decentralized trajectory planning approach is designed. This trajectory representation is applied to multi-agent scenarios, improving the practicality and safety of formation motion. The specific technical solutions of this invention are as follows:

[0005] A multi-agent trajectory planning method for maintaining a formation includes the following steps:

[0006] S1: A mathematical model of the system is established and a formation control law is proposed based on the negative gradient control algorithm.

[0007] S2: A graph theory-based cost function is designed to quantify the similarity of the distance weights of each edge in a multi-agent three-dimensional formation.

[0008] S3: Combining the advantages of the above-mentioned formation control method and trajectory optimization, a trajectory planning method suitable for multi-agent formation is proposed.

[0009] Furthermore, the step S1 is specifically as follows:

[0010] S1-1: For a formation control system with three autonomous agents in a plane, the dynamics of each agent is represented by a single integrator:

[0011]

[0012] where r i =[r xi ,r yi ] T represents the position information of the agent i, v i =[v xi ,v yi ] T represents the velocity input from subject i, i=1,2,3; let r ij =r i -r j Represents the relative position of subjects i and j, j = 1, 2, 3 and j ≠ i.

[0013] S1-2: Let a ij =||r ij || 2 represents the actual distance between communicating agents, d ij Represents the expected distance between agents i and j. In this invention, agents i and j, j∈N i The potential energy function expression between is shown as follows:

[0014]

[0015] Among them, V ij represents the potential energy between agents i and j, V i represents the total potential energy of agent i; obviously, when the distance between agent i and all its neighbors reaches the expected value, V i The minimum value is zero; when the distance between agent i and any of its neighbors is zero, V i In addition, define the following equation:

[0016]

[0017] S1-3: In order to achieve the desired global stable formation, a formation control strategy is proposed based on relative position information. Control rate v i As shown in the following formula:

[0018]

[0019]

[0020]

[0021] in, Represents the total potential energy gradient of agent i, the intermediate variable

[0022] Furthermore, the specific process of step S2 is as follows:

[0023] S2-1: The formation of N agents is represented by an undirected graph G = (V, E), where V = {1, 2, ..., N} is the vertex set, is an edge set. In the graph G, vertex i represents the i-th agent, and the position vector Edge e connecting vertex i∈V and vertex j∈V ij ∈E indicates that agents i and j can measure the geometric distance between each other. In this invention, each agent communicates with all other agents, so the formation graph G is complete. Each edge of the graph G is associated with a non-negative weight. Edge e ij The weight of is given by:

[0024] w ij =||p i -p j || 2 ,(i,j)∈ε

[0025] where ||·|| represents the Euclidean norm and indicates the distance between agents.

[0026] S2-2: So the adjacency matrix of graph G Sum degree matrix has been determined, then the corresponding Laplace matrix is ​​given by:

[0027] L=DA

[0028] Using the above matrix, the symmetric normalized Laplacian matrix of graph G is defined as:

[0029]

[0030] in is the identity matrix.

[0031] S2-3: As a graph representation matrix, the Laplacian matrix contains information about the graph structure. To achieve the desired formation clustering, we use a formation similarity distance metric. Specifically, the formation similarity distance metric f is used to quantify the similarity of the distance weights of each edge in the multi-agent three-dimensional formation:

[0032]

[0033] where is the symmetric normalized Laplacian matrix of the current formation formation, the two-norm ||·|| is used to represent the distance metric, represents the Laplacian matrix of the desired formation.

[0034] Further, the specific process of the step S3 is:

[0035] S3-1: The route map is an undirected connected graph of the environment G E = (V E ,E E ) where each vertex v∈V E corresponds to a location in the free configuration space F, and each edge (u,v)∈E E indicates that there is a linear path in F connecting u and v. We also require that there is a vertex corresponding to each start location s i and for each goal location g i there is a vertex Let be a function that returns the location of each vertex. The specific steps are:

[0036] (1) Build a connected graph: randomly sample in the space, collect a series of points, and detect whether they are in the safe space, connect these sampling points to each other according to a certain step size, retain only the connection lines that have no intersection with the obstacles, combine the start point and the target point, and obtain a connected graph;

[0037] (2) Search for the best path: use a graph search algorithm to search for a collision-free path based on the connected graph described above, and if a route from the start point to the end point can be found, it means that there is a feasible motion planning scheme;

[0038] (3) Set the route map constraints to obtain the route map.

[0039] A route map is usually generated to plan the motion of a single robot. If multiple robots use the same route map, there will be additional constraints, as follows.

[0040] (1) Point-to-point constraint: Two agents may not occupy two vertices very close to each other at the same time.

[0041] (2) Edge-to-edge constraint: Two agents cannot move two edges if a collision can occur during movement.

[0042] (3) Edge-to-point constraint: To prevent a collision between one agent and another stationary agent.

[0043] S3-2: We get an environment roadmap that contains the additional conflict set generated in the previous step. At each time interval, the agent can wait at the current vertex or traverse the edge. The discrete schedule p of each agent i It is composed of K+1 position sequences:

[0044]

[0045] The agents should be synchronized in time so that agent i is at waypoint t at interval k At the waypoint and Assume that the agent is in and The line segment between moves and is represented as

[0046] S3-3: In the continuous refinement stage, the present invention discretely calculates the path point sequence p generated by the planner. i Convert to a smooth trajectory f i We use discrete programming to partition the free space F so that each agent solves an independent smooth trajectory optimization problem within a collision-free region. We define time t k = kΔt is assigned to each discrete time step, where Δt is a user-specified parameter. This is an initial guess for T = KΔt. The exact total time T is calculated in the post-processing stage so that all trajectories meet the given physical constraints. The specific steps are:

[0047] The discrete scheduling p i Convert to a smooth trajectory f i :

[0048] Where k represents the order of the curve, t∈[0,1] and starts from 0 and increases to 10. -k The step size increases to 1; the trajectory f i It includes the starting point, the ending point and the control point; the starting point and the ending point are p 0 and p k The control point is the increment of t in the above formula combined with p i The intermediate points will be continuously generated; eventually the dense control points will be connected to obtain a smooth trajectory curve.

[0049] Beneficial effects of the present invention

[0050] 1. The present invention proposes a distributed formation trajectory optimization method for flying in dense environments. A method for evaluating the similarity between the actual distance and the expected distance between the current formation agents is proposed. Based on the distance-based formation control method, the trajectory is created using three stages: roadmap generation with conflict annotations between agents, discrete programming, and continuous optimization. The roadmap generation stage creates a sparse roadmap for a single agent and can be implemented using any existing algorithm. The roadmap is then annotated with generalized conflicts, and the output is a discrete schedule that, if executed, will require the agents to stop frequently. Continuous optimization can find a smooth trajectory for each agent, which facilitates parallelization and improves the performance of the agent cluster.

[0051] 2. Based on practical considerations, this invention considers various constraints such as obstacle avoidance, formation similarity, and dynamic realism according to the formation control problem, thereby improving the safety and practicality of formation control and providing the possibility for further collaboration among multiple intelligent agents based on formation. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 is a flow chart of the method of the present invention;

[0053] Figure 2 It is a simulation experiment diagram of the present invention; DETAILED DESCRIPTION

[0054] In order to more clearly understand the above-mentioned objects, features and advantages of the present invention, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that the embodiments of the present invention and the features therein can be combined with each other without conflict.

[0055] In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention may also be implemented in other ways different from those described herein. Therefore, the scope of protection of the present invention is not limited to the specific embodiments disclosed below.

[0056] The technical solution of the present invention is as follows: first, a system mathematical model is established, and a multi-agent formation control method is designed based on the negative gradient control algorithm; second, a cost function based on graph theory is designed to quantify the similarity of the distance weights of each edge of the multi-agent three-dimensional formation; finally, combining the advantages of the above-mentioned formation control method and trajectory optimization, a trajectory planning method suitable for multi-agent formation is proposed.

[0057] Specifically, if Figure 1 As shown, a multi-agent trajectory planning method for maintaining a formation includes the following steps:

[0058] S1: A mathematical model of the system is established and a formation control law is proposed based on the negative gradient control algorithm.

[0059] S2: A graph theory-based cost function is designed to quantify the similarity of the distance weights of each edge in a multi-agent three-dimensional formation.

[0060] S3: Combining the advantages of the above-mentioned formation control method and trajectory optimization, a trajectory planning method suitable for multi-agent formation is proposed.

[0061] Furthermore, the step S1 is specifically as follows:

[0062] S1-1: For a formation control system with three autonomous agents in a plane, the dynamics of each agent is represented by a single integrator:

[0063]

[0064] where r i =[r xi ,r yi ] T represents the position information of the agent i, v i =[v xi ,v yi ] T represents the velocity input from subject i, i=1,2,3; let r ij =r i -r j Represents the relative position of subjects i and j, j = 1, 2, 3 and j ≠ i.

[0065] S1-2: Let a ij =||r ij || 2 represents the actual distance between communicating agents, d ij Represents the expected distance between agents i and j. In this invention, agents i and j, j∈N i The potential energy function expression between is shown as follows:

[0066]

[0067] Among them, V ij represents the potential energy between agents i and j, V i represents the total potential energy of agent i; obviously, when the distance between agent i and all its neighbors reaches the expected value, V i The minimum value is zero; when the distance between agent i and any of its neighbors is zero, V i In addition, define the following equation:

[0068]

[0069] S1-3: In order to achieve the desired global stable formation, a formation control strategy is proposed based on relative position information. Control rate v i As shown in the following formula:

[0070]

[0071]

[0072]

[0073] Among them, ri V i Represents the total potential energy gradient of agent i, the intermediate variable

[0074] Furthermore, the specific process of step S2 is as follows:

[0075] S2-1: The formation of N agents is represented by an undirected graph G = (V, E), where V = {1, 2, ..., N} is the vertex set, is an edge set. In the graph G, vertex i represents the i-th agent, and the position vector Edge e connecting vertex i∈V and vertex j∈V ij ∈E indicates that agents i and j can measure the geometric distance between each other. In this invention, each agent communicates with all other agents, so the formation graph G is complete. Each edge of the graph G is associated with a non-negative weight. Edge e ij The weight of is given by:

[0076] w ij =||p i -p j || 2 ,(i,j)∈ε

[0077] where ||·|| represents the Euclidean norm and indicates the distance between agents.

[0078] S2-2: So the adjacency matrix of graph G Sum degree matrix has been determined, then the corresponding Laplace matrix is ​​given by:

[0079] L=DA

[0080] Using the above matrix, the symmetric normalized Laplacian matrix of graph G is defined as:

[0081]

[0082] in is the identity matrix.

[0083] S2-3: As a graph representation matrix, the Laplacian matrix contains information about the graph structure. To achieve the desired formation clustering, we use a formation similarity distance metric. Specifically, the formation similarity distance metric f is used to quantify the similarity of the distance weights of each edge in the multi-agent three-dimensional formation:

[0084]

[0085] in is the symmetric normalized Laplace matrix formed by the current group formation, and the two-norm ||·|| is used to represent the distance metric. The Laplacian matrix representing the desired formation.

[0086] Furthermore, the specific process of step S3 is as follows:

[0087] S3-1: The roadmap is environmental G E =(V E ,E E ) is an undirected connected graph where each vertex v∈V E Corresponding to a position in the free configuration space F, each edge (u,v)∈E E This means that there is a linear path connecting u and v in F. We also require that there exists a vertex Corresponding to each starting position s i , and for each target position g i Vertex exists make is a function that returns the position of each vertex. The specific steps are:

[0088] (1) Constructing a connectivity graph: Randomly sample a series of points in the space and check whether they are in the safe space. Connect these sampled points to each other according to a certain step size, and only keep the lines that do not intersect with obstacles. Combine the starting point and the target point to obtain a connectivity graph.

[0089] (2) Search for the best path: Use a graph search algorithm to search for a collision-free path based on the connected graph. If a route from the starting point to the end point can be found, it means that there is a feasible motion planning solution.

[0090] (3) Set roadmap constraints and obtain the roadmap.

[0091] A roadmap is usually generated to plan the motion of a single robot. If multiple robots use the same roadmap, there are additional constraints, as shown below.

[0092] (1) Point-to-point constraint: Two agents may not occupy two vertices that are very close to each other at the same time.

[0093] (2) Edge-to-edge constraint: two agents cannot move two edges if a collision may occur during the movement.

[0094] (3) Edge-to-point constraint: prevents an agent from colliding with another stationary agent.

[0095] S3-2: We get an environment roadmap that contains the additional conflict set generated in the previous step. At each time interval, the agent can wait at the current vertex or traverse the edge. The discrete schedule p of each agent i It is composed of K+1 position sequences:

[0096]

[0097] The agents should be synchronized in time so that agent i is at waypoint t at interval k At the waypoint and Assume that the agent is in and The line segment between moves and is represented as

[0098] S3-3: In the continuous refinement stage, the present invention discretely calculates the path point sequence p generated by the planner. i Convert to a smooth trajectory f i We use discrete programming to partition the free space F so that each agent solves an independent smooth trajectory optimization problem within a collision-free region. We define time t k = kΔt is assigned to each discrete time step, where Δt is a user-specified parameter. This is an initial guess for T = KΔt. The exact total time T is calculated in the post-processing stage so that all trajectories meet the given physical constraints. The specific steps are:

[0099] The discrete scheduling p i Convert to a smooth trajectory f i :

[0100]

[0101] Where k represents the order of the curve, t∈[0,1] and starts from 0 and increases to 10. -k The step size increases to 1; the trajectory f i It includes the starting point, the ending point and the control point; the starting point and the ending point are p 0 and p k The control point is the increment of t in the above formula combined with p i The intermediate points will be continuously generated; eventually the dense control points will be connected to obtain a smooth trajectory curve.

[0102] To verify the effectiveness of the multi-agent trajectory planning method for maintaining formations, simulations and actual flight tests were conducted. The following describes the multi-agent formation control method based on distance and trajectory optimization algorithms in detail, combined with experiments and accompanying figures.

[0103] 1. Introduction to the Experimental Platform

[0104] The platform consists of three quadrotor drones with a 0.6m wheelbase. Each drone is equipped with an NVIDIA NX embedded computer to implement control algorithms for formation trajectory optimization. A GPS module provides positioning data for the drones, and an RTK module further improves positioning accuracy. Both the ground and airborne systems run the Linux operating system to execute code and process user commands, and then transmit the generated virtual trajectory to the target drone.

[0105] 2. Formation Control Experiment

[0106] The experimental platform consists of three quadrotor drones with a 0.6m wheelbase. Each drone is equipped with an NVIDIA NX embedded computer to implement the control algorithm for formation trajectory optimization. A GPS module provides positioning data for the drones, and an RTK module further improves positioning accuracy. Both the ground and airborne systems run the Linux operating system to execute code and process user commands, and then transmit the generated virtual trajectory to the target drone.

[0107] 2. Formation Control Experiment

[0108] The multi-agent simulation experiment is implemented on the matlab platform. The simulation results are as follows: Figure 2 As shown. This experiment designed a spiral upward movement of four intelligent agents maintaining a triangular pyramid formation. Figure 2 The real-time position curves of each agent in the same coordinate system are depicted, showing that the formation maintains stable formation at all times. The physical experiment used three quadrotor drones equipped with onboard computers to achieve stable flight in a triangular formation.

[0109] The above analysis proves the effectiveness of the algorithm proposed in this invention.

[0110] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A distance-based distributed formation trajectory optimization method for intelligent agents, comprising the following steps: Step S1: Establish a mathematical model of the system and propose a formation control rate based on the negative gradient control algorithm; The step S1 specifically includes: S1-1. For a formation control system with three agents in a plane, the dynamics of each agent is represented by a single integrator: Among them, r i =[r xi ,r yi ] T represents the location information of agent i, v i =[v xi ,v yi ] T represents the speed input of agent i, i=1,2,3; let r ij =r i -r j Indicates the relative position of agents i and j; S1-2, let a ij =||r ij || 2 represents the actual distance between communicating agents i and j, j = 1, 2, 3 and ≠ i; d ij Represents the expected distance between agents; the potential energy function expression between agents i and j is as follows: Among them, V ij represents the potential energy between agents i and j, V i represents the total potential energy of agent i; S1-3, propose formation control rate v i : in, Represents the total potential energy gradient of agent i, the intermediate variable Step S2: quantify the similarity of the distance weights of each edge of the multi-agent three-dimensional formation; Step S3: Perform trajectory planning for the multi-agent formation, including: S3-1, roadmap generation, clarifying that multiple agents use the same roadmap constraints; S3-2, discrete programming, obtain the discrete scheduling p of each agent i In step S3-2, discrete scheduling p i It is composed of K+1 position sequences: In the formula, loc(.) represents the coordinates of the path point of agent i, represents a point on the discrete path of agent i, Indicates a waypoint; S3-3, continuous refinement, discrete scheduling p i Convert to a smooth trajectory f i .

2. The method according to claim 1, characterized in that In S2, the step S2 specifically includes: S2-1. The formation of N agents is represented by an undirected graph G = (V, E), where V = {1, 2, ..., N} is the vertex set. is an edge set; in the graph G, vertex i represents the i-th agent, and the position vector Edge e connecting vertex i∈V and vertex j∈V ij ∈E means that agents i and j can measure the geometric distance between each other; each agent communicates with all other agents, so the graph G is complete; each edge of the graph G is associated with a non-negative weight; edge e ij The weight w ij It is given by: Among them, ||·|| represents the Euclidean norm, which indicates the distance between agents; S2-2. The Laplace matrix L of graph G is: L=DA Among them, the adjacency matrix of graph G is Sum degree matrix It has been determined; The symmetric normalized Laplacian matrix L of the graph G is defined as: L=D -1 / 2 LD -1 / 2 =I-D -1 / 2 AD -1 / 2 in, is the identity matrix; S2-3. The Laplace matrix contains information about the graph structure, so the formation similarity distance metric f is used to quantify the similarity of the distance weights of each edge of the multi-agent three-dimensional formation: f=||LL des || 2 Among them, L des The Laplacian matrix representing the desired formation.

3. The method according to claim 1, characterized in that In S3, in step S3-1, the steps for obtaining the route map are: (1) Constructing a connectivity graph: Randomly sample a series of points in the space and check whether they are in the safe space. Connect these sampled points to each other according to a certain step size, and only keep the lines that do not intersect with obstacles. Combine the starting point and the target point to obtain a connectivity graph. (2) Search for the best path: Use a graph search algorithm to search for a collision-free path based on the connected graph. If a route from the starting point to the end point can be found, it means that there is a feasible motion planning solution. (3) Set roadmap constraints and obtain the roadmap.

4. The method according to claim 1, characterized in that In S3, in step S3-1, the roadmap constraints are as follows: (1) Point-to-point constraint: two agents may not occupy two vertices that are very close to each other at the same time; (2) Edge-to-edge constraint: two agents cannot move two edges if a collision is likely during the movement; (3) Edge-to-point constraint: prevents an agent from colliding with another stationary agent.

Images