Robot formation planning method based on five-dimensional configuration space

Abstract

The application belongs to the field of path planning, and particularly relates to a robot formation planning method based on a five-dimensional configuration space. The method comprises the following steps: workspace and configuration space definition: the workspace is a two-dimensional plane for robot movement, and the configuration space is a five-dimensional space containing formation centroid, direction and scaling factor; sampling method: random sampling in the configuration space, and ensuring that the sampling points do not cause collision between robots; distance function: defining the distance between two points in the configuration space, and eliminating the dependence on the number of robots; search: searching in the configuration space using an algorithm, and constructing a collision-free path tree; path generation: converting the path in the configuration space into the path in the robot workspace, and smoothing using a spline curve. The robot formation can pass through a narrow channel, and split and merge when encountering a large obstacle. The application provides a high-efficiency, flexible and collision-free global path planning method for a multi-robot system.

Description

Technical Field

[0001] This invention belongs to the field of robot path planning, specifically a robot formation planning method based on five-dimensional configuration space. Background Technology

[0002] Multi-robot systems have wide applications in disaster relief, firefighting, agriculture, surveillance, and exploration. Robots need to navigate their environments and perform various tasks, such as reaching a target location independently or collaboratively. Robot formation path planning aims to plan a collision-free path for multi-robot systems, enabling all robots to reach the target configuration from the initial configuration while maintaining formation.

[0003] Existing multi-robot formation path planning algorithms are mainly divided into two categories:

[0004] Centralized path planning algorithm: This method treats all robots as a whole and uses a single robot motion planning algorithm to plan the path for the entire group. This method needs to consider collisions between robots, so the dimension of the configuration space increases with the number of robots, resulting in low computational efficiency.

[0005] Distributed path planning algorithm: This method plans a path independently for each robot and considers the priorities between robots to resolve path conflicts. This method is more suitable for situations where robots move independently, but its efficiency is low when robots need to move collaboratively. Summary of the Invention

[0006] This invention proposes a robot formation planning method based on five-dimensional configuration space. This algorithm is based on... An algorithm is proposed, providing a flexible and efficient representation of formation geometry independent of the number of robots. The algorithm aims to generate optimal paths for multi-robot systems, incorporating the formation centroid, orientation, and scaling. An affine transformation is used to convert the path from configuration space to the robot's workspace. The algorithm employs a novel distance function, eliminating the need for weight adjustments, and utilizes a scaling factor sampling scheme to avoid collisions between robots.

[0007] The technical solution adopted by the present invention to achieve the above objectives is as follows:

[0008] The robot formation planning method based on five-dimensional configuration space includes the following steps:

[0009] Define the robot's workspace and configuration space separately;

[0010] Under the condition of satisfying the sampling constraints, random sampling is performed in the configuration space, and the distance between sampling points is calculated by the distance function;

[0011] Based on the distance between sampling points, using The algorithm searches the configuration space and constructs a collision-free path tree;

[0012] The path in the configuration space is converted into a path in the robot's workspace, and then smoothed using spline curves to complete the global path planning for robot formation.

[0013] The workspace and configuration space are specifically as follows:

[0014] The workspace The two-dimensional plane in which the robot moves. , For real numbers, the robot In robot formation The position in the middle is , , These are the x and y coordinates in the workspace, respectively. All robot positions The matrix is ​​defined as the pose of the queue;

[0015] The configuration space For a five-dimensional space including the formation centroid, orientation, and scaling, Its size is Irrelevant It is a point in configuration space. , As the coordinates of the formation's center of mass, For formation orientation, , These are the scaling factors for the x and y directions, respectively, representing the scaling ratio of the formation in the x and y directions.

[0016] In the configuration space, the reference pose It is the position matrix of all robots in the robot formation, corresponding to the configuration state of the reference pose. for Each configuration state q in the configuration space corresponds to a unique pose. This pose is obtained by referencing the pose. Obtained by performing an affine transformation. Corresponding to a configuration Transformed pose Corresponding to configuration state q, in the transformed pose, the robot Location Corresponding to q, that is:

[0017]

[0018] in, It's a robot. Position in the reference pose.

[0019] The sampling constraints are specifically as follows:

[0020] By limiting the scaling factor, collisions between robots are prevented. The lower bound of the scaling factor is defined as a function of the robot size and an additional safety margin. and The radii including the safety margin are respectively and The robot is valid if and only if the following function is satisfied. Not with Collision occurred:

[0021]

[0022]

[0023] in, and It is a constant;

[0024] For robot formation The total limitation is , including two and The upper limit and two requirements for them Non-negativity constraint Linear constraints come from two-dimensional real space A feasible region consisting of a bounded subset of polygons is used to sample robots so that they do not collide.

