Method for planning fastest advancing path of vehicle based on grid map

Abstract

The invention discloses a method for planning and calculating the fastest path of a vehicle based on a grid map, and relates to the technical field of vehicle path planning. According to the technical scheme, a longitudinal and transverse coordinate system is established through a grid map, calculation planning is conducted on a starting point grid, each grid of a starting point grid right wing and a starting point grid left wing and all lower six-section transverse grids from the starting point grid, and therefore all reachable grids starting from the starting point grid in the planning range are traversed, and the relation is recorded in a longitudinal and transverse table; starting from the coordinates of the central point of the target point grid, searching the coordinates of the central point of the parent grid of each planning point grid in the longitudinal and transverse table until the coordinates of the central point of the starting point grid to obtain a connecting line from the starting point, the central point of the starting point grid, the central point of the target point grid and the target point, and the connecting line is the fastest path of the vehicle from the starting point to the target point. According to the method, the fastest path can be searched globally in a complex environment, and the problem that local optimum is not always global optimum during path planning is solved.

Description

Technical Field

[0001] The present application relates to the field of path planning and autonomous driving technology, and in particular to a method for planning the fastest path for a vehicle based on a grid map. Background Art

[0002] When a vehicle receives a mission, it must start from the stop point and rush to reach the target point on time. When the vehicle encounters complex terrain environments such as roads, bridges, mountains, muddy land, waters, buildings, soil, vegetation, and obstacles or dangerous points whose positions, numbers, and areas cannot be controlled, it must start from the stop point and move to the target point. The shortest time for the vehicle to get from the stop point to the target point is calculated in order to plan the best departure time for the vehicle and, if necessary, to plan to remove obstacles or dangerous points along the way.

[0003] There are many algorithms for path planning, including heuristic algorithms based on graph theory, depth-first algorithm, breadth-first algorithm, D* algorithm, A* algorithm and probabilistic road map method, artificial potential field method based on mechanics, genetic algorithm based on bionics, ant colony algorithm and particle swarm algorithm. Among the existing algorithms, the algorithms based on graph theory are more commonly used for path planning, and the practicality and accuracy of other algorithms need to be improved. The D* algorithm is a heuristic algorithm commonly used for local planning paths when there are dynamic obstacles. The D* algorithm finds the shortest path and moves to the target node based on the cost from the known point of the local terrain to the target node, and then takes the target node as the new known point, and continues to make new local path planning according to the new terrain information until the destination is reached. The A* algorithm is based on the heuristic algorithm, constructs a cost function, takes into account the cost from the new node to the starting point and the cost to the destination, finds the target node from the known point, takes the target node as the new known point, and continues to make new local path planning until the destination is reached. Both the D* algorithm and the A* algorithm obtain the overall path planning through the shortest paths of local planning repeatedly. The result of the shortest path of the local planning is likely not the shortest path of the overall planning. The detour path after encountering obstacles is likely not as good as the overall planned path. The D* algorithm and the A* algorithm emphasize the local visit to neighboring points in sequence, and the node relationship of the local visit is not complicated. For finding a complex and large number of node grids that are suitable for any position, any area, any number, and multiple traffic characteristics under the condition of changing travel speed, it is easy to fall into the local optimum when searching for the target point in an overall and global manner and calculating and planning the shortest path. The existing path planning algorithms based on graph theory are not ideal for finding the target point in an overall, complex, and large number of node relationships. Summary of the invention

[0004] The present application provides a method for planning the fastest path for vehicles based on a grid map. Its technical purpose is to complete the vehicle path planning by searching for target points based on a grid map in a complex terrain where it is impossible to travel at a uniform speed and there are various traffic characteristics, for a large number of node environments with obstacles or dangerous points at any position, any area, and any number.

[0005] The above technical objectives of this application are achieved through the following technical solutions:

[0006] A method for planning the fastest path for a vehicle based on a grid map, comprising:

[0007] Step S1: Determine the planning scope of the grid map, divide the grid map into square grids, divide the square grid into a passable area, a passable area after obstacle removal and a termination area, and set the obstacle removal time of the passable area after obstacle removal; wherein, the side length a of the square grid selects the larger value of the distance between two vehicles when the vehicles are traveling in parallel and the vehicle length, and the four sides of the square grid are parallel to the x-axis and y-axis of the Gauss-Kruger coordinate system respectively; the planning scope is a circular area including the starting point and the target point, and the planning scope includes the passable area and the passable area after obstacle removal, the center of the circular area is the midpoint of the line connecting the starting point and the target point, and the radius of the circular area is r, r>r0, r0=((x1-x0) 2 +(y1-y0) 2 ) 0.5 *e / (e1*2), (x0, y0) represents the Gauss-Krüger coordinates of the starting point, (xl, y1) represents the Gauss-Krüger coordinates of the target point, e represents the speed of the vehicle on the ideal road condition on the horizontal plane; e1 represents the average speed of the vehicle under the complex factor environment within the planning range, where the complex factor environment considers the terrain environment data, vehicle characteristic data, attribute data of obstacles or dangerous points, and the distribution of obstacles or dangerous points;

[0008] Step S2: Establish a vertical and horizontal coordinate system according to the grid map, the vertical axis of the vertical and horizontal coordinate system is parallel to the x-axis of the Gauss-Krüger coordinate system, the horizontal axis is parallel to the y-axis of the Gauss-Krüger coordinate system, and the coordinate origin of the vertical and horizontal coordinate system is the midpoint of the line connecting the starting point to the target point; wherein, (v0, h0) represents the vertical and horizontal coordinates of the center point of the grid where the starting point is located, (v1, h1) represents the vertical and horizontal coordinates of the center point of the grid where the target point is located, (v, h) represents the vertical and horizontal coordinates of the center point of the planning point grid that is being calculated and is constantly changing, and (x, y) represents the Gauss-Krüger coordinates of the center point of the planning point grid;

[0009] Step S3: define the front grid, front grid right wing, front grid left wing, back grid, back grid right wing and back grid left wing of the next vertical grid of each planning point grid, define the order of the 6-segment horizontal grid, and define the first 3-segment horizontal grid; define the axis direction coefficient and the grid direction coefficient; define the variables and arrays representing the direct access characteristics, the number of grids, the obstacle clearance time, the traffic characteristics, the planning characteristics, the distance, the shortest time, the speed coefficient and the elevation; define the termination grid, the cross-boundary termination grid and the planning termination grid;

[0010] Step S4: starting from the starting grid, three sets of judgment logics are used to calculate and plan the starting grid, the right wing of the starting grid, and the left wing of the starting grid, and all the lower 1 vertical and 6 horizontal grids. If the planning conditions are met, the next planning grid is changed into a new planning point grid for calculation, and then the planning calculation is continued for all the lower 1 vertical and 6 horizontal grids of the new planning point grid according to the rules described in step S5. If the return conditions are met, it is returned until each reachable grid starting from the starting grid in the planning range is traversed; wherein the three sets of judgment logics include a first set of judgment logic, a second set of judgment logic, and a third set of judgment logic;

[0011] Step S5: After calculating each planning point grid during the traversal process, each grid in the lower 6-segment horizontal grid of the 6-segment horizontal grid is calculated and planned in the order of the 6-segment horizontal grid; wherein, for the next planning grid of each planning point grid, if it meets the planning conditions, the next planning grid is changed into a new planning point grid for calculation, and then the planning is continued according to the rules described in this step, and if it meets the return conditions, it returns;

[0012] Step S6: Starting from the target point grid center coordinates, find the parent grid center coordinates of each planning point grid in the vertical and horizontal tables until the starting point grid center coordinates, and obtain the line from the starting point, the starting point grid center point, to the target point grid center point, and the target point. This is the fastest path for the vehicle to travel from the starting point to the target point.

[0013] The beneficial effects of the present application are as follows: the present application describes a method for planning the fastest path for vehicles based on a grid map, establishes a vertical and horizontal coordinate system through a grid map, sets the vertical and horizontal coordinate origins, determines the planning range, divides the square grid, sets the traffic characteristics, the obstacle-clearing time of the passable area after the obstacle is cleared, the direct characteristics, the traffic characteristics, the planning characteristics, the distance, the shortest time, etc.; starting from the starting grid, designs 3 sets of judgment logic to calculate and plan each grid of the starting grid, the right wing of the starting grid, and the left wing of the starting grid, and all the lower 1 vertical and 6 horizontal grids, so as to traverse each reachable grid starting from the starting grid within the planning range; during the traversal process, After calculating each planning point grid, calculate and plan each grid in the next 6 horizontal grids of the 6 horizontal grids in the order of 6 horizontal grids; for the next planning grid of each planning point grid, if it meets the planning conditions, the next planning grid will be changed into a new planning point grid, and the planning will continue after calculation, and it will return if it meets the return conditions; starting from the coordinates of the center point of the target point grid, find the coordinates of the center point of the parent grid of each planning point grid in the vertical and horizontal tables until the coordinates of the center point of the starting grid, and get the line from the starting point, the center point of the starting grid, to the center point of the target grid, and the target point, which is the fastest path for the vehicle to travel from the starting point to the target point. This method can globally find the fastest path under complex road conditions, solving the problem that the local optimum is not necessarily the overall optimum during path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 A schematic diagram of the fastest path for a vehicle to travel from a starting point to a target point based on a grid map in an embodiment of the present application;

[0015] Figure 2 This is a calculation planning flow chart of the starting point grid in the embodiment of the present application;

[0016] Figure 3 This is a flow chart of the return of the planning point grid according to the return conditions in the embodiment of the present application. DETAILED DESCRIPTION

[0017] The technical solution of the present application will be described in detail below with reference to the accompanying drawings.

[0018] When a vehicle receives a mission, it needs to travel from the stop point to the target point. The route may have various complex landforms such as roads, bridges, mountains, muddy land, waters, buildings, soil, vegetation, etc., and there may be obstacles or dangerous points set by humans. The vertical and horizontal algorithm is suitable for various complex landforms such as roads, bridges, mountains, muddy land, waters, buildings, soil, vegetation, and environments with obstacles or dangerous points at any position, any area, and any number. It calculates and plans the fastest path for the vehicle.

[0019] The present application discloses a method for planning the fastest path for a vehicle based on a grid map, comprising:

[0020] Step S1: Determine the planning scope of the grid map, divide the grid map into square grids, divide the square grid into a passable area, a passable area after obstacle removal and a termination area, and set the obstacle removal time of the passable area after obstacle removal; wherein, the side length a of the square grid is selected from the larger value of the distance between two vehicles when the vehicles are traveling in parallel and the vehicle length, and the four sides of the square grid are parallel to the x-axis and y-axis of the Gauss-Kruger coordinate system respectively; the planning scope is a circular area including the starting point and the target point, and the planning scope includes the passable area and the passable area after obstacle removal, the center of the circular area is the midpoint of the line connecting the starting point and the target point, and the radius of the circular area is r, and r is selected to be an appropriate value greater than r0, that is, r>r0, r0=((x1-x0) 2 +(y1-y0) 2 ) 0.5 *e / (e1*2), (x0, y0) represents the Gauss-Krüger coordinates of the starting point, (xl, y1) represents the Gauss-Krüger coordinates of the target point, e represents the speed of the vehicle on an ideal road on the horizontal plane; e1 represents the average speed of the vehicle under a complex factor environment within the planning range, wherein the complex factor environment takes into account the terrain environment data, vehicle characteristic data, attribute data of obstacles or dangerous points, and the distribution of obstacles or dangerous points.

[0021] Step S2: Establish a vertical and horizontal coordinate system according to the grid map, the vertical axis (v axis) of the vertical and horizontal coordinate system is parallel to the x axis of the Gauss-Krüger coordinate system, and the horizontal axis (h axis) is parallel to the y axis of the Gauss-Krüger coordinate system. The origin of the coordinate system is the midpoint of the line connecting the starting point to the target point; wherein, (v0, h0) represents the vertical and horizontal coordinates of the center point of the grid where the starting point is located, (v1, h1) represents the vertical and horizontal coordinates of the center point of the grid where the target point is located, (v, h) represents the vertical and horizontal coordinates of the center point of the planning point grid that is constantly changing and being calculated, and (x, y) represents the Gauss-Krüger coordinates of the center point of the planning point grid.

[0022] Specifically, (v0,h0) represents the vertical and horizontal coordinates of the center point of the grid where the starting point is located, and

