Laser radar and camera online drift detection method, correction method, device and storage medium

Abstract

The present invention discloses a method, correction method, device, and storage medium for online drift detection of a laser radar (LiDAR) and a camera. The online drift detection method includes point cloud and image registration and sensor drift detection. The point cloud and image registration includes: searching for the camera image data frame closest to the LiDAR point cloud data frame in the data time domain to complete data synchronization between the LiDAR and the camera; based on the synchronized data, estimating the LiDAR motion and camera motion from the previous N frames to the current frame through point cloud registration and image registration; sensor drift detection includes: solving the motion deviation between the LiDAR and the camera in the corresponding frame based on the estimated LiDAR motion and camera motion; when the motion deviation between the LiDAR and the camera in the corresponding frame is greater than a set threshold, detecting whether sensor drift has occurred in the current frame. The method of the present invention has accurate, comprehensive, and robust feature extraction, good generalization performance, and good drift detection and calibration accuracy and robustness in all directions.

Description

Technical Field

[0001] The present application relates to the field of intelligent driving sensor calibration, and in particular to a method for online drift detection and external parameter correction for lidar and camera. Background Art

[0002] As autonomous vehicles drive, body vibration inevitably causes the relative posture of their LiDAR and camera sensors to drift. Consequently, the external parameters between the two sensors also change. In this case, the previously calibrated parameters become incompatible with the LiDAR and camera coordinate systems, causing the autonomous vehicle's perception performance to degrade or even fail, seriously threatening driving safety.

[0003] To address the issue of sensor drift during vehicle operation, some studies have used the knowledge that depth discontinuities in lidar point cloud data often project onto image edges when calibration parameters are accurate. This approach uses extrinsic parameter correction to determine sensor drift by searching for and determining whether extrinsic parameters maximize the product of the point cloud edge projection and the image edge grayscale value. However, due to parallax between the two sensors, edges formed by distant points in the lidar point cloud often intersect with edges of nearby points, resulting in blurred edges after point cloud projection. Furthermore, a large number of invalid edge features in camera images are unrelated to depth discontinuities in the point cloud, which can lead to feature alignment errors and misdirected algorithm optimization. Deep learning techniques have also been used to address the problem of online sensor calibration. One study iteratively optimized the correction extrinsic parameters by training five neural network models with varying degrees of drift. However, the performance of deep learning-based methods is heavily dependent on the training data, meaning that these methods suffer from poor generalization when the vehicle driving environment or sensor parameters change. Furthermore, these methods lack an effective mechanism for determining sensor drift, requiring calibration to be performed for every frame during algorithm execution, which wastes computational resources.

[0004] In summary, existing technologies face the following challenges: methods based on point cloud and image edge alignment are ineffective at detecting and correcting drift due to sensor parallax and a large number of invalid image edges; deep learning-based technologies lack a mechanism for detecting sensor drift and rely on large amounts of training data, resulting in poor generalization performance. Furthermore, both approaches attempt to extract common environmental features from data of different modalities, which is highly error-prone. Furthermore, both methods perform poorly in directional calibration where the LiDAR beam resolution is low. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide an online sensor drift detection method, correction method, device and storage medium, which are used to solve the problem of online drift detection and automatic correction of lidar and camera during driving of intelligent driving vehicles.

[0006] The present invention first provides a method for online drift detection of a laser radar and a camera, which is characterized by comprising:

[0007] Point cloud and image registration, including:

[0008] Search the camera image data frame closest to the lidar point cloud data frame in the data time domain to complete the data synchronization between the lidar and camera;

[0009] Based on the synchronized data, the lidar motion and camera motion from the previous N frames to the current frame are estimated through point cloud registration and image registration;

[0010] Sensor drift detection, including:

[0011] Based on the estimated lidar motion and camera motion, solve the motion deviation between the lidar and camera in the corresponding frame;

[0012] Set a threshold. When the motion deviation between the lidar and camera in the corresponding frame is greater than the set threshold, detect whether sensor drift occurs in the current frame.

[0013] The present invention also provides a method for online drift extrinsic parameter correction of a laser radar and a camera, which is characterized by comprising:

[0014] Sensor drift detection based on online drift detection method of lidar and camera;

