Path planning method in non-flat environment based on improved A-star algorithm

Abstract

The application discloses a path planning method in a non-flat environment based on an improved A-Star algorithm and relates to the technical field of intelligent vehicle path planning.The path planning method separates an original DEM map, establishes a ground steepness map corresponding to the two sub-DEM maps while separating the original DEM map into two sub-DEM maps on the premise of reserving original environment map information, then plans in the two sub-DEM maps respectively, performs path point normalization conversion calculation in the original DEM map, finally completes the missing values of the path points, and plans an optimal path.Compared with the original A-Star path planning algorithm, the path planning method has an order of magnitude improvement in planning speed, and the roughness of the ground and the related performance parameters of the vehicle are considered during planning, so that the planned path is more flat and more suitable for intelligent vehicle movement.

Description

Technical Field

[0001] This invention relates to the field of intelligent vehicle path planning technology, and to a path planning method for non-flat environments based on an improved A-Star algorithm. Background Technology

[0002] Traditional autonomous driving systems mainly consist of three modules: perception, decision-making, and control. Path planning, as a crucial component of the decision-making module, has always been a research hotspot and challenge in this field. Current research on path planning primarily focuses on structured two-dimensional road environments. These environmental models are too simplistic and fail to accurately reflect environmental characteristics, resulting in algorithms that lack adaptability to rugged and uneven environments such as mountains, hills, and ravines, thus hindering their ability to perform path planning tasks effectively.

[0003] Currently, among numerous path planning algorithms, the A-Star algorithm is widely used due to its unique advantages such as high computational efficiency, simple principle, and short planned path distance. For trajectory planning of autonomous vehicles in non-flat terrain, CN1156515157A discloses a trajectory planning method for autonomous vehicles in non-flat terrain based on convex optimization. First, an energy consumption model of the autonomous vehicle is established to determine the energy consumption generated when the vehicle travels to adjacent nodes. Second, a heuristic function is designed and an energy-optimal initial path is generated based on the A-Star search algorithm. Then, a motion corridor is generated by pruning the landmark points of the initial path to constrain the final trajectory position and kinematic characteristics. Finally, an objective function is designed, and various constraints are applied to obtain the optimized initial trajectory. The optimized final trajectory is further obtained by redistributing the time used to generate the segmented Bezier trajectory.

[0004] However, in complex environments, using the traditional A-Star algorithm for path planning on DEM (Digital Elevation Model) maps is constrained by multiple factors. When the map resolution is too large, the planning speed is low; when the map resolution is too small, the path quality is poor. There is an irreconcilable contradiction between path quality and planning speed.

[0005] In summary, the existing A-Star algorithm used for intelligent vehicle path planning does not achieve the expected planning speed and path quality in non-flat environments. Summary of the Invention

[0006] The purpose of this invention is to provide a path planning method for non-flat environments based on an improved A-Star algorithm, which can effectively improve the planning speed and path quality of the A-Star algorithm in non-flat environments.

[0007] To achieve the above-mentioned technical objectives and effects, the present invention is implemented through the following technical solution:

[0008] This method, based on an improved A-Star algorithm, is a path planning approach for non-flat environments. It separates the original DEM map and establishes sub-DEM maps and a ground steepness map. The starting and ending points of the path are determined, and their mappings in different sub-DEM maps are simultaneously calculated. Based on the mapped starting and ending points, sub-path planning is performed using the A-Star algorithm in the corresponding sub-DEM maps. Normalization transformation calculations are performed on the sub-path plans to obtain the suboptimal path. Missing points in the suboptimal path are filled in to obtain the optimal path, thus completing the planning process.

[0009] Path planning methods for non-flat environments based on the improved A-Star algorithm include:

[0010] S1: Separate the original digital elevation model (DEM) map M, and establish separate maps with the original map resolution. Figure Two One-third of the two DEM maps M1 and M2, and the corresponding steepness map M. σ1 M σ2 ;

