Unmanned vehicle exploration path planning method based on multilayer fusion map

Abstract

The invention discloses an unmanned vehicle exploration path planning method based on a multilayer fusion map. The method comprises the following steps: S1, constructing a multi-layer fusion map: constructing a multi-layer map through segmentation and layering of point cloud data, and superposing the multi-layer map to form the multi-layer fusion map; s2, generalized Voronoi diagram extraction: identifying a false free space in the fused map, incorporating the false free space into an obstacle, and constructing a generalized Voronoi diagram by calculating the distance between a point and the obstacle; and S3, path planning based on the Voronoi diagram: performing path planning based on the generalized Voronoi diagram by adopting a fast random exploration tree algorithm. According to the invention, efficient and autonomous exploration of the UGV in an unknown three-dimensional space can be realized. Through accurate map construction and intelligent path planning, the UGV can perform efficient navigation and exploration in an unknown environment, and meanwhile, the exploration area and efficiency are maximized. The method has an important practical value for rescue, exploration, monitoring and other applications in a complex environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of map fusion and path planning, and in particular relates to an unmanned vehicle exploration path planning method based on multi-layer fusion maps. Background Art

[0002] Autonomous exploration of unmanned vehicles (UGVs) in space is a prominent topic at present. Traditional solutions for autonomous exploration of UGVs focus on analyzing the traversability of terrain. However, these solutions ignore the impact of the complex body structure of UGVs on the exploration task. This omission poses a challenge to planning collision-free paths for UGVs in scenes with complex structures.

[0003] The Voronoi diagram is composed of a set of continuous polygons formed by the perpendicular bisectors of the lines connecting two adjacent points. It has the following characteristics:

[0004] (1) The Voronoi diagram divides the plane into n polygonal domains, each of which has only one generator;

[0005] (2) The distance from each point in the polygon to this generator is shorter than the distance to other generators;

[0006] (3) The distances from the points on the polygon boundary to the two generators that generate this boundary are equal;

[0007] (4) The Voronoi polygon boundaries of the adjacent figures are taken as subsets of the original adjacent boundaries.

[0008] (5) The Voronoi diagram has at most 2*n-5 vertices and 3*n-6 edges.

[0009] (6) The generators inside a polygon are the centers of the outer circles of the triangles formed by the three points that form the three sides, and all these outer circles do not contain any vertices other than these three points (hollow circle property).

[0010] At present, there is still a lack of a method that can effectively enable unstructured unmanned vehicles to complete exploration tasks in complex structural scenarios based on the complex body structure. Summary of the invention

[0011] In response to the above technical problems, the present invention provides an unmanned vehicle exploration planning method based on multi-layer fusion maps.

[0012] The method provided by the present invention relies on multi-layer fusion maps to evaluate the traversability of unmanned vehicles in space. In addition, the present invention introduces a topology-based method to significantly reduce the computational costs associated with spatial exploration problems. The method described in the present invention can effectively enable unstructured unmanned vehicles to complete exploration tasks in complex scenarios.

[0013] The technical solution provided by the present invention is as follows:

[0014] In a first aspect, the present invention provides an unmanned vehicle exploration planning method based on a multi-layer fusion map, comprising the following steps:

[0015] S1, multi-layer fusion map construction: construct multi-layer maps by segmenting and layering point cloud data, and superimpose multi-layer maps to form a fusion map;

[0016] S2, generalized Voronoi diagram extraction: identify the false free spaces in the fused map, incorporate them into obstacles, and then construct a generalized Voronoi diagram by calculating the distance between points and obstacles;

[0017] S3, planning based on Voronoi diagram: a fast random exploration tree algorithm is used to perform path planning based on the generalized Voronoi diagram.

[0018] Further, the S1 comprises the following sub-steps:

[0019] S11, terrain analysis:

[0020] The map construction technology based on sparse 3D LiDAR is adopted, and the terrain accessibility is calculated using Bayesian generalized kernel reasoning. At the same time, Kalman filtering is applied to update the grid elevation value to reduce the impact of moving objects on map construction.

[0021] S12, multi-layer fusion map generation:

[0022] The point cloud observed at time t is defined as a set C, where each point p has horizontal and vertical coordinates and a height coordinate z;

[0023] Define a high-level segmentation function f(z) to filter the point cloud data and divide the set C into three subsets C based on the z coordinate. L , satisfying the conditions:

