Multi-vehicle unknown environment collaborative mapping method based on topological characteristics

Abstract

The invention requests to protect a multi-vehicle unknown environment collaborative mapping method based on topological characteristics, and the method comprises the following steps: a plurality of vehicles establish local maps based on respective laser radars in different regions of an unknown environment, and share the local maps; the method comprises the following steps: performing preliminary fusion on a main vehicle through first-round feature matching, performing clustering analysis on boundary points in a preliminary fusion map to obtain to-be-explored points of the vehicle, and establishing an exploring point model containing topological features; the host vehicle establishes a to-be-explored point cost function, and allocates an appropriate exploration point for each vehicle; each vehicle updates a local map according to the allocated exploration task, and then shares the local map to the host vehicle; and finally, the host vehicle calculates pose transformation between the local maps according to the exploration point model, and local map fusion is carried out until no new exploration point appears, so that a global map is obtained. According to the method, the efficiency and the precision of multi-vehicle collaborative mapping in a large-range unknown environment can be effectively improved.

Description

Technical Field

[0001] The present invention relates to a multi-vehicle unknown environment collaborative mapping method based on topological features, belonging to the field of multi-vehicle autonomous exploration and mapping. Background Art

[0002] Existing LiDAR-based SLAM solutions have problems with map construction for large-scale scenes. If a single vehicle is used to build the map of the entire scene, it will take a long time, require a lot of calculations, and have low work efficiency. Moreover, the larger the scale of the scene, the greater the accumulated error, which will affect the accuracy of the final generated complete map.

[0003] Chinese Patent Application: A method, system, device and storage medium for autonomous collaborative exploration of an unknown environment by multiple agents (Application Number: 202310039534.4) discloses a method for autonomous collaborative exploration of an unknown environment by multiple agents. The method uses a fast random tree strategy to explore and obtain the first set of boundary points, which are assigned to each agent according to a cost function, and then autonomously and collaboratively explore the unknown environment. This method does not establish a topological feature model of the exploration points, resulting in low mapping efficiency.

[0004] Chinese patent application: A collaborative mapping method based on multi-robot autonomous exploration of improved frontier points and feature matching (application number 202311131996.5) discloses a collaborative mapping method based on multi-robot autonomous exploration of improved frontier points and feature matching, establishing local maps of the environment for each robot, and performing pose transformation between local maps, and then performing map fusion until no new boundary points appear. This method fails to establish the topological features of each exploration point during the fusion process, and performs map fusion based on the topological features of the exploration points.

[0005] Map construction plays an important role in the field of unmanned vehicles, but the above methods are likely to result in low efficiency and low accuracy in global map construction in large-scale scenarios, affecting actual use. Summary of the invention

[0006] The present invention aims to solve the above problems of the prior art. A method for collaborative mapping of a multi-vehicle unknown environment based on topological features is proposed. The technical solution of the present invention is as follows:

[0007] A method for collaborative mapping of a multi-vehicle unknown environment based on topological features, comprising the following steps:

[0008] 1.1. Multiple vehicles in different areas of an unknown environment build local maps based on their respective lidars and send the local maps to the main vehicle;

[0009] 1.2. The main vehicle receives the local maps of other vehicles, fuses them through feature matching, performs cluster analysis on the boundary points in the fused map to obtain the vehicle's points to be explored, and establishes an exploration point model with topological features; if there are no new points to be explored, the fused map is output as the global map;

[0010] 1.3. The main vehicle establishes the cost function of the target point, assigns appropriate exploration points to each vehicle, and sends them to each sub-vehicle;

[0011] 1.4. Each vehicle updates its local map based on the assigned exploration points and sends it to the main vehicle;

[0012] 1.5. Repeat 1.2 to 1.4 until no new exploration points appear.

[0013] Furthermore, the specific steps of step 1.2 to search for boundary points in the local map and determine the exploration target points are as follows:

[0014] 2.1. Extract the boundary points {Pi} in the map. For all pi, if the occupancy rate f(pi)>TH, then pi is an invalid boundary point. Otherwise, pi is added to the set of points to be explored S, where TH is a given threshold.

[0015] 2.2. For each point pi in the exploration point set S, establish its corresponding topological feature model Top(pi) = [g1, g2, ..., g7, g8] T , represents the distribution of other unexplored points that need to be explored around the exploration point pi;