[0011] S2: Determine the starting point P of the path. s (x s ,y s ) and target point P g (x g ,y g The algorithm calculates the mappings of these two points in different sub-DEM maps, where sub-DEM map M1 is mapped to P. os (x os ,y os ) and P og (x og ,y og The sub-DEM map M2 is mapped to P. ts (x ts ,y ts ) and P tg (x tg ,y tg );

[0012] S3: Based on the mapped start and end points, perform path planning using the A-Star algorithm in the corresponding sub-DEM maps;

[0013] S4: Perform transformation calculations on the paths path1 and path2 planned in step S3 to obtain the suboptimal path Path;

[0014] S5: Complete the missing values ​​of the path points in the suboptimal path Path to obtain the optimal path PATH, and the planning ends.

[0015] Furthermore, S1 specifically refers to:

[0016] Define the following mapping relationship:

[0017]

[0018] Where: the resolution of the original DEM map M is m×n, and the resolution of the sub-DEM map M1 is... The resolution of sub-DEM map M2 is This means rounding up 'a'.

[0019] The mapping from the original DEM map M to the sub-DEM map M1 is as follows:

[0020]

[0021]

[0022] Specifically:

[0023]

[0024] in: This represents the mapping from the original DEM map M to the sub-DEM map M1; (x k ,y k ) represents node P in the original DEM map M. k The coordinates; (x ok ,y ok ) is P k Node P mapped in sub-DEM map M1 ok The coordinates; For node coordinate values Round up to the nearest integer;

[0025] The mapping from the original DEM map M to the sub-DEM map M2 is as follows:

[0026]

[0027]

[0028] Specifically:

[0029]

[0030] in: This represents the mapping from the original DEM map M to the sub-DEM map M2; (x k ,y k ) represents node P in the original DEM map M. k The coordinates; (x tk ,y tk ) is P kNode P mapped in sub-DEM map M2 tk The coordinates; For node coordinate values Round up to the nearest integer;

[0031] The mapping from sub-DEM map M1 to the original DEM map M is as follows:

[0032]

[0033]

[0034] Specifically:

[0035]

[0036]

[0037]

[0038] in: This represents the mapping from sub-DEM map M1 to the original DEM map M; (x ok ,y ok ) represents node P in sub-DEM map M1 ok The coordinates; {(x k ,y k )} represents node P in sub-DEM map M1. ok The set of nodes {P} mapped in the original DEM map M k The corresponding set of coordinates;

[0039] The mapping from sub-DEM map M2 to the original DEM map M is as follows:

[0040]

[0041]

[0042] Specifically:

[0043]

[0044]

[0045]

[0046] in: This represents the mapping from sub-DEM map M2 to the original DEM map M; (x tk ,y tk ) represents node P in sub-DEM map M2 tk The coordinates; {(x k ,y k)} represents node P in sub-DEM map M2. tk The set of nodes {P} mapped in the original DEM map M k The corresponding set of coordinates;

[0047] After defining the above mapping relationship, the coordinates of any node in the original DEM map M, sub-DEM map M1, and sub-DEM map M2 can be transformed to obtain the corresponding coordinates in the sub-DEM map or the original DEM map.

[0048] Node P in sub-DEM map M1 ok The corresponding elevation value is:

[0049]

[0050] The corresponding steepness is:

[0051]

[0052] Node P in sub-DEM map M2 tk The corresponding elevation value is:

[0053]

[0054] The corresponding steepness is:

[0055]

[0056] Where: sumH(A) represents the sum of the elevation values ​​of all nodes in A in the original DEM map M; size(A) represents the number of nodes in A; singleH i (A) represents the elevation value of the i-th node in A in the original DEM map M (Note: i∈[1,size(A)]);

[0057] Through the above transformation calculations, a resolution approximately equal to that of the original DEM map M can be extracted from the original DEM map M. Figure Two One-third of the two DEM maps M1 and M2, and the corresponding steepness map M. σ1 M σ2 .

[0058] Furthermore, S2 specifically refers to:

[0059] Determine the starting point P of the path in the original DEM map M. s (x s ,y s ) and the path target point P g (x g ,y gThe path starting point P in the sub-DEM map M1 can be obtained through the transformation relationship described in step S1. os (x os ,y os ) and the path target point P og (x og ,y og ), and the path start point P in the sub-DEM map M2. ts (x ts ,y ts ) and the path target point P tg (x tg ,y tg ).

[0060] Furthermore, S3 specifically refers to:

[0061] The starting point P of the path is planned in the sub-DEM map M1 using the A-Star algorithm. os (x os ,y os ) to the target point P on the path og (x og ,y og The sub-path path1 is determined; the starting point P of the path is planned in the sub-DEM map M2 using the A-Star algorithm. ts (x ts ,y ts ) to the target point P on the path tg (x tg ,y tg The subpath path2 of ).

[0062] Specifically:

[0063] The heuristic function for the A-Star algorithm is f(n) = g(n) + h(n).

[0064] Where: f(n) is the total cost of the path, g(n) is the actual cost from the starting point to the current node, and h(n) is the estimated cost from the current node to the destination;

[0065] in:

[0066]

[0067] h(n)=D(n,G)(1+e3σ n )

[0068] Where: D(n-1,n) is the Euclidean distance from node n-1 to node n; θ n Let θ be the slope value of node n. max Δh represents the maximum gradient that an intelligent vehicle is allowed to safely pass under certain conditions. nLet Δh be the step elevation value from node n-1 to node n. max σ represents the maximum step elevation value that allows intelligent vehicles to safely pass under certain conditions. n Let σ be the steepness of node n. max The steepness is the maximum allowable slope for intelligent vehicles to pass safely under certain conditions; e1, e2, and e3 are the weights of the slope cost, step elevation cost, and steepness cost, respectively, and the sum of the three is 1.

[0069] in:

[0070]

[0071] Δh n =|H n -H n-1 |

[0072]

[0073] Specifically:

[0074]

[0075]

[0076] Considering the safety and smoothness of intelligent vehicles during movement, if during path search...

[0077] θ n >θ max Δh n >Δh max σ n >σ max Then g(n) = ∞

[0078] The A-Star algorithm, with the improved heuristic function described above, can plan sub-paths path1 and path2 in sub-DEM maps M1 and M2, respectively.

[0079] Furthermore, S4 specifically includes:

[0080] After establishing the transformation relationship between the original DEM map M, sub-DEM map M1, and sub-DEM map M2 in S1, when node P in sub-DEM map M1... ok Child node P of sub-DEM map M2 tk When spatial intersection exists, a node P in the original DEM map can be uniquely mapped. k ,Right now

[0081]

[0082] By performing normalization transformation calculations on the sub-paths path1 and path2 obtained in S3 using the above transformation relationship, the suboptimal path Path can be obtained.

[0083] Specifically:

[0084]

[0085] Furthermore, S5 specifically includes:

[0086] Through the above S4, a suboptimal path Path = {p1, p2, p3…p} can be obtained. i …p m+1 For this suboptimal path, traverse all path points and calculate the distance L(p) between the coordinates of two adjacent path points. i ,p i+1 Based on this distance, missing values ​​are filled in for the suboptimal path;

[0087] Specifically:

[0088]

[0089] If L(p) i ,p i+1 ) = 1 or but

[0090] If L(p) i ,p i+1 )≠1 and In the original DEM map, p i Starting from p i+1 Using A-Star algorithm to plan the path to the destination, the resulting path is:

[0091] Furthermore, the heuristic function of the A-Star algorithm in step S5 is f(n) = g(n) + h(n).

[0092] Where: f(n) is the total cost of the path, g(n) is the actual cost from the starting point to the current node, and h(n) is the estimated cost from the current node to the destination.

[0093]

[0094]

[0095] Where: D(n-1,n) is the Euclidean distance from node n-1 to node n; θ n Let θ be the slope value of node n. max Δh represents the maximum gradient that an intelligent vehicle is allowed to safely pass under certain conditions. nLet Δh be the step elevation value from node n-1 to node n. max The maximum step elevation value that allows intelligent vehicles to pass safely under certain conditions; σ is a kurtosis constant, always equal to 0. max The steepness is the maximum allowable slope for intelligent vehicles to pass safely under certain conditions; e1, e2, and e3 are the weights of the slope cost, step elevation cost, and steepness cost, respectively, and the sum of the three is 1.

[0096] in:

[0097]

[0098] Δh n =|H n -H n-1 |

[0099]

[0100] Specifically:

[0101]

[0102]

[0103] Considering the safety and smoothness of intelligent vehicle movement, during path search, if:

[0104] θ n >θ max Δh n >Δh max Then g(n) = ∞

[0105] After completing the missing values ​​of the suboptimal path as described above, the optimal path PATH can be obtained.

[0106]

[0107] Path planning complete.

[0108] The beneficial effects of this invention are as follows:

[0109] This invention discloses a path planning method for non-flat environments based on an improved A-Star algorithm. By separating the original DEM map and preserving the original environmental map information, the original DEM map is divided into two sub-DEM maps. At the same time, a ground steepness map corresponding to the sub-DEM map is established. Then, the path is planned separately in the two sub-DEM maps. The path point normalization transformation calculation is performed in the original DEM map. Finally, missing values ​​of the path points are filled in to plan the optimal path.

[0110] This invention discloses a path planning method for non-flat environments based on an improved A-Star algorithm. Compared with the traditional method of directly reducing map resolution, this invention method retains the original map information to a great extent and avoids the loss of local environmental features by using the idea of ​​resolution staggered reduction.

[0111] This invention discloses a path planning method for non-flat environments based on an improved A-Star algorithm. By planning paths separately for sub-maps and normalizing paths on the original map, the method improves algorithm speed while ensuring path quality, thus overcoming the limitations of the original algorithm.

[0112] This invention discloses a path planning method for non-flat environments based on an improved A-Star algorithm. In the path planning process, terrain features and vehicle parameters are used as one of the metrics for measuring path cost. Therefore, it has a wider range of applications and better portability.

[0113] This invention discloses a path planning method for non-flat environments based on an improved A-Star algorithm. Compared with the original A-Star path planning algorithm, the improved A-Star algorithm of this invention improves the planning speed by an order of magnitude. The planning method takes into account the ruggedness of the ground and the vehicle-related performance parameters, so the planned path is flatter and more suitable for intelligent vehicle movement.

[0114] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

[0115] Figure 1 This is a flowchart of the path planning method for non-flat environments based on the improved A-Star algorithm, as described in an embodiment of the present invention.

[0116] Figure 2 The original DEM map with a resolution of 300×300 as described in the embodiments of the present invention;

[0117] Figure 3 The original DEM map in this embodiment of the invention consists of a sub-DEM map M1 with a resolution of 150×150, a sub-DEM map M2 with a resolution of 151×151, and a corresponding ground steepness map.

[0118] Figure 4 The path planning result is the original A-Star algorithm path planning method described in the embodiments of the present invention;

[0119] Figure 5 The path planning result is the path planning method based on the improved A-Star algorithm in a non-flat environment as described in the embodiments of the present invention. Detailed Implementation

[0120] To more clearly illustrate the technical solutions of the embodiments of the present invention, the present invention will be described in detail below with reference to the accompanying drawings.

[0121] This method, based on an improved A-Star algorithm, is a path planning approach for non-flat environments. It separates the original DEM map and establishes sub-DEM maps and a ground steepness map. The starting and ending points of the path are determined, and their mappings in different sub-DEM maps are simultaneously calculated. Based on the mapped starting and ending points, sub-path planning is performed using the A-Star algorithm in the corresponding sub-DEM maps. Normalization transformation calculations are performed on the sub-path plans to obtain the suboptimal path. Missing points in the suboptimal path are filled in to obtain the optimal path, thus completing the planning process.

[0122] Example 1

[0123] like Figure 1 As shown:

[0124] Path planning methods for non-flat environments based on the improved A-Star algorithm include:

[0125] In this embodiment, as Figure 2 , Figure 3 :

[0126] S1: Separate the original DEM map M with a resolution of 300×300, and create sub-DEM maps M1 and M2 with a resolution of 150×150 and a corresponding steepness map M1 respectively. σ1 M σ2 ;

[0127] In this embodiment, the following mapping relationship is defined:

[0128]

[0129] Wherein: the resolution of the original DEM map M is 300×300, the resolution of the sub-DEM map M1 is 150×150, and the resolution of the sub-DEM map M2 is 151×151.

[0130] The mapping from the original DEM map M to the sub-DEM map M1 is as follows:

[0131]

[0132]

[0133] Specifically:

[0134]

[0135] in: This represents the mapping from the original DEM map M to the sub-DEM map M1; (x k ,y k ) represents node P in the original DEM map M. k The coordinates; (x ok ,y ok ) is P k Node P mapped in sub-DEM map M1 ok The coordinates; For node coordinate values Round up to the nearest integer;

[0136] The mapping from the original DEM map M to the sub-DEM map M2 is as follows:

[0137]

[0138]

[0139] Specifically:

[0140]

[0141] in: This represents the mapping from the original DEM map M to the sub-DEM map M2; (x k ,y k ) represents node P in the original DEM map M. k The coordinates; (x tk ,y tk ) is P k Node P mapped in sub-DEM map M2 tk The coordinates; For node coordinate values Round up to the nearest integer.

[0142] The mapping from sub-DEM map M1 to the original DEM map M is as follows:

[0143]

[0144]

[0145] Specifically:

[0146]

[0147]

[0148]

[0149] in: This represents the mapping from sub-DEM map M1 to the original DEM map M; (x ok ,yok ) represents node P in sub-DEM map M1 ok The coordinates; {(x k ,y k )} represents node P in sub-DEM map M1. ok The set of nodes {P} mapped in the original DEM map M k The set of coordinates corresponding to}.

[0150] The mapping from sub-DEM map M2 to the original DEM map M is as follows:

[0151]

[0152]

[0153] Specifically:

[0154]

[0155]

[0156]

[0157] in: This represents the mapping from sub-DEM map M2 to the original DEM map M; (x tk ,y tk ) represents node P in sub-DEM map M2 tk The coordinates; {(x k ,y k )} represents node P in sub-DEM map M2. tk The set of nodes {P} mapped in the original DEM map M k The set of coordinates corresponding to}.

