A collaborative mapping method based on improved frontier point autonomous exploration by multiple robots and feature matching.

Abstract

This invention discloses a multi-robot autonomous exploration method based on improved frontier points and a collaborative mapping method based on feature matching. The method includes: establishing local environmental maps for each robot based on radar data; searching for frontier points in the local maps and performing cluster analysis to obtain target points that the robots need to explore; establishing a reasonable exploration point cost function, including path distance cost, maximum achievable benefit, and penalty function, and assigning appropriate exploration points to each robot; introducing a conflict-based multi-level planning algorithm to find an optimal path for each robot; during the exploration process, each robot independently establishes its own local map; finally, a feature matching method is used to obtain the pose transformations between the local maps, and rapid map fusion is performed until no new boundary points appear, thus obtaining a global map of the environment. This invention addresses the challenge of enabling multiple robots to autonomously search unknown areas in large, unknown environments while simultaneously building a complete environmental map.

Description

Technical Field

[0001] This invention relates to a method for multi-robot autonomous exploration based on improved frontier points and collaborative mapping based on feature matching, belonging to the field of multi-robot autonomous exploration and mapping. Background Technology

[0002] With the rapid development of technology, the application environment of robots is becoming increasingly complex. To improve work efficiency and robustness, the development of multi-robot systems has become a research hotspot. Multi-robot systems are more suitable for complex and changing environments due to their higher execution and collaborative capabilities. In potentially hazardous environments, mobile robots need to be able to autonomously perceive the environment and build maps without human intervention. This process requires an effective autonomous exploration strategy and a stable and reliable map-building method.

[0003] Autonomous exploration and collaborative mapping among multiple robots are fundamental problems in the field of multi-mobile robots. The most typical method for autonomous exploration is based on the exploration of leading edges (i.e., boundary points). Collaborative mapping hinges on map fusion; however, existing algorithms are inefficient at acquiring boundary points, and in multi-robot systems, a suitable allocation strategy is required, resulting in low map fusion accuracy. Introducing a random sampling-based search algorithm into the leading edge exploration algorithm can significantly improve the efficiency of boundary point search. Furthermore, using feature matching on the local maps of different robots can improve the accuracy of the fused global map, thus completing the creation of the global map.

[0004] In complex and unknown environments, multi-robot autonomous exploration suffers from low efficiency, insufficient completeness, and low accuracy in collaborative mapping. Therefore, a method is needed to quickly search for target points to be explored in a local environment map, while simultaneously considering the rational allocation of exploration points among the robots and building a global environment map, in order to improve the efficiency of autonomous exploration and the accuracy of the global map. Summary of the Invention

[0005] The technical problem to be solved by this invention is to find the boundary points between the known and unknown areas of the local environment map as exploration target points when the global environment is unknown, establish the cost function of the exploration points, reasonably allocate exploration points to each robot and perform path planning, and perform map fusion to finally obtain a global map with good exploration completeness and high accuracy.

[0006] To address the aforementioned technical problems, this invention provides a collaborative mapping method based on improved multi-robot autonomous exploration at the frontier and feature matching. This method includes the following steps:

[0007] S1: Using the local environmental grid map established by radar as input, define the centroid of the boundary between the known free area and the unknown area of ​​the grid map as the boundary point, and perform clustering and analysis on it;

[0008] S2: Based on the boundary points obtained in S1, establish the exploration point cost function, including path distance cost, maximum obtainable benefit and penalty function, to obtain the cost of the exploration target point. Combine the cost function with the market method allocation mechanism to allocate appropriate exploration points to each robot.

[0009] S3: Using the exploration points allocated in S2 as the robot's target points, a collision-based multi-level search algorithm is introduced to perform collision-free path planning for the robot, obtaining the shortest collision-free path from the robot's position to the corresponding exploration point.

[0010] S4: During the mobile exploration process, each robot independently establishes and updates its own local map, and uses the feature matching method to obtain the pose transformation matrix between local maps, and performs map fusion until no new frontier points appear, and finally obtains the global map of the environment.

[0011] Preferably, step S1 specifically includes the following process:

[0012] S1.1 represents the local environment model of each robot acquired by radar as a two-dimensional grid map, where white represents known free areas, black represents known obstacles, and gray represents unknown areas. The corresponding grid values ​​are as follows:

[0013]

[0014] Define the centroid of the white known free area grid at the boundary between the white known free area and the gray unknown area as the boundary point;

[0015] S1.2 introduces a random sampling-based RRT algorithm to continuously extract boundary points from the map, thereby improving the efficiency of boundary point acquisition. A mean-shift clustering algorithm is applied to the grid map boundary points to detect invalid boundary points, i.e., any boundary points with an occupancy rate greater than a certain threshold, as well as old boundary points. The filtered set of boundary points is then used as the target points for the robot to explore.

[0016] Preferably, step S2 specifically includes the following process:

[0017] S2.1 Based on the exploration points in the boundary region of S1, and combined with the path distance cost, the path cost f caused by the distance between the robot's position coordinates and the exploration target point's position coordinates is calculated. D (i, j) can be represented by Euclidean distance as:

[0018] f D (i, j) = ||p robot_i -ptarget_j ||

[0019] Where p robot_i p represents the position coordinates of the i-th robot. target_j This represents the coordinates of the j-th exploration target point;

[0020] S2.2 Based on the exploration points in the boundary region of S1, consider the maximum benefit obtainable from exploring the target point, and the benefit function f B (i) The number of grid cells N(p) that the robot can cover in the unknown area when it reaches the target point of exploration. target_j ) represents, where N(p) target_j ) indicates that the target point p is being explored. target_j Centered on the circle, the LiDAR scanning radius is the number of grid cells in the unknown area within the benefit circle formed by the benefit radius. A larger benefit value indicates a greater attraction of the target point to the robot. The benefit function is defined as:

[0021] f B (i)=N(p target_j )

[0022] S2.3 Based on the exploration points in the boundary region of S1, to avoid repeatedly assigning the same exploration target point to multiple robots, a penalty function f is introduced. P The exploration point (i, j) is re-evaluated, and its penalty function is defined according to the following function definition:

[0023]

[0024] Where, p target_j and p target_k S represents the coordinates of the target point in the overlapping region of the two benefit circles. overlap The area of ​​the overlapping region is used to represent the exploration point p. target_k After being assigned to other robots, the i-th robot explores point p. target_j The benefit decreases when the two benefit circles completely overlap; in this case, the benefit value and penalty value cancel each other out, and the target point has no utility value. f The effective diameter is twice the scanning radius of the lidar;

[0025] S2.4 A cost function for each exploration point is established by comprehensively considering the distance to the target point, the information benefit of the target point, and the penalty function. The total cost function f between the i-th robot and the exploration point with index j is also considered. i，j Represented as:

[0026] f i，j =λ B f B (i)-λ D f D(i,j)-λ P f P (i, j)

[0027] Where, λ B ,λ D , λ P These are the weighting coefficients for each cost function.

[0028] S2.5 Based on the cost function between each exploration target point and the robot, the market allocation mechanism is used to allocate target points to each robot, thereby achieving rationalization of multi-robot exploration point allocation.

[0029] Preferably, step S3 specifically includes the following process:

[0030] S3.1 Based on the target point assigned to each robot in S2, the improved CBS (conflict-based search) algorithm is used for path planning. The bidirectional A* algorithm is used at a low level to accelerate the path search efficiency and obtain a shortest path from each robot's position to its corresponding target point.

[0031] S3.2 At a higher level, path conflict detection is performed among the robots. In cases of conflict, the branch with the lowest planning cost is selected, and the lower-level path search is repeated until a valid path is found during the higher-level search process. Based on the search routes of each robot to its corresponding exploration target point, autonomous exploration by the robots is achieved.

[0032] Preferably, step S4 specifically includes the following process:

[0033] In S4.1, during the autonomous exploration process based on S3, each robot uses radar to acquire and update its local map in real time and performs downsampling. The ORB algorithm is used to extract features from the overlapping areas between the local maps, and KNN is used for initial feature point matching. Then, ICP is used to obtain the pose transformation rotation matrix R and translation vector t between the two maps. RANSAC is used to remove mismatched map features to obtain the optimal pose transformation matrix T. i，j , is represented as:

[0034]

[0035] Where, Δθ i，j Δx is the relative rotation angle between the i-th robot local map and the j-th robot local map. i，j and Δy i，j It is a relative displacement;

[0036] S4.2 merges multiple local maps based on the pose transformation matrix of the local maps and publishes them to each robot for map updates until no new frontier points appear, and finally establishes a complete global map.

[0037] Compared with the prior art, the advantages of the present invention are as follows:

[0038] First, the multi-robot autonomous exploration method based on improved frontier points proposed in this invention combines a random sampling search algorithm to search for and select exploration target points, thereby accelerating the real-time performance of the algorithm, establishing a cost function for exploration target points, and providing a reasonable multi-robot task allocation mechanism to effectively reduce redundant exploration. The generated exploration paths are short and have high coverage, thus improving exploration efficiency.

[0039] Secondly, for the multi-robot path planning problem, a multi-layer planning algorithm is used. The bottom layer performs conventional path planning for a single robot, while the top layer performs collision detection on the path. This approach finds the optimal path while improving path safety.

[0040] Finally, the ORB algorithm is used to extract feature points from the local maps of each robot, and the KNN algorithm for coarse alignment and the ICP algorithm for fine alignment are combined to obtain the pose transformation between maps, thereby achieving map fusion, shortening the time to build the global map, and improving the accuracy of the generated global map. Attached Figure Description

[0041] Figure 1 This is the overall system block diagram of the present invention;

[0042] Figure 2 This is a schematic diagram of the leading edge of the present invention;

[0043] Figure 3 This is a flowchart of the multi-robot collaborative mapping algorithm based on feature matching of the present invention. Detailed Implementation

[0044] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.

[0045] Example 1: As Figure 1 As shown, a collaborative mapping method based on improved frontier point autonomous exploration by multiple robots and feature matching is proposed. This method includes the following steps:

[0046] Step S1: Using the local environmental grid map established by radar as input, define the centroids at the boundaries between the known free areas and unknown areas of the grid map as boundary points, and perform clustering and analysis on them;

[0047] S1.1 represents the local environment model of each robot acquired by radar as a two-dimensional grid map, where white represents known free areas, black represents known obstacles, and gray represents unknown areas. The corresponding grid values ​​are as follows:

[0048]

[0049] Define the centroid of the white known free area grid at the boundary between the white known free area and the gray unknown area as the boundary point;

[0050] S1.2 introduces a random sampling-based RRT algorithm to continuously extract boundary points from the map, thereby improving the efficiency of boundary point acquisition. A mean-shift clustering algorithm is applied to the grid map boundary points to detect invalid boundary points, i.e., any boundary points with an occupancy rate greater than a certain threshold, as well as old boundary points. The filtered set of boundary points is then used as the target points for the robot to explore.

[0051] Step S2: Based on the boundary points obtained in S1, establish the exploration point cost function, including path distance cost, maximum obtainable benefit and penalty function, to obtain the cost of the exploration target point. Combine the cost function with the market method allocation mechanism to allocate appropriate exploration points to each robot.

[0052] S2.1 Based on the exploration points in the boundary region of S1, and combined with the path distance cost, the path cost f caused by the distance between the robot's position coordinates and the exploration target point's position coordinates is calculated. D (i, j) can be represented by Euclidean distance as:

[0053] f D (i, j) = ||p robot_i -p target_j ||

[0054] Where p robot_i p represents the position coordinates of the i-th robot. target_j This represents the coordinates of the j-th exploration target point;

[0055] S2.2 Based on the exploration points in the boundary region of S1, consider the maximum benefit obtainable from exploring the target point, and the benefit function f B (i) The number of grid cells N(p) that the robot can cover in the unknown area when it reaches the target point of exploration. target_j ) represents, where N(p) target_j ) indicates that the target point p is being explored. target_j Centered on the circle, the LiDAR scanning radius is the number of grid cells in the unknown area within the benefit circle formed by the benefit radius. A larger benefit value indicates a greater attraction of the target point to the robot. The benefit function is defined as:

[0056] f B (i)=N(ptarget_j )

[0057] S2.3 Based on the exploration points in the boundary region of S1, to avoid repeatedly assigning the same exploration target point to multiple robots, a penalty function f is introduced. P The exploration point (i, j) is re-evaluated, and its penalty function is defined according to the following function definition:

[0058]

[0059] Where, p target_j and p target_k S represents the coordinates of the target point in the overlapping region of the two benefit circles. overlap The area of ​​the overlapping region is used to represent the exploration point p. target_k After being assigned to other robots, the i-th robot explores point p. target_j The benefit decreases when the two benefit circles completely overlap; in this case, the benefit value and penalty value cancel each other out, and the target point has no utility value. f The effective diameter is twice the scanning radius of the lidar;

[0060] S2.4 A cost function for each exploration point is established by comprehensively considering the distance to the target point, the information benefit of the target point, and the penalty function. The total cost function f between the i-th robot and the exploration point with index j is also considered. i，j Represented as:

[0061] f i，j =λ B f B (i)-λ D f D (i,j)-λ P f P (i, j)

[0062] Where, λ B ,λ D ,λ p These are the weighting coefficients for each cost function.

[0063] S2.5 Based on the cost function between each exploration target point and the robot, the market allocation mechanism is used to allocate target points to each robot, thereby achieving rationalization of multi-robot exploration point allocation.

[0064] Step S3: Use the exploration points allocated in S2 as the robot's target points, and introduce a conflict-based multi-level search algorithm to perform collision-free path planning for the robot, and obtain the shortest collision-free path from the robot's position to the corresponding exploration point.

[0065] S3.1 Based on the target point assigned to each robot in S2, the improved CBS (conflict-based search) algorithm is used for path planning. The bidirectional A* algorithm is used at a low level to accelerate the path search efficiency and obtain a shortest path from each robot's position to its corresponding target point.

[0066] S3.2 At a higher level, path conflict detection is performed among the robots. In cases of conflict, the branch with the lowest planning cost is selected, and the lower-level path search is repeated until a valid path is found during the higher-level search process. Based on the search routes of each robot to its corresponding exploration target point, autonomous exploration by the robots is achieved.

[0067] Step S4: During the mobile exploration process, each robot independently builds and updates its own local map, and uses the feature matching method to obtain the pose transformation matrix between local maps. Real map fusion is then performed until no new frontier points appear, and finally a global map of the environment is obtained.

[0068] In S4.1, during the autonomous exploration process based on S3, each robot uses radar to acquire and update its local map in real time and performs downsampling. The ORB algorithm is used to extract features from the overlapping areas between the local maps, and KNN is used for initial feature point matching. Then, ICP is used to obtain the pose transformation rotation matrix R and translation vector t between the two maps. RANSAC is used to remove mismatched map features to obtain the optimal pose transformation matrix T. i,j , is represented as:

[0069]

[0070] Where, Δθ i,j Δx is the relative rotation angle between the i-th robot local map and the j-th robot local map. i,j and Δy i,j It is a relative displacement;

[0071] S4.2 merges multiple local maps based on the pose transformation matrix of the local maps and publishes them to each robot for map updates until no new frontier points appear, and finally establishes a complete global map.

[0072] It should be noted that the above embodiments are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Equivalent substitutions or alternatives made based on the above technical solutions shall all fall within the scope of protection of the present invention.

Claims

