Boundary-based multi-robot collaborative exploration mapping method

Abstract

The invention belongs to the technical field of exploration mapping in unknown fields, and particularly relates to a multi-robot collaborative exploration mapping method based on boundaries. The method comprises the following steps: setting a sub-mapping system of any robot, and setting a mother mapping system of a plurality of robots; establishing an environment local grid map by using laser radar detection data in the sub-mapping system; defining the centroid of the junction of the known free area and the unknown area of the grid map as a boundary point; obtaining a candidate target point of each robot; obtaining a collision-free shortest path from the position of the robot to the corresponding target point; in the mobile exploration process, each robot independently establishes and updates a respective local map; and performing map fusion through a map fusion module to obtain a global map of the environment. According to the method, all the robots do not need to carry out global planning, repeated search points are greatly reduced, and the problems that in the prior art, multiple robots can carry out serious repeated search, exploration time is long, and energy consumption is large are solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of unknown domain exploration and mapping, and specifically relates to a boundary-based multi-robot collaborative exploration and mapping method. Background Art

[0002] With the development of science and technology and the transformation of industry, robotics has become an important branch of modern science and technology, and its application scope is becoming increasingly wide. From industrial production to deep-sea exploration, from medical services to space exploration, robots play an indispensable role. The autonomous exploration capability of robots has always been a technical problem that scholars have been committed to overcoming. In the existing technology, in order to realize the automatic exploration capability of robots and to achieve accuracy, multiple robots are often used to conduct regional exploration. For example, J. Yuan et al. proposed a multi-robot regional exploration collaboration method considering communication restrictions. All robots select local destinations that meet the communication distance constraints, arrive at the destination at the same time, communicate and integrate map information. In this way, each robot needs to pass through the entire unknown area, that is, any robot performs global planning, and multiple robots will have serious repeated searches, long exploration time, and high energy consumption.

[0003] Moreover, multi-robot collaborative exploration and mapping involves three parts: "slam", "path planning" and "autonomous exploration algorithm". According to the existing technology, how to allocate appropriate exploration points to multiple robots to reduce the robots' repeated searches and improve the efficiency of exploration and mapping is a technical problem that needs to be solved urgently. Summary of the invention

[0004] The purpose of the present invention is to overcome the defects of serious repeated search, long exploration time and high energy consumption in the prior art, and provide a boundary-based multi-robot collaborative exploration and mapping method in which multiple robots coordinate with each other and perform local map planning, and fuse multiple local maps through adaptive Monte Carlo positioning and the map_merger function package in ROS to generate a global map.

[0005] The technical solution adopted by the present invention to solve the technical problem is: a boundary-based multi-robot collaborative exploration and mapping method, comprising the following steps:

[0006] Step 1: Set up a sub-mapping system for any robot and set up a parent mapping system for multiple robots; the sub-mapping system includes a laser radar installed on the robot, a chassis ROS driver module, a frontier point calculation module, a candidate target point calculation module, and a map construction module; the parent mapping system includes a task allocation module and a map fusion module;

[0007] Step 2: Use the laser radar detection data in the sub-mapping system to build a local grid map of the environment;

[0008] Step 3: Using the local grid map of the environment established in step 2 as input, define the centroid of the junction between the known free area and the unknown area of ​​the grid map as the boundary point;

[0009] Step 4: Based on the boundary points obtained in step 3, use the frontier point calculation module to obtain the candidate target points of each robot, evaluate the cost of the candidate target points based on the frontier method, and use the market method allocation mechanism;

[0010] Step 5: Take the exploration point assigned to each robot in step 4 as the target point of the robot, introduce the improved A* algorithm fused with the TEB algorithm to perform collision-free path planning for the robot, and obtain the collision-free shortest path from the robot position to the corresponding target point;

[0011] Step 6: During the mobile exploration process, each robot independently builds and updates its own local map through its own sub-mapping system;

[0012] Step 7: Use the feature matching method to obtain the pose transformation matrix between the local maps corresponding to each robot obtained in step 6, and perform map fusion through the map fusion module until no new frontier points appear, and obtain the global map of the environment.

