Multi-robot unknown environment collaborative exploration method based on dynamic layering and partitioning strategy

Abstract

The invention relates to a multi-robot unknown environment collaborative exploration method based on a dynamic layering and zoning strategy, which comprises the steps that a plurality of robots are distributed in different areas of an unknown environment for collaborative exploration, each robot constructs a local map in real time through an SLAM algorithm, and a global map is constructed based on information sharing among the robots; based on the exploration area in the global map, segmenting the two-dimensional exploration area corresponding to the global map into a plurality of sub-areas through a dynamic layering and partitioning strategy to obtain a layering and partitioning result; wherein the layering and partitioning result is a sub-region set when the two-dimensional exploration region reaches the optimal hierarchy; based on the starting point position of each robot, distributing the layering and partitioning results among the robots, and balancing the task load between any two adjacent robots in the multiple robots to obtain a task unit set of each robot; and a minimum coverage path is planned on the optimal level based on the task unit set of each robot, and collaborative exploration is carried out based on the minimum coverage path.

Description

Technical Field

[0001] The present invention relates to the field of robot technology, and in particular to a multi-robot unknown environment collaborative exploration method based on a dynamic hierarchical partitioning strategy. Background Art

[0002] In the field of multi-robot autonomous exploration, the current main multi-robot collaboration strategies include action-based collaboration methods, market-auction-based collaboration methods, and area division-based collaboration methods.

[0003] For the action-based multi-robot collaborative exploration method, each robot independently decides on actions based on its own sensor data and environmental perception; the robot exchanges position and exploration information with nearby robots through close-range communication, and uses it as a basis to adjust its own planning path and exploration goals in real time to avoid repeated exploration and cover all unknown areas as much as possible. In this method, each robot makes decisions independently, and the entire system uses distributed task planning. This method is suitable for dynamic environments, and robots make independent decisions based on real-time changes in the environment; however, due to the lack of a good coordination mechanism, local conflicts may occur among multiple robots during the exploration process, and repeated exploration of some areas may occur, resulting in a decrease in overall exploration efficiency;

[0004] For the collaborative exploration method of collaborative robots based on market-auction, robots auction tasks in a central or decentralized manner. In the centralized case, there is an "auction host" to assign tasks; in the decentralized case, robots autonomously initiate auctions and bids by communicating with each other. The exploration target is "auctioned" under the action of the task allocation mechanism, and each robot "bids" according to its own position status, and is finally assigned to the robot with the lowest bid. This method simulates the process of market transactions. The allocation of tasks is based on the principle of utility maximization. Robots effectively allocate tasks through economic models based on cost competition. It is suitable for complex task segmentation, but has high requirements for communication and computing.

[0005] For the multi-robot collaborative exploration method based on area division, the robot tasks are clear and the efficiency is high in environments with low communication requirements, but the adaptability to dynamic environments is poor. Since the exploration environment is unknown, the environment may change significantly as the exploration process progresses. Fixed division of areas may lead to unbalanced loads. Some robots may complete the task earlier, while other areas still require a lot of exploration. Summary of the invention

[0006] In view of the above analysis, an embodiment of the present invention aims to provide a multi-robot collaborative exploration method for an unknown environment based on a dynamic hierarchical partitioning strategy, so as to solve the technical problems of existing methods such as repeated exploration of robots, difficulty in coping with environmental changes, and high computational complexity.

[0007] The purpose of the present invention is mainly achieved through the following technical solutions:

[0008] The present invention provides a multi-robot unknown environment collaborative exploration method based on a dynamic hierarchical partitioning strategy, comprising the following steps:

[0009] Step 1: Multiple mobile robots are distributed in different areas of an unknown environment for collaborative exploration. Each robot builds a local map in real time through the SLAM algorithm, and builds a global map based on information sharing among multiple robots.

[0010] Step 2: Based on the exploration area in the global map, the two-dimensional exploration area corresponding to the global map is divided into multiple sub-areas through a dynamic hierarchical partitioning strategy to obtain a hierarchical partitioning result; wherein the hierarchical partitioning result is a set of sub-areas when the two-dimensional exploration area reaches the optimal level based on the dynamic hierarchical partitioning strategy;

[0011] Step 3: Based on the starting position of each robot, the hierarchical partitioning result is distributed among the robots, and the task amount between any two adjacent robots in the multi-robots is balanced to obtain a task unit set of each robot;

[0012] Step 4: At the optimal level, a minimum coverage path is planned based on any unit set of each robot, and multiple robots perform collaborative exploration based on the minimum coverage path.

[0013] Furthermore, the global map is a three-dimensional map, and the corresponding two-dimensional exploration area is the projection area of ​​the global map on the two-dimensional plane.

[0014] Furthermore, the step 2 comprises:

[0015] The two-dimensional exploration area is taken as the first layer and decomposed into a plurality of non-overlapping sub-areas;

[0016] Any sub-region in the previous layer is further evenly subdivided into multiple sub-regions in the next layer;

[0017] Record the number of unknown voxels in each sub-region in each layer and the center coordinates of the corresponding voxels, calculate the index value of each sub-region in the two-dimensional plane coordinate system, and save it to the nested array Lgrid;

[0018] When the number of known voxels in all the sub-regions of the layer reaches a preset ratio α u , or the sub-region area reaches the preset minimum area S min When , the iteration of hierarchical partitioning is terminated and the optimal level is reached.

[0019] Furthermore, the step 3 comprises:

[0020] Each robot is assigned the sub-area closest to its starting position as the initial task unit;

[0021] Each robot is paired with other adjacent robots to obtain a pair of robots;

