A Vehicle Route Planning Method in a Complex Environment

Abstract

The present invention relates to a vehicle path planning method in a complex environment and belongs to the field of intelligent transportation technology. This method adopts an incremental loading and search path planning algorithm, reusing the planning results of previously loaded areas in the incremental loading path planning algorithm to further improve the speed of path planning. An incremental method is used when loading map data to avoid loading a large amount of map data at one time, significantly reducing the geographical information processing time. Within the loaded area range, an incremental search path planning algorithm that breaks path symmetry is used to further improve the path planning speed. At the same time, aiming at the symmetric repeated search problem of the existing heuristic search algorithm, a straight-line trajectory deviation value is introduced to distinguish symmetric points with the same original priority to break the path symmetry, reduce the exploration of redundant grids by the algorithm, and improve the planning speed.

Description

Technical Field

[0001] The present invention relates to a vehicle path planning method in a complex environment, belonging to the technical field of intelligent transportation. Background Art

[0002] In the field of intelligent transportation, with the rapid development and application of technologies such as driverless and vehicle-road cooperation, the system has put forward higher and higher requirements for the vehicle path planning ability.

[0003] Vehicle path planning is to quickly plan one or more optimal paths for users to make decisions according to the planning goal, vehicle performance and other constraints through algorithms. With the development of current navigation technology, vehicles are no longer limited to the topological road network environment. In special cases, off-road vehicles in the wild need to study how to efficiently and accurately plan paths in the case of no road network topology or coexistence of the wild and the road network.

[0004] In addition to the traditional urban environment of large-scale road networks, the wild environment with complex terrain and areas without roads also faces path planning requirements. When modeling geographical information in a roadless environment, many environmental terrain factors need to be comprehensively considered, such as rivers, roads, soil quality, etc. Rasterizing the map and then applying classic algorithms such as Dijkstra, A* algorithms and bidirectional pathfinding algorithms based on the two are widely used in raster path planning. However, classic path planning algorithms such as the Dijkstra algorithm and the A * search algorithm are not very efficient in a large-scale grid environment. The Dijkstra algorithm requires a time complexity of O(n 2 ) to find the best path. And the A * algorithm also needs a lot of time to plan to reach the end point when facing dense obstacles, and the amount of calculation will increase exponentially with the increase of the search space. Especially in the application scenario of wild path planning, the map area is usually large, and the massive data combined with limited computing power makes the planning time too long, which is an important reason for the pathfinding performance bottleneck in large maps. How to further improve the path planning efficiency under large-scale maps is the focus of research in this field.

[0005] In addition, in the field of path planning, heuristic algorithms usually have the problem of repeated search. There are generally many possible optimal paths with the same cost between the same starting position and ending position, and these paths are symmetric to each other. The only difference between them is the order of vehicle movement. Solving the symmetric repeated search problem of heuristic path planning algorithms and how to break path symmetry and reduce repeated search in large-scale maps is of great significance. Summary of the Invention

[0006] The object of the present invention is to address the deficiencies and drawbacks existing in the prior art. In order to effectively solve the problems of serious time consumption in vehicle path planning in autonomous driving and the repeated search and low efficiency caused by path symmetry in existing algorithms, a vehicle path planning method in a complex environment is creatively proposed.

[0007] The innovation of this method lies in: for the first time, an Incremental Load and Search Path Planning Algorithm (ILSP) is proposed, aiming to reuse the planning results of previously loaded areas in the incremental load path planning algorithm to further improve the speed of path planning. When loading map data, an incremental method is adopted to avoid loading a large amount of map data at one time, significantly reducing the geographical information processing time. Within the loaded area range, an incremental search path planning algorithm that breaks path symmetry is used to further improve the path planning speed. At the same time, aiming at the symmetric repeated search problem of the existing heuristic search algorithm, a straight-line trajectory deviation value is introduced to distinguish symmetric points with the same priority originally and break the path symmetry, reducing the exploration of redundant grids by the algorithm and improving the planning speed.

[0008] The present invention is implemented by the following technical solutions.

[0009] A vehicle path planning method in a complex environment includes the following steps:

[0010] Step 1: Select the starting point and ending point of the path. Based on the starting and ending points, a rectangle is planned, and using this rectangle as a reference, it is expanded outward to obtain a planning center rectangle that encloses the reference rectangle.

[0011] Then, perform geographical information modeling on the planning center rectangle. Specifically, the following method can be adopted:

[0012] Step 1.1: Perform geographical information modeling on the planning area.