[0013] Specifically, the use of the frontier point calculation module in step 4 to obtain the candidate target points of each robot includes: the frontier in the frontier method is divided into two categories, A and B; when it is a category A frontier, take the laser point directly in front of the robot, connect the laser point with the corresponding laser radar center, and select the point on this connection line that is smaller than the maximum range and adjacent to the maximum range as the candidate target point;

[0014] When it is a Class B frontier, connect the laser radar to the connection point N of the wall, then take a point M from the line segment, so that the distance Bd between the connection points N and M is greater than 1 / 2 of the robot width, and then draw an arc with the laser radar as the center so that it passes through point M, and then draw a perpendicular line through point N to NM, and take a point Q on the perpendicular line. The distance between the connection points N and Q is greater than 1 / 2 of the robot width, connect the midpoint of the laser radar to point Q and extend it to the arc, and the intersection of this line segment and the arc is the candidate target point.

[0015] Specifically, the cost value of the cost evaluation in step 4 includes distance cost and information gain;

[0016] The distance cost value is calculated as follows:

[0017] The expected distance for the robot to reach the candidate target point based on the frontier method is:

[0018]

[0019] The adjustment angle of the robot to reach the candidate target point based on the frontier method is:

[0020]

[0021] In the formula, i is the point whose cost value is currently to be calculated, D dis (i) represents the Euclidean distance between the point and the center of the robot, A angle (i) represents the angle between the straight line connecting the point and the center of the robot and the positive direction of the robot.

[0022] Specifically, the information gain cost value is calculated as follows:

[0023] Information entropy is used to calculate the information gain of the grid cell. Assuming that the occupancy probability of the i-th grid cell is m i , the binary occupancy entropy calculation formula of this unit is as follows:

[0024] H(i)=-m i log(m i )-(1-m i )log(1-m i );

[0025] Calculate the expected measured occupancy probability m of the perceived grid cell + The posterior entropy H + (i * ) and the expected measured unoccupied probability m - The posterior entropy H - (i * ); Perceived posterior expected entropy:

[0026] E(H(i * ))=m + H + (i * )+m - H - (i * );

[0027] Then the expected information gain of the i-th grid cell after sensor perception is:

[0028] I(i)=H(i)-E(H(i * )).

[0029] Specifically, the task allocation strategy using the market allocation mechanism includes the following:

[0030] Set the main goals of task assignment as:

[0031]

[0032] Among them, A ijTo allocate the result matrix, the elements in the matrix are A ij ∈(0,1), the value rule of this element is that when a robot is assigned to a boundary cell, A ij =1, otherwise A ij =0, B ij is the benefit matrix, and the elements in the matrix are defined as the benefits that can be obtained by sending the robot to explore the border cell;

[0033] The calculation formula of the benefit matrix is:

[0034]

[0035] in, Represents all assigned boundary cells and boundary cell I j The positional relationship between them is calculated to reduce B ij The value of radius represents the radius of the circle with the candidate target point unit as the center.

[0036] Specifically, in step 5, the introduction of the improved A* algorithm fused with the TEB algorithm to perform collision-free path planning for the robot includes the following contents:

[0037] Dynamic weighted optimization of the A* algorithm;

[0038] Potential field fusion and smoothing;

[0039] Integrate the improved TEB algorithm to perform dynamic obstacle avoidance;

[0040] Plan the shortest path from the robot to the corresponding target point.

[0041] Specifically, the pose transformation matrix between the local maps corresponding to each robot obtained in step 7 includes the following contents:

[0042] The AMCL algorithm is used for initial positioning between multiple robots;

[0043] Align the local coordinate systems of multiple robots with the global coordinate system;

[0044] Obtain the pose transformation matrix between the local maps corresponding to each robot.

[0045] Specifically, in step six, each robot uses the shortest path corresponding to step five to establish and update its own local map through the map construction module corresponding to each robot.

[0046] Specifically, the map fusion described in step 7 includes the following contents:

[0047] Through the map_merger node, traverse each map grid cell one by one, and process the value of each pixel according to the preset merging logic;

[0048] Obtain and unify multiple data maps after coordinate system conversion;

[0049] Define a blank map;

[0050] Any pixel [i, j] on the blank map is calculated and located to the corresponding pixel position in the local map of any robot according to the splicing parameters, and the final value of any pixel in the blank map is determined in combination with the merging rules, thus completing the fusion of the local maps of multiple robots.

