Multi-agent path planning method coupled with multi-task scheduling and priority inheritance

Abstract

The application belongs to the field of mobile robot path planning, and relates to a multi-agent path planning method coupling multi-task sequencing and priority inheritance, which acquires mobile robot motion region information, constructs a kinematic model of the robot, sets a replanning frequency, resequences the robot task sequence by using a hybrid particle swarm optimization algorithm based on Manhattan distance, processes conflicts among all agents by a high-level multi-agent path planner, and generates a path meeting priority, kinematic model and time constraint for each agent by a low-level single-agent path planner. The path generated by the method can not only avoid collision with obstacles, but also effectively avoid mutual conflicts among agents, can continuously run in a dynamically distributed task implementation environment, and give a robot conflict-free path in a short time, and the planned trajectory does not need to be secondarily optimized and can be directly applied to an actual industrial scene.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of mobile robot path planning, and relates to a multi-agent path planning method coupling multi-task sequencing and priority inheritance. BACKGROUND

[0002] With the wide application of multi-agent systems, especially in the fields of autonomous driving, robot swarm cooperation, logistics distribution, etc., the multi-agent path finding (MAPF) problem has become an important research topic. The traditional MAPF problem focuses on finding conflict-minimized paths for a group of agents in a fixed environment, ensuring that each agent can reach the target point from the starting point and avoid collisions with other agents. However, in the real world, the environment and tasks are often dynamically changing, and agents need to perform tasks over a long period of time and adapt to these changes. This leads to the lifelong multi-agent path finding (Lifelong MAPF) problem. The core challenge of Lifelong MAPF is how to design and implement an efficient path planning strategy so that agents can not only perform tasks in a complex and constantly changing environment, but also dynamically adjust paths and strategies over time to adapt to changes in the environment. The study of this problem is more complex than the traditional MAPF problem, because agents need to have long-term adaptability, planning ability, and conflict resolution ability.

[0003] The MAPF problem aims to plan paths for multiple agents, with the key constraint that all agents can simultaneously follow the policy-derived paths without colliding. Early research on MAPF algorithms focused on improving classic path planning algorithms such as Dijkstra's algorithm, A* algorithm, D* Lite algorithm, artificial potential field algorithm, heuristic swarm optimization algorithm, and reinforcement learning. With further research, specialized optimal solvers for the MAPF problem have been developed, including A*-based, reduction-based, cost-growing tree-based, and conflict-search-based optimal solvers. Simultaneously, many more efficient suboptimal solvers have emerged, including the Enhanced CBS algorithm (ECBS), the Explicit Estimation CBS algorithm (EECBS), the Priority Search-based solver (PBS), and the Multi-Agent Path Planning Algorithm based on Large Neighborhood Search (MAPF-LNS2). These algorithms sacrifice optimality for speed, better meeting the real-time requirements of practical industrial scenarios and exhibiting better performance in certain special scenarios. Based on these algorithms, many methods have been developed to solve the Lifelong MAPF problem. Some treat Lifelong MAPF as a whole, where each agent already knows all target locations and solves the conflict-free path from the starting position to each target location in one go. This method often suffers from long solution time and low flexibility. Others decompose Lifelong MAPF into a series of MAPF instances, replanning a path for each agent at each time step. This method suffers from low efficiency and poor path quality due to excessively frequent replanning. Still others restrict replanning to agents that have just reached the target location; however, this method degenerates into priority planning when only one agent reaches the target location at each time step.

[0004] To address the drawbacks of the aforementioned methods, the RHCR framework has been proposed, which involves replanning every h time steps, with each replanning only resolving the issues from the previous h-time steps. w A conflict at a certain time step, w The priority level is greater than or equal to h. This framework is decoupled from the task allocation algorithm, making it adaptable to online planning in different environments. However, the quality of the planned paths often depends more heavily on the external task allocation algorithm. For example, in random task allocation, redundant paths may be planned, and the agent might prioritize tasks further away based on their order. Furthermore, this method clears the priority binary tree during each planning iteration, requiring a new priority set to be created each time. In practice, this often leads to increased computation time. Summary of the Invention

[0005] In order to solve the above technical problems existing in the prior art, the application provides a multi-agent path planning method coupling multi-task sequencing and priority inheritance, and the specific technical scheme is as follows.

[0006] A multi-agent path planning method coupling multi-task sequencing and priority inheritance, comprising:

[0007] In step S1, the map and obstacle information of the robot motion area are acquired, the kinematic model of the robot is modeled, the global map coordinate system and the local coordinate system of the robot of the motion area are constructed, and the initial pose state and the initial task sequence of each mobile robot are set.

[0008] In step S2, the re-planning frequency h is set, the lower limit of the time step of the robot for completing the current task sequence is calculated, and if the lower limit is lower than the re-planning frequency h, a new task is randomly added to the task sequence.

[0009] In step S3, the task sequence is reordered.

[0010] In step S4, the initial pose state and the task sequence of each mobile robot are input into a low-level single-agent path planner, and the low-level single-agent path planner is used to calculate the obstacle avoidance path of each mobile robot from the initial pose state to each target pose state in the task sequence, which satisfies the priority binary tree.

[0011] In step S5, the obstacle avoidance path is subjected to conflict detection by a high-level multi-agent path planner, and each time, only the conflict of the first w time step is detected, w if the time step is greater than or equal to h, all the detected conflicts are added to a conflict set.

[0012] In step S6, when the conflict set is empty, the obstacle avoidance path of each mobile robot for the first h time steps is output, and a new round of path planning is started with the pose state at the hth time step as the new initial pose state, and the step S2 is returned; when there is a conflict in the conflict set, one of the conflicts is selected according to a strategy, and is added to the priority binary tree in the form of priority, the priority can be inherited between adjacent planning, and the step S4 is returned.

[0013] Further, the map information and obstacle information of the mobile robot motion area are acquired by a mobile robot vehicle-mounted laser radar or a vehicle-mounted depth camera, and are input in the form of a YAML configuration file.

[0014] Further, the kinematic model comprises an Ackerman model and an omni-directional mobile model.

[0015] Further, in step S2, the time step lower limit is the minimum time step from the current starting pose state to reach each target position of the task sequence in turn, and the calculation expression of the time step lower limit of the current task sequence is:

[0016] ,

[0017] wherein s i represents the starting pose state of the intelligent agent, i g i [ j ] represents the intelligent agent i the target position in the task sequence; if the time step lower limit j is less than the re-planning frequency h, a new task is randomly assigned to the task sequence. d

[0018] Further, in step S3, the task sequence is reordered by abstracting the entire task sequence into a TSP problem, and the Manhattan distance-based hybrid particle swarm optimization algorithm is used to calculate the distance between each pair of target positions in the task sequence, and to solve the shortest loop that visits each target position once but does not return to the starting pose state.

[0019] Further, in step S4, the low-level single-agent path planner uses the SIPP algorithm based on the multi-label A* algorithm.

[0020] Further, in step S5, the high-level multi-agent path planner constructs a priority binary tree, the nodes in the priority binary tree are composed of a priority set, a path set and a path cost, the priority set can be inherited in adjacent planning, the path planned according to the node priority set is stored in the path set, and the path cost is calculated.

[0021] Further, the inheritance of the priority set is periodic l , and the priority sets in the period are inherited in turn, that is, the priority set in the root node of the new priority binary tree will inherit the priority set of the output node of the last planning; at the beginning of the period, the priority set of the root node of the new priority binary tree will be emptied.

[0022] Further, in step S6, if the mobile robot completes part of the task within h time steps, the part of the task is deleted from the task sequence.

[0023] Compared with the prior art, the present application has the following beneficial effects:

[0024] ​​1. The application can support mobile robots with different kinematic models and geometric characteristics, model two kinematic models of Ackerman model and omni-directional mobile model respectively, and design planning algorithm based on this. The conflict between all agents is handled by the high-level multi-agent path planner, and the path that meets the priority, kinematic model and time constraint is generated for each agent by the low-level single-agent path planner. The algorithm structure is simple and efficient, the generated path can not only avoid collision with obstacles, but also effectively avoid mutual conflict between agents, and meet the actual application requirements.

[0025] 2. The application couples the task sequencing algorithm on the basis of the RHCR framework, abstracts the problem into a TSP traveling salesman problem, and uses a hybrid particle swarm optimization algorithm for heuristic solution, so that the obtained task sequence is more reasonable, the redundant path of planning is greatly reduced, and the path cost is lower. Meanwhile, the priority experience utilization idea is combined on the basis of the RHCR framework, that is, as a period, the re-planning in the period will inherit the priority set of the last planning in turn, and the priority set is emptied at the beginning of each period, so that the increase of the calculation time caused by re-drafting the priority in the period is reduced. l

