Multi-agent formation dynamic path planning obstacle avoidance method, system, equipment and medium

Abstract

The invention belongs to the field of multi-agent control, and provides a multi-agent formation dynamic path planning obstacle avoidance method, system and device and a medium, and the method comprises the steps: determining a navigator positioning coordinate on an environment grid map; performing node optimization on the path to obtain a global path by taking the minimum cost estimation of the starting point of the path from the current point to the target point as a target based on the environment grid map; based on a formation mode, determining a plurality of follower positioning coordinates through the navigator positioning coordinates; continuously determining real-time sub-target nodes of the follower on the global path; determining a speed sampling space according to the boundary speed and acceleration capability of the follower and an environmental obstacle, and performing trajectory prediction between the follower and the real-time sub-target node in the speed sampling space to obtain a plurality of trajectory prediction results; and evaluating the plurality of trajectory prediction results to obtain an optimal dynamic obstacle avoidance path of the multi-agent formation. According to the invention, efficient path planning and formation stability control in a complex environment are realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-agent control, and particularly relates to a multi-agent formation dynamic path planning and obstacle avoidance method, system, device and medium. Background Art

[0002] The statements in this part only provide background technical information related to the present invention, and do not necessarily constitute prior art.

[0003] With the rapid development of autonomous driving technology, the application prospect of intelligent vehicle fleets is becoming increasingly broad. In the fields of urban transportation, logistics transportation, and unmanned driving services, intelligent vehicle formations are gradually becoming an important technical means to solve traffic efficiency and energy problems due to their synergy, high efficiency, and safety. At the same time, formation technology can also optimize the cargo delivery path. For example, Amazon's unmanned delivery fleet has increased the last-mile delivery efficiency by more than 30% through dynamic formation scheduling. In urban traffic scenarios, intelligent vehicle formations can cooperate with traffic lights to achieve "green wave passing", reducing the number of stops and congestion time. Research shows that a cooperative formation system can improve the traffic efficiency of urban arterial roads by 25% and reduce carbon emissions by 30% at the same time.

[0004] However, the large-scale application of intelligent vehicle formations still faces multiple challenges. When using robot control algorithms to achieve intelligent agent motion planning and behavior decision-making, it is necessary to comprehensively consider formation control to achieve cooperative motion, while multi-intelligent cluster technology can be used to achieve cooperative motion between multi-agents. Multi-intelligent cluster technology refers to multiple autonomous or semi-autonomous intelligent agents that complete complex tasks through cooperative cooperation. Its core lies in distributed decision-making, information sharing, and cooperative control, and is applicable to scenarios such as logistics distribution and disaster relief; for example, UAV formations can synchronously complete precise spraying of large areas of farmland in agricultural pest control; however, there are still problems such as communication delay, task allocation conflict, and inadaptability to dynamic environments during the intelligent vehicle formation process. The traditional leader-follower formation control does not couple trajectory and formation stability constraints and only relies on PID control, which cannot guarantee that the formation shape does not change. The evaluation function weights of the traditional dynamic window method are fixed and do not optimize formation cooperation, resulting in the trajectory tending to be locally optimal rather than in the overall interest. Summary of the Invention

[0005] To solve the above problems, the present invention proposes a multi-agent formation dynamic path planning and obstacle avoidance method, system, device and medium. The present invention is based on the dynamic programming of multi-agent robots and completes the dynamic path planning and obstacle avoidance algorithms through the leader-follower formation algorithm.

[0006] According to some embodiments, the first solution of the present invention provides a multi-agent formation dynamic path planning and obstacle avoidance method, adopting the following technical solutions: Multi-agent formation dynamic path planning and obstacle avoidance method, including: Based on multiple agents to be formed, determine a leader and multiple followers, and determine the positioning coordinates of the leader on the environmental grid map; Based on the environmental grid map, with the goal of minimizing the cost estimation between the starting point, the current point, and the target point of the path, optimize the nodes of the path to obtain the global path; Based on the formation method, determine the positioning coordinates of multiple followers through the positioning coordinates of the leader; According to the positioning coordinates of the followers and the leader, continuously determine the real-time sub-goal nodes of the followers on the global path; According to the boundary speed, acceleration ability of the followers and the environmental obstacles, determine the speed sampling space, and perform trajectory prediction between the followers and the real-time sub-goal nodes in the speed sampling space to obtain multiple trajectory prediction results; Evaluate multiple trajectory prediction results to obtain the optimal dynamic obstacle avoidance path of the multi-agent formation.

[0007] Further, the step of, based on the environmental grid map, with the goal of minimizing the cost estimation between the starting point, the current point, and the target point of the path, optimizing the nodes of the path to obtain the global path is specifically as follows: Based on the positioning coordinates of the leader on the environmental grid map, determine the set of points to be inspected and the set of points that have been inspected on the path; In the set of points to be inspected, sequentially calculate the cost estimation between the starting point, the current point, and the target point until reaching the target point; According to the cost estimation results of different nodes in the set of points to be inspected, select the node corresponding to the minimum sum of all cost estimation results as the global path.

