Dynamic path planning method, system and equipment based on multiple mobile robots and medium

Abstract

The invention belongs to the technical field of path planning, and provides a dynamic path planning method, system and equipment based on multiple mobile robots and a medium based on the multiple mobile robots in order to solve the problems that the calculated amount is increased sharply and unexpected situations cannot be handled in the current path planning of the multiple mobile robots, and the independent path planning of each robot is completed by adopting mixed A *-epsillon. A dual-domain priority queue is introduced, child nodes are generated through node expansion, then a path is re-planned for the robot with constraints, and therefore a conflict-free path of the robot is generated; and performing control sequence sampling on each robot, taking a conflict-free space-time path as a reference path, obtaining an optimal control sequence according to the cost value of each predicted trajectory, and adjusting the path in real time through rolling optimization so as to cope with a dynamic obstacle. The method can actively avoid a dynamic obstacle or an obstacle which is not considered during planning.

Description

Technical Field

[0001] The present invention belongs to the technical field related to robot path planning and cooperative control, and particularly relates to a dynamic path planning method, system, device and medium for multi-mobile robots. Background Technique

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

[0003] With the improvement of the demand for intelligence and automation, multi-mobile robot systems have shown great potential in industrial production, intelligent transportation, warehousing logistics, national defense and military, etc. Such systems can coordinate the cooperative operation of multiple autonomous mobile units, improve efficiency, reduce labor costs, and reduce operation errors. Among them, multi-agent path finding (MAPF) is an important research topic in multi-mobile robot systems.

[0004] The multi-agent path planning problem mainly aims to find the optimal path for a group of agents in a shared environment, requiring each agent to reach its target position according to the plan without colliding with other agents. The conflict-based search (CBS) algorithm is a popular two-level algorithm for solving the multi-agent path planning problem. Among them, the high-level searches for conflicts among a group of robots to impose constraints on each robot; the low-level finds the optimal solution to the single-agent problem under the constraints imposed by the high-level. Traditional CBS algorithms often use grid maps, idealize the kinematics of robots as omnidirectional wheel models, enabling them to translate and turn arbitrarily; simplify the shape of robots into point masses that only occupy one grid, and can only choose 4 or 8 adjacent grids for each movement. In reality, most mobile robots are Ackermann models, with limitations on the minimum turning radius, unable to execute the paths generated by traditional CBS, and their volumes cannot be ignored. At the same time, when facing complex scenarios or an increasing number of robots, the computational complexity increases sharply, making it difficult to meet the real-time requirements. In addition, although there are already CBS algorithms that satisfy the robot kinematic model, there is no controller adapted to the algorithm, especially when facing sudden situations, such as dynamic obstacles or obstacles that suddenly appear during the movement of the robot. These obstacles are not considered in the path planning stage, and their appearance may make the original planning results infeasible.

[0005] In summary, there are problems such as a sharp increase in computational complexity and the inability to handle sudden situations in current multi-mobile robot path planning. Summary of the Invention

[0006] To overcome the deficiencies of the above-mentioned existing technologies, the present invention provides a dynamic path planning method, system, device and medium for multiple mobile robots, ensuring that the robot swarm does not collide with each other during operation and can respond to dynamic environments in real time.

[0007] To achieve the above object, the present invention adopts the following technical solutions: In the first aspect, the present invention provides a dynamic path planning method for multiple mobile robots, including: Based on the initial poses of the robots and the static obstacle information in the environmental map, use the hybrid A*-epsilon algorithm to separately plan spatio-temporal paths for each robot that conform to the Ackermann kinematic model; According to the spatio-temporal paths of each robot, detect spatio-temporal conflicts between the paths of multiple robots, and construct a dual-domain priority queue including a global domain and a focus domain. Start node expansion from the root node of the global domain. Each expanded node is determined by the focus domain. Each expanded child node uses the hybrid A*-epsilon algorithm to re-plan the path for the constrained robot until a spatio-temporal path without any conflicts is obtained; Perform control sequence sampling on each robot to obtain a predicted trajectory within a finite future time. Use the spatio-temporal path without any conflicts as a reference path, calculate the cost value of each predicted trajectory, and then obtain an optimal control sequence and adjust the path in real time through rolling optimization to cope with dynamic obstacles.

