An adaptive vehicle positioning system and method for large scale scenarios

Abstract

The application discloses a kind of adaptive vehicle positioning system and method for large-scale scene, it is related to large-scale scene vehicle positioning technical field.First, the current GNSS latitude and longitude height data and IMU heading data are converted to map coordinate system, and the height error of GNSS is effectively avoided using adaptive height criterion, and the situation that global positioning fails is caused, global positioning and system state initialization of vehicle are realized.Second, using global map management module, according to the current position of vehicle, global submap is adaptively selected, significantly reduces system memory occupancy, improves the operation efficiency of system.Third, during the operation of vehicle, the size of surrounding environment, the degree of feature richness are analyzed in real time, and according to the environment analysis result and adaptive point cloud matching criterion, adjust point cloud matching parameter, improve the precision and robustness of laser matching.Finally, using sensor fusion positioning module, information fusion is realized, and the real-time pose of vehicle is obtained by solving.

Description

Technical Field

[0001] This invention relates to the field of vehicle positioning technology in large-scale scenes, and in particular to an adaptive vehicle positioning system and method for large-scale scenes. Background Technology

[0002] In recent years, autonomous driving technology has received widespread attention, and real-time robust vehicle localization is a prerequisite for this technology. For urban and highway driving scenarios, researchers have improved the accuracy and robustness of vehicle localization by fusing RTK, roadside units, or vehicle-to-vehicle (V2V) collaboration. However, urban scenarios often present weak GNSS environments, such as urban canyons or roads with dense trees, which severely impact the accuracy and robustness of vehicle localization. Achieving robust vehicle localization in these areas is crucial for the safety of autonomous driving. However, deploying roadside units in highway scenarios is very difficult and has low utilization rates. Meanwhile, V2V methods rely on information exchange between different vehicles, which is susceptible to noise, making them inaccurate and unreliable for vehicle localization and reducing operational safety. Therefore, these vehicle localization methods relying on external infrastructure are unsuitable for complex, large-scale scenarios.

[0003] With the continuous cross-fertilization and development of technologies across different fields, Simultaneous Mapping and Localization (SLAM) and map-based matching techniques are increasingly being applied to vehicle pose estimation. SLAM is typically used in indoor or small outdoor scenes, estimating vehicle pose using sensors such as cameras or LiDAR. However, it is not suitable for large-scale outdoor scenes. Map-based matching methods utilize real-time point clouds and pre-built high-precision maps for vehicle pose estimation. However, map-based matching and localization methods for large-scale scenes inevitably lead to significant consumption of computational and storage resources, especially the storage and loading of large-scale high-precision maps, severely reducing the operational efficiency of the vehicle localization system. Therefore, for complex large-scale scenes, researching efficient map management and robust map feature matching algorithms is of great significance for improving the computational efficiency and robustness of vehicle matching and localization algorithms.

[0004] In research on vehicle positioning technology, most current mainstream methods rely on GNSS for vehicle positioning in large-scale scenarios. For example, the Chinese patent "CN202310090814.8 A method and device for vehicle positioning and orientation using a single-antenna GNSS" describes a method that uses a GNSS device to acquire BeiDou and GPS information and calculate the vehicle's current position and heading. This invention uses a single antenna for vehicle positioning, which is simple and convenient to install and implement without affecting driving on the road. Compared with dual antennas, it is more suitable for road driving conditions, the steering wheel angle acquisition is not affected by external factors, it is relatively robust, and the refresh rate is fast. However, this method relies excessively on GNSS. In urban scenarios, there are many weak GNSS environments, such as urban canyons or roads with dense trees, which can seriously affect the accuracy and robustness of vehicle positioning.

[0005] In the research of vehicle positioning technology, many invention patents have emerged that rely on LiDAR and images. For example, the method described in Chinese patent "CN202211247009.3 A vehicle positioning method, device, equipment, and storage medium" obtains relatively accurate and stable positioning information by matching the point cloud data corresponding to the line and surface features in two adjacent image frames. However, this vision-based method is easily affected by lighting conditions. In insufficient lighting, the visual image cannot obtain stable environmental information, which greatly affects the accuracy and stability of vehicle positioning. The method described in Chinese patent "CN202310288571.9 A positioning information determination device, vehicle, method, and program product" uses a combination of visual images and LiDAR data to achieve fast and robust vehicle positioning. However, this method is only suitable for feature-rich urban scenes and does not perform well in complex, large-scale scenes with diverse changes, and cannot guarantee robust vehicle positioning. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention proposes an adaptive vehicle localization system and method for large-scale scenarios. This method enables system initialization from any vehicle location to real-time, high-precision, and robust localization. By dynamically loading maps, the system's memory usage is reduced, improving the efficiency of the proposed method. Furthermore, the proposed adaptive matching parameter method enhances the accuracy and robustness of vehicle localization in complex and dynamic large-scale scenarios.

[0007] This invention provides an adaptive vehicle positioning system for large-scale scenarios, including a GNSS module, an IMU module, a lidar, a wheel speedometer, a sensor data preprocessing module, a global positioning and state initialization module, a global map management and sub-map loading module, an environmental analysis and adaptive laser matching module, and a sensor fusion positioning module.

[0008] The IMU module is used to acquire IMU data from the vehicle; it sends the IMU data to the sensor data preprocessing module and sends the heading data from the IMU data to the global positioning and state initialization module; the IMU data includes heading data, acceleration, and angular velocity, wherein the heading data is represented by quaternions or Euler angles.

[0009] The GNSS module is used to acquire the vehicle's GNSS latitude, longitude, and altitude data and send the GNSS latitude, longitude, and altitude data to the global positioning and status initialization module, and send the longitude and latitude data in the GNSS latitude, longitude, and altitude data to the global map management and sub-map loading module; the GNSS latitude, longitude, and altitude data includes longitude data, latitude data, and altitude data;

[0010] The lidar is used to acquire lidar point cloud data and send the lidar point cloud data to the sensor data preprocessing module.

[0011] The wheel speed meter is used to acquire speed pulse data and send the speed pulse data to the sensor data preprocessing module;

[0012] The global map management and sub-map loading module, during initialization, acquires the global map and sends it to the global positioning and state initialization module. It receives longitude and latitude data from GNSS latitude, longitude, and altitude data, selects the closest global sub-map to the vehicle's current 2D position from the global map based on this data, and sends it to the global positioning and state initialization module. It also selects n closest global sub-maps to the vehicle's current 2D position from the global map based on the same data, downsamples these n sub-maps, and sends them to the environment analysis and adaptive laser matching module. During non-initialization, this module receives the vehicle's 2D position from the sensor fusion positioning module, adaptively selects and loads the n closest global sub-maps to the vehicle's current 2D position using this position, downsamples these n sub-maps, and sends them to the environment analysis and adaptive laser matching module.

[0013] The global positioning and state initialization module is used to receive GNSS latitude, longitude, and altitude data sent by the GNSS module and heading data sent by the IMU module; receive the global map and the nearest global sub-map to the vehicle's current position sent by the global map management and sub-map loading module; convert the longitude and latitude data in the GNSS latitude, longitude, and altitude data to the map coordinate system to obtain the vehicle's initial two-dimensional position in the global map and send it to the sensor data preprocessing module to complete the initialization of the vehicle's two-dimensional position; convert the heading data to the map coordinate system to obtain the vehicle's initial heading in the global map and send it to the sensor data preprocessing module to complete the initialization of the vehicle's attitude; compare the altitude data in the GNSS latitude, longitude, and altitude data with the altitude of the nearest global sub-map to the vehicle's current position; if the difference between the altitude data in the GNSS latitude, longitude, and altitude data and the altitude of the global sub-map is greater than a set threshold, then the altitude of the global sub-map is used as the initial altitude and sent to the sensor data preprocessing module to complete the initialization of the vehicle's altitude; otherwise, the altitude data in the GNSS latitude, longitude, and altitude data is used as the initial altitude and sent to the sensor data preprocessing module to complete the initialization of the vehicle's altitude; the global map includes several global sub-maps.

[0014] Furthermore, the sensor fusion positioning module includes a laser IMU measurement update module and a wheel speed meter measurement update module;

[0015] The sensor data preprocessing module receives IMU data, lidar point cloud data, velocity pulse data, initial two-dimensional position, initial heading, and initial altitude sent by the global positioning and state initialization module, and vehicle state and state covariance updated by the wheel speedometer sent by the sensor fusion positioning module. It removes operational distortion from the lidar point cloud data through distortion correction processing, then downsamples it using a voxel filter to obtain downsampled lidar point cloud data, which is then sent to the environment analysis and adaptive laser matching module. Based on the velocity pulse data, it obtains the vehicle's velocity in the wheel speedometer coordinate system, integrates it to obtain the displacement between the previous and current moments, and then compares the velocity in the wheel speedometer coordinate system with the velocity between the previous and current moments. The displacement generated between previous moments is sent to the sensor fusion positioning module; during the initial preprocessing, the vehicle state is predicted based on the initial 2D position, initial heading, initial altitude, and IMU data to obtain the vehicle state at the next moment, and the error state and state covariance are calculated based on the vehicle state at the next moment and sent to the laser IMU measurement update module; during non-initial preprocessing, the vehicle state is predicted based on the vehicle state and state covariance updated by the wheel speedometer sent by the sensor fusion positioning module, and the IMU data to obtain the vehicle state at the next moment, and the error state and state covariance are calculated based on the vehicle state at the next moment and sent to the laser IMU measurement update module; the leaf size m of the voxel filter. lThe set value is used during the initial preprocessing. During subsequent preprocessing, the leaf size m of the voxel filter sent by the environmental analysis and adaptive laser matching module is used. l The vehicle state includes the vehicle's attitude, position, speed, gyroscope random walk error in the IMU, accelerometer random walk error in the IMU, and gravitational acceleration.

[0016] The environmental analysis and adaptive laser matching module receives downsampled lidar point cloud data and analyzes the characteristics of the vehicle's surrounding environment based on environmental scale criteria and feature richness criteria to obtain environmental scale information and feature richness information, respectively. It performs feature association between the downsampled lidar point cloud data and the global downsampled sub-map sent by the global map management and sub-map loading module to achieve laser feature matching, thereby improving the accuracy and robustness of laser matching. Based on the environmental scale information and feature richness information, it uses adaptive point cloud matching criteria to adjust the leaf size m of the voxel filter at the next time step. l The data is then sent to the sensor data preprocessing module; the environmental scale criterion is used to assess the scale of the surrounding environment; the feature richness criterion is used to determine the feature richness of the surrounding environment; and the adaptive point cloud matching criterion is used to adjust the leaf size m of the voxel filter. l .

