A distributed spatiotemporal joint trajectory planning method for a swarm of agents

Abstract

A kind of distributed space-time joint trajectory planning method of agent cluster, it is related to robot autonomous control technical field, including: the running trajectory of each agent is planned;Collision conflict information between any agent in cluster and other agents is detected in turn, and is sent to other agents by network broadcasting;Any agent receives collision conflict information, and conflict graph is generated;Based on conflict graph, approximate minimum vertex covering algorithm is used to solve the minimum subset of agent that needs to be re-planned trajectory;The running trajectory of the agent in minimum agent subset is re-planned simultaneously;The steps of collision conflict information detection, conflict graph generation, minimum agent subset construction and re-planning running trajectory are repeated until the number of detected conflict information is zero, and the final running trajectory of each agent is determined;It is used to solve the problems that traditional space-time coupling processing is not good, scalability is poor, strong dependence on front-end path and multi-constraint processing is complex.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot autonomous control, and particularly relates to a distributed space-time joint trajectory planning method for an intelligent agent cluster. BACKGROUND

[0002] Robot intelligent agent clusters have core values in strategic emerging industries, such as the “low-altitude economy”, which specifically include: direct application in urban air transportation, such as logistics distribution and manned flight; rapid survey of disaster areas, accurate delivery of materials, and cooperative search and rescue in the field of emergency response; crop monitoring and precision spraying in smart agriculture; and automatic inspection of large infrastructure, such as bridges, power grids, and wind power plants. In these scenarios, a large number of robot intelligent agents must work cooperatively in crowded and uncertain spaces. Therefore, developing a robust, scalable, and complex dynamic environment capable of ensuring the safety and collaborative efficiency of the cluster is a core technical bottleneck for promoting the low-altitude economy from a blueprint to a commercial reality.

[0003] Currently, the existing technologies in the field of multi-agent trajectory planning can be mainly divided into the following categories:

[0004] (1) Two-stage planning framework (front-end path search + back-end trajectory optimization): This is the most mainstream method.

[0005] Front-end path search: First, use graph search or sampling algorithms such as A* and RRT* to find a preliminary, geometric, and collision-free path in a discrete or continuous representation of the environment. This stage usually assumes that the environment is static.

[0006] Back-end trajectory optimization: Then, use numerical optimization methods (such as Fast-Planner and EGO-Planner) to generate a smooth, time-parameterized, and dynamically constrained trajectory using the path points generated by the front-end as a guide. This stage handles obstacle avoidance, usually by constructing a safe flight corridor (SFC) or using a gradient-based penalty function (such as calculating obstacle distance and gradient using an ESDF map).

[0007] Problem: This separate framework has inherent limitations. The front-end path is strongly dependent on the static environment assumption, which may quickly fail in a dynamic environment, making it difficult for the back-end optimization to make significant space-time adjustments. The results of the back-end optimization are highly dependent on the quality of the front-end path, and if the initial path is far from the optimal solution, it will be difficult to find a globally optimal trajectory.

[0008] (2) Centralized planning:

[0009] Implementation: Aggregate the state and goal information of all agents into a central processor, construct a joint state space, and then perform global unified planning. In theory, the global optimal solution can be found.

[0010] Problem: The computational complexity increases exponentially with the number of agents, and there is a "dimensional disaster" problem, which is not suitable for large-scale clusters. At the same time, the "single point failure" risk of the central node is high, and the robustness is poor.

[0011] (3) Decentralized sequential planning:

[0012] Implementation: Assign a fixed priority order to the agents, and then plan the trajectory for each agent in turn. When planning for the current agent, all planned trajectories of higher priority agents are treated as dynamic obstacles that need to be avoided. For example, EGO-Swarm uses chain planning at system startup.

[0013] Problem: The total planning time increases linearly with the number of agents, and the scalability is poor. The quality of the planning result depends heavily on the pre-set priority order, and it is difficult to guarantee global optimality, and it is easy to cause low-priority agents to be "blocked" and unable to find a path.

[0014] (4) Local reactive obstacle avoidance based on velocity obstacle method:

[0015] Implementation: Each agent selects a safe driving speed by calculating the velocity obstacle space (i.e. the set of velocities that will cause future collisions). This is a distributed local obstacle avoidance method.

[0016] Problem: This method is essentially reactive, lacks long-term planning, and is prone to path oscillation or local optimization (deadlock). The generated trajectory is usually not smooth and is relatively conservative, sacrificing efficiency.