[0008] In the second aspect, the present invention provides a dynamic path planning system for multiple mobile robots, including: The first path planning is configured to: based on the initial poses of the robots and the static obstacle information in the environmental map, use the hybrid A*-epsilon algorithm to separately plan spatio-temporal paths for each robot that conform to the Ackermann kinematic model; The second path planning is configured to: according to the spatio-temporal paths of each robot, detect spatio-temporal conflicts between the paths of multiple robots, and construct a dual-domain priority queue including a global domain and a focus domain. Start node expansion from the root node of the global domain. Each expanded node is determined by the focus domain. Each expanded child node uses the hybrid A*-epsilon algorithm to re-plan the path for the constrained robot until a spatio-temporal path without any conflicts is obtained; The control module is configured to: perform control sequence sampling on each robot to obtain a predicted trajectory within a finite future time. Use the spatio-temporal path without any conflicts as a reference path, calculate the cost value of each predicted trajectory, and then obtain an optimal control sequence and adjust the path in real time through rolling optimization to cope with dynamic obstacles.

[0009] In a third aspect, the present invention provides an electronic device, including a memory, a processor, and computer instructions stored on the memory and running on the processor. When the computer instructions are run by the processor, the method described in the first aspect is completed.

[0010] In a fourth aspect, the present invention provides a computer-readable storage medium for storing computer instructions. When the computer instructions are executed by a processor, the method described in the first aspect is completed.

[0011] The above one or more technical solutions have the following beneficial effects: In the present invention, hybrid A*-epsilon is adopted to complete the path planning of each robot individually, then a dual-domain priority queue is introduced to eliminate conflicts, and then child nodes are generated through node expansion. Furthermore, hybrid A*-epsilon is used to re-plan the path of the robot with constraints, so as to generate a conflict-free path with time information suitable for the Ackermann kinematic model robot. The spatio-temporal constraints ensure that the robot swarm will not collide with each other during operation; the control sequence of each robot is sampled to obtain the predicted trajectory within a finite future time. The spatio-temporal path without any conflicts is used as the reference path to obtain the cost value of each predicted trajectory, and the optimal control sequence is obtained and the path is adjusted in real time through rolling optimization to cope with dynamic obstacles. The present invention can respond to dynamic environments or model errors in real time. When facing dynamic obstacles or obstacles not considered during planning, it can actively avoid them and quickly return to the original trajectory after avoidance.

[0012] The advantages of the additional aspects of the present invention will be partially given in the following description, partially become obvious from the following description, or be understood through the practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0014] Figure 1 It is the overall flowchart of the method for dynamic path planning of multiple mobile robots in the first embodiment of the present invention; Figure 2 It is the schematic diagram of the dual-domain priority queue structure in the first embodiment of the present invention; Fig. 3(a) is the simulation effect diagram of spatio-temporal constraint MPPI at t = 0s in a static environment in the first embodiment of the present invention; Fig. 3(b) is the simulation effect diagram of spatio-temporal constraint MPPI at t = 11s in a static environment in the first embodiment of the present invention; Fig. 3(c) is the simulation effect diagram of spatio-temporal constraint MPPI at t = 22s in a static environment in the first embodiment of the present invention; Figure 3(d) shows the simulation effect diagram of spatio-temporal constrained MPPI at t = 33s in a static environment in the first embodiment of the present invention; Figure 4(a) shows the simulation effect diagram of spatio-temporal constrained MPPI under the interference of dynamic obstacles at t = 5s in the first embodiment of the present invention; Figure 4(b) shows the simulation effect diagram of spatio-temporal constrained MPPI under the interference of dynamic obstacles at t = 10s in the first embodiment of the present invention; Figure 4(c) shows the simulation effect diagram of spatio-temporal constrained MPPI under the interference of dynamic obstacles at t = 15s in the first embodiment of the present invention; Figure 4(d) shows the simulation effect diagram of spatio-temporal constrained MPPI under the interference of dynamic obstacles at t = 20s in the first embodiment of the present invention; Figure 5(a) shows the velocity-time schematic diagram in the first embodiment of the present invention; Figure 5(b) shows the front-wheel angle-time schematic diagram in the first embodiment of the present invention. Detailed implementation manners

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

[0016] It should be noted that the terms used herein are only for describing specific implementation manners and are not intended to limit the exemplary implementation manners according to the present invention.

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

