CN113551682A

CN113551682A - Path planning method of amphibious unmanned war chariot considering influence of terrain and topography

Info

Publication number: CN113551682A
Application number: CN202110811903.8A
Authority: CN
Inventors: 王昕炜; 鲍锦秋; 彭海军; 张盛; 李云鹏; 陈飙松; 刘洁; 吕琛
Original assignee: Dalian University of Technology
Current assignee: Dalian University of Technology
Priority date: 2021-07-19
Filing date: 2021-07-19
Publication date: 2021-10-26
Anticipated expiration: 2041-07-19
Also published as: CN113551682B

Abstract

The invention provides a path planning method for an amphibious unmanned combat vehicle considering the influence of terrain and terrain, and belongs to the field of mechanical automation. First, the battlefield environment is discretized into a two-dimensional grid. Second, according to the battlefield environment, establish the corresponding obstacle matrix, terrain matrix and terrain matrix. Third, the evaluation function in the traditional A* algorithm is improved by using the terrain matrix and the terrain matrix. Fourth, according to the improved evaluation function, the A* algorithm is used to generate the optimal path. Fifth, evaluate the quality of the generated path. The invention comprehensively utilizes the obstacles, terrain and terrain information of the battlefield environment, and can realize the rapid planning of the high-quality motion path of the amphibious unmanned combat vehicle, and the generated path has the advantages of short equivalent path and low cumulative gradient; the invention has certain advantages. The practicality of the method can provide a new solution for the path planning problem of amphibious unmanned combat vehicles.

Description

Path planning method of amphibious unmanned war chariot considering influence of terrain and topography

Technical Field

The invention belongs to the field of mechanical automation, and relates to a path planning method of an amphibious unmanned chariot considering influence of terrain and topography.

Background

The unmanned chariot has good flexibility, maneuverability and environmental adaptability, and the firepower striking module of the mission load has the outstanding characteristics of various configurations, quick response and the like. Due to the fact that the sensor can provide urgent field perception capability and diversified breakthrough means for task executing personnel, the sensor is attracted by more and more extensive attention and application in recent years. In order to realize efficient execution of tasks, an optimal traveling route needs to be planned in advance before the unmanned war chariot moves out, so that the unmanned war chariot can quickly reach a target site while avoiding natural or artificial obstacles (such as ravines which cannot be crossed, regions with strong enemy vigor and the like) in a working environment. The study of the path planning of unmanned combat vehicles is an important aspect of their intelligent behavior.

The ground unmanned equipment path planning algorithm is mostly to solve under a two-dimensional framework (such as A, RRT, artificial potential field and the like), the data volume and the complexity are lower than those of a real map, and the influence of the actual topography cannot be considered. The operation environment of the amphibious unmanned war chariot is quite complex, and the movement speeds of the amphibious unmanned war chariot on different terrains have obvious difference. Only by fully and comprehensively utilizing information such as obstacles, terrain, topography and the like in the operation environment, a high-quality motion path can be generated for the amphibious unmanned combat vehicle, so that the combat efficiency is improved.

Disclosure of Invention

In order to solve the problems, the invention provides an improved A-star algorithm considering the influence of terrain and topography to solve the path planning problem of the unmanned combat tank. The algorithm is based on consideration of real geographic environment, and introduces a terrain matrix and a terrain matrix on the basis of a classical two-dimensional A-x algorithm. The path equivalent distance generated by the improved algorithm has the advantages of short equivalent distance, small climbing gradient and the like, and the algorithm has higher calculation efficiency.

In order to achieve the purpose, the invention adopts the technical scheme that:

a path planning method for an amphibious unmanned war chariot considering influence of terrain and topography comprises the steps of firstly, dispersing a battlefield environment into a two-dimensional grid. Secondly, establishing a corresponding obstacle matrix, a terrain matrix and a terrain matrix according to the battlefield environment. Thirdly, the evaluation function in the traditional A-star algorithm is improved by using the terrain matrix and the terrain matrix. Fourth, an optimal path is generated using the a-algorithm based on the improved valuation function. Fifth, the quality of the generated path is evaluated. The calculation flow chart of the method is shown in fig. 1, and comprises the following steps:

step 1: discretizing a battlefield environment into a two-dimensional grid

And establishing a plane rectangular coordinate system xOy for describing the geographic position, and determining the starting position and the target position of the unmanned fighting vehicle.

According to the mission requirement of the unmanned chariot, a rectangular area M is adopted to describe the range of motion of the unmanned chariot, and the range of motion is recorded as M { (x, y) | x_min≤x≤x_max,y_min≤y≤y_max}. Wherein [ x ]_min,x_max]Represents the minimum and maximum values of the region M in the x direction, [ y ]_min,y_max]Representing the minimum and maximum values of the region M in the y direction, where X is equal to X_max-x_min，Y＝y_max-y_min. From the terrain information, the elevation information for each location within the area M may be described by a function z ═ z (x, y). Considering that R terrain exists in the area M, the driving speed of the unmanned war chariot on each terrain has difference, and the formal speed of the unmanned war chariot on the R-th terrain is recorded as v_r(R ═ 1, 2.., R). Remember that unmanned war chariot is on

The ground shape has the fastest moving speed, and the speed is recorded as

Discretizing a two-dimensional area into n_x×n_yA rectangular grid of which r_x＝X/n_xAnd r_y＝Y/n_yDiscrete resolution in the x-direction and y-direction, respectively, typically let r_x＝r_y. Let the notation "grid (i, j)" denote a grid region located within the following coordinate range

M_ij＝{(x,y)|x_min+(i-1)r_x≤x≤x_max+ir_x,y_min+(j-1)r_y≤y≤y_max+jr_y} (1)

Wherein, i is 1,2_x，j＝1,2,...,n_y。

To achieve an accurate description of the geographical environment, the discrete resolution r_xAnd r_yThe selection should satisfy the following three conditions as much as possible simultaneously: the proportion of the area occupied by the obstacles in the specific grid is close to 0 or 1; secondly, only one type of terrain exists in the specific grid; and thirdly, in the specific grid, the terrain change is not obvious.

The starting position of the unmanned war chariot corresponds to the grid (i)^#,j^#) The target position corresponds to the grid (i)^*,j^*)。

Step 2: establishing an obstacle matrix, a terrain matrix and a terrain matrix according to the battlefield environment

Establishing an obstacle matrix according to the obstacle distribution condition in the area M

Each element of which has a value of 0 or 1. If Q (i, j) is 0, representing that no obstacle exists in the grid (i, j), and allowing the unmanned combat vehicle to pass through; if Q (i, j) is 1, it represents that there is an obstacle in the grid (i, j), and the unmanned vehicle cannot pass through.

Establishing a terrain matrix according to the terrain distribution condition in the region M

If the grid (i, j) belongs to the r-th terrain, then the elementE (i, j) has a value v_max/v_rWhich describes over the terrain

To the time respectively spent on the terrain r over the same distance.

Establishing a terrain matrix according to the altitude distribution condition in the area M

The value of the element W (i, j) is the maximum value of the elevation within the grid (i, j), i.e.

And step 3: method for improving estimation function in traditional A-matrix algorithm by using terrain matrix and terrain matrix

The expansion of the nodes is realized in the A-algorithm by the following evaluation function

F(n)＝G(n)+H(n) (2)

Wherein n represents the nth node in the path and corresponds to a certain grid generated in the step 1; f (n) is from the initial grid (i)^#,j^#) Via node n to the target grid (i)^*,j^*) The minimum cost estimate of (2); g (n) is formed by the initial grid (i)^#,j^#) Minimum cost to node n; h (n) is from node n to the target grid (i)^*,j^*) The minimum cost estimate of (2), i.e. the heuristic function. In step 3, the setting of g (n) and h (n) is improved by using the terrain matrix E and the terrain matrix W generated in step 2.

