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 nx×nyA rectangular grid of which rx=X/nxAnd ry=Y/nyDiscrete resolution in the x-direction and y-direction, respectively, typically let rx=ry. Let the notation "grid (i, j)" denote a grid region located within the following coordinate range
Mij={(x,y)|xmin+(i-1)rx≤x≤xmax+irx,ymin+(j-1)ry≤y≤ymax+jry} (1)
Wherein, i is 1,2x,j=1,2,...,ny。
To achieve an accurate description of the geographical environment, the discrete resolution rxAnd ryThe 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 is1< 0 and p2The coefficient more than 0 corresponds to the two conditions of climbing and descending. When coefficient piThe 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 }1,q2According to the grid (a) selected from+,b+) Whether or not a change occurs for the barrier grid, which satisfies
Wherein q is1And q is2Satisfy q1>q2Is greater than 0. H (n) is H1And H2The smaller of these, H (n) min { H }1,H2}。
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,jk) Apparently there is (i)#,j#)=(i1,j1),(i*,j*)=(iN,jN)。
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.
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 bex=100,nyDiscretizing the two-dimensional region into n 120x×nyA rectangular grid of which rx=X/nx0.1km and ry=Y/ny0.1km is the discrete resolution in the x direction and the y direction respectively, and r is satisfiedx=ry. Let the notation "grid (i, j)" denote a grid region located within the following coordinate range
Mij={(x,y)|xmin+(i-1)rx≤x≤xmax+irx,ymin+(j-1)ry≤y≤ymax+jry} (10)
Wherein, i is 1,2x,j=1,2,...,ny。
Discrete resolution rxAnd ryThe 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)。
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, 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.
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
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').
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
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 is1-70 and p2The coefficients for both the uphill and downhill slope are 70. The coefficients q in equation (13) are selected from the set { q }1,q2According to the grid (a) selected from+,b+) Whether or not a change occurs for the barrier grid, which satisfies
Wherein q is1=10,q21. H (n) is H1And H2The smaller of these, H (n) min { H }1,H2}。
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*。
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
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.