Special vehicle path planning method considering terrain uncertainty

Abstract

The invention discloses a special vehicle path planning method considering terrain uncertainty, and relates to the technical field of path planning, and the method comprises the steps: obtaining a two-dimensional regular grid map from a discrete terrain map, defining a label for each grid, and carrying out the parameter variable extraction and processing of the labels; establishing spatial correlation and correlation between parameters based on Gaussian distribution, introducing Monte Carlo sampling to obtain a terrain state set, inputting the terrain state set into a preset neural network model to obtain probability distribution of the maximum stable vehicle speed under a corresponding grid, and obtaining the maximum stable vehicle speed; the method comprises the following steps: introducing maximum stable vehicle speed probability distribution constructed based on landform parameter uncertainty in a rapid expansion random tree path expansion process, calculating conditional risk values, judging whether the conditional risk values of all grids are greater than or equal to a preset threshold value or not, and if the conditional risk values of all grids are greater than or equal to the preset threshold value, determining that a path section is a low-risk path. According to the invention, the path planning traffic safety and robustness of the vehicle in a complex unknown terrain are improved.

Description

Technical Field

[0001] This invention relates to the field of route planning technology, and more specifically to a route planning method for special vehicles that takes into account terrain uncertainty. Background Technology

[0002] Currently, the problem of autonomous driving path planning in complex and unstructured environments has received considerable attention in recent years. In typical application scenarios such as disaster response, field inspection, and unmanned transportation, special vehicles need to complete autonomous navigation tasks in areas lacking clear road markings.

[0003] In such environments, terrain conditions have a far greater impact on the accessibility of special vehicles than in traditional urban road scenarios. Route planning must not only consider geometric accessibility but also incorporate an assessment of the ground's physical properties and their uncertainties.

[0004] Current mainstream methods employ deterministic terrain mapping and static heuristic planning strategies. While modules such as terrain classification and accessibility estimation enhance the environmental adaptability of path planning, they still lack a modeling and propagation mechanism for physical terrain parameters, making it difficult to reflect the physical uncertainties in real-world scenarios. Furthermore, most systems assume vehicle accessibility to be statically binary, neglecting the dynamic constraints of terrain on vehicle speed.

[0005] Therefore, how to improve the environmental adaptability of path planning while taking terrain into account is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0006] In view of this, the present invention provides a special vehicle route planning method that takes into account terrain uncertainty, which improves the environmental adaptability of route planning by taking terrain into account, and realizes the modeling, propagation and route planning of terrain uncertainty.

[0007] To achieve the above objectives, the present invention adopts the following technical solution: A special vehicle route planning method considering terrain uncertainty includes: S1: Obtain a terrain map covering the task scene, discretize the terrain map to obtain a two-dimensional regular grid map, define a terrain type label and a slope level label for each two-dimensional regular grid, extract parameter variables from the labels, and process the parameter variables so that the parameter variables all follow an independent one-dimensional Gaussian distribution. S2: For the parameter variables, establish spatial correlation and inter-parameter correlation based on Gaussian distribution, and obtain the terrain parameter condition distribution at the grid center; S3: Monte Carlo sampling is introduced into the terrain parameter condition distribution of each two-dimensional regular grid to obtain the terrain state set. The terrain state set is input into a preset neural network model to obtain the probability distribution of the maximum stable vehicle speed under the corresponding two-dimensional regular grid. The probability distribution map of the maximum vehicle speed is constructed at the whole map level. S4: In the process of rapidly expanding the random tree path, a maximum stable vehicle speed probability distribution based on the uncertainty of terrain parameters is introduced. For each two-dimensional regular grid in the path segment, the conditional risk value is calculated based on its maximum stable vehicle speed probability distribution. It is then determined whether the conditional risk values ​​of all two-dimensional regular grids are greater than or equal to a preset threshold. If they are all greater than or equal to the preset threshold, the path segment is a low-risk path.

[0008] Preferably, defining terrain type labels and slope grade labels for each two-dimensional regular raster specifically includes discretizing the entire terrain area into a two-dimensional regular raster map. Each two-dimensional regular grid For a fixed location in geographic space, for each two-dimensional regular grid Define two semantic tags: terrain type tag Slope grade label .