[0015] If the detection result is sensor drift, the sensor external parameters are calibrated. The method for calibrating the sensor external parameters includes:

[0016] Estimate the scaled camera motion by matching the 2D feature points in the current image frame with the corresponding 3D points in the point cloud of the previous N frames to solve the scaled camera motion;

[0017] According to the scaled camera motion, the motion matrix between the previous frame and the current frame is obtained and averaged;

[0018] After averaging, the previous frame point cloud without drift is converted to the current position as the reference point cloud, and the final correction external parameters are obtained by aligning it with the current drifting point cloud.

[0019] The sensor drift detection method of this invention uses point cloud registration and image registration techniques to estimate the motion between the lidar and camera frames. Drift is determined by comparing the magnitude of the motion between the two modalities. The correction method of this invention uses sensor motion to correct extrinsic drift online. This method does not require the extraction of common edge features from different modalities and does not require a large amount of training data.

[0020] The present invention also provides a laser radar and camera online drift detection and external parameter correction device, which is characterized by comprising:

[0021] Camera, used to acquire environmental images in real time;

[0022] LiDAR, used to obtain environmental point cloud data in real time;

[0023] as well as,

[0024] The processor executes the steps of the above-mentioned online drift detection and external parameter correction method.

[0025] Compared with the prior art, the present invention has the following beneficial effects:

[0026] 1. More accurate and comprehensive feature extraction: This method is based on the principle of point cloud and image registration, extracting common features from the same modality, and the feature extraction is accurate. The extracted features are more comprehensive and not limited to edge features. Features are easier to align, which will not cause errors in the algorithm optimization direction.

[0027] 2. No need for a large amount of training data: It does not rely on a large amount of training data and has good generalization performance;

[0028] 3. Good robustness of drift detection and correction: It is insensitive to the resolution of the lidar and has good robustness of drift detection and calibration in all directions. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 This is an overall framework diagram of a method for online drift detection and extrinsic parameter correction of a laser radar and camera provided by one embodiment of the present application;

[0030] Figure 2 A typical frequency distribution histogram and fitted Gaussian distribution curve of the laser radar and camera motion deviation when no sensor drift occurs, provided by an embodiment of the present application;

[0031] Figure 3 This is a statistical test result of the drift detection accuracy under different global coefficients α and frame numbers N provided by an embodiment of the present application;

[0032] Figure 4 It is a two-dimensional feature point u provided in one embodiment of the present application. k f and 3D point cloud point p c,k-n Matching schematics;

[0033] Figure 5 This is a graph showing the experimental results of online drift detection and correction using edge alignment and deep learning methods under conditions of no sensor drift, small sensor drift, and large sensor drift, provided by an embodiment of the present application.

[0034] Figure 6An embodiment of the present application provides an image back-projection point cloud map after automatic correction of external parameters using edge alignment-based methods and deep learning methods under small and large sensor drifts. DETAILED DESCRIPTION

[0035] In order to make the objectives, technical solutions and advantages of the embodiments of the present application clearer, the implementation methods of the present application will be further described in detail below with reference to the accompanying drawings.

[0036] Please refer to Figure 1 , which shows the overall framework of a method for online drift detection and extrinsic parameter correction of a laser radar and camera provided by an embodiment of the present application, which is applied during the driving process of an intelligent driving vehicle. The method for online drift detection and extrinsic parameter correction of a laser radar and camera may include:

[0037] In this embodiment, sensor drift refers to the change in position and posture of the lidar coordinate system relative to the camera coordinate system with the camera coordinate system as a reference, which is called sensor drift.

[0038] Step 101, point cloud and image registration: First, search for the camera image data frame closest to the lidar point cloud data frame in the data time domain, complete the data synchronization of the lidar and camera, and use the synchronized data as input data; then estimate the lidar motion and camera motion from the previous N frames to the current frame through point cloud registration and image registration respectively.

[0039] Step 1011, LiDAR motion estimation: First, pre-process the LiDAR point cloud, including removing ground points and distant points, downsampling, etc., to enhance the robustness and processing speed of point cloud registration. Then, according to formula (1), the original external parameter T is used to calculate the l c Convert the point cloud to the camera coordinate system:

[0040]

