A multi-robot path planning method based on conflict search and speed barrier

By dividing multi-robot path planning into offline pre-planning and online real-time planning, combining conflict search and speed obstacle method, multi-robot path planning in the prior art deals with unpredictable events and 'U-type' obstacle scenarios in dynamic environments. The deadlock problem under this achieves the guarantee of independent decision-making of robots and path quality.

CN115268448BActive Publication Date: 2024-11-08DALIAN UNIV OF TECH
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210906238.5
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-07-29
Publication Date
2024-11-08
Estimated Expiration
2042-07-29

Smart Images

  • Figure CN115268448B_ABST
    Figure CN115268448B_ABST
Patent Text Reader

Abstract

The present invention discloses a multi-robot path planning method based on conflict search and speed barrier, which divides the multi-robot path planning into two stages: offline pre-planning and online real-time planning, making full use of the advantages of centralized path planning with global optimization and distributed path planning with distributed real-time execution, so as to enable the robots to have autonomous decision-making capabilities under the premise of ensuring the quality of multi-robot paths. In the offline pre-planning stage, the optimal solution for multi-robot path planning is obtained by using a conflict search algorithm; in the online real-time planning stage, each robot uses the solution obtained in the offline pre-planning stage after pre-processing as the heuristic information of the speed barrier method, so that the multi-robots execute their own path planning in a distributed manner, and overcomes the deadlock problem caused by the original speed barrier method in the "U-shaped" obstacle scene due to the absence of candidate reachable speeds, thereby improving the robustness and applicability of the algorithm, and improving the robot's ability to adapt to the environment and work efficiency.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of robot control and relates to a multi-robot path planning method based on conflict search and speed barrier. Background Art

[0002] As the scale of greenhouses and plant factories becomes larger and larger and the cost of human labor becomes higher and higher, the mechanization and intelligence of fruit and vegetable picking and transportation has become an urgent need for agricultural development in various countries. Among them, path planning between multiple robots is an important research issue. Reasonable and efficient multi-robot path planning can improve the work efficiency and economic benefits of the multi-robot system. Multi-robot path planning has practical applications in the fields of multi-robot collaboration, intelligent warehousing, traffic control and electronic games.

[0003] Multi-robot path planning refers to the process in which each robot moves from its starting position to its destination position in a given map environment while avoiding collisions with environmental obstacles and other robots.

[0004] The conflict-based search (CBS) algorithm is a type of algorithm that can find the optimal path for multi-robot path planning problems. Conflict-based search is an algorithm with a two-layer structure, the lower layer is responsible for solving the optimal path of multiple single robots under constraints, such as A* and other algorithms, and the higher layer is used to solve the path conflicts between multiple robots. The higher layer constructs a binary constraint tree. Each node in the constraint tree includes three parts: constraints, solutions, and costs. The higher layer selects nodes for expansion from the root node in order from low to high cost. If there is no path conflict in the currently expanded node, the current node is used as the target node; otherwise, the corresponding constraints are added to different robots for the conflict in the current node and the left and right child nodes are expanded. The nodes are repeatedly selected and expanded until the target node with a conflict-free solution is obtained.

[0005] The conflict-based algorithm is a centralized algorithm that is optimal for optimizing path costs, which makes it difficult to apply in actual dynamic environments. For example, the patent publication number CN113741454A, "A search-based multi-agent path planning method and system", applies the conflict-based search algorithm to a vehicle model that considers dynamics, but it still uses a centralized method, and the robot does not have complete autonomous decision-making capabilities, so it can only avoid conflicts between multiple known robots and cannot deal with unpredictable dynamic events in the environment, such as dynamic obstacles.

[0006] The velocity obstacle method (VO) is a distributed method that can safely navigate a robot to a target position, and can realize real-time navigation of multiple robots. However, this method will fall into local optimality and cause deadlock in the case of a "U-shaped" obstacle scene. For example, the patent publication number CN110597271A "A mobile robot obstacle avoidance algorithm based on a hierarchical velocity obstacle algorithm" proposes a hierarchical velocity obstacle algorithm, which combines the velocity obstacle method with the generalized velocity obstacle method. Compared with the generalized velocity obstacle method, it reduces the amount of calculation and can realize real-time navigation of multiple robots. However, this method will still fall into local optimality and cause deadlock in the case of a "U-shaped" obstacle scene, resulting in the inability to navigate the robot to its target position. Summary of the invention

[0007] In order to overcome the shortcomings of the prior art in realizing multi-robot centralized path planning based on conflict search algorithm and multi-robot real-time navigation based on speed barrier method, the present invention provides a multi-robot path planning method based on conflict search and speed barrier, realizing centralized pre-planning and distributed real-time planning of multi-robot path planning. Under the premise of ensuring the quality of multi-robot path planning solution, the method has the ability of autonomous decision-making by multi-robots to better deal with unpredictable dynamic events that may occur in the environment, such as dynamic obstacles or other robots, and can also avoid the deadlock of the speed barrier method in the "U-shaped" obstacle scene.

[0008] In order to achieve the above purpose, the present invention adopts the following technical solutions:

[0009] A multi-robot path planning method based on conflict search and speed barrier includes the following steps:

[0010] Step 1: Set the environment map, static obstacles, and the starting and target positions of each robot in the multi-robot path planning problem to be solved, and check the feasibility of the starting and target positions of each robot to ensure that they do not conflict with the static obstacles in the environment. The specific steps are as follows:

[0011] Step 1.1: Abstract the path planning problem into a 2D plane, and use the horizontal and vertical coordinates in the Cartesian coordinate system to represent the starting position and target position of each robot.