[0051] The beneficial effects of the boundary-based multi-robot collaborative exploration and mapping method of the present invention are:

[0052] The mapping method of the present invention first performs boundary detection on any robot, uses the frontier point to obtain the candidate target point of each robot according to the corresponding boundary point, then evaluates the cost of the candidate target point, uses the market allocation mechanism to obtain the allocated exploration point, the exploration point is the target point of the robot, and then performs path planning for the initial position and the target point, plans the local map of each robot, calculates the pose transformation matrix between the local maps corresponding to each robot, and finally performs map fusion through the map fusion module until no new frontier point appears, and obtains the global map of the environment. The mapping method of the present invention first sets the boundary of the robot, and any robot only needs to perform path planning in the boundary area to form a local map, and then uses AMCL (i.e., adaptive Monte Carlo positioning) and the map_merger function package in ros to fuse multiple local maps to generate a global map, which does not require each robot to perform global planning, greatly reduces repeated search points, and solves the problem that multiple robots in the prior art will have serious repeated searches, long exploration time, and high energy consumption.

[0053] In addition, this application uses distance cost and information gain cost to evaluate and select the optimal exploration point, and uses a task allocation strategy based on the potential field method to allocate exploration points to each robot through a task allocation module to ensure the accuracy of the allocated exploration points. At the same time, it takes into account the cost of reaching the target point and the utility of the target point to ensure the accuracy of the target point.

[0054] At the same time, this application adopts the dynamic weighted optimization of the A* algorithm, potential field fusion and smoothing processing, and the fusion-improved TEB algorithm to enable the path planning algorithm to have the ability of dynamic obstacle avoidance, which significantly improves the efficiency and robustness of the path planning algorithm in planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0056] Figure 1 It is a flowchart of the steps of the mapping method according to an embodiment of the present invention.

[0057] Figure 2 4 is a block diagram of a mapping system according to an embodiment of the present invention.

[0058] Figure 3 It is a schematic diagram of the structure of a sub-mapping system and a main mapping system according to an embodiment of the present invention.

[0059] Figure 4 Schematic diagram of selecting Class A frontier points in an embodiment of the present invention.

[0060] Figure 5 Schematic diagram of selecting Class B frontier points in an embodiment of the present invention.

[0061] Figure 6 It is a comparison chart of the effects of the traditional A* algorithm and the improved A* algorithm in the embodiment of the present invention in scenario one.

[0062] Figure 7 It is a comparison chart of the effects of the traditional A* algorithm and the improved A* algorithm in the embodiment of the present invention in scenario 2.

[0063] Figure 8 It is a comparison diagram of the effects of the improved A* algorithm and the fused A* algorithm in scenario one in an embodiment of the present invention.

[0064] Fig. 9 It is a comparison chart of the effects of the improved A* algorithm and the fused A* algorithm in scenario 2 in the embodiment of the present invention.

[0065] Fig.10 It is a comparison chart of the effects of the fused A* algorithm and the fused optimized A* algorithm in scenario one in an embodiment of the present invention.

[0066] Fig.11 It is a comparison chart of the effects of the fused A* algorithm and the fused optimized A* algorithm in scenario 2 in an embodiment of the present invention.

[0067] Fig.12 This is a diagram showing the effect of combining the optimized A* algorithm and the CSC-TEB algorithm in scenario 1 in an embodiment of the present invention.

[0068] Fig.13 This is a diagram showing the effect of combining the optimized A* algorithm and the CSC-TEB algorithm in scenario 2 in an embodiment of the present invention.

[0069] Fig.14 It is a schematic diagram of map splicing of the RRT algorithm in scene 1 in the prior art.

[0070] Fig.15 It is a schematic diagram of map splicing in scenario one of an embodiment of the present invention.

[0071] Fig.16 This is a schematic diagram of map stitching using the RRT algorithm in scene 2 in the prior art.

[0072] Fig.17 It is a schematic diagram of map splicing in scenario 2 according to an embodiment of the present invention.

[0073] Fig.18 It is a schematic diagram of unified alignment in an embodiment of the present invention. DETAILED DESCRIPTION

[0074] The present invention will now be described in further detail with reference to the accompanying drawings. These drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0075] like Figure 1-Figure 3 A specific embodiment of a boundary-based multi-robot collaborative exploration and mapping method of the present invention shown in FIG. 1 includes the following steps:

[0076] Step 1: Set up a sub-mapping system for any robot and set up a parent mapping system for multiple robots; the sub-mapping system includes a laser radar installed on the robot, a chassis ROS driver module, a frontier point calculation module, a candidate target point calculation module, and a map construction module; the parent mapping system includes a task allocation module and a map fusion module;

[0077] Step 2: Use the laser radar detection data in the sub-mapping system to build a local grid map of the environment;

[0078] Step 3: Using the local grid map of the environment established in step 2 as input, define the centroid of the junction between the known free area and the unknown area of ​​the grid map as the boundary point;

[0079] Step 4: Based on the boundary points obtained in step 3, use the frontier point calculation module to obtain the candidate target points of each robot, evaluate the cost of the candidate target points based on the frontier method, and use the market method allocation mechanism;

[0080] Step 5: Take the exploration point assigned to each robot in step 4 as the target point of the robot, introduce the improved A* algorithm fused with the TEB algorithm to perform collision-free path planning for the robot, and obtain the collision-free shortest path from the robot position to the corresponding target point;

[0081] Step 6: During the mobile exploration process, each robot independently builds and updates its own local map through its own sub-mapping system;

[0082] Step 7: Use the feature matching method to obtain the pose transformation matrix between the local maps corresponding to each robot obtained in step 6, and perform map fusion through the map fusion module until no new frontier points appear, and obtain the global map of the environment.

[0083] In step 3, for the detection of the boundary, the resolution, size, origin coordinates and occupancy status of each grid of the grid map are first received, and then for each grid, the occupancy status of the surrounding grids is checked. If the occupancy status value of a grid is zero, and the occupancy status value of its adjacent grid is non-zero, the grid may be at the junction of free space and unknown area or obstacle, that is, the boundary area.

[0084] See also Figure 4 , the use of the frontier point calculation module in step 4 to obtain the candidate target points of each robot includes: the frontier in the frontier method is divided into two categories, A and B; when it is a category A frontier, take the laser point directly in front of the robot, connect the laser point with the corresponding laser radar center, and select the point on this line that is smaller than the maximum range and adjacent to the maximum range as the candidate target point.

[0085] like Figure 5 As shown in the figure, when it is a Class B frontier, connect the laser radar to the connection point N of the wall, and then take a point M from the line segment, so that the distance Bd between the connection points N and M is greater than 1 / 2 of the robot width, and then draw an arc with the laser radar as the center, so that it passes through point M, and then draw a perpendicular line NM through point N, and take a point Q on the perpendicular line, the distance between the connection points N and Q is greater than 1 / 2 of the robot width, connect the midpoint of the laser radar with point Q and extend it to the arc, and the intersection of this line segment and the arc is the candidate target point.

[0086] The mapping method of the present invention first performs boundary detection on any robot, uses the frontier point to obtain the candidate target point of each robot according to the corresponding boundary point, then evaluates the cost of the candidate target point, uses the market allocation mechanism to obtain the allocated exploration point, the exploration point is the target point of the robot, and then performs path planning for the initial position and the target point, plans the local map of each robot, calculates the pose transformation matrix between the local maps corresponding to each robot, and finally performs map fusion through the map fusion module until no new frontier point appears, and obtains the global map of the environment. The mapping method of the present invention first sets the boundary of the robot, and any robot only needs to perform path planning in the boundary area to form a local map, and then uses AMCL (i.e., adaptive Monte Carlo positioning) and the map_merger function package in ros to fuse multiple local maps to generate a global map, which does not require each robot to perform global planning, greatly reduces repeated search points, and solves the problem that multiple robots in the prior art will have serious repeated searches, long exploration time, and high energy consumption.

[0087] It should be further explained that this embodiment calculates the cost value of the robot by considering the distance cost and information gain of the candidate target points, and assigns exploration points to it through the task assignment module. Specifically, the cost value of the cost evaluation in step four includes the distance cost and information gain.

[0088] Specifically, the distance cost is calculated as follows:

[0089] The expected distance for the robot to reach the candidate target point based on the frontier method is:

[0090]

[0091] The adjustment angle of the robot to reach the candidate target point based on the frontier method is:

[0092]