[0041] in, and Represents a three-dimensional point in the camera coordinate system and the lidar coordinate system respectively, and T l c The rotational and translational components of It can be expressed in the form of Euler angles, namely yaw, pitch and roll, [R ψ ,R θ ,R φ ] T ,

[0042] Finally, the first N frames of point cloud and the current point cloud Perform registration to estimate lidar motion between frames in, and They are The rotation and translation components of , n = 1, 2, ..., N. Since the point cloud has been transformed into the camera coordinate system, the lidar motion here also represents the camera motion when there is no sensor drift.

[0043] Step 1012, camera motion estimation: Before motion estimation, the camera has been internally calibrated to correct lens distortion, and the camera internal parameter is K. First, the image feature extraction algorithm is used to extract and match the first N frames of image I k-n ,n=1,2,...,N and current image I k Feature points between Then combine the internal parameter K to solve the essential matrix between frames Finally, the essential matrix Decompose into the first N frames of image I k-n With the current frame image I k The transformation matrix between in, and They are The rotation and translation components of . Due to the inherent scale ambiguity of the monocular camera, the translation component It is a normalized form and has no scale information.

[0044] Step 102, sensor drift detection: After obtaining a set of lidar motion and camera motion, solve the lidar and camera motion deviations of the corresponding frame. When any motion deviation is greater than a set threshold, detect whether sensor drift occurs in the current frame.

[0045] Step 1021, solve the sensor motion deviation: solve the rotation part of the two sensor motion according to formula (2) and Deviation

[0046]

[0047] The deviation According to formula (3), it is converted into the form of Euler angle to clearly show the deviation in each rotation direction:

[0048]

[0049] Among them, e ψ,k-n 、e θ,k-n and e φ,k-nRepresents the motion deviation in yaw, pitch and roll directions, r ij Representation bias Elements in .

[0050] Step 1022, determining the threshold value of the non-drift state: Due to the existence of sensor measurement noise, synchronization error and motion estimation error, even in the absence of sensor drift, the motion deviation e ψ,k-n 、e θ,k-n and e φ,k-n It is not 0 either.

[0051] Please refer to Figure 2 , Figure 2 The figure shows the LiDAR and camera motion deviations e when there is no sensor drift. ψ,k-n 、e θ,k-n and e φ,k-n Typical frequency distribution histogram and fitted Gaussian distribution curve.

[0052] Record the lidar and camera motion deviations e between the previous N frames and the current frame through experiments ψ,k-n 、e θ,k-n and e φ,k-n , n=1,2,...,N, calculate its frequency distribution histogram, and fit the Gaussian distribution function according to formula (4):

[0053]

[0054] where μ j,k-n and σ j,k-n are the mean and variance of the Gaussian distribution function fitted in the j direction of the kn frame respectively. In order to avoid misjudgment when there is no sensor drift, the deviation threshold Set to μ j,k-n -3σ j,k-n and μ j,k-n +3σ j,k-n , j = ψ, φ, θ, when equation (5) exists, it is considered that there is no sensor drift in the current frame,

[0055] μ j,k-n -3σ j,k-n ≤e j,k-n ≤μ j,k-n +3σ j,k-n (5)

[0056] In this embodiment, the test platform is a vehicle platform equipped with a laser radar and a camera. The vehicle travels slowly on a normal road at a low speed of 10 km / h. At the same time, the on-board computer records the perception information of the laser radar and the camera. Under the premise that there is no drift of the sensor, step 101, point cloud and image alignment, and step 1021, sensor motion deviation solution are performed to obtain motion deviation data, generate a frequency distribution histogram of the motion deviation in each rotation direction, and fit a Gaussian function.

[0057] Step 1023, drift state determination: If any Euler angle deviation e j,k-n Exceeding the threshold Then the sensor drift may have occurred in the current frame, and the confidence level of drift on the corresponding rotation axis is C j,k-n It can be described by the following formula:

[0058]

[0059] Considering the maximum Euler angle deviation of each frame and the root mean square of the three Euler angle deviations, when equation (7) holds true, it is considered that sensor drift has occurred in the current frame.

[0060]

[0061] Here, α is the global coefficient.

