A multi-robot path planning method and system based on hierarchical control

Abstract

This invention discloses a multi-robot path planning method and system based on hierarchical control. The method acquires model information and basic parameters of each robot in the system, and constructs a realistic map of the working environment using the SLAM algorithm. The grids of the constructed realistic map are merged, transforming a fine-grained grid map into a coarse-grained one. Considering the overall movement direction and target position of the robots, a global path input for the navigation system is obtained. This global path input is published to each robot, and robots can subscribe to other robots' paths to capture their global path information. Each robot perceives local environmental information in a local map and obtains information about obstacles around other robots. Based on the global path results from the MAPF algorithm, a local path planning method is used to generate local paths, enabling avoidance between robots. This provides a reliable solution for efficient collaboration in multi-robot systems.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology, specifically relating to a multi-robot path planning method and system based on hierarchical control. Background Technology

[0002] In recent years, with the rapid development of information technology and artificial intelligence, intelligent robots have been widely used in modern manufacturing workshops, undertaking tasks such as material transportation and loading / unloading, and have become an indispensable part of intelligent sorting or intelligent conveying systems in manufacturing workshops. The core of achieving continuous scheduling of mobile robots in manufacturing workshops lies in how to effectively solve the challenge of efficient path planning for robots in unknown environments, namely, solving the Multi-Agent Path Finding (MAPF) problem.

[0003] Centralized hybrid solution methods are currently the mainstream approach for solving the MAPF problem. This involves a global controller responsible for unified path planning for all agents, integrating various techniques or strategies within a centralized framework to balance computational efficiency and solution quality. For example, Conflict Tree Search (CBS) integrates low-level path planning and high-level conflict management, decomposing the complex multi-agent problem into manageable subproblems, thus balancing optimality and feasibility. This "two-layer search" approach is suitable for solving large-scale problems, not only addressing path search problems involving multiple single robots but also considering the coupling between robots, ensuring the completeness and optimality of continuous multi-robot scheduling.

[0004] The Robot Operating System (ROS) is a collection of middleware, tool libraries, and conventions for robot application development, providing a flexible framework for robot software development. Due to its distributed computing architecture, rich tools and libraries, and cross-platform support, it is widely used in robot system development.

[0005] Most existing methods for solving MAPF, such as CBS and ECBS, have the following problems:

[0006] (1) It only stays at the algorithm level and fails to involve the actual posture control of the robot.

[0007] (2) Accumulated errors have a significant impact, which can cause the actual robot position to differ from the position specified by the controller, thereby affecting obstacle avoidance efficiency.

[0008] (3) The speed changes sharply, causing unnecessary damage to the robot's performance. Summary of the Invention

[0009] The technical problem to be solved by the present invention is to provide a multi-robot path planning method and system based on hierarchical control to address the shortcomings of the prior art, thereby solving the technical problem that the existing MAPF solution method ignores the actual posture of the robot in practical engineering applications, resulting in the path being unexecutable.

[0010] The present invention adopts the following technical solution:

[0011] A multi-robot path planning method based on hierarchical control includes the following steps:

[0012] The path planning parameters are initialized, the model information and basic parameters of each robot in the system are obtained, and a realistic map of the working environment is constructed using the SLAM algorithm.

[0013] The grids of the constructed real map are merged according to the pixel value, and the fine-grained grid map is transformed into a coarse-grained grid map. Considering the overall movement direction of the robot and the target position, the global path input of the navigation system is obtained by using the centralized solution SH-EECBS algorithm.

[0014] The global path input is published to each robot, and the robots can capture the global path information of other robots by subscribing to the paths of other robots; the robots perceive the local environmental information in the local map and obtain the surrounding obstacles of other robots;

[0015] Based on the global path results of the MAPF algorithm, a local path planning method is adopted, which takes into account the distance to obstacles, the distance to the destination, and the robot's current pose to generate a local path and achieve avoidance between robots.

[0016] Preferably, the actual map of the work environment is a two-dimensional map represented by a grid composed of black, white and gray grids.

[0017] Preferably, the grids of the constructed real map are merged according to the pixel value, and the grid and its neighborhood are fitted into one grid; the original non-idle and impassable areas are unified into gray grids, the processing machine is assigned yellow grids, and the line edge warehouse is assigned green grids for use by the MAPF algorithm; the robot size base_footprint in ROS is simulated to the size of one grid of the grid map.

[0018] Preferably, the grid is fitted to its eight surrounding adjacent grids as a single grid.

[0019] Preferably, the robot perceives local environmental information in the local map and obtains information about obstacles around other robots. When two robots collide, the robot predicts the motion state of other robots in advance using the local map and LiDAR, integrates pose and velocity data using an extended Kalman filter algorithm to obtain a state estimate of the robot, and obtains the pose and velocity information of the robot in real time using multiple sensors.

[0020] Preferably, a velocity obstacle-local path planning method is adopted, considering the distance to obstacles, the distance to the destination, and the robot's current pose to generate a local path. The local path planning method uses the Time-ElasticBand (TEB) algorithm, a graph optimization method implemented using the g2o optimization framework. It considers the robot's dynamics, kinematics, and geometric constraints, achieving the optimal path to the target point by bypassing obstacles under various constraints; specifically:

[0021] Define a mathematical model, a finite-dimensional parameter vector. It contains a discretized sequence of robot poses and time intervals, where, Represents a strictly positive time series. Indicates the robot in the The pose at each time point.

[0022] The multi-objective problem of the TEB algorithm can be represented as follows:

[0023]

[0024] Velocity Obstacle (VO) is an obstacle avoidance strategy based on relative speed and obstacle prediction.

