A method for cluster path planning and formation control of ground unmanned equipment

Abstract

The present invention discloses a method for path planning and formation control of a ground unmanned equipment cluster, comprising the following steps: step 1, constructing a grid map: constructing a grid map of the operating area of ​​a ground unmanned equipment cluster according to map information; step 2, using a bidirectional A* algorithm to complete path planning: searching from a starting point and an end point at the same time, and estimating the cost from a current node to a target node through an evaluation function to complete path planning; step 3, dynamic step adjustment mechanism: dynamically adjusting the search step according to environmental complexity and the state of the unmanned equipment; step 4, performing formation control based on a main path: using a Leader-Follower mode for formation control; step 5, generating a cluster path: using the path planning and the formation control to generate a final action path. The present invention improves the efficiency and adaptability of a multi-ground unmanned equipment system, and improves the stability and safety of formation control.

Description

Technical Field

[0001] The present invention belongs to the technical field of ground unmanned cluster technology, and in particular relates to a ground unmanned equipment cluster path planning and formation control method. Background Art

[0002] In recent years, with the rapid development of unmanned technology, communications, the Internet of Things and other technologies, the application scenarios of ground unmanned equipment clusters have increased. These application scenarios include but are not limited to logistics distribution, agricultural automation, military reconnaissance, etc. In the process of performing tasks, ground unmanned equipment can be equipped with various sensor devices, such as lidar, cameras, GPS, infrared sensors, etc., using their unique technical characteristics to monitor environmental conditions in real time, determine their own positions, flexibly adjust movement postures, and cleverly avoid various obstacles to successfully complete the task. However, in these applications, a large number of ground unmanned equipment are often used. When using a single path planning, it is necessary to plan the equipment frequently, and the planned paths of each device are messy, and there is a lack of coordination between the devices. Therefore, cluster path planning has become the focus of research.

[0003] Path planning and formation control are the core technologies for ground unmanned equipment clusters to successfully perform tasks. Path planning ensures that ground unmanned equipment can find a collision-free and efficient path from the starting point to the end point, while formation control ensures that multiple ground unmanned equipment can work together to maintain a stable formation and avoid conflicts. Through cluster path planning technology, unmanned equipment clusters can realize simultaneous planning of multiple unmanned equipment, thereby reducing cumbersome planning processes, meeting the efficient collaboration of ground unmanned equipment in complex and changing environments, and improving the overall system performance. And because multiple unmanned equipment are planned at the same time, the equipment can share information and optimize the overall path, thereby better meeting the needs of diverse application scenarios.

[0004] The key problem of cluster path planning technology for ground unmanned equipment is how to meet the planning requirements of ground equipment while speeding up the planning speed, and how to achieve control of cluster paths. In existing research, traditional path planning algorithms, such as the A* algorithm, usually use a one-way search method to search for paths from the starting point to the end point. This method takes a long time in complex environments, resulting in low efficiency in path planning. Especially when multiple ground unmanned equipment work together, each ground unmanned equipment independently plans the path, lacks global coordination, and easily causes path overlap and congestion problems. This inefficient search method not only increases the determination burden, but also prolongs the preparation time of the task, affecting the execution efficiency and real-time performance of the overall task. For example, in a dynamic environment, when new obstacles appear or the task requirements change, the traditional path planning method cannot quickly adjust the path, resulting in low efficiency in task execution. In addition, the existing path planning method lacks consideration of uphill and downhill requirements, and usually does not consider the uphill and downhill requirements of the terrain, resulting in unreasonable path planning, and ground unmanned equipment may be unable to pass when performing tasks.

[0005] In addition, traditional formation control methods usually regard the path of the previous ground unmanned equipment as an obstacle, and the subsequent ground unmanned equipment re-plans the path based on this. Although this method is simple and intuitive, it is easy to cause the formation to be scattered, and there are problems such as insufficient stability and poor flexibility. Each ground unmanned equipment independently plans its path, lacks global coordination, and cannot effectively maintain the stability and consistency of the formation. In addition, existing formation control methods can usually only select fixed formations, such as straight lines or diamonds, and cannot dynamically adjust the formation according to task requirements and environmental changes. This inflexible formation control method is particularly insufficient when facing complex tasks and dynamic environments, and it is easy to lead to inefficient task execution or even execution failure.