[0018] Embodiment 1 This embodiment discloses a dynamic path planning method based on multiple mobile robots, including: Based on the initial pose of the robot and the static obstacle information in the environment map, use the hybrid A*-epsilon algorithm to respectively plan a spatio-temporal path that conforms to the Ackermann kinematic model for each robot; According to the spatio-temporal paths of each robot, detect the spatio-temporal conflicts between the paths of multiple robots, and construct a dual-domain priority queue including a global domain and a focus domain. Start node expansion from the root node of the global domain. Each expanded node is determined by the focus domain. Each expanded child node re-plans the path for the robot with constraints using the hybrid A*-epsilon algorithm until a spatio-temporal path without any conflicts is obtained; Sample the control sequence for each robot to obtain the predicted trajectory within a finite future time. Use the spatio-temporal path without any conflicts as the reference path, calculate the cost value of each predicted trajectory, and then obtain the optimal control sequence and adjust the path in real time through rolling optimization to cope with dynamic obstacles.

[0019] In this embodiment, hybrid A*-epsilon is used to complete the path planning for each robot individually. Then, a dual-domain priority queue is introduced to eliminate conflicts. Then, child nodes are generated through node expansion. Furthermore, hybrid A*-epsilon is used to re-plan the path for the robot with constraints, so as to generate a conflict-free path with time information suitable for the robot of the Ackermann kinematic model. The spatio-temporal constraints ensure that the robot swarm will not collide with each other during operation. Sample the control sequence for each robot to obtain the predicted trajectory within a finite future time. Use the spatio-temporal path without any conflicts as the reference path, and detect dynamic obstacles in real time. Obtain the cost value of each predicted trajectory, obtain the optimal control sequence and adjust the path in real time through rolling optimization to cope with dynamic obstacles. The solution of this embodiment can respond to dynamic environments or model errors in real time. When facing dynamic obstacles or obstacles not considered during planning, it can actively avoid them and quickly return to the original trajectory after avoidance.

[0020] The following combines Figure 1 to elaborate in detail on the dynamic path planning method for multiple mobile robots provided in this embodiment: Step 1: Based on the initial pose of the robot and the information of static obstacles in the environment map, use the hybrid A*-epsilon algorithm to separately plan a spatio-temporal path for each robot that conforms to the Ackermann kinematic model.

[0021] Collect map information through sensors such as lidar, cameras, or depth sensors, use the simultaneous localization and mapping technology SLAM to generate the environment map, load the map information, obtain the shape, size, and position of static obstacles, obtain the initial pose and target pose of each robot, and ensure that there is no overlap in the positions of the robots to ensure the rationality of the planning task. The form of the robot pose is , where x , y is the coordinate of the center point of the rear axle of the robot chassis in the global coordinate system, is the heading angle of the robot, that is, the direction the vehicle is facing.

[0022] For each robot, use the hybrid A*-epsilon algorithm to generate a path with time information that conforms to its kinematics. This path only considers the collision between the robot and static obstacles in the environment, and allows possible collisions between the paths of each robot. The paths of all robots together constitute a solution to the MAPF.

[0023] Each robot uses the hybrid A*-epsilon algorithm for separate path planning, specifically including: Step 11: Set an open list Open_List to arrange the nodes to be expanded in ascending order of cost values. Each node, that is, each robot, contains the state t at the current moment and its cost value; set a closed list Close_List to record the expanded nodes to avoid repeated processing; additionally, set a focal list Focal_List as a subset of Open_List to save the nodes whose cost values are not greater than times the minimum cost, that is:

[0024] where ranges from (0, 1). In this embodiment, is taken as 0.3, represents the node with the minimum cost value in the open list Open_List, n' refers to the node in the open list Open_List; while n refers to the node selected from the open list Open_List that meets the conditions of the focal list Focal_List.

[0025] Each time a node is expanded, select the node with the minimum focal heuristic value from the focal list Focal_List with lower cost values. The focal heuristic value is used to measure the number of conflicts between this node and other robots. The definition of conflict is , meaning t at time the robot collides with the robot located at . The determination method of collision is that the distance between the centers of the circumcircles of the two robots is less than the sum of their circumradius.

[0026] Different from the traditional hybrid A* algorithm, the way to expand nodes is no longer to select the node with the minimum cost value, but to select the node with the minimum focal heuristic value from the focal list Focal_List with lower cost values. The focal heuristic value reflects the number of conflicts between this node and the paths of other robots.

[0027] The open list Open_List and the focal list Focal_List are implemented using a Fibonacci heap. This heap is provided with an interface by the Boost library in C++. It has good time complexity characteristics and can meet the frequent priority sorting and addition, deletion, modification, and query operations of the list.

