Vehicle global positioning deviation correction method based on roadside sensing result

Abstract

The invention relates to the technical field related to automatic driving, in particular to a vehicle global positioning deviation correction method based on a roadside sensing result, and the method comprises the steps: obtaining the global pose of a vehicle through a 3D target detection network in which roadside equipment is fused with time sequence features, and carrying out the uncertainty quantification; meanwhile, a vehicle end adopts a tight coupling laser radar and IMU data to realize high-frequency local positioning, a time domain self-adaptive interpolation mechanism is designed for compensating time deviation aiming at the problem of roadside sensing time delay, roadside observation is constructed into a roadside sensing factor of self-adaptive covariance, and finally, through factor graph fusion optimization, a road-side sensing model is obtained. According to the present invention, by combining the IMU pre-integration factor, the laser radar odometer factor and the roadside perception factor, the accurate vehicle pose estimation of the accumulative error correction and the global coordinate system alignment is achieved, and the experiment shows that the method significantly improves the positioning precision and the robustness in the complex environment, and effectively solves the positioning drift problem when the GNSS signal is missing.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving, and particularly relates to a vehicle global positioning deviation correction method based on a roadside perception result. BACKGROUND

[0002] Positioning technology is one of the important technologies supporting the operation of an automatic driving system. Current high-precision positioning systems mostly rely on GNSS (Global Navigation Satellite System) signals for positioning. However, in complex environments such as tunnels and urban canyons, GNSS signals are often blocked, affected by multipath effects and other interference, resulting in a significant decrease in positioning accuracy, and even making it impossible to effectively position. In a GNSS completely denied environment, positioning solely relying on the vehicle's own sensors (such as IMU, lidar, camera, etc.) can lead to long-term accumulation of positioning errors. These sensors are based on local information and lack global positioning references, so they cannot provide accurate global position information. However, with the development of car-road cooperation technology, roadside intelligent facilities are gradually equipped with perception devices and interconnected with vehicles through vehicle-to-road communication (V2X), and these facilities can provide position reference information in the global coordinate system for vehicles through 3D target detection and other technologies. Although the roadside perception system can provide effective position information, due to the errors of the perception system itself, the delay of vehicle-to-road communication, and other problems, how to accurately apply this information to the vehicle positioning system is still a technical problem. SUMMARY

[0003] The purpose of the present application is to provide a vehicle global positioning deviation correction method based on a roadside perception result, to solve the problem that the prior art cannot provide stable and high-precision positioning in a GNSS denied environment.

[0004] To achieve the above-mentioned purpose, the present application provides the following technical scheme: a vehicle global positioning deviation correction method based on a roadside perception result, comprising the following steps:

[0005] S1, high-precision roadside perception and uncertainty quantification: the roadside device uses a 3D target detection network that fuses temporal features to obtain the pose of the vehicle in the global coordinate system, and quantifies the uncertainty of each dimension of the perception result, outputting the pose and its corresponding covariance matrix;

[0006] S2, vehicle-mounted high-frequency local positioning: the vehicle end estimates the high-frequency relative pose of the vehicle in real time by tightly coupling the lidar and inertial measurement unit (IMU) data, and constructs the IMU pre-integration factor and the lidar odometry factor;

[0007] S3, time delay compensation and global constraint construction: for the transmission and calculation time delay of the roadside perception result, a time-domain adaptive interpolation mechanism is used for compensation, and the compensated roadside observation and its uncertainty are constructed as a roadside perception factor;

[0008] S4, factor graph fusion optimization: the roadside perception factor, IMU pre-integration factor and lidar odometry factor are jointly constructed into a factor graph, and a nonlinear optimization is solved to output a vehicle accurate pose which is corrected for cumulative error and aligned to a global coordinate system.

[0009] Preferably, the S1 high-precision roadside perception and uncertainty quantification comprises:

[0010] S101, time sequence feature fusion: for single-frame point cloud perception difficulties caused by occlusion or sparsity, a non-local network module with invalid voxel suppression is designed, and by calculating the time sequence correlation between the bird's eye view (BEV) feature maps of continuous multiple frames of point clouds, the effective feature information of the historical frames is aggregated to the current frame to complete the target feature and improve the detection accuracy;