Firstly, the calculation formula of G (n) is improved as follows:

wherein, assuming that the node n and the node (n-1) correspond to the grid (i, j) and the grid (i ', j'), respectively, the calculation method of each parameter in the formula (3) is as follows: l ═ E (i, j) + E (i ', j'))/2, representing the effect of terrain on travel time; Δ h — W (i ', j') -W (i, j), representing the relative elevation between two nodes; t is the linear distance between the center points of the grid (i, j) and the grid (i ', j').

The calculation formula of the heuristic function H (n) is then improved. Let node n correspond to grid (i, j), consider the following two grids (i, j) to (i)^*,j^*) The following path:

route 1: the unmanned war chariot firstly moves from the grid (i, j) to the grid (i, j) along the y direction^*) (ii) a Then along the x direction, by the grid (i, j)^*) Move to grid (i)^*,j^*)。

Route 2: the unmanned war chariot firstly moves from the grid (i, j) to the grid (i) along the x direction^*J); and then along the y direction, by the grid (i)^*J) movement to grid (i)^*,j^*)。

It is apparent that paths 1 and 2 pass through the same number of grids (excluding the current grid (i, j)), which is denoted as m ═ i-i^*|+|j-j^*L. For path c (c is 1 or 2), the estimated cost is

Wherein g (k) represents the space between two adjacent grids in the path c (the previous and subsequent grids are respectively denoted as grid (a)^-,b^-) Move to the grid (a)⁺,b⁺) Path cost estimate, which may be expressed as

Two terms are in the right end bracket of the medium number in the formula (5). Wherein the first item

The meaning of (2) is the same as that of the second term at the right end of the equation (3), and the term indirectly reflects the time taken by the unmanned chariot to run; the second term Q reflects the path cost (i.e., the effect of path slope) of adjacent grids with respect to elevation, and is defined as follows:

wherein p is₁< 0 and p₂The coefficient more than 0 corresponds to the two conditions of climbing and descending. When coefficient p_iThe larger the absolute value of (i ═ 1,2), the greater the weight to consider for the path gradient. The coefficients q in equation (5) are selected from the set { q }₁,q₂According to the grid (a) selected from⁺,b⁺) Whether or not a change occurs for the barrier grid, which satisfies

Wherein q is₁And q is₂Satisfy q₁＞q₂Is greater than 0. H (n) is H₁And H₂The smaller of these, H (n) min { H }₁,H₂}_。

And 4, step 4: and (4) generating an optimal path by using an A-x algorithm according to the evaluation function improved in the step 3.

Step 4-1: two table data structures OPENLIST and close are created, where the OPENLIST table holds all nodes that have been generated but not explored and the close table holds nodes that have been explored.

Step 4-2: will initiate the grid (i)^#,j^#) Add OPENLIST table.

Step 4-3: calculating the costs G and H of all NODEs in OPENLIST, and selecting the NODE with the lowest F value as a NODE NODE according to a formula (2); NODE was removed from the OPENLIST table and placed in the CLOSELIST table.

Step 4-4: if the target grid (i)^*,j^*) If the path exists in the CLOSELIST table, the path planning is successful. Stopping calculation, connecting the target nodes with each father node in sequence to form a final path, and recording the final path as P^*. Note P^*Is composed of N nodes (including initial grid and target grid), and the k-th node is grid (i)^k,j^k) Apparently there is (i)^#,j^#)＝(i¹,j¹)，(i^*,j^*)＝(i^N,j^N)。

And 4-5: for each NODE that is adjacent to the NODE and not in the close list table, it is determined whether it is in the open list table. If not, adding the product into OPENLIST table; if so, updating the minimum G cost and the parent node.

And 4-6: and 4, repeating the step 4-3.

And 5: evaluating quality of generated paths

