Automatic driving vehicle feature map construction method based on laser radar and low orbit satellite fusion, vehicle initial positioning method and enhanced path planning method

Abstract

The invention discloses a laser radar and low orbit satellite fusion-based autonomous vehicle feature map construction method, a vehicle initial positioning method and an enhanced path planning method, and the method comprises the steps: generating a high-precision DEM feature map through integrating the three-dimensional topographic data collected by a laser radar and the positioning information of a low orbit satellite; and the space precision and the environment perception capability of the automatic driving automobile map are improved. According to the data fusion initialization positioning method, the DEM feature map and the satellite data are effectively fused, and the accuracy of initial positioning is improved. Besides, aiming at the problem that a traditional path planning algorithm only considers distance factors and does not fully consider terrain elevation and gradient information, the invention provides a path planning method based on DEM data enhancement, and by defining a cost function comprehensively considering distance, gradient and elevation difference and optimizing path selection, the driving safety and efficiency of the automatic driving vehicle are improved. Experimental results show that the method has high feasibility and practicability in a complex terrain environment.

Description

Technical Field

[0001] The present invention relates to an autonomous driving map construction technology, specifically a method for constructing a feature map for an autonomous driving vehicle based on the fusion of a laser radar and a low-orbit satellite, a method for initial vehicle positioning, and an enhanced path planning method. Background Art

[0002] In the field of autonomous driving, high-precision maps are critical infrastructure for ensuring safe and efficient vehicle operation. However, current high-precision maps have significant shortcomings in terms of the detailed and real-time nature of terrain data. These maps often lack sufficient elevation detail, limiting the vehicle's ability to understand the environment in complex terrain. Furthermore, terrain data often cannot be updated frequently enough to meet the real-time requirements of autonomous vehicles, further exacerbating these issues.

[0003] Existing high-precision maps have obvious defects in terrain data, mainly manifested in the lack of detailed terrain information, which makes it difficult for autonomous vehicles to accurately understand the environment in complex terrain. For example, in scenarios such as ramps and steep slopes, due to inaccurate terrain data, the vehicle may not be able to make the optimal path selection, thus affecting driving safety and efficiency. In addition, the initial positioning stage also faces challenges, especially in environments lacking obvious features, such as parking lots or rural roads, where the inaccuracy of terrain data further exacerbates positioning errors. In addition, traditional path planning algorithms usually only consider distance factors and do not fully consider terrain elevation and slope information. As a result, the planned path may pass through steep or high-energy consumption sections, affecting the safety and efficiency of autonomous vehicles. Summary of the Invention

[0004] Purpose of the invention: In response to the shortcomings of existing high-precision maps in terms of terrain data, the present invention proposes a DEM feature layer construction method based on the fusion of lidar and low-orbit satellite data. By integrating the high-precision three-dimensional terrain data collected by the on-board lidar and the precise positioning information provided by the low-orbit satellite, a high-precision DEM is generated to improve the spatial accuracy and environmental perception capabilities of the autonomous driving map, support path planning and safe driving, and solve the problems of terrain understanding, initial positioning and path planning optimization in autonomous driving. In response to the difficult problem of inaccurate terrain data affecting path planning and positioning, the present invention uses on-board lidar to obtain high-precision three-dimensional terrain data, and combines it with the precise positioning information of the low-orbit satellite to generate a high-precision DEM feature layer map. At the same time, a data fusion initialization positioning method is proposed to effectively fuse the DEM feature layer map and satellite data to improve the accuracy of initial positioning. In addition, the present invention addresses the problem that traditional path planning algorithms (such as A* or Dijkstra algorithms) generally only consider distance factors and fail to fully consider terrain elevation and slope information. This paper proposes a path planning method based on DEM data enhancement. By defining a cost function that comprehensively considers distance, slope and elevation difference, it optimizes path selection, thereby improving the driving safety and efficiency of autonomous vehicles.

[0005] Technical solution: The process of the autonomous driving feature map construction method based on the fusion of lidar and low-orbit satellite proposed in this invention is as follows: Figure 1 Shown, including:

[0006] S1: Vehicle-side data collection:

[0007] S1.1: Use a high-definition LiDAR sensor mounted on the vehicle to collect high-resolution LiDAR data at a frequency of 10Hz, ensuring detailed coverage of the environment. The LiDAR used is a 128-line HD LiDAR that provides 1 million point cloud data points per second.