[0011] S102, uncertainty modeling of perception results: in the regression branch of the 3D target detection network, an uncertainty prediction head is added, which outputs the variance σ 2 of each regression parameter corresponding to the predicted value while predicting the three-dimensional bounding box parameters (position, size, heading angle);

[0012] S103, uncertainty-driven loss function: an unsupervised negative log-likelihood loss function is used to train the uncertainty prediction head, which encourages the network to output high uncertainty for results with large prediction errors and low uncertainty for results with small prediction errors, and the loss term is defined as:

[0013]

[0014] Preferably, the S2 vehicle-mounted high-frequency local positioning comprises:

[0015] S201, IMU pre-integration factor construction: the IMU measurement data (angular velocity and linear acceleration) between two lidar key frame timestamps are integrated to obtain high-precision relative motion constraints, and are used to correct the motion distortion of the lidar point cloud, and the constraints constitute the IMU pre-integration factor in the factor graph;

[0016] S202, lidar odometry factor construction: geometric features such as edge points and plane points are extracted from the deformed point cloud, and by frame-to-map matching, the distance error of the feature points to the corresponding geometric features is minimized, and the relative pose transformation of the vehicle is calculated, and the constraints constitute the lidar odometry factor in the factor graph.

[0017] Preferably, the S3 time delay compensation and global constraint construction comprises:

[0018] S301, time domain adaptive time delay compensation: when the vehicle receives a roadside observation with a past time timestamp t vehicle at t road , find the time interval [t road , t k ] containing t k+1 in the historical key frame pose sequence of the vehicle end;

[0019] S302, pose forward propagation: using the motion state estimated by IMU pre-integration of the vehicle in the interval [t k , t k+1 ], the roadside observation pose is forward propagated from t road to t k+1 aligned with the vehicle end state by linear interpolation, and the propagation formula is:

[0020]

[0021] Wherein, is the original roadside observation pose, is the pose transformation of the vehicle from t road to t k+1 ;

[0022] S303, uncertainty-aware roadside perception factor construction: the roadside global pose after time delay compensation is constructed as a roadside perception factor, and the covariance matrix thereof is composed of the perception uncertainty σ 2 predicted in S102, realizing adaptive suppression of perception noise.

[0023] Preferably, the S4 factor graph fusion optimization comprises:

[0024] S401, factor graph construction: taking the vehicle pose, speed and IMU bias at each key frame time as the node variable to be optimized;

[0025] S402, constraint construction: constructing three types of constraint edges (factors) in the factor graph, including IMU pre-integration factor connecting consecutive key frames, laser radar odometry factor, and roadside perception factor connecting specific key frame nodes and global coordinate system;

[0026] S403, nonlinear optimization solution: using nonlinear least squares method, the factor graph is iteratively optimized to solve the optimal state quantity that minimizes the weighted sum of squares of errors defined by all factors, and a globally consistent vehicle trajectory is obtained.

[0027] Compared with the prior art, the present application has the following beneficial effects:

[0028] 1. A 3D target detection module that fuses temporal features and uncertainty modeling is constructed, which filters ground, building, and other static background point clouds based on scene priors (such as vehicle drivable areas), reducing more than 90% of invalid data.

[0029] 2. An uncertainty-aware positioning fusion framework is proposed, which quantifies the uncertainty of 3D target detection results and uses them as the covariance of positioning constraints, which enables the positioning system to adaptively weigh the reliability of roadside perception results, effectively suppressing the negative impact of perception errors on positioning accuracy.

[0030] 3. A time-domain adaptive time delay compensation mechanism is designed, which can accurately compensate for the time delay caused by vehicle-road communication and perception computing, accurately aligning the delayed roadside observation data with the real-time motion state of the vehicle, ensuring the effectiveness of data fusion. BRIEF DESCRIPTION OF DRAWINGS

[0031] Figure 1 The overall architecture diagram of the vehicle global positioning bias correction method based on roadside perception results of the present application;

[0032] Figure 2 The structure diagram of the 3D target detection module that fuses temporal features and uncertainty modeling in the present application;

[0033] Figure 3 The principle diagram of time-domain adaptive time delay compensation in the present application;

[0034] Figure 4 The trajectory comparison visualization result diagram of the method and the reference positioning method of the present application;