Two path-related indicators, equivalent distance S and accumulated slope OSlope, are defined as follows. For path P^*S and OSlope are defined as shown in equations (8) and (9), respectively

Wherein the meaning of each variable is the same as in equation (3).

The quality of the path is evaluated using the index S and the index OSlope.

The invention has the beneficial effects that:

a terrain matrix and a terrain matrix are introduced on the basis of a traditional two-dimensional A-star algorithm to realize accurate description of the geographic environment, so that a path planning method of the amphibious unmanned chariot considering the influence of the terrain and the terrain is developed. The method considers the complex operation environment of the amphibious chariot and comprehensively utilizes the obstacles and the geographic information in the environment. Compared with the traditional three-dimensional A-x algorithm, the path generated by the method has the advantages of short equivalent distance, gradual climbing gradient and the like, and has higher calculation efficiency. The method has certain practicability and can provide a new solution thought for the path planning problem of the amphibious unmanned chariot.

Drawings

FIG. 1 is a flow chart of the calculation of the present invention.

Fig. 2 is a schematic diagram of a battlefield environment in an embodiment of the invention.

FIG. 3 is a schematic diagram of an obstacle according to an embodiment of the present invention.

Fig. 4 is a path planning result obtained by using the method of the present invention in the embodiment of the present invention.

Fig. 5 shows the comparison result obtained by using the conventional three-dimensional a-x algorithm in the embodiment of the present invention.

Detailed Description

The present invention is further illustrated by the following specific examples.

The battlefield map is assumed to have a specification of 10km × 12km, which comprises three mountains with a relatively slow slope and a relatively steep slope, and the battlefield comprises 4 kinds of terrains including water, grassland, rock 1 and rock 2. In fig. 2, a schematic diagram of the terrain and topography of the simulation environment is given. The speeds of the unmanned chariot under four terrains are different, the rock ground 2 is selected as a standard, and the terrain matrix element values corresponding to the four terrains are shown in table 1. The distribution of obstacles in a battlefield environment is given in fig. 3.

TABLE 1 speed of unmanned vehicles under terrain and value of terrain matrix elements

A path planning method of an amphibious unmanned war chariot considering terrain and topography influence comprises the following steps:

step 1: discretizing a battlefield environment into a two-dimensional grid

To describe the geographic position, a plane rectangular coordinate system xOy is established, and the starting position (0.05km ) and the target position (7.75km,100.05km) of the unmanned chariot are determined.

According to the mission requirement of the unmanned chariot, the activity range is described by a rectangular area M which is written as M { (x, y) x_min≤x≤x_max,y_min≤y≤y_max}. Wherein [ x ]_min,x_max]＝[0,10]Represents the minimum and maximum values of the region M in the x direction, [ y ]_min,y_max]＝[0,12]The representative region M is in the y directionMinimum and maximum values of (c), let X be X_max-x_min＝10，Y＝y_max-y _min12. From the terrain information, the elevation information for each location within the area M may be described by a function z ═ z (x, y). Considering 4 terrains in the area M, the driving speed of the unmanned war chariot on each terrain has difference, and the formal speed of the unmanned war chariot on the r-th terrain is recorded as v_r(R ═ 1, 2.., R). Remember that unmanned war chariot is on

The ground shape has the fastest moving speed, and the speed is recorded as

Let n be_x＝100，n_yDiscretizing the two-dimensional region into n 120_x×n_yA rectangular grid of which r_x＝X/n_x0.1km and r_y＝Y/n_y0.1km is the discrete resolution in the x direction and the y direction respectively, and r is satisfied_x＝r_y. Let the notation "grid (i, j)" denote a grid region located within the following coordinate range

M_ij＝{(x,y)|x_min+(i-1)r_x≤x≤x_max+ir_x,y_min+(j-1)r_y≤y≤y_max+jr_y} (10)

Wherein, i is 1,2_x，j＝1,2,...,n_y。