[0017] The sensor fusion positioning module is used to fuse positioning calculations to obtain the real-time pose of the vehicle.

[0018] The laser IMU measurement update module is used to receive the vehicle state, error state, and state covariance of the next moment sent by the sensor preprocessing module, and the laser feature matching relationship sent by the environmental analysis and adaptive laser matching module. Based on the vehicle state, error state, and state covariance of the next moment and the laser feature matching relationship, it obtains the updated error state, vehicle state, and state covariance, and sends the updated vehicle state and state covariance to the wheel speed meter measurement update module.

[0019] The wheel speed meter measurement update module is used to receive the speed in the wheel speed meter coordinate system, the displacement generated between the previous moment and the current moment, and the updated vehicle state and state covariance obtained by the laser IMU measurement update module, and perform fusion positioning to finally calculate the updated vehicle state and state covariance of the wheel speed meter.

[0020] In another aspect, the present invention provides an adaptive vehicle localization method for large-scale scenes, comprising the following specific steps:

[0021] Step 1: Use GNSS module, IMU module, lidar, and wheel speedometer to collect data and obtain GNSS latitude, longitude, and altitude data, IMU data, lidar point cloud data, and velocity pulse data at time k.

[0022] Step 2: Initialize the vehicle state using the global positioning and state initialization module to obtain the vehicle's initial position and attitude; the position includes two-dimensional position and height;

[0023] Step 2.1: Use the global map management and sub-map loading module to read the global map, and select a global sub-map from the global map based on the longitude and latitude data in the GNSS latitude, longitude and altitude data at time k. The altitude data h from GNSS latitude, longitude and altitude data cur With the selected global submap height The height error is obtained by subtraction, and the initial height of the vehicle in the global map is obtained by using the adaptive height criterion;

[0024] The global sub-map The global sub-map that is closest to the two-dimensional location corresponding to the vehicle's current longitude and latitude data;

[0025] The adaptive height criterion is as follows: if the height error is greater than a set height error threshold, then the height of the global sub-map will be adjusted. Use this as the initial altitude; otherwise, use the altitude data h from the GNSS latitude and longitude data. cur As the initial height, the expression is:

[0026]

[0027] Among them, h init This is the initial height; h is the height of the global sub-map closest to the vehicle's current two-dimensional location corresponding to its longitude and latitude data; cur This refers to the altitude data in GNSS latitude, longitude, and height data; h thr The threshold for height error;

[0028] Step 2.2: Convert the longitude and latitude data in the GNSS latitude, longitude and height data at time k in the WGS-84 coordinate system to the map coordinate system to obtain the initial two-dimensional position of the vehicle in the global map;

[0029] Specifically, the transformation relationship between the WGS-84 coordinate system and the geocentric coordinate system is first used to convert the longitude and latitude data in the GNSS latitude, longitude and altitude data under the WGS-84 coordinate system to the geocentric coordinate system. Then, the reference point of the northeast-sky coordinate system is set according to the latitude, longitude and altitude information of the map origin. The geocentric coordinate system is then converted to the map coordinate system using the transformation relationship between the geocentric coordinate system and the northeast-sky coordinate system to obtain the initial two-dimensional position of the vehicle in the global map.

[0030] Step 2.3: Based on the rotation relationship between the map coordinate system and the northeast-sky coordinate system, convert the heading data at time k to the map coordinate system to obtain the vehicle's initial heading in the global map;

[0031] Step 3: Using the sensor data preprocessing module, predict the vehicle state based on the vehicle's initial two-dimensional position, initial heading, initial altitude, and IMU data at time k to obtain the vehicle state and error state at time k+1; process the velocity pulse data to obtain the velocity in the wheel speedometer coordinate system, and integrate to obtain the displacement generated from time k to time k+1; remove operational distortion from the lidar point cloud data through distortion removal processing, and then perform downsampling processing to obtain downsampled lidar point cloud data;

[0032] Step 3.1: Using the discrete kinematics model of the IMU, predict the vehicle state based on the vehicle's initial two-dimensional position, initial heading, and initial altitude at time k, and the IMU data at time k, to obtain the predicted vehicle state at time k+1.

[0033] The vehicle prediction state at time k+1 for:

[0034]

[0035]

[0036]

[0037]

[0038]

[0039]

[0040]

[0041] Where, x k Let x be the updated vehicle state at time k. When k = 0, x k From the global positioning and vehicle state initialization module, when k>0, xk Derived from the sensor fusion positioning module; P represents the predicted state of the vehicle at time k+1. k ,t k ,v k ,b gk ,b ak These represent the vehicle attitude, position, velocity, gyroscope random walk error in the IMU, and accelerometer random walk error in the IMU at time k, respectively. These represent the predicted vehicle attitude, position, velocity, gyroscope random walk error in the IMU, and accelerometer random walk error in the IMU at time k+1, respectively; g is the gravitational acceleration. This is the measurement value from the gyroscope in the IMU; n g This is the white noise from the gyroscope measurement in the IMU; Δt is the time interval between time k and time k+1. It is the measurement value from the accelerometer in the IMU; n a White noise for accelerometer measurements in the IMU; n ba This is Gaussian noise from the accelerometer random walk error in the IMU; n bg It is the Gaussian noise of the random walk error of the gyroscope in the IMU; time k represents the previous time, and time k+1 represents the current time.

[0042] Step 3.2: Predict the vehicle state based on the vehicle at time k+1. The error state at time k+1 is calculated. State covariance

[0043] The error state at time k+1 State covariance for:

[0044]

[0045]

[0046] in, This is the error state at time k. This represents the predicted error state at time k+1, where F1 is the state transition matrix, F2 is the noise transfer matrix, and w k For noise and σ k It is the state covariance at time k. Q is the predicted state covariance at time k+1, and Q is the noise covariance.

[0047] Step 3.3: Based on the velocity pulse data, obtain the vehicle's velocity in the wheel speedometer coordinate system and integrate it according to the discrete kinematic model of the wheel speedometer to obtain the vehicle's displacement between time k and time k+1.

[0048] The speed of the vehicle in the wheel speedometer coordinate system is:

[0049]

[0050] in, Δs is the speed measurement value of the wheel speed gauge, f is the output data frequency of the wheel speed gauge, i.e., the frequency of the speed pulse data; Δs is the speed pulse data of the wheel speed gauge.

[0051] The displacement of the vehicle between time k and time k+1

[0052]

[0053] in, This represents the vehicle's displacement between time k and time k+1, where T is the output data period of the wheel speedometer, i.e., the period of the speed pulse data, and i is the sequence number of the speed pulse data between time k and time k+1. This is the i-th velocity data.

[0054] Step 3.4: Perform distortion correction processing on the acquired lidar point cloud data at time k to obtain the distorted lidar point cloud data;

[0055] The coordinates of the αth distortion-corrected laser point are:

[0056] p' α =ΔT*p α (13)

[0057] Among them, p' α p is the coordinate of the α-th laser point after distortion correction. α These are the coordinates of the α-th laser point, and ΔT is the coordinate of the α-th laser point p. α At time k lα Relative to the time k of the first laser point in this laser frame l0 Transformation of k; lα For the α-th laser point p α Current time; l represents the lidar coordinate system;

[0058] Step 3.5: Apply a voxel filter to the distortion-corrected lidar point cloud data for downsampling, resulting in downsampled lidar point cloud data. The leaf size of the voxel filter is m. l The initial value is set during the first downsampling operation;

[0059] Step 4: Based on the longitude and latitude data in the GNSS latitude, longitude and altitude data at time k, the global map management and sub-map loading module uses the adaptive sub-map selection criterion to select the n global sub-maps closest to the vehicle's initial two-dimensional position;

[0060] A sliding window-based global map management method is adopted to load the n global sub-maps closest to the current two-dimensional position of the vehicle. Specifically, the latitude, longitude, and altitude information of each global sub-map in the hard disk are loaded first. The distance between the current two-dimensional position of the vehicle and the latitude and longitude information of the pre-established global sub-maps is calculated using equation (14). Then, the pre-established global sub-maps are sorted in ascending order according to the obtained distance. Finally, the n global sub-maps closest to the current vehicle position are selected, and the n global sub-maps closest to the current two-dimensional position of the vehicle are dynamically loaded through a sliding window method. The latitude, longitude, and altitude information of the global sub-maps include the longitude, latitude, and altitude information of the global sub-maps; the latitude and longitude information of the global sub-maps includes the longitude and latitude information of the global sub-maps.

[0061]

[0062] in, It refers to the conversion relationship between units of length and units of latitude and longitude, T G Transformation between the map coordinate system and the WGS-84 coordinate system; t cur This represents the vehicle's current two-dimensional position in the map coordinate system. Represents the latitude and longitude information of the j-th global sub-map; d j This represents the distance between the vehicle's current 2D position and the reference point of the j-th global sub-map; j is the index of the global sub-map.

[0063] An optimized sub-map reading strategy is adopted to read the global sub-map: The strategy involves loading the n global sub-maps closest to the vehicle's current 2D position. The global sub-maps loaded at the previous time step include sub-maps j to j+n-1, where j is the sub-map index. After the vehicle travels a certain distance, the strategy loads the n global sub-maps closest to the vehicle's current 2D position. Specifically, the last n-1 sub-maps from the previous time step are read from the running memory, and then only the latest global sub-map is read from the hard drive. Finally, the original n-1 global sub-maps are combined with the latest global sub-map to obtain the n sub-maps closest to the vehicle's current 2D position, which are then used for subsequent feature matching.

[0064] Step 5: Downsample the n global sub-maps closest to the vehicle's current 2D position to obtain n global downsampled sub-maps;

[0065] Step 6: The environmental analysis and adaptive laser matching module uses downsampled LiDAR point cloud data to analyze the characteristics of the environment around the vehicle, and performs feature association between the downsampled LiDAR point cloud data and n global downsampled sub-maps to achieve laser matching and obtain laser feature matching relationships.