[0035] Figure 5 The factor graph of the method of the present application. DETAILED DESCRIPTION

[0036] The technical solutions in the embodiments of the present application will be described in detail below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0037] Please refer to Figures 1-5 The present application provides a technical solution: a vehicle global positioning bias correction method based on roadside perception results, comprising the following steps:

[0038] Step 1: High-precision roadside perception based on temporal feature fusion and uncertainty modeling.

[0039] In order to provide accurate external observations for vehicle positioning, the present application proposes an improved 3D target detection algorithm (as shown in Figure 2 ), to improve the perception performance of roadside equipment.

[0040] Step 1 specifically includes the following sub-steps:

[0041] Step 11: Time sequence feature fusion, this step receives a sequence of 4 continuous frames of roadside laser radar point clouds, first, each frame of point cloud is converted into a pseudo-image format through voxelization processing, and the bird's eye view (BEV) feature map is extracted using a 3D backbone network, in view of the problem that the target in the single frame of point cloud is blocked or the point cloud is sparse due to too far distance, the present application designs a non-local network module for invalid voxel suppression, which calculates the pixel-level correlation between the BEV feature map of the current frame and the BEV feature map of the historical frame, forms a time sequence attention weight matrix, and suppresses the blank voxel area without point cloud when calculating to prevent interference. Through the attention weight, the most relevant feature information in the historical frame is weighted and fused into the feature map of the current frame, so as to effectively complete the feature expression of the blocked or sparse target, and finally output the BEV feature map enhanced by time sequence for subsequent processing;

[0042] Step 12: Perception result uncertainty modeling, in order to quantify the reliability of 3D target detection results, in the regression branch of the detection network, an uncertainty prediction head is additionally added in addition to the prediction of target three-dimensional bounding box parameters (center point position (x, y, z), size (w, l, h), yaw angle θ), the prediction head is used to learn a kind of accidental uncertainty, that is, a variance σ 2 is output corresponding to each bounding box regression parameter. In order to train the prediction head, the present application uses unsupervised learning to learn the variance. The uncertainty loss function is:

[0043]

[0044] where y i represents the real Bounding box pose of the vehicle, f i (x) represents the vehicle Bounding box pose detected, σ 2 represents the variance of the data itself, the loss function prompts the network to output low variance for the results with small prediction error, and high variance for the results with large prediction error. Finally, this step outputs the pose estimation of the vehicle in the global coordinate system, and a covariance matrix describing the reliability of the results composed of variances σ

[0045] Step 2: Vehicle local positioning based on laser inertial navigation odometer.

[0046] This step is executed at the vehicle end, fusing the data of LiDAR and IMU in a tightly coupled manner to output the local relative pose of the vehicle at a high frequency (10Hz).

[0047] Step 2 specifically includes the following sub-steps:

[0048] Step 21: IMU pre-integration, integrating the IMU measurements (angular velocity and linear acceleration) at a higher frequency (e.g. 100Hz) between two LiDAR key frames (time interval [t k ,t k+1 ]), to calculate the relative pose transformation, velocity and bias from time k to k+1. This result is used to correct the motion distortion of the next frame of LiDAR point cloud, and also constitutes an IMU pre-integration factor to constrain the relative motion between two consecutive key frames;

[0049] Step 22: LiDAR de-distortion, first, using the IMU pre-integration result obtained in step 21, the original LiDAR point cloud at time k+1 is corrected for motion distortion, and then the edge points and plane points and other geometric features are extracted from the de-distorted point cloud. The feature points extracted in the current frame are matched with the corresponding features in a local map constructed from the historical key frame point clouds, and the optimal relative pose transformation is solved by minimizing the distance error of the feature points to the corresponding geometric features. This relative pose transformation constitutes the LiDAR odometry factor.

[0050] Step 3: Global positioning bias correction by fusing roadside information.

[0051] This step effectively fuses the roadside global observations in step 1 into the vehicle-end local positioning results in step 2 through the framework of factor graph optimization, to eliminate the cumulative error.

[0052] Step 3 specifically includes the following sub-steps:

[0053] Step 31: Vehicle-road communication time delay compensation, due to the time delay Δt in the generation and transmission of roadside perception results, the roadside observation received by the vehicle at time t vehicle is actually the state of the vehicle at time t road =t vehicle -Δt. To solve this spatio-temporal misalignment problem, the present application adopts a time-domain adaptive linear interpolation mechanism as shown in Figure 3 After the vehicle receives the roadside observation with timestamp t road , it finds the time interval [t k ,t k+1 ] containing this timestamp in its historical key frame pose sequence. Subsequently, using the motion state of the vehicle in this interval obtained by IMU pre-integration, the roadside observation pose is compensated from troad t k+1 t

[0054]

[0055] t road is the original time of the roadside observation. t k+1 is the time of the vehicle key frame aligned with the observation. is the original roadside observation pose at t road , which represents the pose of vehicle v in the world coordinate system. is the relative pose transformation of vehicle itself from t road to t k+1 , calculated by the pre-integration of the on-board IMU. is the final roadside observation pose after time delay compensation, aligned to t k+1 , which will be used in the factor graph optimization in step 33.

[0056] Step 32: Construction of the roadside perception factor with uncertainty awareness, the roadside observation after time delay compensation is constructed as a roadside perception factor, which is used to constrain the global pose of the vehicle at k+1 time. The core innovation of this factor is that its covariance matrix (or information matrix) is adaptive, directly determined by the perception uncertainty σ 2 predicted by the network in step 12. When the roadside perception result has high confidence (σ 2 is small), the weight of this factor in the optimization problem is high; on the contrary, when the perception uncertainty is large (for example, the vehicle is severely occluded), the weight is reduced, which realizes the adaptive suppression of low-quality observations and avoids damaging the positioning results.

[0057] Step 33: All factors are unified into a factor graph for joint optimization, as shown in Figure 5 , the nodes in the graph represent the variables to be optimized (vehicle pose, velocity and IMU bias at each key frame time), and the edges in the graph represent the constraint factors, including: IMU pre-integration factor, laser odometry factor and roadside perception factor. The goal of the entire optimization problem is to find a set of optimal node states, so that the weighted sum of errors defined by all factors is minimized. This nonlinear least squares problem is solved by an iterative optimization algorithm. After solving, the global pose of the vehicle at all key frame times is output. This pose trajectory not only maintains the local smoothness of laser inertial navigation, but also eliminates the cumulative drift through the constraint of the roadside factor, and is aligned with the global coordinate system, thereby obtaining the final corrected high-precision vehicle global positioning result.

[0058] Embodiment one, the application builds an experimental platform in two typical scenes (structured campus roads and unstructured car test fields) in Chang'an University campus for field verification. The experimental vehicle is equipped with a 128-line laser radar and a combined navigation system, and the same sensors are deployed on the roadside. We take the output of high-precision GNSS / RTK as the positioning true value, and use the root mean square error (RMSE) of absolute trajectory error (APE) as the core evaluation index. We compare our method (Ours) with the current advanced open-source laser radar inertial navigation positioning algorithm LIO-SAM, and the test results are shown in Table I, as follows:

[0059]

[0060] Wherein, N is the total number of sampling points, and APE-RMSE reflects the average level of overall positioning error.

[0061] Table I comparison test results of positioning algorithm

[0062]

[0063]

[0064] From the visualization results of Table I and Figure 4 It can be seen that LIO-SAM, as a pure local positioning method, has obvious trajectory drift and cumulative error after a long time running. In contrast, the method proposed in the application effectively fuses the roadside perception information with global reference, and its positioning trajectory is highly consistent with the true trajectory. The fusion mechanism of uncertain perception and the time delay compensation mechanism ensure that the roadside constraints are applied stably and accurately, thereby successfully correcting the cumulative deviation of the local odometer and giving the vehicle high-precision global positioning capability in GNSS denial environment. The experimental data fully prove the significant advantages of the application in improving positioning accuracy and robustness.

