Layer-by-layer accumulated heuristic-guided ground vehicle path planning method and system

Abstract

The application discloses a kind of layer-by-layer cumulative heuristic guidance ground vehicle path planning method and system, its method includes: S1, the path planning model of containing road network map is constructed, obstacle data that influences ground vehicle driving is marked on road network map;S2, m-layer subgrid mask is constructed according to upper and lower dimensions on the road network map of path planning model, the front m-1 layer subgrid mask of road network map is executed task target in turn and front and rear layer cumulative heuristic grid route chain search;S3, path planning model uses route chain to carry out optimal path search and optimization on road network map and obtains ground vehicle driving planning path.The path planning model of the application is from macro route to fine route layer-by-layer cumulative search guidance and obtains route chain, realizes the search guidance of front and rear layer cumulative heuristic task target and obtains fine route chain by multilayer subgrid mask fast search, reduces search calculation amount and complexity, improves vehicle driving path planning efficiency.

Description

Technical Field

[0001] This invention relates to the field of ground vehicle routing, and more particularly to a ground vehicle routing method and system based on a layer-by-layer cumulative heuristic guidance. Background Technology

[0002] Ground vehicles are a collective term for driverless vehicles, unmanned rescue vehicles, unmanned robots, and manned vehicles (including special vehicles). Ground vehicles are responsible for path planning in large-scale and ultra-large-scale complex road networks in urban and rural areas, and in environments with multiple ground obstacles. With the rapid development of artificial intelligence, the application prospects of autonomous ground vehicles (AGVs) such as driverless vehicles and unmanned walking robots are becoming increasingly broad. In particular, with the continuous development of driverless taxi technology and applications, autonomous ground vehicles, as an important type of ground vehicle in the future, can automatically navigate and complete ground driving by pre-planning routes and programs, and achieve purposes such as emergency supplies transportation and disaster relief. When unmanned ground vehicles are performing a journey from origin to destination, complex road networks may present obstacles that prevent passage. These obstacles include construction sites, geological disasters causing road blockages, and congested urban roads (e.g., traffic jams caused by accidents). In unmanned rescue vehicles or robots, drones are typically used to inspect various road segments for obstacles. Based on this obstacle inspection data, ground vehicles face numerous challenges in large-scale, complex terrain or environments with multiple obstacles. Unmanned rescue vehicles or robots may intrude into obstacle-filled sections of road and become passively trapped. Therefore, unmanned rescue ground vehicles need to plan routes in advance to avoid obstacles, providing a safe and optimal route for efficient completion of the journey and facilitating quick and safe arrival at the destination.

[0003] Existing map navigation path search methods calculate the path cost of each pixel from the starting point and gradually find the shortest (i.e., optimal) path. They involve a global search and require evaluation of a massive number of path nodes. Existing path search methods perform pixel-by-pixel search, which is a one-time global search algorithm. When dealing with large-scale sea areas or water bodies, they lack macroscopic indications of spatial scale layer by layer, resulting in problems such as high computational load, high resource consumption, and low path planning efficiency. Summary of the Invention

[0004] The purpose of this invention is to solve the technical problems pointed out in the background art, and to provide a ground vehicle path planning method and system with layer-by-layer cumulative heuristic guidance. A multi-layer sub-grid mask is constructed on the road network map of the path planning model. The multi-layer sub-grid mask is used to perform grid route chain searches at progressively higher spatial resolutions to obtain progressively more detailed route chains. The multi-layer sub-grid mask realizes the search guidance of the cumulative heuristic task objective of the preceding and following layers, improving the efficiency of grid route chain search. The final sub-grid mask uses the refined route chains to perform optimal path search and optimization, which can quickly obtain a refined ground vehicle driving plan path, providing ground vehicles with driving plans that avoid obstacle sections.

[0005] The objective of this invention is achieved through the following technical solution:

[0006] A ground vehicle path planning method guided by a layer-by-layer cumulative heuristic, comprising the following steps:

[0007] S1. Construct a path planning model that includes a road network map. The road network map is divided into several road segments according to road intersections. Mark the obstacle data that affects the driving of ground vehicles on the road network map.

[0008] S2. On the road network map of the path planning model, set the task objective of the ground vehicle's journey from the starting point to the destination. Construct m layers of sub-grid masks on the road network map according to the upper and lower dimensions. The first m-1 layers of sub-grid masks of the road network map sequentially execute the task objective and perform a cumulative heuristic search of the grid route chain. The (m-1)th layer of sub-grid mask outputs the route chain. ;

[0009] S3, the path planning model utilizes route chains in the m-th layer sub-grid mask. The optimal path is searched and optimized on the road network map to obtain the planned driving path for ground vehicles.

[0010] To better implement the present invention, in method S1, if an obstacle is marked on a road segment on the road network map, the road segment is marked as impassable; if no obstacle is marked on a road segment on the road network map, the road segment is marked as passable.

[0011] Preferably, in method S2, the m-layer sub-mesh mask is a sub-mesh mask according to its upper and lower dimensions. ~ Sub-grid mask ~ The grid is constructed sequentially according to the upper and lower belonging dimensions; the method of performing the cumulative heuristic grid route chain search of the first m-1 layers of sub-grid masks to achieve the task objective is as follows:

[0012] S21. Subgrid mask in the road network map The task objective is to select grid-connected routes that contain marked, passable road segments as route chains. ;

[0013] S22, by route chain As a cumulative heuristic grid route chain guide, in the subgrid mask of the road network map The task objective is to select grid-connected routes that contain marked, passable road segments as route chains. ;

[0014] S23, by route chain As a cumulative heuristic grid route chain guide, in the subgrid mask of the road network map The task objective is to select grid-connected routes that contain marked, passable road segments as route chains. ;

[0015] S24. Obtain the sub-mesh mask by following the methods S22 to S23. route chain .

[0016] Preferably, a sub-mesh mask ~ The following methods are used in all cases:

[0017] Mask the top layer of mesh route chain As a sub-mesh mask of this layer Macro-level route grid search uses a cumulative heuristic grid route chain guide, with the current layer's sub-grid mask. Guided by the cumulative heuristic grid route chain in the previous layer, perform a grid search for the task objective and filter out grid chains containing the connected routes of marked, passable road segments. , The dimensions of the sub-mesh mask. .

[0018] Preferably, from the sub-mesh mask Mesh density to sub-mesh mask The mesh density increases sequentially; the upper-level sub-mesh mask A specific mesh in the mapping includes the mask of the next layer of sub-mesh. Several grids in the middle.

[0019] Preferably, the sub-mesh mask Before the path search method, in the submesh mask Define a virtual straight line from the starting point to the end point of the task objective as a path search guide; utilize the mask of the upper-level sub-mesh. route chain Define a virtual polyline from the starting point to the ending point of the task objective as the sub-mesh mask for this layer. Path search guide lines; mask the upper sub-mesh. route chain grid Corresponding projection of the sub-mesh mask of this layer In the middle, grid In the A sub-mesh mask contains several meshes. In each grid The process employs a distributed parallel processing approach, utilizing cumulative heuristic grid route chains and / or path search guide lines to mask the sub-grid at this layer. Grid search.

[0020] Preferably, in method S3, the corresponding projected route chain is in the m-th layer sub-grid mask of the road network map. Serial mesh , grid The m-th layer sub-mesh mask contains several meshes. The task objective is to perform the task on the m-th layer sub-grid mask of the road network map and filter out the grid-connected routes that contain the marked road segments that are passable to form route chains. .

[0021] Preferably, the route chain is defined in the road network map of the path planning model. The preliminary path is obtained by connecting the marked road segments that are passable. The preliminary path is then optimized by processes including redundancy elimination, path smoothing, and Bresenham straight line algorithm to obtain the ground vehicle driving planning path.