[0023] v0=ceil((x0-x1) / 2 / a-0.5), h0=cei1((y0-y1) / 2 / a-0.5); (v1, h1) represent the vertical and horizontal coordinates of the center point of the grid where the target point is located, and v1=(ceil((x1-x0) / 2 / a-0.5), h1=ceil((y1-y0) / 2 / a-0.5).

[0024] (v,h) represents the vertical and horizontal coordinates of the center point of the planning point grid (referring to the constantly changing planning point grid being calculated); (x,y) represents the Gauss-Krüger coordinates of the center point of the planning point grid.

[0025] Step S3: define the front grid, front grid right wing, front grid left wing, back grid, back grid right wing and back grid left wing of the next vertical grid and the next 6-segment horizontal grid, define the order of the 6-segment horizontal grid, and define the first 3-segment horizontal grid; define the axial direction coefficient and the grid direction coefficient; define the variables and arrays representing direct access characteristics, number of grids, obstacle clearance time, traffic characteristics, planning characteristics, distance, shortest time, speed coefficient and elevation; define the termination grid, out-of-bounds termination grid and planning termination grid.

[0026] In the present application, the lower 1 vertical 6-segment horizontal grid of the planning point grid includes the front grid, the right wing of the front grid, the left wing of the front grid, the back grid, the right wing of the back grid and the left wing of the back grid of the planning point grid. Different from the usual algorithm, which is divided into 8 directions of up, down, left, right, upper right, upper left, lower right and lower left, the present application merges all connected and passable grids in the 6 directions of right, left, upper right, upper left, lower right and lower left into the right wing and left wing of the planning point grid, the front grid and the back grid, respectively, and merges the left and right directions of adjacent grids to simplify the calculation process and improve the calculation speed. At the same time, the 4 wings of the right wing of the front grid, the left wing of the front grid, the right wing of the back grid and the left wing of the back grid and the upper 1 vertical parent grid can form a local path in the diagonal direction of the rectangle, and the local path can be directly reached by the upper 1 vertical parent grid, and the shortest time for the direct path in the diagonal direction of the rectangle is better.

[0027] In this application, if the planning point grid is located in the positive direction, if its adjacent grid has been planned, the recalculated shortest time and the original shortest time should be compared to take the minimum value. In this application, the horizontally passable or passable grids after clearing obstacles of the longitudinally adjacent grids of the planning point grid are divided into front (back) grids, left wing, and right wing. Calculate and plan each grid of the front (back) grid and its left wing and right wing, and design nested loops in the sub-logic to calculate and plan from the adjacent grids of the front (back) grid to both sides in turn, to ensure that the planned grids are no longer recalculated and recorded as much as possible before continuing to calculate the plan, so as to simplify the calculation process.

[0028] In this application, the center of the circular area of ​​the planning range of the fastest path is the midpoint of the line connecting the starting point to the target point, and the midpoint of the line connecting the starting point to the target point is the origin of the coordinates of the vertical and horizontal coordinate systems. In this way, when the grid calculation of each planning point needs to determine whether it is within the planning range, it can avoid using the angle function every time the conversion is performed, reducing the difficulty of calculation while improving the calculation accuracy. At the same time, the symmetrical distribution of the planning areas of the starting point and the target point can simplify the planning range.

[0029] In this application, there are two ways to deal with obstacles or dangerous points. One is that the vehicle passes through the obstacle area or dangerous point area without the help of other obstacle-breaking vehicles, and the obstacles or dangerous points are broken while passing through, and the speed coefficient is used for adjustment; the other is that the vehicle needs to use other obstacle-breaking vehicles to break the obstacles or dangerous points that affect the crossing when passing through the obstacle area or dangerous point area. When the vehicle reaches the boundary of the obstacle area or dangerous point area, it needs to wait for other obstacle-breaking vehicles to break the obstacles or dangerous points, and the obstacle-breaking time is used for adjustment. The time for other obstacle-breaking vehicles to break the obstacles or dangerous points needs to be converted into the obstacle-breaking time that the vehicle needs to wait for the help of other obstacle-breaking vehicles to break the obstacles or dangerous points that affect the crossing; the third is that the obstacles or dangerous points that affect the crossing cannot be broken or are not broken, the grid in this area belongs to the termination grid, and the local logic returns.

[0030] Specifically, b represents the longitudinal axis direction coefficient. If x0 is less than or equal to x1, b=1, the adjacent grid of the planning point grid along the positive direction of the longitudinal axis is the front grid, and the adjacent grid of the planning point grid along the reverse direction of the longitudinal axis is the back grid; if x0 is greater than x1, b=-1, the adjacent grid of the planning point grid along the reverse direction of the longitudinal axis is the front grid, and the adjacent grid of the planning point grid along the positive direction of the longitudinal axis is the back grid.

[0031] c represents the horizontal axis direction coefficient. If y0 is less than or equal to y1, then c=1, the adjacent horizontal grids of the planning point grid along the positive direction of the horizontal axis are the right-wing adjacent horizontal grids, and the adjacent horizontal grids of the planning point grid along the negative direction of the horizontal axis are the left-wing adjacent horizontal grids; if y0 is greater than y1, then c=-1, the adjacent horizontal grids of the planning point grid along the negative direction of the horizontal axis are the right-wing adjacent horizontal grids, and the adjacent horizontal grids of the planning point grid along the positive direction of the horizontal axis are the left-wing adjacent horizontal grids.

[0032] The front grid and the back grid in the lower 1 vertical and 6 horizontal grids of the planning point grid are collectively referred to as the surface grid.

[0033] The right wing of the surface grid is represented by the adjacent horizontal grid on the right wing of the surface grid to the passable grid in front of the termination grid or the passable grid after clearing obstacles; the left wing of the surface grid is represented by the adjacent horizontal grid on the left wing of the surface grid to the passable grid in front of the termination grid or the passable grid after clearing obstacles.

[0034] The 1 vertical and 6 horizontal grids under the planning point grid include the front grid, the front grid right wing, the front grid left wing, the back grid, the back grid right wing and the back grid left wing under the planning point grid.

[0035] The 4 wings of the lower 1 vertical 6 horizontal grid of the planning point grid include the front grid right wing, the front grid left wing, the back grid right wing and the back grid left wing; each grid in the 4 wings is collectively referred to as a wing grid.

[0036] The order of the next 1 vertical 6 horizontal grids of the planning calculation planning point grid is: front grid, the adjacent horizontal grid on the right wing of the front grid to the farthest grid on its right wing, the adjacent horizontal grid on the left wing of the front grid to the farthest grid on its left wing, back grid, the adjacent horizontal grid on the right wing of the back grid to the farthest grid on its right wing, and the adjacent horizontal grid on the left wing of the back grid until the farthest grid on its left wing.

[0037] The first three sections of horizontal grids include the starting grid, the right wing of the starting grid and the left wing of the starting grid.

[0038] The variable i represents the number of verticals, the variable k represents the direct access characteristics of the grid in the wing grid, which takes into account the parent grid of the planning point and the obstacle removal in the direct access process. The variables f and g both represent the grid direction coefficients. The array variable j[i] represents the total number of grids in the wing of the i-th vertical 6-segment horizontal grid. d[i] represents the number of grids in the wing being planned for the i-th vertical grid being calculated. The array variables m[i] and m[v,h] represent the obstacle removal time that the vehicle being planned needs to wait for to cross the i-th vertical 6-segment horizontal grid with the help of other obstacle removal vehicles to remove obstacles or dangerous points that affect the crossing. The two-dimensional The array variable p[v,h] represents the traffic characteristics of the planning point grid, the two-dimensional array variable q[v,h] represents the planning characteristics of the planning point grid, the two-dimensional array variable s[v,h] represents the distance from the starting point to the center of the planning point grid, the two-dimensional array variable t[v,h] represents the shortest time from the starting point to the center of the planning point grid, the variable t0 represents the shortest planned time for the center of the planning point grid, the two-dimensional array variable n[v,h] represents the speed coefficient of the planning point grid, and the two-dimensional array variable z[v,h] represents the elevation of the planning point grid.

[0039] The grid direction coefficient of the front grid is f=1, and the grid direction coefficient of the back grid is f=-1; the grid direction coefficient of the right wing of the surface grid is g=1, and the grid direction coefficient of the left wing of the surface grid is g=-1.

[0040] The traffic characteristics of the planning point grid are determined based on the different geomorphic environmental data such as roads, bridges, mountains, muddy land, waters, buildings, soil, vegetation, slope, and slope direction at the location of the planning point grid within the planning scope, the vehicle characteristic data such as climbing ability, wading ability, minimum ground clearance, approach angle, and departure angle, the attribute data of obstacles or dangerous points, and whether the obstacles or dangerous points can be removed. The traffic characteristics of the grid located in the passable area are p[v,h]=1, the traffic characteristics of the grid located in the passable area after the obstacle is removed are p[v,h]=2, and the traffic characteristics of the grid located in the termination area are p[v,h]=0.

[0041] The planning characteristic q[v,h]=0 indicates that the planning characteristic of the planning point grid is unplanned; the planning characteristic q[v,h]=1 indicates that the planning characteristic of the planning point grid is planned.

[0042] If the planned point parent grid of the grid in the 4-segment wing is the planned point grid of the previous plan in the same vertical column, then k = 0; if the planned point parent grid of the grid in the 4-segment wing is the parent grid of the previous vertical column and there is no obstacle-breaking time during the direct reach process, then k = 1; if the planned point parent grid of the grid in the wing is the parent grid of the previous vertical column and there is an obstacle-breaking time during the direct reach process, then k = 2.

[0043] If the central point coordinates (v, h) of the planned point grid satisfy ((v * a) 2 + (h * a) 2 ) 0.5 < r, then the planned point grid is within the planned range; if the central point coordinates (v, h) of the planned point grid satisfy ((v * a) 2 + (h * a) 2 ) 0.5 ≥ r, then the planned point grid is at the boundary of the planned range or beyond the planned range, and this planned point grid is an out-of-bounds termination grid.

[0044] For the front grid of the 6-segment horizontal grid, the grids in the right wing of the front grid, and the grids in the left wing of the front grid, if the recalculated shortest time of their central points is not less than the already planned shortest time of the central point of this grid, or for the grids in the back grid of the 6-segment horizontal grid, the right wing of the back grid, and the left wing of the back grid are already planned grids, then these already planned grids are all planned termination grids.

[0045] The out-of-bounds termination grid and the planned termination grid are both termination grids.

[0046] Step S4: Starting from the starting grid, perform calculation and planning on each grid of the starting grid, the right wing of the starting grid, and the left wing of the starting grid and all the 6-segment horizontal grids in the next vertical column through 3 sets of judgment logics. If the planning conditions are met, change the next planned grid to a new planned point grid for calculation, and then continue to perform planning calculations on all the 6-segment horizontal grids in the next vertical column of this new planned point grid according to the rules described in step S5. If the return conditions are met, return until all reachable grids starting from the starting grid within the planned range are traversed; among them, the 3 sets of judgment logics include the first set of judgment logic, the second set of judgment logic, and the third set of judgment logic.

[0047] As Figure 2 shown, the first set of judgment logic performs calculation and planning on each grid of the starting grid, the 6-segment horizontal grids in the next vertical column of the starting grid, and all the 6-segment horizontal grids in all its sub-vertical columns, specifically including:

[0048] Take the starting point grid as the planning point grid, then v=v0, h=h0, i=0, d[0]=0, s[v,h]=0, t[v,h]=0; perform planning calculations on each grid of the 6-segment horizontal grid under the starting grid and all 6-segment horizontal grids of all its sub-vertical sections according to the rules described in step S5. If the planning conditions are met, the next planning grid is changed to a new planning point grid for calculation. Then, according to the rules described in step S5, continue to perform planning calculations on all the 6-segment horizontal grids under the new planning point grid. If the return conditions are met, return until all calculation plans for each grid of the 6-segment horizontal grid under the starting grid and all 6-segment horizontal grids of all its sub-vertical sections are completed, and return to judging the adjacent horizontal grid on the right wing of the starting grid.

[0049] The second set of judgment logic makes judgments and plans on the right wing of the starting grid, including:

[0050] The horizontal grid adjacent to the right wing of the starting grid to the farthest horizontal grid on the right wing is judged and calculated in sequence. A row is inserted into the vertical and horizontal table. The vertical and horizontal coordinates (v, h) of the center point of the parent grid on the planning point are recorded in the parent grid coordinate field. h+=g*c, d[0]+=1, and each grid from the horizontal grid adjacent to the right wing of the starting grid to the farthest horizontal grid on the right wing of the starting grid is calculated as a new planning point grid in sequence. s[v,h]=s[v,(hg*c)]+(a 2 +(z[v,h]-z[v,(hg*c)]) 2 ) 0.5 , t[v,h]=t[v,(hg*c)]+(a 2 +(z[v,h]-z[v,(hg*c)]) 2 ) 0.5 / (n[v,h]*e), q[v,h]=1, record the vertical and horizontal coordinates (v,h) of the new planning point grid center point in the planning point grid coordinate field of the inserted row in the vertical and horizontal table, and perform planning calculations on each grid in the lower 6-segment horizontal grid of each grid on the right wing of the starting grid and all 6-segment horizontal grids of all its sub-vertical grids according to the rules described in step S5. If the planning conditions are met, the next planning grid is changed to the new planning point grid for calculation, and then the planning calculations on all the lower 1 vertical and 6-segment horizontal grids of the new planning point grid are continued according to the rules described in step S5. If the return conditions are met, return until all the calculations and planning of each grid in the lower 1 vertical and 6-segment horizontal grid of each grid on the right wing of the starting grid and all 6-segment horizontal grids of all its sub-vertical grids are completed, and return to the judgment of the adjacent horizontal grid on the left wing of the starting grid.

[0051] The third set of judgment logic makes judgments and plans on the left wing of the starting grid, including:

[0052] The horizontal grid adjacent to the left wing of the starting grid to the farthest horizontal grid on the left wing is judged and calculated in turn. A row is inserted into the vertical and horizontal table. The vertical and horizontal coordinates (v, h) of the center point of the parent grid on the planning point are recorded in the parent grid coordinate field. h+=g*c, d[0]+=1, and each grid from the horizontal grid adjacent to the left wing of the starting grid to the farthest horizontal grid on the left wing of the starting grid is calculated as a new planning point grid in turn. s[v,h]=s[v,(hg*c)]+(a 2 +(z[v,h]-z[v,(hg*c)]) 2 ) 0.5 , t[v,h]=t[v,(hg*c)]+(a 2 +(z[v,h]-z[v,(hg*c)]) 2 ) 0.5 / (n[v,h]*e), q[v,h]=1, record the vertical and horizontal coordinates (v,h) of the new planning point grid center point in the planning point grid coordinate field of the inserted row in the vertical and horizontal table, and calculate and plan each grid in the 6-segment horizontal grid under the left wing of each grid of the starting grid and all 6-segment horizontal grids of all its sub-vertical sections according to the rules described in step S5. If the planning conditions are met, the next planning grid is changed to the new planning point grid for calculation, and then the planning calculation is continued for all the 6-segment horizontal grid under the new planning point grid according to the rules described in step S5. If the return conditions are met, return until all the calculations and plans of each grid in the 6-segment horizontal grid under the left wing of each grid of the starting grid and all 6-segment horizontal grids of all its sub-vertical sections are completed, and the traversal and calculation of the arrival time and distance of all reachable grids starting from the starting point within the planning range are obtained.

[0053] in addition, Figure 2In the formula, 1 indicates that the front grid of the next 6-segment horizontal grid of the planning point grid of the first 3-segment horizontal grid is a passable unplanned grid, a passable unplanned grid after clearing obstacles, or a planned grid whose newly calculated shortest time is less than the original shortest time, or the back grid of the next 6-segment horizontal grid is a passable unplanned grid, a passable unplanned grid after clearing obstacles. 2 indicates that the front grid of the next 6-segment horizontal grid of the planning point grid of the first 3-segment horizontal grid is a passable unplanned grid other than the target point grid, a passable unplanned grid after clearing obstacles, or a planned grid whose newly calculated shortest time is less than the original shortest time, or the back grid of the next 6-segment horizontal grid is a passable unplanned grid other than the target point grid, a passable unplanned grid after clearing obstacles. 3 means that after all calculations and planning are completed for a certain grid in the 6-segment horizontal grid below the first vertical planning point grid, the grid and all its sub-grids are returned. 4 means that if the d[0]th grid on the wing of the first 3-segment horizontal grid is the target point grid, the target point grid is calculated and planned before returning, and the 6-segment horizontal grid below it is no longer calculated and planned. 5 means that if the d[0]th grid on the wing of the first 3-segment horizontal grid is the termination grid or the termination grid has been planned, the d[0]th grid and all its 6-segment horizontal grids below it are calculated and planned before returning, and the (d[0]+1)th grid and the 6-segment horizontal grid below it are no longer calculated and planned.

[0054] Step S5: After calculating each planning point grid during the traversal process, each grid in the lower 6-segment horizontal grid of the 6-segment horizontal grid is calculated and planned in the order of the 6-segment horizontal grid; among them, for the next planning grid of each planning point grid, if it meets the planning conditions, the next planning grid is changed into a new planning point grid for calculation, and then the planning is continued according to the rules of this step, and if the return conditions are met, it is returned.

[0055] After calculating each planning point grid during the traversal process, each grid in the lower 6-segment horizontal grid of the 6-segment horizontal grid is calculated and planned in the order of the 6-segment horizontal grid, specifically including:

[0056] The next level sub-loop for calculating the 6-segment horizontal grid is set, that is, i+=1; first calculate the front grid of the 6-segment horizontal grid, and after calculation, plan and calculate all 6-segment horizontal grids of all sub-verticals of the front grid. After all calculations and plans are completed, return to determine the adjacent horizontal grid on the right wing of the front grid; a mosaic loop is used to calculate and plan each grid from the adjacent horizontal grid on the right wing of the front grid to the farthest grid on the right wing. After calculation of each grid in the right wing of the front grid, all 6-segment horizontal grids of all its sub-verticals are calculated respectively. After all calculations and plans are completed, return to determine the adjacent horizontal grid on the left wing of the front grid; a mosaic loop is used to calculate and plan each grid from the adjacent horizontal grid on the left wing of the front grid to the farthest grid on the left wing. After calculation of each grid in the left wing of the front grid, all 6-segment horizontal grids of all its sub-verticals are calculated respectively. After all calculations and planning are completed, return to judging the back grid; calculate the back grid, and after calculation, plan and calculate all 6 segments of horizontal grids of all sub-verticals of the back grid. After all calculations and planning are completed, return to judging the adjacent horizontal grids on the right wing of the back grid; inlay loop, calculate and plan each grid in the right wing of the back grid adjacent horizontal grid to the farthest grid on the right wing, and after calculating each grid in the right wing of the back grid, plan and calculate all 6 segments of horizontal grids of all its sub-verticals. After all calculations and planning are completed, return to judging the adjacent horizontal grids on the left wing of the back grid; inlay loop, calculate and plan each grid in the left wing of the back grid adjacent horizontal grid to the farthest grid on the left wing, and after calculating each grid in the left wing of the back grid, plan and calculate all 6 segments of horizontal grids of all its sub-verticals. After all calculations and planning are completed, i-=1 returns;

[0057] The distance and shortest time of the center point of the planning point grid are recorded through two-dimensional array variables, and the coordinates of the center point of the planning point grid and the center point of the planning point parent grid are recorded through the vertical and horizontal tables; if the next planning grid of the planning point grid is an unplanned passable grid or a passable grid after clearing obstacles, the next planning grid of the planning point grid becomes a new planning point grid, and the shortest time and distance of the center point of the planning point grid are calculated according to the shortest time, distance, grid side length, vehicle speed, planning point grid speed coefficient, and vertical and horizontal relationship of the planning point parent grid, and the planning characteristics of the planning point grid are set to planned; for the front grid and the front grid If the planning characteristic of the next planning grid in the planning point grid is planned, and the shortest time for the center point of the next planning grid to be recalculated is less than the shortest time for the center point of the next planning grid to be planned, the information of the next planning grid is replaced by the recalculated value, and the corresponding record in the vertical and horizontal table is updated with the replanned information, and the next planning grid continues to calculate and plan all its sub-vertical and all 6 horizontal grids; if the next planning grid of the planning point grid is the target point grid, the target point grid is calculated and recorded and returned; if the next planning grid of the planning point grid is the termination grid, it is returned directly.

[0058] The rules for calculating and planning the surface grid in the 6-segment horizontal grid include:

[0059] (1) After the calculation and planning of the last grid of the front grid, the front grid right wing, and the front grid left wing in the 6-segment horizontal grid are completed, then v- = f * b, that is, return to the back grid for calculation and planning.

[0060] (2) When planning and calculating a certain surface grid in the 6-segment horizontal grid, the vertical and horizontal coordinates of the center point of the original planned point grid are (v, h), and the vertical and horizontal coordinates of the center point of the surface grid are expressed as ((v + f * b), h), d[i] = 0. According to the geomorphic environment data, vehicle characteristic data, attribute data of obstacles or dangerous points, and information on whether the obstacles or dangerous points can be broken at the position of the vertical and horizontal coordinates ((v + f * b), h) of the center point of the surface grid, obtain the trafficability characteristic p[(v + f * b), h] of the surface grid; if the vehicle needs to cross obstacles or dangerous points that need to be broken by other obstacle-breaking vehicles, obtain the obstacle-breaking time m[(v + f * b), h] of the surface grid.

[0061] (3) If both (((v + f * b) * a) 2 +(h * a) 2 ) 0.5 < r, p[(v + f * b), h] = 1, q[(v + f * b), h] = 0, or both (((v + f * b) * a) 2 +(h * a) 2 ) 0.5 < r, p[(v + f * b), h] = 2, p[v, h] = 2, q[(v + f * b), h] = 0, then the parent grid of the planned point is the original planned point grid. Insert a row in the vertical and horizontal table, record the vertical and horizontal coordinates (v, h) of the center point of the original planned point grid in the coordinate field of the parent grid of the planned point, use the surface grid as the new planned point grid, v+ = f * b, k = 1, q[v, h] = 1, s[v, h] = s[(v - f * b), h]+(a 2 +(z[v, h]-z[(v - f * b), h]) 2 ) 0.5 , t[v, h] = t[(v - f * b), h]+(a 2 +(z[v, h]-z[(v - f * b), h]) 2 ) 0.5 / (n[v, h]*e), record the vertical and horizontal coordinates (v, h) of the center point of the new planned point grid in the coordinate field of the planned point grid of the inserted row in the vertical and horizontal table, and continue to calculate and plan all the lower 1 vertical and 6-segment horizontal grids of the new planned point grid according to the rules described in step S5.

[0062] (4) If the front grid simultaneously satisfies (((v + f * b) * + a)2 +(h * a) 2 ) 0.5 <r, p[(v + f * b), h] = 1, q[(v + f * b), h] = 1, or both satisfy (((v + f * b) * a) 2 +(h * a) 2 ) 0.5 <r, p[(v + f * b), h] = 2, p[v, h] = 2, q[(v + f * b), h] = 1, then t0 = t[(v + f * b), h],

[0063] t[(v + f * b), h] = t[v, h] + (a 2 +(z[(v + f * b), h] - z[v, h]) 2 ) 0.5 / (n[v, h] * e), if t[(v + f * b), h] < t0, then the planned point parent grid is the original planned point grid. Find the corresponding record of the vertical and horizontal coordinates ((v + f * b), h) of the center point of the surface grid in the planned point grid coordinate field of the vertical and horizontal table. Update the planned point parent grid coordinate field of this record with the original planned point grid center point vertical and horizontal coordinates (v, h). Take the surface grid as the new planned point grid, k = 1, v += f * b, s[v, h] = s[(v - f * b), h] + (a 2 +(z[v, h] - z[(v - f * b), h]) 2 ) 0.5 , and continue the planning calculation for all the lower 1 vertical and 6 horizontal grids of the new planned point grid according to the rules described in step S5.

[0064] (5) If both satisfy ((v + f * b) * a) 2 +(h * a) 2 ) 0.5 <r, p[(v + f * b), h] = 2, p[v, h] = 1, q[(v + f * b), h] = 0, then the planned point parent grid is the original planned point grid. Insert a row in the vertical and horizontal table, record the original planned point grid center point vertical and horizontal coordinates (v, h) in the planned point parent grid coordinate field. Take the surface grid as the new planned point grid, v += f * b, q[v, h] = 1, k = 2, m[i] = m[v, h],