[0022] The minimum spanning tree algorithm is used to calculate the workload w of each robot in the pair of robots. i Estimation is performed and the task unit of the robot with higher workload in the pair of robots is transferred to the other robot paired with it until the pairing cost L i,j Reaching the preset threshold ε;

[0023] Finally, the task unit set of each robot is output.

[0024] Furthermore, the step 4 comprises:

[0025] Based on the task unit set of each robot, the minimum coverage path problem of multiple robots is transformed into a vehicle path planning (VRP) problem, and a virtual warehouse is introduced to replace the central warehouse in the VRP as an optimized VRP.

[0026] Solve the open-loop path of each robot starting from the virtual warehouse as the initial position and passing through each sub-area in the task unit set in sequence;

[0027] Establish a cost matrix for the optimized VRP, including the connection cost between sub-areas, the cost between the robot and the sub-area, and the connection cost from the virtual warehouse to the robot;

[0028] The global graph G is used to optimize the robot trajectory. Each sub-region is used as a node of G. When there is no collision path between two adjacent sub-regions, a weighted edge is added, and the weight is determined by the path length.

[0029] During the robot exploration process, when the environment changes and causes the sub-region to be updated, the collision-free path between the sub-region and all adjacent sub-regions is recalculated, and the connection edge weights are updated or the corresponding connections are removed on the global graph G;

[0030] Based on the robot's search capacity constraints, use A * The algorithm performs path planning on the weighted optimal level to obtain the shortest path;

[0031] The shortest path of the affected sub-area is updated in real time through an incremental graph search method, and finally the minimum coverage path is obtained.

[0032] Furthermore, for each sub-region node v k , get the number of unknown voxels in this sub-region And assign ρk ;

[0033] If the sub-region is a known region, the ρ of the node k The value is zero;

[0034] Limit the capacity of each robot to a preset percentage α of the total number of unknown voxels ρ ; Let the task unit set of robot qi be recorded as Ri, then the search capacity constraint of the robot is as follows:

[0035]

[0036] Among them, N h is the number of sub-areas in the task unit set of robot qi.

[0037] Further, the virtual warehouse is set as a reference node at a position where the distance from each robot is minimized;

[0038] The path of each robot is open-loop; each robot starts to explore from the virtual warehouse and explores each sub-area in its task unit set without returning to the virtual warehouse.

[0039] Furthermore, the preset ratio α u is 95%, the preset minimum area S min 0.5*0.5m 2 .

[0040] Furthermore, the step 1 comprises:

[0041] Each mobile robot obtains point cloud data based on the laser radar and acceleration information based on the IMU; uses an iterative extended Kalman filter to fuse the point cloud data with the IMU data, builds a local map in real time, and performs robot self-positioning;

[0042] Publish robot state estimation and local map point cloud data to other robots through the ROS topic transfer mechanism;

[0043] Each robot receives the local maps and self-localization information of other robots as input for its own fusion mapping;

[0044] Each robot fuses the local maps of different robots to build a fused global map.

[0045] Furthermore, each of the mobile robots is equipped with a laser radar, an inertial measurement unit and an industrial computer supporting ROS services;

[0046] The laser radar is used to scan the environment, obtain the three-dimensional contour of the environment, and generate point cloud data;

[0047] The inertial measurement unit is used to estimate the posture and motion trajectory of the robot;

[0048] The industrial computer is used to realize information interaction between corresponding mobile robots;

[0049] Each mobile robot is connected via a wireless LAN.

[0050] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0051] 1. The present invention can adaptively adjust the exploration area division through a dynamic hierarchical partitioning strategy, and divide the unknown environment into multiple fine sub-areas step by step for planning the robot's exploration tasks, thereby avoiding the problems of repeated exploration and local conflicts that may occur in the prior art, thereby improving the efficiency of multi-robot collaborative exploration. This spatial decomposition method improves the accuracy of exploration and makes task allocation more flexible and efficient; the present invention can better adapt to environmental changes and update the global map in real time, making the robot's exploration in dynamic and unknown environments more flexible and effective;

[0052] 2. The present invention allocates task sub-areas to each robot through the interaction between two robots, and ensures that the workload between them is balanced. By simplifying the task allocation problem into a sub-problem between paired robots and using the MST minimum spanning tree algorithm to estimate the workload, a more reasonable task allocation is achieved, the workload between robots is balanced, and the computing and communication requirements are reduced;

[0053] 3. During the exploration process, the robot of the present invention exchanges map data with other robots and updates the ownership of sub-areas through a local communication mechanism; by converting the minimum coverage path problem into an optimized VRP problem and combining the global graph G for path planning, the accuracy and efficiency of path planning are improved, and redundant paths in the exploration process are reduced. At the same time, the coverage path CPs are used to optimize the exploration trajectory of the robot, so as to improve the efficiency and coordination of multiple robots in performing exploration tasks;

[0054] 4. The multi-robots in the present invention can adapt to environmental changes and task requirements through real-time information interaction and task redistribution, which enhances the system's ability to respond to abnormal situations and improves the robustness of the multi-robot exploration process.

[0055] In the present invention, the above-mentioned technical solutions can also be combined with each other to achieve more preferred combination solutions. Other features and advantages of the present invention will be described in the subsequent description, and some advantages can become obvious from the description, or can be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0057] Figure 1 This is a flow chart of a multi-robot unknown environment collaborative exploration method based on a dynamic hierarchical partitioning strategy in an embodiment of the present invention;

[0058] Figure 2 A schematic diagram of updating and maintaining hierarchical partitions in an embodiment of the present invention;