[0028] Step 12: Add the initial node, i.e., the starting point of the robot, to the open list Open_List and the focal list Focal_List, and start expanding nodes from the initial node.

[0029] For a robot adopting the Ackermann kinematic model, there are a total of seven motion primitives for node expansion: moving straight forward, moving forward with a left turn, moving forward with a right turn, moving straight backward, moving backward with a left turn, moving backward with a right turn, and waiting in place. The robot pose updates are as follows: Moving straight forward:

[0030] Moving forward with a left turn:

[0031] Moving forward with a right turn:

[0032] Among them, S is the expansion step size, R is the minimum turning radius, is the change in the heading angle, ([[]] x, y ) represents the current coordinate position of the robot, θ represents the heading angle of the robot.

[0033] Moving straight backward, moving backward with a left turn, and moving backward with a right turn are similar to their corresponding forward actions but in the opposite direction.

[0034] In particular, waiting in place is used to reserve the possibility of avoiding other robots when dealing with conflicts. The time t of the node is composed of the time of the parent node plus the unit time. In this embodiment, the unit time is set to 1 second.

[0035] For the newly generated node, if there is no conflict with the static obstacle, perform the following operations: Step 121: If the newly generated node is already in the closed list Close_List, it means it has been explored and will no longer be considered.

[0036] Step 122: If the newly generated node is not in the open list Open_List, it means the newly generated node has never been explored. Calculate its cost value:

[0037] Among them, represents the actual cost of the current node, specifically the value of the parent node plus the path length of the current node's action; The heuristic estimated cost represents the estimated cost from the current node to the target node, i.e., the end position of the robot. It is calculated as the larger value between the Euclidean distance and the Reeds-Shepp distance between the newly generated node and the target node.

[0038] Then calculate the focus heuristic value of the newly generated node, which is the total number of collisions that occur at the same time as other robots for the currently newly generated node. This value is used to determine the next node to be expanded in the Focal_List. After the calculation is completed, add the newly generated node to the Open_List, and decide whether to add it to the Focal_List according to its value.

[0039] Step 123: If the newly generated node is already in the Open_List, and the value obtained by expanding through the parent node of the newly generated node is smaller, then update the of this node in the Open_List.

[0040] After expanding a node once, add the parent node to the Close_List.

[0041] Compared with the hybrid A* algorithm, the hybrid A*-epsilon makes the algorithm more relaxed during the search process by introducing a relaxation factor , thus avoiding overly strict path exploration. While ensuring that the path is close to the optimal one, it pays more attention to the solutions with fewer conflicts, significantly reducing the calculation time.

[0042] Step 13: Repeatedly perform the operation of expanding nodes in Step 12 until the following conditions are met: 1) The Euclidean distance between the node position and the target node position is small enough, i.e., less than the set value, and the size of the set value can be set according to requirements; 2) The node position and the target node position can be directly connected by a Reeds-Shepp curve, and the curve does not conflict with obstacles, then the single-robot path planning is completed.

[0043] Step 2: According to the spatio-temporal paths of each robot, detect the spatio-temporal conflicts between the multi-robot paths, and construct a two-domain priority queue including the global domain and the focus domain. Start node expansion from the root node of the global domain. Each expanded node is determined by the focus domain. Each expanded child node pair uses the hybrid A*-epsilon algorithm to re-plan the path for the robots with constraints until spatio-temporal paths without any conflicts are obtained; where the global domain is a complete binary heap; the focus domain filters out sub-optimal nodes from the global domain and re-orders them according to the number of conflicts.

[0044] Such as Figure 2As shown in the figure, for constructing the dual-domain priority queue and expanding nodes in step 2, the specific steps are as follows: Step 21: Each node in the global domain consists of a MAPF solution composed of all robot paths, node cost, focus heuristic value, and a spatio-temporal constraint. Among them, the node cost represents the sum of all path costs, the focus heuristic value is the total number of pairwise conflicts between all robots, and the spatio-temporal constraint stems from the conflict between two robots and is defined as , which means that the pose of the i th robot at time t cannot be pose.

[0045] The MAPF solution of the root node in the global domain is the initial solution planned in step 1, and the constraint is empty. That is, according to step 1, paths are planned for each robot separately, and the result of the path planning for each robot is used as the root node of the global domain. The focus domain is a subset of the global domain and is used to store nodes whose node costs are not greater than w times the minimum cost.