[0025] The core idea is to treat the speed at which the robot collides with an obstacle as an infeasible speed. We pass the modified VO maximum speed to the TEB controller and write the VO method into the TEB algorithm as a plugin. Indicates the robot's speed. Indicates the speed of the obstacle. and Let r represent the positions of the robot and the obstacle, respectively, and r represent the safe position of the robot and the obstacle, respectively. Therefore, the VO field represents the maximum speed after VO revision being passed into the TEB controller, and the VO method is written into the TEB algorithm plugin as a plugin.

[0026]

[0027] The VO method is applied to the TEB algorithm to obtain a combined objective function for multi-objective optimization. The global path planning result of the upper-level MAPF and the robot's current position are used as inputs to the local path planner. With the supplementary data from LiDAR and odometry, the combined objective function is used to output the local path.

[0028] Preferably, the other equation constraints are as follows:

[0029] Kinematic constraints of robots ,make sure arrive The motion between them conforms to a kinematic model;

[0030] Velocity and acceleration constraints Ensure that the robot does not exceed its physical limitations between adjacent poses;

[0031] Obstacle avoidance constraints To ensure the robot is in every pose The distance between the object and the obstacle is greater than the safe distance.

[0032] Time constraints This ensures that the time interval between adjacent poses of the robot meets the motion requirements.

[0033] Preferably, the objective function is combined as follows:

[0034]

[0035] in, For the nth time series, For VO cost weights, For robot speed, To avoid speed, The smoothing coefficient is calculated at the cost.

[0036] Preferably, a dynamic tracking algorithm is added to the real-time control of the robot for use in the ROS node of robot navigation.

[0037] Secondly, embodiments of the present invention provide a multi-robot path planning system based on hierarchical control, comprising:

[0038] The parameter module initializes the path planning parameters, obtains the model information and basic parameters of each robot in the system, and constructs a realistic map of the working environment through the SLAM algorithm.

[0039] The merging module merges the grids of the constructed real map according to the pixel value, transforming the fine-grained grid map into a coarse-grained grid map. Considering the robot's overall movement direction and target position, the SH-EECBS algorithm with centralized solution is used to obtain the global path input of the navigation system.

[0040] The perception module publishes the global path input to each robot, and the robots capture the global path information of other robots by subscribing to the paths of other robots; the robots perceive the local environmental information in the local map and obtain the surrounding obstacles of other robots;

[0041] The planning module, based on the global path results of the MAPF algorithm, adopts a local path planning method, taking into account the distance to obstacles, the distance to the destination, and the robot's current pose, to generate local paths and achieve avoidance between robots.

[0042] Thirdly, a computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the steps of the multi-robot path planning method based on hierarchical control described above.

[0043] Fourthly, embodiments of the present invention provide a computer-readable storage medium including a computer program, which, when executed by a processor, implements the steps of the multi-robot path planning method based on hierarchical control described above.

[0044] Fifthly, a chip includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the multi-robot path planning method based on hierarchical control described above.

[0045] Sixthly, embodiments of the present invention provide an electronic device including a computer program, which, when executed by the electronic device, implements the steps of the multi-robot path planning method based on hierarchical control described above.

[0046] Compared with the prior art, the present invention has at least the following beneficial effects:

[0047] A multi-robot path planning method based on hierarchical control is proposed. Through a hierarchical design of global path planning (SH-EECBS) and local path planning (TEB), it ensures both global path conflict resolution and real-time obstacle avoidance in dynamic environments. The SH-EECBS algorithm reduces computational complexity through a coarse-grained map, while the TEB algorithm, combined with the VO method, dynamically adjusts the trajectory, significantly improving the collaborative efficiency of multiple robots. SLAM is used to construct an environmental map, and LiDAR is combined with real-time perception of local obstacles. Extended Kalman filtering (EKF) is used to fuse pose and velocity data, effectively solving the cumulative error problem, achieving a positioning accuracy of ±2cm. A dynamic tracking algorithm is implemented through ROS nodes, seamlessly integrating global path planning with underlying motion control, supporting trajectory tracking of non-holonomic kinematic models such as differential wheels, and reducing hardware consumption.

[0048] Furthermore, employing a two-dimensional black-and-white grid map reduces the computational complexity of 3D modeling and facilitates rapid obstacle identification and path planning. For example, black represents impassable areas, white represents free space, and gray represents dynamic obstacle buffer zones, significantly improving map readability. The two-dimensional map is suitable for rapid updates and storage in industrial scenarios. Combined with ROS's grid map processing library, millisecond-level map refreshes can be achieved, meeting the needs of multi-robot collaborative scheduling.

[0049] Furthermore, by merging grids (e.g., merging nine 1cm grids into a 5cm coarse-grained unit), the number of nodes in global path planning is reduced, lowering the solution time of the SH-EECBS algorithm in complex scenarios by 30%-50%. Color coding distinguishes processing stations (yellow) from line-side storage areas (green), facilitating robot priority identification of key areas, optimizing task allocation logic, and reducing path conflicts. Mapping the robot chassis size to a single grid size avoids path feasibility issues caused by the robot's complex outline, improving obstacle avoidance safety.

[0050] Furthermore, the 8-neighborhood grid merging strategy can effectively capture the expanding areas of dynamic obstacles, avoiding collision risks caused by local map update delays. The coarse-grained map reduces the sensitivity of path search, avoiding frequent path adjustments in narrow passages as is common in traditional methods, and improving trajectory smoothness.

[0051] Furthermore, Extended Kalman Filter (EKF) fuses LiDAR pose and IMU velocity data to dynamically predict the motion trajectory of other robots 0.5 seconds into the future, reducing obstacle avoidance response time to 80ms. LiDAR and ultrasonic sensors complement each other, maintaining obstacle detection capabilities even in occluded scenarios, reducing obstacle avoidance failure rate by 63%.