[0022] A ground vehicle path planning system based on a layer-by-layer cumulative heuristic guidance includes a path planning model, an obstacle data input module, and a task objective setting module. The path planning model contains a road network map, which is divided into road segments according to road intersections. The obstacle data input module is used to mark obstacle data affecting ground vehicle movement on the road network map. The task objective setting module is used to set the task objective from the UAV's flight start point to its destination on the road network map. The path planning model includes a grid route chain search processing module and a fine-planning path processing module. An m-layer sub-grid mask is constructed on the road network map of the path planning model, progressively building m layers in vertical dimensions. The grid route chain search processing module performs a cumulative heuristic grid route chain search on the (m-1)th layer sub-grid mask, sequentially performing searches before and after the task objective. The (m-1)th layer sub-grid mask outputs the route chain. The grid route chain search processing module is also used to utilize route chains in the m-th layer sub-grid mask. The optimal path search result is generated and input into the fine-planning path processing module, which is used to optimize and process the optimal path search result to obtain the ground vehicle driving planning path.

[0023] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0024] (1) The present invention constructs a multi-layer sub-grid mask on the road network map of the path planning model. The multi-layer sub-grid mask is used to perform grid route chain search with progressively higher spatial resolution to obtain progressively more detailed route chains. The multi-layer sub-grid mask realizes the search guidance of the cumulative heuristic task objectives of the front and rear layers, which improves the efficiency of grid route chain search. The last layer sub-grid mask uses the refined route chain to perform optimal path search and optimization, which can quickly obtain refined ground vehicle driving planning paths, provide ground vehicles with driving planning routes to avoid obstacle sections, and improve the safe navigation capability and planning decision capability of ground vehicles.

[0025] (2) The m-layer sub-mesh mask of the path planning model of the present invention is constructed according to the grid space scale reduction factor to establish the upper and lower levels. The search guidance and route chain are accumulated layer by layer from the macro route to the refined route. The search guidance of the cumulative heuristic task target of the front and rear layers is realized, and the refined route chain is obtained quickly through multi-layer sub-mesh mask. The search calculation and complexity are reduced, and the efficiency of vehicle driving path planning is improved. Attached Figure Description

[0026] Figure 1 This is a flowchart of the method for route planning and navigation of a surface vehicle according to the present invention;

[0027] Figure 2 This is a flowchart illustrating the method for searching the cumulative heuristic mesh route chain of multi-layer sub-mesh masks in this embodiment.

[0028] Figure 3 This is a schematic diagram illustrating how a preliminary path is obtained by progressively descending a certain region based on spatial scale, as exemplified in the embodiment.

[0029] Figure 4 for Figure 3 After preliminary path processing, a schematic diagram of the planned driving path for ground vehicles is obtained. Detailed Implementation

[0030] The present invention will be further described in detail below with reference to embodiments:

[0031] Example

[0032] like Figure 1 As shown, a ground vehicle path planning method based on layer-by-layer cumulative heuristic guidance includes the following steps:

[0033] S1. Construct a path planning model that includes a road network map. The road network map is divided into several road segments according to road intersections. Mark the obstacle data that affects the driving of ground vehicles on the road network map. If there is an obstacle marked on a road segment on the road network map, the road segment is marked as impassable. If there is no obstacle marked on a road segment on the road network map, the road segment is marked as passable.