Discrete resolution r_xAnd r_yThe selection of (A) satisfies the following three conditions: the proportion of the area occupied by the obstacles in the specific grid is close to 0 or 1; secondly, only one type of terrain exists in the specific grid; and thirdly, in the specific grid, the terrain change is not obvious. Therefore, the adopted dispersion can accurately describe the geographic environment corresponding to the battlefield.

The starting position of the unmanned war chariot corresponds to the grid (i)^#,j^#)(i^#＝0,j^#0), the target position corresponds to the grid (i)^*,j^*)(i^*＝100,j^*＝77)。

If the grid (i, j) belongs to the r-th terrain, the value of the element E (i, j) is v_max/v_rWhich describes over the terrain

To the time respectively spent on the terrain r over the same distance.

Firstly, the calculation formula of G (n) is improved as follows:

wherein, assuming that the node n and the node (n-1) correspond to the grid (i, j) and the grid (i ', j'), respectively, the calculation method of each parameter in the formula (11) is as follows: l ═ E (i, j) + E (i ', j'))/2, representing the effect of terrain on travel time; Δ h — W (i ', j') -W (i, j), representing the relative elevation between two nodes; t is the linear distance between the center points of the grid (i, j) and the grid (i ', j').

Has the same meaning with the second term at the right end of the equation (11), and the term indirectly reflects the time taken by the unmanned chariot to run; the second term Q reflects the adjacent grid phaseThe path cost for elevation (i.e., the effect of path slope) is defined as follows:

wherein p is₁-70 and p₂The coefficients for both the uphill and downhill slope are 70. The coefficients q in equation (13) are selected from the set { q }₁,q₂According to the grid (a) selected from⁺,b⁺) Whether or not a change occurs for the barrier grid, which satisfies

Wherein q is₁＝10，q₂1. H (n) is H₁And H₂The smaller of these, H (n) min { H }₁,H₂}。

Step 4-2: will initiate the grid (i)^#,j^#) Add OPENLIST table.

Step 4-4: if the target grid (i)^*,j^*) If the path exists in the CLOSELIST table, the path planning is successful. Stopping calculation, connecting the target nodes with each father node in sequence to form a final path, and recording the final path as P^*。

And 4-6: and 4, repeating the step 4-3.

And 5: evaluating quality of generated paths

Optimal path P generated in step 4^*As shown in fig. 4, it is composed of N147 nodes. Settlement path P^*And the accumulated slope OSlope. To show the superiority of the algorithm, the problem was solved using the conventional three-dimensional a-algorithm for comparison (the planned path is shown in fig. 5). From visual analysis of the effect graph, the traditional three-dimensional A-star algorithm does not fully consider map information, and a dangerous walking path is generated. The optimal path generated by considering the terrain and the topography of the amphibious chariot is avoided, the amphibious chariot is prevented from climbing over a steep hill, and the amphibious chariot is allowed to climb over a relatively slow hill. Further, the performance indicators of the present invention and the conventional three-dimensional a-algorithm are summarized in table 2. The equivalent distance and the accumulated gradient of the path generated by the method are superior to those of the traditional three-dimensional A-star algorithm, and the method has obvious advantages in calculation efficiency.

Table 2 comparison of the performance of the inventive method with the performance of the conventional three-dimensional a-x algorithm

The method is based on a two-dimensional A-star algorithm, the description of the complex geographic environment is realized by constructing the terrain, the terrain and the obstacle matrix, the valuation function is reasonably constructed, and the path planning of the amphibious unmanned chariot in the complex environment is realized. Meanwhile, a path evaluation method taking the equivalent distance and the accumulated gradient as indexes is established. Compared with the optimal path obtained by the basic A-star algorithm, the path distance equivalent distance generated by the method is shorter and more gradual, the calculation efficiency is higher, and a new solution thought is provided for the path planning of the amphibious unmanned chariot in the complex geographic environment.