[0059] Figure 3 This is a schematic diagram of an example of collaborative exploration by three robots in an embodiment of the present invention. DETAILED DESCRIPTION

[0060] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not used to limit the scope of the present invention.

[0061] In view of the above technical problems, the present invention adopts a hierarchical partitioning strategy to divide the unknown exploration environment from coarse to fine hierarchical partitioning to give full play to the efficiency of multi-robot collaborative exploration. In the method proposed by the present invention, each robot integrates the local map and positioning information generated by itself to give full play to the efficiency, flexibility and robustness of multi-robot exploration.

[0062] A specific embodiment of the present invention discloses a multi-robot unknown environment collaborative exploration method based on a dynamic hierarchical partitioning strategy, which is characterized by comprising the following steps:

[0063] Step 1: Multiple mobile robots are distributed in different areas of an unknown environment for collaborative exploration. Each robot builds a local map in real time through the SLAM algorithm, and builds a global map based on information sharing among multiple robots.

[0064] Step 2: Based on the exploration area in the global map, the two-dimensional exploration area corresponding to the global map is divided into multiple sub-areas through a dynamic hierarchical partitioning strategy to obtain a hierarchical partitioning result; wherein the hierarchical partitioning result is a set of sub-areas when the two-dimensional exploration area reaches the optimal level based on the dynamic hierarchical partitioning strategy;

[0065] Step 3: Based on the starting position of each robot, the hierarchical partitioning result is distributed among the robots, and the task amount between any two adjacent robots in the multi-robots is balanced to obtain a task unit set of each robot;

[0066] Step 4: At the optimal level, a minimum coverage path is planned based on any unit set of each robot, and multiple robots perform collaborative exploration based on the minimum coverage path.

[0067] The global map constructed by each robot is a three-dimensional map, and the two-dimensional exploration area corresponding to the global map is a two-dimensional map.

[0068] The global map is a three-dimensional map, and the corresponding two-dimensional exploration area is the projection of the global map on the two-dimensional plane.

[0069] The 2D exploration area completely covers the global map.

[0070] The step 1 comprises:

[0071] Each mobile robot acquires point cloud data based on the laser radar and acceleration information based on the IMU; uses an iterative extended Kalman filter to fuse the point cloud data with the IMU data, builds a local map in real time, and performs robot self-positioning;

[0072] Publish robot state estimation and local map point cloud data to other robots through the ROS topic transfer mechanism;

[0073] Each robot receives the local maps and self-localization information of other robots as input for its own fusion mapping;

[0074] Each robot fuses the local maps of different robots to build a fused global map.

[0075] Assume that there are three robots exploring from the same starting point. Figure 3 shown.

[0076] Each of the mobile robots is equipped with a laser radar, an inertial measurement unit, and an industrial computer supporting ROS (Robot Operating System) services;

[0077] The laser radar is used to scan the environment, obtain the three-dimensional contour of the environment, and generate point cloud data;

[0078] The inertial measurement unit is used to estimate the posture and motion trajectory of the robot;

[0079] The industrial computer is used to realize information interaction between corresponding mobile robots;

[0080] Each mobile robot is connected via a wireless LAN.

[0081] Industrial Personal Computer (IPC) is a computer designed specifically for industrial sites. It is durable, reliable, and has strong anti-interference capabilities, making it very suitable for use in harsh industrial environments. In the robot system, the IPC can be used as a control and computing platform. The IPC is installed with the Linux operating system and supports ROS services, because ROS is developed based on Linux.

[0082] Each robot obtains point cloud data based on the LiDAR and obtains posture and motion trajectory based on the IMU. It uses an iterative extended Kalman filter to fuse the point cloud data obtained by the LiDAR with the IMU data, thereby building a local map in real time and performing self-positioning and mapping during the exploration process.

[0083] The topic transfer mechanism of ROS is used to publish the robot state estimation and local map point cloud data obtained by the positioning algorithm. Other robots obtain the corresponding data information by subscribing to the ROS topic, thereby sharing data between robots.

[0084] The content shared by each robot includes: 3D point cloud data of the robot's exploration environment and the robot's positioning information, where the robot's positioning information is the robot's position and orientation information in the local map;

[0085] Each robot receives the local map and positioning information of other robots as input for fusion mapping.

[0086] Multiple robots are connected by wireless LAN Wi-Fi to ensure communication between them;

[0087] After each robot receives the local maps of other robots, it performs map fusion to generate a global map.

[0088] Since each robot is started at a different position, the local map of each robot is in its own coordinate system. After receiving the local map and positioning information shared by other robots, each robot converts the local maps of different robots from the local coordinate systems of other robots into its own coordinate system, performs coordinate conversion and alignment, uses transformation matrices for translation and rotation, uses wheel odometers to obtain the robot's moving distance and angular change, combines point cloud information with feature point scanning and matching methods to estimate the pose transformation between different robots, and uses point cloud registration algorithms to fuse point cloud data to align local maps of different robots.

[0089] After the fusion processing is completed, each robot outputs a global map and the global positioning information corresponding to each robot, which serves as the basis for multi-robot collaborative exploration of unknown environments.

[0090] Each robot obtains the updated local map and positioning information of other robots through the ROS topic mechanism. The global map generated by the fusion map in this step provides the basis for subsequent collaborative exploration by multiple robots.

[0091] The purpose of step 1 is to enable each robot to obtain a fused global map and the global positioning information of each robot in the global map.

[0092] The step 2 comprises:

[0093] The two-dimensional exploration area is taken as the first layer and decomposed into a plurality of non-overlapping sub-areas;

[0094] Any sub-region in the previous layer is further evenly subdivided into multiple sub-regions in the next layer;