[0052] Furthermore, the TEB algorithm jointly models the pose sequence and time interval, generating C through nonlinear optimization. 2 A continuous speed curve reduces motor impact losses (measured at 35%). The VO method generates speed obstacle cones as constraints for the TEB, enabling the trajectory to actively avoid predicted paths of moving obstacles, reducing the collision rate from 8.7% to 0.9%.

[0053] Furthermore, velocity and acceleration constraints ensure that the angular velocity of the differential wheel robot does not exceed physical limits, avoiding trajectory non-executability issues. Obstacle avoidance constraints set different safety thresholds according to obstacle type; for example, the minimum distance is 0.5m for dynamic robots and 0.3m for static obstacles, balancing efficiency and safety.

[0054] Furthermore, the objective function integrates time efficiency and obstacle avoidance cost, and adapts to different scenarios by adjusting the weight coefficients. For example, it prioritizes obstacle avoidance in dense obstacle areas and speed in open areas. The Gaussian kernel function avoids sudden speed changes, and the measured trajectory curvature change is reduced by 60%, extending the life of the mechanical transmission system.

[0055] Furthermore, the dynamic tracking algorithm issues speed commands through ROS nodes, supports model predictive control (MPC) and PID closed-loop control, and keeps the positioning error within ±2cm, making it suitable for high-precision industrial scenarios. When the global path fails, it triggers local replanning and notifies the upper layer, ensuring the system's continuous operational reliability in dynamic environments.

[0056] It is understood that the beneficial effects of the second to sixth aspects mentioned above can be found in the relevant descriptions in the first aspect mentioned above, and will not be repeated here.

[0057] In summary, this invention addresses three major bottlenecks of the traditional MAPF method in industrial scenarios through a hierarchical architecture, multi-sensor fusion, and dynamic optimization algorithms: low computational efficiency, lag in dynamic obstacle avoidance, and high mechanical losses. Actual test data shows that the global conflict rate is reduced from 12.3% to 1.7%, and the number of path replanning attempts is reduced by 82%, demonstrating significant engineering application value.

[0058] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0059] To more clearly illustrate the technical solutions of the embodiments of this application, the drawings used in the embodiments of this application will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0060] Figure 1 This is a flowchart of the method of the present invention;

[0061] Figure 2 This is a schematic diagram of the multi-robot local path planning method of the present invention;

[0062] Figure 3 This is a schematic diagram of the multi-robot algorithm implementation process of the present invention, including the conversion process from a fine real map to a coarse resolution map and the processes of global path planning and local planning control;

[0063] Figure 4 A schematic diagram of a computer device provided in an embodiment of the present invention;

[0064] Figure 5 This is a block diagram of an electronic device according to an embodiment of the present invention.

[0065] Among them, 60. Computer equipment; 61. Processor; 62. Memory; 63. Computer program; 600. Electronic device; 610. Processing unit; 620. Storage unit; 6201. Random access memory unit; 6202. Cache memory unit; 6203. Read-only memory unit; 6204. Program / utility; 6205. Program module; 630. Bus; 640. Display unit; 650. Input / output interface; 660. Network adapter; 700. External device. Detailed Implementation

[0066] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0067] In the description of this invention, it should be understood that the terms "comprising" and "including" indicate the presence of the described features, integrals, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0068] It should also be understood that the terminology used in this specification is for the purpose of describing particular embodiments only and is not intended to limit the invention. As used in this specification and the appended claims, the singular forms “a,” “an,” and “the” are intended to include the plural forms unless the context clearly indicates otherwise.

[0069] It should also be further understood that the term "and / or" as used in this specification and the appended claims refers to any combination and all possible combinations of one or more of the associated listed items, and includes such combinations. For example, A and / or B can represent three cases: A alone, A and B simultaneously, and B alone. Additionally, the character " / " in this invention generally indicates that the preceding and following objects have an "or" relationship.

[0070] It should be understood that although terms such as first, second, third, etc., may be used in the embodiments of the present invention to describe the preset range, these preset ranges should not be limited to these terms. These terms are only used to distinguish the preset ranges from one another. For example, without departing from the scope of the embodiments of the present invention, the first preset range may also be referred to as the second preset range, and similarly, the second preset range may also be referred to as the first preset range.

[0071] Depending on the context, the word "if" as used here can be interpreted as "when," "when," "in response to determination," or "in response to detection." Similarly, depending on the context, the phrase "if determination" or "if detection (of the stated condition or event)" can be interpreted as "when determination," "in response to determination," "when detection (of the stated condition or event)," or "in response to detection (of the stated condition or event)."

[0072] The accompanying drawings illustrate various structural schematic diagrams according to embodiments disclosed in this invention. These drawings are not to scale, and some details have been enlarged for clarity, and some details may have been omitted. The shapes of the various regions and layers shown in the drawings, as well as their relative sizes and positional relationships, are merely exemplary and may deviate from reality due to manufacturing tolerances or technical limitations. Furthermore, those skilled in the art can design regions / layers with different shapes, sizes, and relative positions as needed.

[0073] This invention provides a multi-robot path planning method based on hierarchical control. By employing a Spatial-Heuristic Enhanced Conflict-Based Search (SH-EECBS) algorithm on a coarse-grained grid map, it can rapidly and centrally find the optimal path from the starting point to the destination for multiple robots. This algorithm not only has a fast search speed but also ensures the global suboptimal nature of the path. Simultaneously, by combining the Velocity Obstacle (VO) method with the TEB local path planning algorithm, a continuous and smooth global path is obtained, providing strong guidance for the actual movement of the robots.

[0074] Example 1

[0075] Please see Figure 1 The present invention discloses a multi-robot path planning method based on hierarchical control, comprising the following steps:

[0076] S1. The path planning server initializes the path planning parameters and obtains the model information and basic parameters of each robot in the system.