[0016] Suppose there is an exploration point sequence P = {P1, P2, P3, ..., Pn}, where n is the number of exploration points. Take the i-th exploration point Pi as the origin to establish a polar coordinate system. Then, according to the polar angle of π / 4, the plane is divided into 8 sector intervals {R1, R2, R3, ..., R8}. The remaining exploration points fall into the corresponding intervals respectively. The statistical interval R i The sum of the polar diameters of the target and the origin g i As the eigenvalue of this interval, we finally get the 8-dimensional eigenvector topi=[g1,g2,...,g7,g8], which is the topological feature of the exploration point Pi.

[0017] Furthermore, the specific steps of establishing the cost function of the target point in step 1.3 are as follows:

[0018] 3.1. Establish the gravitational potential field function U according to the distance between the vehicle and the local target point att (x), construct the gravitational function F according to the gravitational potential field function att (X) is:

[0019]

[0020] Among them, ε is the gravitational gain coefficient, β is the gravitational compensation gain coefficient, X g is the current position of the vehicle, ρ g is the target point position, ρ(X,X g ) is the Euclidean distance between the current point and the exploration point, ρ a To explore the maximum influence range of point gravity;

[0021] 3.2. Establish the repulsive potential field function U according to the distance between vehicles req (x), the repulsive force function F is established according to the repulsive force potential field function req (d) is:

[0022]

[0023] Where d is the distance between the vehicle and the obstacle, η is the repulsive potential energy gain coefficient, ρ 0 is the critical distance of the obstacle repulsion field to the vehicle. When the distance between the vehicle and the obstacle is less than or equal to this critical distance, the repulsion function begins to take effect and the vehicle will be affected by the repulsion; when the distance between the vehicle and the obstacle is greater than this critical distance, the value of the repulsion function is 0, indicating that the vehicle is not affected by the repulsion.

[0024] 3.3. The cost function of the exploration point is established based on the combined force of the vehicle's gravity and repulsion as follows:

[0025] C(X,d)=k 1 F att (X)+k 2 F rep (d)#(3)

[0026] Among them, F att (X) is the gravitational function, F rep (d) is the repulsion function, X is the position of the target point, and d(X,Q) is the distance between the target point and vehicle Q;

[0027] In the vehicle exploration task allocation, if the vehicle resources are limited and the task is time-sensitive and the target points are independent of each other, then each vehicle is assigned an exploration point denoted as P 1 ; If the task requires multiple target coverage or the target points are related, multiple exploration points are assigned to each vehicle, denoted as P 1 , P 2 ,…,P n , where n≥2; when the vehicle fails or the cost of the target point is too high, it will not participate in the allocation;

[0028] Construct a matrix C, the elements in the matrix C ijIt is the cost function value of vehicle i corresponding to the exploration target point j; where i represents the number of the vehicle, i=1, 2, ..., m, m is the total number of vehicles, j represents the number of the exploration target point, j=1, 2, ..., n, n is the total number of vehicles and the total number of target points, which is calculated according to formula (3) using the attraction function and repulsion function mentioned above.

[0029] Furthermore, the specific steps of fusing the map in step 1.2 are as follows:

[0030] 4.1. For the global map G and the set of exploration points S, for P in s i , and P i The corresponding local map G i , extract G and G i The features of the overlapping areas between the target points are combined with the topological feature model of the target points to match the feature points.

[0031] According to the assigned exploration point P n During the exploration process of each vehicle, the local maps are obtained and updated in real time, the features of the overlapping areas between the local maps are extracted and combined with the topological feature model Top(pi) of the target point to obtain the pose transformation rotation matrix R and translation vector t between the two maps, and finally the optimal pose transformation matrix T is obtained. i It is expressed as:

[0032]

[0033] Among them, Δθ i,j is the relative rotation angle between the local map of the i-th vehicle and the local map of the j-th vehicle, Δx i,j and Δy i,j is the relative displacement;

[0034] 4.2. According to the posture transformation matrix T obtained in the previous step i Multiple local maps are rotated and translated for fusion, and then released to each vehicle for map update;

[0035] 4.3. Delete pi from S. If S is not empty, repeat 4.1-4,2, otherwise output G as the global map.

[0036] An electronic device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the method for autonomous collaborative exploration of an unknown environment of multiple vehicles as described in any one of the items is implemented.

