Multi-robot formation path planning method and system based on FLA star algorithm

Abstract

The invention discloses a multi-robot formation path planning method and system based on an FLA star algorithm. The method comprises the following steps: obtaining a pre-constructed obstacle grid map; setting a starting point and a target point in a coordinate form of a formation center or a reference point, calculating and deducing a specific starting position and a specific target position of each robot based on the center point or the reference point, and determining formation parameters of the multi-robot formation; initializing an FLA star algorithm and performing path search, traversing a grid point set adjacent to a starting point in a grid map, and adopting a cost function as a basis for node updating in the traversing process; according to the FLA star algorithm, the whole formation is regarded as a whole, formation constraint is directly considered in the search process, so that a group of path sets meeting the formation geometric structure are generated, the path sets naturally meet the formation keeping requirement, and the pressure of back-end trajectory optimization or control algorithms is remarkably relieved.

Description

Technical Field

[0001] The present invention relates to the technical field of distributed systems and multi-robot formation control, and in particular to a multi-robot formation path planning method and system based on the FLA star algorithm. Background Art

[0002] With the rapid development of robotics, multi-robot systems have found widespread application in industries such as industry, logistics, agriculture, and services. Multi-robot formation control, as one of the core technologies for multi-robot collaborative tasks, can, to a certain extent, enable multiple robots to move in a specific formation, thereby improving task efficiency, enhancing system robustness, and optimizing resource utilization.

[0003] Traditional path planning algorithms, such as the A* algorithm, Dijkstra algorithm, and RRT algorithm, perform well in single-robot path search, but have limitations when directly applied to multi-robot formation scenarios. First, multi-robot systems need to consider the relative position constraints between robots to ensure the stability of the formation during movement. Second, traditional algorithms often face the problems of high computational complexity and insufficient real-time performance when dealing with multi-robot collaborative paths. The A* algorithm is widely used in the field of path planning due to its high efficiency and heuristic search characteristics. However, the traditional A* algorithm mainly focuses on searching for the optimal path of a single robot and does not fully consider the formation constraints and collaborative requirements in multi-robot formations. In recent years, some improved A* algorithms have attempted to solve the multi-robot path planning problem by introducing multi-objective optimization, dynamic weight adjustment, or hierarchical search strategies. However, the balance between formation maintenance, computational efficiency, and path feasibility in these methods still needs to be further optimized. Summary of the Invention

[0004] In response to the problems existing in the prior art, the present invention provides a multi-robot formation path planning method and system based on the FLA star algorithm, wherein the FLA star algorithm regards the entire formation as a whole, directly considers the formation constraints during the search process, and thus generates a set of paths that conform to the formation's geometric structure. This method effectively overcomes the shortcomings of traditional path planning algorithms in multi-robot formation scenarios, and can maintain the formation stability of the formation while ensuring safe obstacle avoidance and completing mission objectives, thereby improving the collaborative efficiency and task execution performance of the multi-robot system.

[0005] The technical solutions of the present invention are as follows:

[0006] In a first aspect of the present invention, a multi-robot formation path planning method based on the FLA star algorithm is provided, comprising the following steps:

[0007] Obtain a pre-built obstacle grid map and identify obstacle information by the status of the grid cells;

[0008] The starting point and target point are set in the form of the coordinates of the formation center or reference point. The specific starting position and target position of each robot are calculated based on the center point or reference point, and the formation parameters of the multi-robot formation are clarified.

[0009] Initialize the FLA star algorithm and perform path search. Traverse the grid point set adjacent to the starting point in the grid map. Use the cost function as the basis for node update during the traversal process. Use the node with the smallest cost value for subsequent node expansion and path backtracking.

[0010] The reference point path is generated by backtracking, and all robot paths are generated according to the formation parameters and smoothed.

[0011] In some embodiments of the present invention, the formation parameters of the multi-robot formation include the number of robots required to form the formation and the relative position between each robot and a formation reference point.

[0012] In some embodiments of the present invention, initializing the FLA star algorithm includes:

[0013] The starting point of the formation reference point is expanded to a state node containing the position and formation direction as the initial parent node, and the initial parent node is stored in the open set; during initialization, the open set only contains the initial parent node, assigns an initial cost to the starting point, and initializes the closed set to empty.

[0014] In some embodiments of the present invention, evaluating the feasibility of a set of child nodes during the traversal process specifically includes:

[0015] Taking a grid point adjacent to the parent node as the reference point and the grid point as the center, the formation robot rotates counterclockwise by a set angle to form a posture corresponding to the position set. Check the grid status corresponding to each robot position. If the grid status of any position is occupied, the node is eliminated and does not participate in the subsequent cost evaluation.

[0016] In some embodiments of the present invention, the cost function is defined as:

[0017] f=g+h+λ×d

[0018] Where f represents the comprehensive cost; g represents the actual path cost of moving from the starting point to the adjacent grid point; h is the heuristic cost, which is used to estimate the cost of moving from the adjacent grid point to the target point; λ represents the weight coefficient of formation rotation, which is used to reflect the influence of the orientation difference between parent and child nodes; d represents the average movement distance of each robot between the parent node and the child node due to formation rotation.

[0019] In some embodiments of the present invention, the child nodes that can participate in the cost function are brought into the cost function for calculation, and the child nodes with the smallest cost value are counted and placed in the open set; the node corresponding to the minimum value of f in the open set is taken out and placed in the closed set, and the node is used as the parent node for subsequent node expansion and path backtracking.

[0020] In some embodiments of the present invention, backtracking to generate a reference point path and generating all robot paths according to formation parameters specifically includes:

[0021] Starting from the node corresponding to the target point, trace back to the starting node through the parent-child node relationship. During the backtracking process, the key information of each node is recorded to form a complete path from the starting position to the target position. According to the relative position relationship between the robots and the reference point in the formation, combined with the movement path information of the reference point, the actual movement path of each robot in the formation is obtained.

[0022] In some embodiments of the present invention, the path set generated by each robot should meet the following conditions:

[0023] Each robot path has no collision in the grid map, that is, the grid states corresponding to the path points are all blank;

[0024] The path set as a whole complies with the formation constraint, that is, the relative position deviation between the robots at any time is within the allowable range;

[0025] The path length is globally optimal under the comprehensive cost definition.

[0026] In some embodiments of the present invention, a B-spline curve is used to smooth the path of each robot, and a secondary obstacle avoidance check is performed on the smoothed path to ensure that the new path points do not collide with obstacles in the grid map. If a collision is found, new control points are added near the collision point to make the curve bypass the obstacle, and local B-spline smoothing is re-performed.

[0027] In a second aspect of the present invention, a multi-robot formation path planning system based on the FLA algorithm is provided, characterized in that it includes:

[0028] The grid map acquisition module is configured to: acquire a pre-built obstacle grid map and identify obstacle information by the state of the grid cells;

[0029] The formation parameter setting module is configured to: set the starting point and target point in the form of the coordinates of the formation center or reference point, calculate and derive the specific starting position and target position of each robot based on the center point or reference point, and specify the formation parameters of the multi-robot formation;

[0030] The path search module is configured to: initialize the FLA star algorithm and perform path search, traverse the set of grid points adjacent to the starting point in the grid map, use the cost function as the basis for node update during the traversal process, and use the node with the smallest cost value for subsequent node expansion and path backtracking;

[0031] The path generation module is configured to: backtrack and generate the reference point path, generate all robot paths according to the formation parameters and perform smoothing.

[0032] One or more technical solutions of the present invention have the following beneficial effects:

[0033] (1) The FLA Star algorithm proposed in this paper considers the formation as a whole for path search and directly considers formation constraints in the node expansion stage, so that the generated path set naturally meets the formation maintenance requirements, avoiding the subsequent complex conflict adjustment process. Specifically, the FLA Star node not only contains the position of the formation reference point, but also introduces the formation orientation and integrates the formation rotation weight and the average movement distance of the robots into the cost function, thereby simultaneously optimizing the path feasibility and formation stability during the search process. Compared with traditional methods, the FLA Star algorithm can generate a path set that meets the formation constraints in one go, reducing the number of iterative optimizations and significantly improving computational efficiency. It is suitable for large-scale multi-robot collaborative tasks.