[0093] In the formula, i is the point whose cost value is currently to be calculated, D dis (i) represents the Euclidean distance between the point and the center of the robot, A angle (i) represents the angle between the straight line connecting the point and the center of the robot and the positive direction of the robot.

[0094] The information gain cost is calculated as follows:

[0095] When calculating the information gain after sensor perception, the information entropy is first used to calculate the information gain of the grid unit. Assuming that the occupancy probability of the i-th grid unit is, the binary occupancy entropy calculation formula of the unit is as follows:

[0096] H(i)=-mi log(m i )-(1-m i )log(1-m i );

[0097] Calculate the expected measured occupancy probability m of the perceived grid cell + The posterior entropy H + (i * ) and the expected measured unoccupied probability m - The posterior entropy H - (i * ); Perceived posterior expected entropy:

[0098] E(H(i * ))=m + H + (i * )+m - H - (i * );

[0099] Then the expected information gain of the i-th grid cell after sensor perception is:

[0100] I(i)=H(i)-E(H(i * )).

[0101] Finally, in order to enable the system to effectively assign appropriate target point exploration tasks to each robot and improve the efficiency of the system, this paper adopts a task allocation strategy based on the market law; the task allocation strategy using the market law allocation mechanism includes the following contents:

[0102] Set the main goals of task assignment as:

[0103]

[0104] Among them, A ij To allocate the result matrix, the elements in the matrix are A ij ∈(0,1), the value rule of this element is that when a robot is assigned to a boundary cell, A ij =1, otherwise A ij =0, B ij is the benefit matrix, and the elements in the matrix are defined as the benefits that can be obtained by sending the robot to explore the border cell;

[0105] The calculation formula of the benefit matrix is:

[0106]

[0107] in, Represents all assigned boundary cells and boundary cell Ij The positional relationship between them, and by calculating the positional relationship to reduce B ij value, and radius represents the radius with the candidate target point unit as the center. It can be seen from the above formula that when the robot searches for a position that has been searched by other robots, B ij will decrease. Therefore, the robot will explore a boundary unit near the boundary unit that has been assigned to other robots with a lower probability, so as to reduce the repeated search of the robot.

[0108] Take the exploration points assigned to each robot in step four as the target points of the robot, introduce the improved A* algorithm combined with the TEB algorithm to perform collision-free path planning for the robot, and obtain the collision-free shortest path from the robot's position to the corresponding target point. Among them, the introduction of the improved A* algorithm combined with the TEB algorithm to perform collision-free path planning for the robot in step five includes the following contents: dynamic weighted optimization of the A* algorithm; potential field fusion and smoothing processing; fusion of the improved TEB algorithm to perform dynamic obstacle avoidance; plan the shortest path from the robot to the corresponding target point.

[0109] Specifically, the cost function of the traditional A* algorithm is: f(x) = g(x) + h(x);

[0110] In the formula, g(x) represents the actual cost from the starting point to the current point, and h(x) represents the estimated cost from the current point to the end point. Since in the actual search process, if h(x) < g(x), it will lead to a small number of key inflection points in the planned path and it is difficult to plan the optimal route; if h(x) > g(x), there may be more key inflection points in the path planning, which will waste computing resources. Therefore, in this embodiment, weights are assigned to g(x) and h(x), and a dynamic weight coefficient is added to g(x) and h(x). The improved cost function is:

[0111]

[0112] In the formula, a is the Euclidean distance from the current point to the starting point, b is the Euclidean distance from the current point to the end point, and D is the Euclidean distance from the starting point to the end point.

[0113] In the improved cost function, g(x) and h(x) are effectively balanced. When the distance from the starting point is relatively close, the weight of the actual cost is less than the weight of the estimated cost, which improves the convergence speed of the search towards the target point. When the distance from the end point is relatively close, the weight of the actual cost is higher than the weight of the estimated cost, making the robot more inclined to plan the optimal path, well balancing the influence of the actual cost and the estimated cost, and effectively improving the search efficiency of the robot. For specific reference, see Figure 6 and Figure 7 , in scenario one and scenario two, the path diagrams planned by the improved cost function of this embodiment.

[0114] In order to further improve the search efficiency and shorten the path length, the artificial potential energy is integrated with the improved A* algorithm and smoothed.