[0013] The present invention adopts a rasterization processing method to divide the map into several small grids according to the set grid side length. Among them, the side length l of a single grid grid is calculated by Equation 1:

[0014]

[0015] where, l grid represents the side length of a single grid, S i represents the area of the polygon inside the region (such as lakes, residential areas, etc.), and n represents the number of polygons in the region.

[0016] The geographical information data book of the map consists of multiple vector layers, and each layer stores a set of characteristic geographical information in the area. The geographical information data is divided into grids. For example, Figure 1 As shown, for the rasterized planning area, by traversing the grids, the row and column numbers are converted into Gaussian coordinates during the traversal, and the required layer data is obtained with the help of the geographical system engine.

[0017] Step 1.2: Processing of soil layer information.

[0018] For the information of the soil layer, when planning the path, refer to the average driving speed of the vehicle on different soils. Taking the driving speed of the vehicle on flat sandy land as the benchmark, set the soil speed ratio u of grid u during the modeling stage ratio , which is calculated by Equation 2:

[0019]

[0020] where, v soil is the driving speed of the vehicle corresponding to the soil of the current grid, and v Flatsand is the driving speed of the vehicle when the soil is flat sandy land.

[0021] Step 1.3: Topological road network modeling.

[0022] The present invention uses the directed graph structure in graph theory to store the road network for modeling and generates a directed graph that meets the planning requirements.

[0023] Specifically, the data in the map road vector layer is constructed into a topological model, which is expressed as the following formula:

[0024] G=(V a ,E a ,W a ) (3)

[0025] V a ={v i |i∈1,2,3...,n} (4)

[0026] W a ={w ij |i,j∈1,2,3,...,n} (5)

[0027] E a ={e i |i∈1,2,3,...,n} (6)

[0028] where, G represents the entire road topological graph, V a represents the starting and ending points of the roads in the road network and the vertices abstracted from different road connection points, and v i represents the i-th vertex; W a represents the cost value of the arc, and wij Represents the cost value between the i-th and j-th nodes; E a is the set of edges abstracted from the roads, e i represents the i-th edge. Each element in the set is a road, which is composed of the intersections connected by the road and the attributes of the road, and is expressed as:

[0029] e a =(S id , E id , Q a , P v ) (7)

[0030] where S id is the starting point id of the road, E id is the ending point id of the road; Q a is the attribute information of the road, including the road grade and the length of the road, and P v is the set of positioning points of the road.

[0031] After modeling the road network as a directed graph, the vector road network data is stored in the form of a graph structure, and the road network is stored in the form of an adjacency list.

[0032] Step 2: Adopt an incremental loading path planning algorithm to improve the vehicle path planning efficiency.

[0033] After determining the planning center rectangle, that is, the entire planning area, divide the planning center rectangle into sp parts, define the loading area identifier as mark, and the number of planned rows and columns in each area are ld_rows mark , ld_cols mark , and initialize them to 0 respectively. Define the number of rows and columns increased in each loading map round of the incremental loading map as rows and cols respectively.

[0034] where the calculation methods of rows and cols are as follows:

[0035] rows = [ROWS / sp] (8)

[0036] cols = [COLS / sp] (9)

[0037] where ROWS is the total number of rows of the planning center rectangle, COLS is the total number of columns of the planning center rectangle, and [] is the ceiling symbol.

[0038] In each new round of incremental loading, the number of rows and columns in each area is ld_rows mark , ld_cols mark, mark refers to the area identifier of the map loaded each time in the planned central rectangle. mark takes values of 0, 1, 2, 3. mark = 0 represents the planned central rectangle area where the starting point is located, mark = 1 represents the rectangle area where the destination is located, mark = 2 represents the rectangle area on the same left or right side as the starting point, and mark = 3 represents the area on the same right or left side as the end point.

[0039] If the total number of rows minus the number of rows or columns that have been loaded is less than the base increment value rows or cols each time, then the increment value of the number of rows / columns of the planned area rectangle is equal to the value of the total number of rows / columns minus the number of rows / columns that have been loaded, otherwise it still increases by rows or cols.

[0040] When mark = 0 and mark = 1, the calculation formula of l_rows mark is as follows:

[0041]

[0042] l_cols mark The calculation formula is as follows:

[0043]

[0044] When mark = 2 and mark = 3, the calculation formula of l_rows mark is as follows:

[0045]

[0046] l_cols mark The calculation formula is as follows:

[0047]

[0048] Step 3: Incremental loading path planning.

[0049] For the case where there is no path, load the map data of the corresponding area according to the mark value, and use the heuristic algorithm to calculate the feasible path. Specifically, the method is as follows:

[0050] First, perform grid modeling on the map according to Step 2, model the road network, assign the initial mark as 0, then load the corresponding area map according to the value of mark, and use the heuristic algorithm for calculation.

[0051] Step 3.1: When mark = 0, first load the geographical data within the rectangle centered at the starting point with side lengths ld_rows0 and ld_cols0. The loaded content includes all geographical information required for path planning (such as common elevation information, water system information, soil quality information, and road information, etc.). After the data loading is completed, use a heuristic path planning algorithm (such as A*, LPA* algorithm, etc.) to calculate the feasible path from the grid where the starting point is located to the grid where the ending point is located.

[0052] Different from the general path planning process, in the present invention, grids beyond the currently loaded rectangular range are considered impassable, and if the grid points to be updated exceed the range of the loaded rectangle, that grid will not be considered. The search ends until the priority queue of grid points to be expanded stored during the planning process is empty. In the present invention, the grid taken out from the priority queue is the expanded grid point, and the expanded grid and the grids in eight directions centered on the expanded grid are collectively referred to as the explored grid points. Update the values of ld_rows0 and ld_cols0 according to equations (10) and (11) in Step 2. If the search encounters the mark = 1 area, go to Step 7. Otherwise, assign mark as 1.

[0053] Step 3.2: When mark = 1, load the geographical data within the rectangle centered at the ending point with side lengths ld_rows1 and ld_cols1. Use a heuristic search algorithm to perform path planning from the ending point to the starting point within the rectangle.

[0054] Similarly, grids beyond the currently loaded rectangular range are not considered. If the search encounters the mark = 0 area, go to Step 7. Otherwise, assign mark as 0, update the values of ld_rows1 and ld_cols1 according to equations (10) and (11) in Step 2, and perform the next search.

[0055] Step 3.3: When there are obstacles in the mark = 0 and mark = 1 areas and no encounter occurs after all searches are completed, let mark = 2, with side lengths ld_rows2 and ld_cols2, and with the intersection row and column of the mark = 0 and mark = 1 rectangular areas as the boundary, load the geographical information of the mark = 2 area, and continue to explore from the starting grid point in the direction of the ending grid point. If the search encounters the mark = 1 area, go to Step 7. Otherwise, assign mark as 3, update the values of ld_rows2 and ld_cols2 according to equations (12) and (13) in Step 2, and perform the next search.

[0056] Step 3.4: When mark = 3, with the intersection rows and columns of the mark = 0 and mark = 1 regions as the boundary, and ld_rows3 and ld_cols3 as the side lengths, and the termination point on the same side of the intersection column. Search from the starting grid point in the direction of the termination grid point. If it encounters the mark = 1 region during the search, go to Step 7. Otherwise, assign mark as 2, and update the values of ld_rows3 and ld_cols3 according to equations (12) and (13) in Step 2. Conduct the next search.

[0057] If no encounter occurs after all grids have been searched, it is returned that there is no feasible path between the starting and ending points.

[0058] Thus, it can be seen that the present invention switches regions according to the planned situation after loading in different mark regions, so that only a small area of data is loaded each time, avoiding the problems of insufficient machine space caused by loading a large amount of data at one time or slow model calculation speed due to loading a large amount of data during the calculation process.

[0059] Step 4: For path planning in the coexistence of the wild and road networks.

[0060] Since the speed and safety of vehicles driving on the road network are higher than those in the wild area, therefore, it is assumed that when the vehicle encounters the road network during the search, it chooses to drive on the road.

[0061] When no road entry point is encountered, the pure wild path planning in Step 3 is adopted. When the road network is encountered during the planning, when the road entry point A is encountered during the search in the mark = 0 region, freeze this region, and search in the mark = 1 region. If the road entry point B is also encountered, then conduct path planning from A to B on the road network topology. If the road network planning fails (there is no connected road network between A and B), then find the road exit point closest to the target point, and restart the wild path planning based on the road exit point.

[0062] Step 5: Regarding the path symmetry problem in the planning, introduce a "cost preference value" in the LPA* algorithm to break the path symmetry, reduce the exploration of redundant grids, and improve the planning efficiency. Use the incremental search algorithm Lifelong Planning A* (LPA*) in the loaded area, reuse the information in the previous incremental loading process, avoid having to recalculate the path from the starting grid point when loading a new area, and improve the planning efficiency.

[0063] At the same time, introduce a "cost preference value" in the LPA* algorithm to break the path symmetry, reduce the exploration of redundant grids, further improve the planning efficiency, and using the cost preference value of the present invention can reduce the search for symmetric paths, break the symmetry of the search path, and improve the efficiency.