[0008] S1.2: Receive precise positioning data via low earth orbit (LEO) satellites. Satellite data provides a global coordinate reference for the lidar point cloud, ensuring the geographic consistency of the data.

[0009] S2: Digital Elevation Model (DEM) Generation and Feature Map Creation:

[0010] S2.1: Preprocessing of LiDAR and LEO satellite data, including noise removal, coordinate system alignment, and time stamp synchronization, ensures data accuracy and consistency. Furthermore, a multi-source data fusion algorithm is employed to further optimize data quality, eliminate sensor errors, and enhance data accuracy.

[0011] S2.2: Kriging interpolation is used to generate a high-precision DEM from the fused data. Smoothing is then performed to reduce noise and improve the continuity and accuracy of the DEM. Subsequently, a quality assessment is performed, including comparison with measured elevation data and error analysis, to ensure the accuracy and reliability of the DEM.

[0012] S2.3: Extract terrain features such as slope, height, and terrain type from the high-precision DEM processed in S2.2 and integrate them into a navigable feature map compatible with the autonomous driving system. Furthermore, a mapping algorithm is used to generate a point cloud map using the data fused in S2.1 for positioning and navigation.

[0013] S3: Application of DEM feature layer map:

[0014] S3.1: Initial vehicle positioning using feature maps. Initial vehicle positioning is performed using feature maps through feature matching algorithms to ensure accurate positioning within the environment.

[0015] S3.2: Enhanced Path Planning with DEM Data: Integrating DEM data into path planning algorithms (such as A* or Dijkstra) optimizes paths based on terrain elevation, ensuring safer and more efficient navigation.

[0016] Preferably, the pre-processing of the lidar and satellite data in S2.1 is specifically as follows:

[0017] In the present invention, LiDAR data denoising uses statistical filters to perform local neighborhood analysis. By calculating the neighborhood height mean and standard deviation of each point in the point cloud, noise points exceeding the threshold are identified and removed, and then the LiDAR ground point is obtained using the angle differential algorithm.

[0018] Satellite data processing includes outlier detection and removal, data interpolation, and time synchronization to ensure data accuracy and consistency. Coordinate system alignment is achieved by determining the transformation matrix, using ground control points for registration, and automatically fitting rotation, translation, and scaling parameters.

[0019] Finally, the data fusion uses particle filters to fuse lidar data and low-orbit satellite data to optimize the coordinates of the location control points.

[0020] First, the LiDAR data is denoised and ground points are extracted. A point in the point cloud is selected and its neighborhood range is defined (such as a spherical area with a radius of r). The mean μ and standard deviation σ of the height Z coordinates of all points in the neighborhood are calculated. The next step is to detect outliers. For each point, the difference between its height and the mean is calculated. For points with a height of z p point, if |z p-u|>kσ, it is considered to be noise. All points marked as noise are removed to obtain cleaner point cloud data. The neighborhood radius r should be adjusted according to the point cloud density and the expected noise scale. The threshold coefficient k needs to be determined experimentally to balance the denoising effect and retain details. In the last step, the three-dimensional coordinates of each point and its adjacent points in the lidar point cloud are used to calculate the slope angle of the line between them relative to the horizontal plane. Subsequently, the calculated angle is compared with a maximum ground slope threshold (for example, 5 degrees) that is preset (and adjustable) based on terrain experience. If the absolute value of the angle is less than or equal to this threshold, the terrain at the point is considered to change gently and it is identified as a ground point. Finally, the Z coordinate values ​​of these identified ground points (that is, their vertical height in the lidar coordinate system) are directly extracted as the elevation information of the point.

[0021] Next is the low-orbit satellite data processing, the detailed steps are as follows:

[0022] The first step is outlier detection and removal: a median filter is used to remove impulse noise from satellite signals, and thresholds are set for altitude and speed to remove data points that are outside a reasonable range.

[0023] The second step is to interpolate the data: for missing data points, linear interpolation or spline interpolation method is used for estimation. The linear interpolation formula is:

[0024]

[0025] where t i and t i+1 is the acquisition time corresponding to the known data points i and i+1, y(t i ) and y(t i+1 ) is the corresponding data value, t is the interpolation time point, and y(t) refers to the satellite measurement data location information (such as longitude, latitude, height / altitude)

[0026] The third step is time synchronization and coordinate system alignment. First, the timestamp difference between the lidar and low-orbit satellite data is determined. This difference is then used to time-align the satellite and lidar data. Coordinate system alignment involves obtaining the fitted transformation matrix T, which includes rotation, translation, and scaling parameters, using extrinsic calibration software (autoware). A selection of data is selected and the coordinate difference before and after alignment is calculated, ensuring that the difference is within the allowable error range.