[0037] A non-transitory computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the autonomous collaborative exploration method for an unknown environment of multiple vehicles as described in any one of the items. The advantages and beneficial effects of the present invention are as follows:

[0038] The present invention proposes a method for collaborative mapping of an unknown environment with multiple vehicles based on topological features. The method is divided into four steps: constructing a local map, establishing a target point model containing topological features, allocating exploration points, and fusing local maps. The present invention establishes an artificial potential field and determines the gravitational function and repulsive function of the corresponding target, thereby determining the cost function of the point to be explored and improving the rationality of the exploration point allocation of the entire system. Then, a topological feature model of the exploration point is established, which solves the limitations of map fusion based on local sub-graphs, accurately and quickly performs feature matching, improves the speed and accuracy of map fusion, and thus improves the efficiency of mapping and the positioning accuracy of vehicles. According to the above multi-vehicle collaborative mapping method, not only the problem of low efficiency of mapping of a single vehicle in a large-scale environment is solved, but also the problem of increasing the exploration coverage in an unknown environment can be increased, thereby improving the accuracy and efficiency of mapping in an unknown environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is an overall framework diagram of a preferred embodiment of the present invention, a method for collaborative mapping of a multi-vehicle unknown environment based on topological features.

[0040] Figure 2 This is an interval division diagram for the topological feature extraction method of the present invention.

[0041] Figure 3 This is a flow chart of the fusion map module of the present invention. DETAILED DESCRIPTION

[0042] The following will describe the technical solutions in the embodiments of the present invention in detail in conjunction with the accompanying drawings in the embodiments of the present invention. The described embodiments are only part of the embodiments of the present invention.

[0043] The technical solution of the present invention to solve the above technical problems is:

[0044] Figure 1 The figure shows the overall framework of a method for collaborative mapping of an unknown environment with multiple vehicles based on topological features of the present invention. The method includes:

[0045] Steps for building local maps: Each vehicle is dispersed in different areas of the unknown environment and works together to share local maps. Each vehicle scans the surrounding environment, builds a local grid map of the current environment, and sends the local grid map to the main vehicle. The main vehicle fuses the local maps and sends them to other vehicles.

[0046] Further, the host vehicle is represented by a two-dimensional grid map based on the preliminary fused local map, where m grid = 0 indicates a known free area, m grid =a indicates a known obstacle, m grid=-1 indicates unknown area, and its corresponding grid values ​​are as follows:

[0047]

[0048] Define the centroid of the white known free area grid at the junction of the known free area and the unknown area as the boundary point; continuously extract boundary points in the map to improve the efficiency of obtaining boundary points, extract boundary points {Pi} in the map, for all pi, if the occupancy rate f(pi)>TH, then pi is an invalid boundary point, otherwise pi is added to the set of points to be explored S, where TH is a given threshold;

[0049] like Figure 2 As shown, topological features are extracted for the exploration points, and an exploration point model Top(pi) containing topological features is established.

[0050] Top(pi)=[g1,g2,...,g7,g8] T , which represents the distribution of other unexplored points that need to be explored around the exploration point pi.

[0051] Furthermore, the exploration point sequence P = {P1, P2, P3, ..., Pn}, where n is the number of exploration points, takes the i-th exploration point Pi as the origin to establish a polar coordinate system, and then divides the plane into 8 sector intervals {R1, R2, R3, ..., R8} according to the polar angle size of π / 4, and the remaining exploration points fall into the corresponding intervals respectively, and the statistical interval R i The sum of the polar diameters of the target and the origin g i As the eigenvalue of this interval, we finally get the 8-dimensional eigenvector topi=[g1,g2,...,g7,g8], which is the topological feature of the exploration point Pi.

[0052] The steps to construct the cost function and allocate exploration points based on the artificial potential field are as follows:

[0053] The gravitational potential field function is selected according to the distance between the vehicle and the exploration point. The gravitational potential field function U att (x) is:

[0054]

[0055] According to the gravitational potential field function, the gravitational function F att (X) is:

[0056]

[0057] Among them, ε is the gravitational gain coefficient, β is the gravitational compensation gain coefficient, X g is the current position of the vehicle, ρ g is the target point position, ρ(X,X g) is the Euclidean distance between the current point and the exploration point, ρ a To explore the maximum influence range of point gravity.

[0058] Repulsive potential field function U req (d) is:

[0059]