[0046] Step 22: Start from the root node of the global domain and perform node expansion.

[0047] In chronological order, detect the first conflict in the node. The conflict consists of two robots and is defined as , which means that at time t, the robot in pose and the robot in pose will collide. According to this conflict, generate two child nodes. The node constraints correspond to the two robots involved in the conflict, which are and .

[0048] For each child node, re-execute the hybrid A*-epsilon algorithm in step 1 for the robots involved in the constraints, and calculate the node cost and focus heuristic value of the child node. Determine whether the newly generated child node is added to the focus domain according to the node cost of the child node. Since the child nodes generated in the global domain are actually re-planning the robots involved in the constraints using A*-epsilon while keeping the trajectories of other robots unchanged, and recalculating the node cost and focus heuristic value. Because the re-planning takes into account the constraints generated according to the conflict, this conflict is resolved.

[0049] Among them, when expanding new child nodes, first generate spatio-temporal constraints according to the conflicts of the old nodes, re-plan the robots involved in the constraints using A*-spsilon, and replace the old trajectories of these robots. While the trajectories of other robots remain unchanged. The MAPF solution is the set of all robot trajectories, and the node cost and focus heuristic value are calculated based on the trajectories of all robots. The MAPF solution, node cost, focus heuristic value, and spatio-temporal constraints of the nodes are all different.

[0050] Step 23: Select the node with the smallest focus heuristic value in the focus domain for expansion. Since a smaller focus heuristic value means fewer conflicts in the node, expanding this node can find a conflict-free feasible solution more quickly. Expand the node according to the method in Step 22 until a multi-robot spatio-temporal path without any conflicts is obtained.

[0051] When selecting and expanding nodes, it is more inclined to calculate paths with smaller computational costs but not strictly optimal. The cost of the finally obtained solution is no greater than w times the optimal cost. It is this relaxation mechanism that broadens the search breadth, significantly reduces the search space while keeping the quality of the solution controllable, and improves the operating efficiency.

[0052] Step 3: Sample the control sequence for each robot to obtain the predicted trajectory within a finite future time.

[0053] The kinematic model of the Ackerman-type chassis is as follows:

[0054] where, 、 is the Cartesian coordinate of the rear axle of the robot chassis at time t, is the heading angle at time t, L is the wheelbase of the chassis, v is the speed of the robot, is the front wheel steering angle. The range of the control quantity is limited: ; . The discrete time step , the prediction horizon , then the length of the predicted control sequence . This system is a non-linear time-varying system.

[0055] Step 31: Linearly interpolate the obtained conflict-free spatio-temporal path to ensure that the time interval between the interpolated trajectory points is consistent with the control time step of MPPI, i.e., the model predictive path integral.

[0056] Step 32: Use sensors such as lidar, cameras, or millimeter-wave radars to collect environmental data in real time, and combine methods such as point cloud processing, object detection, and Kalman filtering to extract the coordinate information of dynamic obstacles.

[0057] Step 33: Since a large number of control sequences need to be sampled and the sequence cost needs to be calculated, to accelerate the calculation speed and meet the real-time requirement, it is necessary to call GPU for parallel computing. Allocate and initialize relevant variables in the GPU, including the parameters of the MPPI algorithm and the obstacle coordinate information, and at the same time generate M arrays of control sequences. The form of each control sequence is:

[0058] Among them, , it is indicated that the control variables are the speed and front wheel angle of the robot; let the control discrete time step be , and the prediction time domain is T, then the length of the predicted control sequence .

[0059] Step 34: Generate multiple samplings for each control input , that is:

[0060] Among them, obeys a two-dimensional normal distribution with a mean of and a covariance of ; among them, represents a random variable, is the covariance matrix of the random variable, which is given artificially.

[0061] Step 45: Input each sampled control sequence into the system kinematic model to predict the corresponding sampled trajectory:

[0062] Among them, .

[0063] Step 4: Use the spatio-temporal path without any conflicts as the reference path, obtain the cost value of each predicted trajectory, obtain the optimal control sequence and adjust the path in real time through rolling optimization to deal with dynamic obstacles.

[0064] As shown in Figures 5(a) - 5(b), the schematic diagrams of the speed and front wheel angle in the optimal control sequence versus time.

[0065] The specific steps for Step 4 are as follows: Step 41: Calculate the cost value for each sampled trajectory. For the t-th moment, the cost function adopted in this embodiment is:

[0066]