[0017] Therefore, we propose a trajectory planning method suitable for large-scale clusters, capable of handling complex dynamic scenarios, and simplifying calculations. SUMMARY

[0018] The purpose of the present application is to provide a distributed spatiotemporal joint trajectory planning method for agent clusters, which solves the problems of poor traditional spatiotemporal coupling processing, poor scalability, strong dependence on front-end paths, and complex multi-constraint processing.

[0019] The present application is implemented by the following technical solutions:

[0020] A distributed spatiotemporal joint trajectory planning method for agent clusters, specifically comprising:

[0021] Planning the running trajectory of each agent in the agent cluster;

[0022] Based on the running track of each agent, collision conflict information between any agent in the cluster and other agents is detected in turn, and the collision conflict information is sent to other agents through network broadcast;

[0023] After any agent receives the collision conflict information, an independent and decentralized conflict graph is generated;

[0024] Based on the conflict graph, an approximate minimum vertex cover algorithm is used to solve the minimum subset of agents that need to replan the track;

[0025] The agents in the minimum agent subset are simultaneously replanned the running track;

[0026] The steps of collision conflict information detection, conflict graph generation, minimum agent subset construction, and running track re-planning are repeated until the number of detected collision conflict information is zero, and the final running track of each agent is determined.

[0027] Further, the running track of each agent in the planned agent cluster is specifically as follows:

[0028] Set the initial state and the target state of the agent;

[0029] Define a plurality of space-time control points, and the space-time control points include an intermediate space control point, , wherein , and a time period duration , wherein the duration of any time period ;

[0030] Using a standard spline fitting operator , according to the initial state , the target state , and the space-time control points , an initial track is generated;

[0031] Based on the space-time control points and the initial track , a unified cost function is constructed;

[0032] The unified cost function is minimized by an optimization engine to obtain an optimal solution ;

[0033] According to the initial state , the target state , and the optimal solution generating a running trajectory .

[0034] Further, if the trajectory time needs to be precisely controlled, an initial trajectory is generated, and a time allocation proportion vector

[0035] replacing segment duration ;

[0036] In the process of minimizing the unified cost function, the actual segment duration is calculated as:

[0037]

[0038] wherein, is the total time of the desired trajectory.

[0039] Further, the unified cost function is:

[0040]

[0041] wherein, is the main optimization objective, is the unified collision avoidance cost, is the dynamics constraint cost, is the time constraint cost, is the target point reaching cost, , , , and are the weights of the corresponding costs respectively.

[0042] Further, the main optimization objective is the time , and the calculation formula is:

[0043]

[0044] If the main optimization objective is the minimum trajectory energy consumption , the calculation formula is:

[0045] .

[0046] Further, the calculation formula of the unified collision avoidance cost is:

[0047]

[0048] wherein, is an indicator function, is the obstacle at time A three-dimensional space region occupied by the agent, is an obstacle at time A three-dimensional space region occupied by the agent.

[0049] Further, the kinetic constraint cost is calculated by the following formula:

[0050]

[0051]

[0052]

[0053] wherein, is a speed penalty, is an acceleration penalty, and are weights of the speed penalty, and are weights of the acceleration penalty, is a preset maximum speed, is a preset maximum acceleration.

[0054] Further, the target point reaching cost is calculated by the following formula:

[0055]

[0056] wherein, is a position of the trajectory at the termination time , is a position of the target point at the termination time .

[0057] Further, the optimal solution is calculated by the following formula:

[0058]

[0059] wherein, is a unified cost function, is a dimension of the optimization vector , and are lower and upper bounds of the optimization vector , is a population size of the meta-heuristic algorithm, is a maximum iteration number.

[0060] Further, the simultaneous re-planning of the running trajectories of the agents in the minimum agent subset is specifically as follows:

[0061] The trajectory of the agent not selected for re-planning is regarded as a fixed dynamic obstacle;

[0062] with the current optimal solution The running trajectory of the agent is re-planned in combination with the fixed dynamic obstacle as a new spatiotemporal control point.

[0063] The technical solution of the application has at least the following advantages and beneficial effects:

[0064] The application discloses a distributed spatiotemporal joint trajectory planning method for an agent cluster.

[0065] In addition, the close coupling and joint planning in the spatiotemporal dimension are realized by directly optimizing the spatiotemporal control point.