[0060] According to the repulsive potential field function, the repulsive function F req (d) is:

[0061]

[0062] Among them, d is the distance between the vehicle and the obstacle, n is the repulsive potential energy gain coefficient, and p is the influence distance of the obstacle repulsive field on the vehicle.

[0063] The cost function of establishing the exploration point based on the combined force of the vehicle's gravity and repulsion is as follows:

[0064] C(X,d)=k 1 F att (X)+k 2 F rep (d)#(18)

[0065] Among them, F att (X) is the gravitational function, F rep (d) is the repulsion function, X is the position of the target point, and d(X,Q) is the distance between the target point and vehicle Q.

[0066] According to the cost function of each exploration point obtained, the market allocation mechanism is used to allocate exploration points to each vehicle, thereby realizing the rationalization of exploration point allocation for multiple vehicles.

[0067] When vehicle resources are limited, mission timeliness is high, and each target point is relatively independent, each vehicle is assigned an exploration point;

[0068] When the vehicle mission requires comprehensive coverage of multiple related target points or there are correlations between the target points that require the same vehicle to explore them in sequence, multiple exploration points are assigned to the vehicle;

[0069] When a vehicle fails to perform a task, or the cost function value of the current target point is extremely high for the vehicle (far beyond its tolerable range, for example, the distance is too far and the target value is very low), it will not participate in the allocation of exploration points.

[0070] Construct a matrix C, the elements in the matrix C ijIt is the cost function value of vehicle i corresponding to the exploration target point j. Where i represents the number of the vehicle (i = 1, 2, ..., m, m is the total number of vehicles), j represents the number of the exploration target point (j = 1, 2, ..., n, n is the total number of vehicles and the total number of target points), which is calculated according to formula (3) using the attraction function and repulsion function mentioned above.

[0071] For example, assuming there are 3 vehicles and 4 exploration targets, the cost matrix constructed is as follows:

[0072]

[0073] Steps for integrating the global map: During the process of autonomous exploration, each vehicle uses radar to obtain and update its own local map in real time, and share it with the main vehicle. The features of the overlapping areas between the local maps are extracted and combined with the topological feature model of the target point to match the feature points once, and then the pose transformation rotation matrix R and translation vector t between the two maps are obtained to obtain the optimal pose transformation matrix T i , expressed as:

[0074]

[0075] Among them, Δθ i,j is the relative rotation angle between the local map of the i-th vehicle and the local map of the j-th vehicle, Δx i,j and Δy i,j . is the relative displacement.

[0076] like Figure 3 As shown, according to the posture transformation matrix T obtained in the previous step i Multiple local maps are rotated and translated for fusion, and then released to each vehicle for map update. Delete pi from S. If S is not empty, repeat 4.1-4,2, otherwise output G as the global map.

[0077] The systems, devices, modules or units described in the above embodiments may be implemented by computer chips or entities, or by products with certain functions.

[0078] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0079] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0080] The above embodiments should be understood to be only used to illustrate the present invention and not to limit the protection scope of the present invention. After reading the contents of the present invention, technicians can make various changes or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. A method for collaborative mapping of a multi-vehicle unknown environment based on topological features, characterized in that it comprises the following steps: 1.

1. Multiple vehicles in different areas of an unknown environment build local maps based on their respective lidars and send the local maps to the main vehicle; 1.

2. The main vehicle receives the local maps of other vehicles, fuses them through feature matching, performs cluster analysis on the boundary points in the fused map to obtain the vehicle's points to be explored, and establishes an exploration point model with topological features; if there are no new points to be explored, the fused map is output as the global map; 1.

3. The main vehicle establishes the cost function of the target point, assigns appropriate exploration points to each vehicle, and sends them to each sub-vehicle; 1.

4. Each vehicle updates its local map based on the assigned exploration points and sends it to the main vehicle; 1.

5. Repeat 1.2 to 1.4 until no new exploration points appear.

2. The autonomous collaborative exploration method of multiple vehicles in an unknown environment according to claim 1, characterized in that: The specific steps of step 1.2 to search for boundary points in the local map and determine the exploration target points are as follows: 2.

1. Extract the boundary points {Pi} in the map. For all pi, if the occupancy rate f(pi)>TH, then pi is an invalid boundary point. Otherwise, pi is added to the set of points to be explored S, where TH is a given threshold. 2.