[0026] 3. The mobile robot online path planning method of the application has high success rate, fast single planning solving speed, can give a conflict-free path of the robot cluster in a short time, can continuously run in the environment of dynamic task implementation allocation, and the planned trajectory does not need to be optimized again and can be directly applied to the actual industrial scene. BRIEF DESCRIPTION OF DRAWINGS

[0027] Figure 1 It is the multi-agent path planning method flowchart of the application embodiment coupling multi-task sequencing and priority inheritance;

[0028] Figure 2 It is the kinematic model modeling of the mobile robot of the application embodiment based on Ackerman model;

[0029] Figure 3 It is a part of the execution situation diagram of the application embodiment in a simulation environment. DETAILED DESCRIPTION

[0030] In order to make the purpose, technical scheme and technical effect of the application clearer, the application is further described in detail in combination with the drawings and embodiments of the specification.

[0031] As shown in the drawings, Figure 1 ​As shown, the embodiment discloses a multi-agent path planning method coupling multi-task sequencing and priority inheritance, acquires mobile robot motion region information, constructs a robot kinematic model, sets a replanning frequency, uses a hybrid particle swarm optimization algorithm based on Manhattan distance to reorder the robot task sequence, calculates the obstacle avoidance path of each mobile robot from the starting pose state to the target pose state in the task sequence in sequence to meet the priority binary tree through a low-level single-agent path planner, simultaneously inputs the obstacle avoidance path of each mobile robot into a high-level multi-agent path planner for conflict detection, and solves the conflict between mobile robots based on priority search time constraints, and finally plans the conflict-free obstacle avoidance path of each mobile robot. Specifically, the following steps are included:

[0032] In step S1, the map information and obstacle information of the mobile robot motion region are acquired through the mobile robot vehicle-mounted laser radar or vehicle-mounted depth camera, and are input in the form of a YAML configuration file.

[0033] The mobile robot is modeled according to the kinematic model, which includes the Ackerman model and the omni-directional mobile model. The Ackerman model is a geometric model used to describe the steering characteristics of a car, which is based on the Ackerman steering geometry principle, i.e., when the car turns, the inside tire has a smaller turning radius than the outside tire to achieve more stable driving. The omni-directional mobile model mainly uses the Mecanum wheel, i.e., the vehicle can realize left and right movement.

[0034] A global map coordinate system and a robot local coordinate system are established for the motion region of the mobile robot, and the starting pose state and the initial task sequence of each mobile robot are set; the number of mobile robots is an integer greater than or equal to 2.

[0035] As shown in Figure 2 , the global map coordinate system is established with the lower left corner of the map as the origin and the positive east direction as the X-axis and the positive north direction as the Y-axis. The robot local coordinate system is established with the center of the rear axle of the mobile robot as the origin and the positive front direction of the mobile robot as the X' axis and the left direction of the mobile robot as the Y' axis. The angle difference between the global map coordinate system and the robot local coordinate system is θ .

[0036] Through time discretization processing, the pose state of the robot at t time can be obtained:

[0037] Ackerman kinematic equation:

[0038] ;

[0039] Omni-directional mobile kinematic equation:

[0040] ;

[0041] wherein, s 1t the pose state of the robot at time t , s 2t the pose state of the robot at time t , θ is the yaw angle of the robot, i.e. the angle difference between the global map coordinate system and the robot local coordinate system, T s is the time interval between time t and time t -1, v is the linear velocity of the robot, L is the front-rear wheel distance of the robot, Ф is the steering angle of the front wheel of the robot, when the steering angle is fixed as Ф , the angular velocity of the robot is v / L x tan Ф .

[0042] Step S2, the lower limit of the time step for completing the current task sequence is calculated, and if it is lower than the time step of the re-planning frequency h, a new task is randomly added to the task sequence. The lower limit of the time step is the minimum time step for sequentially reaching each target position of the task sequence from the current starting pose state, and h time steps are the frequency of re-planning, i.e. multi-agent path planning is performed once every h time steps.

[0043] The calculation expression of the lower limit of the time step for completing the current task sequence is:

[0044] ,

[0045] wherein s i represents the starting pose state of the agent i , g i [ j ] represents the i th target position in the task sequence of the agent j . If the lower limit of the time step d is less than the re-planning frequency h, a new task needs to be assigned, and the task assignment method in the present application is random assignment.

[0046] Step S3, the task sequence is reordered so that the planned path is more reasonable and the path cost consumed is as little as possible.