[0065] s[v, h] = s[(v - f * b), h] + (a 2 +(z[v, h] - z[(v - f * b), h]) 2 ) 0.5 ,

[0066] t[v, h] = m[i] + t[(v - f * b), h] + (a 2+(z[v,h] - z[(v - f*b),h]) 2 ) 0.5 / (n[v,h]*e), the horizontal and vertical coordinates (v, h) of the center point of the surface grid are recorded in the planned point grid coordinate field of the above-inserted row in the horizontal and vertical table. Continue to perform planning calculations on all the lower 1 vertical and 6 horizontal grids of the new planned point grid according to the rules described in step S5.

[0067] (6) If the front grid simultaneously satisfies (((v + f*b)*a) 2 +(h*a) 2 ) 0.5 <r, p[(v + f*b),h] = 2, q[(v + f*b),h] = 1, p[v,h] = 1, then t0 = t[(v + f*b),h],

[0068] t[(v + f*b),h] = m[(v + f*b),h] + t[v,h] + (a 2 +(z[(v + f*b),h] - z[v,h]) 2 ) 0.5 / (n[v,h]*e); if t[(v + f*b),h] < t0, then the planned point parent grid is the original planned point grid. Find the corresponding record of the horizontal and vertical coordinates ((v + f*b),h) of the surface grid center point in the planned point grid coordinate field of the horizontal and vertical table. Update the planned point parent grid coordinate field of this record with the original planned point grid center point horizontal and vertical coordinates (v, h). Take the surface grid as the new planned point grid, k = 2, v += f*b, s[v,h] = s[(v - f*b),h] + (a 2 +(z[v,h] - z[(v - f*b),h]) 2 ) 0.5 , m[i] = m[v,h]. Continue to perform planning calculations on all the lower 1 vertical and 6 horizontal grids of the new planned point grid according to the rules described in step S5.

[0069] (7) If the vertical and horizontal coordinates of the center point of the 6-segment horizontal grid satisfy v = ceil((x1-x0) / 2 / a-0.5)-f*b and h = ceil((yl-y0) / 2 / a-0.5), then the surface grid is the target point grid, and the surface grid satisfies q[v+f*b,h] = 0 and has not been planned, or the surface grid satisfies q[v+f*b,h] = 1 and has been planned and is a front grid and the recalculated shortest time is small, then v+=b takes the target point grid as the new planning point grid, calculates the distance, shortest time and process variables of the center point of the target point grid, records the new planning point grid coordinates and the original planning point grid coordinates in the vertical and horizontal tables, and then returns to the back grid of the 1 vertical and 6-segment horizontal grid under the planning point grid, and no longer plans to calculate the 6 vertical and 6-segment horizontal grid under the front grid, nor plans to calculate the right wing and left wing of the front grid.

[0070] (8) When continuing to plan and calculate all the next 6 horizontal grids of the new planning point grid, i+=1, the level goes to the next vertical, and each grid in the 6 horizontal grids is calculated in the order of the 6 horizontal grids, and the 4 wings in the 6 horizontal grids are planned and calculated; for the next planning grid of each planning point grid, if it meets the planning conditions, the next planning grid is changed to the new planning point grid, and the planning is continued after calculation. If it meets the return condition, i-=1, and it is returned according to the return condition; until it returns to the vertical where the 6 horizontal grids are located, it returns to the next planning grid for judging the order of the 6 horizontal grids.

[0071] (9) If the surface grid belongs to the termination grid, then the surface grid, the right wing of the surface grid and the left wing of the surface grid are no longer planned and calculated; if the surface grid is the front grid, return to determine the back grid of the 6-segment horizontal grid; if the surface grid is the back grid, return to determine the upper 1 vertical parent grid of the 6-segment horizontal grid.

[0072] The rules for calculating the planning for each grid in the 4 wings of the 6-segment horizontal grid include:

[0073] (1) After the calculation and planning of the last grid of the right wing among the four wings is completed, h-=c*d[i] is required, that is, returning to the left wing of the opposite grid for judgment.

[0074] (2) When planning and calculating each grid of a wing in the four-segment wing, the loop corresponding to the wing is embedded in the sub-loop for calculating the six-segment horizontal grids. Starting from the corresponding surface grid, each grid passing through the wing-adjacent horizontal grid of the surface grid until the grid with the farthest surface distance from the surface grid is used as the planning point grid in turn, and its wing-adjacent horizontal grid is calculated.

[0075] (3) The horizontal and vertical coordinates of the center point of the planned point grid are (v, h), and the horizontal and vertical coordinates of the center point of the adjacent horizontal grid of the planned point grid wing are (v, (h + g * c)). According to the geomorphic environment data, vehicle characteristic data, attribute data of obstacles or dangerous points, and information on whether the obstacles or dangerous points can be broken at the position where the horizontal and vertical coordinates (v, (h + g * c)) of the center point of the adjacent horizontal grid of the planned point grid wing are located, the passing characteristic p[v, (h + g * c)] of the adjacent horizontal grid of the planned point grid wing is obtained; if the vehicle needs to cross an obstacle or dangerous point that needs to be broken by other obstacle-breaking vehicles, the obstacle-breaking time m[v, (h + g * c)] of the surface grid is obtained.

[0076] (4) If the adjacent horizontal grid of the planned point grid of the 6-segment horizontal grid satisfies v = ceil((x1 - x0) / 2 / a - 0.5) and h = ceil((yl - y0) / 2 / a - 0.5) - g * c, then this adjacent grid is the target point grid. If this target point grid satisfies q[v, (h + g * c)] = 0 and has not been planned, or this target point grid satisfies q[v, (h + g * c)] = 1 and has been planned and belongs to the wing of the positive grid and the recalculated shortest time is smaller, then this target point grid is used as the new planned point grid, and the distance, shortest time, and process variables of the center point of this target point grid are calculated. The coordinates of this target point grid and the coordinates of the parent grid of this target point grid are recorded in the horizontal and vertical table, and then return.