[0066] Due to the global search ability of the meta-heuristic algorithm and the unified cost function guidance, the method can explore a wider solution space and find a high-quality safe trajectory.

[0067] In addition, the unified cost function integrates various complex constraints such as collision with static obstacles, dynamic obstacles, other agents and self-dynamics into a unified optimization objective, greatly simplifying the complexity of system design and multi-constraint processing. DETAILED DESCRIPTION

[0068] Figure 1 A method flowchart of the application is shown in the figure.

[0069] Figure 2 A schematic diagram of accurate time tracking of dynamic target points and space control points and time allocation in a two-dimensional dynamic environment of a single agent is shown in the figure.

[0070] Figure 3 An optimization flowchart of the collision avoidance cost is shown in the figure.

[0071] Figure 4 An optimization flowchart of the dynamics constraint cost is shown in the figure.

[0072] Figure 5 A process diagram of the minimum vertex cover solution based on the maximum degree greedy algorithm is shown in the figure.

[0073] Figure 6 A process display schematic diagram of 20-agent cluster crossing a narrow channel and exchanging positions is shown in the figure.

[0074] Figure 7 A schematic diagram is shown for the minimum spacing curve;

[0075] Figure 8 Schematic diagram of the process of 10 intelligent agents traversing a narrow channel and accurately landing on a recovery platform in 32 seconds;

[0076] Figure 9 Schematic diagram of the process of 100 intelligent agents traversing a dynamic environment and landing on a mobile recovery platform.

[0077] Figure 10 The speed curve diagram of 10 agents uniformly sampled from 100 agents;

[0078] Figure 11 Schematic diagram of a typical experimental scenario for 80,000 experiments. DETAILED DESCRIPTION

[0079] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings herein can be arranged and designed in various different configurations.

[0080] Example 1

[0081] like Figure 1 As shown in FIG, a distributed spatiotemporal joint trajectory planning method for a cluster of intelligent agents specifically includes:

[0082] Plan the trajectory of each agent in the agent cluster;

[0083] In addition, the specific steps of planning the running trajectory of each agent in the agent cluster are as follows:

[0084] The agent The three-dimensional trajectory is parameterized by The present invention adopts the quintic spline to ensure the continuity of high order: velocity, acceleration, jerk, and acceleration are continuous, that is, Continuous; a cubic polynomial can also be used, and the trajectory satisfies the continuity of velocity and acceleration. continuous.

[0085] Then set the initial state of the agent , including position, velocity, acceleration, etc., as well as target state ;

[0086] A plurality of spatio-temporal control points (STP) are defined, and the spatio-temporal control points include one intermediate spatial control point, wherein , and means a real set of three-dimensional space, and one time period duration , wherein the duration of any time period ; the spatio-temporal control point effectively discards the dependence on the front-end static path, and gives the optimizer excellent global adjustment capability, so that it can flexibly cope with complex dynamic scenarios that are difficult to handle by the traditional two-stage method;

[0087] A standard spline fitting operator is used to generate an initial trajectory according to the initial state of the agent , the target state , and the spatio-temporal control point , which is uniquely determined, satisfies all interpolation points, boundary conditions, and is everywhere high-order continuous;

[0088] In addition, in combination with the attached Figure 2 , the spatio-temporal optimization effect of a single agent in a two-dimensional example is shown. Through accurate spatio-temporal joint planning, the spatial shape and speed allocation of the trajectory are adjusted, and the dynamic target point is tracked exactly at 20s in the case of avoiding dynamic and static obstacles;

[0089] In addition, in time-sensitive tasks such as urban logistics, in order to strictly meet the preset total flight time , an accurate time control mechanism can be introduced, that is:

[0090] If the trajectory time needs to be accurately controlled, when generating the initial trajectory , a time allocation ratio vector

[0091] is used to replace the segment duration ;

[0092] When minimizing the unified cost function, the actual segment duration calculation formula is:

[0093]

[0094] In the formula, is the total time of the expected trajectory; in this way, the total time of the expected trajectory is always true, realizing accurate control of the flight time.

[0095] Based on the spatio-temporal control point and the initial trajectory Constructing a unified cost function ;

[0096] The unified cost function is:

[0097]

[0098] Where, As the main optimization goal, To unify the collision avoidance cost, is the dynamic constraint cost, is the time constraint cost, is the cost of reaching the target point, which means that when precise time control is not adopted, this item is used to punish the deviation between the total flight time and the expected time, and , is the cost of reaching the target point, 、 、 、 and are the weights of the corresponding costs respectively.