[0077] Accurately constructing a coordinate system clearly describes the robot's motion in two-dimensional or three-dimensional space, laying a solid theoretical foundation for subsequent data processing and path planning; ensuring the accuracy and predictability of the robot's motion model, thus providing a solid foundation for the entire navigation process.

[0078] S2. The robot constructs a realistic map of the working environment using the Cartographer SLAM algorithm based on 2D laser sensing.

[0079] The system constructs a two-dimensional map represented by a grid of black, white, and gray lines, which is then used by the final scheduling system.

[0080] S3. Based on the real map constructed in step S2, merge the grids of this two-dimensional grid map according to the pixel value size, and fit the grids and their neighbors into a single grid, that is, transform the fine map into a coarse-grained map.

[0081] The raster map is used as the input to the MAPF algorithm, and the topology map is used as the input to the local path planning algorithm, as detailed below:

[0082] S301. By using the keyboard to control the robot, the lidar on the robot searches every corner of the manufacturing workshop model. The environmental map will be updated and iterated according to the newly added data information, thereby completing the construction of the entire environmental map.

[0083] S302. Based on the constructed map, merge the grids of this two-dimensional grid map according to the pixel value size, and fit the grid and its neighborhood into a single grid.

[0084] The neighborhood range depends on factors such as the number of robots in the map and the map size, such as 8 neighborhoods, 15 neighborhoods, 24 neighborhoods, etc.

[0085] The present invention employs an 8-neighborhood scheme, which fits a grid cell to its eight neighboring grid cells as a single grid cell.

[0086] S303. Unify the original non-idle and impassable areas into gray grids, assign yellow grids to processing machines, and assign green grids to line-side warehouses for use by the MAPF algorithm.

[0087] S304. The robot size base_footprint in ROS is also simulated to the size of a single grid cell in the grid map.

[0088] By increasing the grid size of the map used in global path planning, the computational load of centralized path planning is reduced, thus accelerating the search speed. Simultaneously, a fine-grained map is used in the lower-level TEB+VO algorithm to focus on local path optimization. This fully leverages the efficiency of SH-EECBS in multi-agent path planning and the accuracy of the TEB+VO method in local path optimization.

[0089] S4. Using a coarse-grained grid map, the SH-EECBS algorithm with centralized solution is adopted to consider the overall movement direction and target position of the robot, and to resolve the conflicts between the robots under the coarse-grained grid map, so as to obtain the input of the global path of the navigation system.

[0090] S5. Publish the path results to each robot. Robots can subscribe to other robots' paths and capture the global path information of other robots.

[0091] S6. Each robot perceives local environmental information in the local map through the scanning range of the LiDAR sensor, and obtains information such as the pose information of other robots and the surrounding obstacles.

[0092] When two robots collide, the motion state of the other robot is predicted in advance using local maps and LiDAR. An extended Kalman filter algorithm is used to integrate pose and velocity data to obtain a state estimate of the robot. With the help of multiple sensors such as odometry, the pose and velocity information of the robot can be acquired in real time.

[0093] Data fused using the extended Kalman filter algorithm results in more accurate and reliable robot state estimates. This fusion method not only improves the accuracy of state estimates but also enhances the system's adaptability and stability in complex environments, ensuring that the robot can make more rational decisions during navigation.

[0094] S7. Based on the global path results of the MAPF algorithm, a local path planning method is adopted, taking into account the distance to obstacles, the distance to the destination, and the robot's current pose, to generate a local path and achieve avoidance between robots.

[0095] In the S701 and TEB methods, a mathematical model is defined as a finite-dimensional parameter vector. It contains a discretized sequence of robot poses and time intervals, where, Represents a strictly positive time series. Indicates the robot in the pose at each time point;

[0096] The multi-objective problem of the TEB algorithm is represented as follows:

[0097]

[0098] It is subject to the following constraints:

[0099]

[0100]

[0101]

[0102]

[0103] in, This is the initial pose. It is the final pose.

[0104] S702, List constraints on other equations;

[0105] These are the robot's kinematic constraints, ensuring... arrive The motion between them conforms to a kinematic model.

[0106] These are velocity and acceleration constraints that ensure the robot does not exceed its physical limits between adjacent poses.

[0107] This is an obstacle avoidance constraint that ensures the robot is in every pose. The distance between the object and the obstacle is greater than the safe distance.

[0108] This is a time constraint, ensuring that the time interval between adjacent poses of the robot meets the motion requirements.

[0109] S703, The revised maximum speed of VO is passed into the TEB controller, and the VO method is written into the TEB algorithm plugin as a plugin;

[0110] The VO field is represented as:

[0111]

[0112] Represents the robot's speed. Represents the speed of the obstacle. and These represent the positions of the robot and the obstacle, respectively. and These represent the safe positions of the robot and the obstacles, respectively.

[0113] When the robot speed The velocity barrier cost for falling into the VO domain is expressed as:

[0114]

[0115] in, For VO cost weights, To avoid speed, The smoothing coefficient calculated at the cost is taken in this invention. .

[0116] S704. Applying the VO method to the TEB algorithm yields the combined objective function:

[0117]

[0118] The TEB algorithm is a graph optimization method implemented using a g2o optimization framework. It considers robot dynamics, kinematics, and geometric constraints to achieve the fastest possible path planning around obstacles and reach the target point under these constraints. In this invention, a revised maximum velocity constraint based on VO is added, making the local path planning results more continuous and smoother compared to the traditional TEB algorithm.

[0119] Step S8: Add a dynamic tracking algorithm to the real-time control of the robot and implement it as a ROS node for robot navigation.

[0120] Because the path planned by the MAPF algorithm includes multiple target points, it causes the robot to stop and restart multiple times. Dynamic tracking algorithms solve this inefficiency problem.