[0077] (5) If both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, k = l, p[(v - f * b), (h + g * c)] = 1, p[v, (h + g * c)] = 1, q[v, (h + g * c)] = 0, or both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, k = 1, p[v, (h + g * c)] = 2, p[v, h] = 2, p[(v - f * b), (h + g * c)] = 2, p[(v - f * b), h] = 2, q[v, (h + g * c)] = 0, then the parent grid of the planned point is the upper 1 vertical parent grid of the planned point. Insert a row in the horizontal and vertical table, record the horizontal and vertical coordinates ((v - f * b), (h - g * c * d[i])) of the center point of the upper 1 vertical parent grid of the planned point in the parent grid coordinate field of the planned point, use the adjacent horizontal grid of the planned point grid wing as the new planned point grid, h += g * c, d[i] += 1, q[v, h] = 1, s[v, h] = s[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2+(z[v,h] - z[(v - f*b),(h - g*c*d[i])]) 2 ) 0.5 , t[v,h] = t[(v - f*b),(h - g*c*d[i])] + (a 2 +(d[i]*a) 2 +(z[v,h] - z[(v - f*b),(h - g*c*d[i])]) 2 ) 0.5 / (n[v,h]*e), the grid coordinates of the planned point grid field in the vertical and horizontal table record the vertical and horizontal coordinates (v,h) of the center point of the new planned point grid. Continue to perform the planning calculation on all the lower 1 vertical and 6 horizontal grids of the new planned point grid according to the rules described in step S5.

[0078] (6) If the right wing of the front grid or the left wing of the front grid simultaneously satisfies ((v*a) 2 +((h + g*c)*a) 2 ) 0.5 <r, k = 1, p[v,(h + g*c)] = 1, q[v,(h + g*c)] = 1, p[(v - f*b),(h + g*c)] = 1, or simultaneously satisfies ((v*a) 2 +((h + g*c)*a) 2 ) 0.5 <r, k = 1, q[v,(h + g*c)] = 1, p[v,(h + g*c)] = 2, p[v,h] = 2, p[(v - f*b),(h + g*c)] = 2, p[(v - f*b),h] = 2, then t0 = t[v,(h + g*c)], t[v,(h + g*c)] = t[(v - f*b),(h - g*c*d[i])] + (a 2 +((d[i] + 1)*a) 2 +(z[v,(h + g*c)] - z[(v - f*b),(h - g*c*d[i])]) 2 ) 0.5 / (n[v,h]*e). If t[v,(h + g*c)] < t0, then the parent grid of the planned point is the upper 1 vertical parent grid of the planned point. Find the corresponding record of the vertical and horizontal coordinates (v,(h + g*c)) of the center point of the adjacent horizontal grid of the planned point grid wing in the planned point grid coordinate field of the vertical and horizontal table. The planned point parent grid coordinate field of this record is updated with the vertical and horizontal coordinates ((v - f*b),(h - g*c*d[i])) of the upper 1 vertical parent grid center point of the planned point. Take the adjacent horizontal grid of the planned point grid wing as the new planned point grid, h += g*c, d[i] += 1,

