A method and system for constructing an unstructured environment map based on semantic information

Abstract

The application discloses a kind of unstructured environment map construction method and system based on semantic information, which comprises: obtaining laser radar point cloud image data and pre-processing;Based on the laser radar point cloud image data after pre-processing, grid division and point cloud hit probability prediction are carried out, and an occupancy probability grid map is constructed;Based on the laser radar point cloud image data after pre-processing, height difference value prediction is carried out, and a three-dimensional elevation map is constructed;Obtain environmental image semantic segmentation information;Introduce environmental image semantic segmentation information, integrate occupancy probability grid map and three-dimensional elevation map and carry out GPU parallel computing, and construct environment grid cost map;According to the environment grid cost map, the optimal path planning of unmanned vehicle is carried out.The application can improve the real-time performance of road planning and minimize the cost of payment.The application can be widely applied to the technical field of unmanned vehicle path planning as a kind of unstructured environment map construction method and system based on semantic information.

Description

Technical Field

[0001] This invention relates to the field of autonomous vehicle path planning technology, and in particular to a method and system for constructing unstructured environmental maps based on semantic information. Background Technology

[0002] With the development of automotive intelligence, autonomous driving technology has become an important research direction in the field of intelligent vehicles, and path planning is one of its key technologies. Path planning for intelligent vehicles refers to planning an effective path that is collision-free and can safely reach the target point, based on a certain environmental model and given a starting point and a target point, according to performance indicators.

[0003] Existing technologies for path planning of autonomous vehicles are basically based on two approaches. The first is a path planning method based on extracting point cloud data of the road surface in the current scene. Therefore, the subsequent path planning is carried out within the extracted area. However, this technology has a problem: in the wild environment, road boundaries are difficult to define, and the point cloud structure differs greatly from the point cloud features in structured urban scenes. Therefore, traditional plane fitting methods or deep learning-based point cloud segmentation methods are difficult to achieve good extraction results. The second approach is a path planning method based on extracting obstacles in the current scene. This technology considers all points in the current scene that are not obstacles to be safe points, and the subsequent path is planned within the point cloud data excluding obstacles. However, it has several problems: First, the obstacle recognition of this technology mostly relies on data annotation, so it can only identify a few categories specified in the data annotation. However, it is difficult to predict obstacles that may exist in the real wild in advance, and it will fail when it encounters data that has not been trained. Second, not all obstacles are insurmountable. For example, low and sparse grass may be classified as obstacles by laser or image neural networks, but medium and large unmanned vehicles can run over them normally without affecting safety. If the idea of ​​simply avoiding all obstacles is adopted, the path may miss the optimal planning and take a detour. Summary of the Invention

[0004] To address the aforementioned technical problems, the present invention aims to provide a method and system for constructing unstructured environmental maps based on semantic information, which can improve the real-time performance of road planning and minimize costs.

[0005] The first technical solution adopted in this invention is: a method for constructing unstructured environment maps based on semantic information, comprising the following steps:

[0006] Acquire lidar point cloud image data and perform preprocessing to obtain preprocessed lidar point cloud image data;

[0007] Based on the preprocessed lidar point cloud image data, grid division and point cloud hit probability prediction are performed to construct an occupation probability grid map.

[0008] Based on the preprocessed lidar point cloud image data, height difference prediction is performed to construct a three-dimensional elevation map.

[0009] Based on the SAM model, semantic segmentation of the environment of autonomous vehicles is performed to obtain semantic segmentation information of the environment image.

[0010] By incorporating semantic segmentation information from environmental images, integrating occupancy probability raster maps and 3D elevation maps, and performing GPU parallel computation, an environmental raster cost map is constructed.

[0011] Optimal path planning for autonomous vehicles is performed based on an environmental grid cost map.

[0012] Furthermore, the step of acquiring and preprocessing the lidar point cloud image data to obtain preprocessed lidar point cloud image data specifically includes:

[0013] Acquire lidar point cloud image data;

[0014] The lidar point cloud image data is processed to correct lidar motion distortion, and the lidar point cloud image data is preliminarily corrected.

[0015] The horizontal angle correction process of the initially corrected lidar point cloud image data is performed to obtain the corrected lidar point cloud image data.

[0016] The corrected lidar point cloud image data is filtered to obtain filtered lidar point cloud image data.

[0017] The filtered lidar point cloud image data is subjected to voxel grid filtering to obtain preprocessed lidar point cloud image data.

[0018] Furthermore, the step of performing lidar motion distortion correction processing on the lidar point cloud image data to obtain preliminarily corrected lidar point cloud image data specifically includes:

[0019] Based on the lidar point cloud image data, reference point cloud data and moving point cloud data are obtained. The reference point cloud data represents the set of real point cloud points without distortion, and the moving point cloud data represents the set of real point cloud data collected.

[0020] Initialize and set rotation and translation matrices, which are used to perform rotation and translation transformations on the moving point cloud data and align it with the reference point cloud data;

[0021] The rotation and translation matrices are iteratively calculated using the ICP point cloud registration method until the error between the reference point cloud data and the moving point cloud data meets the preset accuracy requirements, thus obtaining the preliminarily corrected lidar point cloud image data.