[0064] Specifically, it includes the following steps:

[0065] Step 5.1: Let g * (s) represent the start distance, that is, the actual shortest path from node s start to s. Once the start distances of all points are known, a shortest path from the starting point to the ending point can be traced by simply decreasing the start distance (similar to tracing the shortest distance from the target point to the starting point by iterating through the parent nodes in the A* algorithm). If the cost of some points changes, only calculate the g * (s) affected by the changed cost of that point. The start distances of the unaffected points do not need to be recalculated.

[0066]

[0067] Among them, pred(s) represents the set of all predecessor points of vertex s in the graph; c(s', s) represents the cost value from s' to s.

[0068] LPA* calculates the g value introduced by the A* algorithm (the estimated shortest distance from s start to s during the search process, which can be regarded as an estimated value of g * (s)). The g values of all vertices satisfy:

[0069]

[0070] In addition, LPA* also calculates a second estimate of the start distance g*(s): rhs(s), which satisfies the following relationship:

[0071]

[0072] If the g value of a point is the same as its rhs value, it is called locally consistent. If the g values of all vertices satisfy local consistency, then trace back to a shortest path from the starting point s start to any point u.

[0073] Step 5.2: Break path symmetry to further reduce redundant search. When planning the vehicle path, the heuristic function h(s) is usually taken as the distance between the node and the target point. On the basis of the original heuristic value h(s), a straight-line trajectory deviation evaluation value sh is added. The value of sh is affected by the straight-line distance from the current point s to the starting point to the ending point s end . Adding sh to the heuristic value h makes the search process more inclined to select the points near the straight line from the starting point to the ending point, reducing the search for symmetric paths, thereby improving the search efficiency.

[0074] The calculation method of the heuristic function h(s) is as follows:

[0075] cross = |(s.x - send .x)*(s start .y - s end .y)-(s start .x - s end .x)*(s.y - s end .y)| (17)

[0076]

[0077]

[0078]

[0079] sh = d * tri(21)

[0080] h = h old + sh(22)

[0081] Wherein, x and y respectively represent the abscissa and ordinate of point s; cross is the vector from the starting point to the target point The cross product between the current point and the target vector d represents the perpendicular distance from the current point s to the straight line where the starting point and the ending point are located, tri is the sum of the distances from the current point to the starting point and the ending point minus the distance from the starting point to the ending point. The closer the point s is to the straight line connecting the starting point and the ending point, the smaller this value is. h old is the heuristic value obtained according to the distance between the node and the ending point. sh is calculated from d and tri and is the evaluation value of the deviation of the straight-line trajectory. |a| represents the Euclidean distance between the starting point and the ending point.

[0082] After adding the straight-line trajectory deviation value sh, it can make the points closer to the middle connection line between the starting point and the ending point in the priority queue have higher priority, thus achieving the goal of breaking the symmetric path, reducing the redundant search for symmetric paths, further narrowing the search range required by the algorithm, and improving the algorithm efficiency.

[0083] Step 6: For the road network break situation in the complex map with both wild areas and road networks. In practical applications, due to poor map quality, no-go areas, etc., the road network planning between two starting points often fails. The solution of the present invention is as follows.

[0084] Step 6.1: Determine whether the road network is broken. First, perform wild path planning from the starting point and the ending point according to Steps 2 to 5 respectively to find the starting points. Let the starting points close to the starting point and the ending point be a1 and b1 respectively, and then perform road network planning between a1 and b1.

[0085] The A* algorithm is used on the adjacency list structure converted from road network data. Since using the Euclidean distance is not suitable for estimating the heuristic value in the road network planning stage, in the present invention, the distance estimation from a vertex to the termination point on the road network is calculated using the SPFA-BR (Shortest Path Faster Algorithm - Broken Road) algorithm, which adds the function of judging road network breakage and the function of searching for the best detour point on the basis of the classic SPFA (Shortest Path Faster Algorithm). The distance from point to point adopts the true value of the road segment distance in the road network layer of the map. When the road network planning for a1 and b1 fails, it means there is a broken road, and the process transfers to the broken road handling.

[0086] Step 6.2: Search for detour points. The principle for selecting detour points on the termination point side is to choose the point b2 that is the closest to the starting point in the road network planning as the detour point. The principle for selecting detour points on the starting point side is to choose the point a2 that is the closest to the point b2 in the road network planning as the detour point.

[0087] Step 6.3: Obtain the broken road network planning result. Keep the path planning result from the starting point to the point a2 as path1, and the path planning result from the point b2 to the termination point as path2. Use the A* algorithm to plan a path path3 in the wild area between a2 and b2. The final planning result path = path1 ∪ path2 ∪ path3.

[0088] Step 7: Organize the path points of the two-way planning into the final path and output the planning result.

[0089] Beneficial effects

[0090] The method of the present invention has the following advantages compared with the prior art:

[0091] 1. The present invention adopts an incremental method when loading map data, avoiding loading all map information into the memory space at one time, greatly reducing the geographical information processing time. In the path planning stage after loading geographical information, a heuristic search algorithm is used for path planning, effectively shortening the planning time and significantly improving the search efficiency. Experiments prove that this method can effectively shorten the planning time.

[0092] 2. This method proposes an effective solution to problems such as road network breakage caused by missing map data or task planning requirements. It can judge whether there is a break in the road network. If there is a break, for the road network starting from the starting point, the point closest to the end point is selected as the detour point, and for the road network planned in the reverse direction starting from the end point, the point closest to the starting point is selected as the detour point. Finally, it ensures that a feasible path can be planned even when there is a break in the road network. Brief description of the drawings

[0093] Figure 1 Schematic diagram of rasterized geographic information modeling for the method of the present invention;

[0094] Figure 2 Flow chart of the method of the present invention in a specific implementation example;

[0095] Figure 3 Schematic diagram of the first loading of map information in the area where mark = 0 in the method of the present invention in a specific implementation example;

[0096] Figure 4 Schematic diagram after the first loading of the area where mark = 1 in the method of the present invention in a specific implementation example;

[0097] Figure 5 Schematic diagram of successful path planning for incremental map loading in a specific implementation example;

[0098] Figure 6 Schematic diagram of map information loading in the areas where mark = 2 and mark = 3 in a specific implementation example;

[0099] Figure 7 Schematic diagram of successful path planning after incremental loading of the areas where mark = 2 and mark = 3 in a specific implementation example;

[0100] Figure 8 Schematic diagram of encountering a road network in a specific implementation example. Specific implementation manner

[0101] The following further elaborates on the method of the present invention in detail in conjunction with the accompanying drawings and embodiments.

[0102] Embodiment

[0103] This embodiment describes the specific implementation process of a vehicle path planning method in a complex environment of the present invention, Figure 2 Schematic diagram of the implementation process of this embodiment.

[0104] From Figure 2 it can be seen that the specific implementation steps of the present invention and this embodiment are as follows:

[0105] Step 1: After selecting the starting point and the ending point, determine a rectangle based on the starting and ending points, and expand it outward by N meters with this rectangle as the reference to obtain a planned central rectangle that encloses the reference rectangle. Perform geographic information modeling on the planned central rectangle.

[0106] Step 1.1: Calculate the rasterized side length according to Equation (1) to rasterize the entire area.

[0107] Step 1.2: Read and save all road network information within the area.

[0108] Step 1.3: As Figure 3 shown, initialize mark = 0, read and save the road network, water system, and soil information within the area.

[0109] Step 2: In the case of not encountering a road network, incrementally load the terrain and perform heuristic search.

[0110] Step 2.1: Load the terrain information of the corresponding area according to the mark value. After the calculation is completed when mark = 0 (the upper left part of the entire area), switch to mark = 1 (the lower right part of the entire area) for calculation, as Figure 4 shown.

[0111] Step 2.2: Taking Figure 5 as an example, the area where the starting point is located is mark = 0, and the area where the ending point is located is mark = 1. Symmetrically load and search the areas of mark = 0 and 1, use the LPA* algorithm (or heuristic path planning algorithms such as the A* algorithm) and calculate the heuristic information f = g + h of the loaded points using equations (16) and (22), and plan the loaded area according to the heuristic information. Update the grid area loaded each time according to equations (10) and (11) until a meeting point appears in the two areas, then the path planning is successful. As Figure 5 shown. The white grids in the figure are unloaded grids, the light gray grids represent the grids explored during planning, the black grids represent the grids not explored during heuristic search, and the white dotted lines represent the planned paths.

[0112] Step 2.3: In the case of encountering an obstacle, taking Figure 6 as an example, where the area where the starting point is located is mark = 0, the area where the ending point is located is mark = 1, the area on the same side as the starting point is mark = 2 ( Figure 6 is the lower left area grid in the figure), and the area on the same side as the ending point is mark = 3 ( Figure 6 is the upper right area grid in the figure). When the areas of mark = 0 and 1 are loaded and there is no meeting (such as blocked by the black grid area marked as impassable in Figure 6 ), symmetrically load and search the areas of mark = 2 and 3, update the grid area loaded each time according to equations (12) and (13), until a point that can connect the starting and ending point areas appears in the two areas, then the path planning is successful. As Figure 7 shown.

[0113] Step 3: When encountering an on-road point, switch to road network planning.

[0114] Step 3.1: As Figure 8As shown in the figure. When the upper road point P1 is encountered during the search in the mark = 0 area, the mark = 0 area is frozen, and geographical information is continuously loaded and searched in the mark = 1 rectangular area until the grid P3 with a road also exists is encountered, and the search in the mark = 1 area is stopped. The A* algorithm is used to plan the path between P1 and P3 on the road network topology. Finally, the path from the starting point to P1, the road network path between P1 and P3, and the path from P3 to the end point together form a passable path from the starting point to the end point.

[0115] Step 3.2: When the road network planning fails, that is, there is no connected road network between the upper road point P1 and P3 in Step 3.1, appropriate lower road points Q1 and Q3 are found according to P1 and P3 respectively, and then the pure wild path planning is carried out using the wild path planning algorithm in Step 2 with Q1 and Q3 as the starting and ending points respectively. Finally, the path planning result from the starting point to point Q1 is used as path1, the path planning result from point Q3 to the end point is used as path2, and a path path3 is planned in the wild area between Q1 and Q3. The final planning result path = path1 ∪ path2 ∪ path3.

[0116] According to Steps 1 to 3, the incremental loading and search planning of the vehicle path in a complex environment can be completed. Through the method in the present invention, the number of grids that need to load the terrain is reduced, and the planning time is greatly shortened. The search for symmetric roads is further reduced by breaking the path symmetry, which also improves the planning efficiency. In addition, using the road-breaking solution proposed in the present invention can also successfully solve the common road-breaking problem in path planning.

Claims

1. A vehicle path planning method in a complex environment, characterized in that It includes the following steps: Step 1: Select the starting point and ending point of the path; Based on the starting and ending points, a rectangle is planned. Taking this rectangle as a reference, it is expanded outward to obtain a planned central rectangle that wraps the reference rectangle. Then, geographic information modeling is performed on the planned central rectangle; Step 2: Adopt an incremental loading path planning algorithm to improve the vehicle path planning efficiency; After determining the planned central rectangle, i.e., the entire planned area, divide the planned central rectangle into sp parts, define the loading area identifier as mark, and initialize the number of planned rows and columns in each area to ld_rows mark and ld_cols mark respectively to 0; define the number of rows and columns added to the incremental loading map in each map loading round as rows and cols; Among them, the calculation methods of rows and cols are as follows: rows = [ROWS / sp] cols = [COLS / sp] Among them, ROWS is the total number of rows of the planned central rectangle, COLS is the total number of columns of the planned central rectangle, and [] is the ceiling symbol; In each incremental loading of a new round, the number of planned rows and columns in each area is ld_rows mark and ld_cols mark , where mark refers to the area identifier of the planned central rectangle of the map loaded each time; mark takes values of 0, 1, 2, and 3. mark = 0 represents the planned central rectangle area where the starting point is located, mark = 1 represents the rectangular area where the destination is located, mark = 2 represents the rectangular area on the same left or right side as the starting point, and mark = 3 is the area on the same right or left side as the ending point; If the total number of rows minus the number of loaded rows or columns is less than the basic increment value rows or cols each time, the increment value of the number of rows / columns of the planned area rectangle this time is equal to the value of the total number of rows / columns minus the number of loaded rows / columns, otherwise it still increases according to rows or cols; When mark = 0 and mark = 1, ld_rows mark is calculated as follows: ld_cols mark The calculation formula is as follows: When mark = 2 and mark = 3, the calculation formula for ld_rows mark is as follows: ld_cols mark The calculation formula is as follows: Step 3: Incremental loading path planning; For the case where there is no path, load the map data of the corresponding area according to the mark value, and use a heuristic algorithm to calculate the feasible path: First, perform grid modeling on the map according to Step 2, model the road network, assign the initial mark as 0, then load the map of the corresponding area according to the value of mark, and use a heuristic algorithm for calculation; Step 3.1: When mark = 0, first load the geographic data within the rectangle with the starting point as the center and ld_rows0 and ld_cols0 as the side lengths; the loaded content includes all geographic information required for path planning; after the data loading is completed, use the heuristic path planning algorithm to calculate the feasible path from the grid where the starting point is located to the grid where the ending point is located; The grids exceeding the current loaded rectangle range will be considered impassable. If the grid points to be updated exceed the range of the loaded rectangle, this grid will not be considered until the priority queue of the grid points to be expanded stored during the planning process is empty, and this search ends; Take the grid from the priority queue as the expanded grid point. The expanded grid and the grids in eight directions centered on the expanded grid are collectively called the explored grid points; update the values of ld_rows0 and ld_cols0 through the method in Step 2; if it meets the mark = 1 area during the search, go to Step 7; otherwise, assign mark as 1; Step 3.2: When mark = 1, load the geographic data within the rectangle with the ending point as the center and ld_rows1 and ld_cols1 as the side lengths; use the heuristic search algorithm to perform path planning from the ending point to the starting point within the rectangle; Similarly, grids exceeding the current loaded rectangle range are not considered; if it meets the mark = 0 area during the search, go to Step 7, otherwise assign mark as 0, update the values of ld_rows1 and ld_cols1 according to the method in Step 2, and perform the next search; Step 3.3: When there are obstacles in the mark = 0 and 1 areas and no encounter occurs after all searches are completed, set mark = 2. With ld_rows2 and ld_cols2 as the side lengths and the intersection rows and columns of the mark = 0 and mark = 1 rectangular areas as the boundaries, load the geographical information of the mark = 2 area and continue to explore from the starting grid point in the direction of the ending grid point; if an encounter occurs with the mark = 1 area during the search, go to Step 7; otherwise, assign mark the value of 3, update the values of ld_rows2 and ld_cols2 according to the method in Step 2, and conduct the next search; Step 3.4: When mark = 3, with the intersection rows and columns of the mark = 0 and mark = 1 areas as the boundaries, with ld_rows3 and ld_cols3 as the side lengths, and on the same side of the intersection column as the ending point; continue to search from the starting grid point in the direction of the ending grid point; if an encounter occurs with the mark = 1 area during the search, go to Step 7; otherwise, assign mark the value of 2, update the values of ld_rows3 and ld_cols3 according to the method in Step 2; conduct the next search; If no encounter occurs after all grids are searched, it is returned that there is no feasible path between the starting and ending points; Step 4: For path planning in the coexistence of the wild and road networks; Since the speed and safety of vehicles driving on the road network are higher than those in the wild area, therefore, it is assumed that the vehicle chooses to drive on the road when encountering the road network during the search process; When no road entry point is encountered, the pure wild path planning in Step 3 is adopted; when the road network is encountered during the planning, when the road entry point A is encountered during the search in the mark = 0 area, freeze this area, search in the mark = 1 area, and if the road entry point B is also encountered, then conduct path planning from A to B on the road network topology; if the road network planning fails, that is, there is no connected road network between A and B, then find the road exit point closest to the target point and restart the wild path planning based on the road exit point; Step 5: For the path symmetry problem in the planning, use the incremental search algorithm LPA* in the loaded area, introduce the "cost preference value" in the LPA* algorithm, reuse the information in the previous incremental loading process, avoid the need to recalculate the path from the starting grid point when loading a new area, and improve the planning efficiency; Step 6: For the road network break situation in the complex map with the coexistence of the wild and road networks: Step 6.1: Determine whether the road network is broken; first, conduct wild path planning from the starting and ending points respectively according to Steps 2 to 5 to find the road entry points; let the road entry points close to the starting point and the ending point be a1 and b1 respectively, and then conduct road network planning between a1 and b1; Use the A* algorithm on the adjacency list structure converted from the road network data, and use the SPFA-BR algorithm to calculate the distance estimate from the vertex on the road network to the ending point. Add the function of judging the road network break and the function of searching for the best road exit point on the basis of the SPFA algorithm; the distance between points is the true value of the road segment distance in the road network layer of the map. When the road network planning for a1 and b1 fails, it means that there is a road break, and transfer to the road break handling; Step 6.2: Find the off-road points. The principle for selecting the off-road point on the termination point side is to choose the point b2 which is the closest to the starting point in the road network planning as the off-road point; the principle for selecting the off-road point on the starting point side is to choose the point a2 which is the closest to the point b2 in the road network planning as the off-road point; Step 6.3: Obtain the result of the broken road network planning; retain the path planning result from the starting point to the point a2 as path1, and the path planning result from the point b2 to the termination point as path2. Use the A* algorithm to plan a path path3 in the wild area between a2 and b2; the final planning result path = path1 ∪ path2 ∪ path3; Step 7: Organize the path points of the two-way planning into the final path and output the planning result.

2. The vehicle path planning method in a complex environment according to claim 1, characterized in that In Step 1, perform geographic information modeling on the planned central rectangle, including the following steps: Step 1.1: Perform geographic information modeling on the planned area; Adopt a rasterization processing method to divide the map into several small grids according to the set grid side length; among them, the side length \(l\) of a single grid grid is calculated by the following formula: Among them, l grid represents the side length of a single grid, S i represents the area of the polygon inside the region, and n represents the number of polygons in the region; The geographic information data of the map consists of multiple vector layers, and each layer stores a set of feature geographic information in the area respectively; divide the geographic information data into grids, and for the rasterized planned area, traverse the grids. When traversing, convert the row and column numbers into Gaussian coordinates, and obtain the required layer data with the help of the geographic system engine; Step 1.2: Process the soil layer information; For the information of soil layers, when planning the path, refer to the average driving speed of the vehicle on different soils. Taking the driving speed of the vehicle on flat sandy land as the benchmark, set the soil speed ratio u of grid u in the modeling stage ratio , which is calculated by the following formula: Among them, v soil is the vehicle driving speed corresponding to the current grid soil quality, and v Flatsand is the vehicle driving speed when the soil quality is flat sandy land; Step 1.3: Model the topological road network; Use the directed graph structure in graph theory to store the road network for modeling, and generate a directed graph that meets the planning requirements; construct the data in the map road vector layer into a topological model, which is expressed as the following formula: G = (V a , E a , W a ) V a = {v i | i ∈ 1, 2, 3..., n} W a = {w ij | i, j ∈ 1, 2, 3,..., n} E a = {e i | i ∈ 1, 2, 3,..., n} Among them, G represents the entire road topology graph, V a represents the start and end points of roads in the road network and the vertices abstracted from different road connection points, v i represents the i-th vertex; W a represents the cost value of the arc, w ij represents the cost value between the i-th and j-th nodes; E a is the edge set abstracted from the road, e i represents the i-th edge, and each element e in the set a is a road, which is composed of the intersections connected by the road and the attributes of the road, and is expressed as: e a = (S id , E id , Q a , P v ) Among them, S id is the starting point id of this road, and E id is the ending point id of this road; Q a is the attribute information of this road, including the road grade and length of this road, and P v is the set of positioning points of the road; After modeling the road network with a directed graph, store the vector road network data in the form of a graph structure and store the road network in the form of an adjacency list.

3. A vehicle path planning method in a complex environment according to claim 1, characterized in that, Step 5 includes the following steps: Step 5.1: Use g * (s) to represent the start distance, that is, the actual shortest path from node s start to s; once the start distances of all points are known, a shortest path from the starting point to the ending point can be traced by simply decreasing the start distances; if the costs of some points change, only calculate g * (s) affected by the change in the cost of that point, and the start distances of the unaffected points do not need to be recalculated; Among them, pred(s) represents the set of all predecessors of vertex s in the graph; c(s', s) represents the cost value from s' to s; LPA* calculates the g value introduced by the A* algorithm, and the g values of all vertices satisfy: In addition, LPA* also calculates the second estimate of the start distance g*(s): rhs(s), which satisfies the following relationship: If the g-value of a point is the same as its rhs-value, it is called locally consistent; if the g-values of all vertices satisfy local consistency, then backtrack to a shortest path from the starting point s start to any point u; Step 5.2: Break the path symmetry and further reduce redundant searches; when planning the vehicle path, the heuristic function h(s) is taken as the distance between the node and the target point. On the basis of the original heuristic value h(s), the straight-line trajectory deviation evaluation value sh is added. The value of sh is affected by the straight-line distance from the current point s to the starting point to the end point s end and is added to the heuristic value h, making the search process more inclined to select points near the straight line from the starting point to the end point and reducing the search for symmetric paths; The calculation method of the heuristic function h(s) is as follows: cross = |(s.x - s end .x) * (s start .y - s end .y) - (s start .x - s end .x) * (s.y - s end .y)| sh = d * tri h = h old + sh Among them, x and y respectively represent the abscissa and ordinate of point s; cross is the vector from the starting point to the target point and the cross product between the current point and the target vector d represents the perpendicular distance from the current point s to the straight line where the starting point and the ending point are located. tri is the sum of the distances from the current point to the starting point and the ending point minus the distance from the starting point to the ending point. The closer the point s is to the straight line connecting the starting point and the ending point, the smaller this value is. h old is the heuristic value obtained according to the distance between the node and the ending point. sh is calculated from d and tri and is the evaluation value of the deviation of the straight-line trajectory; |a| represents the Euclidean distance between the starting point and the ending point.

Images