[0095] Record the number of unknown voxels in each sub-region in each layer and the center coordinates of the corresponding voxels, calculate the index value of each sub-region in the two-dimensional plane coordinate system, and save it to the nested array Lgrid;

[0096] When the number of known voxels in all the sub-regions of the layer reaches a preset ratio α u , or the sub-region area reaches the preset minimum area S min When , the iteration of hierarchical partitioning is terminated and the optimal level is reached.

[0097] Task allocation in the multi-robot collaborative exploration process is based on global Figure 3 The corresponding two-dimensional exploration area of ​​the dimensional exploration space is subjected to hierarchical regional decomposition.

[0098] Step 2 is divided into steps 21-22.

[0099] Step 21: Based on the exploration area in the global map, the two-dimensional exploration area corresponding to the global map is divided into multiple sub-areas through a dynamic hierarchical partitioning strategy.

[0100] By continuously decomposing the entire two-dimensional exploration area into multiple sets of non-intersecting and non-overlapping sub-areas, each sub-area represents a basic task unit distributed among robots; the decomposed sub-areas completely cover the two-dimensional exploration area corresponding to the global map.

[0101] The hierarchical partitioning decomposition adopted by the present invention is not a one-step decomposition, but a hierarchical partitioning structure from coarse division to fine division.

[0102] like Figure 2 As shown, any sub-area in level n will be further evenly subdivided into 4 sub-areas in level n+1, and so on, layer by layer.

[0103] When the exploration area is irregular in shape, the irregular area can be covered by continuously subdividing the sub-areas and removing the sub-areas occupied by obstacles.

[0104] Initially, the entire two-dimensional exploration area is roughly divided into four sub-areas; as the exploration proceeds, parts of the roughly divided sub-areas become partially unknown;

[0105] The sub-region is further subdivided into four smaller sub-regions. The present invention uses sub-regions of different sizes to represent unknown regions, which provides sufficient flexibility and accuracy for subsequent allocation.

[0106] for Figure 2 The meaning of each icon is as follows:

[0107] Green dots: represent the grid center of each sub-region;

[0108] Unknown obstacle area: indicates the obstacle area that the robot has not explored;

[0109] Known obstacle area: indicates the obstacle area within the robot's exploration range;

[0110] Unknown feasible area: refers to the drivable area within the range that the robot has not explored;

[0111] Known feasible area: represents the drivable area within the robot's exploration range;

[0112] Exploration grid cell: represents the sub-area unit where hierarchical partitioning is performed;

[0113] Partition grid unit: represents the subdivided sub-region unit obtained by performing hierarchical partitioning;

[0114] Removing grid cells: Indicates that the sub-region cells whose area is occupied by obstacles exceeding 90% are removed from the exploration target and are not allocated.

[0115] The largest square is used as the initial state of the two-dimensional exploration area, on this basis, the sub-areas are divided into layers and levels, and the grid center is not set for the initial two-dimensional exploration area.

[0116] Remove the part occupied by obstacles from the sub-area to achieve robot exploration coverage;

[0117] By judging whether there is point cloud data in the voxels in the sub-region, if yes, it is judged that there is an obstacle, otherwise it is judged that there is no obstacle.

[0118] In the process of exploration and mapping, the sub-areas stored by each robot are built based on the two-dimensional plane coordinate system of the two-dimensional exploration area projected from the global map;

[0119] For example, it is assumed that there are n levels of sub-regions {S1, S2, S3, ..., Sn}, among which the sub-region area in S1 is the largest and that in Sn is the smallest; S2 is a part of S1,

[0120] For Sn, the number of unknown voxels in the range and the center coordinates of the voxels (the geometric center of the voxel cube) are recorded, and the following sub-region mapping method is used:

[0121] There are h in the Sl (where 1≤l≤n) level. l =h l,x ×h l,y sub-regions, where h l,x 、h l,y The two variables are the number of sub-areas divided horizontally and vertically at the current division level.

[0122] Among them, the two-dimensional coordinates of a sub-region are (il, jl), and the corresponding index value is calculated as shown in formula (1):

[0123] index(il,jl)=il·h l,y +jl·h l,x Formula (1)

[0124] Among them, h l,x 、h l,y are the number of sub-regions in the x-axis and y-axis directions respectively; l is the corresponding number of levels.

[0125] The role of step 21 is to subdivide the unknown two-dimensional exploration area into multiple non-overlapping sub-areas through a dynamic hierarchical partitioning strategy, providing an accurate basic area division for task allocation and collaborative exploration between robots.

[0126] Step 22: Update and maintain the multi-robot hierarchical partitioning results, and obtain the hierarchical partitioning results when the hierarchical partitioning reaches the optimal level.

[0127] All sub-regions decomposed from the unknown region are stored in the sub-region list Lgrid. Initially, the entire two-dimensional exploration area is completely unknown and is decomposed into the coarsest sub-regions, that is, the sub-region list Lgrid only contains the sub-region information of the S1 layer.

[0128] Lgrid is nested data, in which the stored data include: the unique identification index value index(il,jl) of the sub-region, the center coordinates of the sub-region (il,jl), and the number of voxels within the sub-region.

[0129] Lgrid stores the sub-region data to be explored.

[0130] As each robot continues to explore, build and share local maps, some areas become partially unknown, so it is necessary to further subdivide the sub-areas where there are unknown areas and represent the unknown sub-areas with finer resolution. For the sub-areas Sn marked as to be subdivided, they are further divided into smaller sub-areas.