[0022] Furthermore, the specific expressions for iteratively solving the rotation and translation matrices using the ICP point cloud registration method are as follows:

[0023]

[0024]

[0025] X' I =X I -u x

[0026] P' I =P i -u p

[0027] In the above formula, u x u represents the geometric center of the moving point cloud data. p X' represents the geometric center of the reference point cloud data. I P' represents the point cloud points in the decentralized motion point cloud data. I N represents the coordinates of points in the decentralized reference point cloud data. x X represents the number of point clouds. i X represents all points in the point cloud cluster X. I Let X represent the centralized point cloud cluster, Np represent the number of point clouds clusters P, and Pi represent all the point clouds in the point cloud cluster P.

[0028] Furthermore, the step of constructing an occupancy probability grid map based on preprocessed lidar point cloud image data for grid division and point cloud hit probability prediction specifically includes:

[0029] The preprocessed lidar point cloud image data is divided into grids to obtain an occupied grid map. The occupied grid map includes several grids, and the state of the grids includes occupied state, idle state and unknown state.

[0030] Determine the probability of a grid being occupied and the probability of a grid not being occupied, wherein the unoccupied state includes an idle state and an unknown state;

[0031] Calculate the ratio of the probability of a grid being occupied to the probability of a grid not being occupied, perform a transformation, and construct the grid occupancy conditional probability value.

[0032] The probability of a grid being occupied is added to the probability value of the grid occupancy condition to obtain the probability value of the first grid state.

[0033] The probability of a grid not being occupied is calculated by subtracting the probability value of the grid being occupied from the probability value of the grid being occupied.

[0034] Integrate the state probability values ​​of the first grid cell and the state probability values ​​of the second grid cell to construct an occupation probability grid map.

[0035] Furthermore, the expression for the occupancy probability grid map is as follows:

[0036] l(m i |x 1:t ,z 1:t )=l(m i |x t ,z t )+l(m i |x 1:t-1 ,z 1:t-1 )-l(m i )

[0037] In the above formula, x 1:t z represents the map building process over the first t time intervals. 1:t m represents the sensor reading at time t. i Represents a raster, x t Let z represent the predicted value at time t. t Let x represent the hidden state at time t. 1:t-1 Indicates t1 to t t-1 The total observation value at time z 1:t-1 Let l(m) represent the total hidden state at the preceding time step. i |x 1:t ,z 1:t ) represents the final probability of occupying the current time, l(m) i |x t ,z t ) represents the model value at that time given the current observation and hidden state, l(m i |x 1:t-1 ,z 1:t-1 ) represents the occupancy value at the preceding time step, l(m) i ) represents the probability that a grid cell is occupied.

[0038] Furthermore, the step of predicting height differences based on preprocessed lidar point cloud image data and constructing a three-dimensional elevation map specifically includes:

[0039] The preprocessed lidar point cloud image data is divided into raster segments to obtain the raster to be processed.

[0040] The raster to be processed is transformed into the world coordinate system to obtain the coordinate values ​​of the raster;

[0041] The translational and rotational Jacobian matrices of the grid to be processed are obtained by the sensor, and the variance is calculated to obtain the elevation variance value within the grid.

[0042] The translational and rotational Jacobian matrices of the grid to be processed are obtained by the sensor, and the covariance is calculated to obtain the elevation covariance value within the grid.

[0043] The current and previous grid height values ​​of the grid to be processed are obtained by the sensor and fused by one-dimensional Kalman filtering to obtain the grid elevation estimate.

[0044] By integrating the coordinate values ​​of the raster, the elevation variance values ​​within the raster, the elevation covariance values ​​within the raster, and the elevation estimates of the raster, a three-dimensional elevation map is constructed.

[0045] Furthermore, the step of introducing environmental image semantic segmentation information, integrating the occupancy probability raster map and the 3D elevation map, and performing GPU parallel computing to construct the environmental raster cost map specifically includes:

[0046] The collision probability of a grid cell is determined based on the occupancy probability grid map;

[0047] The roughness parameters, slope parameters, and height uncertainty parameters of the raster are determined based on the 3D elevation map.

[0048] By integrating the roughness parameter, slope parameter, and height uncertainty parameter of the grid, the passage cost of the grid is determined.

[0049] By combining the collision probability and passage cost of the grid and performing multi-threaded GPU computation, an environmental grid cost map is constructed.

[0050] Furthermore, the expression for the corresponding grid cost of the environmental grid cost map is as follows:

[0051] Cost i =f a *cost occupancy +f b *cost elevation -f c *vehicle traversability

[0052] In the above formula, Cost i f represents the raster cost. a f b and f c Cost represents the normalization factor.occupancy The cost represents the collision probability of a grid. elevation The vehicle represents the passage cost of each grid cell. traversability This indicates the off-road capability of driverless cars.

[0053] The second technical solution adopted in this invention is: an unstructured environment map construction system based on semantic information, comprising:

[0054] The preprocessing module is used to acquire and preprocess lidar point cloud image data to obtain preprocessed lidar point cloud image data.