[0065] Although embodiments of the application have been shown and described, it will be understood by those having ordinary skill in the art that various changes, modifications, substitutions and alterations can be made therein without departing from the principles and spirit of the application, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for vehicle global positioning bias correction based on roadside perception results, characterized in that, Comprising the following steps: S1, high-precision roadside perception and uncertainty quantification: the roadside device adopts a 3D target detection network that fuses time sequence features to obtain the pose of the vehicle in the global coordinate system, and quantifies the uncertainty of each dimension of the perception result, and outputs the pose and the corresponding covariance matrix; S2, vehicle high-frequency local positioning: the vehicle end estimates the high-frequency relative pose of the vehicle in real time through tightly coupled lidar and inertial measurement unit (IMU) data, and constructs IMU pre-integration factors and lidar odometry factors; S3, time delay compensation and global constraint construction: for the transmission and calculation time delay of the roadside perception result, a time domain adaptive interpolation mechanism is used for compensation, and the compensated roadside observation and its uncertainty are constructed as roadside perception factors; S4, factor graph fusion optimization: the roadside perception factors, IMU pre-integration factors and lidar odometry factors are jointly constructed into a factor graph, and the vehicle accurate pose corrected for cumulative error and aligned to the global coordinate system is output by nonlinear optimization solution. 2.The method of claim 1, wherein, The S1 high-precision roadside perception and uncertainty quantification comprises: S101, time sequence feature fusion: for the difficulty of single-frame point cloud perception caused by occlusion or sparsity, a non-local network module with invalid voxel suppression is designed, the time sequence correlation between the bird's eye view (BEV) feature maps of continuous multiple frames of point cloud is calculated, the effective feature information of the historical frames is aggregated to the current frame to complete the target feature and improve the detection accuracy; S102, perception result uncertainty modeling: in the regression branch of the 3D target detection network, an uncertainty prediction head is added, which outputs the accidental uncertainty of each regression parameter, i.e. the variance σ corresponding to the predicted value, while predicting the three-dimensional bounding box parameters (position, size, heading angle) 2 ; S103, uncertainty-driven loss function: an unsupervised negative log-likelihood loss function is used to train the uncertainty prediction head, which prompts the network to output high uncertainty for the results with large prediction error and low uncertainty for the results with small prediction error, and the loss term is defined as: 3.The method of claim 1, wherein, The S2 vehicle high-frequency local positioning comprises: S201, IMU pre-integration factor construction: the IMU measurement data (angular velocity and linear acceleration) between the time stamps of two laser radar key frames is integrated to obtain high-precision relative motion constraints, and is used to correct the motion distortion of the laser radar point cloud, and the constraint constitutes the IMU pre-integration factor in the factor graph; S202, laser radar odometry factor construction: geometric features such as edge points and plane points are extracted from the deformed point cloud, the distance error of the feature points to the corresponding geometric features is minimized through frame-to-map matching, and the relative pose transformation of the vehicle is calculated, and the constraint constitutes the laser radar odometry factor in the factor graph.

4. The method of claim 1, wherein, The S3 time delay compensation and global constraint construction comprises: S301、time domain adaptive time delay compensation: when the vehicle receives the roadside observation with the past time timestamp t vehicle at t road , find the time interval [t road , t k ] containing t k+1 in the historical key frame pose sequence at the vehicle end S302, pose forward propagation: using the motion state estimated by IMU pre-integration in the interval [t k ,t k+1 ], the roadside observation pose is forward propagated from t road time to t k+1 time aligned with the vehicle state through linear interpolation, and the propagation formula is: wherein, is the original roadside observed pose, is the pose transformation of the vehicle from t road to t k+1 ; S303, uncertainty-aware roadside perception factor construction: the roadside global pose compensated by the time delay is constructed as a roadside perception factor, and a covariance matrix of the roadside perception factor is constructed by the perception uncertainty σ predicted in S102 2 The adaptive suppression of the perception noise is achieved.

5. The method of claim 1, wherein, The S4 factor graph fusion optimization comprises: S401, factor graph construction: the vehicle pose, speed and IMU bias at each key frame time are taken as the node variables to be optimized; S402, constraint construction: three types of constraint edges (factors) are constructed in the factor graph, including IMU pre-integration factors connecting consecutive key frames, laser radar odometry factors, and roadside perception factors connecting specific key frame nodes and the global coordinate system; S403, nonlinear optimization solution: using nonlinear least squares method, iteration optimization is carried out on the factor graph, the optimal state quantity is solved, which makes the weighted sum of squares of all factor defined errors minimum, and the global consistent vehicle trajectory is obtained.

Images