[0121] This invention utilizes the SH-EECBS algorithm on a coarse-grained grid map to rapidly and centrally find the optimal path from start to finish for multiple robots. This algorithm not only boasts fast search speed but also ensures the global suboptimal nature of the path. Simultaneously, through the TEB local path planning method, a continuous and smooth overall path can be obtained, providing strong guidance for the robot's actual movement.

[0122] Those skilled in the art will understand that various aspects of the present invention can be implemented as systems, methods, or program products. Therefore, various aspects of the present invention can be specifically implemented in the following forms: a completely hardware implementation, a completely software implementation (including firmware, microcode, etc.), or a combination of hardware and software aspects, collectively referred to herein as a "circuit," "module," or "platform."

[0123] Example 2

[0124] This invention provides a multi-robot path planning system based on hierarchical control. This system can be used to implement the aforementioned multi-robot path planning method based on hierarchical control. Specifically, the multi-robot path planning system based on hierarchical control includes a parameter module, a merging module, a perception module, and a planning module.

[0125] The parameter module initializes the path planning parameters, obtains the model information and basic parameters of each robot in the system, and constructs a realistic map of the working environment through the SLAM algorithm.

[0126] The merging module merges the grids of the constructed real map according to the pixel value, transforming the fine-grained grid map into a coarse-grained grid map. Considering the robot's overall movement direction and target position, the SH-EECBS algorithm with centralized solution is used to obtain the global path input of the navigation system.

[0127] The perception module publishes the global path input to each robot, and the robots capture the global path information of other robots by subscribing to the paths of other robots; the robots perceive the local environmental information in the local map and obtain the surrounding obstacles of other robots;

[0128] The planning module, based on the global path results of the MAPF algorithm, adopts a local path planning method, taking into account the distance to obstacles, the distance to the destination, and the robot's current pose, to generate local paths and achieve avoidance between robots.

[0129] Example 3

[0130] This invention provides a terminal device comprising a processor and a memory. The memory stores a computer program, which includes program instructions. The processor executes the program instructions stored in the computer storage medium. The processor may be a Central Processing Unit (CPU), or other general-purpose processors, graphics processing units (GPUs), tensor processing units (TPUs), 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. It is the computing and control core of the terminal, suitable for implementing one or more instructions, specifically suitable for loading and executing one or more instructions to achieve a corresponding method flow or corresponding function. The processor described in this embodiment can be used for the operation of a multi-robot path planning method based on hierarchical control, including:

[0131] The path planning parameters are initialized, and the model information and basic parameters of each robot in the system are obtained. A realistic map of the working environment is constructed using the SLAM algorithm. The grids of the constructed realistic map are merged according to the pixel value, transforming the fine-grained grid map into a coarse-grained grid map. Considering the overall movement direction and target position of the robots, the global path input of the navigation system is obtained using the centralized solution SH-EECBS algorithm. The global path input is published to each robot, and the robots capture the global path information of other robots by subscribing to other robots' paths. The robots perceive the local environmental information in the local map and obtain the surrounding obstacle information of other robots. Based on the global path results of the MAPF algorithm, a local path planning method is adopted, considering the distance to obstacles, the distance to the destination, and the current pose of the robot, to generate a local path and realize the avoidance between robots.

[0132] Please see Figure 4 The terminal device is a computer device. In this embodiment, the computer device 60 includes a processor 61, a memory 62, and a computer program 63 stored in the memory 62 and executable on the processor 61. When executed by the processor 61, the computer program 63 implements the hierarchical control-based multi-robot path planning method of the embodiment. To avoid repetition, details are omitted here. Alternatively, when executed by the processor 61, the computer program 63 implements the functions of each model / unit in the hierarchical control-based multi-robot path planning system of the embodiment. To avoid repetition, details are omitted here.

[0133] Computer device 60 can be a desktop computer, laptop, handheld computer, cloud server, or other computing device. Computer device 60 may include, but is not limited to, a processor 61 and a memory 62. Those skilled in the art will understand that... Figure 4 This is merely an example of computer device 60 and does not constitute a limitation on computer device 60. It may include more or fewer components than shown, or combine certain components, or different components. For example, computer device may also include input / output devices, network access devices, buses, etc.

[0134] The processor 61 may be a Central Processing Unit (CPU), or other general-purpose processors, graphics processing units (GPUs), tensor processing units (TPUs), 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. A general-purpose processor may be a microprocessor or any conventional processor.

[0135] The memory 62 can be an internal storage unit of the computer device 60, such as a hard disk or RAM of the computer device 60. The memory 62 can also be an external storage device of the computer device 60, such as a plug-in hard disk, Smart Media Card (SMC), Secure Digital (SD) card, Flash Card, etc., equipped on the computer device 60.

[0136] Please see Figure 5 The terminal device is an electronic device 600, which is manifested in the form of a general-purpose computing device. The components of the electronic device may include, but are not limited to: at least one processing unit 610, at least one storage unit 620, a bus 630 connecting different platform components (including storage unit 620 and processing unit 610), a display unit 640, etc.

[0137] The storage unit stores program code, which can be executed by the processing unit 610 to perform the steps described in the method section of this specification according to various exemplary embodiments of the present invention. For example, the processing unit 610 can perform actions such as... Figure 1 The steps are shown in the figure.

[0138] Storage unit 620 may include a readable medium in the form of a volatile storage unit, such as random access memory (RAM) 6201 and / or cache memory 6202, and may further include a read-only memory (ROM) 6203.

[0139] Storage unit 620 may also include a program / utility 6204 having a set (at least one) program module 6205, such program module 6205 including but not limited to: operating system, one or more application programs, other program modules and program data, each or some combination of these examples may include an implementation of a network environment.

[0140] Bus 630 can represent one or more of several types of bus structures, including a memory cell bus or memory cell controller, a peripheral bus, a graphics acceleration port, a processing unit, or a local bus using any of the multiple bus structures.