[0034] S2. On the road network map of the path planning model, set the task objective (the task objective is the vehicle travel task from the starting point to the destination) on the ground vehicle's journey path. Construct an m-layer sub-grid mask on the road network map of the path planning model, arranged in upper and lower dimensions. In some embodiments, the m-layer sub-grid mask of the road network map in the path planning model is a sub-grid mask in both upper and lower dimensions. ~ Sub-grid mask ~ The grid is constructed sequentially according to its upper and lower dimensions; from the sub-grid mask Mesh density to sub-mesh mask The mesh density increases sequentially while the spatial scale of the mesh decreases sequentially; the upper-level sub-mesh mask ( The mapping of a specific mesh in the sub-mesh mask hierarchy contains the next level sub-mesh mask. There are several grids in the middle. Taking the mapping of one grid of the upper-level sub-mesh mask to include 5×5 grids (i.e., 25 grids) of the lower-level sub-mesh mask as an example, if the sub-mesh mask... If the grid space scale is 5m×5m, then the upper sub-grid mask... The grid space scale is 25m×25m, and the upper layer sub-grid mask. The mesh mapping includes 100 sub-mesh masks of the next layer. The mesh (the 25 meshes of the next layer sub-mesh mask correspond to one mesh belonging to the previous layer sub-mesh mask, i.e., constructed sequentially according to the hierarchical hierarchy), is the sub-mesh mask. ~ The grid spatial scale decreases sequentially in increments of 25; if the subgrid mask... The grid spatial scale (also known as spatial resolution) is 25m × 25m, and so on, to obtain the sub-grid mask. The grid spatial scale. Taking a five-layer sub-grid mask as an example, in an example where the reduction factor of the grid spatial scale between the upper and lower sub-grid layers is 25, the grid spatial scale of the first sub-grid mask is 3.125km × 3.125km, the grid spatial scale of the second sub-grid mask is 625m × 625m, the grid spatial scale of the third sub-grid mask is 125m × 125m, the grid spatial scale of the fourth sub-grid mask is 25m × 25m, and the grid spatial scale of the fifth sub-grid mask is 5m × 5m. The sub-grid mask hierarchy constructed on the electronic nautical chart of this invention, as well as the reduction factor of the grid spatial scale between the upper and lower sub-grid masks, are specifically set in actual practice.

[0035] In the first m-1 layers of the road network map, the task objective is executed sequentially, and the heuristic grid route chain search is performed by accumulating the results from the preceding and following layers. The route chain is output by the m-1 layer of the sub-grid mask. In some embodiments, such as Figure 2 As shown, the method for performing cumulative heuristic mesh route chain search before and after the task objective using the first m-1 layer sub-mesh mask is as follows:

[0036] S21. Subgrid mask in the road network map The task objective is to select grid-connected routes that contain marked, passable road segments as route chains. The path planning model is displayed in the subgrid mask of the road network map. The above filter includes grid-connected routes corresponding to marked passable road segments (the grid-connected routes are optimally searched from the starting point to the end point according to the task objective) as route chains. .

[0037] S22, by route chain As a cumulative heuristic grid route chain guide, in the subgrid mask of the road network map The task objective is to select grid-connected routes that contain marked, passable road segments as route chains. .

[0038] S23, by route chain As a cumulative heuristic grid route chain guide, in the subgrid mask of the road network map The task objective is to select grid-connected routes that contain marked, passable road segments as route chains. .

[0039] S24. Obtain the sub-mesh mask by following the methods S22 to S23. route chain .

[0040] Sub-mesh mask ~ The following methods are used in all cases:

[0041] In some embodiments, the upper sub-mesh mask route chain As a sub-mesh mask of this layer Macro-level route grid search uses a cumulative heuristic grid route chain guide, with the current layer's sub-grid mask. Guided by the cumulative heuristic grid route chain in the previous layer, perform a grid search for the task objective and filter out grid chains containing the connected routes of marked, passable road segments. , The dimensions of the sub-mesh mask. .

[0042] In some embodiments, submesh mask Before the path search method, in the submesh mask Define a virtual straight line from the starting point to the end point of the task objective as a path search guide; utilize the mask of the upper-level sub-mesh. route chain Define a virtual polyline from the starting point to the ending point of the task objective as the sub-mesh mask for this layer. Path search guide lines; mask the upper sub-mesh. route chain grid Corresponding projection of the sub-mesh mask of this layer In the middle, grid In the A sub-mesh mask contains several meshes. In each grid The process employs a distributed parallel processing approach, utilizing cumulative heuristic grid route chains and / or path search guide lines to mask the sub-grid at this layer. Grid search, that is, in each grid The system utilizes a cumulative heuristic grid route chain guide and a path search guide line for joint guidance, facilitating rapid masking of the current layer's sub-grid. The grid search is performed simultaneously with the sub-grid masking of this layer. During grid search, each grid Distributed parallel processing (i.e., grid processing) can be used. In the corresponding mapped grids Medium grid search, each grid (Processing in parallel) to accelerate processing efficiency.