[0131] During the robot exploration process, the hierarchical partitioning is continuously performed in real time. The voxel center and the number of voxels contained in the Sn sub-region range are recalculated, and Lgrid is updated in real time. The visited sub-regions are removed from LGrid, and the hierarchical partitioned sub-regions to be explored are added.

[0132] Assuming that the size of Sn is L×W, for example, each sub-region is subdivided into n×n smaller sub-units: the sub-region size is During this process, the number of voxels contained in each explored sub-region is reduced accordingly, and the center position of the voxels is also updated and adjusted accordingly.

[0133] When the number of known voxels contained in Sn reaches a certain preset ratio α u .

[0134] For example, α u The default value is 95%.

[0135] The robot exploration covers 95% of the area, which has basically achieved complete exploration coverage of the sub-area. For robot exploration, achieving 95% coverage is already a very high coverage rate, and 100% coverage cannot be achieved at present.

[0136] If the sub-region division of Sn has not reached the optimal division level, that is, Sn has not reached the smallest sub-region, further subdivision is performed on it;

[0137] Delete the sub-region in Sn from Lgrid, and add the sub-region Sn+1 of the next level of subdivision to Lgrid;

[0138] When the number of unknown voxels is less than a certain threshold 1-α u (ie 5%), and when the sub-region division of the current Sn has reached the optimal level or the maximum level, the sub-region is removed from Lgrid, and it is considered that the exploration traversal has been completed.

[0139] In addition, the sub-region area reaches the preset minimum area S min When , the hierarchical partitioning iteration operation is terminated and the optimal level is reached.

[0140] For the preset ratio α u Or preset minimum area S min , both, if any of the conditions are met, the hierarchical partitioning iteration operation is terminated.

[0141] For example, the sub-region area reaches 0.5*0.5m 2 The maximum level.

[0142] The preset ratio α u is 95%, the preset minimum area S min 0.5*0.5m 2 .

[0143] The purpose of step 2 is to subdivide the two-dimensional exploration area in the global map into multiple sub-areas through a dynamic hierarchical partitioning strategy, and update and maintain it in real time to achieve the optimal level of hierarchical partitioning results, so as to achieve effective segmentation and coverage of the unknown environment, and provide accurate basic space division for subsequent robot collaborative exploration and task allocation.

[0144] The step 3 comprises:

[0145] Each robot is assigned the sub-area closest to its starting position as the initial task unit;

[0146] Each robot is paired with other adjacent robots to obtain a pair of robots;

[0147] The minimum spanning tree algorithm is used to calculate the workload w of each robot in the pair of robots. i Estimation is performed and the task unit of the robot with higher workload in the pair of robots is transferred to the other robot paired with it until the pairing cost L i,j Reaching the preset threshold ε;

[0148] Finally, the task unit set of each robot is output.

[0149] Each robot is initially assigned the sub-area that is closest to it. Then, by optimizing the workload of paired robots, the entire allocation problem is simplified into a series of sub-problems of redistributing task units between paired robots.

[0150] Because only two paired robots need to communicate at a time, the communication requirements are reduced and the complexity of the problem is greatly reduced.

[0151] Each robot is paired with a neighboring robot to optimize and balance the workload of each robot.

[0152] If the task unit set of 1 robot is adjacent to the task unit set of 4 robots, the robot will calculate the pairing optimization workload 4 times.

[0153] For example, solve the task unit set T of robots i and j i and T jEssentially, it is an assignment problem, which is to assign the union of the task units belonging to robots i and j (i.e., T i ∪T j ) is re-divided into two new subsets T i ′ and T j ′, as follows:

[0154] Starting from the initial assignment, the workload of the two robots is optimized by moving the task unit corresponding to one robot to the other robot. One task unit corresponds to one sub-area.

[0155] In each iteration, the robot workload is estimated by formula (2). The robot with higher workload is selected to give the task unit to the paired robot, making the pairing cost L ij Reduce the maximum.

[0156] This step continues iteratively until the pairing cost L ij If the decrease compared with L of the previous iteration is less than a certain proportion (exemplarily, the proportion is 5%), it is considered that no further reduction can be achieved.

[0157] The MST (Minimum Spanning Tree) algorithm is used to calculate the workload w of the paired robots. i The estimation is as shown in formula (2):

[0158] ||S i ||≤w i <2||S i || Formula (2)

[0159] ||S i || represents the total distance required to connect all points by minimizing the edge weights of the task unit set Ti of robot i, w i is the estimated distance between task units.

[0160] Move the task unit of the robot with the higher workload in the pair of robots to the other robot it is paired with until the pairing cost L i，j Reach the preset threshold ε.

[0161] Formula (2) is used to estimate the task load of the robot's task unit set.

[0162] S i is the minimum spanning tree of the undirected graph Gi(Vi) of robot i, and extracts the point set Vi={pi,t 1 ,…,t Mi}, where pi is the current position of robot i, t1 ,…,t Mi is the task unit set T of robot i i The task unit coordinates in ; the undirected graph Gi(Vi) is established based on the optimal hierarchy corresponding to the two-dimensional exploration area.

[0163] Each robot explores at a basically constant speed during the exploration process, so the total exploration time of all robots is the longest exploration distance of a certain robot.

[0164] Based on this consideration, the present invention explores the efficiency measurement index as shown in formula (3):

[0165] L=max(||S 1 ||,||S 2 ||,…,||S N ||) Formula (3)

[0166] Among them, L is the measure of exploration efficiency, that is, the longest exploration distance, ||S 1 ||,||S 2 ||,…,||S N || represents the minimum total distance connecting all nodes in the task unit set of robots 1, 2, ..., N respectively.