[0025] The distance function is specifically:

[0026] The distance function defines the proximity between two formation poses in the workspace, assuming... For robots Position in the reference pose and It corresponds to the workspace location and Two configuration states, robot grouping exist and Total distance traveled between for:

[0027]

[0028] in, , , It is a constant:

[0029] ; ;

[0030] in, denoted as x and y, respectively, represent the distances of the i-th robot in the x and y directions.

[0031] The use The algorithm searches the configuration space and includes the following steps:

[0032] (1) Initialize the collision-free path tree T with the initial configuration state as the root node, and use Under sampling constraints, the function uniformly samples random points in the configuration space of a polygon. Indicates scaling factor The square of, in the direction of Random sampling within the site, with the centroid randomly sampled based on the site size limitation;

[0033] (2) Use The function converts the configuration state into a formation pose and checks for point collisions at each robot position in the known obstacle map. It adds the robot formation states that did not experience collisions to free space. If the sampled configuration state... In free space, then use Find the node closest to tree T ;

[0034] (3) Use The function gives a value from the initial five-dimensional configuration space. In Nodes in direction and through Function found , It is located in Given a set of all neighbors within radius k, select the parent node with the lowest total path cost and no collisions. As a set Find the parent node of all elements in the list, thus obtaining the path from the start node to the end node. The shortest collision-free path;

[0035] (4) Use The function will edge Add to tree T; tree construction stops if the target is found or the user-defined number of nodes cannot be added.

[0036] (5) Use The function checks the conditions when the robot reaches the target area and returns the five-dimensional collision-free path in the configuration state.

[0037] A robot formation planning system based on five-dimensional configuration space includes the following:

[0038] The space definition module is used to define the robot's workspace and configuration space, respectively.

[0039] The random sampling module is used to randomly sample in the configuration space under the condition of satisfying the sampling constraints, and calculate the distance between the sampling points through the distance function;

[0040] The spatial search module is used to search based on the distance between sampling points. The algorithm searches the configuration space and constructs a collision-free path tree;

[0041] The path conversion module is used to convert paths in the configuration space into paths in the robot's workspace and smooth them using spline curves to complete global path planning for robot formation.

[0042] A robot formation planning device based on five-dimensional configuration space includes a memory and a processor; the memory is used to store a computer program; the processor is used to implement the robot formation planning method based on five-dimensional configuration space when the computer program is executed.

[0043] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the robot formation planning method based on five-dimensional configuration space.

[0044] The present invention has the following beneficial effects and advantages:

[0045] 1. This invention proposes a robot formation planning method based on five-dimensional configuration space, which can effectively plan collision-free paths for multi-robot systems, enabling robots to collectively and safely reach the target area. This algorithm is particularly suitable for scenarios where robots need to maintain formation while performing tasks, such as monitoring and group transportation.

[0046] 2. Compared to centralized path planning algorithms that treat the robot as a whole, the algorithm of this invention abstracts the multi-robot system as a polygon and uses affine transformations to map the path from the configuration space to the robot's workspace. This representation makes the dimension of the configuration space independent of the number of robots, thereby improving computational efficiency.

[0047] 3. The global path planning algorithm for robot formation based on sampling methods uses a new distance function that eliminates the need for manual weight adjustment and makes distance calculation independent of the number of robots.

[0048] 4. A uniform random sampling scheme was adopted to generate scaling parameters, thereby avoiding collisions between robots and ensuring the safety of the planned path. Attached Figure Description

[0049] Figure 1 The main flowchart of this invention.

[0050] Figure 2 A schematic diagram of the reference pose and affine transformation.

[0051] Figure 3 A schematic diagram of collision constraints between robots.

[0052] Figure 4 Flowchart of the sampling method.

[0053] Figure 5 based on Flowchart of the algorithm's path search. Detailed Implementation

[0054] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments.

[0055] A robot formation planning method based on five-dimensional configuration space includes the following steps:

[0056] Workspace and configuration space are defined as follows: Workspace is a two-dimensional plane in which the robot moves, and configuration space is a five-dimensional space that includes the formation centroid, orientation, and scaling factor.

[0057] Sampling method: Random sampling is performed in the configuration space, ensuring that the sampling points do not cause collisions between robots;

[0058] Distance function: Defines the distance between two points in configuration space, eliminating the dependence on the number of robots; Search: Use The algorithm searches the configuration space and constructs a collision-free path tree;

[0059] Path generation: Convert the path in configuration space into a path in the robot workspace and smooth it using spline curves.