[0079] s[v, h] = s[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2 + (z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 , continue the planning calculation for all the 1 lower row and 6 adjacent horizontal grids of the new planned point grid according to the rules described in step S5.

[0080] (7) If ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 < r, p[v, (h + g * c)] ≠ 0, p[(v - f * b), (h + g * c)] ≠ 0, q[v, (h + g * c)] = 0, k = 2 are simultaneously satisfied, then the parent grid of the planned point is the 1 upper row parent grid of the planned point. Insert a row in the vertical and horizontal table, record the vertical and horizontal coordinates ((v - f * b), (h - g * c * d[i])) of the center point of the 1 upper row parent grid of the planned point in the coordinate field of the parent grid of the planned point. Take the adjacent horizontal grids of the wing of the planned point grid as the new planned point grid, h += g * c, d[i] += 1, q[v, h] = 1,

[0081] s[v, h] = s[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2 + (z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 , t[v, h] = m[i] + t[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2 + (z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 / (n[v, h] * e), record the vertical and horizontal coordinates (v, h) of the center point of the new planned point grid in the coordinate field of the planned point grid of the inserted row in the vertical and horizontal table, and continue the planning calculation for all the 1 lower row and 6 adjacent horizontal grids of the new planned point grid according to the rules described in step S5.

[0082] (8) If ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 < r, p[v, (h + g * c)] = 2, q[v, (h + g * c)] = 0, k = 1, p[v, h] = 1, p[(v - f * b), (h + g * c)] ≠ 0 are simultaneously satisfied, or ((v * a)2 +((h + g * c) * a) 2 ) 0.5 * <r, p[v, (h + g * c)] = 2, q[v, (h + g * c)] = 0, k = l, p[(v - f * b), (h + g * c)] = 1, or simultaneously satisfy ((v * a) 2 +((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, q[v, (h + g * c)] = 0, k = 1, p[(v - f * b), h] = 1, then the planned point's parent grid is the upper 1 - vertical parent grid of the planned point. Insert a row in the vertical - horizontal table, record the vertical - horizontal coordinates of the center point of the upper 1 - vertical parent grid of the planned point ((v - f * b, (h - g * c * d[i])) in the planned point's parent grid coordinate field. Take the adjacent horizontal grids of the planned point grid's wing as the new planned point grid, h += g * c, d[i] += 1, m[i] = m[v, h], q[v, h] = 1, s[v, h] = s[(v - f * b), (h - g * c * d[i])] + (a 2 +(d[i] * a) 2 +(z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 , t[v, h] = m[i] + t[(v - f * b), (h - g * c * d[i])] + (a 2 +(d[i] * a) 2 +(z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 / (n[v, h] * e), k = 2. Record the vertical - horizontal coordinates (v, h) of the center point of the new planned point grid in the planned point grid coordinate field of the inserted row in the vertical - horizontal table above. Continue the planning calculation for all the lower 1 - vertical 6 - segment horizontal grids of the new planned point grid according to the rules described in step S5.

[0083] (9) If the right wing of the front grid or the left wing of the front grid simultaneously satisfy ((v * a) 2 +((h + g * c) * a) 2 ) 0.5 <r, k = 2, p[v, (h + g * c)] ≠ 0, p[(v - f * b), (h + g * c)] ≠ 0, q[v, (h + g * c)] = 1, then t0 = t[v, (h + g * c)], t[v, (h + g * c)] = t[(v - f * b), (h - g * c * d[i])] + (a 2 +((d[i] + 1) * a) 2+(z[v,(h + g*c)] - z[(v - f*b),(h - g*c*d[i])]) 2 ) 0.5 / (n[v,h]*e), if t[v,(h + g*c)] < t0, then the parent grid of the planned point is the upper 1 - vertical parent grid of the planned point. Find the corresponding record of the horizontal and vertical coordinates (v,(h + g*c)) of the center point of the adjacent horizontal grid of the planned point grid wing in the horizontal and vertical table. Update the parent grid coordinate field of this record with the horizontal and vertical coordinates ((v - f*b),(h - g*c*d[i])) of the center point of the upper 1 - vertical parent grid of the planned point. Take the adjacent horizontal grid of the planned point grid wing as the new planned point grid, h += g*c, d[i] += 1,

[0084] s[v,h] = s[(v - f*b),(h - g*c*d[i])]+(a 2 +(d[i]*a) 2 +(z[v,h] - z[(v - f*b),(h - g*c*d[i])]) 2 ) 0.5 , and continue the planning calculation for all the lower 1 - vertical 6 - segment horizontal grids of this new planned point grid according to the rules described in step S5.

[0085] (10) If the right wing or left wing of the front grid simultaneously satisfies ((v*a) 2 +((h + g*c)*a) 2 ) 0.5 < r, p[v,(h + g*c)] = 2, q[v,(h + g*c)] = 1, k = l, p[v,h] = 1, p[(v - f*b),(h + g*c)] ≠ 0, or simultaneously satisfies ((v*a) 2 +((h + g*c)*a) 2 ) 0.5 < r, p[v,(h + g*c)] = 2, q[v,(h + g*c)] = 1, k = 1, p[(v - f*b),(h + g*c)] = 1, or simultaneously satisfies ((v*a) 2 +((h + g*c)*a) 2 ) 0.5 < r, p[v,(h + g*c)] = 2, k = 1, q[v,(h + g*c)] = 1, p[(v - f*b),h] = 1, then t0 = t[v,(h + g*c)],

[0086] t[v,(h + g*c)] = m[v,(h + g*c)]+t[(v - f*b),(h - g*c*d[i])]+(a 2 +((d[i]+1)*a)2 +(z[v,(h + g*c)] - z[(v - f*b),(h - g*c*d[i])]) 2 ) 0.5 / (n[v,h]*e), if t[v,(h + g*c)] < t0, then the parent grid of the planned point is the upper 1 - vertical parent grid of the planned point. Find the corresponding record of the horizontal and vertical coordinates (v,(h + g*c)) of the center point of the adjacent horizontal grid of the planned point grid wing in the horizontal and vertical table's planned point grid coordinate field. Update the planned point parent grid coordinate field of this record with the horizontal and vertical coordinates ((v - f*b),(h - g*c*d[i])) of the center point of the upper 1 - vertical parent grid of the planned point. Take the adjacent horizontal grid of the planned point grid wing as the new planned point grid, h += g*c, d[i] += 1, k = 2, m[i] = m[v,h], s[v,h] = s[(v - f*b),(h - g*c*d[i])]+(a 2 +(d[i]*a) 2 +(z[v,h] - z[(v - f*b),(h - g*c*d[i])])), continue the planning calculation for all the lower 1 - vertical 6 - segment horizontal grids of this new planned point grid according to the rules described in step S5.

[0087] (11) If both ((v*a) 2 +((h + g*c)*a) 2 ) 0.5 < r, p[v,(h + g*c)] = 1, k = 0, q[v,(h + g*c)] = 0, or both ((v*a) 2 +((h + g*c)*a) 2 ) 0.5 < r, p[v,h] = 2, p[v,(h + g*c)] = 2, q[v,(h + g*c)] = 0, k = 0, or both ((v*a) 2 +((h + g*c)*a) 2 ) 0.5 < r, p[v,(h + g*c)] = 1, p[(v - f*b),(h + g*c)] = 0, q[v,(h + g*c)] = 0, or both ((v*a) 2 +((h + g*c)*a) 2 ) 0.5<r, p[v, (h + g * c)] = 2, p[v, h] = 2, p[(v - f * b), (h + g * c)] = 0, q[v, (h + g * c)] = 0, then the planning point's parent grid is the planning point grid. Insert a row in the vertical and horizontal table, record the vertical and horizontal coordinates (v, h) of the center point of the planning point grid in the coordinate field of the planning point's parent grid. Take the horizontally adjacent grid of the planning point grid's wing as the new planning point grid, h += g * c, k = 0, d[i] += 1, q[v, h] = 1, s[v, h] = s[v, (h - g * c)] + (a 2 +(z[v, h] - z[v, (h - g * c)]) 2 ) 0.5 , t[v, h] = t[v, (h - g * c)] + (a 2 +(z[v, h] - z[v, (h - g * c)]) 2 ) 0.5 / (n[v, h] * e). Record the vertical and horizontal coordinates (v, h) of the center point of the horizontally adjacent grid of the planning point grid in the coordinate field of the planning point grid in the above - inserted row in the vertical and horizontal table. Continue the planning calculation for all the 1 - vertical and 6 - horizontal grids below this new planning point grid according to the rules described in step S5.

[0088] (12) If the right wing of the front grid or the left wing of the front grid simultaneously satisfies ((v * a) 2 + ((h + g * c) * n) 2 ) 0.5 <r, p[v, (h + g * c)] = 1, q[v, (h + g * c)] = 1, k = 0, or simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, p[v, h] = 2, q[v, (h + g * c)] = 1, k = 0, or simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 1, p[(v - f * b), (h + g * c)] = 0, q[v, (h + g * c)] = 1, or simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, p[v, h] = 2, p[(v - f * b), (h + g * c)] = 0, q[v, (h + g * c)] = 1, then t0 = t[v, (h + g * c)],

[0089] t[v, (h + g * c)] = t[v, h] + (a2 +(z[v,(h + g*c)] - z[v,h]) 2 ) 0.5 / (n[v,h]*e), if

[0090] t[v,(h + g*c)] < t0, then the planned point's parent grid is the planned point grid. Find the corresponding record of the horizontal and vertical coordinates (v,(h + g*c)) of the center point of the adjacent horizontal grid of the planned point grid's wing in the horizontal and vertical table's planned point grid coordinate field. Update the planned point's parent grid coordinate field in this record with the horizontal and vertical coordinates (v,h) of the center point of the planned point grid. Take the adjacent horizontal grid of the planned point grid's wing as the new planned point grid, h += g*c, d[i] += 1, k = 0,

[0091] s[v,h] = s[v,(h - g*c)] + (a 2 +(z[v,h] - z[v,(h - g*c)]) 2 ) 0.5 , and continue the planning calculation for all the 1 lower vertical and 6 - segment horizontal grids of this new planned point grid according to the rules described in step S5.

[0092] (13) If both ((v*a) 2 +((h + g*c)*a) 2 ) 0.5 < r, p[v,(h + g*c)] = 2, p[v,h] = 1, q[v,(h + g*c)] = 0, k = 0, or both ((v*a) 2 *+((h + g*c)*a) 2 ) 0.5 < r, p[v,(h + g*c)] = 2, p[(v - f*b),(h + g*c)] = 0, q[v,(h + g*c)] = 0, p[v,h] = 1, then the planned point's parent grid is the planned point grid. Insert a row in the horizontal and vertical table, record the horizontal and vertical coordinates (v,h) of the center point of the planned point grid in the planned point's parent grid coordinate field. Take the adjacent horizontal grid of the planned point grid's wing as the new planned point grid, h += g*c, d[i] += 1, k = 0, q[v,h] = 1, s[v,h] = s[v,(h - g*c)] + (a 2 +(z[v,h] - z[v,(h - g*c)]) 2 ) 0.5 ,

[0093] t[v,h] = m[v,h] + t[v,(h - g*c)] + (a 2 +(z[v,h] - z[v,(h - g*c)]) 2 ) 0.5 / (n[v,h]*e), record the horizontal and vertical coordinates (v, h) of the center point of the new planned point grid in the planned point grid coordinate field of the above-inserted row in the vertical and horizontal table, and continue to perform planning calculations on all the lower 1 vertical and 6 horizontal grids of this new planned point grid according to the rules described in step S5.

[0094] (14) If the right wing of the front grid or the left wing of the front grid simultaneously satisfies ((v*a) 2 +((h+g*c)*a) 2 ) 0.5 <r, p[v,(h+g*c)] = 2, p[v,h] = 1, q[v,(h+g*c)] = 1, k = 0, or simultaneously satisfies ((v*a) 2 +((h+g*c)*a) 2 ) 0.5 <r, p[v,(h+g*c)] = 2, q[v,(h+g*c)] = 1, p[(v-f*b),(h+g*c)] = 0, p[v,h] = 1, then t0 = t[v,(h+g*c)],

[0095] t[v,(h+g*c)] = m[v,(h+g*c)] + t[v,h] + (a 2 +(z[v,(h+g*c)]-z[v,h]) 2 ) 0.5 / (n[v,h]*e), if t[v,(h+g*c)] < t0, then the planned point parent grid is the planned point grid. Find the corresponding record of the horizontal and vertical coordinates (v,(h+g*c)) of the center point of the adjacent horizontal grid of the planned point grid wing in the planned point grid coordinate field of the vertical and horizontal table. Update the planned point parent grid coordinate field of this record with the horizontal and vertical coordinates (v,h) of the center point of the planned point grid. Take the adjacent horizontal grid of the planned point grid wing as the new planned point grid, h += g*c, d[i] += 1, k = 0,

[0096] s[v,h] = s[v,(h-g*c)] + (a 2 +(z[v,h]-z[v,(h-g*c)]) 2 ) 0.5 , and continue to perform planning calculations on all the lower 1 vertical and 6 horizontal grids of this new planned point grid according to the rules described in step S5.

[0097] (15) When continuing to plan and calculate all the next 1 vertical and 6 horizontal grids of the new planning point grid, i+=1, the level reaches the next vertical, and each grid in the 6 horizontal grids is calculated in the order of the 6 horizontal grids, and the 4 wings in the 6 horizontal grids are planned and calculated; for the next planning grid of each planning point grid, if it meets the planning conditions, the next planning grid is changed into the new planning point grid, and the planning is continued after calculation. If it meets the return condition, i-=1, and it is returned according to the return condition; until it returns to the vertical where the 6 horizontal grids are located, it returns to the next planning grid for judging the order of the 6 horizontal grids;

[0098] (16) If the wing adjacent horizontal grid of the planning point grid belongs to the termination grid, it is returned according to the return condition.

[0099] like Figure 3 As shown, for the next planning grid of each planning point grid, the return rules that meet the return conditions include:

[0100] (1) When the planning point grid is located in the 6-segment horizontal grid, the position of the return point is in the order of the 6-segment horizontal grid; after all the grids of the front grid and all its sub-vertical grids are calculated and planned, it returns to determine the adjacent horizontal grids on the right wing of the front grid; after all the grids of the right wing of the front grid and all its sub-vertical grids are calculated and planned, each grid returns to determine the adjacent horizontal grids on the right wing of the front grid; after all the grids of the farthest grid in the right wing of the front grid and all its sub-vertical grids are calculated and planned, it returns to determine the adjacent horizontal grids on the left wing of the front grid; after all the grids of the farthest grid in the left wing of the front grid and all its sub-vertical grids are calculated and planned, it returns to determine the adjacent horizontal grids on the left wing of the front grid. ; After all the grids of the back grid and all its sub-vertical grids are calculated and planned, it returns to judging the adjacent horizontal grids on the right wing of the back grid; after all the grids of the back grid and all its sub-vertical grids are calculated and planned, each grid in the right wing of the back grid returns to judging the adjacent horizontal grids on the right wing, and after all the grids of the farthest grid in the right wing of the back grid and all its sub-vertical grids are calculated and planned, it returns to judging the adjacent horizontal grids on the left wing of the back grid; after all the grids of the farthest grid in the left wing of the back grid and all its sub-vertical grids are calculated and planned, the farthest grid in the left wing of the back grid is used as the last position of the 6-segment horizontal grid and returns to judging the next planned grid of the upper 1 vertical parent grid.

[0101] (2) If the front grid of the 6-segment horizontal grid is the target point grid, the right wing of the front grid and the left wing of the front grid will not be planned after calculation, and the process will return to the back grid judgment; if the back grid of the 6-segment horizontal grid is the target point grid, the right wing of the back grid and the left wing of the back grid will not be planned after calculation, and the process will return to the next planning grid of the upper 1 vertical parent grid; if the front grid of the 6-segment horizontal grid is the termination grid, the right wing of the front grid and the left wing of the front grid will not be planned after calculation, and the process will return to the back grid judgment; if the back grid of the 6-segment horizontal grid is the termination grid, the right wing of the front grid and the left wing of the front grid will not be planned after calculation, and the process will return to the back grid judgment; The grid is the termination grid, and the right wing and the left wing of the back grid are no longer planned, and the grid returns to the next planning grid of the upper vertical parent grid; if a wing of the 6-segment horizontal grid has no grid, the position of the return point is postponed and the wing is not considered; if the 6-segment horizontal grid has no left wing of the back grid, the farthest grid of the right wing of the back grid in the 6-segment horizontal grid is used as the last position of the 6-segment horizontal grid; if the 6-segment horizontal grid has no left wing and right wing of the back grid, the back grid in the 6-segment horizontal grid is used as the last position of the 6-segment horizontal grid, and so on according to the order of the 6-segment horizontal grid.

[0102] (3) When the upper vertical parent grid is not located at the last position of the six horizontal grids, the return point is the next planned grid of the upper vertical parent grid; when the upper vertical parent grid is located at the last position of the six horizontal grids, it returns to determine the next planned grid of the upper vertical parent grid.

[0103] (4) When the upper vertical parent grid of the upper vertical parent grid is not located at the last position of the six-segment horizontal grid, the position of the return point is the next planned grid of the upper vertical parent grid of the upper vertical parent grid; when the upper vertical parent grid of the upper vertical parent grid is located at the last position of the six-segment horizontal grid, return to determine the next planned grid of the upper vertical parent grid of the upper vertical parent grid, and so on.

[0104] (5) When the parent grid is located in the first vertical direction, and the parent grid located in the first vertical direction is not located at the last position of the three horizontal grids in the first vertical direction, the position of the return point is the next planned grid of the parent grid in the first vertical direction; when the parent grid is located at the last position of the three horizontal grids in the first vertical direction, the traversal and calculation of the arrival time and distance from the starting point to all reachable grids are completed.

[0105] in addition, Figure 3In the figure, 1 indicates that the grid judged in the 6-segment horizontal grid is a passable unplanned grid, a passable unplanned grid after removing obstacles, or a planned grid whose new calculated plan's shortest time is smaller than the original planned shortest time value. 2 indicates that the grid judged in the 6-segment horizontal grid is a passable unplanned grid other than the target point grid, a passable unplanned grid after removing obstacles, or a planned grid whose new calculated plan's shortest time is smaller than the original planned shortest time value. 3 indicates that the grid judged in the 6-segment horizontal grid is a passable unplanned grid other than the target point grid, and a passable unplanned grid after removing obstacles. 4 indicates that after all the next vertical grids of each grid of the next 1 vertical 6-segment horizontal grid and each grid of the next 1 vertical 6-segment horizontal grid are calculated and planned, the next planned grid is returned. 5 means that if the d[i]th grid of the wing of the 6-segment horizontal grid is the target grid, then the calculation and planning of the d[1]th grid is completed and the program returns, and the calculation and planning of the next 6-segment horizontal grid of the target grid is not continued, and the d[i]+1th grid of the wing is not determined. 6 means that the d[1]+1th grid of the wing of the 6-segment horizontal grid is the termination grid or the termination grid has been planned, then the calculation and planning of the d[i]th grid of the wing and all the next 6-segment horizontal grids are completed and the program returns.

[0106] Step S6: Starting from the target point grid center coordinates, find the parent grid center coordinates of each planning point grid in the vertical and horizontal tables until the starting point grid center coordinates, and obtain the line from the starting point, the starting point grid center point, to the target point grid center point, and the target point. This is the fastest path for the vehicle to travel from the starting point to the target point.

[0107] Specifically, Figure 1 As shown, green lines are used to mark the grid map. The coordinates of the target point grid center are (cei1((x1-x0) / 2 / a-0.5), ceil((y1-y0) / 2 / a-0.5)). The line connecting the target point and the target point grid center is drawn with a green line.

[0108] The vertical and horizontal coordinates of the center point of the target point grid are used as the grid coordinates of the planning point, and the record whose grid coordinate field of the planning point is the coordinates of the center point of the target point grid is found in the vertical and horizontal table to obtain the parent grid coordinates of the planning point of the record; the line connecting the center point of the target point grid and the center point of the parent grid of the planning point is drawn with a green line;

[0109] The parent grid coordinates of the planning point found are used as the new grid coordinates of the planning point, and the record whose grid coordinate field of the planning point is the new grid coordinates of the planning point is found in the vertical and horizontal tables to obtain the parent grid coordinates of the planning point of the record; the connection line of the grid center points obtained twice is drawn with green lines;

[0110] The parent grid coordinates of the planning point found last time are used as the new planning point grid coordinates in turn, and the record whose planning point grid coordinate field is the new planning point grid coordinates is found in the vertical and horizontal tables to obtain the parent grid coordinates of the planning point of the record; the connection line of the grid center points obtained twice is drawn with a green line;

[0111] Until the parent grid coordinates of the planning point found in the vertical and horizontal table are the coordinates of the center point of the starting grid (ceil((x0-x1) / 2 / a-0.5), cei1((y0-y1) / 2 / a-0.5))), use a green line to draw the connection between the last grid center point and the newly found starting grid center point, and then use a green line to draw the connection between the starting grid center point and the starting point (x0, y0);

[0112] The green line connecting the starting point (x0, y0) and the target point (x1, y1) is the fastest path for the vehicle to travel from the starting point (x0, y0) to the target point (x1, y1).

[0113] The above are exemplary embodiments of the present application, and the protection scope of the present application is achieved by the claims and their equivalents.

Claims

1. A method for planning the fastest path for a vehicle based on a grid map, characterized in that include: Step S1: Determine the planning scope of the grid map, divide the grid map into square grids, divide the square grid into a passable area, a passable area after obstacle removal and a termination area, and set the obstacle removal time of the passable area after obstacle removal; wherein, the side length a of the square grid selects the larger value of the distance between two vehicles when the vehicles are traveling in parallel and the vehicle length, and the four sides of the square grid are parallel to the x-axis and y-axis of the Gauss-Kruger coordinate system respectively; the planning scope is a circular area including the starting point and the target point, and the planning scope includes the passable area and the passable area after obstacle removal, the center of the circular area is the midpoint of the line connecting the starting point and the target point, and the radius of the circular area is r, r>r0, r0=((x1-x0) 2 +(y1-y0) 2 ) 0.5 *e / (e1*2), (x0, y0) represents the Gauss-Krüger coordinates of the starting point, (xl, y1) represents the Gauss-Krüger coordinates of the target point, e represents the speed of the vehicle on the ideal road condition on the horizontal plane; e1 represents the average speed of the vehicle under the complex factor environment within the planning range, where the complex factor environment considers the terrain environment data, vehicle characteristic data, attribute data of obstacles or dangerous points, and the distribution of obstacles or dangerous points; Step S2: Establish a vertical and horizontal coordinate system according to the grid map, the vertical axis of the vertical and horizontal coordinate system is parallel to the x-axis of the Gauss-Krüger coordinate system, the horizontal axis is parallel to the y-axis of the Gauss-Krüger coordinate system, and the coordinate origin of the vertical and horizontal coordinate system is the midpoint of the line connecting the starting point to the target point; wherein, (v0, h0) represents the vertical and horizontal coordinates of the center point of the grid where the starting point is located, (v1, h1) represents the vertical and horizontal coordinates of the center point of the grid where the target point is located, (v, h) represents the vertical and horizontal coordinates of the center point of the planning point grid that is being calculated and is constantly changing, and (x, y) represents the Gauss-Krüger coordinates of the center point of the planning point grid; Step S3: define the front grid, front grid right wing, front grid left wing, back grid, back grid right wing and back grid left wing of the next vertical grid of each planning point grid, define the order of the 6-segment horizontal grid, and define the first 3-segment horizontal grid; define the axis direction coefficient and the grid direction coefficient; define the variables and arrays representing the direct access characteristics, the number of grids, the obstacle clearance time, the traffic characteristics, the planning characteristics, the distance, the shortest time, the speed coefficient and the elevation; define the termination grid, the cross-boundary termination grid and the planning termination grid; Step S4: starting from the starting grid, three sets of judgment logics are used to calculate and plan the starting grid, the right wing of the starting grid, and the left wing of the starting grid, and all the lower 1 vertical and 6 horizontal grids. If the planning conditions are met, the next planning grid is changed into a new planning point grid for calculation, and then the planning calculation is continued for all the lower 1 vertical and 6 horizontal grids of the new planning point grid according to the rules of step S5. If the return conditions are met, it is returned until each reachable grid starting from the starting grid in the planning range is traversed; wherein the three sets of judgment logics include a first set of judgment logic, a second set of judgment logic, and a third set of judgment logic; Step S5: After calculating each planning point grid during the traversal process, each grid in the lower 6-segment horizontal grid of the 6-segment horizontal grid is calculated and planned in the order of the 6-segment horizontal grid; wherein, for the next planning grid of each planning point grid, if it meets the planning conditions, the next planning grid is changed into a new planning point grid for calculation, and then the planning is continued according to the rules described in this step, and if it meets the return conditions, it returns; Step S6: Starting from the target point grid center coordinates, find the parent grid center coordinates of each planning point grid in the vertical and horizontal tables until the starting point grid center coordinates, and obtain the line from the starting point, the starting point grid center point, to the target point grid center point, and the target point. This is the fastest path for the vehicle to travel from the starting point to the target point.

2. The method according to claim 1, characterized in that In step S2: (v0,h0) represents the vertical and horizontal coordinates of the center point of the grid where the starting point is located, and v0 = ceil((x0-x1) / 2 / a-0.5), h0 = cei1((y0-y1) / 2 / a-0.5); (v1,h1) represents the vertical and horizontal coordinates of the center point of the grid where the target point is located, and v1 = (ceil((x1-x0) / 2 / a-0.5), h1 = ceil((y1-y0) / 2 / a-0.5); (x,y) represents the Gauss-Krüger coordinates of the center point of the planning point grid.

3. The method according to claim 1, characterized in that In step S3: b represents the longitudinal direction coefficient. If x0 is less than or equal to x1, then b=1, the adjacent grid of the planning point grid along the positive direction of the longitudinal axis is the front grid, and the adjacent grid of the planning point grid along the reverse direction of the longitudinal axis is the back grid; if x0 is greater than x1, then b=-1, the adjacent grid of the planning point grid along the reverse direction of the longitudinal axis is the front grid, and the adjacent grid of the planning point grid along the positive direction of the longitudinal axis is the back grid; c represents the horizontal axis direction coefficient. If y0 is less than or equal to y1, then c=1, the adjacent horizontal grids of the planning point grid along the positive direction of the horizontal axis are the adjacent horizontal grids on the right wing, and the adjacent horizontal grids of the planning point grid along the negative direction of the horizontal axis are the adjacent horizontal grids on the left wing; if y0 is greater than y1, then c=-1, the adjacent horizontal grids of the planning point grid along the negative direction of the horizontal axis are the adjacent horizontal grids on the right wing, and the adjacent horizontal grids of the planning point grid along the positive direction of the horizontal axis are the adjacent horizontal grids on the left wing; The front grid and the back grid in the lower 1 vertical 6 horizontal grid of the planning point grid are collectively referred to as the surface grid; The right wing of the surface grid is represented by the adjacent horizontal grid on the right wing of the surface grid to the passable grid before the termination grid or the passable grid after the obstacle is removed; the left wing of the surface grid is represented by the adjacent horizontal grid on the left wing of the surface grid to the passable grid before the termination grid or the passable grid after the obstacle is removed. The 6-section horizontal grid under the planning point grid includes the front grid, the right wing of the front grid, the left wing of the front grid, the back grid, the right wing of the back grid and the left wing of the back grid under the planning point grid; The four wings of the lower 1 vertical and 6 horizontal grids of the planning point grid include the front grid right wing, the front grid left wing, the back grid right wing and the back grid left wing; each grid in the four wings is collectively referred to as a wing grid; The order of the next 1 vertical 6 horizontal grids of the planning calculation planning point grid is: front grid, the horizontal grid adjacent to the right wing of the front grid to the farthest grid on its right wing, the horizontal grid adjacent to the left wing of the front grid to the farthest grid on its left wing, the back grid, the horizontal grid adjacent to the right wing of the back grid to the farthest grid on its right wing, and the horizontal grid adjacent to the left wing of the back grid until the farthest grid on its left wing; The first three sections of horizontal grids include the starting grid, the right wing of the starting grid and the left wing of the starting grid; The variable i represents the number of verticals, the variable k represents the direct access characteristics of the grid in the wing grid, which takes into account the parent grid of the planning point and the obstacle removal in the direct access process. The variables f and g both represent the grid direction coefficients. The array variable j[i] represents the total number of grids in the wing of the i-th vertical 6-segment horizontal grid. d[i] represents the number of grids in the wing being planned for the i-th vertical grid being calculated. The array variables m[i] and m[v,h] represent the obstacle removal time that the vehicle being planned needs to wait for to cross the i-th vertical 6-segment horizontal grid with the help of other obstacle removal vehicles to remove obstacles or dangerous points that affect the crossing. The two-dimensional The array variable p[v,h] represents the traffic characteristics of the planning point grid, the two-dimensional array variable q[v,h] represents the planning characteristics of the planning point grid, the two-dimensional array variable s[v,h] represents the distance from the starting point to the center of the planning point grid, the two-dimensional array variable t[v,h] represents the shortest time from the starting point to the center of the planning point grid, the variable t0 represents the shortest planned time for the center of the planning point grid, the two-dimensional array variable n[v,h] represents the speed coefficient of the planning point grid, and the two-dimensional array variable z[v,h] represents the elevation of the planning point grid; The grid direction coefficient of the front grid is f=1, and the grid direction coefficient of the back grid is f=-1; the grid direction coefficient of the right wing of the surface grid is g=1, and the grid direction coefficient of the left wing of the surface grid is g=-1; The passability characteristic of the grid located in the passable area is p[v, h] = 1, the passability characteristic of the grid located in the passable area after the obstacle is removed is p[v, h] = 2, and the passability characteristic of the grid located in the termination area is p[v, h] = 0; Planning characteristic q[v,h]=0 means that the planning characteristic of the planning point grid is unplanned; planning characteristic q[v,h]=1 means that the planning characteristic of the planning point grid is planned; If the planning point parent grid of the grid in the 4-segment wing is the planning point grid of the previous planning in the same vertical direction, then k=0; if the planning point parent grid of the grid in the 4-segment wing is the parent grid of the previous vertical direction, and there is no obstacle-breaking time in the direct process, then k=1; if the planning point parent grid of the grid in the wing is the parent grid of the previous vertical direction, and there is obstacle-breaking time in the direct process, then k=2; If the central point coordinates (v, h) of the planned point grid satisfy ((v * a) 2 + (h * a) 2 ), 0.5 < r, then the planned point grid is within the planned range; if the central point coordinates (v, h) of the planned point grid satisfy ((v * a) 2 + (h * a) 2 ), 0.5 ≥ r, then the planned point grid is on the boundary of the planned range or outside the planned range, and this planned point grid is an out-of-bounds termination grid; The front grid, the grid in the right wing of the front grid and the grid in the left wing of the front grid of the 6-segment horizontal grid are the planned grids whose shortest time of center point recalculation is not less than the shortest time of center point of the grid planned, or the back grid, the grid in the right wing of the back grid and the left wing of the back grid of the 6-segment horizontal grid are the planned grids, then the planned grids are all the planned termination grids; The out-of-bounds termination grid and the planned termination grid are both termination grids.

4. The method according to claim 1, characterized in that In step S4: The first set of judgment logic calculates and plans the starting grid, each grid in the 6-segment horizontal grid under the starting grid, and all 6-segment horizontal grids of all its sub-vertical grids, including: The starting grid is used as the planning point grid, v=v0, h=h0, i=0, d[0]=0, s[v,h]=0, t[v,h]=0; according to the rule described in step S5, each grid of the 6-segment horizontal grid under the starting grid and all 6-segment horizontal grids of all its sub-vertical sections are planned and calculated. If the planning conditions are met, the next planning grid is changed into a new planning point grid for calculation, and then the planning calculation of all the 6-segment horizontal grids under the new planning point grid is continued according to the rule described in step S5. If the return conditions are met, return until all the calculation plans of each grid in the 6-segment horizontal grid under the starting grid and all 6-segment horizontal grids of all its sub-vertical sections are completed, and return to the judgment of the adjacent horizontal grid on the right wing of the starting grid; The second set of judgment logic makes judgments and plans on the right wing of the starting grid, including: The horizontal grid adjacent to the right wing of the starting grid to the farthest horizontal grid on the right wing is judged and calculated in sequence. A row is inserted into the vertical and horizontal table. The vertical and horizontal coordinates (v, h) of the center point of the parent grid on the planning point are recorded in the parent grid coordinate field. h+=g*c, d[0]+=1, and each grid from the horizontal grid adjacent to the right wing of the starting grid to the farthest horizontal grid on the right wing of the starting grid is calculated as a new planning point grid in sequence. s[v,h]=s[v,(hg*c)]+(a 2 +(z[v,h]-z[v,(hg*c)]) 2 ) 0.5 , t[v,h]=t[v,(hg*c)]+(a 2 +(z[v,h]-z[v,(hg*c)]) 2 ) 0.5 / (n[v,h]*e), q[v,h]=1, record the vertical and horizontal coordinates (v,h) of the center point of the new planning point grid in the planning point grid coordinate field of the inserted row in the vertical and horizontal table, and perform planning calculations on each grid in the lower 6-segment horizontal grid of each grid on the right wing of the starting grid and all 6-segment horizontal grids of all its sub-vertical grids according to the rules described in step S5. If the planning conditions are met, the next planning grid is changed into a new planning point grid for calculation, and then the planning calculations are continued for all the lower 1 vertical and 6-segment horizontal grids of the new planning point grid according to the rules described in step S5. If the return conditions are met, return until all the calculations and plannings of each grid in the lower 1 vertical and 6-segment horizontal grid of each grid on the right wing of the starting grid and all 6-segment horizontal grids of all its sub-vertical grids are completed; h-=d[0], return to determine the adjacent horizontal grid on the left wing of the starting grid; The third set of judgment logic makes judgments and plans on the left wing of the starting grid, including: The horizontal grid adjacent to the left wing of the starting grid to the farthest horizontal grid on the left wing is judged and calculated in turn. A row is inserted into the vertical and horizontal table. The vertical and horizontal coordinates (v, h) of the center point of the parent grid on the planning point are recorded in the parent grid coordinate field. h+=g*c, d[0]+=1, and each grid from the horizontal grid adjacent to the left wing of the starting grid to the farthest horizontal grid on the left wing of the starting grid is calculated as a new planning point grid in turn. s[v,h]=s[v,(hg*c)]+(a 2 +(z[v,h]-z[v,(hg*c)]) 2 ) 0.5 , t[v,h]=t[v,(hg*c)]+(a 2 +(z[v,h]-z[v,(hg*c)]) 2 ) 0.5 / (n[v,h]*e), q[v,h]=1, record the vertical and horizontal coordinates (v,h) of the new planning point grid center point in the planning point grid coordinate field of the inserted row in the vertical and horizontal table, and calculate and plan each grid in the 6-segment horizontal grid under the left wing of each grid of the starting grid and all 6-segment horizontal grids of all its sub-vertical sections according to the rules described in step S5. If the planning conditions are met, the next planning grid is changed to the new planning point grid for calculation, and then the planning calculation is continued for all the 6-segment horizontal grid under the new planning point grid according to the rules described in step S5. If the return conditions are met, return until all the calculations and plans of each grid in the 6-segment horizontal grid under the left wing of each grid of the starting grid and all 6-segment horizontal grids of all its sub-vertical sections are completed, and the traversal and calculation of the arrival time and distance of all reachable grids starting from the starting point within the planning range are obtained.

5. The method according to claim 1, characterized in that In the step S5, after calculating each planning point grid during the traversal process, each grid in the lower 6-segment horizontal grid of the 6-segment horizontal grid is calculated and planned in the order of the 6-segment horizontal grid, specifically including: The next level sub-loop for calculating the 6-segment horizontal grid is set, that is, i+=1; first calculate the front grid of the 6-segment horizontal grid, and after calculation, plan and calculate all 6-segment horizontal grids of all sub-verticals of the front grid. After all calculations and plans are completed, return to determine the adjacent horizontal grid on the right wing of the front grid; a mosaic loop is used to calculate and plan each grid from the adjacent horizontal grid on the right wing of the front grid to the farthest grid on the right wing. After calculation of each grid in the right wing of the front grid, all 6-segment horizontal grids of all its sub-verticals are calculated respectively. After all calculations and plans are completed, return to determine the adjacent horizontal grid on the left wing of the front grid; a mosaic loop is used to calculate and plan each grid from the adjacent horizontal grid on the left wing of the front grid to the farthest grid on the left wing. After calculation of each grid in the left wing of the front grid, all 6-segment horizontal grids of all its sub-verticals are calculated respectively. After all calculations and planning are completed, return to judging the back grid; calculate the back grid, and after calculation, plan and calculate all 6 segments of horizontal grids of all sub-verticals of the back grid. After all calculations and planning are completed, return to judging the adjacent horizontal grids on the right wing of the back grid; inlay loop, calculate and plan each grid in the right wing of the back grid adjacent horizontal grid to the farthest grid on the right wing, and after calculating each grid in the right wing of the back grid, plan and calculate all 6 segments of horizontal grids of all its sub-verticals. After all calculations and planning are completed, return to judging the adjacent horizontal grids on the left wing of the back grid; inlay loop, calculate and plan each grid in the left wing of the back grid adjacent horizontal grid to the farthest grid on the left wing, and after calculating each grid in the left wing of the back grid, plan and calculate all 6 segments of horizontal grids of all its sub-verticals. After all calculations and planning are completed, i-=1 returns; The distance and shortest time of the center point of the planning point grid are recorded through two-dimensional array variables, and the coordinates of the center point of the planning point grid and the center point of the planning point parent grid are recorded through the vertical and horizontal tables; if the next planning grid of the planning point grid is an unplanned passable grid or a passable grid after clearing obstacles, the next planning grid of the planning point grid becomes a new planning point grid, and the shortest time and distance of the center point of the planning point grid are calculated according to the shortest time, distance, grid side length, vehicle speed, planning point grid speed coefficient, and vertical and horizontal relationship of the planning point parent grid, and the planning characteristics of the planning point grid are set to planned; for the front grid and the front grid If the planning characteristic of the next planning grid in the planning point grid is planned, and the shortest time for the center point of the next planning grid to be recalculated is less than the shortest time for the center point of the next planning grid to be planned, the information of the next planning grid is replaced by the recalculated value, and the corresponding record in the vertical and horizontal table is updated with the replanned information, and the next planning grid continues to calculate and plan all its sub-vertical and all 6 horizontal grids; if the next planning grid of the planning point grid is the target point grid, the target point grid is calculated and recorded and returned; if the next planning grid of the planning point grid is the termination grid, it is returned directly.

6. The method according to claim 5, characterized in that When calculating and planning the surface grid in the 6-segment horizontal grid, it specifically includes: When the calculation and planning of the last grid of the front grid, the right wing of the front grid, and the left wing of the front grid in the 6-segment horizontal grid is completed, v-=f*b, that is, return to the back grid for calculation and planning; When planning and calculating a certain surface grid in the six-segment horizontal grid, the vertical and horizontal coordinates of the center point of the original planning point grid are (v, h), and the vertical and horizontal coordinates of the center point of the surface grid are expressed as ((v+f*b), h), d[i] = 0, and the traffic characteristics p[(v+f*b), h] of the surface grid are obtained according to the geomorphic environment data, vehicle characteristic data, attribute data of obstacles or dangerous points, and whether the obstacles or dangerous points can be removed at the location of the vertical and horizontal coordinates ((v+f*b), h) of the center point of the surface grid; if the vehicle needs to use other obstacle-removing vehicles to remove the obstacles or dangerous points affected by the obstacles, the surface grid obstacle-removing time m[(v+f*b), h] is obtained; If the vertical and horizontal coordinates of the center point of the 6-segment horizontal grid satisfy v=ceil((x1-x0) / 2 / a-0.5)-f*b and h=ceil((yl-y0) / 2 / a-0.5), then the surface grid is the target point grid, and the surface grid satisfies q[v+f*b,h]=0 and is not planned, or the surface grid satisfies q[v+f*b,h]=1 and is planned and is a front grid and the recalculated shortest time is small, then v+=b takes the target point grid as the new planning point grid, calculates the distance, shortest time and process variables of the center point of the target point grid, records the new planning point grid coordinates and the original planning point grid coordinates in the vertical and horizontal tables, and then returns to the back grid of the 1 vertical and 6-segment horizontal grid under the planning point grid, and no longer plans to calculate the next 6 vertical horizontal grids under the front grid, and no longer plans to calculate the right wing of the front grid and the left wing of the front grid; If (((v + f * b) * a) 2 +(h * a) 2 ) 0.5 <r, p[(v + f * b), h] = 1, q[(v + f * b), h] = 0, or if (((v + f * b) * a) 2 +(h * a) 2 ) 0.5 <r, p[(v + f * b), h] = 2, p[v, h] = 2, q[(v + f * b), h] = 0, then the parent grid of the planned point is the original planned point grid. Insert a row in the vertical and horizontal table, record the vertical and horizontal coordinates (v, h) of the center point of the original planned point grid in the coordinate field of the parent grid of the planned point, take the surface grid as the new planned point grid, v += f * b, k = 1, q[v, h] = 1, s[v, h] = s[(v - f * b), h] + (a 2 +(z[v, h] - z[(v - f * b), h]) 2 ) 0.5 , t[v, h] = t[(v - f * b), h] + (a 2 +(z[v, h] - z[(v - f * b), h]) 2 ) 0.5 / (n[v, h] * e), record the vertical and horizontal coordinates (v, h) of the center point of the new planned point grid in the coordinate field of the planned point grid in the inserted row in the vertical and horizontal table, and continue the planning calculation for all the lower 1 vertical and 6 horizontal grids of the new planned point grid according to the rules described in step S5; If the front grid simultaneously satisfies (((v + f * b) * + a) 2 +(h * a) 2 ) 0.5 <r, p[(v + f * b), h] = 1, q[(v + f * b), h] = 1, or simultaneously satisfies (((v + f * b) * a) 2 +(h * a) 2 ) 0.5 <r, p[(v + f * b), h] = 2, p[v, h] = 2, q[(v + f * b), h] = 1, then t0 = t[(v + f * b), h], t[(v + f * b), h] = t[v, h] + (a 2 +(z[(v + f * b), h] - z[v, h]) 2 ) 0.5 / (n[v, h] * e), if t[(v + f * b), h] < t0, then the parent grid of the planned point is the original planned point grid. Find the corresponding record of the vertical and horizontal coordinates ((v + f * b), h) of the center point of the surface grid in the vertical and horizontal coordinate fields of the planned point grid in the vertical and horizontal table. Update the parent grid coordinate field of this record with the vertical and horizontal coordinates (v, h) of the center point of the original planned point grid. Take the surface grid as the new planned point grid, k = 1, v += f * b, s[v, h] = s[(v - f * b), h] + (a 2 +(z[v, h] - z[(v - f * b), h]) 2 ) 0.5 , continue the planning calculation for all the 1 - vertical and 6 - horizontal grids below this new planned point grid according to the rules described in step S5; If ((v + f * b) * a) is satisfied simultaneously 2 + (h * a) 2 ) 0.5 <r, p[(v + f * b), h] = 2, p[v, h] = 1, q[(v + f * b), h] = 0, then the parent grid of the planned point is the original planned point grid. Insert a row in the vertical and horizontal table, record the vertical and horizontal coordinates (v, h) of the center point of the original planned point grid in the coordinate field of the parent grid of the planned point. Take the surface grid as the new planned point grid, v += f * b, q[v, h] = 1, k = 2, m[i] = m[v, h], s[v, h] = s[(v - f * b), h] + (a 2 + (z[v, h] - z[(v - f * b), h]) 2 ) 0.5 , t[v, h] = m[i] + t[(v - f * b), h] + (a 2 + (z[v, h] - z[(v - f * b), h]) 2 ) 0.5 / (n[v, h] * e). Record the vertical and horizontal coordinates (v, h) of the center point of the surface grid in the coordinate field of the planned point grid of the inserted row in the vertical and horizontal table. Continue the planning calculation for all the 1 vertical and 6 horizontal grids below the new planned point grid according to the rules described in step S5; If the front grid simultaneously satisfies (((v + f * b) * a) 2 +(h * a) 2 ) 0.5 <When r, p[(v + f * b), h] = 2, q[(v + f * b), h] = 1, and p[v, h] = 1, then t0 = t[(v + f * b), h], and t[(v + f * b), h] = m[(v + f * b), h] + t[v, h] + (a 2 +(z[(v + f * b), h] - z[v, h]) 2 ) 0.5 / (n[v, h] * e); if t[(v + f * b), h] < t0, then the parent grid of the planned point is the original planned point grid. Find the corresponding record of the vertical and horizontal coordinates ((v + f * b), h) of the center point of the surface grid in the vertical and horizontal table field of the planned point grid coordinates. Update the planned point parent grid coordinate field of this record with the original vertical and horizontal coordinates (v, h) of the center point of the planned point grid. Take the surface grid as the new planned point grid, k = 2, v += f * b, s[v, h] = s[(v - f * b), h] + (a 2 +(z[v, h] - z[(v - f * b), h]) 2 ) 0.5 , m[i] = m[v, h], and continue the planning calculation for all the lower 1 vertical and 6 horizontal grids of the new planned point grid according to the rules described in step S5; When continuing to plan and calculate all the next 1 vertical and 6 horizontal grids of the new planning point grid, i+=1, the level reaches the next vertical, and each grid in the 6 horizontal grids is calculated in the order of the 6 horizontal grids, and the 4 wings in the 6 horizontal grids are planned and calculated; for the next planning grid of each planning point grid, if it meets the planning conditions, the next planning grid is changed into a new planning point grid, and the planning is continued after calculation. If it meets the return conditions, i-=1, and it is returned according to the return rule; until it returns to the vertical where the 6 horizontal grids are located, and returns to the next planning grid for judging the order of the 6 horizontal grids; If the face grid belongs to the termination grid, then the face grid, the right wing of the face grid and the left wing of the face grid are no longer planned and calculated; if the face grid is the front grid, return to judge the back grid of the 6-segment horizontal grid; if the face grid is the back grid, return to the upper vertical parent grid of the 6-segment horizontal grid.

7. The method according to claim 5, characterized in that The planning calculations for the 4 wings in the 6 horizontal grids include: After the last grid calculation and planning of the right wing in the 4-segment wing is completed, h-=c*d[i] is required, that is, return to the left wing of the opposite grid for judgment; When planning and calculating each grid of a wing in the four-segment wing, the loop corresponding to the wing is inlaid in the sub-loop for calculating the six-segment horizontal grids. Starting from the corresponding surface grid, each grid passing through the wing-adjacent horizontal grid of the surface grid to the grid farthest from the surface grid is used as a planning point grid in turn to calculate its wing-adjacent horizontal grid; The vertical and horizontal coordinates of the center point of the planning point grid are (v, h), and the vertical and horizontal coordinates of the center point of the adjacent horizontal grid of the planning point grid wing are (v, (h+g*c)). According to the geomorphic environment data, vehicle characteristic data, attribute data of obstacles or dangerous points, and information on whether obstacles or dangerous points can be cleared at the location of the vertical and horizontal coordinates (v, (h+g*c)) of the center point of the adjacent horizontal grid of the planning point grid wing, the traffic characteristics p[v, (h+g*c)] of the adjacent horizontal grid of the planning point grid wing are obtained; if the vehicle needs to use other obstacle-clearing vehicles to clear the obstacles or dangerous points affected by the vehicle passing through, the surface grid obstacle-clearing time m[v, (h+g*c)] is obtained; If the adjacent horizontal grid of the planning point grid of the 6-segment horizontal grid satisfies v=ceil((x1-x0) / 2 / a-0.5) and h=ceil((yl-y0) / 2 / a-0.5)-g*c at the same time, then the adjacent grid is the target point grid. If the target point grid satisfies q[v,(h+g*c)]=0 and is not planned, or the target point grid satisfies q[v,(h+g*c)]=1 and is planned and belongs to the wing of the front grid and the recalculated shortest time is small, then the target point grid is used as the new planning point grid, the distance, shortest time and process variables of the center point of the target point grid are calculated, the target point grid coordinates and the parent grid coordinates of the target point grid are recorded in the vertical and horizontal tables, and then returned; If both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, k = l, p[(v - f * b), (h + g * c)] = 1, p[v, (h + g * c)] = 1, q[v, (h + g * c)] = 0, or if both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, k = 1, p[v, (h + g * c)] = 2, p[v, h] = 2, p[(v - f * b), (h + g * c)] = 2, p[(v - f * b), h] = 2, q[v, (h + g * c)] = 0, then the planning point's parent grid is the upper 1 - vertical parent grid of the planning point. Insert a row in the vertical - horizontal table, record the vertical - horizontal coordinates of the center point of the upper 1 - vertical parent grid of the planning point ((v - f * b), (h - g * c * d[i])) in the planning point's parent grid coordinate field. Take the adjacent horizontal grids of the planning point grid's wing as the new planning point grid, h += g * c, d[i] += 1, q[v, h] = 1, s[v, h] = s[(v - f * b), (h - g * c * d[i])]+(a 2 +(d[i] * a) 2 +(z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 , t[v, h] = t[(v - f * b), (h - g * c * d[i])]+(a 2 +(d[i] * a) 2 +(z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 / (n[v, h] * e). Record the vertical - horizontal coordinates (v, h) of the center point of the new planning point grid in the planning point grid coordinate field of the inserted row in the vertical - horizontal table, and continue the planning calculation for all the lower 1 - vertical 6 - segment horizontal grids of the new planning point grid according to the rules described in step S5; If the right wing of the front grid or the left wing of the front grid simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, k = 1, p[v, (h + g * c)] = 1, q[v, (h + g * c)] = 1, p[(v - f * b), (h + g * c)] = 1, or simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, k = 1, q[v, (h + g * c)] = 1, p[v, (h + g * c)] = 2, p[v, h] = 2, p[(v - f * b), (h + g * c)] = 2, p[(v - f * b), h] = 2, then t0 = t[v, (h + g * c)], t[v, (h + g * c)] = t[(v - f * b), (h - g * c * d[i])] + (a 2 + ((d[i] + 1) * a) 2 + (z[v, (h + g * c)] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 / (n[v, h] * e). If t[v, (h + g * c)] < t0, then the parent grid of the planned point is the upper 1 - vertical parent grid of the planned point. Find the corresponding record of the horizontal and vertical coordinates (v, (h + g * c)) of the center point of the adjacent horizontal grid of the planned - point grid wing in the planned - point grid coordinate field of the horizontal - vertical table. Update the parent - grid coordinate field of this record with the horizontal and vertical coordinates ((v - f * b), (h - g * c * d[i])) of the center point of the upper 1 - vertical parent grid of the planned point. Take the adjacent horizontal grid of the planned - point grid wing as the new planned - point grid, h += g * c, d[i] += 1, s[v, h] = s[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2 + (z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 , and continue the planning calculation for all the lower 1 - vertical 6 - segment horizontal grids of this new planned - point grid according to the rules described in step S5; If ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 < r, p[v, (h + g * c)] ≠ 0, p[(v - f * b), (h + g * c)] ≠ 0, q[v, (h + g * c)] = 0, k = 2, then the planned point's parent grid is the upper 1 - vertical parent grid of the planned point. Insert a row in the vertical - horizontal table, record the vertical - horizontal coordinates ((v - f * b), (h - g * c * d[i])) of the center point of the upper 1 - vertical parent grid of the planned point in the planned point's parent grid coordinate field. Take the adjacent horizontal grids of the planned point grid's wing as the new planned point grid, h += g * c, d[i] += 1, q[v, h] = 1, s[v, h] = s[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2 + (z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 , t[v, h] = m[i] + t[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2 + (z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 / (n[v, h] * e). Record the vertical - horizontal coordinates (v, h) of the center point of the new planned point grid in the planned point grid coordinate field of the inserted row in the vertical - horizontal table above. Continue the planning calculation for all the lower 1 - vertical and 6 - segment horizontal grids of the new planned point grid according to the rules described in step S5; If both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, q[v, (h + g * c)] = 0, k = 1, p[v, h] = 1, p[(v - f * b), (h + g * c)] ≠ 0, or both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 * <r, p[v, (h + g * c)] = 2, q[v, (h + g * c)] = 0, k = l, p[(v - f * b), (h + g * c)] = 1, or both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, q[v, (h + g * c)] = 0, k = 1, p[(v - f * b), h] = 1, then the planning point's parent grid is the upper 1 - vertical parent grid of the planning point. Insert a row in the vertical - horizontal table, record the vertical - horizontal coordinates of the center point of the upper 1 - vertical parent grid of the planning point ((v - f * b, (h - g * c * d[i]))) in the planning point's parent grid coordinate field. Take the adjacent horizontal grids of the planning point grid's wing as the new planning point grid, h += g * c, d[i] += 1, m[i] = m[v, h], q[v, h] = 1, s[v, h] = s[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2 + (z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 , t[v, h] = m[i] + t[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2 + (z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 / (n[v, h] * e), k = 2. Record the vertical - horizontal coordinates (v, h) of the center point of the new planning point grid in the planning point grid coordinate field of the inserted row in the vertical - horizontal table above, and continue the planning calculation for all the lower 1 - vertical 6 - segment horizontal grids of the new planning point grid according to the rules described in step S5; If the right wing of the front grid or the left wing of the front grid simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, k = 2, p[v, (h + g * c)] ≠ 0, p[(v - f * b), (h + g * c)] ≠ 0, q[v, (h + g * c)] = 1, then t0 = t[v, (h + g * c)], t[v, (h + g * c)] = t[(v - f * b), (h - g * c * d[i])] + (a 2 + ((d[i] + 1) * a) 2 + (z[v, (h + g * c)] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 / (n[v, h] * e). If t[v, (h + g * c)] < t0, then the parent grid of the planned point is the upper 1 - longitudinal parent grid of the planned point. Find the corresponding record of the horizontal and vertical coordinates (v, (h + g * c)) of the center point of the adjacent horizontal grid of the planned point grid wing in the planned point grid coordinate field of the horizontal - vertical table. Update the parent grid coordinate field of this record with the horizontal and vertical coordinates ((v - f * b), (h - g * c * d[i])) of the center point of the upper 1 - longitudinal parent grid of the planned point. Take the adjacent horizontal grid of the planned point grid wing as the new planned point grid, h += g * c, d[i] += 1, s[v, h] = s[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2 + (z[v, h] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 , and continue the planning calculation for all the lower 1 - longitudinal 6 - segment horizontal grids of this new planned point grid according to the rules described in step S5; If the right wing of the front grid or the left wing of the front grid simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, q[v, (h + g * c)] = 1, k = l, p[v, h] = 1, p[(v - f * b), (h + g * c)] ≠ 0, or simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, q[v, (h + g * c)] = 1, k = 1, p[(v - f * b), (h + g * c)] = 1, or simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, k = 1, q[v, (h + g * c)] = 1, p[(v - f * b), h] = 1, then t0 = t[v, (h + g * c)], t[v, (h + g * c)] = m[v, (h + g * c)] + t[(v - f * b), (h - g * c * d[i])] + (a 2 + ((d[i] + 1) * a) 2 + (z[v, (h + g * c)] - z[(v - f * b), (h - g * c * d[i])]) 2 ) 0.5 / (n[v, h] * e), if t[v, (h + g * c)] < t0, then the parent grid of the planned point is the upper 1 - vertical parent grid of the planned point. Find the corresponding record of the vertical and horizontal coordinates (v, (h + g * c)) of the center point of the adjacent horizontal grid of the planned point grid wing in the vertical and horizontal table. Update the parent grid coordinate field of this record with the vertical and horizontal coordinates ((v - f * b), (h - g * c * d[i])) of the center point of the upper 1 - vertical parent grid of the planned point. Take the adjacent horizontal grid of the planned point grid wing as the new planned point grid, h += g * c, d[i] += 1, k = 2, m[i] = m[v, h], s[v, h] = s[(v - f * b), (h - g * c * d[i])] + (a 2 + (d[i] * a) 2 + (z[v, h] - z[(v - f * b), (h - g * c * d[i])])), and continue the planning calculation for all the lower 1 - vertical 6 - segment horizontal grids of this new planned point grid according to the rules described in step S5; If both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 1, k = 0, q[v, (h + g * c)] = 0, or both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, h] = 2, p[v, (h + g * c)] = 2, q[v, (h + g * c)] = 0, k = 0, or both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 1, p[(v - f * b), (h + g * c)] = 0, q[v, (h + g * c)] = 0, or both ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, p[v, h] = 2, p[(v - f * b), (h + g * c)] = 0, q[v, (h + g * c)] = 0, then the planned point's parent grid is the planned point grid, insert a row in the vertical and horizontal table, record the vertical and horizontal coordinates (v, h) of the center point of the planned point grid in the planned point's parent grid coordinate field, use the horizontally adjacent grid of the planned point grid's wing as the new planned point grid, h += g * c, k = 0, d[i] += 1, q[v, h] = 1, s[v, h] = s[v, (h - g * c)] + (a 2 + (z[v, h] - z[v, (h - g * c)]) 2 ) 0.5 , t[v, h] = t[v, (h - g * c)] + (a 2 + (z[v, h] - z[v, (h - g * c)]) 2 ) 0.5 / (n[v, h] * e), record the vertical and horizontal coordinates (v, h) of the center point of the horizontally adjacent grid of the planned point grid in the planned point grid coordinate field of the above - inserted row in the vertical and horizontal table, and continue the planning calculation for all the lower 1 - vertical and 6 - segment horizontal grids of this new planned point grid according to the rules described in step S5; If the right wing of the front grid or the left wing of the front grid simultaneously satisfies ((v * a) 2 + ((h + g * c) * n) 2 ) 0.5 <r, p[v, (h + g * c)] = 1, q[v, (h + g * c)] = 1, k = 0, or simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, p[v, h] = 2, q[v, (h + g * c)] = 1, k = 0, or simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 1, p[(v - f * b), (h + g * c)] = 0, q[v, (h + g * c)] = 1, or simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, p[v, h] = 2, p[(v - f * b), (h + g * c)] = 0, q[v, (h + g * c)] = 1, then t0 = t[v, (h + g * c)], t[v, (h + g * c)] = t[v, h] + (a 2 + (z[v, (h + g * c)] - z[v, h]) 2 ) 0.5 / (n[v, h] * e), if t[v, (h + g * c)] < t0, then the planning point parent grid is the planning point grid. Find the corresponding record of the horizontal and vertical coordinates (v, (h + g * c)) of the center point of the adjacent horizontal grid of the planning point grid wing in the horizontal and vertical table's planning point grid coordinate field. Update the planning point parent grid coordinate field of this record with the horizontal and vertical coordinates (v, h) of the center point of the planning point grid. Take the adjacent horizontal grid of the planning point grid wing as the new planning point grid, h += g * c, d[i] += 1, k = 0, s[v, h] = s[v, (h - g * c)] + (a 2 + (z[v, h] - z[v, (h - g * c)]) 2 ) 0.5 , and continue the planning calculation for all the next 1 vertical and 6 - segment horizontal grids of this new planning point grid according to the rules described in step S5; If ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, p[v, h] = 1, q[v, (h + g * c)] = 0, k = 0, or if ((v * a) 2 * + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, p[(v - f * b), (h + g * c)] = 0, q[v, (h + g * c)] = 0, p[v, h] = 1, then the planning point parent grid is the planning point grid, insert a row in the vertical and horizontal table, record the vertical and horizontal coordinates (v, h) of the center point of the planning point grid in the coordinate field of the planning point parent grid, use the horizontally adjacent grid of the wing of the planning point grid as the new planning point grid, h += g * c, d[i] += 1, k = 0, q[v, h] = 1, s[v, h] = s[v, (h - g * c)] + (a 2 + (z[v, h] - z[v, (h - g * c)]) 2 ) 0.5 , t[v, h] = m[v, h] + t[v, (h - g * c)] + (a 2 + (z[v, h] - z[v, (h - g * c)]) 2 ) 0.5 / (n[v, h] * e), record the vertical and horizontal coordinates (v, h) of the center point of the new planning point grid in the coordinate field of the planning point grid of the above - inserted row in the vertical and horizontal table, and continue the planning calculation for all the lower 1 - vertical and 6 - horizontal grids of the new planning point grid according to the rules described in step S5; If the right wing of the front grid or the left wing of the front grid simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, p[v, h] = 1, q[v, (h + g * c)] = 1, k = 0, or simultaneously satisfies ((v * a) 2 + ((h + g * c) * a) 2 ) 0.5 <r, p[v, (h + g * c)] = 2, q[v, (h + g * c)] = 1, p[(v - f * b), (h + g * c)] = 0, p[v, h] = 1, then t0 = t[v, (h + g * c)], t[v, (h + g * c)] = m[v, (h + g * c)] + t[v, h] + (a 2 + (z[v, (h + g * c)] - z[v, h]) 2 ) 0.5 / (n[v, h] * e), if t[v, (h + g * c)] < t0, then the parent grid of the planned point is the planned point grid. Find the corresponding record of the horizontal and vertical coordinates (v, (h + g * c)) of the center point of the adjacent horizontal grid of the planned point grid wing in the coordinate field of the planned point grid in the horizontal and vertical table. Update the coordinate field of the parent grid of the planned point in this record with the horizontal and vertical coordinates (v, h) of the center point of the planned point grid. Take the adjacent horizontal grid of the planned point grid wing as the new planned point grid, h += g * c, d[i] += 1, k = 0, s[v, h] = s[v, (h - g * c)] + (a 2 + (z[v, h] - z[v, (h - g * c)]) 2 ) 0.5 , and continue the planning calculation for all the lower 1 vertical and 6 - segment horizontal grids of this new planned point grid according to the rules described in step S5; When continuing to plan and calculate all the next 1 vertical and 6 horizontal grids of the new planning point grid, i+=1, the level reaches the next vertical, and each grid in the 6 horizontal grids is calculated in the order of the 6 horizontal grids, and the 4 wings in the 6 horizontal grids are planned and calculated; for the next planning grid of each planning point grid, if it meets the planning conditions, the next planning grid is changed into a new planning point grid, and the planning is continued after calculation. If it meets the return conditions, i-=1, and it is returned according to the return rule; until it returns to the vertical where the 6 horizontal grids are located, and returns to the next planning grid for judging the order of the 6 horizontal grids; If the wing adjacent horizontal grid of the planning point grid belongs to the termination grid, it is returned according to the return rule.

8. The method according to claim 1, characterized in that: The return conditions include: When the planning point grid is located in the 6-segment horizontal grid, the position of the return point follows the order of the 6-segment horizontal grid; after all the grids of the front grid and all its sub-vertical grids are fully calculated and planned, it returns to judging the adjacent horizontal grids on the right wing of the front grid; after all the grids of the right wing of the front grid and all its sub-vertical grids are fully calculated and planned, each grid returns to judging the adjacent horizontal grids on the right wing of the front grid, and after all the grids of the farthest grid in the right wing of the front grid and all its sub-vertical grids are fully calculated and planned, it returns to judging the adjacent horizontal grids on the left wing of the front grid; after all the grids of the left wing of the front grid and all its sub-vertical grids are fully calculated and planned, each grid returns to judging the adjacent horizontal grids on the left wing of the front grid, and after all the grids of the farthest grid in the left wing of the front grid and all its sub-vertical grids are fully calculated and planned, it returns to judging the back grid; After all the grids of the back grid and all its sub-vertical grids are calculated and planned, the grid returns to the judgment of the adjacent horizontal grids on the right wing of the back grid; after all the grids of the back grid and all its sub-vertical grids are calculated and planned, each grid in the right wing of the back grid returns to the judgment of the adjacent horizontal grids on the right wing of the back grid, and after all the grids of the farthest grid in the right wing of the back grid and all its sub-vertical grids are calculated and planned, the grid returns to the judgment of the adjacent horizontal grids on the left wing of the back grid; after all the grids of the farthest grid in the left wing of the back grid and all its sub-vertical grids are calculated and planned, the farthest grid in the left wing of the back grid is used as the last position of the 6-segment horizontal grid and returns to the judgment of the next planning grid of the upper 1 vertical parent grid; If the front grid of the 6-segment horizontal grid is the target point grid, the right wing and the left wing of the front grid will not be planned after calculation, and the process will return to the back grid judgment; if the back grid of the 6-segment horizontal grid is the target point grid, the right wing and the left wing of the back grid will not be planned after calculation, and the process will return to the next planning grid of the upper 1 vertical parent grid; if the front grid of the 6-segment horizontal grid is the termination grid, the right wing and the left wing of the front grid will not be planned after calculation, and the process will return to the back grid judgment; if the back of the 6-segment horizontal grid The grid is the termination grid, and the right wing and the left wing of the back grid are no longer planned, and the grid returns to the next planning grid of the upper vertical parent grid; if a certain wing of the 6-segment horizontal grid has no grid, the position of the return point is postponed; if the 6-segment horizontal grid has no left wing of the back grid, the farthest grid of the right wing of the back grid in the 6-segment horizontal grid is used as the last position of the 6-segment horizontal grid; if the 6-segment horizontal grid has no left wing and right wing of the back grid, the back grid in the 6-segment horizontal grid is used as the last position of the 6-segment horizontal grid, and so on according to the order of the 6-segment horizontal grid; When the upper 1 vertical parent grid is not located at the last position of the 6-segment horizontal grid, the position of the return point is the next planned grid of the upper 1 vertical parent grid; when the upper 1 vertical parent grid is located at the last position of the 6-segment horizontal grid, it returns to determine the next planned grid of the upper 1 vertical parent grid; When the upper 1 vertical parent grid of the upper 1 vertical parent grid is not located at the last position of the 6-segment horizontal grid, the position of the return point is the next planned grid of the upper 1 vertical parent grid of the upper 1 vertical parent grid; when the upper 1 vertical parent grid of the upper 1 vertical parent grid is located at the last position of the 6-segment horizontal grid, return to determine the next planned grid of the upper 1 vertical parent grid of the upper 1 vertical parent grid, and so on; When the parent grid is located in the first vertical direction, and the parent grid in the first vertical direction is not located at the last position of the three horizontal grids in the first vertical direction, the position of the return point is the next planned grid of the parent grid in the first vertical direction; when the parent grid is located at the last position of the three horizontal grids in the first vertical direction, the traversal and calculation of the arrival time and distance from the starting point to all reachable grids are completed.

9. The method according to claim 1, characterized in that: The step S6 comprises: Use green lines to draw on the grid map. The coordinates of the target grid center are (cei1((x1-x0) / 2 / a-0.5), ceil((y1-y0) / 2 / a-0.5)). Use green lines to draw the line connecting the target point and the grid center of the target point. The vertical and horizontal coordinates of the center point of the target point grid are used as the grid coordinates of the planning point, and the record whose grid coordinate field of the planning point is the coordinates of the center point of the target point grid is found in the vertical and horizontal table to obtain the parent grid coordinates of the planning point of the record; the line connecting the center point of the target point grid and the center point of the parent grid of the planning point is drawn with a green line; The parent grid coordinates of the planning point found are used as the new grid coordinates of the planning point, and the record whose grid coordinate field of the planning point is the new grid coordinates of the planning point is found in the vertical and horizontal tables to obtain the parent grid coordinates of the planning point of the record; the connection line of the grid center points obtained twice is drawn with green lines; The parent grid coordinates of the planning point found last time are used as the new planning point grid coordinates in turn, and the record whose planning point grid coordinate field is the new planning point grid coordinates is found in the vertical and horizontal tables to obtain the parent grid coordinates of the planning point of the record; the connection line of the grid center points obtained twice is drawn with a green line; Until the parent grid coordinates of the planning point found in the vertical and horizontal table are the coordinates of the center point of the starting grid (ceil((x0-x1) / 2 / a-0.5), cei1((y0-y1) / 2 / a-0.5))), use a green line to draw the connection between the last grid center point and the newly found starting grid center point, and then use a green line to draw the connection between the starting grid center point and the starting point (x0, y0); The green line connecting the starting point (x0, y0) and the target point (x1, y1) is the fastest path for the vehicle to travel from the starting point (x0, y0) to the target point (x1, y1).

Images