[0055] The first construction module is used to perform grid division and point cloud hit probability prediction based on preprocessed lidar point cloud image data, and to construct an occupation probability grid map.

[0056] The second construction module is used to predict the height difference based on the preprocessed lidar point cloud image data and construct a three-dimensional elevation map.

[0057] The semantic segmentation module is used to perform semantic segmentation processing of the autonomous vehicle environment based on the SAM model to obtain semantic segmentation information of the environment image.

[0058] The third building module is used to introduce environmental image semantic segmentation information, integrate the occupancy probability raster map and the three-dimensional elevation map and perform GPU parallel computing to build an environmental raster cost map.

[0059] The optimal path planning module is used to plan the optimal path for autonomous vehicles based on the environmental grid cost map.

[0060] The beneficial effects of the method and system of this invention are as follows: This invention acquires and preprocesses LiDAR point cloud image data to avoid the influence of motion distortion when the vehicle moves at high speed and excessive point cloud points when there are many objects in the field environment. Furthermore, based on the preprocessed LiDAR point cloud image data, it divides the data into grids to construct an occupancy probability grid map and a three-dimensional elevation map. The occupancy probability grid map can provide the collision probability of the grid, and the three-dimensional elevation map can display information such as terrain undulation and obstacle height. This enables the assignment of passage cost for each type of obstacle, thereby avoiding the possibility of missing the optimal path planning. Further, it performs environmental semantic segmentation processing for unmanned vehicles, assigning costs when the vehicle drives through roads, low grass, dense bushes, and areas occupied by solid obstacles. Finally, it constructs an environmental grid cost map for optimal path planning of unmanned vehicles, improving the real-time performance of road planning and minimizing cost. Attached Figure Description

[0061] Figure 1This is a flowchart illustrating the steps of an embodiment of the present invention for constructing an unstructured environment map based on semantic information;

[0062] Figure 2 This is a structural block diagram of an unstructured environment map construction system based on semantic information according to an embodiment of the present invention;

[0063] Figure 3 This is a schematic diagram of the process for constructing an environmental grid cost map according to a specific embodiment of the present invention;

[0064] Figure 4 This is a schematic diagram of the structure of a real point cloud data set collected in a specific embodiment of the present invention;

[0065] Figure 5 This is a schematic diagram of the structure for laser radar motion distortion correction processing according to a specific embodiment of the present invention;

[0066] Figure 6 This is a schematic diagram of the structure of the lidar after motion distortion correction processing according to a specific embodiment of the present invention;

[0067] Figure 7 This is a schematic diagram of the process for performing horizontal angle correction of point cloud data according to a specific embodiment of the present invention;

[0068] Figure 8 This is a schematic diagram of the preprocessing process for lidar point cloud image data according to a specific embodiment of the present invention;

[0069] Figure 9 This is a schematic diagram illustrating the process of constructing an occupancy probability grid map according to a specific embodiment of the present invention. Detailed Implementation

[0070] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments. The step numbers in the following embodiments are only for ease of explanation and do not limit the order of the steps. The execution order of each step in the embodiments can be adapted according to the understanding of those skilled in the art.

[0071] First, the technical terms of this invention will be explained:

[0072] Unstructured environment: Compared to the clearly defined roads, regularly shaped buildings, and fewer types of obstacles in urban scenes, the edges of roads in the wild are difficult to define accurately, and the types of obstacles in the scene are difficult to define in advance, exhibiting the characteristics of an unstructured scene.

[0073] Occupied Grid Probability Map: This represents an effective robot navigation method that estimates the state probability of each grid cell based on sensor data and updates the map accordingly. It divides a two-dimensional space into grids, each corresponding to a probability value representing the likelihood that the grid cell represents an obstacle. In an occupied grid map, each grid cell has only three states: Occupied, Free, and Unknown. Occupied indicates that the corresponding two-dimensional space is occupied (i.e., there is an obstacle), Free indicates no obstacle, and Unknown indicates that the grid cell has not yet been detected or its state cannot be determined. Sensor data is typically used to acquire obstacle information when constructing an occupied grid map. However, due to noise and limitations in sensor data, it is often impossible to accurately obtain the state information of each grid cell. Therefore, when processing sensor data, assumptions and probabilistic models are usually used to estimate the state probability of each grid cell. For example, a Gaussian distribution can be used to characterize the noise problem, and different parameters can be set to represent the probability distribution of different states. When updating the map, data from two laser sensors is typically used to obtain the position and distance information of obstacles. Then, the state probability of each grid is updated based on this information. For example, if one laser sensor measures a grid closer than another sensor, then that grid is considered occupied. Conversely, if the measurements from the two sensors differ significantly, then the grid is considered idle.