[0024] C L ={p∈C|-Δz≤z≤Δz}

[0025] C M ={p∈C|z mid -Δz≤z≤z mid +Δz}

[0026] C H ={p∈C|z high -Δz≤z≤z high +Δz}

[0027] Δz represents a given height floating parameter, which depends on the setting of the lidar; z mid and z high It is a predetermined height benchmark, which is adjusted according to the morphological characteristics of the unmanned vehicle;

[0028] For each subset C v , v = L, M, H, define a probability grid map function g v (C v ), g v (C v ) By determining the grid cell to which the point cloud belongs and updating the occupancy probability, C v The point cloud in is mapped to a two-dimensional grid map M v , the following conditions are met:

[0029] M v is a matrix composed of n×m grid cells; each grid cell m ij (i=1,...,n;j=i,...,m) represents an area, whose size depends on the resolution of the grid map;

[0030] Each grid cell m ij Has an occupancy probability p(m ij ), indicating the possibility that the area is occupied by the point cloud; p(m ij ) ranges from 0 to 1, where 0 indicates idle, 1 indicates occupied, and 0.5 indicates unknown occupancy.

[0031] Further, the S2 comprises the following sub-steps:

[0032] S21, eliminate the false space area caused by single-layer map:

[0033] Use set C fakefree represents pseudo free space, C free represents free space, C′ represents the obstacle point set; x and y are two points in space, x x represents the x-coordinate of point x, x y represents the y coordinate of point x, y x represents the x coordinate of point y, y y represents the y coordinate of point y; R 3 Represents the set of all points in space;

[0034]

[0035] Among them, C i , C j and C k A set of points representing three obstacles;

[0036] The pseudo free space is an area that the unmanned vehicle cannot pass through, which is divided into obstacles extracted by the generalized Voronoi diagram;

[0037] Using C′ i ,C′ j ,C′ kDefine the set of points that eliminate obstacles for false free space:

[0038]

[0039] Among them, C′ i To eliminate the obstacle C of the pseudo free space i , C′ j To eliminate the obstacle C of the pseudo free space j , C k To eliminate the obstacle C of the pseudo free space k ;

[0040] False free space was eliminated by redefining obstacles;

[0041] S22, extraction of generalized Voronoi graph GVG:

[0042] Build a GVG that ensures that the driverless car does not collide, using F 2 To represent the set of projections of equidistant surfaces onto the bottom map, use F 3 to represent the set formed by the projection of the intersection lines of equidistant surfaces on the bottom map;

[0043] F ij = {x∈R 3 |d(x,C′ i )=d(x,C′ j )}

[0044]

[0045] Among them, F ij is the point set C′ i and C′ j The edges of GVG are equidistant, d(x,C′ i ) represents the point x and the point set C′ i The distance, d(x,C′ j ) represents the point x and the point set C′ j , d(x,C′) represents the distance between point x and obstacle point set C′, The shortest distance between point x in the space and point y in the obstacle point set C′;

[0046] When there are n obstacles, the following formula is used to define free space, pseudo free space, and F 2 and F 3 ; Set F 2 is the edge of GVG, set F 3 are the nodes that make up GVG, and the union of these two sets is GVG;

[0047]

[0048] Among them, C 1 to C n represents n obstacles, x and y represent points in space; i = 1, ..., n-1, j = i+1, ..., n, k = j+1, ..., n; z bottom Represents the height reference of the preset underlying map, E edge is the edge of GVG, V is the point of GVG;

[0049] S23, adaptive adjustment based on the shape of the unmanned vehicle: expand the map outline according to the geometric shape of the unmanned vehicle and adjust the generalized Voronoi diagram.

[0050] Furthermore, in step S23, a circle with a maximum radius and equal diameter of a geometric shape corresponding to the height of the unmanned vehicle is used to process the image;

[0051]

[0052] Among them, Map represents the map outline; B(R) means B(R) a , is a circle with point a in the two-dimensional plane as the center and the maximum radius R of the unmanned vehicle geometry as the radius; symbol Represents the expansion operation; E 2 Represents a two-dimensional plane; Is an empty set.

[0053] Further, the S3 includes the following sub-steps:

[0054] S31, initialize graph G, G = (V, E edge ), where V is the vertex of the graph, E edge is the edge of the graph;