[0066] Step 6.1: Analyze the characteristics of the vehicle's surrounding environment according to the environmental scale criterion and the feature richness criterion to obtain environmental subscale information and feature richness information; the environmental subscale information represents the size of the vehicle in different directions; the feature richness information represents the feature richness of the environment in which the vehicle is located;

[0067] The environmental scale criterion is as follows: First, obtain the threshold D. thr1 Laser point S1 within the range, and then select threshold D. thr2 Laser points S1 and S2 within the range are then voxelized, and their voxel ratios in the x, y, and z directions are obtained. Environmental scale analysis is then performed to obtain environmental subscale information. The threshold D is used as an example. thr1 Threshold D thr2 Where y is the forward direction, x is the rightward direction of the vehicle's forward direction, and z is the upward direction perpendicular to the ground plane.

[0068] r s =Voxelize(S2) / Voxelize(S1) (15)

[0069] in, Indicates the voxel ratio, Indicates the voxel ratio in the x-direction. This represents the voxel ratio in the y-direction. It is the voxel ratio in the z-direction. If This indicates that the vehicle's scale is smaller along the x-axis and larger along the y-axis. This represents the maximum set threshold value; Voxelize() represents the voxelization function. This indicates the minimum value of the set threshold.

[0070] The feature richness criterion is as follows: First, an iterative k-dimensional tree (ikd-tree) is constructed based on the n globally downsampled sub-maps closest to the vehicle's current 2D position. Then, the laser points in the current downsampled LiDAR point cloud data are searched in the ikd-tree to obtain the N features in the globally downsampled sub-maps. m N map points, then N m Several planes are obtained by fitting map points. Finally, the laser points corresponding to the planes with a fitting degree greater than a threshold are statistically analyzed to obtain the number of effective laser points N. effAnd perform feature richness analysis, if N eff >N thr A positive value indicates rich environmental features around the vehicle, while a negative value indicates sparse environmental features around the vehicle. N represents this. thr It is the set threshold for the number of valid laser points.

[0071] Step 6.2: Perform feature association between the downsampled lidar point cloud data and n global downsampled sub-maps to achieve lidar matching and obtain the lidar feature matching relationship;

[0072] Based on the fitted planes obtained in step 6.1, the residual function of the following formula is constructed using the distance formula from a point to a plane to perform feature association and obtain the laser feature matching relationship. The laser feature matching relationship is characterized by the residual function.

[0073]

[0074] in, It is the α-th laser point in the lidar coordinate system. The current attitude of the vehicle; v represents the vehicle's current two-dimensional position. α The normal vector of the fitted plane; The residual function characterizing the laser feature matching relationship; The center point of the fitted plane; This is the rotation matrix between the laser and the IMU. is the translation vector between the laser and the IMU; m represents the map coordinate system, l represents the laser radar coordinate system, and b represents the IMU coordinate system;

[0075] Step 7: Use the sensor fusion positioning module to achieve sensor fusion positioning and obtain the real-time pose of the vehicle;

[0076] Step 7.1: Use the laser IMU measurement update module to obtain the error state after the iterative update through iterative calculation, and obtain the updated vehicle state and state covariance based on the error state after the iterative update;

[0077] The error state after the iterative update is completed is as follows:

[0078]

[0079] in, This is the error state after the iterative update is completed; This represents the prediction error state at time k+1. is the predicted state covariance at time k+1, and c is the number of laser points participating in point cloud matching. To characterize the residual function of laser feature matching relationship, σl It is the covariance matrix of the laser sensor.

[0080] The updated vehicle state and state covariance are:

[0081]

[0082]

[0083]

[0084] in, This is the updated vehicle status. Here, K1 is the predicted vehicle state, J is the filter gain, J is the Jacobian matrix of the error state definition, and H1 is... Jacobian matrix, σ l It is the covariance matrix of the laser sensor. It is the predicted state covariance at time k+1. It is the state covariance of the laser IMU after the update at time k+1, and I is the identity matrix.

[0085] Step 7.2: Input the updated vehicle state and state covariance obtained by the laser IMU measurement update module into the wheel speed meter measurement update module, and use the wheel speed meter measurement update module to perform fusion positioning and finally calculate the updated vehicle state and state covariance of the wheel speed meter. The updated vehicle state of the wheel speed meter includes the real-time pose of the vehicle.

[0086] Step 7.2.1: Based on the vehicle speed and displacement obtained in Step 3.3, construct the observation equation of the wheel speed meter to obtain the vehicle pose in the wheel speed meter coordinate system;

[0087]

[0088] in, The vehicle speed is calculated from the wheel speedometer pulse data. and displacement H2 is the observation matrix of the wheel speed gauge, n o It is Gaussian white noise with zero mean and variance R; It is the vehicle's position and pose in the wheel speedometer coordinate system obtained using wheel speedometer data.

[0089] Step 7.2.2: Based on the Kalman filter principle, construct the state update equation and covariance update equation of the wheel speed meter and perform vehicle state and state covariance updates to obtain the updated vehicle state and state covariance of the wheel speed meter.

[0090] The state update equation and covariance update equation are as follows:

[0091]

[0092]

[0093]

[0094] Where, x k+1 This is the vehicle status updated via wheel speed gauges. H1 represents the observations in the wheel speed meter coordinate system, and H2 is the wheel speed meter observation matrix. It is after being updated by laser IMU measurement. Transform the vehicle state to the wheel speedometer coordinate system. It is after being updated by laser IMU measurement. The rotating part in It refers to the rotational relationship between the wheel speedometer coordinate system and the IMU coordinate system. It is the state covariance updated by laser IMU measurement, σ k+1 It is the state covariance updated by the wheel speed meter, K2 is the Kalman filter gain, I is the identity matrix, and R is the noise covariance of the wheel speed meter.

[0095] Step 8: Use the GNSS module, IMU module, lidar, and wheel speedometer to collect data again, and obtain the GNSS latitude, longitude, and altitude data, IMU data, lidar point cloud data, and velocity pulse data at time k+1.

[0096] Step 9: Using the sensor data preprocessing module, predict the vehicle state based on the updated vehicle state and state covariance obtained from the wheel speed meter in Step 7 and the IMU data at time k+1 to obtain the vehicle state at time k+2; process the velocity pulse data to obtain the velocity in the wheel speed meter coordinate system, and integrate to obtain the displacement generated from time k+1 to time k+2; first perform distortion correction on the lidar point cloud data, and then perform downsampling to obtain downsampled lidar point cloud data.

[0097] At this time, the leaf size m of the voxel filter in the sensor data preprocessing module l The leaf size m of the voxel filter obtained by the adaptive point cloud matching criterion for the environmental analysis and adaptive laser matching module. l ;

[0098]

[0099] Where m0 is the set increment of the filter leaf, and a1 and a2 are set scaling factors. a1 is a set value, which is determined based on the environmental analysis results in step 6.1, when N eff <N thr Set a2 < 0, when N eff >Nthr or or Set a2>0.

[0100] Step 10: Based on the vehicle's two-dimensional position in the vehicle status obtained in Step 7, the global map management and sub-map loading module uses the adaptive sub-map selection criterion to select the n global sub-maps that are closest to the vehicle's current two-dimensional position;

[0101] Step 11: Downsample the n global sub-maps selected in Step 10 that are closest to the vehicle's current 2D position to obtain n global downsampled sub-maps;

[0102] Step 12: Environmental analysis and adaptive laser matching module. Analyze the characteristics of the environment around the vehicle using the downsampled lidar point cloud data from step 9. Perform feature association between the downsampled lidar point cloud data and the n global downsampled sub-maps obtained in step 11 to achieve laser matching and obtain the laser feature matching relationship.

[0103] Step 13: Use the sensor fusion positioning module to achieve sensor fusion positioning and obtain the real-time pose of the vehicle.

[0104] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0105] This invention proposes an adaptive vehicle positioning system and method for large-scale scenes. When a vehicle is powered on and started at any location in the environment, the state initialization module uses GNSS latitude, longitude, and altitude data and IMU data, along with an adaptive altitude criterion, to achieve global positioning and state initialization of the vehicle. The global map management and sub-map loading module proposed in this invention adaptively selects and loads global sub-maps near the vehicle's current location, effectively reducing system resource consumption and improving the operating efficiency of the positioning system. The environmental analysis and adaptive point cloud matching method proposed in this invention can analyze the scale and feature richness of the surrounding environment in real time, and adjust the point cloud matching parameters based on the environmental analysis results and the adaptive point cloud matching criterion, improving the accuracy and robustness of laser matching. Attached Figure Description

[0106] Figure 1 This is a structural diagram of an adaptive vehicle positioning system for large-scale scenes according to a specific embodiment of the present invention;

[0107] Figure 2 This is a flowchart of an adaptive vehicle localization method for large-scale scenes according to a specific embodiment of the present invention.

[0108] Figure 3 This is a schematic diagram illustrating dynamic sub-map updates in a specific embodiment of the present invention. Detailed Implementation

[0109] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0110] An adaptive vehicle localization system for large-scale scenarios, such as Figure 1 As shown, it includes a GNSS module, an IMU module, a lidar, a wheel speedometer, a sensor data preprocessing module, a global positioning and state initialization module, a global map management and sub-map loading module, an environmental analysis and adaptive laser matching module, and a sensor fusion positioning module.

[0111] The IMU module is used to acquire IMU data from the vehicle; it sends the IMU data to the sensor data preprocessing module and sends the heading data from the IMU data to the global positioning and state initialization module; the IMU data includes heading data, acceleration, and angular velocity, wherein the heading data is represented by quaternions or Euler angles.

[0112] The GNSS module is used to acquire the vehicle's GNSS latitude, longitude, and altitude data and send the GNSS latitude, longitude, and altitude data to the global positioning and status initialization module, and send the longitude and latitude data in the GNSS latitude, longitude, and altitude data to the global map management and sub-map loading module; the GNSS latitude, longitude, and altitude data includes longitude data, latitude data, and altitude data;

[0113] The lidar is used to acquire lidar point cloud data and send the lidar point cloud data to the sensor data preprocessing module.