[0027] The fourth step is data fusion. The details are as follows:

[0028] The first step is to construct the state equation, which is as follows:

[0029] x k =Fx k-1 +Gu k-1 +wk-1 (2)

[0030] In formula (2), x k =[p,v] T is the system state vector, p is the position, v is the velocity, which can be calculated by transforming the coordinates of the two-frame point cloud. F is the transfer matrix, which describes how the system state naturally evolves from the previous moment k-1 to the current moment k according to its intrinsic dynamics. G is the control matrix, which controls the input u k-1 The effect is mapped to the change of the state vector, u k-1 is the control input, representing the known external control effect applied to the system, w k-1 It is process noise, which represents the random uncertainty or disturbance that cannot be accurately described by the model itself and affects the actual state evolution of the system.

[0031] The observation equation is as follows:

[0032] z k =Hx k +v k (3)

[0033] Among them, z k is the observation value, which is the filtered low-orbit satellite data. H is the observation matrix, which defines the relationship between the true state of the system and the ideal (error-free) measurement value of the sensor, that is, how to map from the true state to the measurement space, v k is the observation noise, which represents the random errors or uncertainties in the measurement process (such as sensor accuracy limitations, signal interference, etc.).

[0034] Generate a set of particles based on the equation of state:

[0035]

[0036] This formula describes the prediction step in the particle filter algorithm, which uses the state transition probability defined by the state prediction equation and process noise. Random sampling is performed to construct a particle set representing the current state prediction. N is the number of particles, x k is the system state vector.

[0037] Then calculate the observation weights:

[0038]

[0039] is the observation weight, which indicates the matching degree between the particle and the observation value.

[0040] The last step is state estimation, which estimates the current state based on the particle's weight:

[0041]

[0042] By combining the lidar and low-orbit satellite data in this way, high-precision ground control point coordinate information and elevation information are obtained after data fusion. This data is used to calibrate the lidar and low-orbit satellite data and verify the accuracy of the DEM.

[0043] Preferably, S2.2 is specifically: using the Kriging interpolation method to combine the lidar ground point cloud data, low-orbit satellite data and ground control point position information to calculate the digital elevation model (DEM). The Kriging interpolation method is an interpolation method based on spatial autocorrelation, which can use the elevation values ​​of known points to predict the elevation values ​​of unknown points.

[0044] The variogram is the core tool of the Kriging interpolation method and is used to describe the autocorrelation of spatial data. It quantifies the variability of spatial data by calculating the relationship between the elevation difference and distance between known points. The calculation formula of the variogram is:

[0045]

[0046] γ(h) is the variance function value when the distance is h, Z(s i ) is s i The data comes from LiDAR point cloud data and satellite data with known elevation points.

[0047] Calculate the distance h between all known points. For each distance h, calculate the average of the squared elevation differences between pairs of points to obtain γ(h). Based on the experimental variogram results, fit a theoretical variogram model, including spherical and Gaussian models. Using the elevation values ​​of known points and the variogram model, calculate the predicted value of the unknown point. To this end, it is necessary to construct a Kriging equation system and solve the weight coefficient λ. i .

[0048]

[0049] λ i is the weight coefficient, indicating that the known point s i Contribution to the unknown point s0, γ(s i ,s j ) represents the known point s i and s j The variogram value between two points. μ is the Lagrange multiplier, used as a constraint, s0 is the unknown point to be predicted, and n represents the total number of known data points used in the kriging interpolation calculation. The data comes from the elevation values ​​of the known points and the fitted theoretical variogram model.

[0050] After completing the variogram analysis and weight calculation, Kriging interpolation can be performed on each grid point in the study area to generate a DEM. The prediction formula of Kriging interpolation is:

[0051]

[0052] is the predicted elevation value at position s0, Z(s i ) is the position s i The elevation value of the location, λ i is the Kriging weight coefficient. The data comes from lidar point cloud data, satellite data and ground control points.

[0053] In this way, the study area is divided into regular grid points, and the predicted elevation value of each grid point s0 is calculated using the Kriging interpolation formula. The elevation values ​​of all grid points are combined to generate a DEM.

[0054] However, the generated DEM may contain noise or unreasonable elevation values, which requires smoothing. The DEM is convolved with a Gaussian kernel to smooth the elevation values.