[0034] (2) The FLA star algorithm of the present invention pre-places the formation constraints in the path search phase, and the generated path set naturally meets the formation geometry requirements, thereby significantly reducing the adjustment pressure of the back-end control. Specifically, the FLA star algorithm will check the accessibility of all robots in the formation in real time when the node is expanded, ensure that the path points are in the obstacle-free area, and optimize the formation rotation angle to reduce unnecessary formation adjustments. Since the path set already meets the basic formation constraints, the back-end trajectory optimization only needs basic smoothing or local adjustment to achieve high-quality motion planning, which effectively solves the technical problem of excessive reliance on back-end optimization for formation maintenance in traditional path planning methods. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 Flowchart of the multi-robot formation path planning method based on the FLA star algorithm of the present invention;

[0036] Figure 2 is a flow chart of the FLA star algorithm of the present invention;

[0037] Figure 3 It is a node diagram of the FLA star algorithm of the present invention. DETAILED DESCRIPTION

[0038] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0039] Example 1

[0040] In a typical embodiment of the present invention, a multi-robot formation path planning method based on the FLA algorithm is proposed. The formation-level FLA path search algorithm is different from the traditional A* algorithm that searches for a single robot independently. The FLA algorithm regards the entire formation as a whole and directly considers the formation constraints during the search process, thereby generating a set of paths that conform to the formation geometry. This path set naturally meets the formation maintenance requirements and significantly reduces the pressure on the back-end trajectory optimization or control algorithm. The flow chart of the present invention is as follows Figure 1 The detailed steps are as follows:

[0041] Obtain a pre-built obstacle grid map and identify obstacle information by the status of the grid cells;

[0042] The starting point and target point are set in the form of the coordinates of the formation center or reference point. The specific starting position and target position of each robot are calculated based on the center point or reference point, and the formation parameters of the multi-robot formation are clarified.

[0043] Initialize the FLA star algorithm and perform path search. Traverse the grid point set adjacent to the starting point in the grid map. Use the cost function as the basis for node update during the traversal process. Use the node with the smallest cost value for subsequent node expansion and path backtracking.

[0044] The reference point path is generated by backtracking, and all robot paths are generated according to the formation parameters and smoothed.

[0045] Specifically:

[0046] Step 1: Obtain a pre-built obstacle grid map and identify obstacle information by the status of the grid cells.

[0047] A status of free indicates that the cell has no obstacles, and a status of occupied indicates that there are obstacles. At the same time, the overall size of the grid map is determined to clarify the search space range and the resolution of the map.

[0048] Step 2: Set the starting point and target point in the form of the coordinates of the formation center or reference point, calculate and deduce the specific starting position and target position of each robot based on the center point or reference point, and clarify the formation parameters of the multi-robot formation.

[0049] Specifically, set the starting point start=(x start ,y start ) and the target point goal=(x goal ,y goal), are expressed in terms of the coordinates of the formation center or reference point, rather than the set of individual robot positions. The specific starting position of each robot is calculated based on the formation center or reference point according to the predefined formation geometry. Furthermore, the formation parameters of the multi-robot formation include the number of robots n required to form the formation and the relative positions of the robots and the formation reference point in the formation swarm = {s0, s1, ..., s i ,…,s n}, determine the specific geometric constraints of the formation, where s i represents the position of robot i relative to the reference point, s i =(s xi ,s yi ).

[0050] Step 3: Initialize the FLA star algorithm and perform path search. Traverse the grid point set adjacent to the starting point in the grid map. During the traversal process, use the cost function as the basis for node update. The node with the smallest cost value is used for subsequent node expansion and path backtracking. Figure 2 and Figure 3 As shown, specifically including:

[0051] 1. Initialize the FLA algorithm. Expand the starting point of the formation reference point to a state node containing the position and formation direction as the initial parent node, denoted as (start, θ start )=(x start ,y start ,θ start ), where θ start Indicates the initial orientation of the formation at the starting point, which is used to define the direction constraint of the formation at the beginning of the search. start ) is stored in the open set OPEN. The open set OPEN is used to store candidate nodes for expansion. Initially, the open set contains only the starting node, assigning an initial cost to the starting point. This method sets the initial cost of the initial parent node to 0. The closed set CLOSE is initialized to empty and is used to record nodes that have been expanded.

[0052] 2. FLA star algorithm performs path search. In the grid map, traverse the set of grid points adjacent to start N = {neigh0, neigh1, ..., neigh7}. Take one of the grid points neigh∈N as an example. Similar to the starting point, expand it to a state node containing the position and formation direction as the starting node (start, θ start )’s child node set, denoted as (neigh,θ neigh )=(x neight ,y neight ,θneigh ), where (x neight ,y neight ) represents the position of the grid point neigh in the grid map, θ neigh represents the set of formation directions with the grid point neigh as the formation reference point, θ neigh = {0,π / 6,π / 3,…,2π-π / 6}, a total of 12 elements, which evenly divide a circle into 12 equal parts (a total of 12 directions). If real-time performance is not a high requirement, the number of divisions can be further increased to improve the quality of feasible solutions.

[0053] 3. Calculate the child node set (neigh,θ neigh ) feasibility and cost. Taking the child node (neigh,θ) as an example, θ∈θ neigh , determine the grid point neigh as the reference point and the reference point as the rotation center, and then rotate the formation counterclockwise by an angle of θ to obtain the formation position corresponding to the posture of θ in the node in, represents the position of robot i in the grid map after rotation, Traversal Determine the corresponding grid status and pay attention to check The grid state corresponding to each robot position in , if the grid state corresponding to any robot position is occupied, the node of this posture does not participate in the calculation of the subsequent cost function.

[0054] Furthermore, the cost function of the FLA star algorithm is designed as the basis for node update, specifically as follows: f = g + h + λ × d, where f represents the comprehensive cost; g represents the actual path cost of moving from the starting point start to the grid point where neigh is located; h is the heuristic cost, which is used to estimate the movement cost from the grid point where neigh is located to the target point. This embodiment uses the Euclidean distance for calculation, that is, h = ||neigh-goal||; λ represents the weight coefficient of formation rotation, which is used to reflect the degree of formation posture transformation between the parent node and the child node, λ∝|θ fath -θ child |; d represents the average moving distance of each robot in the formation between the parent node and the child node due to the formation rotation represents the position of robot i in the parent node, Indicates the position of robot i in the child node. The child nodes that can participate in the cost function are brought into the cost function for calculation, and the child node with the smallest cost value (neigh,θ min ) and put it into the open set OPEN.

[0055] Repeat the above process until all adjacent grid point sets N are traversed, ending the current round of node expansion.

[0056] During the path search process of the FLA algorithm, the node with the lowest comprehensive cost f is extracted from the open set OPEN and moved to the closed set CLOSE. This node is then set as the parent node for subsequent node expansion and path backtracking. Based on this parent node, the above steps are repeated until the termination condition is met. The termination condition can be divided into two cases: if the state node corresponding to the grid where the target point is located is added to the open set OPEN, it indicates that the global shortest path has been found; if the open set OPEN becomes empty, meaning there are no expandable nodes, the path search has failed, indicating that no feasible path solution exists under the current environment and formation constraints.

[0057] Step 4: Backtrack to generate the reference point path, generate all robot paths based on the formation parameters and perform smoothing, specifically including:

[0058] 1. Backtrack to generate a path and map it to each robot. In the path planning process of the FLA star algorithm, if the global shortest path is successfully found during the path search process, the path backtracking and mapping phase will be entered to generate the specific movement path of each robot. The specific operations are as follows:

[0059] From the node corresponding to the target point (goal, θ start ) and trace back to the starting node (start,θ start ), in the backtracking process, record the key information of each node. Taking a certain node in the middle as an example, its key information is defined as (node,θ node ), where node represents the position coordinate of the formation reference point at the node, θ node Indicates the direction of the formation at this position. By backtracking, a complete path Γ={(start,θ start ),…,(node,θ node ),…,(goal,θ start )}, which represents the moving trajectory of the formation reference point. Based on the relative position relationship between each robot in the formation and the reference point swarm, combined with the moving path of the reference point, the actual moving path of each robot in the formation is calculated. For each node (node,θ node ), according to the reference point position node and the formation direction θ node , calculate the position of robot i at the node (x i ,y i ), where x i =x node +||s i||×cosθ,y i =y node +||s i ||×sinθ. Generate the complete movement trajectory p of robot i by traversing all nodes in the path Γ i ={(x starti ,y starti ),…,(x i ,y i ),…,(x goali ,y goali )}, where (x starti ,y starti ) represents the coordinates of the starting position of robot i, (x goali ,y goali ) represents the coordinates of the target position of robot i.

[0060] Traverse all robots and generate the final path set P = {p1,p2,…,p n}, p i Each path is the path of robot i. The generated path set meets the following conditions:

[0061] (1) Each robot path has no collision in the grid map, that is, the grid states corresponding to the path points are all free;

[0062] (2) The path set as a whole complies with the formation constraint, that is, the relative position deviation between the robots at any time is within the allowable range;

[0063] (3) The path length is globally optimal under the definition of the comprehensive cost f. If a path is found to violate the constraints, the search parameters need to be readjusted, such as the weight coefficient λ or the refined orientation segmentation.

[0064] 2. In the FLA algorithm's path planning, after generating a path set for each robot, each path is smoothed using existing B-spline curves to improve continuity and motion smoothness. Specifically, a cubic B-spline curve is constructed using path points as control points, and a smoothed path point sequence is generated through uniform sampling. The smoothed path is then subjected to a secondary obstacle avoidance check. The positions of all robots at each sampling point are calculated and the corresponding grid status is verified. If any position is found to be occupied, a collision is marked and a new control point is added near the collision point to guide the curve around the obstacle. Local B-spline smoothing is then performed again until the path meets the obstacle avoidance requirements. Finally, the smoothed path set is output to ensure formation maintenance, safety, and motion smoothness.

[0065] Example 2

[0066] In a typical embodiment of the present invention, a multi-robot formation path planning system based on the FLA algorithm is provided, comprising:

[0067] The grid map acquisition module is configured to: acquire a pre-built obstacle grid map and identify obstacle information by the state of the grid cells;

[0068] The formation parameter setting module is configured to: set the starting point and target point in the form of the coordinates of the formation center or reference point, calculate and derive the specific starting position and target position of each robot based on the center point or reference point, and specify the formation parameters of the multi-robot formation;

[0069] The path search module is configured to: initialize the FLA star algorithm and perform path search, traverse the set of grid points adjacent to the starting point in the grid map, use the cost function as the basis for node update during the traversal process, and use the node with the smallest cost value for subsequent node expansion and path backtracking;

[0070] The path generation module is configured to: backtrack and generate the reference point path, generate all robot paths according to the formation parameters and perform smoothing.

[0071] Although the above describes the specific embodiments of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without any creative work are still within the scope of protection of the present invention.

Claims

1. A multi-robot formation path planning method based on the FLA star algorithm, characterized in that: The following steps are involved: Obtain a pre-built obstacle grid map and identify obstacle information by the status of the grid cells; The starting point and target point are set in the form of the coordinates of the formation center or reference point. The specific starting position and target position of each robot are calculated based on the center point or reference point, and the formation parameters of the multi-robot formation are clarified. Initialize the FLA star algorithm and perform path search. Traverse the grid point set adjacent to the starting point in the grid map. Use the cost function as the basis for node update during the traversal process. Use the node with the smallest cost value for subsequent node expansion and path backtracking. The reference point path is generated by backtracking, and all robot paths are generated according to the formation parameters and smoothed.