[0167] Time is not easy to measure accurately, so we use distance to measure it, which is equivalent to converting time into distance (distance is equal to the product of time and constant speed).

[0168] Since the robots establish communication with their neighboring robots, that is, establish a pairing relationship, the optimization goal of the allocation algorithm is transformed into solving the problem of minimizing the exploration efficiency, as shown in formula (4):

[0169]

[0170] Among them, T i，j represents the set of task units after the optimal allocation between robot i and robot j, Minimize the exploration coverage path length; max(||S i ||,||S j ||) represents the longest task load among paired robots i and j;

[0171] For multi-robot collaborative exploration, the time to complete the exploration task is actually determined by the robot that completes the exploration task last. Assuming that the speed of each robot in the entire multi-robot system during the exploration process is considered constant, the time it takes for the entire system to complete the exploration goal is determined by the robot that completes the exploration task last. Therefore, from an overall perspective, the goal that needs to be optimized is the maximum workload.

[0172] By continuously iterating the hierarchical partitioning, the sub-areas of the two robots are redistributed based on the coverage exploration path length of the paired robots until the final cost L i，j Achieve the best.

[0173] By iteratively optimizing the task allocation of two paired robots (robot i and robot j), their workload differences are gradually reduced, thereby optimizing the allocation of the entire task.

[0174] Exemplarily, the task set T={t1, t2, ..., tn} consisting of the initial exploration task units of the two robots, wherein each task unit needs to be assigned to robot i or j, and t1, t2, ..., tn is the set of exploration task units of the two robots.

[0175] The workload cost L of paired robots i and j to perform their respective task unit sets Ti, Tj i，j Defined by formula (4).

[0176] Identify the robot with the higher load: In each iteration, first identify which robot has the higher load.

[0177] For example, assuming that the robot with the higher load is i, select task units from the task set Ti of the robot i with the higher load, and evaluate the possibility of moving one of the task units to robot j according to formula (4).

[0178] For each task unit tk∈T, calculate the workload L after transferring it to robot j i，j .

[0179] Among all candidate task units tk, select the task unit that reduces the total cost the most: select the one that can maximize the reduction of the total cost L i，j task unit and execute it.

[0180] The algorithm stops converging when the total cost cannot be reduced by further task unit moves.

[0181] At the same time, in order to increase the iteration speed, a threshold is set as the stop condition. i，j The iteration stops when the change in is lower than the preset threshold ε. The final output is the redistributed task unit set T i ′ and T j ′.

[0182] For example, in an exploration area of ​​100*100m, the preset threshold ε is set to 0.5m.

[0183] This pairing relationship can be generalized. Consider any two adjacent robots that establish information exchange during the exploration process. Both parties allocate their respective task points through the above algorithm to minimize the path length required to traverse the sub-area.

[0184] Assume T i and T j are the corresponding task unit sets of the two robots in their initial states, and two new task unit sets T are constructed through reasonable allocation i ′ and T j ′, so that the robot can explore T i ′ and T j The total length of the path covered by ′ is the shortest. Finally, the task unit set of each robot is obtained.

[0185] The role of step 3 is to reasonably distribute the sub-areas divided based on the starting position among the robots through a pairing optimization algorithm and balance their workloads to achieve the generation of an efficient set of collaborative exploration task units.

[0186] The step 4 comprises:

[0187] Based on the task unit set of each robot, the minimum coverage path problem of multiple robots is transformed into a vehicle path planning (VRP) problem, and a virtual warehouse is introduced to replace the central warehouse in the VRP as an optimized VRP;

[0188] Solve the open-loop path of each robot starting from the virtual warehouse as the initial position and passing through each sub-area in the task unit set in sequence;

[0189] Establish a cost matrix for the optimized VRP, including the connection cost between sub-areas, the cost between the robot and the sub-area, and the connection cost from the virtual warehouse to the robot;

[0190] The global graph G is used to optimize the robot trajectory. Each sub-region is used as a node of G. When there is no collision path between two adjacent sub-regions, a weighted edge is added, and the weight is determined by the path length.

[0191] During the robot exploration process, when the environment changes and causes the sub-region to be updated, the collision-free path between the sub-region and all adjacent sub-regions is recalculated, and the connection edge weights are updated or the corresponding connections are removed on the global graph G;

[0192] Based on the robot's search capacity constraints, use A * The algorithm performs path planning on the weighted optimal level to obtain the shortest path;

[0193] The shortest path of the affected sub-area is updated in real time through an incremental graph search method, and finally the minimum coverage path is obtained.

[0194] In the hierarchical partition exploration process, the minimum coverage path is planned and the trajectory is optimized based on the optimal level corresponding to the two-dimensional exploration area.

[0195] After completing the task allocation, the essence of solving the robot's minimum coverage path problem is the vehicle path planning problem VRP (Vehicle Routing Problem).

[0196] Assume there are two paired robots q1 and q2: The standard VRP problem consists of a central warehouse and the paths of the robots q1 and q2 forming the entire closed loop;

[0197] The present invention is different from the standard VRP problem in that what needs to be solved is an open-loop path starting from the initial positions of robots q1 and q2 respectively and passing through the task unit set in sequence without returning to the central warehouse;

[0198] Therefore, by introducing a virtual warehouse to replace the central warehouse in the VRP problem and reasonably designing the connection cost, the problem is transformed into a standard VRP problem.

[0199] The virtual warehouse serves as a reference node and is set at a position where the distance from each robot is minimized;

[0200] The path of each robot is open-loop; each robot starts to explore from the virtual warehouse and explores each sub-area in its task unit set without returning to the virtual warehouse.