[0114] The wheel speedometer is used to acquire speed pulse data and send it to the sensor data preprocessing module. The global map management and sub-map loading module, during initialization, acquires the global map and sends it to the global positioning and status initialization module. It receives longitude and latitude data from GNSS latitude, longitude, and altitude data, selects the closest global sub-map to the vehicle's current two-dimensional position from the global map based on the longitude and latitude data, and sends it to the global positioning and status initialization module. It also selects n closest global sub-maps to the vehicle's current two-dimensional position from the global map based on the longitude and latitude data from the GNSS latitude, longitude, and altitude data. The system first generates a map, then downsamples n global sub-maps to obtain n downsampled global sub-maps, which are then sent to the environment analysis and adaptive laser matching module. During non-initialization, this module receives the vehicle's 2D position from the sensor fusion positioning module, adaptively selects and loads the n global sub-maps closest to the vehicle's current 2D position using the vehicle's 2D position, and downsamples these n sub-maps to obtain n downsampled global sub-maps, which are then sent to the environment analysis and adaptive laser matching module. The purpose of this method is to reduce the amount of global map data, effectively reduce system resource consumption, and improve the operating efficiency of the positioning system.

[0115] The global positioning and state initialization module is used to receive GNSS latitude, longitude, and altitude data sent by the GNSS module and heading data sent by the IMU module; receive the global map and the nearest global sub-map to the vehicle's current position sent by the global map management and sub-map loading module; convert the longitude and latitude data in the GNSS latitude, longitude, and altitude data to the map coordinate system to obtain the vehicle's initial two-dimensional position in the global map and send it to the sensor data preprocessing module to complete the initialization of the vehicle's two-dimensional position; convert the heading data to the map coordinate system to obtain the vehicle's initial heading in the global map and send it to the sensor data preprocessing module to complete the initialization of the vehicle's attitude; compare the altitude data in the GNSS latitude, longitude, and altitude data with the altitude of the nearest global sub-map to the vehicle's current position; if the difference between the altitude data in the GNSS latitude, longitude, and altitude data and the altitude of the global sub-map is greater than a set threshold, then the altitude of the global sub-map is used as the initial altitude and sent to the sensor data preprocessing module to complete the initialization of the vehicle's altitude; otherwise, the altitude data in the GNSS latitude, longitude, and altitude data is used as the initial altitude and sent to the sensor data preprocessing module to complete the initialization of the vehicle's altitude; the global map includes several global sub-maps.

[0116] Furthermore, the sensor fusion positioning module includes a laser IMU measurement update module and a wheel speed meter measurement update module;

[0117] The sensor data preprocessing module receives IMU data, lidar point cloud data, velocity pulse data, initial two-dimensional position, initial heading, and initial altitude sent by the global positioning and state initialization module, and vehicle state and state covariance updated by the wheel speedometer sent by the sensor fusion positioning module. It removes operational distortion from the lidar point cloud data through distortion correction processing, then downsamples it using a voxel filter to obtain downsampled lidar point cloud data, which is then sent to the environment analysis and adaptive laser matching module. Based on the velocity pulse data, it obtains the vehicle's velocity in the wheel speedometer coordinate system, integrates it to obtain the displacement between the previous and current moments, and then compares the velocity in the wheel speedometer coordinate system with the velocity between the previous and current moments. The displacement generated between previous moments is sent to the sensor fusion positioning module; during the initial preprocessing, the vehicle state is predicted based on the initial 2D position, initial heading, initial altitude, and IMU data to obtain the vehicle state at the next moment, and the error state and state covariance are calculated based on the vehicle state at the next moment and sent to the laser IMU measurement update module; during non-initial preprocessing, the vehicle state is predicted based on the vehicle state and state covariance updated by the wheel speedometer sent by the sensor fusion positioning module, and the IMU data to obtain the vehicle state at the next moment, and the error state and state covariance are calculated based on the vehicle state at the next moment and sent to the laser IMU measurement update module; the leaf size m of the voxel filter. l The set value is used during the initial preprocessing. During subsequent preprocessing, the leaf size m of the voxel filter sent by the environmental analysis and adaptive laser matching module is used. l The vehicle state includes the vehicle's attitude, position, speed, gyroscope random walk error in the IMU, accelerometer random walk error in the IMU, and gravitational acceleration.

[0118] The environmental analysis and adaptive laser matching module receives downsampled lidar point cloud data and analyzes the characteristics of the vehicle's surrounding environment based on environmental scale criteria and feature richness criteria to obtain environmental scale information and feature richness information, respectively. It performs feature association between the downsampled lidar point cloud data and the global downsampled sub-map sent by the global map management and sub-map loading module to achieve laser feature matching, thereby improving the accuracy and robustness of laser matching. Based on the environmental scale information and feature richness information, it uses adaptive point cloud matching criteria to adjust the leaf size m of the voxel filter at the next time step. l The data is then sent to the sensor data preprocessing module; the environmental scale criterion is used to assess the scale of the surrounding environment; the feature richness criterion is used to determine the feature richness of the surrounding environment; and the adaptive point cloud matching criterion is used to adjust the leaf size m of the voxel filter. l This improves the point cloud matching effect.

[0119] The sensor fusion positioning module is used to fuse positioning calculations to obtain the real-time pose of the vehicle.

[0120] The laser IMU measurement update module is used to receive the vehicle state, error state, and state covariance of the next moment sent by the sensor preprocessing module, and the laser feature matching relationship sent by the environmental analysis and adaptive laser matching module. Based on the vehicle state, error state, and state covariance of the next moment and the laser feature matching relationship, it obtains the updated error state, vehicle state, and state covariance, and sends the updated vehicle state and state covariance to the wheel speed meter measurement update module.

[0121] The wheel speed meter measurement update module is used to receive the speed in the wheel speed meter coordinate system, the displacement generated between the previous moment and the current moment, and the updated vehicle state and state covariance obtained by the laser IMU measurement update module, and perform fusion positioning to finally calculate the updated vehicle state and state covariance of the wheel speed meter.

[0122] When the vehicle is powered on and started at any location in the environment, the positioning system, based on the received raw sensor data, first converts the current GNSS latitude, longitude, and altitude data and IMU heading data to a map coordinate system. An adaptive altitude criterion effectively avoids global positioning failures caused by large GNSS altitude errors, achieving global vehicle positioning and system state initialization. Secondly, using the global map management and sub-map loading module, a global sub-map is adaptively selected based on the vehicle's current location, significantly reducing system memory usage and improving computational efficiency. Thirdly, during vehicle operation, the environmental analysis module analyzes the surrounding environment in real time for scale and feature richness. Based on the environmental analysis results and adaptive point cloud matching criteria, the point cloud matching parameters are adjusted to improve the accuracy and robustness of laser matching. Finally, the sensor fusion positioning module fuses information from laser, IMU, and wheel speed sensors to calculate the vehicle's real-time pose.

[0123] An adaptive vehicle localization method for large-scale scenes, such as Figure 2 As shown, the specific steps include the following:

[0124] Step 1: Use GNSS module, IMU module, lidar, and wheel speedometer to collect data and obtain GNSS latitude, longitude, and altitude data, IMU data, lidar point cloud data, and velocity pulse data at time k.

[0125] Step 2: Initialize the vehicle state using the global positioning and state initialization module to obtain the vehicle's initial position and attitude; the position includes two-dimensional position and height;

[0126] When a vehicle starts up at any location in the environment, its current pose is unknown to the vehicle's positioning system. Therefore, global positioning is required to initialize the system state. Considering the significant elevation error acquired by the GNSS module, this invention proposes an adaptive altitude criterion to improve the accuracy of the initial altitude and increase the success rate of global positioning. This invention utilizes current GNSS latitude, longitude, and altitude data, uses the adaptive altitude criterion to determine the vehicle's current position, and determines the vehicle's attitude based on IMU heading data, thus jointly initializing the vehicle's state.

[0127] Step 2.1: Use the global map management and sub-map loading module to read the global map, and select a global sub-map from the global map based on the longitude and latitude data in the GNSS latitude, longitude and altitude data at time k. The altitude data h from GNSS latitude, longitude and altitude data cur With the selected global submap height The height error is obtained by subtraction, and the initial height of the vehicle in the global map is obtained by using the adaptive height criterion;

[0128] The global sub-map The global sub-map that is closest to the two-dimensional location corresponding to the vehicle's current longitude and latitude data;

[0129] The adaptive height criterion is as follows: if the height error is greater than a set height error threshold, then the height of the global sub-map will be adjusted. Use this as the initial altitude; otherwise, use the altitude data h from the GNSS latitude and longitude data. cur As the initial height, the expression is:

[0130]

[0131] Among them, h init This is the initial height; h is the height of the global sub-map closest to the vehicle's current two-dimensional location corresponding to its longitude and latitude data; cur This refers to the altitude data in GNSS latitude, longitude, and height data; h thr The threshold for height error;

[0132] The purpose of this is to select a more accurate initial altitude for the vehicle on the global map;

[0133] In order to achieve data fusion between the global coordinate system and the local coordinate system, this invention first needs to perform transformations between different coordinate systems.

[0134] Step 2.2: Convert the longitude and latitude data in the GNSS latitude, longitude and height data at time k in the WGS-84 coordinate system to the map coordinate system to obtain the initial two-dimensional position of the vehicle in the global map;

[0135] Specifically, the transformation relationship between the WGS-84 coordinate system and the geocentric coordinate system is first used to convert the longitude and latitude data in the GNSS latitude, longitude and altitude data under the WGS-84 coordinate system to the geocentric coordinate system. Then, the reference point of the northeast-sky coordinate system is set according to the latitude, longitude and altitude information of the map origin. The geocentric coordinate system is then converted to the map coordinate system using the transformation relationship between the geocentric coordinate system and the northeast-sky coordinate system to obtain the initial two-dimensional position of the vehicle in the global map.

[0136] Step 2.3: Based on the rotation relationship between the map coordinate system and the northeast-sky coordinate system, convert the heading data at time k to the map coordinate system to obtain the vehicle's initial heading in the global map;

[0137] Using the above method, the present invention can obtain a more accurate initial vehicle pose, achieve global positioning more robustly, and complete system state initialization.

[0138] Step 3: Using the sensor data preprocessing module, predict the vehicle state based on the vehicle's initial two-dimensional position, initial heading, initial altitude, and IMU data at time k to obtain the vehicle state and error state at time k+1; process the velocity pulse data to obtain the velocity in the wheel speedometer coordinate system, and integrate to obtain the displacement generated from time k to time k+1; remove operational distortion from the lidar point cloud data through distortion removal processing, and then perform downsampling processing to obtain downsampled lidar point cloud data;