[0074] A 3D elevation map is a virtual map constructed using 3D modeling technology, displaying information such as terrain undulations and building heights. This type of map is generated by collecting and processing data from sources such as LiDAR and satellite imagery, then using software like GIS (Geographic Information System) for data processing and modeling to ultimately produce a 3D elevation map. In a 3D elevation map, terrain and buildings are presented in three dimensions, allowing users to observe and browse the map from different angles. Users can also perform terrain analysis, view analysis, and route planning. This type of map is of great significance for urban planning, land resource surveys, and environmental protection, and can also be used in tourism, gaming, and other fields. Generating a 3D elevation map requires collecting high-precision LiDAR and satellite imagery data, followed by data processing and modeling using GIS software. During modeling, the data needs to be denoised and filtered to reduce the impact of noise and errors on map accuracy. Terrain analysis and view analysis are also required to determine information such as terrain undulations and building heights. Finally, the processed data is imported into 3D modeling software for model construction and rendering, ultimately generating the 3D elevation map. Therefore, a 3D elevation map is a virtual map constructed using 3D modeling technology, capable of displaying information such as terrain undulations and building heights. This type of map is of great significance in urban planning, land resource surveys, and environmental protection, and can also be used in tourism, gaming, and other fields. Generating a 3D elevation map requires collecting high-precision lidar data and satellite imagery data, and then using GIS and other software for data processing and modeling.

[0075] This invention provides a map construction method for the safe driving of unmanned vehicles in unstructured outdoor environments. This method utilizes data information provided by various types of sensors, and comprehensively combines the positional information of objects in the environment from LiDAR with the semantic information of objects from cameras. Finally, it obtains the regional passage cost at each location in the path, thereby helping the unmanned vehicle to achieve optimal path planning.

[0076] Reference Figure 1 and Figure 3 This invention provides a method for constructing unstructured environment maps based on semantic information, the method comprising the following steps:

[0077] S1. Acquire lidar point cloud image data and perform preprocessing to obtain preprocessed lidar point cloud image data.

[0078] Specifically, because the raw lidar data collected may be affected by factors such as coordinate axis shifts caused by vehicle bumps, adverse reflections in bad weather, motion distortion when the vehicle is moving at high speeds, and excessive point cloud points when there are many objects in the field, preprocessing of the lidar data points is necessary. Figure 8 As shown.

[0079] S11, LiDAR motion distortion correction;

[0080] Specifically, lidar images require an infrared light transmitter to emit and return infrared light, which is then detected using Time-of-Flight (ToF) detection to determine the distance and location of environmental obstacles. However, there is a time difference between the infrared light emission time t0 and the reception time t1. When the vehicle travels at a high speed, the point cloud image at time t0 will be distorted, thus requiring distortion correction. In this embodiment of the invention, the ICP (Iterative Closest Point) iterative method is used for motion distortion removal. First, reference point cloud data and moving point cloud data are acquired, and the expression is:

[0081] X = {X1, X2, X3, ..., Xn}

[0082] P = {P1, P2, P3, ..., Pn}

[0083]

[0084] In the above formula, X represents the collected point cloud set, P represents the real point cloud set without distortion, n represents the number of point cloud points, R represents the rotation matrix between the two sets, t represents the translation matrix between the two sets, E(R,t) represents the transformation matrix between the two coordinate systems, Np represents the number of point cloud points in the point cloud cluster, Pi represents a point cloud point in the point cloud cluster P, and Xi represents a point cloud point in the point cloud cluster X that corresponds to that point in P.

[0085] like Figure 4 , Figure 5 and Figure 6 As shown, the original problem is transformed into a mathematical optimization problem: finding the optimal rotation and translation matrices to minimize the error between the two point cloud sets. Since the true point cloud positions are unpredictable, we need to randomly assign initial R and t matrix values ​​and iterate continuously to minimize the cumulative error between the two sets. The solution method is shown below:

[0086]

[0087]

[0088] X' I =X I -u x

[0089] P' I =P i -u p

[0090] In the above formula, u xu represents the geometric center of the moving point cloud data. p X' represents the geometric center of the reference point cloud data. I P' represents the point cloud points in the decentralized motion point cloud data. I N represents the coordinates of points in the decentralized reference point cloud data. x X represents the number of point clouds. i X represents all points in the point cloud cluster X. I Let X represent the centralized point cloud cluster, Np represent the number of point clouds clusters P, and Pi represent all the point clouds in point cloud clusters P.

[0091] The vectors of the two sets can then be centered as follows:

[0092]

[0093] In the above formula, T represents the matrix transpose, λ represents the matrix eigenvalues, W represents the vector centering representation, and n P U represents the number of points in the point cloud cluster, and U and V are intermediate matrix expressions obtained after matrix decomposition.

[0094] The optimal solution for ICP is:

[0095] R = VU T

[0096] t = u x -Ru p

[0097] In the above formula, u p This represents the geometric center of the point cloud in point cloud cluster P.

[0098] S12, Point cloud data horizontal angle correction;

[0099] Specifically, in unstructured outdoor environments, the supporting plane experiences significant turbulence, causing large angular abrupt changes in the horizontal direction of each frame of the point cloud image. This can lead to a shift in the relative coordinate position of the same object between consecutive frames. Therefore, an attitude sensor unit is needed to correct the horizontal angle of the point cloud. For example... Figure 7 As shown, the light gray plane is the plane before correction, and the black plane is the plane after correction.

[0100] S13, Point cloud data filtering;