[0115] Among them, the expression of gravitational potential field is:

[0116]

[0117] Where: K att is the weight of the gravitational potential field, P is the robot coordinate position, P goal is the target point coordinate position, ρ(P,P goal ) is the distance from the robot to the target point;

[0118] The gravitational function is obtained by taking the negative gradient derivative of the gravitational potential field, as shown below:

[0119]

[0120] The expression of repulsive potential field is:

[0121]

[0122] Where: K rep is the weight of the repulsive potential field, ρ0 is the influence range of the obstacle, P obs is the obstacle coordinate position, ρ(P,P obs ) is the distance between the robot and the obstacle;

[0123] Similarly, taking the derivative of the repulsive potential field, we can get the repulsive function:

[0124]

[0125] By setting the gravitational field and the repulsive field, the heuristic function of the A* algorithm is further improved, and the further improved cost function is:

[0126]

[0127] like Figure 8 and Fig. 9 As shown, the two robots plan paths in the corresponding scenes respectively. The green node is the starting point, the red node is the end point, the black is the obstacle area, the red line is the path planned by the improved A* algorithm, and the yellow line is the path planned by the fused A* algorithm.

[0128] Table 1 is a comparison table of the improved A* algorithm and the fused A* algorithm:

[0129]

[0130]

[0131] As can be seen from the above charts, the maps of the two algorithms are the same, both are 30*30 grid maps. Compared with the improved A* algorithm, the path length of the A* algorithm after algorithm fusion is reduced by 0.92% and 0.87% in the two scenarios respectively, and the search efficiency is improved by 33.3% and 30.4% in the two scenarios respectively, and the number of inflection points is also reduced. This shows that the fused A* algorithm is effective in reducing path length and improving search speed.

[0132] In order to increase the smoothness of the path, the key point extraction strategy is used to optimize the nodes, reduce the number of turns, and further use the Bezier curve to optimize the path to make it smoother. The Bezier curve will not be described in detail here. Fig.10 As shown in the path planning diagram in scenario 1, Fig.11 This is the route planning diagram in scenario 2. The red line is the path planned by the A* algorithm after fusion with the APF algorithm, and the blue line is the path planned after the A* algorithm is fused and optimized. It can be seen from the figure that by applying the key point extraction strategy and Bessel smoothing processing to the path planned by the new global path planning algorithm, the path is not only further reduced in length, but also the smoothness of the path is greatly improved.

[0133] The improved TEB algorithm in this embodiment is integrated to perform dynamic obstacle avoidance. In order to ensure that the robot can reduce sharp turns or acceleration and deceleration when turning to avoid obstacles to achieve a safe and smooth obstacle avoidance process, the TEB algorithm is improved to the CSC-TEB algorithm and the speed control factor α is introduced. v ,in: In the formula, α v The value of is between (0,1), and Δθ represents the i+1th posture S of the mobile robot. i+1 The velocity angle θ i+1 With the i-th posture S i The velocity angle θ i The difference between the robot's maximum instantaneous velocity v i The calculation formula is: i =v imax *α v ; In the formula, v imax Indicates the maximum speed when the algorithm is initialized.

[0134] like Fig.12 and Fig.13As shown, this embodiment uses MATLAB to combine the fused and optimized A* algorithm with the CSC-TEB algorithm. The planned path can be adjusted in real time to adapt to environmental changes, and the planned path maintains a sufficient distance from obstacles. At the same time, the planned path is smooth enough to avoid the robot from making sharp turns or accelerating / decelerating, which shows that the planned path is also more in line with the robot's movement.

[0135] The map fusion described in step seven includes the following: through the map_merger node, traverse the grid cells of each map one by one, process the value of each pixel according to the preset merging logic, obtain and unify multiple data maps after coordinate system conversion, define a blank map, calculate and locate any pixel [i, j] on the blank map to the corresponding pixel position in any robot local map according to the splicing parameters, and determine the final value of any pixel in the blank map in combination with the merging rules, thus completing the fusion of the local maps of multiple robots.

[0136] Table 2 is a comparison table of the search length and exploration time of the embodiment of the present invention and the multi-robot RRT algorithm in the prior art:

[0137] method Total driving distance / m Exploration time / S CSC-TEB algorithm 86 35 RRT Algorithm 109 52