[0055] S32, randomly sample a point q from the obstacle point set in graph G rand , find the point q closest to this point near , and calculate q rand and q near The Euclidean distance between

[0056] S33, determining whether there is a collision along the connection path between the two points;

[0057] S34, the collision-free point q rand Add to the vertex set V, and add q near and q rand The connecting line (q near ,q rand ) is added to the edge set E and q is calculated rand The set of nearby points Q near ; Carry out iterative updates;

[0058] S35, for Q near For each point q near in near , if the path connecting q rand to q edge is collision-free, update the edge set E rand ; remove the edge from the parent node of q rand to q near , add the new edge (q rand , q edge ) to E near ; check and update the parent node of q

[0059] S36, after the maximum number of iterations K, return the constructed graph G, and select the optimal path according to the graph G.

[0060] Furthermore, in the step S32, find the point q near nearest to this point, and the formula for calculating the Euclidean distance between q rand and q near is as follows:

[0061]

[0062] where O represents the set of points that meet the conditions, point x is a random point in the point set C i , E 2 represents the two-dimensional plane, n represents the total number of obstacle sets, i = 1,..., n represents different obstacle set numbers; d(x, C i ) < R means that the distance between point x and the obstacle set C i is less than the inflation radius R; d represents the distance function; is the distance function, which calculates the Euclidean distance from point x to the nearest point in the obstacle set C i , and g represents any point in the obstacle set.

[0063] Furthermore, in the step S33, the formula for collision judgment is as follows:

[0064]

[0065] where the CollisionFree function represents checking whether a collision occurs; x i , x j are the coordinates of the two ends of the path to be detected for collision respectively, and x i x j is the path itself with the two points as endpoints; when the function value is 1, the path has no collision, and when the value is 0, the path has a collision.

[0066] In a second aspect, the present invention provides an electronic device comprising 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 unmanned vehicle exploration and planning method based on a multi-layer fusion map as described in the first aspect is implemented.

[0067] In a third aspect, the present invention provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the unmanned vehicle exploration and planning method based on a multi-layer fusion map as described in the first aspect.

[0068] In a fourth aspect, the present invention provides a computer program product, including a computer program, which, when executed by a processor, implements the unmanned vehicle exploration and planning method based on a multi-layer fusion map as described in the first aspect.

[0069] The beneficial effects of the present invention are as follows:

[0070] The method provided by the present invention uses a multi-layer fusion map to more comprehensively construct obstacles in three-dimensional space while reducing computational costs. The layered map consists of multiple layers of vertically arranged grid maps. Each layer represents a different height range of the environment. The layered map will also be expanded according to the geometry of the unmanned vehicle, taking into full account the geometry and size of different unmanned vehicles to ensure the freedom of exploration of the unmanned vehicle. In the path planning part, a fast random exploration tree (RRT) method based on a generalized Voronoi graph (GVG) is used to achieve planning. By using a layered map model, the present invention can achieve efficient and robust exploration of unmanned vehicles in three-dimensional space.

[0071] The method provided by the present invention constructs a fused map by superimposing multiple layers of maps together to form a comprehensive, multi-dimensional representation of the environment. This multi-layer map structure enables the unmanned vehicle to more accurately understand its surrounding environment, including obstacles and features that may not be fully displayed on a single-layer two-dimensional map.

[0072] The method provided by the present invention includes generalized Voronoi graph extraction, which not only improves the accuracy of path planning, but also ensures the safe navigation of the unmanned vehicle in a complex environment.

[0073] The method provided by the present invention adopts a fast random exploration tree algorithm for the optimization path planning algorithm in continuous space in the path planning stage to generate an optimal path that avoids obstacles. On this basis, combined with the generalized Voronoi diagram and the fast random exploration tree algorithm, a safe and efficient path can be planned for the unmanned vehicle, avoiding potential obstacles while taking into account the specific size and shape of the unmanned vehicle. This method improves the efficiency and safety of path planning, and is particularly suitable for exploration in complex and changing environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 This is a flow chart of the unmanned vehicle exploration planning method based on multi-layer fusion maps according to the present invention;

[0075] Figure 2 A schematic diagram of the fusion map composition;

[0076] Figure 3 It is a schematic diagram of the fusion map generation process;

[0077] Figure 4 The Voronoi diagram generated based on the fused map. Specific implementation methods

[0079] In order to facilitate those skilled in the art to understand and implement the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and specific implementation methods. The content of the present invention is not limited thereto.