[0101] Specifically, due to the dense vegetation in the field, the number of reflected lidar point clouds is enormous, which drastically increases the computation time required by the computing unit. Furthermore, in adverse weather conditions, air humidity and dew can affect lidar reflection, necessitating proper filtering of noise points in the resulting point cloud images. Therefore, voxel grid filtering and statistical filtering are employed to process the point cloud data.

[0102] S14, Voxel grid filtering;

[0103] Specifically, voxel grid filtering is a commonly used filtering method in point cloud processing. It divides the point cloud data into cubes (or voxels), and then samples or averages the points within each voxel, thereby reducing the density of the point cloud data. Statistical outlier removal is a method commonly used to remove outliers from point clouds. It is based on statistical principles to identify and remove points that are significantly different from their surroundings. Its basic principle is to calculate the statistical characteristics (such as mean and standard deviation) of each point's surroundings, and then determine whether a point is an outlier based on a set threshold. If a point's attribute value deviates significantly from the mean of its surrounding points, it is considered an outlier. The expression is:

[0104]

[0105] In the above formula, P avg P represents the point cloud representation of each grid cell after downsampling. i It represents all point cloud points in a given grid.

[0106] S2. Based on the preprocessed lidar point cloud image data, perform grid division and point cloud hit probability prediction to construct an occupation probability grid map.

[0107] Specifically, such as Figure 9 As shown, by dividing the current scene into multiple grids and considering the number and attributes of LiDAR point cloud points in each grid, a probabilistic occupancy grid suitable for unmanned vehicle path planning can be established. Each grid can be divided into three states: occupied P(mi) = 1, idle P(mi) = 0, or unknown P(mi) = 0.5. Therefore, establishing an occupancy grid map can be described as follows:

[0108] p(m|x 1:t ,z 1:t )=Πp(m i |x 1:t ,z 1:t

[0109] In the above formula, x1:t z represents the sensor observations of the map over the first t time intervals. 1:t p(m|x) represents the sum of states at time t. 1:t ,z 1:t ) represents the sum of hidden states and sensor observations given in the preceding time steps, which gives the probability of the event occurring at the current time step.

[0110] The conversion formula for occupying a raster map can be converted into the probability that mi is occupied, as shown below:

[0111]

[0112] In the above formula, p(m) i |x 1:t ,z 1:t ) represents the given observation data x 1:t and hidden state z 1:t The conditional probability of event m represents the probability of finding the desired event m given the observed data and the hidden state. i The probability of occurrence; p(z) t |m i ,z 1:t-1 ,x 1:t ) represents a given model m i The hidden state z at time t-1 1:t-1 and observation data x 1:t The hidden state z at the current moment t Conditional probability; p(m) i ∣z 1:t-1 ,x 1:t The expression represents the model m given the hidden state and observed data at previous time steps. i The probability of occurrence; p(z) t ∣z 1:t-1 ,x 1:t The expression represents the model m given the hidden state and observed data at the previous time step. i The probability of occurrence; p(z) t |m i ,x t p(m) represents the conditional probability of the hidden state at the current time. i ∣z 1:t-1 ,x 1:t-1 p(z) represents the model probability at the current time step given the hidden state and observations from previous time steps. t ∣z 1:t-1 ,x 1:t z represents the current hidden state obtained given the hidden state at the previous time step and the observed value. 1:t-1 Let x represent the total hidden state representation of the preceding time step. 1:t-1z represents the total number of observations at preceding time steps. t Indicates the current state, x t This represents the observed value at the current moment.

[0113] Similarly, for Probability of not being occupied:

[0114]

[0115] In the above formula, Represents grid m i The probability of not being occupied. Indicates a given preceding time t1~t t The conditional probability that a grid cell is not occupied, summing the observed values ​​and hidden states. p(z) represents the conditional probability that a given observation and hidden state will not be occupied. t |x t () represents the conditional probability value of the state at the current time, derived from the sensor observations at the current time. Indicates a given preceding time t1~t t-1 The conditional probability that a grid cell is not occupied, summing the observed values ​​and hidden states. This indicates the probability that a given moment is not occupied, derived solely from sensor observations. Represents grid m i The probability of not being occupied.

[0116] in,

[0117]

[0118] pass This can be converted to the following:

[0119]

[0120] For term p(x), the occupancy probability that each lidar point within the corresponding grid can provide is log-odd (conditional probability value):

[0121]

[0122] In the above formula, l(x) represents the formula for calculating the conditional probability value, and p(x) represents the probability that a certain grid cell is occupied.

[0123] Therefore, the final description of the occupancy probability grid can be derived as follows:

[0124] l(m i |x 1:t ,z 1:t )=l(m i |xt ,z t )+l(m i |x 1:t-1 ,z 1:t-1 )-l(m i )

[0125] In the above formula, x 1:t z represents the map building process over the first t time intervals. 1:t m represents the sensor reading at time t. i Represents a raster, x t Let z represent the predicted value at time t. t Let x represent the hidden state at time t. 1:t-1 Indicates t1 to t t-1 The total observation value at time z 1:t-1 Let l(m) represent the total hidden state at the preceding time step. i |x 1:t ,z 1:t ) represents the final probability of occupying the current time, l(m) i |x t ,z t ) represents the model value at that time given the current observation and hidden state, l(m i |x 1:t-1 ,z 1:t-1 ) represents the occupancy value at the preceding time step, l(m) i ) represents the probability that a grid cell is occupied.