[0067] Among them, represents the pose error cost, ， ， is the t + i pose of the reference trajectory at the moment; and

[0068]

[0069] Among them, represents the obstacle cost, is the obstacle cost weight. To ensure that the robot can avoid dynamic obstacles, the value is usually several orders of magnitude larger than to increase the penalty for encountering dynamic obstacles.

[0070]

[0071] Among them, represents the control regularization cost to limit excessive fluctuations in the control input. is the regularization weight, and are the sampled speed and front wheel steering angle values.

[0072] Given the parallel nature of the MPPI algorithm, this embodiment makes full use of the large-scale parallel computing power of the GPU. Through the CUDA parallel computing platform, each thread block is responsible for the calculation of a control sequence, and the thread blocks run independently without sequential sampling calculation, so that the calculation of thousands of control sequences can be quickly completed.

[0073] Step 42: Calculate the weights according to the cost values of each sampling sequence , for the i-th sampling sequence:

[0074] Among them, is the minimum cost value among all sampling sequences, is the temperature parameter, and its size directly affects the optimization degree of the control input. The larger the value, the stronger the constraint on control smoothness, but a certain degree of flexibility may be sacrificed; represents i the cost value of the -th sampling sequence. In this embodiment,

[0075] is taken as 1. :

[0076] Among them, M is the total number of sampling sequences, represents the control sequence of the i -th sampling sequence.

[0077] Step 44: Perform a moving average filter on the obtained optimal control sequence .

[0078] Establish a moving window with a length of n, and all values within the window are Move the window from the start point to the end point of the sequence, and calculate the moving average value at each position according to the following formula :

[0079] where is the optimal control sequence in the j th control sequence

[0080] For the head and tail of the sequence, since the sliding window cannot completely cover, the average value of the edge data may be small. Therefore, according to the actual window size involved in the calculation, its average value is scaled up to the level of the complete window. The moving average filtering effectively reduces the high-frequency jitter in the control sequence and makes the robot path smoother

[0081] Step 45: Spatiotemporal constrained MPPI is a method of rolling optimization. In each iteration, only the control quantity of the first step, that is, the optimal control at the current moment, is executed, and then the environmental information is re-obtained and the optimal control sequence is updated. As time progresses, the optimization window also slides forward until the robot reaches the target pose. Rolling optimization not only improves the real-time performance of the algorithm, but also enhances the adaptability to dynamic obstacles and changing targets

[0082] The simulation of spatiotemporal constrained MPPI in a static environment is shown in Figures 3(a)-3(d). The number of robots is 20, and the map size is 100m*100m. The gray circles in the figure are static obstacles, the dashed lines are the reference spatiotemporal paths, the solid lines are the actual trajectories of the robots' movements, the dashed boxes represent the poses at the end of the paths, and the clusters of lines in front of the robots represent the predicted trajectories under different sampled control sequences; the simulation results under the interference of dynamic obstacles are shown in Figures 4(a)-4(d). The number of robots is 5, and the map size is 50m*50m. The black circles in the figure represent dynamic obstacles, and the obstacles move in a uniform straight line at a speed of 0.5m / s. The simulation results show that the robots can actively avoid when encountering dynamic obstacles and can quickly return to the reference path after avoidance, proving that the method proposed in this embodiment has good robustness

[0083] This embodiment uses the idea of hierarchical solution. At the lower level, a hybrid A*-epsilon is used to complete the single-robot planning. At the higher level, a dual-domain priority queue is introduced to resolve conflicts. By relaxing the selection conditions of the optimal nodes in the focus domain, the algorithm efficiency is improved. As a result, a conflict-free path with time information suitable for the Ackermann kinematic model robot is generated, that is, at the same moment, any two robots will not appear at the same position. This spatio-temporal constraint ensures that the robot swarm will not collide with each other during operation, and also poses higher requirements on the control algorithm. Spatio-temporal constrained MPPI is a sampling-based method. By sampling a large number of control sequences at each time step, the path integral method is used to evaluate the cost of each trajectory and generate weights, and then the best control strategy is obtained by weighted averaging. Since spatio-temporal constrained MPPI directly performs sampling optimization based on the nonlinear dynamic model of the system, it does not require linearization of the model, and the cost function is designed flexibly. Therefore, it can handle complex nonlinear systems, can respond to dynamic environments or model errors in real time. When facing dynamic obstacles or obstacles not considered in the planning, it can actively avoid them and quickly return to the original trajectory after avoidance.

