A method for planning a path of a special vehicle considering terrain uncertainty

Abstract

The application discloses a special vehicle path planning method considering terrain uncertainty, and relates to the technical field of path planning, and comprises the following steps: acquiring a two-dimensional regular grid map of a discrete terrain map, defining a label for each grid, and extracting and processing parameter variables of the label; establishing spatial correlation and parameter correlation based on Gaussian distribution, introducing Monte Carlo sampling, obtaining a terrain state set, inputting the terrain state set into a preset neural network model, obtaining a probability distribution of the maximum stable vehicle speed under the corresponding grid, introducing the maximum stable vehicle speed probability distribution constructed based on terrain parameter uncertainty in the process of rapidly expanding the random tree path expansion, calculating the conditional risk value, judging whether the conditional risk values of all grids are greater than or equal to a preset threshold value, and if all the conditional risk values are greater than or equal to the preset threshold value, the path segment is a low-risk path. The application improves the path planning traffic safety and robustness of the vehicle in the complex unknown terrain.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of path planning, in particular to a special vehicle path planning method considering terrain uncertainty. BACKGROUND

[0002] At present, the automatic driving path planning problem in complex unstructured environment has been highly valued 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 signs.

[0003] In such an environment, the influence of terrain conditions on the passability of special vehicles is much higher than that in traditional urban road scenarios. Path planning not only needs to consider geometric accessibility, but also needs to integrate the evaluation of ground physical properties and their uncertainty.

[0004] Current mainstream methods use deterministic terrain mapping and static heuristic planning strategies. Although terrain classification and passability estimation modules are introduced to improve the environmental adaptability of path planning, they still lack modeling and propagation mechanisms for physical terrain parameters, making it difficult to reflect physical uncertainty in real scenarios. In addition, most systems assume that vehicle passability is a static binary value, ignoring the dynamic constraint effect of terrain on vehicle speed.

[0005] Therefore, how to improve the environmental adaptability of path planning considering terrain is a problem that needs to be solved by those skilled in the art. SUMMARY

[0006] Therefore, the present application provides a special vehicle path planning method considering terrain uncertainty, which improves the environmental adaptability of path planning considering terrain, and realizes modeling, propagation and path planning of terrain uncertainty.

[0007] In order to achieve the above purpose, the present application adopts the following technical solutions:

[0008] A special vehicle path planning method considering terrain uncertainty, comprising:

[0009] S1: Obtain a terrain map covering a task scene, discretize the terrain map to obtain a two-dimensional regular grid map, define terrain type labels and slope level labels for each two-dimensional regular grid, extract parameter variables from the labels, and process the parameter variables so that the parameter variables all conform to independent one-dimensional Gaussian distribution;

[0010] S2: For the parameter variables, establish spatial correlation and parameter correlation based on Gaussian distribution to obtain terrain parameter conditional distribution of the center of the grid;

[0011] S3: Introduce Monte Carlo sampling to the terrain parameter condition distribution of each two-dimensional regular grid, obtain a terrain state set, input the terrain state set into a preset neural network model, and obtain a probability distribution of the maximum stable vehicle speed under the corresponding two-dimensional regular grid, and construct a probability distribution map of the maximum vehicle speed at the whole map level;

[0012] S4: Introduce the maximum stable vehicle speed probability distribution constructed based on the terrain parameter uncertainty in the rapid expansion of the random tree path expansion process, calculate the conditional risk value of each two-dimensional regular grid in the path segment based on the maximum stable vehicle speed probability distribution thereof, and judge whether the conditional risk values of all two-dimensional regular grids are greater than or equal to a preset threshold value, if all the conditional risk values are greater than or equal to the preset threshold value, the path segment is a low-risk path.

[0013] Preferably, the definition of the terrain type label and the slope grade label for each two-dimensional regular grid specifically comprises discretizing the entire terrain area into a two-dimensional regular grid map , each two-dimensional regular grid corresponds to a fixed position on the geographic space, and for each two-dimensional regular grid , two semantic labels are defined: a terrain type label and a slope grade label .

[0014] Preferably, the parameter variable extraction of the label specifically comprises: defining a terrain classification map and a slope classification map based on the terrain type label and the slope grade label, respectively, each two-dimensional regular grid of each terrain type label contains three physical parameter variables: an internal friction resistance coefficient , a friction angle , and a shear coefficient , and each two-dimensional regular grid of each slope grade label contains one parameter variable: a slope .

[0015] The prior distribution of the physical parameters of the terrain type is as follows:

[0016] ;

[0017] wherein, represents the average internal friction resistance coefficient of the terrain type , is the standard deviation of the internal friction resistance coefficient, represents the average friction angle of the terrain type , is the standard deviation of the friction angle, represents the average shear coefficient of the terrain type , For the standard deviation of the shear coefficient, for the slope class with the slope angle distribution as

[0018] ;

[0019] wherein, denotes the average slope of the terrain type , is the standard deviation of the slope.

[0020] Preferably, S2 comprises:

[0021] For any two-dimensional regular grid in a two-dimensional regular grid map, its neighborhood window is defined as a local area with it as the center and a size of , wherein is the grid neighborhood size;

[0022] ;