[0139] Step 3.1: Using the discrete kinematics model of the IMU, predict the vehicle state based on the vehicle's initial two-dimensional position, initial heading, and initial altitude at time k, and the IMU data at time k, to obtain the predicted vehicle state at time k+1.

[0140] In the IMU prediction section, this invention predicts the vehicle state at time k+1 based on the discrete kinematics model of the IMU.

[0141] for:

[0142]

[0143]

[0144]

[0145]

[0146]

[0147]

[0148] Where, x k Let x be the updated vehicle state at time k. When k = 0, x k From the global positioning and vehicle state initialization module, when k>0, x k Derived from the sensor fusion positioning module; R represents the predicted state of the vehicle at time k+1. k ,t k ,v k ,b gk ,b ak These represent the vehicle attitude, position, velocity, gyroscope random walk error in the IMU, and accelerometer random walk error in the IMU at time k, respectively. These represent the predicted vehicle attitude, position, velocity, gyroscope random walk error in the IMU, and accelerometer random walk error in the IMU at time k+1, respectively; g is the gravitational acceleration. This is the measurement value from the gyroscope in the IMU; n g This is the white noise from the gyroscope measurement in the IMU; Δt is the time interval between time k and time k+1. It is the measurement value from the accelerometer in the IMU; n a White noise for accelerometer measurements in the IMU; n ba This is Gaussian noise from the accelerometer random walk error in the IMU; n bg It is the Gaussian noise of the random walk error of the gyroscope in the IMU; time k represents the previous time, and time k+1 represents the current time.

[0149] Step 3.2: Predict the vehicle state based on the vehicle at time k+1. The error state at time k+1 is calculated. State covariance

[0150] According to the formula "Error State = True State - Estimated State", that is... Error status can be obtained in This is the error state at time k+1. It is the truth state at time k+1. It is the rotating part of the error state at time k+1. It represents the position of the error state at time k+1. It is the velocity part of the error state at time k+1. It represents the random walk error component of the gyroscope in the IMU at time k+1. It is the random walk error component of the accelerometer in the IMU at time k+1. It is the gravitational acceleration component of the error state at time k+1. It is the truth pose at time k+1, R k+1 It is the estimated attitude at time k+1.

[0151] Based on the motion model of the IMU described above, a kinematic model based on the error state can be obtained:

[0152]

[0153]

[0154] in, This is the error state at time k. This represents the predicted error state at time k+1, where F1 is the state transition matrix, F2 is the noise transfer matrix, and w k For noise and σ k It is the state covariance at time k. Q is the predicted state covariance at time k+1, and Q is the noise covariance.

[0155] Step 3.3: Based on the velocity pulse data, obtain the vehicle's velocity in the wheel speedometer coordinate system and integrate it according to the discrete kinematic model of the wheel speedometer to obtain the vehicle's displacement between time k and time k+1.

[0156] In the wheel speed meter integration section, assuming the frequency of the wheel speed meter data is f and the period is T, this invention can obtain the vehicle's speed in the wheel speed meter coordinate system based on the speed pulse data Δs from the wheel speed meter:

[0157]

[0158] in, Δs is the speed measurement value of the wheel speed gauge, f is the output data frequency of the wheel speed gauge, i.e., the frequency of the speed pulse data; Δs is the speed pulse data of the wheel speed gauge.

[0159] Based on the discrete kinematic model of the wheel speed gauge, the vehicle displacement between time k and time k+1 can be obtained.

[0160]

[0161] in, This represents the vehicle's displacement between time k and time k+1, where T is the output data period of the wheel speedometer, i.e., the period of the speed pulse data, and i is the sequence number of the speed pulse data between time k and time k+1. This is the i-th velocity data.

[0162] Step 3.4: Perform distortion correction processing on the acquired lidar point cloud data at time k to obtain the distorted lidar point cloud data;

[0163] Assuming that the integration result obtained from the IMU data can yield the α-th laser point p α At time k lα Relative to the time k of the first laser point in this laser frame l0 The transformation ΔT, where At time k l0 The vehicle's position and posture rotation component. At time k lα The vehicle's position and posture rotation component. At time k lα The vehicle's position and orientation. At time k l0 Given the vehicle's pose position, the coordinates of the α-th laser point after motion distortion compensation are:

[0164] p' α =ΔT*p α (13)

[0165] Among them, p' α p is the coordinate of the α-th laser point after distortion correction. α These are the coordinates of the α-th laser point, and ΔT is the coordinate of the α-th laser point p. α At time k lα Relative to the time k of the first laser point in this laser frame l0 Transformation of k; lα For the α-th laser point p α The current time; l represents the lidar coordinate system; summing up all the compensated motion distortion lidar points yields the distortion-free lidar point cloud data.

[0166] Step 3.5: Apply a voxel filter to the distortion-corrected lidar point cloud data for downsampling, resulting in downsampled lidar point cloud data. The leaf size of the voxel filter is m. l The initial value is set during the first downsampling operation;

[0167] First, the distortion-corrected LiDAR point cloud data is divided into three-dimensional voxels according to a set voxel size. Then, a point within each voxel is selected as its feature point. Finally, all selected feature points are aggregated to obtain downsampled LiDAR point cloud data. Based on the above principles, downsampling processing of the distortion-corrected LiDAR point cloud data can be achieved to obtain downsampled LiDAR point cloud data.

[0168] Step 4: Based on the longitude and latitude data in the GNSS latitude, longitude and altitude data at time k, the global map management and sub-map loading module uses the adaptive sub-map selection criterion to select the n global sub-maps closest to the vehicle's initial two-dimensional position;

[0169] In large-scale scenarios, efficiently and accurately finding the corresponding global sub-map based on the vehicle's current location is crucial for improving vehicle localization efficiency. To address this issue, this invention proposes a sliding window-based global map management method. The adaptive sub-map selection criterion autonomously selects the nearest global sub-map based on the current vehicle location, achieving efficient map management.

[0170] Specifically, the following steps are taken: First, the latitude, longitude, and altitude information of each global sub-map in the hard disk are loaded. Then, the distance between the vehicle's current two-dimensional position and the latitude and longitude information of the pre-established global sub-map is calculated using equation (14). Next, the pre-established global sub-maps are sorted in ascending order based on the obtained distance. Finally, the n global sub-maps closest to the current vehicle position are selected, and the n global sub-maps closest to the current two-dimensional position of the vehicle are dynamically loaded using a sliding window method. The global map management method proposed in this invention can significantly reduce the system's memory usage for vehicle operation and improve the algorithm's running efficiency. The latitude, longitude, and altitude information of the global sub-map includes the longitude, latitude, and altitude information of the global sub-map; the latitude and longitude information of the global sub-map includes the longitude and latitude information of the global sub-map.

[0171]

[0172] in, It refers to the conversion relationship between units of length and units of latitude and longitude, T G Transformation between the map coordinate system and the WGS-84 coordinate system; t cur This represents the vehicle's current two-dimensional position in the map coordinate system. Represents the latitude and longitude information of the j-th global sub-map; d j This represents the distance between the vehicle's current 2D position and the reference point of the j-th global sub-map; j is the index of the global sub-map.

[0173] Considering that the CPU's speed of reading data from the hard drive is much slower than its speed of reading data from RAM, this invention employs an optimized sub-map reading strategy to read as many sub-maps as possible from RAM, thereby improving sub-map reading efficiency and effectively reducing sub-map data reading time. For example... Figure 3 As shown, assuming this invention loads the n global sub-maps closest to the vehicle's current 2D position, the global sub-maps loaded at the previous time step include sub-maps j to j+n-1, where j is the sub-map index. After the vehicle travels a certain distance, the n global sub-maps closest to the vehicle's current 2D position are loaded. Specifically, the last n-1 sub-maps from the previous time step are read from the running memory, and then only the latest global sub-map is read from the hard disk. Finally, the original n-1 global sub-maps are combined with the latest global sub-map to obtain the n sub-maps closest to the vehicle's current 2D position, which are then used for subsequent feature matching. The optimized sub-map reading strategy proposed in this invention not only improves the sub-map reading speed and reduces the sub-map reading time, but also reduces the impact of map loading on the positioning program, improving the robustness of the positioning system.

[0174] Step 5: Downsample the n global sub-maps closest to the vehicle's current 2D position to obtain n global downsampled sub-maps;

[0175] Step 6: The environmental analysis and adaptive laser matching module uses downsampled LiDAR point cloud data to analyze the characteristics of the environment around the vehicle, and performs feature association between the downsampled LiDAR point cloud data and n global downsampled sub-maps to achieve laser matching and obtain laser feature matching relationships.

[0176] To address complex and varied scenarios, such as feature-rich urban scenes, sparse and open highway scenes, and tunnel scenes, this invention proposes a rapid and efficient environmental analysis method capable of identifying and analyzing the characteristics of the surrounding environment. This method consists of two parts: an environmental scale criterion and a feature richness criterion.

[0177] Step 6.1: Analyze the characteristics of the vehicle's surrounding environment according to the environmental scale criterion and the feature richness criterion to obtain environmental subscale information and feature richness information; the environmental subscale information represents the size of the vehicle in different directions; the feature richness information represents the feature richness of the environment in which the vehicle is located;

[0178] The environmental scale criterion is achieved by determining the proportion of voxels within a set range through voxelization of the original lidar point cloud data.

[0179] First, obtain the threshold D. thr1The laser point S1 within the range is selected. Secondly, considering that laser points near the vehicle play a greater role in laser matching, the threshold D is chosen. thr2 Within the range of laser point S2, this invention voxels both laser points S1 and S2, obtaining the voxel ratios in the x, y, and z directions respectively, and performs environmental scale analysis to obtain environmental subscale information. The threshold D is used as an example. thr1 Threshold D thr2 Where y is the forward direction, x is the rightward direction of the vehicle's forward direction, and z is the upward direction perpendicular to the ground plane.

[0180] r s =Voxelize(S2) / Voxelize(S1) (15)