[0009] Preferably, the extraction of parameter variables from the labels specifically includes: defining terrain classification maps based on terrain type labels and slope grade labels respectively. Slope classification map Two raster maps, each with a two-dimensional regular raster containing the internal friction coefficient for each terrain type label. Friction angle Shear coefficient Three physical parameter variables, each slope grade label is represented by a two-dimensional regular grid containing the slope. One parameter variable; For terrain type The prior distribution of its physical parameters is as follows: ; in, Indicates terrain type The average internal friction coefficient, The standard deviation of the internal friction coefficient. Indicates terrain type The average friction angle, The standard deviation of the friction angle. Indicates terrain type The average shear coefficient, The standard deviation of the shear coefficient is given for slope grade. Its slope angle distribution is as follows: ; in, Indicates terrain type average slope This represents the standard deviation of the slope.

[0010] Preferably, S2 includes: For any two-dimensional regular raster in a two-dimensional regular raster image Define its neighborhood window as centered on it and with a size of A local area, in which The size of the grid neighborhood; ; in, For a two-dimensional regular grid The neighborhood geometry centered on, A two-dimensional rule grid, with a unified definition of the two-dimensional rule grid. slope Internal friction coefficient Friction angle and shear coefficient The four continuous terrain parameters are , , and ; ; Neighbor windows are arranged in a fixed order. this All parameters of a two-dimensional regular raster are obtained. Dimensional vector: ; in, For the center point Terrain parameters: ; The joint distribution is split into the following structure: ; in, Indicates the remaining neighbors All parameters of a two-dimensional regular raster. Represents the mean of the central grid cell. Represents the mean of neighboring grid cells. It is the autocovariance of the central grid. It is the cooperation between the center and its neighbors. It is the covariance between neighbors. The conditional distribution of the central grid is derived based on the multivariate Gaussian conditional distribution formula: ; in, It is the conditional distribution of the central grid given the neighbor conditions. It is the observation vector of the neighboring grid; Modeling spatial correlation and inter-parameter correlation separately: For the same parameter, the spatial correlation between different two-dimensional regular rasters is defined using a Gaussian kernel function: ; in, The covariance between two variables It is a parameter variance It is the kernel width that controls the decay rate; For the same two-dimensional regular raster, different parameters , , and Introducing a fixed empirical covariance matrix : ; in, For the first The variance of each parameter, For the first The and the first The covariance among the parameters, and the overall covariance matrix is ​​a combination of the Kronecker product of the spatial structure and the parameter structure: ; in, for Spatial kernel similarity matrix between two-dimensional regular grids This indicates the Kronecker product operation.

[0011] Preferably, step S3 includes: processing each two-dimensional regular grid. Subsequent sampling yields a set of terrain parameter samples. Input sample set ; in each sample parameter Below, a high-fidelity multibody dynamics simulation platform is used to evaluate the maximum stable speed that the vehicle can achieve in this terrain. The output sample set is established based on the maximum stable speed. ; Output sample set and input sample set Training a pre-defined neural network model: ; ; in, It is a deep feedforward neural network. For the first High-fidelity simulation output of the samples. This represents the number of training samples.

[0012] Preferably, S4 includes: Initialize a fast-expanding random tree, setting the starting node to... In each expansion, nodes are randomly sampled in free space. And find the nearest node in the tree. This generates a path from point to Extended path segment ,cover A two-dimensional regular grid, for each grid in the path The trained neural network model predicts the maximum stable vehicle speed corresponding to each set of parameters. The maximum stable vehicle speed probability distribution for this grid is constructed based on the vehicle speed samples, and the conditional risk value is defined as: ; in, Confidence level Conditional risk value, Confidence level The quantile risk value below is defined as: ; For each grid Maximum vehicle speed distribution Its conditional risk value is: ; Set minimum acceptable vehicle speed threshold ;like If established, then Add a fast-expanding random tree. If it is a path segment, then discard the current expansion direction and repeatedly sample nodes randomly in free space. and judge This continues until the tree structure successfully reaches the target area and completes the planning.

[0013] As can be seen from the above technical solution, compared with the prior art, the present invention discloses a special vehicle route planning method that considers terrain uncertainty, and the beneficial effects are: 1. This invention introduces terrain physical attributes as mapping elements, constructs a joint probability distribution for each grid, and generates diverse terrain states through Monte Carlo sampling. Compared with traditional binary maps, it can more accurately reflect the complexity and uncertainty of terrain.

[0014] 2. This invention addresses the nonlinear relationship between high-dimensional terrain parameters and vehicle dynamics performance by employing a neural network to map terrain combinations to the maximum stable vehicle speed, replacing the real-time simulation process and improving the real-time performance of path planning.