2. The multi-robot formation path planning method based on the FLA star algorithm according to claim 1, characterized in that: The formation parameters of the multi-robot formation include the number of robots required to form the formation and the relative position between each robot and the formation reference point.

3. The multi-robot formation path planning method based on the FLA star algorithm according to claim 1, characterized in that: Initializing the FLA star algorithm includes: The starting point of the formation reference point is expanded to a state node containing the position and formation direction as the initial parent node, and the initial parent node is stored in the open set; during initialization, the open set only contains the initial parent node, assigns an initial cost to the starting point, and initializes the closed set to empty.

4. The multi-robot formation path planning method based on the FLA star algorithm according to claim 1, characterized in that: Evaluate the feasibility of the child node set during the traversal process, specifically including: Taking a grid point adjacent to the parent node as the reference point and the grid point as the center, the formation robot rotates counterclockwise by a set angle to form a posture corresponding to the position set. Check the grid status corresponding to each robot position. If the grid status of any position is occupied, the node is eliminated and does not participate in the subsequent cost evaluation.

5. The multi-robot formation path planning method based on the FLA algorithm according to claim 4, characterized in that: The cost function is defined as: f=g+h+λ×d Where f represents the comprehensive cost; g represents the actual path cost of moving from the starting point to the adjacent grid point; h is the heuristic cost, which is used to estimate the cost of moving from the adjacent grid point to the target point; λ represents the weight coefficient of formation rotation, which is used to reflect the influence of the orientation difference between parent and child nodes; d represents the average movement distance of each robot between the parent node and the child node due to formation rotation.

6. The multi-robot formation path planning method based on the FLA algorithm according to claim 5, characterized in that: The child nodes that can participate in the cost function are brought into the cost function for calculation, and the child nodes with the smallest cost value are counted and placed in the open set; the node corresponding to the minimum value of f in the open set is taken out and placed in the closed set, and the node is used as the parent node for subsequent node expansion and path backtracking.

7. The multi-robot formation path planning method based on the FLA star algorithm according to claim 1, characterized in that: Backtrack to generate the reference point path and generate all robot paths based on the formation parameters, including: Starting from the node corresponding to the target point, trace back to the starting node through the parent-child node relationship. During the backtracking process, the key information of each node is recorded to form a complete path from the starting position to the target position. According to the relative position relationship between the robots and the reference point in the formation, combined with the movement path information of the reference point, the actual movement path of each robot in the formation is obtained.

8. The multi-robot formation path planning method based on the FLA algorithm according to claim 7, characterized in that: The path set generated by each robot should meet the following conditions: Each robot path has no collision in the grid map, that is, the grid states corresponding to the path points are all blank; The path set as a whole complies with the formation constraint, that is, the relative position deviation between the robots at any time is within the allowable range; The path length is globally optimal under the definition of comprehensive cost f.

9. The multi-robot formation path planning method based on the FLA algorithm according to claim 1, characterized in that: A B-spline curve is used to smooth the path of each robot. The smoothed path is then subjected to a secondary obstacle avoidance check to ensure that the new path points do not collide with obstacles in the grid map. If a collision is found, new control points are added near the collision point to make the curve bypass the obstacle and the local B-spline smoothing is performed again.

10. A multi-robot formation path planning system based on the FLA star algorithm, characterized in that: include: The grid map acquisition module is configured to: acquire a pre-built obstacle grid map and identify obstacle information by the state of the grid cells; The formation parameter setting module is configured to: set the starting point and target point in the form of the coordinates of the formation center or reference point, calculate and derive the specific starting position and target position of each robot based on the center point or reference point, and specify the formation parameters of the multi-robot formation; The path search module is configured to: initialize the FLA star algorithm and perform path search, traverse the set of grid points adjacent to the starting point in the grid map, use the cost function as the basis for node update during the traversal process, and use the node with the smallest cost value for subsequent node expansion and path backtracking; The path generation module is configured to: backtrack and generate the reference point path, generate all robot paths according to the formation parameters and perform smoothing.

Images