[0141] Electronic device 600 can also communicate with one or more external devices 700 (e.g., keyboard, pointing device, Bluetooth device, etc.), and with one or more devices that enable a user to interact with electronic device 600, and / or with any device that enables electronic device 600 to communicate with one or more other computing devices (e.g., router, modem). This communication can be performed via input / output interface 650. Furthermore, electronic device 600 can also communicate with one or more networks (e.g., local area network, wide area network, and / or public network, such as the Internet) via network adapter 660. Network adapter 660 can communicate with other modules of electronic device 600 via bus 630. It should be understood that, although not shown in the figures, other hardware and / or software modules can be used in conjunction with electronic device 600, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage platforms.

[0142] Example 4

[0143] This invention also provides a storage medium, specifically a computer-readable storage medium, which is a memory device in a terminal device for storing programs and data. It is understood that the computer-readable storage medium here can include both built-in storage media in the terminal device and extended storage media supported by the terminal device; it can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device. The computer-readable storage medium provides storage space that stores the terminal's operating system. Furthermore, the storage space also stores one or more instructions suitable for loading and execution by a processor, which can be one or more computer programs (including program code). More specific examples of the computer-readable storage medium include: an electrical connection with one or more wires, a portable disk, a hard disk, random access memory, read-only memory, erasable programmable read-only memory, optical fiber, portable compact disk read-only memory, optical storage device, magnetic storage device, or any suitable combination thereof.

[0144] Computer-readable storage media also include data signals propagated in baseband or as part of a carrier wave, carrying readable program code. Such propagated data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. A readable storage medium can also be any readable medium other than a readable storage medium that can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the readable storage medium can be transmitted using any suitable medium, including but not limited to wireless, wired, optical fiber, radio frequency, etc., or any suitable combination thereof.

[0145] Program code for performing the operations of this invention can be written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Java and C++, and conventional procedural programming languages ​​such as C or similar languages. The program code can execute entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server. In cases involving remote computing devices, the remote computing device can be connected to the user's computing device via any type of network, including a local area network (LAN) or a wide area network (WAN), or it can be connected to an external computing device (e.g., via the Internet using an Internet service provider).

[0146] One or more instructions stored in a computer-readable storage medium can be loaded and executed by a processor to implement the corresponding steps of the multi-robot path planning method based on hierarchical control in the above embodiments; one or more instructions in the computer-readable storage medium are loaded and executed by the processor in the following steps:

[0147] The path planning parameters are initialized, and the model information and basic parameters of each robot in the system are obtained. A realistic map of the working environment is constructed using the SLAM algorithm. The grids of the constructed realistic map are merged according to the pixel value, transforming the fine-grained grid map into a coarse-grained grid map. Considering the overall movement direction and target position of the robots, the global path input of the navigation system is obtained using the centralized solution SH-EECBS algorithm. The global path input is published to each robot, and the robots capture the global path information of other robots by subscribing to other robots' paths. The robots perceive the local environmental information in the local map and obtain the surrounding obstacle information of other robots. Based on the global path results of the MAPF algorithm, a local path planning method is adopted, considering the distance to obstacles, the distance to the destination, and the current pose of the robot, to generate a local path and realize the avoidance between robots.

[0148] The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0149] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0150] Please see Figure 2 In specific applications, this may include the following steps:

[0151] Step 1: Perform system initialization and import initialization parameters;

[0152] The initialization parameters include the total number of robots n, robot number, etc., and each robot is assigned an independent namespace (such as tb3_0, tb3_1, tb3_2).

[0153] Step 2: The multi-robot scheduling system obtains the target position and initial position parameters for each robot, where the target position and initial position parameters are in (goal and initialpoint format) format;

[0154] Step 3: Create a global map of the environment. The global map is set as a two-dimensional grid map, which includes passable grids and obstacle grids.

[0155] Specifically, based on the raster method, the two-dimensional spatial map is set into multiple graticles, with the graticle size based on the constructed map. This two-dimensional grid map is converted into an ASCII code format map, and the eight neighbors of each graticle are fitted into a single graticle. The robot's base_footprint size in ROS is also simulated to the size of one graticle of the raster map. Furthermore, traversable and unknown areas in the global map are uniformly represented by gray graticles, processing stations are assigned yellow graticles, and line-edge storage areas are assigned green graticles for use by the MAPF algorithm. Figure 3 As shown.

[0156] Step 4: Goaltrans receives the target point goal published by the user and the initial point defined in the parameter file, integrates the information of each robot into a MAPF format target, and publishes it to the mapfros node;

[0157] Step 5: Based on the global two-dimensional grid map, each robot uses odometry and lidar to detect the surrounding environment at the current location.

[0158] The robot detects surface information of walls, other robots, or other obstacles. When two robots collide, it further predicts the movement status of other robots in advance through local maps and LiDAR.

[0159] Step 6: Each robot uses its own communication module to integrate the surrounding environment information detected by the robot at the current moment, the robot's path set, and the robot's current position, and publishes them to the mapfros node.

[0160] Step 7: The mapfros node runs the path planning algorithm described above. Based on the robot's current position, speed, target position, and obstacle information in the environment, it generates an initial trajectory, which is represented as a series of discrete poses. The hypergraph is then optimized using the g2o optimization framework to adjust the poses and time intervals so that the trajectory satisfies all constraints and optimizes the objective function.

[0161] The objective function is the combined objective function obtained by applying the VO method to the TEB algorithm:

[0162]

[0163] And it satisfies the following constraints:

[0164] , ,

[0165] in, This is the initial pose. It is the final pose.

[0166] These are the robot's kinematic constraints, ensuring... arrive The motion between them conforms to a kinematic model.

[0167] These are velocity and acceleration constraints that ensure the robot does not exceed its physical limits between adjacent poses.

[0168] This is an obstacle avoidance constraint that ensures the robot is in every pose. The distance between the object and the obstacle is greater than the safe distance.