[0055] The final step is quality assessment, which aims to verify the accuracy and reliability of the DEM. The details are as follows:

[0056] Calculate the RMSE of the difference between the DEM elevation value estimated at the ground control point and the actual elevation value. The elevation value at the ground control point is calculated by S2.1, and the calculation formula is as follows:

[0057]

[0058] Z DEM (p i ) is p i The estimated elevation value at Z(p i ) is p i The true elevation value of the ground control point location.

[0059] If the calculated RMSE is large, it indicates that the accuracy of the DEM is low. It may be necessary to recalculate the DEM and adjust the parameters of the Kriging interpolation method and the variation function model. If the number of known points is insufficient, more known elevation points can be added to improve the interpolation accuracy.

[0060] Preferably, S2.3 specifically includes the following: After generating a high-precision DEM, terrain features (such as slope, height, and terrain type) can be further extracted and integrated into a navigable feature map compatible with the autonomous driving system. Simultaneously, a point cloud map is generated using a mapping algorithm using the fused data (such as lidar point cloud and image data) for positioning and navigation of the autonomous driving system.

[0061] Slope, the degree of inclination of a terrain surface, is one of the most important terrain features in autonomous driving systems. Slope information can help vehicles determine the drivability of a road. The first step is slope extraction, using DEM data to calculate the slope of each grid point. The slope calculation formula is:

[0062]

[0063] and It is the rate of change of elevation Z in the x and y directions. The slope value is stored as raster data in degrees.

[0064] Height information is a direct output of DEM, indicating the elevation of each grid point. For example:

[0065] Flat land is defined as slope < 5°, and slope is defined as 5° < slope < 20°. The terrain type classification results are stored as raster data.

[0066] Terrain type is the result of classifying the land surface based on slope, height, and other terrain features (such as curvature). Common terrain types include flat land, slopes, ridges, valleys, etc.

[0067] Finally, the fused data (lidar point cloud and low-orbit satellite data) is used to generate a point cloud map through a mapping algorithm for positioning and navigation of the autonomous driving system.

[0068] Preferably, S3.1 specifically includes: utilizing GPS to provide a rough position and correcting the GPS altitude information using a DEM to improve vertical positioning accuracy. Terrain features (such as slope) are extracted from the DEM and matched with environmental features acquired by the vehicle's lidar sensor to achieve rapid initial positioning.

[0069] Preferably, S3.2 specifically includes: Integrating digital elevation model (DEM) data into route planning can significantly improve navigation safety and efficiency. By considering terrain elevation and slope information, the present invention can optimize route selection and avoid steep or high-energy-consumption sections.

[0070] Define a cost function that takes into account distance, slope, and elevation values:

[0071] Cost = distance × (1 + k1 slope + k2 elevation) (11)

[0072] k1 and k2 are weight coefficients, which are adjusted according to vehicle performance and actual needs.

[0073] If Dijkstra's algorithm is used for planning, the weight of each edge is calculated using the above cost function, and the path with the minimum total cost is selected.

[0074] For sections with steep slopes, set them to be impassable or assign them extremely high costs. Consider special treatment for downhill sections, such as for energy conservation or safety reasons.

[0075] Beneficial effects of the present invention:

[0076] 1. To address the problem of insufficient accuracy caused by a single data source in traditional DEM generation, this paper proposes a method that fuses lidar and low-orbit satellite data and uses kriging interpolation to generate high-precision DEM.

[0077] 2. To address the problem of insufficient initial positioning accuracy in autonomous driving, the present invention proposes an initialization positioning method based on the fusion of DEM feature maps and satellite data.

[0078] 3. In view of the fact that traditional path planning algorithms (such as A* or Dijkstra algorithm) usually only consider the distance factor but fail to fully consider the terrain elevation and slope information, this paper proposes a path planning method based on DEM data enhancement. By defining a cost function that comprehensively considers distance, slope and elevation difference, the path selection is optimized. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 It is the overall flow chart of the present invention.

[0080] Figure 2 Visualization data of lidar ground control points output by S2.1.

[0081] Figure 3 Output digital elevation model for S2.2 of the present invention. DETAILED DESCRIPTION