[0201] During the robot's exploration process, a virtual warehouse is introduced to replace the central warehouse. After solving the minimum coverage path, the connection between the central warehouse and the virtual warehouse is removed.

[0202] The design of the connection cost is as follows.

[0203] Assuming that under the current Sh-level sub-area division, robot i has Nh sub-areas, then solving the VRP involves a total of Nh+3 nodes: among them, the current positions of robots q1 and q2 are counted as 2 nodes, and the virtual warehouse is 1 node.

[0204] Build a cost matrix The form is:

[0205]

[0206] in, represents the connection cost between all sub-region points, represents the cost between the robot and the sub-region, -M inf =-[M inf , M inf ] represents the connection cost from the virtual warehouse to the two robots, Minf is a large value, which is used to reduce the overall cost of the output path; thus, the robot will try its best to find the nearest node as the initial starting point when starting from the virtual warehouse.

[0207] For the robot exploration capacity constraint design, it is as follows:

[0208] Based on the optimization of connection cost, the restriction on robot exploration capacity is introduced to further balance the workload assigned to the robot.

[0209] For each sub-region node ν k , get the number of unknown voxels in this sub-region And assign ρ k ;

[0210] If the sub-region is a known region, the ρ of the node k The value is zero;

[0211] Limit the capacity of each robot to a preset percentage α of the total number of unknown voxels ρ ; Let the task unit set of robot qi be Ri, then the search capacity constraint of the robot is as shown in formula (6):

[0212]

[0213] Among them, N h is the number of sub-areas in the task unit set of robot qi.

[0214] The physical meaning of formula (6) is to constrain the robot's exploration capacity. Formula (4) only optimizes the distance between different sub-areas traversed by the robot. The complexity of the exploration tasks in different sub-areas may be different, so formula (6) is introduced here to constrain it.

[0215] The optimization trajectory of the optimal level corresponding to the two-dimensional exploration area is as follows:

[0216] In the above process, solving VRP involves C h (connection cost between sub-region points) and C q，h The calculation of (the cost between the robot and the sub-region) requires a path search for nodes on the optimal level of the two-dimensional exploration area, which incurs a large computational cost when exploring in a larger-scale environment.

[0217] This computational burden is alleviated through a global graph G; the nodes in the global graph G represent sub-areas, the edges represent the connection paths between nodes, and the weights of the edges represent the cost of robot movement.

[0218] The global graph G is an undirected graph structure consisting of key position nodes in the optimal level of the two-dimensional exploration area and the connectivity between them. The sub-area division results of the optimal level are combined into the global graph G. The sub-areas correspond to the nodes in the global graph G, and the node information in the global graph is used to perform global path search;

[0219] Each sub-region in the optimal level of the two-dimensional exploration area represents a fixed spatial area. Each sub-region can be regarded as a node, and when there is a collision-free path between two adjacent sub-region units, a weighted edge is attached to them in the global graph, and the weight is determined by the path length.

[0220] During the exploration process, when the environment changes, some sub-regions are updated. Whenever a sub-region is updated, the collision-free path between the sub-region and all adjacent sub-regions is recalculated. If a valid path is rediscovered, the weights of the connecting edges with the adjacent sub-regions are updated at the optimal level, and the weights are adjusted according to the new path length. If a collision-free path cannot be found, the corresponding connection is removed.

[0221] During the path search process, the weighted global graph G can be used to perform path planning using A * The algorithm is used to find the shortest path (a series of consecutive nodes representing the optimal route from the starting point to the task unit). When each robot's local map is updated, the global map is updated in real time based on environmental changes. Through the incremental graph search method, the shortest path in the affected area is quickly updated, thereby greatly improving the efficiency of path planning.

[0222] Through the above method, the minimized coverage paths CPs (Coverage Paths) are finally obtained.

[0223] The purpose of step 4 is to convert the task unit set into an optimized VRP problem and use the global graph G for path planning to calculate the minimum coverage path for each robot, realize multi-robot collaborative exploration, and update the global map in real time according to environmental changes.

[0224] In summary, the multi-robot unknown environment collaborative exploration method based on the dynamic hierarchical partitioning strategy of the embodiment of the present invention has the following beneficial effects:

[0225] 1. The present invention can adaptively adjust the exploration area division through a dynamic hierarchical partitioning strategy, and divide the unknown environment into multiple fine sub-areas step by step for planning the robot's exploration tasks, thereby avoiding the problems of repeated exploration and local conflicts that may occur in the prior art, thereby improving the efficiency of multi-robot collaborative exploration. This spatial decomposition method improves the accuracy of exploration and makes task allocation more flexible and efficient; the present invention can better adapt to environmental changes and update the global map in real time, making the robot's exploration in dynamic and unknown environments more flexible and effective;

[0226] 2. The present invention allocates task sub-areas to each robot through the interaction between two robots, and ensures that the workload between them is balanced. By simplifying the task allocation problem into a sub-problem between paired robots and using the MST minimum spanning tree algorithm to estimate the workload, a more reasonable task allocation is achieved, the workload between robots is balanced, and the computing and communication requirements are reduced;

[0227] 3. During the exploration process, the robot of the present invention exchanges map data with other robots and updates the ownership of sub-areas through a local communication mechanism; by converting the minimum coverage path problem into an optimized VRP problem and combining the global graph G for path planning, the accuracy and efficiency of path planning are improved, and redundant paths in the exploration process are reduced. At the same time, the exploration trajectory of the robot is optimized using the coverage path CPs to improve the efficiency and coordination of multiple robots in performing exploration tasks;