[0099] The main optimization goals , such as minimizing trajectory time or energy consumption, if it is to minimize time , the calculation formula is:

[0100]

[0101] If the energy consumption of the trajectory is minimized , usually approximated by the integral square of higher-order derivatives, such as minimizing the square integral of jerk, the calculation formula is:

[0102] .

[0103] In addition, the unified collision avoidance cost , which is one of the core of the present invention, will uniformly handle the collision risk with static obstacles, dynamic obstacles and other intelligent bodies. Its value is defined as the penalty path length of the trajectory passing through these obstacles (and their safe expansion zones), such as Figure 3 As shown in Figure 2, this not only determines whether there is a collision, but also provides the optimizer with gradient information about the collision depth, guiding the trajectory to "push away" from the obstacle area; its calculation formula is:

[0104]

[0105] Where, is the characteristic function, which means that when the agent Entering obstacles The space-time occupied area , or enter an obstacle The space-time occupied area , takes 1, otherwise it takes 0, For obstacles At the moment The three-dimensional space area occupied by For obstacles At the moment In addition, to simplify the calculation and ensure the safety margin, in actual collision detection, the volume of the agent itself is usually equivalently attached to the occupied area of ​​the obstacle or other agents.

[0106] As needed, combine with Figure 4 , dynamic constraint cost To ensure that the generated trajectory is physically executable, the calculation formula is:

[0107]

[0108]

[0109]

[0110] Where, For speed penalty, is the acceleration penalty, and are the weights of speed penalty and acceleration penalty respectively, is the preset maximum speed, is the preset maximum acceleration, where is the speed penalty term, if , , there is no punishment; if , , then the penalty value is proportional to the speeding amount; the acceleration penalty term This is consistent with the judgment logic of the speed penalty item.

[0111] In addition, the target point arrival cost , this is to ensure that the agent can reach its designated target point, and in our spatiotemporal joint planning framework, the target point itself can also be dynamic. Assuming that the agent The trajectory of the target point at time is (For static target points, is a constant vector). The cost of reaching the target point can be defined as the trajectory at the end time Location The target point position at this time The Euclidean distance between them is calculated as follows:

[0112]

[0113] where, is the position of the trajectory at the termination time , is the position of the target point at the termination time ; and in practical optimization, if the agent can reach the target point exactly within the preset total time and under the dynamics constraint, it means that the end position constraint can be directly used as the last spatial control point in spline fitting or enforced by end boundary conditions, which can reduce the optimization variables. However, if it cannot be reached exactly due to dynamics limitations or insufficient time, it is still valid as a soft constraint.

[0114] By optimizing engine minimizing the unified cost function, the optimal solution is obtained; wherein the optimization engine is a meta-heuristic optimization algorithm such as artificial bee colony algorithm ABC, gradient optimization algorithm GBO, etc. The engine regards the spatiotemporal control points as a high-dimensional decision vector, and performs global search within the preset search space to minimize the above unified cost function as the goal, and finally gives the optimal solution found;

[0115] and the calculation formula of the optimal solution is:

[0116]

[0117] where, is the unified cost function, is the dimension of the optimization vector , for intermediate spatial control points and time periods (or scaling factors), , and are the lower and upper bounds of the optimization vector , respectively, used to constrain the search range of the spatial control points and the value range of the time periods (or scaling factors), is the population size of the meta-heuristic algorithm, is the maximum number of iterations, which is one of the main termination conditions.

[0118] Under this framework, the optimizer regards the group of spatiotemporal control points as an abstract dimensional vector without directly understanding its specific physical meaning (i.e. spatial points or time). The cost function serves as a "black box" that receives a candidate solution , parses it into specific spatiotemporal control points (or ), then generates a trajectory through a spline fitting operator , and finally computes the integrated cost value of the trajectory.

[0119] Based on the initial state , target state , and optimal solution of the agent, a running trajectory is generated;

[0120] Therefore, the construction of the unified cost function and the optimization process of the optimization engine make the generation of the running trajectory can be regarded as an iterative loop:

[0121] 1. Initialization: Randomly generate a population containing multiple candidate solutions (each solution is a set);

[0122] 2. Evaluation: For each candidate solution, generate a trajectory through spline fitting and calculate its cost function value;