[0082] To address the shortcomings of existing high-precision maps in terms of terrain data, this paper proposes a method for generating digital elevation models (DEMs) based on the fusion of lidar and low-orbit satellite data. This method aims to address the challenges of terrain understanding, initial positioning, and path planning optimization in autonomous driving. By integrating high-precision three-dimensional terrain data collected by on-board lidar with precise positioning information provided by low-orbit satellites, a high-precision DEM feature layer map is generated, significantly improving the spatial accuracy and environmental perception capabilities of autonomous driving maps. This solution proposes a data fusion initialization positioning method that effectively integrates the DEM feature layer map with satellite data to improve initial positioning accuracy. Furthermore, to address the problem that traditional path planning algorithms (such as A* or Dijkstra) typically only consider distance and fail to fully account for terrain elevation and slope information, this paper proposes a path planning method based on DEM data enhancement. By defining a cost function that comprehensively considers distance, slope, and elevation difference, this method optimizes path selection, thereby improving the safety and efficiency of autonomous vehicles. Experimental results demonstrate the feasibility and practicality of this method in complex terrain environments, effectively supporting path planning and initial positioning for autonomous vehicles in a variety of complex scenarios, including urban roads, mountainous terrain, and underground parking lots.

[0083] The present invention will be further described below with reference to the accompanying drawings and examples.

[0084] Example 1. Use Jiangsu University scene data to complete the construction of the autonomous driving map feature layer by integrating lidar and low-orbit satellite.

[0085] This case study, conducted on the Jiangsu University campus, demonstrated the practical effectiveness of a method for constructing autonomous driving map feature layers based on the fusion of lidar and low-orbit satellite data. The Jiangsu University campus features diverse terrain, including flat land, slopes, buildings, and complex roads, providing ideal conditions for testing DEM generation and path planning algorithms. The experimental equipment included a 128-channel high-definition lidar and a low-orbit satellite receiver. The lidar collected high-resolution point cloud data at a 10Hz rate, delivering 1 million points per second, ensuring detailed coverage of the environment. The low-orbit satellite data, sampled at a 1Hz rate, provided GPS coordinates and elevation information, providing a global coordinate reference for the lidar point cloud and ensuring geographic consistency. Multiple ground control points were deployed on the campus, and their precise coordinates were obtained using high-precision surveying equipment for DEM generation and verification. As the experimental vehicle drove at a constant speed on campus, the lidar continuously collected point cloud data of the surrounding environment, covering various terrain features such as roads, buildings, and slopes. Satellite data, including the vehicle's GPS coordinates and elevation information, was collected simultaneously to ensure data preprocessing. Denoising of the lidar data was performed first. A statistical filter is used for local neighborhood analysis. By calculating the mean and standard deviation of the neighborhood height for each point in the point cloud, noise points exceeding a threshold are identified and removed. Next, an angular differential algorithm is used to extract ground points and obtain their elevation values ​​(Z coordinates). For satellite data, outlier detection and removal are first performed. A median filter is used to remove impulse noise from the satellite signal. Height and velocity thresholds are set to remove data points outside a reasonable range. Missing data points are estimated using linear or spline interpolation to ensure data continuity. Time synchronization and coordinate system alignment are key steps in data preprocessing. The satellite and lidar data are time-aligned by calculating the timestamp difference between the lidar and satellite data and interpolating them. Coordinate system alignment is achieved using extrinsic calibration software (such as Autoware). The transformation matrix T, including rotation, translation, and scaling parameters, is fitted to ensure that the lidar and satellite data are in the same coordinate system. Finally, a particle filter is used to fuse the lidar and satellite data to optimize the coordinates of the ground control points, generating high-precision ground control point coordinates and elevation information for subsequent DEM generation and verification. The temporal synchronization and spatial consistency of the generated lidar ground control point visualization data are as follows: Figure 2 shown.

[0086] During the DEM generation phase, Kriging interpolation is used to calculate a digital elevation model (DEM) using LiDAR point cloud data, low-orbit satellite data, and ground control point location information. Kriging interpolation is an interpolation method based on spatial autocorrelation that predicts the elevation of unknown points using the elevation values ​​of known points. First, the distance h between all known points is calculated. For each distance h, the average of the squared elevation differences between pairs of points is calculated to obtain an experimental variogram. Based on the experimental variogram results, a theoretical variogram model is fitted. Commonly used models include spherical and Gaussian models. Using the elevation values ​​of the known points and the variogram model, a system of Kriging equations is constructed, and the weight coefficients are solved to calculate the predicted elevation values ​​of the unknown points. The study area is divided into a regular grid of points. The predicted elevation value is calculated for each grid point using the Kriging interpolation formula. The elevation values ​​of all grid points are then combined to generate the DEM. The generated DEM may contain noise or unreasonable elevation values, so smoothing is required. Convolution of the DEM with a Gaussian kernel smoothes the elevation values ​​to improve the continuity and accuracy of the DEM. Finally, perform a quality assessment and calculate the difference between the elevation value of the DEM at the ground control point and the actual elevation value to verify the accuracy and reliability of the DEM. If the calculated RMSE is large, it means that the accuracy of the DEM is low, and you may need to recalculate the DEM, adjust the parameters and variation function model of the Kriging interpolation method, or add more known elevation points to improve the interpolation accuracy. The effect of the generated digital elevation model DEM is as follows: Figure 3 shown.