[0008] Further, the cost estimation between the starting point, the current point, and the target point is the sum of the Euclidean distance from the starting point to the current point and the Euclidean distance from the current point to the target point.

[0009] Further, the formation methods include fixed-direction formation mode and target-direction formation mode; The fixed-direction formation mode is that the formation adopts fixed relative position parameters in the same coordinate system. The followers change according to the change of the motion state of the leader and will perform dynamic obstacle avoidance according to the position of the obstacles; The target-direction formation mode is to calculate in real time the formation position in the small coordinate system with the head pointing direction as the positive direction through the direction of the main vehicle in the overall coordinate system during the movement process, that is, the relevant formation parameters, and map them to the real-time formation parameters in the overall coordinate system.

[0010] Further, predicting trajectories between the follower and the real-time sub-goal nodes in the velocity sampling space to obtain multiple trajectory prediction results specifically includes: Discretizing the velocity sampling space to obtain multiple pairs of linear velocity and angular velocity; Traversing each pair of velocities, substituting them into the motion model of the follower to simulate the trajectory for trajectory prediction; Obtaining the trajectory prediction results corresponding to multiple pairs of velocities.

[0011] Further, evaluating the multiple trajectory prediction results to obtain the optimal dynamic obstacle avoidance path for the multi-agent formation specifically includes: Calculating each trajectory prediction result according to the trajectory evaluation function to obtain multiple trajectory evaluation results; Taking the trajectory evaluation result with the minimum value as the optimal dynamic obstacle avoidance path for the multi-agent formation.

[0012] Further, the trajectory evaluation function specifically includes: ; where are the coefficients of each sub-evaluation function, is the direction angle sub-evaluation function, is the velocity sub-evaluation function, is the distance sub-evaluation function.

[0013] According to some embodiments, the second solution of the present invention provides a multi-agent formation dynamic path planning and obstacle avoidance system, which adopts the following technical solutions: The multi-agent formation dynamic path planning and obstacle avoidance system includes: A leader positioning coordinate determination module, configured to determine a leader and multiple followers based on multiple agents to be formed, and determine the leader positioning coordinates on the environmental grid map; A global path determination module, configured to optimize the nodes of the path based on the environmental grid map with the goal of minimizing the cost estimation between the starting point, the current point, and the target point of the path to obtain the global path; A follower positioning coordinate determination module, configured to determine multiple follower positioning coordinates based on the formation method through the leader positioning coordinates; A real-time sub-goal node determination module, configured to continuously determine the real-time sub-goal nodes of the followers on the global path according to the follower positioning coordinates and the leader positioning coordinates; A trajectory prediction module, configured to determine the velocity sampling space according to the boundary velocity, acceleration ability of the followers and environmental obstacles, and predict the trajectories between the followers and the real-time sub-goal nodes in the velocity sampling space to obtain multiple trajectory prediction results; The optimal obstacle avoidance path determination module is configured to evaluate multiple trajectory prediction results to obtain the optimal dynamic obstacle avoidance path for the multi-agent formation.

[0014] According to some embodiments, a third aspect of the present invention provides a computer-readable storage medium.

[0015] A computer-readable storage medium stores a computer program, and when the program is executed by a processor, it implements the steps in the multi-agent formation dynamic path planning and obstacle avoidance method described in the first aspect above.

[0016] According to some embodiments, a fourth aspect of the present invention provides a computer device.

[0017] A computer device includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the steps in the multi-agent formation dynamic path planning and obstacle avoidance method described in the first aspect above.

[0018] Compared with the prior art, the beneficial effects of the present invention are as follows: The present invention realizes efficient path planning and formation stability control in a complex environment through a hierarchical collaborative architecture. First, a formation control model is constructed based on the leader-follower strategy, and fixed-direction or target-direction formation modes are realized by defining the relative pose constraints of the followers. The global path planning layer uses the A* algorithm to generate an initial optimal path, and the computational complexity of local planning is reduced through a redundant node deletion strategy and smoothness. Between adjacent nodes, the dynamic window approach (DWA) is used as the core of local path planning to generate multiple sets of candidate trajectories through dynamic constraints in the velocity space, and the optimal obstacle avoidance trajectory is selected based on a multi-objective evaluation function. To avoid formation deformation caused by obstacle avoidance, a follower pose correction algorithm based on the Lyapunov stability theory is proposed to adjust the velocity command and steering angle of the follower in real time to ensure the consistency of the formation geometry during dynamic obstacle avoidance. In addition, the state information of the leader and followers is synchronized through a distributed communication framework, and a timestamp verification and data redundancy transmission mechanism is used to solve the communication delay problem. Simulations and experiments show that the algorithm not only ensures the global optimality of the path but also significantly improves the obstacle avoidance efficiency and formation robustness of the multi-agent system in a dynamic obstacle environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] The accompanying drawings forming a part of this specification are used to provide a further understanding of the present invention. The schematic embodiments and descriptions thereof of the present invention are used to explain the present invention and do not constitute an improper limitation of the present invention.