[0047] The task sequence reordering described above abstracts the entire task sequence into a Task-Solving Problem (TSP). It calculates the distance between each pair of target positions in the task sequence and finds the shortest loop that visits each target position once without returning to the starting pose. The task sequence reordering algorithm employs a hybrid particle swarm optimization algorithm based on Manhattan distance. This algorithm can solve discrete-domain TSP problems and combines the ideas of genetic algorithms and simulated annealing to plan a suboptimal shortest path that can visit all target positions. After reordering, the path quality is improved, redundant path segments are reduced, and path costs are lowered.

[0048] Step S4: Input the initial pose state and task sequence of each mobile robot into the low-level single-agent path planner. The low-level single-agent path planner calculates the obstacle avoidance path that satisfies the priority binary tree, allowing each mobile robot to sequentially reach the target pose states in the task sequence from its initial pose state. The mobile robot's position information can be obtained using GPS or other commonly used robot positioning technologies.

[0049] The low-level single-agent path planner employs the SIPP algorithm based on the multi-label A* algorithm to solve the multi-task path planning problem. The SIPP algorithm is based on discrete space, and before each planning iteration, the safe intervals of all map cells need to be calculated. Therefore, the overall search dimension is expanded from four dimensions to five dimensions. To adapt to continuous space, a region list needs to be maintained to store regions with known safe intervals. Each time neighbor regions are expanded, the safe intervals of neighbor regions are calculated based on the region list. A five-dimensional search space is used. （x ; y ; θ ; t ; s） Path planning for a single mobile robot. x, y, θ It is continuous, representing the robot's position and orientation; t It is discrete, representing time; s Let be a set of time intervals, representing the safe time intervals during which the robot's current pose will not result in a collision. The multi-label A* algorithm can handle multi-task single-agent path planning problems. Each task in the agent's task sequence is assigned a label corresponding to its order. The label is used to identify the currently executing task and the number of remaining tasks. The algorithm sets a global label for the current task, and after each task is completed, the label is updated to the label of the next task, until all tasks are completed.

[0050] The low-layer single-agent path planner can support the movement of mobile robots with different kinematic models, and the actions of the path nodes of the Ackerman model mobile robot can be expanded to include forward left turn, forward straight, forward right turn, backward left turn, backward straight, backward right turn, and wait in place, and the actions of the path nodes of the omni-directional mobile model robot can be expanded to include straight, backward, left, right, and wait in place.

[0051] In step S5, the obstacle avoidance path is input into the high-layer multi-agent path planner for conflict detection, and only the conflicts of the previous w h time steps are detected each time, w h, all the detected conflicts are added to the conflict set.

[0052] The high-layer multi-agent path planner is based on priority search, and the priority is used to solve the conflicts between the mobile robots.

[0053] In step S6, when the conflict set is empty, the obstacle avoidance path of each mobile robot for the previous h time steps is output, if part of the task is completed within the h time steps, the part of the task needs to be deleted from the task sequence, and a new round of path planning is started with the pose state of the h time step as the new starting pose state, and it is stipulated that the priority set needs to be emptied each time the path is re-planned, and the priority set of the periodic re-planning is inherited in turn, and the process returns to step S2; when there is a conflict in the conflict set, one of the conflicts is selected according to the strategy, and is added to the priority binary tree in the form of priority, and the process returns to step S4. l

[0054] The node in the priority binary tree is composed of a priority set, a path set, and a path cost, the priority set can be inherited in adjacent planning, the path planned according to the node priority set is stored in the path set, and the path cost is calculated.

[0055] The conflict screening strategy includes partial expansion of child nodes and adaptive influence priority. The partial expansion of child nodes means that the path planning of only one child node is calculated each time, when the planning of the child node fails, another child node is expanded and the path is calculated. The adaptive influence priority means that the selection of the priority added to the original priority set implicitly adds other priorities.

[0056] The inheritance of the priority set is in l periods, and the priority sets in the periods are inherited in turn, that is, the priority set in the root node of the new priority binary tree will inherit the priority set of the output node of the last planning. At the beginning of the period, the priority set of the root node of the new priority binary tree will be emptied.

[0057] For example Figure 3 ​As shown, the application is tested in a simulation environment: 50 agents mixed with Ackerman model and omni-directional mobile model can be simultaneously online path planning on a 200m*200m test map, the single planning time is less than 2s, the success rate is greater than 95%, and it can run continuously for more than 1000s, and the throughput per hundred seconds is more than 70.