The above-mentioned embodiments only express the embodiments of the present invention, but not should be understood as the limitation of the scope of the invention patent, it should be noted that, for those skilled in the art, many variations and modifications can be made without departing from the concept of the present invention, and these all fall into the protection scope of the present invention.

Claims

1. a path planning method for amphibious unmanned combat vehicles considering terrain and terrain influence, is characterized in that, comprises the following steps:

Step 1: Discrete the battlefield environment into a two-dimensional grid

Establish a plane rectangular coordinate system xOy to describe the geographic location and determine the starting position and target position of the unmanned combat vehicle;

According to the mission requirements of the unmanned combat vehicle, a rectangular area M is used to describe its activity range, which is denoted as M={(x,y)|x _min ≤x≤x _max ,y _min ≤y≤y _max }; where [x _min ,x _max ] represents the minimum and maximum values of the region M in the x direction, [y _min ,y _max ] represents the minimum and maximum values of the region M in the y direction, let X=x _max -x _min , Y= y _max -y _min ; According to the terrain information, the function z=z(x, y) is used to describe the altitude information of each position in the area M; Considering that there are R types of terrain in the area M, the driving of the unmanned combat vehicle on each terrain There are differences in speed, and the formal speed of the unmanned vehicle on the rth terrain is v _r (r=1,2,...,R); the unmanned combat vehicle moves the fastest on the Rth terrain, And record the speed as v _max =v _R ;

Discrete the two-dimensional area into n _x × _ny rectangular grids, where r _x =X/n _x and ry =Y/ _ny are the discrete resolutions in the x and _y directions respectively, usually let r _x = r _y ; let the notation "raster(i,j)" denote the region of the grid that lies within the following coordinate ranges:

M _ij ={(x,y)|x _min +(i-1)r _x ≤x≤x _max +ir _x ,y _min +(j-1)r _y ≤y≤y _max +jr _y } (1 )

Wherein, i=1,2,...,n _x , j=1,2,..., _ny ;

In order to achieve an accurate description of the geographical environment, the selection of discrete resolutions r _x and r _y should meet the following three conditions as far as possible: (1) the proportion of the area occupied by obstacles in a specific grid is close to 0 or 1; (2) a specific grid In the grid, there is only one type of terrain; ③In a specific grid, the terrain changes are not obvious;