[0020] Figure 1 It is a flowchart of a multi-agent formation dynamic path planning and obstacle avoidance method in an embodiment of the present invention; Figure 2 It is an example diagram of the experimental platform in the embodiment of the present invention; Figure 3 It is an introduction diagram of the chassis of the experimental platform in the embodiment of the present invention; Figure 4 It is an example diagram of the robot turning in the embodiment of the present invention; Figure 5 It is an example diagram of the fixed - direction formation and the target - direction formation in the embodiment of the present invention; Figure 6 It is an example coordinate diagram of the target - direction formation in the embodiment of the present invention. Detailed implementation manners

[0021] The present invention will be further described below in conjunction with the drawings and embodiments.

[0022] It should be noted that the following detailed description is exemplary and is intended to provide further illustration of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs.

[0023] It should be noted that the terms used herein are only for describing the specific implementation manners and are not intended to limit the exemplary embodiments of the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0024] In the case of no conflict, the embodiments in the present invention and the features in the embodiments can be combined with each other.

[0025] Embodiment 1 As Figure 1As shown in the figure, this embodiment provides a multi-agent formation dynamic path planning and obstacle avoidance method. This embodiment takes the application of this method to a server as an example for illustration. It can be understood that this method can also be applied to a terminal, and can also be applied to a system including a terminal and a server, and is implemented through the interaction between the terminal and the server. The server can be an independent physical server, or a server cluster or distributed system composed of multiple physical servers, or a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, web servers, cloud communications, middleware services, domain name services, security services CDN, and big data and artificial intelligence platforms. The terminal can be a smart phone, a tablet computer, a notebook computer, a desktop computer, a smart speaker, a smart watch, etc., but is not limited thereto. The terminal and the server can be directly or indirectly connected through wired or wireless communication methods, and this application does not limit this. In this embodiment, the method includes the following steps: Based on multiple agents to be formed into a formation, determine a leader and multiple followers, and determine the positioning coordinates of the leader on the environmental grid map; Based on the environmental grid map, with the goal of minimizing the cost estimate between the starting point, the current point and the target point of the path, optimize the nodes of the path to obtain the global path; Based on the formation method, determine the positioning coordinates of multiple followers through the positioning coordinates of the leader; According to the positioning coordinates of the followers and the positioning coordinates of the leader, continuously determine the real-time sub-goal nodes of the followers on the global path; According to the boundary speed, acceleration ability of the followers and environmental obstacles, determine the speed sampling space, and perform trajectory prediction between the followers and the real-time sub-goal nodes in the speed sampling space to obtain multiple trajectory prediction results; Evaluate multiple trajectory prediction results to obtain the optimal dynamic obstacle avoidance path for the multi-agent formation.

[0026] This embodiment aims to solve the deficiencies of the current technology and proposes a multi-agent leader formation dynamic path planning and obstacle avoidance algorithm. By calculating the position and speed information of the following vehicle based on the state of the position and speed of the leader, and performing dynamic position adjustment through the DWA algorithm to perform autonomous obstacle avoidance.

[0027] To achieve the above objectives, the following technical solutions are adopted.

[0028] Step 1: Build an experimental platform; Step 2: Implement a quadratic programming algorithm for a single vehicle based on the A* and DWA algorithms; Step 3: Implement fixed azimuth formation and target direction formation based on leader formation; For Step 1, a wheeled mobile robot equipped with a 16-line lidar is used to construct the basic kinematic model of the experimental platform; First, the kinematic constraints of the wheeled robot are used to construct the kinematic model. Assume that: 1. The vehicle is a rigid system; 2. The origin of the local coordinate system of the vehicle is located at the center of the rear axle of the vehicle; 3. The vehicle speed changes slowly, and there is no load transfer between the front and the rear.

[0029] As Figure 2 shown, a stationary reference frame S in space and a reference frame moving with the vehicle on the vehicle body are defined. Assume the position of the robot is , where is the orientation angle, and are the positions of the robot in the reference frame S respectively. In the figure, is the geometric center of the robot. Assume the radius of the tire is , and the distance from the wheel to the center of the vehicle body is .

[0030] The motion of the mobile robot satisfies the following constraints: (1); where is the velocity component of the robot center along the y-axis, and is the velocity component of the robot center along the x-axis.

[0031] Writing the above formula in the form of Pfaffian constraint , we can get: (2); where is the pose vector, is the velocity vector of the robot, and is the non-holonomic constraint matrix.

[0032] The right null space of and can be represented by two basis vectors (3); The constraint equation (1) is non-holonomic. When the velocity control quantity is used as the input, the kinematic model is: (4); where is the angular velocity, is the linear velocity, is the dimension of the robot state vector, is the number of constraint equations.