[0062] In this embodiment, a series of statistical experiments were conducted to determine the optimal hyperparameters, namely the global coefficient α and the size of N in the first N frames. A vehicle platform equipped with a lidar and a camera was driven slowly on a normal road at a low speed of 10 km / h. At the same time, the onboard computer recorded the perception data of the lidar and camera as input data. The input lidar point cloud data was multiplied by a random offset matrix T for every interval greater than or equal to N frames. r , as shown in formula (8):

[0063] p c =T r T l c p l (8)

[0064] Under different global coefficients α and frame numbers N, run step 101, point cloud and image registration, step 1021, sensor motion deviation solution, step 1022, non-drift state threshold determination, step 1023, drift state determination. The statistical test results are as follows Figure 3 As shown, it is found that when N is 7 and the global coefficient α is 0.945, the maximum drift state judgment accuracy is 99.63%. Therefore, the above two data are selected as the hyperparameters in step 1023.

[0065] Step 103, automatic correction of external parameters: When the sensor drift is detected in the current frame, the camera motion with scale is estimated first, and the current image frame I is corrected. k The two-dimensional feature points in And the corresponding first N frames point cloud P k-n The three-dimensional point p c,k-n Match and solve the scaled camera motion Combined with step 1011, the camera motion obtained from the laser radar motion estimation Get the motion matrix between the previous frame and the current frame After averaging, the point cloud P k-1 Transform to the current position as the reference point cloud P k R , by comparing with the current drifting point cloud P k Register to obtain the final correction external parameters

[0066] Please refer to Figure 4 , Figure 4 The two-dimensional feature points are shown in With the 3D point cloud point p c,k-n Matching schematics.

[0067] Step 1031, camera motion estimation with scale: First, the laser radar point cloud P without drift is converted into k-n Projected to the corresponding image I k-n superior:

[0068]

[0069] in, is a three-dimensional point p c,k-n The projection point in the pixel coordinate system, π(·) is the projection function.

[0070] Then, in step 1012, the image feature points have been extracted in the camera motion estimation. According to formula (10), according to the image feature points and 3D projection points Euclidean distance search between image feature points The nearest 3D projection point

[0071]

[0072] Among them, t a is the range threshold. In this embodiment, t a Set to 2 pixels. If there are multiple 3D projection points near the image feature point Then select the three-dimensional point p with the smallest depth c,k-n The corresponding projection point Combine it with the image feature points match.

[0073] In step 1012, in camera motion estimation, the first N frames of images I have been extracted and matched. k-n and the current image I k-n The feature points between them. Then we can build the point cloud P of the first N frames k-n A three-dimensional point p in c,k-n and the two-dimensional feature points in the current image The corresponding relationship between the two is solved by the PnP algorithm to solve the camera motion with scale.

[0074] Step 1032, transformation matrix solution: In step 1011, in the laser radar motion estimation, the first N frames of point cloud P can be obtained k-n The lidar motion between each adjacent frame in Because the point cloud has been converted to the camera coordinate system, and there is no sensor drift in the first N frames, so the It also represents the camera motion. Based on this, we can further establish motion constraints:

[0075]

[0076] This can be used to obtain a series of motion matrices between the previous frame and the current frame. The averaged motion matrix is ​​obtained by formula (12):

[0077]

[0078] in, It corresponds to Rotational component The quaternion of and They are The rotational and translational components of .

[0079] Step 1033, drift point cloud correction: correct the point cloud P without drift in the previous frame. k-1 Through the motion matrix Transform to the current position as the reference point cloud P k R , and then take the current drifting point cloud P k With the reference point cloud P k R Registration to obtain the final correction matrix Finally, the external parameters between the lidar and the camera are corrected by equation (13):

[0080]

[0081] Figure 5 The following graphs show the experimental results of online drift detection and correction for this method under conditions of no sensor drift, small drift, and large drift, compared with edge alignment-based methods and deep learning-based methods. It can be seen that this method consistently identifies the correct calibration, while the edge alignment-based method makes some misjudgments. The deep learning-based method lacks a drift detection mechanism, so it runs an automatic correction routine for each frame, but its generalization ability is poor and it cannot converge to the correct extrinsic parameters. In subsequent conditions of small and large sensor drift, this method performs well in all directions, effectively detecting drift and correcting extrinsic parameters, completely outperforming other methods.