[0181] in, Indicates the voxel ratio, Indicates the voxel ratio in the x-direction. This represents the voxel ratio in the y-direction. This refers to the voxel ratio in the z-direction. Since the vehicle primarily moves on the ground, the z-axis variation is small; therefore, we focus on voxel variations in the x and y directions. If... This indicates that the vehicle's scale is smaller along the x-axis and larger along the y-axis. This represents the maximum set threshold value; Voxelize() represents the voxelization function. This indicates the minimum value of the set threshold.

[0182] The feature richness criterion is achieved by using the nearest neighbor search algorithm to count the number of laser points that can be found in the global sub-map in the real-time point cloud frame of the LiDAR.

[0183] First, an iterative k-dimensional tree (ikd-tree) is constructed based on the n globally downsampled sub-maps closest to the vehicle's current 2D position. Then, the laser points in the current downsampled LiDAR point cloud data are searched in the ikd-tree to obtain the N globally downsampled sub-maps. m N map points, then N m Several planes are obtained by fitting map points. Finally, the laser points corresponding to the planes with a fitting degree greater than a threshold are statistically analyzed to obtain the number of effective laser points N. eff And perform feature richness analysis, if N eff >N thr A positive value indicates rich environmental features around the vehicle, while a negative value indicates sparse environmental features around the vehicle. N represents this. thr It is the set threshold for the number of valid laser points.

[0184] Step 6.2: Perform feature association between the downsampled lidar point cloud data and n global downsampled sub-maps to achieve lidar matching and obtain the lidar feature matching relationship;

[0185] The method for achieving laser matching through feature association is as follows: based on the several planes obtained by fitting in step 6.1, the residual function of the following formula is constructed using the distance formula from a point to a plane to perform feature association and obtain the laser feature matching relationship. The laser feature matching relationship is characterized by the residual function.

[0186]

[0187] in, It is the α-th laser point in the lidar coordinate system. The current attitude of the vehicle; v represents the vehicle's current two-dimensional position. α The normal vector of the fitted plane; The residual function characterizing the laser feature matching relationship; The center point of the fitted plane; This is the rotation matrix between the laser and the IMU. is the translation vector between the laser and the IMU; m represents the map coordinate system, l represents the laser radar coordinate system, and b represents the IMU coordinate system;

[0188] Step 7: Use the sensor fusion positioning module to achieve sensor fusion positioning and obtain the real-time pose of the vehicle;

[0189] Step 7.1: Use the laser IMU measurement update module to obtain the error state after the iterative update through iterative calculation, and obtain the updated vehicle state and state covariance based on the error state after the iterative update;

[0190] The error state after the iterative update is completed is as follows:

[0191]

[0192] in, This is the error state after the iterative update is completed; This represents the prediction error state at time k+1. is the predicted state covariance at time k+1, and c is the number of laser points participating in point cloud matching. To characterize the residual function of laser feature matching relationship, σ l It is the covariance matrix of the laser sensor.

[0193] The updated vehicle state and state covariance are:

[0194]

[0195]

[0196]

[0197] in, This is the updated vehicle status. Here, K1 is the predicted vehicle state, J is the filter gain, J is the Jacobian matrix of the error state definition, and H1 is... Jacobian matrix, σ l It is the covariance matrix of the laser sensor. It is the predicted state covariance at time k+1. It is the state covariance of the laser IMU after the update at time k+1, and I is the identity matrix.

[0198] Step 7.2: Input the updated vehicle state and state covariance obtained by the laser IMU measurement update module into the wheel speed meter measurement update module, and use the wheel speed meter measurement update module to perform fusion positioning and finally calculate the updated vehicle state and state covariance of the wheel speed meter. The updated vehicle state of the wheel speed meter includes the real-time pose of the vehicle.

[0199] Step 7.2.1: Based on the vehicle speed and displacement obtained in Step 3.3, construct the observation equation of the wheel speed meter to obtain the vehicle pose in the wheel speed meter coordinate system;

[0200]

[0201] in, The vehicle speed is calculated from the wheel speedometer pulse data. and displacement H2 is the observation matrix of the wheel speed gauge, n o It is Gaussian white noise with zero mean and variance R; It is the vehicle's position and pose in the wheel speedometer coordinate system obtained using wheel speedometer data.

[0202] Step 7.2.2: Based on the Kalman filter principle, construct the state update equation and covariance update equation of the wheel speed meter and perform vehicle state and state covariance updates to obtain the updated vehicle state and state covariance of the wheel speed meter.

[0203] The state update equation and covariance update equation are as follows:

[0204]

[0205]

[0206]

[0207] Where, x k+1 This is the vehicle status updated via wheel speed gauges. H1 represents the observations in the wheel speed meter coordinate system, and H2 is the wheel speed meter observation matrix. It is after being updated by laser IMU measurement. Transform the vehicle state to the wheel speedometer coordinate system. It is after being updated by laser IMU measurement. The rotating part in It refers to the rotational relationship between the wheel speedometer coordinate system and the IMU coordinate system. It is the state covariance updated by laser IMU measurement, σ k+1 It is the state covariance updated by the wheel speed meter, K2 is the Kalman filter gain, I is the identity matrix, and R is the noise covariance of the wheel speed meter.

[0208] After updating the laser measurement and wheel speedometer measurement in the sensor fusion positioning module, the present invention can finally obtain the current position and pose of the vehicle.

[0209] Step 8: Use the GNSS module, IMU module, lidar, and wheel speedometer to collect data again, and obtain the GNSS latitude, longitude, and altitude data, IMU data, lidar point cloud data, and velocity pulse data at time k+1.

[0210] Step 9: Using the sensor data preprocessing module, predict the vehicle state based on the updated vehicle state and state covariance obtained from the wheel speed meter in Step 7 and the IMU data at time k+1 to obtain the vehicle state at time k+2; process the velocity pulse data to obtain the velocity in the wheel speed meter coordinate system, and integrate to obtain the displacement generated from time k+1 to time k+2; first perform distortion correction on the lidar point cloud data, and then perform downsampling to obtain downsampled lidar point cloud data.

[0211] At this time, the leaf size m of the voxel filter in the sensor data preprocessing module l The leaf size m of the voxel filter obtained by the adaptive point cloud matching criterion for the environmental analysis and adaptive laser matching module. l ;

[0212] This invention first utilizes the environmental analysis module described above to analyze the environmental scale and feature richness. Then, based on the current environmental characteristics, it adjusts the leaf element m of the system filter using the following formula. l The size is adjusted to dynamically change the number of laser points used for point cloud matching, i.e., in large-scale scenes with sparse features, the leaf size of the filter is reduced. l The value of increases the number of laser points participating in point cloud matching, thereby improving the accuracy of point cloud matching.

[0213]

[0214] Where m0 is the set increment of the filter leaf, and a1 and a2 are set scaling factors. a1 is a set value, which is determined based on the environmental analysis results in step 6.1, when N eff <N thr In this invention, a2 is set to < 0, when N eff >N thr or or In this invention, a2 is set to 0.

[0215] Step 10: Based on the vehicle's two-dimensional position in the vehicle status obtained in Step 7, the global map management and sub-map loading module uses the adaptive sub-map selection criterion to select the n global sub-maps that are closest to the vehicle's current two-dimensional position;

[0216] Step 11: Downsample the n global sub-maps selected in Step 10 that are closest to the vehicle's current 2D position to obtain n global downsampled sub-maps;

[0217] Step 12: Environmental analysis and adaptive laser matching module. Analyze the characteristics of the environment around the vehicle using the downsampled lidar point cloud data from step 9. Perform feature association between the downsampled lidar point cloud data and the n global downsampled sub-maps obtained in step 11 to achieve laser matching and obtain the laser feature matching relationship.

[0218] Step 13: Use the sensor fusion positioning module to achieve sensor fusion positioning and obtain the real-time pose of the vehicle.

Claims