[0123] 3. Update: Update the population according to specific algorithm rules (such as the honeybee search behavior of ABC) to generate better candidate solutions;

[0124] 4. Termination: When the maximum number of iterations is reached or a satisfactory solution that meets all constraints is found, the optimization ends, and the optimal spatiotemporal control points are output, resulting in the final smooth trajectory that is collision-free and dynamically feasible.

[0125] Based on the running trajectory of each agent, the collision conflict information between any agent in the swarm and other agents is detected in turn, and the collision conflict information is sent to other agents through network broadcast;

[0126] Each agent detects whether its initial trajectory and the trajectories of all other agents exist spatiotemporal collision. Then, the agent broadcasts its conflict information (i.e. which agents have collided) to all other agents in the swarm through a low-bandwidth network.

[0127] Upon receiving the collision conflict information, any agent generates an independent and decentralized conflict graph;

[0128] Each agent independently and decentralized builds a same conflict graph after receiving the conflict information of all other agents . Wherein each node of the conflict graph represents an agent, and each edge represents a collision risk between the trajectories of the agents and .

[0129] Based on the conflict graph, an approximate minimum vertex cover algorithm is used to solve the minimum subset of agents whose trajectories need to be replanned;

[0130] That is, the conflict resolution problem is converted into finding an approximate minimum vertex cover (MVC) of the conflict graph G. The MVC is a minimum subset of nodes such that each edge in the graph has at least one endpoint in . In this embodiment, an efficient and deterministic maximum degree greedy algorithm is used to solve the approximate MVC, as shown in Figure 5 :

[0131] (a) Initialize an empty replanning set ;

[0132] (b) Loop: as long as there are edges in the conflict graph, select the node with the largest degree (i.e., the most conflicts) in the current graph to join , and remove all edges connected to . If the degrees are the same, select according to predetermined rules such as agent ID to ensure determinism;

[0133] (c) Loop ends, output .

[0134] Since all agents are based on the same global conflict information and the same deterministic algorithm, they can reach a consistent decision on the replanning set without central coordination.

[0135] Simultaneously replanning the running trajectories of the agents in the minimum subset of agents;

[0136] In addition, the simultaneous replanning of the running trajectories of the agents in the minimum subset of agents includes the following steps:

[0137] The trajectories of the agents not selected for replanning are regarded as fixed dynamic obstacles; that is, the trajectories of the agents not replanned are fixed in space-time, and the space region occupied by them becomes a time-varying forbidden zone;

[0138] With the current optimal solution Replanning the running trajectory of the agent as a new spatiotemporal control point combined with the fixed dynamic obstacle;

[0139] That is, forcing the re-planned agent to generate a new trajectory that completely avoids these forbidden zones, so that the final output of the full cluster trajectory is absolutely collision-free, and the agent forming the original trajectory forms a "moving roadblock". The re-planned agent must detour or adjust the speed to avoid insertion.

[0140] Repeat the steps of collision conflict information detection, conflict graph generation, construction of the smallest agent subset, and re-planning of the running trajectory until the number of detected collision conflict information is zero, and the final running trajectory of each agent is determined.

[0141] Based on the content of the embodiment, an extreme test of "20-agent cluster crossing a narrow channel and exchanging positions" is performed, and the test results are as shown in Figure 6 Figure 7 The minimum distance curve in the test results shows that the agent distance can always be maintained above the safety threshold of 0.3 meters under the condition of ensuring smooth trajectory without excessive twists and turns, proving the strong ability of the application to achieve complex cluster behavior through fine optimization of time and space in extreme crowded and strongly coupled scenarios.

[0142] In addition, Figure 8 and Figure 9 The task of the agent crossing the narrow channel and accurately landing on the recovery platform within 32s is shown, which limits the agent cluster to accurately land on the moving recovery platform in an environment containing a large number of dynamic and static obstacles, wherein Figure 8 (a) is the initial state; (b) is the ordered passage of the cluster through the narrow channel, avoiding dynamic obstacles; (c) is the cluster leaving the channel; (d) is all agents accurately landing on the moving recovery platform at the preset time of 35 seconds; Figure 9 (a) is the agent cluster crossing the complex dynamic obstacle area and approaching the moving recovery platform, (b) is all agents accurately landing on their respective target points on the moving recovery platform at the preset time of 50 seconds; and the trajectory planned by the embodiment enables the agent cluster to successfully complete the task, and through the innovative time allocation ratio optimization mechanism, the preset arrival time (35 seconds and 50 seconds, respectively) is strictly met. As shown in the speed curve of Figure 10 The speed of the agent smoothly decreases to zero at the target time, proving its accurate time control and dynamic target tracking ability.