[0060] The workspace and configuration space are specifically defined as follows:

[0061] The workspace is defined as the two-dimensional plane in which the robot moves, using express. Robots Position in robot formation , Indicates the position of all robots in the queue. The matrix is ​​defined as the pose of the formation. The configuration space is defined as a five-dimensional space containing the formation centroid, orientation, and scaling. This indicates that its size is similar to Irrelevant. It is a point in configuration space, defined as follows:

[0062]

[0063] In the formula For the center of gravity of the formation, For formation orientation, , These are the scaling factors for the x and y directions, respectively, representing the scaling ratio of the formation in the x and y directions.

[0064] Reference pose This is the position matrix of all robots in the formation. The selection of these positions should reflect the desired formation shape, and the formation centroid in the reference pose should be located at the origin o, i.e., the reference orientation pose will be set to zero. The configuration state corresponding to the reference pose. for Each configuration state q in the configuration space corresponds to a unique pose. This pose can be used as a reference pose. The affine transformation is used for calculation. Corresponding to a configuration Transformed pose Corresponding to configuration state q, in the transformed pose, robot's position Corresponding to q, we get the following:

[0065]

[0066] in yes The robot's position in the reference pose. The choice of configuration space parameters allows the robot system to translate, change its size, and self-orient, which makes it move more efficiently toward the target configuration while avoiding obstacles, rather than in rigid formation.

[0067] The sampling method is as follows:

[0068] Randomly sample the configuration space and add new nodes to the tree. Each node represents a configuration state, including the centroid, scaling factor, and polygon orientation. Sampling constraints are defined to ensure no collisions between robots. The centroid is sampled within the field boundaries, and the orientation... Between 0 and 2 Variations between radians.

[0069] Collisions between robots are only possible when scaling, not rotating. Furthermore, the scaling cap does not affect robot collisions because a larger polygon scale means the robot (the vertices of the polygon) moves further. Therefore, it can be safely defined using communication constraints or the size of the arena.

[0070] The lower bound of the scaling factor must be defined as a function of the robot size and the additional safety margin. Let... The robot's radius (including safety margin) is , The robot's radius is , Robots do not A collision occurs if and only if,

[0071]

[0072]

[0073] in and It is a constant. Let x and y be the distances of the i-th robot in the x and y directions, respectively. Let be the distances of the j-th robot in the x and y directions, respectively. Therefore, the above formula is... and Linearly constrained systems. For The general limitation of robots is , including two and The upper limit and two requirements for them Non-negativity constraint. Linear constraints come from two-dimensional real space A feasible region consisting of a bounded subset of polygons. Sampling of this region ensures that no collisions occur between robots.

[0074] The distance function is specifically as follows:

[0075] The distance function defines the proximity between two formation poses in the workspace. A natural approach is to calculate it as the total Euclidean distance traveled by each robot, but this becomes dependent on the number of robots. This paper shows that the total distance between two poses in the workspace can also be represented by the two configuration states of the formation pose, thus eliminating this dependency. Let... for The robot's position in the reference pose is set as follows: and It corresponds to the workspace location and Two configuration states. Robots The position in the middle is ,exist The position in the middle is . Robot in configuration and The Euclidean distance between them is:

[0076]

[0077] Robots and Total distance traveled between for:

[0078]

[0079] therefore Robot in configuration and The Euclidean distance between them can be written as:

[0080]

[0081] in This represents the vector dot product operator.

[0082]

[0083]

[0084] From the above formula, we can obtain the following results:

[0085]

[0086] in , , It is a constant and can be pre-calculated using reference formation coordinates, as shown below:

[0087] ; ;

[0088] use Calculating distance does not require converting configuration states into workspace positions, making the algorithm highly efficient because distance calculations no longer depend on the number of robots.

[0089] The aforementioned The search specifically refers to:

[0090] Adopting standards The algorithm acts as a path planner, searching the five-dimensional configuration space. The initial configuration state, reference pose, sampling constraints, and target configuration state are the inputs to the path planner.

[0091] (1) Initialize tree T with the initial configuration state as the root node. The sampling constraints for the centroid depend on the site size and the robot size. Use the function Random points within the polygon are uniformly sampled, as defined by the scaling factor constraint given in claim 3. Sampling points Indicates scaling factor The square of, in the direction of Random sampling is performed within the site, and random sampling of the centroid is limited by the size of the site.

[0092] (2) Use The function converts the configuration state into a formation pose and checks for point collisions at each robot position in the known obstacle map. If the sampled configuration state... In free space, then use Find the node closest to tree T .