[0012] Step 1.2: Abstract the static obstacles in the map into planar rectangular areas, and abstract each robot into a planar circular area.

[0013] Step 1.3: Detect whether the starting position and the target position conflict with the static obstacle and whether they are within the map range, and detect whether each starting position and the target position conflict with each other.

[0014] Step 2: For the multi-robot path planning problem in step 1, use a conflict-based algorithm to perform offline pre-planning to obtain the optimal solution to the multi-robot path planning problem in step 1.

[0015] Step 3: pre-process the optimal path of the multi-robot path planning problem obtained in step 2 and use it as heuristic information for the multi-robots in the online real-time planning stage.

[0016] Step 4: When the robot's critical path information queue is not empty, that is, when the robot has not reached its target position, the robot takes out the next critical position information from its critical path information queue as its local target position; if the robot reaches its target position, the robot execution ends.

[0017] Step 5: Each robot calculates its expected velocity unit vector based on its current position. Then robot a i The expected velocity unit vector

[0018]

[0019] where g i and p i Respectively represent robot a i The target position and current position, ||g i -p i ||2 indicates robot a i The 2-norm of the vector difference between the target position and the current position.

[0020] Step 6: Each robot obtains information about surrounding obstacles and other robots through its own sensors, and based on its current speed v i Calculate their joint speed barrier O i .

[0021] Step 7: Each robot is based on its current speed v i And the kinematic and dynamic constraints, calculate the achievable velocity set A i (v i );

[0022] Step 8: Each robot is based on its joint speed obstacle O i and the achievable speed set A i (v i ) Calculate the new speed

[0023] Step 9: Each robot starts from its new speed Calculate the control input in , apply the new control input to the robot's actuator; determine whether the robot has reached its current local target position, if so, go to step 4; otherwise, go to step 5.

[0024] Further features, the step 2 is as follows:

[0025] Step 2.1: Use the spatiotemporal hybrid A* algorithm as a low-level path planning algorithm based on the conflict search algorithm, and perform a n} respectively call the low-level path planning algorithm to obtain the starting position of each robot from I = {s1, s2, ..., s n} to the target position G = {g1, g2, ..., g n The optimal path P = {p1,p2,...,p n}.

[0026] Step 2.2: Construct the root node R of the high-level constraint tree. The optimal path P obtained in step 2.1 is P = {p1, p2, ..., p n} as the solution of the root node R, that is, S R ={p1,p2,...,p n}; Let c i For robot a i The path p i The cost of the root node is The root node constraint in the high-level constraint is empty, that is, A priority queue for storing constraint tree nodes is maintained in the high-level constraint tree, and the cost of the constraint tree node is used as the key value; the root node R is added to the priority queue.

[0027] Step 2.3: Expand the constraint tree nodes in the priority queue. When the priority queue is not empty, take out the node N with the lowest cost for expansion, and verify whether there is a conflict in node N. If there is no conflict, go to step 2.9. If there is a conflict C, go to step 2.4.

[0028] Step 2.4, detect the critical conflict type of conflict C. For the solution of node N, verify whether there is a critical conflict in the path solution between each pair of robots, that is, whether the current paths of the two robots with conflict C are both unique shortest paths. If they are both unique shortest paths, conflict C is a critical conflict, and go to step 2.5; if conflict C is not a critical conflict, go to step 2.8;

[0029] Step 2.5: Determine the vertex or edge conflict type of conflict C. If conflict C is a vertex conflict (a i ,a j ,v,t), that is, robot a i and a j At time t, they jointly occupy vertex v, and go to step 2.6. If conflict C is an edge conflict (a i,a j ,u,v,t), that is, robot a i and a j At time t, pass through the edge (u,v) in the opposite direction and go to step 2.7.

[0030] Step 2.6: Resolve vertex conflicts. i ,a j ,v,t) in which the robot a conflicts i and a j Add vertex constraints to robot a i Add constraints (a i ,v,t) means that robot a is prohibited i At time t, it occupies position v and generates the left child node L; for robot a j Add constraints (a j ,v,t) means that robot a is prohibited j At time t, it occupies position v and generates the right child node R, and then robot a i and a j Call the low-level path planning algorithm to solve the optimal path p that meets the constraints i ' and p j ', and the robot a in the node i and a j The path is updated to p i ' and p j ', that is, S L ={p1,...p i ',...,p n} and S R ={p1,...p' j ,...,p n}, and calculate the cost value of L and R, that is, and Then add the new child nodes L and R to the priority queue; then go to step 2.3.

[0031] Step 2.7: Resolve edge conflicts. i ,a j ,u,v,t) in which the robot a conflicts i and a j Add edge constraints to robot a i Add constraints (a i ,u,v,t) means that robot a is prohibited i At time t, the edge (u, v) is occupied and the left child node L is generated; for robot a j Add constraints (a j ,u,v,t) means that robot a is prohibited jAt time t, the edge (u, v) is occupied, and the right child node R is generated. Then, robot a i and a j Call the low-level path planning algorithm to solve the optimal path p that meets the constraints i ' and p j ', and the robot a in the node i and a j The path is updated to p i ' and p j ', that is, S L ={p1,...p i ',...,p n} and S R ={p1,...p' j ,...,p n}, and calculate the cost value of L and R, that is, and Then add the new child nodes L and R to the priority queue; then go to step 2.3.

[0032] Step 2.8, find an effective detour in conflict C. Add corresponding constraints to each robot in conflict C to generate a new child node. If the cost of the new child node A is the same as the cost of the node N being expanded and the number of conflicts contained in A is smaller, it means that the solution in A contains an effective detour, then add A to the priority queue and go to step 2.3; if A does not contain an effective detour, go to step 2.5.