[0082] Figure 6 Figure 1 shows the back-projected point cloud images after automatic extrinsic correction using our method, compared to edge-alignment-based methods and deep learning-based methods under small and large sensor drift. The dashed boxes in the figure indicate areas of misalignment. As can be seen, when depth changes significantly, our method's results exhibit some slight deviations, such as when distant greenery is projected onto the top of the vehicle. However, compared to the other two methods, our method achieves higher alignment accuracy and robustness overall.

[0083] In summary, the embodiment of the present application provides a method for online drift detection and external parameter correction of laser radar and camera, which takes the synchronized laser radar and camera data as input data, and estimates the laser radar motion and camera motion from the previous N frames to the current frame through point cloud registration and image registration respectively; solves the laser radar and camera motion deviation, and when any motion deviation is greater than the set threshold, detects whether the current frame has sensor drift; when it is detected that the current frame has sensor drift, first estimates the scaled camera motion, and combines the camera motion to calculate the deviation of the laser radar and camera motion. Get the motion matrix between the previous frame and the current frame After averaging, the point cloud P k-1 Transform to the current position as the reference point cloud P k R , by comparing with the current drifting point cloud P k Register to obtain the final correction external parameters The proposed online drift detection and extrinsic parameter correction method for lidar and camera is based on the principle of point cloud and image registration, extracting common features from the same modality. The feature extraction is accurate, comprehensive and robust, does not rely on a large amount of training data, has good generalization performance, is insensitive to the size of lidar resolution, and has good drift detection and calibration robustness in all directions.

[0084] The present embodiment also provides a computer-readable storage medium, which includes volatile or non-volatile, removable or non-removable media implemented in any method or technology for storing information (such as computer-readable instructions, data structures, computer program modules or other data). Computer-readable storage media include, but are not limited to, RAM (Random Access Memory), ROM (Read-Only Memory), EEPROM (Electrically Erasable Programmable read only memory), flash memory or other memory technology, CD-ROM (Compact Disc Read-Only Memory), digital versatile disk (DVD) or other optical disk storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other medium that can be used to store the desired information and can be accessed by a computer.

[0085] The computer-readable storage medium in this embodiment can be used to store one or more computer programs, and the one or more computer programs stored therein can be executed by a processor to implement the steps of the online drift detection and external parameter correction method of the lidar and camera in the above-mentioned embodiment.

Claims

1. A laser radar and camera online drift detection method, characterized in that: include: Point cloud and image registration, including: Search the camera image data frame closest to the lidar point cloud data frame in the data time domain to complete the data synchronization between the lidar and camera; Based on the synchronized data, the lidar motion and camera motion from the previous N frames to the current frame are estimated through point cloud registration and image registration; Sensor drift detection, including: Based on the estimated lidar motion and camera motion, solve the motion deviation between the lidar and camera in the corresponding frame; Set a threshold. When the motion deviation between the lidar and camera in the corresponding frame is greater than the set threshold, detect whether sensor drift occurs in the current frame. Estimate the lidar motion and camera motion from the previous N frames to the current frame through point cloud registration and image registration, including: Use the original external parameter T l c Convert the point cloud to the camera coordinate system: in, and Represents a three-dimensional point in the camera coordinate system and the lidar coordinate system respectively, and T l c The rotational and translational components of Expressed in the form of Euler angles, namely yaw, pitch and roll, [R ψ ,R θ ,R φ ] T , The first N frames of point cloud and the current point cloud Perform registration to estimate lidar motion between frames in, and They are The rotational and translational components of , n = 1, 2, ..., N; Extract and match the first N frames of image I k-n With the current image I k Feature points between Then, the intrinsic matrix between frames is solved by combining the camera intrinsic parameter K The essential matrix Decompose into the first N frames of image I k-n With the current frame image I k The transformation matrix between: in, and They are The rotational and translational components of Based on the estimated LiDAR motion and camera motion, the motion deviation between the LiDAR and camera for the corresponding frame is solved, including: Solving the rotational portion of the two-sensor motion and Deviation The deviation Converted into Euler angle form: e ψ,k-n =atan2(r 21 ,r 11 ) e φ,k-n =atan2(r 32 ,r 33 ) Among them, e ψ,k-n 、e θ,k-n and e φ,k-n Represents the motion deviation in yaw, pitch and roll directions, r ij Representation bias Elements in Set a threshold. When the motion deviation between the lidar and camera in the corresponding frame is greater than the set threshold, detect whether sensor drift occurs in the current frame, including: Based on the lidar and camera motion deviation e between the previous N frames and the current frame ψ,k-n 、e θ,k-n and e φ,k-n , n=1,2,...,N, get the deviation threshold μ j,k-n -3σ j,k-n and μ j,k-n +3σ j,k-n , where μ j,k-n and σ j,k-n are the mean and variance of the Gaussian distribution function fitted in the j direction of the kn frames, j = ψ, φ, θ; The fitted Gaussian distribution function is: When μ j,k-n -3σ j,k-n ≤e j,k-n ≤μ j,k-n +3σ j,k-n When , it is considered that there is no sensor drift in the current frame; If any Euler angle deviation e j,k-n Exceeding the threshold Calculate the confidence C of drift on the corresponding rotation axis j,k-n : Considering the maximum Euler angle deviation of each frame and the root mean square of the three Euler angle deviations, if the following equation holds, it is considered that sensor drift has occurred in the current frame. Here, α is the global coefficient.