[0087] During the initial vehicle positioning phase, a feature map is used for initial positioning. Real-time LiDAR data is matched to the feature map using a feature matching algorithm to ensure precise positioning within the environment. First, a rough GPS position is provided, and the GPS elevation information is corrected using a DEM to improve vertical positioning accuracy. Next, terrain features (such as slope and aspect) are extracted from the DEM and matched with environmental features acquired by vehicle sensors (such as LiDAR). Real-time point cloud data is matched to the feature map using a feature matching algorithm (such as ICP or NDT) to calculate the vehicle's position and attitude. By combining DEM data with GPS correction, horizontal positioning accuracy is better than 1 meter and vertical positioning accuracy is better than 0.5 meter, ensuring precise positioning in complex environments. During the path planning phase, an enhanced path planning algorithm that integrates DEM data significantly improves navigation safety and efficiency. Traditional path planning algorithms (such as A* or Dijkstra's algorithm) typically only consider distance and fail to fully account for terrain elevation and slope information, potentially resulting in planned paths passing through steep or energy-intensive sections. This solution integrates DEM data into the path planning algorithm, defines a cost function that comprehensively considers distance, slope, and elevation difference, and optimizes path selection. The cost function formula is a weighted combination of path length, average slope, and elevation change. Road sections with excessive slopes are set to be impassable or assigned extremely high costs to prevent vehicles from entering dangerous areas. Special treatment of downhill sections, such as energy conservation or safety factors, is considered to further optimize path planning results. In campus road tests, the path planning algorithm successfully avoided steep slopes and high-energy consumption sections, selecting safer and more energy-efficient paths, significantly improving the safety and efficiency of vehicle driving.

[0088] Example 2. Further experiments on constructing an autonomous driving map feature layer were conducted on Guyang Road in Jingkou District, Zhenjiang City.

[0089] This example was conducted on Guyang Road in Jingkou District, Zhenjiang City. Guyang Road is a typical urban road with high traffic volume and a complex and changing road environment, including multiple intersections, ramps, and buildings. This provides a more challenging scenario for testing the autonomous driving map feature layer construction method. The experimental equipment is the same as in Example 1, including a 128-line high-definition lidar, a low-orbit satellite receiver, and ground control points to ensure high-precision and consistent data acquisition.

[0090] The experimental vehicle traveled at a constant speed on Guyang Road, while a lidar collected high-resolution point cloud data. Low-orbit satellites simultaneously provided GPS coordinates and elevation information. Data preprocessing included lidar data denoising, satellite data outlier detection and interpolation, and coordinate registration. A statistical filter was used to remove noise points and extract ground points. Median filtering and linear interpolation were performed on the satellite data to ensure data continuity. Finally, a particle filter was used to fuse the lidar and satellite data to generate high-precision ground control point coordinates.

[0091] A high-precision DEM is generated using Kriging interpolation. A spherical model is fitted, the Kriging equations are constructed, and the weight coefficients are solved. The generated DEM is Gaussian smoothed to reduce noise. Features such as slope, height, and terrain type are extracted from the DEM to generate a navigable feature map and point cloud map. Slope information is used to determine road drivability, and terrain type classification results (e.g., flat land, slope) are stored as raster data for route planning.

[0092] GPS information is corrected using feature maps and DEM data, and real-time lidar data is matched with feature maps through feature matching algorithms (such as ICP) to ensure accurate vehicle positioning. Horizontal positioning accuracy is better than 1 meter, and vertical positioning accuracy is better than 0.5 meter. In path planning, DEM data is integrated to optimize the A* algorithm, and a cost function that comprehensively considers distance, slope, and elevation difference is defined to optimize path selection. For sections with excessive slopes, they are set to impassable or assigned extremely high costs to prevent vehicles from entering dangerous areas. In actual road tests on Guyang Road, the path planning algorithm successfully avoided steep slopes and high-energy consumption sections and selected a safer and more energy-efficient path.