[0093] (3) Use The function gives a value from a given value in five-dimensional space. of Nodes in direction .Then turn up , It is located in The set of all neighbors within radius k. Final selection. As a set Find the parent node of all elements in the list, thus obtaining the path from the start node to the end node. The shortest collision-free path.

[0094] (4) Use The function will edge Add to tree T. Tree construction stops if the target is found or the user-defined number of nodes cannot be added.

[0095] (5) Use The function checks the conditions when the robot reaches the target area and returns the five-dimensional collision-free path in the configuration state.

[0096] Example

[0097] Figure 1 This is the main flowchart of the technical solution of this invention. For example... Figure 1 As shown, the present invention proposes a robot formation planning method based on five-dimensional configuration space, which includes the following steps:

[0098] (1) Definition of workspace and configuration space: The workspace is a two-dimensional plane in which the robot moves, and the configuration space is a five-dimensional space that includes the formation centroid, orientation and scaling factor.

[0099] (2) Sampling method: Random sampling in configuration space, and ensuring that the sampling points do not cause collisions between robots.

[0100] (3) Determine if the sampling nodes collide: If the nodes do not collide, add the nodes to the list. In the tree, otherwise return (2) to resample.

[0101] (4) Search: Use The algorithm searches the configuration space to construct a collision-free path tree, enabling multiple robots to reach the target point without collision.

[0102] Figure 2 This is a schematic diagram of the reference pose and affine transformation. For example... Figure 2 As shown, reference pose This is the position matrix of all robots in the formation. The selection of these positions should reflect the desired formation shape, and the formation centroid in the reference pose should be located at the origin o, i.e., the reference orientation pose will be set to zero. The configuration state corresponding to the reference pose. for Each configuration state q in the configuration space corresponds to a unique pose. This pose can be used as a reference pose. The affine transformation is used for calculation. Corresponding to a configuration Transformed pose Corresponding to configuration state q, in the transformed pose, robot's position Corresponding to q, we get the following:

[0103]

[0104] in yes The robot's position in the reference pose. The choice of configuration space parameters allows the robot system to translate, change its size, and self-orient, which makes it move more efficiently toward the target configuration while avoiding obstacles, rather than in rigid formation.

[0105] Figure 3 This is a schematic diagram of collision constraints between robots. (Example) Figure 3As shown, collisions between robots can only occur when the polygon is scaled, not rotated. Furthermore, the scaling cap does not affect collisions between robots because a larger polygon scale means the robot (the vertices of the polygon) moves further. Therefore, it can be safely defined using communication constraints or the size of the field.

[0106] The lower bound of the scaling factor must be defined as a function of the robot size and the additional safety margin. Let... The robot's radius (including safety margin) is , The robot's radius is , Robots do not A collision occurs if and only if,

[0107]

[0108]

[0109] in and It is a constant. Therefore, the above equation is a... and Linearly constrained systems. For The general limitation of robots is , including two and The upper limit and two requirements for them Non-negativity constraint. Linear constraints come from two-dimensional real space A feasible region consisting of a bounded subset of polygons. Sampling of this region ensures that no collisions occur between robots.

[0110] Figure 4 This is a flowchart of the sampling method. For example... Figure 4 As shown, the sampling method of the present invention includes the following steps:

[0111] (1) Randomly select nodes in the configuration space.

[0112] (2) Randomly sample the scaling factor, random sampling position and direction within the polygonal region that satisfies the collision avoidance constraint.

[0113] (3) Check for collisions. Check whether the scaling factor, position, and orientation meet the collision avoidance constraints. If the collision avoidance constraints are met, output the sampling results. Otherwise, return to (2) and resample.

[0114] Figure 5 yes A flowchart of the path search process. (e.g.) Figure 5 As shown, based on The path search method of the algorithm includes the following steps:

[0115] (1) Create an empty tree T containing a root node whose configuration state is the initial configuration state. Define the range of configuration space sampling constraints, define the robot safety region radius array, and define the reference pose. .

[0116] (2) Repeat the sampling steps until a configuration state that meets the conditions is found. Sample the scaling factor within the feasible region of the polygon defined by the scaling factor constraint; randomly sample the orientation and center of gravity to ensure that the center of gravity is within the field; convert the configuration state to the working state and check whether each robot position in the working state collides with the obstacle; if there is no collision, assign it to the configuration state.

[0117] (3) Add collision-free nodes to tree T, using the traditional method. The algorithm reselects parent nodes and reconnects until the target point is found.

Claims