[0158] In this embodiment, after defining the above mapping relationship, the coordinates of any node in the original DEM map M, sub-DEM map M1, and sub-DEM map M2 can be obtained from the corresponding coordinates in the sub-DEM map or the original DEM map through the relevant transformation relationship.

[0159] Node P in sub-DEM map M1 ok The corresponding elevation value is:

[0160]

[0161] The corresponding steepness is:

[0162]

[0163] Node P in sub-DEM map M2 tk The corresponding elevation value is:

[0164]

[0165] The corresponding steepness is:

[0166]

[0167] Where: sumH(A) represents the sum of the elevation values ​​of all nodes in A in the original DEM map M; size(A) represents the number of nodes in A; singleH i (A) represents the elevation value of the i-th node in A in the original DEM map M (Note: i∈[1,size(A)]).

[0168] In this embodiment, through the above conversion calculation, a map with a resolution approximately equal to that of the original DEM map M can be separated. Figure Two One-third of the two DEM maps M1 and M2, and the corresponding steepness map M. σ1 M σ2 ,like Figure 2 , Figure 3 .

[0169] S2: Determine the starting point P of the path. s (x s ,y s ) and target point P g (x g ,y g The algorithm calculates the mappings of these two points in different sub-DEM maps, where sub-DEM map M1 is mapped to P. os (x os ,y os ) and P og (x og ,y og The sub-DEM map M2 is mapped to P. ts (x ts ,y ts ) and P tg (x tg ,y tg );

[0170] In this embodiment:

[0171] Determine the starting point P of the path in the original DEM map M. s (x s ,y s (1,1) is the path to the target point P. g (x g ,y g Given (300, 300), the path starting point P in the sub-DEM map M1 can be obtained through the transformation relationship described in step S1. os (xos ,y os (1,1) is the path to the target point P. og (x og ,y og The coordinates are (150, 150), and the path starting point P in the sub-DEM map M2. ts (x ts ,y ts (1,1) is the path to the target point P. tg (x tg ,y tg (151, 151) is (151, 151).

[0172] S3: Based on the mapped start and end points, perform path planning using the A-Star algorithm in the corresponding sub-DEM maps;

[0173] In this embodiment:

[0174] The starting point P of the path is planned in the sub-DEM map M1 using the A-Star algorithm. os (x os ,y os ) to the target point P on the path og (x og ,y og The sub-path path1 is determined; the starting point P of the path is planned in the sub-DEM map M2 using the A-Star algorithm. ts (x ts ,y ts ) to the target point P on the path tg (x tg ,y tg The subpath path2 of ).

[0175] Specifically:

[0176] The heuristic function for the A-Star algorithm is f(n) = g(n) + h(n).

[0177] Where f(n) is the total cost of the path, g(n) is the actual cost from the starting point to the current node, and h(n) is the estimated cost from the current node to the destination.

[0178] in:

[0179]

[0180] h(n)=D(n,G)(1+e3σ n )

[0181] Where: D(n-1,n) is the Euclidean distance from node n-1 to node n; θ n Let θ be the slope value of node n. maxΔh represents the maximum gradient that an intelligent vehicle is allowed to safely pass under certain conditions. n Let Δh be the step elevation value from node n-1 to node n. max σ represents the maximum step elevation value that allows intelligent vehicles to safely pass under certain conditions. n Let σ be the steepness of node n. max The steepness is the maximum allowable slope for intelligent vehicles to pass safely under certain conditions; e1, e2, and e3 are the weights of the slope cost, step elevation cost, and steepness cost, respectively, and the sum of the three is 1.

[0182] in:

[0183]

[0184] Δh n =|H n -H n-1 |

[0185]

[0186] Specifically:

[0187]

[0188]

[0189] Considering the safety and smoothness of intelligent vehicles during movement, if during path search...

[0190] θ n >θ max Δh n >Δh max σ n >σ max Then g(n) = ∞

[0191] The A-Star algorithm, with the improved heuristic function described above, can plan sub-paths path1 and path2 in sub-DEM maps M1 and M2, respectively.

[0192] S4: Calculate the paths path1 and path2 planned in step S3 to find the suboptimal path Path;

[0193] In this embodiment:

[0194] After establishing the transformation relationship between the original DEM map M, sub-DEM map M1, and sub-DEM map M2 in step S1, when node P in sub-DEM map M1... ok Child node P of sub-DEM map M2 tkWhen spatial intersection exists, a node P in the original DEM map can be uniquely mapped. k ,Right now

[0195]

[0196] In this embodiment, by performing normalization transformation calculation on the sub-paths path1 and path2 obtained in step S3 through the above transformation relationship, the suboptimal path Path can be obtained.

[0197] Specifically:

[0198]

[0199] S5: Complete the missing values ​​of the path points in the suboptimal path Path to obtain the optimal path PATH, and the planning ends.

[0200] In this embodiment:

[0201] Through the above step S4, a suboptimal path Path = {p1, p2, p3…p} can be obtained. i …p m+1 For this suboptimal path, traverse all path points and calculate the distance L(p) between the coordinates of two adjacent path points. i ,p i+1 Based on this distance, missing values ​​are filled in for the suboptimal path.

[0202] Specifically:

[0203]

[0204] If L(p) i ,p i+1 ) = 1 or but

[0205] If L(p) i ,p i+1 )≠1 and In the original DEM map, p i Starting from p i+1 Using A-Star algorithm to plan the path to the destination, the resulting path is:

[0206] In this embodiment, the heuristic function of the A-Star algorithm in step S5 is f(n) = g(n) + h(n).

[0207] Where f(n) is the total cost of the path, g(n) is the actual cost from the starting point to the current node, and h(n) is the estimated cost from the current node to the destination.

[0208] Specifically:

[0209]

[0210]

[0211] Where: D(n-1,n) is the Euclidean distance from node n-1 to node n; θ n Let θ be the slope value of node n. max Δh represents the maximum gradient that an intelligent vehicle is allowed to safely pass under certain conditions. n Let Δh be the step elevation value from node n-1 to node n. max The maximum step elevation value that allows intelligent vehicles to pass safely under certain conditions; σ is a kurtosis constant, always equal to 0. max The steepness is the maximum allowable slope for intelligent vehicles to pass safely under certain conditions; e1, e2, and e3 are the weights of the slope cost, step elevation cost, and steepness cost, respectively, and the sum of the three is 1.

[0212] in:

[0213]

[0214] Δh n =|H n -H n-1 |

[0215]

[0216] Specifically:

[0217]

[0218]

[0219] Considering the safety and smoothness of intelligent vehicle movement, during path search, if:

[0220] θ n >θ max Δh n >Δh max Then g(n) = ∞

[0221] After completing the missing values ​​of the suboptimal path as described above, the optimal path PATH can be obtained.

[0222]

[0223] Path planning complete.

[0224] Example 2

[0225] like Figures 4-5 As shown:

[0226] In this embodiment, path planning was performed using both the original A-Star algorithm and the path planning method based on the improved A-Star algorithm in non-flat environments. The average planning time of the original A-Star algorithm was 2.92 seconds, while the average planning time of the path planning method of this invention was 0.98 seconds.

[0227] This invention provides a path planning method for non-flat environments based on an improved A-Star algorithm, which plans a flatter path while reducing the planning time to only 33% of that of the original A-Star algorithm.

[0228] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to the specific implementations described. Many modifications and variations can be made based on the content of this specification. These embodiments have been selected and specifically described in this specification to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention. The invention is limited only by the claims and their full scope and equivalents.

Claims

1. A path planning method for non-flat environments based on an improved A-Star algorithm, characterized by: Separate the original DEM map and create a sub-DEM map and a ground steepness map; determine the starting point and target point of the path, and simultaneously calculate the mapping of these two points in different sub-DEM maps; based on the mapped starting point and target point, use the A-Star algorithm to plan sub-paths in the corresponding sub-DEM maps; perform normalization transformation calculations on the sub-path plans to obtain the suboptimal path; The missing values ​​of the path points in the suboptimal path are filled in to obtain the optimal path, thus completing the planning. The path planning method includes: S1: Separate the original digital elevation model (DEM) map Two sub-DEM maps with a resolution of half that of the original map were created respectively. , And the corresponding steepness map , ; S2: Determine the starting point of the path With the target point And simultaneously calculate the mapping of these two points in different sub-DEM maps, where the sub-DEM maps Mapped to and Sub-DEM map Mapped to and ; S3: Based on the mapped start and end points, perform path planning using the A-Star algorithm in the corresponding sub-DEM maps; S4: Perform transformation calculations on the paths path1 and path2 planned in step S3 to obtain the suboptimal path Path; S5: Complete the missing values ​​of the path points in the suboptimal path Path to obtain the optimal path PATH, and the planning ends; Specifically, S1 is: Define the following mapping relationship: Among them: Original DEM map The resolution is Sub-DEM map The resolution is Sub-DEM map The resolution is , Indicates to Round up; Original DEM map Xiangzi DEM Map The mapping is: Specifically: in: Represents the original DEM map Xiangzi DEM Map Mapping; Original DEM map Middle node The coordinates; for Mapped on sub-DEM map Nodes in The coordinates; For node coordinate values Round up to the nearest integer; Original DEM map Xiangzi DEM Map The mapping is: Specifically: in: Represents the original DEM map Xiangzi DEM Map Mapping; Original DEM map Middle node The coordinates; for Mapped on sub-DEM map Nodes in The coordinates; For node coordinate values Round up to the nearest integer; Sub-DEM Map To the original DEM map The mapping is: Specifically: in: Sub-DEM map To the original DEM map Mapping; For sub-DEM map Middle node The coordinates; For sub-DEM map Nodes in Mapped on the original DEM map The set of nodes in The corresponding set of coordinates; Sub-DEM Map To the original DEM map The mapping is: Specifically: in: Sub-DEM map To the original DEM map Mapping; For sub-DEM map Middle node The coordinates; For sub-DEM map Nodes in Mapped on the original DEM map The set of nodes in The corresponding set of coordinates; After defining the above mapping relationship, the original DEM map Sub-DEM Map Sub-DEM Map The coordinates of any node in the map can be transformed to obtain the corresponding coordinates in the sub-DEM map or the original DEM map. Sub-DEM Map Middle node The corresponding elevation value is: The corresponding steepness is: Sub-DEM Map Middle node The corresponding elevation value is: The corresponding steepness is: in: This indicates that all nodes in A are in the original DEM map. The sum of the corresponding elevation values; This represents the number of nodes in A; This indicates that the i-th node in A is in the original DEM map. The corresponding elevation value, ; Through the above transformation calculations, the original DEM map can be obtained. Two sub-DEM maps with a resolution approximately half that of the original map were separated. , And the corresponding steepness map , .

2. The path planning method for non-flat environments based on the improved A-Star algorithm as described in claim 1, characterized in that: Specifically, S2 is: Determine the original DEM map Path start point and path target point The sub-DEM maps can be obtained respectively through the transformation relationship described in step S1. Path start point and path target point and sub-DEM maps Path start point and path target point .

3. The path planning method for non-flat environments based on the improved A-Star algorithm as described in claim 1, characterized in that: Specifically, S3 is: In sub-DEM map The A-Star algorithm is used to plan the starting point of the path. To the destination point The subpath path1; In sub-DEM map The A-Star algorithm is used to plan the starting point of the path. To the destination point The subpath path2; Specifically: The heuristic function of the A-Star algorithm is: in, The total cost of the path. The actual cost from the starting point to the current node. This represents the estimated cost from the current node to the destination. in: in: For nodes To the node Euclidean distance; For nodes The slope value, This refers to the maximum gradient that allows intelligent vehicles to safely pass under certain conditions. For nodes To the node The step elevation value, The maximum step elevation value that allows intelligent vehicles to pass safely under certain conditions; For nodes steepness, The maximum steepness under certain conditions that allows intelligent vehicles to safely pass through; , , The weights are respectively the slope cost, step elevation cost, and steepness cost, and the sum of the three is 1.

4. The path planning method for non-flat environments based on the improved A-Star algorithm as described in claim 3, characterized in that: in: Specifically: Considering the safety and smoothness of intelligent vehicles during movement, if during path search... , , ,but The improved A-Star algorithm described above can be used to plan sub-paths path1 and path2 on sub-DEM maps M1 and M2, respectively.

5. The path planning method for non-flat environments based on the improved A-Star algorithm as described in claim 1, characterized in that: Specifically, S4 is: The original DEM map is created using S1. Sub-DEM Map Sub-DEM Map After the corresponding transformation relationship, when the sub-DEM map Nodes in With sub-DEM map child nodes When spatial intersection exists, a node in the original DEM map can be uniquely mapped. ,Right now By performing normalization transformation calculations on the sub-paths path1 and path2 obtained in S3 using the above transformation relationship, the suboptimal path Path can be obtained. Specifically: 。 6. The path planning method for non-flat environments based on the improved A-Star algorithm as described in claim 1, characterized in that: Specifically, S5 is: A suboptimal path can be obtained through the above S4. For this suboptimal path, traverse all path points and calculate the distance between the coordinates of two adjacent path points. Based on this distance, missing values ​​are filled in for the suboptimal path; Specifically: like or ,but like and In the original DEM map, with Starting from, Using A-Star algorithm to plan the path to the destination, the resulting path is: .

7. The path planning method for non-flat environments based on the improved A-Star algorithm as described in claim 1, characterized in that: The heuristic function of the A-Star algorithm in step S5 is: in: The total cost of the path. The actual cost from the starting point to the current node. This represents the estimated cost from the current node to the destination. in: Where: D(n-1,n) is the Euclidean distance from node n-1 to node n; For nodes The slope value, This refers to the maximum gradient that allows intelligent vehicles to safely pass under certain conditions. For nodes To the node The step elevation value, The maximum step elevation value that allows intelligent vehicles to pass safely under certain conditions; It is a kurtosis constant, always equal to 0. The maximum steepness under certain conditions that allows intelligent vehicles to safely pass through; , , The weights are respectively the slope cost, step elevation cost, and steepness cost, and the sum of the three is 1; in: Specifically: Considering the safety and smoothness of intelligent vehicle movement, during path search, if: , ,but After completing the missing values ​​of the suboptimal path as described above, the optimal path PATH can be obtained. Path planning complete.

8. The application of the path planning method based on the improved A-Star algorithm in non-flat environments as described in any one of claims 1-7 in path planning in non-flat environments.

Images