2. A method for online drift extrinsic parameter correction of laser radar and camera, characterized in that: include: Sensor drift detection based on the online drift detection method of lidar and camera according to claim 1; If the detection result is sensor drift, the sensor external parameters are calibrated.

3. The method for online drift extrinsic parameter correction of laser radar and camera according to claim 2, characterized in that: The method for calibrating the external parameters of the sensor includes: Estimate the scaled camera motion by matching the 2D feature points in the current image frame with the corresponding 3D points in the point cloud of the previous N frames to solve the scaled camera motion; According to the scaled camera motion, the motion matrix between the previous frame and the current frame is obtained and averaged; After averaging, the previous frame point cloud without drift is converted to the current position as the reference point cloud, and the final correction external parameters are obtained by aligning it with the current drifting point cloud.

4. The method for online drift extrinsic parameter correction of laser radar and camera according to claim 3, characterized in that: Methods for solving scaled camera motion include: The laser radar point cloud P that has not drifted k-n Projected to the corresponding image I k-n superior: in, is a three-dimensional point p c,k-n The projection point in the pixel coordinate system, π(·) is the projection function; According to the image feature points and 3D projection points The Euclidean distance between the image feature points The nearest 3D projection point Among them, t a Is the range threshold; if there are multiple three-dimensional projection points near the image feature point Then select the three-dimensional point p with the smallest depth c,k-n The corresponding projection point Combine it with the image feature points match; Create the first N frames of point cloud P k-n A three-dimensional point p in c,k-n and the two-dimensional feature points in the current image The corresponding relationship is solved by the PnP algorithm according to the following formula to solve the camera motion with scale According to the scaled camera motion, the motion matrix between the previous frame and the current frame is obtained and averaged, including: In the laser radar motion estimation, the first N frames of point cloud P are obtained k-n The lidar motion between each adjacent frame in n=2,3,...,N; Establish motion constraints and obtain a series of motion matrices between the previous frame and the current frame The motion constraints are: Get the averaged motion matrix in, It corresponds to Rotational component The quaternion of and They are The rotational and translational components of After averaging, the previous frame point cloud without drift is transferred to the current position as the reference point cloud. The final correction external parameters are obtained by registering it with the current drifting point cloud, including: The point cloud P of the previous frame without drift k-1 Through the motion matrix Transform to the current position as the reference point cloud P k R ; The current drifting point cloud P k With the reference point cloud P k R Registration to obtain the final correction matrix According to the final correction matrix The external parameters between the lidar and the camera are corrected by the following formula:

5. A laser radar and camera online drift detection and external parameter correction device, characterized in that: include: Camera, used to acquire environmental images in real time; LiDAR, used to obtain environmental point cloud data in real time; as well as, The processor executes the steps of the online external parameter calibration method for the laser radar and camera described in any one of claims 2-4.

6. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by the processor, the steps of the online external parameter calibration method for the laser radar and camera as described in any one of claims 2 to 4 are implemented.

Images