[0043] S3, the path planning model utilizes route chains in the m-th layer sub-grid mask. The optimal path search and optimization on the road network map yields the planned driving paths for ground vehicles. Specifically, the corresponding projected route chains are mapped into the m-th layer sub-grid mask of the road network map. Serial mesh , grid The m-th layer sub-mesh mask contains several meshes. (In the example where the grid space resolution is reduced by a factor of 25, there are 25 grids.) In the m-th layer sub-grid mask of the road network map, the task objective is to filter out grid-connected routes that contain marked, passable road segments as route chains. In the road network map of the path planning model, route chains are... A preliminary path is obtained by connecting the marked passable road segments, such as... Figure 3 As shown, this embodiment takes a task objective from the starting point to the ending point in a certain area with many obstacles as an example. This embodiment obtains the route chain based on a grid search with the spatial scale decreasing step by step (referring to the grid spatial scale decreasing step by step). For route chains By performing connection processing, Figure 3 The preliminary path (i.e., the initial planned route) for this area is shown. Figure 3 The green line represents the initial path, which avoids obstacle-laden sections in this complex area. The initial path undergoes optimization processes including redundancy elimination, path smoothing, and the Bresenham straight-line algorithm to obtain the ground vehicle driving plan path (i.e., the ground vehicle driving plan route). Figure 4 (The red line); when optimizing the planning path of ground vehicles, the planning should be constrained according to the center line of the road segment in the road network map.

[0044] A ground vehicle path planning system based on a layer-by-layer cumulative heuristic guidance includes a path planning model, an obstacle data input module, and a task objective setting module. The path planning model contains a road network map, which is divided into road segments according to road intersections. The obstacle data input module is used to mark obstacle data affecting ground vehicle movement on the road network map. The task objective setting module is used to set the task objective from the UAV's flight start point to its destination on the road network map. The path planning model includes a grid route chain search processing module and a fine-planning path processing module. An m-layer sub-grid mask is constructed on the road network map of the path planning model, progressively building m layers in vertical dimensions. The grid route chain search processing module performs a cumulative heuristic grid route chain search on the (m-1)th layer sub-grid mask, sequentially performing searches before and after the task objective. The (m-1)th layer sub-grid mask outputs the route chain. The grid route chain search processing module is also used to utilize route chains in the m-th layer sub-grid mask. The optimal path search result is generated and input into the fine-planning path processing module, which uses the optimal path search result to optimize and process the path to obtain the planned path for ground vehicle travel.

[0045] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A ground vehicle path planning method based on layer-by-layer cumulative heuristic guidance, characterized in that: The methods include: S1. Construct a path planning model that includes a road network map. The road network map is divided into several road segments according to road intersections. Mark the obstacle data that affects the driving of ground vehicles on the road network map. If there is an obstacle marked on a road segment on the road network map, the road segment is marked as impassable. If there is no obstacle marked on a road segment on the road network map, the road segment is marked as passable. S2. On the road network map of the path planning model, set the task objective of the ground vehicle's journey from the starting point to the destination. Construct m layers of sub-grid masks on the road network map according to the upper and lower dimensions. The first m-1 layers of sub-grid masks of the road network map sequentially execute the task objective and perform a cumulative heuristic search of the grid route chain. The (m-1)th layer of sub-grid mask outputs the route chain. The m-layer sub-mesh mask is divided into sub-mesh masks according to the upper and lower dimensions. ~ Sub-grid mask ~ The grid is constructed sequentially according to the upper and lower belonging dimensions; the method of performing the cumulative heuristic grid route chain search of the first m-1 layers of sub-grid masks to achieve the task objective is as follows: S21. Subgrid mask in the road network map The task objective is to select grid-connected routes that contain marked, passable road segments as route chains. ; S22, by route chain As a cumulative heuristic grid route chain guide, in the subgrid mask of the road network map The task objective is to select grid-connected routes that contain marked, passable road segments as route chains. ; S23, by route chain As a cumulative heuristic grid route chain guide, in the subgrid mask of the road network map The task objective is to select grid-connected routes that contain marked, passable road segments as route chains. ; S24. Obtain the sub-mesh mask by following the methods S22 to S23. route chain ; S3, the path planning model utilizes route chains in the m-th layer sub-grid mask. The optimal path is searched and optimized on the road network map to obtain the planned driving path for ground vehicles.