[0023] wherein, is the neighborhood geometry with the two-dimensional regular grid as the center, a total of two-dimensional regular grids, uniformly define the slope , , , and shear coefficient four continuous terrain parameters of , , and ;

[0024] ;

[0025] Arrange all parameters of the two-dimensional regular grids in the neighborhood window in a fixed order to obtain a dimensional vector:

[0026] ;

[0027] wherein, , for the terrain parameters of the center point :

[0028] ;

[0029] Split the joint distribution as follows:

[0030] ;

[0031] wherein, denotes the rest of the neighborhood

[0032]

[0033]

[0034]

[0035]

[0036]

[0037]

[0038]

[0039]

[0040]

[0041]

[0042] ​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​This indicates the Kronecker product operation.

[0043] 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. ;

[0044] Output sample set and input sample set Training a pre-defined neural network model:

[0045] ;

[0046] ;

[0047] 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.

[0048] Preferably, S4 includes:

[0049] 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:

[0050] ;

[0051] in, Confidence level Conditional risk value, Confidence level The quantile risk value below is defined as:

[0052] ;

[0053] the maximum vehicle speed distribution of each grid , whose conditional risk value is:

[0054] ;

[0055] Set the minimum acceptable vehicle speed threshold ; if is true, add to the rapidly expanding random tree, is the path segment; otherwise, discard the current expansion direction, repeat the random sampling of nodes in the free space , and judge , until the tree structure successfully reaches the target area to complete the planning.

[0056] Through the above technical solutions, compared with the prior art, the special vehicle path planning method considering terrain uncertainty is provided, and the beneficial effects are:

[0057] 1. The terrain physical properties are introduced as the mapping elements in the present application, the joint probability distribution of each grid is constructed, and diversified terrain states are generated through Monte Carlo sampling, which can more finely reflect the complexity and uncertainty of the terrain compared with the traditional binary map.

[0058] 2. The present application adopts a neural network to map the terrain combination to the maximum stable vehicle speed in view of the nonlinear relationship between the high-dimensional terrain parameters and the vehicle dynamics performance, replacing the real-time simulation process, and improving the real-time performance of the path planning.

[0059] 3. The present application introduces the conditional risk value as the passing safety index in the rapidly expanding random tree, dynamically evaluates the lowest vehicle speed in the worst case, and compared with the traditional path expansion strategy based on static heuristic, can effectively avoid the high-risk area, and improve the task success rate and robustness. DETAILED DESCRIPTION

[0060] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or the prior art description will be briefly introduced below, and obviously, the drawings in the following description are only embodiments of the present application, and for those skilled in the art, other drawings can be obtained without creative labor on the basis of the provided drawings.

[0061] Figure 1 The two-dimensional regular grid label assignment and physical property extraction step schematic diagram provided by the present application;

[0062] Figure 2 ​This is a schematic diagram of the grid conditional distribution modeling steps based on spatial-parameter cocorrelation provided by the present invention;

[0063] Figure 3 The flowchart of terrain response modeling and neural network learning combined with high-fidelity simulation provided by this invention;

[0064] Figure 4 A schematic diagram of the safety constraint RRT path planning for fusion condition risk assessment provided by the present invention. Detailed Implementation

[0065] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0066] 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:

[0067] (1) Terrain type label ,represent A predefined landform type, the label of which can be determined by a semantic segmentation model or prior rules;

[0068] (2) Slope rating label ,represent Each slope range is derived from a digital elevation map or slope calculation.

[0069] 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:

[0070] ;

[0071] 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:

[0072] ;

[0073] 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.

[0074] 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.

[0075] ;

[0076] 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 Four consecutive terrain parameters are , , and .

[0077] ;

[0078] Neighbor windows are arranged in a fixed order. this All parameters of a two-dimensional regular raster are obtained. Dimensional vector:

[0079] ;

[0080] in, For the center point Terrain parameters:

[0081] ;

[0082] The joint distribution is split into the following structure:

[0083] ;

[0084] 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:

[0085] ;

[0086] in, It is the conditional distribution of the central grid given the neighbor conditions. It is the observation vector of the neighboring grid;

[0087] Spatial correlation and inter-parameter correlation are modeled separately.

[0088] (1) Spatial correlation modeling

[0089] For the same parameter, the spatial correlation between different two-dimensional regular rasters is defined using a Gaussian kernel function:

[0090] ;

[0091] in, The covariance between two variables It is a parameter variance It is the kernel width that controls the decay rate.

[0092] (2) Modeling the correlation between parameters

[0093] For the same two-dimensional regular raster, different parameters , , and Introducing a fixed empirical covariance matrix :

[0094] ;

[0095] 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:

[0096] ;

[0097] 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.

[0098] ;

[0099] in For rank, It is a unit array.

[0100] 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. .

[0101] 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:

[0102] ;

[0103] 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.

[0104] ;

[0105] 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.

[0106] ;

[0107] 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. .

[0108] 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:

[0109] ;

[0110] in, Confidence level Conditional risk value, Confidence level The quantile risk value (Value-at-Risk, VaR) is defined as follows:

[0111] ;

[0112] For each grid Maximum vehicle speed distribution Its conditional risk value is:

[0113] ;

[0114] 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:

[0115] ;

[0116] 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.

[0117] 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.

[0118] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those 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 invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded 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 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.

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 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.

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 first 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