[0058] The application solves the online path planning problem of multiple mobile robots meeting the Ackerman kinematics model and the omni-directional mobile model. Each mobile robot has an independent initial pose and task sequence. The proposed online planning algorithm can efficiently and quickly generate the obstacle avoidance path of all agents in each planning, the path trajectory of the robot meeting the Ackerman model conforms to the Reeds-Shepp curve, that is, the vehicle can realize the reverse parking, and can continuously run in the environment with dynamic task implementation allocation. The proposed coupling task sorting algorithm and priority inheritance can reduce the generation of redundant path segments, improve the quality of the planned path, reduce the path cost, and at the same time reduce the calculation time of each re-planning, improve the solving efficiency.

[0059] The above is only the preferred embodiment of the application, and does not limit the application in any form. Although the implementation process of the application has been described in detail in the foregoing, those skilled in the art can still modify the technical solutions recorded in the foregoing examples, or replace some technical features with equivalent ones. Any modification, equivalent replacement, etc. within the spirit and principles of the application shall be included in the protection scope of the application.

Claims

1. A multi-agent path planning method coupling multi-task sequencing and priority inheritance, characterized in that, The application relates to a multi-robot path planning method based on hierarchical planning. The method comprises the following steps: S1, acquiring a map and obstacle information of a robot motion area, modeling a kinematic model of the robot, constructing a global map coordinate system and a local coordinate system of the robot, and setting a starting pose state and an initial task sequence of each mobile robot; S2, setting a re-planning frequency h, calculating a time step lower limit of the robot for completing a current task sequence, and adding a new task to the task sequence if the time step lower limit is lower than the re-planning frequency h; S3, reordering the task sequence; S4, inputting the starting pose state and the task sequence of each mobile robot to a low-level single-agent path planner, and calculating an obstacle avoidance path of each mobile robot from the starting pose state to each target pose state in the task sequence through the low-level single-agent path planner; S6, when the conflict set is empty, outputting an obstacle avoidance path of each mobile robot in the first h time steps, and starting a new round of path planning with the pose state in the hth time step as a new starting pose state and returning to step S2; and when there is a conflict in the conflict set, selecting one of the conflicts according to a strategy and adding the conflict to a priority binary tree in the form of priority, and the priority can be inherited between adjacent planning, and returning to step S4. The map information and obstacle information of the mobile robot motion area are acquired through a vehicle-mounted laser radar or a vehicle-mounted depth camera of the mobile robot, and are input in the form of a YAML configuration file. The kinematic model comprises an Ackerman model and an omni-directional mobile model. In step S2, the time step lower limit is the minimum time step for sequentially reaching each target position in the task sequence from the current starting pose state, and a calculation expression of the time step lower limit for completing the current task sequence is as follows: Step S5, the obstacle-avoiding path passes through the high-level multi-agent path planner for conflict detection, and each time only the conflicts of the previous w time steps are detected, w If h is greater than or equal to h, all the detected conflicts are added to the conflict set. In step S3, the task sequence is reordered, a TSP problem is abstracted from the whole task sequence, a Manhattan distance-based hybrid particle swarm optimization algorithm is used to calculate the distance between each pair of target positions in the task sequence, and a shortest loop for visiting each target position once but not returning to the starting pose state is solved.

2. The multi-agent path-planning method of claim 1, wherein, In step S4, the low-level single-agent path planner adopts a SIPP algorithm based on a multi-label A* algorithm.

3. The multi-agent path-planning method of claim 1, wherein, In step S5, the high-level multi-agent path planner constructs a priority binary tree, a node in the priority binary tree is composed of a priority set, a path set and a path cost, the priority set can be inherited between adjacent planning, a path planned according to the node priority set is stored in the path set, and the path cost is calculated.

4. The multi-agent path-planning method of claim 1, wherein, In step S6, if the mobile robot completes part of the tasks in h time steps, the part of the tasks is deleted from the task sequence. ， in s i Represents intelligent agents i The initial pose state. g i [ j ] represents an intelligent agent i The first task in the sequence j Target location; if the lower limit of the time step d If the frequency is less than h, then new tasks are randomly assigned to the task sequence.

5. The multi-agent path-planning method of claim 1, wherein, ​ 6. The multi-agent path-planning method of claim 1, wherein, ​ 7. The multi-agent path-planning method of claim 1, wherein, ​ 8. The multi-agent path-planning method of claim 7, wherein, The inheritance of the priority set is in a cycle l The priority set of the new priority binary tree root node will inherit the priority set of the last planning output node in turn in the cycle. The priority set of the new priority binary tree root node will be emptied at the beginning of the cycle.

9. The multi-agent path-planning method of claim 1, wherein, ​

Images