[0033] In the actual situation, the differential drive robot uses the right drive wheel speed and the left drive wheel speed as the speed control input quantity. Therefore, from the robot kinematic model, it can be known that: (5); where the distance from the wheel to the center of the vehicle body is .

[0034] In the process of trajectory planning, the kinematic model is used to satisfy the geometric constraints of the wheeled robot, and the trajectory is relatively smooth. However, the kinematic model does not consider the influence of the force and torque of the robot on its motion and does not meet the requirements of vehicle body stability. The Euler-Lagrange method is used to perform dynamic modeling and analysis of the vehicle.

[0035] A mobile vehicle system with dimensional configuration space and non-holonomic constraints can be represented as: (6); where, is the pose vector in the vehicle system, is the system symmetric positive definite inertia matrix, is the centrifugal force and Coriolis force matrix, is the gravity matrix, is the input transformation matrix, is the input vector, is the non-holonomic constraint matrix of the system, is the system constraint force vector. Since the vehicle trajectory in this embodiment is constrained to the horizontal plane, the gravity term is zero.

[0036] According to , it can be converted to: (7); where, is the equivalent mass matrix under the constraint system, is the input control matrix, is the equivalent Coriolis force and centrifugal force matrix under the constraint system; is the derivative of the Lyapunov function with respect to time, is the rate of change of the state function with respect to time.

[0037] Therefore, the kinematic equation of the vehicle under non-holonomic constraints is simplified to: (8); where, , , .

[0038] Building an experimental platform requires multi-sensing of radar + vision + voice and real-time computing of complex algorithm models. Ordinary computing platforms are difficult to meet the corresponding requirements. Finally, a 2-3 layer experimental platform is built. The chassis of the experimental platform is as Figure 3 shown. A vision module is set at the position of the upper chassis close to the front of the vehicle. A 16-line radar is arranged next to the vision module. An upper computer emergency stop button and a lower computer emergency stop button are arranged on both sides of the 16-line radar; the upper chassis also includes a lithium battery pack, and a charging port and a main switch are arranged on both sides of the lithium battery pack; a front universal wheel is arranged at the position of the lower chassis close to the front of the vehicle, and a USB splitter is arranged on one side of the front universal wheel; a power supply module and an STM32 control board are also arranged on the lower chassis. The power supply module is connected to the STM32 control board, and the STM32 control board is also connected to the USB splitter. The motors of the two rear wheels of the lower chassis are used to drive the movement of the chassis, and the dual motors are controlled by the STM32. By receiving the chassis control parameters given by the computing platform, they are used to control the motor actions and return the parameter status, and a display screen is equipped to display the chassis status information such as battery voltage in real time.

[0039] The experimental platform of this embodiment adopts a distributed system framework. Each robot plans its own trajectory according to the local information it obtains, and constraints are imposed between robots during the planning process to maintain the formation; the distributed communication method of the trolley is completed by using the Flask framework. The specific implementation method is as follows: The master vehicle starts the Flask service. Deploy the Flask application on the master vehicle as the central node of communication, and be responsible for providing an interface for status information for the following trolleys to read.

[0040] The follower makes requests at specified time intervals. The following trolley accesses the Flask service of the master vehicle through an HTTP request in the local area network or through server forwarding to obtain the latest status information of the master vehicle.

[0041] Anti-redundancy and delay judgment of additional information. Each time the status information is sent, the current timestamp is attached. Before processing the data at the receiving end, compare the timestamps to ensure that the latest data is processed, and avoid affecting the system performance due to outdated information caused by delays. If there is a delay due to network communication or other problems, the previous data is still used, and the request rate is appropriately increased.

[0042] Before performing multi-agent path planning, first use a visualization tool to complete the map establishment through a lidar, construct a map model with complete environmental information, and convert it into a grid map. Based on this, a feasible path is planned.

[0043] For step 2, the A* algorithm is used to determine the global path of the leader. By adopting the strategy of deleting redundant nodes, the number of times of path planning by DWA is reduced, and the calculation efficiency is improved. Under the guidance of the global path, it can be ensured that the formation can reach the end smoothly without falling into the local optimum.

[0044] Based on the environmental grid map, with the goal of minimizing the cost estimate from the starting point of the path through the current point to the target point, the path is optimized for nodes to obtain the global path. Specifically: Based on the positioning coordinates of the leader on the environmental grid map, the set of points to be inspected and the set of points that have been inspected on the path are determined; In the set of points to be inspected, the cost estimate from the starting point through the current point to the target point is calculated in turn until the target point is reached; According to the cost estimate results of different nodes in the set of points to be inspected, the node corresponding to the minimum sum of all cost estimate results is selected as the global path.

[0045] The A* algorithm is used to determine the global path of the leader. With its efficient heuristic search characteristics, the A* algorithm provides a reliable navigation solution for multi-agent systems. By effectively deleting redundant nodes in the path, the number of calculations in the path planning process by the Dynamic Window Approach (DWA) is significantly reduced, thus improving the overall calculation efficiency. The path planning process is optimized. Under the guidance of the global path, the formation can reach the predetermined end point smoothly and avoid falling into the local optimal solution.

[0046] The A* algorithm uses a path quality evaluation function as follows: (9); Where is the cost estimate from the starting point through the current point to the target point, is the cost estimate from the starting point to the current point, is the cost estimate from the current point to the target point. The A* algorithm needs to maintain two state tables and . Where is composed of nodes to be inspected, is composed of nodes that have been inspected.

[0047] The cost estimate from the starting point through the current point to the target point is the sum of the Euclidean distance from the starting point to the current point and the Euclidean distance from the current point to the target point; The Euclidean distance is used as the cost estimate calculation. The cost estimate of point I is: (10); Where , , The coordinate information of the current point, the initial point, and the target point respectively. Here, is the number of nodes.

[0048] Put the nodes with smaller values into , and then continue the iterative calculation until the target node is reached.

[0049] The core idea of the DWA algorithm is to simulate the trajectory of a wheeled robot according to its current position and speed states, evaluate the trajectory through an evaluation function, and finally select the speed corresponding to the trajectory with the best evaluation as the movement speed of the mobile robot. This process is repeated until the mobile robot reaches the target point.

[0050] As Figure 4 shown, the kinematic model corresponding to the wheeled robot is as follows: (11); Among them, is the pose change of the robot in the x direction according to the speed input, is the pose change of the robot in the x direction according to the speed input.

[0051] (12); Among them, , , are the coordinates and direction angles in the direction at time respectively, is the time interval. is the same as the robot's orientation at the current moment mentioned before, refers to the simulation of the robot's direction at the current moment.

[0052] Perform trajectory prediction between the follower and the real-time sub-goal node in the speed sampling space, and obtain multiple trajectory prediction results. Specifically: Discretize the speed sampling space to obtain multiple sets of speed pairs composed of linear velocity and angular velocity; Traverse each set of speed pairs, substitute them into the follower's motion model to simulate the trajectory for trajectory prediction; Obtain the trajectory prediction results corresponding to multiple speed pairs.

[0053] The steps of the DWA algorithm for local path planning are as follows: 1. Speed sampling; Due to the limitations of the wheeled robot's vehicle hardware, structure, and environment, etc., there are certain constraints in the speed sampling space: Boundary velocity constraint: Affected by the hardware structure of the wheeled robot, it is restricted by its maximum and minimum speeds. The available speed sampling space at this time is as follows: (13); Among them, are the minimum linear velocity and the maximum linear velocity of the vehicle respectively, are the minimum angular velocity and the maximum angular velocity of the vehicle respectively.

[0054] Acceleration ability constraint: Due to the limitations of the drive motors of the wheeled robot, there are boundary limitations for the linear acceleration and angular acceleration of the vehicle. The available speed sampling space at this time is: (14); Among them, and represent the current linear velocity and angular velocity respectively, and are the maximum linear acceleration and the maximum angular acceleration of the vehicle respectively.

[0055] Environmental obstacle limitation: To ensure safety, the wheeled robot does not collide with surrounding obstacles. Under the condition of maximum acceleration, the available speed sampling space is: (15); Among them, represents the shortest distance between the trajectory and the obstacle. Under the condition of no obstacle, is a very large constant. When the wheeled robot operates near the obstacle, it can achieve safe deceleration until it avoids the obstacle under the constraint of the maximum deceleration.

[0056] Combining the above constraint conditions, the final speed sampling space is expressed as: (16).

[0057] 2. Trajectory prediction; For the determined speed sampling space perform discretization to obtain multiple sets of speeds , and by traversing each set of speeds, substitute them into the motion model of the wheeled robot to simulate the trajectory for trajectory prediction.

[0058] 3. Trajectory evaluation; Evaluate the multiple trajectory prediction results to obtain the optimal dynamic obstacle avoidance path for multi-agent formation, specifically: Calculate each trajectory prediction result according to the trajectory evaluation function to obtain multiple trajectory evaluation results; The trajectory with the smallest evaluation result is taken as the optimal dynamic obstacle avoidance path for the multi-agent formation.

[0059] After trajectory prediction, the multiple groups of sampled trajectories are evaluated and selected. The optimal trajectory is obtained by comparing the scores through standard evaluation trajectories. The evaluation function is: (17); Among them, is the initial direction angle evaluation function, is the initial velocity sub-evaluation function, is the initial distance evaluation function. are the coefficients of each sub-evaluation function. Since the collected data are of different orders of magnitude, in order to avoid the value of one of them being too large and accounting for too large a proportion, normalization operation is performed.

[0060] (18); (19); (20); in, For the The sampling tracks, is the total number of sampled trajectories.

[0061] To sum up, the final evaluation function is: (twenty one); in, are the coefficients of each sub-evaluation function, is the direction angle evaluation function, is the speed sub-evaluation function, is the distance evaluation function.

[0062] This combination of A* algorithm and DWA provides a reliable dynamic path planning solution for multi-agent formations. Through precise path guidance and real-time obstacle avoidance, the entire formation can move forward confidently in complex and changing environments, ensuring that it reaches the intended target efficiently and safely. The effectiveness of this method provides important theoretical support and practical reference for the future application of multi-agent systems.

[0063] Traditional global planning algorithms such as A*, Dijstra and RRT algorithms are fast and efficient, but lack the function of real-time obstacle avoidance and have a minimum window size that determines the smoothness of the planned path, which may affect the normal path driving of the intelligent agent to a certain extent. Therefore, the quadratic planning algorithm DWA is introduced.

[0064] For Step 3, first determine the formation method, and obtain the position information of the followers from the position information of the leader. Continuously determine the sub-goal points on the nodes of the global path, judge whether the selected sub-goal points are the end points, and continuously iterate until reaching the end point.

[0065] The formation methods include the fixed-direction formation mode and the target-direction formation mode; The fixed-direction formation mode is that the formation adopts fixed relative position parameters in the same coordinate system, and the followers change according to the change of the leader's motion state, and will perform dynamic obstacle avoidance according to the position of the obstacles; The target-direction formation mode is to calculate the formation position in the small coordinate system with the head pointing direction as the positive direction in real time through the direction of the host vehicle in the overall coordinate system during the movement process, that is, the relevant formation parameters, and map it to the real-time formation parameters in the overall coordinate system.

[0066] In this embodiment, two modes are used for formation control. The first mode is the fixed-direction formation mode, that is, the formation adopts fixed relative position parameters in the same coordinate system, and the followers completely change according to the change of the leader's motion state, but will perform dynamic obstacle avoidance according to the position of the obstacles, and can be applied to the scenario of lateral movement formation. The second mode is the target-direction formation, that is, through the direction of the host vehicle in the coordinate system during the movement process, that is, the relevant formation parameters, calculate the formation position in the small coordinate system with the head pointing direction as the positive direction in real time, and map it to the real-time formation parameters in the overall coordinate system, which can be applied to more flexible formation scenarios where the direction property is more important.

[0067] As Figure 5 shown, the left is the fixed-direction formation, and the right is the target-direction formation, where Leader is the leader, and Follower1 and Follower2 are two followers. For the fixed-direction formation, only the corresponding target fixed position, that is, the position parameters of the main coordinate system, needs to be specified to maintain the corresponding planned position, while for the target-direction formation, it is necessary to give the formation formation in the coordinate system with the Leader's forward direction as the y-axis and the positions of Follower1 and Follower2 calculated dynamically. The relevant formula derivation is as Figure 6 shown, the in the figure is the direction pointed by the head of the host vehicle, and it is also the longitudinal coordinate axis in the secondary coordinate system.

[0068] First, obtain the deflection angle of the vehicle head relative to the main coordinate system according to the grid implementation result , the current position of the host vehicle : (22); Among them, is the abscissa of the master vehicle in the master coordinate system, is the ordinate of the master vehicle in the master coordinate system.

[0069] Subsequently, the target formation coordinates of the preset directional formation are obtained, which are the following positions in the secondary coordinate system. Set its position parameter as , is the abscissa of the follower vehicle in the secondary coordinate system, is the ordinate of the follower vehicle in the secondary coordinate system, represents the follower vehicle The coordinates in the secondary coordinate system. The formula can be used to calculate both Follower1 and Follower2 according to the different parameter settings of the follower vehicles in the formation method.

[0070] Then the distance between the follower vehicle and the master vehicle can be obtained as: (23); At the same time, the deflection angle of the follower vehicle relative to the master vehicle in the secondary coordinate can be obtained : (24); wherein, (25); is the deflection angle obtained by processing the angle value to 0~360 degrees.

[0071] By adding them up, the deflection angle of the follower vehicle in the master coordinate system can be obtained : (26); Then the position parameters of the real-time follower vehicle in the master coordinate system can be obtained: (27); wherein, is the coordinate of the follower vehicle in the master coordinate system, is the abscissa of the follower vehicle in the master coordinate system, is the ordinate of the follower vehicle in the master coordinate system, is the abscissa of the master vehicle in the master coordinate system, is the ordinate of the master vehicle in the master coordinate system.

[0072] According to the above content, in a multi-agent system, the positioning and information transmission of the leader are the basis for achieving efficient formation and path planning. In this embodiment, the leader first obtains its own position information and accurately calculates its positioning coordinates in the environment. By sending its own position information to the followers, the leader provides the necessary initial data for each agent in the formation.

[0073] After determining the position of the leader, the positions of the followers are set through the formation method and formation pattern. The choice of formation method has an important impact on the dynamic performance of the entire formation. Different formation patterns can adapt to different mission requirements and environmental conditions. Therefore, in practical applications, it is crucial to select the appropriate formation pattern according to the specific scenario requirements. In this way, the followers can not only maintain their relative positions with respect to the leader but also keep the formation stable in a dynamic environment.

[0074] Once the positions of the followers are determined, the leader uses the A* algorithm for global path planning. The A* algorithm is a graph-based search algorithm that efficiently finds the optimal path from the starting point to the ending point by combining path cost and heuristic evaluation. Based on the global path and considering the requirements of the formation shape, the leader further calculates the real-time formation target coordinates of the followers. This process not only depends on the current position information of the leader but also needs to consider environmental changes and the dynamic states of other agents.

[0075] After obtaining the target coordinates of the followers, DWA is introduced to achieve real-time path adjustment and dynamic obstacle avoidance. The DWA algorithm analyzes the speed and acceleration of the agent, evaluates the feasible movement modes in the current state, and ensures avoiding static and dynamic obstacles in the shortest time. The dynamic nature of this algorithm enables the agent to quickly respond to uncertainties and sudden obstacles, thus maintaining the integrity and safety of the formation. Between the leader and the followers, the DWA algorithm continuously iterates, evaluates the path between the current position and the sub-goal point, and performs real-time path adjustment.

[0076] Embodiment 2 This embodiment provides a multi-agent formation dynamic path planning and obstacle avoidance system, including: A leader positioning coordinate determination module, configured to determine a leader and multiple followers based on multiple agents to be formed, and determine the leader positioning coordinates on the environmental grid map; A global path determination module, configured to optimize the nodes of the path with the goal of minimizing the cost estimate from the starting point, through the current point to the target point of the path based on the environmental grid map to obtain the global path; A follower positioning coordinate determination module, configured to determine multiple follower positioning coordinates based on the formation method through the leader positioning coordinates; A real-time sub-goal node determination module, configured to continuously determine the real-time sub-goal nodes of the followers on the global path according to the follower positioning coordinates and the leader positioning coordinates; A trajectory prediction module, configured to determine a speed sampling space according to the follower's boundary speed, acceleration ability, and environmental obstacles, and perform trajectory prediction between the follower and the real-time sub-goal node in the speed sampling space to obtain multiple trajectory prediction results; An optimal obstacle avoidance path determination module, configured to evaluate multiple trajectory prediction results to obtain an optimal dynamic obstacle avoidance path for multi-agent formation.

[0077] The examples and application scenarios implemented by the above modules and corresponding steps are the same, but are not limited to the content disclosed in the first embodiment above. It should be noted that the above modules, as part of the system, can be executed in a computer system such as a set of computer executable instructions.

[0078] In the above embodiments, the descriptions of each embodiment have their own focuses. For parts not detailed in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0079] The proposed system can be implemented in other ways. For example, the above-described system embodiments are merely illustrative. For example, the division of the above modules is only a logical function division. In actual implementation, there can be other division methods. For example, multiple modules can be combined or integrated into another system, or some features can be ignored or not executed.

[0080] Embodiment III This embodiment provides a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, it implements the steps in the multi-agent formation dynamic path planning and obstacle avoidance method as described in the first embodiment above.

[0081] Embodiment IV This embodiment provides a computer device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the steps in the multi-agent formation dynamic path planning and obstacle avoidance method as described in the first embodiment above.

[0082] Those skilled in the art should understand that the embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of hardware embodiments, software embodiments, or embodiments combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memories and optical memories, etc.) containing computer-usable program code.

[0083] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present invention. It should be understood that each flow and / or block in the flowchart and / or block diagram, and the combination of flows and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices produce a means for implementing the functions specified in one process Figure 1 one process or multiple processes and / or blocks Figure 1 or a means for implementing the functions specified in multiple blocks.

[0084] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory produce a manufactured article including an instruction means that implements the functions specified in one process Figure 1 one process or multiple processes and / or blocks Figure 1 or a means for implementing the functions specified in multiple blocks.

[0085] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one process Figure 1 one process or multiple processes and / or blocks Figure 1 or a means for implementing the functions specified in multiple blocks.

[0086] Those of ordinary skill in the art can understand that all or part of the processes of implementing the above-described embodiment methods can be completed by instructing relevant hardware through a computer program. The program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the above-described method embodiments. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM), etc.

[0087] Although the specific implementation manners of the present invention have been described above in conjunction with the accompanying drawings, they are not limitations on the protection scope of the present invention. Those skilled in the art should understand that, based on the technical solutions of the present invention, various modifications or deformations that can be made by those skilled in the art without creative efforts are still within the protection scope of the present invention.

Claims

1. A multi-agent formation dynamic path planning and obstacle avoidance method, characterized in that: include: Determine a leader and multiple followers based on multiple intelligent agents to be formed, and determine the positioning coordinates of the leader on the environmental grid map; Based on the environment grid map, the path node is optimized to obtain the global path with the goal of minimizing the cost estimate from the starting point of the path through the current point to the target point; Based on the formation mode, the positioning coordinates of multiple followers are determined by the positioning coordinates of the leader; According to the follower's positioning coordinates and the leader's positioning coordinates, the follower's real-time sub-target nodes are continuously determined on the global path; The velocity sampling space is determined according to the boundary velocity, acceleration capability and environmental obstacles of the follower, and the trajectory between the follower and the real-time sub-target node is predicted in the velocity sampling space to obtain multiple trajectory prediction results; Multiple trajectory prediction results are evaluated to obtain the optimal dynamic obstacle avoidance path for the multi-agent formation.

2. The multi-agent formation dynamic path planning and obstacle avoidance method as claimed in claim 1, characterized in that: Based on the environment grid map, the path is optimized to obtain a global path by minimizing the cost estimate between the starting point of the path through the current point and the target point, specifically: Determine the set of points to be inspected and the set of inspected points on the path based on the navigator's positioning coordinates on the environmental grid map; In the set of points to be examined, the cost estimate from the starting point through the current point to the target point is calculated in sequence until the target point is reached; According to the cost estimation results of different nodes in the set of points to be examined, the node corresponding to the minimum sum of all cost estimation results is selected as the global path.

3. The multi-agent formation dynamic path planning and obstacle avoidance method as claimed in claim 1, characterized in that: The cost estimate between the starting point and the target point via the current point is the sum of the Euclidean distance from the starting point to the current point and the Euclidean distance from the current point to the target point.

4. The multi-agent formation dynamic path planning and obstacle avoidance method as claimed in claim 1, characterized in that: The formation modes include a fixed direction formation mode and a target direction formation mode; The fixed direction formation mode is that the formation adopts fixed relative position parameters in the same coordinate system. The follower changes according to the change of the movement state of the leader, and will perform dynamic obstacle avoidance according to the position of the obstacle; The target direction formation mode uses the direction of the main vehicle in the overall coordinate system, that is, the relevant formation parameters, to calculate the formation position in the small coordinate system with the direction of the front of the vehicle as the positive direction in real time, and maps it to the real-time formation parameters in the overall coordinate system.

5. The multi-agent formation dynamic path planning and obstacle avoidance method as claimed in claim 1, characterized in that: The trajectory prediction between the follower and the real-time sub-target node is performed in the speed sampling space to obtain multiple trajectory prediction results, specifically: Discretize the velocity sampling space to obtain multiple groups of velocity pairs consisting of linear velocities and angular velocities; Traverse each set of velocity pairs and substitute them into the follower's motion model to simulate the trajectory for trajectory prediction; The trajectory prediction results corresponding to multiple speed pairs are obtained.

6. The multi-agent formation dynamic path planning and obstacle avoidance method as claimed in claim 1, characterized in that: The multiple trajectory prediction results are evaluated to obtain the optimal dynamic obstacle avoidance path for the multi-agent formation, specifically: Calculate each trajectory prediction result according to the trajectory evaluation function to obtain multiple trajectory evaluation results; The trajectory with the smallest evaluation result is taken as the optimal dynamic obstacle avoidance path for the multi-agent formation.

7. The multi-agent formation dynamic path planning and obstacle avoidance method as claimed in claim 6, characterized in that: The trajectory evaluation function is specifically: ； in, are the coefficients of each sub-evaluation function, is the direction angle evaluation function, is the speed sub-evaluation function, is the distance evaluation function.

8. Multi-agent formation dynamic path planning and obstacle avoidance system, characterized by: include: A navigator positioning coordinate determination module is configured to determine a navigator and multiple followers based on multiple intelligent agents to be formed, and determine the navigator positioning coordinates on the environmental grid map; The global path determination module is configured to optimize the nodes of the path to obtain the global path based on the environmental grid map with the goal of minimizing the cost estimate between the starting point of the path through the current point and the target point; A follower positioning coordinate determination module is configured to determine a plurality of follower positioning coordinates based on the formation mode and through the leader positioning coordinates; A real-time sub-target node determination module is configured to continuously determine the real-time sub-target node of the follower on the global path according to the follower positioning coordinates and the navigator positioning coordinates; A trajectory prediction module is configured to determine a speed sampling space according to the boundary speed, acceleration capability and environmental obstacles of the follower, and perform trajectory prediction between the follower and the real-time sub-target node in the speed sampling space to obtain multiple trajectory prediction results; The optimal obstacle avoidance path determination module is configured to evaluate multiple trajectory prediction results to obtain the optimal dynamic obstacle avoidance path for the multi-agent formation.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps in the multi-agent formation dynamic path planning and obstacle avoidance method as described in any one of claims 1-7 are implemented.

10. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, it implements the steps in the multi-agent formation dynamic path planning and obstacle avoidance method as described in any one of claims 1-7.

Images