[0143] In addition, based on the trajectory planning method of the embodiment, 80000 large-scale cluster experiments are performed, and the experimental scene is as shown in Figure 11 The experimental results are shown in Table 1: ​

[0144] Table 1 Performance comparison of 80000 experiments

[0145]

[0146] The data shows that when the number of agents increases from 16 to 100, the average number of iterations required to solve all conflicts only increases slightly from 2.07 to 3.78 (taking MetaPlanner-ABC as an example), and remains at a very low level, and does not appear to be explosive with the size of the cluster. This proves that the method of the present application successfully decouples the planning overhead from the size of the cluster; at the same time, the average initial planning time and the average re-planning time increase with the number of agents, but remain at the millisecond level, and the growth trend is gentle, fully meeting the real-time planning needs of large-scale clusters. The time-consuming of re-planning does not increase significantly, which also proves the effectiveness of the "hot start" mechanism. This is in sharp contrast to the traditional chain planning, which has a linear increase in total time with the number of agents.

[0147] Example 2

[0148] As an example, for the step of generating the initial trajectory, any mathematical tool that can describe a smooth continuous trajectory with a finite set of parameters can be used as an alternative to this step:

[0149] Alternative curve types: In addition to polynomial splines, B-splines, Bézier curves, or NURBS can also be used. These curve types are also defined by a set of control points and have good local control characteristics. By optimizing the positions of their control points, smooth trajectories can also be generated.

[0150] Alternative function bases: Trajectories can also be represented as a linear combination of an orthogonal function basis, such as a Fourier series or Chebyshev polynomials. The optimization process will become optimizing the coefficients before these function bases, which can also generate smooth trajectories that meet the requirements.

[0151] Example 3

[0152] As an example, in Example 1, all constraints are integrated into a single cost function as weighted penalty terms, and there are also a variety of equivalent alternatives:

[0153] Alternative collision cost forms: The present embodiment calculates the length of the trajectory inside the obstacle. Similarly, the collision cost can be defined as:

[0154] The inverse of the minimum distance between the trajectory and the obstacle.

[0155] The maximum depth or volume of the trajectory invading the obstacle.

[0156] Based on the artificial potential field method, obstacles are modeled as repulsive forces, and the cost is the sum of potential energy on the trajectory.

[0157] Alternative constraint handling methods: In addition to the weighted sum of soft constraints, the following methods can also be used:

[0158] Hard constraints: In the optimization process, any solution that violates collision or dynamics constraints is directly assigned an infinite value, thus discarded by the optimizer.

[0159] Lexicographic or hierarchical optimization: Constraints are prioritized, for example, first find a set of solutions that satisfy safety (no collision) among all solutions, then find a solution that satisfies dynamics constraints in this subset, and finally optimize the main objective such as time or energy consumption.

[0160] Embodiment 4

[0161] As an embodiment, for the initial trajectory planned in Embodiment 1, which needs to be optimized, the artificial bee colony algorithm (ABC) and gradient optimizer (GBO) can be used as optimizers. However, any optimization algorithm with global or local search capability in complex high-dimensional space can be used to solve the cost function proposed in this invention. Therefore, other evolutionary algorithms such as genetic algorithm (Genetic Algorithm, GA), differential evolution algorithm (Differential Evolution, DE), particle swarm optimization algorithm (Particle Swarm Optimization, PSO) can also be used. These algorithms follow the iteration paradigm of "population initialization-evaluation-update" and can seamlessly replace the optimizer in this embodiment.

[0162] Other random search algorithms: such as simulated annealing (Simulated Annealing, SA), covariance matrix adaptive evolution strategy (CMA-ES), etc. They explore the solution space through different random strategies and are also suitable for solving the "black box" cost function of this invention.

[0163] Embodiment 5

[0164] As an embodiment, for Embodiment 1, the conflict graph and approximate minimum vertex cover (MVC) are used to determine the re-planning set. This decentralized decision-making mechanism also has multiple alternatives:

[0165] Alternative graph theory problem model: The conflict resolution problem can be modeled not only as MVC, but also as other graph theory problems, for example, maximum independent set: finding the largest set of conflict-free agents in the graph, while the agents outside this set need to be re-planned.