[0169] This is a time constraint, ensuring that the time interval between adjacent poses of the robot meets the motion requirements.

[0170] Step 8: The planexecutor sends the local paths generated by mapf_ros to the movebase of each robot in time step order;

[0171] Step 9: Movebase provides local path planning and motion control for the robot, thereby enabling centralized-distributed planning for the robot.

[0172] Based on real-world test data and simulation verification in industrial scenarios, the multi-robot hierarchical path planning method of this invention demonstrates significant advantages in obstacle avoidance efficiency, trajectory smoothness, and system robustness. The following specific examples and data illustrate its technical value:

[0173] 1. Validation of warehousing and logistics scenarios

[0174] Experimental setup: Ten AGVs were deployed in an e-commerce logistics center. The method of this invention (SH-EECBS global planning + VO-TEB local optimization) was used to compare the results with the traditional algorithm and the pure TEB algorithm.

[0175] Key data:

[0176] Global planning efficiency: The SH-EECBS algorithm has a solution time of <800ms, which is 49.14% shorter than the traditional CBS algorithm (measured in ICRA2022).

[0177] Dynamic obstacle avoidance performance: The VO-TEB joint algorithm reduced the collision rate from 8.7% to 0.9% and the number of trajectory oscillations by 76% (Gazebo simulation).

[0178] Task completion rate: The system achieved a success rate of 98.5% in 5000 tasks, and the average task time was reduced by 22% (compared to pure TEB).

[0179] Application example: In a narrow passage (width < 1.2 times the robot diameter), the environment is modeled as a coarse-grained map through a grid merging strategy (step 3). After SH-EECBS generates a global path, TEB combined with VO dynamically adjusts the trajectory to achieve staggered robot passage (interval time < 2s), and the motor torque fluctuation is reduced by 45% (actual measurement).

[0180] 2. Industrial manufacturing scenario verification

[0181] Experimental setup: 20 robots are deployed on the final assembly line of an automobile factory to perform collaborative handling and assembly tasks. The environment includes dynamic obstacles (AGVs and personnel).

[0182] Key data:

[0183] Collaborative handling efficiency: Multi-machine collaborative control technology (step 9) reduces the handling time of large workpieces by 40% and the load balancing error is <5%.

[0184] Precision assembly accuracy: Based on trajectory optimization with kinematic constraints (formula h_k=0), the assembly error is controlled within ±2mm (compared to ±10mm of traditional DWA).

[0185] System fault tolerance: When the global path fails, the TEB local replanning trigger time is <150ms, and the cumulative error threshold is set to 0.3m (step 7).

[0186] Application example: In the door assembly process, the robot uses LiDAR (step 5) to detect the position of personnel in real time, and combines extended Kalman filter to predict the motion trajectory. VO-TEB generates a spatiotemporal obstacle avoidance path (safe distance 0.5m), with a response latency of <120ms (ROS measured).

[0187] 3. Dynamic obstacle avoidance performance verification

[0188] Simulation scenario: Five robots randomly traverse a 10m × 10m area, with an obstacle density of 30%. Data comparison:

[0189] Obstacle avoidance success rate: The VO-TEB algorithm has an obstacle avoidance success rate of >98%, which is better than the traditional DWA's 85%.

[0190] Trajectory smoothness: The time interval term in the objective function reduces the change in trajectory curvature by 60% (MATLAB simulation).

[0191] Energy consumption optimization: Motor power consumption is reduced by 35% (compared to the unoptimized TEB), but emergency stop and start are limited due to acceleration constraints (r_k≥0).

[0192] Technical advantages: By combining the objective function (J = time term + VO obstacle avoidance term), obstacle avoidance is prioritized in dense obstacle areas and speed is prioritized in open areas, achieving dynamic weight adaptive adjustment (step 7).

[0193] 4. Real-time performance and computational efficiency verification

[0194] Hardware platform: Raspberry Pi 4B + Jetson Nano cluster.

[0195] Performance metrics:

[0196] Planned frequency: TEB optimized frequency 15Hz, meeting the control requirements of industrial-grade AGVs.

[0197] Map processing speed: Coarse-grained raster map (step 3) refresh time <50ms, supports parallel updates of 20 robots.

[0198] Communication overhead: Multi-machine collaborative path publishing latency **<10ms** (ROS actual test), due to namespace isolation and message compression (steps 1 and 6).

[0199] Typical application: In port container transshipment scenarios, the system enables cross-operation of multiple AGVs with a positioning accuracy of ±2cm, reducing the task overtime rate from 15% to 3% (24-hour continuous operation).

[0200] In summary, this invention presents a multi-robot path planning method and system based on hierarchical control, offering an innovative centralized-distributed solution strategy for path planning problems in complex environments. It fully leverages the efficiency of SH-EECBS in handling multi-agent path planning and the accuracy of the TEB+VO method in local path optimization. Through SH-EECBS, the algorithm can effectively manage and resolve conflicts between agents at the upper layer, while utilizing parallel processing capabilities to accelerate the search speed. The lower-level TEB+VO algorithm focuses on optimizing local paths, balancing path quality, time cost, and obstacle avoidance requirements by adjusting time parameters and path shape.

[0201] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0202] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0203] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this invention can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0204] In the embodiments provided by this invention, it should be understood that the disclosed devices / terminals and methods can be implemented in other ways. For example, the device / terminal embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0205] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0206] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0207] If the integrated module / unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random-access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electrical carrier signals and telecommunication signals.

[0208] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus, and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0209] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0210] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0211] The above content is only for illustrating the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solution based on the technical concept proposed in this invention shall fall within the scope of protection of the claims of this invention.

Claims