[0006] In summary, the current cluster path planning algorithm has problems such as unreasonable path planning and unstable formation control. In the execution of actual tasks, with the increase of unmanned equipment, path planning efficiency is low and conflicts are frequent. There is a lack of consideration of actual terrain and a lack of effective formation control methods. It is difficult to maintain a stable formation, which greatly affects the overall planning efficiency of the ground unmanned equipment cluster. Summary of the invention

[0007] The present invention aims to provide a method for cluster path planning and formation control of ground unmanned equipment, thereby improving the efficiency and adaptability of a multi-ground unmanned equipment system and improving the stability and safety of formation control.

[0008] In order to achieve the purpose of the present invention, the present invention provides a method for cluster path planning and formation control of ground unmanned equipment, comprising the following steps:

[0009] Step 1: Build a grid map: Build a grid map of the operating area of ​​the ground unmanned equipment cluster based on the map information;

[0010] Step 2: Use the bidirectional A* algorithm to complete the path planning: Search from the starting point and the end point at the same time, estimate the cost from the current node to the target node through the evaluation function, and complete the path planning;

[0011] Step 3: Dynamic step length adjustment mechanism: dynamically adjust the search step length according to the complexity of the environment and the status of the unmanned equipment;

[0012] Step 4: Formation control based on the main path: Use the Leader-Follower mode to control the formation;

[0013] Step 5: Generate a cluster path: Generate a final action path using the path planning and the formation control.

[0014] Compared with the prior art, the significant progress of the present invention is as follows: (1) The present invention reduces the search space of the unmanned equipment and improves the search efficiency by combining the bidirectional A* algorithm with the dynamic step size mechanism, and can dynamically adjust the search step size according to the complexity of the environment and the state of the ground unmanned equipment, further optimize the search process, thereby shortening the path planning time and improving the quality of the path; (2) The present invention optimizes obstacle judgment, considers uphill and downhill requirements, obtains the height information of each node based on the elevation map or terrain data, determines the slope between each node and its adjacent nodes, and evaluates whether the slope meets the uphill and downhill requirements, thereby ensuring that the path planning is reasonable and the ground unmanned equipment can smoothly pass through the complex terrain area; (3) The present invention adopts the Leader-Follower mode for formation control, dynamically adjusts the formation position through real-time communication and feedback mechanism, and dynamically adjusts the formation by considering multiple factors such as roads and environment, ensuring that multiple ground unmanned equipment can flexibly respond to various task requirements and maintain the stability and safety of the formation.

[0015] In order to more clearly illustrate the functional characteristics and structural parameters of the present invention, further description is given below in conjunction with the accompanying drawings and specific implementation methods. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of this application. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0017] Figure 1 It is a schematic diagram of the overall framework of an embodiment of the present invention;

[0018] Figure 2 is a schematic diagram of a bidirectional A* process according to an embodiment of the present invention;

[0019] Figure 3 is a schematic diagram of a dynamic step size adjustment mechanism flow chart of an embodiment of the present invention;

[0020] Figure 4 is a schematic diagram of a formation control process according to an embodiment of the present invention;

[0021] Figure 5 2 is a diagram of simulation results of an embodiment of the present invention. DETAILED DESCRIPTION

[0022] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments; based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0023] A method for ground unmanned equipment cluster path planning and formation control, combined with Figure 1-Figure 4 , including the following steps:

[0024] Step 1: Build a grid map: Build a grid map of the operating area of ​​the ground unmanned equipment cluster based on the map information, where each grid represents a passable or impassable unit, as well as the corresponding terrain height information, which is used for subsequent path planning and obstacle judgment;

[0025] 1-1. Collect detailed geographic information of the ground unmanned equipment cluster operation area through sensors such as lidar, cameras, and GPS;

[0026] 1-2. Divide the operation area into grids of uniform size, each grid representing a passable or impassable unit, and the size of the grid is determined according to the actual application scenario and accuracy requirements to ensure the accuracy and practicality of the map;

[0027] 1-3. Obstacle identification is performed on the grid to determine whether it is passable; if the grid contains an obstacle, it is marked as impassable, otherwise it is marked as passable;

[0028] 1-4. Obtain and record the terrain height information of the grid through the elevation map or terrain data, and assign a height value to the center point of the grid , further determining the passability of the grid based on a slope threshold;

[0029] 1-5. Integrate the grid's accessibility, terrain height, and slope information to construct a grid map matrix .

[0030] Step 2: Use the bidirectional A* algorithm to complete the path planning: Search from the starting point and the end point at the same time, estimate the cost from the current node to the target node through the evaluation function, and complete the path planning;