[0138] From the above table Figure 14 to Figure 17 It can be seen that the CSC-TEB algorithm in this embodiment is significantly shorter in search path length and exploration time than the multi-robot RRT algorithm.

[0139] Specifically, in this embodiment, taking robot1 and robot2 as examples, their local coordinate systems are aligned to ensure that their position representations in the same coordinate system are consistent. Fig.18 As shown, it clearly shows the position and orientation relationship of robot1 and robot2 in the unified coordinate system. In the main coordinate system of the multi-robot system, the z-axis is always perpendicular to the plane where the robots are located. For the convenience of representation, only the xy two-dimensional plane is drawn. The z-axis always exists, and its direction conforms to the right-hand rule and is perpendicular to the paper and outward. Fig.18 In, X world O world Y world is the global coordinate system, X robot1 / map O robot1 / map Y robot1 / map is the local coordinate system of robot 1 after positioning in AMCL, X robot2 / map O robot2 / map Y robot2 / mapis the local coordinate system of robot2 after positioning in AMCL. [x1,y1]T and [x2,y2]T are the translation vectors of robot1 / map and robot2 / map relative to world, and θrobot1 / map and θrobot1 / map are the rotation angles of robot1 / map and robot2 / map relative to world. Therefore, the homogeneous transformation matrix of robot1 / map and robot2 / map relative to world is:

[0140]

[0141] That is, the homogeneous transformation matrices of robot1 / map and robot2 / map relative to the world coordinate system are obtained through the AMCL method. and

[0142] In this embodiment, the size of the blank map is determined based on the range after robot1 / map and robot2 / map are superimposed to ensure that there is enough space to accommodate the merged map information. In the process of constructing the blank map M, for each pixel [i, j] on it, the corresponding pixel position in the original map robot1 / map and robot2 / map is accurately calculated and located according to the splicing parameters. Subsequently, based on the values ​​of the corresponding pixels in these local maps and combined with the merging rules, the final value of the [i, j] pixel in the blank map M is determined. This process ensures that the blank map M can accurately reflect the comprehensive information of the multi-robot exploration area, while avoiding the occlusion problem caused by direct superposition.

[0143] It should be understood that the specific embodiments described above are only used to explain the present invention, and are not used to limit the present invention. Obvious changes or modifications derived from the spirit of the present invention are still within the protection scope of the present invention.

Claims

1. A boundary-based multi-robot collaborative exploration and mapping method, characterized in that: The following steps are involved: Step 1: Set up a sub-mapping system for any robot and set up a parent mapping system for multiple robots; the sub-mapping system includes a laser radar installed on the robot, a chassis ROS driver module, a frontier point calculation module, a candidate target point calculation module, and a map construction module; the parent mapping system includes a task allocation module and a map fusion module; Step 2: Use the laser radar detection data in the sub-mapping system to build a local grid map of the environment; Step 3: Using the local grid map of the environment established in step 2 as input, define the centroid of the junction between the known free area and the unknown area of ​​the grid map as the boundary point; Step 4: Based on the boundary points obtained in step 3, the frontier point calculation module is used to obtain the candidate target points of each robot. The cost of the candidate target points is evaluated based on the frontier method, and the priority of each candidate target point is calculated. The market allocation mechanism is used to allocate the candidate target points with high priority as exploration points for each robot through the task allocation module. Step 5: Take the exploration point assigned to each robot in step 4 as the target point of the robot, introduce the improved A* algorithm fused with the TEB algorithm to perform collision-free path planning for the robot, and obtain the collision-free shortest path from the robot position to the corresponding target point; Step 6: During the mobile exploration process, each robot independently builds and updates its own local map through its own sub-mapping system; Step 7: Use the feature matching method to obtain the pose transformation matrix between the local maps corresponding to each robot obtained in step 6, and perform map fusion through the map fusion module until no new frontier points appear, and obtain the global map of the environment.