[0080] like Figure 1 FIG. 1 is a flowchart of the unmanned vehicle exploration planning method based on a multi-layer fusion map according to the present invention, comprising the following steps:

[0081] S1, multi-layer fusion map construction: multi-layer maps are constructed by segmenting and layering point cloud data, and multi-layer maps are superimposed to form a single-layer fusion map.

[0082] In a possible implementation, S1 includes the following sub-steps:

[0083] S11, terrain analysis:

[0084] In the terrain analysis part, a sparse 3D LiDAR-based map construction technique is used to calculate terrain traversability using Bayesian generalized kernel reasoning. This method reduces the computational burden associated with traversability evaluation. At the same time, a Kalman filter is applied to update the grid elevation values ​​to mitigate the impact of moving objects on map construction. This method is used to evaluate and calculate terrain traversability when building the bottom layer map. Although the generation process of the middle and top layers in the multi-layer map is similar to that of the bottom layer map, their spatial layering eliminates the need for terrain traversability calculation.

[0085] S12, multi-layer fusion map generation:

[0086] The point cloud observed at time t is defined as a set C, where each point p has horizontal and vertical coordinates and a height coordinate z.

[0087] Define a high-level segmentation function f(z) to filter the point cloud data and divide the point cloud set C into three subsets C based on the z coordinate. 1L (bottom layer), C M (Middle level), C H (Top level), meeting the conditions:

[0088] C L ={p∈C|-Δz≤z≤Δz}

[0089] C M ={p∈C|z mid -Δz≤z≤z mid +Δz}

[0090] C 3H ={p∈C|z high -Δz≤z≤z high +Δz}

[0091] Δz is a floating parameter that represents the given height and depends on the lidar settings.

[0092] z mid and z gigh is a predetermined height reference that can be adjusted according to the morphological characteristics of the unmanned vehicle. For example, the top height is h top , the height of the center of mass is h cg The unmanned vehicle can high Set to h top , z mid Set to h cg .

[0093] For each subset C v (v=L,M,H), define a probability grid map function g v (C v ), g v (C v ) By determining the grid cell to which the point cloud belongs and updating the occupancy probability, C iv The point cloud in is mapped to a two-dimensional grid map M v , the following conditions are met:

[0094] M v is a matrix consisting of n×m grid cells. Each grid cell m ij (i=1,...,n; j=i,...,m) represents an area, whose size depends on the resolution of the grid map.

[0095] Each grid cell m ij Has an occupancy probability p(m ij ), indicating the possibility that the area is occupied by the point cloud. p(m ij ) ranges from 0 to 1, where 0 indicates idle, 1 indicates occupied, and 0.5 indicates unknown occupancy. Define the resolution δx and δy of the grid map, that is, the size of each grid cell. Initialize a matrix M of size n×m v , where each unit m ij The initial occupancy probability p(mij ) is set to 0.5 (indicating unknown occupancy). Traverse subset C v For each point p = (x, y, z) in the grid, determine the grid cell m to which the point belongs. ij :

[0096]

[0097] Among them, k represents the row index to which point p belongs, which is calculated by dividing the horizontal coordinate x of the point by the grid width δx and rounding down; l represents the column index to which point p belongs, which is calculated by dividing the vertical coordinate y of the point by the grid height δy and rounding down.

[0098] For each grid cell m ij , update its occupancy probability p(m ij )Law:

[0099] p(m ij )=1-exp(-λN ij )

[0100] Among them, N ij is located in grid cell m ij The number of points in , λ is the adjustment parameter. This step is the function g v (C v ):C v →M v implementation process.

[0101] For each grid cell, the three levels of occupancy probabilities are fused using the Bayesian rule. Assuming that the occupancy probabilities of the bottom, middle, and top maps are p1, p2, and p3, respectively, the fused occupancy probability p_f can be calculated by the following formula: p_f = 1-(1-p1)·(1-p2)·(1-p3).

[0102] During the operation phase of the unmanned vehicle, the selection of an appropriate height reference can significantly enhance the accurate construction of spatial obstacles. The present invention designs a series of adaptive solutions to select the height reference. These solutions are designed to ensure minimal resource consumption while ensuring that spatial obstacles do not collide with the unmanned vehicle, as follows:

[0103] Use laser radar or other sensors to obtain the current top height h of the unmanned vehicle in real time top , center of mass height h cg and the ground height h base These height data are used to determine different height benchmarks. The center of mass height h of the unmanned vehicle cg It is a key parameter that affects the balance and stability of the unmanned vehicle. The center of mass height can be calculated by the following formula:

[0104]

[0105] Where: m i is the mass of each part of the unmanned vehicle, h i is the height of each part, and n is the number of parts that make up the unmanned vehicle.

[0106] The chassis height of the unmanned vehicle h base is the distance from the bottom of the unmanned vehicle to the ground. This height can be obtained by the following formula:

[0107] h base =h wheel +h chassis

[0108] Where: h wheel is the wheel height of the unmanned vehicle, h chassis It is the height of the chassis of the unmanned vehicle.

[0109] According to the top height h of the unmanned vehicle top , center of mass height h cg and the ground height h base , set three height references z high 、z mid and z low These height benchmarks are used to segment the point cloud data to build a multi-layer fused map. high =h top , z mid =h cg , z low =h base .

[0110] S2, Generalized Voronoi graph (GVG) extraction in multi-layer fused maps: Identify the pseudo-free spaces in the fused map, incorporate them into obstacles, and then construct a generalized Voronoi graph by calculating the distance between points and obstacles.

[0111] In the case of three non-overlapping obstacles, the obstacles are defined as the point set C i , C j , C k In the free area, the points equidistant from the two obstacles form a point set F 2 , similarly, the points equidistant from the three obstacles form a point set F 3 , the subscript is represented by the combination of the subscripts of its equally spaced obstacles, such as F ij and F ijk .

[0112] In a possible implementation, S2 includes the following sub-steps:

[0113] S21, eliminate false spatial areas in the fused map:

[0114] In a single-layer map, some areas are passable in terrain analysis, but are actually impassable due to the distribution of obstacles in space. The present invention uses "pseudo free space" to define these areas. Using the set C fakefree To represent the pseudo free space, C free represents free space, C′ represents the obstacle point set. x and y are two points in space, x x represents the x-coordinate of point x, x y represents the y coordinate of point x, y x represents the x coordinate of point y, y y Represents the y coordinate of point y. 3 Represents the set of all points in space.

[0115]

[0116] Since the pseudo free space is still an area that the driverless car cannot pass through, it is divided into obstacles extracted by the generalized Voronoi diagram. i ,C′ j ,C′ k A set of points that defines obstacles that eliminate false free space.

[0117]

[0118] After such processing, false free space is eliminated.

[0119] S22, extraction of generalized Voronoi graph GVG:

[0120] Build a GVG that ensures that the driverless car does not collide, using F 2 to represent the set formed by the projection of the equidistant surface on the bottom map, and use F 3 To represent the set of projections of equidistant surface intersections onto the base map.

[0121] F ij = {x∈R 3 |d(x,C′ i )=d(x,C′ j )}

[0122]

[0123] Among them, F ij is the point set C′ i and C′ j The edges of GVG are equidistant, d(x,C′ i ) represents the point x and the point set C′ i The distance, d(x,C′ j ) represents the point x and the point set C j′, d(x,C′) represents the distance between point x and obstacle point set C′, It represents the shortest distance between point x in space and point y in obstacle point set C′, where obstacle point set C′ represents the point set of any obstacle adjacent to x.

[0124] When the situation is extended to n obstacles, the free space, pseudo free space, and F are defined using the following formulas: 2 and F 3 . Set F 2 is the edge of GVG, set F 3 are the nodes that make up GVG. The union of these two sets is GVG.

[0125]

[0126] Among them, C 1 to C n represents a set of n obstacles, x and y represent points in space. i = 1, ..., n-1, j = i + 1, ..., n, k = j + 1, ..., n. z bottom Represents the height reference of the preset underlying map, E edge is the edge of GVG, and V is the point of GVG.

[0127] S23, adaptive adjustment based on the shape of the unmanned vehicle: expand the map outline according to the geometric shape of the unmanned vehicle and adjust the generalized Voronoi diagram.

[0128] As a specific embodiment, in order to ensure that unmanned vehicles of different sizes can adapt to the GVG constructed in this embodiment, it is necessary to expand the map outline according to the geometric shape of the unmanned vehicle. Due to the uncertainty of the posture of the unmanned vehicle when passing through a certain area, it may be incomplete to expand the map using a specific geometric shape. In view of this, this embodiment uses a maximum radius of a geometric shape corresponding to the height of the unmanned vehicle and a circle of equal diameter to process the image.