[0031] 2-1. Initialize the unmanned equipment conditions; determine the equipment's starting point, target point, and grid map information; define variables for the bidirectional A* algorithm: starting point S, end point G, and an open list starting from the starting point , an open list starting from the end point , Close list from the start point , a closed list starting from the end point ; The initialization is to add the starting point S to the open list starting from the starting point , add the end point G to the open list starting from the end point ;

[0032] 2-2. By searching the path from the starting point and the end point at the same time, the search tree is gradually expanded;

[0033] The search process includes forward search and backward search:

[0034] The forward search: from the open list starting from the starting point Select the forward cost function The smallest node , the forward cost function The smallest node From the open list starting from the starting point Move to the closed list , and the forward cost function The smallest node The adjacent nodes of Expand and determine the forward root node to the adjacent node The cost function And the forward cost function ; If the adjacent node Not in the closed list starting from the start point and the open list starting from the end point , then add it to the open list starting from the starting point , if the adjacent node Already in the open list starting from the end point If the search is in progress, stop searching;

[0035] The backward search: from the open list starting from the end point Select the backward cost function The smallest node , which is removed from the open list starting from the end point Move to the closed list starting from the end , and the backward cost function The smallest node The adjacent nodes Expand and determine the backward root node to the adjacent node The cost function and the backward cost function , if the adjacent node Not in the closed list starting from the start point and the open list starting from the end point , then add it to the open list starting from the end point , if the adjacent node Already in the open list starting from the starting point If the search is in progress, the search stops.

[0036] 2-3. Obstacle optimization. Ground unmanned equipment is limited by terrain and has the need to go up and down slopes. It is necessary to optimize obstacles, that is, to evaluate the feasibility of the movement of unmanned equipment between grids; by determining the slope between the grids , evaluate obstacles:

[0037] ;

[0038] in, and are the terrain heights of grids i and j, is the Euclidean distance between grids i and j. This formula can be used to determine whether the slope between grids is within the range accessible by unmanned equipment and whether the grid is considered an obstacle.

[0039] According to the preset slope threshold , evaluate whether the slope meets the uphill and downhill requirements; if , then the path is marked as infeasible, otherwise it is feasible;

[0040] Finally, in the heuristic function Add slope cost to :

[0041] ;

[0042] in, is the slope cost coefficient;

[0043] 2-4. Construct a planning path through the intersection nodes of the search tree to ensure that the found path is the best; when the open list starting from the starting point and the open list starting from the end point When the nodes in intersect, Construct the final path. From the starting point To the intersection node The forward path of , from the intersection node To the end point The backward path of , the final path is the forward path and the backward path connection, that is ;

[0044] 2-5. Perform curve fitting on the path points of the planned path to ensure that the generated path meets the motion characteristics of the ground unmanned equipment and complete the path planning;

[0045] First, inflection points are detected in the path, that is, nodes where the direction of the path changes significantly. Then, a curve fitting algorithm (such as Bézier curve or spline curve) is used to smooth the path between the inflection points to generate a smooth curved path. The curvature of the fitting curve is determined to ensure that the curvature is within a preset threshold range to avoid excessive curvature that prevents ground unmanned equipment from passing smoothly. Finally, the straight line segments in the original path are replaced by the fitted curved path to generate the final smooth path.

[0046] Step 3: Dynamic step adjustment mechanism: Combine Figure 3 , dynamically adjust the search step size according to the environmental complexity and the status of the unmanned equipment. The environmental complexity is evaluated by obtaining environmental information through sensors such as lidar and cameras. The status of the unmanned equipment includes speed, direction and battery power to adapt to different environments and task requirements;

[0047] 3-1. Evaluate the complexity of the environment. Obtain environmental information through sensors such as lidar and cameras, evaluate the environment, and obtain the environmental complexity E. In the dynamic step mechanism, first define the environmental complexity :

[0048] ;

[0049] in, is the number of obstacles, is the size of the search space.

[0050] 3-2. Evaluate the current status of the ground unmanned equipment and obtain the status score S; Evaluate the status score of the ground unmanned equipment :

[0051] ;

[0052] Among them, V is the speed of the ground unmanned equipment, D is the direction of the equipment, and B is the battery power of the equipment. , , The speeds are , the direction and the amount of electricity The weight coefficient of .