[0084] Embodiment 2 The purpose of this embodiment is to provide a dynamic path planning system for multiple mobile robots, including: The first path planning is configured to: based on the initial pose of the robot and the static obstacle information in the environment map, use the hybrid A*-epsilon algorithm to respectively plan a spatio-temporal path that conforms to the Ackermann kinematic model for each robot; The second path planning is configured to: according to the spatio-temporal paths of each robot, detect spatio-temporal conflicts between the paths of multiple robots, and construct a dual-domain priority queue including a global domain and a focus domain. Start node expansion from the root node of the global domain. Each expanded node is determined by the focus domain. Each expanded child node uses the hybrid A*-epsilon algorithm to re-plan the path for the robot with constraints until a spatio-temporal path without any conflicts is obtained; The control module is configured to: sample the control sequence for each robot to obtain the predicted trajectory within a finite future time. Use the spatio-temporal path without any conflicts as the reference path, calculate the cost value of each predicted trajectory, and then obtain the optimal control sequence and adjust the path in real time through rolling optimization to cope with dynamic obstacles.

[0085] In more embodiments, there is also provided: An electronic device includes a memory, a processor, and computer instructions stored on the memory and running on the processor. When the computer instructions are run by the processor, the method described in Embodiment 1 is completed. For the sake of brevity, it will not be elaborated here.

[0086] It should be understood that in this embodiment, the processor may be a central processing unit (CPU), or the processor may also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor, or the processor may also be any conventional processor, etc.

[0087] The memory may include a read-only memory and a random access memory, and provide instructions and data to the processor. A part of the memory may also include a non-volatile random access memory. For example, the memory may also store information about the device type.

[0088] A computer-readable storage medium for storing computer instructions, which when executed by the processor, implement the method described in the first embodiment.

[0089] The method in the first embodiment can be directly implemented by a hardware processor, or by a combination of hardware and software modules in the processor. The software module may be located in a mature storage medium in the art, such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory, or an electrically erasable programmable memory, a register, etc. This storage medium is located in the memory, and the processor reads the information in the memory and combines its hardware to complete the steps of the above method. To avoid repetition, it will not be described in detail here.

[0090] Those of ordinary skill in the art can realize that the units and algorithm steps of the examples described in conjunction with this embodiment can be implemented by electronic hardware or a combination of computer software and electronic hardware. Whether these functions are executed in a hardware or software manner depends on the specific application and design constraints of the technical solution. Professional technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of this application.

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

Claims

1. A dynamic path planning method based on multiple mobile robots, characterized in that, Including: Based on the initial pose of the robot and the information of static obstacles in the environment map, use the hybrid A*-epsilon algorithm to separately plan a spatio-temporal path for each robot that conforms to the Ackermann kinematic model; According to the spatio-temporal paths of each robot, detect the spatio-temporal conflicts between multi-robot paths, and construct a two-domain priority queue including a global domain and a focus domain. Start node expansion from the root node of the global domain. Each expanded node is determined by the focus domain. Each expanded child node re-plans the path for the constrained robot using the hybrid A*-epsilon algorithm until a spatio-temporal path without any conflicts is obtained; Perform control sequence sampling on each robot to obtain a predicted trajectory within a finite future time. Use the spatio-temporal path without any conflicts as the reference path, calculate the cost value of each predicted trajectory, and then obtain the optimal control sequence and adjust the path in real time through rolling optimization to cope with dynamic obstacles.

2. The dynamic path planning method for multiple mobile robots according to claim 1, wherein Using the hybrid A*-epsilon algorithm to separately plan a spatio-temporal path for each robot that conforms to the Ackermann kinematic model, specifically: Create an open list, a closed list, and a focus list; among them, the focus list is used to store nodes in the open list whose cost values are not greater than times the minimum cost, is a relaxation factor, and the nodes include the robot state at the current moment and the corresponding cost values; Based on the motion primitives of the robot when using the Ackermann kinematic model, expand the nodes until the Euclidean distance between the node position and the target point position is less than the set value, and the connection between the node position and the target point position is a Reeds-Shepp curve and the Reeds-Shepp curve does not conflict with static obstacles, completing the spatio-temporal path planning of a single robot.

3. The dynamic path planning method for multiple mobile robots according to claim 2, wherein When performing node expansion, for the newly generated node, if it does not conflict with static obstacles, perform the following operations: If the newly generated node is already in the closed list, it indicates that the newly generated node has been explored; If the newly generated node is not in the open list, calculate the cost value and focus heuristic value of the newly generated node, and add the newly generated node to the open list, and determine whether to add it to the focus list according to the calculated cost value; If the newly generated node is in the open list and the cost value obtained by expanding the parent node of the newly generated node is less than the cost value of the newly generated node, update the cost value of the newly generated node in the open list. After completing a round of node expansion, add the parent node of the newly generated node to the closed list.

4. The dynamic path planning method for multiple mobile robots according to claim 1, wherein According to the spatio-temporal paths of each robot, detect the spatio-temporal conflicts between multi-robot paths, and construct a two-domain priority queue including a global domain and a focus domain. Start node expansion from the root node of the global domain. Each expanded node is determined by the focus domain. Each expanded child node re-plans the path for the constrained robot using the hybrid A*-epsilon algorithm until a spatio-temporal path without any conflicts is obtained, specifically: The two-domain priority queue consists of a global domain and a focus domain. Each node in the global domain includes the path solution composed of all robot paths, the node cost, the focus heuristic value, and the spatio-temporal constraints; Starting from the root node of the global domain, detect the spatio-temporal conflicts of the nodes, generate child nodes, re-use the hybrid A*-epsilon algorithm for path planning for the robots with constraints for each child node, and calculate the node cost and the focus heuristic value of the newly generated child nodes, and determine whether to add them to the focus domain according to the node cost of the newly generated child nodes; Select the node with the smallest focus heuristic value from the focus domain for expansion until a spatio-temporal path without any conflicts is obtained.

5. The dynamic path planning method for multiple mobile robots according to claim 1, characterized in that, Use the spatio-temporal path without any conflicts as a reference path, obtain the cost value of each predicted trajectory, obtain the optimal control sequence and adjust the path in real time through rolling optimization to cope with dynamic obstacles, specifically; Use the spatio-temporal path without any conflicts as a reference path, obtain the cost value of each predicted trajectory; among them, the cost function of the predicted trajectory includes pose error cost, dynamic obstacle cost and control regularization cost; Determine the weights according to the cost values of each predicted trajectory, and obtain the optimal control sequence through weighted average; Update the robot state according to the optimal control sequence, and adjust the path in real time through rolling optimization to cope with dynamic obstacles.

6. The dynamic path planning method for multiple mobile robots according to claim 5, characterized in that, Determine the weights according to the cost values of each predicted trajectory, and obtain the optimal control sequence through weighted average, specifically: Calculate the weights of each predicted trajectory by means of exponential weights; Perform weighted average on all predictions, and perform moving average filtering on the sequence obtained by weighted average to form the optimal control sequence.

7. The dynamic path planning method for multiple mobile robots according to any one of claims 1-6, characterized in that The focus heuristic value is used to measure the number of conflicts between the node and other robots. The conflict is defined as ,for t At the moment Robot with Robot A collision occurs. The collision is determined when the distance between the centers of the two robots' circumscribed circles is less than the sum of the radii of their circumscribed circles.

8. The multi-mobile robot-based dynamic path planning system is characterized in that, Including: The first path planning is configured to: based on the initial pose of the robot and the static obstacle information in the environment map, use the hybrid A*-epsilon algorithm to respectively plan the spatio-temporal paths that conform to the Ackermann kinematic model for each robot; The second path planning is configured to: according to the spatio-temporal paths of each robot, detect the spatio-temporal conflicts between the multi-robot paths, and construct a dual-domain priority queue including a global domain and a focus domain, start node expansion from the root node of the global domain, and each expanded node is determined by the focus domain. For each expanded child node, re-plan the path for the robot with constraints by using the hybrid A*-epsilon algorithm until a spatio-temporal path without any conflicts is obtained; The control module is configured to: sample the control sequence for each robot to obtain the predicted trajectories within a finite future time, use the spatio-temporal path without any conflicts as a reference path, calculate the cost value of each predicted trajectory, and then obtain the optimal control sequence and adjust the path in real time through rolling optimization to cope with dynamic obstacles.

9. An electronic device, characterized in that, Including a memory, a processor, and computer instructions stored on the memory and running on the processor. When the computer instructions are run by the processor, the method described in any one of claims 1-7 is completed.

10. A computer-readable storage medium, characterized in that, For storing computer instructions, when the computer instructions are executed by the processor, the method described in any one of claims 1-7 is completed.

Images