[0015] 3. This invention introduces conditional value of risk as a traffic safety indicator in rapidly expanding random trees, dynamically evaluating the minimum vehicle speed in the worst case. Compared with traditional path expansion strategies based on static heuristics, it can effectively avoid high-risk areas and improve task success rate and robustness. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0017] Figure 1 A schematic diagram illustrating the steps of assigning values ​​to two-dimensional regular grid labels and extracting physical attributes provided by this invention; Figure 2 This is a schematic diagram of the grid conditional distribution modeling steps based on spatial-parameter cocorrelation provided by the present invention; Figure 3 The flowchart of terrain response modeling and neural network learning combined with high-fidelity simulation provided by this invention; Figure 4 A schematic diagram of the safety constraint RRT path planning for fusion condition risk assessment provided by the present invention. Detailed Implementation

[0018] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0019] This invention discloses a special vehicle route planning method considering terrain uncertainty, see reference. Figure 1 The process involves collecting a map dataset and discretizing it into a two-dimensional regular raster map, which is then assigned terrain-related parameters. First, a global or approximate terrain map covering the task scenario is acquired; this map can be obtained through methods such as LiDAR point cloud modeling, remote sensing image interpretation, or aerial surveying. The entire terrain region is then discretized into a two-dimensional regular raster map. Each two-dimensional regular grid This corresponds to a fixed location in geographic space. For each two-dimensional regular raster... Define two semantic tags: (1) Terrain type label ,represent A predefined landform type, the label of which can be determined by a semantic segmentation model or prior rules; (2) Slope rating label ,represent Each slope range is derived from a digital elevation map or slope calculation.

[0020] Define terrain classification maps based on the two labels. Slope classification map Two raster maps. The two-dimensional regular raster for each terrain type label contains the internal friction coefficient. Friction angle Shear coefficient Three physical parameter variables constitute the soil's mechanical properties. Each slope grade label's two-dimensional regular grid contains the slope. One parameter variable describes the terrain geometry. These four variables are defined to follow independent one-dimensional Gaussian distributions for each terrain type or slope grade. For terrain type... The prior distribution of its physical parameters is as follows: ; in, Indicates terrain type The average internal friction coefficient, The standard deviation of the internal friction coefficient. Indicates terrain type The average friction angle, The standard deviation of the friction angle. Indicates terrain type The average shear coefficient, This represents the standard deviation of the shear coefficient. For slope grade... Its slope angle distribution is as follows: ; in, Indicates terrain type average slope This represents the standard deviation of the slope. The values ​​of the mean and variance can be obtained from empirical tables or simulation calibration.

[0021] See Figure 2 Raster terrain parameters are interconnected, influenced not only by their own raster parameters but also by surrounding parameters. Therefore, spatial correlation modeling and inter-parameter correlation modeling are introduced. For any two-dimensional regular raster in a two-dimensional regular raster map... Define its neighborhood window as centered on it and with a size of A local area, in which This represents the size of the grid neighborhood.

[0022] ; in, For a two-dimensional regular grid The neighborhood geometry centered on, A two-dimensional regular grid, whose physical properties collectively determine the terrain consistency and local vehicle feasibility of the area. A unified definition of the two-dimensional regular grid. slope Internal friction coefficient Friction angle and shear coefficient The four continuous terrain parameters are , , and .

[0023] ; Neighbor windows are arranged in a fixed order. this All parameters of a two-dimensional regular raster are obtained. Dimensional vector: ; in, For the center point Terrain parameters: ; The joint distribution is split into the following structure: ; in, Indicates the remaining neighbors All parameters of a two-dimensional regular raster. Represents the mean of the central grid cell. Represents the mean of neighboring grid cells. It is the autocovariance of the central grid. It is the cooperation between the center and its neighbors. It is the covariance between neighbors. The conditional distribution of the central grid is derived based on the multivariate Gaussian conditional distribution formula: ; in, It is the conditional distribution of the central grid given the neighbor conditions. It is the observation vector of the neighboring grid; Spatial correlation and inter-parameter correlation are modeled separately.

[0024] (1) Spatial correlation modeling

[0025] For the same parameter, the spatial correlation between different two-dimensional regular rasters is defined using a Gaussian kernel function: ; in, The covariance between two variables It is a parameter variance It is the kernel width that controls the decay rate.

[0026] (2) Modeling the correlation between parameters

[0027] For the same two-dimensional regular raster, different parameters , , and Introducing a fixed empirical covariance matrix : ; in, For the first The variance of each parameter, For the first The and the first The covariance among the parameters, and the overall covariance matrix is ​​a combination of the Kronecker product of the spatial structure and the parameter structure: ; in, for Spatial kernel similarity matrix between two-dimensional regular grids This represents the Kronecker product operation. When data volume is insufficient and faster sampling is required, the covariance matrix is ​​approximated with a lower rank. ; in For rank, It is a unit array.