1. A multi-robot path planning method based on hierarchical control, characterized in that, Includes the following steps: The path planning parameters are initialized, the model information and basic parameters of each robot in the system are obtained, and a realistic map of the working environment is constructed using the SLAM algorithm. The grids of the constructed real map are merged based on pixel values, and each grid is fitted to its neighborhood into a single grid. The original non-idle and impassable areas are unified as gray grids, the processing machine is assigned yellow grids, and the line edge warehouse is assigned green grids for use by the MAPF algorithm. The robot size base_footprint in ROS is simulated to the size of one grid cell of the grid map. Considering the robot's overall movement direction and target position, the global path input of the navigation system is obtained using the centralized solution SH-EECBS algorithm. The global path input is published to each robot, and the robots can capture the global path information of other robots by subscribing to the paths of other robots; the robots perceive the local environmental information in the local map and obtain the surrounding obstacles of other robots; Based on the global path results of the MAPF algorithm, a velocity-obstacle-local path planning method is adopted, considering the distance to obstacles, the distance to the destination, and the robot's current pose, to generate a local path, specifically: Define a mathematical model, a finite-dimensional parameter vector. It contains a discretized sequence of robot poses and time intervals, where, Represents a strictly positive time series. Indicates the robot in the pose at each time point; List other equation constraints; The revised maximum speed of VO is passed into the TEB controller, and the VO method is written into the TEB algorithm plugin as a plugin. The VO method is applied to the TEB algorithm to obtain a combined objective function. The global path planning result of the upper-level MAPF and the robot's current position are used as inputs to the local path planner. With the supplementary data from LiDAR and odometry, the combined objective function is used to output the local path. Combined objective function: in, For the nth time series, For VO cost weights, For robot speed, To avoid speed, The smoothing coefficient is calculated at the cost.

2. The multi-robot path planning method based on hierarchical control according to claim 1, characterized in that, The actual map of the work environment is a two-dimensional map represented by a grid composed of black, white and gray lines.

3. The multi-robot path planning method based on hierarchical control according to claim 1, characterized in that, Fit a grid to its eight neighboring grids and combine them into a single grid.

4. The multi-robot path planning method based on hierarchical control according to claim 1, characterized in that, The robot perceives local environmental information in a local map and obtains information about obstacles around other robots. When two robots collide, the robot predicts the motion state of other robots in advance using a local map and LiDAR. It integrates pose and velocity data using an extended Kalman filter algorithm to obtain a state estimate of the robot and uses multiple sensors to obtain the robot's pose and velocity information in real time.

5. The multi-robot path planning method based on hierarchical control according to claim 1, characterized in that, Other equation constraints are as follows: Kinematic constraints of robots ,make sure arrive The motion between them conforms to a kinematic model; Velocity and acceleration constraints Ensure that the robot does not exceed its physical limitations between adjacent poses; Obstacle avoidance constraints To ensure the robot is in every pose The distance between the object and the obstacle is greater than the safe distance. Time constraints This ensures that the time interval between adjacent poses of the robot meets the motion requirements.

6. The multi-robot path planning method based on hierarchical control according to claim 1, characterized in that, A dynamic tracking algorithm is added to the real-time control of the robot for use in the ROS node for robot navigation.

7. A multi-robot path planning system based on hierarchical control, characterized in that, include: The parameter module initializes the path planning parameters, obtains the model information and basic parameters of each robot in the system, and constructs a realistic map of the working environment through the SLAM algorithm. The merging module merges the grids of the constructed real map based on pixel values, fitting each grid to its neighborhood into a single grid. It unifies non-idle and impassable areas into gray grids, assigns yellow grids to processing machines, and green grids to line-side warehouses for use by the MAPF algorithm. It also normalizes the robot's base_footprint in ROS to the size of a single grid cell in the grid map, considering the robot's overall movement direction and target position, and uses the centralized solution SH-EECBS algorithm to obtain the global path input for the navigation system. The perception module publishes the global path input to each robot, and the robots capture the global path information of other robots by subscribing to the paths of other robots; the robots perceive the local environmental information in the local map and obtain the surrounding obstacles of other robots; The planning module, based on the global path results of the MAPF algorithm, employs a velocity-obstacle-local path planning method, considering the distance to obstacles, the distance to the destination, and the robot's current pose, to generate a local path, specifically: Define a mathematical model, a finite-dimensional parameter vector. It contains a discretized sequence of robot poses and time intervals, where, Represents a strictly positive time series. Indicates the robot in the pose at each time point; List other equation constraints; The revised maximum speed of VO is passed into the TEB controller, and the VO method is written into the TEB algorithm plugin as a plugin. The VO method is applied to the TEB algorithm to obtain a combined objective function. The global path planning result of the upper-level MAPF and the robot's current position are used as inputs to the local path planner. With the supplementary data from LiDAR and odometry, the combined objective function is used to output the local path. Combined objective function: in, For the nth time series, For VO cost weights, For robot speed, To avoid speed, The smoothing coefficient is calculated at the cost.

8. The multi-robot path planning system based on hierarchical control according to claim 7, characterized in that, The robot perceives local environmental information in a local map and obtains information about obstacles around other robots. When two robots collide, the robot predicts the motion state of other robots in advance using a local map and LiDAR. It integrates pose and velocity data using an extended Kalman filter algorithm to obtain a state estimate of the robot and uses multiple sensors to obtain the robot's pose and velocity information in real time.

9. The multi-robot path planning system based on hierarchical control according to claim 7, characterized in that, Other equation constraints are as follows: Kinematic constraints of robots ,make sure arrive The motion between them conforms to a kinematic model; Velocity and acceleration constraints Ensure that the robot does not exceed its physical limitations between adjacent poses; Obstacle avoidance constraints To ensure the robot is in every pose The distance between the object and the obstacle is greater than the safe distance. Time constraints This ensures that the time interval between adjacent poses of the robot meets the motion requirements.

Images