2. For each point pi in the exploration point set S, establish its corresponding topological feature model Top(pi) = [g1, g2, ..., g7, g8] T , represents the distribution of other unexplored points that need to be explored around the exploration point pi; Suppose there is an exploration point sequence P = {P1, P2, P3, ..., Pn}, where n is the number of exploration points. Take the i-th exploration point Pi as the origin to establish a polar coordinate system. Then, according to the polar angle of π / 4, the plane is divided into 8 sector intervals {R1, R2, R3, ..., R8}. The remaining exploration points fall into the corresponding intervals respectively. The statistical interval R i The sum of the polar diameters of the target and the origin g i As the eigenvalue of this interval, we finally get the 8-dimensional eigenvector topi=[g1,g2,...,g7,g8], which is the topological feature of the exploration point Pi.

3. The autonomous collaborative exploration method of multiple vehicles in an unknown environment according to claim 1, characterized in that: The specific steps of the method for establishing the cost function of the target point in step 1.3 are as follows: 3.

1. Establish the gravitational potential field function U according to the distance between the vehicle and the local target point att (x), construct the gravitational function F according to the gravitational potential field function att (X) is: Among them, ε is the gravitational gain coefficient, β is the gravitational compensation gain coefficient, X g is the current position of the vehicle, ρ g is the target point position, ρ(X,X g ) is the Euclidean distance between the current point and the exploration point, ρ a To explore the maximum influence range of point gravity; 3.

2. Establish the repulsive potential field function U according to the distance between vehicles req (x), the repulsive force function F is established according to the repulsive force potential field function req (d) is: Where d is the distance between the vehicle and the obstacle, η is the repulsive potential energy gain coefficient, and ρ0 is the distance of influence of the obstacle repulsive field on the vehicle; 3.

3. The cost function of the exploration point is established based on the combined force of the vehicle's gravity and repulsion as follows: C(X,d)=k1F att (X)+k2F rep (d)#(3) Among them, F att (X) is the gravitational function, F rep (d) is the repulsion function, X is the position of the target point, and d(X,Q) is the distance between the target point and vehicle Q; In the vehicle exploration task allocation, if the vehicle resources are limited and the task is time-sensitive and the target points are independent of each other, one exploration point is assigned to each vehicle, denoted as P1; if the task requires multiple target coverage or the target points are related, multiple exploration points are assigned to each vehicle, denoted as P1, P2, ..., P n , where n≥2; when the vehicle fails or the cost of the target point is too high, it will not participate in the allocation; Construct a matrix C, the elements in the matrix C ij It is the cost function value of vehicle i corresponding to the exploration target point j; where i represents the number of the vehicle, i=1, 2, ..., m, m is the total number of vehicles, j represents the number of the exploration target point, j=1, 2, ..., n, n is the total number of vehicles and the total number of target points, which is calculated according to formula (3) using the attraction function and repulsion function mentioned above.

4. The autonomous collaborative exploration method of multiple vehicles in an unknown environment according to claim 1, characterized in that: The specific steps of fusing the map in step 1.2 are as follows: 4.

1. For the global map G and the set of exploration points S, for P in s i , and P i The corresponding local map G i , extract G and G i The features of the overlapping areas between the target points are combined with the topological feature model of the target points to match the feature points. According to the assigned exploration point P n During the exploration process of each vehicle, the local maps are obtained and updated in real time, the features of the overlapping areas between the local maps are extracted and combined with the topological feature model Top(pi) of the target point to obtain the pose transformation rotation matrix R and translation vector t between the two maps, and finally the optimal pose transformation matrix T is obtained. i It is expressed as: Among them, Δθ i,j is the relative rotation angle between the local map of the i-th vehicle and the local map of the j-th vehicle, Δx i,j and Δy i,j is the relative displacement; 4.

2. According to the posture transformation matrix T obtained in the previous step i Multiple local maps are rotated and translated for fusion, and then released to each vehicle for map update; 4.

3. Delete pi from S. If S is not empty, repeat 4.1-4,2, otherwise output G as the global map.

5. An electronic device, characterized in that: The method comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the method for autonomous collaborative exploration of an unknown environment of multiple vehicles as claimed in any one of claims 1 to 4 is implemented.

6. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the autonomous collaborative exploration method for an unknown environment of multiple vehicles as described in any one of claims 1 to 4 is implemented.

Images