1. An adaptive vehicle positioning system for large-scale scenes, characterized in that, It includes a GNSS module, an IMU module, a lidar, a wheel speedometer, a sensor data preprocessing module, a global positioning and state initialization module, a global map management and sub-map loading module, an environmental analysis and adaptive laser matching module, and a sensor fusion positioning module; The IMU module is used to acquire IMU data from the vehicle; it sends the IMU data to the sensor data preprocessing module and sends the heading data from the IMU data to the global positioning and state initialization module; the IMU data includes heading data, acceleration, and angular velocity, wherein the heading data is represented by quaternions or Euler angles. The GNSS module is used to acquire the vehicle's GNSS latitude, longitude, and altitude data and send the GNSS latitude, longitude, and altitude data to the global positioning and status initialization module, and send the longitude and latitude data in the GNSS latitude, longitude, and altitude data to the global map management and sub-map loading module; the GNSS latitude, longitude, and altitude data includes longitude data, latitude data, and altitude data; The lidar is used to acquire lidar point cloud data and send the lidar point cloud data to the sensor data preprocessing module. The wheel speedometer is used to acquire speed pulse data and send the speed pulse data to the sensor data preprocessing module; The global map management and sub-map loading module, during initialization, acquires the global map and sends it to the global positioning and state initialization module. It receives longitude and latitude data from GNSS latitude, longitude, and altitude data, selects the closest global sub-map to the vehicle's current 2D position from the global map based on this data, and sends it to the global positioning and state initialization module. It also selects n closest global sub-maps to the vehicle's current 2D position from the global map based on the same data, downsamples these n sub-maps to obtain n downsampled global sub-maps, and sends them to the environment analysis and adaptive laser matching module. During non-initialization, this module receives the vehicle's 2D position from the sensor fusion positioning module, adaptively selects and loads the n closest global sub-maps to the vehicle's current 2D position using this position, downsamples these n sub-maps, and sends them to the environment analysis and adaptive laser matching module. The global positioning and state initialization module is used to receive GNSS latitude, longitude, and altitude data sent by the GNSS module and heading data sent by the IMU module; receive the global map and the nearest global sub-map to the vehicle's current position sent by the global map management and sub-map loading module; convert the longitude and latitude data in the GNSS latitude, longitude, and altitude data to the map coordinate system to obtain the vehicle's initial two-dimensional position in the global map and send it to the sensor data preprocessing module to complete the initialization of the vehicle's two-dimensional position; convert the heading data to the map coordinate system to obtain the vehicle's initial heading in the global map and send it to the sensor data preprocessing module to complete the initialization of the vehicle's attitude; compare the altitude data in the GNSS latitude, longitude, and altitude data with the altitude of the nearest global sub-map to the vehicle's current position; if the difference between the altitude data in the GNSS latitude, longitude, and altitude data and the altitude of the global sub-map is greater than a set threshold, then the altitude of the global sub-map is used as the initial altitude and sent to the sensor data preprocessing module to complete the initialization of the vehicle's altitude; otherwise, the altitude data in the GNSS latitude, longitude, and altitude data is used as the initial altitude and sent to the sensor data preprocessing module to complete the initialization of the vehicle's altitude; the global map includes several global sub-maps. Furthermore, the sensor fusion positioning module includes a laser IMU measurement update module and a wheel speed meter measurement update module; The sensor data preprocessing module receives IMU data, lidar point cloud data, velocity pulse data, initial two-dimensional position, initial heading, and initial altitude sent by the global positioning and state initialization module, and vehicle state and state covariance updated by the wheel speedometer sent by the sensor fusion positioning module. It removes operational distortion from the lidar point cloud data through distortion removal processing, and then downsamples it through a voxel filter to obtain downsampled lidar point cloud data, which is then sent to the environmental analysis and adaptive laser matching module. Based on the speed pulse data, the vehicle's speed in the wheel speed meter coordinate system is obtained, and the displacement generated between the previous moment and the current moment is obtained by integration. The speed in the wheel speed meter coordinate system and the displacement generated between the previous moment and the current moment are sent to the sensor fusion positioning module. During the initial preprocessing, the vehicle state is predicted based on the initial 2D position, initial heading, initial altitude, and IMU data to obtain the vehicle state at the next moment. The error state and state covariance are then calculated based on the vehicle state at the next moment and sent to the laser IMU measurement update module. During subsequent preprocessing, the vehicle state is predicted based on the vehicle state and state covariance updated by the wheel speedometer sent by the sensor fusion positioning module, as well as the IMU data. The error state and state covariance are then calculated based on the vehicle state at the next moment and sent to the laser IMU measurement update module. The leaf size m of the voxel filter... l The set value is used during the initial preprocessing. During subsequent preprocessing, the leaf size m of the voxel filter sent by the environmental analysis and adaptive laser matching module is used. l The vehicle state includes the vehicle's attitude, position, speed, gyroscope random walk error in the IMU, accelerometer random walk error in the IMU, and gravitational acceleration. The environmental analysis and adaptive laser matching module receives downsampled lidar point cloud data and analyzes the characteristics of the vehicle's surrounding environment based on environmental scale criteria and feature richness criteria to obtain environmental scale information and feature richness information, respectively. It performs feature association between the downsampled lidar point cloud data and the global downsampled sub-map sent by the global map management and sub-map loading module to achieve laser feature matching, thereby improving the accuracy and robustness of laser matching. Based on the environmental scale information and feature richness information, it uses adaptive point cloud matching criteria to adjust the leaf size m of the voxel filter at the next time step. l The data is then sent to the sensor data preprocessing module; the environmental scale criterion is used to assess the scale of the surrounding environment; the feature richness criterion is used to determine the feature richness of the surrounding environment; and the adaptive point cloud matching criterion is used to adjust the leaf size m of the voxel filter. l ; The sensor fusion positioning module is used to fuse positioning calculations to obtain the real-time pose of the vehicle; The laser IMU measurement update module is used to receive the vehicle state, error state, and state covariance of the next moment sent by the sensor preprocessing module, and the laser feature matching relationship sent by the environmental analysis and adaptive laser matching module. Based on the vehicle state, error state, and state covariance of the next moment and the laser feature matching relationship, it obtains the updated error state, vehicle state, and state covariance, and sends the updated vehicle state and state covariance to the wheel speed meter measurement update module. The wheel speedometer measurement update module is used to receive the speed in the wheel speedometer coordinate system, the displacement generated between the previous moment and the current moment, and the updated vehicle state and state covariance obtained by the laser IMU measurement update module, and perform fusion positioning to finally calculate the updated vehicle state and state covariance of the wheel speedometer.

2. An adaptive vehicle localization method for large-scale scenes, based on the adaptive vehicle localization system for large-scale scenes described in claim 1, is characterized in that... The specific steps include the following: Step 1: Use GNSS module, IMU module, lidar, and wheel speedometer to collect data and obtain GNSS latitude, longitude, and altitude data, IMU data, lidar point cloud data, and velocity pulse data at time k. Step 2: Initialize the vehicle state using the global positioning and state initialization module to obtain the vehicle's initial position and attitude; the position includes two-dimensional position and height; Step 3: Use the sensor data preprocessing module to predict the vehicle state based on the vehicle's initial two-dimensional position, initial heading, initial altitude, and IMU data at time k, to obtain the vehicle state and error state at time k+1. The velocity pulse data is processed to obtain the velocity in the wheel speed meter coordinate system, and the displacement generated from time k to time k+1 is obtained by integration. The LiDAR point cloud data is processed to remove the running distortion, and then downsampled to obtain downsampled LiDAR point cloud data. Step 4: Based on the longitude and latitude data in the GNSS latitude, longitude and altitude data at time k, the global map management and sub-map loading module uses the adaptive sub-map selection criterion to select the n global sub-maps closest to the vehicle's initial two-dimensional position; Step 5: Downsample the n global sub-maps closest to the vehicle's current 2D position to obtain n global downsampled sub-maps; Step 6: The environmental analysis and adaptive laser matching module uses downsampled LiDAR point cloud data to analyze the characteristics of the environment around the vehicle, and performs feature association between the downsampled LiDAR point cloud data and n global downsampled sub-maps to achieve laser matching and obtain laser feature matching relationships. Step 7: Use the sensor fusion positioning module to achieve sensor fusion positioning and obtain the real-time pose of the vehicle; Step 8: Use the GNSS module, IMU module, lidar, and wheel speedometer to collect data again, and obtain the GNSS latitude, longitude, and altitude data, IMU data, lidar point cloud data, and velocity pulse data at time k+1. Step 9: Using the sensor data preprocessing module, predict the vehicle state based on the updated vehicle state and state covariance obtained from the wheel speed meter in Step 7 and the IMU data at time k+1 to obtain the vehicle state at time k+2; process the velocity pulse data to obtain the velocity in the wheel speed meter coordinate system, and integrate to obtain the displacement generated from time k+1 to time k+2; first perform distortion correction on the lidar point cloud data, and then perform downsampling to obtain downsampled lidar point cloud data. Step 10: Based on the vehicle's two-dimensional position in the vehicle status obtained in Step 7, the global map management and sub-map loading module uses the adaptive sub-map selection criterion to select the n global sub-maps that are closest to the vehicle's current two-dimensional position; Step 11: Downsample the n global sub-maps selected in Step 10 that are closest to the vehicle's current 2D position to obtain n global downsampled sub-maps; Step 12: Environmental analysis and adaptive laser matching module. Analyze the characteristics of the environment around the vehicle using the downsampled lidar point cloud data from step 9. Perform feature association between the downsampled lidar point cloud data and the n global downsampled sub-maps obtained in step 11 to achieve laser matching and obtain the laser feature matching relationship. Step 13: Use the sensor fusion positioning module to achieve sensor fusion positioning and obtain the real-time pose of the vehicle.

3. The adaptive vehicle localization method for large-scale scenes according to claim 2, characterized in that, Step 2 specifically includes: Step 2.1: Use the global map management and sub-map loading module to read the global map, and select a global sub-map from the global map based on the longitude and latitude data in the GNSS latitude, longitude and altitude data at time k. The altitude data h from GNSS latitude, longitude and altitude data cur With the selected global submap height The height error is obtained by subtraction, and the initial height of the vehicle in the global map is obtained by using the adaptive height criterion; The global sub-map The global sub-map that is closest to the two-dimensional location corresponding to the vehicle's current longitude and latitude data; The adaptive height criterion is as follows: if the height error is greater than a set height error threshold, then the height of the global sub-map will be adjusted. Use this as the initial altitude; otherwise, use the altitude data h from the GNSS latitude and longitude data. cur As the initial height, the expression is: Among them, h init This is the initial height; h is the height of the global sub-map closest to the vehicle's current two-dimensional location corresponding to its longitude and latitude data; cur This refers to altitude data from GNSS latitude, longitude, and height data; h thr The threshold for height error; Step 2.2: Convert the longitude and latitude data in the GNSS latitude, longitude and height data at time k in the WGS-84 coordinate system to the map coordinate system to obtain the initial two-dimensional position of the vehicle in the global map; Specifically, the transformation relationship between the WGS-84 coordinate system and the geocentric coordinate system is first used to convert the longitude and latitude data in the GNSS latitude, longitude and altitude data under the WGS-84 coordinate system to the geocentric coordinate system. Then, the reference point of the northeast-sky coordinate system is set according to the latitude, longitude and altitude information of the map origin. The geocentric coordinate system is then converted to the map coordinate system using the transformation relationship between the geocentric coordinate system and the northeast-sky coordinate system to obtain the initial two-dimensional position of the vehicle in the global map. Step 2.3: Based on the rotation relationship between the map coordinate system and the northeast-sky coordinate system, convert the heading data at time k to the map coordinate system to obtain the vehicle's initial heading in the global map.