2. The boundary-based multi-robot collaborative exploration and mapping method according to claim 1, characterized in that: The method of using the frontier point calculation module to obtain the candidate target points of each robot in step 4 includes: the frontier in the frontier method is divided into two categories: A and B; when it is a category A frontier, the laser point directly in front of the robot is taken, the laser point is connected to the corresponding laser radar center, and the point on this connection line that is smaller than the maximum range and adjacent to the maximum range is selected as the candidate target point; When it is a Class B frontier, connect the laser radar to the connection point N of the wall, then take a point M from the line segment, so that the distance Bd between the connection points N and M is greater than 1 / 2 of the robot width, and then draw an arc with the laser radar as the center so that it passes through point M, and then draw a perpendicular line through point N to NM, and take a point Q on the perpendicular line. The distance between the connection points N and Q is greater than 1 / 2 of the robot width, connect the midpoint of the laser radar to point Q and extend it to the arc, and the intersection of this line segment and the arc is the candidate target point.

3. The boundary-based multi-robot collaborative exploration and mapping method according to claim 1, characterized in that: The cost value of the cost evaluation described in step 4 includes distance cost and information gain; The distance cost is calculated as follows: The expected distance for the robot to reach the candidate target point based on the frontier method is: The adjustment angle of the robot to reach the candidate target point based on the frontier method is: In the formula, i is the candidate target point of the current cost value to be calculated, D dis (i) represents the Euclidean distance between the point and the center of the robot, A angle (i) represents the angle between the straight line connecting the point and the center of the robot and the positive direction of the robot.

4. The boundary-based multi-robot collaborative exploration and mapping method according to claim 3, characterized in that: The information gain cost is calculated as follows: Information entropy is used to calculate the information gain of the grid cell. Assuming that the occupancy probability of the i-th grid cell is m i , the binary occupancy entropy calculation formula of this unit is as follows: H(i)=-m i log(m i )-(1-m i )log(1-m i ); Calculate the expected measured occupancy probability m of the perceived grid cell + The posterior entropy H + (i * ) and the expected measured unoccupied probability m - The posterior entropy H - (i * ); Perceived posterior expected entropy: E(H(i * ))=m + H + (i * )+m - H - (i * ); Then the expected information gain of the i-th grid cell after sensor perception is: I(i)=H(i)-E(H(i * ))。 5. The boundary-based multi-robot collaborative exploration and mapping method according to claim 4, characterized in that: The task allocation strategy using the market allocation mechanism includes the following: Set the main goals of task assignment as: Among them, A ij To allocate the result matrix, the elements in the matrix are A ij ∈(0,1), the value rule of this element is that when a robot is assigned to a boundary cell, A ij =1, otherwise A ij =0, B ij is the benefit matrix, and the elements in the matrix are defined as the benefits that can be obtained by sending the robot to explore the border cell; The calculation formula of the benefit matrix is: in, Represents all assigned boundary cells and boundary cell I j The positional relationship between them is calculated to reduce B ij The value of radius represents the radius of the circle with the candidate target point unit as the center.

6. The boundary-based multi-robot collaborative exploration and mapping method according to claim 1, characterized in that: In step 5, the introduction of the improved A* algorithm fused with the TEB algorithm to perform collision-free path planning for the robot includes the following contents: Dynamic weighted optimization of the A* algorithm; Potential field fusion and smoothing; Integrate the improved TEB algorithm to perform dynamic obstacle avoidance; Plan the shortest path from the robot to the corresponding target point.

7. The boundary-based multi-robot collaborative exploration and mapping method according to claim 1, characterized in that: The pose transformation matrix between the local maps corresponding to each robot obtained in step 7 includes the following: The AMCL algorithm is used for initial positioning between multiple robots; Align the local coordinate systems of multiple robots with the global coordinate system; Obtain the pose transformation matrix between the local maps corresponding to each robot.

8. The boundary-based multi-robot collaborative exploration and mapping method according to claim 1, characterized in that: In step 6, each robot uses the shortest path corresponding to step 5 to build and update its own local map through the map building module corresponding to each robot.

9. The boundary-based multi-robot collaborative exploration and mapping method according to claim 6, characterized in that: The map fusion described in step 7 includes the following: Through the map_merger node, traverse each map grid cell one by one, and process the value of each pixel according to the preset merging logic; Obtain multiple data maps after conversion to a unified coordinate system; Define a blank map; Any pixel [i, j] on the blank map is calculated and located to the corresponding pixel position in the local map of any robot according to the splicing parameters, and the final value of any pixel in the blank map is determined in combination with the merging rules, thus completing the fusion of the local maps of multiple robots.

Images