[0228] 4. The multi-robots in the present invention can adapt to environmental changes and task requirements through real-time information interaction and task redistribution, which enhances the system's ability to respond to abnormal situations and improves the robustness of the multi-robot exploration process.

[0229] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A multi-robot collaborative exploration method for unknown environments based on a dynamic hierarchical partitioning strategy, characterized in that: The steps include: Step 1: Multiple mobile robots are distributed in different areas of an unknown environment for collaborative exploration. Each robot builds a local map in real time through the SLAM algorithm, and builds a global map based on information sharing among multiple robots. Step 2: Based on the exploration area in the global map, the two-dimensional exploration area corresponding to the global map is divided into multiple sub-areas through a dynamic hierarchical partitioning strategy to obtain a hierarchical partitioning result; wherein the hierarchical partitioning result is a set of sub-areas when the two-dimensional exploration area reaches the optimal level based on the dynamic hierarchical partitioning strategy; Step 3: Based on the starting position of each robot, the hierarchical partitioning result is distributed among the robots, and the task amount between any two adjacent robots in the multi-robots is balanced to obtain a task unit set of each robot; Step 4: At the optimal level, a minimum coverage path is planned based on any unit set of each robot, and multiple robots perform collaborative exploration based on the minimum coverage path.

2. The method according to claim 1, characterized in that: The global map is a three-dimensional map, and the corresponding two-dimensional exploration area is the projection area of ​​the global map on the two-dimensional plane.

3. The method according to claim 1, characterized in that: The step 2 comprises: The two-dimensional exploration area is taken as the first layer and decomposed into a plurality of non-overlapping sub-areas; Any sub-region in the previous layer is further evenly subdivided into multiple sub-regions in the next layer; Record the number of unknown voxels in each sub-region in each layer and the center coordinates of the corresponding voxels, calculate the index value of each sub-region in the two-dimensional plane coordinate system, and save it to the nested array Lgrid; When the number of known voxels in all the sub-regions of the layer reaches a preset ratio α u , or the sub-region area reaches the preset minimum area S min When , the iteration of hierarchical partitioning is terminated and the optimal level is reached.

4. The method according to claim 3, characterized in that: The step 3 comprises: Each robot is assigned the sub-area closest to its starting position as the initial task unit; Each robot is paired with other adjacent robots to obtain a pair of robots; The minimum spanning tree algorithm is used to calculate the workload w of each robot in the pair of robots. i Estimation is performed and the task unit of the robot with higher workload in the pair of robots is transferred to the other robot paired with it until the pairing cost L i,j Reaching a preset threshold ε; Finally, the task unit set of each robot is output.

5. The method according to claim 1, characterized in that: The step 4 comprises: Based on the task unit set of each robot, the minimum coverage path problem of multiple robots is transformed into a vehicle path planning (VRP) problem, and a virtual warehouse is introduced to replace the central warehouse in the VRP as an optimized VRP; Solve the open-loop path of each robot starting from the virtual warehouse as the initial position and passing through each sub-area in the task unit set in sequence; Establish a cost matrix for the optimized VRP, including the connection cost between sub-areas, the cost between the robot and the sub-area, and the connection cost from the virtual warehouse to the robot; The global graph G is used to optimize the robot trajectory. Each sub-region is used as a node of G. When there is no collision path between two adjacent sub-regions, a weighted edge is added, and the weight is determined by the path length. During the robot exploration process, when the environment changes and causes the sub-region to be updated, the collision-free path between the sub-region and all adjacent sub-regions is recalculated, and the connection edge weights are updated or the corresponding connections are removed on the global graph G; Based on the robot's search capacity constraints, use A * The algorithm performs path planning on the weighted optimal level to obtain the shortest path; The shortest path of the affected sub-area is updated in real time through an incremental graph search method, and finally the minimum coverage path is obtained.

6. The method according to claim 5, characterized in that: For each sub-region node ν k , get the number of unknown voxels in this sub-region And assign ρ k ; If the sub-region is a known region, the ρ of the node k The value is zero; Limit the capacity of each robot to a preset percentage α of the total number of unknown voxels ρ ; Let the task unit set of robot qi be recorded as Ri, then the search capacity constraint of the robot is as follows: Among them, N h is the number of sub-areas in the task unit set of robot qi.

7. The method according to claim 5, characterized in that: The virtual warehouse serves as a reference node and is set at a position where the distance from each robot is minimized; The path of each robot is open-loop; each robot starts to explore from the virtual warehouse and explores each sub-area in its task unit set without returning to the virtual warehouse.

8. The method according to claim 3, characterized in that: The preset ratio α u is 95%, the preset minimum area S min 0.5*0.5m 2 .

9. The method according to claim 1, characterized in that: The step 1 comprises: Each mobile robot acquires point cloud data based on the laser radar and acceleration information based on the IMU; uses an iterative extended Kalman filter to fuse the point cloud data with the IMU data, builds a local map in real time, and performs robot self-positioning; Publish robot state estimation and local map point cloud data to other robots through the ROS topic transfer mechanism; Each robot receives the local maps and self-localization information of other robots as input for its own fusion mapping; Each robot fuses the local maps of different robots to build a fused global map.

10. The method according to any one of claims 1 to 9, characterized in that: Each of the mobile robots is equipped with a laser radar, an inertial measurement unit, and an industrial computer supporting ROS services; The laser radar is used to scan the environment, obtain the three-dimensional contour of the environment, and generate point cloud data; The inertial measurement unit is used to estimate the posture and motion trajectory of the robot; The industrial computer is used to realize information interaction between corresponding mobile robots; Each mobile robot is connected via a wireless LAN.

Images