[0166] Minimum dominating set: Find the minimum number of agents that can "cover" all other agents' conflicts.

[0167] Alternative approximation algorithm: instead of the maximum-degree greedy algorithm, other efficient deterministic or randomized approximation algorithms can be used, such as the classic 2-approximation algorithm (i.e., constantly select an edge and add both of its endpoints to the cover).

[0168] Alternative decentralized decision mechanism: instead of graph theory, other distributed coordination mechanisms can be used to decide the replanning agent, for example, a strategy based on dynamic priority: all agents calculate a dynamic priority according to a globally consistent rule (e.g., distance to goal, ID size, energy remaining, etc.). When a conflict occurs, the agent with lower priority is responsible for replanning to avoid.

[0169] Mechanism based on negotiation or auction: conflicting agents can negotiate or bid through a simple communication protocol to decide who has priority on the path, and the loser then replans.

[0170] The above merely describes the preferred embodiments of the present application and is not used to limit the present application. Various modifications and changes can be made by those skilled in the art based on the spirit and principles of the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A method for distributed spatiotemporal joint trajectory planning of a swarm of agents, the method comprising: Specifically comprising: planning the running trajectories of the agents in the agent cluster; The specific steps are: Setting the initial state of the agent and the goal state ; A plurality of spatiotemporal control points are defined, and the spatiotemporal control points include one intermediate spatial control point , wherein , and one time period duration , wherein the duration of any time period ; using standard spline fitting operators , according to the initial state of the agent , the target state , and the space-time control points , an initial trajectory is generated ; spatiotemporal control points and initial trajectory constructing a unified cost function ; By optimizing the engine Minimizing the unified cost function to get the optimal solution ; According to the initial state of the agent , the target state , and the optimal solution , a running track is generated; Based on the running trajectories of the agents, the collision conflict information between any agent and other agents in the cluster is detected in turn, and the collision conflict information is broadcasted to other agents through the network; After any agent receives the collision conflict information, an independent and decentralized conflict graph is generated; Based on the conflict graph, an approximate minimum vertex cover algorithm is used to solve the minimum subset of agents that need to be re-planned trajectories; The running trajectories of the agents in the minimum agent subset are re-planned simultaneously; The steps of collision conflict information detection, conflict graph generation, minimum agent subset construction, and running trajectory re-planning are repeated until the number of detected collision conflict information is zero, and the final running trajectory of each agent is determined.

2. The method of claim 1, wherein: If precise control of the trajectory time is required, then an initial trajectory is generated The time allocation proportion vector is used The segment duration is replaced ; In the minimization of the unified cost function, the actual duration calculation formula of each segment is: In the formula, is the total time of the desired trajectory.

3. The method of claim 1, wherein: The unified cost function is: wherein is the main optimization objective, is the uniform collision avoidance cost, is the dynamics constraint cost, is the time constraint cost, is the goal point reaching cost, , , , and are the weights of the respective costs.

4. The method of claim 3, wherein: Main optimization objective , if minimizing time , the calculation formula is: If the trajectory energy consumption is to be minimized The calculation formula is: 。 5. The method of claim 3, wherein: The unified collision avoidance cost The formula for calculating is: wherein is an indicator function, is an indicator function, is an indicator function, is an indicator function, is an indicator function, is an indicator function.

6. The method of claim 3, wherein: Kinetic constraint cost The formula for calculating the kinetic constraint cost is: wherein is a speed penalty, is an acceleration penalty, and are weights for the speed penalty, and are weights for the acceleration penalty, is a preset maximum speed, is a preset maximum acceleration.

7. The method of claim 3, wherein: The target point reaching cost The calculation formula is: wherein is the position of the trajectory at the end time , is the position of the target point at the end time . 8.The method of claim 1, wherein: the optimal solution The calculation formula is: Where, is a unified cost function, is the optimization vector x i = Q i T i Dimensions, and They are the optimization vectors The lower and upper bounds of is the population size of the metaheuristic algorithm, is the maximum number of iterations. 9.The method of claim 1, wherein: The specific steps of re-planning the running trajectories of the agents in the minimum agent subset are: The trajectories of the agents not selected for re-planning are regarded as fixed dynamic obstacles; with the current optimal solution Replan the trajectory of the agent as a new spatiotemporal control point combined with the fixed dynamic obstacles.

Images