[0053] 3-3. Dynamically adjust the search step length according to the complexity of the environment and the status of the ground unmanned equipment :

[0054] ;

[0055] in, is the basic step length, is the influence coefficient of the environmental complexity E, is the influence coefficient of the ground unmanned equipment state S; when the environment is simple and the ground unmanned equipment is in good condition, a larger step size is used; when the environment is complex and the ground unmanned equipment is in poor condition, a smaller step size is used.

[0056] Step 4: Formation control based on the main path: The Leader-Follower mode is used for formation control, with one unmanned device acting as the leader and the other unmanned devices acting as followers. The followers make adjustments based on the path determined by the leader to maintain the stability and consistency of the formation.

[0057] 4-1. Leader selection: Select a suitable leader based on distance, environmental complexity, and speed factors to ensure that the formation can complete the task efficiently and stably. In the Leader-Follower mode, first define the leader. and followers ; When selecting a leader, define the selection function as follows:

[0058] ;

[0059] in, is the distance from the pilot to the target, The speed of the navigator, The complexity of the environment around the navigator, , are the distances from the pilot to the target respectively , the pilot speed and the complexity of the pilot environment The weight coefficient of The smallest ground unmanned equipment serves as the navigator;

[0060] 4-2. Follower control: By using distance control and angle control, the follower can adjust its speed and direction according to the path and position of the leader to maintain the stability of the formation;

[0061] Follower control includes distance control and angle control, defining control parameters: the distance between the follower and the leader and relative angle ;

[0062] The distance control formula is as follows:

[0063] ;

[0064] in, and The pilots of Axis coordinates and Axis coordinates, and The followers of Axis coordinates and Axis coordinates;

[0065] By adjusting the speed of the follower , maintain distance from the pilot Within the preset range;

[0066] The angle control formula is:

[0067] ;

[0068] By adjusting the direction of the follower, the relative angle θ with the leader is kept within a preset range.

[0069] 4-3. Formation adjustment strategy: Dynamically select formations based on mission requirements and environmental changes;

[0070] Obtain environmental information through sensors and evaluate environmental changes; define environmental factors: road width and environmental complexity , get the formation selection function :

[0071] ;

[0072] in, The road widths are and the complexity of the environment The weight coefficient of

[0073] when When , choose a straight line formation; when When choosing a diamond formation, , select other custom formations;

[0074] in, , are different threshold constants.

[0075] Step 5, generating a cluster path: using the path planning and the formation control, generating a final action path, including the path points and formation information of each unmanned device, to ensure that the path planning is reasonable, the ground unmanned devices can smoothly pass through areas with complex terrain, and maintain a stable formation when performing tasks.

[0076] Combination Figure 5 Through the above steps, according to the given ground unmanned equipment cluster and terrain information, an improved two-way A* algorithm with dynamic step size adjustment and obstacle optimization can be used to obtain a main path of equipment affected by multiple factors such as terrain factors and unmanned equipment factors. On this basis, the Leader-Follower mode formation control is used to control the path, action angle and formation of the ground unmanned equipment cluster, ensuring better collaboration between unmanned equipment.

[0077] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0078] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A method for path planning and formation control of a ground unmanned equipment cluster, characterized in that: The following steps are involved: Step 1: Build a grid map: Build a grid map of the operating area of ​​the ground unmanned equipment cluster based on the map information; Step 2: Use the bidirectional A* algorithm to complete the path planning: Search from the starting point and the end point at the same time, estimate the cost from the current node to the target node through the evaluation function, and complete the path planning; Step 3: Dynamic step length adjustment mechanism: dynamically adjust the search step length according to the complexity of the environment and the status of the unmanned equipment; 3-1. Evaluate the complexity of the environment and obtain the environmental complexity E: ； in, is the number of obstacles, is the size of the search space; 3-2. Evaluate the current status of the ground unmanned equipment and obtain the status score S: ； in, is the speed of ground unmanned equipment, is the orientation of the device, is the battery level of the device. , , The speeds are , the direction and the amount of electricity The weight coefficient of The weight coefficient is dynamically set according to task requirements; 3-3. Dynamically adjust the search step length according to the complexity of the environment and the status of the ground unmanned equipment : ； in, is the basic step length, is the influence coefficient of the environmental complexity E, is the influence coefficient of the ground unmanned equipment state S; Step 4: Formation control based on the main path: Use the Leader-Follower mode to control the formation; Step 5: Generate a cluster path: Generate a final action path using the path planning and the formation control.

2. A method for ground unmanned equipment cluster path planning and formation control according to claim 1, characterized in that: The step 1 is specifically as follows: 1-1. Collect detailed geographic information of the ground unmanned equipment cluster operation area through sensors; 1-2. Divide the working area into grids of uniform size; 1-3. Obstacle identification is performed on the grid; if the grid contains an obstacle, it is marked as impassable, otherwise it is marked as impassable; 1-4. Obtain and record the terrain height information of the grid through the elevation map or terrain data, and assign a height value to the center point of the grid , further determining the passability of the grid based on a slope threshold; 1-5. Integrate the grid's accessibility, terrain height, and slope information to construct a grid map matrix .

3. A method for path planning and formation control of ground unmanned equipment cluster according to claim 1, characterized in that: The step 2 is specifically as follows: 2-1. Initialize the unmanned equipment conditions; determine the equipment's starting point, target point, and grid map information; 2-2. Searching the path through the starting point and the target point at the same time, and gradually expanding the search tree; 2-3. By determining the slope between the grids , evaluate obstacles: 2-4. Constructing a planning path through the intersection nodes of the search tree; 2-5. Perform curve fitting on the path points of the planned path to complete the path planning.

4. A method for ground unmanned equipment cluster path planning and formation control according to claim 1, characterized in that: The step 4 is specifically as follows: 4-1. Navigator selection: Select a suitable navigator based on distance factors, environmental complexity factors, and speed factors, and select the function As shown below: ； in, is the distance from the pilot to the target, The speed of the navigator, The complexity of the environment around the navigator, , are the distances from the pilot to the target respectively , the pilot speed and the complexity of the pilot environment The weight coefficient of 4-2. Follower control: by adopting distance control and angle control Adjust its own speed and direction; the distance control for: ； in, and The pilots of Axis coordinates and Axis coordinates, and The followers of Axis coordinates and Axis coordinates; The angle control for: ； 4-3. Formation adjustment strategy: Dynamically select formation according to task requirements and environmental changes.

5. A method for ground unmanned equipment cluster path planning and formation control according to claim 3, characterized in that: The specific steps of step 2-1 are as follows: define variables for the bidirectional A* algorithm: starting point S, end point G, and an open list starting from the starting point , an open list starting from the end point , Close list from the start point , a closed list starting from the end point ; The initialization is to add the starting point S to the open list starting from the starting point , add the end point G to the open list starting from the end point .

6. A method for ground unmanned equipment cluster path planning and formation control according to claim 5, characterized in that: The search process of step 2-2 includes forward search and backward search: The forward search: from the open list starting from the starting point Select the forward cost function The smallest node , the forward cost function The smallest node From the open list starting from the starting point Move to the closed list , and the forward cost function The smallest node The adjacent nodes of Expand and determine the forward root node to the adjacent node The cost function And the forward cost function ; If the adjacent node Not in the closed list starting from the start point and the open list starting from the end point , then add it to the open list starting from the starting point , if the adjacent node Already in the open list starting from the end point If the search is in progress, stop searching; The backward search: from the open list starting from the end point Select the backward cost function The smallest node , which is removed from the open list starting from the end point Move to the closed list starting from the end , and the backward cost function The smallest node The adjacent node Expand and determine the backward root node to the adjacent node The cost function and the backward cost function , if the adjacent node Not in the closed list starting from the start point and the open list starting from the end point , then add it to the open list starting from the end point , if the adjacent node Already in the open list starting from the starting point If the search is in progress, the search stops.

7. A method for ground unmanned equipment cluster path planning and formation control according to claim 6, characterized in that: The slope of steps 2-3 for: ； in, and are the terrain heights of grids i and j, is the Euclidean distance between grids i and j; Based on the slope threshold , evaluate whether the slope meets the uphill and downhill requirements; if , then the path is marked as infeasible, otherwise it is feasible; Finally, in the heuristic function Add slope cost to : ； in, is the slope cost factor.

8. A method for ground unmanned equipment cluster path planning and formation control according to claim 4, characterized in that: The steps 4-3 are as follows: Obtain environmental information through sensors and evaluate environmental changes; define environmental factors: road width and environmental complexity , get the formation selection function : ； in, are weight coefficients of the road width W and the environmental complexity respectively; when When , choose a straight line formation; when When choosing a diamond formation, , select other custom formations; in, , are different threshold constants.

Images