Method, system, and device for global path planning for unmanned vehicle in off-road environment
12242286 ยท 2025-03-04
Assignee
Inventors
- Shida Nie (Beijing, CN)
- Yujia Xie (Beijing, CN)
- Zhihao Liao (Beijing, CN)
- Hui Liu (Beijing, CN)
- Lijin Han (Beijing, CN)
- Congshuai Guo (Beijing, CN)
Cpc classification
G05D1/644
PHYSICS
G06V10/50
PHYSICS
International classification
G06V10/50
PHYSICS
G05D1/246
PHYSICS
G05D1/644
PHYSICS
Abstract
Provided are a method, system and device for global path planning for an unmanned vehicle in an off-road environment. The method includes: obtaining satellite elevation data and a satellite remote sensing image of a current off-road environment; constructing a digital elevation model (DEM); determining slope and land surface relief of each grid in the current off-road environment; performing gray processing on the satellite remote sensing image to obtain grayscale values of the grids; determining traversal costs of the grids corresponding to different ground types; constructing a global grid map based on the slope and the land surface relief of each grid, as well as the traversal costs corresponding to the different ground types; determining a rugged terrain potential field and path costs; and searching for paths using a Bresenham's line algorithm and Theta* algorithm based on the rugged terrain potential field and the path costs, to generate a global path.
Claims
1. A method for global path planning for an unmanned vehicle in an off-road environment, comprising: obtaining satellite elevation data and a satellite remote sensing image of a current off-road environment; constructing a digital elevation model (DEM) based on the satellite elevation data; determining slope and land surface relief of each grid in the current off-road environment based on the DEM; performing gray processing on the satellite remote sensing image to obtain grayscale values of the grids; determining traversal costs of the grids corresponding to different ground types based on the grayscale values of the grids; constructing a global grid map based on the slope and the land surface relief of each grid, as well as the traversal costs corresponding to the different ground types; determining a rugged terrain potential field and path costs based on the global grid map; and searching for paths using a Bresenham's line algorithm and Theta* algorithm based on the rugged terrain potential field and the path costs, to generate a global path; wherein said searching for paths using the Bresenham's line algorithm and Theta* algorithm based on the rugged terrain potential field and the path costs, to generate the global path comprises: S1: calculating the slope based on the DEM, to initialize a slope table; calculating a repulsive potential field based on the land surface relief, to initialize a Rep table; initializing a pass table based on the satellite remote sensing image; initializing grid status based on the ground type and the slope; initializing Openlist and Closelist, setting a start point and a target point, and adding the start point to the Openlist; S2: checking whether the Openlist is empty; if the Openlist is empty, stopping running; otherwise, starting pathfinding; and selecting a point with a minimum cost value currently in the Openlist as an expanded node; S3: checking whether a current expanded node n is the target point; if yes, finishing pathfinding, backtracking parent nodes of the node n, storing the backtracked nodes into a path table sequentially to generate a path, until the start point is reached; otherwise, expanding to 8 neighboring nodes surrounding the node n, removing the node n from the Openlist, and adding the node n to the Closelist; S4: during expansion to a neighboring node n, first checking whether the grid status of the neighboring node n is free or obstacle; if the grid status is obstacle, skipping the neighboring node n, and if the grid status is free, proceeding to S5; and S5: checking whether the neighboring node n is in the Closelist; if yes, performing no operation; otherwise, calculating a cost value from the node n to the neighboring node n and setting the node n as a parent node of the neighboring node n; calculating a cost value between the neighboring node n and a parent node of the node n, and if the cost value between the neighboring node n and the parent node of the node n is smaller than a cost value between the parent node of the node n and the node n plus a cost value between the neighboring node n and the node n, updating a cost value of the neighboring node n and setting a parent node of the neighboring node n as the parent node of the node n; then checking whether the neighboring node n is in the Openlist; if not, adding the neighboring node n to the Closelist; if the neighboring node n is in the Openlist, comparing a previous cost value of the neighboring node n with a newly calculated cost value; if the newly calculated cost value is smaller, updating the cost value of the neighboring node n; otherwise, performing no operation; and then returning to S2.
2. The method for global path planning for an unmanned vehicle in an off-road environment according to claim 1, wherein said determining the slope and the land surface relief of each grid in the current off-road environment based on the DEM comprises: determining the slope of each grid in the current off-road environment using a third-order unweighted difference model; calculating a mean square deviation of elevation in a local area based on the DEM; and determining a mean square deviation of an i-th grid, which serves as land surface relief of the i-th grid, based on the mean square deviation of elevation in the local area as well as an elevation value of the i-th grid.
3. The method for global path planning for an unmanned vehicle in an off-road environment according to claim 1, wherein said performing gray processing on the satellite remote sensing image to obtain the grayscale values of the grids comprises: obtaining the grayscale values of the grids using an arithmetic mean method based on the satellite remote sensing image.
4. The method for global path planning for an unmanned vehicle in an off-road environment according to claim 1, wherein said determining the traversal costs of the grids corresponding to the different ground types based on the grayscale values of the grids comprises: determining the traversal costs of the grids corresponding to the different ground types using the following formula:
5. A device for global path planning for an unmanned vehicle in an off-road environment, comprising at least one processor, at least one memory, and computer program instructions stored in the memory, wherein when the computer program instructions are executed by the processor, the processor performs following operations: obtaining satellite elevation data and a satellite remote sensing image of a current off-road environment; constructing a digital elevation model (DEM) based on the satellite elevation data; determining slope and land surface relief of each grid in the current off-road environment based on the DEM; performing gray processing on the satellite remote sensing image to obtain grayscale values of the grids; determining traversal costs of the grids corresponding to different ground types based on the grayscale values of the grids; constructing a global grid map based on the slope and the land surface relief of each grid, as well as the traversal costs corresponding to the different ground types; determining a rugged terrain potential field and path costs based on the global grid map; and searching for paths using a Bresenham's line algorithm and Theta* algorithm based on the rugged terrain potential field and the path costs, to generate a global path; wherein said searching for paths using the Bresenham's line algorithm and Theta* algorithm based on the rugged terrain potential field and the path costs, to generate the global path comprises: S1: calculating the slope based on the DEM, to initialize a slope table; calculating a repulsive potential field based on the land surface relief, to initialize a Rep table; initializing a pass table based on the satellite remote sensing image; initializing grid status based on the ground type and the slope; initializing Openlist and Closelist, setting a start point and a target point, and adding the start point to the Openlist; S2: checking whether the Openlist is empty; if the Openlist is empty, stopping running; otherwise, starting pathfinding; and selecting a point with a minimum cost value currently in the Openlist as an expanded node; S3: checking whether a current expanded node n is the target point; if yes, finishing pathfinding, backtracking parent nodes of the node n, storing the backtracked nodes into a path table sequentially to generate a path, until the start point is reached; otherwise, expanding to 8 neighboring nodes surrounding the node n, removing the node n from the Openlist, and adding the node n to the Closelist; S4: during expansion to a neighboring node n, first checking whether the grid status of the neighboring node n is free or obstacle; if the grid status is obstacle, skipping the neighboring node n, and if the grid status is free, proceeding to S5; and S5: checking whether the neighboring node n is in the Closelist; if yes, performing no operation; otherwise, calculating a cost value from the node n to the neighboring node n and setting the node n as a parent node of the neighboring node n; calculating a cost value between the neighboring node n and a parent node of the node n, and if the cost value between the neighboring node n and the parent node of the node n is smaller than a cost value between the parent node of the node n and the node n plus a cost value between the neighboring node n and the node n, updating a cost value of the neighboring node n and setting a parent node of the neighboring node n as the parent node of the node n; then checking whether the neighboring node n is in the Openlist; if not, adding the neighboring node n to the Closelist; if the neighboring node n is in the Openlist, comparing a previous cost value of the neighboring node n with a newly calculated cost value; if the newly calculated cost value is smaller, updating the cost value of the neighboring node n; otherwise, performing no operation; and then returning to S2.
6. The device according to claim 5, wherein said determining the traversal costs of the grids corresponding to the different ground types based on the grayscale values of the grids comprises: determining the traversal costs of the grids corresponding to the different ground types using the following formula:
7. The device for global path planning for an unmanned vehicle in an off-road environment according to claim 5, wherein the memory is a computer-readable storage medium.
8. The device for global path planning for an unmanned vehicle in an off-road environment according to claim 6, wherein the memory is a computer-readable storage medium.
9. The device according to claim 5, wherein said performing gray processing on the satellite remote sensing image to obtain the grayscale values of the grids comprises: obtaining the grayscale values of the grids using an arithmetic mean method based on the satellite remote sensing image.
10. The device for global path planning for an unmanned vehicle in an off-road environment according to claim 9, wherein the memory is a computer-readable storage medium.
11. The device according to claim 5, wherein said determining the slope and the land surface relief of each grid in the current off-road environment based on the DEM comprises: determining the slope of each grid in the current off-road environment using a third-order unweighted difference model; calculating a mean square deviation of elevation in a local area based on the DEM; and determining a mean square deviation of an i-th grid, which serves as land surface relief of the i-th grid, based on the mean square deviation of elevation in the local area as well as an elevation value of the i-th grid.
12. The device for global path planning for an unmanned vehicle in an off-road environment according to claim 11, wherein the memory is a computer-readable storage medium.
Description
BRIEF DESCRIPTION OF THE DRAWINGS
(1) To describe the technical solutions in embodiments of the present disclosure or in the prior art more clearly, the accompanying drawings required for the embodiments are briefly described below. Apparently, the accompanying drawings in the following description show merely some embodiments of the present disclosure, and those of ordinary skill in the art may still derive other accompanying drawings from these accompanying drawings without creative efforts.
(2)
(3)
(4)
(5)
(6)
(7)
(8)
DETAILED DESCRIPTION OF THE EMBODIMENTS
(9) The technical solutions of the embodiments of the present disclosure are clearly and completely described below with reference to the drawings in the embodiments of the present disclosure. Apparently, the described embodiments are merely a part rather than all of the embodiments of the present disclosure. All other embodiments obtained by those skilled in the art based on the embodiments of the present disclosure without creative efforts shall fall within the protection scope of the present disclosure.
(10) An objective of the present disclosure is to provide a method, system, and device for global path planning for an unmanned vehicle in an off-road environment, to enhance the efficiency of vehicle movement in the off-road environment.
(11) In order to make the above objective, features and advantages of the present disclosure clearer and more comprehensible, the present disclosure will be further described in detail below in combination with accompanying drawings and particular implementation modes.
(12) As shown in
(13) S101: Obtain satellite elevation data and a satellite remote sensing image of a current off-road environment.
(14) The satellite elevation data can be expressed as a function: V.sub.i=(X.sub.i, Y.sub.i, Z.sub.i)(i=1, 2, 3 . . . n.sub.g), where X.sub.i and Y.sub.i are plane coordinates; Z.sub.i is an elevation value corresponding to point (X.sub.i, Y.sub.i); n.sub.g is a maximum value in sequence numbers representing grid positions in a grid map, and is an integer.
(15) S102: Construct DEM based on the satellite elevation data.
(16) The sampling principle is regular grid sampling, and the elevation values corresponding to the grids are stored, as shown in
(17) S103: Determine slope and land surface relief of each grid in the current off-road environment based on the DEM.
(18) S103 specifically includes: determining the slope of each grid in the current off-road environment using a third-order unweighted difference model; calculating a mean square deviation of elevation in a local area based on the DEM; and determining a mean square deviation of an i-th grid based on the mean square deviation of elevation in the local area as well as an elevation value of the i-th grid, and using the mean square deviation of the i-th grid as land surface relief of the i-th grid.
(19) In the DEM model where elevation is discrete and the terrain surface is unknown, slope and aspect calculations are performed using a 33 sliding window method within a local range, as shown in
(20) The slope is calculated using the following method:
(21)
(22) The slope is typically expressed as percentages, where the slope is as follows:
(23)
(24) Considering the power performance of typical off-road vehicles, in the present disclosure, a slope threshold is set to 40%, and grids with slope above this threshold is defined as impassable.
(25) By calculating the mean square deviation of elevation in a local area, the dispersion of elevation values in the local area is measured to assess the terrain ruggedness and evaluate the terrain relief characteristics of the area.
(26) First, a geometric mean
(27)
represents an elevation value of the i-th grid, and n represents the number of grids.
(28) Then, the mean square deviation of the i-th grid is calculated to determine the land surface roughness of the grid, providing land surface relief information d.sub.roughness:
(29)
(30) S104: Perform gray processing on the satellite remote sensing image to obtain grayscale values of the grids.
(31) In S104, the grayscale values of the grids are obtained using an arithmetic mean method based on the satellite remote sensing image. In the satellite remote sensing image, grids corresponding to different terrain properties have distinct pixel values. A grayscale value of a grid is calculated using the geometric mean method:
(32)
represents the grayscale value of the grid, n.sub.t represents the number of color channels in the image, i represents a sequence number of the grid, and C.sub.i represents an RGB value of a pixel.
(33) S105: Determine traversal costs of the grids corresponding to different ground types based on the grayscale values of the grids.
(34) In traditional grid maps, grids are either passable or impassable, which correspond to grid occupation states 1 and 0, respectively. During study on the passability of the off-road environment, a traversal coefficient pass.sub.i is defined for each grid based on the impact of different ground types in the off-road environment on vehicle passability. A larger value of pass.sub.i indicates that the terrain corresponding to the grid i is more difficult to pass, while a smaller value indicates that the terrain is easier to pass, as shown in
(35) Using the actual average driving speed of vehicles as a reference standard, the actual average driving speed in off-road environments is lower than an average driving speed on roads due to factors like soil quality, vegetation, and other terrain properties. Terrain properties in the off-road environment are classified into hard soil roads, natural roads, and low mountain roads, and vehicles are assigned a reference maneuvering speed in the off-road environment based on general off-road vehicle performance, as shown in Table 1.
(36) TABLE-US-00001 TABLE 1 Hard Natural Low Terrain property soil roads roads mountain roads Maneuvering speed 40 12 6
(37) Based on the maneuvering speed of vehicles in the off-road environment, using a passing speed of vehicles on hard soil roads as a benchmark, different terrain properties are assigned corresponding traversal cost values for vehicles. If a grid traversal cost corresponding to the hard soil roads is set to 10, the grid traversal cost can be represented as follows:
(38)
represents a traversal cost of a grid; m represents a code corresponding to a ground type, with 1, 2, and 3 corresponding to hard soil roads, natural roads, and low mountain roads, respectively; and v.sub.m represents a maneuvering speed of a vehicle on a corresponding ground type.
(39) Traversal costs and grayscale values of grids corresponding to different ground types are as shown in Table 2.
(40) TABLE-US-00002 TABLE 2 Traversal Traversal Grayscale No. Terrain property speed cost value 1 Hard soil roads 40 km/h 10 15-30 2 Natural roads 12 km/h 33 31-50 3 Low mountain roads 6 km/h 67 50-85 4 Lakes (obstacles) 0 km/h 99999 85-100
(41) S106: Construct a global grid map based on the slope and the land surface relief of each grid, as well as the traversal costs corresponding to the different ground types.
(42) S107: Determine a rugged terrain potential field and path costs based on the global grid map.
(43) A Gaussian-like function is used to calculate a repulsive potential field:
(44)
A total potential field is a sum of repulsive potential fields of all obstacles. A is an obstacle potential field coefficient, is an influence range coefficient, and d(X, X.sub.0) is a distance between the vehicle and the obstacle. Regions with a mean square deviation of elevation greater than 2.5 are considered rugged terrain for calculating the repulsive potential field, and the established repulsive potential field is as shown in
(45) The total potential field U(i) at grid i in the map is:
(46)
represents the potential field generated by obstacles around grid i.
(47) By combining the grid slope, different set traversal cost values, and the total potential field established above, the cost function of Theta* algorithm is set as follows: (n)=.Math.g(n)+.Math.h(n)+.Math.U(n). (n) represents the total cost; g(n) is an actual cost from the start point to the current point; h(n) is an estimated cost from the current point to the target point calculated using the Manhattan distance; U(n) is the potential field value of current point; , , and are the weight coefficients for each cost factor.
(48) The calculation formulas for g(n) and h(n) are:
(49)
and
(50)
is the current node; n is an expanded child node of node n; pass.sub.i is a traversal cost of the grid corresponding to the expanded node; d.sub.i is a distance between grids, and S.sub.i is the slope of the grid. n.sub.x and n.sub.y are the coordinates of node n, and goal.sub.x and goal.sub.y are the coordinates of the target point.
(51) Considering the vertical travel distance, slope is introduced to represent the actual travel distance of the vehicle. During expansion to the neighboring node, k is set to 1, and the traversal cost and slope of the expanded child node are calculated. When Theta* algorithm is used to calculate the cost between the parent node of the current node n and n, k represents the number of points passed through by the line connecting the two nodes, and the traversal cost of passing through these points is calculated.
(52) S108: Search for paths using a Bresenham's line algorithm and Theta* algorithm based on the rugged terrain potential field as well as the path costs calculated by the cost function (n) constructed based on (n)=.Math.g(n)+.Math.h(n)+.Math.U(n), to generate a global path. That is, a path from the start point to the endpoint with a minimum total cost (where the total cost is a sum of path costs) is searched for in the global grid map.
(53) S108 specifically includes the following steps: SL: Calculate the slope based on the DEM, to initialize a slope table; calculate a repulsive potential field based on the land surface relief, to initialize a Rep table; initialize a pass table based on the satellite remote sensing image; initialize grid status based on the ground type and the slope; initialize Openlist and Closelist, set a start point and a target point, and add the start point to the Openlist;
(54) S2: Check whether the Openlist is empty; if the Openlist is empty, stop running; otherwise, start pathfinding; and select a point with a minimum cost value currently in the Openlist as an expanded node.
(55) S3: Check whether a current expanded node n is the target point; if yes, finish pathfinding, backtracking parent nodes of the node n, store the backtracked nodes into a path table sequentially to generate a path, until the start point is reached; otherwise, expand to 8 neighboring nodes surrounding the node n, remove the node n from the Openlist, and add the node n to the Closelist.
(56) S4: During expansion to a neighboring node n, first check whether the grid status of the neighboring node n is free or obstacle; if the grid status is obstacle, skip the neighboring node n, and if the grid status is free, proceed to S5.
(57) S5: Check whether the neighboring node n is in the Closelist; if yes, perform no operation; otherwise, calculate a cost value from the node n to the neighboring node n and set the node n as a parent node of the neighboring node n; calculate a cost value between the neighboring node n and a parent node of the node n, and if the cost value between the neighboring node n and the parent node of the node n is smaller than a cost value between the parent node of the node n and the node n plus a cost value between the neighboring node n and the node n, update a cost value of the neighboring node n and set a parent node of the neighboring node n as the parent node of the node n; then check whether the neighboring node n is in the Openlist; if not, add the neighboring node n to the Closelist; if the neighboring node n is in the Openlist, compare a previous cost value of the neighboring node n with a newly calculated cost value; if the newly calculated cost value is smaller, update the cost value of the neighboring node n; otherwise, perform no operation; and then return to S2.
(58) Corresponding to the foregoing method, the present disclosure further provides a system for global path planning for an unmanned vehicle in an off-road environment, including: a data obtaining module configured to obtain satellite elevation data and a satellite remote sensing image of a current off-road environment; a DEM construction module configured to construct a DEM based on the satellite elevation data; a slope and land surface relief determining module configured to determine slope and land surface relief of each grid in the current off-road environment based on the digital elevation model; a gray processing module configured to perform gray processing on the satellite remote sensing image to obtain grayscale values of the grids; a traversal cost determining module configured to determine traversal costs of the grids corresponding to different ground types based on the grayscale values of the grids; a global grid map constructing module configured to construct a global grid map based on the slope and the land surface relief of each grid, as well as the traversal costs corresponding to the different ground types; a rugged terrain potential field and path costs determining module configured to determine a rugged terrain potential field and path costs based on the global grid map; and a global path generating module configured to search for paths using a Bresenham's line algorithm and Theta* algorithm based on the rugged terrain potential field and the path costs, to generate a global path.
(59) To perform the method corresponding to the foregoing embodiment and implement the corresponding functions and technical effects, the present disclosure further provides a device for global path planning for an unmanned vehicle in an off-road environment. The device includes at least one processor, at least one memory, and computer program instructions stored in the memory, where when the computer program instructions are executed by the processor, the method is implemented. The memory is a computer-readable storage medium.
(60) Based on such description, the technical solutions of the present disclosure essentially or the part contributing to the prior art may be implemented in a form of a software product. The computer software product is stored in a storage medium, and includes several instructions for enabling a computer device (which may be a personal computer, a server, a network device, or the like) to perform all or some steps of the methods described in the embodiments of the present disclosure. The foregoing computer storage medium includes any medium that can store program code, such as a USB flash drive, a removable hard disk, a read-only memory, a random access memory, a magnetic disk, or an optical disc.
(61)
(62) As shown in
(63) Each embodiment in the description is described in a progressive mode, each embodiment focuses on differences from other embodiments, and references can be made to each other for the same and similar parts between embodiments. Since the system disclosed in an embodiment corresponds to the method disclosed in an embodiment, the description is relatively simple, and for related contents, references can be made to the description of the method.
(64) Particular examples are used herein for illustration of principles and implementation modes of the present disclosure. The descriptions of the above embodiments are merely used for assisting in understanding the method of the present disclosure and its core ideas. In addition, those of ordinary skill in the art can make various modifications in terms of particular implementation modes and the scope of application in accordance with the ideas of the present disclosure. In conclusion, the content of the description shall not be construed as limitations to the present disclosure.