[0028] See Figure 3 To quantify the impact of terrain parameter uncertainties on vehicle passability, this invention uses each two-dimensional regular grid... This paper introduces a statistical modeling method using Monte Carlo sampling, and combines it with a high-fidelity multibody dynamics simulation platform and neural networks to predict the vehicle's maneuverability within a grid. Specifically, it performs [the following steps] on each two-dimensional regular grid. Subsequent sampling yields a set of terrain parameter samples. Each sample This represents a set of terrain parameter combinations that may appear on this two-dimensional regular grid. The entire process constitutes an input sample set. .

[0029] In each sample parameter Below, a high-fidelity multibody dynamics simulation platform is used to evaluate the maximum stable speed that the vehicle can achieve in this terrain. That is: ; in, For high-fidelity simulation models, For terrain parameters The maximum speed that a vehicle can maintain without losing stability. Since high-fidelity simulations have high computational overhead during operation and cannot meet real-time requirements, this invention designs an alternative neural network model to approximate the simulation model output.

[0030] ; in, For deep feedforward neural networks, supervised learning is used for training to minimize the mean square error loss between the network's predicted output and the simulation output.

[0031] ; in, For the first High-fidelity simulation output of the samples. This refers to the number of training samples. Based on the input sample set... and The estimated distribution of the maximum stable vehicle speed under this grid is obtained. .

[0032] See Figure 3 After obtaining the maximum stable vehicle speed distribution corresponding to the two-dimensional regular grid, global path planning is performed using RRT. First, the RRT tree is initialized, setting the starting node as... In each expansion, nodes are randomly sampled in free space. And find the nearest node in the tree. This generates a path from point to Extended path segment ,cover A two-dimensional regular grid. For each grid in the path. Use step 3 to perform Monte Carlo sampling of sub-terrain parameters, using a trained neural network model. Predict the maximum stable vehicle speed corresponding to each set of parameters. The maximum stable vehicle speed probability distribution of the two-dimensional regular grid is constructed based on vehicle speed samples. Conditional Value-at-Risk (CVaR) is introduced as a speed risk assessment index to characterize traffic capacity under adverse terrain disturbances. CVaR is a lower-tail risk measure that measures traffic capacity at a certain confidence level. The random variable in the most unfavorable The expected loss level in this situation is defined as follows: ; in, Confidence level Conditional risk value, Confidence level The quantile risk value (Value-at-Risk, VaR) is defined as follows: ; For each grid Maximum vehicle speed distribution Its conditional risk value is: ; The samples are arranged in ascending order. A minimum acceptable vehicle speed threshold is set. A conservative joint approach is used for path segments. The overall traffic capacity is assessed, requiring that the CvaR of all grids in the path segment be no less than [value missing]. ,Right now: ; like If true, then the path segment is considered to have sufficient dynamic feasibility under uncertain conditions, allowing for... Add to the RRT tree; otherwise, determine that the current expansion direction is too risky and should be discarded. Repeatedly sample nodes in free space. and judge This continues until the tree structure successfully reaches the target area and completes the planning.

[0033] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0034] The above description of the disclosed embodiments is intended to enable one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention is not limited to the embodiments shown herein but is intended to conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A special vehicle route planning method considering terrain uncertainty, characterized in that, include: S1: Obtain a terrain map covering the task scene, discretize the terrain map to obtain a two-dimensional regular grid map, define a terrain type label and a slope level label for each two-dimensional regular grid, extract parameter variables from the labels, and process the parameter variables so that the parameter variables all follow an independent one-dimensional Gaussian distribution. S2: For the parameter variables, establish spatial correlation and inter-parameter correlation based on Gaussian distribution, and obtain the terrain parameter condition distribution at the grid center; S3: Monte Carlo sampling is introduced into the terrain parameter condition distribution of each two-dimensional regular grid to obtain the terrain state set. The terrain state set is input into a preset neural network model to obtain the probability distribution of the maximum stable vehicle speed under the corresponding two-dimensional regular grid. The probability distribution map of the maximum vehicle speed is constructed at the whole map level. S4: In the process of rapidly expanding the random tree path, a maximum stable vehicle speed probability distribution based on the uncertainty of terrain parameters is introduced. For each two-dimensional regular grid in the path segment, the conditional risk value is calculated based on its maximum stable vehicle speed probability distribution. It is then determined whether the conditional risk values ​​of all two-dimensional regular grids are greater than or equal to a preset threshold. If they are all greater than or equal to the preset threshold, the path segment is a low-risk path.