[0126] S3. Based on the preprocessed lidar point cloud image data, perform height difference prediction and construct a three-dimensional elevation map;

[0127] Specifically, the definition of a passable area differs for autonomous vehicles of different sizes. Therefore, it is necessary to statistically analyze the absolute height, height difference, and height covariance of each grid cell in the road to provide height information for the final passage cost map. For each grid cell in the environment, its elevation map can be represented as (X... i ,Y i H i ,σ i ,∑P i Let X be represented as X. i Y i These are the coordinates of the raster, H. i σ is the elevation estimate of the raster. i Let ∑P be the variance of elevation within the grid. i This represents the elevation covariance value within the grid.

[0128] First, obtain the formula for calculating variance:

[0129]

[0130] In the above formula, J s J Φ This represents the Jacobian matrix for translation and rotation generated using sensor coordinates. Represents variance. The covariance matrix representing the sensor rotation;

[0131] J S With J Φ The Jacobian matrices for translation and rotation, generated using sensor coordinates, are calculated as follows:

[0132]

[0133]

[0134] In the above formula, P represents the projection of the 3D point cloud points onto the map space, P = [1, 0, 0], C(Φ SM ) T This represents the mathematical description of the corresponding rotation matrix. This indicates that J is obtained. S The partial derivative, This represents the partial derivative of the rotation matrix from the sensor coordinate system to the map coordinate system. Φ represents the partial derivative of the coordinate terms in the sensor coordinate system. SM This represents the rotation expression from the sensor coordinate system to the map coordinate system. This represents the coordinate expression after projection from the sensor coordinate system through the projection matrix.

[0135] The grid height value measured by the sensor at the current moment is fused with the grid elevation value recorded at the previous moment using a one-dimensional Kalman filter to finally establish the height value of each grid cell:

[0136]

[0137] In the above formula, This represents the raster elevation value at the previous time step. This represents the variance of the raster elevation at previous time steps. This represents the observed value at the current time. Represents variance. This represents the updated height value.

[0138] S4. Perform semantic segmentation of the environment of unmanned vehicles based on the SAM model to obtain semantic segmentation information of the environment image;

[0139] Specifically, the cost incurred by a vehicle when it passes through an area varies depending on the different objects in the environment. For example, the cost incurred when a vehicle passes through a road, low grass, dense bushes, or an area occupied by a solid obstacle is: road < low grass < dense bushes < solid obstacle.

[0140] Therefore, it is necessary to determine the semantic information of each object in the environment. In this embodiment of the invention, the SAM (Segment Anything Model) model is used for semantic segmentation. The SAM model provides significant assistance for image segmentation due to its powerful segmentation capabilities and the ability to require only a small amount of annotation. After running, the SAM model can obtain the semantic categories of different objects in the environment, which helps in subsequent cost calculation.

[0141] S5. Introduce environmental image semantic segmentation information, integrate the occupancy probability raster map and the 3D elevation map and perform GPU parallel computing to construct an environmental raster cost map.

[0142] Specifically, based on the obtained environment occupancy probability grid map, environment grid map, and environmental object semantic information, the passage cost for each grid can be calculated. Specifically, for each grid... i In other words, the collision probability of passing through a grid cell can be obtained using the occupancy probability grid map as follows:

[0143] cost occupancy =P(grid) i )

[0144] In the above formula, P represents the occupancy probability of a grid cell, and cost... occupancy This indicates the cost of passing through this grid.

[0145] Using an elevation map, the roughness, slope, and height uncertainty of the grid can be calculated;

[0146] Roughness is the variance of the elevation within the grid, and its expression is:

[0147]

[0148] In the above formula, cost roughness This indicates the cost of passing through this grid.

[0149] The slope is defined as the difference in elevation between the current grid cell and its eight adjacent grid cells, and its expression is:

[0150]

[0151] In the above formula, cost slopeThis indicates the cost of traversing this grid cell, and height represents the current grid cell height. i This indicates the height value of the surrounding grid.

[0152] High uncertainty is defined as:

[0153] Cost uncertainty =||λ1+λ2+λ3-3||

[0154] In the above formula, λ1, λ2, and λ3 represent the eigenvalues ​​in the covariance matrix, and Cost uncertainty This indicates the cost of passing through this grid.

[0155] Finally, the passage cost per grid obtained using the elevation map is:

[0156] Cost elevation =w r *cost roughness +w s *cost slope +w u *cost uncertainty

[0157] w r +w s +w u =1

[0158] In the above formula, w r w s w u These represent influencing factors, reflecting the magnitude of the impact of roughness, slope, and height measurement uncertainties within the grid on the access cost of that grid.

[0159] Considering all the uncertainties affecting the raster, the cost of the raster is:

[0160] Cost i =f a *cost occupancy +f b *cost elevation -f c *vehicle traversability