[0129]

[0130] Among them, Map represents the map outline; B(R) means B(R) a , is a circle with point a in the two-dimensional plane as the center and the maximum radius R of the unmanned vehicle geometry as the radius; symbol Represents the expansion operation; E 2 represents a two-dimensional plane, and a represents a point in the two-dimensional plane; Is an empty set.

[0131] It can be understood that the expansion operation described in this embodiment refers to adding a circular buffer area with a maximum radius R around each boundary point of the map outline, and merging these buffer areas to form an expanded map outline to ensure that the map can accommodate the maximum geometric shape of the unmanned vehicle and cope with the uncertainty of the posture.

[0132] S3, path planning based on Voronoi diagram: A fast random exploration tree algorithm is used to perform path planning based on the generalized Voronoi diagram.

[0133] After completing the construction of GVG, this embodiment uses a method with a relatively low computational cost to implement path planning. In general RRT-based path planning algorithms, there are often problems such as too many redundant branches in the spanning tree and difficulty in forming a spanning tree from a narrow corridor. If GVG is used to constrain the tree generation process, it can effectively solve the problems that arise in the tree generation process.

[0134] Simply put, GVG can be used to constrain the sampling process so that the sampling points are more concentrated and distributed near GVG. On the one hand, the probability of the generated branches passing through the narrow channel can be increased, and on the other hand, the generation of unnecessary branches can be reduced.

[0135] In one possible implementation, S3 includes the following sub-steps:

[0136] S31, initialize graph G, G = (V, E edge ), where V is the vertex of the graph, E edge is the edge of the graph.

[0137] As a specific embodiment, in the initialization process of step S31, the vertex set V is initialized to {q init}. Among them, q init is the initial node. The edge set E edge Initialize to an empty set

[0138] S32, randomly sample a point q in graph G rand , find the point q closest to this point near , and calculate q rand and q near The Euclidean distance between .

[0139] As a specific embodiment, in step S32, the point q closest to the point is found. near , and calculate q rand and q near The formula for the Euclidean distance between is as follows:

[0140]

[0141] Among them, O represents the set of points that meet the conditions, and point x is a random point in point set C i in, and E 2 represents the two-dimensional plane, n represents the total number of obstacle sets, and i = 1,..., n represents different obstacle set numbers; d(x, C i ) < R means that the distance between point x and obstacle set C i is less than the maximum radius R of the geometry of the driverless vehicle; d represents the distance function; is the distance function, which calculates the Euclidean distance from point x to the nearest point in obstacle set C i , and g represents any point in the obstacle set.

[0142] S33, determine whether there is a collision when moving along the connection path between two points.

[0143] As a specific embodiment, in step S33, the formula for collision judgment is as follows:

[0144]

[0145] Among them, the CollisionFree function represents checking whether a collision occurs; x i , x j are the coordinates of the two ends of the path to be detected for collision respectively, and x i x j is the path itself with two points as endpoints; when the function value is 1, there is no collision on the path, and when the value is 0, there is a collision on the path.

[0146] S34, add the collision-free point q rand to the vertex set V, and add the connection line (q near and q rand connected to the edge set E near , q rand ) and calculate the set of points Q edge near q rand ; perform iterative update. near ; perform iterative update.

[0147] As a specific embodiment, let the number of iterations be k and the maximum number of iterations be K. For each iteration from k = 1 to k = K.

[0148] As a specific embodiment, the method of iterative update is to repeat S32 to S34.

[0149] S35, for each point q near in Q near , if the path connecting q near to q rand is collision-free, then update the edge set E; remove q randThe parent node to q rand edge, add a new edge (q near ,q rand ) to E edge ; Reverse check and update q near The parent node of .

[0150] S36, after the maximum number of iterations K, the constructed graph G is returned, and the optimal path is selected according to the graph G.

[0151] The following is an explanation with reference to the figures.

[0152] Figure 2 It is a fused map, where blue represents the bottom layer map, gray represents the middle layer map, and red represents the high layer map.

[0153] Figure 3 This is a schematic diagram of the fusion map generation process. The bottom map, middle map and high-level map are fused into a fusion map, and the obstacles in the bottom map, middle map and high-level map are uniformly incorporated into the fusion map.