2. The special vehicle route planning method considering terrain uncertainty according to claim 1, characterized in that, The definition of terrain type label and slope grade label for each two-dimensional regular raster specifically includes discretizing the entire terrain region into a two-dimensional regular raster map. Each two-dimensional regular grid For a fixed location in geographic space, each grid cell of a two-dimensional rule Define two semantic tags: terrain type tag Slope grade label .

3. The special vehicle route planning method considering terrain uncertainty according to claim 2, characterized in that, The extraction of parameter variables for the labels specifically includes: defining terrain classification maps based on terrain type labels and slope grade labels respectively. Slope classification map Two raster maps, each with a two-dimensional regular raster containing the internal friction coefficient for each terrain type label. Friction angle Shear coefficient Three physical parameter variables, each slope grade label is represented by a two-dimensional regular grid containing the slope. One parameter variable; For terrain type The prior distribution of its physical parameters is as follows: ； in, Indicates terrain type Average internal friction coefficient, The standard deviation of the internal friction coefficient. Indicates terrain type The average friction angle, The standard deviation of the friction angle. Indicates terrain type The average shear coefficient, The standard deviation of the shear coefficient is given for slope grade. Its slope angle distribution is as follows: ； in, Indicates terrain type The average slope This represents the standard deviation of the slope.

4. The special vehicle route planning method considering terrain uncertainty according to claim 1, characterized in that, S2 includes: For any two-dimensional regular grid in the two-dimensional regular grid diagram Define its neighborhood window as centered on it and with a size of A local area, in which The size of the grid neighborhood; ； in, For a two-dimensional regular grid The neighborhood geometry centered on, A two-dimensional rule grid, with a unified definition of the two-dimensional rule grid. slope Internal friction coefficient Friction angle and shear coefficient Four consecutive terrain parameters are , , and ; ； Neighbor windows are arranged in a fixed order. this All parameters of a two-dimensional regular raster are obtained. Dimensional vector: ； in, For the center point Terrain parameters: ； The joint distribution is split into the following structure: ； in, Indicates the remaining neighbors All parameters of a two-dimensional regular raster. Represents the mean of the central grid cell. Represents the mean of neighboring grid cells. It is the autocovariance of the central grid. It is the cooperation between the center and its neighbors. It is the covariance between neighbors. The conditional distribution of the central grid is derived based on the multivariate Gaussian conditional distribution formula: ； in, It is the conditional distribution of the central grid given the neighbor conditions. It is the observation vector of the neighboring grid; Modeling spatial correlation and inter-parameter correlation separately: For the same parameter, the spatial correlation between different two-dimensional regular rasters is defined using a Gaussian kernel function: ； in, The covariance between two variables It is a parameter variance It is the kernel width that controls the decay rate; For the same two-dimensional regular raster, different parameters , , and Introducing a fixed empirical covariance matrix : ； in, For the The variance of each parameter, For the The and the first The covariance among the parameters, and the overall covariance matrix is ​​a combination of the Kronecker product of the spatial structure and the parameter structure: ； in, for Spatial kernel similarity matrix between two-dimensional regular grids This indicates the Kronecker product operation.

5. A special vehicle route planning method considering terrain uncertainty according to claim 1, characterized in that, S3 includes: performing operations on each two-dimensional regular grid. Subsequent sampling yields a set of terrain parameter samples. Input sample set ; in each sample parameter Below, a high-fidelity multibody dynamics simulation platform is used to evaluate the maximum stable speed that the vehicle can achieve in this terrain. The output sample set is established based on the maximum stable speed. ; Output sample set and input sample set Training a pre-defined neural network model: ； ； in, It is a deep feedforward neural network. For the High-fidelity simulation output of the samples. This represents the number of training samples.

6. The special vehicle route planning method considering terrain uncertainty according to claim 1, characterized in that, S4 includes: Initialize a fast-expanding random tree, setting the starting node to... In each expansion, nodes are randomly sampled in free space. And find the nearest node in the tree. This generates a path from point to Extended path segment ,cover A two-dimensional regular grid, for each grid in the path The trained neural network model predicts the maximum stable vehicle speed corresponding to each set of parameters. The maximum stable vehicle speed probability distribution for this grid is constructed based on the vehicle speed samples, and the conditional risk value is defined as: ； in, Confidence level Conditional risk value, Confidence level The quantile risk value below is defined as: ； For each grid Maximum vehicle speed distribution Its conditional risk value is: ； Set minimum acceptable vehicle speed threshold ;like If established, then Add a fast-expanding random tree. If it is a path segment, then discard the current expansion direction and repeatedly sample nodes randomly in free space. and judge This continues until the tree structure successfully reaches the target area and completes the planning.

Images