4. The adaptive vehicle localization method for large-scale scenes according to claim 2, characterized in that, Step 3 specifically includes: Step 3.1: Using the discrete kinematics model of the IMU, predict the vehicle state based on the vehicle's initial two-dimensional position, initial heading, and initial altitude at time k, and the IMU data at time k, to obtain the predicted vehicle state at time k+1. The vehicle prediction state at time k+1 for: Where, x k Let x be the updated vehicle state at time k. When k = 0, x k From the global positioning and vehicle state initialization module, when k>0, x k Derived from the sensor fusion positioning module; R represents the predicted state of the vehicle at time k+1. k ,t k ,v k ,b gk ,b ak These represent the vehicle attitude, position, velocity, gyroscope random walk error in the IMU, and accelerometer random walk error in the IMU at time k, respectively. These represent the predicted vehicle attitude, position, velocity, gyroscope random walk error in the IMU, and accelerometer random walk error in the IMU at time k+1, respectively; g is the gravitational acceleration. This is the measurement value from the gyroscope in the IMU; n g This is the white noise from the gyroscope measurement in the IMU; Δt is the time interval between time k and time k+1. It is the measurement value from the accelerometer in the IMU; n a White noise for accelerometer measurements in the IMU; n ba This is Gaussian noise from the accelerometer random walk error in the IMU; n bg This is Gaussian noise representing the random walk error of the gyroscope in the IMU; time k represents the previous time, and time k+1 represents the current time. Step 3.2: Predict the vehicle state based on the vehicle at time k+1. The error state at time k+1 is calculated. State covariance The error state at time k+1 State covariance for: in, This is the error state at time k. This represents the predicted error state at time k+1, where F1 is the state transition matrix, F2 is the noise transfer matrix, and w k For noise and σ k It is the state covariance at time k. Q is the predicted state covariance at time k+1, and Q is the noise covariance. Step 3.3: Based on the velocity pulse data, obtain the vehicle's velocity in the wheel speedometer coordinate system and integrate it according to the discrete kinematic model of the wheel speedometer to obtain the vehicle's displacement between time k and time k+1. The speed of the vehicle in the wheel speedometer coordinate system is: in, Δs is the speed measurement value of the wheel speed gauge, f is the output data frequency of the wheel speed gauge, that is, the frequency of the speed pulse data; Δs is the speed pulse data of the wheel speed gauge. The displacement of the vehicle between time k and time k+1 in, This represents the vehicle's displacement between time k and time k+1, where T is the output data period of the wheel speedometer, i.e., the period of the speed pulse data, and i is the sequence number of the speed pulse data between time k and time k+1. This is the i-th velocity data; Step 3.4: Perform distortion correction processing on the acquired lidar point cloud data at time k to obtain the distorted lidar point cloud data; The coordinates of the αth distortion-corrected laser point are: p′ α =ΔT*p α (13) Among them, p' α p is the coordinate of the α-th laser point after distortion correction. α These are the coordinates of the α-th laser point, and ΔT is the coordinate of the α-th laser point p. α At time k lα Relative to the time k of the first laser point in this laser frame l0 Transformation of k; lα For the α-th laser point p α Current time; l represents the lidar coordinate system; Step 3.5: Apply a voxel filter to the distortion-free lidar point cloud data for downsampling to obtain downsampled lidar point cloud data; the leaf size of the voxel filter is m. l The initial value is set during the first downsampling operation.

5. The adaptive vehicle localization method for large-scale scenes according to claim 2, characterized in that, In step 4, a sliding window-based global map management method is used to load the n global sub-maps closest to the vehicle's current two-dimensional position. Specifically, the latitude, longitude, and altitude information of each global sub-map in the hard disk are loaded first. The distance between the vehicle's current two-dimensional position and the latitude and longitude information of the pre-established global sub-maps is calculated using equation (14). Then, the pre-established global sub-maps are sorted in ascending order according to the obtained distance. Finally, the n global sub-maps closest to the current vehicle position are selected, and the n global sub-maps closest to the vehicle's current two-dimensional position are dynamically loaded using a sliding window method. The latitude, longitude, and altitude information of the global sub-maps includes the longitude, latitude, and altitude information of the global sub-maps. The latitude and longitude information of the global sub-maps includes the longitude and latitude information of the global sub-maps. in, It refers to the conversion relationship between units of length and units of latitude and longitude, T G Transformation between the map coordinate system and the WGS-84 coordinate system; t cur This represents the vehicle's current two-dimensional position in the map coordinate system. Represents the latitude and longitude information of the j-th global sub-map; d j This represents the distance between the vehicle's current 2D position and the reference point of the j-th global sub-map; j is the index of the global sub-map.

6. The adaptive vehicle localization method for large-scale scenes according to claim 2, characterized in that, Step 4 employs an optimized sub-map reading strategy to read the global sub-map: The strategy involves loading the n global sub-maps closest to the vehicle's current 2D position. The global sub-maps loaded at the previous moment include sub-maps j to j+n-1, where j is the sub-map number. After the vehicle travels a certain distance, the strategy loads the n global sub-maps closest to the vehicle's current 2D position based on the vehicle's current position. Specifically, the last n-1 sub-maps from the previous moment are read from the running memory, and only the latest global sub-map is read from the hard drive. Finally, the original n-1 global sub-maps are combined with the latest global sub-map to obtain the n sub-maps closest to the vehicle's current 2D position, which are then used for subsequent feature matching.

7. The adaptive vehicle localization method for large-scale scenes according to claim 2, characterized in that, Step 6 specifically includes: Step 6.1: Analyze the characteristics of the vehicle's surrounding environment according to the environmental scale criterion and the feature richness criterion to obtain environmental subscale information and feature richness information; the environmental subscale information represents the size of the vehicle in different directions; the feature richness information represents the feature richness of the environment in which the vehicle is located; The environmental scale criterion is as follows: First, obtain the threshold D. thr1 Laser point S1 within the range, and then select threshold D. thr2 Laser point S2 within the range is then processed. Finally, laser points S1 and S2 are voxelized separately, and their voxel ratios in the x, y, and z directions are obtained. Environmental scale analysis is then performed to obtain environmental subscale information; where the threshold D... thr1 Threshold D thr2 Where y is the forward direction, x is the rightward direction of the vehicle's forward direction, and z is the upward direction perpendicular to the ground plane. r s =Voxelize(S2) / Voxelize(S1) (15) in, Indicates the voxel ratio, Indicates the voxel ratio in the x-direction. This represents the voxel ratio in the y-direction. It is the voxel ratio in the z-direction; if This indicates that the vehicle's scale is smaller along the x-axis and larger along the y-axis. This represents the maximum set threshold value; Voxelize() represents the voxelization function. This indicates the minimum value of the set threshold. The feature richness criterion is as follows: First, an iterative k-dimensional tree (ikd-tree) is constructed based on the n globally downsampled sub-maps closest to the vehicle's current 2D position. Then, the laser points in the current downsampled LiDAR point cloud data are searched in the ikd-tree to obtain the N features in the globally downsampled sub-maps. m 1 map point, then N m Several planes are obtained by fitting map points. Finally, the laser points corresponding to the planes with a fitting degree greater than a threshold are statistically analyzed to obtain the number of effective laser points N. eff And perform feature richness analysis, if N eff >N thr A positive value indicates rich environmental features around the vehicle, while a negative value indicates sparse environmental features around the vehicle. N represents this. thr It is the set threshold for the number of effective laser points; Step 6.2: Perform feature association between the downsampled lidar point cloud data and n global downsampled sub-maps to achieve lidar matching and obtain the lidar feature matching relationship; Based on the fitted planes obtained in step 6.1, the residual function of the following formula is constructed using the distance formula from a point to a plane to perform feature association and obtain the laser feature matching relationship. The laser feature matching relationship is characterized by the residual function. in, It is the α-th laser point in the lidar coordinate system. The current attitude of the vehicle; v represents the vehicle's current two-dimensional position. α The normal vector of the fitted plane; The residual function characterizing the laser feature matching relationship; The center point of the fitted plane; This is the rotation matrix between the laser and the IMU. is the translation vector between the laser and the IMU; m represents the map coordinate system, l represents the lidar coordinate system, and b represents the IMU coordinate system.

8. The adaptive vehicle localization method for large-scale scenes according to claim 2, characterized in that, Step 7 specifically includes: Step 7.1: Use the laser IMU measurement update module to obtain the error state after the iterative update through iterative calculation, and obtain the updated vehicle state and state covariance based on the error state after the iterative update; The error state after the iterative update is completed is as follows: in, This is the error state after the iterative update is completed; This represents the prediction error state at time k+1. is the predicted state covariance at time k+1, and c is the number of laser points participating in point cloud matching. To characterize the residual function of laser feature matching relationship, σ l It is the covariance matrix of the laser sensor; The updated vehicle state and state covariance are: in, This is the updated vehicle status. Here, K1 is the predicted vehicle state, J is the filter gain, J is the Jacobian matrix of the error state definition, and H1 is... Jacobian matrix, σ l It is the covariance matrix of the laser sensor. It is the predicted state covariance at time k+1. It is the state covariance of the laser IMU after the update at time k+1, and I is the identity matrix; Step 7.2: Input the updated vehicle state and state covariance obtained from the laser IMU measurement update module into the wheel speed meter measurement update module, and use the wheel speed meter measurement update module to perform fusion positioning and finally calculate the updated vehicle state and state covariance of the wheel speed meter. The updated vehicle state of the wheel speed meter includes the real-time pose of the vehicle.

9. The adaptive vehicle localization method for large-scale scenes according to claim 2, characterized in that, The leaf size m of the voxel filter in the sensor data preprocessing module described in step 9 l The leaf size m of the voxel filter obtained by the adaptive point cloud matching criterion for the environmental analysis and adaptive laser matching module. l ; Where m0 is the set increment of the filter leaf, and a1 and a2 are set scaling factors; a1 is a set value, which is determined based on the environmental analysis results in step 6.1, when N eff <N thr Set a2 < 0, when N eff >N thr or or Set a2>

0.

10. The adaptive vehicle localization method for large-scale scenes according to claim 8, characterized in that, Step 7.2 specifically includes: Step 7.2.1: Based on the vehicle speed and displacement obtained in Step 3.3, construct the observation equation of the wheel speed meter to obtain the vehicle pose in the wheel speed meter coordinate system; in, The vehicle speed is calculated from the wheel speedometer pulse data. and displacement H2 is the observation matrix of the wheel speed gauge, n o It is Gaussian white noise with zero mean and variance R; It is the vehicle pose in the wheel speedometer coordinate system obtained using wheel speedometer data; Step 7.2.2: Based on the Kalman filter principle, construct the state update equation and covariance update equation of the wheel speed meter and perform vehicle state and state covariance updates to obtain the updated vehicle state and state covariance of the wheel speed meter. The state update equation and covariance update equation are as follows: Where, x k+1 This is the vehicle status updated via wheel speed gauges. H1 represents the observations in the wheel speed meter coordinate system, and H2 is the wheel speed meter observation matrix. It is after being updated by laser IMU measurement. Transform the vehicle state to the wheel speedometer coordinate system. It is after being updated by laser IMU measurement. The rotating part in It refers to the rotational relationship between the wheel speedometer coordinate system and the IMU coordinate system. It is the state covariance updated by laser IMU measurement, σ k+1 It is the state covariance updated by the wheel speed meter, K2 is the Kalman filter gain, I is the identity matrix, and R is the noise covariance of the wheel speed meter.

Images