[0161] In the above formula, Cost i f represents the raster cost. a f b and f c Cost represents the normalization factor. occupancy The cost represents the collision probability of a grid. elevation fc*vehicle represents the passage cost of each grid cell. traversabilityThis indicates the off-road capability of the autonomous vehicle, which is set based on the size of the vehicle and the radius of its wheels.

[0162] Due to the large number of point cloud points and the need to obtain pixel-level semantics from the images, the standard algorithm process consumes significant computational resources. Furthermore, autonomous vehicles travel at high speeds, requiring high real-time planning capabilities; therefore, the entire algorithm needs acceleration. Firstly, leveraging the high image processing speed of GPUs, all matrix operation modules involved in this algorithm can be transferred from the CPU to the GPU, accelerating computation and saving CPU resources for subsequent steps. Simultaneously, elevation map construction and occupancy probability map construction can utilize multi-threaded computation, enabling the concurrent construction of various map types and improving the algorithm's real-time efficiency.

[0163] S6. Optimal path planning for autonomous vehicles is performed based on the environmental grid cost map.

[0164] Specifically, after establishing a cost map for the current environment, the Dijkstra algorithm with weighted paths and A can be used. * The algorithm, or the sampling-based RRT algorithm, is used for subsequent path planning.

[0165] Reference Figure 2 A semantic information-based unstructured environment map construction system includes:

[0166] The preprocessing module is used to acquire and preprocess lidar point cloud image data to obtain preprocessed lidar point cloud image data.

[0167] The first construction module is used to perform grid division and point cloud hit probability prediction based on preprocessed lidar point cloud image data, and to construct an occupation probability grid map.

[0168] The second construction module is used to predict the height difference based on the preprocessed lidar point cloud image data and construct a three-dimensional elevation map.

[0169] The semantic segmentation module is used to perform semantic segmentation processing of the autonomous vehicle environment based on the SAM model to obtain semantic segmentation information of the environment image.

[0170] The third building module is used to introduce environmental image semantic segmentation information, integrate the occupancy probability raster map and the three-dimensional elevation map and perform GPU parallel computing to build an environmental raster cost map.

[0171] The optimal path planning module is used to plan the optimal path for autonomous vehicles based on the environmental grid cost map.

[0172] The content of the above method embodiments is applicable to this system embodiment. The specific functions implemented in this system embodiment are the same as those in the above method embodiments, and the beneficial effects achieved are also the same as those achieved in the above method embodiments.

[0173] The above is a detailed description of the preferred embodiments of the present invention. However, the present invention is not limited to the embodiments described. Those skilled in the art can make various equivalent modifications or substitutions without departing from the spirit of the present invention. All such equivalent modifications or substitutions are included within the scope defined by the claims of this application.

Claims

1. A method for constructing unstructured environment maps based on semantic information, characterized in that, Includes the following steps: Acquire lidar point cloud image data and perform preprocessing to obtain preprocessed lidar point cloud image data; Based on the preprocessed lidar point cloud image data, grid division and point cloud hit probability prediction are performed to construct an occupation probability grid map. Based on the preprocessed lidar point cloud image data, height difference prediction is performed to construct a three-dimensional elevation map. Based on the SAM model, semantic segmentation of the environment of autonomous vehicles is performed to obtain semantic segmentation information of the environment image. The collision probability of a grid cell is determined based on the occupancy probability grid map; Determine the raster roughness parameter, raster slope parameter, and raster height uncertainty parameter based on the 3D elevation map; By integrating the roughness parameter, slope parameter, and height uncertainty parameter of the grid, the passage cost of the grid is determined. By combining the collision probability and passage cost of the grid and performing multi-threaded GPU computation, an environmental grid cost map is constructed. The specific expression for the corresponding grid cost of the environmental grid cost map is as follows: ； In the above formula, Represents the raster cost. , and Represents the normalization factor. The cost representing the collision probability of the grid. This represents the passage cost for each grid cell. Indicates the off-road capability of driverless cars; Optimal path planning for autonomous vehicles is performed based on an environmental grid cost map.

2. The method for constructing unstructured environment maps based on semantic information according to claim 1, characterized in that, The step of acquiring and preprocessing LiDAR point cloud image data to obtain preprocessed LiDAR point cloud image data specifically includes: Acquire lidar point cloud image data; The lidar point cloud image data is processed to correct lidar motion distortion, and the lidar point cloud image data is preliminarily corrected. The horizontal angle correction process of the initially corrected lidar point cloud image data is performed to obtain the corrected lidar point cloud image data. The corrected lidar point cloud image data is filtered to obtain filtered lidar point cloud image data. The filtered lidar point cloud image data is subjected to voxel grid filtering to obtain preprocessed lidar point cloud image data.