1. A method for multi-robot autonomous exploration based on improved frontier points and collaborative mapping based on feature matching, characterized in that, Includes the following steps: S1: Using the local environmental grid map established by radar as input, define the centroid of the boundary between the known free area and the unknown area of ​​the grid map as the boundary point, and perform clustering and analysis on it; S2: Based on the boundary points obtained in S1, establish the exploration point cost function, including path distance cost, maximum obtainable benefit and penalty function, to obtain the cost of the exploration point. Combine the cost function with the market method allocation mechanism to allocate appropriate exploration points to each robot. S3: Using the exploration points allocated in S2 as the robot's target points, a collision-based multi-level search algorithm is introduced to perform collision-free path planning for the robot, obtaining the shortest collision-free path from the robot's position to the corresponding exploration point. S4: During the mobile exploration process, each robot independently builds and updates its own local map, and uses feature matching to obtain the pose transformation matrix between the local maps. The maps are then fused until no new leading edge points appear, and finally a global map of the environment is obtained. The method for clustering and analyzing the boundary points of the local map in step S1 is as follows: S1.1 represents the local environment model of each robot acquired by radar as a two-dimensional grid map, where white represents known free areas, black represents known obstacles, and gray represents unknown areas. The corresponding grid values ​​are as follows: Define the centroid of the white known free area grid at the boundary between the white known free area and the gray unknown area as the boundary point; S1.2 introduces the RRT algorithm based on random sampling to continuously extract boundary points in the map, thereby improving the efficiency of boundary point acquisition. The mean drift clustering algorithm is applied to the boundary points of the grid map to detect invalid boundary points, that is, any boundary points with an occupancy rate greater than a certain threshold, as well as old boundary points. The filtered boundary point set is used as the exploration points that the robot will explore. The cost function for the exploration point in step S2 is established as follows: S2.1 Based on the exploration points in the boundary region in S1, and combined with the path distance cost, the path cost caused by the distance between the robot's position coordinates and the exploration point's position coordinates is calculated. Expressed using Euclidean distance: in This represents the position coordinates of the i-th robot. This represents the coordinates of the j-th exploration point; S2.2 Based on the exploration points in the boundary region of S1, consider the maximum benefit obtainable from the exploration points, and the benefit function. The number of grid cells representing the unknown area that the robot can cover upon reaching the exploration point. It means that among them Indicates exploration point Centered on a circle, the LiDAR scanning radius is the number of grid cells in the unknown region within the benefit circle formed by the benefit radius. A larger utility value indicates a greater attraction of the exploration point to the robot. The benefit function is defined as: S2.3 Based on the exploration points in the boundary region of S1, to avoid repeatedly assigning the same exploration point to multiple robots, a penalty function is introduced. The exploration point is re-evaluated, and its penalty function is defined according to the following function definition: in, and The coordinates of the exploration point where the two benefit circles overlap are given. The area of ​​the overlapping region is used to represent the exploration point. After being assigned to other robots, the exploration point of the i-th robot... The benefit value decreases when the two benefit circles completely overlap; the benefit value and penalty value will cancel each other out, and at this point, the exploration point has no utility value. The effective diameter is twice the scanning radius of the lidar; S2.4 A cost function for each exploration point is established by considering the distance between exploration points, the information benefits of exploration points, and the penalty function. This is the total cost function between the i-th robot and the exploration point with index j. Represented as: in, , , These are the weight coefficients for each cost function. S2.5 Based on the cost function of each exploration point and the robot, the market allocation mechanism is used to allocate exploration points to each robot, thereby achieving rationalization of the allocation of exploration points for multiple robots; Step S3 specifically includes the following: S3.1 Based on the target point assigned to each robot in S2, the improved CBS (conflict-based search) algorithm is used for path planning. The bidirectional A* algorithm is used at a low level to accelerate the path search efficiency and obtain a shortest path from each robot's position to its corresponding target point. S3.2 The high-level system performs path conflict detection among the robots. In the case of conflict, it selects the branch with the lowest planning cost and re-performs the low-level path search until the high-level search process finds an effective path. Based on the search routes of each robot to the corresponding exploration point, the robot's autonomous exploration is realized. Step S4 specifically includes the following: In S4.1, during the autonomous exploration process based on S3, each robot uses radar to acquire and update its local map in real time and performs downsampling. The ORB algorithm is used to extract features from the overlapping areas between the local maps, and KNN is used for initial feature point matching. Then, ICP is used to obtain the pose transformation rotation matrix R and translation vector t between the two maps. RANSAC is used to remove mismatched map features to obtain the optimal pose transformation matrix. , is represented as: in, It is the relative rotation angle between the local map of the i-th robot and the local map of the j-th robot. and It is a relative displacement; S4.2 merges multiple local maps based on the pose transformation matrix of the local maps and publishes them to each robot for map updates until no new frontier points appear, and finally establishes a complete global map.

Images