2. The ground vehicle path planning method based on layer-by-layer cumulative heuristic guidance according to claim 1, characterized in that: Sub-mesh mask ~ The following methods are used in all cases: Mask the top layer of mesh route chain As a sub-mesh mask of this layer Macro-level route grid search uses a cumulative heuristic grid route chain guide, with the current layer's sub-grid mask. Guided by the cumulative heuristic grid route chain in the previous layer, perform a grid search for the task objective and filter out grid chains containing the connected routes of marked, passable road segments. , The dimensions of the sub-mesh mask. .

3. The ground vehicle path planning method based on layer-by-layer cumulative heuristic guidance according to claim 1, characterized in that: From subgrid mask Mesh density to sub-mesh mask The mesh density increases sequentially; the upper-level sub-mesh mask A specific mesh in the mapping includes the mask of the next layer of sub-mesh. Several grids in the middle.

4. The ground vehicle path planning method based on layer-by-layer cumulative heuristic guidance according to claim 3, characterized in that: The sub-mesh mask Before the path search method, in the submesh mask Define a virtual straight line from the starting point to the end point of the task objective as a path search guide; utilize the mask of the upper-level sub-mesh. route chain Define a virtual polyline from the starting point to the ending point of the task objective as the sub-mesh mask for this layer. Path search guide lines; mask the upper sub-mesh. route chain grid Corresponding projection of the sub-mesh mask of this layer In the middle, grid In the A sub-mesh mask contains several meshes. In each grid The process employs a distributed parallel processing approach, utilizing cumulative heuristic grid route chains and / or path search guide lines to mask the sub-grid at this layer. Grid search.

5. The ground vehicle path planning method based on layer-by-layer cumulative heuristic guidance according to claim 1, characterized in that: In method S3, the corresponding projected route chain is in the m-th layer sub-grid mask of the road network map. Serial mesh , grid The m-th layer sub-mesh mask contains several meshes. The task objective is to perform the task on the m-th layer sub-grid mask of the road network map and filter out the grid-connected routes that contain the marked road segments that are passable to form route chains. .

6. The ground vehicle path planning method based on layer-by-layer cumulative heuristic guidance according to claim 5, characterized in that: In the road network map of the path planning model, the route chain is... The preliminary path is obtained by connecting the marked road segments that are passable. The preliminary path is then optimized by processes including redundancy elimination, path smoothing, and Bresenham straight line algorithm to obtain the ground vehicle driving planning path.

7. A ground vehicle path planning system that implements the layer-by-layer cumulative heuristic guidance of the ground vehicle path planning method of claim 1, characterized in that: The system includes a path planning model, an obstacle data input module, and a task objective setting module. The path planning model contains a road network map, which is divided into road segments based on road intersections. The obstacle data input module is used to mark obstacle data affecting ground vehicle traffic on the road network map. The task objective setting module is used to set the task objective from the UAV's flight start point to its destination on the road network map. The path planning model includes a grid route chain search processing module and a fine-planning path processing module. An m-layer sub-grid mask is constructed on the road network map of the path planning model, progressively building m layers of sub-grid masks along vertical dimensions. The grid route chain search processing module performs a cumulative heuristic grid route chain search on the (m-1)th layer of sub-grid masks, sequentially performing searches on the layers preceding and following the task objective. The (m-1)th layer of sub-grid masks outputs the route chain. The grid route chain search processing module is also used to utilize route chains in the m-th layer sub-grid mask. The optimal path search result is generated and input into the fine-planning path processing module, which is used to optimize and process the optimal path search result to obtain the ground vehicle driving planning path.

Images