[0154] Figure 4 The blue line in the figure represents the Voronoi diagram, and the edges of the Voronoi diagram (blue lines) are equidistant from the obstacles (black parts) of the fused map. The red line in the figure represents the optimal path generated, the yellow circle represents the starting point, and the green circle represents the destination point.

[0155] Example 2

[0156] This embodiment provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the unmanned vehicle exploration and planning method based on a multi-layer fusion map as described in Example 1 is implemented.

[0157] Example 3

[0158] This embodiment provides a non-transitory computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the unmanned vehicle exploration planning method based on a multi-layer fusion map as described in Example 1 is implemented.

[0159] Example 4

[0160] This embodiment provides a computer program product, including a computer program, which, when executed by a processor, implements the unmanned vehicle exploration planning method based on a multi-layer fusion map as described in Example 1.

[0161] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Those of ordinary skill in the art may understand and implement it without creative effort.

[0162] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0163] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An unmanned vehicle exploration planning method based on multi-layer fusion maps, characterized in that: The following steps are involved: S1, multi-layer fusion map construction: construct multi-layer maps by segmenting and layering point cloud data, and superimpose multi-layer maps to form a fusion map; S2, generalized Voronoi diagram extraction: identify the false free spaces in the fused map, incorporate them into obstacles, and then construct a generalized Voronoi diagram by calculating the distance between points and obstacles; S3, path planning based on Voronoi diagram: A fast random exploration tree algorithm is used to perform path planning based on the generalized Voronoi diagram.

2. The unmanned vehicle exploration planning method based on multi-layer fusion map according to claim 1 is characterized in that: The S1 comprises the following sub-steps: S11, terrain analysis: The map construction technology based on sparse 3D LiDAR is adopted, and the terrain accessibility is calculated using Bayesian generalized kernel reasoning. At the same time, Kalman filtering is applied to update the grid elevation value to reduce the impact of moving objects on map construction. S12, multi-layer fusion map generation: The point cloud observed at time t is defined as a set C, where each point p has horizontal and vertical coordinates and a height coordinate z; Define a high-level segmentation function f(z) to filter the point cloud data and divide the set C into three subsets C based on the z coordinate. L , C M , C H , satisfying the conditions: C L ={p∈C|-Δz≤z≤Δz} C M ={p∈C|z mid -Δz≤z≤z mid +Δz} C H ={p∈C|z high -Δz≤z≤z high +Δz} Δz represents a given height floating parameter, which depends on the setting of the lidar; z mid and z high It is a predetermined height benchmark, which is adjusted according to the morphological characteristics of the unmanned vehicle; For each subset C v , v = L, M, H, define a probability grid map function g v (C v ), g v (C v ) By determining the grid cell to which the point cloud belongs and updating the occupancy probability, C v The point cloud in is mapped to a two-dimensional grid map M v , the following conditions are met: M v is a matrix composed of n×m grid cells; each grid cell m ij , represents an area, i = 1, ..., n, j = 1, ..., m, and its size depends on the resolution of the grid map; Each grid cell m ij Has an occupancy probability p(m ij ), indicating the possibility that the area is occupied by the point cloud; p(m ij ) ranges from 0 to 1, where 0 indicates idle, 1 indicates occupied, and 0.5 indicates unknown occupancy.