The starting position of the unmanned combat vehicle corresponds to the grid (i ^# ,j ^# ), and the target position corresponds to the grid (i ^* ,j ^* );

Step 2: According to the battlefield environment, establish the obstacle matrix, terrain matrix and terrain matrix

According to the distribution of obstacles in the area M, establish the obstacle matrix

The value of each element is 0 or 1; if Q(i,j)=0, it means that there is no obstacle in the grid (i,j), allowing unmanned vehicles to pass; if Q(i,j)=1, It means that there is an obstacle in the grid (i, j), and the unmanned combat vehicle cannot pass;

According to the terrain distribution in the area M, establish a terrain matrix

If the grid (i,j) belongs to the rth type of terrain, the value of the element E(i,j) is v _max /v _r , which is used to describe the time spent on the terrain R and the terrain r passing the same distance respectively. ratio;

According to the altitude distribution in the area M, establish a terrain matrix

The value of the element W(i,j) is the maximum altitude in the grid (i,j), that is

Step 3: Use the terrain matrix and the terrain matrix to improve the evaluation function in the traditional A* algorithm

The described A* algorithm uses the following evaluation function to achieve node expansion:

F(n)=G(n)+H(n) (2)

Among them, n represents the nth node in the path, corresponding to a grid generated in step 1; F(n) is from the initial grid (i ^# ,j ^# ) via node n to the target grid (i ^* , j ^* ) minimum cost estimate; G(n) is the minimum cost from the initial grid (i ^# , j ^# ) to node n; H(n) is the minimum cost from node n to the target grid (i ^* , j ^* ) minimum cost estimation, i.e. heuristic function; in step 3, the terrain matrix E and terrain matrix W generated in step 2 are used to improve the setting of G(n) and H(n);

First, the calculation formula of the improved G(n) is as follows:

Among them, suppose that node n and node (n-1) correspond to grid (i, j) and grid (i', j') respectively, then the calculation method of each parameter in formula (3) is as follows: l=(E (i,j)+E(i',j'))/2, representing the influence of terrain on the travel time; Δh=W(i',j')-W(i,j), representing the distance between two nodes and The relative elevation of ; t is the straight-line distance between grid (i, j) and grid (i', j') center;

Then, improve the calculation formula of the heuristic function H(n); let node n correspond to grid (i, j), consider the following two equations from grid (i, j) to grid (i ^* , j ^* ) path:

Path 1: The unmanned vehicle first moves from grid (i, j) to grid (i, j ^* ) along the y direction; then moves from grid (i, j ^* ) to grid along the x direction lattice(i ^* ,j ^* );

Path 2: The unmanned combat vehicle first moves from grid (i,j) to grid (i ^* ,j) along the x direction; then moves from grid (i ^* ,j) to grid along the y direction lattice(i ^* ,j ^* );

The number of grids passed by the path 1 and path 2 is the same, and does not include the current grid (i, j), denoted as m=|ii ^* |+|jj ^* |; for path c, its estimated The cost is:

where c=1 or 2; g(k) represents the path cost estimate between two adjacent grids in path c, which can be expressed as:

Among them, the first

The meaning is the same as the second term on the right-hand side of equation (3), which is used to indirectly reflect the time spent by the unmanned combat vehicle; the second term Q reflects the path cost caused by the relative elevation of adjacent grids, It is defined as follows:

Among them, p ₁ <0 and p ₂ >0 are the coefficients corresponding to the two situations of climbing and descending; when the absolute value of the coefficient p _i (i=1, 2) is larger, the weight considered for the path gradient is larger. ; The coefficient q in formula (5) is selected from the set {q ₁ , q ₂ } and changes according to whether the grid (a ⁺ , b ⁺ ) is an obstacle grid, and it satisfies:

Wherein, q ₁ and q ₂ satisfy q ₁ >q ₂ >0; H(n) takes the smaller of H ₁ and H ₂ , that is, H(n)=min{H ₁ ,H ₂ };

Step 4: According to the improved evaluation function in Step 3, use the A* algorithm to generate the optimal path;

Step 4-1: Create two table data structures, OPENLIST and CLOSELIST, where the OPENLIST table saves all nodes that have been generated but not explored, and the CLOSELIST table saves the nodes that have been explored;

Step 4-2: Add the initial grid (i ^# , j ^# ) to the OPENLIST table;

Step 4-3: Calculate the cost G and cost H of all nodes in OPENLIST, select the node with the lowest F value according to formula (2) and record it as node NODE; remove NODE from the OPENLIST table and put it into the CLOSELIST table;

Step 4-4: If the target grid (i ^* ,j ^* ) exists in the CLOSELIST table, it means that the path planning is successful; the calculation stops, and the target node connects each parent node in turn to form the final path, denoted as P ^* ; P ^* consists of N nodes (including the starting grid and the target grid), and the kth node is set as grid (i ^k , j ^k ), obviously (i ^# , j ^# )=(i ¹ , j ¹ ), (i ^* , j ^* )=(i ^N , j ^N );

Step 4-5: For each node that is adjacent to the node NODE and is not in the CLOSELIST table, determine whether it is in the OPENLIST table; if not, add it to the OPENLIST table; if so, update its minimum G cost with the parent. node;

Step 4-6: Repeat steps 4-3;

Step 5: Evaluate the quality of the generated paths

The following two path-related indicators are defined, which are the equivalent distance S and the cumulative slope OSlope respectively, and the indicator S and the indicator OSlope are used to evaluate the quality of the path;

For the path P ^* , S and OSlope are defined as equations (8) and (9), respectively: