Multi-mobile robot dynamic path planning method, system, device and medium

Abstract

The present application belongs to the technical field of path planning, in order to solve the problem of calculation amount increasing and unable to handle sudden conditions in current multi-mobile robot path planning, a dynamic path planning method, system, device and medium based on multi-mobile robot are proposed, hybrid A* - epsilon is used to complete the path planning of each robot separately, a double-domain priority queue is introduced, a sub-node is generated through node expansion, and the path of the robot with constraints is re-planned to generate a conflict-free path of the robot; the control sequence of each robot is sampled, the conflict-free space-time path is taken as a reference path, the optimal control sequence is obtained according to the generation value of each predicted trajectory, and the path is adjusted in real time through rolling optimization to cope with dynamic obstacles. When facing dynamic obstacles or obstacles not considered during planning, the present application can actively avoid.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of robot path planning and cooperative control, and particularly relates to a multi-mobile robot dynamic path planning method, system, device and medium. BACKGROUND

[0002] The statements in this section merely provide background information related to the present application and do not necessarily constitute prior art.

[0003] With the increasing demand for intelligence and automation, multi-mobile robot systems have shown great potential in industrial production, intelligent transportation, warehouse 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 operational errors. Among them, multi-agent path planning (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 multi-agent path planning problems. The high-level search finds 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 a grid map, idealize robot kinematics as an omnidirectional wheel model, allowing it to translate and turn arbitrarily; simplify the robot shape to a point that only occupies one grid, and can only choose 4 or 8 adjacent grids for each move. However, most real-world mobile robots are Ackerman models with a minimum turning radius constraint, which cannot follow the paths generated by traditional CBS, and the volume cannot be ignored. At the same time, when facing complex scenarios or increasing the number of robots, the computational load increases dramatically, making it difficult to meet real-time requirements. In addition, although there are CBS algorithms that meet the kinematic model of the robot, there is no controller that adapts to the algorithm, especially when facing unexpected situations such as dynamic obstacles or obstacles that appear suddenly during robot movement. These obstacles are not considered in the path planning stage, and their appearance may make the original planning result infeasible.

[0005] In summary, there are currently problems such as a sharp increase in computational load and the inability to handle unexpected situations in multi-mobile robot path planning. SUMMARY

[0006] In order to overcome the above-mentioned deficiencies of the prior art, the application provides a multi-mobile robot dynamic path planning method, system, device and medium, which ensures that the robot group will not collide with each other during operation and can respond to dynamic environment in real time.

[0007] In order to achieve the above-mentioned purpose, the application adopts the following technical solutions:

[0008] In the first aspect, the application provides a multi-mobile robot dynamic path planning method, comprising:

[0009] Based on the initial pose of the robot and the static obstacle information in the environment map, a hybrid A*-epsilon algorithm is used to plan a space-time path conforming to the Ackermann kinematic model for each robot respectively;

[0010] According to the space-time path of each robot, the space-time conflict between the paths of the multi-robot is detected, and a two-domain priority queue including a global domain and a focus domain is constructed, node expansion is started from the root node of the global domain, the node expanded each time is determined by the focus domain, each expanded child node re-plans the path for the robot with constraints by using the hybrid A*-epsilon algorithm, and until a space-time path without any conflict is obtained;

[0011] The control sequence sampling is performed on each robot to obtain a predicted trajectory in a limited future time, the space-time path without any conflict is taken as a reference path, the cost value of each predicted trajectory is calculated, and then the optimal control sequence is obtained and the path is adjusted in real time by rolling optimization to respond to the dynamic obstacle.

[0012] In the second aspect, the application provides a multi-mobile robot dynamic path planning system, comprising:

[0013] The first path planning is configured to plan a space-time path conforming to the Ackermann kinematic model for each robot respectively by using a hybrid A*-epsilon algorithm based on the initial pose of the robot and the static obstacle information in the environment map;

[0014] The second path planning is configured to detect the space-time conflict between the paths of the multi-robot according to the space-time path of each robot, 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, determine the node expanded each time by the focus domain, re-plan the path for the robot with constraints by using the hybrid A*-epsilon algorithm for each expanded child node, and until a space-time path without any conflict is obtained;

[0015] A control module configured to: sample a control sequence for each robot to obtain a predicted trajectory in a limited future time, take the time-space path without any conflict as a reference path, calculate a 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.

[0016] In a third aspect, the present application provides an electronic device comprising a memory and a processor, and computer instructions stored in the memory and running on the processor, when the computer instructions are run by the processor, the method of the first aspect is completed.

[0017] In a fourth aspect, the present application provides a computer readable storage medium for storing computer instructions, when the computer instructions are executed by the processor, the method of the first aspect is completed.

[0018] The above one or more technical solutions have the following beneficial effects:

[0019] In the present application, hybrid A*-epsilon is used to complete the path planning of each robot alone, then a double-domain priority queue is introduced to eliminate conflicts, then a node is expanded to generate a child node, and then 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 Ackerman kinematic model robot, and the time-space constraint ensures that the robot group will not collide with each other during operation; the control sequence of each robot is sampled to obtain a predicted trajectory in a limited future time, the time-space path without any conflict is taken as a reference path, the cost value of each predicted trajectory is obtained, the optimal control sequence is obtained, and the path is adjusted in real time through rolling optimization to cope with dynamic obstacles. The present application can cope with dynamic environment or model error in real time, and can actively avoid when facing dynamic obstacles or obstacles not considered during planning, and quickly return to the original trajectory after avoiding.

[0020] The advantages of the additional aspects of the present application will be partially given in the following description, partially will become obvious from the following description, or will be known by the practice of the present application. BRIEF DESCRIPTION OF DRAWINGS

[0021] The drawings accompanying the specification of the present application form a part thereof, serve to provide further understanding of the present application, and together with the description of the exemplary embodiments of the present application and their description serve to explain the present application, and do not constitute improper limitations on the present application.

[0022] Figure 1 It is a whole flow chart of the dynamic path planning method based on multiple mobile robots in the embodiment one of the present application;

[0023] Figure 2 It is a double-domain priority queue structure schematic diagram in the embodiment one of the present application;

[0024] Fig. 3(a) is a simulation result of the spatiotemporal constraint MPPI at t=0s in the static environment according to the first embodiment of the present application;

[0025] Fig. 3(b) is a simulation result of the spatiotemporal constraint MPPI at t=11s in the static environment according to the first embodiment of the present application;

[0026] Fig. 3(c) is a simulation result of the spatiotemporal constraint MPPI at t=22s in the static environment according to the first embodiment of the present application;

[0027] Fig. 3(d) is a simulation result of the spatiotemporal constraint MPPI at t=33s in the static environment according to the first embodiment of the present application;

[0028] Fig. 4(a) is a simulation result of the spatiotemporal constraint MPPI at t=5s in the dynamic obstacle interference according to the first embodiment of the present application;

[0029] Fig. 4(b) is a simulation result of the spatiotemporal constraint MPPI at t=10s in the dynamic obstacle interference according to the first embodiment of the present application;

[0030] Fig. 4(c) is a simulation result of the spatiotemporal constraint MPPI at t=15s in the dynamic obstacle interference according to the first embodiment of the present application;

[0031] Fig. 4(d) is a simulation result of the spatiotemporal constraint MPPI at t=20s in the dynamic obstacle interference according to the first embodiment of the present application;

[0032] Fig. 5(a) is a speed-time diagram according to the first embodiment of the present application;

[0033] Fig. 5(b) is a front wheel angle-time diagram according to the first embodiment of the present application. DETAILED DESCRIPTION

[0034] It should be noted that the following detailed description is merely exemplary in nature and is intended to provide further description of the application. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

[0035] It is also to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting.

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

[0037] Embodiment One

[0038] The present embodiment discloses a dynamic path planning method based on multiple mobile robots, comprising:

[0039] Based on the initial pose of the robot and the static obstacle information in the environment map, a hybrid A*-epsilon algorithm is used to plan a space-time path for each robot in accordance with the Ackermann kinematic model.

[0040] According to the space-time path of each robot, space-time conflicts between the paths of the multiple robots are detected, and a two-domain priority queue including a global domain and a focus domain is constructed. Node expansion is performed from the root node of the global domain, and each expanded node is determined by the focus domain. Each expanded child node is used to re-plan a path for the robot with constraints by using the hybrid A*-epsilon algorithm until a space-time path without any conflict is obtained.

[0041] For each robot, a control sequence is sampled to obtain a predicted trajectory in a limited future time. The space-time path without any conflict is used as a reference path, and the cost value of each predicted trajectory is calculated to obtain an optimal control sequence and adjust the path in real time by rolling optimization to cope with dynamic obstacles.

[0042] In this embodiment, the hybrid A*-epsilon is used to complete the individual path planning of each robot, and then a two-domain priority queue is introduced to eliminate conflicts. Then, child nodes are generated by node expansion, and the hybrid A*-epsilon is used to re-plan a path for the robot with constraints, thereby generating a conflict-free path with time information for the robot in accordance with the Ackermann kinematic model. The space-time constraints ensure that the robot group will not collide with each other during operation. For each robot, a control sequence is sampled to obtain a predicted trajectory in a limited future time. The space-time path without any conflict is used as a reference path, and dynamic obstacles are detected in real time. The cost value of each predicted trajectory is calculated to obtain an optimal control sequence and adjust the path in real time by rolling optimization to cope with dynamic obstacles. The embodiment can cope with dynamic environments or model errors in real time. When facing dynamic obstacles or obstacles not considered during planning, the robot can actively avoid obstacles and quickly return to the original trajectory after avoiding obstacles.

[0043] The following will be described in detail Figure 1 The multi-mobile robot dynamic path planning method provided in this embodiment will be described in detail:

[0044] Step 1: Based on the initial pose of the robot and the static obstacle information in the environment map, a hybrid A*-epsilon algorithm is used to plan a space-time path for each robot in accordance with the Ackermann kinematic model.

[0045] Map information is collected through sensors such as lidar, cameras or depth sensors, and the environment map is generated using SLAM technology. The map information is loaded to obtain the shape, size and position of static obstacles, the initial pose and target pose of each robot are obtained, and it is ensured that the positions of the robots do not overlap with each other to ensure the rationality of the planning task. The robot pose is in the form of ,in 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 of the head.

[0046] A hybrid A*-epsilon algorithm is used for each robot to generate a path that conforms to its kinematics and includes time information. This path only considers collisions between the robot and static obstacles in the environment, while allowing for possible collisions between the robot paths. The paths of all robots together constitute a solution to the MAPF.

[0047] Each robot uses a hybrid A*-epsilon algorithm for independent path planning, including:

[0048] Step 11: Set up an open list Open_List to sort the nodes to be expanded from low to high according to the cost value. Each node, that is, each robot, contains the current time t Status and its cost value; set up a closed list Close_List to record the expanded nodes to avoid repeated processing; set up a focus list Focal_List as a subset of Open_List to store cost values ​​no greater than times the node with the minimum cost, that is:

[0049]

[0050] in, The range is (0,1), in this embodiment, =0.3, Represented as the node with the smallest cost value in the open list Open_List, n' Refers to the nodes in the open list Open_List; and n Refers to the nodes selected from the open list Open_List that meet the conditions of the focus list Focal_List.

[0051] Each time a node is expanded, the node with the smallest focus heuristic value is selected from the Focal_List with the lowest cost. The focus heuristic value is used to measure the number of conflicts between the node and other robots. The conflict is defined as , meaningt At the moment Robot with Robot A collision occurs. The collision is determined when the distance between the centers of the two robots' circumcircles is less than the sum of the radii of their circumcircles.

[0052] Different from the traditional hybrid A* algorithm, the way to expand nodes is no longer to select the node with the smallest cost, but to select the node with the smallest focus heuristic value from the focal list Focal_List with smaller cost. The focus heuristic value reflects the number of conflicts between the node and other robot paths.

[0053] The open list Open_List and the focus list Focal_List are implemented using the Fibonacci heap, which is interfaced by the Boost library in C++ and has good time complexity characteristics. It can meet the needs of frequent priority sorting and addition, deletion, modification and query operations of the list.

[0054] Step 12: Add the initial node, which is the starting point of the robot, to the open list Open_List and the focus list Focal_List, and expand the nodes starting from the initial node.

[0055] For robots using the Ackerman kinematics model, there are seven motion primitives to choose from for node expansion: straight ahead, left turn ahead, right turn ahead, straight ahead backward, left turn backward, right turn backward, and wait in place. The robot pose updates are:

[0056] Go straight ahead:

[0057]

[0058] Turn left and go forward:

[0059]

[0060] Turn right and go forward:

[0061]

[0062] in, S is the step length of expansion, R is the minimum turning radius, is the change in heading angle, ( x, y ) represents the current coordinate position of the robot, θ Indicates the robot's heading angle.

[0063] The straight-ahead backward, left-turn backward, and right-turn backward actions are similar to their forward counterparts, but in opposite directions.

[0064] In particular, the wait-in-place is used to reserve the possibility for avoiding other robots when handling conflicts. The time of a node t The time of a node is composed of the time of its parent plus a unit time, which is set to 1 second in this embodiment.

[0065] For a newly generated node, if it does not conflict with static obstacles, the following operations are performed:

[0066] Step 121: If the newly generated node is already in the closed list Close_List, it means that it has been explored and will not be considered again.

[0067] Step 122: If the newly generated node is not in the open list Open_List, it means that it has never been explored. Its g-value is calculated as follows:

[0068]

[0069] wherein, represents the actual cost of the current node, which is the g-value of the parent node plus the path length of the action of the current node; is the heuristic estimated cost, which represents the estimated cost from the current node to the target node, i.e., the end position of the robot. The calculation method is to take the larger value between the Euclidean distance and the Reeds-Shepp distance between the newly generated node and the target node. Then the focal heuristic value of the newly generated node is calculated, which is the total number of collisions of the current newly generated node with other robots at the same time. This value is used to determine the next node to be expanded in the focal list Focal_List. After the calculation is completed, the newly generated node is added to the open list Open_List, and its

[0070] value is used to determine whether it is added to the focal list Focal_List.

[0071] Step 123: If the newly generated node is already in the open list Open_List, and the g-value obtained by expanding the parent node of the newly generated node is smaller, the g-value of the node in the open list Open_List is updated. .

[0072] After completing the expansion of a node, the parent node is added to the closed list Close_List.

[0073] Compared with the hybrid A* algorithm, the hybrid A*-epsilon introduces a relaxation factor ​​So that the algorithm is more relaxed in the search process, thus avoiding too strict path exploration, in the case of ensuring that the path is close to the optimal, more attention is paid to the scheme with less conflict, and the calculation time is significantly reduced.

[0074] Step 13: Repeat the extended node operation 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 a 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 the obstacle, then the single robot path planning is completed.

[0075] Step 2: According to the space-time path of each robot, detect the space-time conflict between the paths of multiple robots, 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, the node to be expanded each time is determined by the focus domain, and each expanded child node re-plans the path for the robot with constraints using the hybrid A*-epsilon algorithm until a space-time path without any conflict is obtained; wherein the global domain is a complete binary heap; the focus domain is filtered from the global domain, and the conflict number is reordered.

[0076] As shown in Figure 2 , for the construction of the two-domain priority queue in step 2, the specific steps of expanding the node are as follows:

[0077] Step 21: Each node of the global domain is composed of a MAPF solution of all robot paths, a node cost, a focus heuristic value and a space-time constraint. Wherein, the node cost represents the total sum of all path costs, the focus heuristic value is the total number of conflicts between all robots, and the space-time constraint is derived from the conflict between two robots, defined as , which means that the pose of the i robot at time t cannot be pose.

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

[0079] Step 22: Start node expansion from the root node of the global domain.

[0080] In time sequence, detect the first conflict in the node, which is composed of two robots, defined as , which means that at time t, the robot in pose and the robot Robots A collision will occur, according to the conflict, two child nodes are generated, the node constraints correspond to the two robots of the conflict, respectively, And .

[0081] Each child node re-executes the mixed A*-epsilon algorithm in step 1 for the robots involved in the constraint, and calculates the node cost and focus heuristic value of the child node, according to the node cost of the child node, whether the newly generated child node is added to the focus domain. Since the child node generated in the global domain is actually the A*-epsilon planning of the robot involved in the constraint, and the node cost and focus heuristic value are recalculated, the conflict is resolved by the re-planning considering the constraint generated according to the conflict.

[0082] Where, the expansion of the new child node is to re-plan the robot involved in the constraint according to the spatiotemporal constraint generated when the old node conflicts, and replace the old trajectory of this robot. 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 according to all robot trajectories. The MAPF solution of the node, the node cost, the focus heuristic value and the spatiotemporal constraint are all different.

[0083] Step 23: Select the node with the smallest focus heuristic value in the focus domain for expansion, because the smaller the focus heuristic value, the fewer the conflicts in the node, and expanding the node can find a conflict-free feasible solution more quickly. According to the method in step 22, expand the node until a multi-robot spatiotemporal path without any conflict is obtained.

[0084] When selecting and expanding nodes, more preference is given to paths with smaller calculation cost but not strictly optimal, and the final solution has a cost not greater than w times the optimal cost. It is this relaxation mechanism that widens the search breadth, allowing the quality of the solution to be controlled while significantly reducing the search space and improving the running efficiency.

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

[0086] The kinematic model of the Ackermann chassis is:

[0087]

[0088] Where, 、 is the Cartesian coordinate of the rear axle of the robot chassis at time t, is the steering angle at time t, L is the wheelbase of the chassis, and v is the robot speed, is the front wheel steering angle. The range of the control variable is defined as: ; is the discrete time step is the prediction horizon , then the length of the predicted control sequence is . The system is a nonlinear time-varying system.

[0089] Step 31: Linearly interpolate the obtained conflict-free space-time path to ensure that the time interval between the interpolated trajectory points is consistent with the control time step of the model predictive path integral (MPPI).

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

[0091] Step 33: Because a large number of control sequences need to be sampled and the sequence cost needs to be calculated, in order to speed up the calculation and meet the real-time requirements, GPU parallel computing needs to be called. In the GPU, relevant variables are allocated and initialized, including the parameters of the MPPI algorithm and the obstacle coordinate information, and M control sequence arrays are generated. The form of each control sequence is:

[0092]

[0093] where , represents the control variable as the speed and front wheel steering angle of the robot; let the control discrete time step be , the prediction horizon be T, then the length of the predicted control sequence is .

[0094] Step 34: For each control input , generate multiple samples, that is:

[0095]

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

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

[0098]

[0099] where .

[0100] Step 4: Take the time-space path without any conflict as the reference path, get the cost value of each predicted trajectory, get the optimal control sequence and adjust the path in real time through rolling optimization to deal with dynamic obstacles.

[0101] As shown in Figs. 5(a)-5(b), the schematic diagram of velocity and front wheel angle versus time in the optimal control sequence.

[0102] For the specific steps in step 4, the following steps are taken:

[0103] Step 41: Calculate the cost value for each sampled trajectory. For time t, the cost function used in this embodiment is:

[0104]

[0105]

[0106] wherein, represents the pose error cost, ， ， is the pose of the reference trajectory at time t; t+i and and are the weights of the position error and the heading angle error.

[0107]

[0108] wherein, represents the obstacle cost, is the obstacle cost weight, and to ensure that the robot can avoid dynamic obstacles, the numerical value is usually several orders of magnitude larger than to increase the penalty for encountering dynamic obstacles.

[0109]

[0110] wherein, represents the control regularization cost to limit the excessive fluctuation of the control input. is the regularization weight, and are the sampled speed and front wheel angle values.

[0111] In view of the parallel characteristics of the MPPI algorithm, this embodiment fully utilizes the large-scale parallel computing capability of the GPU, realizes that each thread block (block) is responsible for the calculation of one control sequence through the CUDA parallel computing platform, and the thread blocks run independently without sequential sampling calculation, so that the calculation of thousands of control sequences can be quickly completed.

[0112] Step 42: Calculate the weight based on the cost value of each sampling sequence , for the i-th sampling sequence:

[0113]

[0114] in, is the minimum cost value among all sampling sequences, It is a temperature parameter, and its value directly affects the degree of optimization of the control input. The larger the value, the stronger the constraint on control smoothness, but it may sacrifice a certain degree of flexibility. Indicates the i The cost of a sampling sequence. Take 1.

[0115] Step 43: Take the weighted average of all sampling sequences to form the optimal control sequence :

[0116]

[0117] Where M is the total number of sampling sequences, Expression i The control sequence of a sampling sequence.

[0118] Step 44: Get the optimal control sequence Perform sliding average filtering.

[0119] Create a sliding window of length n, and the values ​​in the window are , move the window from the start point to the end point of the sequence, and calculate the sliding average of each position according to the following formula :

[0120]

[0121] in, The optimal control sequence Middle j A control sequence.

[0122] Because the sliding window cannot fully cover the beginning and end of the sequence, the average value of the edge data may be too low. Therefore, the average value is scaled up to the level of the full window based on the actual window size involved in the calculation. The sliding average filter effectively reduces high-frequency jitter in the control sequence, making the robot path smoother.

[0123] Step 45: The spatiotemporal constraint MPPI is a rolling optimization method, which only performs the first step of the control amount, i.e., the optimal control at the current time, in each iteration, and then reacquires the environmental information and updates the optimal control sequence. 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.

[0124] The simulation of the spatiotemporal constraint MPPI in a static environment is shown in FIGS. 3(a)-3(d), the number of robots is 20, the map size is 100m*100m, the gray circles in the figure are static obstacles, the dashed line is the reference spatiotemporal path, the solid line is the actual trajectory of the robot movement, the dashed box represents the pose of the end of the path, and the cluster of lines in front of the robot represents the predicted trajectories under different control sequences; the simulation results under the interference of dynamic obstacles are shown in FIGS. 4(a)-4(d), the number of robots is 5, the map size is 50m*50m, the black circles in the figure represent dynamic obstacles, and the obstacles move at a constant speed of 0.5m / s. The simulation results show that the robot can actively avoid the dynamic obstacles and quickly return to the reference path after avoidance, proving that the method proposed in this embodiment has good robustness.

[0125] This embodiment uses the idea of hierarchical solution, uses hybrid A*-epsilon in the low layer to complete single machine planning, introduces a double-domain priority queue in the high layer to resolve conflicts, relaxes the selection condition of optimal nodes by focal domain, and improves the efficiency of the algorithm. Thus, a conflict-free path with time information suitable for robots with Ackerman kinematics model is generated, i.e., at the same time, any two robots will not be in the same position. This spatiotemporal constraint ensures that the robot group will not collide with each other during operation, and also puts higher requirements on the control algorithm. The spatiotemporal constraint MPPI is a sampling-based method, which samples a large number of control sequences at each time step, evaluates the cost of each trajectory using the path integral method, and generates weights, so as to obtain the best control strategy by weighted average. Since the spatiotemporal constraint MPPI directly samples and optimizes based on the nonlinear dynamic model of the system, it does not need to linearize the model, and the cost function design is flexible, so it can handle complex nonlinear systems, can respond to dynamic environments or model errors in real time, can actively avoid dynamic obstacles or obstacles not considered during planning, and can quickly return to the original trajectory after avoidance.

[0126] Embodiment Two

[0127] The purpose of this embodiment is to provide a dynamic path planning system based on multiple mobile robots, which comprises:

[0128] The first path planning is configured to: based on the initial pose of the robot and the static obstacle information in the environment map, plan a space-time path conforming to the Ackermann kinematic model for each robot respectively by using a hybrid A*-epsilon algorithm.

[0129] The second path planning is configured to: according to the space-time path of each robot, detect space-time conflicts between the paths of the multiple robots, and construct a two-domain priority queue including a global domain and a focus domain; start node expansion from a root node of the global domain, and determine each expanded node by the focus domain; and re-plan a path for each expanded child node by using the hybrid A*-epsilon algorithm for the robot with constraints, until a space-time path without any conflict is obtained.

[0130] The control module is configured to: sample a control sequence for each robot to obtain a predicted trajectory in a limited future time, take the space-time path without any conflict as a reference path, calculate a cost value of each predicted trajectory, and then obtain an optimal control sequence and adjust the path in real time by rolling optimization to cope with dynamic obstacles.

[0131] In more embodiments, there are also provided:

[0132] An electronic device includes a memory and a processor, and computer instructions stored in the memory and run on the processor, when the computer instructions are run by the processor, the method described in Embodiment One is completed. For brevity, this will not be repeated here.

[0133] It should be understood that in the embodiments, the processor can be a central processing unit CPU, and the processor can also be other general-purpose processors, digital signal processors DSP, application-specific integrated circuits ASIC, ready-to-program gate arrays FPGA or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor.

[0134] The memory can include read-only memory and random access memory, and provide instructions and data to the processor, and a portion of the memory can also include non-volatile random access memory. For example, the memory can also store device type information.

[0135] A computer readable storage medium for storing computer instructions, when the computer instructions are executed by a processor, the method described in Embodiment One is completed.

[0136] The method in the embodiment one can be directly embodied as being completed by a hardware processor or being completed by a combination of hardware and software modules in the processor. The software modules can be located in a storage medium in the art, such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory, an electrically erasable programmable memory, a register, or the like. The storage medium is located in a memory, and a processor reads information in the memory and combines hardware to complete the steps of the above method. To avoid repetition, no further detailed description is given here.

[0137] Those skilled in the art can understand that the units and algorithm steps of the examples described in combination with the embodiments can be realized in electronic hardware or a combination of computer software and electronic hardware. Whether the functions are realized in hardware or software manner depends on the specific application and design constraints of the technical solutions. The skilled person can use different methods to realize the described functions for each specific application, but such implementation should not be considered beyond the scope of the present application.

[0138] Although the specific embodiments of the present application are described above in combination with the drawings, the description is not a limitation on the scope of protection of the present application. Those skilled in the art should understand that various modifications or changes made by those skilled in the art on the basis of the technical solutions of the present application without creative labor are still within the scope of protection of the present application.

Claims

1. A dynamic path planning method based on multiple mobile robots, characterized in that: include: Based on the robot's initial position and the static obstacle information in the environment map, a hybrid A*-epsilon algorithm is used to plan a spatiotemporal path for each robot that conforms to the Ackermann kinematic model. Based on the spatiotemporal path of each robot, the spatiotemporal conflicts between the paths of multiple robots are detected, and a dual-domain priority queue consisting of a global domain and a focus domain is constructed. Node expansion is performed starting from the root node of the global domain. The nodes to be expanded each time are determined by the focus domain. For each expanded child node, the hybrid A*-epsilon algorithm is used to replan the path of the robot with constraints until a spatiotemporal path without any conflicts is obtained. Specifically: The dual-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 robots, the node cost, the focus heuristic value, and the time and space constraints. The focus heuristic value is used to measure the number of conflicts between the node and other robots. Starting from the root node of the global domain, the spatiotemporal conflicts of the nodes are detected, and child nodes are generated. For each child node, the hybrid A*-epsilon algorithm is reused to perform path planning for the constrained robot. The node cost and focus heuristic value of the newly generated child node are calculated, and the node cost of the newly generated child node is used to determine whether to join the focus domain. Selecting a node with the smallest focus heuristic value from the focus domain and expanding it until a space-time path without any conflict is obtained; The control sequence of each robot is sampled to obtain the predicted trajectory in a limited time in the future. The conflict-free spatiotemporal path is used as the reference path, and the cost of each predicted trajectory is calculated to obtain the optimal control sequence. The path is adjusted in real time through rolling optimization to cope with dynamic obstacles. In each iterative optimization, only the optimal control at the current moment is executed, and then the environmental information is reacquired and the optimal control sequence is updated.

2. The method for dynamic path planning based on multiple mobile robots according to claim 1, characterized in that: The hybrid A*-epsilon algorithm is used to plan a spatiotemporal path for each robot that conforms to the Ackermann kinematic model. Specifically: Establish an open list, a closed list and a focus list; wherein the focus list is used to store the cost value of the open list is not greater than times the node with the minimum cost, For relaxation induction, the node includes the robot state at the current moment and the corresponding cost value; Based on the motion primitives of the robot using the Ackerman kinematic model, the nodes are expanded until the Euclidean distance between the node position and the target point position is less than the set value, and the node position and the target point position are connected to form a Reeds-Shepp curve, and the Reeds-Shepp curve does not conflict with static obstacles, thus completing the spatiotemporal path planning of a single robot.

3. The method for dynamic path planning based on multiple mobile robots according to claim 2, characterized in that: When expanding a node, if the newly generated node does not conflict with a static obstacle, the following operations are performed: If the newly generated node is already in the closed list, it means that the newly generated node has been explored; If the newly generated node is not in the open list, the cost value and focus heuristic value of the newly generated node are calculated, and the newly generated node is added to the open list. Whether to add it to the focus list is determined based on 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, then the cost value of the newly generated node in the open list is updated. After completing a round of node expansion, the parent node of the newly generated node is added to the closed list.

4. The method for dynamic path planning based on multiple mobile robots according to claim 1, wherein: The conflict-free spatiotemporal path is used as a reference path to 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: Using the conflict-free spatiotemporal path as a reference path, a cost value for each predicted trajectory is obtained; wherein the cost function of the predicted trajectory includes a pose error cost, a dynamic obstacle cost, and a control regularization cost; The weight is determined according to the cost value of each predicted trajectory, and the optimal control sequence is obtained by weighted average; The robot state is updated according to the optimal control sequence, and the path is adjusted in real time to cope with dynamic obstacles through rolling optimization.

5. The method for dynamic path planning based on multiple mobile robots according to claim 4, characterized in that: The weight is determined according to the cost value of each predicted trajectory, and the optimal control sequence is obtained by weighted average, which is: The weight of each predicted trajectory is calculated using an exponential weighting method; All predictions are weighted averaged, and the sequence obtained by weighted average is subjected to sliding average filtering to form the optimal control sequence.

6. The method for dynamic path planning based on multiple mobile robots according to any one of claims 1 to 5, wherein: 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' circumcircles is less than the sum of the radii of their circumcircles.

7. Based on the dynamic path planning system of multiple mobile robots, it is characterized by: include: The first path planning is configured to: plan a spatiotemporal path for each robot that conforms to the Ackermann kinematic model using a hybrid A*-epsilon algorithm based on the robot's initial position and static obstacle information in the environment map; The second path planning is configured as follows: based on the spatiotemporal path of each robot, the spatiotemporal conflicts between the paths of multiple robots are detected, and a dual-domain priority queue including a global domain and a focus domain is constructed. Nodes are expanded starting from the root node of the global domain. The nodes to be expanded each time are determined by the focus domain. Each expanded child node replans the path of the robot with constraints using a hybrid A*-epsilon algorithm until a spatiotemporal path without any conflicts is obtained. Specifically: The dual-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 robots, the node cost, the focus heuristic value, and the time and space constraints. The focus heuristic value is used to measure the number of conflicts between the node and other robots. Starting from the root node of the global domain, the spatiotemporal conflicts of the nodes are detected, and child nodes are generated. For each child node, the hybrid A*-epsilon algorithm is reused to perform path planning for the constrained robot. The node cost and focus heuristic value of the newly generated child node are calculated, and the node cost of the newly generated child node is used to determine whether to join the focus domain. Selecting a node with the smallest focus heuristic value from the focus domain and expanding it until a space-time path without any conflict is obtained; The control module is configured to: sample the control sequence of each robot to obtain a predicted trajectory within a finite time in the future, use the conflict-free spatiotemporal path as a reference path, calculate the cost 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; wherein, in each iterative optimization, only the optimal control at the current moment is executed, and then the environmental information is reacquired and the optimal control sequence is updated.

8. An electronic device, characterized in that: The method comprises a memory and a processor, and computer instructions stored in the memory and executed on the processor, wherein when the computer instructions are executed by the processor, the method according to any one of claims 1 to 6 is completed.

9. A computer-readable storage medium, characterized in that Used to store computer instructions, which, when executed by a processor, complete the method according to any one of claims 1 to 6.

Images