1. A robot formation planning method based on five-dimensional configuration space, characterized in that, Includes the following steps: Define the robot's workspace and configuration space separately; Under the condition of satisfying the sampling constraints, random sampling is performed in the configuration space, and the distance between sampling points is calculated by the distance function; Based on the distance between sampling points, using The algorithm searches the configuration space and constructs a collision-free path tree; The path in the configuration space is converted into a path in the robot's workspace, and then smoothed using spline curves to complete the global path planning for robot formation.

2. The robot formation planning method based on five-dimensional configuration space according to claim 1, characterized in that, The workspace and configuration space are specifically as follows: The workspace The two-dimensional plane in which the robot moves. , For real numbers, the robot In robot formation The position in the middle is , , These represent the x and y coordinates in the workspace. All robot positions The matrix is ​​defined as the pose of the queue; The configuration space For a five-dimensional space including the formation centroid, orientation, and scaling, Its size is Irrelevant It is a point in configuration space. , As the coordinates of the formation's center of mass, For formation orientation, , These are the scaling factors for the x and y directions, respectively, representing the scaling ratio of the formation in the x and y directions.

3. The robot formation planning method based on five-dimensional configuration space according to claim 2, characterized in that, In the configuration space, the reference pose It is the position matrix of all robots in the robot formation, corresponding to the configuration state of the reference pose. for Each configuration state q in the configuration space corresponds to a unique pose. This pose is obtained by referencing the pose. Obtained by performing an affine transformation. Corresponding to a configuration Transformed pose Corresponding to configuration state q, in the transformed pose, the robot Location Corresponding to q, that is: in, It's a robot. Position in the reference pose.

4. The robot formation planning method based on five-dimensional configuration space according to claim 1, characterized in that, The sampling constraints are specifically as follows: By limiting the scaling factor, collisions between robots are prevented. The lower bound of the scaling factor is defined as a function of the robot size and an additional safety margin. and The radii including the safety margin are respectively and The robot is valid if and only if the following function is satisfied. Not with Collision occurred: in, and It is a constant; For robot formation The total limitation is , including two and The upper limit and two requirements for them Non-negativity constraint Linear constraints come from two-dimensional real space A feasible region consisting of a bounded subset of polygons is used to sample robots so that they do not collide.

5. The robot formation planning method based on five-dimensional configuration space according to claim 1, characterized in that, The distance function is specifically: The distance function defines the proximity between two formation poses in the workspace, assuming... For robots Position in the reference pose and It corresponds to the workspace location and Two configuration states, robot grouping exist and Total distance traveled between for: in, , , It is a constant: ； ； in, denoted as x and y, respectively, represent the distances of the i-th robot in the x and y directions.

6. The robot formation planning method based on five-dimensional configuration space according to claim 1, characterized in that, The use The algorithm searches the configuration space and includes the following steps: (1) Initialize the collision-free path tree T with the initial configuration state as the root node, and use Under sampling constraints, the function uniformly samples random points in the configuration space of a polygon, with the sampling points... Indicates scaling factor The square of, in the direction of Random sampling within the site, with the centroid randomly sampled based on the site size limitation; (2) Use The function converts the configuration state into a formation pose and checks for point collisions at each robot position in the known obstacle map. It adds the robot formation states that did not experience collisions to free space. If the sampled configuration state... In free space, then use Find the node closest to tree T ; (3) Use The function gives a value from the initial five-dimensional configuration space. In Nodes in direction and through Function found , It is located in Given a set of all neighbors within radius k, select the parent node with the lowest total path cost and no collisions. As a set Find the parent node of all elements in the list, thus obtaining the path from the start node to the end node. The shortest collision-free path; (4) Use The function will edge Add to tree T; tree construction stops if the target is found or the user-defined number of nodes cannot be added. (5) Use The function checks the conditions when the robot reaches the target area and returns the five-dimensional collision-free path in the configuration state.

7. A robot formation planning system based on five-dimensional configuration space, characterized in that, Including the following: The space definition module is used to define the robot's workspace and configuration space, respectively. The random sampling module is used to randomly sample in the configuration space under the condition of satisfying the sampling constraints, and calculate the distance between the sampling points through the distance function; The spatial search module is used to search based on the distance between sampling points. The algorithm searches the configuration space and constructs a collision-free path tree; The path conversion module is used to convert paths in the configuration space into paths in the robot's workspace and smooth them using spline curves to complete global path planning for robot formation.

8. A robot formation planning device based on five-dimensional configuration space, characterized in that, It includes a memory and a processor; the memory is used to store a computer program; the processor is used to implement the robot formation planning method based on five-dimensional configuration space as described in any one of claims 1-6 when the computer program is executed.

9. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, which, when executed by a processor, implements the robot formation planning method based on five-dimensional configuration space as described in any one of claims 1-6.

Images