[0093] The series of detailed descriptions listed above are only specific descriptions of feasible implementation methods of the present invention. They are not intended to limit the scope of protection of the present invention. Any equivalent methods or changes that do not deviate from the technology of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for constructing a feature map for an autonomous vehicle based on the fusion of lidar and low-orbit satellites, characterized in that: The steps include: S1: Vehicle-side data acquisition: including collecting lidar data and receiving low-orbit satellite positioning data; S2: Generate digital elevation model (DEM) and create feature map based on the collected data.

2. The method for constructing a feature map for an autonomous driving vehicle based on the fusion of laser radar and low-orbit satellite according to claim 1, characterized in that: The specific implementation of S1 includes: S1.1: Use a high-definition lidar sensor mounted on the vehicle to collect high-resolution lidar data at a frequency of 10 Hz, ensuring detailed coverage of the environment. The lidar used is a 128-line high-definition lidar that provides 1 million point cloud data points per second. S1.2: Receive precise positioning data via low-Earth orbit satellites. Satellite data provides a global coordinate reference for the LiDAR point cloud, ensuring the geographic consistency of the data.

3. The method for constructing a feature map for an autonomous driving vehicle based on the fusion of laser radar and low-orbit satellite according to claim 2, characterized in that: The specific implementation of S2 includes: S2.1: Preprocess LiDAR and LEO satellite positioning data to ensure data accuracy and consistency. Simultaneously, a multi-source data fusion algorithm is used to fuse LiDAR and LEO satellite positioning data to optimize data quality, eliminate sensor errors, and improve data accuracy. S2.2: The fused data is interpolated using Kriging to generate a high-precision DEM. Smoothing is then performed to reduce noise and improve the continuity and accuracy of the DEM. Subsequently, the generated DEM is quality assessed to ensure its accuracy and reliability. S2.3: Extract terrain features such as slope, height, and terrain type from the DEM obtained in S2.2, integrate them into a navigable feature map compatible with the autonomous driving system, and use the data fused in S2.1 using a mapping algorithm to generate a point cloud map for positioning and navigation.

4. The method for constructing a feature map for an autonomous driving vehicle based on the fusion of laser radar and low-orbit satellite according to claim 3, characterized in that: The pre-processing method in S2.1 specifically includes: To denoise the LiDAR data, a statistical filter is used to perform local neighborhood analysis. By calculating the mean and standard deviation of the neighborhood height of each point in the point cloud, noise points exceeding the threshold are identified and removed. Then, the LiDAR ground point is obtained using the angle differential algorithm. Low-orbit satellite data processing includes: (1) outlier detection and removal, data interpolation; (2) time synchronization to ensure data accuracy and consistency; (3) coordinate system alignment, which is achieved by determining the transformation matrix, using ground control points for registration, and autoaware fitting of rotation, translation, and scaling parameters.

5. The method for constructing a feature map for an autonomous driving vehicle based on the fusion of laser radar and low-orbit satellite according to claim 3, characterized in that: The multi-source data fusion algorithm in S2.1 uses a particle filter to fuse lidar data and low-orbit satellite data to optimize the coordinates of location control points, as follows: S2.1.1 Denoise the lidar data and extract ground points; select a point in the point cloud and define its neighborhood, such as a spherical area with a neighborhood radius of r, and calculate the mean μ and standard deviation σ of the height Z coordinates of all points in the neighborhood; then perform outlier detection, and for each point, calculate the difference between its height and the mean. p point, if |z p If -u|>kσ, where k is the threshold coefficient, it is considered noise and all points marked as noise are removed to obtain clean point cloud data. Finally, ground points are identified and extracted based on the angle changes between adjacent points in the lidar point cloud to obtain the elevation values ​​of the ground points. S2.1.2 Low-orbit satellite data processing, detailed steps are as follows: The first step is outlier detection and removal: a median filter is used to remove impulse noise from satellite signals, and thresholds are set for altitude and speed to remove data points that are out of range. The second step is to interpolate the data: for missing data points, linear interpolation or spline interpolation method is used for estimation. The linear interpolation formula is: where t i and t i+1 is the time corresponding to the known data points i and i+1, y(t i ) and y(t i+1 ) is the corresponding data value, and t is the interpolation time point; The third step is time synchronization and coordinate system alignment: First, determine the timestamp difference and calculate the timestamp difference between the lidar and low-orbit satellite data. Use this difference to align the satellite data with the lidar data. Coordinate system alignment is to obtain the fitting transformation matrix T through external parameter calibration software, including rotation, translation and scaling parameters, select a number of data, calculate the coordinate difference before and after alignment, and ensure that the difference is within the allowable error range; The fourth step is data fusion: the first step is to construct the state equation: x k =Fx k-1 +Gu k-1 +w k-1 (2) In formula (2), x = [p, v] T is the system state vector, p is the position, v is the velocity, which can be obtained by calculating the coordinate transformation of the two-frame point cloud, F is the transfer matrix, G is the control matrix, and u k-1 is the control input, w k-1 is the process noise; The observation equation is as follows: z k =Hx k +v k (3) Among them, z k is the observation value, which is the filtered low-orbit satellite data, H is the observation matrix, v k is the observation noise; Next, a set of particles is generated according to the equation of state: Where N is the number of particles, x k is the system state vector; Then calculate the observation weights: The weight indicates how well the particle matches the observation; Finally, the state is estimated and the current state is estimated based on the observation weight of the particle:

6. The method for constructing a feature map for an autonomous driving vehicle based on the fusion of laser radar and low-orbit satellite according to claim 5, characterized in that: In S2.1.1, the neighborhood radius r should be adjusted according to the point cloud density and the expected noise scale; the threshold coefficient k needs to be determined through experiments to balance the denoising effect and the preservation of details.

7. The method for constructing a feature map for an autonomous driving vehicle based on the fusion of laser radar and low-orbit satellite according to claim 5, characterized in that: The specific process of generating DEM in S2.2 includes: S2.2.1 Define the variogram, which quantifies the variability of spatial data by calculating the relationship between the elevation difference and the distance between known points. The variogram is as follows: γ(h) is the variance function value when the distance is h, Z(s i ) is s i The elevation value of , N(h) is the number of point pairs when the distance is h, and h represents the distance between the point pairs; S2.2.2 Construct the Kriging equations and solve for the weight coefficient λ i , to calculate the predicted value of the unknown point through the elevation value of the known point and the variation function model; the Kriging equation is as follows: λ i is the weight coefficient, indicating that the known point s i Contribution to the unknown point s0, γ(s i ,s j ) represents the known point s i and s j The variogram value between two points, μ is the Lagrange multiplier used for constraint conditions, and s0 is the unknown point to be predicted; S2.2.3 After completing the variogram analysis and weight calculation, perform Kriging interpolation on each grid point in the area to generate a DEM. The Kriging interpolation formula is: is the predicted elevation value at position s0, Z(s i ) is s i The elevation value of the location, λ i is the Kriging weight coefficient; S2.2.4 Divide the area into regular grid points, use the Kriging interpolation formula to calculate the predicted elevation value of each grid point s0, and combine the elevation values ​​of all grid points to generate a DEM.

8. The method for constructing a feature map for an autonomous driving vehicle based on the fusion of laser radar and low-orbit satellite according to claim 7, characterized in that: In S2.2, the smoothing process is to perform a convolution operation on the DEM using a Gaussian kernel to smooth the elevation values; The quality assessment of the generated DEM is achieved by the following method: Calculate the RMSE of the difference between the elevation value estimated by the DEM at the ground control point and the actual elevation value. The calculation formula is as follows: Z DEM (p i ) is p i The estimated elevation value at Z(p i ) is p i The actual elevation value of the ground control point location; If the RMSE is large, it indicates that the accuracy of the DEM is low and the DEM needs to be recalculated and the parameters of the Kriging interpolation method and the variation function model need to be adjusted.

9. A vehicle initial positioning method based on the digital elevation model (DEM) according to claim 1, characterized in that: The terrain features (slope) are extracted using DEM and matched with the environmental features obtained by the vehicle sensor lidar to achieve rapid initialization positioning.

10. A vehicle enhanced path planning method based on the digital elevation model (DEM) of claim 1, characterized in that: Integrating digital elevation model (DEM) data into route planning optimizes route selection by taking into account terrain elevation and slope information, avoiding steep or high-energy-consumption sections. Specifically: Define a cost function that takes into account distance, slope, and elevation values: Cost = distance × (1 + k1 slope + k2 elevation) (11) k1 and k2 are weight coefficients; If Dijkstra's algorithm is used for planning, the path with the minimum total cost is selected when the weight of each edge is calculated using the above cost function; For road sections with excessively steep slopes, they are set to be impassable or assigned extremely high costs.

Images