3. The method for constructing unstructured environment maps based on semantic information according to claim 2, characterized in that, The step of performing laser radar motion distortion correction processing on the laser radar point cloud image data to obtain preliminarily corrected laser radar point cloud image data specifically includes: Based on the lidar point cloud image data, reference point cloud data and moving point cloud data are obtained. The reference point cloud data represents the set of real point cloud points without distortion, and the moving point cloud data represents the set of real point cloud data collected. Initialize and set rotation and translation matrices, which are used to perform rotation and translation transformations on the moving point cloud data and align it with the reference point cloud data; The rotation and translation matrices are iteratively calculated using the ICP point cloud registration method until the error between the reference point cloud data and the moving point cloud data meets the preset accuracy requirements, thus obtaining the preliminarily corrected lidar point cloud image data.

4. The method for constructing an unstructured environment map based on semantic information according to claim 3, characterized in that, The specific expressions for iteratively solving the rotation and translation matrices using the ICP point cloud registration method are as follows: ； ； ； ； In the above formula, Represents the geometric center of the moving point cloud data. Indicates the geometric center of the reference point cloud data. This represents the points in the decentralized motion point cloud data. This represents the coordinates of points in the decentralized reference point cloud data. Indicates the number of point clouds. express All point points in a point cloud cluster Represents the centralized Dotted clouds, express Number of point clouds, express All the point cloud points in the point cloud cluster.

5. The method for constructing unstructured environment maps based on semantic information according to claim 4, characterized in that, The step of constructing an occupancy probability grid map based on preprocessed LiDAR point cloud image data, involving grid division and point cloud hit probability prediction, specifically includes: The preprocessed lidar point cloud image data is divided into grids to obtain an occupied grid map. The occupied grid map includes several grids, and the state of the grids includes occupied state, idle state and unknown state. Determine the probability of a grid being occupied and the probability of a grid not being occupied, wherein the unoccupied state includes an idle state and an unknown state; Calculate the ratio of the probability of a grid being occupied to the probability of a grid not being occupied, perform a transformation, and construct the grid occupancy conditional probability value. The probability of a grid being occupied is added to the probability value of the grid occupancy condition to obtain the probability value of the first grid state. The probability of a grid not being occupied is calculated by subtracting the probability value of the grid being occupied from the probability value of the grid being occupied. Integrate the state probability values ​​of the first grid cell and the state probability values ​​of the second grid cell to construct an occupation probability grid map.

6. The method for constructing an unstructured environment map based on semantic information according to claim 5, characterized in that, The specific expression for the occupancy probability grid map is as follows: ； In the above formula, Indicates the preceding The map creation process at time intervals, Indicates the preceding Sensor readings at any given time Represents a grid. express Predicted value at time, express Hide your status at all times. express arrive Total observations at time 10:00 This represents the total hidden state in the preceding time step. This represents the probability of the current time step being completed. This represents the model value at that moment, given the current observation and hidden state. This represents the occupancy value at the previous time step. This indicates the probability that a grid cell is occupied.

7. The method for constructing an unstructured environment map based on semantic information according to claim 6, characterized in that, The step of predicting height differences based on preprocessed lidar point cloud image data and constructing a 3D elevation map specifically includes: The preprocessed lidar point cloud image data is divided into raster segments to obtain the raster to be processed. The raster to be processed is transformed into the world coordinate system to obtain the coordinate values ​​of the raster; The translational and rotational Jacobian matrices of the grid to be processed are obtained by the sensor, and the variance is calculated to obtain the elevation variance value within the grid. The translational and rotational Jacobian matrices of the grid to be processed are obtained by the sensor, and the covariance is calculated to obtain the elevation covariance value within the grid. The current and previous grid height values ​​of the grid to be processed are obtained by the sensor and fused by one-dimensional Kalman filtering to obtain the grid elevation estimate. By integrating the coordinate values ​​of the raster, the elevation variance values ​​within the raster, the elevation covariance values ​​within the raster, and the elevation estimates of the raster, a three-dimensional elevation map is constructed.

8. A system for constructing unstructured environment maps based on semantic information, characterized in that, Includes the following modules: The preprocessing module is used to acquire and preprocess lidar point cloud image data to obtain preprocessed lidar point cloud image data. The first construction module is used to perform grid division and point cloud hit probability prediction based on preprocessed lidar point cloud image data, and to construct an occupation probability grid map. The second construction module is used to predict the height difference based on the preprocessed lidar point cloud image data and construct a three-dimensional elevation map. The semantic segmentation module is used to perform semantic segmentation processing of the autonomous vehicle environment based on the SAM model to obtain semantic segmentation information of the environment image. The third building module is used to determine the collision probability of a grid based on the occupancy probability grid map; Determine the raster roughness parameter, raster slope parameter, and raster height uncertainty parameter based on the 3D elevation map; By integrating the roughness parameter, slope parameter, and height uncertainty parameter of the grid, the passage cost of the grid is determined. By combining the collision probability and passage cost of the grid and performing multi-threaded GPU computation, an environmental grid cost map is constructed. The specific expression for the corresponding grid cost of the environmental grid cost map is as follows: ； In the above formula, Indicates the raster cost. , and Represents the normalization factor. The cost representing the collision probability of the grid. This represents the passage cost for each grid cell. Indicates the off-road capability of driverless cars; The optimal path planning module is used to plan the optimal path for autonomous vehicles based on the environmental grid cost map.

Images