[0033] Step 2.9: Return the optimal path. Return the solution that does not conflict in node N, that is, As a solution to the current multi-robot path planning problem.

[0034] Further features, the step 3 is as follows:

[0035] Step 3.1: Preprocess the optimal path solved in the offline pre-planning phase As the heuristic information of each robot. After the offline pre-planning phase is completed, an optimal solution to the multi-robot path planning problem in step 1 is obtained. Before the online real-time planning phase begins, the optimal solution needs to be preprocessed and the path information of each robot needs to be simplified. Specifically, the redundant path information in the path information is removed and only the key path information is retained. The preprocessed path information is The critical path information is defined as the path positions where the path direction changes except the starting position and the target position; preprocessing reduces the memory usage and the amount of algorithm calculation without reducing the quality of the solution.

[0036] Step 3.2: Preprocess the critical path information As the heuristic information of each robot in the online real-time planning stage. Each robot maintains a critical path information queue and sends the preprocessed critical path information to the Send it to each robot A={a1,a2,...,a n}, each robot uses its own critical path information queue to save its critical path information as heuristic information for its online real-time planning phase.

[0037] Further features, the step 6 is as follows:

[0038] Step 6.1. The robot observes its surrounding environment through its own sensors, including static obstacles, other robots, and dynamic obstacles. If the robot observes m other obstacles in its surrounding environment through its own sensors (static obstacles, other robots, and dynamic obstacles are all considered obstacles), the other m obstacles are considered to have a radius in the geometric space, with the sizes of R = {r1, ..., r m}Circular area The positions and velocities of these m obstacles are L = {l1,...,l j ,...l m} and V={v1,...,v j ,...v m}, where l j and v j represent the position and velocity of obstacle j respectively.

[0039] Step 6.2: Map other obstacles into the robot's configuration space. Let a i The radius in the geometric space is r i , for the computing robot a i speed obstacle, the robot a i The other m obstacles observed are mapped to a i In the configuration space of i As a point in the geometric space, other obstacles are enlarged by r i The radius of each of the m circular regions in the geometric space can be expressed as in Indicates that the center of the circle in geometric space is at position l j The radius is r j +r i The circular area is composed of robot a i Current location i To the circular area Make two tangent lines and We get m numbers with a i Current location i Centered on and The collision cone with a plane sector as the boundary Let v i,j =v i -v j It's a robot i Relative velocity to obstacle j, λ i,j v i,j The vertex is at l i , direction is v i,j of rays.

[0040] Step 6.3, calculate robot a i Speed ​​barrier with obstacle j Speed ​​barrier Defined as the collision cone Shift to a i Current location i The velocity vector v of obstacle j is the center j The end point, i.e.

[0041]

[0042] in represents the Minkowski sum.

[0043] Let λ(p,v) represent the ray starting from position p and heading towards v, expressed as

[0044] λ(p,v)={p+tv|t≥0} (3)

[0045] Then obstacle j to robot a i The speed barrier is formally defined as

[0046]

[0047] The definition of the speed barrier states that if the robot a i Speed Then a i and obstacle j will collide at some point in the future if Then a i There will be no collision with obstacle j.

[0048] Step 6.4: Calculate the joint speed obstacle O of the robots i For robot a i m obstacles are observed. To avoid a iColliding with these m obstacles, the m speed obstacles are combined into

[0049]

[0050] It is spatially represented as the union of multiple speed barriers.

[0051] Further features, step 7 is as follows:

[0052] Step 7.1: The robot obtains its current speed v through relevant sensors i .

[0053] Step 7.2: Calculate the robot's reachable speed set A i (v i ). In reality, robots are subject to many constraints. Consider robot a i Maximum speed and maximum acceleration To satisfy the basic kinematic and dynamic constraints, the set of achievable velocities is

[0054]

[0055] where v′ i represents the new speed of the robot when the constraints are met, and Δt represents the sampling interval.

[0056] Further features, step 8 is as follows:

[0057] Step 8.1: When the environment around the robot is not crowded, there is a candidate achievable speed v′ that satisfies both conditions (1) and (2) i :(1) Joint speed barrier O of the robot i (2) In the achievable speed set A i (v i ). When there is an optional speed v′ that satisfies conditions (1) and (2) i When the combined speed barrier O i Outside and in its achievable speed set A i (v i ) select a unit vector that is equal to the desired velocity The new speed with the highest similarity The similarity of vectors can be measured by the cosine value of the angle between two vectors, that is,

[0058]

[0059] in Represents vector v′ i and The inner product of .

[0060] Step 8.2: When the environment around the robot is very crowded, there may not be a candidate achievable speed v′ that satisfies both conditions (1) and (2). i :(1) Joint speed barrier O of the robot i (2) In the achievable speed set A i (v i ). To avoid the robot in a crowded environment where there is no optional speed v′ i The algorithm allows the robots to move at their joint speed obstacle O i Internal selection candidate speed v′ i , but will be subject to corresponding penalty values, candidate speed v′ i The penalty value is a sum v′ i and The distance between them and the expected collision time t i (v′ i ) is represented by

[0061]

[0062] The factor w i It can reflect the robot's ability to respond in a crowded environment. Different robots can set different sizes. The expected collision time t i (v′ i ) can be obtained by solving the equation:

[0063]

[0064] The environment around the robot is very crowded, so there is no candidate achievable speed v′ that satisfies both conditions (1) and (2). i When, from the achievable speed set A i (v i ) Select the speed with the smallest penalty value as robot a i The new speed is expressed as

[0065]

[0066] Compared with the prior art, the present invention has the following technical effects:

[0067] 1. Divide the multi-robot path planning into two stages: offline pre-planning and online real-time planning. Make full use of the advantages of centralized path planning with global optimization and distributed path planning with distributed real-time execution. At the same time, overcome the shortcomings of centralized path planning that cannot achieve distributed execution and distributed path planning that cannot achieve global optimization. Under the premise of ensuring the quality of multi-robot paths, enable the robots to have autonomous decision-making capabilities.

[0068] 2. In the offline pre-planning stage, the conflict search algorithm is used to obtain the optimal solution for multi-robot path planning. The original path is simplified after preprocessing, and only the simplified key path information is retained as heuristic information in the online real-time planning stage, reducing the memory usage and computational complexity of the overall algorithm.

[0069] 3. The multiple robots in the system execute the heuristic information speed obstacle algorithm in a distributed manner to realize multi-robot online path planning. Since each robot has the ability to make autonomous decisions, the robot can better respond to possible dynamic events, such as other robots in the system or unpredictable events such as dynamic obstacles, thereby improving the robot's ability to adapt to the environment and work efficiency.

[0070] 4. The optimal solution of multi-robot path planning obtained based on the conflict search algorithm in the offline pre-planning stage is used as the heuristic information of the speed obstacle method in the online real-time planning stage. At the same time, it overcomes the deadlock problem caused by the lack of candidate reachable speeds for the robots in the "U-shaped" obstacle scenario in the original speed obstacle method, thereby improving the robustness of the algorithm.

[0071] The multi-robot path planning method of the present invention divides the multi-robot path planning problem into two stages: offline pre-planning and online real-time planning, making full use of the advantages of centralization and distribution. Under the premise of ensuring the quality of the multi-robot paths in the system, the robots can also have autonomous decision-making capabilities in the process of navigating to the target position, so that the robots can better respond to possible dynamic events; at the same time, the optimal solution obtained based on the conflict search algorithm is pre-processed to obtain the key path information of each robot as the heuristic information of each robot, overcoming the deadlock problem of the speed barrier method in the "U-shaped" obstacle scenario, and improving the practicality and applicability of the speed barrier method. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 It is a flow chart of the multi-robot path planning method based on conflict search and speed barriers.

[0073] Figure 2 It is a flowchart of offline pre-planning based on conflict search algorithm.

[0074] Figure 3 It is a schematic diagram of calculating the unit vector of the robot's expected velocity.

[0075] Figure 4 It is a schematic diagram of calculating the joint speed obstacles of robots.

[0076] Figure 5 This is a typical U-shaped obstacle scenario.

[0077] Figure 6These are the motion trajectories of each robot obtained using the speed obstacle method in a typical U-shaped obstacle scenario.

[0078] Figure 7 These are the motion trajectories of the robots obtained using the technical solution of the present invention in a typical U-shaped obstacle scene.

[0079] Figure 8 This is a schematic diagram of the path planning problem for ten robots in a random map scenario.

[0080] Fig. 9 It is the motion trajectory of each robot obtained by using the speed obstacle method in the random map scene.

[0081] Fig.10 It is the movement trajectory of each robot obtained by using the technical solution of the present invention in a random map scene

[0082] Fig.11 It is a schematic diagram of a scene with dynamic obstacles.

[0083] Fig.12 It is the motion trajectory of each robot obtained by using the technical solution of the present invention in a scene with dynamic obstacles. DETAILED DESCRIPTION

[0084] The specific embodiments of the present invention are described in detail below in combination with the technical solutions and the accompanying drawings.

[0085] A multi-robot path planning method based on conflict search and speed barrier is applied to multi-robot path planning in plant factories or warehouse environments. The overall flow chart of the method is as follows: Figure 1 As shown, the specific implementation steps are as follows:

[0086] Step 1: Set the environment map and its static obstacles, as well as the starting position and target position of each robot in the multi-robot path planning problem to be solved. Describe the shapes of robots and obstacles as geometric figures, specifically: obstacles are represented by rectangles or circumscribed polygons, and robots are represented by circles; detect whether there are conflicts between the starting positions of each robot and between obstacles, and whether there are conflicts between the target positions of each robot and between obstacles.

[0087] Step 2: For the multi-robot path planning problem in step 1, use the conflict-based algorithm to perform offline pre-planning to obtain the optimal solution to the multi-robot path planning problem in step 1. Figure 2 The flowchart of offline pre-planning based on conflict search algorithm is shown as follows:

[0088] Step 2.1: Call the spatiotemporal hybrid A* algorithm for each robot A = {a1, a2, ..., a n} respectively call the low-level path planning algorithm to obtain the starting position of each robot from I = {s1, s2, ..., s n} to the target position G = {g1, g2, ..., g n The optimal path P = {p1,p2,...,p n}.

[0089] Step 2.2: Construct the root node R of the high-level constraint tree. The optimal path P obtained in step 2.1 is P = {p1, p2, ..., p n} as the solution of the root node R, that is, S R ={p1,p2,...,p n}; Let c i For robot a i The path p i The cost of the root node is The root node constraint in the high-level constraint is empty, that is, A priority queue for storing high-level constraint tree nodes is maintained in the high-level constraint tree, and the cost of the constraint tree node is used as the key value; the root node R is added to the priority queue.

[0090] Step 2.3, expand the constraint tree nodes in the priority queue. When the priority queue is not empty, take out the node N with the lowest cost for expansion, and verify whether there is a conflict in node N. Specifically, perform conflict detection on the paths between all robots at each time point along the timeline. When the distance between two robots is less than the sum of the radii of the two robots at a certain time, there is a conflict between the two robots; if there is no conflict, go to step 2.9; if there is a conflict C, go to step 2.4.

[0091] Step 2.4, detect the critical conflict type of conflict C. For the solution of node N, verify whether there is a critical conflict in the path solution between each pair of robots, that is, whether the current paths of the two robots with conflict C are both unique shortest paths. If they are both unique shortest paths, conflict C is a critical conflict, and go to step 2.5; if conflict C is not a critical conflict, go to step 2.8;

[0092] Step 2.5: Determine the vertex or edge conflict type of conflict C. If conflict C is a vertex conflict (a i ,a j ,v,t), that is, robot a i and a j At time t, they occupy position v together, then robot a i and a j Add constraints (a i ,v,t) and (a j ,v,t), and then robot ai and a j Call the low-level path planning algorithm to solve the optimal path that satisfies the robot's constraints, and generate two new constraint tree nodes inherited from node N, and update robot a i and a j The solution is the newly obtained optimal path; if the conflict C is an edge conflict (a i ,a j ,u,v,t), that is, robot a i and a j At time t, when the robot passes through the edge (u,v) in the opposite direction, i and a j Add constraints (a i ,u,v,t) and (a j ,u,v,t), and then robot a i and a j Call the low-level path planning algorithm to solve the optimal path that satisfies the robot's constraints, and generate two new constraint tree nodes inherited from node N, and update robot a i and a j The solution is the newly obtained optimal path; finally, the two newly generated child nodes are added to the priority queue.

[0093] Step 2.8, find an effective detour in conflict C. Add corresponding constraints to each robot in conflict C to generate a new child node. If the cost of the new child node A is the same as the cost of the node N being expanded and the number of conflicts contained in A is smaller, it means that the solution in A contains an effective detour, then add A to the priority queue and go to step 2.3; if A does not contain an effective detour, go to step 2.5.

[0094] Step 2.9: Return the optimal path. Return the solution that does not conflict in node N, that is, As a solution to the current multi-robot path planning problem.

[0095] Step 3: Preprocess the optimal path of the multi-robot path planning problem obtained in step 2 and use it as heuristic information for the multi-robots in the online real-time planning stage. Simplify the optimal path information of each robot obtained in the offline pre-planning stage, specifically: remove the redundant path information in the path information, and only retain the key path information; the key path information is defined as the path position where the path direction changes except the starting position and the target position; preprocessing reduces the memory usage and the amount of algorithm calculation, while not reducing the quality of the solution; each robot in the system maintains a key path information queue, and sends the preprocessed key path information to each robot A = {a1, a2, ..., a n}’s critical path information queue serves as heuristic information for the robot’s online real-time planning phase.

[0096] Step 4: When the robot has not reached its target position, the robot takes out the next key position information from its key path information queue as its local target position and goes to step 5; if the robot reaches its target position, the robot execution ends.

[0097] Step 5: Each robot calculates its expected velocity unit vector based on its current position. Then robot a i The expected velocity unit vector

[0098]

[0099] where g i and p i Respectively represent robot a i The target position and current position, ||g i -p i ||2 indicates robot a i The 2-norm of the vector difference between the target position and the current position. Figure 3 As shown, if robot a i The current position is p i =(x0,y0), its target position g i =(x i ,y i ), then its expected velocity unit vector

[0100] Step 6: Each robot obtains information about surrounding obstacles and other robots through its own sensors, and based on its current speed v i Calculate their joint speed barrier O i .

[0101] Further, the step 6 is specifically as follows:

[0102] Step 6.1: The robot observes its surrounding environment through its own sensors, including static obstacles, other robots, and dynamic obstacles. If the robot observes three other obstacles in its surrounding environment through its own sensors, the other three obstacles are regarded as a circular area with a radius of R = {r1, r2, r3} in the geometric space. The positions and velocities of these three obstacles are L = {l1, l2, l3} and V = {v1, v2, v3}, where l j and v j represent the position and velocity of obstacle j respectively.

[0103] Step 6.2: Map other obstacles into the robot's configuration space. Let a i The radius in the geometric space is r i , for the computing robot a i speed obstacle, the robot a i The other three obstacles observed are mapped to a i In the configuration space of i As a point in the geometric space, other obstacles are enlarged by r i The radius of the circular area in these three geometric spaces can be expressed as in Indicates that the center of the circle in geometric space is at position l j The radius is r j +r i The circular area is composed of robot a i Current location i To the circular area Make two tangent lines and Then we got 3 i Current location i Centered on and The collision cone with a plane sector as the boundary Let v i,j =v i -v j It's a robot i Relative velocity to obstacle j, λ i,j v i,j The vertex is at l i , direction is v i,j Rays

[0104] Step 6.3, calculate robot a i Speed ​​barrier with obstacle j Speed ​​barrier Defined as the collision cone Shift to a i Current location i The velocity vector v of obstacle j is the center j The end point, i.e.

[0105]

[0106] in represents the Minkowski sum.

[0107] Let λ(p,v) represent the ray starting from position p and heading towards v, expressed as

[0108] λ(p,v)={p+tv|t≥0} (3)

[0109] Then obstacle j to robot a i The speed barrier is formally defined as

[0110]

[0111] The definition of the speed barrier states that if the robot a i Speed Then a i and obstacle j will collide at some point in the future if Then a i There will be no collision with obstacle j. Figure 4 As shown in the middle gray area, robot a i The combined speed barrier is the three speed barriers and The union of .

[0112] Step 6.4: Calculate the joint speed obstacle O of the robots i For robot a i m obstacles are observed. To avoid a i Colliding with these m obstacles, the m speed obstacles are combined into

[0113]

[0114] It is spatially represented as the union of multiple speed barriers.

[0115] Further, step 7 is specifically as follows:

[0116] Step 7.1: The robot obtains its current speed v through relevant sensors i .

[0117] Step 7.2: Calculate the robot's reachable speed set A i (v i ). In reality, robots are subject to many constraints. Consider robot a i Maximum speed and maximum acceleration To satisfy the basic kinematic and dynamic constraints, the set of achievable velocities is

[0118]

[0119] where v′ i represents the new speed of the robot when the constraints are met, and Δt represents the sampling interval.

[0120] Further, step 8 is specifically as follows:

[0121] Step 8.1: When the environment around the robot is not crowded, there is a candidate achievable speed v′ that satisfies both conditions (1) and (2) i :(1) Joint speed barrier O of the robot i (2) In the achievable speed set A i (v i ). When there is an optional speed v′ that satisfies conditions (1) and (2) i When the combined speed barrier O i Outside and in its achievable speed set A i (v i ) select a unit vector that is equal to the desired velocity The new speed with the highest similarity The similarity of vectors can be measured by the cosine value of the angle between two vectors, that is,

[0122]

[0123] in Represents vector v′ i and The inner product of .

[0124] Step 8.2: When the environment around the robot is very crowded, there may not be a candidate achievable speed v′ that satisfies both conditions (1) and (2). i :(1) Joint speed barrier O of the robot i (2) In the achievable speed set A i (v i ). To avoid the robot in a crowded environment where there is no optional speed v′ i The algorithm allows the robots to move at their joint speed obstacle O i Internal selection candidate speed v′ i , but will be subject to corresponding penalty values, candidate speed v′ i The penalty value is a sum v′ i and The distance between them and the expected collision time t i (v′ i ) is represented by

[0125]

[0126] The factor w i It can reflect the robot's ability to respond in a crowded environment. Different robots can set different sizes. The expected collision time t i (v′ i ) can be obtained by solving the equation

[0127]

[0128] Therefore, the environment around the robot is very crowded, resulting in no candidate achievable speed v′ that satisfies both conditions (1) and (2). i When, from the achievable speed set A i (v i ) Select the speed with the smallest penalty value as robot a i The new speed is expressed as

[0129]

[0130] Step 9: Each robot starts from its new speed Calculate the control input in , apply the new control input to the robot's actuator; determine whether the robot has reached its current local target position, if so, go to step 4; otherwise, go to step 5.

[0131] In order to test the performance of the method proposed in the present invention, first Figure 5 In the typical "U-shaped" obstacle scenario shown in the figure, two robots are navigated using the speed obstacle method and the multi-robot path planning method based on conflict search and speed obstacles proposed in this paper. Figure 5 The circular area in the middle represents the starting position of the robot with the corresponding label, and the rectangular area represents the target position of the robot with the corresponding label. Figure 6 and Figure 7 The motion trajectories of the two robots shown are given by Figure 6 and Figure 7 From the obtained motion trajectories of the two robots, it can be seen that the speed obstacle method alone cannot navigate the robot to the target position in the "U-shaped" obstacle scenario. However, the solution obtained by the conflict search algorithm in the offline pre-planning stage is used as the heuristic information of the speed obstacle method after preprocessing, and the robots can be navigated to their respective target positions. In order to test the advantages of the method proposed in this paper in general scenarios, random maps and multi-robot path planning problems are created as follows: Figure 8 As shown in the figure, the speed obstacle method and the method proposed in this paper are used to navigate ten robots, and the results are respectively Fig. 9 and Fig.10 The results shown in Figure 8 In the scenario shown, robots 4 and 9 encountered a "U-shaped" obstacle scenario. From the results of the two figures, we can see that the speed obstacle method can obtain Fig. 9 The motion trajectories of the robots shown in FIG. 1 and FIG. 2 cannot navigate robots 4 and 9 to their target positions when encountering a “U-shaped” obstacle scene. However, the motion trajectories of the robots shown in FIG. 2 and FIG. 2 cannot navigate robots 4 and 9 to their target positions when encountering a “U-shaped” obstacle scene. Fig.10The motion trajectories of the robots shown in the figure can navigate all robots to their target positions. In addition, since each robot has the ability to make autonomous decisions during execution, it can respond well to dynamic events in the environment. In order to verify the situation when the algorithm proposed in this paper encounters dynamic events, Fig.11 There are four robots and two dynamic obstacles in the scene shown. The dynamic obstacles are represented by black solid circular areas. In this test, the two dynamic obstacles move from top to bottom and then return to the initial starting point, and then reciprocate. Using the method proposed in this paper, we can get Fig.11 The motion trajectory of each robot in the presence of dynamic obstacles is as follows: Fig.12 As shown by Fig.12 From the motion trajectories of the robots shown, it can be seen that when a robot encounters a dynamic obstacle, it avoids collisions with other robots and dynamic obstacles by calculating that its speed is not within its joint speed obstacle set but within its reachable speed set, which confirms the effectiveness of this method in dynamic situations.

Claims

1. A multi-robot path planning method based on conflict search and speed barrier, characterized by the following steps: Step 1: Set the environment map and static obstacles, as well as the starting position and target position of each robot in the multi-robot path planning problem to be solved, and detect the feasibility of the starting position and target position of each robot to ensure that they do not conflict with the static obstacles in the environment; the specific steps are as follows: Step 1.1: Abstract the path planning problem into a 2D plane, and use the horizontal and vertical coordinates in the Cartesian coordinate system to represent the starting position and target position of each robot; Step 1.2: Abstract the static obstacles in the map into planar rectangular areas, and abstract each robot into a planar circular area; Step 1.3, detecting whether the starting position and the target position conflict with the static obstacle and whether they are within the map range, and detecting whether each starting position and the target position conflict with each other; Step 2: For the multi-robot path planning problem in step 1, use a conflict-based algorithm to perform offline pre-planning to obtain the optimal solution to the multi-robot path planning problem in step 1; Step 3, preprocessing the optimal path of the multi-robot path planning problem obtained in step 2 and using it as heuristic information for the multi-robots in the online real-time planning stage; Step 4: When the robot's key path information queue is not empty, that is, when the robot has not reached its target position, the robot takes out the next key position information from its key path information queue as its local target position; if the robot reaches its target position, the robot execution ends; Step 5: Each robot calculates its expected velocity unit vector based on its current position. Then robot a i The expected velocity unit vector where g i and p i Respectively represent robot a i The target position and current position, ||g i -p i ||2 indicates robot a i The 2-norm of the vector difference between the target position and the current position; Step 6: Each robot obtains information about surrounding obstacles and other robots through its own sensors, and based on its current speed v i Calculate their joint speed barrier O i ; Step 7: Each robot is based on its current speed v i And the kinematic and dynamic constraints, calculate the achievable velocity set A i (v i ); Step 8: Each robot is based on its joint speed obstacle O i and the achievable speed set A i (v i ) Calculate the new speed Step 9: Each robot starts from its new speed Calculate the control input in , apply the new control input to the robot's actuator; determine whether the robot has reached its current local target position, if so, go to step 4; otherwise, go to step 5.

2. A multi-robot path planning method based on conflict search and speed barrier according to claim 1, characterized in that: In step 2, the steps based on the conflict search algorithm are as follows: Step 2.1: Use the spatiotemporal hybrid A* algorithm as a low-level path planning algorithm based on the conflict search algorithm, and perform a n } respectively call the low-level path planning algorithm to obtain the starting position of each robot from I = {s1, s2, ..., s n } to the target position G = {g1, g2, ..., g n The optimal path P = {p1,p2,...,p n }; Step 2.2: Construct the root node R of the high-level constraint tree; replace the optimal path P obtained in step 2.1 with {p1, p2, ..., p n } as the solution of the root node R, that is, S R ={p1,p2,...,p n }; Let c i For robot a i The path p i The cost of the root node is The root node constraint in the high-level constraint is empty, that is, A priority queue for storing constraint tree nodes is maintained in the high-level constraint tree, and the cost of the constraint tree node is used as the key value; the root node R is added to the priority queue; Step 2.3, expand the constraint tree node in the priority queue; When the priority queue is not empty, take out the node N with the lowest cost for expansion, and verify whether there is a conflict in node N. If there is no conflict, go to step 2.

9. If there is a conflict C, go to step 2.

4. Step 2.4, detect the key conflict type of conflict C; for the solution of node N, verify whether there is a key conflict in the path solution between each pair of robots, that is, whether the current paths of the two robots with conflict C are both unique shortest paths. If they are both unique shortest paths, conflict C is a key conflict, and go to step 2.5; if conflict C is not a key conflict, go to step 2.8; Step 2.5: Determine the vertex or edge conflict type of conflict C; if conflict C is a vertex conflict (a i ,a j ,v,t), that is, robot a i and a j At time t, they jointly occupy vertex v, and go to step 2.

6. If conflict C is an edge conflict (a i ,a j ,u,v,t), that is, robot a i and a j At time t, pass through the edge (u,v) in the opposite direction and go to step 2.7; Step 2.6, resolve vertex conflicts; respectively resolve vertex conflicts (a i ,a j ,v,t) in which the robot a conflicts i and a j Add vertex constraints to robot a i Add constraints (a i ,v,t) means that robot a is prohibited i At time t, it occupies position v and generates the left child node L; for robot a j Add constraints (a j ,v,t) means that robot a is prohibited j At time t, it occupies position v and generates the right child node R, and then robot a i and a j Call the low-level path planning algorithm to solve the optimal path p that meets the constraints i ' and p j ', and the robot a in the node i and a j The path is updated to p i ' and p j ', that is, S L ={p1,...p i ',...,p n } and S R ={p1,...p' j ,...,p n }, and calculate the cost value of L and R, that is, and Then add the new child nodes L and R to the priority queue; Then go to step 2.3; Step 2.7, resolve edge conflicts; respectively, vertex conflicts (a i ,a j ,u,v,t) in which the robot a conflicts i and a j Add edge constraints to robot a i Add constraints (a i ,u,v,t) means that robot a is prohibited i At time t, the edge (u, v) is occupied and the left child node L is generated; for robot a j Add constraints (a j ,u,v,t) means that robot a is prohibited j At time t, the edge (u, v) is occupied, and the right child node R is generated. Then, robot a i and a j Call the low-level path planning algorithm to solve the optimal path p that meets the constraints i ' and p j ', and the robot a in the node i and a j The path is updated to p i ' and p j ', that is, S L ={p1,...p' i ,...,p n } and S R ={p1,...p' j ,...,p n }, and calculate the cost value of L and R, that is, and Then add the new child nodes L and R to the priority queue; Then go to step 2.3; Step 2.8, find an effective detour in conflict C; add corresponding constraints to each robot in conflict C to generate a new child node. If the cost of the new child node A is the same as the cost of the node N being expanded and the number of conflicts contained in A is smaller, it means that the solution in A contains an effective detour, then add A to the priority queue and go to step 2.3; if A does not contain an effective detour, go to step 2.5; Step 2.9, return the optimal path; Return the solution that does not conflict in node N, that is, As a solution to the current multi-robot path planning problem.

3. A multi-robot path planning method based on conflict search and speed barrier according to claim 1 or 2, characterized in that: In step 3, the optimal path of the multi-robot path planning problem obtained in step 2 using the conflict search algorithm is preprocessed and used as heuristic information for the multi-robot online real-time planning stage as follows: Step 3.1: Preprocess the optimal path solved in the offline pre-planning phase As the heuristic information of each robot; after the offline pre-planning phase is completed, an optimal solution to the multi-robot path planning problem in step 1 is obtained Before the online real-time planning phase begins, the optimal solution needs to be preprocessed and the path information of each robot needs to be simplified. Specifically, the redundant path information in the path information is removed and only the key path information is retained. The preprocessed path information is The critical path information is defined as the path position where the path direction changes except the starting position and the target position; preprocessing reduces the memory usage and the amount of algorithm calculation without reducing the quality of the solution; Step 3.2: Preprocess the critical path information As the heuristic information of each robot in the online real-time planning stage; each robot maintains a critical path information queue and sends the preprocessed critical path information Send it to each robot A={a1,a2,...,a n }, each robot uses its own critical path information queue to save its critical path information as heuristic information for its online real-time planning phase.

4. The multi-robot path planning method based on conflict search and speed barrier according to claim 3, characterized in that: In step 6, robot a i Based on its current speed v i Calculate their joint speed barrier O i Here are the steps: Step 6.1: The robot observes its surrounding environment through its own sensors, including static obstacles, other robots, and dynamic obstacles. If the robot observes m other obstacles in its surrounding environment through its own sensors, the other m obstacles are considered to have a radius in the geometric space, with the sizes of R = {r1, ..., r m }Circular area The positions and velocities of these m obstacles are L = {l1,...,l j ,...l m } and V={v1,...,v j ,...v m }, where l j and v j represent the position and velocity of obstacle j respectively; Step 6.2: Map other obstacles into the robot's configuration space; let a i The radius in the geometric space is r i , for the computing robot a i speed obstacle, the robot a i The other m obstacles observed are mapped to a i In the configuration space of i As a point in the geometric space, other obstacles are enlarged by r i The radius of each of the m circular regions in the geometric space is expressed as in Indicates that the center of the circle in geometric space is at position l j The radius is r j +r i The circular area is composed of robot a i Current location i To the circular area Make two tangent lines and We get m numbers with a i Current location i Centered on and The collision cone with a plane sector as the boundary Let v i,j =v i -v j It's a robot i Relative velocity to obstacle j, λ i,j v i,j The vertex is located at l i , direction is v i,j of rays; Step 6.3, calculate robot a i Speed ​​barrier with obstacle j Speed ​​barrier Defined as the collision cone Shift to a i Current location i The velocity vector v of obstacle j is the center j The end point, i.e. in represents the Minkowski sum; Let λ(p,v) represent the ray starting from position p and heading towards v, expressed as λ(p,v)={p+tv|t≥0} (3) Then obstacle j to robot a i The speed barrier is formally defined as The definition of the speed barrier states that if the robot a i Speed Then a i and obstacle j will collide at some point in the future if Then a i There will be no collision with obstacle j; Step 6.4: Calculate the joint speed obstacle O of the robots i ; For robot a i m obstacles are observed. To avoid a i Colliding with these m obstacles, the m speed obstacles are combined into It is spatially represented as the union of multiple speed barriers.

5. A multi-robot path planning method based on conflict search and speed barrier according to claim 1, 2 or 4, characterized in that: In step 7, robot a i Calculate its achievable speed set A i (v i )The steps are as follows: Step 7.1: The robot obtains its current speed v through relevant sensors i ; Step 7.2: Calculate the robot's reachable speed set A i (v i ); In reality, robots are subject to many constraints. Consider robot a i Maximum speed and maximum acceleration To satisfy the basic kinematic and dynamic constraints, the set of achievable velocities is where v' i represents the new speed of the robot when the constraints are met, and Δt represents the sampling interval.

6. A multi-robot path planning method based on conflict search and speed barrier according to claim 5, characterized in that: In step 8, robot a i Calculate its new speed Here are the steps: Step 8.1: When the environment around the robot is not crowded, there is a candidate achievable speed v' that satisfies both conditions (1) and (2) i :(1) Joint speed barrier O of the robot i (2) In the achievable speed set A i (v i ); when there is an optional speed v' that satisfies conditions (1) and (2) i When the combined speed barrier O i Outside and in its achievable speed set A i (v i ) and select a unit vector that is equal to the desired velocity. The new speed with the highest similarity The similarity of vectors is measured by the cosine of the angle between the two vectors, that is, in Represents vector v' i and The inner product of Step 8.2: When the environment around the robot is very crowded, there may not be a candidate achievable speed v' that satisfies both conditions (1) and (2). i :(1) Joint speed barrier O of the robot i (2) In the achievable speed set A i (v i ) within; To avoid the robot in a crowded environment where there is no optional speed v' i The algorithm allows the robots to move at their joint speed obstacle O i Select candidate speed v' i , but will be subject to corresponding penalty values, candidate speed v' i The penalty value is a sum v' i and The distance between them and the expected collision time t i (v' i ) is represented by The factor w i Reflects the robot's ability to respond in a crowded environment. Different robots have different sizes. The expected collision time t i (v i ') is obtained by solving the equation; The environment around the robot is very crowded, so there is no candidate achievable speed v' that satisfies both conditions (1) and (2). i When, from the achievable speed set A i (v i ) Select the speed with the smallest penalty value as robot a i The new speed is expressed as