3. The unmanned vehicle exploration planning method based on multi-layer fusion map according to claim 1 is characterized in that: The S2 comprises the following sub-steps: S21, eliminate false spatial regions in single-layer fusion maps: Use set C fakefree represents pseudo free space, C free represents free space, C′ represents the obstacle point set; x and y are two points in space, x x represents the x-coordinate of point x, x t represents the y coordinate of point x, y x represents the x coordinate of point y, y y represents the y coordinate of point y; R 3 Represents the set of all points in space; Among them, C i , C j and C k A set of points representing three obstacles; The pseudo free space is an area that the unmanned vehicle cannot pass through, which is divided into obstacles extracted by the generalized Voronoi diagram; Using C′ i ,C′ j ,C′ k Define the set of points that eliminate obstacles for false free space: Among them, C′ i To eliminate the obstacle C of the pseudo free space i The point set, C′ j To eliminate the obstacle C of the pseudo free space j The point set, C′ k To eliminate the obstacle C of the pseudo free space k The point set of False free space was eliminated by redefining obstacles; S22, extraction of generalized Voronoi graph GVG: Build a GVG that ensures that the driverless car does not collide, using F 2 To represent the set of projections of equidistant surfaces onto the bottom map, use F 3 to represent the set formed by the projection of the intersection lines of equidistant surfaces on the bottom map; F ij ={x∈R 3 |d(x,C′ i )=d(x,C′ j )} Among them, F ij is the point set C′ i and C′ j The edges of GVG are equidistant, d(x,C′ i ) represents the point x and the point set C′ i The distance, d(x,C′ j ) represents the point x and the point set C′ j , d(x,C′) represents the distance between point x and obstacle point set C′, Represents the shortest distance between point x in space and point y in obstacle point set C′; When there are n obstacles, the following formula is used to define free space, pseudo free space, and F 2 and F 3 ; Set F 2 is the edge of GVG, set F 3 are the nodes that make up GVG, and the union of these two sets is GVG; Among them, C1 to C n represents n obstacles, x and y represent points in space; i = 1, ..., n-1, j = i+1, ..., n,; k = j+1, ..., n; z bottom Represents the height reference of the preset underlying map, E edge is the edge of GVG, V is the point of GVG; S23, adaptive adjustment based on the shape of the unmanned vehicle: expand the map outline according to the geometric shape of the unmanned vehicle and adjust the generalized Voronoi diagram.

4. The unmanned vehicle exploration planning method based on multi-layer fusion map according to claim 3 is characterized in that: In the step S23, a circle with a maximum radius and equal diameter of a geometric shape corresponding to the height of the unmanned vehicle is used to process the image; Among them, Map represents the map outline; B(R) means B(R) a , is a circle with point a in the two-dimensional plane as the center and the maximum radius R of the unmanned vehicle geometry as the radius; symbol Represents the expansion operation; E 2 Represents a two-dimensional plane; Is an empty set.

5. The unmanned vehicle exploration planning method based on multi-layer fusion map according to claim 1 is characterized in that: The S3 comprises the following sub-steps: S31, initialize graph G, g=(V,E edge ), where V is the vertex of the graph, E edge is the edge of the graph; S32, randomly sample a point q in graph G rand , find the point q closest to this point near , and calculate q rand and q near The Euclidean distance between S33, determining whether there is a collision along the connection path between the two points; S34, the collision-free point q ranf Add to the vertex set V, and add q near and q rand The connecting line (q near ,q rand ) is added to the edge set E edge , and calculate q rand The set of nearby points Q near ; Carry out iterative updates; S35, for Q near Each point q in near If you connect q near to q rand If there is no collision on the path, update the edge set E; Remove q rand The parent node to q rand edge, add a new edge (q near ,q rand ) to E edge ; Reverse check and update q near The parent node of S36, after the maximum number of iterations K, the constructed graph G is returned, and the optimal path is selected according to the graph G.

6. The unmanned vehicle exploration planning method based on multi-layer fusion map according to claim 5 is characterized in that: In step S32, find the point q closest to the point near , and calculate q ranf and q near The formula for the Euclidean distance between is as follows: Among them, O represents the set of points satisfying the conditions, and point x is a random point in the obstacle point set C i in, and E 2 represents the two-dimensional plane, n represents the total number of obstacle sets, and i = 1, …, n represents different obstacle set numbers; d(x, C i ) < R means that the distance between point x and the obstacle set C i is less than R; d represents the distance function; is the distance function, which calculates the Euclidean distance from point x to the nearest point in the obstacle set C i where g represents any point in the obstacle set.

7. The unmanned vehicle exploration planning method based on multi-layer fusion map according to claim 6 is characterized in that: In step S33, the collision judgment formula is as follows: Among them, the CollisionFree function indicates whether a collision occurs; x i 、x j are the coordinates of the two ends of the path to be collided, x i x j is the path itself with two points as endpoints; when the function value is 1, there is no collision on the path, and when the value is 0, there is a collision on the path.

8. An electronic device comprising 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 unmanned vehicle exploration planning method based on a multi-layer fusion map as described in any one of claims 1 to 7 is implemented.

9. 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 unmanned vehicle exploration planning method based on a multi-layer fusion map as described in any one of claims 1 to 7 is implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the unmanned vehicle exploration planning method based on a multi-layer fusion map as described in any one of claims 1 to 7 is implemented.

Images